叉乘与反对称矩阵
a
×
b
=
∣
e
⃗
1
e
⃗
2
e
⃗
3
a
1
a
2
a
3
b
1
b
2
b
3
∣
=
[
a
2
b
3
−
a
3
b
2
a
3
b
1
−
a
1
b
3
a
1
b
2
−
a
2
b
1
]
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
b
=
d
e
f
a
∧
b
\boldsymbol{a}\times \boldsymbol{b}=\left\lvert\begin{matrix} {\vec{\boldsymbol{e}}_1} & {\vec{\boldsymbol{e}}_2} & {\vec{\boldsymbol{e}}_3} \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \end{matrix}\right\rvert= \begin{bmatrix} a_2b_3-a_3b_2\\ a_3b_1-a_1b_3\\ a_1b_2-a_2b_1 \end{bmatrix}= \begin{bmatrix} 0 & -a_3 & a_2\\ a_3 & 0 &-a_1\\ -a_2 & a_1 & 0 \end{bmatrix}\boldsymbol{b}\overset{\rm{def}}{=} \boldsymbol{a}^{\wedge}\boldsymbol{b}
a×b=
e1a1b1e2a2b2e3a3b3
=
a2b3−a3b2a3b1−a1b3a1b2−a2b1
=
0a3−a2−a30a1a2−a10
b=defa∧b
对于外积运算,引入
∧
^\wedge
∧符号,把
a
\boldsymbol{a}
a写成一个反对称矩阵。这样就把外积
a
×
b
\boldsymbol{a}\times\boldsymbol{b}
a×b写成了矩阵与向量的乘法
a
∧
b
\boldsymbol{a}^{\wedge}\boldsymbol{b}
a∧b
李群与李代数基础
群
群是一种集合加上一种运算的代数结构。把集合记作 A A A,运算记作 ⋅ \cdot ⋅,那么群可以记作 G = ( A , ⋅ ) G=(A,\cdot) G=(A,⋅)。群要求这个运算满足以下条件:
- 封闭性: ∀ a 1 , a 2 ∈ A , a 1 ⋅ a 2 ∈ A \forall a_1,a_2\in A,a_1\cdot a_2\in A ∀a1,a2∈A,a1⋅a2∈A
- 结合律: ∀ a 1 , a 2 , a 3 ∈ A . ( a 1 ⋅ a 2 ) ⋅ a 3 = a 1 ⋅ ( a 2 ⋅ a 3 ) \forall a_1,a_2,a_3\in A.(a_1\cdot a_2)\cdot a_3=a_1\cdot(a_2\cdot a_3) ∀a1,a2,a3∈A.(a1⋅a2)⋅a3=a1⋅(a2⋅a3)
- 幺元: ∃ a 0 ∈ A , s . t . ∀ a ∈ A , a 0 ⋅ a = a ⋅ a 0 = a \exists a_0 \in A,{\rm s.t.}\ \forall a\in A,a_0\cdot a=a\cdot a_0=a ∃a0∈A,s.t. ∀a∈A,a0⋅a=a⋅a0=a
- 逆: ∀ a ∈ A , ∃ a − 1 ∈ A , s . t . a ⋅ a − 1 = a 0 \forall a\in A,\exists a^{-1}\in A,{\rm s.t.}\ a\cdot a^{-1}=a_0 ∀a∈A,∃a−1∈A,s.t. a⋅a−1=a0
李群是指具有连续(光滑)性质的群。引入李群李代数是为了解决旋转矩阵不可导的问题: R 1 + R 2 ∉ S O ( 3 ) \boldsymbol{R}_1+\boldsymbol{R}_2\notin SO(3) R1+R2∈/SO(3)。
李代数的定义
每个李群都有与之对应的李代数。李代数描述了李群的局部性质,准确地说,是单位元附近的正切空间。一般的李代数定义如下:
李代数由一个集合
V
\mathbb{V}
V、一个数域
F
\mathbb{F}
F和一个二元运算
[
,
]
[,]
[,]组成。如果它们满足如下几条性质,则称
(
V
,
F
,
[
,
]
)
(\mathbb{V},\mathbb{F},[,])
(V,F,[,])为一个李代数,记作
g
\mathfrak{g}
g
- 封闭性: ∀ X , Y ∈ V , [ X , Y ] ∈ V \forall X,Y\in \mathbb{V},[X,Y]\in \mathbb{V} ∀X,Y∈V,[X,Y]∈V
- 双线性:
∀
X
,
Y
,
Z
∈
V
,
a
,
b
∈
F
\forall X,Y,Z\in \mathbb{V},a,b\in \mathbb{F}
∀X,Y,Z∈V,a,b∈F,有
[ a X + b Y , Z ] = a [ X , Z ] + b [ Y , Z ] , [ Z , a X + b Y ] = a [ Z , X ] + b [ Z , Y ] [aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y] [aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y] - 自反性: ∀ X ∈ V , [ X , X ] = 0 \forall X\in \mathbb{V},[X,X]=0 ∀X∈V,[X,X]=0
- 雅可比等价性: ∀ X , Y , Z ∈ V , [ X , [ Y , Z ] ] + [ Z , [ X , Y ] ] + [ Y , [ Z , X ] ] = 0 \forall X,Y,Z\in\mathbb{V},[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0 ∀X,Y,Z∈V,[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0
其中二元运算被称为李括号。
旋转矩阵与 s o ( 3 ) \mathfrak{so}(3) so(3)
对于旋转矩阵
R
\boldsymbol{R}
R,其为正交矩阵,故
R
(
t
)
R
T
(
t
)
=
I
\boldsymbol{R}\left( t\right) \boldsymbol{R}^{T}\left( t\right) =\bold{I}
R(t)RT(t)=I
等式两边对t求导得
R
˙
(
t
)
R
T
(
t
)
+
R
(
t
)
R
˙
(
t
)
T
=
0
R
˙
(
t
)
R
T
(
t
)
=
−
(
R
˙
(
t
)
R
T
(
t
)
)
T
\dot{\boldsymbol{R}}\left( t\right) \boldsymbol{R}^{T}\left( t\right) +\boldsymbol{R}\left( t\right) \dot{\boldsymbol{R}}\left( t\right)^T =0\\ \dot{\boldsymbol{R}}\left( t\right) \boldsymbol{R}^{T}\left( t\right) =-\left( \dot{\boldsymbol{R}} \left( t\right) \boldsymbol{R}^{T}\left( t\right) \right) ^{T}
R˙(t)RT(t)+R(t)R˙(t)T=0R˙(t)RT(t)=−(R˙(t)RT(t))T
这是反对称矩阵的性质。对于任意反对称矩阵,都能找到唯一与之对应的向量
ω
\bm{\omega}
ω,使得其对应的反对称矩阵
ω
∧
(
t
)
=
R
˙
(
t
)
R
T
(
t
)
ω
∧
=
Ω
=
[
0
−
ω
3
ω
2
ω
3
0
−
ω
1
−
ω
2
ω
1
0
]
Ω
∨
=
ω
{\bm{\omega}^{\wedge}}\left( t\right) =\dot{\boldsymbol{R}}\left( t\right) \boldsymbol{R}^{T}\left( t\right) \\ {\bm{\omega}^{\wedge}}=\Omega=\begin{bmatrix}0& -\omega_3&\omega_2\\\omega_3&0&-\omega_1\\-\omega_2&\omega_1&0\end{bmatrix} \\\Omega^{\vee}=\bm{\omega}
ω∧(t)=R˙(t)RT(t)ω∧=Ω=
0ω3−ω2−ω30ω1ω2−ω10
Ω∨=ω
则
R
˙
(
t
)
=
ω
∧
(
t
)
R
(
t
)
⟹
R
(
t
)
=
e
ω
∧
(
t
−
t
0
)
R
(
t
0
)
\dot{\boldsymbol{R}}\left( t\right) ={\bm{\omega}^{\wedge}}\left( t\right) \boldsymbol{R}\left( t\right) \\ \implies \boldsymbol{R}(t)=e^{{\bm{\omega}^{\wedge}}(t-t_0)}\boldsymbol{R}(t_0)
R˙(t)=ω∧(t)R(t)⟹R(t)=eω∧(t−t0)R(t0)
设
R
(
t
0
)
=
I
\boldsymbol{R}(t_0)=\bold{I}
R(t0)=I,则
R
˙
(
t
0
)
=
ω
∧
(
t
0
)
\dot{\boldsymbol{R}}(t_0)={\bm{\omega}^{\wedge}}(t_0)
R˙(t0)=ω∧(t0)
R
(
t
0
+
d
t
)
≈
I
+
R
˙
(
t
0
)
d
t
=
I
+
ω
∧
(
t
0
)
d
t
\boldsymbol{R}\left( t_{0}+dt\right) \approx \bold{I}+\dot{\boldsymbol{R}}\left( t_{0}\right)dt =\bold{I}+\bm{\omega}^{\wedge}(t_0)dt \\
R(t0+dt)≈I+R˙(t0)dt=I+ω∧(t0)dt
这些反对称矩阵对应的向量就组成了李代数
s
o
(
3
)
=
{
ω
∈
R
3
∣
ω
∧
∈
R
3
×
3
}
so(3)=\{ \bm{\omega}\in \mathbb{R}^3|{\bm{\omega}^{\wedge}}\in \mathbb{R}^{3\times 3}\}
so(3)={ω∈R3∣ω∧∈R3×3}
李群SO(3)为
S
O
(
3
)
=
{
R
∈
R
3
×
3
∣
R
T
R
=
I
,
det
(
R
)
=
+
1
}
SO(3)=\{\boldsymbol{R}\in\mathbb{R}^{3\times 3}\vert \boldsymbol{R}^T\boldsymbol{R}=\bold{I},\det(\boldsymbol{R})=+1\}
SO(3)={R∈R3×3∣RTR=I,det(R)=+1}
由前面的推导公式知道如果R是单位矩阵,那它的导数就是一个反对称矩阵,所以反对称矩阵对应的向量组成的空间,即 so(3),我们称之为在在单位矩阵处的正切空间tangent space.
在此定义下:两个向量
ϕ
1
,
ϕ
2
\bm{\phi}_1,\bm{\phi}_2
ϕ1,ϕ2的李括号为
[
ϕ
1
,
ϕ
2
]
=
(
ϕ
1
∧
ϕ
2
∧
−
ϕ
2
∧
ϕ
1
∧
)
∨
[\bm{\phi}_1,\bm{\phi}_2]=(\bm{\phi}_1^{\wedge}\bm{\phi}_2^{\wedge}-\bm{\phi}_2^{\wedge}\bm{\phi}_1^{\wedge})^{\vee}
[ϕ1,ϕ2]=(ϕ1∧ϕ2∧−ϕ2∧ϕ1∧)∨
指数映射
由前所述
R
˙
(
t
)
=
ω
∧
(
t
)
R
(
t
)
\dot{\boldsymbol{R}}(t)={\bm{\omega}^{\wedge}}(t)\boldsymbol{R}(t)
R˙(t)=ω∧(t)R(t)
令
R
(
0
)
=
I
\boldsymbol{R}(0)=\bold{I}
R(0)=I,求解该微分方程得到
R
(
t
)
=
e
ω
∧
t
\boldsymbol{R}(t)=e^{{\bm{\omega}^{\wedge}}t}
R(t)=eω∧t
可以验证,这个指数函数是旋转矩阵
(
e
ω
∧
t
)
−
1
=
e
−
ω
∧
t
=
e
ω
∧
T
t
=
(
e
ω
∧
t
)
T
\left( e^{{\bm{\omega}^{\wedge}}t}\right) ^{-1}=e^{-{\bm{\omega}^{\wedge}}t}=e^{{\bm{\omega}}^{\wedge T}t}=\left( e^{{\bm{\omega}^{\wedge}}t}\right) ^{T}
(eω∧t)−1=e−ω∧t=eω∧Tt=(eω∧t)T
所以,我们能够说指数映射将反对称矩阵映射到了SO(3):
令
ω
∧
t
=
∥
ω
t
∥
ω
∧
∥
ω
∥
=
θ
a
∧
{\bm{\omega}^{\wedge}}t=\Vert \bm{\omega} t\Vert\dfrac{{\bm{\omega}^{\wedge}}}{\Vert \bm{\omega}\Vert}=\theta {\bm{a}^{\wedge}}
ω∧t=∥ωt∥∥ω∥ω∧=θa∧ ,其中
∥
a
∥
=
1
\Vert \bm{a}\Vert=1
∥a∥=1是单位向量,有如下性质
a
∧
2
=
a
a
T
−
I
a
∧
3
=
−
a
∧
{\bm{a}^{\wedge}}^2=\bm{aa}^T-\bold{I}\\ {\bm{a}^{\wedge}}^3=-{\bm{a}^{\wedge}}\\
a∧2=aaT−Ia∧3=−a∧
指数映射的泰勒展开式能够整理成如下形式:
e
ω
∧
t
=
∑
n
=
0
∞
(
θ
a
∧
)
n
n
!
=
I
+
(
θ
−
θ
3
3
!
+
θ
5
5
!
−
…
)
a
∧
+
(
θ
2
2
!
−
θ
4
4
!
+
θ
6
6
!
−
…
)
a
∧
2
=
I
+
sin
(
θ
)
a
∧
+
(
1
−
cos
(
θ
)
)
a
∧
2
=
cos
(
θ
)
I
+
sin
(
θ
)
a
∧
+
(
1
−
cos
(
θ
)
)
a
a
T
e^{{\bm{\omega}^{\wedge}}t}=\sum ^{\infty }_{n=0}\dfrac{\left( \theta{\bm{a}^{\wedge}}\right) ^{n}}{n!}=\bold{I}+\left(\theta -\dfrac{\theta^{3}}{3!}+\dfrac{\theta^{5}}{5!}-\ldots \right) {\bm{a}^{\wedge}}+\left( \dfrac{\theta^{2}}{2!}-\dfrac{\theta^{4}}{4!}+\dfrac{\theta^{6}}{6!}-\ldots \right) {\bm{a}^{\wedge}}^{2} \\ =\bold{I}+\sin(\theta){\bm{a}^{\wedge}}+\left(1-\cos(\theta)\right){\bm{a}^{\wedge}}^2=\cos(\theta)\bold{I}+\sin(\theta){\bm{a}^{\wedge}}+\left(1-\cos(\theta)\right)\bm{aa}^T
eω∧t=n=0∑∞n!(θa∧)n=I+(θ−3!θ3+5!θ5−…)a∧+(2!θ2−4!θ4+6!θ6−…)a∧2=I+sin(θ)a∧+(1−cos(θ))a∧2=cos(θ)I+sin(θ)a∧+(1−cos(θ))aaT
这和罗德里格斯公式如出一辙
R
=
cos
(
θ
)
I
+
sin
(
θ
)
a
∧
+
(
1
−
cos
(
θ
)
)
a
a
T
\boldsymbol{R}=\cos(\theta)\bold{I}+\sin(\theta){\bm{a}^{\wedge}}+\left(1-\cos(\theta)\right)\bm{aa}^T
R=cos(θ)I+sin(θ)a∧+(1−cos(θ))aaT
这表明,
s
o
(
3
)
so(3)
so(3)实际上就是由所谓的旋转向量组成的空间,而指数映射即罗德里格斯公式
t
r
(
R
)
=
3
cos
(
θ
)
+
3
sin
(
θ
)
t
r
(
a
∧
)
+
(
1
−
cos
(
θ
)
)
t
r
(
a
a
T
)
=
3
cos
(
θ
)
+
0
+
(
1
−
cos
(
∥
ω
∥
)
)
t
r
(
a
T
a
)
=
3
cos
(
θ
)
+
(
1
−
cos
(
θ
)
)
=
1
+
2
cos
(
θ
)
⟹
θ
=
arccos
t
r
(
R
)
−
1
2
{\rm tr}(\boldsymbol{R})=3\cos(\theta)+3\sin(\theta){\rm tr}({\bm{a}^{\wedge}})+(1-\cos(\theta)){\rm tr}(\bm{aa}^T)\\=3\cos(\theta)+0+(1-\cos(\Vert \bm{\omega}\Vert)){\rm tr}(\bm{a}^T\bm{a})\\=3\cos(\theta)+(1-\cos(\theta))=1+2\cos(\theta)\\ \implies \theta=\arccos\dfrac{{\rm tr}(\boldsymbol{R})-1}{2}
tr(R)=3cos(θ)+3sin(θ)tr(a∧)+(1−cos(θ))tr(aaT)=3cos(θ)+0+(1−cos(∥ω∥))tr(aTa)=3cos(θ)+(1−cos(θ))=1+2cos(θ)⟹θ=arccos2tr(R)−1
大写指数映射
大写指数映射将旋转向量so(3)转换为旋转矩阵SO(3):
E
x
p
:
R
3
→
S
O
(
3
)
;
v
→
E
x
p
(
v
)
=
e
v
∧
{\rm Exp}:\mathbb{R} ^{3}\rightarrow SO\left( 3\right) ;{\bf{v}} \rightarrow {\rm Exp}\left( \bf{v}\right) =e^{\bf{v}^{\wedge} }
Exp:R3→SO(3);v→Exp(v)=ev∧
刚体变换与 s e ( 3 ) \mathfrak{se}(3) se(3)
除了旋转,刚体变换还有平移。所以,和只有旋转矩阵构成的李群
S
O
(
3
)
SO(3)
SO(3) 一样,我们也可以有由刚体变换得到的李群
S
E
(
3
)
SE(3)
SE(3)
S
E
(
3
)
=
{
g
=
[
R
T
0
1
]
∈
R
4
×
4
∣
R
∈
S
O
(
3
)
,
T
∈
R
3
}
SE\left( 3\right) =\left\{ \bm{g}=\begin{bmatrix} \boldsymbol{R} &\bold{T} \\ \bold{0} & 1 \end{bmatrix}\in \mathbb{R} ^{4\times 4}{\huge\vert} \boldsymbol{R}\in SO\left( 3\right) ,\bm{T}\in \mathbb{R} ^{3}\right\}
SE(3)={g=[R0T1]∈R4×4∣R∈SO(3),T∈R3}
g
˙
(
t
)
g
−
1
(
t
)
=
[
R
˙
(
t
)
T
˙
(
t
)
0
0
]
[
R
T
(
t
)
−
R
T
(
t
)
T
(
t
)
0
1
]
=
[
R
˙
(
t
)
R
T
(
t
)
T
˙
(
t
)
−
R
˙
(
t
)
R
T
(
t
)
T
(
t
)
,
0
0
]
g
˙
(
t
)
=
[
R
˙
(
t
)
R
T
(
t
)
T
˙
(
t
)
−
R
˙
(
t
)
R
T
(
t
)
T
(
t
)
0
0
]
g
(
t
)
\dot{\bm{g}}\left( t\right) \bm{g}^{-1}\left( t\right) =\begin{bmatrix} \dot{\boldsymbol{R}}\left( t\right) & \dot{\bm{T}}\left( t\right) \\ \bold{0} & 0 \end{bmatrix}\begin{bmatrix} \boldsymbol{R}^{T}\left( t\right) & -\boldsymbol{R}^{T}\left( t\right) \bm{T}\left( t\right) \\ 0 & 1 \end{bmatrix}=\begin{bmatrix} \dot{\boldsymbol{R}}\left( t\right) \boldsymbol{R}^{T}\left( t\right) & \dot{\bm{T}}\left( t\right) -\dot{\boldsymbol{R}}\left( t\right) \boldsymbol{R}^{T}\left( t\right) \bm{T}(t), \\ 0 & 0 \end{bmatrix}\\ \dot{\bm{g}}\left( t\right) =\begin{bmatrix} \dot{\boldsymbol{R}} \left( t\right) \boldsymbol{R}^{T}\left( t\right) & \dot{\bm{T}}\left( t\right) -\dot{\boldsymbol{R}}\left( t\right) \boldsymbol{R}^{T}\left( t\right) \bm{T}\left( t\right) \\ \bold{0} & 0 \end{bmatrix}\bm{g}\left( t\right)
g˙(t)g−1(t)=[R˙(t)0T˙(t)0][RT(t)0−RT(t)T(t)1]=[R˙(t)RT(t)0T˙(t)−R˙(t)RT(t)T(t),0]g˙(t)=[R˙(t)RT(t)0T˙(t)−R˙(t)RT(t)T(t)0]g(t)
对这个矩阵的每部分进行分析可知,存在一个反对称矩阵
ω
∧
(
t
)
,
ω
(
t
)
∈
s
o
(
3
)
{\bm{\omega}^{\wedge}}(t),\bm{\omega}(t)\in so(3)
ω∧(t),ω(t)∈so(3)和三维向量
v
(
t
)
∈
R
3
\bm{v}(t)\in\mathbb{R}^3
v(t)∈R3
ω
∧
(
t
)
=
R
˙
(
t
)
R
T
(
t
)
v
(
t
)
=
T
˙
(
t
)
−
R
˙
(
t
)
R
T
(
t
)
T
(
t
)
=
T
˙
(
t
)
−
ω
∧
(
t
)
T
(
t
)
{\bm{\omega}^{\wedge}}\left( t\right) =\dot{\boldsymbol{R}}\left( t\right) \boldsymbol{R}^{T}\left( t\right) \\ \bm{v}\left( t\right) =\dot{\bm{T}}\left( t\right) -\dot{\boldsymbol{R}}\left( t\right) \boldsymbol{R}^{T}\left( t\right) \bm{T}\left( t\right) =\dot{\bm{T}}\left( t\right) -{\bm{\omega}^{\wedge}}\left( t\right) \bm{T}\left( t\right)
ω∧(t)=R˙(t)RT(t)v(t)=T˙(t)−R˙(t)RT(t)T(t)=T˙(t)−ω∧(t)T(t)
故
g
˙
(
t
)
=
[
ω
∧
(
t
)
v
(
t
)
0
0
]
g
(
t
)
\dot{\bm{g}}\left( t\right) =\begin{bmatrix} {\bm{\omega}}^{\wedge}\left( t\right) & \bm{v}\left( t\right) \\ \bold{0} & 0 \end{bmatrix}\bm{g}\left( t\right)
g˙(t)=[ω∧(t)0v(t)0]g(t)
可以和反对称矩阵一样,定一个矩阵
ξ
∧
{\bm{\xi}^{\wedge}}
ξ∧,注意这里这个帽子不代表反对称矩阵了
ξ
∧
(
t
)
=
[
ω
∧
(
t
)
v
(
t
)
0
0
]
[
ω
∧
v
0
0
]
∨
=
[
v
ω
]
∈
R
6
{\bm{\xi}^{\wedge}}\left( t\right) =\begin{bmatrix} {\bm{\omega}^{\wedge}}\left( t\right) & \bm{v}\left( t\right) \\ \bold{0} & 0 \end{bmatrix} \\ \begin{bmatrix} {\bm{\omega}}^{\wedge} & \bm{v} \\ \bold{0} & 0 \end{bmatrix}^{\vee }=\begin{bmatrix} \bm{v} \\ \bm{\omega} \end{bmatrix}\in \mathbb{R} ^{6}
ξ∧(t)=[ω∧(t)0v(t)0][ω∧0v0]∨=[vω]∈R6
该矩阵也可以表示为一个向量
ξ
(
t
)
=
[
v
(
t
)
ω
(
t
)
]
\bm{\xi}(t)=\begin{bmatrix}\bm{v}(t)\\\bm{\omega}(t)\end{bmatrix}
ξ(t)=[v(t)ω(t)],可以称之为在曲线
g
(
t
)
g(t)
g(t)处的”正切向量”,而在机器人领域,我们称它为twist。这个twist呢,就像我们开葡萄酒塞时螺旋的角速度和前进的线速度。于是把这些twist集合起来就有了刚体变换的李代数
s
e
(
3
)
\mathfrak{se}(3)
se(3):
s
e
(
3
)
=
{
ξ
=
[
v
ω
]
∣
ω
∈
S
O
(
3
)
,
v
∈
R
3
,
ξ
∧
=
[
ω
∧
v
0
0
]
∈
R
4
×
4
}
\mathfrak{se}\left( 3\right) =\left\{ \bm{\xi} =\begin{bmatrix} \bm{v} \\ \bm{\omega} \end{bmatrix}{\huge \vert} \bm{\omega} \in SO\left( 3\right) ,\bm{v}\in \mathbb{R} ^{3},{\bm{\xi}^{\wedge}}=\begin{bmatrix} {\bm{\omega}^{\wedge}} & \bm{v} \\ \bold{0} & 0 \end{bmatrix}\in \mathbb{R} ^{4\times 4}\right\}
se(3)={ξ=[vω]∣ω∈SO(3),v∈R3,ξ∧=[ω∧0v0]∈R4×4}
同样,李代数
s
e
(
3
)
\mathfrak{se}(3)
se(3)也有类似于
s
o
(
3
)
\mathfrak{so}(3)
so(3)的李括号:
[
ξ
1
,
ξ
2
]
=
(
ξ
1
∧
ξ
2
∧
−
ξ
2
∧
ξ
1
∧
)
∨
[\bm{\xi}_1,\bm{\xi}_2]=(\bm{\xi}_1^{\wedge}\bm{\xi}_2^{\wedge}-\bm{\xi}_2^{\wedge}\bm{\xi}_1^{\wedge})^{\vee}
[ξ1,ξ2]=(ξ1∧ξ2∧−ξ2∧ξ1∧)∨
和旋转矩阵一样的思路
g
˙
(
t
)
=
ξ
∧
(
t
)
g
(
t
)
g
(
t
+
d
t
)
≈
g
(
t
)
+
ξ
∧
(
t
)
g
(
t
)
d
t
≈
(
I
+
ξ
∧
(
t
)
d
t
)
g
(
t
)
g
(
t
)
=
e
ξ
∧
t
g
(
0
)
\dot{\bm{g}}\left( t\right) ={\bm{\xi}^{\wedge}}\left( t\right) \bm{g}\left( t\right)\\ \bm{g}\left( t+dt\right) \approx \bm{g}\left( t\right) + {\bm{\xi}^{\wedge}}\left( t\right) \bm{g}\left( t\right) dt\approx \left( \bold{I}+{\bm{\xi}^{\wedge}}\left( t\right) dt\right) \bm{g}\left( t\right) \\ \bm{g}\left( t\right) =e^{{\bm{\xi}^{\wedge}}t}\bm{g}\left( 0\right)
g˙(t)=ξ∧(t)g(t)g(t+dt)≈g(t)+ξ∧(t)g(t)dt≈(I+ξ∧(t)dt)g(t)g(t)=eξ∧tg(0)
e
ξ
∧
t
=
∑
n
=
0
∞
1
n
!
[
ω
∧
v
0
0
]
n
t
n
=
I
+
∑
n
=
1
∞
1
n
!
[
(
ω
∧
t
)
n
(
ω
∧
t
)
n
−
1
(
v
t
)
0
0
]
=
[
I
+
∑
n
=
1
∞
1
n
!
(
w
^
t
)
n
(
∑
n
=
1
∞
1
n
!
(
ω
∧
t
)
n
−
1
)
(
v
t
)
0
1
]
=
(
e
ω
∧
t
J
(
v
t
)
0
1
)
=
(
R
J
ρ
0
1
)
e^{{\bm{\xi}^{\wedge}}t}=\sum ^{\infty }_{n=0}\dfrac{1}{n!}\begin{bmatrix} {\bm{\omega}^{\wedge}} & \bm{v} \\ \bold{0} & 0 \end{bmatrix}^{n}t^n=\bold{I} +\sum ^{\infty }_{n=1}\dfrac{1}{n!}\begin{bmatrix} ({\bm{\omega}^{\wedge}}t)^{n} & ({\bm{\omega}^{\wedge}}t)^{n-1}(\bm{v}t) \\ \bold{0} & 0 \end{bmatrix}\\ =\begin{bmatrix} \bold{I}+\sum\limits ^{\infty }_{n=1}\dfrac{1}{n!}(\hat{w}t)^{n} & \left( \sum\limits ^{\infty }_{n=1}\dfrac{1}{n!}({\bm{\omega}^{\wedge}}t)^{n-1}\right)(\bm{v}t) \\ \bold{0} & 1 \end{bmatrix}=\begin{pmatrix} e^{{\bm{\omega}^{\wedge}}t} & \bm{J}(\bm{v}t) \\ \bold{0} & 1 \end{pmatrix}=\begin{pmatrix} \boldsymbol{R} & \bm{J}\bm{\rho} \\ \bold{0} & 1 \end{pmatrix}
eξ∧t=n=0∑∞n!1[ω∧0v0]ntn=I+n=1∑∞n!1[(ω∧t)n0(ω∧t)n−1(vt)0]=
I+n=1∑∞n!1(w^t)n0(n=1∑∞n!1(ω∧t)n−1)(vt)1
=(eω∧t0J(vt)1)=(R0Jρ1)
其中
ρ
=
v
t
\rho=vt
ρ=vt,令
ω
t
=
θ
a
,
∥
a
∥
=
1
\bm{\omega} t=\theta a,\Vert a\Vert=1
ωt=θa,∥a∥=1
J
=
∑
n
=
0
∞
1
(
n
+
1
)
!
(
ω
∧
t
)
n
=
∑
n
=
0
∞
1
(
n
+
1
)
!
(
θ
a
^
)
n
=
I
+
1
2
!
θ
a
^
+
1
3
!
θ
2
a
^
2
+
…
=
I
+
1
θ
(
θ
2
2
!
−
θ
4
4
!
+
…
)
a
^
+
1
θ
(
1
3
!
θ
3
−
1
5
!
θ
5
+
…
)
a
^
2
=
I
+
1
−
cos
θ
θ
a
^
+
θ
−
sin
θ
θ
a
^
2
=
I
+
1
−
cos
θ
θ
a
^
+
(
1
−
sin
0
θ
)
(
a
a
T
−
I
)
=
sin
θ
θ
I
+
(
1
−
sin
θ
θ
)
a
a
T
+
1
−
cos
θ
θ
a
^
J=\sum\limits^{\infty }_{n=0}\dfrac{1}{\left( n+1\right) !}({\bm{\omega}^{\wedge}} t)^{n}=\sum\limits^{\infty }_{n=0}\dfrac{1}{\left( n+1\right) !}(\theta\hat{a})^{n}= \bold{I}+\dfrac{1}{2!}\theta \hat{a}+\dfrac{1}{3!}\theta ^{2}\hat{a}^{2}+\ldots =\\ \bold{I}+\dfrac{1}{\theta }\left( \dfrac{\theta ^{2}}{2!}-\dfrac{\theta ^{4}}{4!}+\ldots \right) \hat{a}+\dfrac{1}{\theta }\left( \dfrac{1}{3!}\theta ^{3}-\dfrac{1}{5!}\theta ^{5}+\ldots \right) \hat{a}^{2}\\ =\bold{I}+\dfrac{1-\cos \theta }{\theta }\hat{a}+\dfrac{\theta -\sin \theta }{\theta }\hat{a}^{2}\\ =\bold{I}+\dfrac{1-\cos \theta }{\theta }\hat{a}+\left( 1-\dfrac{\sin 0}{\theta }\right) \left( aa^{T}-\bold{I}\right) \\ =\dfrac{\sin \theta }{\theta }\bold{I}+\left( 1-\dfrac{\sin\theta }{\theta }\right) aa^{T}+\dfrac{1-\cos \theta }{\theta }\hat{a}
J=n=0∑∞(n+1)!1(ω∧t)n=n=0∑∞(n+1)!1(θa^)n=I+2!1θa^+3!1θ2a^2+…=I+θ1(2!θ2−4!θ4+…)a^+θ1(3!1θ3−5!1θ5+…)a^2=I+θ1−cosθa^+θθ−sinθa^2=I+θ1−cosθa^+(1−θsin0)(aaT−I)=θsinθI+(1−θsinθ)aaT+θ1−cosθa^
使用齐次坐标
X
=
(
x
1
)
X=\begin{pmatrix}x\\1\end{pmatrix}
X=(x1)
X
(
t
)
=
g
(
t
)
X
0
X
˙
(
t
)
=
g
˙
(
t
)
X
0
=
g
˙
(
t
)
g
−
1
(
t
)
X
0
V
∧
(
t
)
=
g
(
t
)
g
−
1
(
t
)
=
(
ω
∧
(
t
)
v
(
t
)
0
0
)
X
˙
(
t
)
=
V
∧
(
t
)
X
(
t
)
X\left( t\right) =g\left( t\right) X_{0}\\ \dot{X}\left( t\right) =\dot{g}\left( t\right) X_{0}=\dot{g}\left( t\right) g^{-1} \left( t\right)X_0 \\ {V^{\wedge}}\left( t\right) =g\left( t\right) g^{-1}\left( t\right) =\begin{pmatrix} {\bm{\omega}^{\wedge}}\left( t\right) & v \left( t\right) \\ 0 & 0 \end{pmatrix}\\ \dot{X}\left( t\right) ={V^{\wedge}}\left( t\right) X\left( t\right)
X(t)=g(t)X0X˙(t)=g˙(t)X0=g˙(t)g−1(t)X0V∧(t)=g(t)g−1(t)=(ω∧(t)0v(t)0)X˙(t)=V∧(t)X(t)
V
∧
(
t
)
{V^{\wedge}}(t)
V∧(t)代表点移动的速度,转换成普通坐标系:
x
(
t
)
=
ω
∧
(
t
)
x
(
t
)
+
v
(
t
)
x\left( t\right) ={\bm{\omega}^{\wedge}}\left( t\right) x\left( t\right) +v\left( t\right)
x(t)=ω∧(t)x(t)+v(t)
考虑与X坐标系固连的坐标系Y
Y
=
g
x
y
x
(
t
)
Y
˙
(
t
)
=
g
x
y
X
˙
(
t
)
=
g
x
y
V
∧
(
t
)
x
(
t
)
=
g
x
y
V
∧
g
x
y
−
1
Y
(
t
)
V
∧
y
=
g
x
y
V
∧
g
x
y
−
1
=
a
d
g
x
y
(
V
∧
)
Y=\bm{g}_{xy}x\left( t\right) \\ \dot{Y}\left( t\right) =\bm{g}_{xy}\dot{X}\left( t\right) =\bm{g}_{xy}{V^{\wedge}}\left( t\right) x\left( t\right) =\bm{g}_{xy}{V^{\wedge}}\bm{g}_{xy}^{-1}Y\left( t\right) \\ {V^{\wedge}}_{y}=\bm{g}_{xy} {V^{\wedge}}\bm{g}_{xy}^{-1}=ad_{\bm{g}_{xy}}\left( {V^{\wedge}}\right)
Y=gxyx(t)Y˙(t)=gxyX˙(t)=gxyV∧(t)x(t)=gxyV∧gxy−1Y(t)V∧y=gxyV∧gxy−1=adgxy(V∧)
这里的
a
d
ad
ad是
s
e
(
3
)
se(3)
se(3)上的伴随映射:
a
d
g
:
s
e
(
3
)
→
s
e
(
3
)
:
ξ
∧
→
g
ξ
∧
g
−
1
ad_g:se\left( 3\right) \rightarrow se\left( 3\right) :{\bm{\xi}^{\wedge}}\rightarrow g{\bm{\xi}^{\wedge}}g^{-1}
adg:se(3)→se(3):ξ∧→gξ∧g−1
SO(3)和SE(3)的伴随性质
a
=
R
b
,
a
,
b
∈
R
3
×
3
,
R
T
R
=
1
,
det
(
R
)
=
1
则有
a
∧
=
(
R
b
)
∧
=
R
b
∧
R
T
证明
:
(
R
b
)
∧
=
R
b
∧
R
T
⟺
(
R
b
)
∧
R
=
R
b
∧
R
T
R
=
R
b
∧
⟺
∀
a
∈
R
3
,
(
R
b
)
∧
R
a
=
R
b
∧
a
⟺
∀
a
∈
R
3
,
(
R
b
)
×
(
R
a
)
=
R
(
b
×
a
)
最后一个公式利用向量叉乘的旋转不变性可证
\begin{array}{l} a=Rb,\quad a,b\in \mathbb{R}^{3\times 3},R^TR=1,\det(R)=1\\ 则有a^{\wedge}=(Rb)^{\wedge}=Rb^{\wedge}R^T\\ 证明:(Rb)^{\wedge}=Rb^{\wedge}R^T \\ \iff (Rb)^{\wedge}R=Rb^{\wedge}R^TR=Rb^{\wedge} \\ \iff \forall a\in \mathbb{R}^{3} , (Rb)^{\wedge}Ra=Rb^{\wedge}a\\ \iff \forall a\in \mathbb{R}^3, (Rb)\times (Ra)=R(b\times a)\\ 最后一个公式利用向量叉乘的旋转不变性可证 \end{array}
a=Rb,a,b∈R3×3,RTR=1,det(R)=1则有a∧=(Rb)∧=Rb∧RT证明:(Rb)∧=Rb∧RT⟺(Rb)∧R=Rb∧RTR=Rb∧⟺∀a∈R3,(Rb)∧Ra=Rb∧a⟺∀a∈R3,(Rb)×(Ra)=R(b×a)最后一个公式利用向量叉乘的旋转不变性可证
由上述定理可得
R
e
p
^
R
T
=
e
R
p
^
R
T
=
e
(
R
p
)
∧
\bm{R}e^{\hat{p}}\bm{R}^T=e^{\bm{R}\hat{\bm{p}}\bm{R}^T}=e^{(\bm{Rp})^{\wedge}}
Rep^RT=eRp^RT=e(Rp)∧
该式被称为
S
O
(
3
)
SO(3)
SO(3)上的伴随性质。同样的,在
S
E
(
3
)
SE(3)
SE(3)上也有伴随性质:
g
e
ξ
∧
g
−
1
=
e
(
g
ξ
∧
g
−
1
)
=
e
(
A
d
(
g
)
ξ
)
∧
\bm{g}e^{{\bm{\xi}^{\wedge}}}\bm{g}^{-1}=e^{( \bm{g}{\bm{\xi}^{\wedge}}\bm{g}^{-1}) }=e^{\left( Ad\left( \bm{g}\right) \bm{\xi} \right) ^{\wedge}}
geξ∧g−1=e(gξ∧g−1)=e(Ad(g)ξ)∧
g
ξ
∧
g
−
1
=
(
R
T
0
1
)
(
ω
∧
v
0
0
)
(
R
T
−
R
T
T
0
1
)
=
(
R
ω
∧
R
v
0
0
)
(
R
T
−
R
T
T
0
1
)
=
(
R
ω
∧
R
T
R
v
−
R
ω
∧
R
T
T
0
0
)
=
[
(
R
ω
)
∧
R
v
−
(
R
ω
)
∧
T
0
0
]
=
(
(
R
ω
)
∧
R
v
+
T
∧
R
ω
0
0
)
=
(
R
v
+
T
∧
R
ω
R
ω
)
∧
=
[
(
R
T
∧
R
0
R
)
(
v
ω
)
]
∧
=
[
A
d
(
g
)
ξ
]
∧
\bm{g}{\bm{\xi}^{\wedge}}\bm{g}^{-1}=\begin{pmatrix} \bm{R} & \bm{T} \\ \bold{0} & 1 \end{pmatrix}\begin{pmatrix} {\bm{\omega}^{\wedge}} & \bm{v} \\ \bold{0} & 0 \end{pmatrix}\begin{pmatrix} \bm{R}^{T} & -\bm{R}^{T}T \\ \bold{0} & 1 \end{pmatrix}\\ =\begin{pmatrix} \bm{R}{\bm{\omega}^{\wedge}} & \bm{Rv} \\ \bold{0} & 0 \end{pmatrix}\begin{pmatrix} \bm{R}^{T} & -\bm{R}^{T}\bm{T} \\ 0 & 1 \end{pmatrix}\\ =\begin{pmatrix} \bm{R}{\bm{\omega}^{\wedge}}\bm{R}^{T} & \bm{Rv}-\bm{R}{\bm{\omega}^{\wedge}}\bm{R}^{T}\bm{T} \\ \bold{0} & 0 \end{pmatrix}\\ =\begin{bmatrix} \left( \bm{R}\bm{\omega}\right) ^{\wedge } & \bm{Rv}-\left( \bm{R}\bm{\omega}\right) ^{\wedge }\bm{T} \\ \bold{0} & 0 \end{bmatrix} \\ =\begin{pmatrix} \left( \bm{R}\bm{\omega}\right) ^{\wedge} & \bm{Rv}+{\bm{T}^{\wedge}}\bm{R}\bm{\omega} \\ \bold{0} & 0 \end{pmatrix} =\begin{pmatrix} \bm{Rv}+{\bm{T}^{\wedge}}\bm{R}\bm{\omega} \\ \bm{R}\bm{\omega} \end{pmatrix}^{\wedge } \\=\left[ \begin{pmatrix} \bm{R} & {\bm{T}^{\wedge}}\bm{R} \\ \bold{0} & \bm{R} \end{pmatrix}\begin{pmatrix} \bm{v} \\ \bm{\omega} \end{pmatrix}\right] ^{\wedge } =\left[ Ad\left( \bm{g}\right) \bm{\xi} \right] ^{\wedge }
gξ∧g−1=(R0T1)(ω∧0v0)(RT0−RTT1)=(Rω∧0Rv0)(RT0−RTT1)=(Rω∧RT0Rv−Rω∧RTT0)=[(Rω)∧0Rv−(Rω)∧T0]=((Rω)∧0Rv+T∧Rω0)=(Rv+T∧RωRω)∧=[(R0T∧RR)(vω)]∧=[Ad(g)ξ]∧
李代数求导与扰动模型
BCH公式与近似形式
exp
(
ϕ
1
∧
)
exp
(
ϕ
2
∧
)
=
exp
(
(
ϕ
1
+
ϕ
2
)
∧
)
?
\exp(\bm{\phi}^{\wedge}_1)\exp(\bm{\phi}^{\wedge}_2)=\exp((\bm{\phi}_1+\bm{\phi}_2)^{\wedge})?
exp(ϕ1∧)exp(ϕ2∧)=exp((ϕ1+ϕ2)∧)?
对于标量成立,但对于矩阵的指数函数不成立。
ln
(
exp
(
A
)
exp
(
B
)
)
=
A
+
B
+
1
2
[
A
,
B
]
+
1
12
[
A
,
[
A
,
B
]
]
−
1
12
[
B
,
[
A
,
B
]
]
+
⋯
\ln \left( \exp \left( \boldsymbol{A}\right) \exp \left( \boldsymbol{B}\right) \right) =\boldsymbol{A}+\boldsymbol{B}+\dfrac{1}{2}\left[ \boldsymbol{A},\boldsymbol{B}\right] +\dfrac{1}{12}\left[ \boldsymbol{A},\left[ \boldsymbol{A},\boldsymbol{B}\right] \right] -\dfrac{1}{12}\left[ \boldsymbol{B},\left[ \boldsymbol{A},\boldsymbol{B}\right] \right] +\cdots
ln(exp(A)exp(B))=A+B+21[A,B]+121[A,[A,B]]−121[B,[A,B]]+⋯
其中
[
]
[]
[]为李括号。BCH公式告诉我们,当处理两个矩阵指数之积时,它们会产生一些由李括号组成的余项。特别地,考虑
S
O
(
3
)
\rm SO(3)
SO(3)上的李代数
ln
(
exp
(
ϕ
1
∧
)
exp
(
ϕ
2
∧
)
)
∨
\ln \left( \exp \left( \bm{\phi}_1^{\wedge}\right) \exp \left( \bm{\phi}_2^{\wedge}\right) \right)^{\vee}
ln(exp(ϕ1∧)exp(ϕ2∧))∨ ,当或为小量时,小量二次以上的项都可以被忽略。此时,BCH拥有线性近似表达:
ln
(
exp
(
ϕ
1
∧
)
exp
(
ϕ
2
∧
)
)
∨
≈
{
J
l
(
ϕ
2
)
−
1
ϕ
1
+
ϕ
2
当
ϕ
1
为小量
J
r
(
ϕ
1
)
−
1
ϕ
2
+
ϕ
1
当
ϕ
2
为小量
J
l
=
J
=
sin
θ
θ
I
+
(
1
−
sin
θ
θ
)
a
a
T
+
1
−
cos
θ
θ
a
∧
J
l
−
1
=
θ
2
cot
θ
2
I
+
(
1
−
θ
2
cot
θ
2
)
a
a
T
−
θ
2
a
∧
\ln \left( \exp \left( \bm{\phi}_1^{\wedge}\right) \exp \left( \bm{\phi}_2^{\wedge}\right) \right)^{\vee}\approx \begin{cases} \boldsymbol{J}_l(\bm{\phi}_2)^{-1}\bm{\phi}_1+\bm{\phi}_2\quad 当\bm{\phi}_1为小量\\ \boldsymbol{J}_r(\bm{\phi}_1)^{-1}\bm{\phi}_2+\bm{\phi}_1\quad 当\bm{\phi}_2为小量 \end{cases}\\ \boldsymbol{J}_l=\boldsymbol{J}=\dfrac{\sin\theta}{\theta}\bold{I}+(1-\dfrac{\sin\theta}{\theta})\boldsymbol{aa}^T+\dfrac{1-\cos\theta}{\theta}\boldsymbol{a}^{\wedge} \\ \boldsymbol{J}_l^{-1}=\dfrac{\theta}{2}\cot\dfrac{\theta}{2}\bold{I}+(1-\dfrac{\theta}{2}\cot\dfrac{\theta}{2})\boldsymbol{aa}^T-\dfrac{\theta}{2}\boldsymbol{a}^{\wedge}
ln(exp(ϕ1∧)exp(ϕ2∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2当ϕ1为小量Jr(ϕ1)−1ϕ2+ϕ1当ϕ2为小量Jl=J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧Jl−1=2θcot2θI+(1−2θcot2θ)aaT−2θa∧
也可以写成:
exp
(
(
ϕ
1
+
ϕ
2
)
∧
)
=
exp
(
(
J
l
(
ϕ
2
)
⋅
ϕ
1
)
∧
)
exp
(
ϕ
2
∧
)
=
exp
(
ϕ
1
∧
)
exp
(
(
J
r
(
ϕ
1
)
⋅
ϕ
2
)
∧
)
\exp((\bm{\phi}_1+\bm{\phi}_2)^{\wedge})=\exp\left((\boldsymbol{J}_l(\bm{\phi}_2)\cdot\bm{\phi}_1)^{\wedge}\right)\exp(\bm{\phi}_2^\wedge)\\ \qquad\qquad\qquad\qquad=\exp(\bm{\phi}_1^\wedge) \exp\left((\boldsymbol{J}_r(\bm{\phi}_1)\cdot\bm{\phi}_2)^{\wedge}\right)
exp((ϕ1+ϕ2)∧)=exp((Jl(ϕ2)⋅ϕ1)∧)exp(ϕ2∧)=exp(ϕ1∧)exp((Jr(ϕ1)⋅ϕ2)∧)
而右乘模型雅可比仅需要对自变量取负号即可:
J
r
(
−
ϕ
)
=
J
l
(
−
ϕ
)
\boldsymbol{J}_r(-\bm{\phi})=\boldsymbol{J}_l(-\bm{\phi})
Jr(−ϕ)=Jl(−ϕ)
这样就可以讨论李群乘法和李代数加法的关系了。
假设对某个旋转
R
\boldsymbol{R}
R,对应的李代数为
ϕ
\bm{\phi}
ϕ。我们给它一个微笑旋转,记作
Δ
R
\Delta \boldsymbol{R}
ΔR ,对应的李代数为
Δ
ϕ
\Delta\bm{\phi}
Δϕ。那么,在李群上得到的结果为
Δ
R
⋅
R
\Delta\boldsymbol{R}\cdot\boldsymbol{R}
ΔR⋅R,而在李代数上,根据BCH近似,为
J
l
−
1
(
ϕ
)
Δ
ϕ
+
ϕ
\boldsymbol{J}_l^{-1}(\bm{\phi})\Delta\bm{\phi}+\bm{\phi}
Jl−1(ϕ)Δϕ+ϕ
exp
(
Δ
ϕ
∧
)
exp
(
ϕ
∧
)
=
exp
(
(
J
l
−
1
(
ϕ
)
Δ
ϕ
+
ϕ
)
∧
)
\exp(\Delta \bm{\phi}^{\wedge}) \exp(\bm{\phi}^{\wedge})=\exp\left(\left(\boldsymbol{J}_l^{-1}(\bm{\phi})\Delta\bm{\phi}+\bm{\phi}\right)^{\wedge}\right)
exp(Δϕ∧)exp(ϕ∧)=exp((Jl−1(ϕ)Δϕ+ϕ)∧)
反之,如果我们在李代数上进行加法,那么可以近似为李群上带左右雅可比的乘法:
exp
(
(
ϕ
+
Δ
ϕ
)
∧
)
=
exp
(
(
J
l
Δ
ϕ
)
∧
)
exp
(
ϕ
∧
)
=
exp
(
ϕ
∧
)
exp
(
(
J
r
Δ
ϕ
)
∧
)
\exp\left(\left(\bm{\phi}+\Delta\bm{\phi}\right)^{\wedge}\right)=\exp \left(\left(\boldsymbol{J}_l\Delta\bm{\phi}\right)^{\wedge}\right) \exp\left(\bm{\phi}^{\wedge}\right)=\exp\left(\bm{\phi}^{\wedge}\right)\exp \left(\left(\boldsymbol{J}_r\Delta\bm{\phi}\right)^{\wedge}\right)
exp((ϕ+Δϕ)∧)=exp((JlΔϕ)∧)exp(ϕ∧)=exp(ϕ∧)exp((JrΔϕ)∧)
李代数求导
我们经常会构建与位姿有关的函数,然后讨论该函数关于位姿的导数,以调整当前的估计值。然而 S O ( 3 ) \rm SO(3) SO(3)和 S E ( 3 ) \rm SE(3) SE(3)上并没有良好定义的加法,它们只是群。如果我们把 T \boldsymbol{T} T当成一个普通矩阵来处理优化,就必须对它加以约束。而从李代数的角度来说,由于李代数由向量组成,具有良好的加法运算,因此使用李代数解决求导问题的思路分为两种:
- 用李代数表示姿态,然后根据李代数加法对李代数求导
- 对李群左乘或右乘微小扰动,然后对该扰动求导,称为左扰动模型和右扰动模型
第一种方式对应李代数的求导模型,
∂ ( R p ) ∂ ϕ = ∂ ( exp ( ϕ ∧ ) p ) ∂ ϕ = lim δ ϕ → 0 exp ( ( ϕ + δ ϕ ) ∧ ) p − exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ → 0 exp ( ( J l δ ϕ ) ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ → 0 [ I + ( J l δ ϕ ) ∧ ] exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ → 0 ( J l δ ϕ ) ∧ exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ → 0 − [ exp ( ϕ ∧ ) p ] ∧ J l δ ϕ δ ϕ = − ( R p ) ∧ J l \dfrac{\partial( \boldsymbol{R}\boldsymbol{p})}{\partial \bm{\phi}}=\dfrac{\partial \left( \exp \left( \bm{\phi} ^{\wedge }\right) \boldsymbol{p}\right) }{\partial \bm{\phi} } =\lim _{\delta\bm{\phi} \rightarrow 0}\dfrac{\exp \left( \left( \bm{\phi} +\delta \bm{\phi} \right) ^{\wedge }\right) \boldsymbol{p}-\exp \left( \bm{\phi} ^{\wedge }\right) \boldsymbol{p}}{\delta\bm{\phi} }\\ =\lim _{\delta \bm{\phi} \rightarrow 0}\dfrac{\exp \left( \left( \boldsymbol{J}_{l}\delta \bm{\phi} \right) ^{\wedge}\right) \exp \left( \bm{\phi} ^{\wedge}\right) \boldsymbol{\boldsymbol{p}}-\exp \left( \bm{\phi} ^{\wedge}\right) \boldsymbol{p}}{\delta\bm{\phi} }\\ =\lim _{\delta \bm{\phi} \rightarrow 0}\dfrac{\left[ \bold{I}+\left( \boldsymbol{J}_{l}\delta \bm{\phi} \right) ^{\wedge }\right] \exp \left( \bm{\phi} ^{\wedge}\right) \boldsymbol{p}-\exp \left( \bm{\phi} ^{\wedge }\right) \boldsymbol{p}}{\delta\bm{\phi} }\\ =\lim _{\delta \bm{\phi} \rightarrow 0}\dfrac{\left( \boldsymbol{J}_{l}\delta \bm{\phi} \right) ^{\wedge }\exp \left( \bm{\phi} ^{\wedge}\right) \boldsymbol{p}}{\delta \bm{\phi} }\\ =\lim _{\delta\bm{\phi} \rightarrow 0}\dfrac{-\left[ \exp \left( \bm{\phi} ^{\wedge}\right) \boldsymbol{p}\right] ^{\wedge}\boldsymbol{J}_{l}\delta\bm{\phi} }{\delta\bm{\phi} }=-\left( Rp\right) ^{\wedge }\boldsymbol{J}_{l} ∂ϕ∂(Rp)=∂ϕ∂(exp(ϕ∧)p)=δϕ→0limδϕexp((ϕ+δϕ)∧)p−exp(ϕ∧)p=δϕ→0limδϕexp((Jlδϕ)∧)exp(ϕ∧)p−exp(ϕ∧)p=δϕ→0limδϕ[I+(Jlδϕ)∧]exp(ϕ∧)p−exp(ϕ∧)p=δϕ→0limδϕ(Jlδϕ)∧exp(ϕ∧)p=δϕ→0limδϕ−[exp(ϕ∧)p]∧Jlδϕ=−(Rp)∧Jl
扰动模型(左乘)
SO(3)
另一种求导方式是对
R
\boldsymbol{R}
R进行一次扰动
Δ
R
\Delta \boldsymbol{R}
ΔR,看结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在
∂
(
R
p
)
∂
φ
=
lim
φ
→
0
exp
(
φ
∧
)
exp
(
ϕ
∧
)
p
−
exp
(
ϕ
∧
)
p
φ
\dfrac{\partial (\boldsymbol{Rp})}{\partial \boldsymbol{\varphi}}=\lim_{\boldsymbol{\varphi}\to 0}\dfrac{\exp(\boldsymbol{\varphi}^{\wedge})\exp(\bm{\phi}^{\wedge})\boldsymbol{p}-\exp(\bm{\phi}^{\wedge})\boldsymbol{p}}{\boldsymbol{\varphi}}
∂φ∂(Rp)=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p
该式的求导更简单,省去了一个雅可比
J
l
\boldsymbol{J}_l
Jl计算
∂ ( R p ) ∂ φ = lim φ → 0 exp ( φ ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p φ = lim φ → 0 ( I + φ ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p φ = lim φ → 0 φ ∧ R p φ = lim φ → 0 − ( R p ) ∧ φ φ = − ( R p ) ∧ \dfrac{\partial \left( \boldsymbol{R}\boldsymbol{\boldsymbol{p}}\right) }{\partial \boldsymbol{\varphi} }=\lim _{\boldsymbol{\varphi} \rightarrow 0}\dfrac{\exp \left( \boldsymbol{\varphi} ^{\wedge} \right) \exp \left( \bm{\phi} ^{\wedge }\right) \boldsymbol{\boldsymbol{p}}-\exp \left( \bm{\phi} ^ {\wedge}\right) \boldsymbol{\boldsymbol{p}}}{\boldsymbol{\varphi} }\\ =\lim _{\boldsymbol{\varphi} \rightarrow 0}\dfrac{\left( \mathbf{I}+\boldsymbol{\varphi} ^{\wedge }\right) \exp \left( \bm{\phi} ^{\wedge }\right) \boldsymbol{\boldsymbol{p}}-\exp \left( \bm{\phi} ^{\wedge}\right) \boldsymbol{\boldsymbol{p}}}{\boldsymbol{\varphi} }\\ =\lim _{\boldsymbol{\varphi} \rightarrow 0}\dfrac{\boldsymbol{\varphi} ^{\wedge}\boldsymbol{R}\boldsymbol{\boldsymbol{p}}}{\boldsymbol{\varphi} }=\lim _{\boldsymbol{\varphi} \rightarrow 0}\dfrac{-\left( \boldsymbol{R}\boldsymbol{\boldsymbol{p}}\right) ^{\wedge }\boldsymbol{\varphi} }{\boldsymbol{\varphi} }=-\left( \boldsymbol{R}\boldsymbol{\boldsymbol{p}}\right) ^{\wedge} ∂φ∂(Rp)=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφ(I+φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφφ∧Rp=φ→0limφ−(Rp)∧φ=−(Rp)∧
SE(3)
假设某空间点 p \boldsymbol{p} p经过一次变换 T \boldsymbol{T} T(对应的李代数为 ξ \bm{\xi} ξ,)得到 T p \boldsymbol{Tp} Tp。现在给 T \boldsymbol{T} T左乘一个扰动 Δ T = exp ( δ ξ ∧ ) \Delta \boldsymbol{T}=\exp(\delta \bm{\xi}^{\wedge}) ΔT=exp(δξ∧),设扰动项的李代数为 δ ξ = [ δ ϕ , δ ρ ] \delta \bm{\xi}=[\delta \bm{\phi},\delta \boldsymbol{\rho}] δξ=[δϕ,δρ],那么:
∂ ( T p ) ∂ δ ξ = lim δ ξ → 0 exp ( δ ξ ∧ ) exp ( ξ ∧ ) p − exp ( ξ ∧ ) p δ ξ = lim δ ξ → 0 ( I + δ ξ ∧ ) exp ( ξ ∧ ) p − exp ( ξ ∧ ) p δ ξ = lim δ ξ → 0 δ ξ ∧ exp ( ξ ∧ ) p δ ξ = lim δ ξ → 0 [ δ ϕ ∧ δ ρ 0 T 0 ] [ R p + t 1 ] δ ξ = lim δ ξ → 0 [ δ ϕ ∧ ( R p + t ) + δ ρ 0 T ] [ δ ϕ , δ ρ ] T = [ − ( R p + t ) ∧ I 0 T 0 T ] = d e f ( T p ) ⊙ \dfrac{\partial \left( \boldsymbol{T} \boldsymbol{p}\right) }{\partial \delta \bm{\xi} }=\lim _{\delta \bm{\xi} \rightarrow \boldsymbol{0}}\dfrac{\exp \left( \delta \bm{\xi} ^{\wedge }\right) \exp \left( \bm{\xi} ^{\wedge }\right) \boldsymbol{p}-\exp \left( \bm{\xi} ^{\wedge }\right) \boldsymbol{p}}{\delta \bm{\xi} }\\ \begin{array}{l} =\lim\limits_{\delta \bm{\xi} \rightarrow \boldsymbol{0}} \dfrac{\left( \bold{I}+\delta \bm{\xi} ^{\wedge }\right) \exp \left( \bm{\xi} ^{\wedge }\right) \boldsymbol{p}-\exp \left( \bm{\xi} ^{\wedge }\right) \boldsymbol{p}}{\delta \bm{\xi}} \\ =\lim\limits_{\delta\bm{\xi} \rightarrow \boldsymbol{0}}\dfrac{\delta \bm{\xi} ^{\wedge }\exp \left( \bm{\xi} ^{\wedge }\right) \boldsymbol{p}}{\delta \bm{\xi} }\\ =\lim \limits_{\delta\bm{\xi}\rightarrow \boldsymbol{0}}\dfrac{\begin{bmatrix} \delta\bm{\phi} ^{\wedge } & \delta \boldsymbol{\rho} \\ \boldsymbol{0}^{T} & \boldsymbol{0} \end{bmatrix}\begin{bmatrix} \boldsymbol{R} \boldsymbol{p} + \boldsymbol{t} \\ 1 \end{bmatrix}}{\delta \bm{\xi} }\\ =\lim \limits_{\delta \bm{\xi} \rightarrow \boldsymbol{0}}\dfrac{\begin{bmatrix} \delta \bm{\phi}^{\wedge}\left( \boldsymbol{R} \boldsymbol{p}+\boldsymbol{t}\right) +\delta \boldsymbol{\rho} \\ \boldsymbol{0}^{T} \end{bmatrix}}{\left[\delta \bm{\phi} ,\delta \boldsymbol{\rho}\right] ^{T}}\\=\begin{bmatrix} -\left( \boldsymbol{R} \boldsymbol{p}+\boldsymbol{t}\right)^{\wedge} &\bold{I} \\ \boldsymbol{0}^{T} & \boldsymbol{0}^{T} \end{bmatrix}\overset{\rm def}{=}(\boldsymbol{Tp})^{\odot} \end{array} ∂δξ∂(Tp)=δξ→0limδξexp(δξ∧)exp(ξ∧)p−exp(ξ∧)p=δξ→0limδξ(I+δξ∧)exp(ξ∧)p−exp(ξ∧)p=δξ→0limδξδξ∧exp(ξ∧)p=δξ→0limδξ[δϕ∧0Tδρ0][Rp+t1]=δξ→0lim[δϕ,δρ]T[δϕ∧(Rp+t)+δρ0T]=[−(Rp+t)∧0TI0T]=def(Tp)⊙
把最后的结果定义成一个算符 ⊙ ^\odot ⊙,它把一个齐次坐标的空间点变换成一个 4 × 6 4\times 6 4×6的矩阵。