第1章：坐标系与变换——飞控的“世界观”
做飞控这些年，我最大的体会就是：坐标系搞不清楚，后面全白干。你想想看，无人机在天上飞，传感器测的是机体数据，导航要的是地理信息，这两者之间怎么搭桥？靠的就是坐标系变换。
这一章，我们就来把坐标系这件事彻底讲透。别觉得它枯燥，我敢说，你遇到的90%的飞控bug，根源都在坐标系上。
1.1 地理坐标系：NED与ENU
先说地理坐标系。说白了，就是给无人机一个“地球上的参考系”。
目前主流的有两种：NED（北-东-地）和ENU（东-北-天）。
| 坐标系 | X轴 | Y轴 | Z轴 | 典型应用 |
|---|---|---|---|---|
| NED | 北（North） | 东（East） | 地（Down） | PX4、ArduPilot、惯性导航 |
| ENU | 东（East） | 北（North） | 天（Up） | ROS、GPS坐标、机器人 |
我个人习惯用NED。为什么？因为PX4和大部分开源飞控都用它。但如果你做ROS相关的项目，ENU更常见。这里有个坑——我曾经在移植代码时，把NED的Z轴向下当成ENU的Z轴向上，结果无人机一解锁就往地里钻。嗯，那次炸机让我记住了这个教训。
1.2 机体坐标系
机体坐标系，就是“长在飞机上的坐标系”。通常定义如下：
- X轴：指向机头（前进方向）
- Y轴：指向机身右侧
- Z轴：指向机身下方（符合右手定则）
你想想看，IMU（惯性测量单元）测出来的加速度、角速度，都是基于机体坐标系的。但你要控制飞机往北飞，就得把机体坐标系下的数据转换到NED坐标系下。
这里有个小技巧：我一般会在代码里把机体坐标系命名为 body_frame，地理坐标系命名为 world_frame。命名清晰，后面调试时少掉头发。
1.3 坐标系之间的旋转关系
旋转关系，说白了就是“怎么把机体坐标系下的向量，转到地理坐标系下”。
常用的旋转顺序是：Z-Y-X（偏航-俯仰-横滚）。也就是：
- 先绕Z轴转偏航角（Yaw，ψ）
- 再绕Y轴转俯仰角（Pitch，θ）
- 最后绕X轴转横滚角（Roll，φ）
对应的旋转矩阵长这样：
R = Rz(ψ) * Ry(θ) * Rx(φ)展开后：
R = [
[cosθ*cosψ, sinφ*sinθ*cosψ - cosφ*sinψ, cosφ*sinθ*cosψ + sinφ*sinψ],
[cosθ*sinψ, sinφ*sinθ*sinψ + cosφ*cosψ, cosφ*sinθ*sinψ - sinφ*cosψ],
[-sinθ, sinφ*cosθ, cosφ*cosθ]
]看着复杂？其实你不需要背。我写代码时都是直接调库（比如Eigen或PX4的math库）。但你要理解它的物理意义——这个矩阵就是把机体坐标系下的向量，投影到地理坐标系下。
1.4 齐次坐标与变换矩阵
旋转矩阵只能处理旋转，不能处理平移。但无人机不光会转，还会飞（平移）。这时候就需要齐次坐标和变换矩阵。
齐次坐标，就是在三维坐标后面加一个1：
三维点 (x, y, z) → 齐次坐标 (x, y, z, 1)变换矩阵是4x4的：
T = [
[R, t],
[0, 1]
]其中R是3x3旋转矩阵，t是3x1平移向量。
举个例子：
// 假设机体坐标系原点在NED坐标系下的位置是 (1, 2, -3)
// 旋转矩阵是 R
// 那么变换矩阵 T 就是：
T = [
[R(0,0), R(0,1), R(0,2), 1],
[R(1,0), R(1,1), R(1,2), 2],
[R(2,0), R(2,1), R(2,2), -3],
[0, 0, 0, 1]
]有了这个矩阵，你就可以把机体坐标系下的任意点，变换到NED坐标系下。
1.5 知识体系总览
下面这张图，是我自己梳理的坐标系变换知识体系。你看一遍，心里就有谱了。
从这张图你可以看到：地理坐标系和机体坐标系通过旋转关系建立联系，再结合平移得到变换矩阵，最终服务于姿态解算、导航控制和传感器融合。每一步都环环相扣。
1.6 避坑指南
最后，分享几个我踩过的坑：
- 我曾经把旋转顺序搞反了。明明代码里写的是Z-Y-X，但实际用的是X-Y-Z。结果无人机姿态完全乱套。后来我养成了一个习惯：在代码注释里明确写出旋转顺序。
- 齐次坐标的最后一维忘记归一化。变换矩阵乘完后，最后一维可能不是1。记得归一化，否则坐标会漂移。
- NED和ENU混用。这个前面说过了，血的教训。
好了，坐标系与变换就讲到这里。记住一句话：坐标系是飞控的“世界观”，变换是飞控的“语言”。搞懂了这些，后面的姿态解算就是水到渠成的事。