河南科技大学硕士学位论文
3.4.2 相邻连杆坐标系的齐次变换矩阵
本文以IRB2400型关节式、六自由度机器人为例进行研究分析。连杆参数：ai代表连杆i的长度；?i为连杆转角；di是沿关节i-1轴线和i轴线两个公垂线的距离，称为偏距。?i是与i-1轴线和i轴线的公垂线垂直平面内的两个公垂线的夹角，称为扭角。机器人各连杆间齐次变换矩阵：?i为关节变量；
表3-1 D-H连杆参数初始值
序号 1 2 3 4 5 6
?i(°) -90 0 -90 90 -90 0
Table3-1 D-H Link parameter initial value
di(m) ai(mm) ?i(°) 0 100 90 0 705 -90 0 135 0 755 0 0 0 0 0 0 0 0
关节角范围 -180°～ 180°
-100°～ 100° -60°～ 65° -200°～ 200° -120°～ 120° -400°～ 400°
图3-6建立了工业机器人IRB2400关节空间坐标系，表3-1列出了其各连杆参数（初始值），这样，用D-H法可以求得各连杆的齐次坐标变换矩阵。
由式3-11可知，相邻连杆的变换矩阵i?1iT为：
?sin?i?cos?i?sin?cos?cos?i?1cos?iii?1i?1Ti???sin?isin?i?1sin?i?1cos?i?00?0ai?1??sin?i?1?disin?i?1?? (3-12) cos?i?1dicos?i?1??01?根据公式3-12及表3-1的关节参数，即可得相邻连杆间的齐次变换矩阵：
?cos?1?sin?1?sin?cos?110?T?1?00?0?000100?0?? (3-13) 0??1?26
第3章 系统运动学分析
?cos?2?01?2T???sin?2??0?sin?20?cos?200a1?10?? (3-14) 00??01?
0a2?00?? (3-15) 10??01?
0a3?1d4?? (3-16) 00??01?
0?0?? (3-17) 0??1??cos?3?sin?3?sin?cos?332?3T??00?0?0?cos?4?03?4T???sin?4??0?sin?40?cos?40?cos?5?sin?50?00?14?5T??sin?5cos?50?00?0
?cos?6?sin?6?005?T?6??sin?6?cos?6?0?0
01000?0?? (3-18) 0??1?求出各相邻连杆的坐标变换矩阵后，接下来就要求出工件坐标系与工具坐标系之间的变换矩阵、工件坐标系与基坐标系之间的变换矩阵。首先求出工件坐标
6系与工具坐标系之间的变换矩阵TT，他们的位置关系如图3-7所示，他们的齐
次变换矩阵为：
?1?06?TT??0??0000?100?? (3-19) 01L??001?工件坐标系{S}与基坐标系{0}的位置关系如图3-8所示，变换矩阵为：
27
河南科技大学硕士学位论文
?1?0S?OT??0??0010001000?00?? (3-20) 1800??01?为了后面计算的方便，需要求出工具坐标系与工件坐标系之间的变换矩阵
STT。由矩阵变换可得
STSO6T?OT6TTT (3-21)
从上述求解中，已经求出了基坐标系{0}与工件坐标系{S}的齐次变换矩阵
SO6T,也求出了工具坐标系{T}与坐标系{6}的齐次变换矩阵TT。
3.4.3 正向运动学方程
正想运动学方程就是求解末端执行器在基坐标系里位姿。前面已经分析了相邻连杆的位姿变换矩阵，根据D-H参数法，第2个连杆相对基坐标系的位姿矩阵为：
021T?0T2T (3-22) 1依此类推，则可以得出第n个连杆相对基系的位置和姿态矩阵：
0n11T?0T2T……n?nT (3-23) 1本课题研究的工业机器人机器人IRB2400由六个连杆组成，则末端执行器的位姿矩阵为：
012345T?T2T3T4T5T6T (3-24) 1
06将各连杆变换矩阵相乘，便得到该六自由度机器人末端执行器相对基础坐标的变换矩阵06T，即为该机器人的正运动学方程：
28
第3章 系统运动学分析
?nx?ny0012345??T?TTTTTT6123456?nz??0oxoyoz0axayaz0px?py?? (3-25) pz??0?式中，
nx?c1[c23(c4c5c6?s4s6)?s23s5c6]?s1(s4c5c6?c4s6)
ny?s1[c23(c4c5c6?s4s6)?s23s5c6]?c1(s4c5c6?c4s6)
nz??s23(c4c5c6?s4s6)?c23s5c6
ox?c1[c23(?c4c5c6?s4s6)?s23s5s6]?s1(c4c6?s4c5c6)
oy?s1[c23(?c4c5s6?s4c6)?s23s5c6]?c1(c4c6?s4c5s6)
oz??s23(?c4c5c6?s4c6)?c23s5s6 ax??c1(c23c4s5?s23c5)?s1s4s5
ay??s1(c23c4s5?s23c5)?c1s4c5
az?s23c4s5?c23c5
px?c1(a2c2?a3c23?d4s23?a1)
py?s1(a2c2?a3c23?d4s23?a1)
pz??a3s23?a2s2?d4c23
（式中s1?sin?1、s23?sin(?2??3)、c1?cos?1、c23?cos(?2??3)??下同）
该矩阵的n、o、a表示了末端执行器相对基坐标的姿态，而p则代表了末端执行器相对基坐标的位置。为了验证计算的正确与否，将表3-1中的连杆角度初始值带入式3-18中，求得，
?0?00?6T??1??0100000?1855?? (3-26) 0840??01?结果与图3-6的姿态相同，验证了运动学正解分析的正确性。
为了运动学逆解的方便，需要求出一些中间结果，对于所有的中间结果这里
29
河南科技大学硕士学位论文
不一一写出，只写出一个中间结果1T6以备求运动学逆解时使用：
?1nx?1112345?ny=T?TTTTT623456?1nz???011ox11axoy1oz0ay1az0px??1py? (3-27) 1pz??0??1式中，
1nx?c23(c4c5c6?s4s6)?s23s5c6 ny??s4c5c6?c4s6
11nz??s23(c4c5c6?s4s6)?c23s5c6
11ox??c23(c4c5c6?s4c6)?s23s5s6 oz?s23(c4c5c6?s4s6)?c23s5s6 ax??c23c4s5?s23c5 ay?s4s5
oy?s4c5s6?c4c6
1111az?s23c4s5?c23c5 px?a2c2?a3c23?d4s23 py?d2c1
111pz??a3s23?a2s2?d4c23
3.5 逆向运动学分析
运动学逆解就是已知末端执行器的位姿和连杆参数，求机器人各关节连杆变量。这是一个很复杂的非线性问题。当已知机器人末端执行器相对于参考坐标系的期望位值和姿态，求其相对应的各关节的转角变化量。很显然，比较正解法，机器人的逆解问题更加复杂和难解。一般情况下其解不是惟一的。有时会存在一些不能实现的位置和方向，有时又会出现求不出数值解的情况。 3.5.1 逆解问题的可解性
1．工作空间
30