|
楼主 |
发表于 2011-4-25 21:10
|
显示全部楼层
elseif j==5
l1x=rB*sin(8*pi/5)*cos(a)*sin(b)-rB*cos(8*pi/5)*sin(a)+xb;
l1x1=-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;
l1x2=-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;
l1y=rB*sin(8*pi/5)*sin(a)*sin(b)+rB*cos(8*pi/5)*cos(a)+yb-rA*cos(pi/4);
l1y1=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;
l1y2=-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;
l1z=rB*sin(8*pi/5)*cos(b)+zb+rA*sin(pi/4);
l1z1=-rB*sin(8*pi/5)*sin(b)*b1+zb1;
l1z2=-rB*sin(8*pi/5)*cos(b)*b1^2-rB*sin(8*pi/5)*sin(b)*b2+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次导数
end
theta1=atan((l1y*cos(fai(1,j))+l1z*sin(fai(1,j)))/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(fai(1,j))-l1z*cos(fai(1,j)))/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(fai(1,j))+l1z1*sin(fai(1,j)))*l1x-(l1y*cos(fai(1,j))+l1z*sin(fai(1,j)))*l1x1)/(l1x^2+(l1y*cos(fai(1,j))+l1z*sin(fai(1,j)))^2);
sheta11=((l1y1*sin(fai(1,j))-l1z1*cos(fai(1,j)))*l1-(l1y*sin(fai(1,j))-l1z*cos(fai(1,j)))*l1d1)/(((l1^2-(l1y*sin(fai(1,j))-l1z*cos(fai(1,j)))^2)^0.5)*l1);
theta12=(((l1y2*cos(fai(1,j))+l1z2*sin(fai(1,j)))*l1x-(l1y*cos(fai(1,j))+l1z*sin(fai(1,j)))*l1x2)*(l1x^2+(l1y*cos(fai(1,j))+l1z*sin(fai(1,j)))^2)-((l1y1*cos(fai(1,j))+l1z1*sin(fai(1,j)))*l1x-(l1y*cos(fai(1,j))+l1z*sin(fai(1,j)))*l1x1)*(2*l1x*l1x1+2*(l1y*cos(fai(1,j))+l1z*sin(fai(1,j)))*(l1y1*cos(fai(1,j))+l1z1*sin(fai(1,j)))))/((l1x^2+(l1y*cos(fai(1,j))+l1z*sin(fai(1,j)))^2)^2);
s101=(l1y2*sin(fai(1,j))-l1z2*cos(fai(1,j)))*l1;s102=l1d2*(l1y*sin(fai(1,j))-l1z*cos(fai(1,j)));
A1=(((l1^2-(l1y*sin(fai(1,j))-l1z*cos(fai(1,j)))^2)^0.5)*l1);B1=((l1y1*sin(fai(1,j))-l1z1*cos(fai(1,j)))*l1-(l1y*sin(fai(1,j))-l1z*cos(fai(1,j)))*l1d1);
s103=l1d1*(l1^2-(l1y*sin(fai(1,j))-l1z*cos(fai(1,j)))^2)^0.5+l1*(l1^2-(l1y*sin(fai(1,j))-l1z*cos(fai(1,j)))^2)^(-0.5)*(l1*l1d1-(l1y*sin(fai(1,j))-l1z*cos(fai(1,j)))*(l1y1*sin(fai(1,j))-l1z1*cos(fai(1,j))));
if j==1
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);
else
sheta12=((s101-s102)*A1-B1*s103)/A1^2;
end
Aij1=[cos(theta1)*cos(sheta1) -sin(theta1) cos(theta1)*sin(sheta1);
cos(fai(1,j))*sin(theta1)*cos(sheta1)+sin(fai(1,j))*sin(sheta1) cos(fai(1,j))*cos(theta1) cos(fai(1,j))*sin(theta1)*sin(sheta1)-sin(fai(1,j))*cos(sheta1);
sin(fai(1,j))*sin(theta1)*cos(sheta1)-cos(fai(1,j))*sin(sheta1) sin(fai(1,j))*cos(theta1) sin(fai(1,j))*sin(theta1)*sin(sheta1)+cos(fai(1,j))*cos(sheta1)]; %支链1上单元的变换矩阵 先算出一个再说,先往后列出式子 看看
Rij1=Aij1.';
% Rij2=Aij2.';
% Rij3=Aij3.';
% Rij4=Aij4.';
% Rij5=Aij5.';
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(fai(1,j))*sin(theta1)*cos(sheta1)+sin(fai(1,j))*sin(sheta1);sin(fai(1,j))*sin(theta1)*cos(sheta1)-cos(fai(1,j))*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(fai(1,j))*cos(theta1)*cos(sheta1)*theta11-cos(fai(1,j))*sin(theta1)*sin(sheta1)*sheta11+sin(fai(1,j))*cos(sheta1)*sheta11,-cos(fai(1,j))*sin(theta1)*theta11,cos(fai(1,j))*cos(theta1)*sin(sheta1)*theta11+cos(fai(1,j))*sin(theta1)*cos(sheta1)*sheta11+sin(fai(1,j))*sin(sheta1)*sheta11;
sin(fai(1,j))*cos(theta1)*cos(sheta1)*theta11-sin(fai(1,j))*sin(theta1)*sin(sheta1)*sheta11-cos(fai(1,j))*cos(sheta1)*sheta11,-sin(fai(1,j))*sin(theta1)*theta11,sin(fai(1,j))*cos(theta1)*sin(sheta1)*theta11+sin(fai(1,j))*sin(theta1)*cos(sheta1)*sheta11-cos(fai(1,j))*sin(sheta1)*sheta11];
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(fai(1,j))*cos(theta1)*cos(sheta1)*(theta12)-cos(fai(1,j))*sin(theta1)*cos(sheta1)*(theta11)^2-cos(fai(1,j))*cos(theta1)*sin(sheta1)*(theta11)*(sheta11)-cos(fai(1,j))*sin(theta1)*sin(sheta1)*(sheta12)-cos(fai(1,j))*cos(theta1)*sin(sheta1)*(theta11)*(sheta11)-cos(fai(1,j))*sin(theta1)*cos(sheta1)*(sheta11)^2+sin(fai(1,j))*cos(sheta1)*(sheta12)-sin(fai(1,j))*sin(sheta1)*(sheta11)^2;
g22ij1=-cos(fai(1,j))*sin(theta1)*(theta12)-cos(fai(1,j))*cos(theta1)*(theta11)^2;
g23ij1=cos(fai(1,j))*cos(theta1)*sin(sheta1)*(theta12)-cos(fai(1,j))*sin(theta1)*sin(sheta1)*(theta11)^2+cos(fai(1,j))*cos(theta1)*cos(sheta1)*(theta11)*(sheta11)+cos(fai(1,j))*sin(theta1)*cos(sheta1)*(sheta12)+cos(fai(1,j))*cos(theta1)*cos(sheta1)*(theta11)*(sheta11)-cos(fai(1,j))*sin(theta1)*sin(sheta1)*(sheta11)^2+sin(fai(1,j))*sin(sheta1)*(sheta12)+sin(fai(1,j))*cos(sheta1)*(sheta11)^2;
g31ij1=sin(fai(1,j))*cos(theta1)*cos(sheta1)*(theta12)-sin(fai(1,j))*sin(theta1)*cos(sheta1)*(theta11)^2-sin(fai(1,j))*cos(theta1)*sin(sheta1)*(theta11)*(sheta11)-sin(fai(1,j))*sin(theta1)*sin(sheta1)*(sheta12)-sin(fai(1,j))*cos(theta1)*sin(sheta1)*(theta11)*(sheta11)-sin(fai(1,j))*sin(theta1)*cos(sheta1)*(sheta11)^2-cos(fai(1,j))*cos(sheta1)*(sheta12)+cos(fai(1,j))*sin(sheta1)*(sheta11)^2;
g32ij1=-sin(fai(1,j))*sin(theta1)*(theta12)-sin(fai(1,j))*cos(theta1)*(theta11)^2;
g33ij1=sin(fai(1,j))*cos(theta1)*sin(sheta1)*(theta12)-sin(fai(1,j))*sin(theta1)*sin(sheta1)*(theta11)^2+sin(fai(1,j))*cos(theta1)*cos(sheta1)*(sheta11)*(theta11)+sin(fai(1,j))*sin(theta1)*cos(sheta1)*(sheta12)+sin(fai(1,j))*cos(theta1)*cos(sheta1)*(theta11)*(sheta11)-sin(fai(1,j))*sin(theta1)*sin(sheta1)*(sheta11)^2-cos(fai(1,j))*sin(sheta1)*(sheta12)-cos(fai(1,j))*cos(sheta1)*(sheta11)^2;
Gij12=[g11ij1,g12ij1,g13ij1;g21ij1,g22ij1,g23ij1;g31ij1,g32ij1,g33ij1];%对Gij1的1次求导
% Gij22=[g11ij2,g12ij2,g13ij2;g21ij2,g22ij2,g23ij2;g31ij2,g32ij2,g33ij2];
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.'];
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.'];
% Gij1 Gij2..Gij5 都是3*3矩阵
Dij1=[-sin(theta1)*cos(sheta1)*theta11-cos(theta1)*sin(sheta1)*sheta11;
cos(fai(1,j))*cos(theta1)*cos(sheta1)*theta11-cos(fai(1,j))*sin(theta1)*sin(sheta1)*sheta11+sin(fai(1,j))*cos(sheta1)*sheta11;
sin(fai(1,j))*cos(theta1)*cos(sheta1)*theta11-sin(fai(1,j))*sin(theta1)*sin(sheta1)*sheta11-cos(fai(1,j))*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];
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];
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];
DDij1=(cos(sheta1))^2*(theta11)^2+(sheta11)^2;
% DDij2=(cos(sheta2))^2*(theta21)^2+(sheta21)^2;
%先算摆动杆上的一个单元上的各个矩阵情况 密度为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].';
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].';
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]];
R1=[e2525,on254,on256;on325,on34,Js1;on425,e44,on46];
% R2=[e2525,on254,on256;on325,on34,Js2;on425,e44,on46];
for k=1:3 %循环
syms x % Lb=0.76 Ls=0.44表示杆摆动杆 伸缩杆上的单元长度
%对摆动杆上的单元 位移型函数以及一些相关矩阵的相乘
%开始求积分Iijp表示对x轴的惯性矩
aij1=4.4*10^-3;
Iijp1=3.3*10^-2; %单位kg.m^2
Iijy1=1.25;
Iijz1=0.53;
Lb=0.76;
if k==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 |
|