第七篇（下），MPC工程化总结

目录

一、引言

二、MPC的理解与实现

2.1 模型设计与实现

2.2 MPC工程实现步骤

三、参考资料

3.1 基础理论

3.2 Refer to Apollo

3.3 其它实例参考

3.4 MATLAB中的MPC

四、demo代码

---

一、引言

接上文，继续。

所谓“上文”

二、MPC的理解与实现

一样，还是直接上干货。

2.1 模型设计与实现

基本和LQR一致，状态方程、AB矩阵、状态与控制的选择、双线性离散化、QR矩阵，参考前面的LQR就好。

不同的是：

• 要定义一个预测步数Np，并基于Np对ABQR等矩阵做增广；
• 而且MPC可以考虑空间状态变量的各种约束，而LQR、PID等只能考虑输入输出的约束；
• MPC还有一个优点，就是允许模型存在误差，不像LQR中的模型精度那么高；
• 损失函数方面，LQR做0~∞积分，MPC做Np有限积分，这也导致了求解方法的不同。

说点儿其它的，在损失函数cost function的计算中，互为转置的项可以合并同类项，理解了这个点有些推导过程才能看的懂。

还有就是，如果状态量不用偏差而直接用相应的参数，则需要改模型改cost function的设计；另外，暂不考虑模型的变化，以横向位置替换横向偏差做状态量为例，则cost function里得减去横向位置参考才合乎常理，这时，代入cost function推导可以得出，最终的结果为quadprog()里的参数f需要减去状态x的参考项。

2.2 MPC工程实现步骤

1.  拿到车体参数；
2. 选取状态量和控制量，建模；
3. 对AB矩阵做离散化；
4. 设定QR矩阵；
5. 设定预测步数Np，并基于Np对ABQR做增广；
6. 设定相关参数的约束、各状态的当前值；
7. 转化为QP问题求解得到Np步的控制向量u；
8. 使用u(0)做控制，并递推一步u迭代到下一轮MPC过程；
9. 慢慢调参吧。。。

三、参考资料

老规矩，首先感谢各位大神。

3.1 基础理论

参考1：

无人驾驶之车辆控制 （2）MPC模型预测控制算法_小郑同学爱学习的博客-CSDN博客_mpc控制算法

你还在用PID？MPC模型预测控制，从公式到代码！_哔哩哔哩_bilibili

MPC学习笔记(一)：手推MPC公式_煜个头头的博客-CSDN博客_mpc推导

参考2：有些代码

MPC模型预测控制（二）-MATLAB代码实现_tingfenghanlei的博客-CSDN博客_mpc代码

MPC模型预测控制_tingfenghanlei的博客-CSDN博客_mpc模型预测控制大牛知乎

自动驾驶——模型预测控制（MPC）理解与实践_wisdom_bob的博客-CSDN博客_mpc 自动驾驶

3.2 Refer to Apollo

参考1：

Apollo-MPC的框架介绍，这个博主有Apollo其它模块的介绍可以看看，前后逻辑比较清楚；用预瞄点的曲率做前馈

自动驾驶（七十六）---------Apollo MPC之代码解析_一实相印的博客-CSDN博客_自动驾驶控制代码

Apollo-MPC二次规划的简单推导

Apollo控制算法MPC分析（一） - 知乎

有一些Apollo的代码

Apollo代码学习(六)—模型预测控制(MPC)_follow轻尘的博客-CSDN博客_apollo mpc代码

参考2：开源Apollo中LQR的位置和计算车体参数、AB矩阵的位置

..\apollo-master\modules\control\controller\mpc_controller.cc

ComputeControlCommand()、Init()、LoadControlConf()

3.3 其它实例参考

参考1：极重要，是我自己实现的主要refer

demo基础参考

自编Matlab代码实现MPC基本算法_玄在天涯的博客-CSDN博客_matlab mpc

基础参考做了引申，固定点跟踪

自编Matlab代码实现MPC定点跟踪_玄在天涯的博客-CSDN博客_mpc算法matlab

参考2：

一个MPC简单matlab实例

模型预测控制（MPC）解析（一） - 古月居

3.4 MATLAB中的MPC

核心是依赖quadprog()，结合着代码讲吧。

四、demo代码

上主菜：

function y = Fcn_MPC(Ca_f, Ca_r, mass_f, mass_r, WheelBase, VehicleSpeed_mps, ...
                     LatErr, LatErr_d, AngleErr, AngleErr_d, ...
                     ts)

WholeMass = mass_f + mass_r;
l_f = WheelBase * (1 - mass_f/WholeMass);
l_r = WheelBase * (1 - mass_r/WholeMass);
Iz = l_f*l_f*mass_f + l_r*l_r*mass_r;

dim_state = 4; %状态4个
dim_ctrl = 1; %控制1个

A = zeros(4,4);
A(1,2) = 1;
A(2,2) = (Ca_f+Ca_r) / WholeMass / VehicleSpeed_mps * -1;
A(2,3) = (Ca_f+Ca_r) / WholeMass;
A(2,4) = (Ca_r*l_r-Ca_f*l_f) / WholeMass / VehicleSpeed_mps;
A(3,4) = 1;
A(4,2) = (Ca_r*l_r-Ca_f*l_f) / Iz / VehicleSpeed_mps;
A(4,3) = (Ca_f*l_f-Ca_r*l_r) / Iz;
A(4,4) = (Ca_f*l_f*l_f+Ca_r*l_r*l_r) / Iz / VehicleSpeed_mps * -1;
A = (eye(4)-ts*0.5*A) \ (eye(4)+ts*0.5*A); % Apollo的双线性离散化

B = [0 0 0 0]';
B(2) = Ca_f / WholeMass;
B(4) = Ca_f * l_f / Iz;
B = B * ts;

Q = zeros(4,4);
Q(1,1) = 2;
Q(2,2) = 2;
Q(3,3) = 1;
Q(4,4) = 1;

R = 0.1;

% 当前状态量
x0 = [LatErr, LatErr_d, AngleErr, AngleErr_d]';

% 预测步长
Np=10;

% 增广
A_ext = zeros(dim_state*Np, dim_state);
B_ext = zeros(dim_state*Np, Np);
Q_ext = zeros(dim_state*Np);
R_ext = zeros(dim_ctrl*Np);
for i=1:1:Np
    At_tmp = A^i;
    for row = 1 : 1 : dim_state
        for col = 1 : 1 : dim_state
            A_ext(dim_state*(i-1)+row, col) = At_tmp(row,col);
        end
    end

    for j=1:1:i
        Bt_tmp = (A^(i-j)) * B;
        for row = 1 : 1 : dim_state
            B_ext(dim_state*(i-1)+row, j) = Bt_tmp(row,1);
        end
    end

    for row = 1 : 1 : dim_state
        for col = 1 : 1 : dim_state
            Q_ext(dim_state*(i-1)+row, dim_state*(i-1)+col) = Q(row,col); % Qt/Rt是对角Q/R阵，这里用row_Q其实代表方阵维数，QR都是方阵
        end
    end

    for row = 1 : 1 : dim_ctrl
        for col = 1 : 1 : dim_ctrl
            R_ext(dim_ctrl*(i-1)+row, dim_ctrl*(i-1)+col) = R(row,col); % Qt/Rt是对角Q/R阵，这里用row_Q其实代表方阵维数，QR都是方阵
        end
    end
end

% 控制量ut的初始值
persistent p_u0;
if isempty(p_u0)
    p_u0 = zeros(Np,1);
end

% quadprog()所需参数
H = 2*(B_ext'*Q_ext*B_ext + R_ext);
f = (2*x0'*A_ext'*Q_ext*B_ext)';
qp_A = [B_ext; -B_ext];
state_constr = zeros(dim_state*Np, 1); % state_constr也要做增广，以匹配公式中的维度
constr_tmp = [0.5 1 0.5/180*pi 1/10]';
for i = 1 : 1 : Np
    for j = 1 : 1 : dim_state
        state_constr(dim_state*(i-1)+j) = constr_tmp(j);
    end
end
b = [state_constr-A_ext*x0; state_constr+A_ext*x0];% b是根据对状态的约束得来的，
                                                   % [0.5 1 0.5/180*pi 1/10]，即横向误差约束到半米以内、变化率1m/s以内、
                                                   % HandingAngleError(也就是HeadingAngle)在0.5°以内、HeadingAngleErrorRate在0.1rad以内
lb = -20/180*pi*ones(Np,1); % 控制量ut的上下限，即方向盘转角
ub = 20/180*pi*ones(Np,1); % ±20°
opts = optimoptions('quadprog','Algorithm','active-set');
% quadprog()所需参数

% solve qp problem
[u, fval, exitflag]=quadprog(H, f, qp_A, b, [], [], lb, ub, p_u0, opts);

p_u0 = [u(2:Np); u(Np)];

y = u(1);

这是一步的操作，实际应用要在每个控制循环调用一次，LQR也是一样。

续上3.2.3里quadprog()的问题，这个一定要做options，否则仿真能过但无法code generation，至于quadprog()的实现原理。。。。。。反正我code generation直接给我造出了5000+的代码量，say byebye就好，下面是做options的方案：

doc quadprog或Generate Code for quadprog- MATLAB & Simulink- MathWorks 中国