哈工大PUMA机器人大作业.doc

2017 年 秋 季学期研究生课程考核（读书报告、研究报告）考核科目: 机器人技术学生所在院（系）: 机电学院学生所在学科: 机械电子学 生 姓 名: 王学 号: 17S学 生 类 别: 学硕考察结果: 阅卷人:PUMA 机器人正逆运动学推导及运动空间解算图1 PUMA 机器人模型任务:1. 建立坐标系；2. 给出D-H参数表；3. 推导正、逆运动学；4. 编程得工作空间。一、 建立坐标系根据PUMA 机器人运动自由度，在各关节处建立坐标系如图2所示。图2 PUMA 机器人坐标系建立图其中与原点交于一点, 与原点交于一点。二、 D-H参数表D-H参数表可根据坐标系设定而得出，见表1。1) 为绕轴，从到的角度；2) 为绕轴，从到的角度；3) 为沿轴，从与交点到的距离；4) 为沿轴，从与到的距离。杆件变量（当前值）变量范围1002304050060表1 PUMA 机器人杆件参数表三、 正运动学推导由坐标系及各杆件参数可得到6个连杆变换矩阵。 根据各连杆变换矩阵相乘，可以得到PUMA 机械手变换矩阵，其矩阵为关节变量的函数。将上述变换矩阵逐个依次相乘可以得到。等式联立，对比等式左右两侧元素可得：nx=sin(c6)*(cos(c4)*sin(c1)-cos(c2+c3)*cos(c1)*sin(c4)+ cos(c6)*(cos(c5)*(sin(c1)*sin(c4)+cos(c2+c3)*cos(c1)*cos(c4)+ sin(c2+c3)*cos(c1)*sin(c5)；ox=cos(c6)*(cos(c4)*sin(c1)-cos(c2+c3)*cos(c1)*sin(c4)- sin(c6)*(cos(c5)*(sin(c1)*sin(c4)+cos(c2+c3)*cos(c1)*cos(c4)+ sin(c2+c3)*cos(c1)*sin(c5)；ax=sin(c5)*(sin(c1)*sin(c4)+cos(c2+c3)*cos(c1)*cos(c4)- sin(c2+c3)*cos(c1)*cos(c5)；px=d6*(sin(c5)*(sin(c1)*sin(c4)+cos(c2+c3)*cos(c1)*cos(c4)- sin(c2+c3)*cos(c1)*cos(c5)-d2*sin(c1)-d4*sin(c2+c3)*cos(c1)+ a2*cos(c1)*cos(c2)+a3*cos(c1)*cos(c2)*cos(c3)- a3*cos(c1)*sin(c2)*sin(c3)；ny=-sin(c6)*(cos(c1)*cos(c4)+cos(c2+c3)*sin(c1)*sin(c4)- cos(c6)*(cos(c5)*(cos(c1)*sin(c4)-cos(c2+c3)*cos(c4)*sin(c1)- sin(c2+c3)*sin(c1)*sin(c5)；oy=sin(c6)*(cos(c5)*(cos(c1)*sin(c4)-cos(c2+c3)*cos(c4)*sin(c1)- sin(c2+c3)*sin(c1)*sin(c5)-cos(c6)*(cos(c1)*cos(c4)+ cos(c2+c3)*sin(c1)*sin(c4)；ay=-sin(c5)*(cos(c1)*sin(c4)-cos(c2+c3)*cos(c4)*sin(c1)- sin(c2+c3)*cos(c5)*sin(c1)；py=d2*cos(c1)-d6*(sin(c5)*(cos(c1)*sin(c4)- cos(c2+c3)*cos(c4)*sin(c1)+sin(c2+c3)*cos(c5)*sin(c1)- d4*sin(c2+c3)*sin(c1)+a2*cos(c2)*sin(c1)+ a3*cos(c2)*cos(c3)*sin(c1)-a3*sin(c1)*sin(c2)*sin(c3)；nz=cos(c6)*(cos(c2+c3)*sin(c5)-sin(c2+c3)*cos(c4)*cos(c5)+ sin(c2 + c3)*sin(c4)*sin(c6)；oz=sin(c2+c3)*cos(c6)*sin(c4)-sin(c6)*(cos(c2+c3)*sin(c5)- sin(c2+c3)*cos(c4)*cos(c5)；az=-cos(c2+c3)*cos(c5)-sin(c2+c3)*cos(c4)*sin(c5)；pz=-d4*cos(c2 + c3)- a3*sin(c2+c3)-d6*(cos(c2+c3)*cos(c5)+ sin(c2+c3)*cos(c4)*sin(c5)-a2*sin(c2)；上述12个公式中，c1-c6分别对应，将关节变量角度的当前值代入以上公式得到： 计算结果与当前位置姿态一致，推导结果无误。四、 逆运动学推导逆运动学推导为末端位置姿态n,o,a,p确定，求解关节变量。 五、 工作空间的逆解运算从上述的推导中我们已经得到了关节变量的推导公式。根据逆解的求解思想，在这里我们参照正解所求得的工作间，将机器的运动空间划分成了x=-1000:n1:1000, y=-1000:n2:1000, z=-1000:n3:1000,所组成的空间点集合，其中n1,n2,n3代表步距，单位为mm。在程序中通过循环，将此空间上的点的坐标一一代入中的，。中n,o,a，所对应的9个参数，代表了末端坐标系（执行器）各坐标轴与零坐标系（固定坐标系）各坐标轴之间的夹角余弦值。在这里为了简便运算，减少运算量，我们讨论固定姿态下机器人的工作空间。令，其余六个参数等于零，即当前PUMA机器人末端执行器相对于固定坐标系的姿态。至此，求解关节变量的参数均已给出。另外考虑到手腕环节（后三个坐标系）对工作空间的影响不大，为了简化运算，在这里只将逆解求解到，而不再讨论。逆解源代码，请参照附录。逆解求解的结果如图。六、 工作空间的正解运算机器人正解的末端执行器（）相对于固定坐标系（）的转换矩阵已经求出。取上的原点,就可得到上的原点在固定坐标系下的坐标，即工作空间中一个点。通过将在对应的变量范围内变换便可以得到整个工作空间。在这里为了减少运算量，同逆解，我们不讨论的变化，只讨论在各自运动范围内的变化。正解源代码及改进的高效率代码请参照附录。逆解正解的结果如图。图3 PUMA 机器人逆解空间（轴测图）图4 PUMA 机器人逆解空间（XOY）图5 PUMA 机器人逆解空间（XOZ）图6 PUMA 机器人逆解空间（YOZ）图7 PUMA 机器人正解空间（轴测图）图8 PUMA 机器人正解空间（XOY） 图9 PUMA 机器人正解空间（XOZ）图10 PUMA 机器人正解空间（YOZ） 附录a) 逆解程序clc;clear;a2=431.8;a3=-20.32;d2=149.09;d4=433.07;d6=56.25;i=1;j=1;t1=0,0;mt1=0,0;x=zeros(450000,1);y=zeros(450000,1);z=zeros(450000,1);for px=-1000:10:1000 for py=-1000:10:1000 for pz=-1000:10:1000 j=j+1; m=1; t=0; k=px2+(py-d6)2-d22; if k0 m=0; else s1=atan2(py-d6,px)-atan2(d2,sqrt(k); s12=atan2(py-d6,px)-atan2(d2,-sqrt(k); end if m=1 if s1-160*pi/180 t1(1)=s1; mt1(1)=1; end if s12-160*pi/180 t1(2)=s12; mt1(2)=1; end if mt1(1)=0&mt1(2)=0 m=0; end end if m=1 for n=1:2 if mt1(n)=1 s1=sin(t1(1); c1=cos(t1(1); k=4*a22*(a32+d42)-(pz2+(px*c1+py*s1-d6*s1)2-a32-d42-a22)2; k0=pz2+(px*c1+py*s1-d6*s1)2-a22-a32-d42; if k0 m=0; else s31=-atan2(d4,a3)-atan2(sqrt(k),k0); s32=-atan2(d4,a3)+atan2(sqrt(k),k0); if (s31-45*pi/180)|(s32-45*pi/180) else m=0; end if m=1 k=a32+d42-a22-pz2-(px*c1+py*s1-d6*s1)2; k0=4*a22*(a32+d42)-(a32+d42-a22-pz2-(px*c1+py*s1-d6*s1)2)2; k1=d6*s1-px*c1-py*s1; if k0=0 t21=-atan2(k,sqrt(k0)-atan2(k1,pz); t22=atan2(k,sqrt(k0)-atan2(k1,pz); if (t21-225*pi/180)|(t22-225*pi/180) t=1; end end end end end end end if t=1 x(i)=px; y(i)=py; z(i)=pz; i=i+1; end end endendscatter3(x(1:(i-1),y(1:(i-1),z(1:(i-1),2,.);xlabel(X/mm)ylabel(Y/mm)zlabel(Z/mm)title(逆解运算，步距10);b) 正解程序clear;clc;syms c1 c2 c3 c4 c5 c6;a2=431.8;d2=149.09;a3=20.32;d4=433.07;d6=56.25;A1=cos(c1),0,-sin(c1),0;. sin(c1),0,cos(c1),0;. 0,-1,0,0;0,0,0,1;A2=cos(c2),-sin(c2),0,a2*cos(c2);. sin(c2),cos(c2),0,a2*sin(c2);. 0,0,1,d2;0,0,0,1;A3=cos(c3),0,-sin(c3),a3*cos(c3);. sin(c3),0,cos(c3),a3*sin(c3);. 0,-1,0,0;0,0,0,1;A4=cos(c4),0,-sin(c4),0;. sin(c4),0,cos(c4),0;. 0,-1,0,d4;. 0,0,0,1;A5=cos(c5),0,sin(c5),0;. sin(c5),0,-cos(c5),0;. 0,1,0,0;0,0,0,1;A6=cos(c6),-sin(c6),0,0;. sin(c6),cos(c6),0,0;. 0,0,1,d6;. 0,0,0,1;c4=pi;c5=0;c6=0;A4=eval(A4);A5=eval(A5);A6=eval(A6);T36=A4*A5*A6;x=zeros(26000,1);y=zeros(26000,1);z=zeros(26000,1);i=0;for c1=(-160:10:160)*pi/180 T01=eval(A1); for c2=(-225:10:45)*pi/180 T12=eval(A2); T02=T01*T12; for c3=(-225:10:45)*pi/180 T23=eval(A3); T03=T02*T23; T06=T03*T36; Object=T06*0;0;0;1; i=i+1; x(i)=Object(1); y(i)=Object(2); z(i)=Object(3); end endendplot3(x(1:i),y(1:i),z(1:i),.);fprintf(1,n循环次数为%dn,i);grid on;xlabel(X);ylabel(Y);zlabel(Z);title(正解运算，步距10);c) 改进的正解程序clear;clc;syms c1 c2 c3;x=zeros(3000000,1);y=zeros(3000000,1);z=zeros(3000000,1);i=0;for c1=(-160:2:160)*pi/180 for c2=(-225:2:45)*pi/180 for c3=(-225:2:45)*pi/180 i=i+1; x(i)=(2159*cos(c1)*cos(c2)/5 - (12233*sin(c2 + c3)*cos(c1)/25 . - (14909*sin(c1)/100 + (508*cos(c1)*cos(c2)*cos(c3)/25 . - (508*cos(c1)*sin(c2)*sin(c3)/25; y(i)=(14909*cos(c1)/100 - (12233*sin(c2 + c3)*sin(c1)/25 . + (2159*co