机器人运动学
2005年 3月 24日
运动学正问题
? 杆件参数的意义
? 坐标系的建立原则
? 杆件坐标系间的变换过程 -相邻关节坐标
系的齐次变换
? 机器人的运动学方程
杆件参数的意义 - 和
? li 关节 Ai轴和 Ai+1轴
线公法线的长度
? 关节 i轴线与 i+1
轴线在垂直于 li平面
内的夹角
串联关节, 每个杆件最多与 2个杆件相连, 如 Ai与 Ai-1和
Ai+1相连 。 由运动学的观点来看, 杆件的作用仅在于它能保
持其两端关节间的形态不变 。 这种形态由两个参数决定, 一
是杆件的长度 li（ ）, 一个是杆件的扭转角
i?ia
Ai
Ai+
1
i?
il
i?
il i
?
杆件参数的意义 - 和
? 是从第 i-1坐标系
的原点到 Zi－１ 轴和
Ｘ i轴的交点沿 Ｚ i-1
轴测量的距离
? 绕 Zi-1轴由 Ｘ i-1
轴转向 Ｘ i轴的关节
角
确定杆件相对位置关系, 由另外 2个参数决定, 一个是杆
件的距离：, 一个是杆件的回转角：
i?i
d
i?
id i?
Ai
Ai+
1
i?
il
id
1?il i?
Ai-1
id
坐标系的建立原则
Ai
Ai+
1
i?
il
id
1?il
i?
Ai-1
1?iz
1?ix
1?iy
1?io
iz
ix
iy
io
? 为右手坐标系
? 原点 Oi：设在 Li与
Ai+1轴线的交点上
? Zi轴：与 Ai+1关节轴
重合，指向任意
? Xi轴：与公法线 Li
重合，指向沿 Li由
Ai轴线指向 Ai+1轴线
? Yi轴：按右手定则
Li— 沿 xi 轴, zi-1 轴与 xi 轴交点到 0i 的距离
αi— 绕 xi 轴, 由 zi-1转向 zi
di— 沿 zi-1 轴, zi-1轴和 xi 交点至 ∑0i–1坐标系原点的距离
θi— 绕 zi-1 轴, 由 xi-1转向 xi
?
杆件坐标系间的变换过程
-相邻关节坐标系的齐次变换
? 将 xi-1轴绕 zi-1轴转 ?i 角度，将其与 xi轴平行；
? 沿 zi-1轴平移距离 di,使 zi-1轴与 zi轴重合；
? 沿 xi轴平移距离 Li，使两坐标系原点及 x轴重
合；
? 绕 xi 轴转 ?i角度，两坐标系完全重合．
机器人的运动学方程
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1
s i n
c o s
0
c o s
s i nc o s
s i ns i n
0
s i n
c o sc o s
c o ss i n
0
0
s i n
c o s
1
i
ii
ii
i
ii
ii
i
ii
ii
i
i
i
i
d
a
a
A
?
?
?
??
??
?
??
??
?
?
0 0 1 1
12
i
iiT A A A
?? ? ? ? ?
D-H变换矩阵
运动学逆问题
? 多解性，剔除多余解原则
?根据关节运动空间合适的解
?选择一个与前一采样时间最接近的解
?根据避障要求得选择合适的解
?逐级剔除多余解
? 可解性
?所有具有转动和移动关节的系统，在一个单一串联中
总共有 6个（或小于 6个）自由度时，是可解的，一般
是数值解，它不是解析表达式，而是利用数值迭代原
理求解，它的计算量要比解析解大
?如若干个关节轴线相交和或多个关节轴线等于 0或 90°
的情况下，具有 6个自由度的机器人可得到解析解
例题：
? 试求立方体中心在机座坐标系 ∑ 0中的位置
? 该手爪从上方把物体抓起，同时手爪的开合方向与物体的 Y轴同向，
那么，求手爪相对于 ∑ 0的姿态是什么？
在机器人工作台上加装一电视摄像机，摄像机可见到固联
着 6DOF关节机器人的机座坐标系原点，它也可以见到被操作
物体（立方体）的中心，如果在物体中心建一局部坐标系，则
摄像机所见到的这个物体可由齐次变换矩阵 T1来表示，如果摄
像机所见到的机座坐标系为矩阵 T2表示。
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
101-00
2001-0
10-001
T
1000
91-00
10001
1010
T 21
xy z
解 1：
x
y z
z
机 y
机
z
物
y
物
x
物
o
O
机
O
物
T T 21 物机机摄物摄 求，，已知 TTT ??
TT 11-2 ）（有,物摄摄机物机 ?? TTT
1000
91-00
10001
1010
1000
101-00
2001-0
10001
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
1100
10001-
11010
∑O 物 根据 T 1 画出
∑O 机 根据 T 2 画出
因此物体位于机座坐标系的（ 11,10,1） T
处，它的 X,Y,Z轴分别与机座坐标系的
-Y,X,Z轴平行。
解 2：
x
y z
z 机 y
机
z 物
y 物
x 物
o
O 机
O 物
手爪
机实际要求 T
pzazsznz
pyaysyny
pxaxsxnx
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
向重合手爪开合方向与物体 ya, Ts ]001[???有
方向相反方向物体的从上向下抓，指出手爪 zab, Ta ]100[ ???则有
Tkji
kji
asnc ]010[00
100
001,??????
?
?
?
?
?
?
?
?
?
?
?
???? ?
??
?
?
?
?
?
?
?
?
?
?
?
?
1-00
001
010
因此：姿态矩阵为
重合时
与物体中心
当手爪中心
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
11-00
10001
11010
T
物
机
x
y z
z 机 y
机
z 物
y 物
x 物
o
O 机
O 物
手爪
机实际要求 T
pzazsznz
pyaysyny
pxaxsxnx
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
向重合手爪开合方向与物体 ya, Ts ]001[ ???有
方向相反方向物体的从上向下抓，指出手爪 zab,
Ta ]100[ ???则有
O
s
?
n
?
a
?
y
z
x
特殊情况坐标系的建立原则
Oi— Ai与 Ai+1关节轴线的交点
Zi— Ai+1轴线
Xi— Zi和 Zi-1构成的面的法线
Yi— 右手定则
Ai
A
i + 1
oi
zi-1
zi
xi
yi
两个关节轴相交
两个关节轴线平行
? 先建立
∑ 0i-1
? 然后建立
∑ 0i+1
? 最后建立
∑ 0i
A
i - 1 A
i
A
i + 1
A
i + 2
l
i - 1
o
i - 1
x
i - 1
y
i - 1
z
i - 1
A
B
C
D
o
i
（ x
i
）
（ y
i
）
z
i
x
i
y
i
o
i + 1
x
i + 1
y
i + 1
z
i + 1
d
i + 1
l
i + 1
d
i
举例,Stanford机器人
A1
A2A3
A4
A5
A6
d1
z1
x1y
1
O1
d2
z2
x2
y2
O2
z3
y3
x3
O3
y4
z4
x4
O4
z5 y5
x5
O5
345
45
,,0o o odd? ? ?重 合
d3
z6
x6
y6
O6
d6
z0
y0 x0
O0
? 为右手坐标系
? 原点 Oi,Ai与 Ai+1
关节轴线的交点
? Zi轴：与 Ai+1关节轴
重合，指向任意
? Xi轴,Zi和 Zi-1构
成的面的法线
? Yi轴：按右手定则
Li— 沿 xi 轴, zi-1 轴与 xi 轴交点到 0i 的距离
αi— 绕 xi 轴, 由 zi-1转向 zi
di— 沿 zi-1 轴, zi-1轴和 xi 交点至 ∑0i–1坐标系原
点的距离
θi— 绕 zi-1 轴, 由 xi-1转向 xi
解：
? 用未知的逆变换逐次左乘，由乘得的矩阵
方程的元素决定未知数，即用逆变换把一
个未知数由矩阵方程的右边移到左边
? 求解这个未知数
? 把下一个未知数移到左边
? 重复上述过程，直到解出所有解
运动学逆问题解法
Paul 等人提出的方法,
Paul 等人提出的方法
65544332211060 TTTTTTT ?
TTTTTT T 6554433221601-10 ?）（ 1 q?
65544332601-101-21 TTTTTTT ?）（）（ 2q?
65601-101-21132143154 TTTT)T()T()T( ???? ）（）（5 q?
? ? ?
E?601-101-65 TT) T( ）（? 6 q?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
6
0
pzazsznz
pyaysyny
pxaxsxnx
T
机器人末端操作器位姿的其它描述方法
? 用矩阵表示刚性体的转动简化了许多运算，
但它需要 9个元素来完全描述旋转刚体的姿
态，因此矩阵并不直接得出一组完备的广义
坐标。
? 一组广义坐标应能描述转动刚体相对于参考
坐标的方向，被称为欧拉角的三个角度,φ,
θ, ψ 就是这种广义坐标。
? 有几种不同的欧拉角表示方法，它们均可描
述刚体相对于固定参考系的姿态。三种最常
见的欧拉角类型列在表中
3种最常见的欧拉角类型
步 1 步 2 步 3
类型 1 绕 OZ轴转 φ 角 绕当前 OU' 轴转 θ角 绕当前 OW″ 轴转 ψ 角
类型 2 绕 OZ轴转 φ 角 绕当前 OV '轴转 θ角 绕当前 OW″ 轴转 ψ 角
类型 3 绕 OX轴转 φ 角 绕 OY轴转 θ角 绕 OZ轴转 ψ 角
φ
φ
φ
u′
v′
w′
①
x(u)
y (v)
z (w)
o
θ
u"
v"
θ
w"
②
u???
③ ψ
ψ
ψ
v???
W???
),( ?ZR ),( ???R ),( ?wR ???N0T
?
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
100
0
0
0
0
001
100
0
0
??
??
??
????
??
cs
sc
cs
sccs
sc
?
?
?
?
?
?
?
?
?
?
????
???
?
?????
????????????
????????????
ccsss
sccccssscccs
ssccsscscscc
类型 1：表示法通常用于陀螺运动
类型 2,所得的转动矩阵为右乘
?
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
????
100
0
0
c0s-
010
s0c
100
0
0
),(),v(),(R
??
??
??
??
??
??
???
cs
sc
cs
sc
wRRZR
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
???
?
1000
pz
pyR
px
T
ccsss
ssccscsscccs
sccssccssccc
?????
????????????
????????????
类型 3,一般称此转动的欧拉角
为横滚、俯仰和偏航角，这种形
式主要用于航空工程中分析飞行
器的运动，其旋转矩阵为（这种
方法也叫做横滚、俯仰和偏航角
表示方法）
?
?
?
?
?
?
?
?
?
?
?
??
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
? ?
?
?
?????
????????????
????????????
??
??
??
??
??
??
???
ccscs
sccssccssscs
sscsccsssccc
cs
sc
cs
sc
cs
sc
xRyRz
0
0
001
0
010
0
100
0
0
),(),(),RR （
Z
Y
X
?
?
?
偏航
俯仰
横滚
斯坦福机器人运动学逆问题解
6533211060 AAAAT ??????
61T?
653321 AAA ?????
式中：
yx
yx
pCpSpf
zpf
pSpCpf
1113
12
1111
)(
)(
)(
???
??
??
211 dpcps yx ???由两端矩阵对应元素相等可得：
作三角变换：
式中：
得到：
即有：
（ ）
由 1,4和 2,4元素对应相等，得：
6261121 TTA ???
式中第四列：
6362132 TTA ???
式中第三列：
微动矩阵和微动齐次变换
? 对象, 微动矩阵主要是描述机器人在微动
范围内各关节的位移运动关系
? 定义, 各关节当角度移小于 5° 时，平移
在 0.1mm左右时，微动矩阵大致可用
设,有一机器人如图，末端执行器在机座坐标系中的
齐次变换为 oTN,做微动,① 绕任意轴 w轴转 ； ② 绕
各坐标轴平移 dx,dy,dz
求,在 中的位置和姿态,
? 定义 为微动齐次变换矩阵
N0? 00?
O 0
n
a
s
X 0
Y 0
Z 0
O n
NdT
Nr a n s TdwRdzdydxTT 0N0 ),(),,( ??变化后）（
Nr a n s TdwRdzdydxTdTT 0NN0 ),(),,( ???记为
TdwRdzdydxTdTT r an s ),(),,( ???
TEdwRdzdydxTdT r an s ]),(),,([ ?? ?
)T(
,),(),,(
的左边在注意，
称为微动率，令
?????
???
TdT
EdwRdzdydxT r a n s ?
在忽略高次项
的情况下：微
动齐次变换与
次序无关
?d
微动平移和微动旋转的齐次变换：
平移：
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
100
010
001
dz
dy
dx
T r
旋转 R,绕任意轴 旋转 角,? ??dw,w? ?d
?
?
?
?
?
??
??
??
?
?
?
?
?
????
????
????
?
??
??
??
????
????
????
?
dd
dd
dd
dddd
dddd
dddd
dw
c os)c os(1r
s i nr)c os(1rr
s i nr)c os(1rr
s i nr)c os(1rrs i nr)c os(1rr
c os)c os(1rs i nr)c os(1rr
s i nr)c os(1rrc os)c os(1r
R
2
z
xzy
yzx
xzyyzx
2
yzyx
zyx
2
x
,
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
1000
01
01
01
s i n,1c o s
??
??
??
???
drdr
drdr
drdr
ddd
xy
xz
yz
在微动范围内绕经意轴转动 角,可以看作绕 x,y,z轴的微转
动的合成 。 因此：
?d
?????? drzddryddrxd zyx ???,,
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
01
01
01
,
xdyd
xdzd
ydzd
dwR
??
??
??
?因此：
因此微动率△ = ? ? ? ?? ? EdwRdzdydxT
r a n s ??,,,
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
0000
0
0
0
dzxdyd
dyxdzd
dxydzd
??
??
??
微动的齐次变换,dT= △ ?T
己知变换矩阵
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0001
3100
7010
T,001.0 kjid ????
kjidp 6.003.0 ???
转动：
平移：求 d T
解：
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
0000
6.001.00
01.000
3.0000
1000
0100
0010
0001
1000
011.00
01.010
0001
1000
6.0100
0010
3.0001
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
????
0000
9.01.000
0001.0
3.0000
1000
0001
3100
7010
0000
6.001.00
01.000
3.0000
TdT
?
?
?
?
?
?
?
?
?
?
?
?
?
??
1000
9.01.001
3101.0
3.7010
dTT
反过来：如果我们要求 Σ 在 Σ 中的齐次交换矩阵为
no oo
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0001
3100
7010
T 实际测得的为
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
9.01.001
3101.0
3.7010
那么末端执行器坐标系要如何运动才能到达期望值？
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
011.00
01.010
0001
转动：
?
?
?
?
?
?
?
?
?
?
?
?
1000
6.0100
0010
3.0001
平移：
等效微动位移的求解
? 前面研究的是动坐标系 ΣOn在 ΣOo中的 b
变换为 T，相对于基准坐标系作微平移和
微转动，来求微动齐次交换。
? 现在我们研究动坐标系 Σ On相对于自身
坐标系做了微位移或微转动，达到绕基
准坐标同样的效果则如何求解。
dT=△ ?T （ 绕基准坐标系 ）
=T?△ T （ 绕动坐标系 ）
左乘,绕基准
右乘,绕动坐标轴
强调等效
TTT ??????
TTT ?????? ? 1
1??????? TT
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
zzzz
yyyy
xxxx
pasn
pasn
pasn
T
? ?
? ?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
??
??
?
1000
1
Paaaa
Pssss
pnnnn
T
T
zyx
T
zyx
T
zyx
??
??
??
设,有：
? ? ? ? ? ?
? ? ? ? ? ?
? ? ? ? ? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
????
????
????
??
0000
0
0
0
pdpdaasdand
pdpdssadsnd
pdpdnnadnsd
T ?????????
??????????
??????????
???
???
???
? ?
? ?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?????
?????
?????
?
0000
0
0
0
pdpdandsd
pdpdsndad
pdpdnsdad
????????
????????
????????
???
???
???
绕自身轴的微动率△ Τ 和绕固定坐标系坐标轴的微动
率△之间的什么关系, 举例说明：
例：一动坐标系相对于固定坐标系的齐
次交换为
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0001
3100
7010
T
n s a p
己知相对固定坐标系的微
动平移和转动
??
?
???
???
kjid
kjidp
001.0
6.003.0
?
求,① △ 与 △ Τ
② 求 dT
③ 求与之等效的绕动坐标系的微平移和微转动
解,① △ =
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
0000
0
0
0
dzxdyd
dyxdzd
dxydyd
??
??
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
000
6.001.00
01.000
3.0000
? ? ? ?
? ? ? ?
? ? ? ? 0100001.0
1.0001001.0
0010001.0
???????
???????
???????
kjikjind
kjikjisd
kjikjiad
??
??
??
?
?
?
? ? ? ? ? ?kjikjikjipdpd 6.003.0037001.0 ??????????? ????
? ?kji
kji
6.003.0
037
001.0 ????
? ? ? ?
kji
kjikji
9.003.0
6.003.03.000
???
??????
? ? ? ?? ?
? ? ? ?? ?
? ? ? ?? ? 09.003.0010
3.09.003.0001
9.09.003.0100
????????
????????
????????
kjikjipdpda
kjikjipdpds
kjikjipdpdn
????
????
????
?
?
?
? ?
? ?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
????
???
???
???
0000
0
0
0
pdpdandsd
pdpdsndad
pdpdnsdad
T ??
??????
????????
????????
???
???
???
?
?
?
?
?
?
?
?
?
?
?
?
?
?
0000
0001.0
3.0000
9.01.000
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
????
0000
9.01.000
0001.0
3.0000
1000
0001
3100
7010
0000
6.001.00
01.000
3.0000
TdT
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
????
0000
9.01.000
0001.0
3.0000
0000
0001.0
3.0000
9.01.000
1000
0001
3100
7010
TTdT
解 ②,
解 ③, 绕自身平移和转动
?
?
?
???
???
TTTT
TTTT
kjid
kjidp
01.00
03.09.0
?
其结果等于绕固定坐标系转
动和旋转
??
?
???
???
kjid
kjidp
001.0
6.003.0
?
等效
说明,如果我们发现末端操作器相对于基准坐标系有了
微位移 （ 平移或转动 ）,我们可以认为末端操作器相对
于自己的坐标系发生了微位移 。 只是微动率 △ 和 △ Τ不
同而己 。 其结果是等效的 。
这些在进行误差补偿和微动时有用,如产生误差
如何补偿？ 可以反向运动末端关节来补偿 ?
?? ?d
dp
微动齐次变换的意义
误差及误差补偿
? 制造和检测误差
? 运算过程中圆整、插补、拟合造成的误差 — 原理性误差
? 构件承受的负载、加速度、重力的变形误差
? 传动误差
? 环境影响误差
误差来源：
? 单关节补偿
? 多关节补偿
误差补偿：
单关节补偿：
? ? ? ? ? ?? ? ? ? 1116561111 ?????? ??? iiiiii AATdAA ?? ??
id?
? ? ? ? ? ? ? ?? ? ? ? ? ? 11111656111211121 ??????????? ??? iiiiiiii AATTdAAA ??? ??
?
?
?
?
?
?
?
???????
???????
???????
6
6
6
5
6
5
6
5
6
5
6
5
6
5
6
5
2
2
2
1
2
1
2
1
2
1
2
1
2
1
2
1
1
1
1
0
1
0
1
0
1
0
1
0
1
0
1
0
AAAAAdA
AAAAAdA
AAAAAdA
?
? ?? ? ? ?6565212110106060 AdAAdAAdATdT ????? ?
忽略高次项：
66651065322221106521111060 ?????? AAAAAAAAATd ???
绕自身
656521106521211065100060 AAAAAAAATd ??????? ????
绕 Σ i-1
多关节补偿：
并联机器人运动学
? 燕山大学 黄真
,并联机器人机构学理论及其控制,
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?????
?
?
??????
?
?
?
?
?
?
?
?
?
?
?
?
?
???
?
11000
1
.,,0
.0,,,
.0),,(
)()()(
,
1000
T
00
2222
62,1
0
0
zib
yib
xib
pzazsznz
pyaysyny
pxaxsxnx
bi z
bi y
bi x
zibyibxib
bibi zbi ybi x
B i zB i yB i x
B i zbi zB i ybi yB i xbi xl
lll
pzazsznz
pyaysyny
pxaxsxnx
i
）是已知的中，（在
中的位置在也就是）现在求（
中是已知的在
易有：
求：
中的位置和姿态在即：已知
?
?
即为所求解
222
iziyixi llll ???
2005年 3月 24日
运动学正问题
? 杆件参数的意义
? 坐标系的建立原则
? 杆件坐标系间的变换过程 -相邻关节坐标
系的齐次变换
? 机器人的运动学方程
杆件参数的意义 - 和
? li 关节 Ai轴和 Ai+1轴
线公法线的长度
? 关节 i轴线与 i+1
轴线在垂直于 li平面
内的夹角
串联关节, 每个杆件最多与 2个杆件相连, 如 Ai与 Ai-1和
Ai+1相连 。 由运动学的观点来看, 杆件的作用仅在于它能保
持其两端关节间的形态不变 。 这种形态由两个参数决定, 一
是杆件的长度 li（ ）, 一个是杆件的扭转角
i?ia
Ai
Ai+
1
i?
il
i?
il i
?
杆件参数的意义 - 和
? 是从第 i-1坐标系
的原点到 Zi－１ 轴和
Ｘ i轴的交点沿 Ｚ i-1
轴测量的距离
? 绕 Zi-1轴由 Ｘ i-1
轴转向 Ｘ i轴的关节
角
确定杆件相对位置关系, 由另外 2个参数决定, 一个是杆
件的距离：, 一个是杆件的回转角：
i?i
d
i?
id i?
Ai
Ai+
1
i?
il
id
1?il i?
Ai-1
id
坐标系的建立原则
Ai
Ai+
1
i?
il
id
1?il
i?
Ai-1
1?iz
1?ix
1?iy
1?io
iz
ix
iy
io
? 为右手坐标系
? 原点 Oi：设在 Li与
Ai+1轴线的交点上
? Zi轴：与 Ai+1关节轴
重合，指向任意
? Xi轴：与公法线 Li
重合，指向沿 Li由
Ai轴线指向 Ai+1轴线
? Yi轴：按右手定则
Li— 沿 xi 轴, zi-1 轴与 xi 轴交点到 0i 的距离
αi— 绕 xi 轴, 由 zi-1转向 zi
di— 沿 zi-1 轴, zi-1轴和 xi 交点至 ∑0i–1坐标系原点的距离
θi— 绕 zi-1 轴, 由 xi-1转向 xi
?
杆件坐标系间的变换过程
-相邻关节坐标系的齐次变换
? 将 xi-1轴绕 zi-1轴转 ?i 角度，将其与 xi轴平行；
? 沿 zi-1轴平移距离 di,使 zi-1轴与 zi轴重合；
? 沿 xi轴平移距离 Li，使两坐标系原点及 x轴重
合；
? 绕 xi 轴转 ?i角度，两坐标系完全重合．
机器人的运动学方程
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1
s i n
c o s
0
c o s
s i nc o s
s i ns i n
0
s i n
c o sc o s
c o ss i n
0
0
s i n
c o s
1
i
ii
ii
i
ii
ii
i
ii
ii
i
i
i
i
d
a
a
A
?
?
?
??
??
?
??
??
?
?
0 0 1 1
12
i
iiT A A A
?? ? ? ? ?
D-H变换矩阵
运动学逆问题
? 多解性，剔除多余解原则
?根据关节运动空间合适的解
?选择一个与前一采样时间最接近的解
?根据避障要求得选择合适的解
?逐级剔除多余解
? 可解性
?所有具有转动和移动关节的系统，在一个单一串联中
总共有 6个（或小于 6个）自由度时，是可解的，一般
是数值解，它不是解析表达式，而是利用数值迭代原
理求解，它的计算量要比解析解大
?如若干个关节轴线相交和或多个关节轴线等于 0或 90°
的情况下，具有 6个自由度的机器人可得到解析解
例题：
? 试求立方体中心在机座坐标系 ∑ 0中的位置
? 该手爪从上方把物体抓起，同时手爪的开合方向与物体的 Y轴同向，
那么，求手爪相对于 ∑ 0的姿态是什么？
在机器人工作台上加装一电视摄像机，摄像机可见到固联
着 6DOF关节机器人的机座坐标系原点，它也可以见到被操作
物体（立方体）的中心，如果在物体中心建一局部坐标系，则
摄像机所见到的这个物体可由齐次变换矩阵 T1来表示，如果摄
像机所见到的机座坐标系为矩阵 T2表示。
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
101-00
2001-0
10-001
T
1000
91-00
10001
1010
T 21
xy z
解 1：
x
y z
z
机 y
机
z
物
y
物
x
物
o
O
机
O
物
T T 21 物机机摄物摄 求，，已知 TTT ??
TT 11-2 ）（有,物摄摄机物机 ?? TTT
1000
91-00
10001
1010
1000
101-00
2001-0
10001
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
1100
10001-
11010
∑O 物 根据 T 1 画出
∑O 机 根据 T 2 画出
因此物体位于机座坐标系的（ 11,10,1） T
处，它的 X,Y,Z轴分别与机座坐标系的
-Y,X,Z轴平行。
解 2：
x
y z
z 机 y
机
z 物
y 物
x 物
o
O 机
O 物
手爪
机实际要求 T
pzazsznz
pyaysyny
pxaxsxnx
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
向重合手爪开合方向与物体 ya, Ts ]001[???有
方向相反方向物体的从上向下抓，指出手爪 zab, Ta ]100[ ???则有
Tkji
kji
asnc ]010[00
100
001,??????
?
?
?
?
?
?
?
?
?
?
?
???? ?
??
?
?
?
?
?
?
?
?
?
?
?
?
1-00
001
010
因此：姿态矩阵为
重合时
与物体中心
当手爪中心
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
11-00
10001
11010
T
物
机
x
y z
z 机 y
机
z 物
y 物
x 物
o
O 机
O 物
手爪
机实际要求 T
pzazsznz
pyaysyny
pxaxsxnx
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
向重合手爪开合方向与物体 ya, Ts ]001[ ???有
方向相反方向物体的从上向下抓，指出手爪 zab,
Ta ]100[ ???则有
O
s
?
n
?
a
?
y
z
x
特殊情况坐标系的建立原则
Oi— Ai与 Ai+1关节轴线的交点
Zi— Ai+1轴线
Xi— Zi和 Zi-1构成的面的法线
Yi— 右手定则
Ai
A
i + 1
oi
zi-1
zi
xi
yi
两个关节轴相交
两个关节轴线平行
? 先建立
∑ 0i-1
? 然后建立
∑ 0i+1
? 最后建立
∑ 0i
A
i - 1 A
i
A
i + 1
A
i + 2
l
i - 1
o
i - 1
x
i - 1
y
i - 1
z
i - 1
A
B
C
D
o
i
（ x
i
）
（ y
i
）
z
i
x
i
y
i
o
i + 1
x
i + 1
y
i + 1
z
i + 1
d
i + 1
l
i + 1
d
i
举例,Stanford机器人
A1
A2A3
A4
A5
A6
d1
z1
x1y
1
O1
d2
z2
x2
y2
O2
z3
y3
x3
O3
y4
z4
x4
O4
z5 y5
x5
O5
345
45
,,0o o odd? ? ?重 合
d3
z6
x6
y6
O6
d6
z0
y0 x0
O0
? 为右手坐标系
? 原点 Oi,Ai与 Ai+1
关节轴线的交点
? Zi轴：与 Ai+1关节轴
重合，指向任意
? Xi轴,Zi和 Zi-1构
成的面的法线
? Yi轴：按右手定则
Li— 沿 xi 轴, zi-1 轴与 xi 轴交点到 0i 的距离
αi— 绕 xi 轴, 由 zi-1转向 zi
di— 沿 zi-1 轴, zi-1轴和 xi 交点至 ∑0i–1坐标系原
点的距离
θi— 绕 zi-1 轴, 由 xi-1转向 xi
解：
? 用未知的逆变换逐次左乘，由乘得的矩阵
方程的元素决定未知数，即用逆变换把一
个未知数由矩阵方程的右边移到左边
? 求解这个未知数
? 把下一个未知数移到左边
? 重复上述过程，直到解出所有解
运动学逆问题解法
Paul 等人提出的方法,
Paul 等人提出的方法
65544332211060 TTTTTTT ?
TTTTTT T 6554433221601-10 ?）（ 1 q?
65544332601-101-21 TTTTTTT ?）（）（ 2q?
65601-101-21132143154 TTTT)T()T()T( ???? ）（）（5 q?
? ? ?
E?601-101-65 TT) T( ）（? 6 q?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
6
0
pzazsznz
pyaysyny
pxaxsxnx
T
机器人末端操作器位姿的其它描述方法
? 用矩阵表示刚性体的转动简化了许多运算，
但它需要 9个元素来完全描述旋转刚体的姿
态，因此矩阵并不直接得出一组完备的广义
坐标。
? 一组广义坐标应能描述转动刚体相对于参考
坐标的方向，被称为欧拉角的三个角度,φ,
θ, ψ 就是这种广义坐标。
? 有几种不同的欧拉角表示方法，它们均可描
述刚体相对于固定参考系的姿态。三种最常
见的欧拉角类型列在表中
3种最常见的欧拉角类型
步 1 步 2 步 3
类型 1 绕 OZ轴转 φ 角 绕当前 OU' 轴转 θ角 绕当前 OW″ 轴转 ψ 角
类型 2 绕 OZ轴转 φ 角 绕当前 OV '轴转 θ角 绕当前 OW″ 轴转 ψ 角
类型 3 绕 OX轴转 φ 角 绕 OY轴转 θ角 绕 OZ轴转 ψ 角
φ
φ
φ
u′
v′
w′
①
x(u)
y (v)
z (w)
o
θ
u"
v"
θ
w"
②
u???
③ ψ
ψ
ψ
v???
W???
),( ?ZR ),( ???R ),( ?wR ???N0T
?
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
100
0
0
0
0
001
100
0
0
??
??
??
????
??
cs
sc
cs
sccs
sc
?
?
?
?
?
?
?
?
?
?
????
???
?
?????
????????????
????????????
ccsss
sccccssscccs
ssccsscscscc
类型 1：表示法通常用于陀螺运动
类型 2,所得的转动矩阵为右乘
?
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
????
100
0
0
c0s-
010
s0c
100
0
0
),(),v(),(R
??
??
??
??
??
??
???
cs
sc
cs
sc
wRRZR
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
???
?
1000
pz
pyR
px
T
ccsss
ssccscsscccs
sccssccssccc
?????
????????????
????????????
类型 3,一般称此转动的欧拉角
为横滚、俯仰和偏航角，这种形
式主要用于航空工程中分析飞行
器的运动，其旋转矩阵为（这种
方法也叫做横滚、俯仰和偏航角
表示方法）
?
?
?
?
?
?
?
?
?
?
?
??
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
? ?
?
?
?????
????????????
????????????
??
??
??
??
??
??
???
ccscs
sccssccssscs
sscsccsssccc
cs
sc
cs
sc
cs
sc
xRyRz
0
0
001
0
010
0
100
0
0
),(),(),RR （
Z
Y
X
?
?
?
偏航
俯仰
横滚
斯坦福机器人运动学逆问题解
6533211060 AAAAT ??????
61T?
653321 AAA ?????
式中：
yx
yx
pCpSpf
zpf
pSpCpf
1113
12
1111
)(
)(
)(
???
??
??
211 dpcps yx ???由两端矩阵对应元素相等可得：
作三角变换：
式中：
得到：
即有：
（ ）
由 1,4和 2,4元素对应相等，得：
6261121 TTA ???
式中第四列：
6362132 TTA ???
式中第三列：
微动矩阵和微动齐次变换
? 对象, 微动矩阵主要是描述机器人在微动
范围内各关节的位移运动关系
? 定义, 各关节当角度移小于 5° 时，平移
在 0.1mm左右时，微动矩阵大致可用
设,有一机器人如图，末端执行器在机座坐标系中的
齐次变换为 oTN,做微动,① 绕任意轴 w轴转 ； ② 绕
各坐标轴平移 dx,dy,dz
求,在 中的位置和姿态,
? 定义 为微动齐次变换矩阵
N0? 00?
O 0
n
a
s
X 0
Y 0
Z 0
O n
NdT
Nr a n s TdwRdzdydxTT 0N0 ),(),,( ??变化后）（
Nr a n s TdwRdzdydxTdTT 0NN0 ),(),,( ???记为
TdwRdzdydxTdTT r an s ),(),,( ???
TEdwRdzdydxTdT r an s ]),(),,([ ?? ?
)T(
,),(),,(
的左边在注意，
称为微动率，令
?????
???
TdT
EdwRdzdydxT r a n s ?
在忽略高次项
的情况下：微
动齐次变换与
次序无关
?d
微动平移和微动旋转的齐次变换：
平移：
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
100
010
001
dz
dy
dx
T r
旋转 R,绕任意轴 旋转 角,? ??dw,w? ?d
?
?
?
?
?
??
??
??
?
?
?
?
?
????
????
????
?
??
??
??
????
????
????
?
dd
dd
dd
dddd
dddd
dddd
dw
c os)c os(1r
s i nr)c os(1rr
s i nr)c os(1rr
s i nr)c os(1rrs i nr)c os(1rr
c os)c os(1rs i nr)c os(1rr
s i nr)c os(1rrc os)c os(1r
R
2
z
xzy
yzx
xzyyzx
2
yzyx
zyx
2
x
,
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
1000
01
01
01
s i n,1c o s
??
??
??
???
drdr
drdr
drdr
ddd
xy
xz
yz
在微动范围内绕经意轴转动 角,可以看作绕 x,y,z轴的微转
动的合成 。 因此：
?d
?????? drzddryddrxd zyx ???,,
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
01
01
01
,
xdyd
xdzd
ydzd
dwR
??
??
??
?因此：
因此微动率△ = ? ? ? ?? ? EdwRdzdydxT
r a n s ??,,,
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
0000
0
0
0
dzxdyd
dyxdzd
dxydzd
??
??
??
微动的齐次变换,dT= △ ?T
己知变换矩阵
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0001
3100
7010
T,001.0 kjid ????
kjidp 6.003.0 ???
转动：
平移：求 d T
解：
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
0000
6.001.00
01.000
3.0000
1000
0100
0010
0001
1000
011.00
01.010
0001
1000
6.0100
0010
3.0001
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
????
0000
9.01.000
0001.0
3.0000
1000
0001
3100
7010
0000
6.001.00
01.000
3.0000
TdT
?
?
?
?
?
?
?
?
?
?
?
?
?
??
1000
9.01.001
3101.0
3.7010
dTT
反过来：如果我们要求 Σ 在 Σ 中的齐次交换矩阵为
no oo
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0001
3100
7010
T 实际测得的为
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
9.01.001
3101.0
3.7010
那么末端执行器坐标系要如何运动才能到达期望值？
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
011.00
01.010
0001
转动：
?
?
?
?
?
?
?
?
?
?
?
?
1000
6.0100
0010
3.0001
平移：
等效微动位移的求解
? 前面研究的是动坐标系 ΣOn在 ΣOo中的 b
变换为 T，相对于基准坐标系作微平移和
微转动，来求微动齐次交换。
? 现在我们研究动坐标系 Σ On相对于自身
坐标系做了微位移或微转动，达到绕基
准坐标同样的效果则如何求解。
dT=△ ?T （ 绕基准坐标系 ）
=T?△ T （ 绕动坐标系 ）
左乘,绕基准
右乘,绕动坐标轴
强调等效
TTT ??????
TTT ?????? ? 1
1??????? TT
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
zzzz
yyyy
xxxx
pasn
pasn
pasn
T
? ?
? ?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
??
??
?
1000
1
Paaaa
Pssss
pnnnn
T
T
zyx
T
zyx
T
zyx
??
??
??
设,有：
? ? ? ? ? ?
? ? ? ? ? ?
? ? ? ? ? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
????
????
????
??
0000
0
0
0
pdpdaasdand
pdpdssadsnd
pdpdnnadnsd
T ?????????
??????????
??????????
???
???
???
? ?
? ?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?????
?????
?????
?
0000
0
0
0
pdpdandsd
pdpdsndad
pdpdnsdad
????????
????????
????????
???
???
???
绕自身轴的微动率△ Τ 和绕固定坐标系坐标轴的微动
率△之间的什么关系, 举例说明：
例：一动坐标系相对于固定坐标系的齐
次交换为
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0001
3100
7010
T
n s a p
己知相对固定坐标系的微
动平移和转动
??
?
???
???
kjid
kjidp
001.0
6.003.0
?
求,① △ 与 △ Τ
② 求 dT
③ 求与之等效的绕动坐标系的微平移和微转动
解,① △ =
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
0000
0
0
0
dzxdyd
dyxdzd
dxydyd
??
??
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
000
6.001.00
01.000
3.0000
? ? ? ?
? ? ? ?
? ? ? ? 0100001.0
1.0001001.0
0010001.0
???????
???????
???????
kjikjind
kjikjisd
kjikjiad
??
??
??
?
?
?
? ? ? ? ? ?kjikjikjipdpd 6.003.0037001.0 ??????????? ????
? ?kji
kji
6.003.0
037
001.0 ????
? ? ? ?
kji
kjikji
9.003.0
6.003.03.000
???
??????
? ? ? ?? ?
? ? ? ?? ?
? ? ? ?? ? 09.003.0010
3.09.003.0001
9.09.003.0100
????????
????????
????????
kjikjipdpda
kjikjipdpds
kjikjipdpdn
????
????
????
?
?
?
? ?
? ?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
????
???
???
???
0000
0
0
0
pdpdandsd
pdpdsndad
pdpdnsdad
T ??
??????
????????
????????
???
???
???
?
?
?
?
?
?
?
?
?
?
?
?
?
?
0000
0001.0
3.0000
9.01.000
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
????
0000
9.01.000
0001.0
3.0000
1000
0001
3100
7010
0000
6.001.00
01.000
3.0000
TdT
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
????
0000
9.01.000
0001.0
3.0000
0000
0001.0
3.0000
9.01.000
1000
0001
3100
7010
TTdT
解 ②,
解 ③, 绕自身平移和转动
?
?
?
???
???
TTTT
TTTT
kjid
kjidp
01.00
03.09.0
?
其结果等于绕固定坐标系转
动和旋转
??
?
???
???
kjid
kjidp
001.0
6.003.0
?
等效
说明,如果我们发现末端操作器相对于基准坐标系有了
微位移 （ 平移或转动 ）,我们可以认为末端操作器相对
于自己的坐标系发生了微位移 。 只是微动率 △ 和 △ Τ不
同而己 。 其结果是等效的 。
这些在进行误差补偿和微动时有用,如产生误差
如何补偿？ 可以反向运动末端关节来补偿 ?
?? ?d
dp
微动齐次变换的意义
误差及误差补偿
? 制造和检测误差
? 运算过程中圆整、插补、拟合造成的误差 — 原理性误差
? 构件承受的负载、加速度、重力的变形误差
? 传动误差
? 环境影响误差
误差来源：
? 单关节补偿
? 多关节补偿
误差补偿：
单关节补偿：
? ? ? ? ? ?? ? ? ? 1116561111 ?????? ??? iiiiii AATdAA ?? ??
id?
? ? ? ? ? ? ? ?? ? ? ? ? ? 11111656111211121 ??????????? ??? iiiiiiii AATTdAAA ??? ??
?
?
?
?
?
?
?
???????
???????
???????
6
6
6
5
6
5
6
5
6
5
6
5
6
5
6
5
2
2
2
1
2
1
2
1
2
1
2
1
2
1
2
1
1
1
1
0
1
0
1
0
1
0
1
0
1
0
1
0
AAAAAdA
AAAAAdA
AAAAAdA
?
? ?? ? ? ?6565212110106060 AdAAdAAdATdT ????? ?
忽略高次项：
66651065322221106521111060 ?????? AAAAAAAAATd ???
绕自身
656521106521211065100060 AAAAAAAATd ??????? ????
绕 Σ i-1
多关节补偿：
并联机器人运动学
? 燕山大学 黄真
,并联机器人机构学理论及其控制,
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?????
?
?
??????
?
?
?
?
?
?
?
?
?
?
?
?
?
???
?
11000
1
.,,0
.0,,,
.0),,(
)()()(
,
1000
T
00
2222
62,1
0
0
zib
yib
xib
pzazsznz
pyaysyny
pxaxsxnx
bi z
bi y
bi x
zibyibxib
bibi zbi ybi x
B i zB i yB i x
B i zbi zB i ybi yB i xbi xl
lll
pzazsznz
pyaysyny
pxaxsxnx
i
）是已知的中，（在
中的位置在也就是）现在求（
中是已知的在
易有：
求：
中的位置和姿态在即：已知
?
?
即为所求解
222
iziyixi llll ???
