第四节：坐标系与旋转矩阵
各位同学，今天我们来聊聊飞控里最基础、也最容易绕晕的一个话题——坐标系与旋转。
说实话，我刚入行那会儿，被欧拉角和四元数折磨得不轻。明明都是描述姿态的东西，为什么搞出这么多花样？后来在项目里摔了几次机，才真正明白：选错坐标系，代码跑得再漂亮也是白搭。
4.1 地理坐标系与机体坐标系
先搞清楚两个最常用的坐标系。
- 地理坐标系（NED）：北东地。X轴指向北，Y轴指向东，Z轴指向地心。说白了，就是地面站里看到的那套参考系。
- 机体坐标系（Body）：固定在飞机上。X轴指向机头，Y轴指向右翼，Z轴指向机腹。你坐在驾驶舱里，这就是你的“身体”坐标系。
为什么要区分？因为传感器数据是在机体坐标系下采集的，而导航指令是在地理坐标系下给出的。中间差了这么一层旋转关系，不转换的话，飞控就会“左右不分”。
核心要点：地理坐标系是“世界”的视角，机体坐标系是“飞机”的视角。所有姿态控制，本质上就是在这两个视角之间来回翻译。
4.2 欧拉角：最直观的姿态描述
欧拉角，说白了就是三个角度：横滚（Roll）、俯仰（Pitch）、偏航（Yaw）。
我习惯这样记：
- Roll：绕X轴转，飞机像滚筒一样左右倾斜。
- Pitch：绕Y轴转，飞机抬头低头。
- Yaw：绕Z轴转，飞机左右转向。
但这里有个坑——万向锁（Gimbal Lock）。当俯仰角接近±90°时，横滚和偏航会“打架”，丢失一个自由度。我在做固定翼飞控时遇到过这个问题，飞机垂直爬升时姿态解算突然跳变，差点炸机。
避坑指南：我曾经在代码里直接用欧拉角做插值，结果在俯仰角90°附近出现了姿态跳变。后来改用四元数才解决。所以，欧拉角适合给人看，不适合给机器算。
4.3 旋转矩阵：数学上的“翻译官”
旋转矩阵，就是把一个向量从机体坐标系“翻译”到地理坐标系（或者反过来）的数学工具。
以绕Z轴旋转为例，旋转矩阵长这样：
Rz(ψ) = [cosψ -sinψ 0]
[sinψ cosψ 0]
[0 0 1]同理，绕X轴和Y轴的旋转矩阵也有类似形式。三个矩阵乘在一起，就得到了完整的旋转矩阵 R。
嗯，这里要注意：旋转矩阵是正交矩阵，它的逆等于它的转置。这个性质在代码里非常有用——求逆运算直接转置就行，省计算量。
个人经验：我在PX4源码里看到，他们用旋转矩阵做坐标系转换时，会先判断旋转顺序。不同的旋转顺序（比如ZYX还是XYZ）会导致不同的矩阵形式。我建议你写代码前先明确旋转顺序，否则调试时会很痛苦。
4.4 四元数：飞控的“秘密武器”
四元数，听起来很玄乎，其实就是一个四维复数：q = w + xi + yj + zk。
为什么飞控里都用它？三个原因：
- 无万向锁：怎么转都不会丢自由度。
- 计算效率高：乘法比矩阵快，适合嵌入式实时计算。
- 插值平滑：做姿态平滑时，四元数插值（Slerp）比欧拉角插值自然得多。
我记得第一次在STM32上跑四元数更新算法时，发现它比旋转矩阵快了将近30%。对于1000Hz的控制循环来说，这30%就是宝贵的余量。
核心公式：四元数更新方程（基于角速度）：
dq/dt = 0.5 * q * ω其中 ω 是机体角速度的四元数形式（0, ωx, ωy, ωz）。
4.5 知识体系总览
下面这张图，是我自己总结的坐标系与旋转关系图。你看一眼，就能明白它们之间的逻辑链条。
4.6 实战中的选择建议
说了这么多，到底该用哪个？我给出一个简单的判断标准：
| 场景 | 推荐工具 | 原因 |
|---|---|---|
| 人机交互（显示姿态） | 欧拉角 | 直观，一眼看懂 |
| 数学推导（论文、公式） | 旋转矩阵 | 线性代数工具丰富 |
| 嵌入式实时计算 | 四元数 | 效率高，无万向锁 |
| 姿态平滑插值 | 四元数 | Slerp插值自然 |
说白了，欧拉角是给人看的，旋转矩阵是给数学用的，四元数是给机器算的。三者之间可以互相转换，但各有各的适用场景。
一个小技巧：我在写飞控代码时，习惯在传感器驱动层用四元数做姿态解算，在日志输出层转成欧拉角方便调试。这样既保证了计算效率，又保留了可读性。