求高手看看这个程序，运行很慢，不出结果不出图，错误原因看不懂
clearq01=1; q02=1; q03=1; q04=1; q05=1; q06=1;
q1i=ones(35,1);
q2i=ones(35,1);
q3i=ones(35,1);
q4i=ones(35,1);
q5i=ones(35,1);
q1d1=ones(35,1);
q2d1=ones(35,1);
q3d1=ones(35,1);
q4d1=ones(35,1);
q5d1=ones(35,1);
q1d2=ones(35,1);
q2d2=ones(35,1);
q3d2=ones(35,1);
q4d2=ones(35,1);
q5d2=ones(35,1);
q1d=ones(151,1);
q2d=ones(151,1);
tt=0:0.01:1;
for j=1:length(tt)
q=;%放在循环里面
t=tt(j);
u1=0.5;u2=0.25;
dt=0.01;%时间步长取0.01
d0=1/(u2*dt^2);d1=u1/(u2*dt);d2=1/(u2*dt);d3=1/(2*u2)-1;d4=u1/u2-1;d5=(dt/2)*(u1/u2-2);d6=dt*(1-u1);d7=u1*dt;
L1i=0.76;L2i=0.88;%L1i摆动杆长度 L2i伸缩杆长度
g=9.8;
xb=980+t; yb=t; zb=t;a=0;b=0;
a1=0;b1=0;a2=0;b2=0;xb1=1;xb2=0;yb1=1;yb2=0;zb1=1;zb2=0;
w=-717.07;
rA=645;
rB=202;
rsx=-59.11;
rsz=-228.4;
Pu1=.';
Pu2=.';
Pu3=.';
Pu4=.';
Pu5=.';
Ps1=.';
Ps2=.';
Ps3=.';
Ps4=.';
Ps5=.';%3*1矩阵
R=;%3*3矩阵
Po=.';%3*1矩阵
L1=R*Ps1+Po-Pu1;%3*1矩阵
L2=R*Ps2+Po-Pu2;
L3=R*Ps3+Po-Pu3;
L4=R*Ps4+Po-Pu4;
L5=R*Ps5+Po-Pu5;%3*1矩阵
s1=R*Ps1+Po;%此处是求解动平台上五个铰链点的矢量坐标以方便以后使用
s2=R*Ps2+Po;
s3=R*Ps3+Po;
s4=R*Ps4+Po;
s5=R*Ps5+Po;
xs1=s1(1,1);ys1=s1(2,1);zs1=s1(3,1);
xs2=s2(1,1);ys2=s2(2,1);zs2=s2(3,1);
xs3=s3(1,1);ys3=s3(2,1);zs3=s3(3,1);
xs4=s4(1,1);ys4=s4(2,1);zs4=s4(3,1);
xs5=s5(1,1);ys5=s5(2,1);zs5=s5(3,1);
l1x=rsx*cos(a)*cos(b)+rsz*sin(a)+xb;
l1x1=-rsx*sin(a)*cos(b)*a1-rsx*cos(a)*sin(b)*b1+rsz*cos(a)*a1+xb1;
l1x2=-rsx*cos(a)*cos(b)*a1^2+2*rsx*sin(a)*sin(b)*a1*b1-rsx*sin(a)*cos(b)*a2-rsx*cos(a)*cos(b)*b1^2-rsx*cos(a)*sin(b)*b2-rsz*sin(a)*a1^2+rsz*cos(a)*a2+xb2;
l1y=rsx*sin(a)*cos(b)-rsz*cos(a)-w+yb;
l1y1=rsx*cos(a)*cos(b)*a1-rsx*sin(a)*sin(b)*b1+rsz*sin(a)*a1+yb1;
l1y2=-rsx*sin(a)*cos(b)*a1^2-2*rsx*cos(a)*sin(b)*a1*b1+rsx*cos(a)*cos(b)*a2-rsx*sin(a)*cos(b)*b1^2-rsx*sin(a)*sin(b)*b2+rsz*cos(a)*a1^2+rsz*sin(a)*a2+yb2;
l1z=-rsx*sin(b)+zb;
l1z1=-rsx*cos(b)*b1+zb1;
l1z2=-rsx*cos(b)*b2+rsx*sin(b)*b1^2+zb2;
l1=(l1x^2+l1y^2+l1z^2)^0.5;
l1d1=(1/l1)*(l1x*l1x1+l1y*l1y1+l1z*l1z1);%支链杆1的1次导数
l1d2=(-l1d1/(l1^2))*(l1x*l1x1+l1y*l1y1+l1z*l1z1)+(1/l1)*(l1x1*l1x1+l1x*l1x2+l1y1*l1y1+l1y*l1y2+l1z1*l1z1+l1z*l1z2);%支链杆1的2次导数
l2x=rB*sin(2*pi/5)*cos(a)*sin(b)-rB*cos(2*pi/5)*sin(a)+xb;
l2x1=-rB*sin(2*pi/5)*sin(a)*sin(b)*a1+rB*sin(2*pi/5)*cos(a)*cos(b)*b1-rB*cos(2*pi/5)*cos(a)*a1+xb1;
l2x2=-rB*sin(2*pi/5)*cos(a)*sin(b)*a1^2-2*rB*sin(2*pi/5)*sin(a)*cos(b)*a1*b1-rB*sin(2*pi/5)*sin(a)*sin(b)*a2-rB*sin(2*pi/5)*cos(a)*sin(b)*b1^2+rB*sin(2*pi/5)*cos(a)*cos(b)*b2+rB*cos(2*pi/5)*sin(a)*a1^2-rB*cos(2*pi/5)*cos(a)*a2+xb2;
l2y=rB*sin(2*pi/5)*sin(a)*sin(b)+rB*cos(2*pi/5)*cos(a)+yb-rA*cos(pi/4);
l2y1=rB*sin(2*pi/5)*cos(a)*sin(b)*a1+rB*sin(2*pi/5)*sin(a)*cos(b)*b1-rB*cos(2*pi/5)*sin(a)*a1+yb1;
l2y2=-rB*sin(2*pi/5)*sin(a)*sin(b)*a1^2+2*rB*sin(2*pi/5)*cos(a)*cos(b)*a1*b1+rB*sin(2*pi/5)*cos(a)*sin(b)*a2-rB*sin(2*pi/5)*sin(a)*sin(b)*b1^2+rB*sin(2*pi/5)*sin(a)*cos(b)*b2-rB*cos(2*pi/5)*cos(a)*a1^2-rB*cos(2*pi/5)*sin(a)*a2+yb2;
l2z=rB*sin(2*pi/5)*cos(b)+zb-rA*sin(pi/4);
l2z1=-rB*sin(2*pi/5)*sin(b)*b1+zb1;
l2z2=-rB*sin(2*pi/5)*cos(b)*b1^2-rB*sin(2*pi/5)*sin(b)*b2+zb2;
l2=(l2x^2+l2y^2+l2z^2)^(1/2);
l2d1=(1/l2)*(l2x*l2x1+l2y*l2y1+l2z*l2z1);%支链杆2的1次导数
l2d2=(-l2d1/(l2^2))*(l2x*l2x1+l2y*l2y1+l2z*l2z1)+(1/l2)*(l2x1*l2x1+l2x*l2x2+l2y1*l2y1+l2y*l2y2+l2z1*l2z1+l2z*l2z2);%支链杆2的2次导数
l3x=rB*sin(4*pi/5)*cos(a)*sin(b)-rB*cos(4*pi/5)*sin(a)+xb;
l3x1=-rB*sin(4*pi/5)*sin(a)*sin(b)*a1+rB*sin(4*pi/5)*cos(a)*cos(b)*b1-rB*cos(4*pi/5)*cos(a)*a1+xb1;
l3x2=-rB*sin(4*pi/5)*cos(a)*sin(b)*a1^2-2*rB*sin(4*pi/5)*sin(a)*cos(b)*a1*b1-rB*sin(4*pi/5)*sin(a)*sin(b)*a2-rB*sin(4*pi/5)*cos(a)*sin(b)*b1^2+rB*sin(4*pi/5)*cos(a)*cos(b)*b2+rB*cos(4*pi/5)*sin(a)*a1^2-rB*cos(4*pi/5)*cos(a)*a2+xb2;
l3y=rB*sin(4*pi/5)*sin(a)*sin(b)+rB*cos(4*pi/5)*cos(a)+yb+rA*cos(pi/4);
l3y1=rB*sin(4*pi/5)*cos(a)*sin(b)*a1+rB*sin(4*pi/5)*sin(a)*cos(b)*b1-rB*cos(4*pi/5)*sin(a)*a1+yb1;
l3y2=-rB*sin(4*pi/5)*sin(a)*sin(b)*a1^2+2*rB*sin(4*pi/5)*cos(a)*cos(b)*a1*b1+rB*sin(4*pi/5)*cos(a)*sin(b)*a2-rB*sin(4*pi/5)*sin(a)*sin(b)*b1^2+rB*sin(4*pi/5)*sin(a)*cos(b)*b2-rB*cos(4*pi/5)*cos(a)*a1^2-rB*cos(4*pi/5)*sin(a)*a2+yb2;
l3z=rB*sin(4*pi/5)*cos(b)+zb-rA*sin(pi/4);
l3z1=-rB*sin(4*pi/5)*sin(b)*b1+zb1;
l3z2=-rB*sin(4*pi/5)*cos(b)*b1^2-rB*sin(4*pi/5)*sin(b)*b2+zb2;
l3=(l3x^2+l3y^2+l3z^2)^(1/2);
l3d1=(1/l3)*(l3x*l3x1+l3y*l3y1+l3z*l3z1);%支链杆3的1次导数
l3d2=(-l3d1/(l3^2))*(l3x*l3x1+l3y*l3y1+l3z*l3z1)+(1/l3)*(l3x1*l3x1+l3x*l3x2+l3y1*l3y1+l3y*l3y2+l3z1*l3z1+l3z*l3z2);%支链杆3的2次导数
l4x=rB*sin(6*pi/5)*cos(a)*sin(b)-rB*cos(6*pi/5)*sin(a)+xb;
l4x1=-rB*sin(6*pi/5)*sin(a)*sin(b)*a1+rB*sin(6*pi/5)*cos(a)*cos(b)*b1-rB*cos(6*pi/5)*cos(a)*a1+xb1;
l4x2=-rB*sin(6*pi/5)*cos(a)*sin(b)*a1^2-2*rB*sin(6*pi/5)*sin(a)*cos(b)*a1*b1-rB*sin(6*pi/5)*sin(a)*sin(b)*a2-rB*sin(6*pi/5)*cos(a)*sin(b)*b1^2+rB*sin(6*pi/5)*cos(a)*cos(b)*b2+rB*cos(6*pi/5)*sin(a)*a1^2-rB*cos(6*pi/5)*cos(a)*a2+xb2;
l4y=rB*sin(6*pi/5)*sin(a)*sin(b)+rB*cos(6*pi/5)*cos(a)+yb+rA*cos(pi/4);
l4y1=rB*sin(6*pi/5)*cos(a)*sin(b)*a1+rB*sin(6*pi/5)*sin(a)*cos(b)*b1-rB*cos(6*pi/5)*sin(a)*a1+yb1;
l4y2=-rB*sin(6*pi/5)*sin(a)*sin(b)*a1^2+2*rB*sin(6*pi/5)*cos(a)*cos(b)*a1*b1+rB*sin(6*pi/5)*cos(a)*sin(b)*a2-rB*sin(6*pi/5)*sin(a)*sin(b)*b1^2+rB*sin(6*pi/5)*sin(a)*cos(b)*b2-rB*cos(6*pi/5)*cos(a)*a1^2-rB*cos(6*pi/5)*sin(a)*a2+yb2;
l4z=rB*sin(6*pi/5)*cos(b)+zb+rA*sin(pi/4);
l4z1=-rB*sin(6*pi/5)*sin(b)*b1+zb1;
l4z2=-rB*sin(6*pi/5)*cos(b)*b1^2-rB*sin(6*pi/5)*sin(b)*b2+zb2;
l4=(l4x^2+l4y^2+l4z^2)^(1/2);
l4d1=(1/l4)*(l4x*l4x1+l4y*l4y1+l4z*l4z1);%支链杆4的1次导数
l4d2=(-l4d1/(l4^2))*(l4x*l4x1+l4y*l4y1+l4z*l4z1)+(1/l4)*(l4x1*l4x1+l4x*l4x2+l4y1*l4y1+l4y*l4y2+l4z1*l4z1+l4z*l4z2);%支链杆4的2次导数
l5x=rB*sin(8*pi/5)*cos(a)*sin(b)-rB*cos(8*pi/5)*sin(a)+xb;
l5x1=-rB*sin(8*pi/5)*sin(a)*sin(b)*a1+rB*sin(8*pi/5)*cos(a)*cos(b)*b1-rB*cos(8*pi/5)*cos(a)*a1+xb1;
l5x2=-rB*sin(8*pi/5)*cos(a)*sin(b)*a1^2-2*rB*sin(8*pi/5)*sin(a)*cos(b)*a1*b1-rB*sin(8*pi/5)*sin(a)*sin(b)*a2-rB*sin(8*pi/5)*cos(a)*sin(b)*b1^2+rB*sin(8*pi/5)*cos(a)*cos(b)*b2+rB*cos(8*pi/5)*sin(a)*a1^2-rB*cos(8*pi/5)*cos(a)*a2+xb2;
l5y=rB*sin(8*pi/5)*sin(a)*sin(b)+rB*cos(8*pi/5)*cos(a)+yb-rA*cos(pi/4);
l5y1=rB*sin(8*pi/5)*cos(a)*sin(b)*a1+rB*sin(8*pi/5)*sin(a)*cos(b)*b1-rB*cos(8*pi/5)*sin(a)*a1+yb1;
l5y2=-rB*sin(8*pi/5)*sin(a)*sin(b)*a1^2+2*rB*sin(8*pi/5)*cos(a)*cos(b)*a1*b1+rB*sin(8*pi/5)*cos(a)*sin(b)*a2-rB*sin(8*pi/5)*sin(a)*sin(b)*b1^2+rB*sin(8*pi/5)*sin(a)*cos(b)*b2-rB*cos(8*pi/5)*cos(a)*a1^2-rB*cos(8*pi/5)*sin(a)*a2+yb2;
l5z=rB*sin(8*pi/5)*cos(b)+zb+rA*sin(pi/4);
l5z1=-rB*sin(8*pi/5)*sin(b)*b1+zb1;
l5z2=-rB*sin(8*pi/5)*cos(b)*b1^2-rB*sin(8*pi/5)*sin(b)*b2+zb2;
l5=(l5x^2+l5y^2+l5z^2)^(1/2);
l5d1=(1/l5)*(l5x*l5x1+l5y*l5y1+l5z*l5z1);%支链杆5的1次导数
l5d2=(-l5d1/(l5^2))*(l5x*l5x1+l5y*l5y1+l5z*l5z1)+(1/l5)*(l5x1*l5x1+l5x*l5x2+l5y1*l5y1+l5y*l5y2+l5z1*l5z1+l5z*l5z2);%支链杆5的2次导数
回复 1 # weideyong8 的帖子
theta1=atan((l1y*cos(0)+l1z*sin(0))/l1x);
theta2=atan((l2y*cos(pi/4)+l2z*sin(pi/4))/l2x);
theta3=atan((l3y*cos(-pi/4)+l3z*sin(-pi/4))/l3x);
theta4=atan((l4y*cos(-3*pi/4)+l4z*sin(-3*pi/4))/l4x);
theta5=atan((l5y*cos(3*pi/4)+l5z*sin(3*pi/4))/l5x);
sheta1=asin((l1y*sin(0)-l1z*cos(0))/l1);
sheta2=asin((l2y*sin(pi/4)-l2z*cos(pi/4))/l2);
sheta3=asin((l3y*sin(-pi/4)-l3z*cos(-pi/4))/l3);
sheta4=asin((l4y*sin(-3*pi/4)-l4z*cos(-3*pi/4))/l4);
sheta5=asin((l5y*sin(3*pi/4)-l5z*cos(3*pi/4))/l5);
theta11=((l1y1*cos(0)+l1z1*sin(0))*l1x-(l1y*cos(0)+l1z*sin(0))*l1x1)/(l1x^2+(l1y*cos(0)+l1z*sin(0))^2);
theta21=((l2y1*cos(pi/4)+l2z1*sin(pi/4))*l2x-(l2y*cos(pi/4)+l2z*sin(pi/4))*l2x1)/(l2x^2+(l2y*cos(pi/4)+l2z*sin(pi/4))^2);
theta31=((l3y1*cos(-pi/4)+l3z1*sin(-pi/4))*l3x-(l3y*cos(-pi/4)+l3z*sin(-pi/4))*l3x1)/(l3x^2+(l3y*cos(-pi/4)+l3z*sin(-pi/4))^2);
theta41=((l4y1*cos(-3*pi/4)+l4z1*sin(-3*pi/4))*l4x-(l4y*cos(-3*pi/4)+l4z*sin(-3*pi/4))*l4x1)/(l4x^2+(l4y*cos(-3*pi/4)+l4z*sin(-3*pi/4))^2);
theta51=((l5y1*cos(3*pi/4)+l5z1*sin(3*pi/4))*l5x-(l5y*cos(3*pi/4)+l5z*sin(3*pi/4))*l5x1)/(l5x^2+(l5y*cos(3*pi/4)+l5z*sin(3*pi/4))^2);
sheta11=((l1y1*sin(0)-l1z1*cos(0))*l1-(l1y*sin(0)-l1z*cos(0))*l1d1)/(((l1^2-(l1y*sin(0)-l1z*cos(0))^2)^0.5)*l1);
sheta21=((l2y1*sin(pi/4)-l2z1*cos(pi/4))*l2-(l2y*sin(pi/4)-l2z*cos(pi/4))*l2d1)/(((l2^2-(l2y*sin(pi/4)-l2z*cos(pi/4))^2)^0.5)*l2);
sheta31=((l3y1*sin(-pi/4)-l3z1*cos(-pi/4))*l3-(l3y*sin(-pi/4)-l3z*cos(-pi/4))*l3d1)/(((l3^2-(l3y*sin(-pi/4)-l3z*cos(-pi/4))^2)^0.5)*l3);
sheta41=((l4y1*sin(-3*pi/4)-l4z1*cos(-3*pi/4))*l4-(l4y*sin(-3*pi/4)-l4z*cos(-3*pi/4))*l4d1)/(((l4^2-(l4y*sin(-3*pi/4)-l4z*cos(-3*pi/4))^2)^0.5)*l4);
sheta51=((l5y1*sin(3*pi/4)-l5z1*cos(3*pi/4))*l5-(l5y*sin(3*pi/4)-l5z*cos(3*pi/4))*l5d1)/(((l5^2-(l5y*sin(3*pi/4)-l5z*cos(3*pi/4))^2)^0.5)*l5);
theta12=(((l1y2*cos(0)+l1z2*sin(0))*l1x-(l1y*cos(0)+l1z*sin(0))*l1x2)*(l1x^2+(l1y*cos(0)+l1z*sin(0))^2)-((l1y1*cos(0)+l1z1*sin(0))*l1x-(l1y*cos(0)+l1z*sin(0))*l1x1)*(2*l1x*l1x1+2*(l1y*cos(0)+l1z*sin(0))*(l1y1*cos(0)+l1z1*sin(0))))/((l1x^2+(l1y*cos(0)+l1z*sin(0))^2)^2);
theta22=(((l2y2*cos(pi/4)+l2z2*sin(pi/4))*l2x-(l2y*cos(pi/4)+l2z*sin(pi/4))*l2x2)*(l2x^2+(l2y*cos(pi/4)+l2z*sin(pi/4))^2)-((l2y1*cos(pi/4)+l2z1*sin(pi/4))*l2x-(l2y*cos(pi/4)+l2z*sin(pi/4))*l2x1)*(2*l2x*l2x1+2*(l2y*cos(pi/4)+l2z*sin(pi/4))*(l2y1*cos(pi/4)+l2z1*sin(pi/4))))/((l2x^2+(l2y*cos(pi/4)+l2z*sin(pi/4))^2)^2);
theta32=(((l3y2*cos(-pi/4)+l3z2*sin(-pi/4))*l3x-(l3y*cos(-pi/4)+l3z*sin(-pi/4))*l3x2)*(l3x^2+(l3y*cos(-pi/4)+l3z*sin(-pi/4))^2)-((l3y1*cos(-pi/4)+l3z1*sin(-pi/4))*l3x-(l3y*cos(-pi/4)+l3z*sin(-pi/4))*l3x1)*(2*l3x*l3x1+2*(l3y*cos(-pi/4)+l3z*sin(-pi/4))*(l3y1*cos(-pi/4)+l3z1*sin(-pi/4))))/((l3x^2+(l3y*cos(-pi/4)+l3z*sin(-pi/4))^2)^2);
theta42=(((l4y2*cos(-3*pi/4)+l4z2*sin(-3*pi/4))*l4x-(l4y*cos(-3*pi/4)+l4z*sin(-3*pi/4))*l4x2)*(l4x^2+(l4y*cos(-3*pi/4)+l4z*sin(-3*pi/4))^2)-((l4y1*cos(-3*pi/4)+l4z1*sin(-3*pi/4))*l4x-(l4y*cos(-3*pi/4)+l4z*sin(-3*pi/4))*l4x1)*(2*l4x*l4x1+2*(l4y*cos(-3*pi/4)+l4z*sin(-3*pi/4))*(l4y1*cos(-3*pi/4)+l4z1*sin(-3*pi/4))))/((l4x^2+(l4y*cos(-3*pi/4)+l4z*sin(-3*pi/4))^2)^2);
theta52=(((l5y2*cos(3*pi/4)+l5z2*sin(3*pi/4))*l5x-(l5y*cos(3*pi/4)+l5z*sin(3*pi/4))*l5x2)*(l5x^2+(l5y*cos(3*pi/4)+l5z*sin(3*pi/4))^2)-((l5y1*cos(3*pi/4)+l5z1*sin(3*pi/4))*l5x-(l5y*cos(3*pi/4)+l5z*sin(3*pi/4))*l5x1)*(2*l5x*l5x1+2*(l5y*cos(3*pi/4)+l5z*sin(3*pi/4))*(l5y1*cos(3*pi/4)+l5z1*sin(3*pi/4))))/((l5x^2+(l5y*cos(3*pi/4)+l5z*sin(3*pi/4))^2)^2);
%分子项比较多 而且比较乱 %举例
s201=(l2y2*sin(pi/4)-l2z2*cos(pi/4))*l2;s202=l2d2*(l2y*sin(pi/4)-l2z*cos(pi/4));
A2=(((l2^2-(l2y*sin(pi/4)-l2z*cos(pi/4))^2)^0.5)*l2);B2=((l2y1*sin(pi/4)-l2z1*cos(pi/4))*l2-(l2y*sin(pi/4)-l2z*cos(pi/4))*l2d1);
s203=l2d1*(l2^2-(l2y*sin(pi/4)-l2z*cos(pi/4))^2)^0.5+l2*(l2^2-(l2y*sin(pi/4)-l2z*cos(pi/4))^2)^(-0.5)*(l2*l2d1-(l2y*sin(pi/4)-l2z*cos(pi/4))*(l2y1*sin(pi/4)-l2z1*cos(pi/4)));
s301=(l3y2*sin(-pi/4)-l3z2*cos(-pi/4))*l3;s302=l3d2*(l3y*sin(-pi/4)-l3z*cos(-pi/4));
A3=(((l3^2-(l3y*sin(-pi/4)-l3z*cos(-pi/4))^2)^0.5)*l3);B3=((l3y1*sin(-pi/4)-l3z1*cos(-pi/4))*l3-(l3y*sin(-pi/4)-l3z*cos(-pi/4))*l3d1);
s303=l3d1*(l3^2-(l3y*sin(-pi/4)-l3z*cos(-pi/4))^2)^0.5+l3*(l3^2-(l3y*sin(-pi/4)-l3z*cos(-pi/4))^2)^(-0.5)*(l3*l3d1-(l3y*sin(-pi/4)-l3z*cos(-pi/4))*(l3y1*sin(-pi/4)-l3z1*cos(-pi/4)));
s401=(l4y2*sin(-3*pi/4)-l4z2*cos(-3*pi/4))*l4;s402=l4d2*(l4y*sin(-3*pi/4)-l4z*cos(-3*pi/4));
A4=(((l4^2-(l4y*sin(-3*pi/4)-l4z*cos(-3*pi/4))^2)^0.5)*l4);B4=((l4y1*sin(-3*pi/4)-l4z1*cos(-3*pi/4))*l4-(l4y*sin(-3*pi/4)-l4z*cos(-3*pi/4))*l4d1);
s403=l4d1*(l4^2-(l4y*sin(-3*pi/4)-l4z*cos(-3*pi/4))^2)^0.5+l4*(l4^2-(l4y*sin(-3*pi/4)-l4z*cos(-3*pi/4))^2)^(-0.5)*(l4*l4d1-(l4y*sin(-3*pi/4)-l4z*cos(-3*pi/4))*(l4y1*sin(-3*pi/4)-l4z1*cos(-3*pi/4)));
s501=(l5y2*sin(3*pi/4)-l5z2*cos(3*pi/4))*l5;s502=l5d2*(l5y*sin(3*pi/4)-l5z*cos(3*pi/4));
A5=(((l5^2-(l5y*sin(3*pi/4)-l5z*cos(3*pi/4))^2)^0.5)*l5);B5=((l5y1*sin(3*pi/4)-l5z1*cos(3*pi/4))*l5-(l5y*sin(3*pi/4)-l5z*cos(3*pi/4))*l5d1);
s503=l5d1*(l5^2-(l5y*sin(3*pi/4)-l5z*cos(3*pi/4))^2)^0.5+l5*(l5^2-(l5y*sin(3*pi/4)-l5z*cos(3*pi/4))^2)^(-0.5)*(l5*l5d1-(l5y*sin(3*pi/4)-l5z*cos(3*pi/4))*(l5y1*sin(3*pi/4)-l5z1*cos(3*pi/4)));
% sheta12=((s101-s102)*A1-B1*s103)/A1^2;
sheta12=(-1/2/((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2)*(93318/25+6*t)+1/((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2)*(46659/25+3*t)-1/2*t/((92089/100+t)^2+(94547/100+t)^2+t^2)^(3/2)*(46659/25+3*t)*(93318/25+6*t)+3*t/((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2))/((92089/100+t)^2+(94547/100+t)^2)^(1/2)/((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2)-1/2*(-((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2)+t/((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2)*(46659/25+3*t))/((92089/100+t)^2+(94547/100+t)^2)^(3/2)/((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2)*(93318/25+4*t)-1/2*(-((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2)+t/((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2)*(46659/25+3*t))/((92089/100+t)^2+(94547/100+t)^2)^(1/2)/((92089/100+t)^2+(94547/100+t)^2+t^2)^(3/2)*(93318/25+6*t);
sheta22=((s201-s202)*A2-B2*s203)/A2^2;
sheta32=((s301-s302)*A3-B3*s303)/A3^2;
sheta42=((s401-s402)*A4-B4*s403)/A4^2;
sheta52=((s501-s502)*A5-B5*s503)/A5^2;
Aij1=[cos(theta1)*cos(sheta1) -sin(theta1) cos(theta1)*sin(sheta1);
cos(0)*sin(theta1)*cos(sheta1)+sin(0)*sin(sheta1) cos(0)*cos(theta1) cos(0)*sin(theta1)*sin(sheta1)-sin(0)*cos(sheta1);
sin(0)*sin(theta1)*cos(sheta1)-cos(0)*sin(sheta1) sin(0)*cos(theta1) sin(0)*sin(theta1)*sin(sheta1)+cos(0)*cos(sheta1)]; %支链1上单元的变换矩阵 先算出一个再说，先往后列出式子 看看
Aij2=[cos(theta2)*cos(sheta2) -sin(theta2) cos(theta2)*sin(sheta2);
cos(pi/4)*sin(theta2)*cos(sheta2)+sin(pi/4)*sin(sheta2) cos(pi/4)*cos(theta2) cos(pi/4)*sin(theta2)*sin(sheta2)-sin(pi/4)*cos(sheta2);
sin(pi/4)*sin(theta2)*cos(sheta2)-cos(pi/4)*sin(sheta2) sin(pi/4)*cos(theta2) sin(pi/4)*sin(theta2)*sin(sheta2)+cos(pi/4)*cos(sheta2)];
Aij3=[cos(theta3)*cos(sheta3) -sin(theta3) cos(theta3)*sin(sheta3);
cos(-pi/4)*sin(theta3)*cos(sheta3)+sin(-pi/4)*sin(sheta3) cos(-pi/4)*cos(theta3) cos(-pi/4)*sin(theta3)*sin(sheta3)-sin(-pi/4)*cos(sheta3);
sin(-pi/4)*sin(theta3)*cos(sheta3)-cos(-pi/4)*sin(sheta3) sin(-pi/4)*cos(theta3) sin(-pi/4)*sin(theta3)*sin(sheta3)+cos(-pi/4)*cos(sheta3)];
Aij4=[cos(theta4)*cos(sheta4) -sin(theta4) cos(theta4)*sin(sheta4);
cos(-3*pi/4)*sin(theta4)*cos(sheta4)+sin(-3*pi/4)*sin(sheta4) cos(-3*pi/4)*cos(theta4) cos(-3*pi/4)*sin(theta4)*sin(sheta4)-sin(-3*pi/4)*cos(sheta4);
sin(-3*pi/4)*sin(theta4)*cos(sheta4)-cos(-3*pi/4)*sin(sheta4) sin(-3*pi/4)*cos(theta4) sin(-3*pi/4)*sin(theta4)*sin(sheta4)+cos(-3*pi/4)*cos(sheta4)];
Aij5=[cos(theta5)*cos(sheta5) -sin(theta5) cos(theta5)*sin(sheta5);
cos(3*pi/4)*sin(theta5)*cos(sheta5)+sin(3*pi/4)*sin(sheta5) cos(3*pi/4)*cos(theta5) cos(3*pi/4)*sin(theta5)*sin(sheta5)-sin(3*pi/4)*cos(sheta5);
sin(3*pi/4)*sin(theta5)*cos(sheta5)-cos(3*pi/4)*sin(sheta5) sin(3*pi/4)*cos(theta5) sin(3*pi/4)*sin(theta5)*sin(sheta5)+cos(3*pi/4)*cos(sheta5)];
Rij1=Aij1.';
Rij2=Aij2.';
Rij3=Aij3.';
Rij4=Aij4.';
Rij5=Aij5.';
on=zeros(3,3);
Rij1heng=;%表示单元广义坐标与单元系统坐标系下的转换矩阵
Rij2heng=;
Rij3heng=;
Rij4heng=;
Rij5heng=;
Bij1=;
Bij2=;
Bij3=;
Bij4=;
Bij5=; % 3*1表示Aij5的第一列
%下面的五个矩阵为支链1、2、3、4、5上单元的变换矩阵的对时间t的导数矩阵
Gij1=[-sin(theta1)*cos(sheta1)*theta11-cos(theta1)*sin(sheta1)*sheta11,-cos(theta1)*theta11,-sin(theta1)*sin(sheta1)*theta11+cos(theta1)*cos(sheta1)*sheta11;
cos(0)*cos(theta1)*cos(sheta1)*theta11-cos(0)*sin(theta1)*sin(sheta1)*sheta11+sin(0)*cos(sheta1)*sheta11,-cos(0)*sin(theta1)*theta11,cos(0)*cos(theta1)*sin(sheta1)*theta11+cos(0)*sin(theta1)*cos(sheta1)*sheta11+sin(0)*sin(sheta1)*sheta11;
sin(0)*cos(theta1)*cos(sheta1)*theta11-sin(0)*sin(theta1)*sin(sheta1)*sheta11-cos(0)*cos(sheta1)*sheta11,-sin(0)*sin(theta1)*theta11,sin(0)*cos(theta1)*sin(sheta1)*theta11+sin(0)*sin(theta1)*cos(sheta1)*sheta11-cos(0)*sin(sheta1)*sheta11];
Gij2=[-sin(theta2)*cos(sheta2)*theta21-cos(theta2)*sin(sheta2)*sheta21,-cos(theta2)*theta21,-sin(theta2)*sin(sheta2)*theta21+cos(theta2)*cos(sheta2)*sheta21;
cos(pi/4)*cos(theta2)*cos(sheta2)*theta21-cos(pi/4)*sin(theta2)*sin(sheta2)*sheta21+sin(pi/4)*cos(sheta2)*sheta21,-cos(pi/4)*sin(theta2)*theta21,cos(pi/4)*cos(theta2)*sin(sheta2)*theta21+cos(pi/4)*sin(theta2)*cos(sheta2)*sheta21+sin(pi/4)*sin(sheta2)*sheta21;
sin(pi/4)*cos(theta2)*cos(sheta2)*theta21-sin(pi/4)*sin(theta2)*sin(sheta2)*sheta21-cos(pi/4)*cos(sheta2)*sheta21,-sin(pi/4)*sin(theta2)*theta21,sin(pi/4)*cos(theta2)*sin(sheta2)*theta21+sin(pi/4)*sin(theta2)*cos(sheta2)*sheta21-cos(pi/4)*sin(sheta2)*sheta21];
Gij3=[-sin(theta3)*cos(sheta3)*theta31-cos(theta3)*sin(sheta3)*sheta31,-cos(theta3)*theta31,-sin(theta3)*sin(sheta3)*theta31+cos(theta3)*cos(sheta3)*sheta31;
cos(-pi/4)*cos(theta3)*cos(sheta3)*theta31-cos(-pi/4)*sin(theta3)*sin(sheta3)*sheta31+sin(-pi/4)*cos(sheta3)*sheta31,-cos(-pi/4)*sin(theta3)*theta31,cos(-pi/4)*cos(theta3)*sin(sheta3)*theta31+cos(-pi/4)*sin(theta3)*cos(sheta3)*sheta31+sin(-pi/4)*sin(sheta3)*sheta31;
sin(-pi/4)*cos(theta3)*cos(sheta3)*theta31-sin(-pi/4)*sin(theta3)*sin(sheta3)*sheta31-cos(-pi/4)*cos(sheta3)*sheta31,-sin(-pi/4)*sin(theta3)*theta31,sin(-pi/4)*cos(theta3)*sin(sheta3)*theta31+sin(-pi/4)*sin(theta3)*cos(sheta3)*sheta31-cos(-pi/4)*sin(sheta3)*sheta31];
Gij4=[-sin(theta4)*cos(sheta4)*theta41-cos(theta4)*sin(sheta4)*sheta41,-cos(theta4)*theta41,-sin(theta4)*sin(sheta4)*theta41+cos(theta4)*cos(sheta4)*sheta41;
cos(-3*pi/4)*cos(theta4)*cos(sheta4)*theta41-cos(-3*pi/4)*sin(theta4)*sin(sheta4)*sheta41+sin(-3*pi/4)*cos(sheta4)*sheta41,-cos(-3*pi/4)*sin(theta4)*theta41,cos(-3*pi/4)*cos(theta4)*sin(sheta4)*theta41+cos(-3*pi/4)*sin(theta4)*cos(sheta4)*sheta41+sin(-3*pi/4)*sin(sheta4)*sheta41;
sin(-3*pi/4)*cos(theta4)*cos(sheta4)*theta41-sin(-3*pi/4)*sin(theta4)*sin(sheta4)*sheta41-cos(-3*pi/4)*cos(sheta4)*sheta41,-sin(-3*pi/4)*sin(theta4)*theta41,sin(-3*pi/4)*cos(theta4)*sin(sheta4)*theta41+sin(-3*pi/4)*sin(theta4)*cos(sheta4)*sheta41-cos(-3*pi/4)*sin(sheta4)*sheta41];
Gij5=[-sin(theta5)*cos(sheta5)*theta51-cos(theta5)*sin(sheta5)*sheta51,-cos(theta5)*theta51,-sin(theta5)*sin(sheta5)*theta51+cos(theta5)*cos(sheta5)*sheta51;
cos(3*pi/4)*cos(theta5)*cos(sheta5)*theta51-cos(3*pi/4)*sin(theta5)*sin(sheta5)*sheta51+sin(3*pi/4)*cos(sheta5)*sheta51,-cos(3*pi/4)*sin(theta5)*theta51,cos(3*pi/4)*cos(theta5)*sin(sheta5)*theta51+cos(3*pi/4)*sin(theta5)*cos(sheta5)*sheta51+sin(3*pi/4)*sin(sheta5)*sheta51;
sin(3*pi/4)*cos(theta5)*cos(sheta5)*theta51-sin(3*pi/4)*sin(theta5)*sin(sheta5)*sheta51-cos(3*pi/4)*cos(sheta5)*sheta51,-sin(3*pi/4)*sin(theta5)*theta51,sin(3*pi/4)*cos(theta5)*sin(sheta5)*theta51+sin(3*pi/4)*sin(theta5)*cos(sheta5)*sheta51-cos(3*pi/4)*sin(sheta5)*sheta51];
% syms theta1theta11 theta12 sheta1 sheta11 sheta12
g11ij1=-sin(theta1)*cos(sheta1)*(theta12)-cos(theta1)*cos(sheta1)*(theta11)^2+sin(theta1)*sin(sheta1)*(theta11)*(sheta11)-cos(theta1)*sin(sheta1)*(sheta12)+sin(theta1)*sin(sheta1)*(theta11)*(sheta11)-cos(theta1)*cos(sheta1)*(sheta11)^2;
g12ij1=-cos(theta1)*(theta12)+sin(theta1)*(theta11)^2;
g13ij1=-sin(theta1)*sin(sheta1)*(theta12)-cos(theta1)*sin(sheta1)*(theta11)^2-sin(theta1)*cos(sheta1)*(theta11)*(sheta11)+cos(theta1)*cos(sheta1)*(sheta12)-sin(theta1)*cos(sheta1)*(theta11)*(sheta11)-cos(theta1)*sin(sheta1)*(sheta11)^2;
g21ij1=cos(0)*cos(theta1)*cos(sheta1)*(theta12)-cos(0)*sin(theta1)*cos(sheta1)*(theta11)^2-cos(0)*cos(theta1)*sin(sheta1)*(theta11)*(sheta11)-cos(0)*sin(theta1)*sin(sheta1)*(sheta12)-cos(0)*cos(theta1)*sin(sheta1)*(theta11)*(sheta11)-cos(0)*sin(theta1)*cos(sheta1)*(sheta11)^2+sin(0)*cos(sheta1)*(sheta12)-sin(0)*sin(sheta1)*(sheta11)^2;
g22ij1=-cos(0)*sin(theta1)*(theta12)-cos(0)*cos(theta1)*(theta11)^2;
g23ij1=cos(0)*cos(theta1)*sin(sheta1)*(theta12)-cos(0)*sin(theta1)*sin(sheta1)*(theta11)^2+cos(0)*cos(theta1)*cos(sheta1)*(theta11)*(sheta11)+cos(0)*sin(theta1)*cos(sheta1)*(sheta12)+cos(0)*cos(theta1)*cos(sheta1)*(theta11)*(sheta11)-cos(0)*sin(theta1)*sin(sheta1)*(sheta11)^2+sin(0)*sin(sheta1)*(sheta12)+sin(0)*cos(sheta1)*(sheta11)^2;
g31ij1=sin(0)*cos(theta1)*cos(sheta1)*(theta12)-sin(0)*sin(theta1)*cos(sheta1)*(theta11)^2-sin(0)*cos(theta1)*sin(sheta1)*(theta11)*(sheta11)-sin(0)*sin(theta1)*sin(sheta1)*(sheta12)-sin(0)*cos(theta1)*sin(sheta1)*(theta11)*(sheta11)-sin(0)*sin(theta1)*cos(sheta1)*(sheta11)^2-cos(0)*cos(sheta1)*(sheta12)+cos(0)*sin(sheta1)*(sheta11)^2;
g32ij1=-sin(0)*sin(theta1)*(theta12)-sin(0)*cos(theta1)*(theta11)^2;
g33ij1=sin(0)*cos(theta1)*sin(sheta1)*(theta12)-sin(0)*sin(theta1)*sin(sheta1)*(theta11)^2+sin(0)*cos(theta1)*cos(sheta1)*(sheta11)*(theta11)+sin(0)*sin(theta1)*cos(sheta1)*(sheta12)+sin(0)*cos(theta1)*cos(sheta1)*(theta11)*(sheta11)-sin(0)*sin(theta1)*sin(sheta1)*(sheta11)^2-cos(0)*sin(sheta1)*(sheta12)-cos(0)*cos(sheta1)*(sheta11)^2;
% syms theta2theta21 theta22 sheta2 sheta21 sheta22
g11ij2=-sin(theta2)*cos(sheta2)*(theta22)-cos(theta2)*cos(sheta2)*(theta21)^2+sin(theta2)*sin(sheta2)*(theta21)*(sheta21)-cos(theta2)*sin(sheta2)*(sheta22)+sin(theta2)*sin(sheta2)*(theta21)*(sheta21)-cos(theta2)*cos(sheta2)*(sheta21)^2;
g12ij2=-cos(theta2)*(theta22)+sin(theta2)*(theta21)^2;
g13ij2=-sin(theta2)*sin(sheta2)*(theta22)-cos(theta2)*sin(sheta2)*(theta21)^2-sin(theta2)*cos(sheta2)*(theta21)*(sheta21)+cos(theta2)*cos(sheta2)*(sheta22)-sin(theta2)*cos(sheta2)*(theta21)*(sheta21)-cos(theta2)*sin(sheta2)*(sheta21)^2;
g21ij2=cos(pi/4)*cos(theta2)*cos(sheta2)*(theta22)-cos(pi/4)*sin(theta2)*cos(sheta2)*(theta21)^2-cos(pi/4)*cos(theta2)*sin(sheta2)*(theta21)*(sheta21)-cos(pi/4)*sin(theta2)*sin(sheta2)*(sheta22)-cos(pi/4)*cos(theta2)*sin(sheta2)*(theta21)*(sheta21)-cos(pi/4)*sin(theta2)*cos(sheta2)*(sheta21)^2+sin(pi/4)*cos(sheta2)*(sheta22)-sin(pi/4)*sin(sheta2)*(sheta21)^2;
g22ij2=-cos(pi/4)*sin(theta2)*(theta22)-cos(pi/4)*cos(theta2)*(theta21)^2;
g23ij2=cos(pi/4)*cos(theta2)*sin(sheta2)*(theta22)-cos(pi/4)*sin(theta2)*sin(sheta2)*(theta21)^2+cos(pi/4)*cos(theta2)*cos(sheta2)*(theta21)*(sheta21)+cos(pi/4)*sin(theta2)*cos(sheta2)*(sheta22)+cos(pi/4)*cos(theta2)*cos(sheta2)*(theta21)*(sheta21)-cos(pi/4)*sin(theta2)*sin(sheta2)*(sheta21)^2+sin(pi/4)*sin(sheta2)*(sheta22)+sin(pi/4)*cos(sheta2)*(sheta21)^2;
g31ij2=sin(pi/4)*cos(theta2)*cos(sheta2)*(theta22)-sin(pi/4)*sin(theta2)*cos(sheta2)*(theta21)^2-sin(pi/4)*cos(theta2)*sin(sheta2)*(theta21)*(sheta21)-sin(pi/4)*sin(theta2)*sin(sheta2)*(sheta22)-sin(pi/4)*cos(theta2)*sin(sheta2)*(theta21)*(sheta21)-sin(pi/4)*sin(theta2)*cos(sheta2)*(sheta21)^2-cos(pi/4)*cos(sheta2)*(sheta22)+cos(pi/4)*sin(sheta2)*(sheta21)^2;
g32ij2=-sin(pi/4)*sin(theta2)*(theta22)-sin(pi/4)*cos(theta2)*(theta21)^2;
g33ij2=sin(pi/4)*cos(theta2)*sin(sheta2)*(theta22)-sin(pi/4)*sin(theta2)*sin(sheta2)*(theta21)^2+sin(pi/4)*cos(theta2)*cos(sheta2)*(sheta21)*(theta21)+sin(pi/4)*sin(theta2)*cos(sheta2)*(sheta22)+sin(pi/4)*cos(theta2)*cos(sheta2)*(theta21)*(sheta21)-sin(pi/4)*sin(theta2)*sin(sheta2)*(sheta21)^2-cos(pi/4)*sin(sheta2)*(sheta22)-cos(pi/4)*cos(sheta2)*(sheta21)^2;
% syms theta3theta31 theta32 sheta3 sheta31 sheta32
g11ij3=-sin(theta3)*cos(sheta3)*(theta32)-cos(theta3)*cos(sheta3)*(theta31)^2+sin(theta3)*sin(sheta3)*(theta31)*(sheta31)-cos(theta3)*sin(sheta3)*(sheta32)+sin(theta3)*sin(sheta3)*(theta31)*(sheta31)-cos(theta3)*cos(sheta3)*(sheta31)^2;
g12ij3=-cos(theta3)*(theta32)+sin(theta3)*(theta31)^2;
g13ij3=-sin(theta3)*sin(sheta3)*(theta32)-cos(theta3)*sin(sheta3)*(theta31)^2-sin(theta3)*cos(sheta3)*(theta31)*(sheta31)+cos(theta3)*cos(sheta3)*(sheta32)-sin(theta3)*cos(sheta3)*(theta31)*(sheta31)-cos(theta3)*sin(sheta3)*(sheta31)^2;
g21ij3=cos(-pi/4)*cos(theta3)*cos(sheta3)*(theta32)-cos(-pi/4)*sin(theta3)*cos(sheta3)*(theta31)^2-cos(-pi/4)*cos(theta3)*sin(sheta3)*(theta31)*(sheta31)-cos(-pi/4)*sin(theta3)*sin(sheta3)*(sheta32)-cos(-pi/4)*cos(theta3)*sin(sheta3)*(theta31)*(sheta31)-cos(-pi/4)*sin(theta3)*cos(sheta3)*(sheta31)^2+sin(-pi/4)*cos(sheta3)*(sheta32)-sin(-pi/4)*sin(sheta3)*(sheta31)^2;
g22ij3=-cos(-pi/4)*sin(theta3)*(theta32)-cos(-pi/4)*cos(theta3)*(theta31)^2;
g23ij3=cos(-pi/4)*cos(theta3)*sin(sheta3)*(theta32)-cos(-pi/4)*sin(theta3)*sin(sheta3)*(theta31)^2+cos(-pi/4)*cos(theta3)*cos(sheta3)*(theta31)*(sheta31)+cos(-pi/4)*sin(theta3)*cos(sheta3)*(sheta32)+cos(-pi/4)*cos(theta3)*cos(sheta3)*(theta31)*(sheta31)-cos(-pi/4)*sin(theta3)*sin(sheta3)*(sheta31)^2+sin(-pi/4)*sin(sheta3)*(sheta32)+sin(-pi/4)*cos(sheta3)*(sheta31)^2;
g31ij3=sin(-pi/4)*cos(theta3)*cos(sheta3)*(theta32)-sin(-pi/4)*sin(theta3)*cos(sheta3)*(theta31)^2-sin(-pi/4)*cos(theta3)*sin(sheta3)*(theta31)*(sheta31)-sin(-pi/4)*sin(theta3)*sin(sheta3)*(sheta32)-sin(-pi/4)*cos(theta3)*sin(sheta3)*(theta31)*(sheta31)-sin(-pi/4)*sin(theta3)*cos(sheta3)*(sheta31)^2-cos(-pi/4)*cos(sheta3)*(sheta32)+cos(-pi/4)*sin(sheta3)*(sheta31)^2;
g32ij3=-sin(-pi/4)*sin(theta3)*(theta32)-sin(-pi/4)*cos(theta3)*(theta31)^2;
g33ij3=sin(-pi/4)*cos(theta3)*sin(sheta3)*(theta32)-sin(-pi/4)*sin(theta3)*sin(sheta3)*(theta31)^2+sin(-pi/4)*cos(theta3)*cos(sheta3)*(sheta31)*(theta31)+sin(-pi/4)*sin(theta3)*cos(sheta3)*(sheta32)+sin(-pi/4)*cos(theta3)*cos(sheta3)*(theta31)*(sheta31)-sin(-pi/4)*sin(theta3)*sin(sheta3)*(sheta31)^2-cos(-pi/4)*sin(sheta3)*(sheta32)-cos(-pi/4)*cos(sheta3)*(sheta31)^2;
% syms theta4theta41 theta42 sheta4 sheta41 sheta42
g11ij4=-sin(theta4)*cos(sheta4)*(theta42)-cos(theta4)*cos(sheta4)*(theta41)^2+sin(theta4)*sin(sheta4)*(theta41)*(sheta41)-cos(theta4)*sin(sheta4)*(sheta42)+sin(theta4)*sin(sheta4)*(theta41)*(sheta41)-cos(theta4)*cos(sheta4)*(sheta41)^2;
g12ij4=-cos(theta4)*(theta42)+sin(theta4)*(theta41)^2;
g13ij4=-sin(theta4)*sin(sheta4)*(theta42)-cos(theta4)*sin(sheta4)*(theta41)^2-sin(theta4)*cos(sheta4)*(theta41)*(sheta41)+cos(theta4)*cos(sheta4)*(sheta42)-sin(theta4)*cos(sheta4)*(theta41)*(sheta41)-cos(theta4)*sin(sheta4)*(sheta41)^2;
g21ij4=cos(-3*pi/4)*cos(theta4)*cos(sheta4)*(theta42)-cos(-3*pi/4)*sin(theta4)*cos(sheta4)*(theta41)^2-cos(-3*pi/4)*cos(theta4)*sin(sheta4)*(theta41)*(sheta41)-cos(-3*pi/4)*sin(theta4)*sin(sheta4)*(sheta42)-cos(-3*pi/4)*cos(theta4)*sin(sheta4)*(theta41)*(sheta41)-cos(-3*pi/4)*sin(theta4)*cos(sheta4)*(sheta41)^2+sin(-3*pi/4)*cos(sheta4)*(sheta42)-sin(-3*pi/4)*sin(sheta4)*(sheta41)^2;
g22ij4=-cos(-3*pi/4)*sin(theta4)*(theta42)-cos(-3*pi/4)*cos(theta4)*(theta41)^2;
g23ij4=cos(-3*pi/4)*cos(theta4)*sin(sheta4)*(theta42)-cos(-3*pi/4)*sin(theta4)*sin(sheta4)*(theta41)^2+cos(-3*pi/4)*cos(theta4)*cos(sheta4)*(theta41)*(sheta41)+cos(-3*pi/4)*sin(theta4)*cos(sheta4)*(sheta42)+cos(-3*pi/4)*cos(theta4)*cos(sheta4)*(theta41)*(sheta41)-cos(-3*pi/4)*sin(theta4)*sin(sheta4)*(sheta41)^2+sin(-3*pi/4)*sin(sheta4)*(sheta42)+sin(-3*pi/4)*cos(sheta4)*(sheta41)^2;
g31ij4=sin(-3*pi/4)*cos(theta4)*cos(sheta4)*(theta42)-sin(-3*pi/4)*sin(theta4)*cos(sheta4)*(theta41)^2-sin(-3*pi/4)*cos(theta4)*sin(sheta4)*(theta41)*(sheta41)-sin(-3*pi/4)*sin(theta4)*sin(sheta4)*(sheta42)-sin(-3*pi/4)*cos(theta4)*sin(sheta4)*(theta41)*(sheta41)-sin(-3*pi/4)*sin(theta4)*cos(sheta4)*(sheta41)^2-cos(-3*pi/4)*cos(sheta4)*(sheta42)+cos(-3*pi/4)*sin(sheta4)*(sheta41)^2;
g32ij4=-sin(-3*pi/4)*sin(theta4)*(theta42)-sin(-3*pi/4)*cos(theta4)*(theta41)^2;
g33ij4=sin(-3*pi/4)*cos(theta4)*sin(sheta4)*(theta42)-sin(-3*pi/4)*sin(theta4)*sin(sheta4)*(theta41)^2+sin(-3*pi/4)*cos(theta4)*cos(sheta4)*(sheta41)*(theta41)+sin(-3*pi/4)*sin(theta4)*cos(sheta4)*(sheta42)+sin(-3*pi/4)*cos(theta4)*cos(sheta4)*(theta41)*(sheta41)-sin(-3*pi/4)*sin(theta4)*sin(sheta4)*(sheta41)^2-cos(-3*pi/4)*sin(sheta4)*(sheta42)-cos(-3*pi/4)*cos(sheta4)*(sheta41)^2;
% syms theta5theta51 theta52 sheta5 sheta51 sheta52
g11ij5=-sin(theta5)*cos(sheta5)*(theta52)-cos(theta5)*cos(sheta5)*(theta51)^2+sin(theta5)*sin(sheta5)*(theta51)*(sheta51)-cos(theta5)*sin(sheta5)*(sheta52)+sin(theta5)*sin(sheta5)*(theta51)*(sheta51)-cos(theta5)*cos(sheta5)*(sheta51)^2;
g12ij5=-cos(theta5)*(theta52)+sin(theta5)*(theta51)^2;
g13ij5=-sin(theta5)*sin(sheta5)*(theta52)-cos(theta5)*sin(sheta5)*(theta51)^2-sin(theta5)*cos(sheta5)*(theta51)*(sheta51)+cos(theta5)*cos(sheta5)*(sheta52)-sin(theta5)*cos(sheta5)*(theta51)*(sheta51)-cos(theta5)*sin(sheta5)*(sheta51)^2;
g21ij5=cos(3*pi/4)*cos(theta5)*cos(sheta5)*(theta52)-cos(3*pi/4)*sin(theta5)*cos(sheta5)*(theta51)^2-cos(3*pi/4)*cos(theta5)*sin(sheta5)*(theta51)*(sheta51)-cos(3*pi/4)*sin(theta5)*sin(sheta5)*(sheta52)-cos(3*pi/4)*cos(theta5)*sin(sheta5)*(theta51)*(sheta51)-cos(3*pi/4)*sin(theta5)*cos(sheta5)*(sheta51)^2+sin(3*pi/4)*cos(sheta5)*(sheta52)-sin(3*pi/4)*sin(sheta5)*(sheta51)^2;
g22ij5=-cos(3*pi/4)*sin(theta5)*(theta52)-cos(3*pi/4)*cos(theta5)*(theta51)^2;
g23ij5=cos(3*pi/4)*cos(theta5)*sin(sheta5)*(theta52)-cos(3*pi/4)*sin(theta5)*sin(sheta5)*(theta51)^2+cos(3*pi/4)*cos(theta5)*cos(sheta5)*(theta51)*(sheta51)+cos(3*pi/4)*sin(theta5)*cos(sheta5)*(sheta52)+cos(3*pi/4)*cos(theta5)*cos(sheta5)*(theta51)*(sheta51)-cos(3*pi/4)*sin(theta5)*sin(sheta5)*(sheta51)^2+sin(3*pi/4)*sin(sheta5)*(sheta52)+sin(3*pi/4)*cos(sheta5)*(sheta51)^2;
g31ij5=sin(3*pi/4)*cos(theta5)*cos(sheta5)*(theta52)-sin(3*pi/4)*sin(theta5)*cos(sheta5)*(theta51)^2-sin(3*pi/4)*cos(theta5)*sin(sheta5)*(theta51)*(sheta51)-sin(3*pi/4)*sin(theta5)*sin(sheta5)*(sheta52)-sin(3*pi/4)*cos(theta5)*sin(sheta5)*(theta51)*(sheta51)-sin(3*pi/4)*sin(theta5)*cos(sheta5)*(sheta51)^2-cos(3*pi/4)*cos(sheta5)*(sheta52)+cos(3*pi/4)*sin(sheta5)*(sheta51)^2;
g32ij5=-sin(3*pi/4)*sin(theta5)*(theta52)-sin(3*pi/4)*cos(theta5)*(theta51)^2;
g33ij5=sin(3*pi/4)*cos(theta5)*sin(sheta5)*(theta52)-sin(3*pi/4)*sin(theta5)*sin(sheta5)*(theta51)^2+sin(3*pi/4)*cos(theta5)*cos(sheta5)*(sheta51)*(theta51)+sin(3*pi/4)*sin(theta5)*cos(sheta5)*(sheta52)+sin(3*pi/4)*cos(theta5)*cos(sheta5)*(theta51)*(sheta51)-sin(3*pi/4)*sin(theta5)*sin(sheta5)*(sheta51)^2-cos(3*pi/4)*sin(sheta5)*(sheta52)-cos(3*pi/4)*cos(sheta5)*(sheta51)^2;
Gij12=;%对Gij1的1次求导
Gij22=;
Gij32=;
Gij42=;
Gij52=;%对Gij5的1次求导 回复 2 # weideyong8 的帖子
Rij1heng1=;
Rij2heng1=;
Rij3heng1=;
Rij4heng1=;
Rij5heng1=;
Rij1heng2=;
Rij2heng2=;
Rij3heng2=;
Rij4heng2=;
Rij5heng2=;
% Gij1Gij2..Gij5都是3*3矩阵
Dij1=[-sin(theta1)*cos(sheta1)*theta11-cos(theta1)*sin(sheta1)*sheta11;
cos(0)*cos(theta1)*cos(sheta1)*theta11-cos(0)*sin(theta1)*sin(sheta1)*sheta11+sin(0)*cos(sheta1)*sheta11;
sin(0)*cos(theta1)*cos(sheta1)*theta11-sin(0)*sin(theta1)*sin(sheta1)*sheta11-cos(0)*cos(sheta1)*sheta11];
Dij2=[-sin(theta2)*cos(sheta2)*theta21-cos(theta2)*sin(sheta2)*sheta21;
cos(pi/4)*cos(theta2)*cos(sheta2)*theta21-cos(pi/4)*sin(theta2)*sin(sheta2)*sheta21+sin(pi/4)*cos(sheta2)*sheta21;
sin(pi/4)*cos(theta2)*cos(sheta2)*theta21-sin(pi/4)*sin(theta2)*sin(sheta2)*sheta21-cos(pi/4)*cos(sheta2)*sheta21];
Dij3=[-sin(theta3)*cos(sheta3)*theta31-cos(theta3)*sin(sheta3)*sheta31;
cos(-pi/4)*cos(theta3)*cos(sheta3)*theta31-cos(-pi/4)*sin(theta3)*sin(sheta3)*sheta31+sin(-pi/4)*cos(sheta3)*sheta31;
sin(-pi/4)*cos(theta3)*cos(sheta3)*theta31-sin(-pi/4)*sin(theta3)*sin(sheta3)*sheta31-cos(-pi/4)*cos(sheta3)*sheta31];
Dij4=[-sin(theta4)*cos(sheta4)*theta41-cos(theta4)*sin(sheta4)*sheta41;
cos(-3*pi/4)*cos(theta4)*cos(sheta4)*theta41-cos(-3*pi/4)*sin(theta4)*sin(sheta4)*sheta41+sin(-3*pi/4)*cos(sheta4)*sheta41;
sin(-3*pi/4)*cos(theta4)*cos(sheta4)*theta41-sin(-3*pi/4)*sin(theta4)*sin(sheta4)*sheta41-cos(-3*pi/4)*cos(sheta4)*sheta41];
Dij5=[-sin(theta5)*cos(sheta5)*theta51-cos(theta5)*sin(sheta5)*sheta51;
cos(3*pi/4)*cos(theta5)*cos(sheta5)*theta51-cos(3*pi/4)*sin(theta5)*sin(sheta5)*sheta51+sin(3*pi/4)*cos(sheta5)*sheta51;
sin(3*pi/4)*cos(theta5)*cos(sheta5)*theta51-sin(3*pi/4)*sin(theta5)*sin(sheta5)*sheta51-cos(3*pi/4)*cos(sheta5)*sheta51]; % 3*1表示Gij5的第一列
GAij1=;%GA矩阵相乘
GAij2=;
GAij3=;
GAij4=;
GAij5=;
GGij1=[(cos(sheta1))^2*theta11^2+sheta11^2,sin(sheta1)*theta11^2*sheta11^2,sin(sheta1)*cos(sheta1)*theta11^2;sin(sheta1)*theta11^2*sheta11^2,theta11^2,-cos(sheta1)*theta11^2*sheta11^2;sin(sheta1)*cos(sheta1)*theta11^2,-cos(sheta1)*theta11^2*sheta11^2,(sin(sheta1))^2*theta11^2+sheta11^2];
GGij2=[(cos(sheta2))^2*theta21^2+sheta21^2,sin(sheta2)*theta21^2*sheta21^2,sin(sheta2)*cos(sheta2)*theta21^2;sin(sheta2)*theta21^2*sheta21^2,theta21^2,-cos(sheta2)*theta21^2*sheta21^2;sin(sheta2)*cos(sheta2)*theta21^2,-cos(sheta2)*theta21^2*sheta21^2,(sin(sheta2))^2*theta21^2+sheta21^2];
GGij3=[(cos(sheta3))^2*theta31^2+sheta31^2,sin(sheta3)*theta31^2*sheta31^2,sin(sheta3)*cos(sheta3)*theta31^2;sin(sheta3)*theta31^2*sheta31^2,theta31^2,-cos(sheta3)*theta31^2*sheta31^2;sin(sheta3)*cos(sheta3)*theta31^2,-cos(sheta3)*theta31^2*sheta31^2,(sin(sheta3))^2*theta31^2+sheta31^2];
GGij4=[(cos(sheta4))^2*theta41^2+sheta41^2,sin(sheta4)*theta41^2*sheta41^2,sin(sheta4)*cos(sheta4)*theta41^2;sin(sheta4)*theta41^2*sheta41^2,theta41^2,-cos(sheta4)*theta41^2*sheta41^2;sin(sheta4)*cos(sheta4)*theta41^2,-cos(sheta4)*theta41^2*sheta41^2,(sin(sheta4))^2*theta41^2+sheta41^2];
GGij5=[(cos(sheta5))^2*theta51^2+sheta51^2,sin(sheta5)*theta51^2*sheta51^2,sin(sheta5)*cos(sheta5)*theta51^2;sin(sheta5)*theta51^2*sheta51^2,theta51^2,-cos(sheta5)*theta51^2*sheta51^2;sin(sheta5)*cos(sheta5)*theta51^2,-cos(sheta5)*theta51^2*sheta51^2,(sin(sheta5))^2*theta51^2+sheta51^2];
%GG矩阵相乘
DDij1=(cos(sheta1))^2*(theta11)^2+(sheta11)^2;
DDij2=(cos(sheta2))^2*(theta21)^2+(sheta21)^2;
DDij3=(cos(sheta3))^2*(theta31)^2+(sheta31)^2;
DDij4=(cos(sheta4))^2*(theta41)^2+(sheta41)^2;
DDij5=(cos(sheta5))^2*(theta51)^2+(sheta51)^2;
%DD矩阵相乘
%先算摆动杆上的一个单元上的各个矩阵情况 密度为7801kg/m^3拉压弹性模量E=211GPa ，剪切弹性模量80GPa 。动平台质量105.85kg
%Pij=积分Nr*A.'*A*Nraij1代表摆动杆横截面积4.4*10^(-3)m^2 aij2代表伸缩杆横截面积1.96*10^(-3)m^2
% Pmidu=7801----代表密度
%杆的单位向量
n1=.';
n2=.';
n3=.';
n4=.';
n5=.';
n1d=[(l1x1*l1-l1x*l1d1)/l1^2,(l1y1*l1-l1y*l1d1)/l1^2,(l1z1*l1-l1z*l1d1)/l1^2].';
n2d=[(l2x1*l2-l2x*l2d1)/l2^2,(l2y1*l2-l2y*l2d1)/l2^2,(l2z1*l2-l2z*l2d1)/l2^2].';
n3d=[(l3x1*l3-l3x*l3d1)/l3^2,(l3y1*l3-l3y*l3d1)/l3^2,(l3z1*l3-l3z*l3d1)/l3^2].';
n4d=[(l4x1*l4-l4x*l4d1)/l4^2,(l4y1*l4-l4y*l4d1)/l4^2,(l4z1*l4-l4z*l4d1)/l4^2].';
n5d=[(l5x1*l5-l5x*l5d1)/l5^2,(l5y1*l5-l5y*l5d1)/l5^2,(l5z1*l5-l5z*l5d1)/l5^2].';
on66=zeros(6,6);on62=zeros(6,2);on63=zeros(6,3);on16=zeros(1,6);
on36=zeros(3,6);on33=zeros(3,3);on13=zeros(1,3);on12=zeros(1,2);
on23=zeros(2,3);on32=zeros(3,2);on26=zeros(2,6);on1818=zeros(18,18);on2929=zeros(29,29);
on188=zeros(18,8);on117=zeros(1,17);on116=zeros(11,6);on611=zeros(6,11);on151=zeros(15,1);
on187=zeros(18,7);on1816=zeros(16,16);on216=zeros(2,16);on1816=zeros(18,16);on316=zeros(3,16);
e66=eye(6);e22=eye(2);e33=eye(3);e1111=eye(11);e1616=eye(16);e1515=eye(15);
Dheng=;
Aijdanyuan1=;%18*32
Aijdanyuan2=,on187];
Aijdanyuan3=]; %Aijdanyuan1 2 3为3个单元的支链广义坐标转换关系矩阵；
Aijdanyuan03=]; %18*32
e44=eye(4);on34=zeros(3,4);on256=zeros(25,6);on46=zeros(4,6);on425=zeros(4,25);on254=zeros(25,4);on325=zeros(3,25);e2525=eye(25);
Js1=];%Js1 Js2 ...Js5为机构系统运动学约束条件矩阵
Js2=];
Js3=];
Js4=];
Js5=];
R1=;
R2=;
R3=;
R4=;
R5=;
Mi1heng=zeros(32,32);Mi2heng=zeros(32,32);Mi3heng=zeros(32,32);Mi4heng=zeros(32,32);Mi5heng=zeros(32,32);
Ci1heng=zeros(32,32);Ci2heng=zeros(32,32);Ci3heng=zeros(32,32);Ci4heng=zeros(32,32);Ci5heng=zeros(32,32);
Ki1heng=zeros(32,32);Ki2heng=zeros(32,32);Ki3heng=zeros(32,32);Ki4heng=zeros(32,32);Ki5heng=zeros(32,32);
Qi1heng=zeros(32,1);Qi2heng=zeros(32,1);Qi3heng=zeros(32,1);Qi4heng=zeros(32,1);Qi5heng=zeros(32,1); 回复 3 # weideyong8 的帖子
for i=1:3%循环
syms x%Lb=0.76Ls=0.44表示杆摆动杆 伸缩杆上的单元长度 对摆动杆上的单元 位移型函数以及一些相关矩阵的相乘,开始求积分Iijp表示对x轴的惯性矩
Pmidu=7801;
aij1=4.4*10^-3;
Iijp1=3.3*10^-2; %单位kg.m^2
Iijy1=1.25;
Iijz1=0.53;
E=2.11*10^11;
G=8*10^10;
m0=105.85;%动平台的质量
Ixx=2.74; Ixy=2.34*10^-8; Ixz=1.62*10^-2;%动平台的质量的各轴惯性矩
Iyx=2.34*10^-8; Iyy=1.92; Iyz=3.37*10^-7;
Izx=1.62*10^-2; Izy=3.37*10^-7; Izz=1.85;
Lb=0.76;
if i>1
Lb=0.44;
aij1=1.96*10^-3; %单位m^2
Iijp1=1.67*10^-3;
Iijy1=1.25;
Iijz1=0.53; %单位kg.m^2
end
if i==1
eb=x/Lb;
nb1=1-10*eb^3+15*eb^4-6*eb^5;
nb2=Lb*(eb-6*eb^3+8*eb^4-3*eb^5);
nb3=0.5*Lb^2*(eb^2-3*eb^3+3*eb^4-eb^5);
nb4=10*eb^3-15*eb^4+6*eb^5;
nb5=Lb*(-4*eb^3+7*eb^4-3*eb^5);
nb6=0.5*Lb^2*(eb^3-2*eb^4+eb^5);
nb7=1-3*eb^2+2*eb^3;
nb8=Lb*(eb-2*eb^2+eb^3);
nb9=3*eb^2-2*eb^3;
nb10=Lb*(-eb^2+eb^3);
Nb1=;
Nb2=;
Nb3=;
Nb4=;
Nbr=.'; %3*18
Aij=;
NAANbij1=Nbr.'*Aij*Nbr; %18*18
NAANbij2=Nbr.'*Aij*Nbr; %18*18
NAANbij3=Nbr.'*Aij*Nbr; %18*18
NAANbij4=Nbr.'*Aij*Nbr; %18*18
NAANbij5=Nbr.'*Aij*Nbr; %18*18Aij代表Aij5.'*Aij5 不管怎么相乘都是单位矩阵 因此这么写为了省时间
NBBNbij1=Nb4.'*Nb4;%18*18Bij1.'*Bij1=1
NBBNbij2=Nb4.'*Nb4;%18*18
NBBNbij3=Nb4.'*Nb4;%18*18
NBBNbij4=Nb4.'*Nb4;%18*18
NBBNbij5=Nb4.'*Nb4;%18*18
NGANbij1=Nbr.'*GAij1*Nbr; %18*18GAij1=Gij1.'*Aij1
NGANbij2=Nbr.'*GAij2*Nbr; %18*18
NGANbij3=Nbr.'*GAij3*Nbr; %18*18
NGANbij4=Nbr.'*GAij4*Nbr; %18*18
NGANbij5=Nbr.'*GAij5*Nbr; %18*18
NDBNbij1=0; %18*18
NDBNbij2=0; %18*18
NDBNbij3=0; %18*18
NDBNbij4=0; %18*18
NDBNbij5=0; %18*18
NGGNbij1=Nbr.'*GGij1*Nbr; %18*18GGij1=Gij1.'*Gij1
NGGNbij2=Nbr.'*GGij2*Nbr; %18*18
NGGNbij3=Nbr.'*GGij3*Nbr; %18*18
NGGNbij4=Nbr.'*GGij4*Nbr; %18*18
NGGNbij5=Nbr.'*GGij5*Nbr; %18*18
NDDNbij1=Nb4.'*DDij1*Nb4; %18*18
NDDNbij2=Nb4.'*DDij2*Nb4; %18*18
NDDNbij3=Nb4.'*DDij3*Nb4; %18*18
NDDNbij4=Nb4.'*DDij4*Nb4; %18*18
NDDNbij5=Nb4.'*DDij5*Nb4; %18*18这些都是列单元动能表达式时的一些矩阵相乘以方便以后用到
nGNbij1=n1d.'*Gij1*Nbr;
nGNbij2=n2d.'*Gij2*Nbr;
nGNbij3=n3d.'*Gij3*Nbr;
nGNbij4=n4d.'*Gij4*Nbr;
nGNbij5=n5d.'*Gij5*Nbr;
DGNbij1=Dij1.'*Gij1*Nbr;
DGNbij2=Dij2.'*Gij2*Nbr;
DGNbij3=Dij3.'*Gij3*Nbr;
DGNbij4=Dij4.'*Gij4*Nbr;
DGNbij5=Dij5.'*Gij5*Nbr;
end
%在摆动杆上求之此处的L是摆动杆上的单元长度与伸缩杆上L长度不一样
intNAANbij1=int(NAANbij1,x,0,Lb);
intNBBNbij1=int(NBBNbij1,x,0,Lb);
Pijb1=Pmidu.*(aij1.*intNAANbij1+Iijp1.*intNBBNbij1);
intNAANbij2=int(NAANbij2,x,0,Lb);
intNBBNbij2=int(NBBNbij2,x,0,Lb);
Pijb2=Pmidu.*(aij1.*intNAANbij2+Iijp1.*intNBBNbij2);
intNAANbij3=int(NAANbij3,x,0,Lb);
intNBBNbij3=int(NBBNbij3,x,0,Lb);
Pijb3=Pmidu.*(aij1.*intNAANbij3+Iijp1.*intNBBNbij3);
intNAANbij4=int(NAANbij4,x,0,Lb);
intNBBNbij4=int(NBBNbij4,x,0,Lb);
Pijb4=Pmidu.*(aij1.*intNAANbij4+Iijp1.*intNBBNbij4);
intNAANbij5=int(NAANbij5,x,0,Lb);
intNBBNbij5=int(NBBNbij5,x,0,Lb);
Pijb5=Pmidu.*(aij1.*intNAANbij5+Iijp1.*intNBBNbij5);
Bijb1pie=Pmidu*(aij1*int(NGANbij1,x,0,Lb)+Iijp1*int(NDBNbij1,x,0,Lb));
Bijb2pie=Pmidu*(aij1*int(NGANbij2,x,0,Lb)+Iijp1*int(NDBNbij2,x,0,Lb));
Bijb3pie=Pmidu*(aij1*int(NGANbij3,x,0,Lb)+Iijp1*int(NDBNbij3,x,0,Lb));
Bijb4pie=Pmidu*(aij1*int(NGANbij4,x,0,Lb)+Iijp1*int(NDBNbij4,x,0,Lb));
Bijb5pie=Pmidu*(aij1*int(NGANbij5,x,0,Lb)+Iijp1*int(NDBNbij5,x,0,Lb));
Bijb1jian=Pmidu*(aij1*int(NGGNbij1,x,0,Lb)+Iijp1*int(NDDNbij1,x,0,Lb));
Bijb2jian=Pmidu*(aij1*int(NGGNbij2,x,0,Lb)+Iijp1*int(NDDNbij2,x,0,Lb));
Bijb3jian=Pmidu*(aij1*int(NGGNbij3,x,0,Lb)+Iijp1*int(NDDNbij3,x,0,Lb));
Bijb4jian=Pmidu*(aij1*int(NGGNbij4,x,0,Lb)+Iijp1*int(NDDNbij4,x,0,Lb));
Bijb5jian=Pmidu*(aij1*int(NGGNbij5,x,0,Lb)+Iijp1*int(NDDNbij5,x,0,Lb));
回复 4 # weideyong8 的帖子
Obij1=;
Obij2=;
Obij3=;
Obij4=;
Obij5=;
Kbij1=E*Iijy1*int(diff(Nb2,x,2).'*diff(Nb2,x,2),x,0,Lb)+E*Iijz1*int(diff(Nb3,x,2).'*diff(Nb3,x,2),x,0,Lb)+G*Iijp1*int(diff(Nb4,x,2).'*diff(Nb4,x,2),x,0,Lb);% 弯曲变形刚度矩阵 Kbij1五个支链杆摆动杆都是这个刚度
if i==1
Dhb1=Pmidu*aij1*int((x*DGNbij1),x,0,Lb);%力那边的
Dhb2=Pmidu*aij1*int((x*DGNbij2),x,0,Lb);
Dhb3=Pmidu*aij1*int((x*DGNbij3),x,0,Lb);
Dhb4=Pmidu*aij1*int((x*DGNbij4),x,0,Lb);
Dhb5=Pmidu*aij1*int((x*DGNbij5),x,0,Lb);
Qxbij1=Dhb1.'-Pmidu*aij1*g*Lb*Obij1.';%单元运动微分等号右边的式子 其中一个力矩阵摆动杆
Qxbij2=Dhb2.'-Pmidu*aij1*g*Lb*Obij2.';
Qxbij3=Dhb3.'-Pmidu*aij1*g*Lb*Obij3.';
Qxbij4=Dhb4.'-Pmidu*aij1*g*Lb*Obij4.';
Qxbij5=Dhb5.'-Pmidu*aij1*g*Lb*Obij5.';%这是力的
h1b=Rij1heng*Aijdanyuan1*R1*q1i;%18*1（18*18）*（18*32）*（32*35）*（35*1）
h2b=Rij2heng*Aijdanyuan1*R2*q2i;
h3b=Rij3heng*Aijdanyuan1*R3*q3i;
h4b=Rij4heng*Aijdanyuan1*R4*q4i;
h5b=Rij5heng*Aijdanyuan1*R5*q5i;%h1..h5表示五个支链上的单元的广义坐标。此处只算摆动杆上的单元1的刚度2矩阵，q1i..q5i表示五个支链上的系统广义坐标，包括动平台的6个广义坐标；
diffNb1x1=diff(Nb1,x,1);diffNb2x1=diff(Nb2,x,1);diffNb3x1=diff(Nb3,x,1);
Nb123jia=diffNb1x1.'*diffNb1x1+diffNb2x1.'*diffNb2x1+diffNb3x1.'*diffNb3x1;
intkb1ij=(diffNb1x1.'+0.5*Nb123jia*h1b)*(diffNb1x1+0.5*h1b.'*Nb123jia);
intkb2ij=(diffNb1x1.'+0.5*Nb123jia*h2b)*(diffNb1x1+0.5*h2b.'*Nb123jia);
intkb3ij=(diffNb1x1.'+0.5*Nb123jia*h3b)*(diffNb1x1+0.5*h3b.'*Nb123jia);
intkb4ij=(diffNb1x1.'+0.5*Nb123jia*h4b)*(diffNb1x1+0.5*h4b.'*Nb123jia);
intkb5ij=(diffNb1x1.'+0.5*Nb123jia*h5b)*(diffNb1x1+0.5*h5b.'*Nb123jia);
Kb1ij=E*aij1*int(intkb1ij,x,0,Lb); %Kb1ij中的1代表支链1
Kb2ij=E*aij1*int(intkb2ij,x,0,Lb); %Kb2ij中的2代表支链2
Kb3ij=E*aij1*int(intkb3ij,x,0,Lb); %Kb3ij中的3代表支链3
Kb4ij=E*aij1*int(intkb4ij,x,0,Lb); %Kb4ij中的4代表支链4
Kb5ij=E*aij1*int(intkb5ij,x,0,Lb); %Kb5ij中的5代表支链5
Kijb1=Kbij1+Kb1ij;%Kijb1=Kbij1+Kb1ij表示摆动杆上的刚度矩阵等于弯曲扭转刚度+拉伸刚度矩阵（计入了几何非线性）
Kijb2=Kbij1+Kb2ij;
Kijb3=Kbij1+Kb3ij;
Kijb4=Kbij1+Kb4ij;
Kijb5=Kbij1+Kb5ij;%求解的单元1的刚度矩阵=弯曲的+拉伸的
elseif i==2
Dhb1=Pmidu*aij1*((l1-L2i)*int(nGNbij1,x,0,Lb)+int((x*DGNbij1),x,0,Lb));%Dh2s1中间的2代表单元2
Dhb2=Pmidu*aij1*((l2-L2i)*int(nGNbij2,x,0,Lb)+int((x*DGNbij2),x,0,Lb));
Dhb3=Pmidu*aij1*((l3-L2i)*int(nGNbij3,x,0,Lb)+int((x*DGNbij3),x,0,Lb));
Dhb4=Pmidu*aij1*((l4-L2i)*int(nGNbij4,x,0,Lb)+int((x*DGNbij4),x,0,Lb));
Dhb5=Pmidu*aij1*((l5-L2i)*int(nGNbij5,x,0,Lb)+int((x*DGNbij5),x,0,Lb));
Qxbij1=Dhb1.'-Pmidu*aij1*g*Lb*Obij1.';%单元运动微分等号右边的式子 其中一个力矩阵摆动杆
Qxbij2=Dhb2.'-Pmidu*aij1*g*Lb*Obij2.';
Qxbij3=Dhb3.'-Pmidu*aij1*g*Lb*Obij3.';
Qxbij4=Dhb4.'-Pmidu*aij1*g*Lb*Obij4.';
Qxbij5=Dhb5.'-Pmidu*aij1*g*Lb*Obij5.';
h1b=Rij1heng*Aijdanyuan2*R1*q1i;%18*1（18*18）*（18*32）*（32*35）*（35*1）j2表示第二个单元
h2b=Rij2heng*Aijdanyuan2*R2*q2i;
h3b=Rij3heng*Aijdanyuan2*R3*q3i;
h4b=Rij4heng*Aijdanyuan2*R4*q4i;
h5b=Rij5heng*Aijdanyuan2*R5*q5i;%h1..h5表示五个支链上的单元的广义坐标。此处只算摆动杆上的单元1的刚度2矩阵，q1i..q5i表示五个支链上的系统广义坐标，包括动平台的6个广义坐标；
diffNb1x1=diff(Nb1,x,1);diffNb2x1=diff(Nb2,x,1);diffNb3x1=diff(Nb3,x,1);
Nb123jia=diffNb1x1.'*diffNb1x1+diffNb2x1.'*diffNb2x1+diffNb3x1.'*diffNb3x1;
intkb1ij=(diffNb1x1.'+0.5*Nb123jia*h1b)*(diffNb1x1+0.5*h1b.'*Nb123jia);
intkb2ij=(diffNb1x1.'+0.5*Nb123jia*h2b)*(diffNb1x1+0.5*h2b.'*Nb123jia);
intkb3ij=(diffNb1x1.'+0.5*Nb123jia*h3b)*(diffNb1x1+0.5*h3b.'*Nb123jia);
intkb4ij=(diffNb1x1.'+0.5*Nb123jia*h4b)*(diffNb1x1+0.5*h4b.'*Nb123jia);
intkb5ij=(diffNb1x1.'+0.5*Nb123jia*h5b)*(diffNb1x1+0.5*h5b.'*Nb123jia);
Kb1ij=E*aij1*int(intkb1ij,x,0,Lb); %Kb1ij中的1代表支链1
Kb2ij=E*aij1*int(intkb2ij,x,0,Lb); %Kb2ij中的2代表支链2
Kb3ij=E*aij1*int(intkb3ij,x,0,Lb); %Kb3ij中的3代表支链3
Kb4ij=E*aij1*int(intkb4ij,x,0,Lb); %Kb4ij中的4代表支链4
Kb5ij=E*aij1*int(intkb5ij,x,0,Lb); %Kb5ij中的5代表支链5
Kijb1=Kbij1+Kb1ij;
Kijb2=Kbij1+Kb2ij;
Kijb3=Kbij1+Kb3ij;
Kijb4=Kbij1+Kb4ij;
Kijb5=Kbij1+Kb5ij;%求解的单元2的刚度矩阵=弯曲的+拉伸的
else i==3
Dhb1=Pmidu*aij1*((l1-L2i+Lb)*int(nGNbij1,x,0,Lb)+int((x*DGNbij1),x,0,Lb));%Dh3s1中间的3代表单元3
Dhb2=Pmidu*aij1*((l2-L2i+Lb)*int(nGNbij2,x,0,Lb)+int((x*DGNbij2),x,0,Lb));
Dhb3=Pmidu*aij1*((l3-L2i+Lb)*int(nGNbij3,x,0,Lb)+int((x*DGNbij3),x,0,Lb));
Dhb4=Pmidu*aij1*((l4-L2i+Lb)*int(nGNbij4,x,0,Lb)+int((x*DGNbij4),x,0,Lb));
Dhb5=Pmidu*aij1*((l5-L2i+Lb)*int(nGNbij5,x,0,Lb)+int((x*DGNbij5),x,0,Lb));
Qxbij1=Dhb1.'-Pmidu*aij1*g*Lb*Obij1.';%单元运动微分等号右边的式子 其中一个力矩阵摆动杆
Qxbij2=Dhb2.'-Pmidu*aij1*g*Lb*Obij2.';
Qxbij3=Dhb3.'-Pmidu*aij1*g*Lb*Obij3.';
Qxbij4=Dhb4.'-Pmidu*aij1*g*Lb*Obij4.';
Qxbij5=Dhb5.'-Pmidu*aij1*g*Lb*Obij5.'; 回复 5 # weideyong8 的帖子
h1b=Rij1heng*Aijdanyuan3*R1*q1i;%18*1（18*18）*（18*32）*（32*35）*（35*1）j2表示第二个单元
h2b=Rij2heng*Aijdanyuan03*R2*q2i;
h3b=Rij3heng*Aijdanyuan03*R3*q3i;
h4b=Rij4heng*Aijdanyuan03*R4*q4i;
h5b=Rij5heng*Aijdanyuan03*R5*q5i;%h1..h5表示五个支链上的单元的广义坐标。此处只算摆动杆上的单元1的刚度2矩阵，q1i..q5i表示五个支链上的系统广义坐标，包括动平台的6个广义坐标；
diffNb1x1=diff(Nb1,x,1);diffNb2x1=diff(Nb2,x,1);diffNb3x1=diff(Nb3,x,1);
Nb123jia=diffNb1x1.'*diffNb1x1+diffNb2x1.'*diffNb2x1+diffNb3x1.'*diffNb3x1;
intkb1ij=(diffNb1x1.'+0.5*Nb123jia*h1b)*(diffNb1x1+0.5*h1b.'*Nb123jia);
intkb2ij=(diffNb1x1.'+0.5*Nb123jia*h2b)*(diffNb1x1+0.5*h2b.'*Nb123jia);
intkb3ij=(diffNb1x1.'+0.5*Nb123jia*h3b)*(diffNb1x1+0.5*h3b.'*Nb123jia);
intkb4ij=(diffNb1x1.'+0.5*Nb123jia*h4b)*(diffNb1x1+0.5*h4b.'*Nb123jia);
intkb5ij=(diffNb1x1.'+0.5*Nb123jia*h5b)*(diffNb1x1+0.5*h5b.'*Nb123jia);
Kb1ij=E*aij1*int(intkb1ij,x,0,Lb); %Kb1ij中的1代表支链1
Kb2ij=E*aij1*int(intkb2ij,x,0,Lb); %Kb2ij中的2代表支链2
Kb3ij=E*aij1*int(intkb3ij,x,0,Lb); %Kb3ij中的3代表支链3
Kb4ij=E*aij1*int(intkb4ij,x,0,Lb); %Kb4ij中的4代表支链4
Kb5ij=E*aij1*int(intkb5ij,x,0,Lb); %Kb5ij中的5代表支链5
Kijb1=Kbij1+Kb1ij;
Kijb2=Kbij1+Kb2ij;
Kijb3=Kbij1+Kb3ij;
Kijb4=Kbij1+Kb4ij;
Kijb5=Kbij1+Kb5ij;%求解的单元3的刚度矩阵=弯曲的+拉伸的
end
Mbij1pie=Rij1heng.'*Pijb1*Rij1heng;%摆动杆上的一个小质量矩阵
Mbij2pie=Rij2heng.'*Pijb2*Rij2heng;
Mbij3pie=Rij3heng.'*Pijb3*Rij3heng;
Mbij4pie=Rij4heng.'*Pijb4*Rij4heng;
Mbij5pie=Rij5heng.'*Pijb5*Rij5heng;
Cbij1pie=2*Rij1heng.'*Pijb1*Rij1heng1-2*Rij1heng.'*Bijb1pie*Rij1heng;%摆动杆上的一个小阻尼矩阵
Cbij2pie=2*Rij2heng.'*Pijb2*Rij2heng1-2*Rij2heng.'*Bijb2pie*Rij2heng;
Cbij3pie=2*Rij3heng.'*Pijb3*Rij3heng1-2*Rij3heng.'*Bijb3pie*Rij3heng;
Cbij4pie=2*Rij4heng.'*Pijb4*Rij4heng1-2*Rij4heng.'*Bijb4pie*Rij4heng;
Cbij5pie=2*Rij5heng.'*Pijb5*Rij5heng1-2*Rij5heng.'*Bijb5pie*Rij5heng;
Qbij1pie=Rij1heng.'*Qxbij1;%18*1
Qbij2pie=Rij2heng.'*Qxbij2;
Qbij3pie=Rij3heng.'*Qxbij3;
Qbij4pie=Rij4heng.'*Qxbij4;
Qbij5pie=Rij5heng.'*Qxbij5;
RPb1Rheng2=Rij1heng.'*Pijb1*Rij1heng2; RBb1Rheng1=Rij1heng.'*Bijb1pie*Rij1heng1; RKBb1Rheng=Rij1heng.'*(Kijb1-Bijb1jian)*Rij1heng;
RPb2Rheng2=Rij2heng.'*Pijb2*Rij2heng2; RBb2Rheng1=Rij2heng.'*Bijb2pie*Rij2heng1; RKBb2Rheng=Rij2heng.'*(Kijb2-Bijb2jian)*Rij2heng;
RPb3Rheng2=Rij3heng.'*Pijb3*Rij3heng2; RBb3Rheng1=Rij3heng.'*Bijb3pie*Rij3heng1; RKBb3Rheng=Rij3heng.'*(Kijb3-Bijb3jian)*Rij3heng;
RPb4Rheng2=Rij4heng.'*Pijb4*Rij4heng2; RBb4Rheng1=Rij4heng.'*Bijb4pie*Rij4heng1; RKBb4Rheng=Rij4heng.'*(Kijb4-Bijb4jian)*Rij4heng;
RPb5Rheng2=Rij5heng.'*Pijb5*Rij5heng2; RBb5Rheng1=Rij5heng.'*Bijb5pie*Rij5heng1; RKBb5Rheng=Rij5heng.'*(Kijb5-Bijb5jian)*Rij5heng;
Kbij1pie=RPb1Rheng2-2*RBb1Rheng1+RKBb1Rheng;%摆动杆上的一个小刚度矩阵此处Kijb1=Kbij1+一个拉伸刚度矩阵
Kbij2pie=RPb2Rheng2-2*RBb2Rheng1+RKBb2Rheng;
Kbij3pie=RPb3Rheng2-2*RBb3Rheng1+RKBb3Rheng;
Kbij4pie=RPb4Rheng2-2*RBb4Rheng1+RKBb4Rheng;
Kbij5pie=RPb5Rheng2-2*RBb5Rheng1+RKBb5Rheng;
if i==1
Mij1danyuan1jian=Aijdanyuan1.'*Mbij1pie*Aijdanyuan1;
Mij2danyuan1jian=Aijdanyuan1.'*Mbij2pie*Aijdanyuan1;
Mij3danyuan1jian=Aijdanyuan1.'*Mbij3pie*Aijdanyuan1;
Mij4danyuan1jian=Aijdanyuan1.'*Mbij4pie*Aijdanyuan1;
Mij5danyuan1jian=Aijdanyuan1.'*Mbij5pie*Aijdanyuan1;
Cij1danyuan1jian=Aijdanyuan1.'*Cbij1pie*Aijdanyuan1;
Cij2danyuan1jian=Aijdanyuan1.'*Cbij2pie*Aijdanyuan1;
Cij3danyuan1jian=Aijdanyuan1.'*Cbij3pie*Aijdanyuan1;
Cij4danyuan1jian=Aijdanyuan1.'*Cbij4pie*Aijdanyuan1;
Cij5danyuan1jian=Aijdanyuan1.'*Cbij5pie*Aijdanyuan1;
回复 6 # weideyong8 的帖子
Kij1danyuan1jian=Aijdanyuan1.'*Kbij1pie*Aijdanyuan1;
Kij2danyuan1jian=Aijdanyuan1.'*Kbij2pie*Aijdanyuan1;
Kij3danyuan1jian=Aijdanyuan1.'*Kbij3pie*Aijdanyuan1;
Kij4danyuan1jian=Aijdanyuan1.'*Kbij4pie*Aijdanyuan1;
Kij5danyuan1jian=Aijdanyuan1.'*Kbij5pie*Aijdanyuan1;
Qij1danyuan1jian=Aijdanyuan1.'*Qbij1pie;%各个单元1 2 3上的力矩阵
Qij2danyuan1jian=Aijdanyuan1.'*Qbij2pie;
Qij3danyuan1jian=Aijdanyuan1.'*Qbij3pie;
Qij4danyuan1jian=Aijdanyuan1.'*Qbij4pie;
Qij5danyuan1jian=Aijdanyuan1.'*Qbij5pie;
elseif i==2
Mij1danyuan1jian=Aijdanyuan2.'*Mbij1pie*Aijdanyuan2;
Mij2danyuan1jian=Aijdanyuan2.'*Mbij2pie*Aijdanyuan2;
Mij3danyuan1jian=Aijdanyuan2.'*Mbij3pie*Aijdanyuan2;
Mij4danyuan1jian=Aijdanyuan2.'*Mbij4pie*Aijdanyuan2;
Mij5danyuan1jian=Aijdanyuan2.'*Mbij5pie*Aijdanyuan2;
Cij1danyuan1jian=Aijdanyuan2.'*Cbij1pie*Aijdanyuan2;
Cij2danyuan1jian=Aijdanyuan2.'*Cbij2pie*Aijdanyuan2;
Cij3danyuan1jian=Aijdanyuan2.'*Cbij3pie*Aijdanyuan2;
Cij4danyuan1jian=Aijdanyuan2.'*Cbij4pie*Aijdanyuan2;
Cij5danyuan1jian=Aijdanyuan2.'*Cbij5pie*Aijdanyuan2;
Kij1danyuan1jian=Aijdanyuan2.'*Kbij1pie*Aijdanyuan2;
Kij2danyuan1jian=Aijdanyuan2.'*Kbij2pie*Aijdanyuan2;
Kij3danyuan1jian=Aijdanyuan2.'*Kbij3pie*Aijdanyuan2;
Kij4danyuan1jian=Aijdanyuan2.'*Kbij4pie*Aijdanyuan2;
Kij5danyuan1jian=Aijdanyuan2.'*Kbij5pie*Aijdanyuan2;
Qij1danyuan1jian=Aijdanyuan2.'*Qbij1pie;%各个单元1 2 3上的力矩阵
Qij2danyuan1jian=Aijdanyuan2.'*Qbij2pie;
Qij3danyuan1jian=Aijdanyuan2.'*Qbij3pie;
Qij4danyuan1jian=Aijdanyuan2.'*Qbij4pie;
Qij5danyuan1jian=Aijdanyuan2.'*Qbij5pie;
else i==3
Mij1danyuan1jian=Aijdanyuan3.'*Mbij1pie*Aijdanyuan3;
Mij2danyuan1jian=Aijdanyuan03.'*Mbij2pie*Aijdanyuan03;
Mij3danyuan1jian=Aijdanyuan03.'*Mbij3pie*Aijdanyuan03;
Mij4danyuan1jian=Aijdanyuan03.'*Mbij4pie*Aijdanyuan03;
Mij5danyuan1jian=Aijdanyuan03.'*Mbij5pie*Aijdanyuan03;
Cij1danyuan1jian=Aijdanyuan3.'*Cbij1pie*Aijdanyuan3;
Cij2danyuan1jian=Aijdanyuan03.'*Cbij2pie*Aijdanyuan03;
Cij3danyuan1jian=Aijdanyuan03.'*Cbij3pie*Aijdanyuan03;
Cij4danyuan1jian=Aijdanyuan03.'*Cbij4pie*Aijdanyuan03;
Cij5danyuan1jian=Aijdanyuan03.'*Cbij5pie*Aijdanyuan03;
Kij1danyuan1jian=Aijdanyuan3.'*Kbij1pie*Aijdanyuan3;
Kij2danyuan1jian=Aijdanyuan03.'*Kbij2pie*Aijdanyuan03;
Kij3danyuan1jian=Aijdanyuan03.'*Kbij3pie*Aijdanyuan03;
Kij4danyuan1jian=Aijdanyuan03.'*Kbij4pie*Aijdanyuan03;
Kij5danyuan1jian=Aijdanyuan03.'*Kbij5pie*Aijdanyuan03;
Qij1danyuan1jian=Aijdanyuan3.'*Qbij1pie;%各个单元1 2 3上的力矩阵
Qij2danyuan1jian=Aijdanyuan03.'*Qbij2pie;
Qij3danyuan1jian=Aijdanyuan03.'*Qbij3pie;
Qij4danyuan1jian=Aijdanyuan03.'*Qbij4pie;
Qij5danyuan1jian=Aijdanyuan03.'*Qbij5pie;
end
% Mi1heng=Mij1danyuan1jian+Mij1danyuan2jian+Mij1danyuan3jian;
Mi1heng=Mij1danyuan1jian+Mi1heng;
Mi2heng=Mij2danyuan1jian+Mi2heng;%Mij2danyuan2jian+Mij2danyuan3jian;
Mi3heng=Mij3danyuan1jian+Mi3heng;%Mij3danyuan2jian+Mij3danyuan3jian;
Mi4heng=Mij4danyuan1jian+Mi4heng;%Mij4danyuan2jian+Mij4danyuan3jian;
Mi5heng=Mij5danyuan1jian+Mi5heng;%Mij5danyuan2jian+Mij5danyuan3jian;
Ci1heng=Cij1danyuan1jian+Ci1heng;
Ci2heng=Cij2danyuan1jian+Ci2heng;
Ci3heng=Cij3danyuan1jian+Ci3heng;
Ci4heng=Cij4danyuan1jian+Ci4heng;
Ci5heng=Cij5danyuan1jian+Ci5heng;
Ki1heng=Kij1danyuan1jian+Ki1heng;
Ki2heng=Kij2danyuan1jian+Ki2heng;
Ki3heng=Kij3danyuan1jian+Ki3heng;
Ki4heng=Kij4danyuan1jian+Ki4heng;
Ki5heng=Kij5danyuan1jian+Ki5heng;
Qi1heng=Qij1danyuan1jian+Qi1heng;
Qi2heng=Qij2danyuan1jian+Qi2heng;%Qij2danyuan2jian+Qij2danyuan3jian;
Qi3heng=Qij3danyuan1jian+Qi3heng;%Qij3danyuan2jian+Qij3danyuan3jian;
Qi4heng=Qij4danyuan1jian+Qi4heng;%Qij4danyuan2jian+Qij4danyuan3jian;
Qi5heng=Qij5danyuan1jian+Qi5heng;%Qij5danyuan2jian+Qij5danyuan3jian;
end 回复 7 # weideyong8 的帖子
M0=;%6*6
M1=R1.'*Mi1heng*R1;%35*35
M2=R2.'*Mi2heng*R2;
M3=R3.'*Mi3heng*R3;
M4=R4.'*Mi4heng*R4;
M5=R5.'*Mi5heng*R5;%35*35
C1=R1.'*Ci1heng*R1;%35*35
C2=R2.'*Ci2heng*R2;%35*35
C3=R3.'*Ci3heng*R3;%35*35
C4=R4.'*Ci4heng*R4;%35*35
C5=R5.'*Ci5heng*R5;%35*35
K1=R1.'*Ki1heng*R1;%35*35
K2=R2.'*Ki2heng*R2;
K3=R3.'*Ki3heng*R3;
K4=R4.'*Ki4heng*R4;
K5=R5.'*Ki5heng*R5;%35*35
Q1=R1.'*Qi1heng+1;%35*1
Q2=R2.'*Qi2heng+1;
Q3=R3.'*Qi3heng+1;
Q4=R4.'*Qi4heng+1;
Q5=R5.'*Qi5heng+1; %35*1 记住此处的Q1 --Q5只是广义力中的通过微分求导计算出来的部分力相当于方程运算简化整理得出的
c1=0;c2=0.00023; %c1 c2为阻尼系数 c1=2.0*10^(-3)c2=3.0*10^(-4)
M=;
CJ=;
K=;
Q=;
M=vpa(M,20);
K=vpa(K,20);
CJ=vpa(CJ,20);
Kc2=c2.*K;
Kc2=vpa(Kc2,20);
% Mc1=c1.*M;
% MK=Mc1+Kc2;
% C=CJ+MK;%c1 c2为阻尼系数 c1=2.0*10^(-3)c2=3.0*10^(-4)
C=CJ+Kc2;
Kyou=K+d0*M+d1*C;
qqian=q;
if j>1
q=Kyou\Q;
dddq=d0*q+d2*q1d+d3*q2d;
ddq=d1*q+d4*q1d+d5*q2d;
Mdq=M*dddq;
Cdq=C*ddq;
Q=Q+Mdq+Cdq;
KL=Kyou;
q1i=;
q2i=;
q3i=;
q4i=;
q5i=;
hold on
end
end 回复 8 # weideyong8 的帖子
程序已经发帖完毕跪求高手帮我审审
程序内容没有什么问题 只是太多了，是求解一个非线性弹性动力学方程的一个求解，利用newmark方法但是找不到出错原因希望高手看看 回复 9 # weideyong8 的帖子
高手帮我运行一下 程序 找找原因 谢谢还有如何提高运行速度，节省空间？？ 这种有较多代码的程序没详细注释还真是很难读懂的！
个人感觉程序运行速度可能和你程序的符号运算有关系！ 回复 1 # weideyong8 的帖子
个人水平专业有限, LZ这麼多代码, 应该花很长时间才能编好吧!
那别人可能需花好几倍时间才能消化吧! 所以想法简化LZ的问题吧, 不然... 回复 12 # ChaChing 的帖子
我正在修改，改成循环结构，简化一下程序，再试试
回复 12 # ChaChing 的帖子
时间很长，但是慢慢写的，是一个关于五自由度并联机床的方程，求解，程序运行报错说：解不唯一，编的时间长，自己的耐心也消磨很多了... 回复 11 # meiyongyuandeze 的帖子
虽然有符号运算 但是中间的符号运算x在计算中是可以消除的，时间t是赋值运算我简化简化程序 看看怎么样
再有问题请教你们谢谢回复！！！！
页:
[1]