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
结果：车辆实际路径基本可以保持在期望路径