3. 刚体运动与姿态表示：旋转矩阵、欧拉角、四元数、李群与李代数基础

各位同学，欢迎来到第三章。这一章，我们聊聊刚体运动。

做SLAM，说白了就是估计相机在空间里怎么动。相机是个刚体，它不会像橡皮泥一样变形。所以，描述它的位置和朝向，就是刚体运动的核心问题。

我个人习惯把姿态表示分成两类：一类是给人看的，比如欧拉角；另一类是给机器算的，比如旋转矩阵和四元数。至于李群和李代数，那是给优化器用的。咱们一个一个来。

3.1 旋转矩阵：最直观的数学表达

旋转矩阵，就是一个3x3的矩阵。它描述了一个坐标系相对于另一个坐标系的旋转关系。

你想想看，一个点在空间里，坐标是(x,y,z)。如果坐标系旋转了，这个点的坐标会怎么变？旋转矩阵就是干这个的。

核心性质：

旋转矩阵是正交矩阵：R^TR = I
行列式为+1：det(R) = 1
逆矩阵等于转置：R^-1 = R^T

我在项目中遇到过一个问题：两个旋转矩阵相乘，顺序搞反了。结果相机轨迹完全乱套。记住，矩阵乘法不满足交换律。R1 * R2 和 R2 * R1 是两码事。

3.2 欧拉角：直观但容易翻车

欧拉角用三个角度表示旋转：偏航(Yaw)、俯仰(Pitch)、滚转(Roll)。

听起来很直观对吧？但这里有个大坑——万向锁(Gimbal Lock)。

避坑指南：

我曾经在调试一个无人机姿态估计时，发现俯仰角接近90度时，偏航和滚转突然耦合了。这就是万向锁。欧拉角在SLAM里，我建议只用来做可视化，别用在核心计算里。

另外，欧拉角的定义顺序也很重要。ZYX、ZYZ、XYZ……不同顺序结果完全不同。你想想看，如果团队里有人用ZYX，有人用XYZ，那对接起来就是灾难。

3.3 四元数：SLAM的标配

四元数，说白了就是一个四维向量：q = w + xi + yj + zk。它没有万向锁问题，而且插值平滑。

在SLAM里，我几乎只用四元数做姿态优化。为什么？因为它只需要4个参数，而旋转矩阵要9个。优化时，约束条件也少很多。

我的习惯：

单位四元数表示旋转。如果四元数的模不是1，先归一化再用。我在一次轨迹评估中，因为忘了归一化，结果误差曲线全是毛刺。嗯，从那以后我写了个自动检查函数。

四元数乘法也不简单。两个四元数相乘，对应旋转的复合。顺序和矩阵一样，不能乱。

// 四元数乘法示例
// q1 * q2 表示先做q2旋转，再做q1旋转
Quaternion multiply(Quaternion q1, Quaternion q2) {
    return Quaternion(
        q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z,
        q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y,
        q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x,
        q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w
    );
}

3.4 李群与李代数：优化的利器

这部分，很多初学者觉得抽象。我换个说法：李群是旋转矩阵的集合，李代数是旋转向量的集合。

为什么需要李代数？因为旋转矩阵有约束（正交且行列式为1），没法直接做加法优化。但李代数没有约束，可以随便加减。

你想想看，在BA（Bundle Adjustment）里，我们要优化相机位姿。如果用旋转矩阵，每次迭代都要保证矩阵是正交的，很麻烦。用李代数，直接加一个增量就行，然后通过指数映射转回旋转矩阵。

核心关系：

SO(3)：三维旋转群（李群）
so(3)：三维旋转的李代数
指数映射：so(3) → SO(3)
对数映射：SO(3) → so(3)

我在做视觉惯性里程计(VIO)时，就用李代数做位姿优化。每次迭代，计算误差的雅可比矩阵，然后更新李代数。嗯，这里要注意，雅可比矩阵的形式和扰动模型有关。我建议用右扰动模型，代码实现起来更顺手。

3.5 知识体系总览

下面这张图，是我自己总结的。它把本章的知识点串起来了。

3.6 实际应用中的选择建议

场景	推荐表示	原因
可视化调试	欧拉角	直观，容易理解
状态估计/滤波	四元数	无万向锁，插值平滑
非线性优化	李代数	无约束，便于求导
坐标变换	旋转矩阵	直接相乘，效率高

个人经验：

我在做轨迹评估时，内部计算全用四元数和李代数。只在最后输出结果时，转成欧拉角给人看。这样既保证了精度，又方便了沟通。

好了，这一章的内容就到这里。记住，姿态表示没有绝对的好坏，关键看你在什么场景下用。多动手写代码，多跑几个数据集，你自然就找到感觉了。