Lane keeping in autonomous driving with Model Predictive Control & PID

The yellow path is the target trajectory and the green path is how our car moves using MPC.

Three major components of autonomous driving are localization (where am I), perception (what is around me), and control (how to drive around). Using lane recognition, we can plan a trajectory for an autonomous car to follow. In this article, we discuss the control of the vehicle’s acceleration, brake, and steering with Model Predictive Control (MPC) using a kinematic bicycle model. The purpose is not only following a target trajectory as close as possible but also as smooth as possible to avoid motion sickness or frequent braking.

MPC is a process to minimize costs while satisfying a set of constraints. For example, we want to adjust the steering and the speed every 100 ms such that the cost function, defined as the difference between the target trajectory point (the yellow dot) and ours (the green dot), is minimized under the constraint that the wheel cannot be steered more than 25°. In MPC, we read from the sensors to determine the current states of the car like speed. Then we consider possible actions within a short period of time (say 1 sec.) based on these readings. Let’s say, we consider steering the wheel by 20° clockwise and then reduce it by 1° every 100 ms. Assuming these actions result in the lowest cost at the end of the one second period, we will then apply the first action of stirring the wheel 20°. But instead of performing the remaining actions later, we wait for 100 ms and read the sensors again. With the new readings, we recompute the next optimal action again. MPC makes the next action by taking advantage of viewing the results of a longer future plan (1 sec.). So it is less vulnerable to short-sighted gain in a greedy method and therefore, plan better.

This is part of a 5-series self-driving. Other articles includes

Kinematic bicycle model

First, we define a kinematic model to describe our car.

Image for post
Image for post

(x, y) is the vehicle’s center of mass. ψ is the current heading of the car and v is the speed of the vehicle. lf is the distance from the center of the mass to the front axle. β is the angle of v with respect to the car axis. In our example, we assume this is zero. (i.e. the car is not sliding)

In our model, we can control the front wheel steering angle δf and the car acceleration a. For simplicity, we assume it is a front-wheel-drive car and we will write δf as δ for now.

MPC in details

At each period (100 ms), we read from the sensors to determine the current state of the vehicle including:

  • location of the vehicle,(x, y)

Trajectory model

Our lane detection system should also give us a trajectory, say, in the form of next 6 waypoints (6 coordinates). In our example, we use the 6 waypoints to fit a 3-rd order polynomial function. We use this model to compute the y coordinate and the heading ψ from x.

Image for post
Image for post

Dynamic model

Next, we will create a dynamic model for predicting the state at time t+1 of our car from the last state at time t. Using the kinematic bicycle model, we can easily deduce the location, the heading and the speed from the last state.

Image for post
Image for post

We add 2 more states to measure the cross-track error (cte — the distance between the green and yellow dot) and the heading error for ψ.

Image for post
Image for post

Cost function

In MPC, we define a cost function to optimize our path with the trajectory. For speed, we penalize the model if the car cannot maintain at a target speed. We want no acceleration and zero steering if possible. But since it is unavoidable, we want the rate change to be as low as possible if it happens. This reduces motion sickness and saves gas. In our model, our cost includes:

  • Cross-track error,

Since those objectives may be contradictory to others, we add weights to the cost to reflect its priority. Here is the cost function:

Image for post
Image for post

Putting it together

Recall that to find our optimal path in MPC, we need a dynamic model to predict the next state, a cost function and the system constraints.

Image for post
Image for post

Optimize MPC

We solve the control problem as an optimization problem using a cost function under constraints include those for actuators controlling the gas/brake pedal and the steering wheel.

  1. We read the next 6 waypoints from the lane detection and compute a 3rd order polynomial to model our trajectory.


In our example, we compute the optimal solution for a period of 1 sec. This parameter is tunable. Longer period gives us a better picture of our actions but the accumulated errors may be too much. In fact, if this number is too large, the car will go off-track instead.

For those that survive the maths, here is a video on what can go wrong with autonomous driving.


A proportional–integral–derivative controller (PID) is another controller for self-driving cars. The video below explains how it works.

In our example, our control function composes of

  • Cross-track error (cte) for the proportional term,
Image for post
Image for post
Source: Wikipedia

We run the same simulation with the PID controller below. Since the cost function is far less sophisticated, the motion is more jerky.


The video is generated by MPC using the simulator provided by the Udacity self-driving class.

Written by

Deep Learning

Get the Medium app

A button that says 'Download on the App Store', and if clicked it will lead you to the iOS App store
A button that says 'Get it on, Google Play', and if clicked it will lead you to the Google Play store