第1章：姿态解算算法——欧拉角与四元数、互补滤波原理、Mahony滤波实现、Madgwick滤波实现
1.1 姿态解算到底在解什么？
各位同学，我们先想一个问题：一架eVTOL在空中，你怎么知道它现在是“抬头”还是“低头”？是“左倾”还是“右倾”？
说白了，姿态解算就是回答这个问题。它把IMU（惯性测量单元）里那些乱七八糟的加速度、角速度数据，变成我们人能理解的“角度”。
我个人习惯把姿态解算比作“翻译官”。传感器说的是“原始数据”，飞控需要的是“姿态角”。中间这层翻译，就是算法干的活。
我在项目中遇到过最头疼的事：传感器数据明明没问题，但解算出来的姿态就是抖得厉害。后来发现，不是算法不行，是滤波参数没调好。嗯，这个坑后面我会细说。
1.2 欧拉角：最直观，但有个致命缺陷
欧拉角大家应该不陌生。三个角度：横滚（Roll）、俯仰（Pitch）、偏航（Yaw）。
优点很明显——直观。你一看Roll=10°，就知道飞机向右倾斜了10度。
但问题来了。你想想看，如果Pitch先转到90°，再转Roll会发生什么？
这就是著名的“万向锁”问题。当Pitch接近±90°时，Roll和Yaw会变得无法区分。说白了，你丢失了一个自由度。
对于eVTOL来说，这很要命。 虽然大部分飞行不会到90°俯仰，但万一遇到极端机动或者传感器噪声，欧拉角就直接“崩”了。
1.3 四元数：数学上优雅，工程上实用
四元数是什么？简单说，它是一个四维复数：q = w + xi + yj + zk。
别被数学吓到。你只需要记住：四元数没有奇点，计算效率高，适合嵌入式实时系统。
我在项目中用过欧拉角，也用过四元数。说实话，自从切换到四元数后，再也没为“万向锁”操过心。而且四元数的更新只需要几个乘法和加法，在STM32F4上跑起来毫无压力。
四元数更新公式（核心）：
// 四元数微分方程
q_dot = 0.5 * q * ω
// 其中 ω 是角速度四元数形式 (0, ωx, ωy, ωz)
// 实际代码中写成矩阵乘法
但四元数有个小毛病——它不直观。你看到q=[0.707, 0.707, 0, 0]，能立刻想到飞机是什么姿态吗？不能。所以实际工程中，我们通常把四元数转成欧拉角给地面站显示，内部计算全用四元数。
1.4 互补滤波原理：简单但有效
为什么要滤波？因为IMU数据有噪声。
加速度计：低频准，高频噪声大。
陀螺仪：高频准，但积分会漂移。
互补滤波的思路很简单：把两者的优点结合起来。
公式就一行：
角度 = α * (角度 + 陀螺仪积分) + (1-α) * 加速度计角度其中α是权重系数，通常取0.98左右。α越大，越信任陀螺仪；α越小，越信任加速度计。
我曾经在调试时把α设成0.999，结果姿态漂移得厉害。后来改成0.98，稳如老狗。这个参数，说白了就是靠试。
1.5 Mahony滤波：互补滤波的升级版
Mahony滤波是互补滤波的一种改进。它引入了一个PI控制器来修正陀螺仪的偏差。
核心思想：用加速度计和磁力计的测量值，计算出一个“误差”，然后用这个误差去修正陀螺仪的角速度。
代码实现（核心部分）：
// 1. 计算误差
error = cross(测量方向, 估计方向)
// 2. PI控制器修正角速度
gyro_bias += Ki * error * dt
gyro_corrected = gyro_raw + Kp * error + gyro_bias
// 3. 更新四元数
q_dot = 0.5 * q * gyro_corrected
我在项目中对比过：Mahony滤波比纯互补滤波的抗干扰能力强很多。特别是在有振动的情况下，Mahony依然能保持稳定。
但要注意：Ki参数不能太大，否则会引入低频振荡。我一般从Ki=0.001开始调，慢慢往上加。
1.6 Madgwick滤波：精度更高，计算量稍大
Madgwick滤波是另一个经典算法。它用梯度下降法来优化姿态估计。
和Mahony的区别：
- Mahony：用PI控制器修正
- Madgwick：用梯度下降法迭代优化
Madgwick的精度通常比Mahony高一些，但计算量也大一些。不过对于现在的MCU来说，这点计算量不算什么。
核心公式：
// 梯度下降
q_new = q - β * ∇f / ||∇f||
// 其中β是步长，∇f是误差函数的梯度
// 然后和陀螺仪积分结果融合
q_final = (1-γ) * q_gyro + γ * q_gradient
我记得第一次用Madgwick时，发现它的收敛速度比Mahony快。特别是在初始化阶段，Madgwick能很快找到正确的姿态。
但有个坑：β参数很敏感。β太大，姿态会抖动；β太小，收敛慢。我一般从β=0.1开始试。
1.7 三种算法对比
| 算法 | 精度 | 计算量 | 适用场景 |
|---|---|---|---|
| 互补滤波 | 低 | 极低 | 简单飞行器、玩具 |
| Mahony | 中 | 低 | eVTOL、多旋翼 |
| Madgwick | 高 | 中 | 高精度需求、无人机 |
我个人建议：eVTOL项目直接用Mahony或Madgwick。互补滤波太简单，精度不够。Madgwick虽然计算量大一点，但现在的MCU完全扛得住。
1.8 本章知识体系
下面这张图展示了本章的核心逻辑：
1.9 避坑指南
最后，分享几个我踩过的坑：
- 陀螺仪零偏校准：上电后一定要静止几秒，采集零偏。我曾经跳过这步，结果悬停时飞机慢慢自旋。
- 加速度计低通滤波：eVTOL的电机振动很大，加速度计数据必须滤波。我一般用20Hz的截止频率。
- 四元数归一化：每次更新后都要归一化，否则四元数会越跑越偏。
- 磁力计干扰：eVTOL的电机和电池会产生强磁场，磁力计数据很容易被干扰。建议用外部磁力计或者做硬铁校准。
好了，这一章就到这里。姿态解算是飞控的基础，后面的控制、导航都依赖它。把这些搞明白，后面的路就好走了。
公众号:蓝海资料掘金营，微信deep3321