数学基础（三）：欧拉角定义与四元数互转
各位同学，今天我们聊一个绕不开的话题——欧拉角。说实话，我刚入行那会儿，觉得欧拉角特别直观，俯仰、横滚、偏航，一听就懂。但后来在项目里栽过跟头，才明白这东西没那么简单。
一、欧拉角的定义
欧拉角用三个角度来描述刚体在三维空间中的姿态。说白了，就是飞机绕三个轴转了多少度。
- 俯仰角（Pitch，θ）：绕 Y 轴旋转。抬头为正，低头为负。范围通常 -90° ~ +90°。
- 横滚角（Roll，φ）：绕 X 轴旋转。右倾为正，左倾为负。范围 -180° ~ +180°。
- 偏航角（Yaw，ψ）：绕 Z 轴旋转。右转为正，左转为负。范围 -180° ~ +180°。
我习惯用 Z-Y-X 的旋转顺序，也就是先偏航、再俯仰、最后横滚。这个顺序在无人机里很常见，你想想看，飞机先调好机头方向，再抬头低头，最后修正左右倾斜——很符合直觉。
重要提醒：旋转顺序不同，最终姿态完全不同。同一个欧拉角值，用 Z-Y-X 和 Z-X-Y 算出来是两个姿态。我在项目里吃过这个亏，调试时怎么都对不上，最后发现是旋转顺序搞反了。
二、万向锁问题
万向锁，英文叫 Gimbal Lock。很多同学第一次听到这个名字，觉得很高深。其实说白了，就是当俯仰角达到 ±90° 时，横滚和偏航会「锁死」在一起，失去一个自由度。
为什么会这样？
你想象一下：飞机垂直向上抬头，机头指向天顶。这时候，你再怎么转横滚和偏航，飞机看起来都是在做同一个动作——绕垂直轴旋转。两个轴的功能重合了。
我记得有一次做飞行仿真，飞机做大机动动作时突然失控。查了半天，发现就是万向锁导致的。欧拉角在俯仰接近 90° 时，姿态解算会出奇异值，矩阵求逆直接炸了。
避坑指南：我曾经在飞控代码里用欧拉角做姿态插值，结果飞机在俯仰 85° 时出现剧烈抖动。后来改用四元数插值，问题立刻消失。所以，如果你要做大角度机动，千万别用欧拉角做插值或微分。
万向锁的数学本质是：旋转矩阵在俯仰角为 ±90° 时，秩从 3 降为 2。也就是说，你无法从矩阵中唯一解出横滚和偏航角。这就是为什么现代飞控都改用四元数。
三、欧拉角与四元数互转
既然欧拉角有万向锁问题，那我们就得学会怎么把它转成四元数。反过来，有时候也需要从四元数还原出欧拉角，方便人看。
先看欧拉角转四元数。假设旋转顺序是 Z-Y-X，也就是先偏航 ψ、再俯仰 θ、最后横滚 φ。那么对应的四元数 q 可以这样算：
// 欧拉角转四元数（Z-Y-X 顺序）
// 输入：roll(φ), pitch(θ), yaw(ψ) 单位：弧度
// 输出：四元数 q = [w, x, y, z]
double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);
double w = cr * cp * cy + sr * sp * sy;
double x = sr * cp * cy - cr * sp * sy;
double y = cr * sp * cy + sr * cp * sy;
double z = cr * cp * sy - sr * sp * cy;这段代码我用了很多年，从来没出过问题。注意每个角度都要先除以 2，因为四元数用的是半角公式。
再看四元数转欧拉角。这个稍微麻烦一点，因为要处理万向锁的边界情况。
// 四元数转欧拉角（Z-Y-X 顺序）
// 输入：四元数 q = [w, x, y, z]
// 输出：roll(φ), pitch(θ), yaw(ψ) 单位：弧度
// 计算俯仰角
double sinp = 2.0 * (w * y - z * x);
double pitch;
if (fabs(sinp) >= 1.0) {
// 万向锁！俯仰角为 ±90°
pitch = copysign(M_PI / 2.0, sinp);
} else {
pitch = asin(sinp);
}
// 计算横滚角
double sinr_cosp = 2.0 * (w * x + y * z);
double cosr_cosp = 1.0 - 2.0 * (x * x + y * y);
double roll = atan2(sinr_cosp, cosr_cosp);
// 计算偏航角
double siny_cosp = 2.0 * (w * z + x * y);
double cosy_cosp = 1.0 - 2.0 * (y * y + z * z);
double yaw = atan2(siny_cosp, cosy_cosp);注意看，当俯仰角接近 ±90° 时，sinp 会接近 ±1。这时候 asin 函数会返回 ±π/2，而横滚和偏航角其实已经不可区分了。我一般会在这种情况下输出一个警告，提醒用户当前处于万向锁区域。
个人经验：在实际飞控中，我很少直接从四元数还原欧拉角给控制律用。一般只在日志记录或地面站显示时，才转成欧拉角给人看。控制律内部全程用四元数，避免任何奇异问题。
四、总结一下
欧拉角直观，但有限制。四元数无奇异，但不够直观。两者互转是飞控开发的基本功。
我建议你记住以下几点：
- 欧拉角适合人看，四元数适合机器算
- 大角度机动时，务必用四元数
- 互转代码要测试边界情况，特别是俯仰 ±90°
- 旋转顺序必须统一，我推荐 Z-Y-X
下一节我们会讲四元数的微分方程和姿态更新，到时候你会看到，四元数在计算效率上也有优势。嗯，今天就到这里，有问题随时问我。
公众号:蓝海资料掘金营，微信deep3321