4. 刚体变换与位姿表示
在激光SLAM前端配准中，我们反复要做的一件事，是把同一束激光在不同时刻、不同位姿下测到的点，统一到同一个坐标系里比较。这个“统一”的数学本质就是刚体变换。若位姿表示选得不合适，配准会变慢、插值会失真、优化会震荡；若表示得当，整个前端会稳定得多。本节把旋转矩阵、欧拉角、四元数、李群李代数、位姿插值与坐标系变换串成一条工程可用的知识链。
一、刚体变换的数学结构
三维刚体变换由旋转与平移组成，通常写成齐次形式：
T = [ R t ] R ∈ SO(3), t ∈ R³
[ 0 1 ] T ∈ SE(3)
p_world = R * p_body + t
p_world_h = T * p_body_h其中 R 是 3×3 正交矩阵，满足 RᵀR=I 且 det(R)=1；t 是三维平移向量。SO(3) 表示三维旋转群，SE(3) 表示三维刚体变换群。激光SLAM中，雷达外参、帧间位姿、地图到基座的变换，都是 SE(3) 中的元素。
二、四种旋转表示的对比
| 表示 | 参数个数 | 优点 | 缺点 | 典型用途 |
|---|---|---|---|---|
| 旋转矩阵 R | 9（约束6） | 复合直接、无奇异 | 冗余、需正交化 | 坐标变换、外参存储 |
| 欧拉角 | 3 | 直观、易读 | 万向锁、依赖顺序 | 人机界面、标定初值 |
| 四元数 q | 4（约束1） | 紧凑、插值平滑 | 双覆盖、需归一化 | 位姿插值、IMU融合 |
| 李代数 ξ | 6 | 无约束、适合优化 | 需指数/对数映射 | 位姿图优化、BA |
欧拉角必须说明旋转顺序，例如 ZYX 表示先绕 Z 转 yaw，再绕 Y 转 pitch，最后绕 X 转 roll。当 pitch 接近 ±90° 时，yaw 与 roll 轴重合，出现万向锁，此时欧拉角不再适合做连续插值。
三、李群与李代数：优化的语言
SO(3) 和 SE(3) 是光滑流形，不能像普通向量一样直接相加。李代数 so(3)、se(3) 是流形在单位元处的切空间，可以看作普通向量空间。通过指数映射 exp 和对数映射 log，可以在李代数与李群之间往返：
so(3): φ = θ * n R = exp(φ^) = I + sinθ n^ + (1-cosθ) n^²
se(3): ξ = [ρ, φ] T = exp(ξ^)
更新: T_new = T_old * exp(δξ^)在激光SLAM前端，我们通常把待优化的位姿写成 T = exp(ξ^)，然后对 ξ 求导。这样每次迭代只需在 6 维向量上做加法，再通过 exp 映射回 SE(3)，既保持约束又便于使用高斯牛顿或 LM 算法。ICP、NDT、GICP 的雅可比推导，几乎都建立在这套语言上。
四、位姿插值与坐标系变换
激光雷达一帧不是瞬时曝光，而是持续约 100 ms 的扫描过程。若机器人在这段时间内运动，每个激光点对应的雷达位姿都不同。前端必须用位姿插值把每个点还原到统一时刻，否则墙壁会弯曲、地面会倾斜。常用做法是：
- 从里程计或 IMU 获取帧起始与结束时刻的位姿 T_start、T_end；
- 对每个点的时间戳 t 计算比例 s = (t - t_start) / (t_end - t_start)；
- 平移用线性插值，旋转用四元数 slerp 或李代数插值；
- 得到 T(t)，将点变换到帧起始坐标系或世界坐标系。
// 四元数 slerp 插值示例
Eigen::Quaterniond q0, q1, qt;
double s = (t - t0) / (t1 - t0);
qt = q0.slerp(s, q1);
// 李代数插值
Sophus::SE3d T0, T1, Tt;
Sophus::SE3d delta = T0.inverse() * T1;
Tt = T0 * Sophus::SE3d::exp(s * delta.log());坐标系变换则要严格区分“点变换”和“坐标系变换”。若 T_ab 表示把 a 坐标系下的点变换到 b 坐标系，则 p_b = T_ab · p_a。链式法则为 T_ac = T_bc · T_ab。工程中最常见的错误是把外参方向写反，导致建图整体旋转 180° 或点云飞到地图外。
1. 欧拉角顺序不统一：标定用 ZYX，代码里按 XYZ 解析，结果姿态完全错误。
2. 四元数未归一化：长时间递推后模长漂移，旋转矩阵不再正交。
3. 插值用线性插值旋转矩阵：中间结果不满足正交约束，点云被拉伸。
4. 外参方向混淆：T_base_lidar 与 T_lidar_base 互为逆，写反后配准永远不收敛。
五、工程复盘与选型建议
在激光SLAM前端，推荐遵循以下实践：存储和传输外参用旋转矩阵加平移向量，避免歧义；优化内部状态用李代数，保证无约束更新；位姿插值和 IMU 融合用四元数，保证平滑；只在人机界面和标定初值处使用欧拉角。每次修改外参或位姿表示后，务必用一个小场景做闭环验证：让机器人原地旋转 360°，观察地图是否闭合、地面是否水平、墙角是否笔直。若出现弯曲或错层，优先检查插值时刻、外参方向和四元数归一化，而不是急于调 ICP 参数。
把刚体变换的表示选对、把坐标系链写清、把插值做准，前端配准的很多“玄学问题”都会变成可定位、可复现的工程问题。