h2.m

此程序为多体动力学中的重要计算程序。主要计算空间板的质量矩阵

h=1e-7;
alpha=1/2;
beta=1/4;
kb=18000;
a0=1/(beta*h^2);
a1=1/(beta*h);
a2=1/(2*beta)-1;
a3=alpha/(beta*h);
a4=alpha/beta-1;
a5=alpha/(2*beta)-1;

QF=[35200;1800;0;1000;0;0];
rou=7800;
l=9;
r0=0.5;







c=zeros(6);
c(1,1)=1600;
c(2,2)=1000;
E=2.1e8;
Iz=6726;
sa=0.01;
Iy=10000;
k=zeros(6);
k(2,2)=kb;
k(3,3)=E*sa/l;
k(4,4)=12*E*Iz/l^3;
k(4,6)=-6*E*Iz/l^2;
k(5,5)=4*E*Iy/l;

k(6,6)=4*E*Iz/l;
k(6,4)=k(4,6);

n=0.01/h;

q=zeros(6,n);
dq=zeros(6,n);
ddq=zeros(6,n);

q(1:6,2)=[0;0.1;0;0;0;0];
dq(1:6,2)=[22;0;0;0;0;0];
ddq(1:6,2)=[1.0549;-0.4056;0;3.1130;-8.6227;0.0105];

q(1:6,1)=q(1:6,2)-h*dq(1:6,2)+h^2/2*ddq(1:6,2);

for i=2:n

m=zeros(6);
m(1,1)=16357785600/7*sin(q(2,i))^2*(-1/256*q(4,i)+1/64*q(6,i))^2+162240/7*q(5,i)^2+681574400*sin(q(2,i))^2*(3/64*q(4,i)-1/8*q(6,i))*(-1/256*q(4,i)+1/64*q(6,i))-102236160*cos(q(2,i))*(1+1/8*q(3,i))*sin(q(2,i))*(-1/256*q(4,i)+1/64*q(6,i))[phone]*sin(q(2,i))^2*(3/64*q(4,i)-1/8*q(6,i))^2-7987200*sin(q(2,i))*(-1/256*q(4,i)+1/64*q(6,i))-15974400*cos(q(2,i))*(1+1/8*q(3,i))*sin(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))-1331200*sin(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))+1331200*cos(q(2,i))^2*(1+1/8*q(3,i))^2+249600*cos(q(2,i))*(1+1/8*q(3,i))+15600;
m(1,2)=47923200/7*q(5,i)*cos(q(2,i))*(-1/256*q(4,i)+1/64*q(6,i))+1064960*q(5,i)*cos(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))+174720*q(5,i)*sin(q(2,i))*(1+1/8*q(3,i));
m(1,3)=-21840*q(5,i)*cos(q(2,i));
m(1,4)=162240/7*q(5,i)*sin(q(2,i));
m(1,5)=-47923200/7*sin(q(2,i))*(-1/256*q(4,i)+1/64*q(6,i))-1064960*sin(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))+174720*cos(q(2,i))*(1+1/8*q(3,i))+15600;
m(1,6)=-183040/7*q(5,i)*sin(q(2,i));
m(2,2)=16357785600/7*(-1/256*q(4,i)+1/64*q(6,i))^2+681574400*(3/64*q(4,i)-1/8*q(6,i))*(-1/256*q(4,i)+1/64*q(6,i))[phone]*(3/64*q(4,i)-1/8*q(6,i))^2+1331200*(1+1/8*q(3,i))^2;
m(2,3)= -21840*q(4,i)+24960*q(6,i);
m(2,4)=174720+21840*q(3,i);
m(2,6)=-199680-24960*q(3,i);
m(3,3)=20800;
m(4,4)=162240/7;
m(4,6)=-183040/7;
m(5,5)=162240/7;
m(6,6)=266240/7;
m(2,1)=m(1,2);
m(3,1)=m(1,3);
m(3,2)=m(2,3);
m(4,1)=m(1,4);
m(4,2)=m(2,4);
m(5,1)=m(1,5);
m(5,2)=m(2,5);
m(5,3)=m(3,5);
m(5,4)=m(4,5);
m(6,1)=m(1,6);
m(6,2)=m(2,6);
m(6,3)=m(3,6);
m(6,4)=m(4,6);
m(6,5)=m(5,6);




 

dm=zeros(6);

dm(1,1)=32715571200/7*sin(q(2,i))*(-1/256*q(4,i)+1/64*q(6,i))^2*cos(q(2,i))[phone]*sin(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))*(-1/256*q(4,i)+1/64*q(6,i))*cos(q(2,i))[phone]*sin(q(2,i))^2*(1+1/8*q(3,i))*(-1/256*q(4,i)+1/64*q(6,i))-102236160*cos(q(2,i))^2*(1+1/8*q(3,i))*(-1/256*q(4,i)+1/64*q(6,i))[phone]*sin(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))^2*cos(q(2,i))-7987200*cos(q(2,i))*(-1/256*q(4,i)+1/64*q(6,i))[phone]*sin(q(2,i))^2*(1+1/8*q(3,i))*(3/64*q(4,i)-1/8*q(6,i))-15974400*cos(q(2,i))^2*(1+1/8*q(3,i))*(3/64*q(4,i)-1/8*q(6,i))-1331200*cos(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))-2662400*cos(q(2,i))*(1+1/8*q(3,i))^2*sin(q(2,i))-249600*sin(q(2,i))*(1+1/8*q(3,i))-12779520*cos(q(2,i))*sin(q(2,i))*(-1/256*q(4,i)+1/64*q(6,i))-1996800*cos(q(2,i))*sin(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))+332800*cos(q(2,i))^2*(1+1/8*q(3,i))+31200*cos(q(2,i))[phone]/7*sin(q(2,i))^2*(-1/256*q(4,i)+1/64*q(6,i))+49920*cos(q(2,i))*(1+1/8*q(3,i))*sin(q(2,i))+10400*sin(q(2,i))+324480/7*q(5,i);
dm(1,2)=-47923200/7*q(5,i)*sin(q(2,i))*(-1/256*q(4,i)+1/64*q(6,i))-1064960*q(5,i)*sin(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))+174720*q(5,i)*cos(q(2,i))*(1+1/8*q(3,i))+21840*q(5,i)*sin(q(2,i))-20800/7*q(5,i)*cos(q(2,i))[phone]/7*cos(q(2,i))*(-1/256*q(4,i)+1/64*q(6,i))+1064960*cos(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))+174720*sin(q(2,i))*(1+1/8*q(3,i));
 
dm(1,3)=21840*q(5,i)*sin(q(2,i))-21840*cos(q(2,i));
dm(1,4)=162240/7*q(5,i)*cos(q(2,i))+162240/7*sin(q(2,i));
dm(1,5)=-47923200/7*cos(q(2,i))*(-1/256*q(4,i)+1/64*q(6,i))-1064960*cos(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))-174720*sin(q(2,i))*(1+1/8*q(3,i))+21840*cos(q(2,i))+20800/7*sin(q(2,i));
dm(1,6)=-183040/7*q(5,i)*cos(q(2,i))-183040/7*sin(q(2,i));
 

dm(2,2)=332800+41600*q(3,i)-41600/7*q(4,i)+166400/7*q(6,i);
dm(2,3)=3120;
dm(2,4)=21840;
dm(2,6)=-24960;

dm(2,1)=dm(1,2);
dm(3,1)=dm(1,3);
dm(3,2)=dm(2,3);
dm(4,1)=dm(1,4);
dm(4,2)=dm(2,4);
dm(5,1)=dm(1,5);
dm(5,2)=dm(2,5);
dm(6,1)=dm(1,6);
dm(6,2)=dm(2,6);

mt=zeros(6);
mt(1,1)=(32715571200/7*sin(q(2,i))*(-1/256*q(4,i)+1/64*q(6,i))^2*cos(q(2,i))[phone]*sin(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))*(-1/256*q(4,i)+1/64*q(6,i))*cos(q(2,i))[phone]*sin(q(2,i))^2*(1+1/8*q(3,i))*(-1/256*q(4,i)+1/64*q(6,i))-102236160*cos(q(2,i))^2*(1+1/8*q(3,i))*(-1/256*q(4,i)+1/64*q(6,i))[phone]*sin(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))^2*cos(q(2,i))-7987200*cos(q(2,i))*(-1/256*q(4,i)+1/64*q(6,i))[phone]*sin(q(2,i))^2*(1+1/8*q(3,i))*(3/64*q(4,i)-1/8*q(6,i))-15974400*cos(q(2,i))^2*(1+1/8*q(3,i))*(3/64*q(4,i)-1/8*q(6,i))-1331200*cos(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))-2662400*cos(q(2,i))*(1+1/8*q(3,i))^2*sin(q(2,i))-249600*sin(q(2,i))*(1+1/8*q(3,i)))*dq(2,i)+(-12779520*cos(q(2,i))*sin(q(2,i))*(-1/256*q(4,i)+1/64*q(6,i))-1996800*cos(q(2,i))*sin(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))+332800*cos(q(2,i))^2*(1+1/8*q(3,i))+31200*cos(q(2,i)))*dq(3,i)+(95846400/7*sin(q(2,i))^2*(-1/256*q(4,i)+1/64*q(6,i))+2129920*sin(q(2,i))^2*(3/64*q(4,i)-1/8*q(6,i))-349440*cos(q(2,i))*(1+1/8*q(3,i))*sin(q(2,i))-31200*sin(q(2,i)))*dq(4,i)+324480/7*q(5,i)*dq(5,i)+(-85196800/7*sin(q(2,i))^2*(-1/256*q(4,i)+1/64*q(6,i))-2129920*sin(q(2,i))^2*(3/64*q(4,i)-1/8*q(6,i))+399360*cos(q(2,i))*(1+1/8*q(3,i))*sin(q(2,i))+41600*sin(q(2,i)))*dq(6,i);
mt(1,2)=(-47923200/7*q(5,i)*sin(q(2,i))*(-1/256*q(4,i)+1/64*q(6,i))-1064960*q(5,i)*sin(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))+174720*q(5,i)*cos(q(2,i))*(1+1/8*q(3,i)))*dq(2,i)+21840*q(5,i)*sin(q(2,i))*dq(3,i)+162240/7*q(5,i)*cos(q(2,i))*dq(4,i)+(47923200/7*cos(q(2,i))*(-1/256*q(4,i)+1/64*q(6,i))+1064960*cos(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))+174720*sin(q(2,i))*(1+1/8*q(3,i)))*dq(5,i)-183040/7*q(5,i)*cos(q(2,i))*dq(6,i);
mt(1,3)=9360*dq(2,i)*q(5,i)*sin(q(2,i))-9360*dq(5,i)*cos(q(2,i));
mt(1,4)=56160/7*dq(2,i)*q(5,i)*cos(q(2,i))+56160/7*dq(5,i)*sin(q(2,i));
mt(1,5)=dq(2,i)*(-7987200/7*cos(q(2,i))*(-1/256*q(4,i)+1/64*q(6,i))-266240*cos(q(2,i))*(3/64*q(4,i)-1/8*q(6,i))-74880*sin(q(2,i))*(1+1/8*q(3,i)))+9360*dq(3,i)*cos(q(2,i))-56160/7*dq(4,i)*sin(q(2,i))+108160/7*dq(6,i)*sin(q(2,i));

mt(2,2)=(332800+41600*q(3,i))*dq(3,i)+(324480/7*q(4,i)-366080/7*q(6,i))*dq(4,i)+(-366080/7*q(4,i)+532480/7*q(6,i))*dq(6,i);
mt(2,3)=-9360*dq(4,i)+16640*dq(6,i);
mt(2,4)=9360*dq(3,i);


mt(2,1)=mt(1,2);
mt(3,1)=mt(1,3);
mt(4,1)=mt(1,4);
mt(5,1)=mt(1,5);
mt(3,2)=mt(2,3);
mt(4,2)=mt(2,4);

EQ(1:6,i+1)=QF+1/2*dq(1:6,i)'*dm*dq(1:6,i)-(k-(2*m)/h^2)*q(1:6,i)-(m/h^2-(c+mt)/(2*h))*q(1:6,i-1);

q(1:6,i+1)=inv(m/h^2+(c+mt)/(2*h))*EQ(1:6,i+1);


dq(1:6,i+1)=a3*(q(1:6,i+1)-q(1:6,i))-a4*dq(1:6,i)-a5*ddq(1:6,i);

ddq(1:6,i+1)=a0*(q(1:6,i+1)-q(1:6,i))-a1*dq(1:6,i)-a2*ddq(1:6,i);


    
    



i
end