STM32+MPU6050滤波实战：从裸数据到互补滤波姿态解算

如果你和我一样，第一次把MPU6050接到STM32之后满心欢喜地打开串口，看到的却是满屏乱跳的加速度和陀螺仪数据，你会瞬间明白一个道理：这颗传感器真正值钱的不是原始寄存器，而是你打算怎么处理它。我最初做的自平衡小车，就是因为拿裸数据直接算角度，车放在桌上好好的，手稍微碰一下桌面，OLED上的角度就开始“蹦迪”。后来正经把所有滤波方案梳理了一遍，才把姿态数据做到静止时几乎不动、快速晃动时也能跟手。这篇文章就围绕STM32 + MPU6050滤波这条主线，把滑动窗口滤波、加速度计算角、陀螺仪零偏校准、互补滤波从原理到代码完整盘一遍，内容适合正在做平衡小车、四轴、云台或者手势控制，卡在“数据不够干净”这个阶段的人。

1. 为什么 MPU6050 裸数据不能直接用

1.1 加速度计和陀螺仪其实是两套完全不同性格的传感器

看名字，你可能会觉得MPU6050就是一个“陀螺仪芯片”，这里面放了两套传感器：一套三轴加速度计，一套三轴陀螺仪。它们各自测量完全不同的物理量：加速度计测的是“加速度”，注意，它分不清重力加速度和运动产生的线加速度，所以静止时它测到的是重力向量，运动时就会叠加车体自身的加速度干扰；陀螺仪测的是“角速度”，也就是物体旋转的快慢，它不会受平移震动影响，动态响应极快。

问题就出在两者的优缺点正好相反。加速度计长期看很准，因为重力的方向是绝对参考，但短时间内的高频抖动大，哪怕你只是用指甲轻轻敲一下桌面，它的输出就可能跳一大截。陀螺仪短时间内的角速度数值很平滑、很跟手，但如果你直接对它的输出做积分来求角度，那个零偏误差会像漏水的桶一样一点点积满，几分钟内角度就往一个方向飘出去十几度。

这听起来就像一个人记性好但爱说谎，另一个人不说谎但记性差。滤波做的事情，本质就是让两个人互相监督：短时间里多信陀螺仪，时间稍微长一点就用加速度计把漂移拉回来。

1.2 抖动、噪声、零漂、温漂，究竟在过滤什么

很多新手一上来就照着网上代码抄，看到别人写了个滤波函数就塞进去，但滤波目标都不清楚，调参当然头大。这里我用工程上常见的分类来说清楚：

• 振动与高频噪声：来自电机、路面、手部颤动，频率通常高于姿态变化频率，表现是相邻采样点之间乱跳。滑动窗口滤波和一阶低通都能压住它。
• 零偏：陀螺仪静止时输出不为0，这个偏置相对固定，是“系统性误差”，最简单的办法是上电静止采集几百个点求平均，做成零偏校准。
• 温漂：陀螺仪的零偏会随温度缓慢变化，开机和运行二十分钟后的零偏可能不一样。对普通项目，定期校准就能接受；追求长期的才需要温度补偿模型。
• 积分漂移：陀螺仪读数集成角度后，不是零偏问题被放大，而是零偏和随机噪声都被时间积分放大。这个漂移只能靠另一个绝对参考来修正。

弄明白这些之后，你才会发现：滑动窗口滤的是第一类高频噪声，零偏校准解决的是第二、三类误差，而互补滤波要处理的核心其实是第四类积分漂移。四件事搅在一起做，自然糊。

1.3 主流的 MPU6050 滤波方案到底该怎么选

做姿态类项目的时候，你会在网上看到几个反复出现的名词：滑动窗口滤波、高通低通、互补滤波、卡尔曼滤波、四元数、DMP姿态解算。先别慌，选型逻辑其实很简单，我做了个对照表：

对于大多数用STM32做平衡车、两轮自平衡、云台稳定这类只需要roll和pitch角度的项目，一阶互补滤波已经完全够用，没有必要为了“显得高级”盲目上卡尔曼。先把互补滤波用明白，比什么都强。

2. 滤波算法核心细节拆解：从原始值到可用姿态角

2.1 先做数据预处理：滑动窗口滤波实现与窗口大小选择

最容易被忽略的是，融合前应该先把加速度计的三轴原始数据分别过一遍滑动窗口。因为最终的角度如果直接被低通，姿态会显得“很肉”；但如果让滑窗滤掉一部分传感器噪声，后续融合再用互补滤波补偿动态响应，效果会更干净。

滑动窗口滤波的原理一句话说清楚：维护一个长度固定的环形缓冲区，每来一个新数据，就丢掉最旧的一个，然后把窗口内所有数据求平均值。这个“窗口”越宽，滤波越平滑，但延迟也大。拿100Hz采样频率来说，窗口选4时每组数据只滞后20毫秒左右，用来滤振动噪声基本够了；选到20以上，角度会明显感觉粘滞。我实测选择窗口范围4到10比较合适，不要盲目加大。

这里给出一份正经好用的环形缓冲实现，避免网上好多代码用数组移位造成的时间浪费。STM32F103这种主频72M的芯片虽然没有FPU，但跑浮点还不是瓶颈，只要别每次把数组整体搬移：

typedef struct { float buf[10]; // 窗口长度，我按10写的 uint16_t idx; uint16_t filled; float sum; } SlideFilter; float slide_put(SlideFilter *f, float x) { if (f->filled >= (uint16_t)(sizeof(f->buf) / sizeof(f->buf[0]))) { f->sum -= f->buf[f->idx]; } else { f->filled++; } f->buf[f->idx] = x; f->sum += x; f->idx++; if (f->idx >= (uint16_t)(sizeof(f->buf) / sizeof(f->buf[0]))) { f->idx = 0; } return f->sum / (float)f->filled; }

代码里那个fillled字段是为了处理系统刚启动、窗口还没填满的情况。一定不要用“前N个值填0”再直接平均，否则传感器上电那几十个毫秒里的滤波结果会被0严重拉低，姿态角会从很大的负值慢慢爬回来，特别难看。

2.2 用加速度计计算 roll 和 pitch 的公式与方向坑

加速度计静止时测到的是重力加速度，大小为1g，方向朝下。既然静止时我们能拿到一个固定的三维向量，那就能反推出在这组向量下板子倾斜了多少。这里用的是标准公式：

#define RAD2DEG 57.29578f float roll_acc = atan2f(ay_f, az_f) * RAD2DEG; float pitch_acc = atan2f(-ax_f, sqrtf(ay_f * ay_f + az_f * az_f)) * RAD2DEG;

其中ax_f、ay_f、az_f为滤波后加速度数值，单位是g，不是原始LSB。为什么要先换算成g？因为atan2f的两个输入需要同一量纲，直接用原始LSB会导致输出完全错误。我在网上真的见过有人拿16384去除了一遍但忘了另外两轴也要除，结果横滚角在0到45度之间完全不对。

有两个点必须提醒：

• yaw（偏航角）不能通过加速度计算出来，因为绕竖直轴旋转时重力向量不会变化，必须依赖磁力计或者陀螺仪积分。所以只用MPU6050做完整三维姿态解算，在数学上就是先天不足的。
• 刚才那套公式里X轴朝前、Y轴朝右、Z轴朝下时成立，但PCB板上的丝印方向和传感器封装方向不同，轴符号就可能反了。典型的现象是：你往右边倾斜板子，串口角度却变成负的。解决方法是交换atan2f里的参量顺序或者把某个轴的量取负，没有统一的万能公式。这也是为什么很多教程代码注释里总有一句“若角度反了改改符号”。

2.3 陀螺仪零偏校准是怎么回事，为什么必须在静止时做

陀螺仪静止时理论上输出应该为0，但实际芯片出的LSB值不会那么干净，可能稳定在某个固定值附近，而且三个轴各不相同。这个值就是陀螺仪的零偏。如果你不做校准，直接把原始值除以灵敏度然后积分，每秒钟都会引入一个固定角速度误差，角度会随时间直线漂走。

校准方法千万条，核心就一条：传感器完全静止，采集一段时间的数值，求平均值作为零偏。我用的是上电后采集100次、延时2毫秒一次的做法，约200毫秒完成校准，对大多数项目足够：

void mpu6050_zero_offset(float *gxo, float *gyo, float *gzo) { int32_t sx = 0, sy = 0, sz = 0; const int N = 100; MPURaw_t raw; for (int i = 0; i < N; i++) { mpu_read(&raw); sx += raw.gx; sy += raw.gy; sz += raw.gz; HAL_Delay(2); } *gxo = (float)sx / N; *gyo = (float)sy / N; *gzo = (float)sz / N; }

要注意校准期间绝不能让桌面震动，风扇、人来回走动都会污染这个平均值。如果校准完角度还缓慢漂移，不要急着怀疑算法，先检查是不是校准过程中板子没放稳，或者体温把芯片烤热导致了温漂。我自己踩过这个坑：手捏着板子做校准，读出来零偏偏了好几度每秒，融合后整车都在慢慢“转圈”，查了一下午才发现是手温影响。

校准得到的零偏在LSB域，读到原始值之后、除以灵敏度之前减掉，顺序不能乱。

2.4 互补滤波的原理与参数 alpha 怎么算

终于说到最核心的姿态融合了。先讲直觉，再给公式：

陀螺仪动态好但会漂，加速度计静态准但有高频噪声。互补滤波的思路是让陀螺仪通过一个高通滤波器，让加速度计通过一个低通滤波器，再把二者相加。姿态变化通常比较快，就让陀螺仪负责高频段；重力方向是慢变量，就让加速度计负责修正长期漂移。最终实现就一个公式：

roll_f = alpha * (roll_f + gx_dps * dt) + (1.0f - alpha) * roll_acc; pitch_f = alpha * (pitch_f + gy_dps * dt) + (1.0f - alpha) * pitch_acc;

其中gx_dps是零偏校准后、除以灵敏度得到的角速度，单位是度每秒；dt是两次融合之间的时间间隔，单位秒；alpha一般取0.95到0.99之间。

alpha背后的物理意义常被人忽略。它对应一个时间常数tau = alpha * dt / (1 - alpha)，可以近似理解为“陀螺仪产生的小漂移需要多久被加速度计修正回来”。比如alpha=0.98、dt=0.01s时，得到tau≈0.49s，这意味着陀螺仪积分角度即使有偏差，约半秒后就会被加速度计拉回正确基线。alpha越大，滤波越平滑但越滞后；alpha越小，动态跟手越好但噪声越大。我的调参顺序是：先设0.98看长时间静止是否稳，再用手快速晃动看是否拖影，如果感觉响应慢就一点一点往下降，不要一次从0.98改到0.85，因为从平滑到抖动的拐点往往非常突然。

2.5 那要不要直接上卡尔曼滤波或者 DMP

有不少人看到“卡尔曼”就兴奋，觉得用了它数据就无敌了。事实是卡尔曼表现上限确实更高，但需要调两个噪声协方差矩阵（过程噪声Q和测量噪声R），概念抽象、调参方向不直观。我在F103上试过一维卡尔曼融合roll角，最终效果并没有比精心调过的互补滤波强到一个量级，代码量却翻了十几倍。

DMP是MPU6050芯片内部的数字运动处理器，配合InvenSense官方MotionDriver库能直接输出四元数，好处是帮你自动完成了姿态解算，且零漂表现很好；坏处是官方库对工程结构有一些要求，移植到HAL库时需要一点功夫，出了问题你很难黑盒里去查。如果你只需要双轴倾角，直接用互补滤波自己控制流程，反而更好维护、更容易排查问题。

3. 基于 STM32 HAL 库的完整实做

3.1 硬件接线和初始化顺序里不容易发现的雷

MPU6050和STM32之间是I2C通信，标准接线就那么几根：VCC接3.3V、GND接GND、SCL接STM32的SCL引脚、SDA接SDA引脚。我在STM32F103C8T6上常用I2C1，也就是PB6和PB7两根脚。AD0引脚接地时器件地址是0x68，接高电平则变成0x69；如果板子上焊了AD0，一定要看原理图确认它的默认电平，否则通信建立不了。

再一个很容易被新手忽略的是上拉电阻。STM32的I2C引脚是开漏模式，需要外部上拉到3.3V，很多最小系统板内部已经带了上拉，但如果你用的是外接模块和杜邦线，最好确认模块上有没有4.7kΩ上拉电阻。没有的话，用两颗4.7kΩ电阻分别把SCL、SDA拉到3.3V。I2C上拉不足最常见的症状是通信有时候成功、有时候失败，甚至读WHO_AM_I返回值正常但批量读数据偶尔超时。

初始化寄存器之前，记得先复位器件、再唤醒，最后配置采样率和数字低通滤波器。给出顺序参考：

uint8_t val; val = 0x80; wr_reg(0x6B, val); // PWR_MGMT_1: 复位整个芯片 HAL_Delay(100); val = 0x01; wr_reg(0x6B, val); // 唤醒，并把时钟源切到陀螺仪X轴PLL val = 0x09; wr_reg(0x19, val); // SMPLRT_DIV: 输出速率约100Hz // 具体为1kHz/(1+9)=100Hz，具体输出速率还要看DLPF配置 val = 0x03; wr_reg(0x1A, val); // CONFIG: DLPF带宽约42Hz，够滤高频 val = 0x00; wr_reg(0x1B, val); // GYRO_CONFIG: ±250°/s，灵敏度131 LSB/(°/s) val = 0x00; wr_reg(0x1C, val); // ACCEL_CONFIG: ±2g，灵敏度16384 LSB/g

Gyro和Accel的量程我刻意选默认最小档，因为对姿态计算来说分辨率最高。可别贪心把陀螺仪设成±2000°/s，那样灵敏度只有16.4，算出来的角速度量化噪声会大不少。除非你的东西转速真的超过250°/s，否则用±250°/s准没错。

3.2 读取14字节原始数据并拼成带符号整型

MPU6050把三轴加速度、温度、三轴角速度按顺序放在从0x3B开始的连续14个寄存器里。因此只需要发一次寄存器地址，连续读取14字节即可。用HAL库时HAL_I2C_Mem_Read很合适：

#define MPU6050_WR_ADDR 0xD0 // 0x68左移一位后的8位写地址 typedef struct { int16_t ax, ay, az; int16_t gx, gy, gz; } MPURaw_t; void mpu_read(MPURaw_t *raw) { uint8_t buf[14]; if (HAL_I2C_Mem_Read(&hi2c1, MPU6050_WR_ADDR, 0x3B, I2C_MEMADD_SIZE_8BIT, buf, 14, 50) != HAL_OK) { return; } raw->ax = (int16_t)((uint16_t)buf[0] << 8 | buf[1]); raw->ay = (int16_t)((uint16_t)buf[2] << 8 | buf[3]); raw->az = (int16_t)((uint16_t)buf[4] << 8 | buf[5]); raw->gx = (int16_t)((uint16_t)buf[6] >> 8 ? 0 : 0); // 占位 // 实际应继续读第8~9字节开始的三轴陀螺 }

上面的写法容易踩坑，补完：

raw->gx = (int16_t)((uint16_t)buf[8] << 8 | buf[9]); raw->gy = (int16_t)((uint16_t)buf[10] << 8 | buf[11]); raw->gz = (int16_t)((uint16_t)buf[12] << 8 | buf[13]);

因为寄存器里是大端存储，高字节在前、低字节在后，所以拼装时要拿高字节左移8位再和低字节做或运算。若你拿反了，静止时角度会呈现一种“数字乱码”般的完全无规律抖动，而不是正常的噪声。这个错误很隐蔽，建议第一次调试时先打印三轴原始数，检查静止时Z轴加速度大约在16000左右、XY轴接近0，再做后续处理。

同时建议先用WHO_AM_I寄存器验证通信，地址0x75，正常返回0x68：

uint8_t id = 0; HAL_I2C_Mem_Read(&hi2c1, MPU6050_WR_ADDR, 0x75, I2C_MEMADD_SIZE_8BIT, &id, 1, 50); if (id != 0x68) { printf("MPU6050 not found, id=%02x\r\n", id); }

这一步能帮你把硬件问题快速切成“没连上”和“数字处理问题”两段，而不是等角度算出来才发现不对劲，回头查半天。

3.3 精确算 dt，不要用 HAL_Delay 计数

互补滤波公式里有dt，它是整个融合的“节拍器”。如果你在循环里写HAL_Delay(10)后以为每轮恰好过了10毫秒，就大错特错了：I2C读取本身要消耗时间，FLASH里printf打印更是不定时的，实际两次融合间隔可能在10到15毫秒之间波动。积分时间不准时，姿态率会按错误的时间累积，车子会表现出一种非常奇怪的“随机慢漂”。

最省事可靠的做法是使用HAL_GetTick()拿到当前毫秒，每次执行前算一下真实差：

static uint32_t last_tick = 0; uint32_t now_tick = HAL_GetTick(); float dt = (now_tick - last_tick) / 1000.0f; if (dt >= 0.005f) { // 执行一次读取和融合 last_tick = now_tick; }

dt>=0.005f这个判断防止循环跑得太快导致同一毫秒内重复执行。如果你对实时性要求更高，可以用TIM2启动一个1MHz的微秒时基，