Robot Path Planning
This page will describe how to create a C++ path planner that navigates an autonomous car through traffic on a highway. The project uses Fernet coordinates, splines, state machines, behavioural prediction and lidar data. Below is a walkthrough of the fundamental principles and methods used in the project and it's subsequent C++ implementation. The information supplied is not exhaustive, and it is recommended to view the source code and comments on GitHub for a deeper understanding of the code. A link to the GitHub repository is here. A video of the project in action can be found here.
This project page will describe the following sections
Collision Avoidance
One of the key areas of trajectory planning is to prevent collisions between both static and dynamic objects. We will perform collision avoidance in the following sequence using a finite state machine.

Check sensor data for surrounding vehicles.

The current system state and FSM.

Check if the objects violate safety or collisions.

Update the state.
Checking for surrounding vehicles
For an autonomous vehicle to create a trajectory path, it must first scan the environment for any obstacles. In our simulator, we have access to the output of the sensor fusion data which provides tracking, distances and speeds of surrounding vehicles. The second benefit of the sensor fusion data is that it is in Frenet coordinates. If the Frenet coordinate system is unknown, the youtube video TNB or Frenet Frame provides a good introduction. In the simulator, the Frenet S coordinate is the distance from the map start point as measured along the roads centre yellow line and the d coordinates are measured from the centre yellow dividing line.
The function used to check for surrounding vehicles iterates through the tracked objects and if their position is in either the current lane or the lanes on the left or the right, being that they are one of the three travelable lanes, as well as only concerning other vehicles if their S distance is within a set distance from the vehicle position, as set with the variables look_ahead_dist and look_behind_dist.
double look_ahead_dist = 80.0; // distance in front of the vehicle to track object (m)
double action_ahead_dist = 50.0; // distance in front of the vehicle to take action (m)
double look_behind_dist = 30.0; // distance behind the vehicle that object will be tracked (m)
double action_behind_dist = 15.0; // distance behind the vehicle to take action (m)
double follow_dist = 25.0; // distance to follow a leading vehicle (m)
// only look at objects within a set distance from the cars current position in front
// that is in the lane to the left, right and same lane as travel.
if((d > (2+4*lane_left1.8)) && (d < (2+4*lane_right+1.8)) && (distance_s >  look_behind_dist) && (distance_s < look_ahead_dist)){
Once we have found another object travelling in the same direction and is in any of the lanes of interest, we will associate a cost for that object. The cost is the sum of four key areas. The speed difference between our vehicle and the object, the objects speed relative to the speed limit, the distance between the vehicle and the object and the objects future position. The cost function is below.
/*
* calculates the cost of a detected object relative to the car
* Args
* object_speed, velocity of the object
* object_car_s, object fernet s position
* Return
* calculated cost
*/
double NextAction::getCost(const double object_speed, const double object_car_s){
const double distance_s = object_car_s  car_s + sensor_offset;
// velocity cost between this vehicle and the object speed
double cost = std::abs(car_speed  object_speed) * relative_vel_cost;
// velocity cost between the speed limit and the objects speed
cost += std::abs(max_vel  object_speed) * max_vel_cost;
// cost for the distance between this vehicle and the object
cost += (1  std::abs(distance_s) / look_ahead_dist) * s_cost;
// cost for the distance between this vehicle and the object 1 second in the future
cost += (std::abs((object_car_s + object_speed)  (car_s + car_speed) + sensor_offset) / look_ahead_dist) * s_cost;
return cost;
}
The costs are used to find the fastest travelling lane with the smallest amount of traffic on it, but will not find a mergeable gap in a lane. Before checking each state we reset the latest vehicle data, by calling the function "NextAction::setVehicleVariables()", a merge gap is assumed and the corresponding variables is_gap_left and is_gap_right are set to true. However, it will be set to false if the position of the object is within a merge window, as set with the variables follow_dist and action_behind_dist.
Now that we have calculated a cost relative to that object, we now need to assign the cost to one of five sections. Left lane behind the vehicle, left lane in front, current travelling lane in front, right lane in front and the right lane behind. As the simulator has collision avoidance for the other vehicles, we will not take into account vehicles travelling behind in the same lane.
As the code for each grouping enacts the same steps, we will only show the steps used for classifying the left lane.
// check if the object is in the left lane
if((d < (2+4*lane_left+2)) && (d > (2+4*lane_left2))) {
if(distance_s > 0) { // is the object in front
left_front_cost += cost;
if(left_front.distance_s > distance_s){
left_front.distance_s = distance_s;
left_front.speed = object_speed;
}
} else {
left_back_cost += cost;
if(left_back.distance_s < distance_s){
left_back.distance_s = distance_s;
left_back.speed = object_speed;
}
}
// check if there is a merge gap
if((distance_s > action_behind_dist) && (distance_s < follow_dist)) {
is_gap_left = false;
}
}
We sum the cost of each section so that a lane with multiple vehicles in it has a higher cost than a lane with only one vehicle present. Each object we find, we will store the closest objects Frenet S, and speed is then stored in a class struct for use in the finite state machine. This will be used when merging into a lane that has another vehicle present so that we accelerate to matching speeds to prevent a collision.
The completed code can be found in the "src/pathstates.h" file, under the "NextAction::checkSurrounding()" function.
System states with an FSM
With the simulator environment simplifying the real world to a threelane highway, we can reduce the system states to five. These states are;

Lane clear  For current lane driving at the speed limit.

Prep change lane  For finding the best adjacent lane to be in and a safe merge gap to change lane into.

Follow  For following a vehicle at a safe distance when a merge is not possible.

Change left  For merging into the left lane.

Change right  For merging into the right lane.
To control each state and the transition between states, we will use a finite state machine or FSM. Below is an FSM diagram detailing each state and their transition between other states.
We have multiple options for state transitions. The default state is to stay in the current lane when no objects are ahead of the vehicle. Once an object has been detected, the vehicle will attempt to change into the lane that has the lowest cost associated with being in that lane. We will place a preference on overtaking on the left, but if the right lane is the better option, we will merge on to it.
To control the transition of the state's, we will use a switch statement. The function "NextAction::updateState()" contains the switch statement and is found in the file "src/pathstates.h". Each state will be discussed below.
case(LANE_CLEAR):
ref_vel = max_vel;
// check the vehicles surrounds for objects
NextAction::checkSurrounding(sensor_fusion);
// Check if there is a vehicle in front and in the same lane
if(center_front.distance_s < action_ahead_dist){
state = PREP_CHANGE_LANE;
}
break;
To take action for our planner, we must get an update from the robot's surroundings, as outlined earlier. Our FSM will then update the next state, dependant on these surroundings. As we are updating the state dependant on the current state and this is delayed by one loop cycle, we are building a Moore FSM.
case(PREP_CHANGE_LANE):
NextAction::checkSurrounding(sensor_fusion);
if(center_front.distance_s < follow_dist){
state = FOLLOW;
} else {
state = LANE_CLEAR;
}
// get the averaged cost of being in each lane
left_cost = left_ma.nextAverage(left_front_cost);
right_cost = right_ma.nextAverage(right_front_cost);
current_lane_cost = current_ma.nextAverage(current_lane_cost);
if(left_ma.getSize() >= filter_size){ // only evaluate costs if the filter is full
// Find the lane with the lowest cost and favour passing on the left
if(is_gap_left && (left_cost <= right_cost) && (left_cost < current_lane_cost)){
// change lanes to the left
state = CHANGE_LEFT;
lane = std::max(lane  1, 0);
} else if(is_gap_right && (right_cost < left_cost) && (right_cost < current_lane_cost)){
// change lanes to the right
state = CHANGE_RIGHT;
lane = std::min(lane + 1, 2);
}
}
break;
Changing lanes is dependent on three elements.

An object is present in the current lane.

The adjacent lanes have a lower cost than the current lane.

There is a safe merge gap present.
When the above three elements are satisfied, the state will transition to either "CHANGE_LEFT" or "CHANGE_RIGHT". Upon setting these states, the new driving lane will be updated, and when the trajectory generator functions, described later, are called, the trajectory will transition smoothly to the new lane.
When it is safe to merge, we can do so with the following left and right states.
case(CHANGE_LEFT):
NextAction::checkSurrounding(sensor_fusion);
// update the vehicles speed to match the speed of the object in the left lane
if((center_front.distance_s < follow_dist)  (left_front.distance_s < follow_dist)) {
ref_vel = NextAction::getFollowSpeed();
} else {
ref_vel = max_vel;
}
// only update the state if the vehicle is in the centre of the lane
if((car_d < (2+4*lane+0.5)) && (car_d > (2+4*lane0.5))){
state = LANE_CLEAR;
}
// reset the moving average filter to start the new lane cost fresh
left_ma.emptyQueue();
right_ma.emptyQueue();
current_ma.emptyQueue();
break;
_
case(CHANGE_RIGHT):
NextAction::checkSurrounding(sensor_fusion);
// update the vehicles speed to match the speed of the object in the right lane
if((center_front.distance_s < follow_dist)  (right_front.distance_s < follow_dist)) {
ref_vel = NextAction::getFollowSpeed();
} else {
ref_vel = max_vel;
}
// only update the state if the vehicle is in the centre of the lane
if((car_d < (2+4*lane+0.5)) && (car_d > (2+4*lane0.5))){
state = LANE_CLEAR;
}
left_ma.emptyQueue();
right_ma.emptyQueue();
current_ma.emptyQueue();
break;
The "CHANGE_LEFT" or "CHANGE_RIGHT" states will only transition to the "LANE_CLEAR" state when the vehicle is within a quarter of a metre tolerance of the new lanes centre position. Otherwise, we will continue to merge at the desired speed limit or a safe following speed if another vehicle is present in the new lane.
Should no lane exist to merge safely into and we are travelling close to the car in front, we need to adjust our speed to follow that vehicle at a safe distance until either it is safe to merge into a lane or the lead car changes into another lane. We perform this check in the "FOLLOW" state which is outlined below.
case(FOLLOW):
NextAction::checkSurrounding(sensor_fusion);
// Check if the object in front is too close and slow down
if(center_front.distance_s < follow_dist) {
state = PREP_CHANGE_LANE;
ref_vel = NextAction::getFollowSpeed();
} else {
state = LANE_CLEAR;
}
break;
Avoiding collisions is performed differently between states, but all state will first check if there is an object in the same lane that requires the vehicle to slow down and follow. The following distance is set with the variable "follow_dist", set in "pathstates.h". When an object is within this range, the desired vehicle velocity is dropped to match the lead vehicles velocity by the following function.
/*
* Calculates the speed required to follow a lead vehicle at the desired distance
*
* Args
* None
*
* Return
* the desired car speed to follow the lead vehicle
*/
double NextAction::getFollowSpeed()
{
double ref_vel;
//stop the car if the object in front is stopped or we are very close to it
if(((center_front.speed <= 0.3) && (center_front.distance_s < 5)) 
(center_front.distance_s < 5)){
ref_vel = 0;
}
// slow and follow the vehicle in front based of it's speed and distance
if(center_front.distance_s < 15){
// slow down to increase the distance between the car in front
ref_vel = center_front.speed/2;
} else if(center_front.distance_s < 10){
// slow a lot as we are too close to the car in front
ref_vel = center_front.speed/4;
} else {
// slow down to the speed of the vehicle in front and follow it
ref_vel = center_front.speed;
}
return ref_vel;
}
The function is only called when in the "FOLLOW", "CHANGE_LEFT" or "CHANGE_RIGHT" states. This feature adjusts the vehicles desired velocity, dependent on the distance to the leading object in the lane that the vehicle is transitioning to and stops the vehicle when the object is stationary or less than 5m away.
The state prep change lane will perform an additional task to prevent collisions. It will compare all lane costs and will attempt to move or stay in the lane that has the lowest cost value. When either the left or right lanes have a lower cost, it will wait until the "is_gap_left" or "is_gap_right" indicates a safe merge gap is present. The code for merge request is handled at the final step of the "PREP_CHANGE_LANE" state.
During each states collision and safety checks, we either transition to a new or remain in the same state. The transition between states is outlined in the FSM diagram shown at the beginning of this section and are only updated after safety and collision checks have passed in each state.
Path Generation
Creating a smooth path for any mobile robot can be tricky. If we do this with an omnidirectional robot, we can use the simple Cartesian coordinate system, however, as our car is not omnidirectional and requires calculating the curve functions for the road coordinates, with the addition of a vehicles' turning curvature. A more straightforward solution around this is to use the Frenet (s, d) coordinate system and lucky for us that the simulator already generates these coordinates which follow the centre yellow divider line. With these coordinate system, all we require to do is state the s value which is the distance along the centre line from the start point as well as the constant d value for which lane we wish to drive along. In our case, the centre of the right lane has a d value of two, the centre is six, and the left is 10.
Now that we have a simplified coordinate system we can create a trajectory just by calculating the function from the current vehicle's position to its future desired position. One way to calculate this is by using a combination of the vehicles dynamics models to create a polynomial that we can calculate and then plug in s and time values to calculate the desired path d values. A benefit of using this method is that we can model the acceleration and jerk on the vehicle to prevent uncomfortable and violent trajectories being generated. While this technique in a complex environment such as the real world is great, it is overkill for what we require in the simulator, so instead, we will use a spline function as our trajectory generator.
While a spline function simplifies the computations as well as ensuring that the function passes through each point, we must now manually check for excess acceleration and jerk in the generated spline trajectory. To minimise the gradient of the spline, we will use a group of s points spaced 25m apart starting at the cars current s position along with the desired lanes d value at the next s point. We accomplish this with the following.
To save us the legwork of creating a spline function we will use an external header file spline tool available from here.
/*
* Transform and convert the spline path into the vehicles reference frame
*
* Args
* x, current vehicles coordinate x point
* y, current vehicles coordinate y point
* map_waypoints_s, global maps s points
* map_waypoints_x, global maps x points
* map_waypoints_y, global maps y points
*
* Return
* None
*/
void Trajectory::makeSplinePts(const vector<double> map_waypoints_s, const vector<double> map_waypoints_x, const vector<double> map_waypoints_y)
{
Trajectory::startPoints(); // get the cars last points for a smooth transition
// Frenet coordinates are referenced from the center yellow lines and
// positive d being on the right.
//calculate the cars centre d position based on the desired lane
double next_d = 2 + 4 * lane;
// get select waypoints in the future to predict a spline function for the desired path
for(int i=1; i <= 3; i++){
vector<double> xy_pts = Trajectory::getXY(car_s+(i*waypoint_step), next_d, map_waypoints_s, map_waypoints_x, map_waypoints_y);
// convert these future points into carcentric coordinates
double shift_x = xy_pts[0]  global_x;
double shift_y = xy_pts[1]  global_y;
ptsx.push_back(shift_x*cos(0global_yaw)  shift_y*sin(0global_yaw));
ptsy.push_back(shift_x*sin(0global_yaw) + shift_y*cos(0global_yaw));
}
}
You may have noticed that after we have the s & d Frenet coordinate points we then convert them to x & y cartesian coordinates which is different to the description above. This is due to our simulator which requires the vehicle waypoints points to be in the Cartesian coordinates. To help simplify things further, we will convert spline points to a vehiclecentric coordinate system.
Once the points have been set, we can calculate the spline function with the following.
/*
* Create the spline that intersects all points on the desired path
*
* Return
* None
*/
void Trajectory::getSpline()
{
// calculate the spline
f_spline.set_points(ptsx, ptsy);
}
All that is needed now is to find the waypoints along the spline which makes our path and then convert them to global coordinates. As the simulator steps the vehicle every 20ms, we will calculate the path points with a 20ms step. The number of lookahead waypoint to generate is set with the variable "way_pts_tot", in the file "trajectory.h".
/*
* Creates the final trajectory path points
*
* Args
* next_x_vals, vector to store the final paths x trajectory points
* next_y_vals, vector to store the final paths y trajectory points
* ref_vel, desired speed to be traveling in this directory
*
* Return
* None
*/
void Trajectory::getTrajectoryPts(vector<double> &next_x_vals, vector<double> &next_y_vals, const double ref_vel)
{
double x_local = 0; // the current x point being considered
double y_local, prev_x_local, prev_y_local;
const int way_pts_tot = 50; // how many way points are to be predicted into the future
// Store the unused old waypoints to create a smooth path transition
for(int i=0; i < prev_size; i++){
next_x_vals.push_back(previous_path_x[i]);
next_y_vals.push_back(previous_path_y[i]);
}
double step;
int next_size = next_x_vals.size();
// get the previous step between the last two previous trajectory points and set
// that as the step size to prevent excess acceleration and jerk
if(next_size < 2){
step = 0;
} else {
double step_x = next_x_vals[next_size  1]  next_x_vals[next_size  2];
double step_y = next_y_vals[next_size  1]  next_y_vals[next_size  2];
step = sqrt(step_x*step_x + step_y*step_y);
prev_x_local = step_x;
prev_y_local = step_y;
}
// add new points onto the old way points upto
for(int i = 1; i < way_pts_tot  prev_size; i++){
// get the new (x,y) points and check excess speeding, acceleration, jerk
Trajectory::getStep(step, ref_vel, x_local, y_local, prev_x_local, prev_y_local);
// convert the reference point from local car reference frame to global coordinates
double x_point = x_local * cos(global_yaw)  y_local * sin(global_yaw);
double y_point = x_local * sin(global_yaw) + y_local * cos(global_yaw);
next_x_vals.push_back(x_point + global_x); // store the new path points
next_y_vals.push_back(y_point + global_y);
}
}
At each step, we need to perform safety checks between points to prevent speeding and excess acceleration and jerk.
/*
* Calculates and adjusts each new (x, y) path point and prevents each step from speeding
* Args
* step, the previous points step value
* ref_vel, desired speed to be travelling in this directory
* x_local, new x point to check if it is speeding. This will be adjusted
* to prevent speeding.
* y_local, new y point to check if it is speeding This will be adjusted
* to prevent speeding.
* prev_x_local, previous x point to calculate the velocity step between
* prev_y_local, previous y point to calculate the velocity step between
*
* Return
* None
*/
void Trajectory::getStep(double &step, const double ref_vel,
double &x_local, double &y_local, double &prev_x_local, double &prev_y_local)
{
// max allowed acceleration per step 6m/s^2
double acceleration = 0.0022;
// 1Mph * 1609.344meter/h / 3600 = 0.44704 m/s
const double mile_ph_to_meter_ps = 1609.344 / 3600.0;
const double max_step = max_vel * mile_ph_to_meter_ps * 0.02;
double ref_step = std::min<double>(ref_vel * mile_ph_to_meter_ps * 0.02, max_step);
// check if we will potentially have a collision and decelerate
if((car_speed > ref_vel)){
step = std::max(step  acceleration, ref_step); // deceleration 6m/s
} else if(car_speed < ref_vel){
step = std::min(step + acceleration, ref_step);
}
x_local += step;
y_local = Trajectory::solveSpline(x_local);
double diff_x = x_local  prev_x_local;
double diff_y = y_local  prev_y_local;
double diff_step = sqrt(diff_x*diff_x + diff_y*diff_y);
int loop = 0;
// Check that the step will not exceed the max velocity step
while(diff_step > max_step && loop < 6){
double error = std::max(max_step/diff_step, 0.97);
x_local *= error; // decrease the x_step by the error amount
y_local = f_spline(x_local); // calculate spline y value at x
diff_x = x_local  prev_x_local;
diff_y = y_local  prev_y_local;
diff_step = sqrt(diff_x*diff_x + diff_y*diff_y);
loop++;
}
prev_x_local = x_local; // update the previous step points
prev_y_local = y_local;
}
Path Pipeline
The above sections describe how to perform particular tasks for collision avoidance and creating a path. The final part is to integrate them together. This process is;

Get the latest sensor fusion data.

Update the class "NextAction" with the latest system variables.

Update the finite state machine.

Initialise a new "Trajectory" class and update its points with the remaining previous path points.

Create a new spline with the updated desired variables from the "NextAction" class.

Update the previous trajectory path with the new waypoints.

Pass the updated trajectory points to the simulator.
This pipeline is controlled from the main loop in the "main.cpp" file and is outlined in the system flow diagram below.
That is it. The complete project code can be viewed and downloaded from GitHub here. Be sure to check out the "writeup.md" for details of the code limitations, bugs and future work with the project.
A video of the path planner in action can be viewed below.
The