李代数与李群的关系
R
˙
R
T
\dot{R}R^{T}
R˙RT 是一个反对称矩阵,所以这个矩阵可以用一个1×3向量进行反对称来表示
R
˙
R
T
=
Φ
^
\dot{R}R^{T}=Φ^{\hat{}}
R˙RT=Φ^ ,
根据十四讲 4.8 的推导,最后则有
R
(
t
)
˙
=
Φ
^
⋅
R
(
t
)
\dot{R(t)}=Φ^{\hat{}}·R(t)
R(t)˙=Φ^⋅R(t)
这个李代数
Φ
Φ
Φ 反映了
R
R
R 的导数性质,所以李代数是李群
S
O
(
3
)
SO(3)
SO(3) 的正切空间,因为这里李群是9维的,所以切向量也是一个空间
李代数由一个方向和夹角构成,
Φ
=
θ
a
,
∣
∣
a
⃗
∣
∣
=
1
Φ=θa,||\vec{a}||=1
Φ=θa,∣∣a∣∣=1
指数映射,也是罗德里格斯公式
e
x
p
(
Φ
∧
)
=
e
x
p
(
θ
a
∧
)
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
a
a
T
+
s
i
n
θ
a
∧
=
R
exp(Φ^{\wedge})=exp(θa^{\wedge})=cosθI+(1-cosθ)aa^{T}+sinθa^{\wedge}=R
exp(Φ∧)=exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧=R
e
x
p
(
Φ
∧
)
=
R
exp(Φ^{\wedge})=R
exp(Φ∧)=R
意思就是李代数
Φ
Φ
Φ 可以由角度
θ
θ
θ 和方向向量
a
⃗
\vec{a}
a 表示,通过罗德里格斯公式可以变换成对应的旋转矩阵
S
O
(
3
)
SO(3)
SO(3)
用李代数表示旋转会有个问题,就是周期性,就是多个李代数可以对应一个旋转矩阵,如果固定旋转角度在±π时就是唯一对应的
由旋转矩阵 R R R 求李代数 Φ Φ Φ , θ = a r c c o s t r ( R ) − 1 2 , R a = a θ=arccos\frac{tr(R)-1}{2},Ra=a θ=arccos2tr(R)−1,Ra=a,由 l n ( R ) ∨ = Φ ln(R)^{\vee}=Φ ln(R)∨=Φ 表示
BCH一阶线性近似表达
l
n
(
e
x
p
(
ϕ
1
∧
)
e
x
p
(
ϕ
2
∧
)
)
∨
ln(exp(\phi^{\wedge}_{1})exp(\phi^{\wedge}_{2}))^{\vee}
ln(exp(ϕ1∧)exp(ϕ2∧))∨
当左边的
ϕ
1
\phi_{1}
ϕ1 为小量时,此时相当于左乘,则
=
>
J
l
(
ϕ
2
)
−
1
ϕ
1
+
ϕ
2
=>J_{l}(\phi_{2})^{-1}\phi_{1}+\phi_{2}
=>Jl(ϕ2)−1ϕ1+ϕ2
其实就是相当于在
ϕ
2
\phi_{2}
ϕ2 的基础上加上微量
J
l
(
ϕ
2
)
−
1
ϕ
1
J_{l}(\phi_{2})^{-1}\phi_{1}
Jl(ϕ2)−1ϕ1
当右边的
ϕ
2
\phi_{2}
ϕ2 为小量时,此时相当于右乘,则
=
>
J
r
(
ϕ
1
)
−
1
ϕ
2
+
ϕ
1
=>J_{r}(\phi_{1})^{-1}\phi_{2}+\phi_{1}
=>Jr(ϕ1)−1ϕ2+ϕ1
SLAM中我们构建了与位姿相关的函数后,需要讨论该函数对位姿的求导,以估计当前值
有两种方法求导
1、用李代数表示位姿,根据李代数加法对李代数求导
2、对李群进行左乘或右乘进行扰动,对扰动求导
由于使用李代数求导要计算 雅可比
J
J
J ,这个形式比较复杂,工程中不用这个方法,都是用扰动模型,所以只看扰动模型
∂
(
R
p
)
∂
φ
=
l
i
m
φ
→
0
e
x
p
(
φ
∧
)
e
x
p
(
ϕ
∧
)
p
−
e
x
p
(
ϕ
∧
)
p
φ
\frac{\partial(Rp)}{\partial\varphi}=lim_{\varphi\rightarrow0}\frac{exp(\varphi^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\varphi}
∂φ∂(Rp)=limφ→0φexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p
e
x
p
(
φ
∧
)
exp(\varphi^{\wedge})
exp(φ∧) 是微量,相当于是对旋转的导数,则等于
(
I
+
φ
∧
)
(I+\varphi^{\wedge})
(I+φ∧)
具体推导如下
左乘扰动求导
这里是旋转
R
R
R 对向量
p
p
p 进行旋转,不停地左乘扰动来改变向量
p
p
p 的方向,左扰动
Δ
R
\Delta{R}
ΔR 对应的李代数
φ
\varphi
φ ,
R
p
Rp
Rp 的结果也是向量
∂
(
R
p
)
∂
φ
=
l
i
m
φ
→
0
e
x
p
(
φ
∧
)
e
x
p
(
ϕ
∧
)
p
−
e
x
p
(
ϕ
∧
)
p
φ
\frac{\partial(Rp)}{\partial\varphi}=lim_{\varphi\rightarrow0}\frac{exp(\varphi^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\varphi}
∂φ∂(Rp)=limφ→0φexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p
=
l
i
m
φ
→
0
(
I
+
φ
∧
)
e
x
p
(
ϕ
∧
)
p
−
e
x
p
(
ϕ
∧
)
p
φ
=lim_{\varphi\rightarrow0}\frac{(I+\varphi^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\varphi}
=limφ→0φ(I+φ∧)exp(ϕ∧)p−exp(ϕ∧)p
乘进去相减,很明显
=
l
i
m
φ
→
0
φ
∧
R
p
φ
=lim_{\varphi\rightarrow0}\frac{\varphi^{\wedge}Rp}{\varphi}
=limφ→0φφ∧Rp
叉乘有一个性质,
a
⃗
×
b
⃗
=
−
b
⃗
×
a
⃗
\vec{a}×\vec{b}=-\vec{b}×\vec{a}
a×b=−b×a
a
⃗
×
b
⃗
=
a
∧
⋅
b
\vec{a}×\vec{b}=a^{\wedge}·b
a×b=a∧⋅b
−
b
⃗
×
a
⃗
=
−
b
∧
⋅
a
-\vec{b}×\vec{a}=-b^{\wedge}·a
−b×a=−b∧⋅a
a
∧
⋅
b
=
−
b
∧
⋅
a
a^{\wedge}·b=-b^{\wedge}·a
a∧⋅b=−b∧⋅a
则上式等于
=
l
i
m
φ
→
0
−
(
R
p
)
∧
φ
φ
=lim_{\varphi\rightarrow0}\frac{-(Rp)^{\wedge}\varphi}{\varphi}
=limφ→0φ−(Rp)∧φ
约掉
φ
\varphi
φ 则
l
i
m
φ
→
0
=
−
(
R
p
)
∧
lim_{\varphi\rightarrow0}=-(Rp)^{\wedge}
limφ→0=−(Rp)∧
这里省去了雅可比
J
J
J 的计算
对右乘也是一样的方法
矩阵转置的性质
(
A
+
B
)
T
=
A
T
+
B
T
(A+B)^{T}=A^{T}+B^{T}
(A+B)T=AT+BT
(
λ
A
)
T
=
λ
A
T
(\lambda{A})^{T}=\lambda{A^{T}}
(λA)T=λAT
(
A
B
)
T
=
B
T
A
T
(AB)^{T}=B^{T}A^{T}
(AB)T=BTAT
SO(3)的伴随性质
R
T
E
x
p
(
ϕ
)
R
=
E
x
p
(
R
T
ϕ
)
R^{T}Exp(\phi)R=Exp(R^{T}\phi)
RTExp(ϕ)R=Exp(RTϕ)
ϕ
\phi
ϕ 为扰动量
Δ
R
\Delta{R}
ΔR 对应的李代数
对复合旋转进行求导
∂ L o g ( R 1 R 2 ) ∂ R 1 \frac{\partial Log(R_{1}R_{2})}{\partial R_{1}} ∂R1∂Log(R1R2)
这里是对两个相乘的旋转矩阵中的其中一个旋转进行求导,上面的左乘扰动例子是对矩阵相乘向量进行求导的,所以可以直接对矩阵进行扰动
但是这里不能直接对矩阵
S
O
(
3
)
SO(3)
SO(3) 进行扰动,不能直接说
R
1
R
2
R_{1}R_{2}
R1R2 对
R
1
R_{1}
R1 或
R
2
R_{2}
R2 的导数,这样就变成矩阵对向量的求导,因为扰动量是可以用向量表示的,前面的例子
R
p
Rp
Rp 相乘后也是个向量,所以可以直接对扰动量进行求导,但是这里两个矩阵相乘还是矩阵,所以得用
L
o
g
Log
Log 将矩阵相乘结果变为李代数,这样才符合求导的定义
L
o
g
(
R
)
=
l
o
g
(
R
)
∨
Log(R)=log(R)^{\vee}
Log(R)=log(R)∨,结果是向量,用
L
o
g
Log
Log 就是为了懒得写
∨
\vee
∨
E
x
p
(
w
)
=
e
x
p
(
w
∧
)
Exp(w)=exp(w^{\wedge})
Exp(w)=exp(w∧),结果是矩阵
对
R
1
R_{1}
R1 进行右扰动
∂
L
o
g
(
R
1
R
2
)
∂
R
1
=
l
i
m
φ
→
0
L
o
g
(
R
1
E
x
p
(
ϕ
)
R
2
)
−
L
o
g
(
R
1
R
2
)
ϕ
\frac{\partial Log(R_{1}R_{2})}{\partial R_{1}}=lim_{\varphi\rightarrow0}\frac{Log(R_{1}Exp(\phi)R_{2})-Log(R_{1}R_{2})}{\phi}
∂R1∂Log(R1R2)=limφ→0ϕLog(R1Exp(ϕ)R2)−Log(R1R2) ①
利用
S
O
(
3
)
SO(3)
SO(3) 的伴随性质:
R
T
E
x
p
(
ϕ
)
R
=
E
x
p
(
R
T
ϕ
)
R^{T}Exp(\phi)R=Exp(R^{T}\phi)
RTExp(ϕ)R=Exp(RTϕ)
把 R T R^{T} RT 换到另一边
= > E x p ( ϕ ) R = R E x p ( R T ϕ ) =>Exp(\phi)R=RExp(R^{T}\phi) =>Exp(ϕ)R=RExp(RTϕ)
所以 E x p ( ϕ ) R 2 = R 2 E x p ( R 2 T ϕ ) Exp(\phi)R_{2} = R_{2}Exp(R^{T}_{2}\phi) Exp(ϕ)R2=R2Exp(R2Tϕ)
∂ L o g ( R 1 R 2 ) ∂ R 1 = l i m φ → 0 L o g ( R 1 R 2 E x p ( R 2 T ϕ ) ) − L o g ( R 1 R 2 ) ϕ \frac{\partial Log(R_{1}R_{2})}{\partial R_{1}}=lim_{\varphi\rightarrow0}\frac{Log(R_{1}R_{2}Exp(R^{T}_{2}\phi))-Log(R_{1}R_{2})}{\phi} ∂R1∂Log(R1R2)=limφ→0ϕLog(R1R2Exp(R2Tϕ))−Log(R1R2) ②
然后利用BCH的一阶线性近似,现在的公式形式相当于小量在最右边,则利用
l
n
(
e
x
p
(
ϕ
1
∧
)
e
x
p
(
ϕ
2
∧
)
)
∨
=
J
r
(
ϕ
1
)
−
1
ϕ
2
+
ϕ
1
ln(exp(\phi^{\wedge}_{1})exp(\phi^{\wedge}_{2}))^{\vee}=J_{r}(\phi_{1})^{-1}\phi_{2}+\phi_{1}
ln(exp(ϕ1∧)exp(ϕ2∧))∨=Jr(ϕ1)−1ϕ2+ϕ1
统一表达式的话就是
L o g ( e x p ( ϕ 1 ∧ ) e x p ( ϕ 2 ∧ ) ) = J r ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 Log(exp(\phi^{\wedge}_{1})exp(\phi^{\wedge}_{2}))=J_{r}(\phi_{1})^{-1}\phi_{2}+\phi_{1} Log(exp(ϕ1∧)exp(ϕ2∧))=Jr(ϕ1)−1ϕ2+ϕ1
也等于
= > L o g ( e x p ( ϕ 1 ∧ ) e x p ( ϕ 2 ∧ ) ) = J r ( L o g ( ϕ 1 ∧ ) ) − 1 L o g ( ϕ 2 ∧ ) + L o g ( ϕ 1 ∧ ) =>Log(exp(\phi^{\wedge}_{1})exp(\phi^{\wedge}_{2}))=J_{r}(Log(\phi_{1}^{\wedge}))^{-1}Log(\phi_{2}^{\wedge})+Log(\phi_{1}^{\wedge}) =>Log(exp(ϕ1∧)exp(ϕ2∧))=Jr(Log(ϕ1∧))−1Log(ϕ2∧)+Log(ϕ1∧)
所以上面的公式推导为
L
o
g
(
R
1
R
2
E
x
p
(
R
2
T
ϕ
)
)
=
J
r
−
1
(
L
o
g
(
R
1
R
2
)
)
L
o
g
(
E
x
p
(
R
2
T
ϕ
)
)
+
L
o
g
(
R
1
R
2
)
Log(R_{1}R_{2}Exp(R^{T}_{2}\phi))=J_{r}^{-1}(Log(R_{1}R_{2}))Log(Exp(R^{T}_{2}\phi))+Log(R_{1}R_{2})
Log(R1R2Exp(R2Tϕ))=Jr−1(Log(R1R2))Log(Exp(R2Tϕ))+Log(R1R2) ③
代入公式②,把 L o g ( R 1 R 2 ) Log(R_{1}R_{2}) Log(R1R2) 减掉,把 ϕ \phi ϕ 消掉,得:
= > J r − 1 ( L o g ( R 1 R 2 ) ) R 2 T =>J_{r}^{-1}(Log(R_{1}R_{2}))R^{T}_{2} =>Jr−1(Log(R1R2))R2T