【实例1：KUKA机器人运动学方程】

以如图1-1所示KUKA KR6-900机器人为例，它采用旋量方法描述机器人的运动。已知机器人初始位姿时各轴的旋量坐标为：

ξ1＝［0　0　0.220 5　0　0　1］T；ξ2＝［0.025　0　0.4　0　1　0］T

ξ3＝［0.48　0.055 5　0.4　0　1　0］T；ξ4＝［0.681　0　0.425　－1　0　0］T

ξ5＝［0.9　0.032　0.425　0　1　0］T；ξ6＝［0.98　0　0.425　－1　0　0］T

试用D-H矩阵方法建立其运动学方程。

1）建立坐标系，确定D-H参数

根据上述各轴在机器人位于初始位姿的旋量坐标以及机器人的结构，各连杆的D-H坐标系建立如图3-9所示。

图3-9　KR6-900机器人的D-H坐标系

各连杆D-H参数见表3-3。　　

表3-3　KR6 900机器人的D-H参数

2）计算D-H矩阵

将表3-3中的D-H参数代入式（3-1）可得

3）求机器人运动学方程

将上述六个矩阵代入式（3-3）可得

其中

r11＝c1［c23（c4c5c6－s4s5）－s23s5c5］＋s1（s4c5c6＋c4s6）

r21＝s1［c23（c4c5c6－s4s6）－s23s5c6］－c1（s4c5c6＋c4s6）

r31＝－s23（c4c5c6－s4s6）－c23s5c6

r12＝c1［c23（－c4c5c6－s4c6）＋s23s5c6］＋s1（c4c6－s4c5s6）

r22＝s1［c23（－c4c5c6－s4c6）＋s23s5c6］－c1（c4c6－s4c5s6）

r32＝－s23（－c4c5c6－s4c6）＋s23s5s6

r13＝－c1（c23c4c5＋s23c5）－s1s4s5

r23＝－s1（c23c4c5＋s23c5）＋c1s4s5

r33＝－s23c4c5＋c23c5

px＝c1（a2c2＋a3c23－d 4s23）－d 3s1

py＝s1（a2c2＋a3c23－d 4s23）＋d 3c1(www.daowen.com)

pz＝－a2s23－a2s2－d 4c23

式中，ci＝cosθi；si＝sinθi；cij＝cos（θi＋θj）；sij＝sin（θi＋θj）。

【实例2：ABB机器人运动学方程】

ABB机器人IRB 120的结构如图3-10所示，已知其D-H参数（表3-4），试求其正向运动学方程。　　

表3-4　IRB 120机器人的D H参数

图3-10　IRB 120机器人

1）建立坐标系，计算D-H矩阵

IRB 120机器人的D-H坐标系建立如图3-11所示。

图3-11　IRB 120机器人的D-H坐标系

将表3-4中的D-H参数分别代入式（3-1），可以确定6个连杆的D-H矩阵：T、T、…、T、T。

2）建立机器人运动学方程

将T、T、…、T、T代入式（3-3）可得

其中

r11＝c1［c23（c4c5c6－s4s6）－s23s5c6］＋s1（s4c5c6＋c4s6）

r12＝c1［c23（－c4c5s6－s4c6）＋s23s5s6］＋s1（c4c6－s4c5s6）

r13＝－c1［s23c5＋c23c4s5］－s1s4s5

r21＝s1［c23（c4c5c6－s4s6）－s23s5c6］－c1（c4s6＋s4c5c6）

r22＝s1［c23（－c4c5s6－s4c6）＋s23s5s6］－c1（c4c6－s4c5s6）

r23＝c1s4s5－s1（c5s23＋c4s5c23）

r31＝s23（s4s6－c4c5c6）－c23s5c6

r32＝s23（s4c6＋c4c5s6）＋c23s5s6

r33＝s23c4s5－c23c5

px＝－s1d 2＋c1（c23a3－s23d 4＋a2c2）

py＝c1d 2＋s1（c23a3－s23d 4＋a2c2）

pz＝－s23a3－c23d 4－a2s2

式中，ci＝cosθi；si＝sinθi；cij＝cos（θi＋θj）；sij＝sin（θi＋θj）。