姿态控制理论基础：欧拉角与旋转矩阵、四元数基础、角速度与姿态变化率的关系、坐标系定义（NED/FRD）
各位同学，欢迎来到第二章。
说实话，姿态控制是整个飞控里最绕不开、也最容易让人栽跟头的部分。我见过不少新手，上来就调PID参数，结果飞机在天上乱晃，根本不知道问题出在哪。其实，很多问题根源不在PID，而在你对姿态的数学表达理解得不够透。
这一章，我们就来把这些基础概念彻底掰扯清楚。你想想看，如果连飞机当前是“头朝上”还是“肚皮朝天”都描述不清楚，那还谈什么控制？
坐标系定义：NED 与 FRD
搞飞控，第一件事就是定规矩——坐标系。没有统一的坐标系，你跟我说的“向前”，我理解的可能是“向左”，那就乱套了。
NED坐标系（北东地），是咱们的“世界坐标系”。
- X轴：指向地理北（North）
- Y轴：指向地理东（East）
- Z轴：指向地心（Down），也就是垂直向下
这个坐标系是固定的，不随飞机转动而转动。我们用它来描述飞机的位置、速度，以及地磁、重力等外部环境量。
FRD坐标系（前右向下），是飞机的“机体坐标系”。
- X轴：指向机头方向（Forward）
- Y轴：指向机身右侧（Right）
- Z轴：指向机身下方（Down）
这个坐标系是绑在飞机上的。你坐在驾驶舱里，你看到的“前方”就是FRD的X轴。IMU（惯性测量单元）测量的角速度和加速度，原始数据就是在这个坐标系下的。
核心要点：控制的目标，就是让飞机的FRD坐标系，按照我们期望的方式，相对于NED坐标系运动。
我个人习惯，在写代码时，所有中间计算尽量在机体坐标系（FRD）下完成，只在最后输出控制量时，才转换到NED。这样能减少很多不必要的坐标变换，逻辑也更清晰。
欧拉角与旋转矩阵
好了，现在有了两个坐标系。怎么描述飞机相对于地面的姿态呢？最直观的方法就是欧拉角。
欧拉角用三个角度来描述旋转：横滚角（Roll, φ）、俯仰角（Pitch, θ）、偏航角（Yaw, ψ）。
- 横滚角（Roll）：绕机体X轴旋转。说白了就是飞机“侧倾”的角度。右翼向下为正。
- 俯仰角（Pitch）：绕机体Y轴旋转。就是飞机“抬头”或“低头”。抬头为正。
- 偏航角（Yaw）：绕机体Z轴旋转。就是飞机“转向”。机头右偏为正。
但是，欧拉角有个著名的“死穴”——万向锁（Gimbal Lock）。当俯仰角达到±90°时，横滚和偏航会失去一个自由度，导致姿态解算出错。我在做固定翼飞控时就踩过这个坑，飞机垂直爬升时，姿态突然跳变，差点炸机。
为了在数学上严谨地描述旋转，我们引入了旋转矩阵。它是一个3x3的正交矩阵，可以将一个向量从一个坐标系旋转到另一个坐标系。
从机体坐标系（FRD）到世界坐标系（NED）的旋转矩阵 R 是这样的（按Z-Y-X顺序旋转）：
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]]
嗯，这里要注意，矩阵乘法不满足交换律，旋转顺序非常重要。PX4默认使用的是Z-Y-X顺序，也就是先偏航，再俯仰，最后横滚。
四元数基础
既然欧拉角有万向锁，旋转矩阵计算又比较笨重（9个参数），那有没有更好的办法？有，就是四元数。
四元数可以理解为“复数在三维空间的推广”。它用一个实部和三个虚部来表示旋转：
q = w + xi + yj + zk
其中，w是实部，x, y, z是虚部。
且满足：i² = j² = k² = ijk = -1
一个单位四元数（模长为1）可以表示三维空间中的任意旋转。相比于欧拉角，它没有万向锁问题；相比于旋转矩阵，它只有4个参数，计算效率更高，插值也更平滑。
在PX4中，姿态估计（EKF2）和姿态控制的核心，用的都是四元数。我个人建议，如果你要深入修改飞控代码，一定要习惯用四元数思考。刚开始可能觉得抽象，但用熟了你会发现它真的很优雅。
小技巧：四元数转欧拉角有现成公式，但要注意奇点。在PX4源码的 matrix/Quaternion.hpp 里，to_euler() 函数已经帮你处理好了边界情况，直接调用就行。
角速度与姿态变化率的关系
这是很多人容易混淆的地方。你遥控器打杆，给的是角速度期望值（比如：每秒转30度）。但姿态控制器最终输出的是力矩，中间需要经过一个关键环节——运动学模型。
简单说，角速度（p, q, r）是机体坐标系下绕各轴的旋转速度，由陀螺仪直接测量得到。而姿态变化率（φ̇, θ̇, ψ̇）是欧拉角随时间的变化快慢。
它们之间不是简单的相等关系，而是通过一个转换矩阵联系起来的：
[φ̇] [1, sin(φ)tan(θ), cos(φ)tan(θ)] [p]
[θ̇] = [0, cos(φ), -sin(φ) ] [q]
[ψ̇] [0, sin(φ)/cos(θ), cos(φ)/cos(θ)] [r]
看到没？当俯仰角 θ 接近 ±90° 时，tan(θ) 和 1/cos(θ) 会趋于无穷大。这就是万向锁在数学上的体现。所以，在PX4的内环控制中，我们通常直接控制角速度（p, q, r），而不是欧拉角变化率，就是为了避开这个奇点。
我曾经在调试一个高速穿越机时，发现偏航响应总是不对。查了半天，最后发现是角速度到姿态变化率的转换矩阵用错了顺序。那一次让我深刻体会到，数学基础不牢，调参就是瞎蒙。
知识体系总览
为了让你更直观地理解这些概念之间的关系，我画了一张图：
重要提醒：千万不要试图在控制回路里直接用欧拉角做微分来得到角速度。我在项目里见过有人这么干，结果高频噪声被无限放大，电机直接啸叫。正确的做法是用陀螺仪测量角速度，然后用四元数运动学方程去更新姿态。
本章小结
这一章的内容，说白了就是给姿态控制打地基。我们搞清楚了：
- 飞机在NED世界里飞，但传感器在FRD坐标系下工作。
- 描述姿态有三种工具：欧拉角（直观但有缺陷）、旋转矩阵（精确但笨重）、四元数（高效且无奇点）。
- 角速度（陀螺仪测的）和姿态变化率（欧拉角的导数）不是一回事，中间有个转换矩阵，而且这个矩阵在俯仰90°时会崩溃。