4、坐标系与变换：刚体变换、旋转矩阵、四元数、欧拉角、齐次坐标与李群李代数基础

激光雷达-相机-IMU 离线标定的本质，是求解多个传感器坐标系之间的空间变换关系。无论是将激光雷达点云投影到图像平面，还是将 IMU 的角速度/加速度积分结果与视觉/激光里程计对齐，都离不开对三维刚体运动的数学描述。本章将系统梳理刚体变换的四种主流表达方式（旋转矩阵、欧拉角、四元数、齐次坐标），并引入李群 SO(3)/SE(3) 与李代数 so(3)/se(3) 的核心概念，为后续章节中的外参优化、状态估计和因子图建模打下坚实的数学基础。

坐标系与变换数学框架总览 刚体变换 T ∈ SE(3) R ∈ SO(3) + t ∈ ℝ³ 旋转矩阵 R 3×3 正交阵，det=+1 9 个元素，6 个约束 欧拉角 (α,β,γ) ZYX / 内旋 / 外旋 直观但存在万向锁 四元数 q q = (w, x, y, z) 单位四元数，无奇异性 齐次坐标 T 4×4 矩阵 [R | t; 0 1] 李群 ↔ 李代数 SO(3) ↔ so(3)（旋转向量 φ） | SE(3) ↔ se(3)（ξ） 指数映射 exp(φ^) = R 从李代数到李群（SO(3)） 对数映射 log(R) = φ^ 从李群到李代数（旋转向量） 应用：外参标定优化 | 点云配准 | 位姿插值 | 扰动模型求导 | 状态估计

上图展示了本章的知识框架：刚体变换 T 是核心，旋转矩阵、欧拉角、四元数、齐次坐标是四种等价表达，而李群/李代数则提供了在流形上进行优化和求导的数学工具。下面我们逐一深入展开。

4.1 核心原理：刚体变换的数学本质

刚体变换描述的是三维空间中物体从一个位姿到另一个位姿的运动，它包含旋转和平移两部分。在标定场景中，我们通常需要求解激光雷达坐标系 {L}、相机坐标系 {C}、IMU 坐标系 {I} 以及世界坐标系 {W} 之间的变换关系。例如，将激光雷达坐标系下的点 p_L 变换到相机坐标系下，需要执行：p_C = R_CL * p_L + t_CL，其中 R_CL 是旋转矩阵，t_CL 是平移向量。这种变换在数学上构成欧几里得群 SE(3)。

旋转矩阵 R 是 SO(3) 群的元素，满足 R^T R = I 且 det(R) = +1。虽然旋转矩阵有 9 个元素，但实际只有 3 个自由度。这种冗余表达在数值计算中具有良好的稳定性，但在优化时需要施加正交约束，增加了问题复杂度。因此，在实际标定代码中，我们通常使用四元数或旋转向量作为优化变量，而旋转矩阵仅用于最终的计算和坐标变换。

刚体变换的复合遵循矩阵乘法规则。假设有变换 T_WC（从相机到世界）和 T_CL（从激光雷达到相机），则从激光雷达到世界的变换为 T_WL = T_WC * T_CL。注意矩阵乘法的顺序至关重要，它反映了坐标系变换的链式法则。在标定中，我们经常构建这样的位姿图，通过最小化重投影误差或点云配准误差来优化这些变换矩阵。

4.2 四种旋转表达方式详解与对比

理解不同旋转表达方式的优劣，是选择优化变量的前提。下面我们分别介绍旋转矩阵、欧拉角、四元数和齐次坐标，并给出它们之间的转换关系。

4.2.1 旋转矩阵与欧拉角

欧拉角将旋转分解为绕三个坐标轴的依次旋转，常见的组合有 ZYX（先绕 Z 轴，再绕 Y 轴，最后绕 X 轴）和 ZYZ。欧拉角的优点是直观，三个角度分别对应横滚角（roll）、俯仰角（pitch）和偏航角（yaw）。然而，欧拉角存在著名的“万向锁”问题：当中间旋转轴旋转到与第一个旋转轴重合时，系统会丢失一个自由度，导致旋转无法唯一表示。此外，欧拉角的插值不平滑，且不同旋转顺序定义不同，容易引起歧义。因此，在标定算法内部，我们几乎不使用欧拉角作为优化变量，仅在可视化调试或人机交互界面中用于显示姿态。

4.2.2 四元数

四元数是一种超复数，由一个实部和三个虚部组成：q = w + xi + yj + zk，其中 w² + x² + y² + z² = 1。单位四元数可以表示三维空间中的任意旋转，且不存在奇异性。四元数乘法对应旋转的复合，其运算效率高于矩阵乘法。更重要的是，四元数在插值（如 SLERP 球面线性插值）和优化中表现优异，因为它是一个流形上的元素，可以通过李代数进行无约束优化。在 ROS 和多数 SLAM 系统中，四元数是位姿的标准存储格式。

4.2.3 齐次坐标与变换矩阵

齐次坐标将旋转和平移统一到一个 4×4 矩阵中：T = [R t; 0 1]。使用齐次坐标后，点的变换可以写成线性乘法：p' = T * p，其中 p = (x, y, z, 1)^T。这种表达方式使得多个变换的复合变得非常简洁，也便于处理投影几何中的无穷远点。在标定中，我们通常用齐次矩阵来存储外参，并利用其逆矩阵 T^{-1} = [R^T | -R^T t; 0 1] 进行反向变换。

表达方式自由度优点缺点典型应用
旋转矩阵 R3（9 个元素）数值稳定，直接用于坐标变换冗余，需满足正交约束点云变换、矩阵复合
欧拉角3直观，便于人机交互万向锁，插值不平滑姿态显示、遥控指令
四元数 q3（4 个元素）无奇异性，插值平滑，紧凑不够直观，需注意单位化优化变量、状态估计、消息传递
齐次矩阵 T6统一旋转和平移，复合方便16 个元素，存储稍大外参存储、坐标变换链
经验提醒：在编写标定代码时，建议内部统一使用四元数+平移向量（即 q, t）作为优化变量，仅在需要计算点云投影或矩阵复合时，才将其转换为齐次矩阵 T。这样可以避免欧拉角的歧义，同时利用四元数的无约束特性简化优化过程。

4.3 李群与李代数：优化与求导的基石

在标定过程中，我们通常需要构建代价函数（如重投影误差）并对其求导，以迭代优化外参。然而，旋转矩阵和四元数所在的 SO(3) 群不是欧几里得空间，直接对其加法求导会破坏流形约束。李群与李代数为我们提供了一种优雅的解决方案：将流形上的优化问题转化为切空间上的普通向量优化。

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

SO(3) 对应的李代数 so(3) 是三维向量 φ ∈ ℝ³，其反对称矩阵 φ^ 满足 φ^ = [0, -φ3, φ2; φ3, 0, -φ1; -φ2, φ1, 0]。指数映射 exp(φ^) 将旋转向量转换为旋转矩阵，这恰好对应罗德里格斯公式。类似地，SE(3) 的李代数 se(3) 是六维向量 ξ = (ρ, φ)，其中 ρ 是平移部分，φ 是旋转部分。通过指数映射，我们可以将六维向量映射为 4×4 的变换矩阵。

4.3.2 扰动模型与雅可比矩阵

在标定优化中，我们常用左乘扰动模型来求导。例如，对于旋转矩阵 R，其微小扰动可以表示为 R ← exp(δφ^) * R，其中 δφ 是切空间中的小量。这样，代价函数对 δφ 的导数就是雅可比矩阵，且该雅可比矩阵是普通的三维向量导数，可以直接用于高斯牛顿法或 LM 算法。这种处理方式避免了在流形上进行复杂的约束优化，是 Ceres、G2O 等优化库的标准做法。

// 以 Ceres 为例，定义四元数扰动模型
// 优化变量：四元数 q（4 参数）和平移 t（3 参数）
// 在残差块中，使用 exp(delta_phi) * q 来施加扰动
bool operator()(const T* const q, const T* const t, T* residual) const {
    // 将 q 转换为旋转矩阵 R
    Eigen::Quaternion<T> quat(q[3], q[0], q[1], q[2]);
    Eigen::Matrix<T,3,3> R = quat.toRotationMatrix();
    // 计算重投影误差
    Eigen::Matrix<T,3,1> p_cam = R * p_lidar + Eigen::Matrix<T,3,1>(t[0], t[1], t[2]);
    // ... 投影到像素平面并计算残差
    return true;
}
核心理解：李代数求导的关键在于“扰动”是加在切空间上的，而不是直接加在旋转矩阵或四元数上。这保证了优化过程中旋转矩阵始终满足正交约束，四元数始终满足单位模长约束。

4.4 实战场景：外参标定中的变换链构建

在激光雷达-相机-IMU 联合标定中，我们通常需要构建一个包含所有传感器位姿的因子图。假设我们有一个标定板，其在世界坐标系下的位姿为 T_WB。激光雷达观测到标定板得到 T_LB，相机观测到标定板得到 T_CB，IMU 测量得到旋转增量 ΔR_I。我们的目标是求解外参 T_CL（激光雷达到相机）和 T_IC（相机到 IMU）。

通过闭环约束，我们可以建立如下方程：T_CB = T_CL * T_LB 和 T_IB = T_IC * T_CB。将多个时刻的观测数据堆叠起来，构建最小二乘问题。在优化过程中，我们使用四元数表示旋转，并使用李代数扰动模型计算雅可比矩阵。具体步骤如下：

  1. 数据采集：同时采集激光雷达点云、相机图像和 IMU 数据，确保时间同步。
  2. 特征提取：在点云中提取标定板的角点或平面，在图像中提取棋盘格角点。
  3. 位姿估计：分别求解 T_LB 和 T_CB（如使用 PnP 算法）。
  4. 构建图优化：将外参 T_CL 和 T_IC 设为待优化变量，添加上述约束。
  5. 迭代优化：使用 LM 算法，结合李代数扰动模型，迭代更新外参直到收敛。
常见误区：初学者容易混淆旋转矩阵的乘法顺序。例如，T_CL 表示“从 L 系到 C 系的变换”，那么点 p_L 变换到 C 系应为 p_C = T_CL * p_L。在代码中，如果使用 Eigen 库，应确保矩阵乘法顺序与数学表达式一致，避免因列主序/行主序存储导致错误。

4.5 常见误区与总结复盘

本章内容虽然偏重数学基础，但直接关系到标定算法的稳定性和精度。以下是工程师在实际开发中容易踩的坑：

  • 四元数未归一化：在优化过程中，如果四元数模长偏离 1，会导致旋转矩阵非正交，进而产生尺度畸变。每次更新后应重新归一化，或使用 Ceres 的 QuaternionParameterization。
  • 欧拉角顺序混淆：不同厂商的 SDK 可能使用不同的欧拉角定义（如 ZYX vs YXZ），直接使用会导致外参完全错误。建议统一转换为四元数或旋转矩阵进行内部计算。
  • 忽略李代数雅可比：如果手动实现优化，直接对旋转矩阵的 9 个元素求导，会导致优化变量过参数化，且难以施加正交约束。应使用扰动模型，将雅可比矩阵限制在 3 维切空间内。
  • 变换链方向错误：在构建 T_WL = T_WC * T_CL 时，务必确认矩阵乘法的左右顺序。建议在代码中注释清楚每个变量的坐标系来源和目标坐标系。
← 上一章 📖 返回目录 下一章 →