LQG主动悬架 maltab simulink实现

clear all %% White Noise Tin=(0:0.0025:100)'; m=length(Tin); n=1; time=1:1:m; p=23; f=1024; ts=400; wr=wgn(m,n,p); %% parameter values %簧上质量 Mb=320; %簧下质量 Mw=40; %悬架弹簧刚度 Ks=2*10^4; %轮胎等效刚度 Kt=10*Ks; %悬架等效阻尼 Cs=1000; %路面不平度系数 G0=1024e-6; %前进速度 u0=20; %下截止频率 f0=0.1; %轮胎动位移加权系数 q1=100; %悬架动行程加权系数 q2=1000; %车身加速度加权系数 q3=0.05; %% Road excitation modeling Ard=-2*pi*f0;Brd=2*pi*sqrt(G0*u0);Crd=1;Drd=0; %% Lsim simulation for Road excitation sys_r=ss(Ard,Brd,Crd,Drd); Y_r=lsim(sys_r,wr,Tin); %% State space form for the Passive suspension moedel Ap=[-Cs/Mb,Cs/Mb,-Ks/Mb,Ks/Mb;Cs/Mw,-Cs/Mw,Ks/Mw,-(Kt+Ks)/Mw;1,0,0,0;0,1,0,0]; Bp=[0;Kt/Mw;0;0]; Cp=[-Cs/Mb,Cs/Mb,-Ks/Mb,Ks/Mb;0,0,0,1;0,0,1,-1]; Dp=[0;-1;0]; %% B, D Matrices for Active suspension(LQG) design Aa=[0,0,-Ks/Mb,Ks/Mb;0,0,Ks/Mw,-(Kt+Ks)/Mw;1,0,0,0;0,1,0,0]; Ba=[0,1/Mb;Kt/Mw,-1/Mw;0,0;0,0]; Ca=[0,0,-Ks/Mb,Ks/Mb;0,0,0,1;0,0,1,-1]; Da=[0,1/Mb;-1,0;0,0]; Q0=[q3,0,0;0,q1,0;0,0,q2]; Fb=Ba(:,2); Fd=Da(:,2); Q=Ca'*Q0*Ca;R=Fd'*Q0*Fd;N=Ca'*Q0*Fd; [Kr,S,E]=lqr(Aa,Fb,Q,R,N); %% Lsim simulation for passive suspension sys_p=ss(Ap,Bp,Cp,Dp); Y_p=lsim(sys_p,Y_r,Tin); accel_p=Y_p(:,1);dtl_p=Y_p(:,2);sws_p=Y_p(:,3); %% Lsim simulation for active suspension sys_a=ss(Aa-Ba(:,2)*Kr,Ba(:,1),Ca-Da(:,2)*Kr,Da(:,1)); Y_a=lsim(sys_a,Y_r,Tin); accel_a=Y_a(:,1);dtl_a=Y_a(:,2);sws_a=Y_a(:,3); %% Ploting the actvie results figure(1) subplot(2,2,1) plot(Tin,accel_p,'r',Tin,accel_a,'b'); legend('Passive','Active') title('Body Acceleration') xlabel('Time(sec)') ylabel('Body Acceleration(m/s^2)') grid on subplot(2,2,2) plot(Tin,dtl_p,'r',Tin,dtl_a,'b') legend('Passive','Active') title('Dynamic Tyre Displacement') xlabel('Time(sec)') ylabel('Dynamic Tyre Displacement(m)') grid on subplot(2,2,3) plot(Tin,sws_p,'r',Tin,sws_a,'b') legend('Passive','Active') title('Suspension Work Space') xlabel('Time(sec)') ylabel('Suspension Work Space(m)') grid on subplot(2,2,4) P=bodeoptions; P.FreqUnits='Hz'; P.Xlim={[0.1,100]}; P.Grid='on'; bodemag(sys_p,'r',sys_a,'b',P) legend('Passive','Active') %% comparison for different q3 %车身加速度加权系数 q3=0.01:0.01:0.1; figure(5) P=bodeoptions; P.FreqUnits='Hz'; P.Xlim={[0.1,100]}; P.Grid='on'; hold on bodemag(sys_p,'r',P); for i=1:10 Q0=[q3(i),0,0;0,q1,0;0,0,q2]; Q=Ca'*Q0*Ca;R=Fd'*Q0*Fd;N=Ca'*Q0*Fd; [Kr,S,E]=lqr(Aa,Fb,Q,R,N); C=Ca-Da(:,2)*Kr; sys_a=ss(Aa-Ba(:,2)*Kr,Ba(:,1),Ca-Da(:,2)*Kr,Da(:,1)); bodemag(sys_a(:,1),'b',P); end hold off