[Title]: Notes for “A micro Lie theory for state estimation in robotics” - 公式详细推导 (I)
文章目录
“A micro Lie theory for state estimation in robotics” 1 这篇论文中比较难的是几个Jacobian矩阵的推导,也是这个笔记的主要内容.
在下面推导笔记中,形如 = ( 31 ) \overset{(31)}= =(31) 或 ⇒ ( 31 ) \overset{(31)}\Rightarrow ⇒(31) 意味着该步推导依据的是论文1 中 eq(31).
I. About “Example 6”
- 对于任意旋转矩阵 R R R及向量 a a a 和 b b b,有
R a × R b = R ( a × b ) Ra \times Rb =R(a \times b) Ra×Rb=R(a×b)
- 向量 u u u,定义对应的线性变换是 u × u_\times u×
u × : v → u × v u_\times: v \rightarrow u \times v u×:v→u×v
也可以写成 u × ≜ [ u ] × u_\times \triangleq [u]_\times u×≜[u]×。以及有
[ a ] × b = a × b = − b × a = − [ b ] × a [a]_\times b = a \times b = - b \times a = -[b]_\times a [a]×b=a×b=−b×a=−[b]×a
对于任意向量
θ
∈
R
3
\theta \in \mathbb{R}^3
θ∈R3 及
R
∈
S
O
(
3
)
R \in SO(3)
R∈SO(3),可以计算
[
R
θ
]
×
v
=
(
R
θ
)
×
v
=
(
R
θ
)
×
(
R
R
−
1
v
)
=
R
(
θ
×
R
−
1
v
)
=
R
(
[
θ
]
×
R
−
1
v
)
=
R
[
θ
]
×
R
−
1
v
\begin{aligned} \left[ R\theta \right]_ \times v &= (R\theta)\times v \\ &= (R\theta)\times (RR^{-1}v) \\ &= R(\theta\times R^{-1}v) \\ &= R([\theta]_\times R^{-1}v) \\ &= R[\theta]_\times R^{-1}v \end{aligned}
[Rθ]×v=(Rθ)×v=(Rθ)×(RR−1v)=R(θ×R−1v)=R([θ]×R−1v)=R[θ]×R−1v
所以:
[
R
θ
]
×
=
R
[
θ
]
×
R
−
1
=
R
[
θ
]
×
R
T
(I-1)
\left[ R\theta \right]_ \times = R[\theta]_\times R^{-1} = R[\theta]_\times R^{\rm T} \tag{I-1}
[Rθ]×=R[θ]×R−1=R[θ]×RT(I-1)
依据"Example 6"中推导,已知
M
=
[
R
t
0
1
]
M = \begin{bmatrix} R &t\\ 0 &1 \end{bmatrix}
M=[R0t1]
时,
A
d
M
=
[
R
[
t
]
×
R
0
R
]
∈
R
6
{\rm Ad}_{\rm M} = \begin{bmatrix} R &[t]_\times R \\ 0 & R \end{bmatrix} \in {\mathbb R}^6
AdM=[R0[t]×RR]∈R6
将
M
−
1
M^{-1}
M−1对应各部分代入,得到
A
d
M
−
1
=
[
R
T
[
−
R
T
t
]
×
R
T
0
R
T
]
{\rm Ad}_{\rm M^{-1}} = \begin{bmatrix} R^{\rm T} &[-R^{\rm T}t]_\times R^{\rm T} \\ 0 & R^{\rm T} \end{bmatrix}
AdM−1=[RT0[−RTt]×RTRT]
以及,计算
A
d
M
\rm Ad_M
AdM的逆
A
d
M
−
1
=
[
R
T
−
R
T
[
t
]
×
0
R
T
]
{{\rm Ad}_{\rm M}}^{-1} = \begin{bmatrix} R^{\rm T} &- R^{\rm T}[t]_\times \\ 0 & R^{\rm T} \end{bmatrix}
AdM−1=[RT0−RT[t]×RT]
根据 (
I
-
1
\rm I\text{-}1
I-1) 有
[
−
R
T
t
]
×
R
T
=
R
T
R
[
−
R
T
t
]
×
R
T
=
R
T
[
R
(
−
R
T
t
)
]
×
=
−
R
T
[
t
]
×
[-R^{\rm T}t]_\times R^{\rm T} = R^{\rm T}R[-R^{\rm T}t]_\times R^{\rm T} = R^{\rm T} [R(-R^{\rm T}t)]_\times = - R^{\rm T}[t]_\times
[−RTt]×RT=RTR[−RTt]×RT=RT[R(−RTt)]×=−RT[t]×
所以有
A
d
M
−
1
=
A
d
M
−
1
\rm Ad_{M^{-1}} = \rm {Ad_M}^{-1}
AdM−1=AdM−1, 即论文中 eq(33).
II. About eq(46)
论文中Figure 9中知,
ϵ
σ
=
ϵ
D
y
D
x
ϵ
τ
(II-1)
^\epsilon \sigma = {\frac{^\epsilon Dy}{Dx}} {^\epsilon\tau} \tag{II-1}
ϵσ=DxϵDyϵτ(II-1)
其中
ϵ
σ
=
A
d
y
y
σ
^\epsilon \sigma = {\rm Ad}_y {^y \sigma}
ϵσ=Adyyσ
ϵ τ = A d x x τ ^\epsilon \tau = {\rm Ad}_x {^x \tau} ϵτ=Adxxτ
将
ϵ
σ
^\epsilon \sigma
ϵσ 和
ϵ
τ
^\epsilon \tau
ϵτ 分别代入
(
I
I
-
1
)
\rm (II\text{-}1)
(II-1) 可以得到
A
d
y
y
σ
=
ϵ
D
y
D
x
A
d
x
x
τ
(II-2)
{\rm Ad}_y {^y \sigma} = {\frac{^\epsilon Dy}{Dx}} {\rm Ad}_x {^x \tau} \tag{II-2}
Adyyσ=DxϵDyAdxxτ(II-2)
又由论文中 Figure 9 中已知
y
σ
=
x
D
y
D
x
x
τ
^y \sigma = {\frac{^x Dy}{Dx}} {^x\tau}
yσ=DxxDyxτ
代入
(
I
I
-
2
)
(\rm II\text{-}2)
(II-2) 得到
A
d
y
x
D
y
D
x
x
τ
=
ϵ
D
y
D
x
A
d
x
x
τ
{\rm Ad}_y {\frac{^x Dy}{Dx}} {^x\tau} = {\frac{^\epsilon Dy}{Dx}} {\rm Ad}_x {^x \tau}
AdyDxxDyxτ=DxϵDyAdxxτ
把
A
d
y
{\rm Ad}_y
Ady 和
A
d
x
{\rm Ad}_x
Adx 都看作矩阵运算,得到
A
d
y
x
D
y
D
x
=
ϵ
D
y
D
x
A
d
x
⇒
ϵ
D
f
(
x
)
D
x
A
d
x
=
A
d
f
(
x
)
x
D
f
(
x
)
D
x
{\rm Ad}_y {\frac{^x Dy}{Dx}} = {\frac{^\epsilon Dy}{Dx}} {\rm Ad}_x \ \ \Rightarrow \ \ {\frac{^\epsilon Df(x)}{Dx}} {\rm Ad}_x = {\rm Ad}_{f(x)} {\frac{^x Df(x)}{Dx}}
AdyDxxDy=DxϵDyAdx ⇒ DxϵDf(x)Adx=Adf(x)DxxDf(x)
就是论文中 eq(46).
III. About eq(68)~eq(74)
论文中 Figure 81 中是关于函数
f
:
M
→
N
f: \mathcal{M}\rightarrow \mathcal{N}
f:M→N 的 Jacobian. 即 Jacobian maps differential increments in the Lie Algebra:
J
:
T
x
M
→
T
f
(
x
)
N
x
τ
→
f
(
x
)
σ
\begin{aligned} J: T_x M \ &\rightarrow \ T_{f(x)} N \\ ^{x}\tau \ &\rightarrow \ ^{f(x)}\sigma \end{aligned}
J:TxM xτ → Tf(x)N→ f(x)σ
流形上的Right Jacobian定义参见论文中 eq(68),需要特别注意 该定义的是流形
M
\mathcal{M}
M 上的 Jacobian,把参数
τ
\boldsymbol \tau
τ 的变化 (李代数,单位元处切空间的变化) 映射为
E
x
p
(
τ
)
{\rm Exp}(\boldsymbol \tau)
Exp(τ) 局部切空间上的变化. 都映射到切空间上,所以 eq(41b) 有
L
o
g
\rm Log
Log.
下面推导 eq(68)
J
r
(
τ
)
=
(
67
)
τ
D
E
x
p
(
τ
)
D
τ
=
(
41
a
)
lim
δ
τ
→
0
E
x
p
(
τ
⊕
δ
τ
)
⊖
E
x
p
(
τ
)
δ
τ
=
(
41
c
)
lim
δ
τ
→
0
L
o
g
[
E
x
p
(
τ
)
−
1
E
x
p
(
τ
+
δ
τ
)
]
δ
τ
\begin{aligned} J_r({\tau}) &\overset{(67)}{=} \frac{^\tau D {\rm Exp}(\tau)}{D\tau}\\ &\overset{(41a)} = {\lim_{\delta \tau \rightarrow 0}} \frac{{\rm Exp}(\tau \oplus \delta \tau)\, \ominus\, {\rm Exp}(\tau) }{\delta \tau}\\ & \overset{(41c)} = {\lim_{\delta \tau \rightarrow 0}} \frac{{\rm Log} \left[ {{\rm Exp}(\tau)}^{-1}\, {\rm Exp}(\tau + \delta \tau) \right] } {\delta \tau}\\ \end{aligned}
Jr(τ)=(67)DττDExp(τ)=(41a)δτ→0limδτExp(τ⊕δτ)⊖Exp(τ)=(41c)δτ→0limδτLog[Exp(τ)−1Exp(τ+δτ)]
当
δ
τ
\delta \tau
δτ 很小时,
J
r
(
τ
)
δ
τ
=
L
o
g
[
E
x
p
(
τ
)
−
1
E
x
p
(
τ
+
δ
τ
)
]
⇒
E
x
p
(
J
r
(
τ
)
δ
τ
)
=
E
x
p
(
τ
)
−
1
E
x
p
(
τ
+
δ
τ
)
⇒
E
x
p
(
τ
)
E
x
p
(
J
r
(
τ
)
δ
τ
)
=
E
x
p
(
τ
+
δ
τ
)
\begin{aligned} J_r({\tau})\, {\delta \tau} & = {\rm Log} \left[ {{\rm Exp}(\tau)}^{-1}\, {\rm Exp}(\tau + \delta \tau) \right] \\ \Rightarrow\ {\rm Exp} \left( J_r({\tau})\, {\delta \tau}\right) & = {{\rm Exp}(\tau)}^{-1}\, {\rm Exp}(\tau + \delta \tau) \\ \Rightarrow\ {{\rm Exp}(\tau)}\, {\rm Exp} \left( J_r({\tau})\, {\delta \tau}\right) & = {\rm Exp}(\tau + \delta \tau) \end{aligned}
Jr(τ)δτ⇒ Exp(Jr(τ)δτ)⇒ Exp(τ)Exp(Jr(τ)δτ)=Log[Exp(τ)−1Exp(τ+δτ)]=Exp(τ)−1Exp(τ+δτ)=Exp(τ+δτ)
得到 eq(68). 注意上面
τ
\tau
τ 和
δ
τ
\delta \tau
δτ 都
∈
R
m
\in \mathbb{R}^{m}
∈Rm,所以
⊕
=
+
\oplus = +
⊕=+;而
E
x
p
(
τ
)
∈
M
{\rm Exp}(\tau) \in \mathcal{M}
Exp(τ)∈M,所示还是 “right-
⊖
\ominus
⊖” defined in eq(26).
eq(72) 与 eq(68) 类似计算.
下面计算 eq(69),由 eq(67) 已知
J
r
(
τ
)
∈
R
m
×
m
J_r (\tau) \in \mathbb{R} ^{m \times m}
Jr(τ)∈Rm×m,所以就当
J
r
(
τ
)
−
1
δ
τ
∈
R
m
{J_r(\tau)}^{-1}\,\delta\tau \in \mathbb{R}^{m}
Jr(τ)−1δτ∈Rm 只是对
δ
τ
∈
R
m
\delta\tau\in \mathbb{R}^{m}
δτ∈Rm 尺寸的变换而不改变
δ
τ
\delta\tau
δτ 所处的空间.
J
r
(
τ
)
=
(
67
)
τ
D
E
x
p
(
τ
)
D
τ
=
(
41
a
)
lim
J
r
−
1
δ
τ
→
0
E
x
p
(
τ
⊕
J
r
−
1
δ
τ
)
⊖
E
x
p
(
τ
)
J
r
−
1
δ
τ
=
(
41
c
)
lim
δ
τ
→
0
L
o
g
[
E
x
p
(
τ
)
−
1
E
x
p
(
τ
+
J
r
−
1
δ
τ
)
]
J
r
−
1
δ
τ
\begin{aligned} J_r({\tau}) &\overset{(67)}{=} \frac{^\tau D {\rm Exp}(\tau)}{D\tau}\\ &\overset{(41a)} = {\lim_{J_{r}^{-1}\delta \tau \rightarrow 0}} \frac{{\rm Exp}(\tau \oplus {J_{r}^{-1}}\delta \tau)\, \ominus\, {\rm Exp}(\tau) }{{J_{r}^{-1}}\delta \tau}\\ & \overset{(41c)} = {\lim_{\delta \tau \rightarrow 0}} \frac{{\rm Log} \left[ {{\rm Exp}(\tau)}^{-1}\, {\rm Exp}(\tau + {J_{r}^{-1}}\delta \tau) \right] } {{J_{r}^{-1}}\delta \tau} \end{aligned}
Jr(τ)=(67)DττDExp(τ)=(41a)Jr−1δτ→0limJr−1δτExp(τ⊕Jr−1δτ)⊖Exp(τ)=(41c)δτ→0limJr−1δτLog[Exp(τ)−1Exp(τ+Jr−1δτ)]
当
δ
τ
\delta \tau
δτ 很小时,
J
r
(
τ
)
J
r
−
1
δ
τ
=
L
o
g
[
E
x
p
(
τ
)
−
1
E
x
p
(
τ
+
J
r
−
1
δ
τ
)
]
⇒
E
x
p
(
δ
τ
)
=
E
x
p
(
τ
)
−
1
E
x
p
(
τ
+
J
r
−
1
δ
τ
)
⇒
E
x
p
(
τ
)
E
x
p
(
δ
τ
)
=
E
x
p
(
τ
+
J
r
−
1
δ
τ
)
⇒
L
o
g
(
E
x
p
(
τ
)
E
x
p
(
δ
τ
)
)
=
τ
+
J
r
−
1
δ
τ
\begin{aligned} J_r({\tau}) {{J_{r}^{-1}}\delta \tau} &= {\rm Log} \left[ {{\rm Exp}(\tau)}^{-1}\, {\rm Exp}(\tau + {J_{r}^{-1}}\delta \tau) \right] \\ \Rightarrow\ {\rm Exp}(\delta \tau) &= {{\rm Exp}(\tau)}^{-1}\, {\rm Exp}(\tau + {J_{r}^{-1}}\delta \tau) \\ \Rightarrow\ {{\rm Exp}(\tau)}\, {\rm Exp}(\delta \tau) &= \, {\rm Exp}(\tau + {{J_{r}}^{-1}}\delta \tau) \\ \Rightarrow\ {\rm Log}\left({{\rm Exp}(\tau)}\, {\rm Exp}(\delta \tau)\right) &= \tau + {{J_{r}}^{-1}}\delta \tau \end{aligned}
Jr(τ)Jr−1δτ⇒ Exp(δτ)⇒ Exp(τ)Exp(δτ)⇒ Log(Exp(τ)Exp(δτ))=Log[Exp(τ)−1Exp(τ+Jr−1δτ)]=Exp(τ)−1Exp(τ+Jr−1δτ)=Exp(τ+Jr−1δτ)=τ+Jr−1δτ
得到 eq(69) 和 eq(70). eq(73) 和 eq(74) 类似.
另外一种解释如下:
参考 “State Estimation for Robotics”,通过BCH公式 (Baker-Campbell-Hausdorff formula) 计算2 ,可以获得关于 τ \boldsymbol \tau τ 和 δ τ \delta \boldsymbol \tau δτ 的多项式,如 "State Estimation for Robotics"中的 (7.74). 因为 δ τ ≪ τ \delta \boldsymbol \tau \ll \boldsymbol \tau δτ≪τ, 可以将多项式 (7.74) 中的 δ τ \delta \boldsymbol \tau δτ 二次项及以上项都去除掉,得到 (7.75), 即可对应论文中的 eq(69)、eq(70)、eq(73)、eq(74). 实际上,Jacobina 及其逆的级数求和形式也都是从该BCH公式近似推导而来.
IV. About eq(75)
由 eq(68) 和 eq(72) 可知
E
x
p
(
τ
)
E
x
p
(
J
r
(
τ
)
δ
τ
)
=
E
x
p
(
J
l
(
τ
)
δ
τ
)
E
x
p
(
τ
)
⇒
E
x
p
(
τ
)
E
x
p
(
J
r
(
τ
)
δ
τ
)
E
x
p
(
τ
)
−
1
=
E
x
p
(
J
l
(
τ
)
δ
τ
)
\begin{aligned} {\rm Exp}(\tau)\,{\rm Exp}(J_{r}(\tau)\,\delta \tau) &={\rm Exp}(J_{l}(\tau)\,\delta \tau)\, {\rm Exp}(\tau)\\ \Rightarrow\ {\rm Exp}(\tau)\,{\rm Exp}(J_{r}(\tau)\,\delta \tau)\, {{\rm Exp}(\tau)}^{-1} & ={\rm Exp}(J_{l}(\tau)\,\delta \tau) \end{aligned}
Exp(τ)Exp(Jr(τ)δτ)⇒ Exp(τ)Exp(Jr(τ)δτ)Exp(τ)−1=Exp(Jl(τ)δτ)Exp(τ)=Exp(Jl(τ)δτ)
上式中
E
x
p
(
J
r
(
τ
)
δ
τ
)
=
(
23
)
e
x
p
[
(
J
r
(
τ
)
δ
τ
)
∧
]
=
(
16
)
I
+
(
J
r
(
τ
)
δ
τ
)
∧
+
1
2
(
J
r
(
τ
)
δ
τ
)
∧
2
+
.
.
.
{\rm Exp}(J_{r}(\tau)\,\delta \tau) \overset{(23)}= {\rm exp}\left[ (J_{r}(\tau)\,\delta \tau) ^{\wedge} \right] \overset{(16)}= I\, +\, (J_{r}(\tau)\,\delta \tau) ^{\wedge} \, + \, \frac{1}{2}{(J_{r}(\tau)\,\delta \tau) ^{\wedge}}^2\, +\, ...\\
Exp(Jr(τ)δτ)=(23)exp[(Jr(τ)δτ)∧]=(16)I+(Jr(τ)δτ)∧+21(Jr(τ)δτ)∧2+...
则
E
x
p
(
τ
)
E
x
p
(
J
r
(
τ
)
δ
τ
)
E
x
p
(
τ
)
−
1
=
I
+
E
x
p
(
τ
)
(
J
r
(
τ
)
δ
τ
)
∧
E
x
p
(
τ
)
−
1
+
E
x
p
(
τ
)
1
2
(
J
r
(
τ
)
δ
τ
)
∧
2
E
x
p
(
τ
)
−
1
(IV-1)
{\rm Exp}(\tau)\,{\rm Exp}(J_{r}(\tau)\,\delta \tau)\, {{\rm Exp}(\tau)}^{-1} = I + {\rm Exp}(\tau) \,(J_{r}(\tau)\,\delta \tau) ^{\wedge}\, {{\rm Exp}(\tau)}^{-1} \, + \, {\rm Exp}(\tau)\,{\frac{1}{2}{(J_{r}(\tau)\,\delta \tau) ^{\wedge}}^2}\, {{\rm Exp}(\tau)}^{-1}\tag{IV-1}
Exp(τ)Exp(Jr(τ)δτ)Exp(τ)−1=I+Exp(τ)(Jr(τ)δτ)∧Exp(τ)−1+Exp(τ)21(Jr(τ)δτ)∧2Exp(τ)−1(IV-1)
另有
E
x
p
(
J
l
(
τ
)
δ
τ
)
=
(
23
)
e
x
p
[
(
J
l
(
τ
)
δ
τ
)
∧
]
=
(
16
)
I
+
(
J
l
(
τ
)
δ
τ
)
∧
+
1
2
(
J
l
(
τ
)
δ
τ
)
∧
2
+
.
.
.
(IV-2)
{\rm Exp}(J_{l}(\tau)\,\delta \tau) \overset{(23)}= {\rm exp}\left[ (J_{l}(\tau)\,\delta \tau) ^{\wedge} \right] \overset{(16)}= I\, +\, (J_{l}(\tau)\,\delta \tau) ^{\wedge} \, + \, \frac{1}{2}{(J_{l}(\tau)\,\delta \tau) ^{\wedge}}^2\, +\, ... \tag{IV-2}
Exp(Jl(τ)δτ)=(23)exp[(Jl(τ)δτ)∧]=(16)I+(Jl(τ)δτ)∧+21(Jl(τ)δτ)∧2+...(IV-2)
其中
δ
τ
∈
R
m
\delta \tau\in \mathbb{R}^m
δτ∈Rm 是任意的向量(变量),把 (
IV-1
\text{IV-1}
IV-1) 和 (
IV-2
\text{IV-2}
IV-2) 看作是
δ
τ
\delta \tau
δτ 的多项式,所以需要 (
IV-1
\text{IV-1}
IV-1) 和 (
IV-2
\text{IV-2}
IV-2) 的
δ
τ
\delta\tau
δτ 对应各次项系数相同. 一次项系数相同可以得到
E
x
p
(
τ
)
(
J
r
(
τ
)
δ
τ
)
∧
E
x
p
(
τ
)
−
1
=
(
J
l
(
τ
)
δ
τ
)
∧
⇒
[
E
x
p
(
τ
)
(
J
r
(
τ
)
δ
τ
)
∧
E
x
p
(
τ
)
−
1
]
∨
=
J
l
(
τ
)
δ
τ
⇒
(
31
)
A
d
E
x
p
(
τ
)
(
J
r
(
τ
)
δ
τ
)
=
J
l
(
τ
)
δ
τ
\begin{aligned} {\rm Exp}(\tau) \,(J_{r}(\tau)\,\delta \tau) ^{\wedge}\, {{\rm Exp}(\tau)}^{-1} &= (J_{l}(\tau)\,\delta \tau) ^{\wedge}\\ \Rightarrow\ \left[{\rm Exp}(\tau) \,(J_{r}(\tau)\,\delta \tau) ^{\wedge}\, {{\rm Exp}(\tau)}^{-1} \right]^{\vee} &= J_{l}(\tau)\,\delta \tau\\ \overset{(31)}\Rightarrow \ {\rm Ad}_{{\rm Exp}(\tau)} ({J_r (\tau)\, \delta\tau}) &= J_{l}(\tau)\, \delta\tau \end{aligned}
Exp(τ)(Jr(τ)δτ)∧Exp(τ)−1⇒ [Exp(τ)(Jr(τ)δτ)∧Exp(τ)−1]∨⇒(31) AdExp(τ)(Jr(τ)δτ)=(Jl(τ)δτ)∧=Jl(τ)δτ=Jl(τ)δτ
根据论文 eq(31) 中,把
A
d
X
{\rm Ad}_X
AdX 作为 “matrix operator”, 即称为 “adjoint matrix”. 这样把
A
d
E
x
p
(
τ
)
{\rm Ad}_{{\rm Exp}(\tau)}
AdExp(τ) 看作矩阵表示的线性映射,根据矩阵乘法的结合律就有
A
d
E
x
p
(
τ
)
J
r
(
τ
)
=
J
l
(
τ
)
⇒
A
d
E
x
p
(
τ
)
=
J
l
(
τ
)
J
r
(
τ
)
−
1
\begin{aligned} {\rm Ad}_{{\rm Exp}(\tau)}\, {J_r (\tau)} &= J_{l}(\tau)\\ \Rightarrow\ {\rm Ad}_{{\rm Exp}(\tau)} &= J_{l}(\tau)\, {{J_r (\tau)}}^{-1} \end{aligned}
AdExp(τ)Jr(τ)⇒ AdExp(τ)=Jl(τ)=Jl(τ)Jr(τ)−1
获得 eq(75).
V. About eq(79)
方法一:
由论文中 eq(57) 下面一行对
J
x
f
(
x
)
J_{x}^{f(x)}
Jxf(x) 的定义, 以及题设中
τ
=
L
o
g
(
x
)
\tau={\rm Log}(x)
τ=Log(x),可知
J
x
L
o
g
(
x
)
=
(
定义
)
D
L
o
g
(
x
)
D
x
=
(
题设
)
D
τ
D
(
E
x
p
(
τ
)
)
=
1
D
(
E
x
p
(
τ
)
)
D
τ
=
(
67
)
J
r
(
τ
)
−
1
\begin{aligned} J_x^{{\rm Log}(x)} &\overset{(定义)}= \frac{D {\rm Log}(x)}{D x}\\ &\overset{(题设)}=\frac{D\tau}{D({\rm Exp}(\tau))}\\ &=\frac{1}{\frac{D({\rm Exp}(\tau))}{D\tau}}\\ &\overset{(67)}={J_r(\tau)}^{-1} \end{aligned}
JxLog(x)=(定义)DxDLog(x)=(题设)D(Exp(τ))Dτ=DτD(Exp(τ))1=(67)Jr(τ)−1
方法二:
方法一中已知
J
x
L
o
g
(
x
)
=
D
L
o
g
(
x
)
D
x
J_x^{{\rm Log}(x)} = \frac{D {\rm Log}(x)}{D x}
JxLog(x)=DxDLog(x)
对应于论文中 Figure 8 和 eq(41a) ,“Right Jacobian of function” 中的函数为
f
(
x
)
=
L
o
g
(
x
)
f(x) = {\rm Log} (x)
f(x)=Log(x). 虽然题设条件中提到
τ
=
L
o
g
(
x
)
\tau = {\rm Log(x)}
τ=Log(x),但条件中讲的是切空间 (李代数) 与流形之间的指数映射,即
E
x
p
(
τ
)
=
x
{\rm Exp}(\tau) = x
Exp(τ)=x . 此处需要注意两个
L
o
g
\rm Log
Log 之间的角色区别. 由 eq(41a) 的定义
x
D
L
o
g
(
x
)
D
x
=
lim
δ
τ
→
0
L
o
g
(
x
⊕
δ
τ
)
⊖
L
o
g
(
x
)
δ
τ
(
n
o
t
e
s
:
f
(
x
)
=
L
o
g
(
x
)
,
x
∈
M
,
δ
τ
∈
R
m
)
=
lim
δ
τ
→
0
L
o
g
(
x
E
x
p
(
δ
τ
)
)
⊖
L
o
g
(
x
)
δ
τ
(
n
o
t
e
s
:
τ
=
L
o
g
(
x
)
∈
R
m
,
x
=
E
x
p
(
τ
)
∈
M
)
=
lim
δ
τ
→
0
L
o
g
(
E
x
p
(
τ
)
E
x
p
(
δ
τ
)
)
−
τ
δ
τ
\begin{aligned} \frac{^{x} D {\rm Log}(x)}{D x} &= \lim_{\delta \tau \rightarrow 0} \frac{{\rm Log} (x \oplus \delta\tau) \ominus {\rm Log}(x)}{\delta \tau}\\ ({\rm \bf notes}:\ f(x) &= {\rm Log} (x),\ x \in \mathcal{M},\ \delta\tau \in \mathbb{R}^m)\\ &=\lim_{\delta \tau \rightarrow 0} \frac{{\rm Log} (x\, {\rm Exp}{(\delta\tau})) \ominus {\rm Log}(x)}{\delta \tau}\\ ({\rm \bf notes}:\ \tau &= {\rm Log}(x) \in \mathbb{R}^m,\ x={\rm Exp}(\tau)\in \mathcal{M})\\ &=\lim_{\delta \tau \rightarrow 0} \frac{{\rm Log} ({\rm Exp}(\tau)\, {\rm Exp}{(\delta\tau})) - \tau}{\delta \tau} \end{aligned}
DxxDLog(x)(notes: f(x)(notes: τ=δτ→0limδτLog(x⊕δτ)⊖Log(x)=Log(x), x∈M, δτ∈Rm)=δτ→0limδτLog(xExp(δτ))⊖Log(x)=Log(x)∈Rm, x=Exp(τ)∈M)=δτ→0limδτLog(Exp(τ)Exp(δτ))−τ
当
δ
τ
\delta\tau
δτ 很小的时候
L
o
g
(
E
x
p
(
τ
)
E
x
p
(
δ
τ
)
)
−
τ
δ
τ
=
J
x
L
o
g
(
x
)
\frac{{\rm Log} ({\rm Exp}(\tau)\, {\rm Exp}{(\delta\tau})) - \tau}{\delta \tau} = J_x^{{\rm Log}(x)}
δτLog(Exp(τ)Exp(δτ))−τ=JxLog(x)
另外由 eq(70) 可知
L
o
g
(
E
x
p
(
τ
)
E
x
p
(
δ
τ
)
)
−
τ
δ
τ
=
J
r
(
τ
)
−
1
\frac{{\rm Log} ({\rm Exp}(\tau)\, {\rm Exp}{(\delta\tau})) - \tau}{\delta \tau} = {J_r(\tau)}^{-1}
δτLog(Exp(τ)Exp(δτ))−τ=Jr(τ)−1
所以 eq(79) 成立,即
J
x
L
o
g
(
x
)
=
J
r
(
τ
)
−
1
J_x^{{\rm Log}(x)} ={J_r(\tau)}^{-1}
JxLog(x)=Jr(τ)−1.
VI. About “Rodrigues rotation formula”
可以计算发现,Rodrigues rotation formula 除了对 S O ( 3 ) SO(3) SO(3) 旋转成立(Example 4),对 2D 空间中的旋转 S 1 S^{1} S1 和 S O ( 2 ) SO(2) SO(2) 一样成立.
1 对 S 1 S^{1} S1 的验证
τ ∧ = [ θ ] × = i θ = [ 1 ] × θ [ 1 ] × = i \tau ^{\wedge} = [\theta]_\times = i\theta = [1]_\times \theta \\ [1]_\times = i τ∧=[θ]×=iθ=[1]×θ[1]×=i
计算 Rodrigues rotation formula
e
x
p
(
[
1
]
×
θ
)
=
1
+
[
1
]
×
s
i
n
(
θ
)
+
[
1
]
×
2
(
1
−
c
o
s
(
θ
)
)
=
1
+
i
s
i
n
(
θ
)
−
(
1
−
c
o
s
(
θ
)
)
=
c
o
s
(
θ
)
+
i
s
i
n
(
θ
)
\begin{aligned} {\rm exp}([1]_\times \theta) & = 1 +[1]_\times {\rm sin}(\theta) + [1]_\times^{2} (1-{\rm cos}(\theta))\\ & = 1 + i {\rm sin}(\theta) - (1-{\rm cos}(\theta))\\ & = {\rm cos}(\theta) + i {\rm sin}(\theta) \end{aligned}
exp([1]×θ)=1+[1]×sin(θ)+[1]×2(1−cos(θ))=1+isin(θ)−(1−cos(θ))=cos(θ)+isin(θ)
2 对 S O ( 2 ) SO(2) SO(2) 的验证
τ ∧ = [ θ ] × = [ 0 − θ θ 0 ] = [ u ] × θ [ u ] × = [ 0 − 1 1 0 ] [ u ] × 2 = [ 0 − 1 1 0 ] [ 0 − 1 1 0 ] = [ − 1 0 0 − 1 ] \tau ^{\wedge} = [\theta]_\times = \begin{bmatrix}0 &-\theta\\ \theta & 0 \end{bmatrix} = [u]_\times \theta\\ [u]_\times = \begin{bmatrix} 0 &-1\\ 1 &0\end{bmatrix}\\ [u]_\times^2 = \begin{bmatrix} 0 &-1\\ 1 &0\end{bmatrix} \begin{bmatrix} 0 &-1\\ 1 &0\end{bmatrix} = \begin{bmatrix} -1 &0\\ 0 &-1\end{bmatrix} τ∧=[θ]×=[0θ−θ0]=[u]×θ[u]×=[01−10][u]×2=[01−10][01−10]=[−100−1]
计算 Rodrigues rotation formula
e
x
p
(
[
u
]
×
θ
)
=
I
+
[
u
]
×
s
i
n
(
θ
)
+
[
u
]
×
2
(
1
−
c
o
s
(
θ
)
)
=
[
1
0
0
1
]
+
[
0
−
1
1
0
]
s
i
n
(
θ
)
+
[
−
1
0
0
−
1
]
(
1
−
c
o
s
(
θ
)
)
=
[
0
−
1
1
0
]
s
i
n
(
θ
)
+
[
1
0
0
1
]
(
c
o
s
(
θ
)
)
=
[
c
o
s
(
θ
)
−
s
i
n
(
θ
)
s
i
n
(
θ
)
c
o
s
(
θ
)
]
\begin{aligned} {\rm exp}([u]_\times \theta) & = I +[u]_\times {\rm sin}(\theta) + [u]_\times^{2} (1-{\rm cos}(\theta))\\ & = \begin{bmatrix} 1 &0\\ 0 &1\end{bmatrix} + \begin{bmatrix} 0 &-1\\ 1 &0\end{bmatrix} {\rm sin}(\theta) + \begin{bmatrix} -1 &0\\ 0 &-1\end{bmatrix}(1-{\rm cos}(\theta))\\ & = \begin{bmatrix} 0 &-1\\ 1 &0\end{bmatrix} {\rm sin}(\theta) + \begin{bmatrix} 1 &0\\ 0 &1\end{bmatrix}({\rm cos}(\theta))\\ & = \begin{bmatrix}{\rm cos}(\theta) &-{\rm sin}(\theta)\\ {\rm sin}(\theta) &{\rm cos}(\theta)\end{bmatrix} \end{aligned}
exp([u]×θ)=I+[u]×sin(θ)+[u]×2(1−cos(θ))=[1001]+[01−10]sin(θ)+[−100−1](1−cos(θ))=[01−10]sin(θ)+[1001](cos(θ))=[cos(θ)sin(θ)−sin(θ)cos(θ)]
3 对 S O ( 3 ) SO(3) SO(3) 的计算
Rodrigues rotation formula 在
S
O
(
3
)
SO(3)
SO(3) 中的计算由论文中 Example 4 给出,下面计算 eq(135)
R
=
e
x
p
(
[
u
θ
]
×
)
=
I
+
[
u
]
×
s
i
n
(
θ
)
+
[
u
]
×
2
(
1
−
c
o
s
(
θ
)
)
[
u
]
×
T
=
−
[
u
]
×
[
u
]
×
2
=
u
u
T
−
I
=
(
[
u
]
×
2
)
T
⇒
R
T
=
I
−
[
u
]
×
s
i
n
(
θ
)
+
[
u
]
×
2
(
1
−
c
o
s
(
θ
)
)
⇒
R
−
R
T
=
2
[
u
]
×
s
i
n
(
θ
)
⇒
R
−
R
T
2
s
i
n
(
θ
)
=
[
u
]
×
⇒
(
R
−
R
T
2
s
i
n
(
θ
)
)
∨
=
u
∈
R
3
⇒
u
θ
=
θ
(
R
−
R
T
)
∨
2
s
i
n
(
θ
)
∈
R
3
R = {\rm exp}([{\mathbf u}\theta]_\times ) = I +[{\bf u}]_\times {\rm sin}(\theta) + [{\bf u}]_\times^{2} (1-{\rm cos}(\theta))\\ [{\bf u}]_\times^{\rm T} = - [{\bf u}]_\times\\ [{\bf u}]_\times^{2} = {\bf u}\,{\bf u}^{\rm T}- I ={([{\bf u}]_\times^{2})}^{\rm T} \\ \Rightarrow R^{\rm T} = I - [{\bf u}]_\times {\rm sin}(\theta) + [{\bf u}]_\times^{2} (1-{\rm cos}(\theta)) \\ \Rightarrow R - R^{\rm T} = 2[{\bf u}]_\times {\rm sin}(\theta)\\ \Rightarrow \frac{R - R^{\rm T}}{2 {\rm sin}(\theta)} = [{\bf u}]_\times\\ \Rightarrow \left(\frac{R - R^{\rm T}}{2 {\rm sin}(\theta)}\right)^{\vee} = {\bf u} \in \mathbb{R}^3\\ \Rightarrow {\bf u} \theta=\frac{\theta(R - R^{\rm T})^{\vee}}{2 {\rm sin}(\theta)} \in \mathbb{R}^3
R=exp([uθ]×)=I+[u]×sin(θ)+[u]×2(1−cos(θ))[u]×T=−[u]×[u]×2=uuT−I=([u]×2)T⇒RT=I−[u]×sin(θ)+[u]×2(1−cos(θ))⇒R−RT=2[u]×sin(θ)⇒2sin(θ)R−RT=[u]×⇒(2sin(θ)R−RT)∨=u∈R3⇒uθ=2sin(θ)θ(R−RT)∨∈R3
其中
u
=
[
u
x
,
u
y
,
u
z
]
T
{\bf u} = [u_x,\, u_y,\, u_z]^{\rm T}
u=[ux,uy,uz]T 是单位向量.
[
u
]
×
=
[
0
−
u
z
u
y
u
z
0
−
u
x
−
u
2
u
x
0
]
[
u
]
×
2
=
u
u
t
−
I
=
[
u
x
2
−
1
u
x
u
y
u
x
u
z
u
x
u
y
u
y
2
−
1
u
y
u
z
u
x
u
z
u
y
u
z
u
z
2
−
1
]
R
=
e
x
p
(
[
u
θ
]
×
)
=
[
1
1
1
]
+
[
0
−
u
z
u
y
u
z
0
−
u
x
−
u
2
u
x
0
]
s
i
n
(
θ
)
+
[
u
x
2
−
1
u
x
u
y
u
x
u
z
u
x
u
y
u
y
2
−
1
u
y
u
z
u
x
u
z
u
y
u
z
u
z
2
−
1
]
(
1
−
c
o
s
(
θ
)
)
⇒
t
r
a
c
e
(
R
)
=
3
+
(
u
x
2
+
u
y
2
+
u
z
2
−
3
)
(
1
−
c
o
s
(
θ
)
)
=
3
−
2
+
2
c
o
s
(
θ
)
⇒
c
o
s
(
θ
)
=
t
r
a
c
e
(
R
)
−
1
2
⇒
θ
=
c
o
s
−
1
(
t
r
a
c
e
(
R
)
−
1
2
)
[{\bf u}]_\times =\begin{bmatrix}0 & -{u_z} & {u_y}\\ {u_z} & 0 & -{u_x}\\ -{u2} & {u_x} & 0\end{bmatrix}\\ [{\bf u}]_\times^{2} = {\bf u}{\bf u}^{\rm t}-I = \begin{bmatrix} u_x^2-1 & {u_x} {u_y} & {u_x} {u_z}\\ {u_x} {u_y} & u_y^2 -1 & {u_y} {u_z}\\ {u_x} {u_z} & {u_y} {u_z} & u_z^2 -1\end{bmatrix}\\ R = {\rm exp}([{\mathbf u}\theta]_\times ) = \begin{bmatrix}1 & &\\ &1 &\\ & &1 \end{bmatrix} +\begin{bmatrix}0 & -{u_z} & {u_y}\\ {u_z} & 0 & -{u_x}\\ -{u2} & {u_x} & 0\end{bmatrix} {\rm sin}(\theta) + \begin{bmatrix} u_x ^2 -1 & {u_x} {u_y} & {u_x} {u_z}\\ {u_x} {u_y} & u_y^2 -1 & {u_y} {u_z}\\ {u_x} {u_z} & {u_y} {u_z} & u_z^2 -1\end{bmatrix} (1-{\rm cos}(\theta))\\ \Rightarrow \ {\rm trace}(R)= 3 + (u_x^2 + u_y^2 + u_z^2-3)(1-cos(\theta))= 3 -2 + 2cos(\theta)\\ \Rightarrow\ cos(\theta) = \frac{{\rm trace}(R) -1} {2} \Rightarrow\ \theta = {\rm cos}^{-1}\left( \frac{{\rm trace}(R) -1}{2}\right)
[u]×=
0uz−u2−uz0uxuy−ux0
[u]×2=uut−I=
ux2−1uxuyuxuzuxuyuy2−1uyuzuxuzuyuzuz2−1
R=exp([uθ]×)=
111
+
0uz−u2−uz0uxuy−ux0
sin(θ)+
ux2−1uxuyuxuzuxuyuy2−1uyuzuxuzuyuzuz2−1
(1−cos(θ))⇒ trace(R)=3+(ux2+uy2+uz2−3)(1−cos(θ))=3−2+2cos(θ)⇒ cos(θ)=2trace(R)−1⇒ θ=cos−1(2trace(R)−1)
得到 eq(135).
— To be continued —
<div id="ref-1"></div>
[1] Joan Solà, Jeremie Deray, Dinesh Atchuthan, "A micro Lie theory for state estimation in robotics", arXiv:1812.01537 [cs.RO]
<div id="ref-2"></div>
[2] Timothy D. Barfoot, "State Estimation for Robotics", Cambridge University Press, 2017
<div id="ref-3"></div>
[3] Bernoulli Number, https://mathworld.wolfram.com/BernoulliNumber.html
<div id="ref-4"></div>
[4] 双曲三角函数与三角函数泰勒展开式, https://zhuanlan.zhihu.com/p/166135510
<div id="ref-5"></div>
[5] 三角函数半角公式, https://zhidao.baidu.com/question/1245914655285565579.html
<div id="ref-6"></div>
[6] Gregory S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups, Volume 2", Birkhäuser Boston, MA, 2011