4. 姿态解算入门：欧拉角与四元数、互补滤波原理、Mahony姿态解算算法详解
各位同学，欢迎来到姿态解算这一章。
说实话，飞控里最核心、也最容易让人头晕的，就是姿态解算。你想想看，飞控板上的IMU（惯性测量单元）只能给你加速度和角速度，但我们要的是——飞机现在到底朝哪边？倾斜了多少度？
这就好比给你一堆零件，你得拼出一张完整的地图。姿态解算，就是干这个活的。
4.1 欧拉角：最直观的“姿态语言”
我们先从最直观的欧拉角说起。
欧拉角用三个角度来描述一个物体的朝向：横滚角（Roll）、俯仰角（Pitch）、偏航角（Yaw）。说白了，就是飞机绕着自己三个轴转了多少度。
- 横滚角（Roll, φ）：绕机体X轴旋转，左右倾斜。我习惯叫它“侧身角度”。
- 俯仰角（Pitch, θ）：绕机体Y轴旋转，抬头低头。也就是“点头角度”。
- 偏航角（Yaw, ψ）：绕机体Z轴旋转，机头朝向。也就是“转身角度”。
欧拉角的好处是直观，一看就懂。但有个致命问题——万向锁（Gimbal Lock）。
4.2 四元数：数学家的“优雅解法”
既然欧拉角有坑，那怎么办？用四元数。
四元数是一个超复数，形式为 q = w + xi + yj + zk。别被它吓到，你只需要记住：四元数用四个数表示一个旋转，没有万向锁，计算效率高。
我个人习惯把四元数想象成一个“旋转轴 + 旋转角度”的打包。比如，绕某个轴转30度，四元数就能完美描述，不会出现奇点。
四元数和欧拉角可以互相转换。这里给一个常用的转换公式（从四元数到欧拉角）：
// 四元数转欧拉角
roll = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y))
pitch = asin(2*(w*y - z*x))
yaw = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z))
4.3 互补滤波原理：简单粗暴但有效
好了，有了数学工具，我们怎么把IMU的原始数据变成姿态呢？
IMU有两个主要传感器：陀螺仪和加速度计。
- 陀螺仪：测角速度，动态响应快，但会漂移（积分久了就偏了）。
- 加速度计：测重力方向，静态准确，但动态时受振动干扰大。
你想想看，一个快但不准，一个准但怕动。怎么办？互补滤波就是干这个的——取长补短。
互补滤波的核心思想很简单：
姿态 = α × (陀螺仪积分结果) + (1-α) × (加速度计修正结果)
其中 α 是一个权重系数，通常取0.98左右。陀螺仪占大头，加速度计用来“拉”住漂移。
我在项目中遇到过一个问题：如果α设得太接近1（比如0.999），陀螺仪漂移会慢慢累积，飞机飞着飞着就“歪头”了。如果α设得太小（比如0.9），飞机一震动，姿态就跟着抖。嗯，这里要注意，α的取值需要根据实际传感器噪声来调。
4.4 Mahony姿态解算算法：实战中的“老兵”
互补滤波虽然简单，但精度有限。工程上更常用的是Mahony算法（也叫Madgwick算法的一种变体）。
Mahony算法的核心是：用加速度计和磁力计的测量值，去修正陀螺仪的积分误差。它本质上是一个PI控制器。
算法流程大致如下：
- 用陀螺仪积分得到当前四元数姿态。
- 用当前姿态预测出“应该测到的加速度”和“应该测到的磁场”。
- 对比实际测量值和预测值，算出误差。
- 用PI控制器把这个误差反馈到陀螺仪角速度上，修正积分漂移。
- 更新四元数。
代码实现（核心部分）：
// Mahony 算法核心步骤（简化版）
void MahonyAHRSupdate(float gx, float gy, float gz,
float ax, float ay, float az,
float mx, float my, float mz) {
// 1. 归一化加速度计和磁力计数据
// 2. 计算重力方向误差
// 3. 计算磁场方向误差
// 4. 用PI控制器修正陀螺仪角速度
// 5. 更新四元数
}
说实话，Mahony算法在STM32这类MCU上跑得飞快，占用资源少，效果也够用。很多开源飞控（比如PX4、ArduPilot）的早期版本都在用。
4.5 本章知识体系总览
为了让你更直观地理解这一章的知识结构，我画了一张图：
从这张图你可以看到，整个姿态解算的流程是：原始数据 → 数学工具 → 解算算法 → 输出姿态 → 实际应用。每一步都有坑，但也有技巧。
好了，这一章的内容就到这里。姿态解算是飞控的“眼睛”，眼睛准了，飞机才能飞得稳。下一章我们会深入代码，手把手教你实现一个完整的姿态解算模块。