ode45求解问题

ukman

用ode45求解一个7自由度系统二阶运动微分方程，结果求出来的位移和速度数量级居然为10e304。激励我是随便选的0.001*sin(100*t），其他参数应该没什么问题。目标函数如下：
function dq=Est(t,q)
global  H I1 I4 I5 tha10 tha20 tha30 tha40 tha50  m1 m2 m3 m4 m5 I2 I3 r1 r2 r3 rc4 k1f c1f k1v c1v k4f c4f k4v c4v kt1 ct1 kt2 ct2 kt3 ct3 kt4 ct4 g;
%global value,accounting for different mass ,height and sitting postures
stha10=sind(tha10);stha20=sind(tha20);stha30=sind(tha30);stha40=sind(tha40);stha50=sind(tha50);
ctha10=cosd(tha10);ctha20=cosd(tha20);ctha30=cosd(tha30);ctha40=cosd(tha40);ctha50=cosd(tha50);
ctha1020=cosd(tha10-tha20);ctha1030=cosd(tha10-tha30);ctha2030=cosd(tha20-tha30);ctha4050=cosd(tha40-tha50);
l1=0.06*H;l2=0.06*H;l4=0.2*H;l5=0.21*H;r4=0.4278*l4;r5=0.4284*l5;

%q=[q1,q2,q3,q4,q5,q6,q7,q1dot,q2dot,q3dot,q4dot,q5dot,q6dot,q7dot];
dq=zeros(14,1);
dq(1)=q(8);
dq(2)=q(9);
dq(3)=q(10);
dq(4)=q(11);
dq(5)=q(12);
dq(6)=q(13);
dq(7)=q(14);

dq(8)=((m1*r1*stha10+m2*l1*stha10+m3*l1*stha10)*dq(10)+(m2*r2*stha20+m3*l2*stha20)*dq(11)+m3*r3*stha30*dq(12)...
    -(m4*r4*stha40+m5*l4*stha40)*dq(13)-m5*r5*stha50*dq(14)+(c1f+c4f)*q(8)+(k1f+k4f)*q(1)-k4f*rc4*stha40*q(6)-c4f*rc4*stha40*q(13))/(m1+m2+m3+m4+m5);
dq(9)=g+((-m1*r1*ctha10-m2*l1*ctha10-m3*l1*ctha10)*dq(10)-(m2*r2*ctha20+m3*l2*ctha20)*dq(11)-m3*r3*ctha30*dq(12)...
    +(m4*r4*ctha40+m5*l4*ctha40)*dq(13)+m5*r5*ctha50*dq(14)+(c1v+c4v)*q(9)+(k1v+k4v)*q(2)+k4v*rc4*ctha40*q(6)+c4v*rc4*ctha40*q(13)...
    -k1v*0.001*sin(100*t)-c1v*0.005*cos(100*t)-k4v*0.001*sin(100*t)-c4v*0.005*cos(100*t))/(m1+m2+m3+m4+m5);
dq(10)=((m1*r1*stha10+m2*l1*stha10+m3*l1*stha10)*dq(8)-(m1*r1*ctha10+m2*l1*ctha10+m3*l1*ctha10)*dq(9)+(m2*r2*l1*ctha1020+m3*l1*l2*ctha1020)*dq(11)+m3*r3*l1*ctha1030*dq(12)...
    +kt1*(q(3)+q(6))+ct1*(q(10)+q(13))-(m2+m3)*g*l1*ctha10-m1*g*r1*ctha10)/(I1+m1*r1^2+m2*l1^2+m3*l1^2);
dq(11)=((m2*r2*stha20+m3*l2*stha20)*dq(8)-(m2*r2*ctha20+m3*l2*ctha20)*dq(9)+(m2*r2*l1*ctha1020...
    +m3*l1*l2*ctha1020)*dq(10)+m3*r3*l2*ctha2030*dq(12)+kt2*q(4)+ct2*q(11)-m3*g*l2*ctha20-m2*g*r2*ctha20)/(I2+m2*r2^2+m3*l2^2);
dq(12)=(m3*r3*stha30*dq(8)-m3*r3*ctha30*dq(9)+m3*r3*l1*ctha1030*dq(10)+m3*r3*l2*ctha2030*dq(11)+kt3*q(5)+ct3*q(12))/(I3+m3*r3^2);
dq(13)=((m4*r4*stha40+m5*l4*stha40)*dq(8)-(m4*r4*ctha40+m5*l4*ctha40)*dq(9)+m5*r5*l4*ctha4050-k4f*rc4*stha40*q(1)+k4v*rc4*ctha40*q(2)+kt1*(q(3)+q(6))...
    +(-k4f*rc4^2*stha40^2+k4v*rc4^2*ctha40^2)*q(6)-c4f*rc4*stha40*q(8)+c4v*rc4*ctha40*q(9)...
    +(c4f*rc4^2*stha40^2+c4v*rc4^2*ctha40^2+ct1)*q(13)+ct1*q(10)-k4v*rc4*ctha40*0.001*sin(100*t)-c4v*rc4*ctha40*0.005*cos(100*t)+m5*g*l4*ctha40+m4*g*r4*ctha40)/(I4+m4*r4^2+m5*l4^2);
dq(14)=(m5*r5*stha50*dq(8)+m5*r5*ctha50*dq(9)+m5*r5*l4*ctha4050*dq(13)+kt4*q(7)+ct4*q(14)+m5*g*r5*ctha50)/(I5+m5*r5^2);
end
主函数为：
global  H I1 I4 I5 tha10 tha20 tha30 tha40 tha50  m1 m2 m3 m4 m5 I2 I3 r1 r2 r3 rc4 g k1f c1f k1v c1v k4f c4f k4v c4v kt1 ct1 kt2 ct2 kt3 ct3 kt4 ct4;
H=1.74;I1=0.0263;I4=0.348*1.5;I5=0.4;tha10=10;tha20=90;tha30=90;tha40=10;tha50=280;g=9.81;
m1=5.52;m2=15;m3=23.5;m4=15;m5=10.9;
k1v=8625;c1v=1383;k1f=1405;c1f=903;k4v=15140;c4v=2420;k4f=1214;c4f=114;
kt1=100;ct1=20;kt2=128;ct2=165;kt3=328;ct3=1524;kt4=92;ct4=50;
r1=0.073;r2=0.167;r3=0.21;rc4=0.31;I2=0.287;I3=0.4;

ic=[0,0,0,0,0,0,0,0,0,0,0,0,0,0];
[T,Q]=ode45(@Est,[0:0.01:3],ic)
要命的是也不提示错误在哪里，结果全是10e305*0.0000或者NaN。