车辆二自由度动力学模型

close all;
clear all;
clc;

lf=1.463;
lr=1.585;
cf=-62618;
cr=-80185;
m=1818;
Iz=3885;
y_dot=0;
phi_dot=0;
vx=2;

ts=0.01;
delta=pi/20;
phi(1)=0;
X(1)=0;
Y(1)=0;
y_dot(1)=0;
phi_dot(1)=0;

for i=1:1:10/ts
y_dot2=y_dot(i)*(cf+cr)/(m*vx)+phi_dot(i)*(-vx+(cf*lf-cr*lr)/(m*vx))-cf/m*delta;
phi_dot2=y_dot(i)*(lf*cf-lr*cr)/(Iz*vx)+phi_dot(i)*(lf^2*cf+lr^2*cr)/(Iz*vx)-lf*cf/Iz*delta;

phi_dot(i+1)=phi_dot(i)+phi_dot2*ts;
y_dot(i+1)=y_dot(i)+y_dot2*ts;
phi(i+1)=phi(i)+phi_dot(i)*ts;

X(i+1)=X(i)+(vx*cos(phi(i))-y_dot(i)*sin(phi(i)))*ts;
Y(i+1)=Y(i)+(vx*sin(phi(i))+y_dot(i)*cos(phi(i)))*ts;
end
plot(X,Y);
plot(phi_dot);
plot(y_dot);