从串口第一次打出 MPU6050 原始数据时我的心态是崩溃的板子安安静静放在桌面上加速度计 Z 轴却在 16000 到 16800 LSB 之间来回跳陀螺仪静止状态也带着 ±3°/s 的抖动。那会儿我用的还是 STM32F103C8T6 最小系统板加一根十几厘米的杜邦线电源来自开发板的 USB 口。后来我才想明白MPU6050 的姿态解算难关从来不在能不能读到数据而在读到的数据你信几分、怎么让它变得能信。这篇就专门讲 STM32 搭配 MPU6050 做滤波这件事。我会把几种常用的滤波手段拆开讲透包括滑动窗口滤波、一阶低通滤波、互补滤波还会把我在实际工程里踩过的坑一并列出来。不管你是刚点亮 I2C 的新手还是已经在做平衡车、云台、四轴的老手这篇都值得你存下来当参考。1. 从原始 MPU6050 数据说起为什么你的波形一直在抖1.1 噪声到底从哪里来很多人的第一反应是传感器坏了或者买到假芯片。实际上MPU6050 的模拟前端在十年前已经算不错的水平了真正的问题通常不在芯片本身而在你给它的工作环境。第一类噪声来自 MEMS 结构内部的机械热噪声。加速度计内部是一个微型质量块它悬浮在硅结构上温度变化下无时无刻不在做布朗运动。这些微小位移被电容检测电路放大再经过 16 位 ADC 量化就成了你看到的随机跳动。这类噪声属于白噪声频谱比较平坦靠软件滤波能压下去。第二类噪声来自电源和地。开发板 USB 供电本身就有纹波电机启动、继电器吸合或者 PWM 驱动大电流负载时3.3V 上会叠加明显的毛刺。MPU6050 内部的 ADC 参考电压直接取自 VDD电源一动读数必然跟着动。这类噪声往往带有明显的周期性比如 50Hz、100Hz你可以在串口波形上观察到规律性的上下波动。第三类噪声最容易被忽略就是 I2C 采样时序抖动。MPU6050 的数据寄存器是内部同时锁存的但你通过 I2C 一个字节一个字节往外读如果总线速率不稳定或者中断插入导致两次读取之间间隔不均匀那数据本身没问题采样点在时间轴上却是不均匀的。后续做滤波、做姿态积分时把不均等的采样间隔当成均等间隔算等于给算法注入额外误差。1.2 先做噪声体检再谈滤波参数我建议拿到模块之后不要急着写滤波代码先用一组简单数据判断问题出在哪。把模块可靠固定让系统静止以 100Hz 到 200Hz 采样率连续读取 1000 组原始数据分别统计每个轴的均值、方差和最大最小值。判断方法很简单如果加速度计 Z 轴在静止时跳动幅度超过 ±200 LSB折合 ±0.012g优先检查电源和杜邦线而不是急着上滤波。如果陀螺仪静止时输出均值明显偏离零比如稳定在 50 或 -80那不是随机噪声是零偏需要做校准。如果 Z 轴静止均值严重偏离 16384 LSB±2g 量程下 1g 对应的数值大概率是传感器倾斜放置或者模块本身没有水平安装。做完这步你才能知道自己需要的到底是滑窗平滑、低通滤波还是先把 PCB 布局重做一遍。2. 动手前先选型滑动窗口、低通、互补滤波你到底需要哪一个2.1 滑动窗口滤波简单但别在闭环里用大窗口滑动窗口滤波就是取最近 N 个采样点的平均值每来一个新数据丢掉最旧的那个窗口向前滑动一格。实现最简单效果直观很多热词里提到的滑动窗口滤波模型说的就是它。代码逻辑不复杂但代价是输出延迟。N 个点的窗口输出相位大约滞后 N/2 个采样周期。假设采样率 100Hz、窗口 20 点那输出就滞后了 100ms。这个滞后对离线查看波形没影响但你要是拿滤波后的姿态角去做平衡车的 PD 闭环100ms 滞后基本等于先把系统判了死刑。我见过有人把窗口开到 50 甚至 100 点去平滑 MPU6050波形确实漂亮一通电姿态却慢半拍最后怎么调 PID 都调不稳。所以我把滑动窗口的定位说直接一点适合对实时性要求不高的数据预处理、离线显示、或者做降采样前的抗混叠。如果你只是想让串口波形好看它很合适。要是做闭环控制请把窗口控制在 8 点以内或者干脆用下面的一阶低通。2.2 一阶低通滤波嵌入式里性价比最高的选择一阶低通滤波对应的就是热词里的软件低通滤波simulink 一阶滤波模块rc 滤波截止频率这些概念。RC 硬件滤波器的传递函数搬到软件里就是那个经典的递推式y[n] y[n-1] α × (x[n] - y[n-1])也可以写成 y[n] α × x[n] (1 - α) × y[n-1]。这个 α 就是滤波系数也就是 RC 滤波器里R 和 C共同决定的那个时间常数。α 越大新数据占的权重越大输出越灵敏但滤波效果越弱α 越小历史数据占的权重越大输出越平滑但延迟越大。我的习惯是用截止频率 fc 来反算 α而不是拍脑袋随便写个数。这样写出来的滤波器才有物理意义α 1 - exp(-2π × fc × dt)其中 dt 是采样周期也就是 1 / 采样率。举个例子采样率 100Hzdt0.01s想把 10Hz 以上的毛刺滤掉fc 取 10Hz算出来 α≈0.466。把 fc 降到 5Hzα≈0.268。同样的采样率下fc 每往下降一半输出就更平滑一点。2.3 互补滤波姿态解算里的主力方案如果你最终想得到的是姿态角也就是 roll、pitch那滤波就不只是平滑一下数据那么简单了而是要把加速度计和陀螺仪两路各有缺点的数据融合起来。加速度计测量的是重力矢量静止时能很准地给出水平姿态但它怕振动。车身一颠或者电机一转加速度计测到的不只是重力还有运动加速度姿态角当场就乱飘。陀螺仪测角速度积分能得到角度短时间非常平滑不怕振动但它有积分漂移。即使零偏校准得很好积分几分钟后角度还是会慢慢跑偏。互补滤波干的事就是取长补短把陀螺仪积分出来的角度做高通处理保留快速响应的部分把加速度计算出来的角度做低通处理保留长期稳定的部分。两者一叠加就得到了既不飘、又抗振的姿态角。核心代码只有一行angle α × (angle gyroRate × dt) (1 - α) × accAngle这里的 α 一般取 0.95 到 0.98意思是 95% 信任陀螺仪的短时积分5% 信任加速度计的长期测量。你不需要上卡尔曼滤波那是小学级难度的解法。互补滤波在平衡车、云台、四轴上已经够用。3. STM32 读取 MPU6050 的工程落地从 I2C 细节到数据对齐3.1 初始化寄存器顺序和连续读取虽然这篇的主角是滤波但滤波之前的数据读取如果埋着雷后面全是白做。先说最基本的初始化顺序。MPU6050 的 I2C 地址取决于 AD0 引脚。接地时地址是 0x68接高时是 0x69。在 HAL 库中HAL_I2C_Mem_Read的地址参数要左移一位0x68 对应 0xD00x69 对应 0xD2。我常用的初始化寄存器序列如下// 唤醒传感器 uint8_t data 0x00; HAL_I2C_Mem_Write(hi2c1, 0xD0, 0x6B, I2C_MEMADD_SIZE_8BIT, data, 1, 100); // 采样率分频1000Hz / (7 1) 125Hz data 0x07; HAL_I2C_Mem_Write(hi2c1, 0xD0, 0x19, I2C_MEMADD_SIZE_8BIT, data, 1, 100); // DLPF 配置设置带宽约 44Hz data 0x03; HAL_I2C_Mem_Write(hi2c1, 0xD0, 0x1A, I2C_MEMADD_SIZE_8BIT, data, 1, 100); // 陀螺仪 ±250°/s 量程灵敏度 131 LSB/(°/s) data 0x00; HAL_I2C_Mem_Write(hi2c1, 0xD0, 0x1B, I2C_MEMADD_SIZE_8BIT, data, 1, 100); // 加速度计 ±2g 量程灵敏度 16384 LSB/g data 0x00; HAL_I2C_Mem_Write(hi2c1, 0xD0, 0x1C, I2C_MEMADD_SIZE_8BIT, data, 1, 100);读取时从 0x3B 地址一次性连续读 14 个字节。顺序是 ACCEL_X_H、ACCEL_X_L、ACCEL_Y_H、ACCEL_Y_L、ACCEL_Z_H、ACCEL_Z_L、TEMP_H、TEMP_L、GYRO_X_H、GYRO_X_L、GYRO_Y_H、GYRO_Y_L、GYRO_Z_H、GYRO_Z_L。uint8_t raw[14]; HAL_I2C_Mem_Read(hi2c1, 0xD0, 0x3B, I2C_MEMADD_SIZE_8BIT, raw, 14, 100); int16_t ax_raw ((int16_t)raw[0] 8) | raw[1]; int16_t ay_raw ((int16_t)raw[2] 8) | raw[3]; int16_t az_raw ((int16_t)raw[4] 8) | raw[5]; int16_t gx_raw ((int16_t)raw[8] 8) | raw[9]; int16_t gy_raw ((int16_t)raw[10] 8) | raw[11]; int16_t gz_raw ((int16_t)raw[12] 8) | raw[13]; float ax ax_raw / 16384.0f; float ay ay_raw / 16384.0f; float az az_raw / 16384.0f; float gx gx_raw / 131.0f; float gy gy_raw / 131.0f; float gz gz_raw / 131.0f;注意 16 位数据高低字节合并必须用int16_t做强转如果你只声明uint16_t负数值会被当成无符号数后面滤波直接算出错误结果。3.2 数据对齐问题高字节和低字节分家MPU6050 内部的数据寄存器6 轴数据其实是同一时刻锁存的。但如果你不是一次性连读 14 字节而是分两次读比如先读高字节过一会儿再读低字节中间夹了一个 1ms 的中断那高字节和低字节就可能不是同一时刻的样本。合并出来的数值会偶尔跳出一个明显异常的点。后面做滑动窗口或者低通滤波时这一个异常点会扰动输出好一阵子。类似的问题也会出现在使用硬件 I2C 的 DMA 模式时。DMA 传输期间如果被中断打断传输不完整数据对齐错位后面整串数值都会变。我的做法是在定时器中断里只置一个数据已就绪标志主循环检测到标志之后再发一次HAL_I2C_Mem_Read保证读取过程是原子化的。把传感器读取和计算姿态放在不同的时隙里能避免不少硬伤。4. 三种滤波的代码实现与实测调参笔记4.1 滑动窗口滤波实现与资源占用先给一个最精简的滑动窗口滤波结构体。下面的代码针对单轴数据多轴使用时为每个轴创建一个实例即可。#include stdint.h #define WINDOW_SIZE 12 typedef struct { int16_t buf[WINDOW_SIZE]; uint8_t idx; uint8_t cnt; int32_t sum; } MovingAvg; int16_t MovingAvg_Input(MovingAvg *avg, int16_t sample) { int16_t old; if (avg-idx WINDOW_SIZE) { avg-idx 0; } old avg-buf[avg-idx]; avg-buf[avg-idx] sample; avg-idx; if (avg-cnt WINDOW_SIZE) { avg-cnt; } avg-sum sample - old; return (int16_t)(avg-sum / avg-cnt); }这里用了一个环形缓冲的思想维护一个累加和sum每次更新时先把最老的样本减掉再加新样本最后直接取平均。窗口 12 点时单轴最大sum是 12 × 32768 393216int32_t 足够用。如果你一口气把窗口拉到 100 点那就要留意 int32_t 是否可能溢出尤其是陀螺仪原始数据在某些量程下也能达到接近 32768 的值。实测下来对 MPU6050 的加速度计数据用 12 点窗口静态波动能削掉大概 70%但输出滞后约 60ms按 100Hz 采样率算。做平衡车的时候我会换成 4 点窗口只做轻度平滑。4.2 一阶低通滤波实现与系数确定一阶低通的代码比滑动窗口更短typedef struct { float y; float alpha; } LowPass; float LowPass_Filter(LowPass *f, float x) { f-y f-alpha * x (1.0f - f-alpha) * f-y; return f-y; }使用之前记得用第一帧原始数据初始化f-y否则滤波器从 0 开始爬爬升过程会拖慢响应。我整理了一张常用系数表采样率统一按 100Hz、dt0.01s 计算方便直接查截止频率 fcα 计算值实际效果参考2 Hz0.118输出非常平滑适合静态姿态估计5 Hz0.268常见折中既平滑又不太滞后10 Hz0.466响应快适合姿态动态变化大的场景如果把采样率改成 200Hzdt0.005s同样 fc5Hz 时 α≈0.154。这里特别提醒一句提高采样率之后α 如果不跟着调小实际截止频率会被抬高滤波效果反而不如原来。每一处参数都要从截止频率出发去推导而不是凭手感来回试。在 STM32F103 这类没有 FPU 的 MCU 上如果你对速度有强迫症可以把 α 量化成定点数。把 α 放大 1024 倍算完再右移 10 位int32_t alpha_q10 274; // 0.268 * 1024 int32_t y_q10 0; // 上一次输出定点表示 int32_t x_q10 ax_raw; // ADC 原始值 y_q10 (alpha_q10 * x_q10 (1024 - alpha_q10) * y_q10) 10;不过说实话F103 就算跑 float 版本的低通滤波每轴一次也就几个周期完全不是瓶颈。真正吃资源的是后面姿态解算里的 atan2 和 sqrt。4.3 互补滤波的姿态解算代码互补滤波的姿态角更新核心代码很短但几个细节决定成败。#define GYRO_ALPHA 0.98f #define DT 0.01f float roll 0.0f; float pitch 0.0f; void AHRS_Complementary(float gx, float gy, float gz, float ax, float ay, float az) { // 加速度计计算姿态角注意坐标系定义和安装方向 float roll_acc atan2f(ay, az) * 57.29578f; float pitch_acc atan2f(-ax, sqrtf(ay * ay az * az)) * 57.29578f; // 陀螺仪积分 加速度计修正 roll GYRO_ALPHA * (roll gx * DT) (1.0f - GYRO_ALPHA) * roll_acc; pitch GYRO_ALPHA * (pitch gy * DT) (1.0f - GYRO_ALPHA) * pitch_acc; }这里有几个点必须注意gx、gy、gz 是陀螺仪角速度单位是 °/s不是原始 LSB。前面读取时除以 131 那一步不能省。坐标系表达式依赖模块的安装方向。如果模块竖着装或者倒着装atan2 里的轴就要重新映射否则你转 pitch 它出 roll。α 取 0.98配合 dt0.01s相当于加速度计对姿态的修正截止频率大约在 0.3Hz 左右。也就是说只有低频段信任加速度计高频段完全信任陀螺仪。如果你的应用动态很大比如做航模α 可以降到 0.95让加速度计更积极地参与修正。初始化时 roll、pitch 应先用加速度计的输出算一次初始角而不是从 0 开始慢慢收敛。4.4 采样率、截止频率和互补滤波的配合很多人在这一步会陷入一个误区采样率越高越好滤波系数随便配反正在线调。实际上采样率决定的是你能处理的信号带宽。MPU6050 内部 DLPF 默认带宽 44Hz意味着 44Hz 以上的信号已经被硬件滤掉一部分了。软件端再用 5Hz 截止频率的一阶低通相当于进一步收窄。对平衡车这种运动频率通常在 2Hz 以下的系统5Hz 软滤波完全够用。互补滤波的截止频率则和采样周期、α 直接相关近似关系是fc ≈ (1 - α) / (2π × dt)拿 0.98 和 0.01s 代进去fc 约 0.32Hz。这说明转角信号的快变成分被陀螺仪接管慢变成分由加速度计校准。如果你觉得姿态角被加速度计拉得过于软就增大 α如果觉得静止时姿态角受振动影响大就减小 α。这个公式比瞎调 α 靠谱得多。5. 实测中容易被忽略的坑从 I2C 到电源再到校准5.1 I2C 读数据时老出错的排查链路我见过不止一个人卡在读出来数据突然跳变这个问题上。如果你在串口波形里看到每隔几十个点就出现一个异常尖峰而且尖峰前后数据完全错乱十有八九是 I2C 通信质量不行而不是滤波参数不对。排查顺序我一般是这么走的第一把 I2C 时钟从 400kHz 降到 100kHz。MPU6050 官方支持 400kHz但杜邦线、面包板和模块上拉电阻组成的电路实际容性负载可能超标降到 100kHz 后波形瞬间干净。第二确认模块 SCL、SDA 上拉电阻存在。很多模块板载了 4.7k 或 10k 上拉但如果你用的是自己画的板子千万别漏。第三读取 WHO_AM_I寄存器 0x75确认地址正确。这个寄存器通常固定返回 0x68读到别的值说明 I2C 总线上的设备不是 MPU6050或者总线冲突。第四给每次HAL_I2C_Mem_Read设置合理超时并检查返回值。如果 I2C 总线卡住HAL 函数会一直阻塞在主循环里看起来就是程序跑着跑着不动了。另外提一句很多人在 Windows 设备管理器里遇到STM32 Virtual COM Port带叹号的问题那是虚拟串口驱动没装好不影响 I2C 数据本身但它会让你的 printf 打印失效干扰你看波形。调试串口建议用 DMA 发送避免阻塞。5.2 电源、杜邦线和电磁干扰对滤波效果的影响如果你把滤波调好后发现某些场合数据依然抖动猛烈先别怀疑算法。拿一个典型的电机场景来说PWM 驱动电机转动时电刷产生的火花会在电源线上感应出几十毫伏的毛刺这个毛刺经过杜邦线直接耦合进 MPU6050 的 VDD 和 I2C 信号线。我的处理办法是在 MPU6050 模块的 VDD 和 GND 之间并一个 10μF 电解电容和一个 0.1μF 陶瓷电容位置尽量靠近模块电源引脚。让 I2C 线避开电机驱动板的大电流回路在 PCB 上两侧铺地隔离。如果只能走杜邦线尽量把线缩短并且 SCL、SDA、VDD、GND 四根线用双绞方式走减少环路面积。这些看起来像是硬件工程师的事但对嵌入式软件开发者来说遇到滤波无效时排查一轮硬件节省的时间绝对比重新调参多得多。5.3 先校准再滤波顺序千万不能反过来滤波只能处理随机噪声处理不了系统偏差。如果你陀螺仪的零偏是 50 LSB/s不管用多强的低通滤波积分出来的角度都会稳定地向某个方向漂移。真正能让姿态稳定的是校准不是滤波。我的操作习惯是每次上电后先采样 1000 组静止数据计算陀螺仪三轴的均值作为零偏存到全局变量里之后每个读数都先减去这个零偏再做滤波和姿态解算#define CALIBRATION_SAMPLES 1000 void Gyro_Calibrate(float *gyro_offset_x, float *gyro_offset_y, float *gyro_offset_z) { float sum_x 0.0f, sum_y 0.0f, sum_z 0.0f; for (uint16_t i 0; i CALIBRATION_SAMPLES; i) { // 读取 gx、gy、gz单位 °/s sum_x gx; sum_y gy; sum_z gz; HAL_Delay(5); } *gyro_offset_x sum_x / CALIBRATION_SAMPLES; *gyro_offset_y sum_y / CALIBRATION_SAMPLES; *gyro_offset_z sum_z / CALIBRATION_SAMPLES; }加速度计的六面校准稍微麻烦一点但原理也一样每个轴分别朝上和朝下静止采样记录均值推算出零偏和增益误差。做一轮这样的校准你的陀螺仪静态漂移可以从每分钟几度降到每分钟零点几度效果比任何高级滤波算法都立竿见影。5.4 打印和计算的时序分配把滤波和姿态解算放进定时器中断里然后中断里再调用 printf这在 F103 上是灾难。printf 重定向到串口后一次打印几百甚至上千微秒采样周期直接乱套。我的做法是分三层定时器中断里只触发 I2C 读取读完把原始数据存入全局结构体。主循环里检测到新数据标志后做滤波和姿态解算。串口打印单独用每 10 次循环打印一次或者用 DMA 发送让打印过程不阻塞主循环。这样做的代价是姿态解算比采样时刻晚了一个主循环周期对于 100Hz 采样和几Hz 的运动带宽来说完全可以接受。我自己在调各类 MPU6050 项目时最后都会回到同一个习惯先花一小时把校准和电源做好而不是把希望寄托在滤波代码的复杂程度上。滤波只是锦上添花真正决定姿态数据可用性的是你对噪声来源的判断是否准确。
STM32+MPU6050滤波实战：从噪声分析到互补滤波调参
从串口第一次打出 MPU6050 原始数据时我的心态是崩溃的板子安安静静放在桌面上加速度计 Z 轴却在 16000 到 16800 LSB 之间来回跳陀螺仪静止状态也带着 ±3°/s 的抖动。那会儿我用的还是 STM32F103C8T6 最小系统板加一根十几厘米的杜邦线电源来自开发板的 USB 口。后来我才想明白MPU6050 的姿态解算难关从来不在能不能读到数据而在读到的数据你信几分、怎么让它变得能信。这篇就专门讲 STM32 搭配 MPU6050 做滤波这件事。我会把几种常用的滤波手段拆开讲透包括滑动窗口滤波、一阶低通滤波、互补滤波还会把我在实际工程里踩过的坑一并列出来。不管你是刚点亮 I2C 的新手还是已经在做平衡车、云台、四轴的老手这篇都值得你存下来当参考。1. 从原始 MPU6050 数据说起为什么你的波形一直在抖1.1 噪声到底从哪里来很多人的第一反应是传感器坏了或者买到假芯片。实际上MPU6050 的模拟前端在十年前已经算不错的水平了真正的问题通常不在芯片本身而在你给它的工作环境。第一类噪声来自 MEMS 结构内部的机械热噪声。加速度计内部是一个微型质量块它悬浮在硅结构上温度变化下无时无刻不在做布朗运动。这些微小位移被电容检测电路放大再经过 16 位 ADC 量化就成了你看到的随机跳动。这类噪声属于白噪声频谱比较平坦靠软件滤波能压下去。第二类噪声来自电源和地。开发板 USB 供电本身就有纹波电机启动、继电器吸合或者 PWM 驱动大电流负载时3.3V 上会叠加明显的毛刺。MPU6050 内部的 ADC 参考电压直接取自 VDD电源一动读数必然跟着动。这类噪声往往带有明显的周期性比如 50Hz、100Hz你可以在串口波形上观察到规律性的上下波动。第三类噪声最容易被忽略就是 I2C 采样时序抖动。MPU6050 的数据寄存器是内部同时锁存的但你通过 I2C 一个字节一个字节往外读如果总线速率不稳定或者中断插入导致两次读取之间间隔不均匀那数据本身没问题采样点在时间轴上却是不均匀的。后续做滤波、做姿态积分时把不均等的采样间隔当成均等间隔算等于给算法注入额外误差。1.2 先做噪声体检再谈滤波参数我建议拿到模块之后不要急着写滤波代码先用一组简单数据判断问题出在哪。把模块可靠固定让系统静止以 100Hz 到 200Hz 采样率连续读取 1000 组原始数据分别统计每个轴的均值、方差和最大最小值。判断方法很简单如果加速度计 Z 轴在静止时跳动幅度超过 ±200 LSB折合 ±0.012g优先检查电源和杜邦线而不是急着上滤波。如果陀螺仪静止时输出均值明显偏离零比如稳定在 50 或 -80那不是随机噪声是零偏需要做校准。如果 Z 轴静止均值严重偏离 16384 LSB±2g 量程下 1g 对应的数值大概率是传感器倾斜放置或者模块本身没有水平安装。做完这步你才能知道自己需要的到底是滑窗平滑、低通滤波还是先把 PCB 布局重做一遍。2. 动手前先选型滑动窗口、低通、互补滤波你到底需要哪一个2.1 滑动窗口滤波简单但别在闭环里用大窗口滑动窗口滤波就是取最近 N 个采样点的平均值每来一个新数据丢掉最旧的那个窗口向前滑动一格。实现最简单效果直观很多热词里提到的滑动窗口滤波模型说的就是它。代码逻辑不复杂但代价是输出延迟。N 个点的窗口输出相位大约滞后 N/2 个采样周期。假设采样率 100Hz、窗口 20 点那输出就滞后了 100ms。这个滞后对离线查看波形没影响但你要是拿滤波后的姿态角去做平衡车的 PD 闭环100ms 滞后基本等于先把系统判了死刑。我见过有人把窗口开到 50 甚至 100 点去平滑 MPU6050波形确实漂亮一通电姿态却慢半拍最后怎么调 PID 都调不稳。所以我把滑动窗口的定位说直接一点适合对实时性要求不高的数据预处理、离线显示、或者做降采样前的抗混叠。如果你只是想让串口波形好看它很合适。要是做闭环控制请把窗口控制在 8 点以内或者干脆用下面的一阶低通。2.2 一阶低通滤波嵌入式里性价比最高的选择一阶低通滤波对应的就是热词里的软件低通滤波simulink 一阶滤波模块rc 滤波截止频率这些概念。RC 硬件滤波器的传递函数搬到软件里就是那个经典的递推式y[n] y[n-1] α × (x[n] - y[n-1])也可以写成 y[n] α × x[n] (1 - α) × y[n-1]。这个 α 就是滤波系数也就是 RC 滤波器里R 和 C共同决定的那个时间常数。α 越大新数据占的权重越大输出越灵敏但滤波效果越弱α 越小历史数据占的权重越大输出越平滑但延迟越大。我的习惯是用截止频率 fc 来反算 α而不是拍脑袋随便写个数。这样写出来的滤波器才有物理意义α 1 - exp(-2π × fc × dt)其中 dt 是采样周期也就是 1 / 采样率。举个例子采样率 100Hzdt0.01s想把 10Hz 以上的毛刺滤掉fc 取 10Hz算出来 α≈0.466。把 fc 降到 5Hzα≈0.268。同样的采样率下fc 每往下降一半输出就更平滑一点。2.3 互补滤波姿态解算里的主力方案如果你最终想得到的是姿态角也就是 roll、pitch那滤波就不只是平滑一下数据那么简单了而是要把加速度计和陀螺仪两路各有缺点的数据融合起来。加速度计测量的是重力矢量静止时能很准地给出水平姿态但它怕振动。车身一颠或者电机一转加速度计测到的不只是重力还有运动加速度姿态角当场就乱飘。陀螺仪测角速度积分能得到角度短时间非常平滑不怕振动但它有积分漂移。即使零偏校准得很好积分几分钟后角度还是会慢慢跑偏。互补滤波干的事就是取长补短把陀螺仪积分出来的角度做高通处理保留快速响应的部分把加速度计算出来的角度做低通处理保留长期稳定的部分。两者一叠加就得到了既不飘、又抗振的姿态角。核心代码只有一行angle α × (angle gyroRate × dt) (1 - α) × accAngle这里的 α 一般取 0.95 到 0.98意思是 95% 信任陀螺仪的短时积分5% 信任加速度计的长期测量。你不需要上卡尔曼滤波那是小学级难度的解法。互补滤波在平衡车、云台、四轴上已经够用。3. STM32 读取 MPU6050 的工程落地从 I2C 细节到数据对齐3.1 初始化寄存器顺序和连续读取虽然这篇的主角是滤波但滤波之前的数据读取如果埋着雷后面全是白做。先说最基本的初始化顺序。MPU6050 的 I2C 地址取决于 AD0 引脚。接地时地址是 0x68接高时是 0x69。在 HAL 库中HAL_I2C_Mem_Read的地址参数要左移一位0x68 对应 0xD00x69 对应 0xD2。我常用的初始化寄存器序列如下// 唤醒传感器 uint8_t data 0x00; HAL_I2C_Mem_Write(hi2c1, 0xD0, 0x6B, I2C_MEMADD_SIZE_8BIT, data, 1, 100); // 采样率分频1000Hz / (7 1) 125Hz data 0x07; HAL_I2C_Mem_Write(hi2c1, 0xD0, 0x19, I2C_MEMADD_SIZE_8BIT, data, 1, 100); // DLPF 配置设置带宽约 44Hz data 0x03; HAL_I2C_Mem_Write(hi2c1, 0xD0, 0x1A, I2C_MEMADD_SIZE_8BIT, data, 1, 100); // 陀螺仪 ±250°/s 量程灵敏度 131 LSB/(°/s) data 0x00; HAL_I2C_Mem_Write(hi2c1, 0xD0, 0x1B, I2C_MEMADD_SIZE_8BIT, data, 1, 100); // 加速度计 ±2g 量程灵敏度 16384 LSB/g data 0x00; HAL_I2C_Mem_Write(hi2c1, 0xD0, 0x1C, I2C_MEMADD_SIZE_8BIT, data, 1, 100);读取时从 0x3B 地址一次性连续读 14 个字节。顺序是 ACCEL_X_H、ACCEL_X_L、ACCEL_Y_H、ACCEL_Y_L、ACCEL_Z_H、ACCEL_Z_L、TEMP_H、TEMP_L、GYRO_X_H、GYRO_X_L、GYRO_Y_H、GYRO_Y_L、GYRO_Z_H、GYRO_Z_L。uint8_t raw[14]; HAL_I2C_Mem_Read(hi2c1, 0xD0, 0x3B, I2C_MEMADD_SIZE_8BIT, raw, 14, 100); int16_t ax_raw ((int16_t)raw[0] 8) | raw[1]; int16_t ay_raw ((int16_t)raw[2] 8) | raw[3]; int16_t az_raw ((int16_t)raw[4] 8) | raw[5]; int16_t gx_raw ((int16_t)raw[8] 8) | raw[9]; int16_t gy_raw ((int16_t)raw[10] 8) | raw[11]; int16_t gz_raw ((int16_t)raw[12] 8) | raw[13]; float ax ax_raw / 16384.0f; float ay ay_raw / 16384.0f; float az az_raw / 16384.0f; float gx gx_raw / 131.0f; float gy gy_raw / 131.0f; float gz gz_raw / 131.0f;注意 16 位数据高低字节合并必须用int16_t做强转如果你只声明uint16_t负数值会被当成无符号数后面滤波直接算出错误结果。3.2 数据对齐问题高字节和低字节分家MPU6050 内部的数据寄存器6 轴数据其实是同一时刻锁存的。但如果你不是一次性连读 14 字节而是分两次读比如先读高字节过一会儿再读低字节中间夹了一个 1ms 的中断那高字节和低字节就可能不是同一时刻的样本。合并出来的数值会偶尔跳出一个明显异常的点。后面做滑动窗口或者低通滤波时这一个异常点会扰动输出好一阵子。类似的问题也会出现在使用硬件 I2C 的 DMA 模式时。DMA 传输期间如果被中断打断传输不完整数据对齐错位后面整串数值都会变。我的做法是在定时器中断里只置一个数据已就绪标志主循环检测到标志之后再发一次HAL_I2C_Mem_Read保证读取过程是原子化的。把传感器读取和计算姿态放在不同的时隙里能避免不少硬伤。4. 三种滤波的代码实现与实测调参笔记4.1 滑动窗口滤波实现与资源占用先给一个最精简的滑动窗口滤波结构体。下面的代码针对单轴数据多轴使用时为每个轴创建一个实例即可。#include stdint.h #define WINDOW_SIZE 12 typedef struct { int16_t buf[WINDOW_SIZE]; uint8_t idx; uint8_t cnt; int32_t sum; } MovingAvg; int16_t MovingAvg_Input(MovingAvg *avg, int16_t sample) { int16_t old; if (avg-idx WINDOW_SIZE) { avg-idx 0; } old avg-buf[avg-idx]; avg-buf[avg-idx] sample; avg-idx; if (avg-cnt WINDOW_SIZE) { avg-cnt; } avg-sum sample - old; return (int16_t)(avg-sum / avg-cnt); }这里用了一个环形缓冲的思想维护一个累加和sum每次更新时先把最老的样本减掉再加新样本最后直接取平均。窗口 12 点时单轴最大sum是 12 × 32768 393216int32_t 足够用。如果你一口气把窗口拉到 100 点那就要留意 int32_t 是否可能溢出尤其是陀螺仪原始数据在某些量程下也能达到接近 32768 的值。实测下来对 MPU6050 的加速度计数据用 12 点窗口静态波动能削掉大概 70%但输出滞后约 60ms按 100Hz 采样率算。做平衡车的时候我会换成 4 点窗口只做轻度平滑。4.2 一阶低通滤波实现与系数确定一阶低通的代码比滑动窗口更短typedef struct { float y; float alpha; } LowPass; float LowPass_Filter(LowPass *f, float x) { f-y f-alpha * x (1.0f - f-alpha) * f-y; return f-y; }使用之前记得用第一帧原始数据初始化f-y否则滤波器从 0 开始爬爬升过程会拖慢响应。我整理了一张常用系数表采样率统一按 100Hz、dt0.01s 计算方便直接查截止频率 fcα 计算值实际效果参考2 Hz0.118输出非常平滑适合静态姿态估计5 Hz0.268常见折中既平滑又不太滞后10 Hz0.466响应快适合姿态动态变化大的场景如果把采样率改成 200Hzdt0.005s同样 fc5Hz 时 α≈0.154。这里特别提醒一句提高采样率之后α 如果不跟着调小实际截止频率会被抬高滤波效果反而不如原来。每一处参数都要从截止频率出发去推导而不是凭手感来回试。在 STM32F103 这类没有 FPU 的 MCU 上如果你对速度有强迫症可以把 α 量化成定点数。把 α 放大 1024 倍算完再右移 10 位int32_t alpha_q10 274; // 0.268 * 1024 int32_t y_q10 0; // 上一次输出定点表示 int32_t x_q10 ax_raw; // ADC 原始值 y_q10 (alpha_q10 * x_q10 (1024 - alpha_q10) * y_q10) 10;不过说实话F103 就算跑 float 版本的低通滤波每轴一次也就几个周期完全不是瓶颈。真正吃资源的是后面姿态解算里的 atan2 和 sqrt。4.3 互补滤波的姿态解算代码互补滤波的姿态角更新核心代码很短但几个细节决定成败。#define GYRO_ALPHA 0.98f #define DT 0.01f float roll 0.0f; float pitch 0.0f; void AHRS_Complementary(float gx, float gy, float gz, float ax, float ay, float az) { // 加速度计计算姿态角注意坐标系定义和安装方向 float roll_acc atan2f(ay, az) * 57.29578f; float pitch_acc atan2f(-ax, sqrtf(ay * ay az * az)) * 57.29578f; // 陀螺仪积分 加速度计修正 roll GYRO_ALPHA * (roll gx * DT) (1.0f - GYRO_ALPHA) * roll_acc; pitch GYRO_ALPHA * (pitch gy * DT) (1.0f - GYRO_ALPHA) * pitch_acc; }这里有几个点必须注意gx、gy、gz 是陀螺仪角速度单位是 °/s不是原始 LSB。前面读取时除以 131 那一步不能省。坐标系表达式依赖模块的安装方向。如果模块竖着装或者倒着装atan2 里的轴就要重新映射否则你转 pitch 它出 roll。α 取 0.98配合 dt0.01s相当于加速度计对姿态的修正截止频率大约在 0.3Hz 左右。也就是说只有低频段信任加速度计高频段完全信任陀螺仪。如果你的应用动态很大比如做航模α 可以降到 0.95让加速度计更积极地参与修正。初始化时 roll、pitch 应先用加速度计的输出算一次初始角而不是从 0 开始慢慢收敛。4.4 采样率、截止频率和互补滤波的配合很多人在这一步会陷入一个误区采样率越高越好滤波系数随便配反正在线调。实际上采样率决定的是你能处理的信号带宽。MPU6050 内部 DLPF 默认带宽 44Hz意味着 44Hz 以上的信号已经被硬件滤掉一部分了。软件端再用 5Hz 截止频率的一阶低通相当于进一步收窄。对平衡车这种运动频率通常在 2Hz 以下的系统5Hz 软滤波完全够用。互补滤波的截止频率则和采样周期、α 直接相关近似关系是fc ≈ (1 - α) / (2π × dt)拿 0.98 和 0.01s 代进去fc 约 0.32Hz。这说明转角信号的快变成分被陀螺仪接管慢变成分由加速度计校准。如果你觉得姿态角被加速度计拉得过于软就增大 α如果觉得静止时姿态角受振动影响大就减小 α。这个公式比瞎调 α 靠谱得多。5. 实测中容易被忽略的坑从 I2C 到电源再到校准5.1 I2C 读数据时老出错的排查链路我见过不止一个人卡在读出来数据突然跳变这个问题上。如果你在串口波形里看到每隔几十个点就出现一个异常尖峰而且尖峰前后数据完全错乱十有八九是 I2C 通信质量不行而不是滤波参数不对。排查顺序我一般是这么走的第一把 I2C 时钟从 400kHz 降到 100kHz。MPU6050 官方支持 400kHz但杜邦线、面包板和模块上拉电阻组成的电路实际容性负载可能超标降到 100kHz 后波形瞬间干净。第二确认模块 SCL、SDA 上拉电阻存在。很多模块板载了 4.7k 或 10k 上拉但如果你用的是自己画的板子千万别漏。第三读取 WHO_AM_I寄存器 0x75确认地址正确。这个寄存器通常固定返回 0x68读到别的值说明 I2C 总线上的设备不是 MPU6050或者总线冲突。第四给每次HAL_I2C_Mem_Read设置合理超时并检查返回值。如果 I2C 总线卡住HAL 函数会一直阻塞在主循环里看起来就是程序跑着跑着不动了。另外提一句很多人在 Windows 设备管理器里遇到STM32 Virtual COM Port带叹号的问题那是虚拟串口驱动没装好不影响 I2C 数据本身但它会让你的 printf 打印失效干扰你看波形。调试串口建议用 DMA 发送避免阻塞。5.2 电源、杜邦线和电磁干扰对滤波效果的影响如果你把滤波调好后发现某些场合数据依然抖动猛烈先别怀疑算法。拿一个典型的电机场景来说PWM 驱动电机转动时电刷产生的火花会在电源线上感应出几十毫伏的毛刺这个毛刺经过杜邦线直接耦合进 MPU6050 的 VDD 和 I2C 信号线。我的处理办法是在 MPU6050 模块的 VDD 和 GND 之间并一个 10μF 电解电容和一个 0.1μF 陶瓷电容位置尽量靠近模块电源引脚。让 I2C 线避开电机驱动板的大电流回路在 PCB 上两侧铺地隔离。如果只能走杜邦线尽量把线缩短并且 SCL、SDA、VDD、GND 四根线用双绞方式走减少环路面积。这些看起来像是硬件工程师的事但对嵌入式软件开发者来说遇到滤波无效时排查一轮硬件节省的时间绝对比重新调参多得多。5.3 先校准再滤波顺序千万不能反过来滤波只能处理随机噪声处理不了系统偏差。如果你陀螺仪的零偏是 50 LSB/s不管用多强的低通滤波积分出来的角度都会稳定地向某个方向漂移。真正能让姿态稳定的是校准不是滤波。我的操作习惯是每次上电后先采样 1000 组静止数据计算陀螺仪三轴的均值作为零偏存到全局变量里之后每个读数都先减去这个零偏再做滤波和姿态解算#define CALIBRATION_SAMPLES 1000 void Gyro_Calibrate(float *gyro_offset_x, float *gyro_offset_y, float *gyro_offset_z) { float sum_x 0.0f, sum_y 0.0f, sum_z 0.0f; for (uint16_t i 0; i CALIBRATION_SAMPLES; i) { // 读取 gx、gy、gz单位 °/s sum_x gx; sum_y gy; sum_z gz; HAL_Delay(5); } *gyro_offset_x sum_x / CALIBRATION_SAMPLES; *gyro_offset_y sum_y / CALIBRATION_SAMPLES; *gyro_offset_z sum_z / CALIBRATION_SAMPLES; }加速度计的六面校准稍微麻烦一点但原理也一样每个轴分别朝上和朝下静止采样记录均值推算出零偏和增益误差。做一轮这样的校准你的陀螺仪静态漂移可以从每分钟几度降到每分钟零点几度效果比任何高级滤波算法都立竿见影。5.4 打印和计算的时序分配把滤波和姿态解算放进定时器中断里然后中断里再调用 printf这在 F103 上是灾难。printf 重定向到串口后一次打印几百甚至上千微秒采样周期直接乱套。我的做法是分三层定时器中断里只触发 I2C 读取读完把原始数据存入全局结构体。主循环里检测到新数据标志后做滤波和姿态解算。串口打印单独用每 10 次循环打印一次或者用 DMA 发送让打印过程不阻塞主循环。这样做的代价是姿态解算比采样时刻晚了一个主循环周期对于 100Hz 采样和几Hz 的运动带宽来说完全可以接受。我自己在调各类 MPU6050 项目时最后都会回到同一个习惯先花一小时把校准和电源做好而不是把希望寄托在滤波代码的复杂程度上。滤波只是锦上添花真正决定姿态数据可用性的是你对噪声来源的判断是否准确。
