Implementing MPC for Autonomous Driving with Various Solvers

In this post, I introduce how Model Predictive Control (MPC) can be applied to longitudinal and lateral control for the motion of a self-driving car, and demonstrate how to implement it using two optimization libraries: IPOPT and ACADO.

To implement autonomous driving, it is essential to achieve precise longitudinal (speed) and lateral (steering) control to ensure the vehicle follows its intended path accurately and safely. One of the most effective approaches to solving this control problem is MPC.

You’ll also learn how to integrate automatic differentiation libraries like CppAD for IPOPT, and how ACADO can generate fast code for embedded MPC controllers. With step-by-step code examples, this post aims to help you apply MPC to real-world autonomous vehicle control systems.

MPC for longitudinal and lateral control for autonomous driving

MPC is an advanced method of process model that is used to control a process while satisfying a set of constraints.

There are many advantages of MPC

• Straight-forward design procedure. Given a model of the system, including constraints, one only needs to set up an objective function that incorporates the control objectives.
• System latency can be considered easily
• if the references are know in advance, they can be used in order to optimize the reference tracking
• It can be used for MIMO and SISO systems.

Vehicle model

There are two kinds famous model for the vehicle modeling

• Kinematic model
• Dynamic model

Kinematic models are simplifications of dynamic models that ignore tire forces, gravity, and mass. This simplification reduces the accuracy of the models, but it also makes them more tractable.

Dynamic models encompass tire forces, longitudinal and lateral forces, inertia, gravity, air resistance, drag, mass, and the geometry of the vehicle. Therefore, it is more accurate than the kinematic model.

So, which model is more suitable for using MPC for autonomous driving control?

In this paper, thankfully, the kinematic model and dynamic model were analyzed from the perspective of MPC application in autonomous vehicles. As a result, it was shown that the performance difference between the kinematic and the dynamic model was not significant at 200ms discretization time.

Therefore, I chosen the kinematic model because the dynamic model need tire coefficient which is hard to estimate.

Kinematic model equation:

If you want to deeply understanding the vehicle models, I highly recommend this book which is written by Rajamani.

Error states

I will use three error types for the MPC.

• Cross track error ($ cte $)

  • Express the error between the reference path and the vehicle’s position.
  • $ cte_{t+1}=cte_{t}+v_t \cdot sin(e\psi_t) \cdot dt $
  • In vehicle coordinate, If we assume the reference path is $f(x_t)$, then cte is $cte_{t} = f(x_t)-y_t$.
  • Finally, we can express the $ cte $ as $cte_{t+1}=f(x_{t})-y_{t}+v_t \cdot sin(e\psi_t) \cdot dt$

• Heading error ($ e\psi $)

  • Express the heading error between the reference path and the vehicle’s heading angle
  • $ e\psi_{t+1}=e\psi_t+{v_{t}\over{L_{f}}} \cdot \delta_{t} \cdot dt $
  • Error $\psi$ means $e\psi_t=\psi_t-\psi des_t$
  • Therefore, $e\psi_{t+1}=\psi_t-\psi des_{t}+{v_{t}\over{L_{f}}} \cdot \delta_{t} \cdot dt$

• Speed error

  • Express the error between reference speed and the vehicle’s speed

MPC formulation

Then, MPC can be formulated:

$N$ is prediction horizon

MPC will optimize inputs that minimize error states, the magnitude of inputs and derivative of inputs. The inputs are steering angle and throttle value.

MPC implementation (IPOPT)

Code: repo

Now, let’s implement MPC by ipopt library. Ipopt (Interior Point Optimizer, pronounced “Eye-Pea-Opt”) is an open source software package for large-scale nonlinear optimization. In order to use ipopt for the optimization, we also need the differentiation library. In this post, CppAD will be used for it. This link is well documented for the ipopt with CppAD.

In ipopt, cost function is in variable fg[0] . In above MPC formulation it is $J$

    for (int t = 0; t < N; t++){
      
      // error states
      // cte (cross-track error)
      fg[0] += w_cte*CppAD::pow(vars[cte_start + t], 2);
      // epsi (heading error)
      fg[0] += w_epsi*CppAD::pow(vars[epsi_start + t], 2);
      // speed error
      fg[0] += w_verr*CppAD::pow(vars[v_start + t] - desired_speed, 2);  
      
      // inputs
      if (t < N - 1) {
        // steering
        fg[0] += w_delta*CppAD::pow(vars[delta_start + t], 2);
        // acceleration
        fg[0] += w_a*CppAD::pow(vars[a_start + t], 2);
      }
      // delta inputs
      if (t < N - 2) {
        //steering
        fg[0] += w_d_delta*CppAD::pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);
        //acceleration
        fg[0] += w_d_a*CppAD::pow(vars[a_start + t + 1] - vars[a_start + t], 2);    
      }
    }

Then, let’s make equality condition. In this project, 3-order polynomial trajectory was chosen as reference trajectory

       // 3rd order polynomial
      AD<double> f0 = coeffs[0] + coeffs[1]*x0 + coeffs[2]*pow(x0,2) + coeffs[3]*pow(x0,3);
      AD<double> psides0 = CppAD::atan(coeffs[1] + 2*coeffs[2]*x0 + 3*coeffs[3]*pow(x0,2));
      // Setup the rest of the model constraints
      fg[1 + x_start + t]    = x1    - (x0 + v0 * CppAD::cos(psi0) * dt);
      fg[1 + y_start + t]    = y1    - (y0 + v0 * CppAD::sin(psi0) * dt);
      fg[1 + psi_start + t]  = psi1  - (psi0 + v0 * delta0 / Lf * dt);
      fg[1 + v_start + t]    = v1    - (v0 + a0 * dt);
      fg[1 + cte_start + t]  = cte1  - ((f0 - y0) + (v0 * CppAD::sin(epsi0) * dt));
      fg[1 + epsi_start + t] = epsi1 - ((psi0 - psides0) + v0 * delta0 / Lf * dt);  

Also, inequality condition.

  // The upper and lower limits of delta are set to -25 and 25
  // degrees (values in radians).
  for (int i = delta_start; i < a_start; i++) {
    // limits set at +/- 0.4363 (radian) to represent 25 degrees
    vars_lowerbound[i] = -0.4363;
    vars_upperbound[i] =  0.4363;
  }

  // Acceleration/decceleration upper and lower limits.
  for (int i = a_start; i < n_vars; i++) {
    vars_lowerbound[i] = -1;
    vars_upperbound[i] =  1;
  } 
  

Simulation

• green line: MPC optimal trajectory (IPOPT)
• Yellow line: Reference trajectory

MPC implementation (ACADO)

Code: repo
ACADO is a toolkit for automatic code generation of optimal controllers, especially useful in embedded applications.

Model & Constraints

ACADO defines states and controls using a symbolic differential equation setup. It generates static and dynamic libraries from the MPC formulation.

DifferentialState x, y, v, phi;
Control a, delta;

f << dot(x) == v * cos(phi);
f << dot(y) == v * sin(phi);
f << dot(v) == a;
f << dot(phi) == v * tan(delta) / L;

OCP setup

OCP ocp(0, N * Ts, N);
ocp.subjectTo( f );
ocp.subjectTo( -1.0 <= a <= 1.0 );
ocp.subjectTo( -0.4363 <= delta <= 0.4363 );
ocp.minimizeLSQ(W, rf);
ocp.minimizeLSQEndTerm(WN, rfN);

Interfacing to the project with generated libraries: link

Simulation

• green line: MPC optimal trajectory (ACADO)
• Yellow line: Reference trajectory

Both IPOPT and ACADO are powerful solvers for MPC. IPOPT offers more flexibility and is suitable for general nonlinear problems, while ACADO excels in generating efficient embedded code for real-time control tasks.

I hope this post helps you get started implementing MPC for your own autonomous driving project!