Carsim應用：LKA車道保持輔助系統（LQR控制器模型）

原創

weixin_45644911

2020-04-21 01:41

二輪轉向汽車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模塊
按照公式進行搭建