目录
一、引言
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是为了综合考虑位置、角度,希望调参过程能让我满意。
三、尚未解决的疑惑
3.1 那个r是什么?
看好多文章里LQR都有个像参考输入的东西r,见下图,没太理解。
3.2 晕车、吐?
无论人驾还是智驾,方向盘打的太猛会导致极差的驾乘体验,我想方向盘转动角速度是个关键参数,但怎么用上它呢?
当状态量好像很合理,当个假设的控制量好像也行,按照理论LQR好像不容易做约束,MPC好像能做,不过没想好怎么做,应该要做在cost function里吧?
还请大神指点。
3.3 新人挑战老资格
说的是LQR/MPC与PID,状态空间/最优控制所谓的兼顾多个参数,真的那么靠谱么?谁有调参经验能不能说说,控制效果真的明显强于PID吗?
四、总结与牢骚
说实在的,这么多年我都是做工程落地的,相比于自我感觉良好的理论,我更倾向于能创造实际价值的工程,对理论说教颇为不齿,不喜勿喷。LKA把LQR上了,希望调参体验能好。
记个小tip:
Apollo的单纵向是位置+速度PID,单横向是LQR+曲率前馈,横纵耦合是MPC。