Model predictive control for autonomous driving

Model Predictive Control for autonomous driving

This post explains model predictive control formulation for longitudinal and lateral control of the self driving car.

Model Predictive Control

Model predictive control (MPC) is an advanced method of process model that is used to control a process while satisfying a set of constraints. – wikidepia

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*sin(e\psi_t) *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*sin(e\psi_t) *dt$

• Heading error (EPSI)

  • 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}}} *\delta_{t} *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}}} *\delta_{t} *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 by using the IPOPT library

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
• Yellow line: Reference trajectory

If you would like to see overall codes, Please visit my repo