4、姿态表示方法对比：欧拉角、旋转矩阵、四元数的优缺点及在ArduPilot中的选择
做飞控的人，天天都在跟姿态打交道。但姿态怎么表示，这里头门道不少。
我刚入行那会儿，觉得欧拉角挺直观的——俯仰、横滚、偏航，多好理解。后来真上了项目，才发现事情没那么简单。你想想看，无人机在空中翻个筋斗，欧拉角直接就炸了。嗯，这就是我们今天要聊的核心问题。
核心观点：ArduPilot 内部统一使用四元数进行姿态解算和控制，仅在用户交互和日志记录时转换为欧拉角。这不是偶然，而是工程上的最优选择。
4.1 三种表示方法的基本原理
先简单回顾一下这三种方法是什么。我尽量不堆公式，但有些东西绕不开。
4.1.1 欧拉角
欧拉角用三个角度描述旋转：绕 Z 轴转（偏航 Yaw）、绕 Y 轴转（俯仰 Pitch）、绕 X 轴转（横滚 Roll）。顺序很重要，ArduPilot 用的是 Z-Y-X 顺序（即 321 顺序）。
- 优点：直观，人脑能理解。调试时看个角度变化，心里就有数。
- 缺点：有万向锁问题。当俯仰角接近 ±90° 时，横滚和偏航会耦合，丢失一个自由度。另外，做插值也不平滑。
4.1.2 旋转矩阵
一个 3×3 的矩阵，每一列代表机体坐标系在参考系中的投影。它没有奇点，但参数多。
- 优点：没有万向锁，可以唯一表示任意姿态。组合旋转就是矩阵乘法，很直接。
- 缺点：9 个参数，冗余。每次更新必须做正交化修正，否则误差会累积。计算量也大。
4.1.3 四元数
一个四维复数：q = w + xi + yj + zk，满足 w² + x² + y² + z² = 1。它用一根旋转轴和一个旋转角来描述姿态。
- 优点：无奇点，紧凑（4 个参数），插值平滑（球面线性插值），计算效率高。
- 缺点：不直观。你看着 q = [0.707, 0.707, 0, 0] 很难想象飞机是什么姿势。
4.2 关键特性对比
我把它们放在一张表里，方便你对照。这张表我改了好几版，是实际项目中踩坑总结出来的。
| 特性 | 欧拉角 | 旋转矩阵 | 四元数 |
|---|---|---|---|
| 参数数量 | 3 | 9 | 4 |
| 奇异性 | 有（万向锁） | 无 | 无 |
| 计算效率 | 高 | 低 | 高 |
| 插值平滑性 | 差 | 一般 | 好 |
| 直观性 | 好 | 差 | 差 |
| 内存占用 | 小 | 大 | 小 |
| 正交化需求 | 无 | 需要 | 需要（归一化） |
我的经验：曾经有个同事坚持用旋转矩阵做姿态控制，结果每次迭代都要做 Gram-Schmidt 正交化，算力吃紧不说，还容易引入数值误差。后来换成四元数，问题迎刃而解。
4.3 为什么 ArduPilot 选择四元数？
说白了，飞控是个实时系统，每秒要跑几百次姿态更新。选型时主要看三点：无奇点、计算快、插值稳。
- 无奇点：无人机做特技动作、穿越机翻滚时，欧拉角直接失效。四元数没有这个限制。
- 计算效率：四元数乘法比矩阵乘法快得多。在 STM32F4 这种级别的 MCU 上，差距很明显。
- 插值平滑：做姿态平滑、轨迹规划时，四元数的球面线性插值（SLERP）能保证旋转路径最短、最平滑。
ArduPilot 的代码里，核心姿态数据结构是 Quaternion 类。IMU 数据进来，先转成四元数增量，再更新全局姿态。只有在给 GCS 发数据、或者写日志时，才转成欧拉角给人看。
注意：四元数虽然好，但也不是银弹。它需要定期归一化，否则模长偏离 1 会导致姿态漂移。ArduPilot 在每个控制循环里都会做一次归一化检查。
4.4 知识体系结构图
下面这张图是我自己画的，把三种表示方法的关系和 ArduPilot 的选择逻辑串起来了。你看完应该能有个整体印象。
4.5 实际代码中的体现
我们来看看 ArduPilot 里是怎么用四元数的。这是 AP_AHRS 里的一段核心逻辑，我简化了一下：
// 从陀螺仪获取角速度 (rad/s)
Vector3f gyro = _ins.get_gyro();
// 构造四元数增量
Quaternion dq;
dq.from_axis_angle(gyro * dt);
// 更新姿态四元数
_quat *= dq;
// 归一化，防止漂移
_quat.normalize();
// 需要输出时，转成欧拉角
Vector3f euler = _quat.to_euler();
你看，核心就这几步。没有复杂的矩阵运算，没有奇点判断。简洁、高效、可靠。
避坑指南：我曾经在移植代码时忘了做归一化，结果飞机悬停时姿态慢慢漂移，差点炸机。从那以后，我每次更新四元数后第一件事就是检查模长。记住：normalize() 不是可选项，是必选项。
4.6 什么时候用欧拉角？
虽然内部用四元数，但欧拉角并没有被抛弃。在以下场景，它依然是首选：
- 地面站显示：飞行员要看的是俯仰 10°、横滚 5°，而不是一堆小数。
- 日志分析：出问题时，看角度变化曲线比看四元数直观得多。
- 简单控制：比如云台控制，角度范围有限，不存在万向锁问题。
所以 ArduPilot 的做法是：内部用四元数，接口用欧拉角。各取所长，这才是工程智慧。
4.7 小结
三种表示方法各有千秋。欧拉角直观但有奇点，旋转矩阵通用但冗余，四元数高效且无奇点。ArduPilot 选择四元数作为内部表示，是综合考虑了实时性、可靠性和全姿态能力的必然结果。
你如果自己写飞控，我建议也走这条路。别在欧拉角上死磕，除非你确定飞机永远不会翻过来。