第4讲 李群与李代数

4.1 概述

4.1.1 目标

上一讲介绍了旋转矩阵、欧拉角、四元数、旋转向量等表示方法，但在 SLAM 里，仅仅“能表示”还不够，更重要的是：

• 如何把位姿作为未知量进行估计；
• 如何在优化中更新位姿；
• 如何对位姿求导；
• 如何把带约束的旋转/变换问题改写成无约束优化问题。

为了解决这些问题，本讲引入：

• 李群 SO(3)、SE(3)；
• 李代数 so(3)、se(3)；
• 指数映射与对数映射；
• BCH 近似；
• 李代数上的求导与扰动模型；
• Sophus 库中的实际编程操作。

李群负责描述“全局姿态”，李代数负责描述“局部增量”，从而让姿态估计能够进入优化与微积分框架。

4.1.2 为什么需要李群李代数

在 SLAM 中，我们常常要估计位姿 R, t，通常会建立类似下面的优化目标：

min error (R, t)

问题在于：

• R 不是任意矩阵，而必须满足正交约束 R R^T = I；
• 且 det(R)=1；
• T 也必须满足特定结构。

如果把旋转矩阵直接作为优化变量，就会遇到约束优化的问题，处理起来很麻烦。

李群与李代数提供的思路是：

• 用李群表示真实的姿态；
• 用李代数表示局部微小变化；
• 在李代数空间中做加法和优化；
• 再通过指数映射把增量送回李群。

这样可以把原本“带约束”的优化问题转化为“局部无约束”的优化问题。

---

4.2 群、李群、李代数的基本概念

4.2.1 群是什么

群是“一个集合 + 一种运算”的代数结构，要求满足四条性质：

1. 封闭性：集合中任意两个元素运算后仍在集合内；
2. 结合律：运算满足结合律；
3. 幺元：存在单位元；
4. 逆元：每个元素都有逆。

旋转矩阵和变换矩阵都不对加法封闭，但对乘法封闭，所以它们构成群。

常见矩阵群：

• GL(n)：一般线性群，可逆矩阵；
• SO(n)：特殊正交群，旋转矩阵群；
• SE(n)：特殊欧氏群，欧氏变换群。

4.2.2 李群是什么

李群就是具有连续光滑结构的群。

这点非常符合刚体运动的直觉：相机在空间中的位姿变化通常是连续的，因此 SO(3) 与 SE(3) 都是李群。

4.2.3 李代数是什么

李代数描述的是李群在单位元附近的局部线性结构，可以理解成李群的切空间。

它由：

• 一个集合 V；
• 一个数域 F；
• 一个二元运算 [ , ]（李括号）

组成，并满足封闭性、双线性、自反性和雅可比恒等式。

直观上：

• 李群适合表示“姿态本身”；
• 李代数适合表示“姿态的微小变化”；
• 优化、线性化、求导通常都在李代数上进行。

---

4.3 SO(3) 与 so(3)

4.3.1 SO(3) 的定义

三维旋转矩阵构成特殊正交群：

SO(3) = {R ∈ ℝ^3 × 3 ∣ RR^T = I, det (R) = 1}

它表示三维空间中的纯旋转。

4.3.2 so(3) 的元素

设旋转矩阵随时间变化为 R(t)，由约束条件：

R(t)R(t)^T = I

两边对时间求导，可以得到：

Ṙ(t)R(t)^T

是一个反对称矩阵。

而任意反对称矩阵都可以与一个三维向量一一对应，因此引入：

• ∧：向量到反对称矩阵（hat）；
• ∨：反对称矩阵到向量（vee）。

若

ϕ = [ϕ₁, ϕ₂, ϕ₃]^T

则

这个三维向量 ϕ 就是 so(3) 的元素。

4.3.3 so(3) 的含义

so(3) 的元素本质上是：

• 三维向量；
• 或其对应的反对称矩阵；
• 它描述的是旋转在单位元附近的局部变化率。

所以：

so(3) = {ϕ ∈ ℝ³}

并通过 hat/vee 与反对称矩阵互相转换。

4.3.4 so(3) 的李括号

两个 so(3) 元素 ϕ1, ϕ2 的李括号定义为：

[ϕ₁, ϕ₂] = (ϕ₁^∧ϕ₂^∧ − ϕ₂^∧ϕ₁^∧)^∨

它反映了两个局部变化之间的非交换性。

---

4.4 SE(3) 与 se(3)

4.4.1 SE(3) 的定义

三维刚体变换构成特殊欧氏群：

其中：

• R ∈ SO(3)；
• t ∈ R^3。

它表示旋转和平移组成的刚体位姿。

4.4.2 se(3) 的定义

SE(3) 对应的李代数是 se(3)，它是一个六维向量：

其中：

• ρ 表示平移部分；
• ϕ 表示旋转部分。

其 hat 形式为：

注意这里书中的约定是：平移在前，旋转在后。

4.4.3 se(3) 的含义

可以把 se(3) 理解成：

• 旋转的局部增量；
• 平移的局部增量；
• 它们组成了刚体运动在单位元附近的切空间。

在优化中，位姿更新量通常就写成一个六维向量 δξ。

---

4.5 SO(3) 上的指数与对数映射

4.5.1 指数映射

若 ϕ = θ a，其中：

• θ = ||ϕ|| 为旋转角；
• a 为单位旋转轴；

则：

R = exp (ϕ^∧) = exp (θa^∧)

推导后可以得到罗德里格斯公式：

exp (θa^∧) = cos θ I + (1 − cos θ)aa^T + sin θ a^∧

这说明：

• so(3) 实际上就是旋转向量空间；
• 指数映射本质上就是把旋转向量变成旋转矩阵。

4.5.2 对数映射

对数映射则是反过来，把 SO(3) 中的旋转矩阵还原为 so(3) 中的旋转向量：

ϕ = log (R)^∨

实际计算中通常不会用泰勒展开，而是直接利用旋转矩阵的迹 tr(R) 来求旋转角，再恢复旋转轴。

4.5.3 单值性问题

指数映射不是一一映射，而是满射。

原因在于旋转具有周期性：

• 转 0° 和转 360° 对应同一个旋转矩阵；
• 因此多个李代数元素可能映射到同一个李群元素。

如果把角度限制在 [-π, π]，就能得到局部一一对应关系。

---

4.6 SE(3) 上的指数与对数映射

4.6.1 指数映射形式

对于 ξ = [ρ, ϕ]^T，有：

其中：

• R = exp(ϕ^)；
• J 是一个与旋转相关的雅可比矩阵。

4.6.2 左雅可比 J

书中给出的形式是：

它的作用非常关键：

• se(3) 里的平移部分 ρ，经过指数映射后并不会直接变成 t；
• 而是先经过 J 的变换，即 t = Jρ。

这说明刚体运动中的平移和旋转并不是完全独立的。

4.6.3 对数映射

若已知变换矩阵 T，则：

1. 先从左上角的 R 求出旋转向量 ϕ；
2. 再通过线性方程

t = Jρ

解出 ρ。

因此，SE(3) 的对数映射本质上分成两步：

• 先解旋转；
• 再用旋转诱导出的雅可比去解平移。

---

4.7 BCH 公式

4.7.1 为什么需要 BCH

在普通标量里：

exp (a)exp (b) = exp (a + b)

但在矩阵中一般不成立，因为矩阵乘法通常不可交换。

于是就有：

log (exp (A)exp (B)) ≠ A + B

两者之间的真实关系由 BCH（Baker-Campbell-Hausdorff）公式给出。

4.7.2 BCH 展开

前几项是：

这表明：

• 李代数中的“加法”并不完全等价于李群中的“乘法”；
• 两者之间会多出李括号相关项；
• 这些额外项本质上来自非交换性。

4.7.3 BCH 的线性近似

当其中一个增量很小时，高阶项可以忽略，于是有近似：

log (exp (ϕ₁^∧)exp (ϕ₂^∧))^∨ ≈ J_l(ϕ₂)⁻¹ϕ₁ + ϕ₂

或

 ≈ J_r(ϕ₁)⁻¹ϕ₂ + ϕ₁

这两个公式分别对应：

• 左乘微小增量；
• 右乘微小增量。

4.7.4 BCH 的价值

这是本讲最重要的工程意义之一。

它告诉我们：

• 在李群上做一个微小乘法更新；
• 可以近似看作在李代数上做一次加法；
• 但需要通过左右雅可比进行修正。

这就是后面位姿优化里“增量更新”的理论基础。

---

4.8 左雅可比与右雅可比

4.8.1 左雅可比

书中采用左乘模型，因此重点使用左雅可比 Jl：

J_l = J

其形式与前面 SE(3) 指数映射中的 J 一致。

4.8.2 左雅可比逆

左雅可比逆为：

4.8.3 右雅可比

右雅可比与左雅可比关系为：

J_r(ϕ) = J_l(−ϕ)

4.8.4 工程理解

左右雅可比并不是抽象装饰，它们回答的是：

• “一个微小扰动加在左边还是右边？”
• “它在李代数坐标系里应该如何解释？”

如果模型采用左扰动，就必须配套使用左雅可比；采用右扰动，就必须使用右雅可比。

---

4.9 李代数求导与扰动模型

这一部分是整讲最关键的应用层内容，因为 SLAM 的优化离不开雅可比和线性化。

4.9.1 典型优化背景

设空间点 p 被位姿 T 变换后，在相机中产生观测 z：

z = Tp + w

于是误差定义为：

e = z − Tp

有多个点之后，就形成一个最小二乘问题：

min ∑_i‖z_i − Tp_i‖²

想要求解它，就必须对位姿求导。

但李群本身不能直接做普通加法，因此求导通常有两种思路：

1. 直接对李代数求导；
2. 对李群施加微扰，再对扰动求导。

第二种就是所谓的扰动模型。

4.9.2 SO(3) 上的直接李代数求导

考虑旋转后的点：

Rp = exp (ϕ^∧)p

要求它对 ϕ 的导数：

利用 BCH 线性近似和泰勒展开，书中推导得到：

这个结果是正确的，但里面含有 Jl，形式较复杂，因此在实际计算中不够方便。

4.9.3 SO(3) 上的左扰动模型

左扰动定义

令当前旋转为：

R = exp (ϕ^∧)

给它左乘一个很小的扰动：

ΔR = exp (φ^∧)

于是新的旋转为：

ΔR R

对扰动求导

此时求：

利用一阶近似：

exp (φ^∧) ≈ I + φ^∧

可以直接得到：

4.9.4 为什么扰动模型更常用

和直接李代数求导相比，扰动模型少了 Jl：

• 公式更简单；
• 计算代价更低；
• 更适合实现高斯牛顿、列文伯格-马夸尔特等优化算法。

因此，工程上普遍更偏向使用扰动模型。

4.9.5 SE(3) 上的左扰动模型

设当前变换为：

T = exp (ξ^∧)

给它一个小扰动：

ΔT = exp (δξ^∧)

其中：

δξ = [δρ, δϕ]^T

则对齐次点 Tp 关于扰动求导，可得到：

书中把这个结果记作一个特殊算符：

(Tp)^⊙

它会把齐次坐标点变成一个 4×6 的矩阵。

4.9.6 几何解释

这个雅可比矩阵表达的是：

• 平移扰动对点坐标的影响是直接的；
• 旋转扰动的影响与当前位置 Rp+t 有关；
• 旋转影响点的位置方式，本质上表现为叉积矩阵。

这也是 SLAM 中大量位姿雅可比推导的基础模板。