机器人运动学
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
物
机
机器人末端操作器位姿的其它描述方法
? 用矩阵表示刚性体的转动简化了许多运算，
但它需要 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 （
解：
斯坦福机器人运动学逆问题解
6533211060 AAAAT ??????
61T?
653321 AAA ?????
式中：
yx
yx
pCpSpf
zpf
pSpCpf
1113
12
1111
)(
)(
)(
???
??
??
211 dpcps yx ???由两端矩阵对应元素相等可得：
作三角变换：
式中：
得到：
即有：
（ ）
由 1,4和 2,4元素对应相等，得：
6261121 TTA ???
式中第四列：
6362132 TTA ???
式中第三列：
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
物
机
机器人末端操作器位姿的其它描述方法
? 用矩阵表示刚性体的转动简化了许多运算，
但它需要 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 （
解：
斯坦福机器人运动学逆问题解
6533211060 AAAAT ??????
61T?
653321 AAA ?????
式中：
yx
yx
pCpSpf
zpf
pSpCpf
1113
12
1111
)(
)(
)(
???
??
??
211 dpcps yx ???由两端矩阵对应元素相等可得：
作三角变换：
式中：
得到：
即有：
（ ）
由 1,4和 2,4元素对应相等，得：
6261121 TTA ???
式中第四列：
6362132 TTA ???
式中第三列：
