KUKA30-3机器人的正解、逆解及仿真分析

KUKA30-3的位姿矩阵的计算解：（1）建立坐标系参数表如下所示:iai-1li-1di-10・i1000012-90。l100230120034-9（。l3d304590。00056—90。0006其中l=150l=570l=155J=6401233Tl0=Tl0=相邻两杆间得位姿矩阵：11SCTo二ii1000000_001001■SC01501■C11SCTo二ii1000000_001001■SC01501■C-S0570122330010SC00T1二T2=332C-S0030010220001_0001■C-S015544001640T3=4-S-C00440001-S00100100T56100160-C60001对六个坐标系应用公式：T对六个坐标系应用公式：T0二T0T1T2T3T4T5二6123456其中noapxxxxnoapyyyynoapzzzz0001n=C「（SC+CS）CCCn=C「（SC+CS）CCC-（SS-CC）SC-（SC+CS）SS1+Sx123234562323562323461CC+CS45646n=S「（SC+CS）CCC-（SS-CC）SC-（SC+CS）SS1-Cy123234562323562323461CC+CS45646n=-SS（CCC-SS）-CS（SC+CCC+SC）-SCSC（SCS-CC）145646（SCS-CC）145646（SCS-CC）145646oxoy=-C「（SC+CS）CCS+（SS-CC）SS+（SC+CS）oxoyTOC\o"1-5"\h\z12323456232356232346=-S「（SC+CS）CCS+（SS-CC）SS+（SC+CS）SC1+C12323456232356232346o=SS（CCS+SC）+CS（SS-CCS-SC）+SCSSz23456462356456462356145=-C「（SC+CS）CS+SSC-CCC1-SSS1451232345235235145=-S「（SC+CS）CS+SSC-CCC1+CSS1451232345235235a二一CSCS+SSCS-CSC-SCCz23452345235235p=155（CSC+CCS）-640（CSS-CCC）+570CS+150Cx123123123123121p=155（SSC+SCS）-640（SSS-SCC）+570SS+150Sy123123123123121p=155（CC-SS）-640（CS+SC）+570Cz232323232(3)matlab计算程序clearall;symsthetsymsthet1thet2thet3thet4thet5thet6;thet1=0;thet2=0.5*pi;thet3=0.5*pi;thet4=0;thet5=0;thet6=0;rotz=[cos(thet)-sin(thet)00;sin(thet)cos(thet)00;%未简化%未简化155mm的拐角T100=eye(4,4);T210=[010150;0010;1000;0001];T320=[100570;0100;0010;0001];T430=[100155;001640;0-100;0001];T540=[1000;00-10;0100;0001];T650=[1000;0010;0-100;0001];Tg0=[100155;0100;0010;0001];T10=subs(T100*rotz,thet,thet1)T21=subs(T210*rotz,thet,thet2)T32=subs(T320*rotz,thet,thet3)Tg=subs(Tg0*rotz,thet,thet3);%添加额外坐标系，用于计算155拐点坐标T43=subs(T430*rotz,thet,thet4)T54=subs(T540*rotz,thet,thet5)T65=subs(T650*rotz,thet,thet6)T60=T10*T21*T32*T43*T54*T65disp(['末端x坐标为:'num2str(T60(l,4))])disp(['末端y坐标为:'num2str(T60(2,4))])disp(['末端z坐标为:'num2str(T60(3,4))])pl=Tl0*T2lp2=Tl0*T2l*T32pg=Tl0*T2l*T32*Tgp3=Tl0*T2l*T32*T43x=[0pl(l,4)];y=[0pl(2,4)];z=[0pl(3,4)];plot3(x,y,z,'r','LineWidth',8);gridon;holdon;x=[pl(l,4)p2(l,4)];y=[pl(2,4)p2(3,4)];plot3(x,y,z,'b','LineWidth',4);x=[p2(l,4)x=[pl(l,4)p2(l,4)];y=[pl(2,4)p2(3,4)];plot3(x,y,z,'b','LineWidth',4);x=[p2(l,4)pg(l,4)];y=[p2(2,4)pg(3,4)];plot3(x,y,z,'k','LineWidth',2);x=[pg(l,4)p3(l,4)];y=[pg(2,4)p3(3,4)];plot3(x,y,z,'k','LineWidth',2);plot3(0,0,0,'d');p2(2,4)];z=[pl(3,4)pg(2,4)];z=[p2(3,4)p3(2,4)];z=[pg(3,4)运算结果：1000010000100001T21=1.00000.00000150.0000001.000000.0000-1.0000000001.0000T32=0.0000-1.00000570.00001.00000.000000001.000000001.0000001001001001T43=1000T54=1000T65=1000TOC\o"1-5"\h\z0015501640-1000010000-10100001000010-100pp#=0.0000-1.00000720.0000001.00000-1.0000-0.000000.00000001.0000pg=-1.0000-0.00000720.0000-1.0000-0.00000720.00001.0000-0.00001.00000-155.00001.0000p3=0.0000-1.0000-0.00001.00000-155.00001.0000p3=0.0000-1.000080.0000-1.0000-1.0000-0.0000-155.0000-1.0000-0.0000-155.00001.0000QQFigure1I=■|回1.00000.00000150.0000001.000000.0000-1.0000000001.0000p2=0.0000-1.00000720.0000001.00000-1.0000-0.000000.00000001.0000pg=-1.0000-0.00000720.0000001.00000-0.00001.00000-55.00000001.0000p3=0.00000-1.000080.00000-1.000000-1.00000-0.0000-155.00000001.0000姿态矩阵逆解1.前三关节转角的推导a)关节1转角的推导TOC\o"1-5"\h\z因为HP6机器人所有关节均处于同一截面，由几何关系:cpdix土；p2+p2ix土；p2+p2-d2xxy3b)关节3转角的推导1P由先前建立的六个位姿矩阵，知Ti二TT2T3T4T5623456提取其中的[1,4][3,4]元素，可得等式：f155sin(0+0)+640cos(0+0)+150+570sin(0)=cos(0)P+sin(0)P2232321x1yI155cos(0+0)-640sin(0+0)+570cos(0)=PV23232z将以上两式平方相加，又由cos(0)P+sin(0)P=0，可得:1x1yP2+(cos(0)P+sin(O)P-150)2=758525+176700*cos(0)-729600*sin(0)z1x1y33由于其中0为已知，上式可简化为a*cosx+b*sinx+c二0的形式，可求解1c=p2+(sin(0)*p+cos(0)*p一150)2-758525z1y1x0=arctan2(—32/24716625*c+31/98866500*(—c2+563539050000)/2),31/98866500*c+32/24716625*(—c2+563539050000)/2)3c)关节2转角的推导至此计算(b)方程组中0已求出，原方程组可化简为：3<sin(0)[155cos(0)-640sin(0)+570]+cos(0)[155sin(0)+640cos(0)]=sin(0)p+cos(0)p1xsin(0)[-155*sin(0)-640*cos(0)]+cos(0)[155*cos(0)-640*sin(0)+570]=p233233z方程组中只有sin(0)及cos(02)两个未知数，可得到sin(0)准确解，再由反正弦求222解0,具体计算见逆解程序。22.位姿逆解matlab程序%两步法变换矩阵的逆解%需要分别带入pz,py,pz%程序中的a、b、c为临时计算变量clearall;px=80;py=0;pz=-155;%输入末端坐标thet1=atan2(py,px)c=pz^2+(sin(thet1)*py+cos(thet1)*px-150)人2-758525;%thet3利用坐标元素相等，平方和相加得到thet3=atan2(-32/24716625*c+31/98866500*(-cA2+563539050000)A(1/2),31/98866500*c+32/24716625*(-22+563539050000)人(1/2))a=sin(thet1)*py+cos(thet1)*px-150;%开始利用方程组求解thet2b=pz;C=[ab]';A=[155*cos(thet3)-640*sin(thet3)+570155*sin(thet3)+640*cos(thet3);-155*sin(thet3)-640*cos(thet3)155*cos(thet3)-640*sin(thet3)+570];B=A\C;thet2=asin(B(1)/B(2))%由thet2的正切值反求弧度disp(['弧度：thet1=',num2str(thet1)'thet2=',num2str(thet2)'thet3=',num2str(thet3)])disp(['角度：thet1=',num2str(thet1*180/pi)'thet2=',num2st