变换矩阵
坐标系描述
用
A
p
B
o
^Ap_{Bo}
ApBo表示在参考坐标系
{
A
}
\{A\}
{A}下坐标系
{
B
}
\{B\}
{B}的原点坐标，并用旋转矩阵
B
A
R
^A_BR
BAR描述坐标系
{
B
}
\{B\}
{B}在参考坐标系
{
A
}
\{A\}
{A}下的姿态，则坐标系
{
B
}
\{B\}
{B}的姿态可表示为：
{
B
}
=
{
A
p
B
o
B
A
R
}
\{B\}=\{\enspace^Ap_{Bo}\quad{}^A_BR\enspace\}
{B}={ApBoBAR}
坐标系的描述概括了刚体的位置和姿态的描述，当表示位置时，旋转矩阵
B
A
R
=
I
{}^A_BR=I
BAR=I；当表示姿态时，位移矢量
A
p
B
o
=
0
^Ap_{Bo}=0
ApBo=0
平移映射
当两坐标系具有相同的方位，当坐标原点不重合时，可用位移矢量 A p B o ^Ap_{Bo} ApBo描述坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的位置：
称
A
p
B
o
^Ap_{Bo}
ApBo为坐标系
{
B
}
\{B\}
{B}相对于
{
A
}
\{A\}
{A}的平移矢量，若点
p
p
p在坐标系
{
B
}
\{B\}
{B}下的位置为
B
p
^Bp
Bp，则它相对于坐标系
{
A
}
\{A\}
{A}的位置
A
p
^Ap
Ap可表示为：
A
p
=
B
p
+
A
p
B
o
{}^Ap={}^Bp+{}^Ap_{Bo}
Ap=Bp+ApBo
旋转映射
当两坐标系原点重合，但方位不同时，可用旋转矩阵 B A R {}^A_BR BAR描述坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的姿态：
称
B
A
R
{}^A_BR
BAR为坐标系
{
B
}
\{B\}
{B}相对于
{
A
}
\{A\}
{A}的方位，若点
p
p
p在坐标系
{
B
}
\{B\}
{B}下的位置为
B
p
^Bp
Bp，则它相对于坐标系
{
A
}
\{A\}
{A}的位置
A
p
^Ap
Ap可表示为：
A
p
=
B
A
R
B
p
{}^Ap={}^A_BR\:{}^Bp
Ap=BARBp
同样，用
A
B
R
{}^B_AR
ABR表示坐标系
{
A
}
\{A\}
{A}相对于
{
B
}
\{B\}
{B}的方位，则可由旋转矩阵的正交约束得：
A
B
R
=
B
A
R
−
1
=
B
A
R
T
{}^B_AR={}^A_BR^{-1}={}^A_BR^T
ABR=BAR−1=BART
一般刚体变换
在三维空间中，最常见的情况如下：坐标系 { B } \{B\} {B}与 { A } \{A\} {A}不但原点不重合，同时姿态也不相同。此时采用位移矢量 A p B o ^Ap_{Bo} ApBo描述坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的位置，用旋转矩阵 B A R {}^A_BR BAR描述坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的姿态：
则若点
p
p
p在坐标系
{
B
}
\{B\}
{B}下的位置为
B
p
^Bp
Bp，则它相对于坐标系
{
A
}
\{A\}
{A}的位置
A
p
^Ap
Ap可表示为：
A
p
=
B
A
R
B
p
+
A
p
B
o
{}^Ap={}^A_BR\:{}^Bp+{}^Ap_{Bo}
Ap=BARBp+ApBo
也即，为旋转映射与平移映射的复合。先将坐标系
{
B
}
\{B\}
{B}根据旋转矩阵进行旋转，在沿着位移矢量进行平移。
将上式进行齐次变换，得到变换矩阵
B
A
T
{}^A_BT
BAT：
[
A
p
1
]
=
[
B
A
R
A
p
B
o
0
1
]
[
B
p
1
]
=
B
A
T
B
p
\begin{bmatrix}{}^Ap\\1\end{bmatrix}=\begin{bmatrix}{}^A_BR&{}^Ap_{Bo}\\0&1\end{bmatrix}\begin{bmatrix}{}^Bp\\1\end{bmatrix}={}^A_BT\:{}^Bp
[Ap1]=[BAR0ApBo1][Bp1]=BATBp
其中齐次变换矩阵
B
A
T
{}^A_BT
BAT为
4
×
4
4\times4
4×4的矩阵，它表示了由坐标系
{
B
}
\{B\}
{B}到坐标系
{
A
}
\{A\}
{A}的变换关系：
B
A
T
=
[
B
A
R
A
p
B
o
0
1
]
{}^A_BT=\begin{bmatrix}{}^A_BR&{}^Ap_{Bo}\\0&1\end{bmatrix}
BAT=[BAR0ApBo1]
齐次坐标
若一点的
p
p
p的坐标为
[
x
y
z
]
T
\begin{bmatrix}x&y&z\end{bmatrix}^T
[xyz]T，则它的齐次坐标表示如下，齐次坐标并不唯一，可将隔行同乘一个常数
ω
\omega
ω表示的仍为同一坐标：
p
=
[
x
y
z
1
]
=
[
ω
x
ω
y
ω
z
ω
]
p=\begin{bmatrix}x\\y\\z\\1\end{bmatrix}=\begin{bmatrix}\omega x\\\omega y\\\omega z\\\omega\end{bmatrix}
p=⎣⎢⎢⎡xyz1⎦⎥⎥⎤=⎣⎢⎢⎡ωxωyωzω⎦⎥⎥⎤
同时，规定坐标
[
0
0
0
0
]
T
\begin{bmatrix}0&0&0&0\end{bmatrix}^T
[0000]T无意义，各个轴的无穷远处如下表示：
X
轴
:
[
1
0
0
0
]
Y
轴
:
[
0
1
0
0
]
Z
轴
:
[
0
0
1
0
]
任
意
无
穷
远
处
:
[
a
b
c
0
]
X轴:\begin{bmatrix}1\\0\\0\\0\end{bmatrix}\quad Y轴:\begin{bmatrix}0\\1\\0\\0\end{bmatrix}\quad Z轴:\begin{bmatrix}0\\0\\1\\0\end{bmatrix}\quad 任意无穷远处: \begin{bmatrix}a\\b\\c\\0\end{bmatrix}
X轴:⎣⎢⎢⎡1000⎦⎥⎥⎤Y轴:⎣⎢⎢⎡0100⎦⎥⎥⎤Z轴:⎣⎢⎢⎡0010⎦⎥⎥⎤任意无穷远处:⎣⎢⎢⎡abc0⎦⎥⎥⎤
变换矩阵运算与刚体群
运动算子
变换矩阵
B
A
T
{}^A_BT
BAT可分解为两个矩阵相乘的形式，其中用
T
r
a
n
s
(
A
p
B
o
)
Trans({}^Ap_{Bo})
Trans(ApBo)表示平移变换矩阵，用
R
o
t
(
k
,
θ
)
Rot(k,\theta)
Rot(k,θ)旋转变换矩阵：
B
A
T
=
[
B
A
R
A
p
B
o
0
1
]
=
[
I
3
×
3
A
p
B
o
0
1
]
[
B
A
R
(
k
,
θ
)
0
0
1
]
=
T
r
a
n
s
(
A
p
B
o
)
R
o
t
(
k
,
θ
)
\begin{aligned} {}^A_BT =&\begin{bmatrix}{}^A_BR&{}^Ap_{Bo}\\0&1\end{bmatrix}\\ =&\begin{bmatrix}I_{3\times3}&{}^Ap_{Bo}\\0&1\end{bmatrix}\begin{bmatrix}{}^A_BR(k,\theta)&0\\0&1\end{bmatrix}\\ =&Trans({}^Ap_{Bo})Rot(k,\theta) \end{aligned}
BAT===[BAR0ApBo1][I3×30ApBo1][BAR(k,θ)001]Trans(ApBo)Rot(k,θ)
T
r
a
n
s
(
A
p
B
o
)
Trans({}^Ap_{Bo})
Trans(ApBo)表示沿着位移矢量
A
p
B
o
{}^Ap_{Bo}
ApBo进行平移，
R
o
t
(
k
,
θ
)
Rot(k,\theta)
Rot(k,θ)则表示绕着过原点的轴
k
k
k旋转
θ
\theta
θ角。
平移算子
相对坐标系
{
A
}
\{A\}
{A}，位移矢量
A
p
1
{}^Ap_1
Ap1沿着
A
p
{}^Ap
Ap移动至位移矢量
A
p
2
{}^Ap_2
Ap2可用矢量相加表示：
A
p
2
=
A
p
1
+
A
p
{}^Ap_2={}^Ap_1+{}^Ap
Ap2=Ap1+Ap
将其写为算子形式如下：
A
p
2
=
T
r
a
n
s
(
A
p
)
A
p
1
{}^Ap_2=Trans({}^Ap)\:{}^Ap_1
Ap2=Trans(Ap)Ap1
平移算子为
T
r
a
n
s
(
A
p
)
Trans({}^Ap)
Trans(Ap)表示沿着位移矢量
A
p
{}^Ap
Ap的大小和方向进行平移。
旋转算子
相对坐标系
{
A
}
\{A\}
{A}，某点由
A
p
1
{}^Ap_1
Ap1旋转至
A
p
2
{}^Ap_2
Ap2可表示为：
A
p
2
=
R
A
p
1
{}^Ap_2=R\:{}^Ap_1
Ap2=RAp1
将其写为算子形式如下：
A
p
2
=
R
o
t
(
k
,
θ
)
A
p
1
{}^Ap_2=Rot(k,\theta)\:{}^Ap_1
Ap2=Rot(k,θ)Ap1
旋转算子
R
o
t
(
k
,
θ
)
Rot(k,\theta)
Rot(k,θ)表示沿着过原点的轴
k
k
k旋转角度
θ
\theta
θ。
运动算子一般形式
齐次变换矩阵可用作为运动算子，描述某点在坐标系内的运动（包括平移和旋转）。当变换矩阵作为运动算子使用时，不带上、下标。
例如，质点
p
p
p在坐标系
{
A
}
\{A\}
{A}中的运动轨迹为时间
t
t
t的函数
A
p
(
t
)
{}^Ap(t)
Ap(t)，其初始位置为
A
p
(
0
)
{}^Ap(0)
Ap(0)，则用运动算子表示该质点的运动轨迹如下：
A
p
(
t
)
=
T
(
t
)
A
p
(
0
)
{}^Ap(t)=T(t)\:{}^Ap(0)
Ap(t)=T(t)Ap(0)
变换矩阵的运算
变换矩阵乘法
给定变换矩阵
B
A
T
、
C
B
T
{}^A_BT、{}^B_CT
BAT、CBT，则可以得到变换矩阵
C
A
T
{}^A_CT
CAT：
C
A
T
=
B
A
T
C
B
T
=
[
B
A
R
C
B
R
B
A
R
B
p
C
o
+
A
p
B
o
0
1
]
{}^A_CT={}^A_BT\:{}^B_CT=\begin{bmatrix}{}^A_BR\:{}^B_CR&{}^A_BR\:{}^Bp_{Co}+{}^Ap_{Bo}\\0&1\end{bmatrix}
CAT=BATCBT=[BARCBR0BARBpCo+ApBo1]
也即，坐标系
{
C
}
\{C\}
{C}在坐标系
{
A
}
\{A\}
{A}中的位姿，可以拆为两步进行：先将坐标系
{
C
}
\{C\}
{C}转换至坐标系
{
B
}
\{B\}
{B}中的位姿；再将此位姿根据坐标系
{
B
}
\{B\}
{B}至
{
A
}
\{A\}
{A}的变换进行变换，从而得到最终变换矩阵。
变换矩阵乘法满足如下条件：
- 运动相对固定坐标系而言：满足左乘规则，即变换顺序从右至左
- 运动相对运动坐标系而言：满足右乘规则，即变换顺序从左至右
- 变换的顺序不能调换进行
例如，对于一个变换矩阵 B A T = T r a n s ( 1 − 3 4 ) R o t ( y , 90 ° ) R o t ( z , 90 ° ) {}^A_BT=Trans(\begin{matrix}1&-3&4\end{matrix})\:Rot(y,90\degree)\:Rot(z,90\degree) BAT=Trans(1−34)Rot(y,90°)Rot(z,90°)
若相对固定坐标系（坐标系 { A } \{A\} {A}）而言，采用左乘规则进行：首先绕 z A z_A zA轴旋转 90 ° 90\degree 90°，再绕 y A y_A yA轴旋转 90 ° 90\degree 90°，最后相对 { A } \{A\} {A}平移 [ 1 − 3 4 ] T \begin{bmatrix}1&-3&4\end{bmatrix}^T [1−34]T即可。
若相对于运动坐标系（坐标系 { B } \{B\} {B}）而言，采用右乘规则进行：首先相对坐标系 { A } \{A\} {A}平移 [ 1 − 3 4 ] T \begin{bmatrix}1&-3&4\end{bmatrix}^T [1−34]T，再绕 y B y_B yB轴旋转 90 ° 90\degree 90°，最后绕 z B z_B zB轴旋转 90 ° 90\degree 90°即可。
变换矩阵求逆
若已知变换矩阵 B A T {}^A_BT BAT，此时需要求解坐标系 { A } \{A\} {A}相对于坐标系 { B } \{B\} {B}的变换 A B T {}^B_AT ABT，则需要对变换矩阵进行求逆。
此处利用齐次变换矩阵特点，简化计算：已知 B A T {}^A_BT BAT求解 A B T {}^B_AT ABT，只需要由 B A R {}^A_BR BAR和 A p B o {}^Ap_{Bo} ApBo求解 A B R {}^B_AR ABR和 B p A o {}^Bp_{Ao} BpAo即可。
首先，通过旋转矩阵正交约束知：
A
B
R
=
B
A
R
−
1
=
B
A
R
T
{}^B_AR={}^A_BR^{-1}={}^A_BR^T
ABR=BAR−1=BART
随后，根据平移映射关系，可知在坐标系
{
A
}
\{A\}
{A}下坐标系
{
B
}
\{B\}
{B}的原点
A
p
B
o
{}^Ap_{Bo}
ApBo，在坐标系
{
B
}
\{B\}
{B}的坐标
B
(
A
p
B
o
)
{}^B({}^Ap_{Bo})
B(ApBo)：
B
(
A
p
B
o
)
=
A
B
R
A
p
B
o
+
B
p
A
o
{}^B({}^Ap_{Bo})={}^B_AR\:{}^Ap_{Bo}+{}^Bp_{Ao}
B(ApBo)=ABRApBo+BpAo
已知，在坐标系
{
B
}
\{B\}
{B}中，点
B
o
Bo
Bo为原点，即上式右侧为零：
B
p
A
o
=
−
A
B
R
A
p
B
o
=
−
B
A
R
T
A
p
B
o
{}^Bp_{Ao}=-{}^B_AR\:{}^Ap_{Bo}=-{}^A_BR^T\:{}^Ap_{Bo}
BpAo=−ABRApBo=−BARTApBo
由此得到变换矩阵的逆矩阵：
A
B
T
=
[
B
A
R
T
−
B
A
R
T
A
p
B
o
0
1
]
{}^B_AT=\begin{bmatrix}{}^A_BR^T&-{}^A_BR^T\:{}^Ap_{Bo}\\0&1\end{bmatrix}
ABT=[BART0−BARTApBo1]
易得：
A
B
T
=
B
A
T
−
1
{}^B_AT={}^A_BT^{-1}
ABT=BAT−1
刚体变换群
变换矩阵 T T T由旋转矩阵 R R R和位移矢量 t t t决定，也即任意刚体的位姿由 ( t , R ) : t ∈ ℜ 3 , R ∈ S O ( 3 ) (t,R):t\in\Re^{3},R\in SO(3) (t,R):t∈ℜ3,R∈SO(3)决定。
定义刚体变换群
S
E
(
3
)
SE(3)
SE(3)为乘积空间
ℜ
3
×
S
O
(
3
)
\Re^{3}\times SO(3)
ℜ3×SO(3)：
S
E
(
3
)
=
{
T
=
[
R
t
0
T
1
]
∈
ℜ
4
×
4
∣
R
∈
S
O
(
3
)
,
t
∈
ℜ
3
}
=
ℜ
3
×
S
O
(
3
)
SE(3)=\bigl\{T=\begin{bmatrix}R&t\\0^T&1\end{bmatrix}\in \Re^{4\times4}\big\vert R\in SO(3),t\in\Re^{3}\bigr\}=\Re^{3}\times SO(3)
SE(3)={T=[R0Tt1]∈ℜ4×4∣∣R∈SO(3),t∈ℜ3}=ℜ3×SO(3)
S
E
(
3
)
SE(3)
SE(3)又称为三维空间的特殊欧式群（Special Euclidean Group ），推广到n维空间中可得：
S
E
(
n
)
=
{
T
=
[
R
t
0
T
1
]
∈
ℜ
n
+
1
×
n
+
1
∣
R
∈
S
O
(
n
)
,
t
∈
ℜ
n
}
=
ℜ
n
×
S
O
(
n
)
SE(n)=\bigl\{T=\begin{bmatrix}R&t\\0^T&1\end{bmatrix}\in \Re^{n+1\times n+1}\big\vert R\in SO(n),t\in\Re^{n}\bigr\}=\Re^{n}\times SO(n)
SE(n)={T=[R0Tt1]∈ℜn+1×n+1∣∣R∈SO(n),t∈ℜn}=ℜn×SO(n)
S
E
(
n
)
SE(n)
SE(n)为李群，当n=2时，
S
E
(
2
)
SE(2)
SE(2)表示刚体的平面运动，其单位元为
I
3
I_3
I3；当n=3时，
S
E
(
3
)
SE(3)
SE(3)表示刚体