3. 姿态解算：欧拉角与四元数、旋转矩阵、互补滤波与Mahony滤波算法详解

姿态解算是飞控的“眼睛”。没有它，你连飞机是正着飞还是倒着飞都不知道。

我刚开始做飞控那会儿，踩过最大的坑就是姿态解算。传感器数据明明看着没问题，一上天飞机就开始“抽风”。后来才发现，不是传感器坏了，是算法没选对。

这一章，咱们就把姿态解算的底裤扒干净。从数学基础到工程实现，一条龙讲透。

3.1 姿态的数学表示：三种语言，一个意思

描述一个物体的朝向，说白了就是描述“它绕三个轴转了多少”。但同一个旋转，可以用三种不同的数学工具来表达。我习惯把它们叫做“三种语言”。

3.1.1 欧拉角：最直观，但最危险

欧拉角就是俯仰角(Pitch)、横滚角(Roll)、偏航角(Yaw)。你拿手机转一下，就能直观感受到。

但这里有个大坑——万向锁。当俯仰角接近±90°时，横滚和偏航会“耦合”在一起，丢失一个自由度。

3.1.2 旋转矩阵：数学上最完美

旋转矩阵是一个3×3的正交矩阵。它能把一个向量从一个坐标系变换到另一个坐标系。

举个例子，你要把机体坐标系下的加速度，转换到地理坐标系下，旋转矩阵就是那个“翻译官”。

// 绕Z轴旋转ψ角的旋转矩阵
R_z(ψ) = [cosψ  -sinψ  0]
         [sinψ   cosψ  0]
         [0      0     1]

// 绕Y轴旋转θ角
R_y(θ) = [cosθ   0  sinθ]
         [0      1  0   ]
         [-sinθ  0  cosθ]

// 绕X轴旋转φ角
R_x(φ) = [1   0      0   ]
         [0   cosφ  -sinφ]
         [0   sinφ   cosφ]

// 完整旋转矩阵（Z-Y-X顺序）
R = R_z(ψ) * R_y(θ) * R_x(φ)

但旋转矩阵有9个元素，而且必须保持正交约束。每次更新后还得重新正交化，计算量不小。

3.1.3 四元数：飞控工程师的“真香”选择

四元数只有4个参数，没有奇点，插值平滑。说白了，它就是为姿态解算而生的。

// 四元数定义
q = w + xi + yj + zk
// 或者写成向量形式
q = [w, x, y, z]

// 用四元数旋转向量 v
v' = q * v * q_conjugate

// 四元数乘法（核心运算）
q1 * q2 = [w1*w2 - x1*x2 - y1*y2 - z1*z2,
           w1*x2 + x1*w2 + y1*z2 - z1*y2,
           w1*y2 - x1*z2 + y1*w2 + z1*x2,
           w1*z2 + x1*y2 - y1*x2 + z1*w2]

3.2 传感器融合：为什么需要互补滤波？

你想想看，陀螺仪测角速度，积分得到角度。但陀螺仪有零偏，积分会漂移。加速度计和磁力计没有漂移，但噪声大，动态响应慢。

一个高频不准但短期可靠，一个低频准确但长期稳定。互补滤波就是把两者“互补”起来。

3.2.1 互补滤波原理

核心思想很简单：

• 陀螺仪积分角度 → 通过高通滤波器（信任短期变化）
• 加速度计/磁力计解算角度 → 通过低通滤波器（信任长期稳定）
• 两者相加 → 得到最终姿态

// 一阶互补滤波伪代码
angle = 0.98 * (angle + gyro * dt) + 0.02 * acc_angle;

// 0.98 和 0.02 是权重系数
// 权重取决于你对陀螺仪和加速度计的信任程度

这个0.98和0.02怎么来的？说白了就是截止频率。我一般这样算：

// 截止频率 fc = 1 / (2 * π * τ)
// 其中 τ = (1 - α) / (α * dt)
// α 是陀螺仪权重

// 示例：dt=0.004s(250Hz), 希望截止频率2Hz
α = 1 / (1 + 2 * π * 2 * 0.004) ≈ 0.95
// 所以代码里写 0.95 * gyro + 0.05 * acc

3.2.2 Mahony滤波：工程界的“标准答案”

Mahony滤波是互补滤波的进阶版。它不直接融合角度，而是融合“误差”。

我当年第一次看到Mahony论文时，觉得这算法真巧妙——它用一个PI控制器来修正陀螺仪的偏差。

// Mahony滤波核心步骤（简化版）
// 1. 用当前四元数计算理论重力方向
g_estimated = q * [0, 0, 0, 1] * q_conj;

// 2. 与加速度计实测方向做叉积，得到误差
error = acc_measured × g_estimated;

// 3. PI控制器修正陀螺仪
gyro_corrected = gyro_raw + Kp * error + Ki * ∫error dt;

// 4. 用修正后的角速度更新四元数
q_dot = 0.5 * q * gyro_corrected;
q += q_dot * dt;

// 5. 归一化
q = q / ||q||;

3.3 完整姿态解算流程

说了这么多，咱们把整个流程串起来。这是我个人在STM32上跑过无数次的框架。

3.4 工程实现中的避坑指南

理论讲完了，咱们聊聊实战。这些坑，我当年一个一个踩过来的。

3.4.1 传感器数据同步

陀螺仪和加速度计的数据不是同一时刻采的，怎么办？

• 用硬件FIFO：MPU6050等传感器自带FIFO，可以保证数据时间对齐。
• 软件插值：如果实在没办法，用线性插值把时间戳对齐。

3.4.2 磁力计干扰

电机一转动，磁力计数据就乱飞。我遇到过最夸张的一次，偏航角直接漂了30°。

• 做硬铁/软铁校准
• 在代码里加磁力计信任度判断——如果磁场强度超出正常范围，直接降低磁力计权重

3.4.3 初始化姿态

飞机上电时，四元数初始值怎么设？

// 从欧拉角初始化四元数
// 假设初始俯仰=0, 横滚=0, 偏航=0
q = [1, 0, 0, 0];

// 如果飞机不是水平的，用加速度计算初始俯仰和横滚
roll  = atan2(acc_y, acc_z);
pitch = atan2(-acc_x, sqrt(acc_y*acc_y + acc_z*acc_z));
// 然后用欧拉角转四元数公式初始化

3.5 总结

姿态解算没有银弹。欧拉角直观但怕万向锁，旋转矩阵完美但计算量大，四元数才是飞控的“真命天子”。

互补滤波简单可靠，Mahony滤波是工程界的标准方案。你如果刚开始做飞控，我建议直接从Mahony入手，把Kp和Ki调明白，比什么都强。

嗯，这一章的内容就到这儿。代码写再多，不如自己跑一遍试试。找个开发板，把MPU6050焊上，跑一跑Mahony，你会有更深的体会。