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

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

2.1 地球坐标系与机体坐标系

先说说两个最常用的坐标系。

地球坐标系，也叫NED坐标系。N是北，E是东，D是地。原点通常选在起飞点。这个坐标系是固定的，不随飞机转动而转动。说白了，它就是你在航点规划时用的那个参考系。

机体坐标系，这个就好理解了。它绑在飞机上，原点在飞机重心。X轴指向机头，Y轴指向右翼，Z轴指向下。飞机一翻身，这个坐标系也跟着翻。

我刚开始做飞控时犯过一个低级错误——把机体坐标系下的加速度计读数直接当成地球坐标系下的数据去积分算位置。结果飞机明明平飞，位置却往天上飘。嗯，坐标系转换没做对，后面全是白费功夫。

2.2 欧拉角：直观但暗藏陷阱

欧拉角大家应该不陌生。滚转角φ、俯仰角θ、偏航角ψ。这三个角度描述了机体坐标系相对于地球坐标系的旋转关系。

旋转顺序是固定的：先偏航，再俯仰，最后滚转。为什么是这个顺序？你想想看，如果先滚转再偏航，那偏航轴的方向就变了，后面没法算。

欧拉角的好处是直观。我调试时最喜欢看欧拉角曲线，一眼就能看出飞机姿态稳不稳。

但欧拉角有个致命问题——万向锁。当俯仰角接近±90°时，滚转和偏航就分不清了。我在做垂直起降固定翼时遇到过这个问题。飞机做90°俯仰的过渡飞行，欧拉角直接跳变，姿态解算炸了。

2.3 旋转矩阵：数学上最干净

旋转矩阵R，就是一个3×3的矩阵。它能把机体坐标系下的向量转到地球坐标系下。

从欧拉角到旋转矩阵的公式是这样的：

R = Rz(ψ) * Ry(θ) * Rx(φ)

其中：
Rx(φ) = [1,    0,      0;
         0,  cosφ,  -sinφ;
         0,  sinφ,   cosφ]

Ry(θ) = [cosθ,  0,  sinθ;
         0,     1,     0;
        -sinθ,  0,  cosθ]

Rz(ψ) = [cosψ, -sinψ,  0;
         sinψ,  cosψ,  0;
         0,     0,     1]

旋转矩阵的好处是连续、可逆、没有奇点。但缺点也很明显——9个参数，计算量大。在STM32F4这种级别的芯片上，每次更新都要做9次乘加运算，实时性要求高的时候有点吃力。

2.4 四元数：飞控的标配

四元数，说白了就是一个标量加三个虚数：q = w + xi + yj + zk。它用4个参数表示三维空间的旋转。

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

• 无奇点——随便你怎么转，四元数都能表示
• 计算快——只有4个参数，乘法比矩阵少一半
• 插值平滑——两个姿态之间可以平滑过渡

四元数到旋转矩阵的转换公式：

R = [1-2(y²+z²), 2(xy-wz),   2(xz+wy);
     2(xy+wz),   1-2(x²+z²), 2(yz-wx);
     2(xz-wy),   2(yz+wx),   1-2(x²+y²)]

看着复杂，但代码实现起来其实很简单。我一般用Mahony或Madgwick的互补滤波算法来更新四元数，效果很稳定。

2.5 三种表示方法的对比

特性	欧拉角	旋转矩阵	四元数
参数数量	3	9	4
奇点问题	有（万向锁）	无	无
计算效率	高	低	高
直观性	高	中	低
插值平滑性	差	中	好
飞控常用场景	调试、数据显示	离线仿真	实时控制

2.6 知识体系结构图

下面这张图展示了本章的知识脉络，我建议你把它记在脑子里：

这张图把本章的核心逻辑串起来了。左边是坐标系，右边是姿态表示方法，底部是它们之间的转换关系。你把这个结构记牢了，后面学姿态解算和控制器设计就会轻松很多。