三自由度汽车操稳性动力学模型

%%%%三自由度解函数
function [t,wr,B,p,n]=solve3dot(m,ms,a,b,C1,C2,D1,D2,hs,k1,k2,E1,E2,Ix,Iz,N1,N2,Ixz,u,g,qt,q,t2,t3)%%t2为仿真采样周期，t3为采样终止时间
M=[Iz 0 Ixz 0;0 m*u -ms*hs 0;Ixz -ms*hs*u Ix 0;0 0 0 1];
%%矩阵C
Nr=-2*(k1*a^2+k2*b^2)/u;Nb=2*(-a*k1+b*k2+N1+N2);Nu=2*(-b*E2*k2+a*E1*k1);Yr=2*(-a*k1+b*k2)/u;
Yb=-2*(k1+k2);Yu=2*(k1*E1+k2*E2);Lp=-(D1+D2);Lu=-(C1+C2-ms*g*hs);
C=[Nr Nb 0 Nu;Yr-m*u Yb 0 Yu;ms*hs*u 0 Lp Lu;0 0 1 0];
%% 矩阵n
Nq=2*(a*k1-N1);Yq=2*k1;
n=[Nq;Yq;0;0];
%% 
tspan=0:t2:t3;%%%%仿真时间矩阵
condition=[0;0;0;0];%%%%仿真初始条件
[t,X]=ode45(@(t,X) f(t,X,qt,q,M,C,n),tspan,condition);
X=transpose(X);%坐标修复
t=transpose(t);
wr=X(1,:);%横摆角速度转化为一维矩阵
B=X(2,:);%质心侧偏角转化为一维矩阵
p=X(3,:);%侧倾角变化率转化为一维矩阵
n=X(4,:);%侧倾角转化为一维矩阵
end
function equation=f(t,X,qt,q,M,C,n)
%X=[wr;angle1;n2;n1];
q=interp1(qt,q,t);
equation=M\C*X+M\n*q;