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.

Transforming all the data points from Cartesian coordinates to Frenet coordinates shows to be really helpful especially in a highway settings. Although the transformation introduces approximation errors the Frenet coordinates make calculations much easier. It uses the variables sand dto describe a vehicle’s position on the road. s is the longitudinal displacement or distance along the road (like the miles marks on a road) and the dcoordinate represents the lateral displacement, which can correspond directly to which lane the car is on.

A highway map with all waypoints data is provided in the in the filedata/highway_map.txt.

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);

In order to comply with the requirement that the car increases or reduces its velocity with constant acceleration (<10 m/s²) I made sure the car reference velocity only changed at increments of +/- 5 m/s² . The code also divides the final spline into evenly spaced points (on every 2/100 secs) so that the ego car can travel at the desired reference velocity. Some geometry gymnastics is then required by converting the three anchor points (at 30 m, 60 m and 90 m) to the ego vehicle coordinates and have the number of spaces to be calculated. [Credit to Aaron Brown and David Silver in the walkthrough video]

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.

The simulator provides the program with sensor fusion data so it can detect other cars and the traffic configuration. It’s a 2D vector of cars where each car’s elements contains:

  • 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.

The video below shows an accelerated ride of the vehicle simulation for 6.7 miles in the highway.

The code repository is found here.

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