kevin-y-wang
commented
4 years ago Answer of your question (1) : In Apollo MPC control, the target is to solve the optimization problem with the optimal feedback control u as follows: optimal feedback control u = arg min (X - X_ref)^T Q (X - X_ref) + u^T R u which can be transfer to a QP problem and eventually the solution will be generated in the form as: u = M AA X + M CC Vx curvature (where, the details of matrix M, AA, CC can be found at https://github.com/ApolloAuto/apollo/blob/master/modules/common/math/mpc_solver.cc) then, make K = M AA and Kr = M CC, the optimal feedback control u can be written as u = K X + Kr Vx curvature here, K = control_gain[0](0, i) and Kr = addition_gain[0](0, 0) Then, the overall control u = K X + Kr Vx * curvature + u_feedforward;
(while in LQR, to quickly get the solution with Riccati equation, we simplify the nonlinear dynamic models to some extent, so the solution does not contain Kr Vx curvature)
Answer of your question (2) : In MPC controller, the overall feedforward control = steer_angle_feedforwardtermupdated + steer_angle_ff_compensation If we check the equations about steer_angle_feedforwardtermupdated and steer_angle_ff_compensation, you will find they are almost equal to the LQR feedforward, except that the MPC has the additional team "addition_gain[0](0, 0) v" (About why there is a addition_gain[0](0, 0) v, the answer to your question 1 actually has already given the explanation).
sjiang2018
Dear Sir,
In Apollo MPC control code, steer feedforward_compensation seems to be calculated according to unconstrained_control and lqr feedforward value, but why unconstrained_control is calculated as below:
for (int i = 0; i < basic_statesize; ++i) { unconstrained_control += control_gain[0](0, i) matrixstate(i, 0); } unconstrained_control += addition_gain[0](0, 0) v * debug->curvature();
I have no idea about (v * debug->curvature())
And why feedforward_compensation like:
steer_angle_ff_compensation = Wheel2SteerPct(debug->curvature() (control_gain[0](0, 2) (lr - lf / cr * mass v v / wheelbase_) - addition_gain[0](0, 0) * v));
the formula （controlgain[0](0, 2) * (lr - lf / cr mass_ v * v / wheelbase_ ）not equal to lqr.
Thanks.