📚 课程目录第3章刚体运动与位姿变换

3. 刚体运动与位姿变换

在SLAM前端里程计中，我们每时每刻都在回答同一个问题：机器人从上一时刻到当前时刻，究竟移动了多少、转了多少？这个“移动+旋转”的组合，就是刚体运动。激光雷达扫描匹配的输出，本质上就是一个位姿变换；而位姿变换如何表示、如何求导、如何优化，直接决定了里程计的精度与稳定性。本节把刚体运动的几种主流表示方式串起来，建立从直观到代数、从二维到三维的统一认识。

一、核心原理：刚体运动是“旋转+平移”的复合

刚体意味着物体内部任意两点距离不变，因此运动只包含旋转和平移，不包含缩放和形变。在二维平面中，位姿有3个自由度：x、y、偏航角θ，对应SE(2)；在三维空间中，位姿有6个自由度：3个平移+3个旋转，对应SE(3)。SLAM中常见的激光雷达是2D或3D，因此SE(2)与SE(3)是最常用的两个群。

一个位姿变换可以写成齐次形式：

T = [ R  t ]
    [ 0  1 ]

其中 R 为旋转矩阵，t 为平移向量。
对点 p 做变换：p' = R * p + t
齐次形式：p'_h = T * p_h

旋转矩阵R满足正交性且行列式为1，即R^TR=I，det(R)=1。它没有奇异性，但冗余参数多：三维旋转只有3个自由度，却用9个数表示，且必须满足6个约束。这给优化带来不便，于是有了欧拉角、四元数、李代数等不同表示。

二、四种表示方式的对比与选择

不同表示方式各有取舍，理解它们的差异是写好位姿优化代码的前提。

表示方式	参数个数	优点	缺点	典型用途
旋转矩阵 R	9	无奇异，复合直接相乘	冗余，需正交约束	变换计算、坐标转换
欧拉角 (roll,pitch,yaw)	3	直观，易读	万向锁，依赖旋转顺序	人机界面、姿态显示
四元数 q	4	紧凑，无万向锁，插值平滑	有单位约束，物理意义不直观	姿态估计、SLAM后端
李代数 so(3)/se(3)	3/6	无约束，适合求导与优化	需指数/对数映射	非线性优化、位姿图

在扫描匹配中，如果只是做坐标变换，用旋转矩阵最方便；如果要优化位姿，通常用李代数作为优化变量，再通过指数映射转回旋转矩阵。

三、李群与李代数：为什么优化离不开它

旋转矩阵构成特殊正交群SO(3)，位姿变换构成特殊欧氏群SE(3)。它们是李群——既是群，又是光滑流形。问题在于：旋转矩阵不能直接做加法，R+ΔR不再正交。于是我们引入李代数so(3)和se(3)，它们是向量空间，可以自由加减，再通过指数映射exp回到李群。

so(3) 元素：φ = θ * n   （旋转向量，方向为轴，模长为角）
指数映射：R = exp(φ^) = I + sinθ/θ * φ^ + (1-cosθ)/θ * φ^2
对数映射：φ = log(R)

se(3) 元素：ξ = [ρ; φ]  （ρ为平移部分，φ为旋转部分）
指数映射：T = exp(ξ^)
其中 ξ^ 为4x4矩阵：
ξ^ = [ φ^  ρ ]
     [ 0   0 ]

在优化中，我们通常对位姿T左乘一个小扰动exp(δξ^)，然后对δξ求导。这样求导结果干净，且更新后的T仍然在SE(3)上，不会破坏约束。这就是李代数在SLAM中不可替代的原因。

四、案例：2D激光里程计中的位姿更新

假设2D激光雷达在t时刻的位姿为T_t∈SE(2)，扫描匹配得到相对变换ΔT。更新公式为：

T_{t+1} = T_t * ΔT

其中 T = [ cosθ  -sinθ  x ]
        [ sinθ   cosθ  y ]
        [ 0      0     1 ]

若用李代数优化，则：
T_{t+1} = T_t * exp(δξ^)
δξ = [δx, δy, δθ]^T

在真实场景中，比如一个差速轮机器人沿走廊行驶，扫描匹配会输出一个小的ΔT。如果直接用欧拉角累加，长时间运行后角度会因万向锁或归一化问题产生漂移；而用四元数或李代数维护位姿，则能保持数值稳定。这也是为什么主流SLAM框架（如Cartographer、LOAM）内部都用四元数或李代数管理位姿。

常见误区：把欧拉角直接用于迭代优化。欧拉角在pitch=±90°时出现万向锁，导致一个自由度丢失，优化会不稳定。另一个误区是四元数忘记归一化，长时间累乘后模长偏离1，旋转矩阵会失真。

五、复盘：表示方式的选择清单

坐标变换计算：优先用旋转矩阵或齐次变换矩阵，乘法直接、无歧义。
姿态显示与调试：转成欧拉角输出，方便人看，但不要用于内部累乘。
姿态插值：用四元数球面线性插值slerp，避免欧拉角插值路径异常。
非线性优化：用李代数作为优化变量，通过exp/log在群与代数间转换。
2D场景：SE(2)足够，角度用θ表示，注意归一化到[-π,π]。
3D场景：SE(3)配合四元数或so(3)李代数，注意旋转顺序与坐标系约定。

下面这张图把刚体运动的表示体系、转换关系与在SLAM中的使用位置串在一起，帮助建立整体印象。

总结来说，旋转矩阵适合计算，欧拉角适合展示，四元数适合插值与存储，李代数适合优化。SE(2)与SE(3)是SLAM中位姿的“容器”，而李群李代数则是让这个容器在优化中保持合法的数学工具。理解它们之间的转换关系，是读懂扫描匹配与位姿图优化的基础。