大家好我是CSDN的一名技术博主。在嵌入式开发、传感器数据处理尤其是无人机、平衡车、机器人姿态解算等领域你是否经常被传感器数据中的高频噪声所困扰是否在尝试融合陀螺仪和加速度计数据时发现角度漂移和抖动难以平衡本文将为你系统性地拆解低通滤波与互补滤波这两大核心算法从原理推导到代码实现再到参数整定与工程实践手把手带你从理论“速通”到实战落地。无论你是正在做课程设计的学生还是需要优化产品算法的工程师这篇文章都能为你提供一套清晰、完整、可直接复用的解决方案。1. 背景与核心概念为什么需要滤波在嵌入式系统和机器人控制中我们常常需要从各类传感器如MPU6050陀螺仪/加速度计、编码器、超声波等获取数据。然而原始传感器数据几乎总是伴随着噪声。这些噪声可能来源于传感器本身的电子热噪声、电源纹波、机械振动或环境电磁干扰。如果不加处理直接使用这些“脏数据”会导致系统控制不稳定、显示数值跳动、姿态解算发散等一系列问题。因此滤波成为了传感器数据处理中不可或缺的一环。它的核心目的就是从混合了噪声的信号中尽可能地提取出我们关心的真实信号。在姿态解算这个经典场景中我们通常面临两个核心传感器陀螺仪测量角速度积分后得到角度。其优点是对快速运动响应好、动态性能佳缺点是存在积分漂移即使传感器静止微小的零偏也会随着时间累积导致角度输出逐渐偏离真实值。加速度计测量比力在静态或低速情况下可以通过重力分量解算出姿态角滚转、俯仰。其优点是在低频段静态或准静态精度高、无漂移缺点是对高频振动和运动加速度极其敏感动态响应差。如何扬长避短融合两者的优点得到一个既稳定无长期漂移又灵敏动态响应好的姿态角这就是互补滤波要解决的核心问题。而低通滤波则是实现互补滤波以及预处理传感器数据的基础工具。简单来说低通滤波允许低频信号通过抑制高频噪声。像一个“筛子”滤掉高频的“毛刺”。互补滤波一种数据融合策略将两个或多个传感器的信息以“互补”的方式结合起来通常是一个传感器擅长低频另一个擅长高频融合后得到全频段都较好的估计。2. 环境准备与思想实验在深入代码之前我们先明确本文的实践环境。滤波算法是纯数学和信号处理问题其核心思想与编程语言无关。为了最广泛的适用性本文将使用C语言进行算法实现和演示因为它是嵌入式开发领域最通用的语言。所需环境与工具编程语言 ANSI C / C99。本文代码不依赖特定平台库可在任何支持标准C的编译器上运行。编译器 GCC, Keil, IAR, 或者任何你熟悉的嵌入式IDE。验证工具 为了直观展示滤波效果我们将生成模拟的传感器数据并建议使用Python (Matplotlib)或Excel来绘制波形图对比滤波前后效果。这不是必须的但强烈推荐。核心数学 只需要基础的四则运算和三角函数仅互补滤波最后一步用到atan2f。示例项目结构设想filter_demo/ ├── filters.c # 滤波算法实现 ├── filters.h # 算法头文件 ├── main.c # 主程序模拟数据并调用滤波器 ├── data_output.txt # 生成的模拟数据可选用于绘图 └── README.md版本说明本文重点在于算法原理和通用实现不依赖特定硬件或库的版本。所有代码片段力求简洁、独立你可以轻松移植到你的STM32、ESP32、Arduino或任何其他平台上。3. 核心原理与算法拆解3.1 一阶低通滤波从直观理解到公式一阶低通滤波器是最简单、最常用的滤波器。你可以把它想象成一个“惯性系统”比如一个装满水的杯子你突然晃动杯子水面不会立刻达到新的高度而是缓慢地变化。这个“缓慢变化”的特性就是滤除了快速抖动高频噪声。1. 公式推导后向差分法一阶低通滤波器的传递函数在连续域s域为H(s) 1 / (τs 1)其中τ是时间常数τ越大滤波效果越强截止频率越低响应越慢。在嵌入式系统中我们处理的是离散的数字信号。通过后向差分法将其离散化可以得到迭代公式Y(n) α * X(n) (1 - α) * Y(n-1)这就是算法实现的核心公式。2. 公式解读X(n)当前时刻的原始输入值带噪声的传感器读数。Y(n)当前时刻的滤波输出值。Y(n-1)上一时刻的滤波输出值。α滤波系数取值范围(0, 1]。它是截止频率和采样周期的函数。α越大越接近1信任当前输入X(n)越多滤波器响应快但抑噪能力弱。α越小越接近0信任历史输出Y(n-1)越多滤波效果好但滞后严重响应慢。3. 如何设置 αα与截止频率fc和采样时间dt的关系为α dt / (dt 1/(2π * fc))通常我们更关心截止频率fc。例如如果你的传感器数据中有效信号在5Hz以下噪声在20Hz以上你可以设置fc 5 Hz。假设你的控制循环周期dt 0.01s(100Hz)那么α ≈ 0.01 / (0.01 1/(2*3.14*5)) ≈ 0.01 / (0.01 0.0318) ≈ 0.239这意味着当前输入值的权重约为24%。3.2 二阶低通滤波简介一阶滤波器有时在陡峭度和相位延迟上不能满足要求。二阶低通滤波器提供了更陡的滚降特性衰减高频更快但设计和参数整定更复杂相位延迟也可能更大。一个常见的数字二阶低通滤波器是二阶巴特沃斯滤波器的双线性变换形式。其差分方程形式为Y(n) b0*X(n) b1*X(n-1) b2*X(n-2) - a1*Y(n-1) - a2*Y(n-2)其中系数b0, b1, b2, a1, a2需要通过期望的截止频率和采样频率计算得到。由于实现和参数整定相对复杂在资源有限的嵌入式系统中一阶滤波或互补滤波更为常见。本文重点放在一阶实现上。3.3 互补滤波融合的艺术互补滤波的核心思想非常直观对信任低频的传感器进行低通滤波对信任高频的传感器进行高通滤波然后将两者结果相加。在姿态解算中加速度计 估计姿态角如俯仰角angle_acc。它在低频时准确高频时噪声大。所以我们对它进行低通滤波只取它的低频成分。angle_lowpass LPF(angle_acc)陀螺仪 测量角速度gyro。积分得到角度变化angle_gyro gyro * dt。积分过程相当于一个对角速度的低通滤波不对恰恰相反陀螺仪积分角本身会累积低频的漂移误差而它对高频运动响应好。在互补滤波框架中我们通常构造一个高通滤波器来提取陀螺仪积分角中的高频即快速变化部分。 如何得到高通一个巧妙的方法是高通输出 原始信号 - 低通输出。 因此陀螺仪积分角的高通成分可以表示为angle_highpass angle_gyro - LPF(angle_gyro)。但更常见的直接推导形式如下。经典互补滤波算法一阶最广为人知的公式其实是由上述高低通思想推导出的一个简洁迭代形式用于直接融合角速度积分和加速度计角度angle (1 - α) * (angle_prev gyro * dt) α * angle_acc或者更常见地写成angle α * (angle_prev gyro * dt) (1 - α) * angle_acc注意α的定义可能互换本质相同让我们重新解读这个公式angle_prev gyro * dt 这是纯陀螺仪积分得到的角度预测。它包含了陀螺仪的全部信息好的动态响应和坏的漂移。angle_acc 这是加速度计解算的瞬间角度测量。它包含了加速度计的全部信息好的静态精度和坏的高频噪声。系数 α 这里的α与低通滤波中的α意义类似但作用对象是信任陀螺仪的程度。α越大例如0.98越信任陀螺仪的积分结果系统动态响应好但长期会缓慢漂移。(1-α)越大越信任加速度计的修正静态精度高但动态响应时会受到运动加速度的干扰。这个 α 本质上就是加速度计所接低通滤波器的系数而(1-α)对应陀螺仪积分所接高通滤波器的系数。因此互补滤波完美地将加速度计的低频特性和陀螺仪的高频特性结合了起来。参数α或通常所说的互补滤波系数Kp的选取就是在“动态响应性”和“静态抗干扰性”之间做权衡。4. 完整实战案例从模拟到实现接下来我们将用C语言实现一个完整的仿真。我们首先模拟一段含噪声的加速度计角度和陀螺仪角速度数据然后分别应用一阶低通滤波和互补滤波并对比效果。4.1 创建项目文件与头文件首先创建算法头文件filters.h。// filters.h #ifndef FILTERS_H #define FILTERS_H // 一阶低通滤波器结构体 typedef struct { float alpha; // 滤波系数 α float prev_output; // 上一次的输出值 } LPF_Filter; // 互补滤波器结构体 typedef struct { float angle; // 融合后的角度估计值 float bias; // 陀螺仪零偏估计可选用于动态补偿 float kp; // 互补滤波系数对应加速度计权重 (1-α) 或 α取决于实现 float ki; // 积分系数用于估计零偏可选 float dt; // 采样周期单位秒 } CompFilter; // 函数声明 void LPF_Init(LPF_Filter* filter, float alpha, float init_value); float LPF_Update(LPF_Filter* filter, float input); void CompFilter_Init(CompFilter* filter, float kp, float ki, float dt, float init_angle); float CompFilter_Update(CompFilter* filter, float accel_angle, float gyro_rate); #endif // FILTERS_H4.2 实现滤波算法然后创建源文件filters.c实现核心算法。// filters.c #include filters.h #include math.h // 为了使用 atan2f // 一阶低通滤波器初始化 void LPF_Init(LPF_Filter* filter, float alpha, float init_value) { filter-alpha alpha; filter-prev_output init_value; } // 一阶低通滤波器更新 float LPF_Update(LPF_Filter* filter, float input) { // 核心公式Y(n) α * X(n) (1 - α) * Y(n-1) float output filter-alpha * input (1.0f - filter-alpha) * filter-prev_output; filter-prev_output output; // 更新状态 return output; } // 互补滤波器初始化 (带PI修正的版本更优) void CompFilter_Init(CompFilter* filter, float kp, float ki, float dt, float init_angle) { filter-angle init_angle; filter-bias 0.0f; // 初始零偏设为0 filter-kp kp; // 比例系数对应加速度计修正的强度 filter-ki ki; // 积分系数用于估计并消除零偏 filter-dt dt; } // 互补滤波器更新 (带陀螺仪零偏估计的改进版) float CompFilter_Update(CompFilter* filter, float accel_angle, float gyro_rate) { // 1. 计算加速度计测量值与当前估计值的误差 // 注意这里误差是加速度计角度减去当前估计角度。有些实现会使用角速度误差本质类似。 // 更常见的做法是计算角速度误差 error (accel_angle - filter-angle) * kp // 然后对误差积分得到零偏补偿。 float error accel_angle - filter-angle; // 2. 使用PI控制器来修正陀螺仪读数 filter-bias filter-ki * error * filter-dt; // 积分项估计零偏 float gyro_corrected gyro_rate filter-kp * error filter-bias; // 比例项修正角速度 // 3. 使用修正后的角速度进行积分更新角度 filter-angle gyro_corrected * filter-dt; // 另一种更简洁的经典形式无零偏估计 // filter-angle filter-kp * (filter-angle gyro_rate * filter-dt) (1 - filter-kp) * accel_angle; return filter-angle; }代码解读LPF_Update函数严格实现了一阶低通滤波的迭代公式。CompFilter_Update函数实现了一个带陀螺仪零偏动态估计的改进互补滤波。这是工程中非常实用的技巧ki系数可以缓慢积分加速度计与陀螺仪之间的误差从而自动估计并补偿陀螺仪的静态零偏进一步抑制漂移。kp决定了加速度计修正的响应速度。4.3 主程序数据模拟与滤波测试现在我们编写main.c来模拟数据并测试滤波器。// main.c #include stdio.h #include stdlib.h #include math.h #include filters.h #define PI 3.14159265358979323846f #define SAMPLE_RATE 100 // 采样频率 100 Hz #define DT (1.0f / SAMPLE_RATE) // 采样周期 0.01秒 #define TOTAL_TIME 10 // 总仿真时间 10秒 #define SAMPLE_COUNT (TOTAL_TIME * SAMPLE_RATE) // 总采样点数 1000 // 模拟一个真实角度信号缓慢正弦波 阶跃 float get_true_angle(float t) { return 30.0f * sin(2 * PI * 0.5f * t) (t 5.0f ? 20.0f : 0.0f); // 0.5Hz正弦 5秒后阶跃 } // 模拟带噪声的加速度计角度测量 float get_accel_angle(float true_angle, float t) { // 加速度计测量 真实角度 高频噪声 运动加速度干扰仿真中简单用另一个高频信号模拟 float high_freq_noise 5.0f * sin(2 * PI * 10.0f * t); // 10Hz高频噪声 float motion_interference (t 2.0f t 2.5f) ? 15.0f * sin(2 * PI * 2.0f * (t-2.0f)) : 0.0f; // 2-2.5秒的干扰 return true_angle high_freq_noise motion_interference (rand()/(float)RAND_MAX - 0.5f)*2.0f; // 再加点随机噪声 } // 模拟陀螺仪角速度测量 (真实角速度的微分 零偏 噪声) float get_gyro_rate(float true_angle, float t, float prev_true_angle) { float true_rate (true_angle - prev_true_angle) / DT; // 近似真实角速度 float gyro_bias 0.5f; // 模拟一个固定的零偏单位度/秒 float noise (rand()/(float)RAND_MAX - 0.5f)*0.5f; // 随机噪声 return true_rate gyro_bias noise; } int main() { // 初始化滤波器 LPF_Filter lpf_acc; LPF_Init(lpf_acc, 0.1f, 0.0f); // 加速度计低通滤波α0.1 CompFilter comp_filter; // kp和ki需要调试。kp大则信任加速度计多响应快但易受干扰ki用于消除零偏通常很小。 CompFilter_Init(comp_filter, 0.1f, 0.001f, DT, 0.0f); // 打开文件用于记录数据方便用Python或Excel绘图 FILE *fp fopen(data_output.txt, w); if (fp NULL) { printf(无法创建数据文件\n); return -1; } fprintf(fp, Time(s)\tTrueAngle\tAccelAngle\tGyroRate\tLPF_Output\tCompFilter_Output\n); float prev_true_angle 0.0f; float true_angle, accel_angle, gyro_rate, lpf_output, comp_output; for (int i 0; i SAMPLE_COUNT; i) { float t i * DT; true_angle get_true_angle(t); accel_angle get_accel_angle(true_angle, t); gyro_rate get_gyro_rate(true_angle, t, prev_true_angle); // 应用低通滤波 lpf_output LPF_Update(lpf_acc, accel_angle); // 应用互补滤波 (注意互补滤波需要加速度计角度和陀螺仪角速度) comp_output CompFilter_Update(comp_filter, accel_angle, gyro_rate); // 记录数据 fprintf(fp, %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n, t, true_angle, accel_angle, gyro_rate, lpf_output, comp_output); prev_true_angle true_angle; } fclose(fp); printf(数据模拟完成已保存到 data_output.txt。\n); printf(可以使用Python (Matplotlib) 或 Excel 绘制图表查看滤波效果。\n); // 简单的Python绘图脚本示例注释形式 printf(\n--- 可选Python绘图脚本示例 ---\n); printf(import matplotlib.pyplot as plt\n); printf(import numpy as np\n); printf(data np.loadtxt(data_output.txt, skiprows1)\n); printf(t, true, acc, gyr, lpf, comp data.T\n); printf(plt.figure(figsize(12,8))\n); printf(plt.subplot(2,1,1)\n); printf(plt.plot(t, true, k-, labelTrue Angle, linewidth2)\n); printf(plt.plot(t, acc, r:, labelAccel (Noisy), alpha0.7)\n); printf(plt.plot(t, lpf, b-, labelLPF Output)\n); printf(plt.legend()\n); printf(plt.grid()\n); printf(plt.ylabel(Angle (deg))\n); printf(plt.title(Low Pass Filter Effect)\n); printf(plt.subplot(2,1,2)\n); printf(plt.plot(t, true, k-, labelTrue Angle, linewidth2)\n); printf(plt.plot(t, comp, g-, labelComplementary Filter Output)\n); printf(plt.legend()\n); printf(plt.grid()\n); printf(plt.xlabel(Time (s))\n); printf(plt.ylabel(Angle (deg))\n); printf(plt.title(Complementary Filter Effect)\n); printf(plt.tight_layout()\n); printf(plt.show()\n); return 0; }4.4 编译、运行与结果分析编译 将filters.c,filters.h,main.c放在同一目录使用你喜欢的C编译器编译。例如使用GCCgcc -o filter_demo main.c filters.c -lm-lm选项用于链接数学库atan2f在标准库中通常不需要但显式链接是好习惯。运行 执行生成的可执行文件。./filter_demo程序会生成一个data_output.txt文件包含时间、真实角度、模拟的加速度计角度、陀螺仪角速度、低通滤波输出和互补滤波输出。可视化 将data_output.txt拷贝到有Python环境的地方运行上面main.c中打印的Python脚本或类似脚本你将看到类似下图的对比效果。预期结果分析第一张图低通滤波 红色的虚线是带高频噪声和脉冲干扰的加速度计原始数据蓝色实线是低通滤波后的结果。你可以看到高频噪声被有效抑制曲线变得平滑。但在2-2.5秒的运动干扰期间滤波后的信号依然会受到影响并且存在明显的相位滞后峰值比真实信号晚出现。第二张图互补滤波 绿色实线是互补滤波的输出。它应该能够很好地跟踪真实角度黑色实线的缓慢变化和阶跃变化。在2-2.5秒的干扰期间由于互补滤波主要信任陀螺仪的高频特性因此受加速度计干扰的影响远小于纯低通滤波。在整个10秒内没有出现明显的漂移得益于加速度计的低频修正和零偏估计ki的作用。5. 常见问题与参数整定指南5.1 低通滤波常见问题问题现象可能原因解决思路滤波后信号滞后严重系统响应变慢。截止频率fc设置过低或滤波系数α太小。增大α提高截止频率。需要在噪声抑制和响应速度间权衡。滤波效果不明显输出仍有大量噪声。截止频率fc设置过高或滤波系数α太大。减小α降低截止频率。检查噪声频率是否低于截止频率。滤波器输出出现“阶跃”或不平滑。初始化时prev_output设置不当如为0与第一个输入值差异过大。使用第一个输入值或一个合理的估计值来初始化prev_output。定点数实现时出现精度丢失或溢出。α和(1-α)用定点数表示时精度不足或中间计算结果溢出。使用更高精度的定点数如Q格式或直接使用浮点数。检查运算顺序。5.2 互补滤波参数整定互补滤波的性能极度依赖于参数kp和ki。整定是一个“试凑”加分析的过程。确定采样时间dt 这是最关键的硬件参数必须准确。它通常是你的控制循环或数据读取周期的固定值。初步设置kpkp决定了加速度计修正的强度。从一个小值开始尝试例如0.01。调大kp 系统会更信任加速度计静态对准快但动态运动时受加速度干扰更明显可能产生“晃动”。调小kp 系统更信任陀螺仪动态性能好但静态时角度会缓慢漂移。经验值 对于大多数无人机、平衡车应用kp通常在0.05到0.2之间。可以手持模块缓慢旋转和快速晃动观察角度输出是否跟手且稳定。精细调整kiki用于积分角度误差估计并补偿陀螺仪的零偏。ki必须非常小例如0.001或更小。调大ki 零偏估计和补偿更快但可能将缓慢的合法转动误判为零偏导致“积分饱和”或响应变差。调小ki 零偏估计慢长期漂移消除得慢。技巧 可以先设ki0调好kp。然后让系统静止几十秒观察角度是否漂移。如果有固定方向的缓慢漂移再逐渐增加ki直到漂移消失。调试方法静态测试 模块静止观察输出角度是否稳定长期几分钟是否漂移。动态测试 缓慢旋转模块观察输出是否平滑跟随。快速晃动观察输出是否出现大的尖峰受加速度干扰。阶跃响应 将模块快速翻转90度观察角度曲线是否快速到达新位置响应速度是否有超调或振荡kp可能太大稳定后是否准确静态精度6. 最佳实践与工程建议将算法从仿真移植到实际工程中需要考虑更多细节。采样时间dt必须精确且恒定 互补滤波中的积分angle gyro * dt对dt的准确性非常敏感。务必使用硬件定时器来确保稳定的采样周期避免使用delay()或非精确循环。传感器数据预处理单位统一 确保陀螺仪读数的单位是弧度/秒或度/秒并与你的角度计算单位一致。常用弧度制。量程对齐 读取的ADC值或原始数据要按数据手册转换为物理量。校准 上电后在静止状态下采集一段时间的陀螺仪数据计算均值作为静态零偏并在后续读数中减去。这是最重要的步骤之一加速度计倾角计算// 计算俯仰角 (pitch) 和滚转角 (roll) // accel_x, accel_y, accel_z 为加速度计三轴数据单位已转换为 g float pitch atan2f(-accel_x, sqrtf(accel_y*accel_y accel_z*accel_z)); // 弧度 float roll atan2f(accel_y, accel_z); // 弧度 // 注意此公式仅在静态或匀速运动时准确。当存在线性加速度时解算误差很大。初始对准 系统启动时由于陀螺仪积分角度为0而加速度计可以立刻给出一个粗略角度。通常用加速度计解算的角度来初始化互补滤波器的angle。float init_angle atan2f(accel_y, accel_z); // 例如初始化滚转角 CompFilter_Init(comp_filter, kp, ki, dt, init_angle);处理奇异点 当使用欧拉角时万向节锁是不可避免的问题。互补滤波通常用于小角度或全角度范围但不过顶的应用如平衡车、四轴。如果需要全姿态应考虑升级到四元数和梯度下降法、Mahony或Madgwick滤波。浮点数与定点数 在低端MCU上浮点运算可能较慢。可以考虑使用定点数运算Q格式来提升速度。但务必注意精度和溢出问题。滤波器结构体封装 如本文所示使用结构体来保存滤波器的状态如prev_output,angle,bias。这使代码更清晰并且可以方便地创建多个独立的滤波器实例例如对X、Y、Z轴分别滤波。性能监控 在调试时除了观察最终角度还可以将误差error、估计的零偏bias等内部变量通过串口输出便于分析滤波器内部工作状态。7. 总结与进阶方向通过本文我们系统地完成了从低通滤波到互补滤波的理论学习和代码实战。你现在应该能够理解原理 清楚一阶低通滤波的离散化公式及其物理意义掌握互补滤波融合高低频信息的思想。实现算法 用C语言编写出结构清晰、可移植的一阶低通滤波和互补滤波函数。参数整定 知道如何调整α,kp,ki等参数来平衡静态精度、动态响应和抗干扰性。工程移植 了解将算法应用到真实嵌入式项目中的关键步骤和注意事项。下一步你可以探索以下更进阶的方向卡尔曼滤波 互补滤波可以看作是一种简化版的卡尔曼滤波。学习卡尔曼滤波能让你从最优估计的理论层面更好地理解传感器融合并能处理更复杂的噪声模型。四元数与姿态解算 当需要全姿态3D旋转且避免万向节锁时必须使用四元数。学习如何用四元数表示旋转并实现基于四元数的互补滤波或梯度下降法如Madgwick、Mahony算法。自适应滤波 根据运动状态动态调整滤波参数。例如当检测到剧烈运动时自动减小对加速度计的信任度减小kp。多传感器融合 引入磁力计来修正航向角Yaw形成完整的AHRS航姿参考系统。滤波是信号处理和嵌入式控制的基石希望这份详细的指南能成为你项目中的得力助手。在实际动手调试的过程中你可能会遇到各种意想不到的情况那时不妨再回来看看原理和参数整定的思路。实践出真知祝你调试顺利