第2章 姿态控制理论基础回顾：四元数、欧拉角、旋转矩阵
姿态控制，说白了就是回答三个问题：“我现在朝哪？我想朝哪？怎么转过去？”。这三个问题背后，就是四元数、欧拉角和旋转矩阵在打架。
我刚开始做飞控时，觉得这些数学工具都差不多。直到有一次在PX4上调试一个高速旋翼机，欧拉角直接给我来了个“万向锁”，飞机在天上翻了个跟头……嗯，从那以后我再也不敢轻视这些基础概念了。
2.1 三种姿态表示法：谁更适合飞控？
先看一张总览图，把三者的关系理清楚。
2.2 欧拉角：直观但危险
欧拉角用三个角度描述旋转：滚转(Roll)、俯仰(Pitch)、偏航(Yaw)。你想想看，这多直观——飞机左倾多少度，抬头多少度，机头朝哪。
但问题来了：万向锁(Gimbal Lock)。当俯仰角接近 ±90° 时，滚转和偏航会“耦合”在一起，丢失一个自由度。我在项目中遇到过一架固定翼在做筋斗动作时，欧拉角直接跳变，姿态估计瞬间崩了。
欧拉角的另一个问题是非唯一性。同样的姿态，可以用多组欧拉角表示。比如 (180°, 0°, 0°) 和 (0°, 180°, 180°) 其实是一样的。这在控制律里会造成歧义。
2.3 旋转矩阵：数学上完美，计算上沉重
旋转矩阵是一个 3×3 的正交矩阵，行列式为 +1。它没有奇点，可以唯一表示任意姿态。
// 绕Z轴旋转ψ角的旋转矩阵示例
R_z(ψ) = [cosψ -sinψ 0]
[sinψ cosψ 0]
[0 0 1]但旋转矩阵有9个元素，每次更新都要做大量三角函数运算。在嵌入式飞控上，这太奢侈了。我记得在STM32F4上做过测试，一次旋转矩阵乘法比四元数乘法慢了将近3倍。
所以，旋转矩阵在飞控里更多是作为中间桥梁存在——比如从四元数转成旋转矩阵，再提取欧拉角给地面站显示。
2.4 四元数：飞控的“真命天子”
四元数由1个实部和3个虚部组成：q = w + xi + yj + zk，且满足 w² + x² + y² + z² = 1。
为什么飞控都用它？三个理由：
- 无奇点：可以表示任意姿态，不存在万向锁
- 计算快：只有4个元素，乘法比矩阵快得多
- 插值平滑：球面线性插值(SLERP)可以生成平滑的旋转路径
2.5 PX4 vs ArduPilot：实现差异对比
两者都使用四元数作为核心姿态表示，但在具体实现上有一些差异。我整理了一张对比表：
| 对比项 | PX4 | ArduPilot |
|---|---|---|
| 核心库 | matrix 库 (C++) | AP_Math 库 (C++) |
| 四元数更新 | 使用角速度积分 + 互补滤波 | 使用角速度积分 + EKF |
| 欧拉角提取 | 从四元数直接转，避免万向锁 | 从四元数转旋转矩阵再提取 |
| 控制律输入 | 四元数误差 → 角速度指令 | 四元数误差 → 旋转矩阵 → 角速度指令 |
| 地面站显示 | 内部四元数，输出时转欧拉角 | 内部四元数，输出时转欧拉角 |
你看，虽然最终都转成欧拉角给人看，但内部处理路径不同。PX4更直接，ArduPilot多了一层旋转矩阵。我个人觉得PX4的方式更高效，但ArduPilot的兼容性更好——毕竟旋转矩阵是“通用语言”。
2.6 代码层面的差异
来看看两者在代码上的具体实现差异：
// PX4: 从四元数直接提取欧拉角
matrix::Eulerf euler = matrix::Quatf(q);
// ArduPilot: 从四元数转旋转矩阵再提取欧拉角
Quaternion q;
q.from_axis_angle(axis, angle);
Matrix3f rot_matrix = q.to_rotation_matrix();
float roll, pitch, yaw;
rot_matrix.to_euler(&roll, &pitch, &yaw);这里有个细节：PX4的 matrix::Eulerf 构造函数内部其实也做了四元数到旋转矩阵的转换，但封装得更好。ArduPilot则把每一步都暴露出来，方便开发者做中间处理。
q.to_euler() 方法，结果发现它返回的欧拉角顺序是 (roll, pitch, yaw)，而PX4默认是 (roll, pitch, yaw) 但内部顺序不同。如果你在两个平台间移植代码，一定要检查欧拉角的旋转顺序！
2.7 仿真中的选择建议
在SITL仿真中，我建议你：
- 控制律内部：全部用四元数，不要碰欧拉角
- 日志记录：同时记录四元数和欧拉角，方便事后分析
- 可视化：用欧拉角显示，但要注意万向锁边界
- 参数调参：如果发现姿态响应异常，先检查四元数是否归一化
嗯，这里要特别强调：四元数归一化是飞控的“生命线”。如果四元数模长偏离1，姿态估计就会漂移。我在PX4的代码里看到过，每次姿态更新后都会强制归一化——这个习惯一定要养成。