2. 坐标系与姿态表示：地球坐标系、机体坐标系、欧拉角与旋转矩阵、四元数基础

做飞控这么多年，我越来越觉得坐标系这东西就像房子的地基。你算法写得再花哨，坐标系搞错了，飞机在天上就是乱转。今天咱们就把这块硬骨头啃下来。

2.1 地球坐标系：我们到底在跟谁说话？

地球坐标系，说白了就是给飞机一个绝对参考系。我习惯叫它「大地坐标系」或者「NED坐标系」——北东地。

• N轴（北）：指向地理北极
• E轴（东）：指向正东方向
• D轴（地）：指向地心，也就是重力方向

你想想看，GPS给的位置信息，本质上就是在这个坐标系下的经纬度和高度。我在做第一次无人机自主返航时，就吃过亏——GPS坐标转换到NED时，忘记把高度取反了，结果飞机一头扎向地面。嗯，从那以后我再也不敢忽略D轴的方向了。

2.2 机体坐标系：飞机自己的小世界

机体坐标系是固定在飞机上的。想象你坐在驾驶舱里，你的前后左右就是机体坐标系。

• X轴（前）：指向机头方向
• Y轴（右）：指向右侧机翼
• Z轴（下）：指向机腹，符合右手定则

为什么要有机体坐标系？因为IMU（惯性测量单元）就装在飞机上，它测到的加速度和角速度都是相对于机体坐标系的。你想想看，如果不用机体坐标系，你怎么告诉飞控「我现在在向右滚转」？

2.3 欧拉角：最直观的姿态表示

欧拉角是大家最容易理解的方式。它用三个角度来描述飞机姿态：横滚（Roll）、俯仰（Pitch）、偏航（Yaw）。

角度	旋转轴	范围	物理意义
横滚 φ	X轴	-180° ~ 180°	左右倾斜
俯仰 θ	Y轴	-90° ~ 90°	抬头低头
偏航 ψ	Z轴	-180° ~ 180°	机头朝向

欧拉角最大的问题是什么？万向锁。当俯仰角接近±90°时，横滚和偏航会耦合在一起，丢失一个自由度。我在做垂直起降固定翼时，就遇到过这个问题——飞机在过渡阶段俯仰角接近90°，欧拉角直接炸了。

2.4 旋转矩阵：数学上的优雅表达

旋转矩阵是连接两个坐标系的桥梁。从机体坐标系到地球坐标系的旋转矩阵，通常记作 C_bⁿ。

它的形式是这样的（按Z-Y-X顺序旋转）：

C = [
  [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θ]
]

看着复杂，其实本质就是三个基本旋转矩阵的乘积。我个人习惯用这个矩阵来做坐标变换——比如把加速度计测到的「机体加速度」转换到「地球坐标系」，然后减去重力，再积分得到速度。

2.5 四元数基础：飞控的终极武器

四元数，说白了就是没有万向锁的欧拉角。它用四个数来表示旋转：q = [q0, q1, q2, q3]，其中q0是标量部分，q1,q2,q3是矢量部分。

为什么飞控都用四元数？三个原因：

• 无奇异：不会出现万向锁
• 计算快：只有乘法和加法，没有三角函数
• 易插值：可以做平滑的球面线性插值

四元数更新公式（离散形式）：

q_new = q_old + 0.5 * dt * Ω(ω) * q_old

其中 Ω(ω) = [
  [0,   -ωx,  -ωy,  -ωz],
  [ωx,   0,    ωz,  -ωy],
  [ωy,  -ωz,   0,    ωx],
  [ωz,   ωy,  -ωx,   0]
]

这个公式是姿态解算的核心。我在做PX4飞控移植时，发现原版代码里用了这个公式，但没做归一化。结果飞了十几分钟后，四元数模长偏离了1，姿态开始漂移。加上归一化后，问题就解决了。

2.6 知识体系总览

下面这张图是我自己总结的坐标系与姿态表示的知识结构，你看一眼就能明白它们之间的关系：

这张图把今天讲的内容串起来了。你从地球坐标系出发，通过旋转矩阵或四元数，就能转换到机体坐标系。欧拉角是中间产物，方便人理解，但不适合做计算。

好了，坐标系和姿态表示这块，咱们就聊到这儿。记住一句话：选对坐标系，调参少流泪。下次你在代码里看到那些矩阵乘法时，心里就有底了。