前面的文章主要介绍了旋转矩阵,对于刚体的运动,除了旋转外还有平移。在机器人及自动驾驶中,经常用齐次变换矩阵将旋转和平移进行统一。 前面的文章也介绍过齐次变换矩阵,本文算是一个总结。
1. SE(3)
将旋转矩阵和平移向量写在同一个矩阵中,形成的
4
×
4
4\times4
4×4矩阵,称为special Euclidean group,即
S
E
(
3
)
SE(3)
SE(3),
T
=
[
R
p
0
1
]
=
[
r
11
r
12
r
13
p
1
r
21
r
22
r
23
p
2
r
31
r
32
r
33
p
3
0
0
0
1
]
(1)
T=\left[\begin{array}{ll} R & p \\ 0 & 1 \end{array}\right]=\left[\begin{array}{llll} r_{11} & r_{12} & r_{13} & p_{1} \\ r_{21} & r_{22} & r_{23} & p_{2} \\ r_{31} & r_{32} & r_{33} & p_{3} \\ 0 & 0 & 0 & 1 \end{array}\right] \tag{1}
T=[R0p1]=⎣⎢⎢⎡r11r21r310r12r22r320r13r23r330p1p2p31⎦⎥⎥⎤(1)
很容易验证,齐次变换矩阵满足群所具有的性质,即封闭性,结合律,幺元,逆,所以称其为group是合理的。
T
−
1
=
[
R
p
0
1
]
−
1
=
[
R
T
−
R
T
p
0
1
]
(2)
T^{-1}=\left[\begin{array}{cc} R & p \\ 0 & 1 \end{array}\right]^{-1}=\left[\begin{array}{cc} R^{\mathrm{T}} & -R^{\mathrm{T}} p \\ 0 & 1 \end{array}\right] \tag{2}
T−1=[R0p1]−1=[RT0−RTp1](2)
(
T
1
T
2
)
T
3
=
T
1
(
T
2
T
3
)
(3)
(T_1T_2)T_3=T_1(T_2T_3) \tag{3}
(T1T2)T3=T1(T2T3)(3)
此外齐次变换矩阵还能保持变换前后的距离和角度不变,假定
T
∈
S
E
(
3
)
T\in SE(3)
T∈SE(3),同时
x
,
y
,
z
∈
R
3
x,y,z \in \mathbb{R}^3
x,y,z∈R3,则有,
∥
T
x
−
T
y
∥
=
∥
x
−
y
∥
(4)
\|T x-T y\|=\|x-y\| \tag{4}
∥Tx−Ty∥=∥x−y∥(4)
⟨
T
x
−
T
z
,
T
y
−
T
z
⟩
=
⟨
x
−
z
,
y
−
z
⟩
(5)
\langle T x-T z, T y-T z\rangle=\langle x-z, y-z\rangle \tag{5}
⟨Tx−Tz,Ty−Tz⟩=⟨x−z,y−z⟩(5)
2. 齐次变换矩阵的用法
2.1 描述坐标系
如上图所示,
v
b
=
(
0
,
0
,
1.5
)
v_b=(0,0,1.5)
vb=(0,0,1.5),假设fixed frame为
{
S
}
\{S\}
{S},
{
a
}
\{a\}
{a}和
{
S
}
\{S\}
{S}重合,则
{
a
}
\{a\}
{a},
{
b
}
\{b\}
{b},
{
c
}
\{c\}
{c}可以描述为:
T
s
a
=
(
R
s
a
,
p
s
a
)
T
s
b
=
(
R
s
b
,
p
s
b
)
T
s
c
=
(
R
s
c
,
p
s
c
)
(4)
\begin{aligned} T_{sa}&=(R_{sa},p_{sa}) \\ T_{sb}&=(R_{sb},p_{sb}) \\ T_{sc}&=(R_{sc},p_{sc}) \end{aligned} \tag{4}
TsaTsbTsc=(Rsa,psa)=(Rsb,psb)=(Rsc,psc)(4)
其中,
R
s
a
=
[
1
0
0
0
1
0
0
0
1
]
,
R
s
b
=
[
0
0
1
0
−
1
0
1
0
0
]
,
R
s
c
=
[
−
1
0
0
0
0
1
0
1
0
]
R_{s a}=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right], \quad R_{s b}=\left[\begin{array}{rrr} 0 & 0 & 1 \\ 0 & -1 & 0 \\ 1 & 0 & 0 \end{array}\right], \quad R_{s c}=\left[\begin{array}{rrr} -1 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & 1 & 0 \end{array}\right]
Rsa=⎣⎡100010001⎦⎤,Rsb=⎣⎡0010−10100⎦⎤,Rsc=⎣⎡−100001010⎦⎤
p s a = [ 0 0 0 ] , p s b = [ 0 − 2 0 ] , p s c = [ − 1 1 0 ] p_{s a}=\left[\begin{array}{c} 0 \\ 0 \\ 0 \end{array}\right], \quad p_{s b}=\left[\begin{array}{c} 0 \\ -2 \\ 0 \end{array}\right], \quad p_{s c}=\left[\begin{array}{c} -1 \\ 1 \\ 0 \end{array}\right] psa=⎣⎡000⎦⎤,psb=⎣⎡0−20⎦⎤,psc=⎣⎡−110⎦⎤
2.2 向量(坐标系)在不同坐标系下的描述
对于任意三个坐标系, v v v在 { b } \{b\} {b}下为 v b v_b vb,
T
a
b
T
b
c
=
T
a
b̸
T
b̸
c
=
T
a
c
T
a
b
v
b
=
T
a
b
v
b
=
v
a
(5)
\begin{aligned} T_{a b} T_{b c} &=T_{a \not {b}} T_{\not {b} c}=T_{a c} \\ T_{a b} v_{b} &=T_{a b} v_{b}=v_{a} \end{aligned} \tag{5}
TabTbcTabvb=TabTbc=Tac=Tabvb=va(5)
v
v
v在
{
a
}
\{a\}
{a}下为
v
a
v_a
va。
2.3 对向量(坐标系)进行平移和旋转
可以将齐次变换矩阵写为公式(6),这里将
Rot
(
ω
^
,
θ
)
\operatorname{Rot}(\hat{\omega}, \theta)
Rot(ω^,θ)改写为齐次形式,为了便于标记,
T
=
Trans
(
p
)
Rot
(
ω
^
,
θ
)
=
[
1
0
0
p
x
0
1
0
p
y
0
0
1
p
z
0
0
0
1
]
[
R
0
0
1
]
(6)
\begin{aligned} T&=\operatorname{Trans}(p)\operatorname{Rot}(\hat{\omega}, \theta)\\ &=\left[\begin{array}{llll} 1 & 0 & 0 & p_{x} \\ 0 & 1 & 0 & p_{y} \\ 0 & 0 & 1 & p_{z} \\ 0 & 0 & 0 & 1 \end{array}\right]\left[\begin{array}{ll} R & 0 \\ 0 & 1 \end{array}\right] \end{aligned} \tag{6}
T=Trans(p)Rot(ω^,θ)=⎣⎢⎢⎡100001000010pxpypz1⎦⎥⎥⎤[R001](6)
假设body frame { b } \{b\} {b}相对于fixed frame { S } \{S\} {S}的描述为 T s b T_{sb} Tsb,则下面讨论左乘和右乘 T T T的区别,
T
s
b
′
=
T
T
s
b
=
Trans
(
p
)
Rot
(
ω
^
,
θ
)
T
s
b
(fixed frame)
=
[
R
p
0
1
]
[
R
s
b
p
s
b
0
1
]
=
[
R
R
s
b
R
p
s
b
+
p
0
1
]
(7)
\begin{aligned} &T_{s b^{\prime}}=T T_{s b}=\operatorname{Trans}(p) \operatorname{Rot}(\hat{\omega}, \theta) T_{s b} \quad \text { (fixed frame) }\\ &=\left[\begin{array}{cc} R & p \\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} R_{s b} & p_{s b} \\ 0 & 1 \end{array}\right]=\left[\begin{array}{cc} R R_{s b} & R p_{s b}+p \\ 0 & 1 \end{array}\right] \end{aligned} \tag{7}
Tsb′=TTsb=Trans(p)Rot(ω^,θ)Tsb (fixed frame) =[R0p1][Rsb0psb1]=[RRsb0Rpsb+p1](7)
T
s
b
′
′
=
T
s
b
T
=
T
s
b
Trans
(
p
)
Rot
(
ω
^
,
θ
)
(body frame)
=
[
R
s
b
p
s
b
0
1
]
[
R
p
0
1
]
=
[
R
s
b
R
R
s
b
p
+
p
s
b
0
1
]
(8)
\begin{aligned} &T_{s b^{\prime \prime}}=T_{s b} T=T_{s b} \operatorname{Trans}(p) \operatorname{Rot}(\hat{\omega}, \theta) \quad \text { (body frame) }\\ &=\left[\begin{array}{cc} R_{s b} & p_{s b} \\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} R & p \\ 0 & 1 \end{array}\right]=\left[\begin{array}{cc} R_{s b} R & R_{s b} p+p_{s b} \\ 0 & 1 \end{array}\right] \end{aligned} \tag{8}
Tsb′′=TsbT=TsbTrans(p)Rot(ω^,θ) (body frame) =[Rsb0psb1][R0p1]=[RsbR0Rsbp+psb1](8)
以下图为例,假设
ω
^
=
(
0
,
0
,
1
)
\hat{\omega}=(0,0,1)
ω^=(0,0,1),
θ
=
9
0
ο
\theta=90^{\operatorname {\omicron}}
θ=90ο,
p
=
(
0
,
2
,
0
)
p=(0,2,0)
p=(0,2,0),分析左乘和右乘的区别,
首先是左乘,如上图左边所示,根据公式(7):
首先分析
Rot
(
ω
^
,
θ
)
T
s
b
\operatorname{Rot}(\hat{\omega},\theta)T_{s b}
Rot(ω^,θ)Tsb,
T
s
b
T_{s b}
Tsb首先以
{
S
}
\{S\}
{S}为基准进行旋转,
R
R
s
b
R R_{s b}
RRsb为绕着
z
^
s
\hat z_s
z^s旋转
R
R
R,
R
p
s
b
Rp_{s b}
Rpsb将
p
s
b
p_{s b}
psb按照图中①的方式进行了旋转;
然后再左乘
Trans
(
p
)
\operatorname{Trans}(p)
Trans(p),将①的结果沿着
y
s
^
\hat{y_s}
ys^移动2个单位,即得到图中②最终的结果。
其次是右乘,如上图右边所示,根据公式(8):
首先分析
T
s
b
Trans
(
p
)
T_{s b} \operatorname{Trans}(p)
TsbTrans(p),
T
s
b
T_{s b}
Tsb首先以
{
b
}
\{b\}
{b}为基准进行平移,即沿着
y
b
^
\hat{y_b}
yb^平移2个单位,
R
s
b
p
+
p
s
b
R_{s b} p+p_{s b}
Rsbp+psb,即得到图中的①;
然后再右乘
Rot
(
ω
^
,
θ
)
\operatorname{Rot}(\hat{\omega}, \theta)
Rot(ω^,θ),以
{
b
}
\{b\}
{b}为基准进行旋转,即沿着
z
b
^
\hat{z_b}
zb^旋转
θ
=
9
0
ο
\theta=90^{\operatorname {\omicron}}
θ=90ο,得到图中②的结果。
综上所述,
左乘齐次矩阵,首先以
{
S
}
\{S\}
{S}为基准进行旋转,然后以
{
S
}
\{S\}
{S}为基准进行平移;
右乘齐次矩阵,首先以
{
b
}
\{b\}
{b}为基准进行平移,然后以
{
b
}
\{b\}
{b}为基准进行旋转;