§1.4 机器人正向运动学
工业机器人的正向运动学是指已知各关节的类型、相邻 关节之间的尺寸和相邻关节相对运动量的大小时，如何确 定工业机器人末端操作器在固定坐标系中的位姿。
主要包括以下内容： 1) 相对杆件的坐标系的确定； 2) 建立各连杆的模型矩阵A； 3) 正运动学算法；
D-H表示法
学习目标：1. 理解D-H法原理 2. 学会用D-H法对机器人建模
C234a4 ) S234a4 )
进而可得：
4 234 2 3
再 根 据 对 应 项 元 素 相 等， 可 以 得 到
S5 C23（4 C1ax S1a y ) S234az
C5 C1a y S1ax
5

arctan C234 (C1ax S1a x
#

d
a

1
1
0
0
90
2
2
0
a2
0
3
3
0
a3
0
4
4
0
a4
-90
5
5
0
0
90
6
6
0
0
0
nTn1 An1 Rot(z,n1 ) Trans (0,0, dn1 ) Trans (an1 ,0,0) Rot ( x,n1 )
C n1
An1



1 0

0
0
0
1

C3 S3 0 C3a3
A3


S3
0
C3 0
0
S
3a3

1 0

0
0
0
1

C4 0 S4 C4a4
A4


S4
0
0 1
C4 0
S4a4

0

0
0
0
1

C5 0 S5 0
A5


S5
0
0 1
C5 0
S n1
0

0
Sn1C n1 C n1C n1
S n1
0
S n1 S n1 Cn1S n1
C n1
0
an1C n1
a
n1
S
n1

dn1

1
#

d
a

1
1
0
0
90
2
2
0
a2
0
3
3
0
a3
0
4
4
0
a4
-90
5
5
0
O4
x2
z5
y5
x4
O5
y4
z2
y2
关节3
A1 连杆2
O2 坐标系2
x5
o3 , o4 , o5重合 d4 d5 0
关节2 O1
z1
坐标系1
y1 连杆1
x1
d2
关节1 坐标系0
ai—沿 xi 轴， zi-1 轴与 xi 轴交点到Oi 的距离 αi — 绕 xi 轴，由 zi-1 转向zi di — 沿 zi-1 轴，zi-1 轴和 xi 交点至Oi –1 坐标
0
90
C1 0 S1 0
A1


S1
0
0 1
C1 0
0 0

0
0
0
1
6
6
0
0
0
第四步：将参数代入A矩阵，可得到
C1 0 S1 0
A1


S1
0
0 1
C1 0
0 0

0
0
0
1
C2 S2 0 C2a2
A2


S2
0
C2 0
0
S
2a2
• 为右手坐标系 • 原点Oi： Ai与Ai+1关节轴线的交点
A6
y6
z6
A5
连杆5
• zi轴：与Ai+1关节轴重合，指向任意
x6
O6
关节6
关节5 坐标系4
• xi轴： Zi和Zi-1构成的面的法线 • yi轴：按右手定则
坐标系5
d6 z4
A4 z3
关节4 坐标系3
x3
连杆4
y3
O3
连杆3
A3
d3 A2
C234 (C1nx S1ny ) S234nz
C1ny S1nx S234 (C1nx S1ny )
C
234nz

0

C234 (C1ox S1oy )
S234oz C1oy S1ox
S234 (C1ox S1oy )
C 234oz 0
坐标系的确定
1.第一个关节指定为关节 n,第二个关节为n+1,其余 关节以此类推。
2.Z轴确定规则：如果关 节是旋转的，Z轴位于按 右手规则旋转的方向， 转角 为关节变量。如 果关节是滑动的，Z轴为 沿直线运动的方向，连 杆长度d为关节变量。关 节n处Z轴下标为n-1。
3.X轴确定规则 情况1：两关节Z轴既不平行也不相交 取两Z轴公垂线方向作为X轴方向，命名规则同Z轴。

S1a y
)
接下来再一次利用式
pxC1 p y S1 C234a4 C23a3 C2a2
pz S234a4 S23a3 S2a2
由于C12=C1C2-S1S2以及S12=S1C2+C1S2，最后得到：
2

arctan (C3a3 (C3a3

a2 )( pz S234a4 ) S3a3 ( pxC1 py S1 a2 )( pxC1 py S1 C234a4 ) S3a3 ( pz
axC1 a y S1 az
ax S1 a yC1 0
C234C5C6 S234 S6

S234C5C
6Hale Waihona Puke C234 S6
S5C6

0
C234C5C6 S234C6 S234C5C6 C234C6
S5 S6 0
PxC1 Py S1
pz

Px
S1
an α n 0 -900
a2
0
a3 -900
0 900
0 -900
00
例3
对下图所示简单机器人，根据D-H法，建立必要坐标系及 参数表。
第一步：根据D-H法建立坐标系的规则建立坐标系
第二步：将做好的坐标系简化为我们熟悉的线图形式
第三步：根据建立好的坐标系，确定各参数，并写 入D-H参数表
将上面两个方程两边平方相加，并利用和差化积公式得到
S2 S23 C2C23 cos3
于是有：
C3

(
pxC1

py S1
C234a4 )2 ( pz 2a2a3

S234a4 )2

a22

a32
已知 S3 1 C32
于是可得到：
3

arctan
S3 C3
依次类推，分别在方程2.19两边左乘A1~A4的逆，可得到
C234 (C1ax
S1a y ) S234ax C1a y S1a x S234 (C 1ax S1a y )
C 234a z 0
C234 (C1 px S1 py ) S234 pz

C34a2 C4a3 a4

0


S234 (C1Px

S1 Py

S1(C234a4

C23a3 C2a2 S234a4 S23a3
) S2a2

1

求逆运动学方程的解
依次用 A1左1 乘上面两个矩阵，得到：
nxC1 ny S1

nz

nx
S1

nyC1
0
oxC1 oy S1 oZ
ox S1 oyC1 0
情况2：两关节Z轴平行 此时，两Z轴之间有无数条公垂线，可挑选与前一关节的公垂线共线的 一条公垂线。 情况3：两关节Z轴相交
取两条Z轴的叉积方向作为X轴。 4.Y轴确定原则
取X轴、Z轴叉积方向作为Y轴方向。（右手）
5.变量选择原则
用θn+1角表示Xn到Xn+1绕Zn轴的旋转角;dn+1表示从Xn到Xn+1沿Zn测量
0 0

0
0
0
1
C6 S6 0 0
A6


S
6
0
C6 0
0 0 1 0

0
0 0 1
第5步 求出总变化矩阵
RTH A1 A2 A3 A4 A5 A6
C1(C234C5C6 S234 S6 ) S1S5C6

S1(C234C5C6 C1S5 S6
)

C 234
pz

S34a2 S4a3 1

C5C6 C5S6 S5 0

S5C
6
S5S6
C5
0

S6
C6
0 0
0
0
0 1
234

arctan(
az C1ax
S1a y