哈工大PUMA机器人大作业

0 sin 3 a3 cos 3 0 cos 3 a3 sin 3 1 0 0 0 0 1 0 sin 5 0 cos 5 1 0 0 0 0 0 0 1
0 0 0 0 1 d6 0 1
根据各连杆变换矩阵相乘，可以得到 PUMA 机械手变换矩阵，其矩阵为关节 变量的函数。
坐标系（固定坐标系）各坐标轴之间的夹角余弦值。在这里为了简便运算，减少 运算量，我们讨论固定姿态下机器人的工作空间。令 ox =-1 ， a y =1 ， nz =-1 ，其余 六个参数等于零，即当前 PUMA 机器人末端执行器相对于固定坐标系的姿态。至 此，求解关节变量的参数均已给出。 另外考虑到手腕环节（后三个坐标系）对工作空间的影响不大，为了简化运 算，在这里只将逆解求解到 1 3 ，而不再讨论 4 录。逆解求解的结果如图 3 6 。
d4
0
0
d6
266
266
表 1 PUMA 机器人杆件参数表
三、 正运动学推导
由坐标系及各杆件参数可得到 6 个连杆变换矩阵。
cos 1 0 sin 1 sin 0 cos 1 1 0 T1 1 0 0 0 0 0 cos 3 sin 3 2 T3 0 0 cos 5 sin 5 4 T5 0 0
0 0 0 1
cos 2 sin 2 1 1 T2 0 0 cos 4 0 3 1 T4 sin 4 0
sin 2 0 a2 cos 2 0 0 0 1 d 2 0 0 1 sin 4 0 cos 4 0 0 0 1 d 4 0 0 0 1 0 0 0 0 1 d 6 0 1
二、 D-H 参数表
D-H 参数表可根据坐标系设定而得出，见表 1。 1) i 为绕 Z i 1 轴，从 X i 1 到 X i 的角度； 2) ai 为绕 X i 轴，从 Z i 1 到 Z i 的角度； 3) li 为沿 X i 轴，从 Z i 1 与 X i 交点到 Oi 的距离； 4) d i 为沿 Z i 1 轴，从 Z i 1 与 X i 到 Oi 1 的距离。
0 0 0 T6 -1 0
-1 0 0 0
0 -d 2 1 a2 d 4 d 6 0 a3 0 1
计算结果与当前位置姿态一致，推导结果无误。
四、 逆运动学推导
逆运动学推导为末端位置姿态 n,o,a,p 确定，求解关节变量 1 。
cos 1 sin 1 0 0 0 1 0 1 T1 sin 1 cos 1 0 0 0 0
图 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=px^2+(py-d6)^2-d2^2; if k<0 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 && s1>-160*pi/180 t1(1)=s1; mt1(1)=1; end
杆件 i 1 2 3 4 5 6
li
0
i
90
di
0
变量 i （当前值）
变量范围
（ 1 90 ）
（ 2 0）
160
160
a2 a3
0 0 0
0 90 90
90
d2
0
225 225 70
100
45 45 350
100
3 ( 90 )
4 (180 )
5 (0 ) 6 (0 )
cos 3 sin 3 0 a3 0 0 1 0 2 1 T3 0 sin 3 cos 3 0 0 0 1 0 cos 5 0 4 1 T5 sin 5 0 sin 5 0 cos 5 0 0 1 0 0 0 0 0 1
0 0 0 1
cos 2 sin 2 1 T2 0 0 cos 4 sin 4 3 T4 0 0 cos 6 sin 6 5 T6 0 0
sin 2 0 a2 cos 2 cos 2 0 a2 sin 2 0 1 d2 0 0 1 0 sin 4 0 cos 4 1 0 0 0 sin 6 cos 6 0 0 0 0 d4 1
2017 年
秋
季学期研究生课程考核
（读书报告、研究报告）
考
核
科
目:
机 器 人 技 术 机 电 学 院 机 械 电 子 王 17S 学 硕
学生所在院 （系） : 学 生 所 在 学 科: 学 学 学 考 阅 生 察 卷 类 结 生 姓 名: 号: 别: 果: 人:
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)；
nx n 0 T6 y nz 0
ox oy oz 0
ax ay az 0
px py pz 1
在程序中通过循环， 将此空间上的点的坐标一一代入 0T6 中的 p x ，p y ， pz 。
0
T6 中 n,o,a，所对应的 9 个参数，代表了末端坐标系（执行器）各坐标轴与零
6 。逆解源代码，请参照附
六、 工作空间的正解运算
机器人正解的末端执行器 （ O6 ） 相对于固定坐标系 （ O0 ） 的转换矩阵 0T6 已经求出。取 O6 上的原点 O6 (0, 0, 0,1)T , 0T6 * O6 就可得到 O6 上的原点在固 定坐标系下的坐标，即工作空间中一个点。通过将 1 6 在对应的变量范围内变 换便可以得到整个工作空间。 在这里为了减少运算量，同逆解，我们不讨论 4
cos 6 sin 6 sin cos 6 6 5 1 T6 0 0 0 0
五、 工作空间的逆解运算
从上述的推导中我们已经得到了关节变量 1 6 的推导公式。 根据逆解的求 解思想，在这里我们参照正解所求得的工作间，将机器的运动空间划分成了 x=-1000:n1:1000, y=-1000:n2:1000, z=-1000:n3:1000,所组成的空间点集合， 其中 n1,n2,n3 代表步距，单位为 mm。
0
T6 0T1 1 1T2 2 2T3 3 3T4 4 4T5 5 5T6 6
将上述变换矩阵逐个依次相乘可以得到 0T6 。
nx n 0 T6 y nz 0
ox oy oz 0
ax ay az 0
px py pz 1
6 的变化，只讨论 1 3
在各自运动范围内的变化。 正解源代码及改进的高效率代码请参照附录。逆解正 解的结果如图 7 10 。
图 3 PUMA 机器人逆解空间（轴测图）
wk.baidu.com图 4 PUMA 机器人逆解空间（XOY）
图 5 PUMA 机器人逆解空间（XOZ）
图 6 PUMA 机器人逆解空间（YOZ）
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 分别对应 1 6 ，将关节变量角度的当前值代入 以上公式得到：
图 1 PUMA 机器人模型
任务: 1. 建立坐标系； 2. 给出 D-H 参数表； 3. 推导正、逆运动学； 4. 编程得工作空间。
一、 建立坐标系
根据 PUMA 机器人运动自由度，在各关节处建立坐标系如图 2 所示。
图 2 PUMA 机器人坐标系建立图
其中 O0 与 O1 原点交于一点, O4 与 O5 原点交于一点。