2、姿态解算基础：四元数、欧拉角、旋转矩阵，到底用哪个？
刚接触姿态解算的工程师，十有八九会被这三个概念绕晕。四元数、欧拉角、旋转矩阵，它们都能描述“一个物体在三维空间里怎么转的”。但为什么搞出三种？到底该用哪个？
我当年刚入行时也纠结过这个问题。后来踩了几个坑，才慢慢摸清门道。今天咱们就把这事彻底聊透。
2.1 先搞清楚它们分别是什么
说白了，这三种东西是同一件事的三种数学表达。就像描述一个人的位置，你可以说“他在你东边10米”，也可以说“他坐标是(10,0,0)”，还可以画个地图标个点。意思一样，形式不同。
| 表达方式 | 核心思想 | 直观程度 | 典型应用场景 |
|---|---|---|---|
| 欧拉角 | 绕三个轴依次旋转（偏航-俯仰-横滚） | ★★★★★ 最直观 | 遥控器输入、地面站显示、日志分析 |
| 旋转矩阵 | 3×3矩阵，描述坐标系之间的映射 | ★★☆☆☆ 不直观 | 坐标变换、视觉SLAM、机械臂运动学 |
| 四元数 | 四个数(q0,q1,q2,q3)，描述绕一个轴转一个角度 | ★☆☆☆☆ 最抽象 | 飞控核心解算、IMU数据融合、姿态插值 |
一句话总结：欧拉角给人看，旋转矩阵给数学用，四元数给计算机算。
2.2 欧拉角：直观但有毒
欧拉角为什么受欢迎？因为它太符合人类直觉了。你想想看，你描述一架飞机怎么转的，是不是会说“先向左转30度，再抬头20度，最后侧倾10度”？这就是欧拉角——偏航(Yaw)、俯仰(Pitch)、横滚(Roll)。
但这里有个大坑，我称之为“欧拉角的三宗罪”。
- 万向锁（Gimbal Lock）：当俯仰角接近±90°时，偏航和横滚会“锁死”在一起，丢失一个自由度。我曾在一次固定翼试飞中遇到过，飞机垂直爬升时姿态突然乱跳，查了半天才发现是欧拉角在作祟。
- 角度跳变：从359°到0°，数值上跳了359°，但实际只转了1°。做PID控制时这种跳变会让积分项瞬间爆炸。
- 插值困难：在两个姿态之间做平滑过渡，用欧拉角插值出来的路径往往是扭曲的，不是最短路径。
避坑指南：我曾经在日志分析系统里直接用欧拉角做姿态差值计算，结果飞控日志里出现了大量“姿态突变”的误报。后来改用四元数做差值，问题才解决。所以——飞控核心算法里，千万别用欧拉角做运算。
2.3 旋转矩阵：数学上完美，工程上沉重
旋转矩阵是个3×3的矩阵，一共9个数。它最大的优点是：没有奇点，可以表示任意旋转，而且组合旋转就是矩阵乘法，非常优雅。
但问题也很明显：
- 9个数，冗余度太高。实际上只有3个自由度，多出来的6个数是约束条件。
- 必须保持正交性。每次更新后都要重新正交化，否则矩阵会“漂移”，变成非法的旋转矩阵。
- 计算量大。在嵌入式MCU上做矩阵乘法，尤其是做姿态预测和更新时，9个数的运算比4个数多一倍不止。
我个人习惯是：只在需要做坐标系变换时用旋转矩阵，比如把机体坐标系下的加速度转换到导航坐标系。其他时候，尽量用四元数。
2.4 四元数：飞控工程师的“真香”选择
四元数刚接触时确实反人类。一个标量加三个虚部，写成 q = q0 + q1·i + q2·j + q3·k。我第一次看到这个公式时，心想：这玩意儿能描述旋转？
但用久了你会发现，它太适合飞控了：
- 无奇点，可以表示任意姿态，不存在万向锁。
- 只有4个数，比旋转矩阵少5个，计算量小。
- 插值平滑，用球面线性插值(SLERP)可以在两个姿态之间走最短路径。
- 组合旋转简单，两个四元数相乘就等价于两次旋转的叠加。
嗯，这里要注意一点：四元数也需要归一化。不过比旋转矩阵的正交化简单多了，就是除以模长而已。
我的建议：飞控的IMU数据融合（比如Mahony或Madgwick算法）直接用四元数做。只在最后输出给地面站显示时，才把四元数转成欧拉角。这样既保证了计算效率，又方便人看。
2.5 它们之间的转换关系
实际工程中，这三种表达方式经常需要互相转换。我整理了一份常用转换公式，建议收藏。
四元数 → 欧拉角
roll = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1² + q2²))
pitch = asin(2*(q0*q2 - q3*q1))
yaw = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2² + q3²))欧拉角 → 四元数
q0 = cos(yaw/2)*cos(pitch/2)*cos(roll/2) + sin(yaw/2)*sin(pitch/2)*sin(roll/2)
q1 = cos(yaw/2)*cos(pitch/2)*sin(roll/2) - sin(yaw/2)*sin(pitch/2)*cos(roll/2)
q2 = cos(yaw/2)*sin(pitch/2)*cos(roll/2) + sin(yaw/2)*cos(pitch/2)*sin(roll/2)
q3 = sin(yaw/2)*cos(pitch/2)*cos(roll/2) - cos(yaw/2)*sin(pitch/2)*sin(roll/2)四元数 → 旋转矩阵
R = [
[1-2*(q2²+q3²), 2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2)],
[2*(q1*q2+q0*q3), 1-2*(q1²+q3²), 2*(q2*q3-q0*q1)],
[2*(q1*q3-q0*q2), 2*(q2*q3+q0*q1), 1-2*(q1²+q2²)]
]2.6 一张图看懂选择逻辑
下面这张图是我自己总结的，每次做新项目时都会拿出来看一眼。
2.7 实际工程中的选择策略
说了这么多理论，落地到地面站调参时，我的建议很明确：
- 飞控固件里：只用四元数。IMU数据融合、姿态控制、状态估计，全部用四元数运算。
- 地面站显示：把四元数转成欧拉角显示。用户只看偏航、俯仰、横滚，别把q0~q3扔给用户看。
- 参数调优时：如果发现姿态响应异常，先检查四元数是否归一化，再检查欧拉角转换是否有跳变。
- 日志分析：同时记录四元数和欧拉角。四元数用于回放计算，欧拉角用于人眼观察。
核心原则：计算用四元数，展示用欧拉角，变换用旋转矩阵。各取所长，别混着用。
好了，关于三种姿态表达方式的选择，今天就聊到这儿。记住一句话：没有最好的，只有最合适的。搞清楚了各自的优缺点，你就能在项目中做出正确的选择。