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

KUKA30-3的位姿矩阵的计算解：（1）建立坐标系（2）确定参数参数表如下所示：i1i1li1idi1000θ12901l0θ2302l0θ34903l3dθ459000θ569000θ6其中1l=1502l=5703l=1553d=640相邻两杆间得位姿矩阵：22122201500010000001SCTCS33332305700000100001CSSCT4434440155001640000001CSTSC554555000010000001CSTSC665666000010000001CSTSC对六个坐标系应用公式：其中41232345623235623234615646xnCSCCSCCCSSCCSCSCCSSSSSCCCS41232345623235623234615646ynSSCCSCCCSSCCSCSCCSSSCSCCCS23456462356456462356()znSSCCCSSCSSCCCCSCSCSC41232345623235623234615646xoCSCCSCCSSSCCSSSCCSSCSSCSCC41232345623235623234615646yoSSCCSCCSSSCCSSSCCSSCCSCSCC23456462356456462356()zoSSCCSSCCSSSCCSSCSCSS1232345235235145xaCSCCSCSSSCCCCSSS1232345235235145yaSSCCSCSSSCCCCCSS23452345235235zaCSCSSSCSCSCSCC123123123123121155640570150xpCSCCCSCSSCCCCSC123123123123121155640570150ypSSCSCSSSSSCCSSS232323232155640570zpCCSSCSSCC100001000000111101CSSCT100056453423120106zzzzyyyyxxxxpaonpaonpaonTTTTTTT(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;0010;0001];T100=eye(4,4);%未简化155mm的拐角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(1,4))])disp(['末端y坐标为:'num2str(T60(2,4))])disp(['末端z坐标为:'num2str(T60(3,4))])p1=T10*T21p2=T10*T21*T32pg=T10*T21*T32*Tgp3=T10*T21*T32*T43x=[0p1(1,4)];y=[0p1(2,4)];z=[0p1(3,4)];plot3(x,y,z,'r','LineWidth',8);gridon;holdon;x=[p1(1,4)p2(1,4)];y=[p1(2,4)p2(2,4)];z=[p1(3,4)p2(3,4)];plot3(x,y,z,'b','LineWidth',4);x=[p2(1,4)pg(1,4)];y=[p2(2,4)pg(2,4)];z=[p2(3,4)pg(3,4)];plot3(x,y,z,'k','LineWidth',2);x=[pg(1,4)p3(1,4)];y=[pg(2,4)p3(2,4)];z=[pg(3,4)p3(3,4)];plot3(x,y,z,'k','LineWidth',2);plot3(0,0,0,'d');运算结果：T10=1000010000100001T21=1.00000.00000150.0000001.000000.0000-1.0000000001.0000T32=0.0000-1.00000570.00001.00000.000000001.000000001.0000T43=1001550016400-1000001T54=100000-1001000001T65=100000100-1000001T60=0.00000-1.000080.00000-1.000000-1.00000-0.0000-155.00000001.0000末端x坐标为:80末端y坐标为:0末端z坐标为:-155p1=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-155.00000001.0000p3=0.00000-1.000080.00000-1.000000-1.00000-0.0000-155.00000001.0000p1=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-155.00000001.0000p3=0.00000-1.000080.00000-1.000000-1.00000-0.0000-155.00000001.0000姿态矩阵逆解1.前三关节转角的推导a)关节1转角的推导因为HP6机器人所有关节均处于同一截面，由几何关系：b)关节3转角的推导由先前建立的六个位姿矩阵，知112345623456TTTTTT提取其中的[1,4][3,4]元素，可得等式：232321x123232z155sin(+)+640cos(+)+150+570sin()cos()Psin()155cos(+)-640sin(+)+570cos()PyP将以上两式平方相加，又由11cos()sin()0xyPP，可得：221133(cos()sin()150)758525+176700cos()-729600*sin()zxyPPP232231dppdarctgpparctgyxxy由于其中1为已知，上式可简化为*cos*sin0axbxc的形式，可求解2211(()*()*150)758525zyxcpsinpcosp21/221/232(32/24716625*31/98866500*(563539050000)),31/98866500*32/24716625*(563539050000))arctanccccc)关节2转角的推导至此计算（b）方程组中3已求出，原方程组可化简为：23323311233233sin()[155cos()-640sin()570]+cos()[155sin()+640cos()]sin()p+cos()p-150sin()[-155*sin()-640*cos()]cos()[155*cos()-640*sin()+570]pyxz方程组中只有2sin()及2cos()两个未知数，可得到2sin()准确解，再由反正弦求解2,具体计算见逆解程序。2.位姿逆解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*(-c^2+563539050000)^(1/2),31/98866500*c+32/24716625*(-c^2+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=',num2str(thet2*180/pi)'thet3=',num2str(thet3*180/pi)])将前面正解计算出的坐标带回验证，得计算结果：thet1=0thet3=1.5708thet2=-1.5708+37.8070i弧度：thet1=0thet2=-1.5708+37.807ithet3=1.5708角度：thet1=0thet2=-90+2166.1789ithet3=90逆解中的thet1、thet2、thet3与正解中的Px、Py、Pz对应，程序验证通过。