Teaching Cars To Drive — Highway Path Planning

Car Driving On Highway

This is the first project of Term 3 of the Udacity Self-Driving Car Engineer Nanodegree. You can find all code related to this project on github. You can also read my posts on previous projects:

Planning a path that is both safe and efficient is one of the hardest problems in the development of an autonomous vehicle. In fact, this step, known as Path Planning, is still an active area of research. The reason why Path Planning is such a complex task is because it involves all components of a self-driving vehicle, ranging from the low level actuators, the sensors which are fused to create a “snapshot” of the world, along with the localization and prediction modules to understand precisely where we are and what actions different entities in our world (other vehicles, humans, animals, etc) are more likely to take in the next few seconds. One other obvious component is a trajectory generator that can compute candidate trajectories to be evaluated by the planner.

In this post, we will focus on describing how we implemented a C++ highway path planner that is able to generate safe and efficient trajectories in a simulator, using jerk minimisation techniques. The constraints of the project are as follows:

  • No collision at any time with other vehicles
  • Maximum speed of 50 MPH (~ 80 KMH)
  • Maximum acceleration of 10 m/s²
  • Maximum jerk of 10 m/s³
  • Vehicle cannot be in between lanes for more than 3 seconds
  • Vehicle cannot go outside the 3 lanes of the highway
  • Vehicle cannot drive on the wrong side of the highway

We had much trouble completing this project and the gif below shows some of our early constraints violations

Rough Start

To begin, let’s take a closer look at the different layers involved in Path Planning.

Functional Layers In A Self-Driving Car

Layers involved in path planning — from Udacity

As mentioned earlier, path planning requires the cooperation of different layers of an autonomous vehicle. The diagram above provides an overview of how such components may be layered in a given self-driving system:

  • Motion Control: responsible for moving the car and following a reference trajectory as closely as possible. This layer operates with the fastest time scale
  • Sensor Fusion: responsible for merging sensor outputs (e.g. RADAR + LIDAR).
  • Localization: responsible for understanding with as much precision as possible where our vehicle is on the map, and where other entities (e.g. other cars) are with respect to our vehicle
  • Prediction: responsible for identifying the nature of entities detected with sensors (aka perception) as well predicting near-future changes in the scene based on car current trajectory, other vehicles’ trajectories, and miscellaneous elements on the scene (e.g. traffic lights). One important task of this layer is to anticipate collisions as well.
  • Behaviour: coordinating layer that takes in all information from lower layers and decides future state to transition to along with the trajectory to adopt
  • Trajectory: responsible for computing trajectories given a set of constraints (e.g. speed, distance, lane, jerk, etc.)

There are many approaches to trajectory generation, and in this project we have opted for computing trajectories in a Frenet coordinate system.

Making Sense Of Sensor Fusion Data

Our vehicle in the simulator is equipped with a range of sensors whose outputs are fused to produce more accurate measurements. Most companies working on Level 4 autonomy employ Radar, Lidar and Camera in their sensor suite. Having many different types sensors is crucial because each have their strengths and weaknesses. Moreover, having many instances of the same sensor is important as well to mitigate hardware failures in a given sensor.

In our case, the simulator provides the following information from its sensor-fusion module:

  • position, speed, and orientation of our vehicle
  • position and velocity of other vehicles (we can compute orientation using a little trigonometry — see arctangent) in the sensors’ range
  • remainder of the previously submitted trajectory to execute

With this information we are able to compute our vehicle’s distance to all other vehicles. We take this many steps further and attempt to predict collisions based on our vehicle’s trajectory and other vehicles’ extrapolated trajectories too. We use this information to our advantage in the cost functions that we define later.

Trajectory Generation

Frenet Coordinate System

We often use a traditional Cartesian coordinate system to represent a given point (x, y) on a plane, and that’s in fact the default system used in the simulator to identify our car on the road. However, in the real-world, roads are not always straight and therefore “easy” operations that humans perform such as identifying which lane the car is on, are much more difficult to replicate using a Cartesian system for a computer. The image below illustrates the problem we are faced with in a traditional (X, Y) coordinate system:

Curvy road in a Cartesian system (taken from Udacity lesson)

Would it not be easier if our coordinate system espoused the curvature of the road such that with this new coordinate system a trajectory where our car goes forward and stays within its lane appears as a straight line, like the below?

Trajectories in Frenet (left) and normal Cartesian (right) coordinate systems

This is precisely what a Frenet coordinate system offers: in such a system we split our plane into a longitudinal and lateral axis, respectively denoted as S and D. The mathematics behind how such a system is obtained is quite involved so we won’t lay it all bare in this article. But you can imagine the curve that goes through the center of the road determines the S axis and indicates how far along we are on the road. The D axis maps to the lateral displacement of the car. The illustration below shows what this system looks like on a curvy road:

Curvy Road Captured In Frenet Coordinates — from Udacity

Jerk Minimization

Jerk is defined as the rate of change in acceleration over time. Meanwhile, acceleration is defined as the rate of change in velocity over time. Basically jerk and acceleration are respectively derivatives of acceleration and velocity. As passengers, sudden changes in acceleration in a vehicle causes high jerk, which ultimately makes ride uncomfortable. It is therefore essential to minimize jerk when planning a trajectory.

It turns out that it is relatively easy to compute a jerk minimal trajectory in one dimension by extending the kinematic equation to compute a trajectory given our current position s_0, current velocity s_0_v, and current acceleration s_0_a. For a given time frame T (e.g. 1 second), we can express:

  • desired final position (at time t) s_f
  • desired final velocity s_f_v
  • desired final acceleration s_f_a

using quintic polynomials (i.e. degree 5) that go up to the second derivative of jerk. The equations, with respect to the starting values look as follows:

Jerk Minimizing Trajectory equations — from Udacity

The values on the left hand side of the equals sign are the projected values for position, velocity, and acceleration at time t ≤ T. In our case t is set to the controller’s update rate, which is 20 milliseconds (0.02 seconds). We can plug all this into a polynomial solver by setting the appropriate boundary conditions. In our case, we set our desired acceleration to 0 since we would like the reduce jerk. Unfortunately this only sets the final acceleration for t=T and we have no control over the vehicle’s acceleration when t T. We thus need to experiment with different values of T to identify which time horizon to pick to generate smooth jerk minimized trajectories. In our case we chose T = 1.7 seconds. The other issue is that we have a perfect controller which will move the vehicle to any next point in the trajectory, regardless of the laws of physics (e.g. can move 1 KM further in 20 milliseconds), therefore we need to be extremely vigilant with the the trajectories we submit.

Since we are using Frenet coordinates, we can generate 1-D jerk minimal trajectories in s and d dimensions separately. This paper from Werling and Kammel constitutes a good read to get more familiar with this topic.

Since the simulator does not accept trajectories expressed in Frenet coordinates, we convert from Frenet coordinates back to real world coordinates to compute the (x,y) point that maps to a given (s,d) Frenet point.

Improving Localization

Creating Smoother Trajectories

We assume the track has already been pre-mapped and are provided with waypoints which follow the middle yellow line which separates the two sides of the highway. This helps us in determining where we are with respect to the closest waypoints.

Unfortunately, the map waypoints we are given are quite sparse and can lead to very “angular” trajectories generated when we try to convert from Frenet back to real world coordinates. This in turn causes sudden spikes in acceleration and jerk. As the function toRealWorld(s, d) -> (x, y) uses basic interpolation between two waypoints to find the best approximate values for x and y, we always run the risk of generating a non-smooth trajectory.

Lane Change With Coarse Waypoints

What can we do to improve upon this? From some of the projects in previous terms, we have seen that lines derived from a polynomial tend to produce very smooth trajectories. Therefore we should employ this technique instead of the basic interpolation that is currently being used. We resort to using splines created by taking the position s in Frenet coordinates to obtain the real-world coordinates x, y, and offsets dx and dy. We then plug in this formula to obtain the closest real-world coordinates

x = spline_s_x(s) + d * spline_s_dx(s)
y = spline_s_y(s) + d * spline_s_dy(s)

Now we can see how remarkably smooth our trajectory has become.

Smooth Driving From Better Waypoints

2-D State Machine

One of the most intuitive ways of thinking about driving is that we humans transition our vehicle into different states based on our driving style, the outside information that we capture as well as the destination we have in mind. It turns out we can codify states for machines and instruct them which states they could move to based on current state and other layers of our autonomous vehicle stack.

In our case, our finite state machine is quite straightforward and illustrated below:

Final State Machine For Path Planning — from Udacity

The most frequent state will be Keep Lane but whenever we wish to change lanes the car will first transition to a Prepare Lane Change Left/Right state and upon making sure the lane switch is safe, will move to the actual lane change state. The intermediate state we enter prior to switching lanes is akin to vehicles turning on their left/right signals before changing lanes (and of course drivers also ensure the lane change is safe).

Regarding the implementation of our state machine, we took inspiration on the Frenet coordinates to devise this method. we decided to split a given state into its longitudinal and lateral components. The reason for doing so is that we believe it simplifies how we think about driving on a highway with potential lane changes.

Basically the lateral state dictates the potential next states we could find ourselves in, while the cost functions may select a longitudinal state over another. The state machine implementation can be found in the gist below:

Cost Functions

Given that we generally return multiple candidate next states along with their trajectories, we must find a way select the “best” course of action to take. This is where cost functions are useful. Cost functions are necessary to “teach” the vehicle what actions we want to encourage and which ones we penalise to a lesser or harsher degree via different weights.

All our cost functions adhere to the following interface we defined in the file cost_functions.h in the project repository:

typedef function<double (const Vehicle&, const vector<Vehicle>&,  const Trajectory&, const State&, const double&)> CostFunction;

This makes adding a cost function very trivial (the weight is the last parameter). We defined the following cost functions, where the weight is entirely adjustable:

  • speedCostFunction: a function that penalises our vehicle if it goes slowly
  • centerOfLaneDistCostFunction: a function that penalises our vehicle if its trajectory ends outside of the center of the targeted lane
  • laneChangeCostFunction: a function that always penalises a lane change since those are usually more dangerous than driving in the same lane
  • distanceToClosestCarAheadCostFunction: a function that penalises our car the closest it is to the car ahead
  • distanceToClosestCarAheadFutureLaneCostFunction: a function that penalises a vehicle if it is too close to vehicles ahead or behind when operating a lane change
  • averageLaneSpeedDiffCostFunction: a function that penalises the lane the vehicle wants to be in based on the average speed of the vehicles ahead in this lane
  • speedDifferenceWithClosestCarAheadCostFunction: a cost function that penalises our vehicle if it is going slowler than the vehicle ahead
  • collisionTimeCostFunction: a function that penalises our trajectory if it leads to a potential collision with another vehicle
  • futureDistanceToGoalCostFunction: a function that penalises our trajectory the further it is to the goal(i.e. the end of the track at s=6945.554)

We tried many different weight configurations but ultimately decided that since we did not wish to compromise on safety we attribute the collisionTimeCostFunction the highest weight with 10000. Other cost functions’ weights vary widely but we gave for instance very little weight to speedCostFunction since high speed is a nice to have but is nowhere near as critical as no-collision.

Final Results

The current path planner performs quite well and enables the vehicle to drive around the track multiple times. It can however be improved by tweaking weights more and improving some of the cost functions. Moreover, we believe that by incorporating some machine learning into our prediction layer we can eliminate some edge cases that could lead to collisions. One interesting behaviour of our planner is that it is able to quickly change lanes multiple times . We initially thought this was a bug from our final state machine design, but it turns out to be an interesting side effect!

Smooth Double Lane Change (second time because of red car ahead)

You can also watch the video of a full lap uploaded to YouTube:

Video Of Full Lap In Simulator


The current path planner is relatively conservative and is not optimised for top speed. This means that while the velocity can at times go as high as 48 MPH (~77.2 KPH), it generally stays below this speed, and does almost never attain the legal speed limit of 50 MPH (~80.4 KPH). That’s a trade-off we are happy to accept for now but work should be done to enable the car to drive at speeds closer to 50 MPH.

Moreover, the planner only takes into account the vehicle’s adjacent lane and therefore never “sees” when a non-adjacent lane would be a better choice (e.g. car is on lane 1 and planner only evaluates lanes 1 and 2, while lane 3 on the far right may be free and therefore a good candidate lane to move towards). This would require more sophisticated path evaluation method where the planner assesses all lanes and decides ultimately which adjacent lane to move to, based on the fact that the lane next to our adjacent lane will become a viable candidate to move to once the vehicle has reached the adjacent lane. The other concern we have with this approach is related to safety, since it is more dangerous and tricky to execute such a bold move, because of the lateral distance that must be covered as well as the difficulty of predicting the behaviour of other vehicles on the road.

Another improvement we should look into is to employ statistical techniques to better predict other vehicles’ actions, particularly anticipating when they merge into our lane as this increases the risk of a fatal collision. We could have started off by using Naive Bayes but did not have enough time to dedicate to testing and then picking the most discriminative features for our predictor.

Finally, our current planner only generates a single trajectory for a given possible next state, which means that we could be ignoring better trajectories for the same future state (e.g. further away/behind, or more on the left or right of a given lane). We have an implementation for such multiple trajectories per state scheme, which assumes that all final Frenet positions s and d (obtained by computing a jerk minimal trajectory) follow a Gaussian distribution with given means and standard deviation (mean_s, std_s) and (mean_d, std_d) for G(s) and G(d) distributions respectively. We must however choose the appropriate values for standard deviations, while the means will remain fixed at the originally desired end positions end_s and end_d.


This was so far without a doubt the most difficult project I have undertaken as part of this Self-Driving Car nanodegree, across all three terms. I was not sure I could ever complete it and did not even know where to start exactly!

The small tutorial video from David Silver and Aaron Brown helped me to get started. Since I chose a jerk minimization approach to trajectory generation, I did not find as many blog posts or references from students who opted for this technique, as most articles were about using the spline technique in the tutorial from David and Aaron. But the paper from Werling and Kammel on Optimal Trajectory Generation For Dynamic Street Scenarios in a Frenet Frame really helped improve my intuition on this technique (I did not fully understand everything though).

Moreover, I particularly enjoyed and felt inspired by Mithi’s articles on this project, as well as posts by other students. Lastly, I am grateful to all the team at Udacity and their partners at Daimler to have put together such great content and challenging project!

Source: Deep Learning on Medium