2、坐标系与姿态表示：地球坐标系、机体坐标系、欧拉角、旋转矩阵、四元数基础
各位同学，欢迎来到第二章。
这一章，咱们聊聊飞控里最基础、也最容易搞混的东西——坐标系和姿态表示。说实话，我当年刚入行时，在这上面栽过不少跟头。有一次调试四旋翼，明明代码逻辑看着都对，飞机就是翻跟头。查了两天，最后发现是坐标系定义反了。嗯，从那以后，我对坐标系就格外敬畏。
好，咱们正式开始。
2.1 为什么要搞这么多坐标系？
你想想看，飞机在天上飞，它自己感觉的「向前」和地面看到的「向前」是一回事吗？
当然不是。
飞机低头俯冲时，它机头指向地面，但地面坐标系里的「北」还是那个「北」。所以我们需要两套坐标系：一套固定在地球上，一套固定在飞机上。这就是地球坐标系和机体坐标系的由来。
2.2 地球坐标系（NED）
地球坐标系，也叫导航坐标系。我习惯用 NED 表示：
- N（North）：指向地理北
- E（East）：指向地理东
- D（Down）：指向地心（也就是向下）
为什么向下是正？因为飞控里加速度计测的是重力，重力方向朝下。这样定义，很多计算可以少写负号。我个人觉得这是前人留下的好习惯，咱们接着用就行。
重要：NED 坐标系是右手系。你伸出右手，拇指指北，食指指东，中指自然指向下——这就是 NED。
2.3 机体坐标系（Body Frame）
机体坐标系固定在飞机上。原点在飞机重心，三个轴分别是：
- X轴：指向机头（前进方向）
- Y轴：指向飞机右侧
- Z轴：指向飞机下方
注意，这也是右手系。我见过有人把 Y 轴指向左侧，结果旋转方向全反了。所以定义坐标系时，一定要统一标准。
小技巧：写代码时，把坐标系定义写在最前面，加个注释。比如：// Body frame: X-front, Y-right, Z-down。这样半年后回来看代码，不会骂自己。
2.4 欧拉角：最直观的姿态表示
欧拉角，说白了就是用三个角度描述飞机转了多少。
三个角分别是：
- 滚转角（Roll，φ）：绕 X 轴旋转。飞机侧倾的角度。
- 俯仰角（Pitch，θ）：绕 Y 轴旋转。飞机抬头低头的角度。
- 偏航角（Yaw，ψ）：绕 Z 轴旋转。飞机机头指向的角度。
旋转顺序很重要。我习惯用 Z-Y-X 顺序，也就是先偏航、再俯仰、最后滚转。为什么是这个顺序？因为这样最符合人的直觉：先看机头指向哪（偏航），再抬头低头（俯仰），最后侧倾（滚转）。
避坑指南：欧拉角有个致命问题——万向锁。当俯仰角接近 ±90° 时，滚转和偏航会耦合，丢失一个自由度。我曾经在无人机倒飞时遇到过这个问题，姿态解算直接炸了。所以，如果你做全姿态飞行（比如特技无人机），别只用欧拉角。
2.5 旋转矩阵：数学上的优雅表达
欧拉角虽然直观，但做数学运算不方便。这时候旋转矩阵就派上用场了。
旋转矩阵是一个 3×3 的矩阵，它能把机体坐标系下的向量，转换到地球坐标系下。反过来也行。
从机体到地球的旋转矩阵（Z-Y-X 顺序）长这样：
R = Rz(ψ) * Ry(θ) * Rx(φ)
其中：
Rz(ψ) = | cosψ -sinψ 0 |
| sinψ cosψ 0 |
| 0 0 1 |
Ry(θ) = | cosθ 0 sinθ |
| 0 1 0 |
| -sinθ 0 cosθ |
Rx(φ) = | 1 0 0 |
| 0 cosφ -sinφ |
| 0 sinφ cosφ |你可能会问：为什么要乘三个矩阵？因为一次旋转对应一个矩阵，三次旋转就是三个矩阵相乘。注意顺序：先滚转、再俯仰、最后偏航，所以矩阵乘法是从右往左读。
实际应用：在飞控代码里，我们经常用旋转矩阵把加速度计测到的「机体加速度」转换到「地球坐标系」，然后积分得到速度。这一步做错了，位置估计全完蛋。
2.6 四元数：飞控的终极武器
欧拉角有万向锁，旋转矩阵有9个参数（计算量大），那有没有更好的办法？
有，四元数。
四元数用4个参数表示旋转：
q = w + xi + yj + zk
其中 w 是实部，x、y、z 是虚部。
满足：w² + x² + y² + z² = 1四元数的好处：
- 没有万向锁
- 计算量小（只有4个参数）
- 插值平滑（做姿态平滑时特别好用）
我记得第一次在 STM32 上跑四元数姿态解算时，看到它比旋转矩阵快了将近一倍，当时就觉得：嗯，以后就用它了。
2.7 四元数与旋转矩阵的转换
实际写代码时，我们经常需要在四元数和旋转矩阵之间来回转换。比如传感器输出四元数，但控制算法需要旋转矩阵。
从四元数到旋转矩阵：
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²) |从旋转矩阵到四元数：
w = sqrt(1 + R[0][0] + R[1][1] + R[2][2]) / 2
x = (R[2][1] - R[1][2]) / (4 * w)
y = (R[0][2] - R[2][0]) / (4 * w)
z = (R[1][0] - R[0][1]) / (4 * w)经验之谈：转换时注意数值稳定性。如果 w 接近 0，用其他公式算。我一般会加个判断：if (w < 0.001) { 用另一种方法 }。别问我怎么知道的——调试时遇到过 NaN。
2.8 实战：姿态初始化
飞机上电时，我们不知道它的姿态。怎么办？
我的做法是：
- 读取加速度计，得到重力方向（确定俯仰和滚转）
- 读取磁力计，得到地磁方向（确定偏航）
- 用这两个信息初始化四元数
代码大概长这样：
// 从加速度计计算俯仰和滚转
float pitch = atan2(-accel_x, sqrt(accel_y*accel_y + accel_z*accel_z));
float roll = atan2(accel_y, accel_z);
// 从磁力计计算偏航（需要先补偿俯仰和滚转）
float yaw = atan2(mag_y, mag_x);
// 用欧拉角初始化四元数
quaternion_from_euler(roll, pitch, yaw, &q);注意：磁力计很容易受干扰。我曾在钢筋水泥楼里调试，磁力计读数乱跳，偏航角直接漂了 30 度。后来加了磁校准和地磁模型补偿，才算稳定下来。
2.9 本章小结
好了，这一章的内容就这些。咱们捋一下：
- 地球坐标系（NED）：固定在地球上，导航用
- 机体坐标系：固定在飞机上，传感器用
- 欧拉角：直观，但有万向锁
- 旋转矩阵：数学严谨，但计算量大
- 四元数：飞控首选，没有万向锁，计算快
下一章，咱们会把这些知识用到实际代码里，写一个完整的姿态解算器。到时候你会发现，今天学的这些，全是基础中的基础。
有什么问题，欢迎在评论区留言。咱们下章见。