2、坐标系与旋转：地球固联坐标系、机体坐标系、欧拉角定义与万向锁问题
做飞控这么多年，我见过太多新手在坐标系这块栽跟头。说白了，姿态控制就是回答三个问题：飞机在哪儿？机头朝哪？怎么转过去？这三个问题背后，就是坐标系和旋转的数学。
今天咱们就把这块硬骨头啃下来。你想想看，如果连坐标系都搞混，后面那些PID调参、卡尔曼滤波全是白搭。
2.1 地球固联坐标系（NED）
地球固联坐标系，也叫NED坐标系。NED是三个单词的首字母：North（北）、East（东）、Down（地）。
- X轴：指向正北
- Y轴：指向正东
- Z轴：指向地心（向下）
为什么Z轴要向下？这是航空界的惯例。我刚开始学的时候也觉得别扭，明明向上更符合直觉。但你想，飞机的高度是离地高度，加速度计测的是重力加速度（向下），所以Z轴向下反而让计算更直接。
关键点：NED坐标系是惯性参考系，我们说的"高度"、"经纬度"、"航向角"都是在这个坐标系下定义的。ArduPilot里所有导航计算，最终都要统一到NED框架下。
2.2 机体坐标系（Body Frame）
机体坐标系是绑在飞机身上的。不管飞机怎么翻跟头，这个坐标系跟着飞机一起动。
- X轴：指向机头方向（前进方向）
- Y轴：指向右翼
- Z轴：指向机腹（向下）
这里有个容易混淆的点：机体坐标系的Z轴也是向下的。为什么？因为这样在零姿态（飞机水平朝北）时，机体坐标系和NED坐标系完全重合。你想想，如果Z轴向上，那零姿态时两个坐标系就差了180度，每次转换都要多一个负号，多麻烦。
我的习惯：在代码里，我通常把机体坐标系的数据加上前缀 body_，比如 body_accel、body_gyro。这样一眼就能看出数据是在哪个坐标系下测量的。
2.3 欧拉角定义
欧拉角是描述机体坐标系相对于NED坐标系姿态的三个角度。说白了，就是告诉你飞机现在"歪"了多少。
| 角度 | 符号 | 范围 | 描述 |
|---|---|---|---|
| 横滚角（Roll） | φ (phi) | -180° ~ 180° | 绕X轴旋转，右滚为正 |
| 俯仰角（Pitch） | θ (theta) | -90° ~ 90° | 绕Y轴旋转，抬头为正 |
| 偏航角（Yaw） | ψ (psi) | -180° ~ 180° | 绕Z轴旋转，右转为正 |
旋转顺序是：偏航 → 俯仰 → 横滚（Z-Y-X顺序）。这个顺序是航空界的标准，ArduPilot里也是这么用的。
为什么是这个顺序？我举个例子：你先让飞机偏航90度（机头朝东），再俯仰30度（抬头），最后横滚20度（右滚）。如果顺序反过来，结果完全不同。所以欧拉角必须绑定旋转顺序，否则就是耍流氓。
我曾经踩过的坑：有一次调试外部姿态解算模块，对方给的欧拉角没说明旋转顺序。我默认用了Z-Y-X，结果飞机一上天就乱转。后来才发现对方用的是X-Y-Z顺序。从那以后，我拿到任何姿态数据，第一件事就是确认旋转顺序。
2.4 万向锁问题
万向锁（Gimbal Lock）是欧拉角最头疼的问题。当俯仰角接近 ±90° 时，横滚和偏航会失去一个自由度。
为什么会这样？你想想看：
- 飞机垂直向上爬升（俯仰角 = 90°）
- 此时机头指向天，机背朝北
- 你想让飞机向右转（偏航），但机头朝上，向右转和横滚的效果是一样的
- 横滚和偏航变得无法区分
数学上，当 θ = ±90° 时，旋转矩阵中的某些项变成0或1，导致两个轴耦合在一起。具体来说，在Z-Y-X顺序下，当 θ = 90° 时：
cos(θ) = 0, sin(θ) = 1
旋转矩阵中，横滚和偏航的项合并，无法唯一解出 φ 和 ψ这就是万向锁。在固定翼飞机上很少遇到（谁会垂直爬升？），但在多旋翼上很常见——比如你做急停、翻滚动作时，俯仰角很容易接近90°。
解决方案：ArduPilot内部不使用欧拉角做姿态控制，而是用四元数（Quaternion）。四元数没有万向锁问题，而且计算效率更高。但欧拉角作为人机交互的接口（地面站显示、遥控器输入）仍然不可或缺。
2.5 知识体系总览
下面这张图把坐标系、欧拉角和万向锁的关系串起来了。你可以把它当作本章的思维导图。
2.6 实践中的坐标系转换
在ArduPilot代码里，坐标系转换无处不在。最常见的场景是：
- 加速度计数据：传感器测的是机体坐标系下的加速度，需要转换到NED坐标系才能计算速度和位置
- GPS数据：GPS给的是NED坐标系下的位置和速度，但控制指令需要转换到机体坐标系
- 姿态控制：期望姿态（NED）与实际姿态（机体）的误差，需要在机体坐标系下计算控制量
转换的核心是旋转矩阵。给定欧拉角 (φ, θ, ψ)，从机体到NED的旋转矩阵是：
R = Rz(ψ) * Ry(θ) * Rx(φ)
其中：
Rz(ψ) = [cosψ -sinψ 0; sinψ cosψ 0; 0 0 1]
Ry(θ) = [cosθ 0 sinθ; 0 1 0; -sinθ 0 cosθ]
Rx(φ) = [1 0 0; 0 cosφ -sinφ; 0 sinφ cosφ]这个矩阵乘法的顺序不能错。我刚开始写代码时，把顺序搞反了，结果飞机在地上自检时姿态显示就是错的。排查了半天才发现是矩阵乘法顺序的问题。
避坑指南：在ArduPilot里，AP_Math库已经封装好了旋转矩阵和四元数的转换函数。你不需要手写矩阵乘法，但一定要搞清楚输入输出是在哪个坐标系下。我建议你在调用转换函数前，先写一行注释说明当前数据所在的坐标系。
2.7 小结
这一章的内容是姿态控制的基石。NED坐标系是"世界"，机体坐标系是"我"，欧拉角是"我相对于世界的姿态"。万向锁是欧拉角的固有缺陷，所以ArduPilot内部用四元数来规避。
嗯，这里要注意：理解坐标系转换的物理意义，比背公式更重要。公式可以查手册，但坐标系之间的空间关系必须刻在脑子里。下次你调参时，看到某个轴的反应不对，第一时间就能想到是坐标系转换出了问题。