机器人的数学基础齐次变换矩阵及其运算

1

0 0 0
1

1

a′=Trans(Δx, Δy, Δz)a
平移算子
上海电机学院 机械学院
1 0 0 x
Trans(x, y, z) 0 1 0 y 0 0 1 z
0 0 0
1

❖ ① 算子左乘: 表示点的平移是相对固定坐标系进行的坐标变换。
❖ 1.平移的齐次变换
❖ 空间某一点在直角坐标系中的平移,由 A(x, y, z)平移至A′(x′, y′, z′), 即
x' x x y' y y z' z z
x' 1 0 0 x x

y'

0
1
0
y

y
z' 0 0 1 z z
实际上，它与其他变换之间的关系类似于空间尺寸链， GTT 则是封闭环。如图所示，工具坐标系{T}相对于基座坐标系{B} 的描述可用两种变换矩阵的乘积来表示：
BTT WBT WTT BTT BST GST GTT
令上面两式相等，则得变换方程
WBT WTT BST GST GTT
上海电机学院 机械学院
BAR

A A
R

A B
R

B B
R

ny
oy
k
y


sin

cos
0 ny
oy
k
y

nz oz kz 0
0 1 nz oz kz
nx ny
ox oy
kx cos
k
y


sin

sin cos
0 0
T-1 =
nx ny nz - p.n ox oy oz - p.o ax ay az - p.a 000 1
式中的 “ . ” 表示向量的点积。
上海电机学院 机械学院
计算T矩阵的逆矩阵。
0.5 0 0.866 3
T 0.866 0 -0.5 2 0 1 0 5

0
0
{T}代表工具坐标系； {S}代表工作站坐标系； {G}代表目标坐标系； 它们之间的位姿关系用相应
BST 描述工作站坐标系相对于基座的位姿； GST 描述目标坐标系相对于{S}的位姿； WBT 描述腕部{W}相对于基座{B}的位姿；
的齐次变换来描述。
………………
上海电机学院 机械学院
对物体进行操作时，工具坐标系{T}相对于目标坐标系{G}的位 姿 GTT 直接影响操作效果。它是机器人控制和规划的目标。

0
0
1
5

0.866 0.5 0 1.598

0
00
1

上海电机学院 机械学院
6 变换方程
为了描述机器人的操作，必须 建立机器人本身各连杆之间， 机器人与周围环境之间的运算 关系。为此要规定各种坐标系 来描述机器人与环境的相对位 姿关系。
{B}代表基座坐标系；
{W}代表腕部坐标系；
ABT 表示坐标系{C}从 CBT 映射为 CAT 的变换。
(2)坐标系{C}相对于{A}的描述 CAT 是这样得到的：最初{C} 与{A}重合，首先相对于{A}作运动 ABT ，到达{B},然后相 对{B}作运动 CBT ，到达最终位置{C}。
上海电机学院 机械学院
5.变换矩阵求逆
如果知道坐标系｛B｝相对于｛A｝的描述。希望得到｛A｝相对 于｛B｝的描述，这是个齐次变换求逆问题。 求逆问题可以描述为：已知 ABT ，求解 BAT 。
❖ 2．旋转的齐次变换
❖ 点在空间直角坐标系中的旋转如图所示。A(x, y, z)绕Z轴旋 转θ角后至A’(x’, y’, z’),则A与A’之间的关系为 ：
x' x cos y sin
y'

x
sin


y
c os

z' z

x' cos sin 0 0 x


n
y
oy
ay
p
y


nz 0
oz 0
az 0
pz 1

上海电机学院 机械学院
变换可定义为空间的一个运动。 已知一直角坐标系中的某点坐标，那么该点在另一直角坐标系中的 坐标可通过齐次坐标变换来求得。 变换可分为如下形式：
纯平移 纯旋转 平移与旋转的结合
上海电机学院 机械学院
0 1

0
0 0 1
上海电机学院 机械学院
如图所示单操作手臂，并且手腕 也具有一个旋转自由度。已知手 部的起始位姿矩阵为G1.
若手臂绕Z0轴旋转90°，则手臂 到达G2；若手臂不动，仅手部绕 手腕Z1轴转90°，则手部到达 G3.写出手部坐标系G2、G3表达 式。
上海电机学院 机械学院
onxx
ny oy
nz oz
变换方程中的任一变换矩阵都可用其余的变换矩阵来表 示。例如，为了对目标物进行有效操作，工具坐标系{T}相对于 目标坐标系{G}的位姿 GTT 是预先规定的，需要改变 WBT 以达到 这一目的，即通常规定 GTT ，求 WBT 。
根据变换方程，可以立即求出
WBT BST GST GTT WTT 1
E Trans(4,3,7)Rot(y,90)Rot(z,90)u
上海电机学院 机械学院
上海电机学院 机械学院
齐次变换矩阵 ABT 的数学意义： （1）同一点在不同坐标系{B}和{A}中的变换； （2）描述坐标系{B}相对于坐标系{A}的位置和方位； （3）点的运动算子。
A p ABT Bp
0
0 0 1 0

0
0 0 1
同理，绕x轴、Y轴旋转算子内容为：
1 0
0 0
cos 0 sin 0
Rot(x, ) 0
0
c os s in
sin c os
0 0

Rot( y, )
0
sin
1 0
0
c os
0 0
0 0
0 0 1 1
ABT

1 0
0 1
0 0
3 4
0 0 0
1

1 Trans(x, y, z) 0
0 0
0 1 0 0
0 0 1 0
x
y
z
1

cos
Rot(
z,
)

sin 0

0
sin cos
0 0
0 0 1 0
B pA0 ABR ApB0 BART ApB0
BAT


A B
R 0
T

R A T A
B
1
pB
0

上海电机学院 机械学院
下面我们写出变换矩阵的一般表达形式
nx ox ax px ny oy ay py T = nz oz az pz 0001
式中 n, o, a 是旋转变换列向量，p 是平移向量，其逆是
AxA
zAA
zB
k
zB

z

A
yB yA
B xB
xA
xB
yB y 上海电机学院 机A械学院
坐标系{B}绕k轴相对于{A}旋转θ角相当于：坐标系{B’}相对于
{A’}的z轴旋转θ角，保持其他关系不变。则 坐标系{A}经过如下变换到坐标系{B}:
A
BA R

A A
R

上海电机学院 机械学院
旋转变换通式
问题描述：
令 k kxi k y j kzk 是过原点的单位矢量，求绕k旋转
θ角的旋转矩阵R(k,θ)。
令
A B
R

Rot (k ,
)
即R(k,θ)表示坐标系{B}相对于参考系{A}的方位。
坐标系{B}由坐标系{A}绕 k 轴旋转 角得到。
{A} k
表示｛C｝相对于｛A｝的描述，是两变换矩阵的乘积。
注意：变换矩阵相乘不满足“交换律”，变换矩阵的左乘 和右乘的运动解释不同。
上海电机学院 机械学院
CAT ABT CBT


A B
R
0
A
pB0 1


B C
R
0
B
pC 1
0

复合变换可解释为： (1)CAT 和 CBT 分别代表同一坐标系{C}相对于{A}和{B}的描述。则
❖ ② 算子右乘: 表示点的平移是相对动坐标系进行的坐标变换。
❖ ③ 该公式亦适用于坐标系的平移变换、 物体的平移变换, 如机 器人手部的平移变换。
上海电机学院 机械学院
❖ 例 动坐标系{A}相对于固定坐标系的X0、Y0、Z0轴作
❖ (-1,2,2)平移后到{A’}；动坐标系{A}相对于自身坐标系(即动系) 的X、Y、Z轴分别作(-1,2,2)平移后到{A’’}。已知A,写出坐标系 {A’}、 {A’’}
上海电机学院 机械学院
3．复合齐次变换
复合变换是由固 定参考坐标系或 当前运动坐标系 的一系列沿轴平 移和绕轴旋转变 换所组成的。任 何变换都可以分 解为按一定顺序 的一组平移和旋 转变换。
相对于固定坐标系
算子左乘
相对于动坐标系
算子右乘
上海电机学院 机械学院
❖ 已知坐标系中点U的位置矢量 u 7 3 2 1，T 将此点绕Z轴 旋转90°，再绕Y轴旋转90°，如图所示，求旋转变换后 所得的点W。
上海电机学院 机械学院
旋转变换通式
再定义两坐标系{A’}和 {B’}，分别与{A}和{B}固接，但要求
(1){A’}和 {B’}的z轴与k重合。
(2)旋转之前{A’}和 {B’}重合， {A}和{B}也重合。
B
nx ox kx
A A
R

B B
R


ny
oy
k
y

nz oz kz
机器人学基础
——齐次变换矩阵及其运算
LOGO
上海电机学院 机械学院
齐次变换矩阵及其运算
由于各种原因，变换矩 阵应写成方型形式，3*3 或4*4均可.
为保证所表示的矩阵为 方阵，如果在同一矩阵 中既表示姿态又表示位 置，那么可在矩阵中加 入比例因子使之成为4*4 矩阵。
nx ox ax px
F
0 0 0 1
上海电机学院 机械学院
CAT ABT CBT
4．变换矩阵相乘
对于给定的坐标系｛A｝、｛B｝、｛C｝，已知｛B｝相对 ｛A｝的描述为 ABT ，｛C｝相对｛B｝的描述为 CBT ，则
B pCBT Cp
A p ABT Bp ABT CBT Cp
从而定义复合变换
。
CAT ABT CBT

y'


sin

cos
0
0

y
z' 0
0 1 0 z
1


0
0
0
1
1

记为: a′=Rot(z, θ)a
旋转算子
上海电机学院 机械学院
绕Z轴旋转算子内容为：
cos sin 0 0
Rot(z,
)

sin
0
cos
0 1 0 1
A

1 0
0 0
0 1 1 1

0
0
0 1
0 1 0 0
A' 1 0 0 3 0 0 1 3

0
0
0 1
0 1 0 1
A'' 1 0
0
2

0 0 1 1

0
0
0
1

上海电机学院 机械学院
R A
B

BB R
B
zB
又因为
A B
R

cos

sin

0
sin cos
0
B' B
R
R B T
B'
0 0 1 AxA
B xB
所以可以得到：
xA
xB
zAA
k
zB

z

A
yB yA
yB y 上海电机学院 机A械学院
nx ox kx cos sin 0 nx ox kx 1
•对4*4矩阵直接求逆； •利用齐次变换矩阵的特点，简化矩阵求逆运算。
ABT


A B
R
0
A
pB 1
0

BAT


B A
R
0
利用旋转矩阵正交性
A B
R
1

BART
B
pA0 1

利用复合变换公式(2.13) ,求出 A pB0 在{B}中描述。
上海电机学院 机械学院
B ( ApB0 ) ABR ApB0 BpA0 0
W Rot(Y,90)Rot(Z,90)U
0 0 1 0 0 1 0 0 7

0
1
0
0

1
0
0
0
wk.baidu.com3

1 0 0 0 0 0 1 0 2

0
0
0
1 0
0
0
1
1

上海电机学院 机械学院
❖ 平移变换和旋转变换可以组合在一个齐次变换中。上例 中点U若还要作4i-3j+7k的平移，则只要左乘上平移变换 算子即可得到最后的列阵表达式。
0
1
0.5 0.866 0 (3 0.5 2 0.866 5 0)
T 1
0
01
(3 0 2 0 51)

0.866 0.5 0 (3 0.866 2 0.5 5 0)

0
00
1

0.5 0.866 0 3.23