2. 坐标系与刚体运动基础：世界坐标系、机器人坐标系、旋转矩阵、四元数、欧拉角
做轨迹评估，说白了就是回答一个问题：机器人到底走没走对？
要回答这个问题，你首先得有个「尺子」和「参照物」。这个尺子就是坐标系，而机器人怎么动，就是刚体运动。我刚开始接触SLAM时，觉得坐标系这东西太简单了，结果第一次做轨迹对齐就栽了跟头——两个坐标系没搞对，误差曲线画出来跟心电图似的。嗯，从那以后我再也不敢小看这一章了。
2.1 世界坐标系与机器人坐标系
世界坐标系，你可以理解为「上帝视角」。它固定在地面上，通常用 W 表示。机器人坐标系，也叫本体坐标系，跟着机器人走，通常用 B 或 R 表示。
举个例子：
- 世界坐标系：原点在房间角落，X轴朝东，Y轴朝北，Z轴朝天。
- 机器人坐标系：原点在机器人中心，X轴朝前，Y轴朝左，Z轴朝天。
轨迹评估时，我们拿到的往往是机器人坐标系下的位姿估计，但真值通常在世界坐标系下。所以，坐标变换是绕不开的一步。
pW = RWB · pB + tWB2.2 旋转矩阵
旋转矩阵，是描述刚体旋转最直观的方式。它是一个 3×3 的正交矩阵，行列式为 +1。
我个人习惯用旋转矩阵做中间转换，因为它数学性质好，组合方便。但缺点也很明显——9个参数，冗余，而且容易受数值误差影响变成非正交矩阵。
绕 X、Y、Z 轴旋转 θ 角的基本旋转矩阵：
Rx(θ) = [[1, 0, 0],
[0, cosθ, -sinθ],
[0, sinθ, cosθ]]
Ry(θ) = [[cosθ, 0, sinθ],
[0, 1, 0],
[-sinθ, 0, cosθ]]
Rz(θ) = [[cosθ, -sinθ, 0],
[sinθ, cosθ, 0],
[0, 0, 1]]2.3 四元数
四元数，很多人觉得它抽象。说白了，它就是「旋转轴 + 旋转角度」的一种紧凑表示。
一个四元数 q = [w, x, y, z]，其中 w 是实部，(x, y, z) 是虚部。它满足 w² + x² + y² + z² = 1。
为什么用四元数？三个理由：
- 无万向锁——欧拉角的死穴，四元数完美避开
- 插值平滑——做轨迹平滑时，球面线性插值（SLERP）非常自然
- 紧凑——4个参数，比旋转矩阵少5个
我记得有一次做无人机轨迹评估，用欧拉角做插值，结果在俯仰90°时姿态直接飞了。换成四元数，问题瞬间解决。
2.4 欧拉角
欧拉角，最直观的旋转表示。三个角度：滚转（Roll）、俯仰（Pitch）、偏航（Yaw）。
但直观归直观，坑也最多：
- 万向锁：当俯仰角接近 ±90° 时，滚转和偏航会耦合，丢失一个自由度
- 顺序依赖：ZYX、XYZ、ZYZ……不同顺序结果完全不同
- 多值性：同一个旋转对应多组欧拉角
我个人只在可视化或人机交互时用欧拉角，内部计算一律用四元数或旋转矩阵。
| 表示方式 | 参数数量 | 优点 | 缺点 |
|---|---|---|---|
| 旋转矩阵 | 9 | 数学性质好，组合方便 | 冗余，需正交化 |
| 四元数 | 4 | 无万向锁，插值平滑 | 不够直观 |
| 欧拉角 | 3 | 直观，易理解 | 万向锁，顺序依赖 |
2.5 知识体系总览
下面这张图，是我自己总结的坐标系与刚体运动知识结构。你想想看，整个轨迹评估的底层逻辑，其实就围绕这几个概念转。
2.6 避坑指南
- 四元数顺序搞反：Eigen 和 ROS 的 q.w 位置不同，一个在前一个在后
- 欧拉角顺序没确认：对方用 ZYX，我用 XYZ，轨迹直接镜像
- 旋转矩阵忘记正交化：迭代多次后矩阵变形，导致尺度错误
- 内部计算统一用四元数，只在输入输出时转欧拉角
- 每次做坐标变换前，先打印一个已知点验证
- 写单元测试，用恒等变换（旋转0°）做基准测试
好了，坐标系和刚体运动的基础就这些。别看概念简单，实际项目中80%的轨迹评估问题，根源都在这里。搞懂了这一章，后面的误差指标计算才能站得住脚。