坐标系与位姿表示：世界坐标系、机器人坐标系、四元数、旋转矩阵、欧拉角
各位同学，欢迎来到第二章。
上一章我们聊了轨迹评估到底在评估什么。今天要讲的，是评估之前必须搞清楚的底层逻辑——坐标系与位姿表示。
说实话，我见过太多项目，算法跑得飞起，结果评估时发现轨迹漂移了十几米。一查原因，坐标系搞反了。嗯，这种坑，我踩过不止一次。
2.1 世界坐标系与机器人坐标系
先问个问题：你站在操场上，说“前面10米有棵树”。这个“前面”，是以谁为参考？
答案是——以你自己为参考。这就是机器人坐标系（也叫本体坐标系、局部坐标系）。
而如果我说“东经116度，北纬40度，海拔50米”，这就是世界坐标系（全局坐标系）。
在SLAM里，这两个坐标系天天打架。
- 世界坐标系：固定不动，通常以机器人启动位置为原点。所有地图点、轨迹真值都存成世界坐标。
- 机器人坐标系：跟着机器人走。传感器（激光雷达、相机）测得的数据，默认都是机器人坐标系下的。
核心要点：评估轨迹时，必须把估计位姿和真值位姿统一到同一个坐标系下。否则，你算出来的ATE（绝对轨迹误差）就是一堆废纸。
我个人习惯，在代码里用 world_T_robot 这种命名方式。一看就知道是从机器人坐标系到世界坐标系的变换矩阵。别嫌啰嗦，半年后你回头看代码，会感谢自己的。
2.2 旋转矩阵：最直观，但最占内存
旋转矩阵，说白了就是一个3x3的矩阵，描述一个坐标系相对于另一个坐标系的旋转。
// 绕Z轴旋转θ角的旋转矩阵
R_z(θ) = [
[cosθ, -sinθ, 0],
[sinθ, cosθ, 0],
[0, 0, 1]
]优点：直观，容易组合（连乘就行）。
缺点：9个参数，但自由度只有3。冗余！而且必须满足正交性（R^T * R = I）。
我曾经在优化位姿图时，直接拿旋转矩阵的9个元素当优化变量。结果呢？优化出来的矩阵根本不满足正交性，轨迹直接飞了。后来才老老实实用四元数或李代数。
2.3 欧拉角：直观，但小心万向锁
欧拉角用三个角度表示旋转：偏航（Yaw）、俯仰（Pitch）、横滚（Roll）。
你想想看，飞机驾驶员看仪表盘，就是看这三个角。很直观对吧？
但问题来了：万向锁。
当俯仰角接近±90°时，偏航和横滚会变得无法区分。说白了，你丢失了一个自由度。
避坑指南：如果你做的是无人机或自动驾驶，俯仰角通常不会到90°，用欧拉角问题不大。但如果是做机械臂或全景相机，请务必用四元数。
2.4 四元数：SLAM的标配
四元数，听起来玄乎，其实就是四个数：q = [w, x, y, z]，满足 w² + x² + y² + z² = 1。
为什么SLAM里都用它？
- 无万向锁：随便你怎么转，不会丢自由度。
- 插值平滑：做轨迹平滑时，四元数的球面线性插值（Slerp）比欧拉角插值自然得多。
- 紧凑：4个参数，比旋转矩阵的9个少。
// 四元数乘法（表示旋转叠加）
q1 * q2 = [w1*w2 - x1*x2 - y1*y2 - z1*z2,
w1*x2 + x1*w2 + y1*z2 - z1*y2,
w1*y2 - x1*z2 + y1*w2 + z1*x2,
w1*z2 + x1*y2 - y1*x2 + z1*w2]我在项目中遇到过一个问题：用Eigen库时，四元数的存储顺序是 [x, y, z, w]，而ROS的消息格式是 [w, x, y, z]。搞混一次，调试了整整两天。嗯，这种细节，你一定要注意。
2.5 它们之间的转换
实际工作中，你需要在三者之间来回切换。我整理了一个速查表：
| 转换方向 | 方法 | 注意事项 |
|---|---|---|
| 旋转矩阵 → 四元数 | 从R的迹(trace)计算 | 注意数值稳定性，迹接近-1时有奇异性 |
| 四元数 → 旋转矩阵 | 直接用公式展开 | 确保四元数已归一化 |
| 欧拉角 → 四元数 | 按ZYX顺序连乘 | 明确旋转顺序！不同库默认顺序不同 |
| 四元数 → 欧拉角 | 从四元数反算角度 | 注意atan2的象限判断 |
我的建议：在代码里统一用四元数存储位姿，只在可视化或人机交互时转成欧拉角。这样可以减少很多不必要的bug。
2.6 知识体系总览
下面这张图，是我自己总结的坐标系与位姿表示的知识结构。你看一遍，应该能对本章内容有个整体把握。
2.7 实战中的选择建议
说了这么多，到底该用哪个？
我的经验是这样的：
- 做轨迹评估：用四元数。因为ATE和RPE的计算，四元数误差最稳定。
- 做可视化：转成欧拉角。人眼看着舒服。
- 做位姿图优化：用四元数或李代数。别用旋转矩阵，冗余约束会让你头大。
- 做传感器标定：旋转矩阵最方便，因为可以直接写方程。
一个小技巧：如果你用C++，推荐Eigen库的 Quaterniond 和 AngleAxisd。Python的话，scipy.spatial.transform 里的 Rotation 类很好用。别自己手写转换函数，容易出错。
好了，坐标系和位姿表示就聊到这里。下一章我们会把这些知识用到实际的轨迹评估中。到时候你会发现，今天打下的基础，会帮你省下大量调试时间。