自动驾驶技术车道保持功能LQR控制算法建模与仿真

%%车辆参数 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; %%设置ABE矩阵 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]; %%设置权重系数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; %%LQR函数-K矩阵 [P,K]=lqr(A,B,Q,R); %%SYSR矩阵 F=A'-K*B*inv(R)*B'; SY=inv(F)*Q; SR=-inv(F)*K*E; %%中间参数 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); sy13=SY(1,3); sy23=SY(2,3); sr11=SR(1,1); sr21=SR(2,1);