刚体绕瞬心运动的转动方程
是这样的，我们做这个关于刚体绕瞬心运动的内容，用长为2l,质量为m的木杆斜靠在光滑的墙壁与地面之间，在重力作用下做下滑运动，将木杆的运动分成了两个阶段讨论。
这下面是第一个阶段的代码，但是运行出来不是想要的内容。原本是根据书上的，但是不对，这是自己改过的，还是运行的不是自己想要的图，想问下有没有人帮忙看一下，问题出在了哪里。
```%定义x=y(1) dx/dt=y(2) y=y(3) dy/dt=y(4) theta=y(5) d theta/dt =y(6)
M=5;w=0.00;l=4;g=9.81 ; %定义木杆的质量M，加加速度w，杆长l，重力加速度g
Ic=M*1^2/3; %对质心的转动惯量
dt=0.001;
tspan=(0:dt:2) ; %定义变量的积分范围，上下限
y0=[-l*cos(2*pi/3),0,l*sin(2*pi/3),0,(2*pi/3),0] ; %初始条件***********书上存在错误
options=odeset('Events',@events); %开启事件判断功能
[T,Y,TE]=ode45(@fy4,tspan,y0,options);
plot(T,Y,'r-');
title('方程解的关系图')
figure;
N=length(T)-1 ; t=T(2:N); %解方程中的时间变量
xc=Y(2:N,1); v_xc=Y(2:N,2); %质心坐标与速度
yc=Y(2:N,3); v_yc=Y(2:N,4);
fy=Y(2:N,5); v_fy=Y(2:N,6); %转角与角速度
xs=xc-v_yc./v_fy; %瞬心坐标
ys=yc+v_xc./v_fy;
subplot(2,2,1)
plot(xc,yc,'r-',xs,ys,'k-'); %质心轨迹和瞬心轨迹
axis auto
xlabel('x') %给坐标轴命名
ylabel('y')
title('木棍沿墙下滑运动图——前半段'); %图形标题
legend(['质心轨迹';'瞬心轨迹']); %添加图例
hold on
for k=2:50:N %通过角度改变确定木棍的位置***********通过改变步长来详细描述木杆的变化
x_a=xc(k)+l*cos(fy(k));
x_b=xc(k)-l*cos(fy(k));
y_a=yc(k)+l*sin(fy(k));
y_b=0;
pause(0.5)
plot([x_a,x_b], [y_a,0],xc(k),yc(k),'o');
end
dxs=gradient(xs,dt); %求xs的梯度，求瞬心随时间的变化率
dys=gradient(ys,dt);
p=1:200:N;
quiver(xs(p),ys(p),dxs(p),dys(p)); %以200为步长选取数据绘制瞬心变化率的矢量图
subplot(2,2,2)
Is=Ic+M*((xs-xc).^2+(ys-yc).^2); %转动惯量
plot(t,Is); %绘制转动惯量Is与时间t的变化曲线
axis auto
xlabel('t'); %给坐标轴命名
ylabel('Is');
title('Is随时间变化图——前半段') %编辑图形表头
subplot(2,2,3) %选择画图位置
px=M.*v_xc;
py=M.*v_yc; %动量
dd=(dxs.*py-dys.*px);
plot(t,dd);
axis([0,1.4,-60,90]);
xlabel('t');
ylabel('dR/dt \times P')
title('dR/dt \times P P变化图——前半段'); %图形标题
subplot(2,2,4); %选择画图位置
axis auto
hold on
title('方言公式验证图——前半段') %编辑图形表头
xlabel('t');
ylabel('y'); %给坐标轴名命
V=1:70:N; %取部分数据画图
tt=t(V);
dfr=M*g.*(xs-xc);
dfr=dfr(V); %外力矩和（右边第一项）
left = dfr-dd(V); %方言公式等号右边
dfl = gradient(Is.*v_fy,dt);
right =dfl(V); %方言公式等号左边
plot(tt,left, tt,right,'o', tt,dfr,'*'); %绘制图像
legend('公式左边','公式右边','外力矩和');
TE;
y_end=Y(end,:); %输出运动第二阶段的初始条件
function f= fy4(t,y) %待解的常微分方程
l=4;g=9.81;
f=[y(2);
-3*g*cos(y(5))*sin(y(5))/4+1*cos(y(5))*y(6)^2;
y(4);
-3*g*cos(y(5))^2/4-l*sin(y(5))*y(6)^2;
y(6);
-3*g*cos(y(5))/(4*l) ];
end
function [value,isterminal,direction] = events(T,Y)
l=4;g=9.81;
value=-3*g*cos(Y(5))*sin(Y(5))/4+1*cos(Y(5))*Y(6)^2;
isterminal = 1; %表示当value=0时停止积分
direction = []; %不考虑变量的变化方向
end
这下面是书上的内容
书上的错误很多，上面的代码是自己修改过后的，好像还是不太对，有没有人帮忙看看这块内容呢