机器人运动学第二章 数学基础 — 齐次坐标和齐次变换杜志江、纪军红
科学园 C1栋机器人研究所
206室
86414462-12
duzj01@hit.edu.cn
junhong.ji@hit.edu.cn
参考教材
付京逊,机器人学,
蔡自兴,机器人学,
2.1 引言机器人位置和姿态的描述
机器人可以用一个开环关节链来建模
由数个驱动器驱动的转动或移动关节串联而成
一端固定在基座上，另一端是自由的，安装工具，用以操纵物体
i?
n
o a
人们感兴趣的是操作机末端执行器相对于固定参考坐标数的空间几何描述，也就是机器人的运动学问题
机器人的运动学即是研究机器人手臂末端执行器位置和姿态与关节变量空间之间的关系动画示例运动学研究的问题
Where is my hand?
Direct Kinematics
HERE!
How do I put my
hand here?
Inverse Kinematics,
Choose these angles!
运动学正问题运动学逆问题
丹纳维特（ Denavit）和哈顿贝格（ Hartenberg）
于 1955年提出了一种矩阵代数方法解决机器人的运动学问题 — D-H方法
具有直观的几何意义
能表达动力学、计算机视觉和比例变换问题
其数学基础即是齐次变换
2.2 点和面的齐次坐标
2.2.1 点的齐次坐标
一般来说,n维空间的齐次坐标表示是一个（ n+1）维空间实体。有一个特定的投影附加于 n维空间，也可以把它看作一个附加于每个矢量的特定坐标 — 比例系数。
kcjbiav
zy x Tw
w
z
y
x
V?
式中 i,j,k为 x,y,z 轴上的单位矢量，
a=,b=,c=,w为比例系数
w
x
w
y
w
z
显然，齐次坐标表达并不是唯一的，随
w值的不同而不同。在计算机图学中,w
作为通用比例因子，它可取任意正值，但在机器人的运动分析中，总是取 w=1 。
列矩阵
[例 ]：
kjiV 543
可以表示为：
V=[3 4 5 1]T
或 V=[6 8 10 2]T
或 V=[-12 -16 -20 -4]T
齐次坐标与三维直角坐标的区别
V点在 ΣOXYZ坐标系中表示是唯一的（ x,y,z）
而在齐次坐标中表示可以是多值的。不同的表示方法代表的 V点在空间位置上不变。
x
y
z
z
z
x
V
图 2 - 2
o
几个特定意义的齐次坐标：
[0,0,0,n]T— 坐标原点矢量的齐次坐标,n
为任意非零比例系数
[1 0 0 0]T— 指向无穷远处的 OX轴
[0 1 0 0]T— 指向无穷远处的 OY轴
[0 0 1 0]T— 指向无穷远处的 OZ轴
2个常用的公式：
zzyyxx babababa
kbabajbabaibaba
bbb
aaa
kji
ba xyyxzxxzyzzy
zyx
zyx
)()()(
2.2.2 平面的齐次坐标
平面齐次坐标由行矩阵 P=[a b c d ]来表示
当点 v=[x y z w]T处于平面 P内时，矩阵乘积 PV=O，或记为
0
dwczbyax
w
z
y
x
dcbaPV
如果定义一个常数 m=,则有：222 cba
m
d
m
c
w
z
m
b
w
y
m
a
w
x )()( k
m
cj
m
bi
m
ak
w
zj
w
yi
w
x=
可以把矢量 解释为某个平面的外法线，此平面沿着法线方向与坐标原点的距离为。
)( kmcjmbima
m
d?
[例 ]：
因此一个平行于 x,y轴,且在 z轴上的坐标为单位距离的平面 P可以表示为,或有,PV=
1100P2200P
v0
v0
v0
点在平面下方点在平面上点在平面上方例如：点 V=[10 20 1 1]T必定处于此平面内,而点 V=[0 0 2 1]T
处于平 P 的上方点 V=[0 0 0 1]T处于 P平面下方 。 因为：
0
1
1
20
10
101000?
0 1
1
2
0
0
1100
0 -1
1
0
0
0
1-100
与点矢 相仿，平面 也没有意义T00000000
2.2 旋转矩阵及旋转齐次变换
2.2.1 旋转矩阵设固定参考坐标系直角坐标为 ΣOxyz,动坐标系为 ΣO′uvw，
研究旋转变换情况 。
x
y
z
w
v
u
P
o
( O ' )
图 2 - 3
① 初始位置时，动静坐标系重合,O,O′重合，如图。各轴对应重合，设 P点是动坐标系 ΣO′uvw中的一点，且固定不变。
则 P点在 ΣO′uvw中可表示为：
wwuvuuu v w kPjPiPP
、,为坐标系 ΣO′uvw的单位矢量,则 P点在 Σoxyz中可表示为：
ui vj wk
zzyyxxxyz iPiPiPPuvwP xyz
② 当动坐标系 ΣO′uvw绕 O点回转时，求 P点在固定坐标系
Σoxyz中的位置
y
z
x
o
( O ' )
u
v
w
P
Pw
Pv
Pu
图 2 - 4
已知：
P点在 ΣO′uvw中是不变的仍然成立，由于 ΣO′uvw回转，则：
wwuvuuu v w kPjPiPP
)(P wwvvuuxxu v wx kPjPiPiiP
)(P wwvvuuyyu v wy kPjPiPjjP
)(P wwvvuuzzu v wz kPjPiPjjP
用矩阵表示为,
w
v
wzvzz
wvyy
wxvxx
z
y
x
P
P
P
kkjkik
kjjjij
kijiii
P
P
P?
y
（ 2-7）
uv wx y z
wzvzz
wvyy
wxvxx
PRp
kkjkik
kjjjij
kijiii
,R y 则旋转矩阵为：定义
反过来：
xyzu v w PRP 1 RRR d et
*1
T1 RR
Rde t
因此是正交矩阵，的行列式，由于为的伴随矩阵，为 RRRR
2.2.2 旋转齐次变换用齐次坐标变换来表示式（ 2-7）
11000
0
0
0
1
w
v
u
z
y
x
P
P
P
R
P
P
P
11000
0
0
0
1
1
z
y
x
w
v
u
P
P
P
R
P
P
P
2.2.3 三个基本旋转矩阵和合成旋转矩阵三个基本旋转矩阵
),(?xR
即动坐标系 求 的旋转矩阵,也就是求出坐标系 中各轴单位矢量 在固定坐标系中各轴的投影分量,很容易得到在重合时,有：
角，轴转动绕, XOvwO?
vwO?'? wv kji,,? Oxyz?
),(?xR
100
010
001
R
由图 2-5可知,在 y轴上的投影为,在 z轴上的投影为,在 y轴上的投影为,在 z轴上的投影为
，所以有：
vj?cosyj
sinzk?sinyj?wk
coszk
vj
wk
wzvzz
wvyy
wxvxx
kkjkik
kjjjij
kijiii
y)R ( x,
x
y
z
o
u
v
w
U '
V '
W '
O '
图 2 - 5
co m
ii ux
s i n0
s i nc o s0
001
方向余弦阵同理：
c o s0s i n
010
s i n0c o s
)y,R （
100
0c o ss i n
0s i n-c o s
)z,R
（
co ms i n0
s i nc o s0
001
)R ( x,
三个基本旋转矩阵,
x
y
z
o
u
v
w
U '
W '
O '
x
y
z
o
u
v
w
U '
W '
O '
v '
合成旋转矩阵,
例 1：在动坐标中有一固定点,相对固定参考坐标系 做如下运动：① R（ x,90° ）；② R(z,
90° )；③ R(y,90° )。求点 在固定参考坐标系 下的位置。
Tu v wPo 1321'?
Oxyz?
uvwPo' Oxyz?
解 1：用画图的简单方法解 2：用分步计算的方法
① R（ x,90° ）
② R（ z,90° ）
③ R（ y,90° ）
1
2
3
1
1
3
2
1
1000
0010
01-00
0001
'P
1
2
1
3
1
2
3
1
1000
0100
0001
001-0
''P
1
3
1
2
1
2
1
3
1000
0001-
0010
0100
'''P
（ 2-14）
（ 2-15）
（ 2-16）
上述计算方法非常繁琐，可以通过一系列计算得到上述结果。将式（ 2-14）（ 2-15）（ 2-16）联写为如下形式：
11
44
w
v
u
z
y
x
P
P
P
R
P
P
P
R4x4为二者之间的关系矩阵，我们令：
),(),(),RR 44 xRzRy（
定义 1：
当动坐标系 绕固定坐标系 各坐标轴顺序有限次转动时，其合成旋转矩阵为各基本旋转矩阵依旋转顺序 左乘 。
注意,旋转矩阵间不可以交换
uvwO'? Oxyz?
平移齐次变换矩阵
1000
100
010
001
c) b (a T r an sH
c
b
a
注意,平移矩阵间可以交换，
平移和旋转矩阵间不可以交换
z
y
x
o
o′
w′
u ′
v′
a
b
c
2.2.4 相对变换举例说明：
例 1,动坐标系 ∑0′起始位置与固定参考坐标系 ∑0重合,动坐标系
∑0′做如下运动：① R(Z,90o) ② R（ y,90o） ③ Trans(4,-3,7)
，求合成矩阵解 1：用画图的方法：
o′
z
y
x
7
4
- 3
o
w ```
u ```
v ```
v ″
u ″
w ″
z
y
x
o
o ( o′ )
x
y
z
u
v
w
z
y
x
u ′
w′
o ( o′ )
v′
解 2：用计算的方法根据定义 1,我们有：
1000
7010
3001
4100
)R( Z,9 0 )90 R( y,7),3,T r an s ( 4T
以上均以固定坐标系多轴为变换基准,因此矩阵左乘 。
如果我们做如下变换，也可以得到相同的结果：
例 2,① 先平移 Trans (4,-3,7)； ② 绕当前 轴转动 90o；
③ 绕当前 轴转动 90o；求合成旋转矩阵 。
v?
w?
（ 2-20）
解 1：用画图的方法
z
y
x
o ( o′ )
v
w
u
z
y
x
o
o′
w′
u ′
v′
o
z
y
x
o′
w ″
v ″
u ″
x
y
z
o
o′
w ```
u ```
v ```
解 2：用计算的方法
1000
7010
3001
4100
)R ( Z,9 0 )90 R ( y,7),3,T r a n s ( 4T oo
（ 2-21）
式 （ 2-20） 和式 （ 2-21） 无论在形式上,还是在结果上都是一致的 。 因此我们有如下的结论：
动坐标系在固定坐标系中的齐次变换有 2种情况：
定义 1,如果所有的变换都是相对于固定坐标系中各坐标轴旋转或平移,则依次左乘,称为绝对变换 。
定义 2,如果动坐标系相对于自身坐标系的当前坐标轴旋转或平移，则齐次变换为依次右乘，称为相对变换。
结果均为为动坐标系在固定坐标中的位姿 （ 位置 +姿态 ） 。
相对于固定坐标系,轴。轴相当于轴，轴相对于轴，轴相当于 ZYX wv?
也就是说，动坐标系绕自身坐标轴做齐次变换,要达到绕固定坐标系相等的结果，就应该用相反的顺序。
齐次变换矩阵 T 的意义：
机器人用到相对变换的时候比较多
例如机械手抓一个杯子，
如右图所示，手爪需要转动一个角度才抓的牢，
相对于固定坐标系表达太麻烦，可以直接根据手爪的坐标系表示
但也要知道在 ∑O中的位姿，就用右乘的概念。
x
y
z
o
H
2.2.5 绕通过原点的任意轴旋转的齐次变换
有时动坐标系 ∑ O′可能绕过原点 O的而分量分别为 rx、
ry,rz的任意单位矢量 r 转动 φ角。
研究这种转动的好处是可用 ∑ O′绕某轴 r 的一次转动代替绕 ∑ O各坐标轴的数次转动
为推导此旋转矩阵，可作下述变换：
a,绕 X 轴转 α角，
使 r 轴处于 XZ平面内
a,绕 Y 轴转 -β角，使 r
轴与 OZ轴重合
a,绕 OZ轴转动 φ角
b,绕 Y 轴转 β角
c,绕 X 轴转 -α角
X
Y
Z
r x
r y
r z
A B
C
D
B '
O
α
β
5
1
2
43
r
A '
由上图容易求出：
2
z
2
y
y
rr
rs in
2
z
2
y
z
rr
rc o s
x
x r
r
r
r
OCs in
2
z
2
y
2
z
2
y rr
r
rr
OB
C'Bco s
由定义 1和定义 2，上述 5次旋转的合成旋转矩阵为：
c o ss i n0
s i n-c o s0
001
c o s0s i n
010
s i n-0c o s
100
0c o ss i n
0s i n-c o s
c o s0s i n-
010
s i n0c o s
c o ss i n0
s i nc o s0
001
RRRRRR
,x,y,z,y,x,r
（ 2-25）
带入式 （ 2-25），得
c o s)c o s(1r
s i nr)c o s(1rr
s i nr)c o s(1rr
s i nr)c o s(1rrs i nr)c o s(1rr
c o s)c o s(1rs i nr)c o s(1rr
s i nr)c o s(1rrc o s)c o s(1r
R
2
z
xzy
yzx
xzyyzx
2
yzyx
zyx
2
x
,r
2.2.6 齐次交换矩阵的几何意义设 T=,有一个手爪，已知其在 ∑ O的位置，设一个该坐标系 ∑ O′，已知，,那么 ∑ O′在 ∑ O中的齐次坐标变换为,如果手爪转了一个角度，
则：
1000
3
2
1
twzzz
twyyy
twxxx
111' cbao
1000
100
010
001
T
1
1
1
1 c
b
a
1000
p
p
p
T
z
yyy
xxx
zzz
y
x
w
w
w
T反映了 ∑ O′在 ∑ O中的位置和姿态,即表示了该坐标系原点和各坐标轴单位矢量在固定坐标系中的位置和姿态 。
该矩阵可以由 4个子矩阵组成,写成如下形式：
比例系数透视矩阵位置矢量旋转矩阵
1131
1333
wf
PR
T
zzz
yyy
xxx
w
w
w
33R
为姿态矩阵，表示动坐标系 ∑ O′在固定参考坐标系 ∑ O中的姿态，即表示 ∑ O′各坐标轴单位矢量在 ∑ O各轴上的投影为位置矢量矩阵，代表动坐标系 ∑ O′坐标原点在固定参考坐标系 ∑ O中的位置Tzyx ppp ][P 13
为透视变换矩阵，在视觉中进行图像计算，
一般置为 0 ]000[f 31
为比例系数]1[
11w
如果需要求解 ∑ O在 ∑ O′中的位置和姿态，此时的齐次变换矩阵为,即求逆矩阵：1?T
1000
-
-R
-
T
T
T1-
33
T
1
pw
pv
p
）（
）（
）（?
kpjpipp zyx
kji zyx
kvjvivv zyx
kwjwiww zyx
其中：
这些式子以后经常遇到，
在机器人计算中，所要求的就是齐次变换矩阵下
2.2.7 透镜成像的齐次变换
p
p
,
P [ x 1 ]
P P [ x 1 ]
1
()
1
pp
pp
T
T
yz
yz
y p zp zp f
y p zp zp y p f
y p f
zp x p y p f
zp x p y p y p f
y p f y p f y p
y p y p f y p y p f f
xp
xp
y
以光心为原点O，光轴与y轴重合，P为物点，
用齐次坐标表示求 的齐次坐标，即求根据三角形相似原理：
注意 是负值,是正值，所以实际上为相减关系又有设
11
1 0 0 0
0 1 0 0
0 0 1 0
11 1 0 0 0 1
y p zp
y p zp
p y p zp
f f f
xp
x p x p
yp
y p y p
Tfzp
zp zp
yp
f
用矩阵表示：
z
y
P
y p
f
o
z p
f
p
z?
'
P
p
y?
p
p
,
P [ x 1 ]
P P [ x 1 ]
1
()
1
pp
pp
T
T
yz
yz
y p zp zp f
y p zp zp y p f
y p f
zp x p y p f
zp x p y p y p f
y p f y p f y p
y p y p f y p y p f f
xp
xp
y
以 光 心 为 原 点 O,光 轴 与 y 轴 重 合,P 为 物 点,
用 齐 次 坐 标 表 示求 的 齐 次 坐 标,即 求根 据 三 角 形 相 似 原 理,
注 意 是 负 值,是 正 值,所 以 实 际 上 为 相 减 关 系又 有设
11
1 0 0 0
0 1 0 0
0 0 1 0
11 1 0 0 0 1
y p zp
y p zp
p y p y p
f f f
xp
x p x p
yp
y p y p
Tfzp
zp zp
yp
f
用 矩 阵 表 示,
因此，进行机器人运行学计算时，不能省略透视矩阵，有摄像头时，透视矩阵为
[0 - 0]，没有摄像头时为 [0 0 0 ] 。
f
1
10
f
1
0
0100
0010
0001
11
T
1
p
f
Tf
f
y
z
y
x
z
y
x
z
y
x
p
p
p
p
p
p
p
p
p
用矩阵表示：
知识点：
点和面的齐次坐标和齐次变换
三个基本旋转矩阵
绝对变换,如果所有的变换都是 相对于固定坐标系 中各坐标轴旋转或平移,则依次 左乘,称为 绝对变换 。
相对变换,如果动坐标系 相对于自身坐标系 的当前坐标轴旋转或平移，则齐次变换为依次 右乘,称为 相对变换 。
绕任意轴选转,5步顺序
透视变换习题 1：
∑ O′与 ∑ O初始重合,∑ O′作如下运动：①绕 Z轴转动 30o；
②绕 X轴转动 60o；③绕 Y轴转动 90o。求 T。
1000
0100
0030c o s30s i n
0030s i n30c o s
R 1
1000
060c o s60s i n0
060s i n60c o s0
0001
2
R
1000
090c o s090s i n
0010
090s i n090c o s
3
R
1000
002/12/3
02/34/34/1
02/14/34/3
123 RRRT
习题 2：
∑ O′与 ∑ O初始重合,∑ O′作如下运动：①绕 X轴转动 90o；②绕
w轴转动 90o；③绕 Y轴转动 90o。求① T；②改变旋转顺序，如何旋转才能获得相同的结果。
1000
090c o s90s i n0
090s i n-09c o s0
0001
R 1
1000
0100
0090c o s90s i n
0090s i n90c o s
2
R
1000
090c o s090s i n
0010
090s i n090c o s
3
R
1000
0010
0100
0001
213 RRRT
解①：
解 ②,
①绕 Z（ w）轴转动 90o；
②绕 X轴转动 90o；
③绕 Y轴转动 90o。
习题 3：
矢量 在 ∑ O′中表示为,∑ O′相对于 ∑ O的奇次变换为：
P? kjip 2230
1000
1100
20001
10010
T oo
中的矢量在求此时
，轴转动的绕②轴转的①绕当中的矢量在求的位置和姿态在画出：求
0
20X0 90Y0 0)3
00 2)
00 1)
'O
0
p
pp
解,1）
z
x
y
u
w
v
o
o′
解,2）
1
3
23
8
T 00 pp o?
解,3）
，
，
1000
090c o s090s i n
0010
090s i n090c o s
R 1
1000
0100
0010
20001
T
1r
1000
10010
20001
21100
TRT 1
1 or
T?
1
8
23
23
1
2
2
3
1000
10010
20001
21101
00 pTp
习题 4：
如图所示,1）写出,,,； 2）求
1T? 21T 32T 43T 40 T
1000
11-00
301-0
3,5-001
T 1?
1000
101-0
3001-
0100
T 21
1000
0001
5
5
3
5
4
-0
0
5
4
5
3
0
T 32
1000
01-00
0010
3,5001-
T 43
解,1）
o
0
x 0
y
0
z
4
3
3,5
1
1
o
1
x
1
y
1
z
2
o
2
x
2
y
2
z
3
o
3
x
3
y
3
z
4
o
4
x
4
y
4
z
解 2）,根据定义 2,绕自身旋转,右乘
1000
50,6-0,8-0
00,8-0,60
0001-
T T T TT 433221140
习题 1：
∑ O′与 ∑ O初始重合,∑ O′作如下运动：①绕 z轴转动 90o；②绕
v轴转动 90o；③绕 x轴转动 90o。求① T；②改变旋转顺序，如何旋转才能获得相同的结果。
习题 2：
已知齐次变换矩阵要求 R（ f,θ),求 f和 θ 值
1000
0001
0100
0010
H
习题 4：
如图所示,1）写出,,,4T5； 2）求 oT5
1T? 21T 32T 43T
科学园 C1栋机器人研究所
206室
86414462-12
duzj01@hit.edu.cn
junhong.ji@hit.edu.cn
参考教材
付京逊,机器人学,
蔡自兴,机器人学,
2.1 引言机器人位置和姿态的描述
机器人可以用一个开环关节链来建模
由数个驱动器驱动的转动或移动关节串联而成
一端固定在基座上，另一端是自由的，安装工具，用以操纵物体
i?
n
o a
人们感兴趣的是操作机末端执行器相对于固定参考坐标数的空间几何描述，也就是机器人的运动学问题
机器人的运动学即是研究机器人手臂末端执行器位置和姿态与关节变量空间之间的关系动画示例运动学研究的问题
Where is my hand?
Direct Kinematics
HERE!
How do I put my
hand here?
Inverse Kinematics,
Choose these angles!
运动学正问题运动学逆问题
丹纳维特（ Denavit）和哈顿贝格（ Hartenberg）
于 1955年提出了一种矩阵代数方法解决机器人的运动学问题 — D-H方法
具有直观的几何意义
能表达动力学、计算机视觉和比例变换问题
其数学基础即是齐次变换
2.2 点和面的齐次坐标
2.2.1 点的齐次坐标
一般来说,n维空间的齐次坐标表示是一个（ n+1）维空间实体。有一个特定的投影附加于 n维空间，也可以把它看作一个附加于每个矢量的特定坐标 — 比例系数。
kcjbiav
zy x Tw
w
z
y
x
V?
式中 i,j,k为 x,y,z 轴上的单位矢量，
a=,b=,c=,w为比例系数
w
x
w
y
w
z
显然，齐次坐标表达并不是唯一的，随
w值的不同而不同。在计算机图学中,w
作为通用比例因子，它可取任意正值，但在机器人的运动分析中，总是取 w=1 。
列矩阵
[例 ]：
kjiV 543
可以表示为：
V=[3 4 5 1]T
或 V=[6 8 10 2]T
或 V=[-12 -16 -20 -4]T
齐次坐标与三维直角坐标的区别
V点在 ΣOXYZ坐标系中表示是唯一的（ x,y,z）
而在齐次坐标中表示可以是多值的。不同的表示方法代表的 V点在空间位置上不变。
x
y
z
z
z
x
V
图 2 - 2
o
几个特定意义的齐次坐标：
[0,0,0,n]T— 坐标原点矢量的齐次坐标,n
为任意非零比例系数
[1 0 0 0]T— 指向无穷远处的 OX轴
[0 1 0 0]T— 指向无穷远处的 OY轴
[0 0 1 0]T— 指向无穷远处的 OZ轴
2个常用的公式：
zzyyxx babababa
kbabajbabaibaba
bbb
aaa
kji
ba xyyxzxxzyzzy
zyx
zyx
)()()(
2.2.2 平面的齐次坐标
平面齐次坐标由行矩阵 P=[a b c d ]来表示
当点 v=[x y z w]T处于平面 P内时，矩阵乘积 PV=O，或记为
0
dwczbyax
w
z
y
x
dcbaPV
如果定义一个常数 m=,则有：222 cba
m
d
m
c
w
z
m
b
w
y
m
a
w
x )()( k
m
cj
m
bi
m
ak
w
zj
w
yi
w
x=
可以把矢量 解释为某个平面的外法线，此平面沿着法线方向与坐标原点的距离为。
)( kmcjmbima
m
d?
[例 ]：
因此一个平行于 x,y轴,且在 z轴上的坐标为单位距离的平面 P可以表示为,或有,PV=
1100P2200P
v0
v0
v0
点在平面下方点在平面上点在平面上方例如：点 V=[10 20 1 1]T必定处于此平面内,而点 V=[0 0 2 1]T
处于平 P 的上方点 V=[0 0 0 1]T处于 P平面下方 。 因为：
0
1
1
20
10
101000?
0 1
1
2
0
0
1100
0 -1
1
0
0
0
1-100
与点矢 相仿，平面 也没有意义T00000000
2.2 旋转矩阵及旋转齐次变换
2.2.1 旋转矩阵设固定参考坐标系直角坐标为 ΣOxyz,动坐标系为 ΣO′uvw，
研究旋转变换情况 。
x
y
z
w
v
u
P
o
( O ' )
图 2 - 3
① 初始位置时，动静坐标系重合,O,O′重合，如图。各轴对应重合，设 P点是动坐标系 ΣO′uvw中的一点，且固定不变。
则 P点在 ΣO′uvw中可表示为：
wwuvuuu v w kPjPiPP
、,为坐标系 ΣO′uvw的单位矢量,则 P点在 Σoxyz中可表示为：
ui vj wk
zzyyxxxyz iPiPiPPuvwP xyz
② 当动坐标系 ΣO′uvw绕 O点回转时，求 P点在固定坐标系
Σoxyz中的位置
y
z
x
o
( O ' )
u
v
w
P
Pw
Pv
Pu
图 2 - 4
已知：
P点在 ΣO′uvw中是不变的仍然成立，由于 ΣO′uvw回转，则：
wwuvuuu v w kPjPiPP
)(P wwvvuuxxu v wx kPjPiPiiP
)(P wwvvuuyyu v wy kPjPiPjjP
)(P wwvvuuzzu v wz kPjPiPjjP
用矩阵表示为,
w
v
wzvzz
wvyy
wxvxx
z
y
x
P
P
P
kkjkik
kjjjij
kijiii
P
P
P?
y
（ 2-7）
uv wx y z
wzvzz
wvyy
wxvxx
PRp
kkjkik
kjjjij
kijiii
,R y 则旋转矩阵为：定义
反过来：
xyzu v w PRP 1 RRR d et
*1
T1 RR
Rde t
因此是正交矩阵，的行列式，由于为的伴随矩阵，为 RRRR
2.2.2 旋转齐次变换用齐次坐标变换来表示式（ 2-7）
11000
0
0
0
1
w
v
u
z
y
x
P
P
P
R
P
P
P
11000
0
0
0
1
1
z
y
x
w
v
u
P
P
P
R
P
P
P
2.2.3 三个基本旋转矩阵和合成旋转矩阵三个基本旋转矩阵
),(?xR
即动坐标系 求 的旋转矩阵,也就是求出坐标系 中各轴单位矢量 在固定坐标系中各轴的投影分量,很容易得到在重合时,有：
角，轴转动绕, XOvwO?
vwO?'? wv kji,,? Oxyz?
),(?xR
100
010
001
R
由图 2-5可知,在 y轴上的投影为,在 z轴上的投影为,在 y轴上的投影为,在 z轴上的投影为
，所以有：
vj?cosyj
sinzk?sinyj?wk
coszk
vj
wk
wzvzz
wvyy
wxvxx
kkjkik
kjjjij
kijiii
y)R ( x,
x
y
z
o
u
v
w
U '
V '
W '
O '
图 2 - 5
co m
ii ux
s i n0
s i nc o s0
001
方向余弦阵同理：
c o s0s i n
010
s i n0c o s
)y,R （
100
0c o ss i n
0s i n-c o s
)z,R
（
co ms i n0
s i nc o s0
001
)R ( x,
三个基本旋转矩阵,
x
y
z
o
u
v
w
U '
W '
O '
x
y
z
o
u
v
w
U '
W '
O '
v '
合成旋转矩阵,
例 1：在动坐标中有一固定点,相对固定参考坐标系 做如下运动：① R（ x,90° ）；② R(z,
90° )；③ R(y,90° )。求点 在固定参考坐标系 下的位置。
Tu v wPo 1321'?
Oxyz?
uvwPo' Oxyz?
解 1：用画图的简单方法解 2：用分步计算的方法
① R（ x,90° ）
② R（ z,90° ）
③ R（ y,90° ）
1
2
3
1
1
3
2
1
1000
0010
01-00
0001
'P
1
2
1
3
1
2
3
1
1000
0100
0001
001-0
''P
1
3
1
2
1
2
1
3
1000
0001-
0010
0100
'''P
（ 2-14）
（ 2-15）
（ 2-16）
上述计算方法非常繁琐，可以通过一系列计算得到上述结果。将式（ 2-14）（ 2-15）（ 2-16）联写为如下形式：
11
44
w
v
u
z
y
x
P
P
P
R
P
P
P
R4x4为二者之间的关系矩阵，我们令：
),(),(),RR 44 xRzRy（
定义 1：
当动坐标系 绕固定坐标系 各坐标轴顺序有限次转动时，其合成旋转矩阵为各基本旋转矩阵依旋转顺序 左乘 。
注意,旋转矩阵间不可以交换
uvwO'? Oxyz?
平移齐次变换矩阵
1000
100
010
001
c) b (a T r an sH
c
b
a
注意,平移矩阵间可以交换，
平移和旋转矩阵间不可以交换
z
y
x
o
o′
w′
u ′
v′
a
b
c
2.2.4 相对变换举例说明：
例 1,动坐标系 ∑0′起始位置与固定参考坐标系 ∑0重合,动坐标系
∑0′做如下运动：① R(Z,90o) ② R（ y,90o） ③ Trans(4,-3,7)
，求合成矩阵解 1：用画图的方法：
o′
z
y
x
7
4
- 3
o
w ```
u ```
v ```
v ″
u ″
w ″
z
y
x
o
o ( o′ )
x
y
z
u
v
w
z
y
x
u ′
w′
o ( o′ )
v′
解 2：用计算的方法根据定义 1,我们有：
1000
7010
3001
4100
)R( Z,9 0 )90 R( y,7),3,T r an s ( 4T
以上均以固定坐标系多轴为变换基准,因此矩阵左乘 。
如果我们做如下变换，也可以得到相同的结果：
例 2,① 先平移 Trans (4,-3,7)； ② 绕当前 轴转动 90o；
③ 绕当前 轴转动 90o；求合成旋转矩阵 。
v?
w?
（ 2-20）
解 1：用画图的方法
z
y
x
o ( o′ )
v
w
u
z
y
x
o
o′
w′
u ′
v′
o
z
y
x
o′
w ″
v ″
u ″
x
y
z
o
o′
w ```
u ```
v ```
解 2：用计算的方法
1000
7010
3001
4100
)R ( Z,9 0 )90 R ( y,7),3,T r a n s ( 4T oo
（ 2-21）
式 （ 2-20） 和式 （ 2-21） 无论在形式上,还是在结果上都是一致的 。 因此我们有如下的结论：
动坐标系在固定坐标系中的齐次变换有 2种情况：
定义 1,如果所有的变换都是相对于固定坐标系中各坐标轴旋转或平移,则依次左乘,称为绝对变换 。
定义 2,如果动坐标系相对于自身坐标系的当前坐标轴旋转或平移，则齐次变换为依次右乘，称为相对变换。
结果均为为动坐标系在固定坐标中的位姿 （ 位置 +姿态 ） 。
相对于固定坐标系,轴。轴相当于轴，轴相对于轴，轴相当于 ZYX wv?
也就是说，动坐标系绕自身坐标轴做齐次变换,要达到绕固定坐标系相等的结果，就应该用相反的顺序。
齐次变换矩阵 T 的意义：
机器人用到相对变换的时候比较多
例如机械手抓一个杯子，
如右图所示，手爪需要转动一个角度才抓的牢，
相对于固定坐标系表达太麻烦，可以直接根据手爪的坐标系表示
但也要知道在 ∑O中的位姿，就用右乘的概念。
x
y
z
o
H
2.2.5 绕通过原点的任意轴旋转的齐次变换
有时动坐标系 ∑ O′可能绕过原点 O的而分量分别为 rx、
ry,rz的任意单位矢量 r 转动 φ角。
研究这种转动的好处是可用 ∑ O′绕某轴 r 的一次转动代替绕 ∑ O各坐标轴的数次转动
为推导此旋转矩阵，可作下述变换：
a,绕 X 轴转 α角，
使 r 轴处于 XZ平面内
a,绕 Y 轴转 -β角，使 r
轴与 OZ轴重合
a,绕 OZ轴转动 φ角
b,绕 Y 轴转 β角
c,绕 X 轴转 -α角
X
Y
Z
r x
r y
r z
A B
C
D
B '
O
α
β
5
1
2
43
r
A '
由上图容易求出：
2
z
2
y
y
rr
rs in
2
z
2
y
z
rr
rc o s
x
x r
r
r
r
OCs in
2
z
2
y
2
z
2
y rr
r
rr
OB
C'Bco s
由定义 1和定义 2，上述 5次旋转的合成旋转矩阵为：
c o ss i n0
s i n-c o s0
001
c o s0s i n
010
s i n-0c o s
100
0c o ss i n
0s i n-c o s
c o s0s i n-
010
s i n0c o s
c o ss i n0
s i nc o s0
001
RRRRRR
,x,y,z,y,x,r
（ 2-25）
带入式 （ 2-25），得
c o s)c o s(1r
s i nr)c o s(1rr
s i nr)c o s(1rr
s i nr)c o s(1rrs i nr)c o s(1rr
c o s)c o s(1rs i nr)c o s(1rr
s i nr)c o s(1rrc o s)c o s(1r
R
2
z
xzy
yzx
xzyyzx
2
yzyx
zyx
2
x
,r
2.2.6 齐次交换矩阵的几何意义设 T=,有一个手爪，已知其在 ∑ O的位置，设一个该坐标系 ∑ O′，已知，,那么 ∑ O′在 ∑ O中的齐次坐标变换为,如果手爪转了一个角度，
则：
1000
3
2
1
twzzz
twyyy
twxxx
111' cbao
1000
100
010
001
T
1
1
1
1 c
b
a
1000
p
p
p
T
z
yyy
xxx
zzz
y
x
w
w
w
T反映了 ∑ O′在 ∑ O中的位置和姿态,即表示了该坐标系原点和各坐标轴单位矢量在固定坐标系中的位置和姿态 。
该矩阵可以由 4个子矩阵组成,写成如下形式：
比例系数透视矩阵位置矢量旋转矩阵
1131
1333
wf
PR
T
zzz
yyy
xxx
w
w
w
33R
为姿态矩阵，表示动坐标系 ∑ O′在固定参考坐标系 ∑ O中的姿态，即表示 ∑ O′各坐标轴单位矢量在 ∑ O各轴上的投影为位置矢量矩阵，代表动坐标系 ∑ O′坐标原点在固定参考坐标系 ∑ O中的位置Tzyx ppp ][P 13
为透视变换矩阵，在视觉中进行图像计算，
一般置为 0 ]000[f 31
为比例系数]1[
11w
如果需要求解 ∑ O在 ∑ O′中的位置和姿态，此时的齐次变换矩阵为,即求逆矩阵：1?T
1000
-
-R
-
T
T
T1-
33
T
1
pw
pv
p
）（
）（
）（?
kpjpipp zyx
kji zyx
kvjvivv zyx
kwjwiww zyx
其中：
这些式子以后经常遇到，
在机器人计算中，所要求的就是齐次变换矩阵下
2.2.7 透镜成像的齐次变换
p
p
,
P [ x 1 ]
P P [ x 1 ]
1
()
1
pp
pp
T
T
yz
yz
y p zp zp f
y p zp zp y p f
y p f
zp x p y p f
zp x p y p y p f
y p f y p f y p
y p y p f y p y p f f
xp
xp
y
以光心为原点O，光轴与y轴重合，P为物点，
用齐次坐标表示求 的齐次坐标，即求根据三角形相似原理：
注意 是负值,是正值，所以实际上为相减关系又有设
11
1 0 0 0
0 1 0 0
0 0 1 0
11 1 0 0 0 1
y p zp
y p zp
p y p zp
f f f
xp
x p x p
yp
y p y p
Tfzp
zp zp
yp
f
用矩阵表示：
z
y
P
y p
f
o
z p
f
p
z?
'
P
p
y?
p
p
,
P [ x 1 ]
P P [ x 1 ]
1
()
1
pp
pp
T
T
yz
yz
y p zp zp f
y p zp zp y p f
y p f
zp x p y p f
zp x p y p y p f
y p f y p f y p
y p y p f y p y p f f
xp
xp
y
以 光 心 为 原 点 O,光 轴 与 y 轴 重 合,P 为 物 点,
用 齐 次 坐 标 表 示求 的 齐 次 坐 标,即 求根 据 三 角 形 相 似 原 理,
注 意 是 负 值,是 正 值,所 以 实 际 上 为 相 减 关 系又 有设
11
1 0 0 0
0 1 0 0
0 0 1 0
11 1 0 0 0 1
y p zp
y p zp
p y p y p
f f f
xp
x p x p
yp
y p y p
Tfzp
zp zp
yp
f
用 矩 阵 表 示,
因此，进行机器人运行学计算时，不能省略透视矩阵，有摄像头时，透视矩阵为
[0 - 0]，没有摄像头时为 [0 0 0 ] 。
f
1
10
f
1
0
0100
0010
0001
11
T
1
p
f
Tf
f
y
z
y
x
z
y
x
z
y
x
p
p
p
p
p
p
p
p
p
用矩阵表示：
知识点：
点和面的齐次坐标和齐次变换
三个基本旋转矩阵
绝对变换,如果所有的变换都是 相对于固定坐标系 中各坐标轴旋转或平移,则依次 左乘,称为 绝对变换 。
相对变换,如果动坐标系 相对于自身坐标系 的当前坐标轴旋转或平移，则齐次变换为依次 右乘,称为 相对变换 。
绕任意轴选转,5步顺序
透视变换习题 1：
∑ O′与 ∑ O初始重合,∑ O′作如下运动：①绕 Z轴转动 30o；
②绕 X轴转动 60o；③绕 Y轴转动 90o。求 T。
1000
0100
0030c o s30s i n
0030s i n30c o s
R 1
1000
060c o s60s i n0
060s i n60c o s0
0001
2
R
1000
090c o s090s i n
0010
090s i n090c o s
3
R
1000
002/12/3
02/34/34/1
02/14/34/3
123 RRRT
习题 2：
∑ O′与 ∑ O初始重合,∑ O′作如下运动：①绕 X轴转动 90o；②绕
w轴转动 90o；③绕 Y轴转动 90o。求① T；②改变旋转顺序，如何旋转才能获得相同的结果。
1000
090c o s90s i n0
090s i n-09c o s0
0001
R 1
1000
0100
0090c o s90s i n
0090s i n90c o s
2
R
1000
090c o s090s i n
0010
090s i n090c o s
3
R
1000
0010
0100
0001
213 RRRT
解①：
解 ②,
①绕 Z（ w）轴转动 90o；
②绕 X轴转动 90o；
③绕 Y轴转动 90o。
习题 3：
矢量 在 ∑ O′中表示为,∑ O′相对于 ∑ O的奇次变换为：
P? kjip 2230
1000
1100
20001
10010
T oo
中的矢量在求此时
，轴转动的绕②轴转的①绕当中的矢量在求的位置和姿态在画出：求
0
20X0 90Y0 0)3
00 2)
00 1)
'O
0
p
pp
解,1）
z
x
y
u
w
v
o
o′
解,2）
1
3
23
8
T 00 pp o?
解,3）
，
，
1000
090c o s090s i n
0010
090s i n090c o s
R 1
1000
0100
0010
20001
T
1r
1000
10010
20001
21100
TRT 1
1 or
T?
1
8
23
23
1
2
2
3
1000
10010
20001
21101
00 pTp
习题 4：
如图所示,1）写出,,,； 2）求
1T? 21T 32T 43T 40 T
1000
11-00
301-0
3,5-001
T 1?
1000
101-0
3001-
0100
T 21
1000
0001
5
5
3
5
4
-0
0
5
4
5
3
0
T 32
1000
01-00
0010
3,5001-
T 43
解,1）
o
0
x 0
y
0
z
4
3
3,5
1
1
o
1
x
1
y
1
z
2
o
2
x
2
y
2
z
3
o
3
x
3
y
3
z
4
o
4
x
4
y
4
z
解 2）,根据定义 2,绕自身旋转,右乘
1000
50,6-0,8-0
00,8-0,60
0001-
T T T TT 433221140
习题 1：
∑ O′与 ∑ O初始重合,∑ O′作如下运动：①绕 z轴转动 90o；②绕
v轴转动 90o；③绕 x轴转动 90o。求① T；②改变旋转顺序，如何旋转才能获得相同的结果。
习题 2：
已知齐次变换矩阵要求 R（ f,θ),求 f和 θ 值
1000
0001
0100
0010
H
习题 4：
如图所示,1）写出,,,4T5； 2）求 oT5
1T? 21T 32T 43T
