How self driving car navigates on highway?
In one of my previous posts I tried to give an overview of a general process which is used to move self driving car from point A to point B. In this post, we will cover special scenario of that same work-flow applied on highway driving.
Quick recap
Let’s just quickly go through the work-flow once again. In the picture down below, we can see that Localization and Sensor Fusion components give us a location of the car and sensory input from another cars on the highway, respectively. These two inputs will end up in Prediction and Behavior modules which are in charge of deciding what is the next best maneuver (take left lane, take right lane, keep lane, etc.). Once the car figures out what it should do next, Trajectory module actually spits out a concrete plan how to do it by creating a path for a car to follow. Trajectory path is then executed by Motion Control module (the steering and throttle actuators on the car).

Problem definition
As before, this post is tightly coupled with Udacity Self-driving nano degree project. This time, the goal of the project is to navigate a car on a highway (in simulator) respecting the following:
- The car is able to drive at least 4.32 miles without incident.
- The car drives according to the speed limit (50 mph).
- Max Acceleration and Jerk are not exceeded.
- Car does not have collisions.
- The car stays in its lane, except for the time between changing lanes.
- The car is able to change lanes.
Architecture overview
The class architecture will tightly follow the initial diagram.
- Vehicle — represents our car on highway and keeps track of current localization and lane.
- SensorFusion — keeps track of all the vehicles on the highway.
- PredictionPlanner — is able to figure out how far are other vehicles in the lanes and if our car should slow down or speed up.
- BehaviorPlanner — takes input from PredictionPlanner and Vehicle modules to figure out what car should do (change or keep lane). This component knows when car should change from one state (center lane for example) to next one (left lane).
- TrajectoryPlanner — is responsible for finding the right path for changing or keeping the current lane.
All of these modules work together as new sensory data come in 0.2s cycles from the car and its surroundings.
Final State Machine
In order to keep track of the current state (lane) of the car and its transition to new one (if necessary) we needed to introduce a lightweight final state machine with three states (left, right and center lane). The state itself is kept in Vehicle module, but the transition between different states is accomplished by BehaviorPlanner component.
BehaviorPlanner is constantly querying PredictionPlanner whether the car should slow down or speed up depending on the car in front of us. Once our car slows down, BehaviorPlanner will use cost functions for current speed and proximity of other cars in other lanes. If speed cost is high given that we are slowing down constantly, BehaviorPlanner will evaluate cost for changing lanes. If the cost of changing to right lane for example is low (given that cars in that lane are sufficiently far away), it will update its state to new one. This will cause the car to steer to the new lane. TrajectoryPlanner component takes it from here.
Path generation
The path generation is done by the TrajectoryPlanner component. It uses the concept of anchor points which represent the future path which the car should follow. The anchor points are constructed from:
- Previous path points which car has not reached yet
- Points which are 30, 60 and 90m from the current vehicle s value in Frenet coordinate system.
Once anchor points are defined we would need to shift them to car reference points in order to easy the math. Once that is done, we will use cubic spline interpolation in order to get concrete points for our car to visit.
Car simulator acts as a perfect MPC and will visit every single point you send to it.
Final outcome
Wrap up
This project is definitely one of the most difficult ones so far in the course. It is quite open-ended and you can easily spend bunch of time trying to make it perfect.
Source code to my solution can be found here:
