| 回顾
- SLAM 的运动与观测模型
运动模型 : x k = f ( x k − 1 , u k ) + w k x_k = f(x_{k-1},u_k) + w_k xk=f(xk−1,uk)+wk
w k w_k wk : 噪声
u k u_k uk : 运动传感器输入
运动 : 观测从k
到k-1
时刻, 位置x
的变化
观测模型 : z k , j = h ( x k , y j ) + v k , j z_{k,j} = h(x_k,y_j) + v_{k,j} zk,j=h(xk,yj)+vk,j
v k , j v_{k,j} vk,j : 噪声
观测, z k , j z_{k,j} zk,j : 机器人在k
时刻于 x k x_k xk 处探测到一个路标 y j y_j yj
-
描述 x k x_k xk
R ( 旋 转 矩 阵 ) , t ( 偏 移 向 量 ) R(旋转矩阵),t(偏移向量) R(旋转矩阵),t(偏移向量) 或 T ( 变 换 矩 阵 ) T(变换矩阵) T(变换矩阵) -
更准确的估计
x,y ⟹ \Longrightarrow ⟹ ( x , y ) ∗ (x,y)^* (x,y)∗
需要通过求导来得到偏移量 d f ( R ) d R = f ( R + △ R ) − f ( R ) △ R \frac{df(R)}{dR}=\frac{f(R+\triangle R) -f(R)}{\triangle R} dRdf(R)=△Rf(R+△R)−f(R)
但是 R R R经过 R + △ R R+\triangle R R+△R显然不再是旋转矩阵,不再满足特殊正交群的定义
所以不能通过此种方式计算得到导数
| 1. 群
-
三维旋转矩阵构成了特殊正交群( Special Orthogonal Group)
- S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , det ( R ) = 1 } S O(3)=\left\{\boldsymbol{R} \in \mathbb{R}^{3 \times 3} \mid \boldsymbol{R} \boldsymbol{R}^{T}=\boldsymbol{I}, \operatorname{det}(\boldsymbol{R})=1\right\} SO(3)={R∈R3×3∣RRT=I,det(R)=1}
-
三维变换矩阵构成了特殊欧氏群( Special Euclidean Group)
- S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } S E(3)=\left\{\boldsymbol{T}=\left[\begin{array}{cc}R & t \\ 0^{T} & 1\end{array}\right] \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in S O(3), \boldsymbol{t} \in \mathbb{R}^{3}\right\} SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}
-
群( Group )是一种集合加上一种运算的代数结构。
-
记集合为 A A A ,运算为 ⋅ · ⋅ ,那么当运算满足以下性质时,称 ( A , ⋅ ) ( A,· ) (A,⋅) 成群:
-
- 封闭性: ∀ a 1 , a 2 ∈ A , a 1 ⋅ a 2 ∈ A \forall a_{1}, a_{2} \in A, \quad a_{1} \cdot a_{2} \in A ∀a1,a2∈A,a1⋅a2∈A
-
- 结合律: ∀ a 1 , a 2 , a 3 ∈ A , ( a 1 ⋅ a 2 ) ⋅ a 3 = a 1 ⋅ ( a 2 ⋅ a 3 ) \forall a_{1}, a_{2}, a_{3} \in A, \quad (a_{1} \cdot a_{2}) \cdot a_3 = a_1 \cdot (a_2 \cdot a_3) ∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3)
-
- 幺元: ∃ a 0 ∈ A , s.t. ∀ a ∈ A , a 0 ⋅ a = a ⋅ a 0 = a \exists a_{0} \in A, \quad \text { s.t. } \quad\forall a \in A, \quad a_{0} \cdot a=a \cdot a_{0}=a ∃a0∈A, s.t. ∀a∈A,a0⋅a=a⋅a0=a
- 加法的幺元是 0(任何数加0结果还是本身), 乘法的幺元是 1(任何数乘1结果还是本身)
-
- 逆: ∀ a ∈ A , ∃ a − 1 ∈ A , s.t. a ⋅ a − 1 = a 0 \forall a \in A, \quad \exists a^{-1} \in A, \quad \text { s.t. } \quad a \cdot a^{-1}=a_{0} ∀a∈A,∃a−1∈A, s.t. a⋅a−1=a0
-
- 如果交换律成立, 则该群成为(阿贝尔)交换群,(2+3 = 3 + 2 = 5 ⟹ \Longrightarrow ⟹ 加法在整数上成阿贝尔交换群 ( + , Z ) (+,Z) (+,Z) )
容易验证:
🔸 旋转矩阵集合与矩阵乘法构成群
🔸 变换矩阵集合与矩阵乘法构成群
🔹 旋转矩阵
和 变换矩阵
是正交阵, 一定可逆
🔸 因此称它们为旋转矩阵群和变换矩阵群
🔸 群结构保证了在群上的运算具有良好的性质。
| 2. 李群与李代数
|| 李群( Lie Group)
- 具有连续(光滑)性质的群。
- 相机在空间中的运动是个连续的过程
- 既是群也是流形( Mamifold )。
- 在高维空间中 连续,低维的存在 (三维空间中的二维曲面)
- SO(3) 是 九维空间中的三维流形( R 3 R^3 R3)
- 直观上看,一个刚体能够连续地在空间中运动,故 SO(3) 和 SE(3) 都是李群。
- 但是, SO(3) 和 SE(3) 只有定义良好的乘法,没有加法,所以难以进行取极限、求导等操作。
|| 李代数 ( Lie Algebra
):与李群对应的一种结构,位于向量空间。
- 通常记作小写的 so(3) 和 se(3) 。书中以哥特体突出显示。
- 存在一一对应关系 S O ( 3 ) ⇔ s o ( 3 ) SO(3) \Leftrightarrow so(3) SO(3)⇔so(3)
- 对于每个李群都会有一个对应的李代数
- 事实上是李群单位元处的正切空间。
💡从旋转矩阵引出李代数
∀
\forall
∀旋转矩阵
R
R
R, 满足
R
R
T
=
I
RR^T = I
RRT=I
令
R
R
R 随时间变化(连续运动),有:
R
(
t
)
R
(
t
)
T
=
I
R( t)R(t)^T = I
R(t)R(t)T=I
两侧对时间求导:
R
˙
(
t
)
R
(
t
)
T
+
R
(
t
)
R
˙
(
t
)
T
=
0
\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}+\boldsymbol{R}(t) \dot{\boldsymbol{R}}(t)^{T}=0
R˙(t)R(t)T+R(t)R˙(t)T=0
得:
R
˙
(
t
)
R
(
t
)
T
=
−
(
R
˙
(
t
)
R
(
t
)
T
)
T
\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T} = -(\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T})^T
R˙(t)R(t)T=−(R˙(t)R(t)T)T
可看为 A = − A t A = -A^t A=−At , 即A为反对称矩阵
a^ → \rightarrow → A
a → \rightarrow → A v A^v Av
反对称矩阵符号v
a
∧
=
A
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
,
A
∨
=
a
.
\boldsymbol{a}^{\wedge}=\boldsymbol{A}=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right], \quad \boldsymbol{A}^{\vee}=\boldsymbol{a} .
a∧=A=⎣⎡0a3−a2−a30a1a2−a10⎦⎤,A∨=a.
记: R ˙ ( t ) R ( t ) T = ϕ ( t ) ∧ \dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}=\phi(t)^{\wedge} R˙(t)R(t)T=ϕ(t)∧
两侧右乘 R ( t ) R(t) R(t) : R ˙ ( t ) = ϕ ( t ) ∧ R ( t ) \dot{\boldsymbol{R}}(t) = \phi(t)^{\wedge}R(t) R˙(t)=ϕ(t)∧R(t)
✨泰勒展开(一阶项)✨
单位元附近 :
t
0
=
0
,
R
(
0
)
=
I
t_0 = 0, R(0) = I
t0=0,R(0)=I
R
(
t
)
≈
R
(
t
0
)
+
R
˙
(
t
0
)
(
t
−
t
0
)
=
I
+
ϕ
(
t
0
)
∧
(
t
)
\begin{aligned} \boldsymbol{R}(t) & \approx \boldsymbol{R}\left(t_{0}\right)+\dot{\boldsymbol{R}}\left(t_{0}\right)\left(t-t_{0}\right) \\ &=\boldsymbol{I}+\boldsymbol{\phi}\left(t_{0}\right)^{\wedge}(t) \end{aligned}
R(t)≈R(t0)+R˙(t0)(t−t0)=I+ϕ(t0)∧(t)
在
t
0
t_0
t0 附近,假设
ϕ
\phi
ϕ 不变,有微分方程:
R
˙
(
t
)
=
ϕ
(
t
0
)
∧
R
(
t
)
=
ϕ
0
∧
R
(
t
)
\dot{\boldsymbol{R}}(t)=\phi\left(t_{0}\right)^{\wedge} \boldsymbol{R}(t)=\boldsymbol{\phi}_{0}^{\wedge} \boldsymbol{R}(t)
R˙(t)=ϕ(t0)∧R(t)=ϕ0∧R(t)
亦知 :
d
e
a
x
d
x
=
a
e
a
x
\frac{d e^{ax}}{dx} = ae^{ax}
dxdeax=aeax(
f
(
a
x
)
′
=
a
f
(
x
)
f(ax)' = af(x)
f(ax)′=af(x) )
已知初始情况:
R
(
0
)
=
I
R(0) = I
R(0)=I, 求解可得 :
R
(
t
)
=
e
x
p
(
ϕ
0
∧
t
)
R(t) = exp({\phi}_{0}^{\wedge}t)
R(t)=exp(ϕ0∧t)
🔺说明 : 该式说明,对任意 t ,都可以找到一个 R 和一个
ϕ
\phi
ϕ 的对应关系
🔸 该关系称为指数映射(Exponential Map
)
🔸 这里的 称为 SO(3) 对应的李代数: so(3)
➡️ so(3) 的定义和性质?
➡️ 指数映射如何求?
每个李群都有与之对应的李代数。
李代数描述了李群单位元附近的正切空间性质。
李代数由一个集合V (向量空间
) ,一个数域 F 和一个二元运算 [ , ] 组成 . 如果它们满足以下几条性质, 则称
(
V
,
F
,
[
,
]
)
(\mathbb{V}, \mathbb{F},[ , ])
(V,F,[,]) 为一个李代数, 记作 g .
- 封闭性 ∀ X , Y ∈ V , [ X , Y ] ∈ V \forall X,Y \in \mathbb{V},\quad [X,Y] \in \mathbb{V} ∀X,Y∈V,[X,Y]∈V
- 双线性 (
分配率
) ∀ X , Y , Z ∈ V , a , b ∈ F \forall X,Y,Z \in \mathbb{V},\quad a,b \in \mathbb{F} ∀X,Y,Z∈V,a,b∈F, 有:
[ a X + b Y , Z ] = a [ X , Z ] + b [ Y , Z ] , [ Z , a X + b Y ] = a [ Z , X ] + b [ Z , Y ] [aX + bY,Z] = a[X,Z]+b[Y,Z], [Z,aX+bY] = a[Z,X] ~+b[Z,Y] [aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X] +b[Z,Y] - 自反性
∀
X
∈
V
,
[
X
,
X
]
=
0
\forall X \in \mathbb{V}, [X,X] = 0
∀X∈V,[X,X]=0 (
[X,Y]
该运算像比较X,Y的差异性) - 雅可比等价 ∀ X , Y , Z ∈ V , [ X , [ Y , Z ] ] + [ Z , [ Y , X ] ] + [ Y , [ Z , X ] ] = 0 \forall X,Y,Z \in \mathbb{V},\quad [X,[Y,Z]] + [Z,[Y,X]] +[Y,[Z,X]] = 0 ∀X,Y,Z∈V,[X,[Y,Z]]+[Z,[Y,X]]+[Y,[Z,X]]=0
二元运算 被称为李括号( Lie Bracket )。
- 直观上说,李括号表达了两个元素的差异
Eg : 例子:三维空间向量+叉积运算 构成李代数
[
R
3
,
R
,
×
]
[\mathbb{R}^3,R,×]
[R3,R,×]
🔸 S O ( 3 ) ∈ R 3 ∗ 3 ( 三 维 矩 阵 ) SO(3) \in \mathbb{R}^{3*3} (三维矩阵) SO(3)∈R3∗3(三维矩阵) 而 s o ( 3 ) ∈ R 3 ( 向 量 ) so(3)\in\mathbb{R}^{3}(向量) so(3)∈R3(向量)
🔹 李代数so(3) : s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } \mathfrak{s o}(3)=\left\{\phi \in \mathbb{R}^{3}, \Phi=\phi^{\wedge} \in \mathbb{R}^{3 \times 3}\right\} so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}
🔹 其中 : Φ = ϕ ∧ = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] ∈ R 3 × 3 \Phi=\phi^{\wedge}=\left[\begin{array}{ccc}0 & -\phi_{3} & \phi_{2} \\ \phi_{3} & 0 & -\phi_{1} \\ -\phi_{2} & \phi_{1} & 0\end{array}\right] \in \mathbb{R}^{3 \times 3} Φ=ϕ∧=⎣⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎤∈R3×3
李括号: [ ϕ 1 , ϕ 2 ] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) ∨ \left[\phi_{1}, \phi_{2}\right]=\left(\Phi_{1} \Phi_{2}-\Phi_{2} \Phi_{1}\right)^{\vee} [ϕ1,ϕ2]=(Φ1Φ2−Φ2Φ1)∨
同理,
S
E
(
3
)
SE(3)
SE(3)亦有李代数
s
e
(
3
)
se(3)
se(3)
s
e
(
3
)
=
{
ξ
=
[
ρ
ϕ
]
∈
R
6
,
ρ
∈
R
3
,
ϕ
∈
s
o
(
3
)
,
ξ
∧
=
[
ϕ
∧
ρ
0
T
0
]
∈
R
4
×
4
}
\mathfrak{s e}(3)=\left\{\boldsymbol{\xi}=\left[\begin{array}{c} \rho \\ \phi \end{array}\right] \in \mathbb{R}^{6}, \boldsymbol{\rho} \in \mathbb{R}^{3}, \phi \in \mathfrak{s o}(3), \boldsymbol{\xi}^{\wedge}=\left[\begin{array}{cc} \phi^{\wedge} & \rho \\ \mathbf{0}^{T} & 0 \end{array}\right] \in \mathbb{R}^{4 \times 4}\right\}
se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0Tρ0]∈R4×4}
上尖尖 ^ 不再是反对称矩阵,但仍保留记法:
(^
仅将角度部分做了反对称矩阵)
ξ
∧
=
[
ϕ
∧
ρ
0
T
0
]
∈
R
4
×
4
\boldsymbol{\xi}^{\wedge}=\left[\begin{array}{cc} \phi^{\wedge} & \rho \\ 0^{T} & 0 \end{array}\right] \in \mathbb{R}^{4 \times 4}
ξ∧=[ϕ∧0Tρ0]∈R4×4
李括号 : [ ξ 1 , ξ 2 ] = ( ξ 1 ∧ ξ 2 ∧ − ξ 2 ∧ ξ 1 ∧ ) ∨ \left[\xi_{1}, \boldsymbol{\xi}_{2}\right]=\left(\boldsymbol{\xi}_{1}^{\wedge} \boldsymbol{\xi}_{2}^{\wedge}-\boldsymbol{\xi}_{2}^{\wedge} \boldsymbol{\xi}_{1}^{\wedge}\right)^{\vee} [ξ1,ξ2]=(ξ1∧ξ2∧−ξ2∧ξ1∧)∨
| 3. 指数映射和对数映射
|| SO(3) ♻️ so(3)
指数映射反映了从李代数到李群的对应关系:
R
=
e
x
p
(
ϕ
∧
)
R = exp({\phi}^{\wedge})
R=exp(ϕ∧)
ϕ
\phi
ϕ 是一个矩阵,对于矩阵,如何定义求指数运算?
由于
ϕ
\phi
ϕ 是向量,定义其角度和模长:
角度乘单位向量:
ϕ
=
θ
a
\phi = \theta a
ϕ=θa 模长: θ [ θ = ‖ φ ‖ ]
a : 方向 , [‖a‖ = 1]
关于
a
a
a ,可以验证以下性质 :
a
∧
a
∧
=
a
a
T
−
I
a
∧
a
∧
a
∧
=
−
a
∧
[
可
以
有
效
处
理
高
阶
项
]
\begin{aligned} &\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} \quad \quad [可以有效处理高阶项] \end{aligned}
a∧a∧=aaT−Ia∧a∧a∧=−a∧[可以有效处理高阶项]
exp ( ϕ ∧ ) = ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n \quad \exp \left(\phi^{\wedge}\right)=\sum_{n=0}^{\infty} \frac{1}{n !}\left(\phi^{\wedge}\right)^{n} exp(ϕ∧)=n=0∑∞n!1(ϕ∧)n
🔺为化解Taylor 展式中的高阶项提供了有效方法
📢Taylor 展开:
exp
(
ϕ
∧
)
=
exp
(
θ
a
∧
)
=
∑
n
=
0
∞
1
n
!
(
θ
a
∧
)
n
=
I
+
θ
a
∧
+
1
2
!
θ
2
a
∧
a
∧
+
1
3
!
θ
3
a
∧
a
∧
a
∧
+
1
4
!
θ
4
(
a
∧
)
4
+
…
=
a
a
T
−
a
∧
a
∧
+
θ
a
∧
+
1
2
!
θ
2
a
∧
a
∧
−
1
3
!
θ
3
a
∧
−
1
4
!
θ
4
(
a
∧
)
2
+
…
=
a
a
T
+
(
θ
−
1
3
!
θ
3
+
1
5
!
θ
5
−
…
)
a
∧
−
(
1
−
1
2
!
θ
2
+
1
4
!
θ
4
−
…
)
a
∧
a
∧
=
a
∧
a
∧
+
I
+
sin
θ
a
∧
−
cos
θ
a
∧
a
∧
=
(
1
−
cos
θ
)
a
∧
a
∧
+
I
+
sin
θ
a
∧
=
cos
θ
I
+
(
1
−
cos
θ
)
a
a
T
+
sin
θ
a
∧
.
\begin{aligned} \exp \left(\phi^{\wedge}\right) &=\exp \left(\theta \boldsymbol{a}^{\wedge}\right)=\sum_{n=0}^{\infty} \frac{1}{n !}\left(\theta \boldsymbol{a}^{\wedge}\right)^{n} \\ &=\boldsymbol{I}+\theta \boldsymbol{a}^{\wedge}+\frac{1}{2 !} \theta^{2} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\frac{1}{3 !} \theta^{3} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\frac{1}{4 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{4}+\ldots \\ &=\boldsymbol{a} \boldsymbol{a}^{T}-\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\theta \boldsymbol{a}^{\wedge}+\frac{1}{2 !} \theta^{2} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}-\frac{1}{3 !} \theta^{3} \boldsymbol{a}^{\wedge}-\frac{1}{4 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{2}+\ldots \\ &=\boldsymbol{a} \boldsymbol{a}^{T}+\left(\theta-\frac{1}{3 !} \theta^{3}+\frac{1}{5 !} \theta^{5}-\ldots\right) \boldsymbol{a}^{\wedge}-\left(1-\frac{1}{2 !} \theta^{2}+\frac{1}{4 !} \theta^{4}-\ldots\right) \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \\ &=\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\boldsymbol{I}+\sin \theta \boldsymbol{a}^{\wedge}-\cos \theta \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \\ &=(1-\cos \theta) \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\boldsymbol{I}+\sin \theta \boldsymbol{a}^{\wedge} \\ &=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{a} \boldsymbol{a}^{T}+\sin \theta \boldsymbol{a}^{\wedge} . \end{aligned}
exp(ϕ∧)=exp(θa∧)=n=0∑∞n!1(θa∧)n=I+θa∧+2!1θ2a∧a∧+3!1θ3a∧a∧a∧+4!1θ4(a∧)4+…=aaT−a∧a∧+θa∧+2!1θ2a∧a∧−3!1θ3a∧−4!1θ4(a∧)2+…=aaT+(θ−3!1θ3+5!1θ5−…)a∧−(1−2!1θ2+4!1θ4−…)a∧a∧=a∧a∧+I+sinθa∧−cosθa∧a∧=(1−cosθ)a∧a∧+I+sinθa∧=cosθI+(1−cosθ)aaT+sinθa∧.
📢 结果 : exp ( ϕ a ∧ ) = cos θ I + ( 1 − cos θ ) a a T + sin θ a ∧ \exp (\phi a^{\wedge}) = \cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{a} \boldsymbol{a}^{T}+\sin \theta \boldsymbol{a}^{\wedge} exp(ϕa∧)=cosθI+(1−cosθ)aaT+sinθa∧
🔶 罗德里格斯公式 : exp ( ϕ a ∧ ) = cos θ I + ( 1 − cos θ ) a a T + sin θ a ∧ \exp (\phi a^{\wedge}) = \cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{a} \boldsymbol{a}^{T}+\sin \theta \boldsymbol{a}^{\wedge} exp(ϕa∧)=cosθI+(1−cosθ)aaT+sinθa∧
🔸 这说明 so(3) 的物理意义就是旋转向量
🔸 反之,给定旋转矩阵时,亦能求李代数:
ϕ
=
ln
(
R
)
∨
=
(
∑
n
=
0
∞
(
−
1
)
n
n
+
1
(
R
−
I
)
n
+
1
)
∨
对
数
映
射
\phi=\ln (\boldsymbol{R})^{\vee}=\left(\sum_{n=0}^{\infty} \frac{(-1)^{n}}{n+1}(\boldsymbol{R}-\boldsymbol{I})^{n+1}\right)^{\vee} \quad 对数映射
ϕ=ln(R)∨=(n=0∑∞n+1(−1)n(R−I)n+1)∨对数映射
- 但实际当中没必要这样求,在旋转向量时已经介绍了矩阵到向量的转换关系:
- θ = arccos ( tr ( R ) − 1 2 ) ⋅ R n = n \theta=\arccos \left(\frac{\operatorname{tr}(\boldsymbol{R})-1}{2}\right) \cdot \quad \boldsymbol{R} \boldsymbol{n}=\boldsymbol{n} θ=arccos(2tr(R)−1)⋅Rn=n
🔺以上,说明了 SO(3) 与 so(3) 的对应关系。 |
|| SE(3) ♻️ se(3)
se(3) 到 SE(3) 的指数映射:
exp
(
ξ
∧
)
=
[
∑
n
=
0
∞
1
n
!
(
ϕ
∧
)
n
∑
n
=
0
∞
1
(
n
+
1
)
!
(
ϕ
∧
)
n
ρ
0
T
1
]
≜
[
R
J
ρ
0
T
1
]
=
T
\begin{aligned} \exp \left(\boldsymbol{\xi}^{\wedge}\right) &=\left[\begin{array}{cc} \sum_{n=0}^{\infty} \frac{1}{n !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n} & \sum_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n} \boldsymbol{\rho} \\ \mathbf{0}^{T} & 1 \end{array}\right] \\ & \triangleq\left[\begin{array}{cc} \boldsymbol{R} & \boldsymbol{J} \boldsymbol{\rho} \\ \mathbf{0}^{T} & 1 \end{array}\right]=\boldsymbol{T} \end{aligned}
exp(ξ∧)=[∑n=0∞n!1(ϕ∧)n0T∑n=0∞(n+1)!1(ϕ∧)nρ1]≜[R0TJρ1]=T
其中
J
J
J 为雅可比矩阵
J
=
sin
θ
θ
I
+
(
1
−
sin
θ
θ
)
a
a
T
+
1
−
cos
θ
θ
a
∧
.
J=\frac{\sin \theta}{\theta} \boldsymbol{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a} a^{T}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge} .
J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧.
🔺 SE(3) 与 se(3) 的对应关系。 |
🔻 总结 |
| 4.李代数求导与扰动模型
• SLAM 的定位即位姿估计
• 但李群无加法:
R
1
+
R
2
∉
S
O
(
3
)
R_1 + R_2 \notin SO(3)
R1+R2∈/SO(3) , 导数无从定义
• 解决办法:
S
O
(
3
)
:
R
SO(3):R
SO(3):R →
s
o
(
3
)
:
ϕ
\mathfrak{s o}(3) : \phi
so(3):ϕ
• 利用李代数上加法定义李群元素的导数
exp
(
(
ϕ
+
δ
ϕ
)
∧
)
\exp \left((\phi+\delta \phi)^{\wedge}\right)
exp((ϕ+δϕ)∧)
• 使用指数映射和对数映射完成变换关系。
e
a
e
b
=
e
a
+
b
e^ae^b = e^{a+b}
eaeb=ea+b
• 基本问题:当在李代数中做加法时,是否等价于在李群上做乘法?
exp
(
ϕ
1
∧
)
exp
(
ϕ
2
∧
)
=
exp
(
(
ϕ
1
+
ϕ
2
)
∧
)
❓
\exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)=\exp \left(\left(\phi_{1}+\phi_{2}\right)^{\wedge}\right) \quad ❓
exp(ϕ1∧)exp(ϕ2∧)=exp((ϕ1+ϕ2)∧)❓
🔺 在使用标量的情况下,该式明显成立 |
但 ϕ ∧ \phi^{\wedge} ϕ∧ 为矩阵 , 不成立
完整形式由 BCH (Baker Campbell Hausdorff ) 公式给出:
ln
(
exp
(
A
)
exp
(
B
)
)
=
∑
n
=
1
∞
(
−
1
)
n
−
1
n
∑
r
i
+
s
i
>
0
1
⩽
i
⩽
n
(
∑
i
=
1
n
(
r
i
+
s
i
)
)
−
1
∏
i
=
1
n
r
i
!
s
i
!
[
A
r
1
B
s
1
A
r
2
B
s
2
⋯
A
r
n
B
s
n
]
\begin{aligned} &\ln (\exp (\boldsymbol{A}) \exp (\boldsymbol{B})) \\ &\quad=\sum_{n=1}^{\infty} \frac{(-1)^{n-1}}{n} \sum_{r_{i}+s_{i}>0 \atop 1 \leqslant i \leqslant n} \frac{\left(\sum_{i=1}^{n}\left(r_{i}+s_{i}\right)\right)^{-1}}{\prod_{i=1}^{n} r_{i} ! s_{i} !}\left[\boldsymbol{A}^{r_{1}} \boldsymbol{B}^{s_{1}} \boldsymbol{A}^{r_{2}} \boldsymbol{B}^{s_{2}} \cdots \boldsymbol{A}^{r_{n}} \boldsymbol{B}^{s_{n}}\right] \end{aligned}
ln(exp(A)exp(B))=n=1∑∞n(−1)n−11⩽i⩽nri+si>0∑∏i=1nri!si!(∑i=1n(ri+si))−1[Ar1Bs1Ar2Bs2⋯ArnBsn]
其中 :
[
A
r
1
B
s
1
A
r
2
B
s
2
⋯
A
r
n
B
s
n
]
=
[
A
,
⋯
[
A
,
⏟
r
1
[
B
,
⋯
[
B
⏟
s
1
⋯
[
A
,
⋯
[
A
,
⏟
r
n
[
B
,
⋯
[
B
,
B
]
⋯
]
⏟
s
n
]
⋯
]
⋯
]
⋯
]
]
⋯
]
\begin{aligned} &{\left[\boldsymbol{A}^{r_{1}} \boldsymbol{B}^{s_{1}} \boldsymbol{A}^{r_{2}} \boldsymbol{B}^{s_{2}} \cdots \boldsymbol{A}^{r_{n}} \boldsymbol{B}^{s_{n}}\right]} \\ &=\underbrace{[\boldsymbol{A}, \cdots[\boldsymbol{A},}_{r_{1}}[\underbrace{\boldsymbol{B}, \cdots[\boldsymbol{B}}_{s_{1}} \cdots \underbrace{[\boldsymbol{A}, \cdots[\boldsymbol{A},}_{r_{n}} \underbrace{[\boldsymbol{B}, \cdots[\boldsymbol{B}, \boldsymbol{B}] \cdots]}_{s_{n}}] \cdots] \cdots] \cdots]] \cdots] \end{aligned}
[Ar1Bs1Ar2Bs2⋯ArnBsn]=r1
[A,⋯[A,[s1
B,⋯[B⋯rn
[A,⋯[A,sn
[B,⋯[B,B]⋯]]⋯]⋯]⋯]]⋯]
对于前几项:
l
n
(
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] ] + ...
ln(exp(A)exp(B))=A+B+21[A,B]+121[A,[A,B]]−121[B,[A,B]]+...
🔔 可见, 李括号阶数越高系数越小, 所以当 A, B 中一个为小量时, 可近似认为二阶以上可以忽略
即 : 当其中一个量为小量时,忽略其高阶项, BCH 具有线性近似形式:
ln
(
exp
(
ϕ
1
∧
)
exp
(
ϕ
2
∧
)
)
∨
≈
{
J
l
(
ϕ
2
)
−
1
ϕ
1
+
ϕ
2
如果
ϕ
1
是小量
J
r
(
ϕ
1
)
−
1
ϕ
2
+
ϕ
1
如果
ϕ
2
是小量
\ln \left(\exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)\right)^{\vee} \approx\left\{\begin{array}{l} \boldsymbol{J}_{l}\left(\phi_{2}\right)^{-1} \phi_{1}+\phi_{2} \quad \text { 如果} \phi_{1} \text { 是小量 } \\ \boldsymbol{J}_{r}\left(\phi_{1}\right)^{-1} \phi_{2}+\phi_{1} \quad \text {如果} \phi_{2} \text { 是小量 } \end{array}\right.
ln(exp(ϕ1∧)exp(ϕ2∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2 如果ϕ1 是小量 Jr(ϕ1)−1ϕ2+ϕ1如果ϕ2 是小量
这里的
J
l
=
J
=
sin
θ
θ
I
+
(
1
−
sin
θ
θ
)
a
a
T
+
1
−
cos
θ
θ
a
∧
−
左
雅
可
比
J
l
−
1
=
θ
2
cot
θ
2
I
+
(
1
−
θ
2
cot
θ
2
)
a
a
T
−
θ
2
a
∧
J
r
(
ϕ
)
=
J
l
(
−
ϕ
)
−
右
雅
可
比
\begin{aligned} &\boldsymbol{J}_{l}=\boldsymbol{J}=\frac{\sin \theta}{\theta} \boldsymbol{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a} \boldsymbol{a}^{T}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge} \quad - 左雅可比\\ &\boldsymbol{J}_{l}^{-1}=\frac{\theta}{2} \cot \frac{\theta}{2} \boldsymbol{I}+\left(1-\frac{\theta}{2} \cot \frac{\theta}{2}\right) \boldsymbol{a} \boldsymbol{a}^{T}-\frac{\theta}{2} \boldsymbol{a}^{\wedge} \\ &\boldsymbol{J}_{r}(\boldsymbol{\phi})=\boldsymbol{J}_{l}(-\boldsymbol{\phi}) \quad - 右雅可比 \end{aligned}
Jl=J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧−左雅可比Jl−1=2θcot2θI+(1−2θcot2θ)aaT−2θa∧Jr(ϕ)=Jl(−ϕ)−右雅可比
• 以左乘为例( 直观写法 )
• 在李群上左乘小量时,李代数上的加法相差左雅可比的逆
exp
(
Δ
ϕ
∧
)
exp
(
ϕ
∧
)
=
exp
(
(
ϕ
+
J
l
−
1
(
ϕ
)
Δ
ϕ
)
∧
)
\exp \left(\Delta \phi^{\wedge}\right) \exp \left(\phi^{\wedge}\right)=\exp \left(\left(\phi+J_{l}^{-1}(\phi) \Delta \phi\right)^{\wedge}\right)
exp(Δϕ∧)exp(ϕ∧)=exp((ϕ+Jl−1(ϕ)Δϕ)∧)
• 反之
• 李代数上进行小量加法时,相当于李群上左(右)乘一个带左(右)雅可比的量
exp
(
(
ϕ
+
Δ
ϕ
)
∧
)
=
exp
(
(
J
l
Δ
ϕ
)
∧
)
exp
(
ϕ
∧
)
=
exp
(
ϕ
∧
)
exp
(
(
J
r
Δ
ϕ
)
∧
)
\exp \left((\phi+\Delta \phi)^{\wedge}\right)=\exp \left(\left(J_{l} \Delta \phi\right)^{\wedge}\right) \exp \left(\phi^{\wedge}\right)=\exp \left(\phi^{\wedge}\right) \exp \left(\left(J_{r} \Delta \phi\right)^{\wedge}\right)
exp((ϕ+Δϕ)∧)=exp((JlΔϕ)∧)exp(ϕ∧)=exp(ϕ∧)exp((JrΔϕ)∧)
• SE(3)
exp
(
Δ
ξ
∧
)
exp
(
ξ
∧
)
≈
exp
(
(
J
l
−
1
Δ
ξ
+
ξ
)
∧
)
exp
(
ξ
∧
)
exp
(
Δ
ξ
∧
)
≈
exp
(
(
J
r
−
1
Δ
ξ
+
ξ
)
∧
)
\begin{aligned} &\exp \left(\Delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \approx \exp \left(\left(\mathcal{J}_{l}^{-1} \Delta \boldsymbol{\xi}+\boldsymbol{\xi}\right)^{\wedge}\right) \\ &\exp \left(\boldsymbol{\xi}^{\wedge}\right) \exp \left(\Delta \boldsymbol{\xi}^{\wedge}\right) \approx \exp \left(\left(\mathcal{J}_{r}^{-1} \Delta \boldsymbol{\xi}+\boldsymbol{\xi}\right)^{\wedge}\right) \end{aligned}
exp(Δξ∧)exp(ξ∧)≈exp((Jl−1Δξ+ξ)∧)exp(ξ∧)exp(Δξ∧)≈exp((Jr−1Δξ+ξ)∧)
• 其中 :
J
r
(
ξ
)
=
[
J
r
Q
r
0
J
r
]
J
ℓ
(
ξ
)
=
[
J
ℓ
Q
ℓ
0
J
ℓ
]
\begin{gathered} \mathcal{J}_{r}(\boldsymbol{\xi})=\left[\begin{array}{cc} \boldsymbol{J}_{r} & \boldsymbol{Q}_{r} \\ \boldsymbol{0} & \boldsymbol{J}_{r} \end{array}\right] \\ \mathcal{J}_{\ell}(\boldsymbol{\xi})=\left[\begin{array}{cc} \boldsymbol{J}_{\ell} & \boldsymbol{Q}_{\ell} \\ \boldsymbol{0} & \boldsymbol{J}_{\ell} \end{array}\right] \end{gathered}
Jr(ξ)=[Jr0QrJr]Jℓ(ξ)=[Jℓ0QℓJℓ]
Q
ℓ
(
ξ
)
=
∑
n
=
0
∞
∑
m
=
0
∞
1
(
n
+
m
+
2
)
!
(
ϕ
∧
)
n
ρ
∧
(
ϕ
∧
)
m
=
1
2
ρ
∧
+
(
ϕ
−
sin
ϕ
ϕ
3
)
(
ϕ
∧
ρ
∧
+
ρ
∧
ϕ
∧
+
ϕ
∧
ρ
∧
ϕ
∧
)
+
(
ϕ
2
+
2
cos
ϕ
−
2
2
ϕ
4
)
(
ϕ
∧
ϕ
∧
ρ
∧
+
ρ
∧
ϕ
∧
ϕ
∧
−
3
ϕ
∧
ρ
∧
ϕ
∧
)
+
(
2
ϕ
−
3
sin
ϕ
+
ϕ
cos
ϕ
2
ϕ
5
)
(
ϕ
∧
ρ
∧
ϕ
∧
ϕ
∧
+
ϕ
∧
ϕ
∧
ρ
∧
ϕ
∧
)
\begin{aligned} \boldsymbol{Q}_{\ell}(\boldsymbol{\xi})=& \sum_{n=0}^{\infty} \sum_{m=0}^{\infty} \frac{1}{(n+m+2) !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n} \boldsymbol{\rho}^{\wedge}\left(\boldsymbol{\phi}^{\wedge}\right)^{m} \\ =& \frac{1}{2}\boldsymbol{\rho}^{\wedge} +\left(\frac{\phi-\sin \phi}{\phi^{3}}\right)\left(\boldsymbol{\phi}^{\wedge} \boldsymbol{\rho}^{\wedge}+\boldsymbol{\rho}^{\wedge} \boldsymbol{\phi}^{\wedge}+\boldsymbol{\phi}^{\wedge} \boldsymbol{\rho}^{\wedge} \boldsymbol{\phi}^{\wedge}\right) \\ &+\left(\frac{\phi^{2}+2 \cos \phi-2}{2 \phi^{4}}\right)\left(\boldsymbol{\phi}^{\wedge} \boldsymbol{\phi}^{\wedge} \boldsymbol{\rho}^{\wedge}+\boldsymbol{\rho}^{\wedge} \boldsymbol{\phi}^{\wedge} \boldsymbol{\phi}^{\wedge}-3 \boldsymbol{\phi}^{\wedge} \boldsymbol{\rho}^{\wedge} \boldsymbol{\phi}^{\wedge}\right) \\ & \quad+\left(\frac{2 \phi-3 \sin \phi+\phi \cos \phi}{2 \phi^{5}}\right)\left(\phi^{\wedge} \boldsymbol{\rho}^{\wedge} \boldsymbol{\phi}^{\wedge} \boldsymbol{\phi}^{\wedge}+\boldsymbol{\phi}^{\wedge} \boldsymbol{\phi}^{\wedge} \boldsymbol{\rho}^{\wedge} \boldsymbol{\phi}^{\wedge}\right) \end{aligned}
Qℓ(ξ)==n=0∑∞m=0∑∞(n+m+2)!1(ϕ∧)nρ∧(ϕ∧)m21ρ∧+(ϕ3ϕ−sinϕ)(ϕ∧ρ∧+ρ∧ϕ∧+ϕ∧ρ∧ϕ∧)+(2ϕ4ϕ2+2cosϕ−2)(ϕ∧ϕ∧ρ∧+ρ∧ϕ∧ϕ∧−3ϕ∧ρ∧ϕ∧)+(2ϕ52ϕ−3sinϕ+ϕcosϕ)(ϕ∧ρ∧ϕ∧ϕ∧+ϕ∧ϕ∧ρ∧ϕ∧)
Q
r
(
ξ
)
=
Q
ℓ
(
−
ξ
)
=
C
Q
ℓ
(
ξ
)
+
(
J
ℓ
ρ
)
∧
C
J
ℓ
\boldsymbol{Q}_{r}(\boldsymbol{\xi})=\boldsymbol{Q}_{\ell}(-\boldsymbol{\xi})=\boldsymbol{C} \boldsymbol{Q}_{\ell}(\boldsymbol{\xi})+\left(\boldsymbol{J}_{\ell} \boldsymbol{\rho}\right)^{\wedge} \boldsymbol{C} \boldsymbol{J}_{\ell}
Qr(ξ)=Qℓ(−ξ)=CQℓ(ξ)+(Jℓρ)∧CJℓ
• 通过 BCH 线性近似,可以定义李代数上的导数
• 考虑一个基本问题:旋转后的点关于旋转的导数
不
严
谨
地
记
为
:
∂
(
R
p
)
∂
R
不严谨地记为:\frac{\partial (\boldsymbol R \boldsymbol p)}{\partial \boldsymbol R}
不严谨地记为:∂R∂(Rp)
• 由于 R 没有加法,导数无从定义
• 存在两种解决办法:
• 对 R 对应的李代数加上小量,求相对于小量的变化率(导数模型);
• 对 R 左乘或右乘一个小量,求相对于小量的李代数的变化率(扰动模型)。
• 导数模型 (
R
R
R →
ϕ
\phi
ϕ )
:
∂ ( exp ( ϕ ∧ ) p ) ∂ ϕ = lim δ ϕ → 0 exp ( ( ϕ + δ ϕ ) ∧ ) p − exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ → 0 exp ( ( J l δ ϕ ) ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p δ ϕ B C H ≈ lim δ ϕ → 0 ( I + ( J l δ ϕ ) ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p δ ϕ T a y l o r 一 阶 展 开 = lim δ ϕ → 0 ( J l δ ϕ ) ∧ exp ( ϕ ∧ ) p δ ϕ a ∧ b = − b ∧ a = lim δ ϕ → 0 − ( exp ( ϕ ∧ ) p ) ∧ J l δ ϕ δ ϕ = − ( R p ) ∧ J l . \begin{aligned} \frac{\partial\left(\exp \left(\phi^{\wedge}\right) \boldsymbol{p}\right)}{\partial \phi} &=\lim _{\delta \boldsymbol{\phi} \rightarrow 0} \frac{\exp \left((\phi+\delta \phi)^{\wedge}\right) \boldsymbol{p}-\exp \left(\phi^{\wedge}\right) \boldsymbol{p}}{\delta \phi} \\ &=\lim _{\delta \boldsymbol{\phi} \rightarrow 0} \frac{\exp \left(\left(\boldsymbol{J}_{l} \delta \phi\right)^{\wedge}\right) \exp \left(\phi^{\wedge}\right) p-\exp \left(\phi^{\wedge}\right) \boldsymbol{p}}{\delta \phi} \quad \textcolor{red}{BCH}\\ & \approx \lim _{\delta \boldsymbol{\phi} \rightarrow 0} \frac{\left(\boldsymbol{I}+\left(\boldsymbol{J}_{l} \delta \phi\right)^{\wedge}\right) \exp \left(\phi^{\wedge}\right) p-\exp \left(\boldsymbol{\phi}^{\wedge}\right) p}{\delta \phi} \quad \textcolor{red}{Taylor 一阶展开} \\ &=\lim _{\delta \boldsymbol{\phi} \rightarrow 0} \frac{\left(\boldsymbol{J}_{l} \delta \phi\right)^{\wedge} \exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\phi}} \quad \textcolor{red}{a^{\wedge}b = -b^{\wedge}a}\\ &=\lim _{\delta \boldsymbol{\phi} \rightarrow 0} \frac{-\left(\exp \left(\boldsymbol{\phi}^{\wedge}\right) p\right)^{\wedge} \boldsymbol{J}_{l} \delta \boldsymbol{\phi}}{\delta \phi}=-(\boldsymbol{R} p)^{\wedge} \boldsymbol{J}_{l} . \end{aligned} ∂ϕ∂(exp(ϕ∧)p)=δϕ→0limδϕexp((ϕ+δϕ)∧)p−exp(ϕ∧)p=δϕ→0limδϕexp((Jlδϕ)∧)exp(ϕ∧)p−exp(ϕ∧)pBCH≈δϕ→0limδϕ(I+(Jlδϕ)∧)exp(ϕ∧)p−exp(ϕ∧)pTaylor一阶展开=δϕ→0limδϕ(Jlδϕ)∧exp(ϕ∧)pa∧b=−b∧a=δϕ→0limδϕ−(exp(ϕ∧)p)∧Jlδϕ=−(Rp)∧Jl.
🔺 希望避免雅可比计算 |
• 扰动模型 [
更实用 ]
(左乘)
• 左乘小量,令其李代数为零
∂
(
R
p
)
∂
φ
=
lim
φ
→
0
exp
(
φ
∧
)
exp
(
ϕ
∧
)
p
−
exp
(
ϕ
∧
)
p
φ
≈
lim
φ
→
0
(
1
+
φ
∧
)
exp
(
ϕ
∧
)
p
−
exp
(
ϕ
∧
)
p
φ
T
a
y
l
o
r
一
阶
展
开
=
lim
φ
→
0
φ
∧
R
p
φ
=
lim
φ
→
0
−
(
R
p
)
∧
φ
φ
=
−
(
R
p
)
∧
\begin{aligned} \frac{\partial(\boldsymbol{R} \boldsymbol{p})}{\partial \boldsymbol{\varphi}} &=\lim _{\varphi \rightarrow 0} \frac{\exp \left(\boldsymbol{\varphi}^{\wedge}\right) \exp \left(\phi^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\varphi} \\ & \approx \lim _{\varphi \rightarrow 0} \frac{\left(1+\boldsymbol{\varphi}^{\wedge}\right) \exp \left(\phi^{\wedge}\right) p-\exp \left(\phi^{\wedge}\right) p}{\varphi} \quad \textcolor{red}{Taylor 一阶展开}\\ &=\lim _{\varphi \rightarrow 0} \frac{\varphi^{\wedge} R p}{\varphi}=\lim _{\varphi \rightarrow 0} \frac{-(\boldsymbol{R} p)^{\wedge} \boldsymbol{\varphi}}{\varphi}=-(\boldsymbol{R} p)^{\wedge} \end{aligned}
∂φ∂(Rp)=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p≈φ→0limφ(1+φ∧)exp(ϕ∧)p−exp(ϕ∧)pTaylor一阶展开=φ→0limφφ∧Rp=φ→0limφ−(Rp)∧φ=−(Rp)∧
• SE(3) 上的扰动模型 [
∂
(
T
P
)
∂
T
\color {red}{ \frac{\partial(TP)}{\partial T}}
∂T∂(TP)]:
∂
(
T
p
)
∂
δ
ξ
=
lim
δ
ξ
→
0
exp
(
δ
ξ
∧
)
exp
(
ξ
∧
)
p
−
exp
(
ξ
∧
)
p
δ
ξ
在
T
前
乘
小
的
扰
动
δ
≈
lim
δ
ξ
→
0
(
I
+
δ
ξ
∧
)
exp
(
ξ
∧
)
p
−
exp
(
ξ
∧
)
p
δ
ξ
T
a
y
l
o
r
一
阶
展
开
=
lim
δ
ξ
→
0
δ
ξ
∧
exp
(
ξ
∧
)
p
δ
ξ
=
lim
δ
ξ
→
0
[
δ
ϕ
∧
δ
ρ
0
T
0
]
[
R
p
+
t
1
]
δ
ξ
=
[
δ
ρ
,
δ
ϕ
]
=
[
I
−
(
R
p
+
t
)
∧
0
T
0
T
]
≜
(
T
p
)
⊙
.
\begin{aligned} \frac{\partial(\boldsymbol{T} \boldsymbol{p})}{\partial \delta \boldsymbol{\xi}} &=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\exp \left(\delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}} \quad \textcolor{red}{在 T 前乘小的扰动 \delta} \\ & \approx \lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\left(\boldsymbol{I}+\delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}} \quad \textcolor{red}{Taylor 一阶展开}\\ &=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\delta \boldsymbol{\xi}^{\wedge} \exp \left(\boldsymbol{\xi}^{\wedge}\right) p}{\delta \boldsymbol{\xi}} \\ &=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\left[\begin{array}{cc} \delta \boldsymbol{\phi}^{\wedge} & \delta \boldsymbol{\rho} \\ \mathbf{0}^{T} & 0 \end{array}\right]\left[\begin{array}{c} R \boldsymbol{p}+\boldsymbol{t} \\ 1 \end{array}\right]}{\delta \boldsymbol{\xi} \quad \textcolor{red}{= [\delta \rho, \delta \phi]}}=\left[\begin{array}{cc} \boldsymbol{I} & -(\boldsymbol{R} p+\boldsymbol{t})^{\wedge} \\ \mathbf{0}^{T} & 0^{T} \end{array}\right] \triangleq(\boldsymbol{T} \boldsymbol{p})^{\odot} . \end{aligned}
∂δξ∂(Tp)=δξ→0limδξexp(δξ∧)exp(ξ∧)p−exp(ξ∧)p在T前乘小的扰动δ≈δξ→0limδξ(I+δξ∧)exp(ξ∧)p−exp(ξ∧)pTaylor一阶展开=δξ→0limδξδξ∧exp(ξ∧)p=δξ→0limδξ=[δρ,δϕ][δϕ∧0Tδρ0][Rp+t1]=[I0T−(Rp+t)∧0T]≜(Tp)⊙.
• 小结
• 利用 BCH 线性近似,可以推导 so(3) 与 se(3) 上的导数和扰动模型
• 通常情况下,扰动模型更为简洁实用