Model Predictive Control for Autonomous Vehicle: An In-depth Guide

What is MPC and why is it beneficial for autonomous driving? How to implement MPC for autonomous vehicles?

Photo by Remy Gieling on Upslash

Table of Contents

I. What is MPC?

II. Why MPC?

1. Advantages of MPC
2. Disadvantages of MPC

III. MPC Concepts

1. Definition of MPC
2. States, X
3. Control Inputs, U
4. Model
5. Reference
6. Constraint
7. Cost Function
8. Prediction horizon, N & Sample Time, dt

IV. How to implement MPC

1. Latency
2. Outro

V. MPC: Beyond the Basics

1. Dynamics Model
2. Optimization Library
3. Run MPC Faster

VI. Resources

Are you looking for an in-depth Model Predictive Control tutorial for the autonomous vehicle (AV)? Then you come to the right place.

What is MPC?

Model Predictive Control (MPC) is the industry standard for autonomous navigation control algorithms. Essentially, MPC helps the vehicle to predict and plan for future events by continuously generating a series of control actions that will get the vehicle closer to its desired trajectory. Engineers commonly use MPC in longitudinal (speed) and lateral control (steering).

This is only part of the whole story of MPC. Before diving deeper into the MPC concepts. Let’s discuss why MPC is the state-of-the-art method for controlling AV.

Why MPC?

Advantages of MPC

Unlike other reactive controllers such as PID, Pure Pursuit or Stanley Controller, MPC is a predictive control method that predicts the future states of the vehicle and plans its control actions accordingly.

MPC algorithm can handle non-linear and complex vehicle dynamics like tire force models and actuator models, allowing precise and accurate trajectory tracking.

Another powerful feature of MPC is it can take multiple constraints. Want to limit jerks for comfortability? Want to prevent actuator saturation? Want to set the maximum speed? MPC can do all this by introducing constraints into the controller.

You can even include fuel efficiency or energy consumption in your MPC optimization, resulting in improved efficiency compared to other controllers.

But how about the uncertainties in my vehicle model? What if my vehicle model cannot explain well the vehicle dynamics?

Not to worry. You can incorporate the uncertainties in the vehicle dynamics and environment into the control strategy, ensuring robust and reliable performance in changing conditions.

Disadvantages of MPC

We came across dozen advantages of MPC. How about its downside? We definitely must recognize its weakness if we want to utilize it.

Yes, there are some drawbacks to using MPC. One of the most significant drawbacks is computational complexity. MPC requires a substantial amount of computation to generate optimal control actions. This can be more challenging for real-time implementation in embedded systems.

MPC relies on accurate models for the system to be controlled. This can be difficult when you have not understood the underlying system, or it is hard to model.

Implementing MPC is a tough job, but it is worth the effort for AV when safety comes to the primary requirement. If you want to find a control algorithm on your mobile robot, MPC is usually overkilled.

At this point, we have gone through both sides of MPC. While designing your MPC, you should have them in mind, especially its high computational consumption. I will explain in detail later.

MPC Concepts

Definition of MPC:

MPC is an optimal control technique that computes control actions by minimizing a cost function based on the prediction of the vehicle states from a vehicle model over horizon.

Figure 1: Control Block Diagram (Image by author)

At each timestamp, the MPC receives the measured outputs to predict the current states. Then it computes the series of control inputs, which minimizes the cost over the prediction horizon by solving a constrained optimization problem. The optimization problem relies on the vehicle model, references and the current state. Eventually, the MPC controller applies the first computed control input to the vehicle, ignoring the following ones. The process repeats for the following timestamps.

How the MPC method may look over the few timestamps:

Figure 2: Photo from Wikipedia

In this graph, MPC predicts the control inputs over the prediction horzion, p+1 in a way that the predicted output will merge with the reference trajectory. Then, it uses the predicted control input at time k only for actuation.

There are some critical elements of the MPC:

1. States: The parameters that describe the vehicle behavior at a particular time, e.g., position, speed, cross-track error etc.
2. Control Inputs: The control parameters computed by MPC (the ultimate answer we want), e.g., acceleration and steering angle.
3. Vehicle Model: The mathematical model that describes the vehicle’s motion. It is either a kinematics or dynamics model.
4. Reference: The desired states for the vehicle being controlled, e.g., reference trajectory and desired speed.
5. Constraints: The maximum and minimum values for the states and control inputs. It can be their rate of change as well.
6. Cost Function: The mathematical expression representing the control system’s objective.
7. Prediction Horizon: The number of future samples over which MPC predicts the outputs and control inputs.
8. Sample Time: The time duration between two consecutive control actions.

All these elements are essential. We will dive deep into them one after another. Let’s start with the most fundamental: states and control inputs.

States, X

The states are the characteristic that describes the vehicle’s behavior. Let’s say I want to use the bicycle kinematics model for simplicity as it ignores the forces, weight and gravity. The vehicle states will be as follows:

1. Position x (x): the x-coordinate of the vehicle’s center of gravity (CoG).
2. Position y (y): the y-coordinate of the vehicle’s CoG.
3. Heading (ψ): the vehicle’s orientation measured as the angle between the vehicle’s longitudinal axis and the global reference frame.
4. Velocity (v): the forward velocity of the vehicle.
5. Cross-track error (cte): the tangential distance between the vehicle’s position and the reference trajectory.
6. Heading error (eψ): the difference between the vehicle’s yaw and the reference heading.

Control Inputs, U

The control inputs are the ultimate values we want to control the vehicle. For the model I used, they are:

1. Acceleration (a): the acceleration of the vehicle.
2. Steering angle (δ): the steering wheel angles.

Note that the states and control inputs might vary depending on the model you used.

Model

As mentioned above, the model we are using here is the non-linear bicycle kinematics model with the six states:

Figure 3: Bicycle Kinematics Model (by Udacity)

Where Lf is the distance between the vehicle’s CoG and its front axle and dt is the sample time. The model is based on the vehicle’s CoG frame.

The model also can be written in a discrete-time state space model:

X(t+1) = AX(t) + BU(t)

X is a states vector containing the six states, U is a vector containing two control inputs, A is a 6x6 coefficient matrix, and B is a 6x2 coefficient matrix.

Reference

The references for controlling AV commonly are waypoints with target speeds. The waypoints are used to compute the cross-track and heading errors utilizing the model, while the target speeds will be directly used in the cost function below.

Constraint

Constraint is the upper and lower bound value that we can set for the states, control inputs and their rate of change. You will always want to limit the speed first:

• velocity, v ∈ [-4 ms⁻¹, 25 ms⁻¹]

Due to actuator constraints, we would need to limit control inputs:

• acceleration, a ∈ [-3 ms⁻², 2 ms⁻²]
• steering wheel angle, δ ∈ [-30°, 30°]

You can also set the rate of change of the control inputs for comfortability:

• jerk, da/dt ∈ [-0.6 ms⁻³, 0.5 ms⁻³]
• steer rate, dδ/dt ∈ [ -6°s⁻¹, 6°s⁻¹]

The constraints may differ based on the vehicle hardware and circumstances.

Cost Function

Figure 4: MPC Cost Function (by Udacity)

The cost function of the MPC can also be known as the mathematical expression of our objective — trajectory tracking and speed control. This is well explained in the 1st line, minimizing cross-track error, heading error and velocity error.

We also want to penalize the actuation since performing throttle, brake, or steer comes with a cost (fuel, energy and comfort). In other words, introducing these costs can prevent overshot.

Last but not least, the costs for the rate of change of control inputs are for comfortability.

The cost functions might varies based on your use case. So, we introduce weights on each cost to reflect its priority.

Prediction horizon, N & Sample Time, dt

The prediction horizon is the number of future samples for which the control algorithm predicts and plans control actions. This is a critical factor determining the computational cost and overall performance of the MPC controller.

The sample time is the time interval between consecutive predictions and control actions. Identical to the prediction horizon, sample time will affect the computational cost and MPC performance.

A longer horizon and a shorter sample time require more computational resources but can lead to better control. Therefore, it is a trade-off between computational cost and control performance.

Do note that the sample time should be at least the duration for a single MPC control cycle (complete cycle in Figure 1) so that the algorithm has enough time to calculate control inputs. If the sample time is too low, the MPC algorithm may have difficulty catching up with the fast-changing system, leading to suboptimal control performance.

How to implement MPC?

The code we are using in this tutorial is from an assignment from Udacity’s Self-driving Car Nanodegree done by Jusheng Fu using C++:

GitHub — JunshengFu/Model-Predictive-Control: This project is to use Model Predictive Control (MPC)…

To implement MPC, let’s begin with what’s our input and output for MPC:

Input: Reference waypoints
Result: Control actions (a & δ)

The waypoints are generally computed by a path-planning algorithm and are coordinates in a global frame. We will first convert the waypoints to the local vehicle CoG frame by map2car() in the main.cpp because the vehicle model we used is referenced to the local frame.

Local Vhielce CoG Frame (Image by author)

Next, we will need to perform 3rd-order polynomial fitting (polyfit()) on the waypoints to ensure a smooth and continuous trajectory provided to the MPC solver.

The extract polynomial coefficients are used to calculate current cte and eψ as follows:

double epsi = 0 - atan(coeffs[1]) - v / Lf * steer_angle * latency;
double cte = polyeval(coeffs, 0) - 0 + v * sin(0- atan(coeffs[1])) * latency;

After that, we have all the measured states (outputs) for the current timestamp. The last thing is to parse the states and polynomial coefficients to the MPC solver:

// call MPC solver
auto vars = mpc.Solve(state, coeffs);

Latency

In real-world applications, there is always a latency from the moment program sends the commands to the mechanical actuation. Therefore, we need to accommodate a latency in the program.

Outro

All the processes we discussed so far are done in the main.cpp file. The MPC concepts details that we discussed earlier, such as defining the vehicle model, constraints and cost function, are performed in the MPC.cpp.

I won’t go through how to write in code as the code implementation details depend on the optimization library you used. (The optimization library Udacity used is Ipopt).

Nevertheless, the MPC concept is the same. You still need to define all the main MPC elements.

MPC: Beyond the Basics

We have gone through how to implement an MPC, but there is more to consider when solving real-world problems.

Dynamics Model

We applied the kinematics model for the MPC earlier, but the kinematics model never accounts for forces, tire slip and inertial. As a result, MPC may not perform well when the vehicle speed is getting faster, and the tire slips significantly affect vehicle motion.

But dynamics models account for forces while calculating a vehicle’s output, resulting in more accurate output prediction.

If you are keen to use a bicycle dynamics model. Here is a code example:

GitHub - alexliniger/MPCC: Model Predictive Contouring Controller (MPCC) for Autonomous Racing

Optimization Library

When writing an MPC program, it is good to use an optimization library for your MPC optimization problem as they are well designed to do this.

Here are some libraries other than Ipopt:

• CasADi
• osqp-eigen
• OR-Tools

Run MPC Faster

Usually, the MPC will spend most of your computational power, slowing down other programs you are running. To tackle the issue, there are some approaches you can try:

1. Reduce the model order
2. Shorten the prediction horizon
3. Reduce the number of constraints
4. Use suboptimal solutions while exceeding specific computation time
5. Try another optimization library

Resources

1. What is Model Predictive Control?
2. Why Use Model Predictive Control?
3. Textbook: Vehicle Dynamics and Control by Rajesh Rajamani
4. Research Paper: Model Predictive Control for Autonomous and Semiautonomous Vehicles by Yiqi Gao

Level Up Coding

Thanks for being a part of our community! Before you go:

• 👏 Clap for the story and follow the author 👉
• 📰 View more content in the Level Up Coding publication
• 🔔 Follow us: Twitter | LinkedIn | Newsletter

🚀👉 Join the Level Up talent collective and find an amazing job