2. 坐标系与位姿表示：全局坐标系与局部坐标系、刚体位姿描述（位置与姿态）、旋转矩阵与欧拉角、四元数基础
做轨迹跟踪，第一件事不是写代码，而是搞清楚「我在哪，我要去哪」。
这个问题听起来简单，但实际项目里栽跟头的人可不少。我记得刚入行那会儿，有个同事把全局坐标和局部坐标搞混了，结果机器人原地转圈跑了半天，数据全废了。嗯，咱们今天就把这事彻底理清楚。
2.1 全局坐标系与局部坐标系
说白了，全局坐标系就是「世界」的参考系。你在地上画个原点，定好 X、Y、Z 轴，整个空间里所有东西的位置都拿这个来量。局部坐标系呢？它是跟着物体走的。比如你手里的手机，它自己有个坐标系——屏幕朝哪、顶部朝哪，都是相对于手机本身说的。
我习惯把全局坐标系叫「地图系」，局部坐标系叫「载体系」。做轨迹跟踪时，这两者之间的转换是家常便饭。
举个例子：你站在操场上（全局系），手里拿着一个指南针（局部系）。你转身的时候，指南针的北和操场的北就不一样了。轨迹跟踪里，传感器装在机器人身上（局部系），但你要知道它在世界里的位置（全局系）。
pos_global、vel_body。别嫌啰嗦，这能省掉你半夜 debug 的时间。
2.2 刚体位姿描述：位置与姿态
刚体在空间里有 6 个自由度——3 个位置，3 个姿态。位置好说，就是 (x, y, z)。姿态呢？就是它「朝哪看、怎么歪着」。
我见过不少新手，只关心位置不关心姿态。结果呢？轨迹画出来挺漂亮，但实际机器人一跑就撞墙。为什么？因为姿态不对，控制指令发错了方向。
位置用向量表示：
p = [x, y, z]^T姿态就复杂一点。咱们有三种主流表示方法：旋转矩阵、欧拉角、四元数。下面一个一个说。
2.3 旋转矩阵
旋转矩阵是个 3×3 的方阵，它能把一个向量从一个坐标系转到另一个坐标系。数学上长这样：
R = [r11 r12 r13]
[r21 r22 r23]
[r31 r32 r33]它有个重要性质：正交且行列式为 +1。说白了，旋转不改变向量的长度，只改变方向。
我在项目里用旋转矩阵最多的地方，是把传感器数据从「机体坐标系」转到「全局坐标系」。比如 IMU 测到的加速度是相对于飞机本身的，但你要算它在世界里的运动，就得乘一个旋转矩阵。
2.4 欧拉角
欧拉角是大家最直观的理解方式：绕 X 轴转叫「横滚」（roll），绕 Y 轴转叫「俯仰」（pitch），绕 Z 轴转叫「偏航」（yaw）。
你想想看，飞机转弯的时候，飞行员说的就是「左转 30 度」，这就是偏航角。欧拉角的好处是直观，三个数就能说清楚姿态。
但欧拉角有个大坑——万向锁。当俯仰角达到 ±90° 时，横滚和偏航就分不清了，自由度丢失一个。
欧拉角的转换顺序也很重要。常见的顺序有 ZYX、ZYZ 等。不同顺序得到的角度值不一样，千万别搞混。
| 表示方法 | 元素数 | 优点 | 缺点 |
|---|---|---|---|
| 旋转矩阵 | 9 | 无奇异，可组合 | 冗余，需正交化 |
| 欧拉角 | 3 | 直观，易理解 | 万向锁，顺序依赖 |
| 四元数 | 4 | 无奇异，可插值 | 不够直观 |
2.5 四元数基础
四元数是个「超复数」，形式是 q = w + xi + yj + zk。其中 w 是实部，x、y、z 是虚部。它用 4 个数表示旋转，没有万向锁，而且插值平滑。
我刚开始学四元数时也觉得抽象，后来发现一个窍门：把它想象成「旋转轴 + 旋转角度」的紧凑形式。一个单位四元数，w 部分代表 cos(θ/2)，(x,y,z) 部分代表 sin(θ/2) 乘以旋转轴方向。
四元数的乘法不满足交换律，这个要注意。两个旋转先后作用，顺序不同结果不同。
// 四元数乘法示例
q1 = [w1, x1, y1, z1]
q2 = [w2, x2, y2, z2]
q_result = [
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
]2.6 三种表示方法的转换
实际项目中，这三种表示方法经常需要来回转换。我整理了一个常用转换表：
| 转换方向 | 方法 | 注意事项 |
|---|---|---|
| 旋转矩阵 → 欧拉角 | 从矩阵元素反解角度 | 注意万向锁时的处理 |
| 欧拉角 → 旋转矩阵 | 按顺序乘基本旋转矩阵 | 确认旋转顺序 |
| 四元数 → 旋转矩阵 | 用四元数元素构造矩阵 | 确保四元数已归一化 |
| 旋转矩阵 → 四元数 | 从矩阵迹和元素求解 | 注意数值稳定性 |
嗯，这里要注意：转换时一定要检查数值稳定性。特别是从旋转矩阵转四元数时，如果矩阵的迹接近 -1，标准公式会出问题。我一般会加个分支判断，用不同的公式处理边界情况。
这张图把咱们这章的核心内容串起来了。从位姿表示出发，分坐标系、位置、姿态三条线，姿态下面又分出三种表示方法，最后它们之间可以相互转换。你把这个图记在脑子里，后面学轨迹跟踪的算法时，就不会迷路。