从IMU加速度与角速度到欧拉角：互补滤波与四元数融合的实战解析-优快云博客

1. 从传感器数据到姿态角：一个嵌入式工程师的实战起点

大家好，我是老张，一个在嵌入式系统和传感器领域摸爬滚打了十多年的工程师。今天我想和你聊聊一个在机器人、无人机、可穿戴设备里几乎都会遇到的问题：如何让IMU（惯性测量单元）告诉你设备当前“歪”了多少度？ 换句话说，就是怎么把IMU输出的那一串串原始的加速度和角速度数据，变成我们人能直观理解的滚转、俯仰、偏航这三个欧拉角。

你可能刚从淘宝买回来一块MPU6050或者ICM20948，兴冲冲地接上单片机，读出了数据，却发现事情没那么简单。加速度计在静止时能告诉你重力方向，算个角度好像挺准，但设备一动起来，读数就“飘”得厉害；陀螺仪积分出来的角度短时间内很准，可没过几秒，它就自己“跑偏”了，角度越积越大。这就像你蒙着眼睛走路，只靠感觉（陀螺仪）会慢慢偏离方向，只靠偶尔摸一下墙（加速度计）又无法应对快速转弯。

我刚开始做四轴飞行器那会儿，在这个问题上踩过不少坑。最经典的一次是，我直接用加速度计算出的角度去控制电机，结果飞机一加速，姿态估计完全错误，直接“炸机”。后来才明白，必须把两者的优点结合起来，这就是传感器融合的核心思想。今天，我就把两种最常用、也最实用的融合方法——互补滤波和四元数融合（以Mahony算法为例）——从原理到代码，掰开揉碎了讲给你听。我的目标是，哪怕你刚接触嵌入式开发，也能跟着这篇文章，把手头的IMU用起来，得到稳定可靠的姿态数据。

我们先统一一下“语言”。假设你的IMU输出两组数据：加速度 (ax, ay, az)，单位是重力加速度g（静止时，指向地面的轴读数约为1g或-1g）；角速度 (gx, gy, gz)，单位是度每秒（dps）。我们的目标，就是算出滚转角（Roll, φ）、俯仰角（Pitch, θ）、偏航角（Yaw, ψ）。这里采用航空航天领域最常用的Z-Y-X顺序（即先绕Z轴转得到偏航，再绕新Y轴转得到俯仰，最后绕新X轴转得到滚转）。记住这个顺序，因为后续所有的公式推导和代码实现都基于它。

2. 拆解原理：加速度计与陀螺仪的“性格分析”

在动手写代码之前，我们必须先摸清手里这两个“伙计”的脾气。这就像你要带团队，得先了解每个成员的长处和短板。

2.1 加速度计：沉稳但“迟钝”的向导

加速度计测量的是包括重力在内的所有线加速度。当你的设备静止或匀速运动时，它感受到的几乎只有重力。这时候，它就像一个绝对的方向基准，能非常准确地告诉你哪个方向朝下。

如何从加速度算出姿态？ 想象一下，把一个重力向量 [0, 0, 1g]（假设Z轴向上）放在你手里，然后你开始旋转设备。这个重力向量在设备自身的XYZ坐标系下的投影，就是加速度计读到的 (ax, ay, az)。通过一些三角函数关系，我们可以反推出旋转的角度。对于滚转和俯仰，有两个非常实用的公式：

// 假设传感器平放时，Z轴向上指向天空
roll_rad = atan2(ay, az);
pitch_rad = atan2(-ax, sqrt(ay*ay + az*az));

这里用 atan2(y, x) 而不是 atan(y/x)，是为了避免除零错误，并且能自动处理四个象限，得到 -π 到 π 范围内的完整角度。sqrt 那部分其实就是重力在水平面上的投影大小。

它的局限性是什么？ 加速度计最大的问题是“傻傻分不清”。它无法区分重力加速度和设备运动产生的线加速度。当你突然移动或震动设备时，额外的加速度会严重污染重力信号，导致计算出的角度完全错误。此外，它根本无法感知绕垂直轴（Z轴）的旋转，也就是偏航角（Yaw）。因为无论你怎么水平旋转设备，重力在水平面的投影是不变的。所以，单靠加速度计，你只能得到没有偏航的“残缺”姿态。

2.2 陀螺仪：灵敏但“健忘”的记分员

陀螺仪测量的是角速度，也就是旋转的快慢。对它进行积分（角速度乘以时间），理论上就能得到角度变化量。它响应极其迅速，对运动非常敏感，而且不受线加速度干扰。

如何用陀螺仪得到姿态？ 最简单的方法就是欧拉积分：当前角度 = 上一时刻角度 + 角速度 * 时间间隔(dt)。在代码里，可能就是一句 angle += gyro * dt;。

它的致命缺陷：漂移 陀螺仪的输出永远不是完美的零。即使设备完全静止，它也会有一个微小的常数值输出，这叫零偏。这个零偏在积分过程中会被不断累加，导致算出的角度像脱缰的野马一样，随时间无限增长，这就是积分漂移。哪怕零偏只有0.1度/秒，一分钟就能漂出6度，十分钟就完全不能看了。所以，陀螺仪是个出色的“短期记忆者”，但长期来看，它不可信。

3. 第一种武器：简单粗暴的互补滤波

既然知道了两者的优缺点，最直观的想法就是：用陀螺仪的数据来跟踪快速变化，用加速度计的数据来纠正长期漂移。互补滤波就是这个思想的直接体现，它简单到令人惊讶，但效果却出奇地好，特别适合资源有限的单片机。

3.1 一阶互补滤波：一个加权平均的魔法

它的核心公式就是一个加权平均： 融合角度 = α * (上一时刻角度 + 陀螺仪增量) + (1 - α) * 加速度计角度

这里的 α 是一个介于0和1之间的滤波系数，它决定了你更相信谁。α 越接近1（比如0.98），系统就越信任陀螺仪的高频特性，响应快，但长期会漂；α 越接近0，就越信任加速度计的低频基准，长期稳定，但动态响应差。

我通常把 α 理解为一个高通滤波器的系数，让陀螺仪信号（高频）通过；同时对加速度计信号施加一个具有相同截止频率的低通滤波器。两者“互补”，就得到了全频段都靠谱的信号。

下面是一个可以直接用在你的STM32或Arduino上的C代码片段：

#include <math.h>
#define PI 3.14159265358979323846f
#define DEG_TO_RAD (PI/180.0f)

// 全局状态变量
float roll = 0.0f, pitch = 0.0f; // 单位：弧度

void complementary_filter_update(float ax_g, float ay_g, float az_g,
                                 float gx_dps, float gy_dps, float gz_dps,
                                 float dt_seconds) {
    // 1. 单位转换：陀螺仪从度/秒转为弧度/秒
    float gx_rad = gx_dps * DEG_TO_RAD;
    float gy_rad = gy_dps * DEG_TO_RAD;
    // 注意：互补滤波通常只用gx, gy来更新roll和pitch，gz用于偏航（但这里没有融合源）

    // 2. 从加速度计计算滚转和俯仰角
    // 注意：这里假设传感器平放时，Z轴向上。如果你的板子Z轴向下，需要把az取反。
    floa