4. 三维空间刚体运动

在IMU与激光点云融合定位中，所有传感器数据都依附于各自的坐标系，而融合的本质就是不断进行坐标系之间的对齐与变换。三维空间刚体运动描述的是物体在不发生形变的前提下，在空间中的位置与姿态变化。理解旋转矩阵、四元数、欧拉角以及平移变换，是理解后续外参标定、点云配准、惯性导航解算的数学基石。本节将系统梳理这四种表达方式的数学定义、相互转换关系、工程适用场景，并重点剖析坐标系之间转换的完整链路。

三维刚体运动表达与坐标系转换关系图 三维刚体运动 (R, t) 旋转矩阵 R 3×3正交阵，行列式=+1 四元数 q w+xi+yj+zk，单位范数 欧拉角 (α,β,γ) Yaw/Pitch/Roll，直观但有万向锁 平移向量 t 3×1向量，描述位置偏移 坐标系转换链路 IMU坐标系 → 车体坐标系 → 激光雷达坐标系 外参标定：T_IB, T_LB 融合定位：将点云变换到IMU系或世界系 核心公式：p_world = T_WI * T_IB * p_body 表达方式转换 旋转矩阵 ↔ 四元数：q = f(R) 旋转矩阵 ↔ 欧拉角：atan2 求解 四元数 ↔ 欧拉角：需注意万向锁 所有表达最终统一为齐次变换矩阵 T 工程建议：内部计算用四元数，人机交互用欧拉角，存储与传输用旋转矩阵或四元数

上图展示了刚体运动四种表达方式之间的内在联系以及坐标系转换的工程链路。旋转矩阵、四元数、欧拉角本质上都是描述同一旋转的不同数学工具，而平移向量则独立描述位置变化。在融合定位系统中，我们通常使用齐次变换矩阵将旋转和平移统一为一个4×4矩阵，从而方便地进行坐标系级联变换。

4.1 旋转矩阵：数学本质与几何意义

4.1.1 核心原理

旋转矩阵是一个3×3的正交矩阵，满足 RTR = I 且 det(R) = +1。它描述了一个坐标系相对于另一个坐标系的姿态关系。设世界坐标系为{W}，刚体坐标系为{B}，则旋转矩阵 WRB 的每一列表示{B}系的基向量在{W}系中的投影。旋转矩阵的乘法对应旋转的复合，但矩阵乘法不满足交换律，因此旋转顺序至关重要。

关键性质：

  • 正交性：RT = R-1，逆矩阵即为转置，计算高效。
  • 行列式为+1，保证是纯旋转而非镜像变换。
  • 旋转矩阵的9个元素存在6个约束条件（正交性3个+行列式1个），因此实际自由度为3。
  • 连续旋转：Rtotal = R2 · R1，先旋转R1再旋转R2。

4.1.2 判断方法与操作流程

在实际工程中，判断一个矩阵是否为合法旋转矩阵，可通过以下步骤：

  1. 验证矩阵维度是否为3×3。
  2. 计算 RTR，检查结果是否接近单位矩阵（允许微小数值误差）。
  3. 计算行列式，确认是否接近+1。
  4. 若用于坐标变换，确认旋转方向约定（左乘或右乘）。

在C++ Eigen库中，常用 Eigen::Matrix3d 存储旋转矩阵，并通过 R.transpose() 求逆。在IMU数据处理中，旋转矩阵常用于将加速度计和陀螺仪的测量值从机体坐标系变换到导航坐标系。

4.1.3 案例拆解：IMU姿态初始化

假设无人机起飞前静止，加速度计测得重力加速度在机体坐标系下的分量为 ab = [0, 0, -9.8]T m/s²，而导航坐标系下重力为 gn = [0, 0, -9.8]T。此时机体坐标系与导航坐标系重合，旋转矩阵为单位阵。若无人机俯仰角为30°，则加速度计读数变为 ab = [4.9, 0, -8.49]T。通过 ab = Rnb · gn 可以反解出旋转矩阵。实际工程中，我们利用加速度计和磁力计联合求解初始旋转矩阵，作为后续EKF/因子图优化的初始值。

4.1.4 常见误区与总结

误区1：混淆旋转矩阵的乘法顺序。在融合定位中，RWB 表示将{B}系下的向量变换到{W}系，即 pW = RWB · pB。若顺序颠倒，会导致姿态完全错误。

误区2：忽略数值漂移。长时间运行后，旋转矩阵可能因累积误差不再满足正交性，需定期正交化（如SVD分解后重新投影）。

误区3：旋转矩阵不适合插值。两个旋转矩阵的线性插值结果不再是旋转矩阵，应使用四元数的球面线性插值（SLERP）。

4.2 四元数：紧凑且无奇异的旋转表达

4.2.1 核心原理

四元数由英国数学家哈密顿于1843年提出，形式为 q = w + xi + yj + zk，其中 w 为实部，(x, y, z) 为虚部。单位四元数满足 w² + x² + y² + z² = 1，可用于表示三维旋转。四元数相比旋转矩阵的优势在于：存储量小（4个浮点数 vs 9个）、无万向锁问题、插值平滑。旋转操作通过 p' = q ⊗ p ⊗ q* 实现，其中 p 为纯虚四元数（实部为0），q* 为共轭四元数。

四元数的乘法定义为：

q1 ⊗ q2 = (w1*w2 - x1*x2 - y1*y2 - z1*z2) +
           (w1*x2 + x1*w2 + y1*z2 - z1*y2)i +
           (w1*y2 - x1*z2 + y1*w2 + z1*x2)j +
           (w1*z2 + x1*y2 - y1*x2 + z1*w2)k

4.2.2 判断方法与操作流程

在工程中，使用四元数前需进行以下检查：

  • 单位化：计算范数 ||q||，若偏离1则需归一化。
  • 符号一致性：q 和 -q 表示同一旋转，但在插值时需确保点积为正以避免绕远路。
  • 与旋转矩阵的转换：给定旋转矩阵 R，四元数可通过以下公式提取：
w = sqrt(1 + R(0,0) + R(1,1) + R(2,2)) / 2
x = (R(2,1) - R(1,2)) / (4w)
y = (R(0,2) - R(2,0)) / (4w)
z = (R(1,0) - R(0,1)) / (4w)

当 w 接近0时，需改用其他元素计算以避免数值不稳定。

4.2.3 案例拆解：IMU姿态解算中的四元数更新

在IMU惯性导航中，陀螺仪输出角速度 ω = [ωx, ωy, ωz]T，四元数随时间更新公式为：

q_dot = 0.5 * q ⊗ ω_quat

其中 ω_quat = [0, ωx, ωy, ωz]。离散化后采用一阶龙格-库塔法：

q_new = q_old + q_dot * dt
q_new = q_new / ||q_new||  // 归一化

假设陀螺仪以100Hz采样，dt=0.01s，初始四元数为 [1,0,0,0]，角速度为 [0.1, 0.2, 0.3] rad/s，则一步更新后四元数实部变化约为0.5×0.01×0.3=0.0015，虚部相应变化。经过长时间积分，四元数会累积漂移，因此需要融合加速度计和磁力计进行修正（如互补滤波或卡尔曼滤波）。

4.2.4 常见误区与总结

经验提醒：四元数乘法不满足交换律，q1 ⊗ q2 ≠ q2 ⊗ q1。在组合旋转时，务必注意旋转顺序。例如，先绕Z轴旋转再绕Y轴旋转，与先绕Y轴再绕Z轴的结果不同。

误区：将四元数的虚部直接当作旋转轴向量。实际上，旋转轴为 (x, y, z)/sin(θ/2)，旋转角 θ = 2·arccos(w)。

4.3 欧拉角：直观但存在万向锁

4.3.1 核心原理

欧拉角将旋转分解为绕三个正交轴的依次旋转，常见的有ZYX顺序（偏航-俯仰-横滚）和ZYZ顺序。在航空航天领域，通常使用ZYX顺序：先绕Z轴偏航（Yaw），再绕Y轴俯仰（Pitch），最后绕X轴横滚（Roll）。欧拉角的优势是直观，便于人机交互和姿态可视化。但存在著名的万向锁问题：当俯仰角为±90°时，偏航和横滚的旋转轴重合，丢失一个自由度，导致姿态奇异。

4.3.2 判断方法与操作流程

从旋转矩阵提取ZYX欧拉角的公式为：

pitch = asin(-R(2,0))
yaw   = atan2(R(1,0), R(0,0))
roll  = atan2(R(2,1), R(2,2))

当 pitch 接近 ±90° 时，atan2 的参数接近0/0，数值不稳定。工程中应避免在万向锁附近使用欧拉角进行姿态控制或插值。

4.3.3 案例拆解：激光雷达点云与IMU姿态对齐

在融合定位中，IMU提供的姿态通常以四元数输出，但调试时工程师习惯用欧拉角观察。假设IMU输出四元数 q = [0.9239, 0.2209, 0.2209, 0.2209]，转换为ZYX欧拉角：

yaw = atan2(2(w*z + x*y), 1 - 2(y² + z²)) ≈ 30°
pitch = asin(2(w*y - z*x)) ≈ 25°
roll = atan2(2(w*x + y*z), 1 - 2(x² + y²)) ≈ 20°

此时若将激光雷达点云从雷达坐标系变换到IMU坐标系，需要用到旋转矩阵或四元数，而非欧拉角。因为欧拉角在级联变换中容易引入顺序歧义。

4.3.4 常见误区与总结

风险提示：欧拉角的旋转顺序必须明确约定。同一组欧拉角值，在不同旋转顺序下对应完全不同的姿态。在跨团队协作时，务必在接口文档中注明旋转顺序和参考坐标系。

总结：欧拉角适合用于显示和简单调试，不适合用于内部计算和存储。在融合定位系统中，应统一使用四元数或旋转矩阵作为姿态的数学表达。

4.4 平移变换与齐次变换矩阵

4.4.1 核心原理

平移变换描述坐标系原点的位置偏移，用3×1向量 t = [tx, ty, tz]T 表示。将旋转和平移统一为齐次变换矩阵：

T = | R  t |
    | 0  1 |

其中 T 为4×4矩阵。齐次变换矩阵使得坐标变换可以表示为线性乘法：p' = T · p，其中 p 和 p' 为齐次坐标（4维向量，最后一维为1）。连续变换可通过矩阵乘法级联：TW = TW1 · T12 · T2B。

4.4.2 操作流程：坐标系转换的标准步骤

  1. 明确源坐标系和目标坐标系：例如将激光雷达点云从雷达坐标系{L}变换到IMU坐标系{I}。
  2. 获取外参：通过标定得到 TIL（从L到I的变换）。
  3. 坐标变换：pI = TIL · pL。
  4. 若需变换到世界坐标系：再乘以 TWI（IMU在世界系中的位姿）。

在融合定位中，典型的变换链为：点云L → 点云I → 点云W。其中 TWI 由IMU里程计或SLAM系统实时估计。

4.4.3 案例拆解：外参标定与点云投影

假设激光雷达与IMU之间的外参为：旋转矩阵 RIL（绕Z轴旋转90°），平移向量 tIL = [0.1, 0, 0.2]T 米。雷达坐标系下某点 pL = [1, 2, 3]T，则变换到IMU坐标系：

p_I = R_IL * p_L + t_IL
    = | 0 -1 0 |   |1|   |0.1|
      | 1  0 0 | * |2| + |0  |
      | 0  0 1 |   |3|   |0.2|
    = [-2+0.1, 1+0, 3+0.2] = [-1.9, 1, 3.2]

若IMU在世界系中的位姿 TWI 已知，则可继续变换到世界系。该过程在每一帧点云中重复执行，是融合定位的基础操作。

4.4.4 常见误区与总结

关键误区：平移向量 t 是在目标坐标系下表示的，还是源坐标系下表示的？在公式 pI = RIL · pL + tIL 中，tIL 表示{L}系原点在{I}系中的坐标。若方向搞反，会导致厘米级甚至米级误差。

工程建议：在代码中统一使用齐次变换矩阵，避免手动拆分旋转和平移。Eigen库中可使用 Eigen::Isometry3d 或 Eigen::Affine3d 管理变换矩阵。

4.5 坐标系转换关系与融合定位中的应用

4.5.1 核心原理：多坐标系级联

在IMU与激光雷达融合定位系统中，至少涉及四个坐标系：世界坐标系{W}、IMU机体坐标系{I}、激光雷达坐标系{L}、车体/机器人坐标系{B}。各坐标系之间的转换关系如下表所示：

变换含义获取方式更新频率
TWIIMU在世界系中的位姿IMU积分/融合估计100-1000Hz
TIL雷达在IMU系中的外参离线标定固定
TIBIMU在车体系中的外参离线标定固定
TWL雷达在世界系中的位姿TWI · TIL随IMU更新

4.5.2 操作流程：点云去畸变与融合

激光雷达在扫描过程中，车辆可能正在运动，导致一帧点云中的每个点对应不同的传感器位姿。去畸变流程如下：

  1. 根据IMU数据插值出每个点云点对应的时刻 ti 的位姿 TWI(ti)。
  2. 将每个点从雷达坐标系变换到世界坐标系：pW = TWI(ti) · TIL · pL。
  3. 将所有点统一到帧末时刻 tend 的坐标系下，完成去畸变。

该过程需要高频IMU数据支撑，若IMU频率不足，可采用线性插值或B样条插值。

4.5.3 案例拆解：外参误差对融合定位的影响

假设外参标定存在0.5°的旋转误差和1cm的平移误差。在10米距离处，0.5°旋转误差导致点云位置偏差约为 10×tan(0.5°) ≈ 8.7cm。该误差在融合定位中会表现为点云与地图的错位，导致ICP配准收敛到局部极小值。通过以下步骤可诊断外参误差：

  • 静态场景下，采集一帧点云并投影到地图，观察边缘是否对齐。
  • 旋转车辆90°，再次投影，若误差方向随旋转变化，则存在旋转外参误差。
  • 平移误差通常表现为整体偏移，旋转误差表现为距离越远偏差越大。

解决方法是重新进行外参标定，使用如Autoware的标定工具或基于互信息的手动微调。

4.5.4 常见误区与总结

经验总结：坐标系转换是融合定位中最容易出错但也是最基础的部分。建议在代码中封装统一的坐标变换接口，并添加单元测试验证变换矩阵的正确性。例如，验证 TIL · TLI = I。

最佳实践：在ROS中，使用 tf2 库管理坐标系变换，通过 lookupTransform 获取任意两个坐标系之间的变换，避免手动计算。

本节系统梳理了三维刚体运动的四种数学表达及其在IMU与激光点云融合定位中的应用。旋转矩阵适合理论推导和坐标变换，四元数适合姿态更新和插值，欧拉角适合人机交互，平移变换与齐次矩阵则统一了旋转和平移。在实际工程中，应根据场景选择合适的表达方式，并严格遵循坐标系转换的规范流程，确保融合定位的精度与稳定性。

← 上一章 📖 返回目录 下一章 →