这是一个关于《李群与李代数》在三维刚体运动（SLAM）中应用的详细讲解。根据你提供的课件内容，我将这些知识点整理为逻辑严密的六个部分：动机、基本概念（群与代数）、指数映射、BCH公式与求导、扰动模型以及代码实践。

1. 为什么要引入李群与李代数？（动机）

在做SLAM或机器人状态估计时，我们核心任务是对位姿（旋转和平移）进行估计和优化。

  • 传统方法的局限性：

    • 三维旋转矩阵 $R$ 必须满足正交且行列式为1的约束（$RR^T=I, \det(R)=1$）。

    • 作为优化变量时，这些额外约束会使优化问题变得非常困难（有约束优化通常比无约束优化难解）。

  • 李代数的优势：

    • 李群（如旋转矩阵）没有定义良好的加法，难以求导。

    • 李代数对应于李群单位元处的切空间（Tangent Space）。

    • 在李代数上，我们可以把位姿变为向量形式，进行无约束优化，这对非线性优化（如Gauss-Newton法）至关重要。

2. 基础概念：李群 (Lie Group)

群 (Group) 是一种集合加上一种运算的代数结构。

  • 四大性质： 封闭性、结合律、幺元、逆 1。

  • 常见的群：

    • 一般线性群 $GL(n)$： $n \times n$ 可逆矩阵。

    • 特殊正交群 $SO(3)$： 即旋转矩阵群。$R \in \mathbb{R}^{3\times 3}, RR^T=I, \det(R)=1$。

    • 特殊欧氏群 $SE(3)$： 即变换矩阵群。包含旋转 $R$ 和平移 $t$，$T = \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix}$。

李群是指具有连续（光滑）性质的群。直观上看，刚体在空间中的运动是连续的，所以 $SO(3)$ 和 $SE(3)$ 都是李群。

3. 核心概念：李代数 (Lie Algebra)

每个李群都有与之对应的李代数，描述了群在单位元附近的局部性质（切空间）。李代数由一个集合 $\mathbb{V}$、数域 $\mathbb{F}$ 和二元运算（李括号 [,]）组成。

3.1 $\mathfrak{so}(3)$：对应三维旋转

  • 定义： 这是一个三维向量 $\phi \in \mathbb{R}^3$。

  • 反对称矩阵（Hat Operator $^\wedge$）：

    将向量 $\phi = [\phi_1, \phi_2, \phi_3]^T$ 映射为一个反对称矩阵 $\Phi = \phi^\wedge$：

    $$\phi^\wedge = \begin{bmatrix} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{bmatrix}$$

  • 物理意义： $\mathfrak{so}(3)$ 的物理意义就是旋转向量。

3.2 $\mathfrak{se}(3)$：对应三维变换

  • 定义： 这是一个六维向量 $\xi \in \mathbb{R}^6$。

    $$\xi = \begin{bmatrix} \rho \\ \phi \end{bmatrix}$$

    • $\rho \in \mathbb{R}^3$：平移部分（注意：虽然叫平移，但不直接等同于 $SE(3)$ 中的 $t$，二者差一个线性变换）。

    • $\phi \in \mathbb{R}^3$：旋转部分（与 $\mathfrak{so}(3)$ 一致）。

  • 扩展的Hat Operator： 在 $\mathfrak{se}(3)$ 中，$\wedge$ 符号将其映射为 $4 \times 4$ 矩阵：

    $$\xi^\wedge = \begin{bmatrix} \phi^\wedge & \rho \\ 0^T & 0 \end{bmatrix}$$

4. 桥梁：指数映射 (Exponential Map)

指数映射建立了**李代数（向量/矩阵）到李群（旋转/变换矩阵）**的联系。

$$R = \exp(\phi^\wedge)$$

4.1 SO(3) 的指数映射（罗德里格斯公式）

通过泰勒展开并利用反对称矩阵的性质（$a^\wedge a^\wedge = aa^T - I$），可以得到著名的罗德里格斯公式 (Rodrigues' Formula)：

$$\exp(\theta a^\wedge) = \cos\theta I + (1-\cos\theta)aa^T + \sin\theta a^\wedge$$

其中 $\theta$ 是旋转角度，$a$ 是旋转轴（单位向量）。

  • 反向映射（对数映射）： 给定旋转矩阵 $R$，也可以求出对应的李代数 $\phi$（即求转轴和转角）。

4.2 SE(3) 的指数映射

对于六维向量 $\xi$，其指数映射为：

$$\exp(\xi^\wedge) = \begin{bmatrix} R & J\rho \\ 0^T & 1 \end{bmatrix} = T$$

  • 注意： 平移部分不是直接相加，而是多了一个左雅可比矩阵 $J$。$J$ 描述了李代数上的平移分量如何转换到群上的平移分量。

5. 关键难点：求导与扰动模型

我们在优化中需要求导，但李群只有乘法没有加法：$\exp(\phi_1^\wedge)\exp(\phi_2^\wedge) \neq \exp((\phi_1+\phi_2)^\wedge)$。这导致无法直接定义导数。

5.1 BCH 公式 (Baker-Campbell-Hausdorff)

BCH 公式解决了“两个指数映射的乘积等于什么”的问题。

当其中一个量为小量（$\Delta \phi$）时，BCH 提供了线性近似：

$$\ln(\exp(\phi^\wedge)\exp(\Delta \phi^\wedge))^\vee \approx \phi + J_l^{-1}(\phi)\Delta \phi$$

这说明：在李群上左乘一个小量，等于在李代数上加上一个“带左雅可比逆”的小量。

5.2 两种求导思路

为了对位姿求导（例如求 $\frac{\partial (Rp)}{\partial R}$），有两种方法：

  1. 导数模型（不推荐）：

    • 思路：在李代数上直接加一个小量 $\delta \phi$，即 $\exp((\phi + \delta\phi)^\wedge)$。

    • 结果：由于 BCH 公式的存在，结果中会引入复杂的雅可比矩阵 $J_l$，计算极其繁琐。

  2. 扰动模型（Perturbation Model，推荐）：

    • 思路：在李群上左乘（或右乘）一个微小扰动 $\exp(\varphi^\wedge)$，令 $\varphi \to 0$。

    • 公式推导（左扰动为例）：

      $$\lim_{\varphi \to 0} \frac{\exp(\varphi^\wedge)\exp(\phi^\wedge)p - \exp(\phi^\wedge)p}{\varphi}$$

    • 最终结果：

      $$\frac{\partial (Rp)}{\partial \varphi} = -(Rp)^\wedge$$

    • 优势： 结果非常简洁，避开了复杂的雅可比矩阵 $J_l$。这也是视觉SLAM中通常使用的方法。

6. 实践：Sophus 库

课件最后介绍了 Sophus 库，这是一个基于 Eigen 的 C++ 库，专门用于处理李群李代数 2。

  • 核心功能：

    • 支持 $SO(3)$ 和 $SE(3)$ 的构造与运算。

    • 提供 exp() 和 log() 函数进行群与代数的转换。

    • 支持 hat() (向量转矩阵) 和 vee() (矩阵转向量) 操作。

  • 代码示例要点：

    • Sophus::SO3::exp(phi)：将旋转向量转为旋转矩阵。

    • Sophus::SE3 的构造通常先把旋转和平移定义好，注意平移在前还是旋转在前的约定（Sophus 内部通常是平移在前，旋转在后，但构造函数可能不同，需看具体版本）。

    • 编译： 这是一个 CMake 工程，依赖 Eigen，需要标准的 cmake .. 和 make 流程 3。

总结

  • 李群 (Group): 旋转/变换矩阵本身，有约束，难优化。

  • 李代数 (Algebra): 对应的向量形式，无约束，易优化。

  • 映射: 指数映射 ($\exp$) 和对数映射 ($\ln$) 在两者间转换。

  • 求导: 使用左乘扰动模型，将复杂的求导问题转化为简单的矩阵运算（$-(Rp)^\wedge$），这是 SLAM 后端优化的数学基础。

下一步建议：

你是否需要我解释一下左扰动模型和右扰动模型的具体区别，或者用一个具体的数学例子（比如推导 $-(Rp)^\wedge$ 的过程）来加深理解？


标签: none

评论已关闭