机器人学导论

第二讲 机器人位置运动学I
齐次变换矩阵

黄之峰
广东工业大学
自动化学院
主要内容：
1. 机器人运动学的矩阵表示
2. 齐次变换矩阵
3. 变换矩阵的逆
 机器人位置运动学解决的主要问题
Where is my hand?

运动学正问题

Direct Kinematics
HERE!

How do I put my
hand here?
运动学逆问题

Inverse Kinematics:
Choose these angles!
第二讲
1 机器人运动学的矩阵表示

空间点的坐标表示形式 空间向量的表示
𝑎𝑥
P=(𝑎𝑥, 𝑏𝑦, 𝑐𝑧)
𝑃= 𝑏𝑦
𝑐𝑧
仅仅能表示位置 即表示位置又
z z 表示方向！！

P P

cz cz y
y

ax
ax
by
by
x
x
第二讲
1 机器人运动学的矩阵表示
比例因子w 原始向量分量
比例因子w的作用：
𝑥 • 缩放向量的长度，通常大于等
𝑥 𝑎𝑥 =
𝑤
𝑃= 𝑦𝑧 其中 𝑦 于0
𝑏𝑦 =
𝑤 𝑤 • 当w =1时，向量长度不变，
z 𝑧
𝑐𝑧 = • 当w > 1时，向量放大
𝑤
• 当w < 1时，向量缩小
P
• 当w = 0时，无穷大，转化为方
cz y
向向量
ax
by
x
第二讲
1 机器人运动学的矩阵表示

例：
   
V = 3i + 4j + 5k
可表示为：

V = [ 3 4 5 1]T

V = [ 1.5 2 2.5 0.5]T

V = [ -12 - 16 - 20 - 4]T
第二讲
1 机器人运动学的矩阵表示

单位方向向量：
作用：表示方向
特征：1，向量的模为1。
2，比例系数为0。

𝑎𝑥
𝑏𝑦
𝑃= 𝑐 , 且 𝑎𝑥 2 +𝑏𝑦 2 +𝑐𝑧 2=1
𝑧
0
第二讲
1 机器人运动学的矩阵表示
例2.1：
有一个向量 𝑖 + 5𝑗෡ + 2𝑘෡
𝑃=3෡ ，按如下要求将其表示成
矩阵形式：
① 比例因子为2
② 将它表示为方向的单位向量
第二讲
1 机器人运动学的矩阵表示
坐标系在固定参考坐标系原点的表示

𝑛𝑥 𝑜𝑥 𝑎𝑥 在笛卡尔坐标系中，确定一
𝐹 = 𝑛𝑦 𝑜𝑦 𝑎𝑦 个坐标系通常需要三个单位
𝑛𝑧 𝑜𝑧 𝑎𝑧
向量n,o,a
z

o
讨论，n,o,a 三者
a
∑R 有什么关系？
y
∑U

n
x
第二讲
1 机器人运动学的矩阵表示
坐标系在固定参考坐标系中的表示
坐标系的三个轴的单位方向向量
z a

∑R R o 𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑃𝑥
𝑛𝑦 𝑜𝑦 𝑎𝑦 𝑃𝑦
P 𝐹=
𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑃𝑧
∑U
n y 0 0 0 1

x
坐标系的原点在固定参考坐标
系的位置
第二讲
1 机器人运动学的矩阵表示
例2.2 如图2.7所示的F坐标系位于参考坐标系中3,5,7的位
置，它的n轴与x轴平行，o轴相对于y轴的角度为45°，a轴
相对于z轴的角度为45°。则该坐标可表示为：
第二讲
1 机器人运动学的矩阵表示
刚体的表示
通过在其上面固连一个坐标系，再将该固连的坐标
系在空间表示出来。 z a
𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑃𝑥
𝑛𝑦 𝑜𝑦 𝑎𝑦 𝑃𝑦 o
𝐹=
𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑃𝑧
0 0 0 1 P

n y
空间中的刚
体有几个自
由度？
x
第二讲
1 机器人运动学的矩阵表示
刚体在空间中具有6个自由度，包括沿着x,y,z三个轴移动，
以及绕着三个轴旋转。因此要全面的定义空间的一个物体，
需要六条独立的信息来描述物体原点在参考坐标系中相对
三个参考坐标轴的位置以及物体关于这三个坐标轴的姿态。
z a
𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑃𝑥
𝑛𝑦 𝑜𝑦 𝑎𝑦 𝑃𝑦
𝐹= o
𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑃𝑧
0 0 0 1 P

n y
问题，虽然只需要6个独立信息，但
是矩阵中有12个参数，需要12个方
程才能求解，怎么办？ x
第二讲
1 机器人运动学的矩阵表示
隐含的坐标系特性， 𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑃𝑥
𝑛𝑦 𝑜𝑦 𝑎𝑦 𝑃𝑦
• 三个向量n,o,a相互垂直。 𝐹=
𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑃𝑧
• 每个单位向量的长度必须为1。 0 0 0 1

上述约束条件可以转换为下面的六个约束方程
𝑛∙𝑜 =0 𝑛𝑥 𝑜𝑥 + 𝑛𝑦 𝑜𝑦 + 𝑛𝑧 𝑜𝑧 = 0
𝑛∙𝑎 =0 𝑛𝑥 𝑎𝑥 + 𝑛𝑦 𝑎𝑦 + 𝑛𝑧 𝑎𝑧 = 0
𝑎∙𝑜 =0 𝑎𝑥 𝑜𝑥 + 𝑎𝑦 𝑜𝑦 + 𝑎𝑧 𝑜𝑧 = 0
𝑛 =1 𝑛𝑥 2 + 𝑛𝑦 2 + 𝑛𝑧 2 = 1
𝑜 =1
𝑜𝑥 2 + 𝑜𝑦 2 + 𝑜𝑧 2 = 1
𝑎 =1
𝑎𝑥 2 + 𝑎𝑦 2 + 𝑎 𝑧 2 = 1
第二讲
1 机器人运动学的矩阵表示
例2.3 对于下列坐标系，求解所稀缺的元素的值，
并用矩阵来表示这个坐标系 （作业）

? 0 ? 5
? ? −0.707 3
𝐹= 0 ? ? 2
0 0 0 1
第二讲
2 齐次变换矩阵

怎么计算机
器人手掌在
坐标系W中
的位置？

世界坐标系∑w和手臂的局部坐标系∑a
（a）初始状态下， ∑w和∑a方位一致；
（b） ∑a和手臂一起转动
第二讲
2 齐次变换矩阵 z a

姿态 位置 o

𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑃𝑥 P
𝑛𝑦 𝑜𝑦 𝑎𝑦 𝑃𝑦 n y
𝐹=
𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑃𝑧
0 0 0 1
x
齐次，即行列数目相同

作用：
1，描述运动坐标系在世界坐标系中的位姿（本质）
2，描述运动坐标系在世界坐标系中的运动。
3，将运动坐标系中的位置转换到世界坐标系中描述
第二讲
2 齐次变换矩阵
① 纯平移，坐标系（物体）在空间以不变的姿态运动

z 1 0 0 𝑑𝑥
a T= 0 1 0 𝑑𝑦
∑F
o a
o
0 0 1 𝑑𝑧
d 0 0 0 1
P n ∑Fnew
n
∑U y

x
第二讲
2 齐次变换矩阵
① 纯平移，坐标系（物体）在空间以不变的姿态运动
𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑃𝑥 𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑃𝑥 + 𝑑𝑥
𝑛 𝑜𝑦 𝑎𝑦 𝑃𝑦 𝑛 𝑜𝑦 𝑎𝑦 𝑃𝑦 + 𝑑𝑦
𝐹= 𝑦 𝐹𝑛𝑒𝑤 = 𝑦
𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑃𝑧 𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑃𝑧 + 𝑑𝑧
0 0 0 1 0 0 0 1

1 0 0 𝑑𝑥 𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑃𝑥 𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑃𝑥 + 𝑑𝑥
0 𝑑𝑦 𝑛 𝑜𝑦 𝑎𝑦 𝑃𝑦 𝑛 𝑜𝑦 𝑎𝑦 𝑃𝑦 + 𝑑𝑦
𝐹new= 0 1 × 𝑦 = 𝑦
z 0 0 1 𝑑𝑧 𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑃𝑧 𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑃𝑧 + 𝑑𝑧
0 0 0 1 0 0 0 1 0 0 0 1
a
o a
∑F o
d
P n
n
∑Fnew 1 0 0 𝑑𝑥
0 𝑑𝑦
y T= 0 1 𝐹new =Trans(𝑑𝑥 , 𝑑𝑦 , 𝑑𝑧 )× 𝐹
∑U 0 0 1 𝑑𝑧
0 0 0 1

x
第二讲
2 齐次变换矩阵
② 绕轴纯旋转
P为坐标系F中的一点，假设坐标系F原先与固定参考坐标系U
重合，然后绕x轴逆时针旋转θ角，求P相对于固定参考坐标
系的新坐标 𝑃𝑥 = 𝑃𝑛
第二讲
2 齐次变换矩阵
② 绕轴纯旋转
𝑃𝑥 ′ = 𝑃𝑛
𝑃𝑦 ′ = 𝑙1 − 𝑙2 =𝑃𝑜 cos 𝜃 − 𝑃𝑎 𝑠𝑖𝑛𝜃
沿着X轴进行观察
𝑃𝑧 ′ = 𝑙3 + 𝑙4 =𝑃𝑜 𝑠𝑖𝑛𝜃 + 𝑃𝑎 cos 𝜃

𝑃𝑥 ′ 1 0 0 𝑃𝑛
𝑃𝑦 ′ = 0 cos 𝜃 − sin 𝜃 𝑃𝑜
𝑃𝑧 ′ 0 sin 𝜃 cos 𝜃 𝑃𝑎

𝑃𝑥yz ′ =Rot(𝑥,𝜃)× 𝑃𝑛𝑜𝑎

 第二讲
2 齐次变换矩阵
② 绕轴纯旋转

1 0 0
𝑈 𝑈
Rot(x,𝜃)= 0
0
cos 𝜃
sin 𝜃
− sin 𝜃
cos 𝜃
𝑃= 𝑇𝑅 × 𝑅𝑃
cos 𝜃 0 sin 𝜃
Rot(y,𝜃)= 0 1 0 a. P点在固定参考坐标系U中的位置
− sin 𝜃 0 cos 𝜃
b. 旋转矩阵：R坐标系相对于固定参
cos 𝜃 − sin 𝜃 0 考坐标系的旋转
Rot(z,𝜃)= sin 𝜃 cos 𝜃 0
0 0 1 c. P点在R坐标系中的位置
第二讲
2 齐次变换矩阵

再回来看下我们
刚刚的问题

世界坐标系∑w和手臂的局部坐标系∑a
（a）初始状态下， ∑w和∑a方位一致；
（b） ∑a和手臂一起转动
第二讲
2 齐次变换矩阵
③ 复合变换的表示
复合变换是由固定参考坐标系或当前运动坐标系的一系列沿
轴平移和绕轴旋转变换所组成的。任何变换都可以分解为按
一定顺序的一组平移和旋转变换。

𝑈 𝑈
𝑃= 𝑇𝑅 × 𝑅𝑃

𝑃𝑥yz = Rot(y,𝛽) × Trans(𝑙1 ,𝑙2 , 𝑙3 ) ×Rot(𝑥,𝛼)× 𝑃𝑛𝑜𝑎

第三步 第二步 第一步
第二讲
2 齐次变换矩阵
③ 复合变换的表示
例2.6 固连在坐标系（𝑛,𝑜,𝑎）上的点P(7,3,2)T 经历如下
变换，求出变换后该店相对于固定参考坐标系的坐标。
（1）绕z轴旋转90°；
（2）接着绕y轴旋转90°；
（3）接着再平移[4,-3,7]；
𝑃𝑥yz = Trans(4, − 3,7) × Rot(y,90) ×Rot(z,90)× 𝑃𝑛𝑜𝑎 =
1 0 0 4 0 0 1 0 0 −1 0 0 7 6
0 1 0 −3 × 0 1 0 0 × 1 0 0 0 × 3 = 4
0 0 1 7 −1 0 0 0 0 0 1 0 2 10
0 0 0 1 0 0 0 1 0 0 0 1 1 1
第二讲
3 齐次变换矩阵 P(7,3,2)T
③ 复合变换的表示
z

绕z轴旋转90°

o y
n

绕y轴旋转90°
x

平移[4,-3,7]
第二讲
2 齐次变换矩阵
③ 复合变换的表示
例2.7 固连在坐标系（𝑛,𝑜,𝑎）上的点P(7,3,2)T 经历如下
变换，求出变换后该店相对于固定参考坐标系的坐标。
（1）绕z轴旋转90°；
（2）接着再平移[4,-3,7]；
（3）接着绕y轴旋转90°；
𝑃𝑥yz = Rot(y,90) × Trans(4, − 3,7) ×Rot(z,90)× 𝑃𝑛𝑜𝑎 =
0 0 1 0 1 0 0 4 0 −1 0 0 7 9
0 1 0 0 × 0 1 0 −3 × 1 0 0 0 × 3 = 4
−1 0 0 0 0 0 1 7 0 0 1 0 2 −1
0 0 0 1 0 0 0 1 0 0 0 1 1 1
第二讲
2 齐次变换矩阵 P(7,3,2)T

③ 复合变换的表示
z

绕z轴旋转90°

o y
n
平移[4,-3,7]
x

绕y轴旋转90°
第二讲
2 齐次变换矩阵

再看看这个图

世界坐标系∑w和手臂的局部坐标系∑a
（a）初始状态下， ∑w和∑a方位一致；
（b） ∑a和手臂一起转动
第二讲
2 齐次变换矩阵
新的问题：
z
新的问题：机 a o
械手得怎么调
整才能抓起水 n
杯

x
第二讲
2 齐次变换矩阵

z
z
ao
o
末端手腕关 an
节旋转90度 n

y
y

x
x
第二讲
2 齐次变换矩阵
相对于运动（当前）坐标系的变换
例2.7 固连在坐标系（𝑛,𝑜,𝑎）上的点P(7,3,2)T 经历如下变换，求出变换后
该点相对于固定参考坐标系的坐标。
绝对变换 相对变换
（1）绕z轴旋转90°； （1）绕a轴旋转90°；
（2）接着再平移[4,-3,7]； （2）再沿着noa平移[4,-3,7]；
（3）接着绕y轴旋转90°； （3）接着绕o轴旋转90°；

绝对变换： 𝑃𝑥yz = Rot(y,90) × Trans(4, − 3,7) ×Rot(z,90)× 𝑃𝑛𝑜𝑎

（3） （2） （1）

相对变换： 𝑃𝑥yz = Rot(𝑎,90) × Trans(4, − 3,7) × Rot(𝑜,90) × 𝑃𝑛𝑜𝑎

（1） （2） （3）
第二讲
2 齐次变换矩阵
右乘法：相对于运动（当前）坐标系的变换
例2.7 固连在坐标系（𝑛,𝑜,𝑎）上的点P(7,3,2)T 经历如下变换，求出变换后
z
该店相对于固定参考坐标系的坐标。
绝对变换
（1）平移[4,-3,7]； a
o y
（2）接着绕y轴旋转90°； n

z z
x
答案：P’(9,0,-11)T
a

n o y
y

x
x 平移[4,-3,7]； a
o 接着绕y轴旋转90°
n
第二讲
2 齐次变换矩阵
右乘法：相对于运动（当前）坐标系的变换
例2.7 固连在坐标系（𝑛,𝑜,𝑎）上的点P(7,3,2)T 经历如下变换，求出变换后
z
该店相对于固定参考坐标系的坐标。
相对变换
（1）沿着noa平移[4,-3,7]；
a
y
（2）接着绕o轴旋转90°； n
o

z
z
x

a
答案：P’(6,0,0)T

n o
y a o
n y

x 沿着noa平移[4,-3,7]； 绕o轴旋转90°；
x
第二讲
2 齐次变换矩阵
知识点小结：
1，齐次变换矩阵的作用，用于求解运动坐标系R中的固定
点P在固定参考坐标系U中的位置。

𝑈 𝑈 𝑅
𝑃= 𝑇𝑅 × 𝑃
2，齐次变换矩阵通常是，由一组的平移和旋转按一定次序
组成的复合变换。

3，坐标变换共有两种：绝对变换和相对变换
第二讲
2 齐次变换矩阵
知识点小结：
4. 绝对变换：如果所有的变换都是相对于固定坐标系U中
各坐标轴旋转或平移，则依次左乘，称为绝对变换。

𝑃𝑥yz = Rot(y,90) × Trans(4, − 3,7) ×Rot(z,90)× 𝑃𝑛𝑜𝑎

（3） （2） （1）

5. 相对变换：如果动坐标系R相对于自身坐标系的当前坐
标轴旋转或平移，则齐次变换为依次右乘，称为相对变换。

𝑃𝑥yz = Rot(𝑎,90) × Trans(4, − 3,7) × Rot(𝑜,90) × 𝑃𝑛𝑜𝑎

（1） （2） （3）
第二讲
3 变换矩阵的逆

𝑈 𝑈 𝑅 𝐻 𝑈 𝑃
𝑇𝐸 = 𝑇𝑅 𝑇𝐻 𝑇𝐸 = 𝑇𝑃 𝑇𝐸
未知
第二讲
3 变换矩阵的逆
显然，为了对机器人运
动学进行分析，需要能
够计算变换矩阵的逆。

𝑈 𝑈 𝑅 𝐻 𝑈 𝑃
𝑇𝐸 = 𝑇𝑅 𝑇𝐻 𝑇𝐸 = 𝑇𝑃 𝑇𝐸

𝑈 −1 𝑈 𝑅 𝐻 𝐻 −1 𝑈 −1 𝑈 𝑃 𝐻 −1
𝑇𝑅 ( 𝑇𝑅 𝑇𝐻 𝑇𝐸 ) 𝑇𝐸 = 𝑇𝑅 𝑇𝑃 𝑇𝐸 𝑇𝐸
𝑅 𝑈 −1 𝑈 𝑃 𝐻 −1
𝑇𝐻 = 𝑇𝑅 𝑇𝑃 𝑇𝐸 𝑇𝐸
第二讲
3 变换矩阵的逆
旋转矩阵的逆 逆阵的定义
𝐴−1 𝐴 = 𝐸
1 0 0
Rot(x,𝜃)= 0 cos 𝜃 − sin 𝜃 𝐴𝐴−1 = 𝐸
0 sin 𝜃 cos 𝜃 𝐴𝐸 = 𝐴

1 0 0
Rot(x,𝜃)−1 = 0 cos 𝜃 sin 𝜃
0 −sin 𝜃 cos 𝜃

重要性质: 旋转矩阵的逆等于旋转矩阵的转置（酉矩阵）

Rot(x,𝜃)−1 = Rot(x,𝜃)𝑇 关于y轴和z轴的旋转矩阵也是

 第二讲
3 变换矩阵的逆
齐次变换矩阵求逆的方法
𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑃𝑥
𝑈
𝑛𝑦 𝑜𝑦 𝑎𝑦 𝑃𝑦 = 𝑅ot 𝑅 𝑃
T=
𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑃𝑧 000 1
0 0 0 1
𝑈 𝑈 𝑈
𝑅ot 𝑅 𝑃 𝑋 𝑌
= 𝑅ot 𝑅 𝑋 𝑅ot 𝑅 𝑌 + 𝑃
000 1 000 1 000 1
𝑈 𝑈 −1 𝑈 𝑇
𝑅ot 𝑅 𝑋 = 𝐸 𝑋 = 𝑅ot 𝑅 == 𝑅ot 𝑅

𝑈 −1 𝑇
𝑅ot 𝑅 𝑌 + 𝑃 =0 𝑈
𝑌 = − 𝑅ot 𝑅 𝑈
𝑃 = − 𝑅ot 𝑅 𝑃
第二讲
3 变换矩阵的逆
齐次变换矩阵求逆的方法
𝑈 𝑈 −1 𝑈 𝑇
𝑅ot 𝑅 𝑋 = 𝐸 𝑋 = 𝑅ot 𝑅 == 𝑅ot 𝑅

𝑈 −1 𝑇
𝑅ot 𝑅 𝑌 + 𝑃 =0 𝑈
𝑌 = − 𝑅ot 𝑅 𝑈
𝑃 = − 𝑅ot 𝑅 𝑃

𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑃𝑥 𝑛𝑥 𝑛𝑦 𝑛𝑧 −𝑛 ∙ 𝑃ത
𝑛𝑦 𝑜𝑦 𝑎𝑦 𝑃𝑦 𝑜𝑥 𝑜𝑦 𝑜𝑧 −𝑜 ∙ 𝑃ത
T=
𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑇 −1 = 𝑎𝑦 𝑎𝑧
𝑃𝑧 𝑎𝑥 −𝑎 ∙ 𝑃ത
0 0 0 1 0 0 0 1
第二讲
3 变换矩阵的逆
计算下述变换矩阵的逆(作业，要求过程)

0.527 −0.574 0.628 2

T= 0.369 0.819 0.439 5
−0.766 0 0.643 3
0 0 0 1
第二讲
课后作业
制作动画月球轨道模拟 在配套例程LnYa Matlab Simulation
Little_Mov_Frame_Lnya 基础上进行
改动制作动画模拟月球轨道。

1，已地球作为固定参考坐标系，绘制
显示；
2，使用圆形或椭圆轨迹模拟月球轨道
并用虚线或点线绘制，（以地球为中心，
轨道面倾斜角度Roll 30°，Pitch45°）
3，绘制运动坐标系模拟月亮运动，要
求x轴指向地心。
第二讲
3 变换矩阵的逆
例题：

题干的目的，求出机器人末端E与物体obj之间的相对转换

5
𝑇𝐻 𝐻𝑇𝐸 𝐸𝑇𝑜𝑏𝑗 = 5𝑇𝑐𝑎𝑚 𝑐𝑎𝑚𝑇𝑜𝑏𝑗
第二讲 附录
逆矩阵的定义
第二讲 附录
逆矩阵的定义

返回