四元数与欧拉角转换：从四元数到欧拉角，从欧拉角到四元数

各位同学，欢迎来到第四讲。今天咱们聊聊四元数和欧拉角之间的“翻译”工作。说白了，就是怎么把一种姿态表示法，换成另一种。

我刚开始做惯性导航那会儿，也纠结过：明明欧拉角那么直观，为什么非要用四元数？后来在项目里摔过几次跟头，才明白——嗯，各有各的命门。

为什么需要互相转换？

先说说我的理解。欧拉角（俯仰、横滚、航向）是人类友好的。你一看“俯仰30度”，脑子里就有画面。但它在算法里有两个硬伤：

• 万向锁：当俯仰角接近±90°时，横滚和航向会“耦合”在一起，丢失一个自由度。我在做无人机飞控时遇到过，飞机垂直爬升时姿态突然乱跳，查了半天才发现是万向锁在作怪。
• 三角函数运算：每次更新都要算sin、cos，嵌入式芯片扛不住啊。

四元数呢？它没有万向锁，运算只有乘法和加法，效率高。但缺点也很明显——不直观。你看着四元数(q0, q1, q2, q3) = (0.707, 0.707, 0, 0)，能想象出飞机是什么姿势吗？反正我不能。

所以实际工程中，我们通常这样分工：

• 算法内部：用四元数做姿态更新、互补滤波
• 对外输出：转成欧拉角给用户看、给控制律用

这就是转换的必要性。

从四元数到欧拉角

先讲正向转换。假设你有一个归一化的四元数 q = [q0, q1, q2, q3]，想得到欧拉角（通常用Z-Y-X顺序，即航向-俯仰-横滚）。

公式长这样：

// 俯仰角 (Pitch)  - 绕Y轴
pitch = asin( 2 * (q0*q2 - q3*q1) )

// 横滚角 (Roll)   - 绕X轴
roll  = atan2( 2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2) )

// 航向角 (Yaw)    - 绕Z轴
yaw   = atan2( 2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3) )

注意，这里的 atan2 是四象限反正切，能自动判断角度在哪个象限。我习惯用C语言的 atan2f，省一个字节是一个字节。

从欧拉角到四元数

反过来，给你欧拉角（Yaw, Pitch, Roll），怎么合成四元数？

这里要用到半角公式。为什么是半角？因为四元数旋转的本质是“绕轴转两倍角度”，所以输入角度要除以2。

// 假设角度单位是弧度
float cy = cos(yaw * 0.5f);
float sy = sin(yaw * 0.5f);
float cp = cos(pitch * 0.5f);
float sp = sin(pitch * 0.5f);
float cr = cos(roll * 0.5f);
float sr = sin(roll * 0.5f);

q0 = cr * cp * cy + sr * sp * sy;
q1 = sr * cp * cy - cr * sp * sy;
q2 = cr * sp * cy + sr * cp * sy;
q3 = cr * cp * sy - sr * sp * cy;

你想想看，这个公式其实就是三个旋转矩阵的乘积，只不过用四元数乘法表达出来了。我个人习惯在写完这段代码后，立即调用归一化函数，防止累积误差。

转换中的常见陷阱

我整理了一个表格，把容易踩的坑列出来：

陷阱	现象	解决方法
四元数未归一化	欧拉角输出抖动、漂移	每次转换前检查模长，接近1才用
欧拉角顺序搞错	姿态完全不对	明确约定Z-Y-X顺序，并在注释里写清楚
万向锁附近数值不稳定	横滚和航向跳变	检测俯仰角接近±90°时，固定航向或改用其他表示
角度单位混淆	输出值差57.3倍	统一用弧度，只在最后输出时转度

我记得有一次，同事把角度单位搞混了，飞控输出的航向一直在转圈圈。排查了两天才发现——嗯，从那以后我就在代码里加了个 #define DEG2RAD 宏，强制统一。

知识体系图

下面这张图，是我自己画的一个转换逻辑图，帮你理清思路：

实际项目中的选择建议

说了这么多，到底什么时候用哪种？我个人的经验是：

• 如果你在做姿态解算（互补滤波、卡尔曼滤波）：全程用四元数。只在最后输出时转一次欧拉角。
• 如果你在做控制律（PID）：看情况。有些控制器直接吃四元数误差，有些需要欧拉角。我建议统一用四元数做误差计算，避免欧拉角的非线性问题。
• 如果你在写上位机显示：必须转欧拉角。没人看得懂四元数波形。

好了，这一章就到这里。记住，转换本身不难，难的是理解为什么转、什么时候转。下次你在代码里看到 atan2 和 asin，希望你能想起今天聊的这些坑。

---

公众号:蓝海资料掘金营，微信deep3321