3. 坐标系与坐标变换：世界坐标系、机体坐标系、传感器坐标系、欧拉角与四元数、齐次变换矩阵

坐标系这东西，说白了就是给无人机或机器人一个「我在哪」的答案。我刚开始做航迹规划时，觉得坐标系就是个数学概念，直到第一次实飞测试，飞机在天上乱转——嗯，坐标系搞反了。从那以后，我每次写代码第一件事就是确认坐标系定义。

3.1 三大核心坐标系

任务规划里，我们打交道最多的就是这三个坐标系。我习惯把它们想象成三个不同的「视角」：

• 世界坐标系（World Frame）：也叫全局坐标系。通常用 W 表示，原点固定在地面某点（比如起飞点），X 轴指向北，Y 轴指向东，Z 轴指向地心。说白了，这就是我们常说的「北东地」坐标系。
• 机体坐标系（Body Frame）：用 B 表示，原点在飞行器质心。X 轴指向机头，Y 轴指向右翼，Z 轴指向机腹。你想想看，飞行员说的「左转」就是绕 Z 轴转，这就是机体坐标系在说话。
• 传感器坐标系（Sensor Frame）：用 S 表示，原点在传感器光心或安装位置。比如摄像头、激光雷达都有自己的坐标系。我遇到过最坑的事，就是摄像头坐标系和机体坐标系差了 90 度没校准，结果避障算法把墙当成了地板。

3.2 欧拉角：直观但危险

欧拉角是描述旋转最直观的方式——三个角度：偏航角（Yaw）、俯仰角（Pitch）、横滚角（Roll）。我刚开始教新人时，总说「你想象一下飞机点头、侧身、转头就行了」。

但这里有个大坑：万向锁（Gimbal Lock）。当俯仰角达到 ±90° 时，偏航和横滚会失去一个自由度。我曾经在仿真里遇到过，飞机做筋斗动作时姿态解算突然崩了，查了半天才发现是欧拉角在作祟。

欧拉角	符号	范围	说明
偏航角	ψ	[-π, π]	绕 Z 轴旋转，机头指向
俯仰角	θ	[-π/2, π/2]	绕 Y 轴旋转，抬头低头
横滚角	φ	[-π, π]	绕 X 轴旋转，左右倾斜

3.3 四元数：优雅的数学工具

四元数听起来吓人，其实就是一个三维向量加一个标量：q = w + xi + yj + zk。我个人的理解是：它描述了一个旋转轴和绕该轴旋转的角度。

为什么用四元数？三个理由：

• 无万向锁——随便你怎么转，不会死锁
• 插值平滑——做航迹规划时，两个姿态之间可以丝滑过渡
• 计算高效——比旋转矩阵少几个乘法，嵌入式上跑得快

举个实际例子，我在做无人机航迹生成时，需要让相机始终对准地面目标。用四元数做姿态插值，每帧更新一次，画面稳得像三脚架。如果用欧拉角，中间会有抖动——你想想看，目标跟踪时画面一抖，后果是什么？

// 四元数乘法：组合两个旋转
// q1 是当前姿态，q2 是目标姿态
// 结果 q 就是平滑过渡后的姿态
q = q1 * q2;

// 四元数插值（SLERP）
// t 从 0 到 1，q 从 q1 平滑过渡到 q2
q = slerp(q1, q2, t);

3.4 齐次变换矩阵：统一旋转与平移

齐次变换矩阵是个好东西。它把旋转和平移塞进一个 4×4 矩阵里，一次乘法搞定坐标变换。我习惯叫它「万能转换器」。

结构很简单：

| R   t |
| 0   1 |

其中 R 是 3×3 旋转矩阵，t 是 3×1 平移向量。最后一行是 [0 0 0 1]，保持齐次形式。

实际工作中，我经常需要把传感器坐标系下的障碍物位置转换到世界坐标系。步骤就是：

1. 传感器坐标系 → 机体坐标系（用传感器安装矩阵 T_sb）
2. 机体坐标系 → 世界坐标系（用无人机位姿矩阵 T_bw）
3. 最终：T_sw = T_bw × T_sb

3.5 知识体系总览

下面这张图是我自己总结的坐标系与坐标变换知识结构，你可以把它当作学习路线图：

这张图把整个知识体系串起来了。你从三大坐标系出发，选择旋转表示方法（欧拉角、四元数或旋转矩阵），然后通过齐次变换矩阵统一处理旋转和平移，最后应用到航迹生成、姿态控制、传感器融合等实际场景中。

---