雅可比
雅可比分为左乘雅可比和右乘雅可比,分别记为
J
l
J_l
Jl和
J
r
J_r
Jr。左乘雅可比即为李代数se(3)中的
J
J
J
J
l
=
J
=
sin
θ
θ
I
+
(
1
−
sin
θ
θ
)
a
a
T
+
1
−
cos
θ
θ
a
∧
J_l=J=\frac{\sin\theta}{\theta}I+\Bigl(1-\frac{\sin\theta}{\theta}\Bigr)aa^T+\frac{1-\cos\theta}{\theta}a^\land
Jl=J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧
它的逆如下:
J
l
−
1
=
θ
2
cot
θ
2
I
+
(
1
−
θ
2
cot
θ
2
)
a
a
T
−
θ
2
a
∧
J_l^{-1}=\frac{\theta}{2}\cot\frac{\theta}{2}I+\Bigl(1-\frac{\theta}{2}\cot\frac{\theta}{2}\Bigr)aa^T-\frac{\theta}{2}a^\land
Jl−1=2θcot2θI+(1−2θcot2θ)aaT−2θa∧
右乘雅可比只需对自变量取负即可:
J
r
(
ϕ
)
=
J
l
(
−
ϕ
)
J_r(\phi)=J_l(-\phi)
Jr(ϕ)=Jl(−ϕ)
BCH近似
李群的指数映射之积由BCH公式给出,其展开式的前几项如下:
ln
(
e
x
p
(
A
)
e
x
p
(
B
)
)
=
A
+
B
+
1
2
[
A
,
B
]
+
1
12
[
A
,
[
A
,
B
]
]
−
1
12
[
B
,
[
A
,
B
]
]
+
⋯
\ln(exp(A)exp(B))=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+\cdots
ln(exp(A)exp(B))=A+B+21[A,B]+121[A,[A,B]]−121[B,[A,B]]+⋯
考虑
S
O
(
3
)
SO(3)
SO(3)上的李代数
ln
(
e
x
p
(
ϕ
1
∧
)
e
x
p
(
ϕ
2
∧
)
)
∨
\ln(exp(\phi_1^\land)exp(\phi_2^\land))^\lor
ln(exp(ϕ1∧)exp(ϕ2∧))∨,当
ϕ
1
\phi_1
ϕ1或
ϕ
2
\phi_2
ϕ2为小量时,二次以上的项可被忽略。此时,BCH有线性近似表达:
ln
(
e
x
p
(
ϕ
1
∧
)
e
x
p
(
ϕ
2
∧
)
)
∨
≈
{
J
l
(
ϕ
2
)
−
1
ϕ
1
+
ϕ
2
ϕ
1
为
小
量
J
r
(
ϕ
1
)
−
1
ϕ
2
+
ϕ
1
ϕ
2
为
小
量
\ln(exp(\phi_1^\land)exp(\phi_2^\land))^\lor\approx\begin{cases}J_l(\phi_2)^{-1}\phi_1+\phi_2&\quad\phi_1为小量\\J_r(\phi_1)^{-1}\phi_2+\phi_1&\quad\phi_2为小量\end{cases}
ln(exp(ϕ1∧)exp(ϕ2∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2Jr(ϕ1)−1ϕ2+ϕ1ϕ1为小量ϕ2为小量
SO(3)
对
S
O
(
3
)
SO(3)
SO(3)的BCH的近似进行定义:假设存在某个旋转
R
R
R,对应李代数为
ϕ
\phi
ϕ。对其左乘一个微小旋转
Δ
R
\Delta R
ΔR,对应李代数
Δ
ϕ
\Delta\phi
Δϕ。则在李群上,得到
Δ
R
⋅
R
\Delta R\cdot R
ΔR⋅R,对应李代数上,由BCH近似得到:
J
l
−
1
(
ϕ
)
Δ
ϕ
+
ϕ
J_l^{-1}(\phi)\Delta\phi+\phi
Jl−1(ϕ)Δϕ+ϕ。
e
x
p
(
Δ
ϕ
∧
)
e
x
p
(
ϕ
∧
)
=
e
x
p
(
(
J
l
−
1
Δ
ϕ
+
ϕ
)
∨
)
e
x
p
(
ϕ
∧
)
e
x
p
(
Δ
ϕ
∧
)
=
e
x
p
(
(
J
r
−
1
Δ
ϕ
+
ϕ
)
∨
)
exp(\Delta\phi^\land)exp(\phi^\land)=exp\bigl((J_l^{-1}\Delta\phi+\phi)^\lor\bigr)\\ exp(\phi^\land)exp(\Delta\phi^\land)=exp\bigl((J_r^{-1}\Delta\phi+\phi)^\lor\bigr)\\
exp(Δϕ∧)exp(ϕ∧)=exp((Jl−1Δϕ+ϕ)∨)exp(ϕ∧)exp(Δϕ∧)=exp((Jr−1Δϕ+ϕ)∨)
反之,若在李代数上进行加法,即
ϕ
\phi
ϕ加上一个
Δ
ϕ
\Delta\phi
Δϕ,则由BCH近似得到李群上的乘法:
e
x
p
(
(
ϕ
+
Δ
ϕ
)
∧
)
=
e
x
p
(
(
J
l
Δ
ϕ
)
∧
)
e
x
p
(
ϕ
∧
)
e
x
p
(
(
ϕ
+
Δ
ϕ
)
∧
)
=
e
x
p
(
ϕ
∧
)
e
x
p
(
(
J
r
Δ
ϕ
)
∧
)
exp\bigl((\phi+\Delta\phi)^\land\bigr)=exp\bigl((J_l\Delta\phi)^\land\bigr)exp(\phi^\land)\\ exp\bigl((\phi+\Delta\phi)^\land\bigr)=exp(\phi^\land)exp\bigl((J_r\Delta\phi)^\land\bigr)\\
exp((ϕ+Δϕ)∧)=exp((JlΔϕ)∧)exp(ϕ∧)exp((ϕ+Δϕ)∧)=exp(ϕ∧)exp((JrΔϕ)∧)
SE(3)
同样,对于
S
E
(
3
)
SE(3)
SE(3)上的李代数也可由BCH近似得到:
e
x
p
(
Δ
ξ
∧
)
e
x
p
(
ξ
∧
)
=
e
x
p
(
(
J
l
−
1
Δ
ξ
+
ξ
)
∨
)
e
x
p
(
ξ
∧
)
e
x
p
(
Δ
ξ
∧
)
=
e
x
p
(
(
J
r
−
1
Δ
ξ
+
ξ
)
∨
)
exp(\Delta\xi^\land)exp(\xi^\land)=exp\bigl((\mathcal{J}_l^{-1}\Delta\xi+\xi)^\lor\bigr)\\ exp(\xi^\land)exp(\Delta\xi^\land)=exp\bigl((\mathcal{J}_r^{-1}\Delta\xi+\xi)^\lor\bigr)\\
exp(Δξ∧)exp(ξ∧)=exp((Jl−1Δξ+ξ)∨)exp(ξ∧)exp(Δξ∧)=exp((Jr−1Δξ+ξ)∨)
在李代数上进行加法同样可得李群上的乘法:
e
x
p
(
(
ξ
+
Δ
ξ
)
∧
)
=
e
x
p
(
(
J
l
Δ
ξ
)
∧
)
e
x
p
(
ξ
∧
)
e
x
p
(
(
ξ
+
Δ
ξ
)
∧
)
=
e
x
p
(
ξ
∧
)
e
x
p
(
(
J
r
Δ
ξ
)
∧
)
exp\bigl((\xi+\Delta\xi)^\land\bigr)=exp\bigl((\mathcal{J}_l\Delta\xi)^\land\bigr)exp(\xi^\land)\\ exp\bigl((\xi+\Delta\xi)^\land\bigr)=exp(\xi^\land)exp\bigl((\mathcal{J}_r\Delta\xi)^\land\bigr)\\
exp((ξ+Δξ)∧)=exp((JlΔξ)∧)exp(ξ∧)exp((ξ+Δξ)∧)=exp(ξ∧)exp((JrΔξ)∧)
其中
J
l
\mathcal{J}_l
Jl为
6
×
6
6\times6
6×6矩阵,表达式如下:
J
l
(
ξ
)
=
[
J
l
Q
l
0
J
l
]
J
r
(
ξ
)
=
[
J
r
Q
r
0
J
r
]
J
r
(
ξ
)
=
J
l
(
−
ξ
)
\mathcal{J}_l(\xi)=\begin{bmatrix}J_l&Q_l\\0&J_l\end{bmatrix}\\ \mathcal{J}_r(\xi)=\begin{bmatrix}J_r&Q_r\\0&J_r\end{bmatrix}\\ \mathcal{J}_r(\xi)=\mathcal{J}_l(-\xi)
Jl(ξ)=[Jl0QlJl]Jr(ξ)=[Jr0QrJr]Jr(ξ)=Jl(−ξ)
其中
Q
Q
Q表达如下:
Q
l
(
ξ
)
=
∑
n
=
0
∞
∑
m
=
0
∞
1
(
n
+
m
+
2
)
!
(
ϕ
∧
)
n
ρ
∧
(
ϕ
∧
)
m
Q
r
(
ξ
)
=
Q
l
(
−
ξ
)
Q_l(\xi)=\sum_{n=0}^{\infty}\sum_{m=0}^\infty\frac{1}{(n+m+2)!}(\phi^\land)^n\rho^\land(\phi^\land)^m\\ Q_r(\xi)=Q_l(-\xi)
Ql(ξ)=n=0∑∞m=0∑∞(n+m+2)!1(ϕ∧)nρ∧(ϕ∧)mQr(ξ)=Ql(−ξ)
雅可比矩阵的逆如下:
J
l
−
1
=
[
J
l
−
1
−
J
l
−
1
Q
l
J
l
−
1
0
J
l
−
1
]
J
r
−
1
=
[
J
r
−
1
−
J
r
−
1
Q
r
J
r
−
1
0
J
r
−
1
]
\mathcal{J}_l^{-1}=\begin{bmatrix}J_l^{-1}&-J_l^{-1}Q_lJ_l^{-1}\\0&J_l^{-1}\end{bmatrix}\\ \mathcal{J}_r^{-1}=\begin{bmatrix}J_r^{-1}&-J_r^{-1}Q_rJ_r^{-1}\\0&J_r^{-1}\end{bmatrix}
Jl−1=[Jl−10−Jl−1QlJl−1Jl−1]Jr−1=[Jr−10−Jr−1QrJr−1Jr−1]
李代数求导
设空间内一点 p p p进行旋转 R R R,得到旋转后点 R p Rp Rp。记旋转后点相对于旋转的导数为: ∂ ( R p ) ∂ R \frac{\partial(Rp)}{\partial R} ∂R∂(Rp)
S O ( 3 ) SO(3) SO(3)无加法,该导数无法按照导数定义进行计算。设 R R R对应李代数 ϕ \phi ϕ,则转而计算: ∂ ( e x p ( ϕ ∧ ) p ) ∂ ϕ \frac{\partial (exp(\phi^\land)p)}{\partial\phi} ∂ϕ∂(exp(ϕ∧)p)
求解该式可使用导数模型或扰动模型。
S O ( 3 ) SO(3) SO(3)导数模型
由导数定义进行计算:
∂
(
e
x
p
(
ϕ
∧
)
p
)
∂
ϕ
=
lim
δ
ϕ
→
0
e
x
p
(
(
ϕ
+
δ
ϕ
)
∧
)
p
−
e
x
p
(
ϕ
∧
)
p
δ
ϕ
1
:
=
lim
δ
ϕ
→
0
e
x
p
(
(
J
l
δ
ϕ
)
∧
)
e
x
p
(
ϕ
∧
)
p
−
e
x
p
(
ϕ
∧
)
p
δ
ϕ
2
:
≈
lim
δ
ϕ
→
0
(
I
+
(
J
l
δ
ϕ
)
∧
)
e
x
p
(
ϕ
∧
)
p
−
e
x
p
(
ϕ
∧
)
p
δ
ϕ
3
:
=
lim
δ
ϕ
→
0
(
J
l
δ
ϕ
)
∧
e
x
p
(
ϕ
∧
)
p
δ
ϕ
4
:
=
lim
δ
ϕ
→
0
−
(
e
x
p
(
ϕ
∧
)
p
)
∧
J
l
δ
ϕ
δ
ϕ
5
:
=
−
(
R
p
)
∧
J
l
\begin{aligned} \frac{\partial (exp(\phi^\land)p)}{\partial\phi}&=\lim_{\delta\phi\to0}\frac{exp\bigl((\phi+\delta\phi)^\land\bigr)p-exp(\phi^\land)p}{\delta\phi}\\ 1:\qquad&=\lim_{\delta\phi\to0}\frac{exp\bigl((J_l\delta\phi)^\land\bigr)exp(\phi^\land)p-exp(\phi^\land)p}{\delta\phi}\\ 2:\qquad&\approx\lim_{\delta\phi\to0}\frac{\bigl(I+(J_l\delta\phi)^\land\bigr)exp(\phi^\land)p-exp(\phi^\land)p}{\delta\phi}\\ 3:\qquad&=\lim_{\delta\phi\to0}\frac{(J_l\delta\phi)^\land exp(\phi^\land)p}{\delta\phi}\\ 4:\qquad&=\lim_{\delta\phi\to0}\frac{-\bigl(exp(\phi^\land)p\bigr)^\land J_l\delta\phi}{\delta\phi}\\ 5:\qquad&=-(Rp)^\land J_l \end{aligned}
∂ϕ∂(exp(ϕ∧)p)1:2:3:4:5:=δϕ→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
计算思路如下:
- 第一步:由李代数的加法转为李群的乘法
- 第二步:对 e x p ( J l δ ϕ ) ∧ exp(J_l\delta\phi)^\land exp(Jlδϕ)∧进行一阶泰勒展开
- 第三步:约去相同项
- 第四步:反对称符号实际为叉乘计算,由 a × b = − b × a a\times b=-b\times a a×b=−b×a知 a ∧ b = − b ∧ a a^\land b=-b^\land a a∧b=−b∧a,代入第三步分子
- 第五步:约去分母项,并将李代数写回李群
从而得到: ∂ ( R p ) ∂ R = − ( R p ) ∧ J l \frac{\partial(Rp)}{\partial R}=-(Rp)^\land J_l ∂R∂(Rp)=−(Rp)∧Jl
导数模型含有雅可比项,计算较为复杂,一般采用扰动模型进行计算。
SO(3)扰动模型(左乘)
对于旋转
R
R
R,假设它受到一个扰动
Δ
R
\Delta R
ΔR,对应的李代数为
φ
\varphi
φ。求导也即看扰动结果相对于扰动的变换率。此处采用左扰动模型,也即左乘一个扰动
Δ
R
\Delta R
ΔR
∂
(
R
p
)
∂
R
=
lim
φ
→
0
e
x
p
(
φ
∧
)
e
x
p
(
ϕ
∧
)
p
−
e
x
p
(
ϕ
∧
)
p
φ
1
:
≈
lim
φ
→
0
(
I
+
φ
∧
)
e
x
p
(
ϕ
∧
)
p
−
e
x
p
(
ϕ
∧
)
p
φ
2
:
=
lim
φ
→
0
φ
∧
R
p
φ
3
:
=
lim
φ
→
0
−
(
R
p
)
∧
φ
φ
4
:
=
−
(
R
p
)
∧
\begin{aligned} \frac{\partial(Rp)}{\partial R}&=\lim_{\varphi\to0}\frac{exp(\varphi^\land)exp(\phi^\land)p-exp(\phi^\land)p}{\varphi}\\ 1:\qquad &\approx \lim_{\varphi\to0}\frac{(I+\varphi^\land)exp(\phi^\land)p-exp(\phi^\land)p}{\varphi}\\ 2:\qquad &= \lim_{\varphi\to0}\frac{\varphi^\land Rp}{\varphi}\\ 3:\qquad &= \lim_{\varphi\to0}\frac{-(Rp)^\land\varphi}{\varphi}\\ 4:\qquad &= -(Rp)^\land \end{aligned}
∂R∂(Rp)1:2:3:4:=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p≈φ→0limφ(I+φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφφ∧Rp=φ→0limφ−(Rp)∧φ=−(Rp)∧
计算思路如下:
- 第一步:对 e x p ( φ ∧ ) exp(\varphi^\land) exp(φ∧)进行一阶泰勒展开
- 第二步:约去相同项,将李代数写回李群
- 第三步:左右互换
- 第四步:约去坟墓想,得到结果。
从而得到左扰动模型: ∂ ( R p ) ∂ R = − ( R p ) ∧ \frac{\partial(Rp)}{\partial R}=-(Rp)^\land ∂R∂(Rp)=−(Rp)∧
S E ( 3 ) SE(3) SE(3)扰动模型(左乘)
假设空间某点
p
p
p经过变换
T
T
T后得到点
T
p
Tp
Tp,变换
T
T
T对应的李代数为
ξ
\xi
ξ。现在左乘一个扰动
Δ
T
=
e
x
p
(
δ
ξ
∧
)
\Delta T=exp(\delta\xi^\land)
ΔT=exp(δξ∧),对应李代数为
δ
ξ
=
[
δ
ρ
δ
ϕ
]
T
\delta\xi=\begin{bmatrix}\delta\rho&\delta\phi\end{bmatrix}^T
δξ=[δρδϕ]T,则:
∂
(
T
p
)
∂
δ
ξ
=
lim
δ
ξ
→
0
e
x
p
(
δ
ξ
∧
)
e
x
p
(
ξ
∧
)
p
−
e
x
p
(
ξ
∧
)
p
δ
ξ
1
:
≈
lim
δ
ξ
→
0
(
I
+
δ
ξ
∧
)
e
x
p
(
ξ
∧
)
p
−
e
x
p
(
ξ
∧
)
p
δ
ξ
2
:
=
lim
δ
ξ
→
0
δ
ξ
∧
e
x
p
(
ξ
∧
)
p
δ
ξ
3
:
=
lim
δ
ξ
→
0
[
δ
ϕ
∧
δ
ρ
0
T
0
]
[
e
x
p
(
ϕ
∧
)
J
ρ
0
T
1
]
p
δ
ξ
4
:
=
lim
δ
ξ
→
0
[
δ
ϕ
∧
δ
ρ
0
T
0
]
[
R
p
+
t
1
]
δ
ξ
5
:
=
lim
δ
ξ
→
0
[
δ
ϕ
∧
(
R
p
+
t
)
+
δ
ρ
0
]
[
δ
ρ
δ
ϕ
]
6
:
=
[
I
−
(
R
p
+
t
)
∧
0
T
0
T
]
7
:
≜
(
T
p
)
⊙
\begin{aligned} \frac{\partial(Tp)}{\partial\delta\xi}&=\lim_{\delta\xi\to0}\frac{exp(\delta\xi^\land)exp(\xi^\land)p-exp(\xi^\land)p}{\delta\xi}\\ 1:\qquad &\approx\lim_{\delta\xi\to0}\frac{\bigl(I+\delta\xi^\land\bigr)exp(\xi^\land)p-exp(\xi^\land)p}{\delta\xi} \\ 2:\qquad &=\lim_{\delta\xi\to0}\frac{\delta\xi^\land exp(\xi^\land)p}{\delta\xi} \\ 3:\qquad &=\lim_{\delta\xi\to0}\frac{\begin{bmatrix}\delta\phi^\land&\delta\rho\\0^T&0\end{bmatrix}\begin{bmatrix}exp(\phi^\land)&J\rho\\0^T&1\end{bmatrix}p}{\delta\xi} \\ 4:\qquad&=\lim_{\delta\xi\to0}\frac{\begin{bmatrix}\delta\phi^\land&\delta\rho\\0^T&0\end{bmatrix}\begin{bmatrix}Rp+t\\1\end{bmatrix}}{\delta\xi}\\ 5:\qquad&=\lim_{\delta\xi\to0}\frac{\begin{bmatrix}\delta\phi^\land(Rp+t)+\delta\rho\\0\end{bmatrix}}{\begin{bmatrix}\delta\rho\\\delta\phi\end{bmatrix}}\\ 6:\qquad&=\begin{bmatrix}I&-(Rp+t)^\land\\0^T&0^T\end{bmatrix}\\ 7:\qquad&\triangleq(Tp)^\odot \end{aligned}
∂δξ∂(Tp)1:2:3:4:5:6:7:=δξ→0limδξexp(δξ∧)exp(ξ∧)p−exp(ξ∧)p≈δξ→0limδξ(I+δξ∧)exp(ξ∧)p−exp(ξ∧)p=δξ→0limδξδξ∧exp(ξ∧)p=δξ→0limδξ[δϕ∧0Tδρ0][exp(ϕ∧)0TJρ1]p=δξ→0limδξ[δϕ∧0Tδρ0][Rp+t1]=δξ→0lim[δρδϕ][δϕ∧(Rp+t)+δρ0]=[I0T−(Rp+t)∧0T]≜(Tp)⊙
计算思路如下:
- 第一步:对 e x p ( δ ξ ∧ ) exp(\delta\xi^\land) exp(δξ∧)进行一阶泰勒展开
- 第二步:约去相同项
- 第三步:将 ξ ∧ \xi^\land ξ∧矩阵形式以及 S E ( 3 ) SE(3) SE(3)的指数映射代入替换
- 第四步:指数映射同点坐标(齐次)进行矩阵乘法
- 第五步:分子矩阵乘法,分母写为矩阵形式
- 第六步:计算
- 第七步:定义算符 ⊙ \odot ⊙表示将一个齐次坐标写为 4 × 6 4\times6 4×6的矩阵形式
应注意矩阵求导的顺序,若
a
、
b
、
x
、
y
a、b、x、y
a、b、x、y为列向量,则有:
d
[
a
b
]
d
[
x
y
]
=
(
d
[
a
b
]
T
d
[
x
y
]
)
T
=
[
d
a
d
x
d
b
d
x
d
a
d
y
d
b
d
y
]
T
=
[
d
a
d
x
d
a
d
y
d
b
d
x
d
b
d
y
]
\frac{d\begin{bmatrix}a\\b\end{bmatrix}}{d\begin{bmatrix}x\\y\end{bmatrix}}=\Biggl(\frac{d\begin{bmatrix}a&b\end{bmatrix}^T}{d\begin{bmatrix}x\\y\end{bmatrix}}\Biggr)^T=\begin{bmatrix}\frac{da}{dx}&\frac{db}{dx}\\\frac{da}{dy}&\frac{db}{dy}\end{bmatrix}^T=\begin{bmatrix}\frac{da}{dx}&\frac{da}{dy}\\\frac{db}{dx}&\frac{db}{dy}\end{bmatrix}
d[xy]d[ab]=(d[xy]d[ab]T)T=[dxdadydadxdbdydb]T=[dxdadxdbdydadydb]