📚 课程目录第3章三维空间刚体运动

3. 三维空间刚体运动

各位同学，欢迎来到多传感器融合定位的第一道硬菜。

说实话，刚接触SLAM的人，十有八九会被李群李代数劝退。我当年也一样，看着SO(3)、se(3)这些符号，脑子里全是问号。但后来在实际项目中吃过亏——直接用旋转矩阵做优化，结果矩阵不满足正交约束，整个系统直接崩了。从那以后，我老老实实把这块啃了下来。

今天我们就把它讲透。你不需要成为数学家，只需要理解：李群和李代数，是解决旋转求导问题的利器。

3.1 为什么需要李群与李代数？

先问一个问题：在SLAM里，我们要优化什么？

答案是位姿。位姿由旋转+平移组成。旋转可以用旋转矩阵R表示，平移用向量t。但问题来了——

旋转矩阵R是正交矩阵，有9个参数，但自由度只有3。直接优化R，你很难保证优化后的结果还是正交矩阵。
求导困难。旋转矩阵对时间的导数，不是简单的线性关系。

李群和李代数，就是来解决这两个问题的。它把旋转矩阵映射到一个无约束的向量空间，在这个空间里做优化，再映射回旋转矩阵。

核心思想：在流形（李群）上做优化太麻烦，我们到切空间（李代数）里做，做完再映射回去。

3.2 李群 SO(3) 与 SE(3)

先看定义。

SO(3)：特殊正交群。就是所有3x3旋转矩阵的集合。

SO(3) = { R ∈ ℝ³ˣ³ | RᵀR = I, det(R) = 1 }

SE(3)：特殊欧氏群。就是所有4x4变换矩阵的集合。

SE(3) = { T = [R t; 0 1] ∈ ℝ⁴ˣ⁴ | R ∈ SO(3), t ∈ ℝ³ }

这两个都是群。群是什么？简单说就是：集合 + 一种运算（乘法），满足封闭性、结合律、有单位元、有逆元。

旋转矩阵乘法满足封闭性——两个旋转矩阵相乘，结果还是旋转矩阵。这就是群。

我的经验：刚开始不用深究群论的公理。你只需要记住：SO(3)里的元素是旋转矩阵，SE(3)里的元素是变换矩阵。它们可以相乘，可以求逆。

3.3 李代数 so(3) 与 se(3)

李代数，是李群在单位元处的切空间。

听起来抽象？换个说法：李代数就是旋转的“速度”空间。

so(3)：对应SO(3)的李代数。元素是3x3的反对称矩阵。

so(3) = { Φ = φ^ ∈ ℝ³ˣ³ | φ ∈ ℝ³ }

其中φ是一个三维向量，φ^表示它的反对称矩阵：

φ = [φ₁, φ₂, φ₃]ᵀ

φ^ = [0   -φ₃  φ₂;
      φ₃   0  -φ₁;
     -φ₂  φ₁   0 ]

se(3)：对应SE(3)的李代数。元素是4x4矩阵。

se(3) = { ξ^ = [φ^  ρ; 0 0] ∈ ℝ⁴ˣ⁴ | ρ, φ ∈ ℝ³ }

这里ξ = [ρ, φ]ᵀ是一个六维向量。ρ是平移部分，φ是旋转部分。

记住这个对应关系：

李群	李代数	维度
SO(3)	so(3)	3
SE(3)	se(3)	6

3.4 指数映射与对数映射

这是最核心的部分。李群和李代数之间，通过指数映射和对数映射相互转换。

3.4.1 SO(3) 上的指数映射

从so(3)到SO(3)：指数映射。

给定一个旋转向量φ（就是so(3)里的元素），指数映射得到旋转矩阵R：

R = exp(φ^)

这个公式怎么算？用罗德里格斯公式：

R = I + sinθ · n^ + (1 - cosθ) · n^²

其中θ = ||φ||，n = φ/θ。

反过来，从SO(3)到so(3)：对数映射。

φ = ln(R)^∨

给定旋转矩阵R，可以求出旋转向量φ。θ = arccos((tr(R)-1)/2)，n = (R - Rᵀ)∨ / (2sinθ)。

注意：当θ接近0时，公式有数值不稳定性。我曾在IMU初始化时踩过这个坑——旋转角度极小，arccos直接算出来NaN。解决方案是用泰勒展开近似。

3.4.2 SE(3) 上的指数映射

从se(3)到SE(3)：

T = exp(ξ^) = [R  t; 0  1]

其中：

R = exp(φ^)
t = Jρ

J是左雅可比矩阵：

J = sinθ/θ · I + (1 - sinθ/θ) · nnᵀ + (1 - cosθ)/θ · n^

反过来，从SE(3)到se(3)：

φ = ln(R)^∨
ρ = J⁻¹t

我的建议：不要死记硬背这些公式。实际开发中，我们直接用Eigen或Sophus库。但你要理解背后的几何意义——指数映射把“旋转速度”变成“旋转”，对数映射反过来。

3.5 李代数求导与扰动模型

终于到了最实用的部分。在SLAM中，我们要对位姿求导。比如：

给定一个点p，经过变换T得到p' = Tp
我们要计算p'对T的导数

直接在SE(3)上求导？不行。因为SE(3)不是向量空间，加法不封闭。

但在李代数上可以。李代数是向量空间，可以自由加减求导。

有两种方式：

李代数求导：对R对应的李代数φ求导
扰动模型：在R左边乘一个小扰动ΔR，对扰动求导

实际中，扰动模型更常用。因为它形式简单，不需要计算雅可比矩阵。

∂(Rp)/∂φ = -(Rp)^

这个公式非常简洁。在代码里，就是一行的事。

避坑指南：我曾经在VIO系统中，因为用了错误的求导方式，导致优化不收敛。后来换成扰动模型，问题立刻解决。记住：扰动模型是SLAM中的标准做法。

3.6 代码实战：使用Sophus库

理论说完了，我们看看代码。Sophus是常用的李代数库，基于Eigen。

#include <sophus/so3.hpp>
#include <sophus/se3.hpp>

// 创建SO3
Eigen::Vector3d phi(0.1, 0.2, 0.3);
Sophus::SO3d R = Sophus::SO3d::exp(phi);

// 指数映射
Sophus::SO3d R2 = Sophus::SO3d::exp(phi);

// 对数映射
Eigen::Vector3d phi2 = R2.log();

// 创建SE3
Eigen::Vector3d t(1, 2, 3);
Sophus::SE3d T(R, t);

// 扰动模型求导
Eigen::Vector3d p(0.5, 0.6, 0.7);
Eigen::Vector3d dp_dphi = -Sophus::SO3d::hat(T * p);

这段代码，基本覆盖了日常开发中90%的李代数操作。

个人习惯：我一般在CMakeLists.txt里这样配置Sophus：

find_package(Sophus REQUIRED)
target_link_libraries(my_node Sophus::Sophus)

3.7 本章小结

来，我们捋一遍：

李群：SO(3)是旋转矩阵集合，SE(3)是变换矩阵集合
李代数：so(3)是旋转向量，se(3)是六维向量
指数/对数映射：连接李群和李代数的桥梁
扰动模型：SLAM中求导的标准方法

这些概念，你会在后续的每一章中反复用到。不管是视觉SLAM、IMU预积分，还是激光雷达定位，都离不开它们。

嗯，今天就到这里。下一章我们讲四元数——另一个绕不开的话题。