【SLAM数学基础】李群与李代数 & BCH近似公式

最新推荐文章于 2024-08-21 20:20:33 发布

原创最新推荐文章于 2024-08-21 20:20:33 发布 · 1.9k 阅读

2 ·

CC 4.0 BY-SA版权

文章标签：

#机器人 #数学建模 #算法 #矩阵 #人工智能

机器人与SLAM数学基础专栏收录该内容

4 篇文章

订阅专栏

本文介绍了三维旋转群SO(3)和三维变换群SE(3)的李代数，重点讨论了指数映射、对数映射以及BCH公式，特别是如何在自动驾驶和机器人领域的SLAM技术中应用BCH近似来处理旋转和变换问题。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

三维旋转构成了三维旋转群 SO(3)，其对应的李代数为 $so\mathfrak{so}$ (3)；三维变换构成了三维变换群 SE(3)，其对应的李代数为 $se\mathfrak{se}$ (3)。

1.指数映射

李代数元素到李群元素的映射为指数映射，其中 $so\mathfrak{so}$ (3) 至 SO(3) 的指数映射为：

$exp⁡(ϕ∧)=R=Exp(ϕ)\exp(\bm{\phi}^{\wedge}) = \bm{R} = \text{Exp}(\bm{\phi})$

具体计算公式由罗德里格斯公式给出：

$\bm{R} = \cos\theta \cdot \bm{I} + (1-\cos \theta) \bm{n}\bm{n}^T + \sin\theta\cdot \bm{n}^{\wedge} = \exp(\bm{\phi}^{\wedge})$

其中 $ϕ∈so(3)\bm{\phi}\in \mathfrak{so}(3)$ 可以分解为 $ϕ=θn\bm{\phi} =\theta\bm{n}$ .

2.对数映射

从李群元素到李代数元素的映射为对数映射，记作

$ϕ=log⁡(R)∨=Log(R)\bm{\phi} = \log(\bm{R})^{\vee} = \text{Log}(\bm{R})$

其中 $ϕ∈so(3)\bm{\phi}\in \mathfrak{so}(3)$ 可以分解为 $ϕ=θn\bm{\phi} =\theta\bm{n}$ ，具体的计算过程由下式给出：

$θ=arccos⁡(tr(R)−12)Rn=n\theta = \arccos\left(\frac{tr(\bm{R}) - 1}{2}\right) \\ \bm{R}\bm{n} = \bm{n}$

其中转轴 $n\bm{n}$ 是矩阵 $R$ 特征值1对应的单位特征向量。

3. BCH公式及其线性近似表达

两个李代数指数映射乘积的完整形式，由 Baker-Campbell-Hausdorff 公式（BCH公式）给出，其展开式的前几项为：

$ln⁡(exp⁡(A)exp⁡(B))=A+B+12[A,B]+112[A,[A,B]]−112[B,[A,B]]+⋯\ln\left(\exp(A)\exp(B)\right) = A + B + \frac{1}{2}[A, B] + \frac{1}{12}[A,[A,B]] - \frac{1}{12}[B,[A,B]] + \cdots$

其中 [ ] 为李括号。特别地，考虑 SO(3) 上的李代数 $ln⁡(exp⁡(ϕ1∧)exp⁡(ϕ2∧))∨\ln\left(\exp(\bm{\phi}^{\wedge}_1)\exp(\bm{\phi}^{\wedge}_2)\right)^{\vee}$ ，当 $ϕ1\bm{\phi}_1$ 或 $ϕ2\bm{\phi}_2$ 为小量时，小量二次以上的项可以忽略，此时 BCH 具有线性近似表达：

在这里插入图片描述

第一个近似公式表明，当对一个旋转矩阵 $R2\bm{R}_2$ (李代数为 $ϕ2\bm{\phi}_2$ ) 左乘一个微小旋转矩阵 $R1\bm{R}_1$ (李代数为 $ϕ1\bm{\phi}_1$ )时，可以近似看作在原来的李代数 $ϕ2\bm{\phi}_2$ 加上了一项 $Jl(ϕ2)−1ϕ1\bm{J}_l(\bm{\phi}_2)^{-1}\bm{\phi}_1$ 。同理，第二个近似公式描述了右乘一个微小旋转的情况。

因此，李代数在BCH近似下分成了左乘和右乘近似两种，使用时需要注意区分。

4. SO(3)上的BCH近似公式

BCH公式给出了李代数上的小量加法与李群上小量乘法之间的关系（李代数加法 $⇔\Leftrightarrow$ 李群乘法），其线性近似公式广泛应用于各种函数的线性化。

在SO(3)中，某个旋转 $R\bm{R}$ 对应的李代数为 $ϕ\bm{\phi}$ ，左乘一个微小旋转，记作 $ΔR\Delta \bm{R}$ ，对应的李代数为 $Δϕ\Delta \bm{\phi}$ ，那么在李群上得到的结果就是 $ΔRR\Delta \bm{R} \bm{R}$ ，而在李代数上，根据BCH近似，为 $Jl(ϕ)−1Δϕ+ϕ\bm{J}_l(\bm{\phi})^{-1}\Delta\bm{\phi} + \bm{\phi}$ ，合并后可以简单写成：

$\Delta \bm{R} \bm{R} = \exp(\Delta \bm{\phi}^{\wedge})\exp(\bm{\phi}^{\wedge}) = \exp \left( (\bm{\phi} + \bm{J}_l(\bm{\phi})^{-1}\Delta\bm{\phi})^{\wedge} \right)$

反过来，如果在李代数上进行加法，让一个 $ϕ\bm{\phi}$ 加上小量 $Δϕ\Delta \bm{\phi}$ ，那么可以近似为李群上带左右雅克比矩阵的乘法：

$\exp((\bm{\phi} + \Delta \bm{\phi})^{\wedge}) = \exp((\bm{J}_l(\bm{\phi})\Delta \bm{\phi})^\wedge) \exp(\bm{\phi}^\wedge) = \exp( \bm{\phi}^\wedge) \exp((\bm{J}_r(\bm{\phi})\Delta\bm{\phi})^\wedge)$

其中SO(3)的左雅克比矩阵为

$Jl(θa)=sin⁡θθI+(1−sin⁡θθ)aaT+(1−cos⁡θθ)a∧Jl−1(θa)=θ2cot⁡θ2I+(1−θ2cot⁡θ2)aaT−θ2a∧\bm{J}_l(\theta\bm{a}) = \frac{\sin\theta}{\theta} \bm{I} + (1-\frac{\sin\theta}{\theta})\bm{a}\bm{a}^T+(\frac{1-\cos\theta}{\theta})\bm{a}^{\wedge} \\ \bm{J}^{-1}_l(\theta\bm{a}) = \frac{\theta}{2}\cot\frac{\theta}{2} \bm{I} + (1-\frac{\theta}{2}\cot\frac{\theta}{2})\bm{a}\bm{a}^T-\frac{\theta}{2}\bm{a}^{\wedge}$

而SO(3)的右雅克比矩阵为

$Jr(ϕ)=Jl(−ϕ)\bm{J}_r(\bm{\phi}) = \bm{J}_l(-\bm{\phi})$

值得注意的是，由于李代数 $ϕ\bm{\phi}$ 可以和旋转矩阵 $R\bm{R}$ 简单地对应起来，因此有时也把 $Jr(ϕ)\bm{J}_r(\bm{\phi})$ 简单地记作 $Jr(R)\bm{J}_r(\bm{R})$ 而不是 $Jr(Log(R))\bm{J}_r(\text{Log}(\bm{R}))$ 。另外，在很多情况下，也会省略 $Jr(ϕ)\bm{J}_r(\bm{\phi})$ 括号里的内容，而直接记为 $Jr\bm{J}_r$ 和 $Jl\bm{J}_l$ ，这都是为了让公式看上去更加简洁。