哈工大PUMA机器人大作业

2017年秋季学期研究生课程考核（读书报告、研究报告）考核科目:机器人技术学生所在院（系）:机电学院学生所在学科:机械电子学生姓名:王学号:17S学生类别:学硕考察结果:阅卷人:PUMA机器人正逆运动学推导及运动空间解算图1PUMA机器人模型任务:1.建立坐标系；2.给出D-H参数表；3.推导正、逆运动学；4.编程得工作空间。一、建立坐标系根据PUMA机器人运动自由度，在各关节处建立坐标系如图2所示。图2PUMA机器人坐标系建立图其中0O与1O原点交于一点,4O与5O原点交于一点。二、D-H参数表D-H参数表可根据坐标系设定而得出，见表1。1)i为绕1iZ轴，从1iX到iX的角度；2)ia为绕iX轴，从1iZ到iZ的角度；3)il为沿iX轴，从1iZ与iX交点到iO的距离；4)id为沿1iZ轴，从1iZ与iX到1iO的距离。杆件iiliid变量i（当前值）变量范围109001（90）16016022a02d2（0）2254533a9003(90)2254540904d4(180)70350509005(0)1001006006d6(0)266266表1PUMA机器人杆件参数表三、正运动学推导由坐标系及各杆件参数可得到6个连杆变换矩阵。111101cos0sin0sin0cos001000001T22222222122cossin0cossincos0sin0010001aaTd3333333323cos0sincossin0cossin01000001aaT4444344cos0sin0sin0cos00100001Td555545cos0sin0sin0cos001000001T6666566cossin00sincos000010001Td根据各连杆变换矩阵相乘，可以得到PUMA机械手变换矩阵，其矩阵为关节变量的函数。00123456112233445566TTTTTTT将上述变换矩阵逐个依次相乘可以得到06T。060001xxxxyyyyzzzznoapnoapTnoap等式联立，对比等式左右两侧元素可得：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分别对应16，将关节变量角度的当前值代入以上公式得到：22460630-10-001-1000001daddTa计算结果与当前位置姿态一致，推导结果无误。四、逆运动学推导逆运动学推导为末端位置姿态n,o,a,p确定，求解关节变量1。1101111cossin000010sincos000001T222221122cossin0sincos000010001aTd33321333cossin00010sincos000001aT44431444cossin00001sincos000001dT5541555cossin000010sincos000001T66665166cossin00sincos000010001Td五、工作空间的逆解运算从上述的推导中我们已经得到了关节变量16的推导公式。根据逆解的求解思想，在这里我们参照正解所求得的工作间，将机器的运动空间划分成了x=-1000:n1:1000,y=-1000:n2:1000,z=-1000:n3:1000,所组成的空间点集合，其中n1,n2,n3代表步距，单位为mm。060001xxxxyyyyzzzznoapnoapTnoap在程序中通过循环，将此空间上的点的坐标一一代入06T中的xp，yp，zp。06T中n,o,a，所对应的9个参数，代表了末端坐标系（执行器）各坐标轴与零坐标系（固定坐标系）各坐标轴之间的夹角余弦值。在这里为了简便运算，减少运算量，我们讨论固定姿态下机器人的工作空间。令=-1xo，=1ya，=-1zn，其余六个参数等于零，即当前PUMA机器人末端执行器相对于固定坐标系的姿态。至此，求解关节变量的参数均已给出。另外考虑到手腕环节（后三个坐标系）对工作空间的影响不大，为了简化运算，在这里只将逆解求解到13，而不再讨论46。逆解源代码，请参照附录。逆解求解的结果如图36。六、工作空间的正解运算机器人正解的末端执行器（6O）相对于固定坐标系（0O）的转换矩阵06T已经求出。取6O上的原点6(0,0,0,1)TO,066*TO就可得到6O上的原点在固定坐标系下的坐标，即工作空间中一个点。通过将16在对应的变量范围内变换便可以得到整个工作空间。在这里为了减少运算量，同逆解，我们不讨论46的变化，只讨论13在各自运动范围内的变化。正解源代码及改进的高效率代码请参照附录。逆解正解的结果如图710。图3PUMA机器人逆解空间（轴测图）图4PUMA机器人逆解空间（XOY）图5PUMA机器人逆解空间（XOZ）图6PUMA机器人逆解空间（YOZ）图7PUMA机器人正解空间（轴测图）图8PUMA机器人正解空间（XOY）图9PUMA机器人正解空间（XOZ）图10PUMA机器人正解空间（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);forpx=-1000:10:1000forpy=-1000:10:1000forpz=-1000:10:1000j=j+1;m=1;t=0;k=px^2+(py-d6)^2-d2^2;ifk0m=0;elses1=atan2(py-d6,px)-atan2(d2,sqrt(k));s12=atan2(py-d6,px)-atan2(d2,-sqrt(k));endifm==1ifs1160*pi/180&&s1-160*pi/180t1(1)=s1;mt1(1)=1;endifs12160*pi/180&&s12-160*pi/180t1(2)=s12;mt1(2)=1;endifmt1(1)==0&&mt1(2)==0m=0;endendifm==1forn=1:2ifmt1(n)==1s1=sin(t1(1));c1=cos(t1(1));k=4*a2^2*(a3^2+d4^2)-(pz^2+(px*c1+py*s1-d6*s1)^2-a3^2-d4^2-a2^2)^2;k0=pz^2+(px*c1+py*s1-d6*s1)^2-a2^2-a3^2-d4^2;ifk0m=0;elses31=-atan2(d4,a3)-atan2(sqrt(k),k0);s32=-atan2(d4,a3)+atan2(sqrt(k),k0);if(s31225*pi/180&&s31-45*pi/180)…||(s32225*pi/180&&s32-45*pi/180)elsem=0;endifm==1k=a3^2+d4^2-a2^2-pz^2-(px*c1+py*s1-d6*s1)^2;k0=4*a2^2*(a3^2+d4^2)-(a3^2+d4^2-a2^2-pz^2-…(px*c1+py*s1-d6*s1)^2)^2;k1=d6*s1-px*c1-py*s1;ifk0=0t21=-atan2(k,sqrt(k0))-atan2(k1,pz);t22=atan2(k,sqrt(k0))-atan2(k1,pz);if(t2145*pi/180&&t21-225*pi/180)…||(t2245*pi/180&&t22-225*pi/180)t=1;endendendendendendendift==1x(i)=px;y(i)=py;z(i)=pz;i=i+1;endendendendscatte