Acado solver for the MPC in OpenPilot

This post will dig into the acado mpc solver in OP.

First, let’s look at the acado struct used in the mpc. AcadoVaraibles are commonly used.

Variable counts are first defined as:

#define NX  ACADO_NX  /* Number of differential state variables.  */
#define NXA ACADO_NXA /* Number of algebraic variables. */
#define NU  ACADO_NU  /* Number of control inputs. */
#define NOD ACADO_NOD  /* Number of online data values. */
#define NY  ACADO_NY  /* Number of measurements/references on nodes 0..N - 1. */
#define NYN ACADO_NYN /* Number of measurements/references on node N. */
#define N  ACADO_N   /* Number of intervals in the horizon. */

N denotes the number of intervals in the horizon, and NY means the number of measurements on nodes (0,1,…N-1). NYN is the number of measurements on the final node N.

The init function of the longitudinal_mpc works:

void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost){
  acado_initializeSolver();
  int    i;
  const int STEP_MULTIPLIER = 3;
/* Initialize the states and controls. */
  for (i = 0; i < NX * (N + 1); ++i)  acadoVariables.x[ i ] = 0.0;
  for (i = 0; i < NU * N; ++i)  acadoVariables.u[ i ] = 0.0;
/* Initialize the measurements/reference. */
  for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
  for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;
/* MPC: initialize the current state feedback. */
  for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;

  // Set weights
for (i = 0; i < N; i++) {
    int f = 1;
    if (i > 4){
      f = STEP_MULTIPLIER;
    }
    // Setup diagonal entries
    acadoVariables.W[NY*NY*i + (NY+1)*0] = ttcCost * f; 
    // exponential cost for time-to-collision (ttc)
    acadoVariables.W[NY*NY*i + (NY+1)*1] = distanceCost * f; 
    // desired distance
    acadoVariables.W[NY*NY*i + (NY+1)*2] = accelerationCost * f; 
    // acceleration
    acadoVariables.W[NY*NY*i + (NY+1)*3] = jerkCost * f; 
    // jerk
  }
  acadoVariables.WN[(NYN+1)*0] = ttcCost * STEP_MULTIPLIER; 
  // exponential cost for danger zone
  acadoVariables.WN[(NYN+1)*1] = distanceCost * STEP_MULTIPLIER; 
  // desired distance
  acadoVariables.WN[(NYN+1)*2] = accelerationCost * STEP_MULTIPLIER;       // acceleration
}

We see that the MPC controllers initialize with some cost parameters, those parameters are defined in the longitudinal_mpc.py function.

def setup_mpc(self):
    ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
    self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
                     MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)

And the most important MPC cost parameters are imported from:

from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG

In file selfdrive.controls.lib.drive_helpers, the MPC_COST_LONG are predefined as:

class MPC_COST_LONG:
  TTC = 5.0 # time to collision
  DISTANCE = 0.1 # desired distance
  ACCELERATION = 10.0 # acceleration
  JERK = 20.0 # jerk 

So, actually the parameters for the mpc controller are just defined as above.

Notice that the cost parameters are time-varying because they are multiplied by a step_multiplier=3, which applies when the step-index is larger than 4.

The key step: run_mpc():

It calculates the lead_vehicle predictions, like setting up the boundary constraint for the optimization problem, then uses the cost function to find the solution.

Now we show how a MPC controller is defined in Acado:

Quote from acado documentation, to set-up of a MPC in acado, it comprises six main steps:

1. Introducing all variables and constants.
2. Setting up the car ODE model.
3. Setting up a least-squares objective function by defining the five components of the measurement function h and an appropriate weighting matrix.
4. Defining a complete optimal control problem (OCP) comprising the dynamic model, the objective function as well as constraints on the input.
5. Setting up a RealTimeAlgorithm defined by the OCP to be solved at each sampling instant together with a sampling time specifying the time lag between two sampling instants. Moreover, several options can be set and plot windows flushed.
6. Setting up a Controller by specifying a control law, i.e. the real-time algorithm solving our OCP in this case, and a reference trajectory to be tracked. In this example, the reference trajectory is read from a file where the value of all components are defined over time. (Note that the reference trajectory can be left away when calling the Controller constructor which is equivalent to all entries zero over the whole simulation horizon.)

The optimization problem for the mpc controller is probably defined in the generator.cpp file. Note that here we can clearly see how the cost function is defined, including the desired distance our vehicle want to keep.

Here is the detail of the desired distance:

#define G 9.81
#define TR 1.8
#define RW(v_ego, v_l) (v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G))

We also add the details from the source code of acado tools, which shows how the minimizeLSQ() works.