Finnal6.zip_KJ9_并联 动力学_并联 动力学_并联机构matlab_并联机构动力

close all clc lui=0.378; %质心到动平台胡克铰ui的距离 lsi=0.452; %质心到动平台铰链si的距离 mui=26.15;%摆动杆的质量 msi=8.45; %伸缩杆的质量 mB=36.28;%并联机构动平台的质量 mdj=0;%各种机床参数赋值(mdj为刀头质量) h=0; rA=0.645;%定平台上四个虎克铰u1u2u3u4均匀布置于半径645mm的圆上 rB=0.202;%动平台上五个铰链s1s2s3s4s5均匀布置在本经202mm的圆上 mq=pi/4;%定平台上四个胡克铰的间隔角度 Q=2*pi/5;%动平台上五个铰链的间隔角度 W=0.71707;%球副S中心位于半径717.07mm处 g=[9.8,0,0].';%重力取值 %铰节点位置参数赋值，1.1支链坐标系的建立 a=0.025;%动平台运动轨迹中的给定参数 syms t f1x f1z f2x f3x f4x f5x %syms函数：创建符号变量的捷径 tt=0:0.01:4; for j=1:length(tt) t=tt(j) ms1=0;ms2=0;%对于FEDi、FEmi的叠加运算赋初值 %ma=(pi/36)*t*t*t;mb=0;mc=-pi/2;%欧拉角赋值 %ma=(pi/72)*t;mb=(pi/72)*t;mc=-pi/2;%欧拉角赋值 %ma=0;mb=0;mc=(-pi/2)*t;%欧拉角赋值 ma=0;mb=0;mc=-pi/2;%欧拉角赋值 %ma=-(pi/18)*t;mb=0;mc=-pi/2;%欧拉角赋值 %aaq1=(pi/12)*t*t;bbq1=0;ccq1=0;%对欧拉角求一阶导数 %aaq11=(pi/6)*t;bbq11=0;ccq11=0;%对欧拉角求二阶导数 %aaq1=0;bbq1=0;ccq1=-pi/2;%对欧拉角求一阶导数 %aaq11=0;bbq11=0;ccq11=0;%对欧拉角求二阶导数 %aaq1=-pi/18;bbq1=0;ccq1=0;%对欧拉角求一阶导数 aaq1=0;bbq1=0;ccq1=0;%对欧拉角求一阶导数 aaq11=0;bbq11=0;ccq11=0;%对欧拉角求二阶导数 A=[cos(ma) -sin(ma) 0;sin(ma) cos(ma) 0;0 0 1]; B=[cos(mb) 0 sin(mb);0 1 0;-sin(mb) 0 cos(mb)]; C=[1 0 0;0 cos(mc) -sin(mc);0 sin(mc) cos(mc)]; ABR=A*B*C; %欧拉转换矩阵(B相对于A的) 3*3 N=inv(ABR);%欧拉转换矩阵(A相对于B的)后面有用 3*3 X=0.90+0.025*cos(2*t);%动系坐标原点在静坐标系下的坐标赋值 式40 Y=-0.10+0.025*sin(2*t); Z=0.202*cos(mb)*cos(mc);%动系坐标原点在静坐标系下的坐标赋值 P=[X;Y;Z]; %3*1 %AaVpB=ABR*[-0.120*cos(2*t);-0.120*sin(2*t);0]%动平台在静坐标下的加速度 %求基本量mni(沿着杆长方向的单位向量)5个杆的，RPS也要用到 ma1=[0;W;0]; %定平台上5个胡克铰在静坐标系下的坐标 ma2=[0;rA*cos(mq);rA*sin(mq)]; ma3=[0;rA*cos(3*mq);rA*sin(3*mq)]; ma4=[0;rA*cos(5*mq);rA*sin(5*mq)]; ma5=[0;rA*cos(7*mq);rA*sin(7*mq)]; %3*1 lApu1=[ma1(2,1).^2].^(1/2); %定平台上5个胡克铰在静坐标系A下的模 lApu2=[ma2(3,1).^2+ma2(2,1).^2].^(1/2); lApu3=[ma3(3,1).^2+ma3(2,1).^2].^(1/2); lApu4=[ma4(3,1).^2+ma4(2,1).^2].^(1/2); lApu5=[ma5(3,1).^2+ma5(2,1).^2].^(1/2); %1*1 mA1=N*(ma1-P); %定平台上5个胡克铰在动坐标系下的坐标 mA2=N*(ma2-P); mA3=N*(ma3-P); mA4=N*(ma4-P); mA5=N*(ma5-P); %N=inv(ABR),欧拉转换矩阵(A相对于B的), 3*1 mb1=[0;0;-rB]; %动平台上5个铰在动坐标系下的坐标 mb2=[0;rB*sin(Q);-rB*cos(Q)]; mb3=[0;rB*sin(2*Q);-rB*cos(2*Q)]; mb4=[0;rB*sin(3*Q);-rB*cos(3*Q)]; mb5=[0;rB*sin(4*Q);-rB*cos(4*Q)]; %3*1 mL1=[(0-mA1(1,1))^2+(0-mA1(2,1))^2+(-rB-mA1(3,1))^2].^(1/2);%5个杆的长度 mL2=[(0-mA2(1,1))^2+(rB*sin(Q)-mA2(2,1))^2+(-rB*cos(Q)-mA2(3,1))^2].^(1/2); mL3=[(0-mA3(1,1))^2+(rB*sin(2*Q)-mA3(2,1))^2+(-rB*cos(2*Q)-mA3(3,1))^2].^(1/2); mL4=[(0-mA4(1,1))^2+(rB*sin(3*Q)-mA4(2,1))^2+(-rB*cos(3*Q)-mA4(3,1))^2].^(1/2); mL5=[(0-mA5(1,1))^2+(rB*sin(4*Q)-mA5(2,1))^2+(-rB*cos(4*Q)-mA5(3,1))^2].^(1/2); %1*1 mn1=(mb1-mA1)/mL1; %5个杆的单位方向矢量(在动系B中)=动平台上五个铰在动平台下的坐标mb1-定平台上五个胡克铰在动坐标系下的坐标mA1的差值除以五个杆的长度mL1 mn2=(mb2-mA2)/mL2; mn3=(mb3-mA3)/mL3; mn4=(mb4-mA4)/mL4; mn5=(mb5-mA5)/mL5; %3*1 An1=ABR*mn1; %5个杆的单位方向矢量(在定系A中) An2=ABR*mn2; An3=ABR*mn3; An4=ABR*mn4; An5=ABR*mn5; %3*1 cs1=An1(1,1); %求杆与定系x轴的夹角的余弦值 cs2=An2(1,1); cs3=An3(1,1); cs4=An4(1,1); cs5=An5(1,1); %1*1 sn1=(1-(cs1)^2 )^(1/2); %求杆与定系x轴的夹角的正弦值 sn2=(1-(cs2)^2 )^(1/2); sn3=(1-(cs3)^2 )^(1/2); sn4=(1-(cs4)^2 )^(1/2); sn5=(1-(cs5)^2 )^(1/2); %1*1 d1=cos(0*mq);%求另一个角的余弦？？？ d2=cos(2*pi-1*mq); d3=cos(2*pi-3*mq); d4=cos(2*pi-5*mq); d5=cos(2*pi-7*mq); %1*1 c1=sin(2*pi-0*mq); %求另一个角的正弦？？？ c2=sin(2*pi-1*mq); c3=sin(2*pi-3*mq); c4=sin(2*pi-5*mq); c5=sin(2*pi-7*mq); %1*1 %c1=(1-(d1)^2 )^(1/2); %c2=(1-(d2)^2 )^(1/2); %c3=(1-(d3)^2 )^(1/2); %c4=(1-(d4)^2 )^(1/2); %c5=(1-(d5)^2 )^(1/2);%1*1 AT1=[d1 -c1*cs1 c1*sn1;c1 d1*cs1 -d1*sn1;0 sn1 cs1];%并联支链i相对于定系A的旋转矩阵ATi AT2=[d2 -c2*cs2 c2*sn2;c2 d2*cs2 -d2*sn2;0 sn2 cs2]; AT3=[d3 -c3*cs3 c3*sn3;c3 d3*cs3 -d3*sn3;0 sn3 cs3]; AT4=[d4 -c4*cs4 c4*sn4;c4 d4*cs4 -d4*sn4;0 sn4 cs4]; AT5=[d5 -c5*cs5 c5*sn5;c5 d5*cs5 -d5*sn5;0 sn5 cs5]; %3*3 BT1=[c1 d1*cs1 -d1*sn1;-d1 c1*cs1 -c1*sn1;0 sn1 cs1];%并联支链i相对于动系B的旋转矩阵BTi BT2=[c2 d2*cs2 -d2*sn2;-d2 c2*cs2 -c2*sn2;0 sn2 cs2]; BT3=[c3 d3*cs3 -d3*sn3;-d3 c3*cs3 -c3*sn3;0 sn3 cs3]; BT4=[c4 d4*cs4 -d4*sn4;-d4 c4*cs4 -c4*sn4;0 sn4 cs4]; BT5=[c5 d5*cs5 -d5*sn5;-d5 c5*cs5 -c5*sn5;0 sn5 cs5]; %3*3 %并联机构驱动杆线速度与角速度分析 J2A=[1 0 0 0 0;0 1 0 0 0;0 0 0 -0.202*cos(mc)*sin(mb) -0.202*cos(mb)*sin(mc);0 0 0 -sin(ma) cos(ma)*cos(mb);0 0 0 cos(ma) sin(ma)*cos(mb);0 0 1 0 -sin(mb)];%论文中的映射矩阵H4 6*5 AWB=[-sin(ma)*bbq1+cos(ma)*cos(mb)*ccq1;cos(ma)*bbq1+sin(ma)*cos(mb)*ccq1;aaq1-sin(mb)*ccq1]; %动平台B相对定平台A转动角速度 3*1 AEB=[-sin(ma)*bbq11+cos(ma)*cos(mb)*ccq11-aaq1*cos(ma)*bbq1+(-aaq1*sin(ma)*cos(mb)-bbq1*cos(ma)*sin(mb))*ccq1;cos(ma)*bbq11+sin(ma)*cos(mb)*ccq11-aaq1*sin(ma)*bbq1+(aaq1*cos(ma)*sin(mb)-bbq1*sin(ma)*sin(mb))*ccq1;aaq11-sin(mb)*ccq11-bbq1*cos(mb)*ccq1];%动平台角加速度用欧拉角表示的角加速度（3*1） pp=[-sin(ma)*bbq1+cos(ma)*cos(mb)*ccq1;cos(ma)*bbq1+sin(ma)*cos(mb)*ccq1;aaq1-sin(mb)*ccq1];%这里的pp就是AWB 3*1 AWBB=[0 -pp(3,1) pp(2,1);pp(3,1) 0 -pp(1,1);-pp(2,1) pp(1,1) 0];%AWB反对称算子 AvBO=J2A*[-0.050*sin(2*t);0.050*cos(2*t);0;0;0];%动平台的六维速度：动平台B的原点相对定平台A的运动速度 6*1 AaBO=[-0.100*cos(2*t);-0.100*sin(2*t);-0.202*cos(mc)*sin(mb)*bbq11-0.202*cos(mb)*sin(mc)*ccq11+(-0.202*bbq1*cos(mc)*cos(mb)+0.202*ccq1*sin(mb)*sin(mc))*bbq1+(0.202*bbq1*sin(mb)*sin(mc)-0.202*ccq1*cos(mc)*cos(mb))*ccq1;-sin(ma)*bbq11+cos(ma)*cos(mb)*ccq11-aaq1*cos(ma)*bbq1+(-aaq1*sin(ma)*cos(mb)-bbq1*cos(ma)*sin(mb))*ccq1;cos(ma)*bbq11+sin(ma)*cos(mb)*ccq11-aaq1*sin(ma)*bbq1+(aaq1*cos(ma)*sin(mb)-bbq1*sin(ma)*sin(mb))*ccq1;aaq11-sin(mb)*ccq11-bbq1*cos(mb)*ccq1];%动平台B的原点相对定平台A的运动加速度 %6*1 %Iui=[1.28 0 0;0 1.28 0;0 0 0.0346];%摆动杆惯量 %Isi=[0.528 0 0;0 0.528 0;0 0 0.00167];%伸缩杆惯量 Iui=[0.0346 0 0;0 1.28 0;0 0 1.28];%摆动杆惯量（正确）已算出 3*3 Isi=[0.00167 0 0;0 0.528 0;0 0 0.528];%伸缩杆惯量（正确）已算出 3*3 BFBe=[0;0;0];%在动系下产生的力 3*1 BMBe=[0;0;0];%在动系下产生的力矩 3*1 AFBe=ABR*BFBe;%在定系下产生的力 3*1 AMBe=ABR*BMBe;%在定系下产生的力矩 3*1 An=[ABR*mn1 ABR*mn2 ABR*mn3 ABR*mn4 ABR*mn5]; %5个杆在静坐标系A下的单位方向矢量Ani 3*5 Ars=[ABR*mb1 ABR*mb2 ABR*mb3 ABR*mb4 ABR*mb5]; %在静坐标系A下铰点s相对OB的矢径 3*5 %aa=An(:,i); %Ani aa1=An(:,1);%An中第一列所有数 aa2=An(:,2); aa3=An(:,3); aa4=An(:,4); aa5=An(:,5); %bb=Ars(:,i);%Arsi bb1=Ars(:,1);%Ars中第一列所有数 bb2=Ars(:,2); bb3=Ars(:,3); bb4=Ars(:,4); bb5=Ars(:,5); %cc=[0 -aa(3,1) aa(2,1);aa(3,1) 0 -aa(1,1);-aa(2,1) aa(1,1) 0]; %Ani反对称算子 cc1=[0 -aa1(3,1) aa1(2,1);aa1(3,1) 0 -aa1(1,1);-aa1(2,1) aa1(1,1) 0]; %？？？ cc2=[0 -aa2(3,1) aa2(2,1);aa2(3,1) 0 -aa2(1,1);-aa2(2,1) aa2(1,1) 0]; cc3=[0 -aa3(3,1) aa3(2,1);aa3(3,1) 0 -aa3(1,1);-aa3(2,1) aa3(1,1) 0]; cc4=[0 -aa4(3,1) aa4(2,1);aa4(3,1) 0 -aa4(1,1);-aa4(2,1) aa4(1,1) 0]; cc5=[0 -aa5(3,1) aa5(2,1);aa5(3,1) 0 -aa5(1,1);-aa5(2,1) aa5(1,1) 0]; %3*3 %dd=[0 -bb(3,1) bb(2,1);bb(3,1) 0 -bb(1,1);-bb(2,1) bb(1,1) 0]; %Ari反对称算子 dd1=[0 -bb1(3,1) bb1(2,1);bb1(3,1) 0 -bb1(1,1);-bb1(2,1) bb1(1,1) 0]; dd2=[0 -bb2(3,1) bb2(2,1);bb2(3,1) 0 -bb2(1,1);-bb2(2,1) bb2(1,1) 0]; dd3=[0 -bb3(3,1) bb3(2,1);bb3(3,1) 0 -bb3(1,1);-bb3(2,1) bb3(1,1) 0]; dd4=[0 -bb4(3,1) bb4(2,1);bb4(3,1) 0 -bb4(1,1);-bb4(2,1) bb4(1,1) 0]; dd5=[0 -bb5(3,1) bb5(2,1);bb5(3,1) 0 -bb5(1,1);-bb5(2,1) bb5(1,1) 0]; %3*3 %AEi=1/(mL(1,i))*([cc -cc*dd]*AaBO-cc*AWBB*dd*AWB)-2/(mL(1,i).^2)*([cc -cc*dd]*AvBO*[aa' (dd*aa)']*AvBO);%摆动杆的摆