姿态解算基础：欧拉角与四元数

做飞控这么多年，我始终觉得姿态解算是整个系统的灵魂。说白了，就是让飞控知道「我现在是头朝上还是脚朝下」。没有准确的姿态，什么PID控制、航线规划都是空中楼阁。

今天咱们就从最基础的两个数学工具说起——欧拉角和四元数。嗯，这两个东西，你搞懂了，姿态解算就入门了一半。

欧拉角：直观但容易「翻车」

欧拉角的概念其实很简单。就是三个角度：横滚角（Roll）、俯仰角（Pitch）、偏航角（Yaw）。你想想看，一个直升机在空中，无非就是左右倾斜、前后低头抬头、以及机头朝向。

我在项目中遇到过不少新手，上来就用欧拉角做姿态控制。直观嘛，好理解。但问题很快就来了——万向锁。

所以，欧拉角适合做人机交互的显示，比如地面站上显示飞机姿态。但做姿态解算的核心运算，我建议你换四元数。

四元数：数学上的「防抖」方案

四元数是个什么东西？说白了就是一个四维的复数扩展。形式是：

q = w + xi + yj + zk

其中 w 是实部，x、y、z 是虚部。约束条件是 w² + x² + y² + z² = 1。

为什么用四元数？三个原因：

• 无万向锁——随便你怎么旋转，不会丢自由度
• 计算量小——只有乘法和加法，没有三角函数
• 插值平滑——做姿态平滑过渡时，效果比欧拉角好太多

我个人习惯，在飞控代码里全程用四元数做运算。只在最后输出给地面站时，才转成欧拉角显示。

欧拉角 ↔ 四元数 转换

这里给出一段我常用的转换代码。注意，这是C语言版本，适合嵌入式环境：

// 欧拉角转四元数
void euler_to_quat(float roll, float pitch, float yaw, float *q) {
    float cr = cosf(roll * 0.5f);
    float sr = sinf(roll * 0.5f);
    float cp = cosf(pitch * 0.5f);
    float sp = sinf(pitch * 0.5f);
    float cy = cosf(yaw * 0.5f);
    float sy = sinf(yaw * 0.5f);

    q[0] = cr * cp * cy + sr * sp * sy;  // w
    q[1] = sr * cp * cy - cr * sp * sy;  // x
    q[2] = cr * sp * cy + sr * cp * sy;  // y
    q[3] = cr * cp * sy - sr * sp * cy;  // z
}

// 四元数转欧拉角
void quat_to_euler(float *q, float *roll, float *pitch, float *yaw) {
    *roll  = atan2f(2.0f * (q[0]*q[1] + q[2]*q[3]), 
                    1.0f - 2.0f * (q[1]*q[1] + q[2]*q[2]));
    *pitch = asinf(2.0f * (q[0]*q[2] - q[3]*q[1]));
    *yaw   = atan2f(2.0f * (q[0]*q[3] + q[1]*q[2]), 
                    1.0f - 2.0f * (q[2]*q[2] + q[3]*q[3]));
}

---

Mahony互补滤波算法原理

好了，数学工具准备好了，接下来就是怎么用传感器数据来算姿态。这里我重点讲 Mahony 互补滤波。

为什么叫「互补」？因为我们要融合两种传感器的数据：

• 陀螺仪——动态响应快，但有积分漂移
• 加速度计/磁力计——没有漂移，但噪声大、动态响应慢

说白了，就是让陀螺仪负责「快」的部分，加速度计负责「准」的部分。两者互补，得到既快又准的姿态。

核心思想：PI控制器修正陀螺仪偏差

Mahony 算法的核心，其实就是一个 PI 控制器。我画个图你就明白了：

流程其实就三步：

1. 用陀螺仪积分——得到当前姿态的预测值
2. 用加速度计/磁力计算误差——把预测的重力方向跟实际测量的重力方向做叉积，得到误差
3. PI控制器修正——用误差去修正陀螺仪的偏置，同时修正姿态

这里的关键参数是两个：Kp 和 Ki。Kp 决定了加速度计修正的强度，Ki 决定了陀螺仪零偏的收敛速度。

---

Madgwick算法对比

说到 Mahony，就不得不提另一个经典算法——Madgwick。这两个算法经常被放在一起比较。

对比项	Mahony	Madgwick
核心方法	PI控制器修正	梯度下降法
计算量	小（约 50-80 次浮点运算/步）	大（约 200-300 次浮点运算/步）
收敛速度	中等	快（梯度下降步长大）
高频抖动抑制	好（PI有低通特性）	一般（梯度下降对噪声敏感）
参数调优	2个参数（Kp, Ki）	1个参数（β，梯度步长）
适用场景	嵌入式实时系统	需要快速收敛的场景

从表里能看出来，Madgwick 的优势是参数少、收敛快。但代价是计算量大，而且对传感器噪声更敏感。

我记得有一次在 STM32F103 上跑 Madgwick，姿态更新频率只能跑到 200Hz。换成 Mahony 后，轻松跑到 500Hz。对于无人直升机这种需要高更新率的场景，计算量就是硬指标。

---

我在项目中为何偏爱Mahony

说了这么多，你可能会问：既然 Madgwick 收敛更快，为什么我偏偏喜欢 Mahony？

原因有三：

1. 计算量小，适合嵌入式

无人直升机的飞控，CPU 资源很宝贵。你要跑控制律、通信、日志、故障检测……留给姿态解算的时间窗口就那么几毫秒。Mahony 的计算量只有 Madgwick 的 1/3 到 1/4，省下来的算力可以干别的事。

2. 参数物理意义明确

Kp 和 Ki 就是 PI 控制器的参数，搞控制的工程师一看就懂。调起来心里有数。Madgwick 的 β 参数，说实话，我刚开始用的时候完全靠试。β 大了抖，小了飘，很难找到那个「甜点」。

3. 工程稳定性好

我在多个项目里验证过，Mahony 在传感器噪声大、采样率不稳定的情况下，表现更鲁棒。有一次飞控的加速度计受到振动干扰，Madgwick 直接发散，Mahony 只是抖了一下就稳住了。

嗯，这就是我为什么一直用 Mahony 的原因。不是因为它最好，而是因为它最适合工程落地。做飞控这么多年，我越来越明白一个道理：在嵌入式世界里，稳定比先进更重要。

---