目录
一、引言
二、LQR
2.1 LQR的理解与实现
2.1.1 模型设计与实现
2.1.2 LQR工程实现步骤
2.2 参考资料
2.2.1 基础理论与模型的推导
2.2.2 Refer to Apollo
2.2.3 其它实例参考
2.2.4 MATLAB中的LQR
2.3 demo代码
三、尚未解决的疑惑
3.1 那个r是什么？
3.2 晕车、吐？
3.3 新人挑战老资格
四、总结与牢骚
2022年5月，因为做ADAS、LKA，花了半个多月的时间系统学习了LQR、MPC，并做了demo，还把LQR替换了PID用在了LKA里，算是真正落地了，静待调参。
参考的文章链接、自己写的心得、做的demo都留在了公司内网，想着这是辛勤劳动的重要收获，怎么也得给自己留下资产，并且秉着分享与批评的态度，要把它整理整理放到CSDN上。
主要参考的资料来自CSDN，理论无非就是单车模型、二次规划这些，有些大神写的真是通俗易懂；工程实践参考的Apollo，自己的demo实现用matlab/simulink，这里我po上去的代码也都是用simulink里的matlab function写的，支持code generation，尽量从工程可实现的角度供读者参考。
好，言归正传，下面开始。
理论公式和图片们我就不放了，后边我参考的链接里都有，大家自己看，这里直接上干货。
既然参考的Apollo，那我模型的建立就是动力学，状态量4个：横向偏差、横向偏差变化率、角度偏差、角度偏差变化率，这个和Apollo是一样的。不一样的是Apollo做的是全域大地坐标系下的轨迹跟踪，而我ADAS-LKA做的是自车坐标系下的误差控制，状态量的生成方式要改一下。
具体的，因为我拿到的是摄像头给的C0C1C2C3，控制目标是车与车道线的距离≥某个数值Cx并且HeadingAngle接近于0，所以：
控制量Apollo用的是前轮转角δ再转化成方向盘转角，这里我就直接用方向盘转角了。
好的，总体思路有了，该做具体实现了，基于动力学模型的话，首先要有轮胎侧偏刚度、轴距等等必要的车体参数，来生成AB矩阵；然后根据选取的状态量和控制量的个数决定了QR矩阵的维度，我这里就是Q是4维，R退化成一个标量，再就是根据各元素的权重初步设定QR矩阵对角线上的取值，为什么叫初步呢，因为调参肯定要改个稀巴烂。重点来了：
另外要注意的还有几点：
最后，考虑到弯道的稳态误差，需要根据曲率做前馈补偿，具体实现方式自己定，我的比较简单上不了台面就不po了。
感谢各位大神！
参考1：
自动驾驶（七十二）---------LQR控制算法_一实相印的博客-CSDN博客_lqr控制算法
Apollo控制算法LQR分析（一） - 知乎
https://zhuanlan.zhihu.com/p/72702387
参考2：单车模型、阿克曼转向、转弯半径/曲率、差动转向，这里边还有运动学的彩蛋
Apollo学习笔记（7）车辆运动学模型_碎步の流年的博客-CSDN博客_车辆运动学模型
这里注意，连续模型和离散模型的推导与计算都是不一样的，咱们工程实践得用离散的方法，我刚开始就搞错了，避坑。
参考3：对N矩阵的解释
N代表更一般化性能指标中交叉乘积项的加权矩阵；
N很少用，就如上文解释的: 这个是一个交叉乘积，意思是是状态和输入的乘积，这一项因素就是耦合状态和输入两部分而产生的cost, 一般情况下我们也不会使用。
线性二次型最优控制器LQR设计原理以及matlab实现_gophae的博客-CSDN博客_lqr控制器原理
参考1：
讲解清楚的车辆动力学模型、Apollo模型推导、有AB矩阵的推导重要！
模型里的Vx是纵向速度，注意
Apollo控制算法车辆动力学模型分析（一） - 知乎
车辆动力学及控制_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()
单级倒立摆模型
如何在simulink 中实现LQR控制器的设计 – MATLAB中文论坛
另一例，比较简单
如何用simulink搭建LQR控制器？ - 知乎
可能借鉴的实例、AB矩阵的推导
Carsim应用：LKA车道保持辅助系统（LQR算法推导）_摩罗果的博客-CSDN博客_车道保持算法
https://blog.csdn.net/qq_35763436/article/details/102810098
8_LQR 控制器_状态空间系统Matlab/Simulink建模分析 - 北极星！ - 博客园
参数对性能的影响
倒立摆的PID与LQR控制算法的对比研究 - 豆丁网
MATLAB中的LQR函数用法_【快资讯】
用matlab实现离散LQR调节器,线性二次型调节器LQR/LQC算法解析及求解器代码（matlab）..._嗨嗨嗨夏天的博客-CSDN博客
主菜来了：
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是为了综合考虑位置、角度，希望调参过程能让我满意。
看好多文章里LQR都有个像参考输入的东西r，见下图，没太理解。
无论人驾还是智驾，方向盘打的太猛会导致极差的驾乘体验，我想方向盘转动角速度是个关键参数，但怎么用上它呢？
当状态量好像很合理，当个假设的控制量好像也行，按照理论LQR好像不容易做约束，MPC好像能做，不过没想好怎么做，应该要做在cost function里吧？
还请大神指点。
说的是LQR/MPC与PID，状态空间/最优控制所谓的兼顾多个参数，真的那么靠谱么？谁有调参经验能不能说说，控制效果真的明显强于PID吗？
说实在的，这么多年我都是做工程落地的，相比于自我感觉良好的理论，我更倾向于能创造实际价值的工程，对理论说教颇为不齿，不喜勿喷。LKA把LQR上了，希望调参体验能好。
记个小tip：
Apollo的单纵向是位置+速度PID，单横向是LQR+曲率前馈，横纵耦合是MPC。