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
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.
Spline interpolation
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:
xandyare the waypoint’s map coordinate position.sis the distance along the road to get to that waypoint in meters.dxanddydefine 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
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.
Sensor fusion
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
xposition in map coordinatesyposition in map coordinatesxdotvelocity in m/sydotvelocity in m/ssposition in Frenet coordinates,dposition 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 video below shows an accelerated ride of the vehicle simulation for 6.7 miles in the highway.
The code repository is found here.
