第1章：坐标系与姿态表示

各位同学，今天我们来聊聊无人机最基础、也最容易搞混的一个话题——坐标系与姿态表示。

说实话，我刚开始做飞控那会儿，被坐标系绕得晕头转向。有一次调试，飞机明明应该朝北飞，结果它偏要往东跑。查了半天，原来是坐标系定义搞反了。嗯，从那以后，我对坐标系就格外上心。

1.1 地球坐标系：你的飞机在哪儿？

地球坐标系，说白了就是告诉无人机「你在哪儿、往哪儿飞」。常用的有两种：

• NED坐标系：北-东-地。X轴指北，Y轴指东，Z轴指向地心。我个人习惯用这个，因为直观。
• ENU坐标系：东-北-天。X轴指东，Y轴指北，Z轴朝天。GPS数据通常用这个。

你想想看，如果飞控里用的是NED，但GPS给的是ENU，那数据就全乱了。我在项目里就踩过这个坑——当时飞机悬停时一直漂移，查了两天才发现是坐标系没对齐。

1.2 机体坐标系：飞机自己的视角

机体坐标系是固定在飞机上的。想象你坐在驾驶舱里：

• X轴：指向机头（前进方向）
• Y轴：指向右翼
• Z轴：指向机腹（向下）

这个坐标系用来描述「飞机自身的姿态」。比如，IMU测到的加速度、角速度，都是基于机体坐标系的。

我记得有一次，一个学生问我：「为什么飞机水平飞行时，Z轴加速度不是0？」

答案很简单：因为重力。在机体坐标系下，重力分量会投影到三个轴上。这就是为什么我们需要姿态估计——把机体坐标系的数据转换到地球坐标系。

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

欧拉角是大家最容易理解的姿态表示方式。三个角度：

角度	符号	范围	说明
横滚角	φ (phi)	-180° ~ 180°	绕X轴旋转
俯仰角	θ (theta)	-90° ~ 90°	绕Y轴旋转
偏航角	ψ (psi)	-180° ~ 180°	绕Z轴旋转

但是，欧拉角有个致命问题——万向锁。当俯仰角接近±90°时，横滚和偏航会耦合，导致自由度丢失。

1.4 四元数：数学家的优雅方案

四元数听起来高大上，其实就是一个四维向量：

q = [w, x, y, z]

其中 w 是实部，x、y、z 是虚部。它满足：

w² + x² + y² + z² = 1

为什么用四元数？三个理由：

• 无万向锁：随便你怎么转，不会丢失自由度
• 计算高效：只需要乘法和加法，没有三角函数
• 插值平滑：做姿态平滑时，四元数插值比欧拉角自然得多

我建议所有飞控代码里，内部运算都用四元数。只在需要显示或日志输出时，才转成欧拉角。

1.5 旋转矩阵：坐标转换的桥梁

旋转矩阵是一个3×3的正交矩阵，用来把一个坐标系下的向量转换到另一个坐标系。

从机体坐标系到地球坐标系的旋转矩阵 R 可以这样计算：

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]

举个例子，如果IMU测到机体坐标系下的加速度 a_body = [0, 0, -9.8]（飞机水平悬停），那么地球坐标系下的加速度就是：

a_earth = R * a_body

结果应该是 [0, 0, 9.8]（重力朝上，因为NED坐标系Z轴朝下）。

1.6 它们之间的转换关系

这三种表示方式可以互相转换。我整理了一张表，方便你查阅：

从→到	欧拉角	四元数	旋转矩阵
欧拉角	—	用三角函数计算	三个旋转矩阵相乘
四元数	用atan2计算	—	用四元数元素构造
旋转矩阵	用反三角函数	用矩阵元素推导	—

具体公式我就不列了，网上到处都有。但我要强调一点：转换时注意象限判断。比如从旋转矩阵求欧拉角，atan2比atan更安全，因为它能处理四个象限。

我曾经在代码里用atan，结果偏航角在±180°边界上跳变，导致航向控制震荡。换成atan2后，问题立刻解决。

1.7 实战建议

最后，给你几个我多年积累的经验：

1. 统一用四元数做内部运算，只在输入输出时转换
2. 定期归一化四元数，防止数值误差累积
3. 欧拉角只用于调试和显示，别用它做控制
4. 坐标系定义写在代码最前面，用注释说明清楚

好了，这一章就到这里。下一章我们讲传感器模型，看看IMU、GPS、磁力计各自有什么脾气。

记住：坐标系搞对了，飞控就成功了一半。