|
楼主 |
发表于 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); |
|