高博的书上给出了
S
O
(
3
)
SO(3)
SO(3)的指数映射推导，但对于
S
E
(
3
)
SE(3)
SE(3),仅介绍了结论，没有给出详细推导。
最近在补李群和李代数基础，就当做加深理解，自己推一遍。
设
ξ
=
[
ρ
,
ϕ
]
T
∈
s
e
(
3
)
\boldsymbol{\xi} = \left[ \rho , \phi\right] ^T \in \mathfrak{se}(3)
ξ=[ρ,ϕ]T∈se(3)，它的指数映射为:
exp
(
ξ
∧
)
=
[
∑
n
=
0
∞
1
n
!
(
ϕ
∧
)
n
∑
n
=
0
∞
1
(
n
+
1
)
!
(
ϕ
∧
)
n
ρ
0
T
1
]
\exp(\mathfrak{\boldsymbol{\xi}}^{\wedge}) = \begin{bmatrix} \sum\limits_{n=0}^{\infty} \frac{1}{n!}(\phi^{\wedge})^n & \sum\limits_{n=0}^{\infty} \frac{1}{(n+1)!}(\phi^{\wedge})^n \boldsymbol{\rho} \\ \boldsymbol{0}^T & 1 \end{bmatrix}
exp(ξ∧)=⎣⎡n=0∑∞n!1(ϕ∧)n0Tn=0∑∞(n+1)!1(ϕ∧)nρ1⎦⎤
令
ρ
=
θ
a
\boldsymbol{\rho}=\theta \boldsymbol{a}
ρ=θa，那么:
∑
n
=
0
∞
1
(
n
+
1
)
!
(
ϕ
∧
)
n
=
sin
θ
θ
I
+
(
1
−
sin
θ
θ
)
a
a
T
+
1
−
cos
θ
θ
a
∧
=
J
.
\sum\limits_{n=0}^{\infty} \frac{1}{(n+1)!}(\phi ^{\wedge})^n = \frac{\sin \theta}{\theta}I + (1- \frac{\sin \theta}{\theta})\boldsymbol{a}\boldsymbol{a}^T + \frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge} = \boldsymbol{J}.
n=0∑∞(n+1)!1(ϕ∧)n=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧=J.
下面证明：
已知
ξ
∧
=
[
ϕ
∧
ρ
0
T
,
0
]
\boldsymbol{{\xi}}^{\wedge} = \begin{bmatrix} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ \boldsymbol{0}^{T}, & 0 \end{bmatrix}
ξ∧=[ϕ∧0T,ρ0]
那么
exp
(
ξ
∧
)
=
∑
n
=
0
∞
1
n
!
[
ϕ
∧
ρ
0
T
,
0
]
n
=
I
+
∑
n
=
1
∞
1
n
!
[
θ
a
∧
ρ
0
T
,
0
]
n
=
[
I
0
0
1
]
+
∑
n
=
1
∞
1
n
!
[
(
θ
a
∧
)
n
(
θ
a
∧
)
n
−
1
ρ
0
T
,
0
]
=
[
∑
n
=
0
∞
1
n
!
(
θ
a
∧
)
n
∑
n
=
1
∞
1
n
!
(
θ
a
∧
)
n
−
1
ρ
0
T
,
1
]
\begin{aligned} \exp(\boldsymbol{{\xi}}^{\wedge}) &= \sum\limits_{n=0}^{\infty} \frac{1}{n!}\begin{bmatrix} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ \boldsymbol{0}^{T}, & 0 \end{bmatrix}^n \\ &= \boldsymbol{I} + \sum\limits_{n=1}^{\infty} \frac{1}{n!}\begin{bmatrix} {\theta}\boldsymbol{a}^{\wedge} & \boldsymbol{\rho} \\ \boldsymbol{0}^{T}, & 0 \end{bmatrix}^n \\ &= \begin{bmatrix} \boldsymbol{I} & \boldsymbol{0} \\ 0 & 1 \end{bmatrix} + \sum\limits_{n=1}^{\infty} \frac{1}{n!}\begin{bmatrix} (\theta\boldsymbol{a}^{\wedge})^n & (\theta\boldsymbol{a}^{\wedge})^{n-1 }\boldsymbol{\rho} \\ \boldsymbol{0}^{T}, & 0 \end{bmatrix} \\ &= \begin{bmatrix} \sum\limits_{n=0}^{\infty} \frac{1}{n!}(\theta\boldsymbol{a}^{\wedge})^n & \sum\limits_{n=1}^{\infty} \frac{1}{n!}(\theta\boldsymbol{a}^{\wedge})^{n-1 }\boldsymbol{\rho} \\ \boldsymbol{0}^{T}, & 1 \end{bmatrix} \end{aligned}
exp(ξ∧)=n=0∑∞n!1[ϕ∧0T,ρ0]n=I+n=1∑∞n!1[θa∧0T,ρ0]n=[I001]+n=1∑∞n!1[(θa∧)n0T,(θa∧)n−1ρ0]=⎣⎡n=0∑∞n!1(θa∧)n0T,n=1∑∞n!1(θa∧)n−1ρ1⎦⎤
现在令
n
=
n
−
1
n=n-1
n=n−1,那么
n
=
n
+
1
n=n+1
n=n+1（做一步替换变量），带入上式右上角的一项，得
exp
(
ξ
∧
)
=
[
∑
n
=
0
∞
1
n
!
(
θ
a
∧
)
n
∑
n
=
0
∞
1
(
n
+
1
)
!
(
θ
a
∧
)
n
ρ
0
T
,
1
]
\exp(\boldsymbol{\xi}^\wedge)= \begin{bmatrix} \sum\limits_{n=0}^{\infty} \frac{1}{n!}(\theta\boldsymbol{a}^{\wedge})^{n} & \sum\limits_{n=0}^{\infty} \frac{1}{(n+1)!}(\theta\boldsymbol{a}^{\wedge})^{n}\boldsymbol{\rho} \\ \boldsymbol{0}^{T}, & 1 \end{bmatrix}
exp(ξ∧)=⎣⎡n=0∑∞n!1(θa∧)n0T,n=0∑∞(n+1)!1(θa∧)nρ1⎦⎤
S
E
(
3
)
SE(3)
SE(3)的指数映射形式推导完毕。下面推导左雅克比的形式
设
ρ
=
θ
a
\boldsymbol{\rho} = \theta \boldsymbol{a}
ρ=θa，
当
∥
a
∥
=
1
\Vert \boldsymbol{a} \Vert = 1
∥a∥=1时，已知可以用来化简的结论：
a
∧
a
∧
=
a
a
T
−
I
a
∧
a
∧
a
∧
=
a
∧
(
a
T
−
I
)
=
−
a
∧
\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge} = \boldsymbol{a}\boldsymbol{a}^T - \boldsymbol{I} \\ \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge} = \boldsymbol{a}^{\wedge}(\boldsymbol{a}^T - \boldsymbol{I}) = -\boldsymbol{a}^{\wedge}
a∧a∧=aaT−Ia∧a∧a∧=a∧(aT−I)=−a∧
进而
J
=
∑
n
=
0
∞
1
(
n
+
1
)
!
(
θ
a
∧
)
n
=
I
+
1
2
!
(
θ
a
∧
)
+
1
3
!
(
θ
a
∧
)
2
+
1
4
!
(
θ
a
∧
)
3
+
⋯
+
1
(
n
+
1
)
!
(
θ
a
∧
)
n
=
I
+
1
2
!
(
θ
a
∧
)
+
1
3
!
(
θ
a
∧
)
2
−
1
4
!
θ
3
(
a
∧
)
−
1
5
!
θ
4
(
a
∧
)
2
+
1
6
!
θ
5
(
a
∧
)
−
1
7
!
θ
7
(
a
∧
)
2
+
⋯
=
1
θ
(
1
2
!
θ
2
−
1
4
!
θ
4
+
⋯
)
a
∧
+
1
θ
(
1
3
!
θ
3
−
1
5
!
θ
5
+
⋯
)
(
a
∧
)
2
+
I
\begin{aligned} \boldsymbol{J} &= \sum\limits_{n=0}^{\infty} \frac{1}{(n+1)!}(\theta \boldsymbol{a}^{\wedge})^n \\ &= \boldsymbol{I} + \frac{1}{2!}(\theta \boldsymbol{a}^{\wedge}) + \frac{1}{3!}(\theta \boldsymbol{a}^{\wedge})^2 + \frac{1}{4!}(\theta \boldsymbol{a}^{\wedge})^3 + \cdots + \frac{1}{(n+1)!}(\theta \boldsymbol{a}^{\wedge})^n \\ &=\boldsymbol{I} + \frac{1}{2!}(\theta \boldsymbol{a}^{\wedge}) + \frac{1}{3!}(\theta \boldsymbol{a}^{\wedge})^2 - \frac{1}{4!}\theta^3 (\boldsymbol{a}^{\wedge}) - \frac{1}{5!}\theta^4 (\boldsymbol{a}^{\wedge})^2 + \frac{1}{6!}\theta^5(\boldsymbol{a}^{\wedge}) - \frac{1}{7!}\theta^7 (\boldsymbol{a}^{\wedge})^2 +\cdots \\ &= \frac{1}{\theta}(\frac{1}{2!}\theta^2 - \frac{1}{4!}\theta^4 + \cdots)\boldsymbol{a}^{\wedge} + \frac{1}{\theta}(\frac{1}{3!}\theta^3 - \frac{1}{5!}\theta^5 + \cdots)(\boldsymbol{a}^{\wedge})^2 + \boldsymbol{I} \end{aligned}
J=n=0∑∞(n+1)!1(θa∧)n=I+2!1(θa∧)+3!1(θa∧)2+4!1(θa∧)3+⋯+(n+1)!1(θa∧)n=I+2!1(θa∧)+3!1(θa∧)2−4!1θ3(a∧)−5!1θ4(a∧)2+6!1θ5(a∧)−7!1θ7(a∧)2+⋯=θ1(2!1θ2−4!1θ4+⋯)a∧+θ1(3!1θ3−5!1θ5+⋯)(a∧)2+I
由
sin
θ
\sin\theta
sinθ和
cos
θ
\cos\theta
cosθ的泰勒展开公式
sin
θ
=
θ
−
1
3
!
θ
3
+
1
5
!
θ
5
−
1
7
!
θ
7
+
⋯
cos
θ
=
1
−
1
2
!
θ
2
+
1
4
!
θ
4
−
1
6
!
θ
6
+
⋯
\begin{aligned} \sin\theta &= \theta - \frac{1}{3!}\theta^3 + \frac{1}{5!}\theta^5 - \frac{1}{7!}\theta^7+ \cdots \\ \cos\theta &= 1 - \frac{1}{2!}\theta^2 + \frac{1}{4!}\theta^4 - \frac{1}{6!}\theta^6+ \cdots \end{aligned}
sinθcosθ=θ−3!1θ3+5!1θ5−7!1θ7+⋯=1−2!1θ2+4!1θ4−6!1θ6+⋯
J
\boldsymbol{J}
J可进一步化简为
J
=
1
θ
(
1
−
cos
θ
)
(
a
∧
)
+
θ
−
sin
θ
θ
(
a
a
T
−
I
)
+
I
=
sin
θ
θ
I
+
(
1
−
sin
θ
θ
)
a
a
T
+
1
−
cos
θ
θ
a
∧
\begin{aligned} \boldsymbol{J} &= \frac{1}{\theta}(1- \cos\theta)(\boldsymbol{a}^{\wedge}) + \frac{\theta- \sin \theta}{\theta}( \boldsymbol{a}\boldsymbol{a}^T - \boldsymbol{I}) + \boldsymbol{I} \\ &= \frac{\sin \theta}{\theta}I + (1- \frac{\sin \theta}{\theta})\boldsymbol{a}\boldsymbol{a}^T + \frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge} \end{aligned}
J=θ1(1−cosθ)(a∧)+θθ−sinθ(aaT−I)+I=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧
左雅克比矩阵的形式推导完毕。
参考：https://blog.csdn.net/zengxyuyu/article/details/105646751#commentBox
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
