写这篇教程的起因很简单我在调试一个基于MPU6050的两轮自平衡小车时读出来的原始数据简直没法看。角度在静止状态下能自己飘出两三度加速度计的数值上叠加了一堆高频毛刺陀螺仪零漂更是随着温度和时间不断变化。一开始我以为是传感器坏了换了模块、查了接线、调了I2C速率问题依旧。后来才意识到问题根本不在硬件上而是我没有对原始数据做任何滤波处理。MPU6050这颗六轴传感器几乎是入门姿态控制、平衡小车、云台稳定器绕不开的器件但绝大多数教程只教你如何读原始数据没人告诉你读出来之后该怎么办。这篇文章把我实际调试中踩过的坑、试过的方案、最终跑通的滤波思路全部整理出来。内容从为什么必须滤波讲起然后给出滑动窗口均值滤波、一阶低通滤波、互补滤波三种方案的完整STM32代码和调参经验最后附上我在调试中遇到的高频问题和排查方法。适合正在做平衡小车、机械臂、手势识别或者任何需要稳定的姿态数据的同学参考。1. 项目整体思路与滤波方案选型1.1 为什么MPU6050原始数据非得滤波不可先看MPU6050的输出特性。加速度计输出的是三轴加速度值单位是g它最致命的弱点是高频噪声特别大。你用手轻轻碰一下桌面示波器抓到的数据瞬间能跳出去几十甚至上百个LSB。这是因为加速度计本质上是一个质量块加电容式检测结构任何微小的机械振动都会在信号上叠加干扰。而且电机转动、PWM驱动产生的电磁干扰也会耦合到传感器输出上。陀螺仪输出的是三轴角速度单位是度每秒。它的噪声水平比加速度计低一些但有一个更头疼的问题——零漂。标称零速率输出258 LSB/deg/s理论上静止时输出应该是0但实际上因为制造误差和温度漂移静止时输出常常在几十到几百LSB之间波动。换算成角速度就是每秒零点几度到几度。如果直接对陀螺仪角速度做积分求角度这个零漂会被不断累积几秒钟之后角度就完全失真了。另外还有一层问题。I2C读取本身也会引入噪声。MPU6050只有8位数据线16位的原始数据要分两次读读高字节和读低字节之间传感器还在实时更新如果正好赶上数据更新瞬间读出来的两个字节可能来自不同的采样时刻造成偶发的大幅度跳变。我实测过在主循环里用轮询方式读取不加任何滤波时静止状态下加速度计Z轴的跳动范围能达到80到120个LSB换算成角度大概是2到3度。这个精度的数据直接用来做闭环控制系统会一直抖根本稳不下来。所以滤波不是可选项而是必选项。问题是怎么滤波才能既去掉噪声又不牺牲实时性。下面是我的选型过程。1.2 各种滤波方案的优缺点对比我前后试过四种方案简单均值滤波、滑动窗口均值滤波、一阶低通滤波也叫指数滑动平均、互补滤波。它们各有各的适用场景关键是搞清楚自己要的是什么东西。简单均值滤波就是把连续采样的N个值加起来除以N。实现最容易但有一个致命缺点每次输出都需要重新累计N个数据相当于把输出频率降到了原来的1/N。如果系统需要200Hz的输出更新率而传感器最高只能到1kHz采样率那N最多只能到5。更关键的是它没有递推结构每一个新输出都要重新算一遍N个数的和实时性差。滑动窗口均值滤波是对简单均值滤波的改进。它维护一个长度为N的窗口每进来一个新数据把最老的那个数据丢出去然后更新窗口内的平均值。计算量小实时性好每个采样点都能出一个滤波结果非常适合单片机这种资源有限的平台。一阶低通滤波本质上是一个递归结构本次输出等于上次输出乘以一个系数加上本次输入乘以另一个系数。它的优势是内存占用极小只需要两个变量而且截止频率可以通过系数灵活调节。但它的相位滞后比滑动窗口滤波更明显对阶跃输入的响应是渐进逼近而不是立即到位。互补滤波是另一个层面的东西。它解决的不是高频噪声问题而是加速度计和陀螺仪各自的缺陷互补问题。加速度计低频准长期稳定但高频噪声大陀螺仪高频准短期稳定但低频零漂大。互补滤波高通陀螺仪积分得到的角度、低通加速度计计算得到的角度再按权重相加同时得到高频和低频都可靠的姿态角。最终我的方案是对加速度计原始数据用滑动窗口均值滤波去掉高频毛刺对陀螺仪原始数据用滑动窗口均值滤波配合高通滤波抑制零漂然后把两组处理后的数据送进互补滤波做姿态解算。这套方案在平衡小车上跑下来角度波动控制在正负0.2度以内实时性完全能满足200Hz的控制频率需求。滤波方案噪声抑制能力实时性内存占用实现难度适用场景简单均值滤波中低中极低静态测量、低频数据滑动窗口均值中高中低实时控制、嵌入式滤波一阶低通高中极低低通用噪声抑制、信号平滑互补滤波高高低中姿态解算、惯性导航2. 滤波算法原理与STM32实现细节2.1 滑动窗口均值滤波的原理与代码实现滑动窗口均值滤波的思想可以类比成排队做核酸队伍固定长度是N个人每来一个新的人排在队尾队头的人离开窗口往前平移一格然后统计当前窗口里N个人的平均身高。在数据滤波里人就是采样值身高就是数值本身。实现上需要三个东西一个环形缓冲区存窗口内的数据、一个索引指针指向当前写入位置、一个累加器存窗口内所有数据的和。每来一个新数据先用累加器减去即将被覆盖的最老数据再把新数据写进去累加器加上新数据最后累加器除以窗口长度就是滤波输出。整个过程时间复杂度O(1)不管窗口多大耗时都一样。我用STM32的HAL库写了一个通用滑动窗口滤波函数直接操作数组和指针没有用任何动态内存分配避免堆碎片问题。#define WINDOW_SIZE 10 // 窗口长度根据实际采样率调整 typedef struct { int16_t buffer[WINDOW_SIZE]; uint8_t index; int32_t sum; } sliding_window_filter_t; void sliding_window_filter_init(sliding_window_filter_t *filt) { memset(filt-buffer, 0, sizeof(filt-buffer)); filt-index 0; filt-sum 0; } int16_t sliding_window_filter_apply(sliding_window_filter_t *filt, int16_t new_data) { // 减去将被覆盖的最老数据 filt-sum - filt-buffer[filt-index]; // 写入新数据 filt-buffer[filt-index] new_data; filt-sum new_data; // 更新索引环形回绕 filt-index; if (filt-index WINDOW_SIZE) { filt-index 0; } // 返回窗口平均值 return (int16_t)(filt-sum / WINDOW_SIZE); }注意几个细节。累加器用了int32_t而不是int16_t因为10个int16_t相加的最大值是10×32767327670int16_t根本放不下会溢出。这是很多新手最容易踩的坑。另外窗口长度取10是基于我的采样频率和期望的截止频率算出来的不是拍脑袋定的这个计算过程在2.3节详细说。实际使用中我为加速度计的X、Y、Z三轴分别定义一个滤波器实例放在一个结构体里管理这样代码清爽不易错。typedef struct { sliding_window_filter_t ax; sliding_window_filter_t ay; sliding_window_filter_t az; sliding_window_filter_t gx; sliding_window_filter_t gy; sliding_window_filter_t gz; } mpu6050_filter_group_t;2.2 一阶低通滤波器的推导和实现一阶低通滤波器的数字实现非常简洁核心就一行代码output alpha * input (1 - alpha) * output_prev;alpha越大滤波越轻响应越快alpha越小滤波越重响应越慢。这个公式推导过程在教科书里有一大堆变换实际工程应用只需要理解两个核心点alpha的取值范围是0到1它决定了截止频率这个滤波器是有相位滞后的alpha越小滞后越大会让控制闭环的反应变慢。截止频率和alpha的关系式是alpha T / (RC T)其中T是采样周期RC是滤波器时间常数。如果采样频率fs已知期望截止频率fc已知可以先用fc算出RC 1 / (2πfc)再用T 1/fs代入公式得到alpha。举个例子我控制回路跑200Hz也就是采样周期T 5ms想要滤除50Hz以上的振动噪声那么RC 1 / (2×3.14×50) ≈ 0.00318salpha 0.005 / (0.00318 0.005) ≈ 0.611。这意味着每个新采样值有61.1%的比例进入输出滤波作用不算太强但足以压掉大部分高频振动。实际代码里我用浮点计算alpha但在单片机上为了速度会预先算好alpha为浮点常量每个采样点做一次乘加运算开销非常小。typedef struct { float alpha; float output; } low_pass_filter_t; void low_pass_filter_init(low_pass_filter_t *filt, float alpha) { filt-alpha alpha; filt-output 0.0f; } float low_pass_filter_apply(low_pass_filter_t *filt, float input) { filt-output filt-alpha * input (1.0f - filt-alpha) * filt-output; return filt-output; }2.3 互补滤波做姿态解算的原理和完整代码加速度计可以直接通过反正切算俯仰角和横滚角计算方式很粗暴pitch_acc atan2(accel_y, sqrt(accel_x * accel_x accel_z * accel_z)) * 180.0 / M_PI; roll_acc atan2(-accel_x, accel_z) * 180.0 / M_PI;这样得到的角度在静态下很准但一旦运动起来线性加速度会混入测量值角度满屋乱跑。陀螺仪积分得到的角度则相反短期内非常平滑但零漂随时间累积会越飘越远。互补滤波就是二者的折中让陀螺仪主导短期变化让加速度计慢慢把漂移拉回来。我的实现里用了kP比例系数做互补滤波的加权。实际就是每一帧计算// 陀螺仪积分得到角度增量 delta_angle gyro * dt; // 融合加速度计计算的角度用kP控制修正速度 pitch pitch delta_angle * (1 - kP) (pitch_acc - pitch) * kP;注意这里第一项不是pitch delta_angle而是pitch delta_angle × (1-kP)加修正项。我调试的时候一开始写错了总是把积分项完全保留导致加速度计的修正力度不够角度长期回不到真实值。完整姿态解算代码如下#define M_PI 3.14159265358979323846 #define kP 0.35f // 互补滤波系数越大加速度计修正越强 #define RAD_TO_DEG 57.295779513082320876798154814105f typedef struct { float pitch; float roll; float dt; } attitude_t; void complementary_filter_update(attitude_t *att, float accel_x, float accel_y, float accel_z, float gyro_x, float gyro_y, float dt) { // 从加速度计计算姿态角 float pitch_acc atan2(accel_y, sqrt(accel_x * accel_x accel_z * accel_z)) * RAD_TO_DEG; float roll_acc atan2(-accel_x, accel_z) * RAD_TO_DEG; // 陀螺仪积分注意陀螺仪输出是度/秒乘以秒得到度 float gyro_pitch gyro_x * dt; float gyro_roll gyro_y * dt; // 互补融合 att-pitch (att-pitch gyro_pitch) * (1.0f - kP) pitch_acc * kP; att-roll (att-roll gyro_roll) * (1.0f - kP) roll_acc * kP; att-dt dt; }关于kP取值我试过的经验范围是0.2到0.5。kP太小角度会被陀螺仪零漂带着走长期跑会飘kP太大加速度计的高频噪声直接进入输出角度会抖。0.35是我在平衡车上相对折中的值动态响应和静态稳定性都还算理想。不同项目要根据动作剧烈程度重新调如果你做的是机械臂末端姿态检测运动速度快、加减速度大kP应该降到0.1左右优先相信陀螺仪如果是静态倾角传感器kP可以加大到0.5以上。2.4 滤波参数的计算方法窗口长度N和低通滤波alpha值不是拍脑袋定的它们由采样频率和目标截止频率共同决定。这里给出一个经验公式滑动窗口均值滤波器的等效截止频率约为fc fs / (2 * N)其中fs是采样频率N是窗口长度。我的控制频率是200Hz也就是fs200Hz。希望截止频率在20Hz左右那么N fs / (2×fc) 200 / (2×20) 5。但我实际用了10对应截止频率10Hz滤波更狠一些延后通过试验验证对系统稳定性没有影响。如果控制频率更高比如1kHz窗口长度可以用到20甚至更多滤波效果更好且滞后影响更小。低通滤波的alpha我按2.2节的方式算。不过要注意滑动窗口滤波本身已经引入了滞后再加一级低通滤波滞后会叠加。所以实际项目中我倾向于只用滑动窗口滤波去掉噪声姿态解算交给互补滤波不再额外加低通。只有对数据平滑度要求很高的场景比如显示用才会叠加低通滤波器。3. 实操过程从裸数据到干净姿态角的完整流程3.1 硬件接线与工程初始化我用的是STM32F103C8T6最小系统板加MPU6050模块I2C1接口。硬件接线非常简单SCL接PB6SDA接PB7VCC接3.3VGND接GND。这里有个小坑MPU6050的I2C地址是0x68还是0x69取决于AD0引脚的电位。AD0悬空或者接地是0x68接高电平是0x69。我遇到过买到模块上AD0被硬拉高的情况读不到数据排查了半天所以拿到模块建议先用I2C扫描工具确认一下地址。工程直接用STM32CubeMX生成选好芯片型号后配置I2C1为快速模式400kHz串口1用于数据输出调试打开一个定时器做固定采样频率触发。定时器我选了TIM2配置为100Hz更新中断在中断里读取MPU6050并执行滤波和姿态解算。用定时器中断做数据采样比在主循环里延时可靠得多采样时间间隔稳定滤波效果才有保障。CubeMX配置要点RCC使能HSE外部晶振时钟树设为最高72MHzI2C1配置为Fast Mode时钟速度400kHzUSART1配置为115200-8-N-1使能中断TIM2配置为1kHz内部时钟、预分频7200-1、自动重载值10-1产生100Hz更新中断使能I2C1、USART1、TIM2的全局中断生成工程后在main.c里添加MPU6050的驱动代码。驱动分三层底层I2C读写函数、MPU6050寄存器操作函数、上层数据读取函数。底层读写用HAL库的HAL_I2C_Mem_Read和HAL_I2C_Mem_Write非常方便。3.2 MPU6050初始化和数据读取代码MPU6050上电后需要先等100ms让它内部时钟稳定然后执行以下初始化唤醒传感器退出休眠模式、配置陀螺仪量程为±250deg/s、配置加速度计量程为±2g、配置数字低通滤波器DLPF为21Hz、关闭I2C总线上的其他设备。这里重点说DLPF配置。MPU6050内部自带一个数字低通滤波器配置寄存器为0x1A。DLPF可以设置不同的截止频率比如0x06对应5Hz0x01对应184Hz0x00对应260Hz。需要注意的是DLPF会同时作用于加速度计和陀螺仪而且会引入额外延迟。我的做法是把DLPF设为0x01约184Hz仅用来滤掉高频电气噪声剩下的机械振动噪声交给软件滤波处理。完整初始化代码void mpu6050_init(void) { uint8_t val; HAL_Delay(100); // 唤醒传感器 val 0x01; HAL_I2C_Mem_Write(hi2c1, MPU6050_ADDR, 0x6B, 1, val, 1, 100); HAL_Delay(50); // 配置陀螺仪量程为正负250度每秒寄存器0x1B val 0x00; HAL_I2C_Mem_Write(hi2c1, MPU6050_ADDR, 0x1B, 1, val, 1, 100); // 配置加速度计量程为正负2g寄存器0x1C val 0x00; HAL_I2C_Mem_Write(hi2c1, MPU6050_ADDR, 0x1C, 1, val, 1, 100); // 配置DLPF为184Hz寄存器0x1A val 0x01; HAL_I2C_Mem_Write(hi2c1, MPU6050_ADDR, 0x1A, 1, val, 1, 100); // 关闭I2C Master模式寄存器0x6A val 0x00; HAL_I2C_Mem_Write(hi2c1, MPU6050_ADDR, 0x6A, 1, val, 1, 100); }数据读取要注意MPU6050的数据寄存器从0x3B开始连续存放依次是ACCEL_XOUT_H、ACCEL_XOUT_L、ACCEL_YOUT_H、ACCEL_YOUT_L、ACCEL_ZOUT_H、ACCEL_ZOUT_L、TEMP_OUT_H、TEMP_OUT_L、GYRO_XOUT_H、GYRO_XOUT_L、GYRO_YOUT_H、GYRO_YOUT_L、GYRO_ZOUT_H、GYRO_ZOUT_L一共14个字节。用连续读一次把14字节全读出来然后拆成6个16位值比分别读6次效率高得多。I2C连续读不会遇到数据更新竞态问题因为MPU6050在读取一组数据时内部有锁定机制。数据拼接时有个大坑因为加速度计和陀螺仪的输出是有符号16位整数直接移位拼接会出问题。高字节在低字节前面正确的拼法是int16_t combine_bytes(uint8_t high, uint8_t low) { return (int16_t)((high 8) | low); }如果写成 (uint16_t)((high 8) | low) 然后把结果赋值给int16_t变量在某些编译器上会报警告。严谨的做法是先把高字节转成int16_t再移位或者像上面这样强转。uint8_t左移8位在C语言中会被提升为int再移所以结果没问题但为了消除编译器警告我还是加了显式转换。3.3 完整滤波流程代码前面各种滤波器的初始化、定时器中断里的整合、数据输出逻辑我放在一起形成完整的处理链路。定时器中断每10ms触发一次在中断里做四件事读取MPU6050原始数据、滑动窗口滤波、互补滤波姿态解算、通过串口发送结果。// 全局变量 sliding_window_filter_t filt_ax, filt_ay, filt_az; sliding_window_filter_t filt_gx, filt_gy, filt_gz; low_pass_filter_t lp_pitch, lp_roll; attitude_t attitude; void TIM2_IRQHandler(void) { if (TIM2-SR TIM_SR_UIF) { TIM2-SR 0; uint8_t raw_data[14]; int16_t accel_x, accel_y, accel_z; int16_t gyro_x, gyro_y, gyro_z; float ax, ay, az, gx, gy, gz; float dt; // 1. 读取原始数据 HAL_I2C_Mem_Read(hi2c1, MPU6050_ADDR, 0x3B, 1, raw_data, 14, 10); accel_x ((int16_t)raw_data[0] 8) | raw_data[1]; accel_y ((int16_t)raw_data[2] 8) | raw_data[3]; accel_z ((int16_t)raw_data[4] 8) | raw_data[5]; gyro_x ((int16_t)raw_data[8] 8) | raw_data[9]; gyro_y ((int16_t)raw_data[10] 8) | raw_data[11]; gyro_z ((int16_t)raw_data[12] 8) | raw_data[13]; // 2. 转换为实际物理值 // 加速度计满量程2g灵敏度16384 LSB/g ax accel_x / 16384.0f; ay accel_y / 16384.0f; az accel_z / 16384.0f; // 陀螺仪满量程250deg/s灵敏度131 LSB/deg/s gx gyro_x / 131.0f; gy gyro_y / 131.0f; gz gyro_z / 131.0f; // 3. 滑动窗口滤波 ax sliding_window_filter_apply(filt_ax, (int16_t)(ax * 16384.0f)) / 16384.0f; ay sliding_window_filter_apply(filt_ay, (int16_t)(ay * 16384.0f)) / 16384.0f; az sliding_window_filter_apply(filt_az, (int16_t)(az * 16384.0f)) / 16384.0f; gx sliding_window_filter_apply(filt_gx, (int16_t)(gx * 131.0f)) / 131.0f; gy sliding_window_filter_apply(filt_gy, (int16_t)(gy * 131.0f)) / 131.0f; gz sliding_window_filter_apply(filt_gz, (int16_t)(gz * 131.0f)) / 131.0f; // 4. 计算时间间隔 dt 0.01f; // 5. 互补滤波姿态解算 complementary_filter_update(attitude, ax, ay, az, gx, gy, dt); // 6. 输出结果 char buf[64]; sprintf(buf, pitch:%.2f roll:%.2f\n, attitude.pitch, attitude.roll); HAL_UART_Transmit(huart1, (uint8_t*)buf, strlen(buf), 10); } }这里滑动窗口滤波我重新做了个小技巧先在原始整数域做滤波再转成浮点物理值。为什么不直接用浮点做滑动窗口因为滑动窗口的累加器如果累加浮点数窗口大了会有浮点误差累积而且每次加法都要额外做浮点运算。整数域滤波效率更高结果也精确。代价是两次转换但STM32F103硬件浮点单元FPU处理这个转换很快几乎不影响实时性。3.4 滤波效果实测对比我用串口把数据发到PC端分别记录了三种情况下的数据不滤波原始数据、只用滑动窗口滤波后的数据、滑动窗口加互补滤波后的姿态角。测试条件是板子静止放在桌面上。原始加速度计X轴数据数值在正负84到正负210之间跳动换算成g就是约0.005g到0.013g的噪声看起来不大但换算成角度误差就有0.3度到0.8度。陀螺仪X轴静止输出在正负15到正负35 LSB之间波动换算成角速度就是0.11到0.27度每秒的零漂。滑动窗口滤波后窗口长度10加速度计X轴噪声峰值降到正负28个LSB大约是原来的七分之一。陀螺仪零漂波动降到正负6个LSB左右。作为对比我用仿真软件同时模拟了效果滑动窗口对白噪声的衰减幅度理论上可以达到sqrt(N)倍N10就是约3.16倍实测7倍说明原始噪声并不是纯白噪声含有较多低频分量滑动窗口对高频部分衰减更强。互补滤波输出的姿态角板子静止时pitch和roll角度波动范围控制在正负0.15度以内。给板子一个突然的扰动再放回桌面角度能在0.5秒内回到真实值附近过渡过程没有明显过冲。这个性能用在平衡小车的直立环控制上是完全够用的。串口调试波形如果要用可视化工具观察我习惯把数据通过串口发到PC用VOFA或者匿名上位机随意选一个来画波形。VOFA免费且跨平台支持JustFloat协议做滤波前后数据对比非常直观。4. 常见问题与排查技巧实录4.1 滤波后数据仍然很毛糙怎么办这是最常见的问题。滤波后数据还是跳动特别大先检查采样率是不是稳定。如果你的采样是靠主循环延时实现的那么采样间隔本身就存在抖动滤波效果会大打折扣。优先改用定时器中断采样固定采样频率是滤波的前提。再检查I2C总线速率和上拉电阻。MPU6050的数据线需要2.2k到4.7k的上拉电阻如果模块上没有STM32内部虽然可以配置上拉但驱动力有限I2C波形畸变会导致读取数据偶发的错误。用示波器看SDA和SCL的波形上升沿如果超过1微秒就说明上拉电阻太大或者总线上挂载设备太多。滤波参数也可能有问题。窗口长度N过小滤不干净过大则滞后太多。我调试时习惯先用串口把滤波前后的数据同时打印出来观察两个序列的相位差和幅值。如果滤波后的曲线比原始数据平整但对于真实角度变化的响应慢了半拍把窗口长度调小试试。如果滤波后还是毛糙加大窗口长度同时注意看控制闭环是否因此变得不稳定。4.2 零漂补偿的具体做法陀螺仪零漂是MPU6050的老大难问题。滑动窗口均值滤波能降低零漂的波动幅度但消不掉固定的零漂偏移。我在互补滤波之前先对陀螺仪原始数据做一次静态零漂校准。校准方法上电后让板子完全静止连续采样1000个陀螺仪输出取平均值作为零漂值。之后运行时每次读取陀螺仪数据都减去这个零漂值再送给滤波器和姿态解算。void gyro_calibrate(void) { float sum_x 0, sum_y 0, sum_z 0; int i; float gx_raw, gy_raw, gz_raw; for (i 0; i 1000; i) { // 读取原始陀螺仪值并转换为角速度 gx_raw read_gyro_x() / 131.0f; gy_raw read_gyro_y() / 131.0f; gz_raw read_gyro_z() / 131.0f; sum_x gx_raw; sum_y gy_raw; sum_z gz_raw; HAL_Delay(1); } gyro_offset_x sum_x / 1000.0f; gyro_offset_y sum_y / 1000.0f; gyro_offset_z sum_z / 1000.0f; }校准之后实际运行中温度变化还会引起零漂缓慢变化所以更完善的方案是动态零漂修正当检测到角速度绝对值长时间小于某个阈值比如0.1度每秒且加速度计模长接近1g时判定为静止状态用它当前的读数去更新零漂值。这个逻辑我放进了后续改进的计划里目前固定校准已经能应付大多数调试场景了。4.3 角度积分漂移越来越快的处理方案如果你只用陀螺仪积分求角度角度会不可避免地越飘越快。这不是陀螺仪坏了是物理规律决定的。解决方案有两个层面。第一层是软件层面用一个高频的加速度计角度来纠正积分漂移这就是互补滤波的用途。kP参数决定了纠正力度。如果姿态角回不到真实值别急着加大kP先检查加速度计计算角度的公式是否正确。我见过有人把atan2的两个参数写反导致姿态解算结果在某个角度区间突变误以为是滤波问题。第二层是算法层面的选择。互补滤波适合大多数入门场景但如果你的运动动态特别强加速度计输出受到剧烈运动干扰互补滤波也会被带偏。这时候需要上卡尔曼滤波或者Mahony姿态解算算法。卡尔曼滤波器是最优线性状态估计器能融合传感器数据和运动模型给出最优估计代价是实现复杂、参数调优困难而且STM32F103跑完整的6维卡尔曼姿态解算需要约200微秒每次对于100Hz的控制频率压力并不大。Mahony算法比卡尔曼简单通过比例积分控制器修正陀螺仪偏差是无人机飞控中最常用的姿态解算方案。这里我只做提示具体实现篇幅太长另外开一篇讲。4.4 实时性与滤波效果的平衡技巧滤波本质上是在用时间换精度。实时性高的控制回路对相位滞后极其敏感。我记得有一次把滑动窗口从10增加到30数据是平得多了平衡小车反而站不稳了因为角度变化被平滑掉了太多控制系统的相位裕度被压缩到接近零。一个心得是分层处理。高频控制回路直立环只做轻滤波保证实时性低频通信回路比如发送到上位机显示、蓝牙调试做重滤波保证观感。两者用不同的滤波器实例互不干扰。我在平衡小车上100Hz的直立控制环用窗口长度5的滑动窗口滤波显示用的另一个窗口长度15的实例。这样控制实时性和显示平滑性都保住了。还有一个细节是检查定时器中断优先级和阻塞时间。HAL_I2C_Mem_Read在阻塞模式下会忙等I2C传输完成100kHz I2C下读14字节大约需要1.2ms在100Hz中断频率下占用了12%的CPU时间。如果后续还要跑PID控制算法、编码器读取、PWM输出累积阻塞时间可能超过采样周期导致滤波失效。解决办法是用中断方式或DMA方式操作I2C把采样和计算分离开。不过这是另一个复杂的话题了建议先把阻塞模式的流程调通性能优化永远在功能正确之后进行。5. 项目扩展思路滤波调通之后能做的事情就多了。最直接的应用是平衡小车用互补滤波输出的pitch角做PD平衡控制滚转角可以用作转向辅助。云台稳定器则是完全用三轴角度加三轴角速度做闭环控制抑制手抖带来的高频扰动。还有手势识别、跌倒检测、姿态辅助导航本质上都是在滤波之后的干净数据上做特征提取和模式判断。我个人的建议是完成这一套滤波系统后请一定亲手做几次对比实验不加滤波直接积分加滑动窗口滤波加互补滤波三种情况分别记录姿态角的静置漂移和动态跟随数据。只有亲眼看过数据差异才能真正理解每种滤波器的价值在哪里。我在教新人的时候也是要求他们必须跑完这组对比实验才能碰控制算法没有干净可靠的姿态数据后面所有的控制逻辑都是建立在流沙之上。最后顺手再分享一个小技巧调试过程中把滤波参数做成可以在运行时通过串口修改的变量这样就不用每次都重新编译下载固件了。我通常是用串口接收固定格式的命令帧比如W10表示设置窗口长度为10P0.3表示设置互补滤波系数为0.3在串口中断里直接修改全局变量。整套调试效率能提升一个量级尤其是调参这种天天都要做的事。