第七篇（上），LQR工程化总结

qq_42466012

已于 2023-12-12 17:17:31 修改

阅读量5.8k

点赞数 38

分类专栏： Apollo笔记文章标签：自动驾驶算法 matlab

于 2022-06-11 16:56:57 首次发布

本文链接：https://blog.csdn.net/qq_42466012/article/details/125235046

版权

Apollo笔记专栏收录该内容

5 篇文章 9 订阅

订阅专栏

2.2.2 Refer to Apollo

一、引言

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计算增益矩阵 $K=R^{-1}B^{T}P$ ；
计算控制量 $u=-Kx$ ；
慢慢调参吧。。。

2.2 参考资料

感谢各位大神！

2.2.1 基础理论与模型的推导

参考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控制器原理

2.2.2 Refer to Apollo

参考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_{D}=(I-\frac{T}{2}A)^{-1}(I+\frac{T}{2}A)$ ，其中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中文论坛

另一例，比较简单

如何用simulink搭建LQR控制器？ - 知乎

可能借鉴的实例、AB矩阵的推导

Carsim应用：LKA车道保持辅助系统（LQR算法推导）_摩罗果的博客-CSDN博客_车道保持算法

https://blog.csdn.net/qq_35763436/article/details/102810098

8_LQR 控制器_状态空间系统Matlab/Simulink建模分析 - 北极星！ - 博客园

参数对性能的影响

倒立摆的PID与LQR控制算法的对比研究 - 豆丁网

2.2.4 MATLAB中的LQR

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是为了综合考虑位置、角度，希望调参过程能让我满意。