第1章：坐标系与姿态表示
各位同学好，我是你们的飞控算法讲师。今天咱们来聊聊飞控里最基础、也最绕不开的话题——坐标系与姿态表示。
说实话，我刚入行那会儿，被这几个概念绕得晕头转向。地球坐标系、机体坐标系、欧拉角、四元数、旋转矩阵……每个词都认识，但串在一起就懵了。后来踩了不少坑，才慢慢理清楚。
这一章，我就把压箱底的经验掏出来，帮你把这些概念彻底搞明白。
1.1 地球坐标系：飞控的“绝对参考系”
地球坐标系，也叫导航坐标系。说白了，就是给无人机一个“绝对位置”的参考。
我们通常用NED坐标系——北(North)、东(East)、地(Down)。
- X轴：指向正北
- Y轴：指向正东
- Z轴：指向地心（也就是向下）
为什么选NED？因为大部分传感器（比如GPS、磁力计）都是基于这个坐标系输出的。你想想看，GPS给你的是经纬度，磁力计告诉你北在哪，这不正好对上吗？
重要概念：地球坐标系是惯性系，我们假设它固定不动。所有姿态解算，最终都要转换到这个坐标系下。
我在项目中遇到过一个问题：有次试飞，无人机起飞后一直往南偏。查了半天，发现是磁力计校准没做好，导致北向基准偏了5度。嗯，从那以后，我每次上电都会先做磁力计校准。
1.2 机体坐标系：无人机自己的“身体坐标”
机体坐标系是固定在无人机上的。想象一下，你坐在飞机里，你的前后左右就是机体坐标系。
- X轴：机头方向（前进方向）
- Y轴：机身右侧
- Z轴：机身下方（符合右手定则）
这个坐标系有什么用？所有IMU（惯性测量单元）的数据，比如加速度计、陀螺仪，都是基于机体坐标系输出的。
个人习惯：我一般把机体坐标系的原点放在无人机的重心位置。这样力矩计算会方便很多。
你可能会问：为什么不用地球坐标系直接算？因为传感器装在飞机上，它只能感知自己“身体”的运动。所以我们必须先拿到机体坐标系的数据，再想办法转换到地球坐标系。
1.3 欧拉角：最直观的姿态表示
欧拉角是描述姿态最直观的方式。它用三个角度来表示：横滚(Roll)、俯仰(Pitch)、偏航(Yaw)。
| 角度 | 定义 | 范围 |
|---|---|---|
| 横滚(Roll, φ) | 绕X轴旋转 | -180° ~ 180° |
| 俯仰(Pitch, θ) | 绕Y轴旋转 | -90° ~ 90° |
| 偏航(Yaw, ψ) | 绕Z轴旋转 | -180° ~ 180° |
欧拉角的好处是容易理解。你一看“横滚30度”，就知道飞机向右倾斜了30度。
注意：欧拉角有个致命问题——万向锁(Gimbal Lock)。当俯仰角接近±90度时，横滚和偏航会失去一个自由度。说白了，就是姿态解算会“卡住”。
我曾经在调试一个特技飞行的无人机时，就遇到了万向锁。飞机在做筋斗动作时，姿态突然跳变，差点炸机。从那以后，我内部计算只用四元数，欧拉角只用来显示和调试。
1.4 四元数：飞控的“秘密武器”
四元数听起来高大上，其实就是一个四维复数。它用四个数来表示旋转：
q = w + xi + yj + zk其中w是实部，x、y、z是虚部。约束条件：w² + x² + y² + z² = 1
为什么飞控都用四元数？三个原因：
- 无万向锁：随便你怎么转，都不会卡住
- 计算效率高：只需要乘法和加法，没有三角函数
- 插值平滑：做姿态插值时，四元数比欧拉角平滑得多
核心公式：四元数乘法可以组合旋转。比如先旋转q1再旋转q2，结果就是q = q2 * q1。
我建议你记住这个公式：
// 四元数乘法
q_out.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
q_out.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y;
q_out.y = q1.w*q2.y - q1.x*q2.z + q1.y*q2.w + q1.z*q2.x;
q_out.z = q1.w*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.w;这段代码我手写过不下100遍，闭着眼睛都能写出来。
1.5 旋转矩阵：连接两个坐标系的“桥梁”
旋转矩阵是一个3x3的矩阵，它能把一个向量从一个坐标系转换到另一个坐标系。
从机体坐标系到地球坐标系的旋转矩阵（用欧拉角表示）：
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θ |看着复杂，但实际用起来很简单。比如你要把机体坐标系下的加速度转换到地球坐标系：
// 加速度转换
accel_earth = R * accel_body;实战技巧：我一般用四元数生成旋转矩阵，而不是用欧拉角。因为四元数生成的矩阵更稳定，不会出现万向锁问题。
四元数转旋转矩阵的代码：
// 四元数转旋转矩阵
R[0][0] = 1 - 2*(q.y*q.y + q.z*q.z);
R[0][1] = 2*(q.x*q.y - q.w*q.z);
R[0][2] = 2*(q.x*q.z + q.w*q.y);
R[1][0] = 2*(q.x*q.y + q.w*q.z);
R[1][1] = 1 - 2*(q.x*q.x + q.z*q.z);
R[1][2] = 2*(q.y*q.z - q.w*q.x);
R[2][0] = 2*(q.x*q.z - q.w*q.y);
R[2][1] = 2*(q.y*q.z + q.w*q.x);
R[2][2] = 1 - 2*(q.x*q.x + q.y*q.y);1.6 四种表示方式的对比
| 表示方式 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|
| 欧拉角 | 直观、易理解 | 万向锁、计算慢 | 显示、调试 |
| 四元数 | 无万向锁、计算快、插值平滑 | 不直观、难理解 | 内部姿态解算 |
| 旋转矩阵 | 通用性强、易于组合 | 占用内存大、有冗余 | 坐标转换 |
| 轴角表示 | 物理意义明确 | 插值困难 | 特殊场景 |
1.7 实战建议
说了这么多，我总结一下我的个人经验：
- 内部计算用四元数：所有姿态解算、控制律计算，都用四元数。稳定、高效。
- 显示和日志用欧拉角：调试时看欧拉角最直观。但注意避开万向锁区域。
- 坐标转换用旋转矩阵：需要把向量从一个坐标系转到另一个时，用旋转矩阵最方便。
- 初始化用欧拉角：上电时，先用欧拉角初始化姿态，然后立即转成四元数。
避坑指南：我曾经在代码里混用了欧拉角和四元数，结果姿态解算完全乱掉。记住，一个系统里只用一种内部表示，我推荐四元数。
好了，这一章的内容就到这里。下一章我们会讲姿态解算的具体算法，包括互补滤波和卡尔曼滤波。到时候你会看到，今天学的这些坐标系和姿态表示，是怎么在实际代码中发挥作用的。
记住一句话：坐标系是飞控的“世界观”，姿态表示是飞控的“语言”。搞懂了这些，后面的内容就水到渠成了。