Self-Driving Car Path Planner

19 Aug 2017

Goals (as per Udacity)

In this project the goal is to safely navigate around a virtual highway with other traffic that is driving +-10 MPH of the 50 MPH speed limit. The car’s localization and sensor fusion data is provided, and there is also a sparse map list of waypoints around the highway. The car should try to go as close as possible to the 50 MPH speed limit, which means passing slower traffic when possible (note that other cars will try to change lanes too). The car should avoid hitting other cars at all cost and drive inside of the marked road lanes at all times, unless going from one lane to another. The car should be able to make one complete loop around the 6946m highway. Since the car is trying to go 50 MPH, it should take a little over 5 minutes to complete 1 loop. Also the car should not experience total acceleration over 10 m/s^2 and jerk that is greater than 50 m/s^3.

Route Planning, Smooth Trajectories

The simulator provides localization and sensor fusion every 20ms via the uWebSockets framework in C++. A spline library is used to create a smooth trajectory with minimal jerk. A spline is fitted to a minimal set of points, including the last two points from the previous path, and future points at 30m, 60m, and 90m ahead (based on map waypoints). The future points are provided by converting the Frenet coordinates of the car (s, d) to (x, y) coordinates. The lane is an integer variable that factors into the lateral displacement (d) such that changing lanes results in a recalculated spline. Note that the (x, y) points are first transformed to the car’s local coordinate space to simplify calculations. The simulator returns a list of points from the previous path that have not yet been processed. For instance, if the simulator processed 5 points, it would return a list with the remaining 45 points. These points are pushed to the new path list. The remaining future points are calculated by spacing x-coordinates according to the desired reference velocity, finding the corresponding y-coordinates from the spline, and transforming back out of local coordinate space.

Prediction, Behavior Planning, Control

On each cycle, the planner iterates through sensor fusion data for all surrounding vehicles. It determines the closest forward and rear vehicle, as well as the closest side vehicles, both ahead and behind. It also tracks the velocity of the vehicles. Frenet coordinates are used to determine lane positions of vehicles as well as distances to vehicles along the road. Side vehicles are flagged if there is an unsafe gap (based on distance and relative velocity). If the forward gap is less than 20m, ego decelerates rapidly, but if the forward gap is between 20m and 30m, a Proportional-Differential (PD) controller seeks to maintain a constant 25m distance and match the leader velocity as follows:

a(t+1) = Kp * (Dr(t) - Dd(t)) + Kd * Vr(t)

where:

a(t+1) is acceleration at time t+1
Dr(t) is relative distance at time t (current forward gap)
Dd(t) is the desired distance at time t (25m)
Vr(t) is the relative velocity at time t ((leader velocity - ego velocity)/50) 
Kp is the gain for relative distance (chosen as 0.04)
Kd is the gain for relative velocity (chosen as 0.4)

Whenever the forward gap is less than 30m, ego also attempts a lane change, assuming there are no flagged vehicles to the side. If ego is in the center lane and both side lanes are open, it chooses the lane with the larger forward gap. Additionally ego prefers the center lane when it makes sense, waits at least 3s between lane changes, attempts lane changes if the rear gap is very small, and never accelerates unless the current velocity is less than 49.5mph (standard acceleration is 0.4mph per 20ms cycle).

Results

A video of one lap in the simulator is shown below. Note that the green dots in front of the car show the next 50 points generated by the path planner.

Future Work

At rare times, another car may drive erratically, cross lanes, and collide with the ego vehicle. Ego should ultimately track erratic drivers and maintain a safe distance.

The sensor fusion processing and control logic would traditionally be implemented via a Finite State Machine (FSM) with states such as Keep Lane, Prepare Lane Change Left, Prepare Lane Change Right, Lane Change Left, and Lane Change Right. However, FSMs have the potential to become complex and difficult to maintain. Magnus Olsson presents an alternative approach that is also worth investigating in Behavior Trees for decision-making in Autonomous Driving.

Credits

The Udacity project walk-through provides key details around spline-based smooth trajectory generation.