Self-driving path planner using a simulator

Udacity Self-Driving Engineer Nanodegree. Term 3, assignment 1

This is the summary of the implementation of a path planning algorithm for a self-driving car in a simulated highway.

Frenet coordinates

Image for post
Image for post

Spline interpolation

Each waypoint in the list contains the values[x, y, s, dx, dy] where:

  • x and yare the waypoint’s map coordinate position.
  • s is the distance along the road to get to that waypoint in meters.
  • dx and dy define the unit normal vector pointing outward of the highway loop.

Because the waypoints are widely spaced its outline looks very jagged. Using these points as the car trajectory would cause the a lot of jerkness because of acceleration in the sharp corners. The simple solution is to interpolate the points using a spline curve, which creates a piecewise polynomial with continuous derivative.

Also — as suggested in the walkthrough video — instead of using all the Frenet coordinates on tightly spaced points, only three Frenet points are used with their s values spaced by 30 meters.

vector <double> next_wp0 = getXY(car_s+30, (2+4*lane), map_waypoints_s, map_waypoints_x, map_waypoints_y);vector <double> next_wp1 = getXY(car_s+60, (2+4*lane), map_waypoints_s, map_waypoints_x, map_waypoints_y);vector <double> next_wp2 = getXY(car_s+90, (2+4*lane), map_waypoints_s, map_waypoints_x, map_waypoints_y);

Max acceleration and speed limit

Since the speed limit is 50 mph when there are no other cars ahead of the ego vehicle I increase the reference velocity up to 49.5 mph.

Sensor fusion

  • car’s unique ID
  • x position in map coordinates
  • y position in map coordinates
  • xdot velocity in m/s
  • ydot velocity in m/s
  • s position in Frenet coordinates,
  • d position in Frenet coordinates.

The program uses the Frenet values verify which lane the car its distance to the ego vehicle. More specifically:

  • closest car distance ahead of ego vehicle in the same lane,
  • closest car distance ahead of ego vehicle in the lane to the left,
  • closest car distance from the back of ego vehicle in the lane to the left,
  • closest car distance ahead of ego vehicle in the lane to the right,
  • closest car distance from the back of ego vehicle in the lane to the right.

If there’s a car too close to the ego vehicle (<30 meters) the planner decides wether to change lane or to reduce velocity.

Simulation in action

The code repository is found here.

Written by

Multi time founder, CTO and product executive. Founder @ridehugobus, EIR: @human_ventures & @techstars. VC: @collabfund

Get the Medium app

A button that says 'Download on the App Store', and if clicked it will lead you to the iOS App store
A button that says 'Get it on, Google Play', and if clicked it will lead you to the Google Play store