Carsim应用：LKA车道保持辅助系统（LQR控制器模型）
二轮转向汽车LKA控制器算法（LQR）
公式推导见上一篇文章
建立控制器模型
由上诉算法可知，算法计算的前提是：
1）获得输入变量：侧向速度(m/s)、横摆角速度(rad/s)、实际侧向偏移量(m)、实际横摆角偏移(rad)、期望横摆角速度(rad/s)、期望路径的曲率(1/m)
2）获得中间变量：k11,k12,k13,k14、k21,k22,k23,k24(K矩阵)、sy12,sy22(SR矩阵)、sr11,sr21(SR矩阵)、b11,b21(B矩阵)、W5(R矩阵)
3）输出变量：前轮偏转角
获得中间变量 利用Matlab语言编写程序
1）设置车辆参数–>A矩阵、B矩阵、E矩阵
m=1270;
Iz=1536.7;
L=2.91;
a=1.015;
b=1.895;
k1=-180000;
k2=-180000;
u=40/3.6;
p=0.85;
g=9.8;
a11=(k1+k2)/m/u;
a12=(a*k1-b*k2)/m/u-u;
a21=(a*k1-b*k2)/Iz/u;
a22=(a^2*k1+b^2*k2)/Iz/u;
b11=-k1/m;
b12=-k2/m;
b21=-a*k1/Iz;
b22=b*k2/Iz;
A=[a11,a12,0,0;a21,a22,0,0;1,0,0,u;0,1,0,0];
B=[b11；b21;0;0];
E=[0;0;0;-u];2）设置权重系数–>Q矩阵、R矩阵
w1=1;
w2=100000;
w3=10000000;
w4=10;
w5=10000;
Q=[w1,0,0,0;0,w2,0,0;0,0,w3,0;0,0,0,w4];
R=w5;3）lqr(A B Q R)函数–>K矩阵
[P,K]=lqr(A,B,Q,R);4）(K A B R Q E)矩阵–>SY SR矩阵
F=A'-K*B*inv(R)*B';
SY=inv(F)*Q;
SR=-inv(F)*K*E;5）获得必须的中间参数
k11=K(1,1);
k12=K(1,2);
k13=K(1,3);
k14=K(1,4);
k21=K(2,1);
k22=K(2,2);
k23=K(2,3);
k24=K(2,4);
sy12=SY(1,2);
sy22=SY(2,2);
sr11=SR(1,1);
sr21=SR(2,1);获得输入变量 利用simulink工具箱：
1）设置carsim车辆模型的输出变量：
车辆侧向速度、横摆角速度、车辆质心大地坐标、期望路径坐标
2）对输出的侧向速度、横摆角速度进行单位转换得到侧向速度(m/s)、横摆角速度(rad/s)
3）利用期望路径横纵坐标计算期望路径的曲率(1/m)
首先计算路径的一阶导数和二阶导数
然后计算期望路径的曲率(1/m)
其中的function代码用于计算(1+y’*y’)^(1.5)
4）根据期望路径的曲率(1/m)、横摆角速度(rad/s)、车辆横向坐标计算期望横摆角速度(rad/s)、实际侧向偏移量(m)、实际横摆角偏移(rad)
其中的function用于平滑
获得输出变量 simulink模块
按照公式进行搭建
联合仿真
1）设置carsim车辆模型的输入变量为方向盘转角
2）将控制器输出变量转化为方向盘转角
前轮偏角乘以转向器的转向比（方向盘转角–>齿条偏移–>前轮偏角）
3）跑一下
先运行m文件 得到中间变量存在工作空间
然后运行simlunk
结果：车辆实际路径基本可以保持在期望路径
