BCH公式与近似公式(Baker-Campbell-haudsorff)
给出了李代数指数映射乘积的完整公式:
[
,
]
:
为
李
括
号
[ ,]:为李括号
[,]:为李括号
l
n
(
e
A
e
B
)
=
A
+
B
+
1
2
[
A
,
B
]
+
1
12
[
A
,
[
A
,
B
]
]
−
1
12
[
B
,
[
A
,
B
]
]
+
.
.
.
ln(e^{A}e^{B})=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+...
ln(eAeB)=A+B+21[A,B]+121[A,[A,B]]−121[B,[A,B]]+...
当
ϕ
1
,
ϕ
2
\phi_1,\phi_2
ϕ1,ϕ2为小量时:可以近似得到两个模型
ϕ
=
θ
∗
u
\phi=\theta*u
ϕ=θ∗u
l
n
(
e
ϕ
1
×
e
ϕ
2
×
)
∨
≈
{
J
l
(
θ
2
)
−
1
θ
1
+
θ
2
左
乘
模
型
J
R
(
θ
1
)
−
1
θ
2
+
θ
1
右
乘
模
型
ln(e^{\phi_1×}e^{\phi_2×})^\vee \approx \begin{cases} J_l(\theta_2)^{-1}\theta_1+\theta_2&左乘模型\\ J_R(\theta_1)^{-1}\theta_2+\theta_1&右乘模型\\ \end{cases}
ln(eϕ1×eϕ2×)∨≈{Jl(θ2)−1θ1+θ2JR(θ1)−1θ2+θ1左乘模型右乘模型
J
l
(
θ
)
=
J
r
(
−
θ
)
=
s
i
n
θ
θ
I
+
(
1
−
s
i
n
θ
θ
)
u
∗
u
T
+
1
−
c
o
s
θ
θ
u
×
J_l(\theta)=J_r(-\theta)=\frac{sin\theta}{\theta}I+(1-\frac{sin\theta}{\theta})u*u^T+\frac{1-cos\theta}{\theta}u×
Jl(θ)=Jr(−θ)=θsinθI+(1−θsinθ)u∗uT+θ1−cosθu×
J
l
(
θ
)
−
1
=
θ
2
c
o
t
θ
2
I
+
(
1
−
θ
2
c
o
t
θ
2
)
u
×
u
T
−
θ
2
u
×
J_l(\theta)^{-1}=\frac{\theta}{2}cot\frac{\theta}{2}I+(1-\frac{\theta}{2}cot\frac{\theta}{2})u×u^T-\frac{\theta}{2}u×
Jl(θ)−1=2θcot2θI+(1−2θcot2θ)u×uT−2θu×
李代数上的乘法在李群上的表示:
s
o
(
3
)
−
>
S
O
(
3
)
so(3)->SO(3)
so(3)−>SO(3)
e
Δ
ϕ
e
ϕ
=
e
(
ϕ
+
J
l
−
1
(
ϕ
)
Δ
ϕ
)
×
e^{\Delta\phi}e^{ \phi}=e^{(\phi+J^{-1}_l(\phi)\Delta\phi)×}
eΔϕeϕ=e(ϕ+Jl−1(ϕ)Δϕ)×
s
e
(
3
)
−
>
S
E
(
3
)
se(3)->SE(3)
se(3)−>SE(3)
e
Δ
ξ
×
e
ξ
×
≈
e
(
J
l
−
1
Δ
ξ
+
ξ
)
×
e^{\Delta \xi×}e^{\xi×} \approx e^{(J_l^{_-1}\Delta\xi+\xi)×}
eΔξ×eξ×≈e(Jl−1Δξ+ξ)×
e
ξ
×
e
Δ
ξ
×
=
e
(
j
r
−
1
Δ
ξ
+
ξ
)
×
e^{\xi×}e^{\Delta \xi×}=e^{(j_r^{-1}\Delta\xi+\xi)×}
eξ×eΔξ×=e(jr−1Δξ+ξ)×
李代数求导(Baker-Campbell-haudsorff)
R
p
:
R_p:
Rp:对空间上点
p
p
p进行旋转后得到:
∂
R
P
∂
ϕ
=
(
−
R
p
)
×
J
l
\frac{\partial R_P}{\partial \phi}=(-R_p)×J_l
∂ϕ∂RP=(−Rp)×Jl
使用扰动模型简化求导(so(3)左乘)
Φ
:
左
扰
动
对
应
的
Δ
R
\Phi:左扰动对应的\Delta R
Φ:左扰动对应的ΔR详细的推导过程看这里
对
Φ
\Phi
Φ求导:
∂
R
p
∂
Φ
=
l
i
m
Φ
=
0
e
Φ
×
e
ϕ
×
p
−
e
ϕ
×
p
Φ
≈
−
R
p
×
\frac{\partial R_p}{\partial \Phi}=lim_{\Phi=0}\frac{e^{\Phi×}e^{\phi×}p-e^{\phi×}p}{\Phi} \approx -R_p×
∂Φ∂Rp=limΦ=0ΦeΦ×eϕ×p−eϕ×p≈−Rp×
使用扰动模型简化求导(se(3)左乘)
∂ ( T p ) ∂ δ ξ = l i m ( δ ξ − > 0 ) e δ ξ × e ξ × p − e ξ × p δ ξ = [ I − ( R p + t ) × 0 T 0 T ] = ( T P ) ⊙ . \frac{\partial (T_p)}{\partial \delta \xi}=lim_{(\delta\xi->0)}\frac{e^{\delta\xi×}e^{\xi×}p-e^{\xi ×}p}{\delta \xi}=\left[\begin{matrix} I&-(Rp+t)×\\ 0^T&0^T\\ \end{matrix}\right]=(T_P)^{ ⊙}. ∂δξ∂(Tp)=lim(δξ−>0)δξeδξ×eξ×p−eξ×p=[I0T−(Rp+t)×0T]=(TP)⊙.
相似变换群与李代数(Sim(3))
定义相似变换群为:
S
i
m
(
3
)
=
{
s
=
[
s
R
t
0
T
1
]
∈
R
4
×
4
}
Sim(3)=\left\{\begin{matrix} s=\left[\begin{matrix} sR&t\\ 0^T&1\\ \end{matrix}\right] \in R^{4×4}\\ \end{matrix}\right\}
Sim(3)={s=[sR0Tt1]∈R4×4}
定义相似变换群的李代数为:
s
i
m
(
3
)
=
{
ζ
∣
ζ
=
[
p
θ
σ
]
∈
R
7
×
1
,
ζ
×
=
[
σ
I
+
ϕ
×
p
0
T
0
]
∈
R
4
×
4
}
sim(3)=\left\{\begin{matrix} \zeta|\zeta=\left[\begin{matrix} p\\ \theta\\ \sigma\\ \end{matrix}\right] \in R^{7×1}, \zeta×=\left[\begin{matrix} \sigma I+\phi×&p\\ 0^T&0\\ \end{matrix}\right]\in R^{4×4}\\ \end{matrix}\right\}
sim(3)=⎩⎨⎧ζ∣ζ=⎣⎡pθσ⎦⎤∈R7×1,ζ×=[σI+ϕ×0Tp0]∈R4×4⎭⎬⎫
相似变换群的李代数到群的变化:
s
i
m
(
3
)
−
>
S
i
m
(
3
)
sim(3)->Sim(3)
sim(3)−>Sim(3)
e
ζ
×
=
[
e
σ
e
ϕ
×
J
s
p
0
T
1
]
e^{\zeta×}=\left[\begin{matrix} e^{\sigma}e^{\phi×}&J_sp\\ 0^T&1\\ \end{matrix}\right]
eζ×=[eσeϕ×0TJsp1]
J
s
=
e
σ
−
1
σ
I
+
σ
e
σ
s
i
n
θ
+
(
1
−
e
σ
c
o
s
θ
)
σ
2
+
θ
2
a
×
+
(
e
σ
−
1
σ
−
(
e
σ
s
i
n
θ
)
θ
+
(
e
σ
c
o
s
θ
−
1
)
σ
σ
2
+
θ
2
)
(
a
×
)
2
J_s=\frac{e^{\sigma}-1}{\sigma}I+\frac{\sigma e^{\sigma}sin\theta+(1-e^{\sigma}cos\theta)}{\sigma^2+\theta^2}a×+(\frac{e^{\sigma}-1}{\sigma}-\frac{ (e^{\sigma}sin\theta)\theta+(e^{\sigma}cos\theta-1)\sigma}{\sigma^2+\theta^2})(a×)^2
Js=σeσ−1I+σ2+θ2σeσsinθ+(1−eσcosθ)a×+(σeσ−1−σ2+θ2(eσsinθ)θ+(eσcosθ−1)σ)(a×)2
相似变换群的扰动求导:
∂
S
p
∂
ζ
=
[
I
−
q
×
q
0
T
0
T
0
]
\frac{\partial Sp}{\partial \zeta}=\left[\begin{matrix} I&-q×&q\\ 0^T&0^T&0\\ \end{matrix}\right]
∂ζ∂Sp=[I0T−q×0Tq0]