The model_mpc designed for end-to-end longitudinal self-driving at OpenPilot, Comma.ai.

Previously, we have talked about the details of a regular rule-based MPC for the longitudinal control. It uses the lead vehicle information, estimates its future motions, and then computes the cost for possible collision, following distance, acceleration, and jerk.

Now, we are looking at the model MPC. As expected, the MPC_model should use the ref trajectories outputted by the neural network, instead of the rule-based estimation of lead vehicle movement.

For an MPC controller, we should first focus on the online data

In run_mpc() function, the first step is, as expected, to get the plan_v, plan_x and plan_a.

int run_mpc(state_t * x0, log_t * solution,
 double x_poly[4], double v_poly[4], double a_poly[4]){
 int i;
   for (i = 0; i < N + 1; ++i){
    // od is online data variables    
    acadoVariables.od[i*NOD+0] = x_poly[0]; 
    acadoVariables.od[i*NOD+1] = x_poly[1];
    acadoVariables.od[i*NOD+2] = x_poly[2];
    acadoVariables.od[i*NOD+3] = x_poly[3];
    acadoVariables.od[i*NOD+4] = v_poly[0];
    acadoVariables.od[i*NOD+5] = v_poly[1];
    acadoVariables.od[i*NOD+6] = v_poly[2];
    acadoVariables.od[i*NOD+7] = v_poly[3];
    acadoVariables.od[i*NOD+8] = a_poly[0];
    acadoVariables.od[i*NOD+9] = a_poly[1];
    acadoVariables.od[i*NOD+10] = a_poly[2];
    acadoVariables.od[i*NOD+11] = a_poly[3];

Note that although the model outputs a vector of 200 (2s), the mpc_model controller only takes 4 points from the vector, which is 0,10,20,30.

Then it updates the current state:

acadoVariables.x[0] = acadoVariables.x0[0]=x0->x_ego

we also see, the two key steps between setting the current state, and get the solution.

Note that the cost of this MPC is to mimic the model outputs

// Setup diagonal entries    
acadoVariables.W[NY*NY*i + (NY+1)*0] = xCost * f;    acadoVariables.W[NY*NY*i + (NY+1)*1] = vCost * f;    acadoVariables.W[NY*NY*i + (NY+1)*2] = aCost * f;    acadoVariables.W[NY*NY*i + (NY+1)*3] = accelCost * f;    acadoVariables.W[NY*NY*i + (NY+1)*4] = jerkCost * f;

The definition of cost functions:

Basically, the costs are the error between plan and reference trajectory, but they are using all three, including speed, acceleration, and position.

// Running cost  Function h;  
h << x_ego - poly_x;  
h << v_ego - poly_v;  
h << a_ego - poly_a;  
h << a_ego * (0.1 * v_ego + 1.0);  
h << j_ego * (0.1 * v_ego + 1.0);   
// Weights are defined in mpc.  
BMatrix Q(5,5); 
Q.setAll(true);

Again, the model MPC here does not use any lead vehicle information.