声振论坛

 找回密码
 我要加入

QQ登录

只需一步,快速开始

查看: 1860|回复: 14

[编程技巧] 求高手看看这个程序,运行很慢,不出结果不出图,错误原因看不懂

[复制链接]
发表于 2011-4-24 22:05 | 显示全部楼层 |阅读模式

马上注册,结交更多好友,享用更多功能,让你轻松玩转社区。

您需要 登录 才可以下载或查看,没有账号?我要加入

x
clear
q01=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=[q1i(1:29,:);q2i(1:29,:);q3i(1:29,:);q4i(1:29,:);q5i(1:29,:);q01;q02;q03;q04;q05;q06];%放在循环里面
    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=[0 w 0].';
Pu2=[0 rA*cos(pi/4) rA*sin(pi/4)].';
Pu3=[0 -rA*cos(pi/4) rA*sin(pi/4)].';
Pu4=[0 -rA*cos(pi/4) -rA*sin(pi/4)].';
Pu5=[0 rA*cos(pi/4) -rA*sin(pi/4)].';
Ps1=[rsx 0 rsz].';
Ps2=[0 rB*sin(2*pi/5) -rB*cos(2*pi/5)].';
Ps3=[0 rB*sin(4*pi/5) -rB*cos(4*pi/5)].';
Ps4=[0 rB*sin(6*pi/5) -rB*cos(6*pi/5)].';
Ps5=[0 rB*sin(8*pi/5) -rB*cos(8*pi/5)].';  %3*1矩阵
R=[cos(a)*cos(b) cos(a)*sin(b) sin(a);sin(a)*cos(b) sin(a)*sin(b) -cos(a);-sin(b) cos(b) 0];  %3*3矩阵
Po=[xb yb zb].';  %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次导数
回复
分享到:

使用道具 举报

 楼主| 发表于 2011-4-24 22:05 | 显示全部楼层
回复 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=[Rij1,on,on,on,on,on;on,Rij1,on,on,on,on;on,on,Rij1,on,on,on;on,on,on,Rij1,on,on;on,on,on,on,Rij1,on;on,on,on,on,on,Rij1];%表示单元广义坐标与单元系统坐标系下的转换矩阵
Rij2heng=[Rij2,on,on,on,on,on;on,Rij2,on,on,on,on;on,on,Rij2,on,on,on;on,on,on,Rij2,on,on;on,on,on,on,Rij2,on;on,on,on,on,on,Rij2];
Rij3heng=[Rij3,on,on,on,on,on;on,Rij3,on,on,on,on;on,on,Rij3,on,on,on;on,on,on,Rij3,on,on;on,on,on,on,Rij3,on;on,on,on,on,on,Rij3];
Rij4heng=[Rij4,on,on,on,on,on;on,Rij4,on,on,on,on;on,on,Rij4,on,on,on;on,on,on,Rij4,on,on;on,on,on,on,Rij4,on;on,on,on,on,on,Rij4];
Rij5heng=[Rij5,on,on,on,on,on;on,Rij5,on,on,on,on;on,on,Rij5,on,on,on;on,on,on,Rij5,on,on;on,on,on,on,Rij5,on;on,on,on,on,on,Rij5];

Bij1=[cos(theta1)*cos(sheta1);cos(0)*sin(theta1)*cos(sheta1)+sin(0)*sin(sheta1);sin(0)*sin(theta1)*cos(sheta1)-cos(0)*sin(sheta1)];
Bij2=[cos(theta2)*cos(sheta2);cos(pi/4)*sin(theta2)*cos(sheta2)+sin(pi/4)*sin(sheta2);sin(pi/4)*sin(theta2)*cos(sheta2)-cos(pi/4)*sin(sheta2)];
Bij3=[cos(theta3)*cos(sheta3);cos(-pi/4)*sin(theta3)*cos(sheta3)+sin(-pi/4)*sin(sheta3);sin(-pi/4)*sin(theta3)*cos(sheta3)-cos(-pi/4)*sin(sheta3)];
Bij4=[cos(theta4)*cos(sheta4);cos(-3*pi/4)*sin(theta4)*cos(sheta4)+sin(-3*pi/4)*sin(sheta4);sin(-3*pi/4)*sin(theta4)*cos(sheta4)-cos(-3*pi/4)*sin(sheta4)];
Bij5=[cos(theta5)*cos(sheta5);cos(3*pi/4)*sin(theta5)*cos(sheta5)+sin(3*pi/4)*sin(sheta5);sin(3*pi/4)*sin(theta5)*cos(sheta5)-cos(3*pi/4)*sin(sheta5)];   % 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 theta1  theta11 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 theta2  theta21 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 theta3  theta31 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 theta4  theta41 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 theta5  theta51 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=[g11ij1,g12ij1,g13ij1;g21ij1,g22ij1,g23ij1;g31ij1,g32ij1,g33ij1];%对Gij1的1次求导
Gij22=[g11ij2,g12ij2,g13ij2;g21ij2,g22ij2,g23ij2;g31ij2,g32ij2,g33ij2];
Gij32=[g11ij3,g12ij3,g13ij3;g21ij3,g22ij3,g23ij3;g31ij3,g32ij3,g33ij3];
Gij42=[g11ij4,g12ij4,g13ij4;g21ij4,g22ij4,g23ij4;g31ij4,g32ij4,g33ij4];
Gij52=[g11ij5,g12ij5,g13ij5;g21ij5,g22ij5,g23ij5;g31ij5,g32ij2,g33ij5];%对Gij5的1次求导
 楼主| 发表于 2011-4-24 22:06 | 显示全部楼层
回复 2 # weideyong8 的帖子

Rij1heng1=[Gij1.',on,on,on,on,on;on,Gij1.',on,on,on,on;on,on,Gij1.',on,on,on;on,on,on,Gij1.',on,on;on,on,on,on,Gij1.',on;on,on,on,on,on,Gij1.'];
Rij2heng1=[Gij2.',on,on,on,on,on;on,Gij2.',on,on,on,on;on,on,Gij2.',on,on,on;on,on,on,Gij2.',on,on;on,on,on,on,Gij2.',on;on,on,on,on,on,Gij2.'];
Rij3heng1=[Gij3.',on,on,on,on,on;on,Gij3.',on,on,on,on;on,on,Gij3.',on,on,on;on,on,on,Gij3.',on,on;on,on,on,on,Gij3.',on;on,on,on,on,on,Gij3.'];
Rij4heng1=[Gij4.',on,on,on,on,on;on,Gij4.',on,on,on,on;on,on,Gij4.',on,on,on;on,on,on,Gij4.',on,on;on,on,on,on,Gij4.',on;on,on,on,on,on,Gij4.'];
Rij5heng1=[Gij5.',on,on,on,on,on;on,Gij5.',on,on,on,on;on,on,Gij5.',on,on,on;on,on,on,Gij5.',on,on;on,on,on,on,Gij5.',on;on,on,on,on,on,Gij5.'];

Rij1heng2=[Gij12.',on,on,on,on,on;on,Gij12.',on,on,on,on;on,on,Gij12.',on,on,on;on,on,on,Gij12.',on,on;on,on,on,on,Gij12.',on;on,on,on,on,on,Gij12.'];
Rij2heng2=[Gij22.',on,on,on,on,on;on,Gij22.',on,on,on,on;on,on,Gij22.',on,on,on;on,on,on,Gij22.',on,on;on,on,on,on,Gij22.',on;on,on,on,on,on,Gij22.'];
Rij3heng2=[Gij32.',on,on,on,on,on;on,Gij32.',on,on,on,on;on,on,Gij32.',on,on,on;on,on,on,Gij32.',on,on;on,on,on,on,Gij32.',on;on,on,on,on,on,Gij32.'];
Rij4heng2=[Gij42.',on,on,on,on,on;on,Gij42.',on,on,on,on;on,on,Gij42.',on,on,on;on,on,on,Gij42.',on,on;on,on,on,on,Gij42.',on;on,on,on,on,on,Gij42.'];
Rij5heng2=[Gij52.',on,on,on,on,on;on,Gij52.',on,on,on,on;on,on,Gij52.',on,on,on;on,on,on,Gij52.',on,on;on,on,on,on,Gij52.',on;on,on,on,on,on,Gij52.'];
% Gij1  Gij2..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=[0,cos(sheta1)*theta11,-sheta11;-cos(sheta1)*theta11,0,-sin(sheta1)*theta11;sheta11,sin(sheta1)*theta11,0];%GA矩阵相乘
GAij2=[0,cos(sheta2)*theta21,-sheta21;-cos(sheta2)*theta21,0,-sin(sheta2)*theta21;sheta21,sin(sheta2)*theta21,0];
GAij3=[0,cos(sheta3)*theta31,-sheta31;-cos(sheta3)*theta31,0,-sin(sheta3)*theta31;sheta31,sin(sheta3)*theta31,0];
GAij4=[0,cos(sheta4)*theta41,-sheta41;-cos(sheta4)*theta41,0,-sin(sheta4)*theta41;sheta41,sin(sheta4)*theta41,0];
GAij5=[0,cos(sheta5)*theta51,-sheta51;-cos(sheta5)*theta51,0,-sin(sheta5)*theta51;sheta51,sin(sheta5)*theta51,0];

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*Nr  aij1代表摆动杆横截面积4.4*10^(-3)m^2 aij2代表伸缩杆横截面积1.96*10^(-3)m^2
% Pmidu=7801----代表密度
%杆的单位向量
n1=[l1x/l1,l1y/l1,l1z/l1].';
n2=[l2x/l2,l2y/l2,l2z/l2].';
n3=[l3x/l3,l3y/l3,l3z/l3].';
n4=[l4x/l4,l4y/l4,l4z/l4].';
n5=[l5x/l5,l5y/l5,l5z/l5].';
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=[on66,on62,on63,on63;e66,on62,on63,on63;on36,on32,on33,e33;on16,on12,on13,on13;on26,e22,on23,on23];

Aijdanyuan1=[Dheng,on1818];%18*32
Aijdanyuan2=[on188,[e66,on611;on117;on116,e1111],on187];
Aijdanyuan3=[on1816,[e1616;on216]];       %Aijdanyuan1 2 3为3个单元的支链广义坐标转换关系矩阵;
Aijdanyuan03=[on1816,[e1515,on151;on316]]; %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=[e33,[0,zs1,-ys1;-zs1,0,xs1;ys1,-xs1,0]];%Js1 Js2 ...Js5为机构系统运动学约束条件矩阵
Js2=[e33,[0,zs2,-ys2;-zs2,0,xs2;ys2,-xs2,0]];
Js3=[e33,[0,zs3,-ys3;-zs3,0,xs3;ys3,-xs3,0]];
Js4=[e33,[0,zs4,-ys4;-zs4,0,xs4;ys4,-xs4,0]];
Js5=[e33,[0,zs5,-ys5;-zs5,0,xs5;ys5,-xs5,0]];

R1=[e2525,on254,on256;on325,on34,Js1;on425,e44,on46];
R2=[e2525,on254,on256;on325,on34,Js2;on425,e44,on46];
R3=[e2525,on254,on256;on325,on34,Js3;on425,e44,on46];
R4=[e2525,on254,on256;on325,on34,Js4;on425,e44,on46];
R5=[e2525,on254,on256;on325,on34,Js5;on425,e44,on46];

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);
 楼主| 发表于 2011-4-24 22:06 | 显示全部楼层
回复 3 # weideyong8 的帖子

for i=1:3  %循环
   syms x  %  Lb=0.76  Ls=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=[1-eb 0 0 0 0 0 0 0 0 eb 0 0 0 0 0 0 0 0];
Nb2=[0 nb1 0 0 0 nb2 0 0 nb3 0 nb4 0 0 0 nb5 0 0 nb6];
Nb3=[0 0 nb1 0 nb2 0 0 nb3 0 0 0 nb4 0 nb5 0 0 nb6 0];
Nb4=[0 0 0 nb7 0 0 nb8 0 0 0 0 0 nb9 0 0 nb10 0 0];
Nbr=[Nb1.', Nb2.',Nb3.'].';   %3*18
Aij=[1,0,0;0,1,0;0,0,1];
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*18  Aij代表Aij5.'*Aij5   不管怎么相乘都是单位矩阵 因此这么写  为了省时间

NBBNbij1=Nb4.'*Nb4;  %18*18  Bij1.'*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*18  GAij1=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*18  GGij1=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));
 楼主| 发表于 2011-4-24 22:06 | 显示全部楼层
回复 4 # weideyong8 的帖子

Obij1=[0.5*cos(theta1)*cos(sheta1),0.5*(-sin(theta1)),0.5*cos(theta1)*sin(sheta1),0,0.1*cos(theta1)*sin(sheta1)*Lb,0.1*(-sin(theta1))*Lb,0,(1/120)*cos(theta1)*sin(sheta1)*Lb^2,(1/120)*(-sin(theta1))*Lb^2,0.5*cos(theta1)*cos(sheta1),0.5*(-sin(theta1)),0.5*cos(theta1)*sin(sheta1),0,-0.1*cos(theta1)*sin(sheta1)*Lb,-0.1*(-sin(theta1))*Lb,0,(1/120)*cos(theta1)*sin(sheta1)*Lb^2,(1/120)*(-sin(theta1))*Lb^2];
Obij2=[0.5*cos(theta2)*cos(sheta2),0.5*(-sin(theta2)),0.5*cos(theta2)*sin(sheta2),0,0.1*cos(theta2)*sin(sheta2)*Lb,0.1*(-sin(theta2))*Lb,0,(1/120)*cos(theta2)*sin(sheta2)*Lb^2,(1/120)*(-sin(theta2))*Lb^2,0.5*cos(theta2)*cos(sheta2),0.5*(-sin(theta2)),0.5*cos(theta2)*sin(sheta2),0,-0.1*cos(theta2)*sin(sheta2)*Lb,-0.1*(-sin(theta2))*Lb,0,(1/120)*cos(theta2)*sin(sheta2)*Lb^2,(1/120)*(-sin(theta2))*Lb^2];
Obij3=[0.5*cos(theta3)*cos(sheta3),0.5*(-sin(theta3)),0.5*cos(theta3)*sin(sheta3),0,0.1*cos(theta3)*sin(sheta3)*Lb,0.1*(-sin(theta3))*Lb,0,(1/120)*cos(theta3)*sin(sheta3)*Lb^2,(1/120)*(-sin(theta3))*Lb^2,0.5*cos(theta3)*cos(sheta3),0.5*(-sin(theta3)),0.5*cos(theta3)*sin(sheta3),0,-0.1*cos(theta3)*sin(sheta3)*Lb,-0.1*(-sin(theta3))*Lb,0,(1/120)*cos(theta3)*sin(sheta3)*Lb^2,(1/120)*(-sin(theta3))*Lb^2];
Obij4=[0.5*cos(theta4)*cos(sheta4),0.5*(-sin(theta4)),0.5*cos(theta4)*sin(sheta4),0,0.1*cos(theta4)*sin(sheta4)*Lb,0.1*(-sin(theta4))*Lb,0,(1/120)*cos(theta4)*sin(sheta4)*Lb^2,(1/120)*(-sin(theta4))*Lb^2,0.5*cos(theta4)*cos(sheta4),0.5*(-sin(theta4)),0.5*cos(theta4)*sin(sheta4),0,-0.1*cos(theta4)*sin(sheta4)*Lb,-0.1*(-sin(theta4))*Lb,0,(1/120)*cos(theta4)*sin(sheta4)*Lb^2,(1/120)*(-sin(theta4))*Lb^2];
Obij5=[0.5*cos(theta5)*cos(sheta5),0.5*(-sin(theta5)),0.5*cos(theta5)*sin(sheta5),0,0.1*cos(theta5)*sin(sheta5)*Lb,0.1*(-sin(theta5))*Lb,0,(1/120)*cos(theta5)*sin(sheta5)*Lb^2,(1/120)*(-sin(theta5))*Lb^2,0.5*cos(theta5)*cos(sheta5),0.5*(-sin(theta5)),0.5*cos(theta5)*sin(sheta5),0,-0.1*cos(theta5)*sin(sheta5)*Lb,-0.1*(-sin(theta5))*Lb,0,(1/120)*cos(theta5)*sin(sheta5)*Lb^2,(1/120)*(-sin(theta5))*Lb^2];

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.';
 楼主| 发表于 2011-4-24 22:07 | 显示全部楼层
回复 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;
 楼主| 发表于 2011-4-24 22:07 | 显示全部楼层
回复 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
 楼主| 发表于 2011-4-24 22:07 | 显示全部楼层
回复 7 # weideyong8 的帖子

M0=[m0,0,0,0,0,0;0,m0,0,0,0,0;0,0,m0,0,0,0;0,0,0,Ixx,Ixy,Ixz;0,0,0,Iyx,Iyy,Iyz;0,0,0,Izx,Izy,Izz];%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=[M1(1:29,1:29),on2929,on2929,on2929,on2929,M1(1:29,30:35);on2929,M2(1:29,1:29),on2929,on2929,on2929,M2(1:29,30:35);on2929,on2929,M3(1:29,1:29),on2929,on2929,M3(1:29,30:35);on2929,on2929,on2929,M4(1:29,1:29),on2929,M4(1:29,30:35);on2929,on2929,on2929,on2929,M5(1:29,1:29),M5(1:29,30:35);M1(30:35,1:29),M2(30:35,1:29),M3(30:35,1:29),M4(30:35,1:29),M5(30:35,1:29),m0+M1(30:35,30:35)+M2(30:35,30:35)+M3(30:35,30:35)+M4(30:35,30:35)+M5(30:35,30:35)];
CJ=[C1(1:29,1:29),on2929,on2929,on2929,on2929,C1(1:29,30:35);on2929,C2(1:29,1:29),on2929,on2929,on2929,C2(1:29,30:35);on2929,on2929,C3(1:29,1:29),on2929,on2929,C3(1:29,30:35);on2929,on2929,on2929,C4(1:29,1:29),on2929,C4(1:29,30:35);on2929,on2929,on2929,on2929,C5(1:29,1:29),C5(1:29,30:35);C1(30:35,1:29),C2(30:35,1:29),C3(30:35,1:29),C4(30:35,1:29),C5(30:35,1:29),C1(30:35,30:35)+C2(30:35,30:35)+C3(30:35,30:35)+C4(30:35,30:35)+C5(30:35,30:35)];
K=[K1(1:29,1:29),on2929,on2929,on2929,on2929,K1(1:29,30:35);on2929,K2(1:29,1:29),on2929,on2929,on2929,K2(1:29,30:35);on2929,on2929,K3(1:29,1:29),on2929,on2929,K3(1:29,30:35);on2929,on2929,on2929,K4(1:29,1:29),on2929,K4(1:29,30:35);on2929,on2929,on2929,on2929,K5(1:29,1:29),K5(1:29,30:35);K1(30:35,1:29),K2(30:35,1:29),K3(30:35,1:29),K4(30:35,1:29),K5(30:35,1:29),K1(30:35,30:35)+K2(30:35,30:35)+K3(30:35,30:35)+K4(30:35,30:35)+K5(30:35,30:35)];
Q=[Q1(1:29,:);Q2(1:29,:);Q3(1:29,:);Q4(1:29,:);Q5(1:29,:);Q1(30:35,:)+Q2(30:35,:)+Q3(30:35,:)+Q4(30:35,:)+Q5(30:35,:)];
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=[q(1:29,1);q(146:151,1)];
    q2i=[q(30:58,1);q(146:151,1)];
    q3i=[q(59:87,1);q(146:151,1)];
    q4i=[q(88:116,1);q(146:151,1)];
    q5i=[q(117:145,1);q(146:151,1)];
    hold on
end
end
 楼主| 发表于 2011-4-24 22:09 | 显示全部楼层
回复 8 # weideyong8 的帖子

程序已经发帖完毕  跪求高手帮我审审
程序内容没有什么问题 只是太多了,是求解一个非线性弹性动力学方程的一个求解,利用newmark方法  但是找不到出错原因  希望高手看看
 楼主| 发表于 2011-4-24 22:11 | 显示全部楼层
回复 9 # weideyong8 的帖子

高手帮我运行一下 程序 找找原因 谢谢  还有如何提高运行速度,节省空间??
发表于 2011-4-24 22:40 | 显示全部楼层
这种有较多代码的程序没详细注释还真是很难读懂的!
个人感觉程序运行速度可能和你程序的符号运算有关系!

点评

赞成: 3.0
赞成: 3
赞同!  发表于 2011-4-25 09:06
发表于 2011-4-24 23:48 | 显示全部楼层
回复 1 # weideyong8 的帖子

个人水平专业有限, LZ这麼多代码, 应该花很长时间才能编好吧!
那别人可能需花好几倍时间才能消化吧! 所以想法简化LZ的问题吧, 不然...

点评

赞成: 5.0
赞成: 5
不然大家集体杯具  发表于 2011-4-25 11:23
 楼主| 发表于 2011-4-25 18:38 | 显示全部楼层
回复 12 # ChaChing 的帖子

我正在修改,改成循环结构,简化一下程序,再试试  
 楼主| 发表于 2011-4-25 18:40 | 显示全部楼层
回复 12 # ChaChing 的帖子

时间很长,但是慢慢写的,是一个关于五自由度并联机床的方程,求解,程序运行报错说:解不唯一,编的时间长,自己的耐心也消磨很多了...
 楼主| 发表于 2011-4-25 19:23 | 显示全部楼层
回复 11 # meiyongyuandeze 的帖子

虽然有符号运算 但是中间的符号运算x在计算中是可以消除的,时间t是赋值运算  我简化简化程序 看看怎么样
再有问题请教你们  谢谢回复!!!!
您需要登录后才可以回帖 登录 | 我要加入

本版积分规则

QQ|小黑屋|Archiver|手机版|联系我们|声振论坛

GMT+8, 2024-11-11 07:45 , Processed in 0.090829 second(s), 22 queries , Gzip On.

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表