用纽马克法解振动方程的一些问题

chx_hitsz

目前正在研究一个两柔性杆的机械臂振动问题，数学模型建好了，是非线性二阶微分方程组，用假设模态法进行离散，取前两阶模态，其中质量矩阵，阻尼矩阵（质量矩阵对时间的导数）都是随时间变化的。打算用纽马克法方法求解。可是总是不能得到与一篇论文上相同的结果（论文只给了结果图）……想请高人指点一二 不胜感激。以下是程序：
m1=10; %柔性杆1的质量
m2=10; %柔性杆2的质量
l1=1;  %杆1长度
l2=1;  %杆1长度
EI=2;  %抗弯刚度
gama=0.5;
delta=0.25;
dt=0.01;
b0=1/(delta*dt^2);
b1=gama/(delta*dt);
b2=1/(delta*dt);
b3=1/(2*delta)-1;
b4=gama/delta-1;
b5=(dt/2)*(gama/delta-2);
b6=dt*(1-gama);
b7=gama*dt;
p1=[1 0 0 0 0 0];
p2=[0 1 0 0 0 0];
p3=[0 0 1 0 0 0];
p4=[0 0 0 1 0 0];
p5=[0 0 0 0 1 0];
p6=[0 0 0 0 0 1];
t_max=10; %仿真时间
i=1;
t(1)=0.01;
q(:,1)=[pi/2;-pi/2;0;0;0;0]; %初始位置（杆1杆2与水平面的夹角和四个模态坐标）
dq(:,1)=[0;0;0;0;0;0];       %初始角速度
ddq(:,1)=[-360*pi;0;0;0;0;0];%角加速度
K=[0 0 0 0 0 0;0 0 0 0 0 0;0 0 (EI*pi^4)/(2*l1^3) 0 0 0;0 0 0 (8*EI*pi^4)/l1^3 0 0;0 0 0 0 (EI*pi^4)/(2*l2^3) 0;0 0 0 0 0 (8*EI*pi^4)/l2^3]; %刚度矩阵
Q1=zeros(6,6);
while t(i)<t_max
        m11=l1*(3*m1*(p3*q(:,i))^2+3*m1*(p4*q(:,i))^2+2*m1*l1^2+6*m2*l1*l2)/6;  %质量矩阵式随时间变化的
        m12=(l2*pi*cos((p1*q(:,i))-(p2*q(:,i)))+4*p5*q(:,i)*sin((p1*q(:,i))-(p2*q(:,i))))*m2*l1*l2/(2*pi);
        m13=l1^2*m1/pi;
        m14=-m13/2;
        m15=(2*m2*l1*l2*cos((p1*q(:,i))-(p2*q(:,i))))/pi;
        m16=0;
        m21=m12;m22=m2*l2*((p5*q(:,i))^2/2+(p6*q(:,i))^2/2+l2^2/3);m23=0;m24=0;m25=l2^2*m2/pi;m26=-m25/2;
        m31=m13;m32=m23;m33=m1*l1/2;m34=0;m35=0;m36=0;
        m41=m14;m42=m24;m43=m34;m44=m33;m45=0;m46=0;
        m51=m15;m52=m25;m53=m35;m54=m45;m55=m2*l2/2;m56=0;
        m61=m16;m62=m26;m63=m36;m64=m46;m65=m56;m66=m55;
    M=[m11 m12 m13 m14 m15 m16;m21 m22 m23 m24 m25 m26;m31 m32 m33 m34 m35 m36;m41 m42 m43 m44 m45 m46;m51 m52 m53 m54 m55 m56;m61 m62 m63 m64 m65 m66];
   dM=[m1*l1*(p3*q(:,i))*(p3*dq(:,i))+m1*l1*(p4*q(:,i))*(p4*dq(:,i)) m2*l1*l2*((p1*dq(:,i))-(p2*dq(:,i)))*(4*(p5*q(:,i))*cos((p1*q(:,i))-(p2*q(:,i)))-l2*pi*sin((p1*q(:,i))-(p2*q(:,i))))/(2*pi)+4*p5*dq(:,i)*sin((p1*q(:,i))-(p2*q(:,i)))*m2*l2*l1/(2*pi) 0 0 -2*m2*l1*l2*((p1*dq(:,i))-(p2*dq(:,i)))*sin((p1*q(:,i))-(p2*q(:,i)))/pi 0;
    m2*l1*l2*((p1*dq(:,i))-(p2*dq(:,i)))*(4*(p5*q(:,i))*cos((p1*q(:,i))-(p2*q(:,i)))-l2*pi*sin((p1*q(:,i))-(p2*q(:,i))))/(2*pi)+4*p5*dq(:,i)*sin((p1*q(:,i))-(p2*q(:,i)))*m2*l2*l1/(2*pi) m2*l2*(p5*q(:,i)*p5*dq(:,i)+p6*q(:,i)*p6*dq(:,i)) 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0;
    -2*m2*l1*l2*((p1*dq(:,i))-(p2*dq(:,i)))*sin((p1*q(:,i))-(p2*q(:,i)))/pi 0 0 0 0 0;0 0 0 0 0 0];
    K1=K+b0*M+b1*dM;
    [L,U]=schur(K1);
    tol1=-800*(pi+p1*q(:,i));tol2=1000*(pi+p2*q(:,i)-p1*q(:,i)); %关节处所加力矩是随角度变化的
        qv1=p1*dq(:,i)*p2*dq(:,i)*m2*l1*l2*(4*(p5*q(:,i))*cos((p1*q(:,i))-(p2*q(:,i)))-l2*pi*sin((p1*q(:,i))-(p2*q(:,i))))/(2*pi)-p5*dq(:,i)*p1*dq(:,i)*2*m2*l1*l2*sin((p1*q(:,i))-(p2*q(:,i)))/pi;
        qv2=-qv1;
        qv3=l1*m1*p3*q(:,i)*(p1*dq(:,i))^2/2;
        qv4=l1*m1*p4*q(:,i)*(p1*dq(:,i))^2/2;
        qv5=2*p1*dq(:,i)*p2*dq(:,i)*m2*l1*l2*sin((p1*q(:,i))-(p2*q(:,i)))/pi+m2*l2*p5*q(:,i)*(p2*dq(:,i))^2/2;
        qv6=(p2*dq(:,i))^2*m2*l2*p6*q(:,i)/2;% qv系列为与速度有关的二次力项
    Q(:,i)=[tol1-tol2+qv1;tol2+qv2;pi*tol1/l1+pi*tol2/l1+qv3;2*pi*tol1/l1-2*pi*tol2/l1+qv4;pi*tol2/l2+qv5;2*pi*tol2/l2+qv6];
    Q1=Q(:,i)+M*(b0*q(:,i)+b2*dq(:,i)+b3*ddq(:,i))+dM*(b1*q(:,i)+b4*dq(:,i)+b5*ddq(:,i));
    q(:,i+1)=L*U*L'\Q1;%QUESTION
    ddq(:,i+1)=b0*(q(:,i+1)-q(:,i))-b2*dq(:,i)-b3*ddq(:,i);
    dq(:,i+1)=dq(:,i)+b6*ddq(:,i)+b7*ddq(:,i+1);
    i=i+1;
    t(i)=t(i-1)+0.01;
end
总是得到理想的结果，这个程序可以运行。高手 专家看看我编的程序有什么问题没有，敬请指正。

chx_hitsz

怎么没人理啊 自己顶一下 希望大家帮帮忙啊