newmark方法.m

这是求多柔体转子纽马克法的程序。以拉格朗日方程为基础

h=1e-5;
n=3/h;
data=1/2;
alpha=1/4;
a0=1/(alpha*h^2);
a1=data/(alpha*h);
a2=1/(alpha*h);
a3=1/(2*alpha)-1;
a4=data/alpha-1;
a5=0;
a6=h*(1-data);
a7=data*h;
q(1:6,1)=[0;0;0;1;0;0];
dq(1:6,1)=[0;0;0;0;0;0];
ddq(1:6,1)=[5;0;0;0;0;0];
qq1=[11.25;0;0;0;0;0];
k1=[0,0,0,0,0,0;0,1.96923*10^8,0,0,0,0;0,0,6.8923*10^8/0.91,0,0,0;0,0,0,2422.86,107.683,1157.59;0,0,0,107.683,174.984,6.73016;
        0,0,0,1157.59,6.73016,753.778];
for i=1:n
    m=[10.8*(1/12+q(3,i)/12+q(3,i)*q(3,i)/12+121*q(4,i)*q(5,i)/1260+13*q(4,i)*q(5,i)/630+31*q(5,i)*q(5,i)/20160-13*q(4,i)*q(6,i)/315-...
            5*q(5,1)*q(6,i)/1008+5*q(6,i)*q(6,i)/1008),0,10.8*(4*q(4,i)/45+7*q(5,i)/720-7*q(6,i)/360),10.8*(-7/180-4*q(3,i)/45),...
            0.45*(-1/6-7*q(3,i)/30),0.9*(2/15+7*q(3,i)/30);
            0,0.9,0,0,0,0;
            10.8*(4*q(4,i)/45+7*q(5,i)/720-7*q(6,i)/360),0,0.9,0,0,0;
            10.8*(-7/180-4*q(3,i)/45),0,0,1.03714,0.11142,-0.222857;
            0.45*(-1/6-7*q(3,i)/30),0,0,0.11142,0.0166,-0.0267857;
            0.9*(2/15+7*q(3,i)/30),0,0,-0.222857,-0.0267857,0.053571];
    dm=[dq(3,i)*(0.9+1.8*q(3,i))+0.222857*dq(5,i)*q(4,i)-0.44571*dq(6,i)*q(4,i)+0.033214*dq(5,i)*q(5,i)-0.053571*dq(6,i)*q(5,i)+...
            dq(4,i)*(2.07429*q(4,i)+0.2228571*q(5,i)-0.44571*q(6,i))-0.053571*dq(5,i)*q(6,i)+0.10714*dq(6,i)*q(6,i),0,0.96*dq(4,i)+...
            0.105*dq(5,i)-0.21*dq(6,i),-0.96*dq(5,i),-0.105*dq(3,i),0.21*dq(3,i);
            0,0,0,0,0,0;
            1.44*dq(4,i)+0.1575*dq(5,i)-0.315*dq(6,i)-0.45*dq(1,i)-0.9*dq(1,i)*q(3,i),0,0,0.48*dq(1,i),0.0525*dq(1,i),-0.105*dq(1,i);
            -1.44*dq(4,i)+dq(1,i)*(-1.0371*q(4,i)-0.1143*q(5,i)+0.222857*q(6,i)),0,-0.48*dq(1,i),0,0,0;
            -0.1575*dq(3,i)+dq(1,i)*(-0.11143*q(4,i)-0.0166*q(5,i)+0.026786*q(6,i)),0,-0.0525*dq(1,i),0,0,0;
            0.315*dq(3,i)+dq(1,i)*(0.222857*q(4,i)+0.026786*q(5,i)-0.053571*q(6,i)),0,0.105*dq(1,i),0,0,0];
    k=k1+a0*m+a1*dm;
    qq(1:6,i+1)=qq1+m*(a0*q(1:6,i)+a2*dq(1:6,i)+a3*ddq(1:6,i))+dm*(a1*q(1:6,i)+a4*dq(1:6,i)+a5*ddq(1:6,i));
    q(1:6,i+1)=inv(k)*qq(1:6,i+1);
    ddq(1:6,i+1)=a0*(q(1:6,i+1)-q(1:6,i))-a2*dq(1:6,i)-a3*ddq(1:6,i);
    dq(1:6,i+1)=dq(1:6,i)+a6*ddq(1:6,i)+a7*ddq(1:6,i+1);
    i
end