一、引言
2022年5月，因为做ADAS、LKA，花了半个多月的时间系统学习了LQR、MPC，并做了demo，还把LQR替换了PID用在了LKA里，算是真正落地了，静待调参。
参考的文章链接、自己写的心得、做的demo都留在了公司内网，想着这是辛勤劳动的重要收获，怎么也得给自己留下资产，并且秉着分享与批评的态度，要把它整理整理放到CSDN上。
主要参考的资料来自CSDN，理论无非就是单车模型、二次规划这些，有些大神写的真是通俗易懂；工程实践参考的Apollo，自己的demo实现用matlab/simulink，这里我po上去的代码也都是用simulink里的matlab function写的，支持code generation，尽量从工程可实现的角度供读者参考。
好，言归正传，下面开始。
二、LQR
2.1 LQR的理解与实现
理论公式和图片们我就不放了，后边我参考的链接里都有，大家自己看，这里直接上干货。
2.1.1 模型设计与实现
既然参考的Apollo，那我模型的建立就是动力学，状态量4个：横向偏差、横向偏差变化率、角度偏差、角度偏差变化率，这个和Apollo是一样的。不一样的是Apollo做的是全域大地坐标系下的轨迹跟踪，而我ADAS-LKA做的是自车坐标系下的误差控制，状态量的生成方式要改一下。
具体的，因为我拿到的是摄像头给的C0C1C2C3，控制目标是车与车道线的距离≥某个数值Cx并且HeadingAngle接近于0，所以：
- 横向偏差可用Cx-C0
- 横向偏差变化率可用基于小角度近似的思想，VehicleSpeed✖sin(atan(C1))
- 角度偏差即HeadingAngle=atan(C1)
- 角度偏差变化率即YawRate
控制量Apollo用的是前轮转角δ再转化成方向盘转角，这里我就直接用方向盘转角了。
好的，总体思路有了，该做具体实现了，基于动力学模型的话，首先要有轮胎侧偏刚度、轴距等等必要的车体参数，来生成AB矩阵；然后根据选取的状态量和控制量的个数决定了QR矩阵的维度，我这里就是Q是4维，R退化成一个标量，再就是根据各元素的权重初步设定QR矩阵对角线上的取值，为什么叫初步呢，因为调参肯定要改个稀巴烂。重点来了：
- Q中元素的大小表示状态量的重要程度，越大表示越重要，控制的快速性会越好，但会增大输入（能量消耗增多），导致超调、震荡可能会比较明显；
- 控制权重矩阵R中元素的大小表示对能量消耗的关注度，越大表示对该输入的能量消耗越敏感，会减小输入，有效降低超调、震荡，控制过程柔和，但快速性较差；
- QR的选取是两种约束互相妥协的过程，重点关注调参。
以上三点是我写在公司内网的原话，够意思吧~
另外要注意的还有几点：
- 车体参数可能以多种形式给出，需要自己做计算转换，可参考Apollo的代码；
- 对于完整的模型来说，除去AB矩阵之外，考虑横摆还有矩阵B1、B2、N，其中B2为常量向量[0 0 0 1]‘，它只对横摆角角度偏差变化率的导数产生影响，往往忽略不计；
- 模型里的Vx是纵向速度，不要误用了车速，不过差别一般不大，参见https://zhuanlan.zhihu.com/p/72599951。
最后，考虑到弯道的稳态误差，需要根据曲率做前馈补偿，具体实现方式自己定，我的比较简单上不了台面就不po了。
2.1.2 LQR工程实现步骤
- 拿到车体参数；
- 选取状态量和控制量，建模；
- 对AB矩阵做离散化；
- 设定QR矩阵；
- 初始化P矩阵为Q；
- 迭代求解Ricatti方程更新P；
- 根据P计算增益矩阵
；
- 计算控制量
；
- 慢慢调参吧。。。
2.2 参考资料
感谢各位大神！
2.2.1 基础理论与模型的推导
参考1：
自动驾驶（七十二）———LQR控制算法_一实相印的博客-CSDN博客_lqr控制算法
https://zhuanlan.zhihu.com/p/72702387
参考2：单车模型、阿克曼转向、转弯半径/曲率、差动转向，这里边还有运动学的彩蛋
Apollo学习笔记（7）车辆运动学模型_碎步の流年的博客-CSDN博客_车辆运动学模型
这里注意，连续模型和离散模型的推导与计算都是不一样的，咱们工程实践得用离散的方法，我刚开始就搞错了，避坑。
参考3：对N矩阵的解释
N代表更一般化性能指标中交叉乘积项的加权矩阵；
N很少用，就如上文解释的: 这个是一个交叉乘积，意思是是状态和输入的乘积，这一项因素就是耦合状态和输入两部分而产生的cost, 一般情况下我们也不会使用。
线性二次型最优控制器LQR设计原理以及matlab实现_gophae的博客-CSDN博客_lqr控制器原理
2.2.2 Refer to Apollo
参考1：
讲解清楚的车辆动力学模型、Apollo模型推导、有AB矩阵的推导重要！
模型里的Vx是纵向速度，注意
车辆动力学及控制_Apollo控制算法车辆动力学模型分析（一）_weixin_39649736的博客-CSDN博客
LQR与汽车横向动力学_lewis_0的博客-CSDN博客_lqr横向控制
参考2：Apollo双线性离散化的理论公式
开发者说 | Apollo控制算法之汽车动力学模型和LQR控制 – 走看看
开发者说 | Apollo控制算法之汽车动力学模型和LQR控制 – 莫回首_love – 博客园
开发者说 | Apollo控制算法之汽车动力学模型和LQR控制
这里注意，Apollo做线性化用的是双离散的方法，其中A是建模的A矩阵，T是采样时间，I是单位阵。
参考3：
由于单车模型把两个轮胎合并，所以侧偏刚度要乘以2，小轿车单轮的侧偏刚度一般范围在5W~8W N/rad，Apollo中是已经2倍后了的，所以如果参考Apollo做的话，前后侧偏刚度应该是在十几万的量级。
Apollo代码学习(三)—车辆动力学模型_follow轻尘的博客-CSDN博客_车辆动力学模型
有对着Apollo代码的讲解，逻辑清晰多了：
Apollo代码学习(五)—横纵向控制_follow轻尘的博客-CSDN博客_纵向控制算法
参考4：开源Apollo中LQR的位置和计算车体参数、AB矩阵的位置
..\apollo-master\modules\control\controller\lat_controller.cc
ComputeControlCommand()、Init()、LoadControlConf()
2.2.3 其它实例参考
单级倒立摆模型
如何在simulink 中实现LQR控制器的设计 – MATLAB中文论坛
另一例，比较简单
可能借鉴的实例、AB矩阵的推导
Carsim应用：LKA车道保持辅助系统（LQR算法推导）_摩罗果的博客-CSDN博客_车道保持算法
https://blog.csdn.net/qq_35763436/article/details/102810098
8_LQR 控制器_状态空间系统Matlab/Simulink建模分析 – 北极星！ – 博客园
参数对性能的影响
2.2.4 MATLAB中的LQR
用matlab实现离散LQR调节器,线性二次型调节器LQR/LQC算法解析及求解器代码（matlab）…_嗨嗨嗨夏天的博客-CSDN博客
2.3 demo代码
主菜来了：
function y = Fcn_LQR(Ca_f, Ca_r, mass_f, mass_r, WheelBase, VehicleSpeed_mps, ...
LatErr, LatErr_d, AngleErr, AngleErr_d, ...
ts)
% 根据理论拆解的工程化实现步骤如下：
% 首先计算相关车体参数，用于系统建模得到必要的系数矩阵如AB
% AB矩阵有了之后要做离散化处理
% 然后定义待优化的状态量及控制量，以定义QR权重矩阵
% ---至此，系统模型和QR矩阵都有了，其中系统模型是必须，QR矩阵是待调试参数
% 然后可以求解Riccati方程了，为启动求解过程可将P初始化为Q
% 根据系统需求、软硬件平台性能等定义求解Riccati的迭代次数和收敛误差
% 求解Riccati得到P，进而计算增益矩阵K
% 最终输出y = K*x*-1;
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;
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;
% B1 = [0 0 0 0]';
% B1(2) = A(2,4) - VehicleSpeed_mps;
% B1(4) = A(4,4);
%B2 = [0 0 0 1]';
% C = [0 0 1 0]; %观测角度
% D = 0;
Q = zeros(4,4);
Q(1,1) = 2;
Q(2,2) = 2;
Q(3,3) = 1;
Q(4,4) = 1;
R = 0.1;
%% 这部分是自己写的，收敛条件可调；可生成代码
P = Q;
iter = 1;
diff = 1000000;
while (iter<=10 && diff>=1)
P_next = A'*P*A - (A'*P*B)/(R+B'*P*B)*(B'*P*A) + Q;
P_diff = P_next - P;
the_max = abs(max(max(P_diff)));
another_max = abs(min(min(P_diff)));
diff = max(the_max, another_max);
iter = iter + 1;
P = P_next;
end
K = (R+B'*P*B) \ (B'*P*A);
%% 这部分是用MATLAB算的，可用来做对比
% coder.extrinsic("lqr");
% coder.extrinsic("idare"); %不加这行运行不了仿真，因为这个simulink模型是按照生成代码配置的
% K = [0 0 0 0]; %设定个初始维度，否则生成代码的时候报y不确定size的bug
% %[K,~,~] = lqr(A,B,Q,R); %这个是连续系统的，不适用
%
% [~,K,~] = idare(A,B,Q,R); %lqr、idare都不支持code generation
x = [LatErr, LatErr_d, AngleErr, AngleErr_d]';
y = K*x*-1;LQR的求解有两个关键参数，即迭代误差和迭代次数，这个要根据软硬件平台、采样时间、可接受的精度等综合考虑。
LQR“能够兼顾多项性能指标”，状态量不再是PID只有一个；用LQR是为了综合考虑位置、角度，希望调参过程能让我满意。
三、MPC
3.1 MPC的理解与实现
一样，还是直接上干货。
3.1.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的参考项。
3.1.2 MPC工程实现步骤
- 拿到车体参数；
- 选取状态量和控制量，建模；
- 对AB矩阵做离散化；
- 设定QR矩阵；
- 设定预测步数Np，并基于Np对ABQR做增广；
- 设定相关参数的约束、各状态的当前值；
- 转化为QP问题求解得到Np步的控制向量u；
- 使用u(0)做控制，并递推一步u迭代到下一轮MPC过程；
- 慢慢调参吧。。。
3.2 参考资料
老规矩，首先感谢各位大神。
3.2.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.2 Refer to Apollo
参考1：
Apollo-MPC的框架介绍，这个博主有Apollo其它模块的介绍可以看看，前后逻辑比较清楚；用预瞄点的曲率做前馈
自动驾驶（七十六）———Apollo MPC之代码解析_一实相印的博客-CSDN博客_自动驾驶控制代码
Apollo-MPC二次规划的简单推导
https://zhuanlan.zhihu.com/p/72738458
有一些Apollo的代码
Apollo代码学习(六)—模型预测控制(MPC)_follow轻尘的博客-CSDN博客_apollo mpc代码
参考2：开源Apollo中LQR的位置和计算车体参数、AB矩阵的位置
..\apollo-master\modules\control\controller\mpc_controller.cc
ComputeControlCommand()、Init()、LoadControlConf()
3.2.3 其它实例参考
参考1：极重要，是我自己实现的主要refer
demo基础参考
自编Matlab代码实现MPC基本算法_玄在天涯的博客-CSDN博客_matlab mpc
基础参考做了引申，固定点跟踪
自编Matlab代码实现MPC定点跟踪_玄在天涯的博客-CSDN博客_mpc算法matlab
参考2：
一个MPC简单matlab实例
3.2.4 MATLAB中的MPC
核心是依赖quadprog()，结合着代码讲吧。
3.3 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 中国
四、尚未解决的疑惑
4.1 那个r是什么？
看好多文章里LQR都有个像参考输入的东西r，见下图，没太理解。
4.2 晕车、吐？
无论人驾还是智驾，方向盘打的太猛会导致极差的驾乘体验，我想方向盘转动角速度是个关键参数，但怎么用上它呢？
当状态量好像很合理，当个假设的控制量好像也行，按照理论LQR好像不容易做约束，MPC好像能做，不过没想好怎么做，应该要做在cost function里吧？
还请大神指点。
4.3 新人挑战老资格
说的是LQR/MPC与PID，状态空间/最优控制所谓的兼顾多个参数，真的那么靠谱么？谁有调参经验能不能说说，控制效果真的明显强于PID吗？
五、总结与牢骚
说实在的，这么多年我都是做工程落地的，相比于自我感觉良好的理论，我更倾向于能创造实际价值的工程，对理论说教颇为不齿，不喜勿喷。LKA把LQR上了，希望调参体验能好。
记个小tip：
Apollo的单纵向是位置+速度PID，单横向是LQR+曲率前馈，横纵耦合是MPC。
文章出处登录后可见！
