SLAM中的李群与李代数【SLAM】

SLAM中为什么要用李代数?

本人从SLAM问题的角度出发得出的结论目前是这样的:在SLAM问题中,为了表示旋转姿态需要旋转矩阵或者四元数,在后端优化过程中为了求解非线性优化问题需要求雅可比矩阵,即求导;在运动学的描述中也需要求导;这两种情况中一般是对位姿求导,这时就产生了一个矛盾:根据导数的定义可知,对某一个变量求导,定义中需要该变量产生微小增量,为了保证物理意义的正确,我们希望该变量对加法是封闭的,而旋转矩阵或者四元数对加法都不封闭,所以引入李代数。李代数对李括号具有封闭性,李代数和李群中的元素通过指数映射和对数映射一一对应(对于 S O ( 3 ) SO(3) SO(3)李群需要规定旋转角在 ± π \pm \pi ±π之间),SLAM中引入的李代数的李括号刚好都是加法,这样我们就可以用李代数进行求导过程。

BCH线性近似解决了什么问题?

BCH线性近似提供了在SLAM中的李代数上做加法 与 李群上做乘法的对应关系。

指数映射与对数映射

每个李群都有与之对应的李代数,李代数描述了李群的局部性质,即单位元的正切空间。

与 S O ( 3 ) SO(3) SO(3)对应的李代数为 s o ( 3 ) \mathfrak{so}(3) so(3):
s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } \mathfrak{so}(3) = \{ \phi \in \mathbb{R}^{3}, \mathit{\Phi} = \phi^{\wedge} \in \mathbb{R}^{3\times3} \} so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}

SO(3)上的指数映射

R = e x p ( ϕ ∧ ) = ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n R = exp(\phi^{\wedge}) = \sum_{n=0}^{\infty} \frac{1}{n!}(\phi^{\wedge})^{n} R=exp(ϕ∧)=n=0∑∞n!1(ϕ∧)n

如果把 ϕ \phi ϕ写成 θ n \theta \mathbf{n} θn,经过推导会发现:
e x p ( θ n ∧ ) = c o s θ I + ( 1 − c o s θ ) n n T + s i n θ n ∧ exp(\theta \mathbf{n}^{\wedge}) = cos\theta I + (1-cos \theta) \mathbf{n} \mathbf{n}^{T} + sin \theta \mathbf{n}^{\wedge} exp(θn∧)=cosθI+(1−cosθ)nnT+sinθn∧

这与罗德里格斯公式如出一辙。

这说明 s o ( 3 ) \mathfrak{so}(3) so(3)的物理意义是和旋转矩阵 R R R对应的旋转向量。

SO(3)上的对数映射

ϕ = l n ( R ) ∨ = ( ∑ n = 0 ∞ ( − 1 ) n n + 1 ( R − I ) n + 1 ) \phi = ln(R)^{\vee} = \left ( \sum_{n=0}^{\infty} \frac{(-1)^{n}}{n+1}(R-I)^{n+1} \right) ϕ=ln(R)∨=(n=0∑∞n+1(−1)n(R−I)n+1)

一般不用,要求 S O ( 3 ) SO(3) SO(3)的对数映射其实就是求对应的旋转向量,直接求 θ \theta θ和特征值为1的特征向量即可。

BCH线性近似

BCH公式提供了两个李代数指数映射乘积的完整形式:
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]+\dots ln(exp(A)exp(B))=A+B+21A,B+121A,\[A,B]−121B,\[A,B]+...

二元运算符 , , ,为李括号。

线性近似表达:
l n ( 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}^{\wedge})exp(\phi_{2}^{\wedge}))^{\vee} \approx \begin{cases} J_{l}(\phi_{2})^{-1}\phi_{1} + \phi_{2} & \text{ 当 } \phi_{1}\text{为小量} \\ J_{r}(\phi_{1})^{-1}\phi_{2} + \phi_{1} & \text{ 当 } \phi_{2}\text{为小量} \end{cases} ln(exp(ϕ1∧)exp(ϕ2∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2Jr(ϕ1)−1ϕ2+ϕ1 当 ϕ1为小量 当 ϕ2为小量

对于 ϕ = θ n \phi = \theta \mathbf{n} ϕ=θn:
J l ( ϕ ) = s i n θ θ I + ( 1 − s i n θ θ ) n n T + 1 − c o s θ θ n ∧ J_{l}(\phi) = \frac{sin\theta}{\theta}I + (1 - \frac{sin\theta}{\theta})\mathbf{n}\mathbf{n}^{T} + \frac{1 - cos\theta}{\theta}\mathbf{n}^{\wedge} Jl(ϕ)=θsinθI+(1−θsinθ)nnT+θ1−cosθn∧

J l ( ϕ ) − 1 = θ 2 c o t θ 2 I + ( 1 − θ 2 c o t θ 2 ) n n T − θ 2 n ∧ J_{l}(\phi)^{-1} = \frac{\theta}{2}cot\frac{\theta}{2}I + (1 - \frac{\theta}{2}cot\frac{\theta}{2})\mathbf{n}\mathbf{n}^{T} -\frac{\theta}{2}\mathbf{n}^{\wedge} Jl(ϕ)−1=2θcot2θI+(1−2θcot2θ)nnT−2θn∧

J r ( ϕ ) = J l ( − ϕ ) J_{r}(\phi) = J_{l}(-\phi) Jr(ϕ)=Jl(−ϕ)

相关推荐
漂流瓶jz30 分钟前
UVA-1615 高速公路 题解答案代码 算法竞赛入门经典第二版
数据结构·算法·贪心算法·贪心·aoapc·算法竞赛入门经典·uva
凌冰_11 小时前
数据结构 - 时间空间复杂度(一)
数据结构·算法·软件工程
Pniubi11 小时前
力扣2 . 两数相加
算法·leetcode·职场和发展
戴·菲瑜11 小时前
为什么PPO难以做出顶级棋类引擎
算法·博弈论·rl·ppo
在所不辞兄14 小时前
为什么数字孪生驱动的产线标定需要在线更新机制,而不能一次标定后长期沿用?
人工智能·神经网络·其他·算法·机器学习
AvocadoZeroOneOI18 小时前
B4500 [GESP202603 三级] 凯撒密码 详细解题报告
c++·算法
cjz389920 小时前
双指针技巧:覆盖式双指针(原地覆盖)
算法
橘子131 天前
【高级数据库】关系代数基本概念
算法
电科_银尘1 天前
【大模型算法与工程 15】手写Self-Attention:Q、K、V与缩放点积
算法
郝学胜-神的一滴2 天前
Effective STL 条款03:容器中的对象复制要廉价且正确
开发语言·数据结构·c++·算法·rpc·stl