wyb.m

倒立双摆动matlab源代码

clear all;
h=0.1;t=0.1;N=15000;
a1=1;a2=0.5;a3=2;
r1=1;r2=1;r3=1;
xm10=0;xm20=0;xm30=0;
xp10=0;xp20=0;xp30=0;
u0=0;k0=0;
f10=0;f20=0;f30=0;
e10=0;e20=0;e30=0
for i=1:N
    xm1=xm10+h*xm20;xm2=xm20+h*xm30;xm3=xm30+h*(-xm10-2*xm20-3*xm30+sin(2*t));
    Xm1(i)=xm1;Xm2(i)=xm2;Xm3(i)=xm3;
    xp1=xp10+h*xp20;xp2=xp20+h*xp30;xp3=xp30+h*(a1*xp10+a2*xp20+a3*xp30+u0);
    Xp1(i)=xp1;Xp2(i)=xp2;Xp3(i)=xp3;
    e1=Xm1(i)-Xp1(i);e2=Xm2(i)-Xp2(i);e3=Xm3(i)-Xp3(i);
    E1(i)=e1; E2(i)=e2; E3(i)=e3;
    k=k0+h*(0.5*e10+13*e20/10+3*e30/5)*sin(2*t);
    f1=f10+h*(0.5*e10+13*e20/10+3*e30/5)*r1*xp1;
    f2=f20+h*(0.5*e10+13*e20/10+3*e30/5)*r2*xp2;
    f3=f30+h*(0.5*e10+13*e20/10+3*e30/5)*r3*xp3;
    u=k*sin(2*t)+f1*xp1+f2*xp2+f3*xp3;
    xm10=xm1;xm20=xm2;xm30=xm3;
    xp10=xp1;xp20=xp2;xp30=xp3;
    k0=k;u0=u;
    f10=f1;f20=f2;f30=f3;
    e10=e1; e20=e2; e30=e3;
    tt(i)=t;t=t+h;
end
figure(1);
plot(tt,Xm1,'r');
hold on;
plot(tt,Xp1);
title('Xm1与Xp1的响应曲线');
figure(2);
plot(tt,Xm2,'r');
hold on;
plot(tt,Xp2);
title('Xm2与Xp2的响应曲线');
figure(3);
plot(tt,Xm3,'r');
hold on;
plot(tt,Xp3);
title('Xm3与Xp3的响应曲线');
figure(4);
plot(tt,E1);
title('E1的响应曲线');
figure(5);
plot(tt,E2);
title('E2的响应曲线');
figure(6);
plot(tt,E3);
title('E3的响应曲线');