欧拉角与四元数：姿态的两种“语言”

做飞控调参，绕不开姿态表示。说白了，你得告诉飞控“现在飞机头朝哪”。

最直观的方式是欧拉角——俯仰、横滚、偏航，三个角度一摆，姿态就出来了。但我在实际项目中吃过它的亏，有一次调试四轴，飞机刚做半个筋斗，IMU 数据直接炸了，姿态解算疯狂跳变。后来才发现，是碰到了万向锁。

欧拉角的定义与万向锁问题

欧拉角用三个旋转角度描述刚体姿态。常见的顺序是 Z-Y-X（偏航→俯仰→横滚）。

• 偏航（Yaw）：绕机体 Z 轴旋转，范围 ±180°
• 俯仰（Pitch）：绕机体 Y 轴旋转，范围 ±90°
• 横滚（Roll）：绕机体 X 轴旋转，范围 ±180°

听起来很清晰对吧？但问题出在俯仰角接近 ±90° 时。

我记得有一次做固定翼的自主降落，飞机大角度俯冲时，欧拉角直接跳变，导致控制量反号。嗯，从那以后，我只要涉及大角度机动，就坚决不用纯欧拉角做内部解算。

四元数的数学基础

四元数本质上是一个超复数，由实部和三个虚部组成：

q = w + xi + yj + zk

其中 i² = j² = k² = ijk = -1。在飞控里，我们通常用单位四元数表示旋转：

q = [w, x, y, z]ᵀ，且 w² + x² + y² + z² = 1

为什么用四元数？说白了，它没有奇点，计算也快。我习惯在飞控内部全程用四元数，只在最后输出给地面站时才转成欧拉角。

你想想看，一个四元数就能表示任意轴任意角度的旋转，而且连续可微——这对控制算法来说太重要了。

四元数与欧拉角的转换关系

实际调参时，你看到的还是欧拉角。但飞控内部跑的是四元数。所以转换关系必须搞明白。

四元数 → 欧拉角（Z-Y-X 顺序）

roll  = atan2(2(wx + yz), 1 - 2(x² + y²))
pitch = asin(2(wy - zx))
yaw   = atan2(2(wz + xy), 1 - 2(y² + z²))

注意这里的 atan2 和 asin。我在项目中遇到过一个问题：pitch 用 asin 时，输入超过 ±1 会导致 NaN。所以一定要做钳位：

float sinp = 2 * (q.w * q.y - q.z * q.x);
if (sinp > 1.0f) sinp = 1.0f;
if (sinp < -1.0f) sinp = -1.0f;
pitch = asin(sinp);

欧拉角 → 四元数

// 半角计算
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);

// 组合四元数
q.w = cr * cp * cy + sr * sp * sy;
q.x = sr * cp * cy - cr * sp * sy;
q.y = cr * sp * cy + sr * cp * sy;
q.z = cr * cp * sy - sr * sp * cy;

知识体系图：姿态表示的核心逻辑

避坑指南

• 我曾经在四元数归一化上吃过亏。连续多次乘法后，四元数会偏离单位球面，导致姿态漂移。一定要在每个控制周期做一次归一化：q /= norm(q)。
• 我曾经在欧拉角转四元数时搞错旋转顺序。不同飞控可能用 Z-Y-X 或 Z-X-Y，一定要确认文档。
• 注意：地面站显示的欧拉角通常已经过滤波，但内部解算用的四元数必须是原始融合结果，不要用显示值做控制。