How self-driving cars get from A to B?

Recently I’ve stumbled upon a really interesting paper which makes great overview of motion planning and control techniques for self-driving cars in urban areas. In this post I’ll go over few parts of that paper which relate to my last Term 2 project for Udacity Self-driving car Nano degree.

The final output from last project in Term 2

One of the common of approaches for building a self-driving car system is to organize sensor perception (radar, LIDAR, camera, etc.) and decision-making process into a hierarchical structure.

Hierarchical structure

Decision-making module of self-driving car can be represented through four components:

1. Route planning — finding a shortest path between point A and point B. Think of what Google and Here maps are doing when you are searching for a nearby restaurant.
2. Behavioral decision making — once the route has been found, this submodule is responsible for selecting driving behavior (lane following, lane change, turn right, etc.) based on other traffic participants and established traffic regulations. These behaviors are finite set of actions and are usually represented as finite state machines with some heuristics for reflecting current context of the vehicle surroundings (other vehicles, road works, etc.). One of the latest developments in this area is AI approach from MIT for modeling this step in decision making process.
3. Motion planning — once behavior is selected, it needs to be translated into a trajectory which can be followed by a low-level feedback controller. Motion planning module is responsible for finding that trajectory/path.
4. Vehicle control — feedback controller does all the hard work of moving the vehicle along side the selected trajectory by using the proper actuators (steering, throttle, breaks) and correcting the track errors.

Decision making process in self-driving cars. Source.

A bit more on Motion planning and Vehicle control

In the final project for Term 2 of the course, we are asked to steer a car in a simulator using Model Predictive Control (MPC in short) which basically covers last two parts of the self-driving car decision process (Motion planing and Vehicle control) from above.

MPC is able to predict the future states (position, orientation and velocity) of the vehicle for a finite time horizon. It is an iterative model which is constantly updating itself and predicting vehicle state for N time slots (dt). Prediction itself is based on mathematical vehicle model.

Creating a correct vehicle model is quite a complex task and it comes with trade offs (whether we should go with simple model with lower accuracy or more complex model with high computational requirements). In our project, we will be using kinematic model somewhat easier model to understand compared to dynamic models (which takes other forces into consideration which impact vehicle dynamics).

Here’s out vehicle model:

Kinematic vehicle model

Vehicle coordinates are represented with x and y. ψ and v correspond to orientation and velocity respectively. Vehicle actuators are modeled with steering angle (δ) and acceleration (a).

Udacity simulator provides us with the way (reference points) of where the vehicle should go. This would come from Route planning step from above hierarchical flow in the real world. A higher order polynomial is being fitted through these points and output is being used to be compared to the MPC predictive model. This would be Motion planning step.

The way MPC works is that it predicts where the vehicle will be in future and tries to minimize the error rate by correctly adjusting the actuators (steering, throttle and breaks). This would represent Vehicle control step.

You’ll notice that Behavioral decision making step is missing in the project but it is due to the simplification of the scenario in Udacity simulator (no other traffic on the road, no road block, etc.).

Delayed actuations

One of the things we should pay attention to is the responsiveness of the actuators in our feedback controller. In real world, the request for changing the steering angle and/or throttle will be executed with a certain delay by the actuators on the car. In the project, an artificial 100ms delay was introduced in order to simulate this behavior. I have handled this by setting kinematic equations to use the actuations from the previous time step. In this particular case, time step (dt) has the same value as the simulated delay.

Final outcome

The source code for the project can be found on my Github profile:

bdjukic/CarND-MPC-Project