Details of the MPC for longitudinal control without a neural network

This post will introduce the details of the regular (without neural nets) MPCs which plan the longitudinal movement of a car.

We have three key functions for a controller:

1. the setup_mpc() function initiates the mpc with the control gains we specify.
2. set_cur_state() function, which usually assigns the current vEgo and aEgo
3. update() function: after updating the current state, we call the mpc solver and get the solution.

Now we look into the details of the MPC update step.

First, it sets the current MPC state, where we update the lead vehicle state.

# Setup current mpc state
    self.cur_state[0].x_ego = 0.0
if lead is not None and lead.status:
      x_lead = lead.dRel
      v_lead = max(0.0, lead.vLead)
      a_lead = lead.aLeadK
   if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
      v_lead = 0.0
      a_lead = 0.0
   self.a_lead_tau = lead.aLeadTau ## lead accel time constant
   self.new_lead = False
   if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
      self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead,  a_lead, self.a_lead_tau)
      self.new_lead = True
   self.prev_lead_status = True
   self.prev_lead_x = x_lead
   self.cur_state[0].x_l = x_lead
   self.cur_state[0].v_l = v_lead
 else:
   self.prev_lead_status = False # no leader
   # Fake a fast lead car, so mpc keeps running
   self.cur_state[0].x_l = 50.0
   self.cur_state[0].v_l = v_ego + 10.0
   a_lead = 0.0
   self.a_lead_tau = _LEAD_ACCEL_TAU

Note that a_lead_tau is the lead vehicle acceleration time, it will be used for the mpc controller to predict the movement of the lead vehicle.

Calculate the mpc is quite simple, which is done by C++ code

# Calculate mpc
t = sec_since_boot()
self.n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
self.duration = int((sec_since_boot() - t) * 1e9)

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

We can see, a_lead_tau is referred to as l in the following function, and used for estimating the acceleration of the lead vehicle by:

a_l = a_l_0*exp(-l*t*t/2)

Here the online data (acadoVariables.od) is the predicted lead vehicle speed and distance. As a comparison, for the mpc_model, the online data is the plan output by the neural network.

Get the solution from mpc:

# Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
self.v_mpc = self.mpc_solution[0].v_ego[1]
self.a_mpc = self.mpc_solution[0].a_ego[1]
self.v_mpc_future = self.mpc_solution[0].v_ego[10]

The above code shows how to call the mpc solver, but how does the mpc work?

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

Introducing all variables and constants.

Setting up the car ODE model.

Setting up a least-squares objective function by defining the five components of the measurement function h and an appropriate weighting matrix.

Defining a complete optimal control problem (OCP) comprising the dynamic model, the objective function as well as constraints on the input.

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.

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.

First, the mpc needs the dynamics of the system, which is just the ODE:

// Equations of motion
f << dot(x_ego) == v_ego;
f << dot(v_ego) == a_ego;
f << dot(a_ego) == j_ego;

Then, it defines the cost function using the following equations and some weights:

// Running cost
 Function h;
 h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) — 1; //66, ttc
 h << (d_l — desired) / (0.05 * v_ego + 0.5); //66, distance
 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(4,4); Q.setAll(true);

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

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

From the perspective of a CF model, we are especially interested in the desired distance 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))
desired_distance = 
3.6*v_ego - 1.8*v_l + v_ego*v_ego/(2*9.81) - v_l*v_l/(2*9.81)

Finally, the mpc is solved by minimizing the LSQ.