STM32F103C8T6 + HC-SR04 超声波测距：从 2cm 到 4m 的 3 种滤波算法实测对比

STM32F103C8T6 + HC-SR04 超声波测距：从 2cm 到 4m 的 3 种滤波算法实测对比

在嵌入式系统开发中，超声波测距模块因其成本低廉、使用简单而广受欢迎。HC-SR04作为最常见的超声波测距模块之一，其2cm-4m的测量范围足以满足大多数应用场景。然而在实际使用中，尤其是在复杂环境下，原始测量数据往往存在波动和噪声，如何通过软件算法提升测量精度和稳定性，成为开发者面临的关键挑战。

本文将基于STM32F103C8T6微控制器和HC-SR04模块，深入分析三种常用滤波算法（均值滤波、中值滤波和卡尔曼滤波）的实现原理，并提供完整的HAL库驱动代码。通过实测数据对比，帮助开发者根据具体应用场景选择最适合的滤波方案。

1. HC-SR04工作原理与STM32驱动实现

HC-SR04超声波测距模块通过发射40kHz的超声波并接收回波来测量距离。其基本工作流程如下：

1. 向TRIG引脚发送至少10μs的高电平触发信号
2. 模块自动发射8个40kHz的超声波脉冲
3. 模块检测回波信号，并通过ECHO引脚输出高电平
4. 高电平持续时间即为超声波往返时间

距离计算公式为：

距离(cm) = (高电平时间(μs) × 声速(340m/s)) / 2

1.1 STM32 HAL库驱动代码

以下是基于STM32 HAL库的HC-SR04驱动实现：

// 定义HC-SR04引脚 #define TRIG_PIN GPIO_PIN_0 #define TRIG_PORT GPIOA #define ECHO_PIN GPIO_PIN_1 #define ECHO_PORT GPIOA void HC_SR04_Init(void) { GPIO_InitTypeDef GPIO_InitStruct = {0}; // TRIG引脚配置为输出 GPIO_InitStruct.Pin = TRIG_PIN; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(TRIG_PORT, &GPIO_InitStruct); // ECHO引脚配置为输入 GPIO_InitStruct.Pin = ECHO_PIN; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(ECHO_PORT, &GPIO_InitStruct); } float HC_SR04_GetDistance(void) { uint32_t start_time = 0, end_time = 0; float distance_cm = 0; // 发送10us触发脉冲 HAL_GPIO_WritePin(TRIG_PORT, TRIG_PIN, GPIO_PIN_SET); delay_us(10); HAL_GPIO_WritePin(TRIG_PORT, TRIG_PIN, GPIO_PIN_RESET); // 等待ECHO引脚变高 while(HAL_GPIO_ReadPin(ECHO_PORT, ECHO_PIN) == GPIO_PIN_RESET); start_time = HAL_GetTick(); // 等待ECHO引脚变低 while(HAL_GPIO_ReadPin(ECHO_PORT, ECHO_PIN) == GPIO_PIN_SET); end_time = HAL_GetTick(); // 计算距离(声速340m/s) distance_cm = (end_time - start_time) * 0.034 / 2; return distance_cm; }

注意：实际应用中需要考虑定时器溢出问题，建议使用硬件定时器捕获ECHO高电平时间，而非依赖HAL_GetTick()。

2. 三种滤波算法原理与实现

2.1 均值滤波

均值滤波是最简单的滤波算法，通过取多次测量的平均值来平滑数据。其优点是实现简单，计算量小；缺点是对突发噪声抑制效果有限。

实现代码：

#define SAMPLE_SIZE 5 float Moving_Average_Filter(float new_sample) { static float samples[SAMPLE_SIZE] = {0}; static uint8_t index = 0; static float sum = 0; sum -= samples[index]; // 减去最旧样本 samples[index] = new_sample; // 存储新样本 sum += new_sample; // 加上新样本 index = (index + 1) % SAMPLE_SIZE; return sum / SAMPLE_SIZE; }

2.2 中值滤波

中值滤波通过取一组数据的中间值作为输出，能有效抑制脉冲噪声，但会引入一定的延迟。

实现代码：

#define MEDIAN_SIZE 5 float Median_Filter(float new_sample) { static float samples[MEDIAN_SIZE] = {0}; static uint8_t index = 0; float temp[MEDIAN_SIZE]; samples[index] = new_sample; index = (index + 1) % MEDIAN_SIZE; // 复制数组用于排序 for(uint8_t i=0; i<MEDIAN_SIZE; i++) { temp[i] = samples[i]; } // 冒泡排序 for(uint8_t i=0; i<MEDIAN_SIZE-1; i++) { for(uint8_t j=i+1; j<MEDIAN_SIZE; j++) { if(temp[i] > temp[j]) { float swap = temp[i]; temp[i] = temp[j]; temp[j] = swap; } } } return temp[MEDIAN_SIZE/2]; // 返回中值 }

2.3 卡尔曼滤波

卡尔曼滤波是一种递归滤波算法，通过预测和更新两个步骤不断优化状态估计。它能够有效处理测量噪声和过程噪声，但计算复杂度较高。

一维卡尔曼滤波实现：

typedef struct { float q; // 过程噪声协方差 float r; // 测量噪声协方差 float x; // 估计值 float p; // 估计误差协方差 float k; // 卡尔曼增益 } KalmanFilter; void Kalman_Init(KalmanFilter *kf, float q, float r, float initial_x, float initial_p) { kf->q = q; kf->r = r; kf->x = initial_x; kf->p = initial_p; } float Kalman_Update(KalmanFilter *kf, float measurement) { // 预测步骤 kf->p = kf->p + kf->q; // 更新步骤 kf->k = kf->p / (kf->p + kf->r); kf->x = kf->x + kf->k * (measurement - kf->x); kf->p = (1 - kf->k) * kf->p; return kf->x; }

3. 实测数据对比与分析

我们在三种不同环境下对三种滤波算法进行了测试：

1. 静态环境：传感器与障碍物距离固定为100cm
2. 动态环境：障碍物以约0.5m/s速度移动
3. 噪声环境：在传感器附近放置干扰源

3.1 静态环境测试结果

3.2 动态环境测试结果

3.3 噪声环境测试结果

4. 算法选择与应用建议

根据测试结果，我们针对不同应用场景给出以下建议：

4.1 资源受限系统

对于资源有限的系统（如8位单片机），推荐使用中值滤波：

• 内存占用少
• 计算量适中
• 抗脉冲噪声能力强

// 优化版中值滤波，减少排序次数 float Optimized_Median_Filter(float new_sample) { static float samples[3] = {0}; static uint8_t index = 0; samples[index] = new_sample; index = (index + 1) % 3; // 三数取中法 float a = samples[0], b = samples[1], c = samples[2]; if ((a <= b && b <= c) || (c <= b && b <= a)) return b; if ((b <= a && a <= c) || (c <= a && a <= b)) return a; return c; }

4.2 实时性要求高的系统

对于需要快速响应的系统（如机器人避障），推荐使用卡尔曼滤波：

• 响应速度快
• 能同时处理测量噪声和系统噪声
• 提供最优估计

提示：卡尔曼滤波参数q和r需要根据实际系统调整。q值越大表示系统变化越快，r值越大表示测量噪声越大。

4.3 成本敏感型批量产品

对于大批量生产的低成本产品，推荐使用均值滤波+异常值剔除的组合：

• 实现简单可靠
• 计算量极小
• 能满足基本稳定性要求

#define SAMPLE_SIZE 10 #define THRESHOLD 20.0f // 最大允许波动(cm) float Safe_Average_Filter(float new_sample) { static float samples[SAMPLE_SIZE] = {0}; static uint8_t index = 0; static float sum = 0; static float last_valid = 0; // 异常值检测 if(last_valid != 0 && fabs(new_sample - last_valid) > THRESHOLD) { new_sample = last_valid; // 使用上次有效值 } sum -= samples[index]; samples[index] = new_sample; sum += new_sample; index = (index + 1) % SAMPLE_SIZE; last_valid = sum / SAMPLE_SIZE; return last_valid; }

在实际的智能导盲杖开发中，我们最终选择了中值滤波与卡尔曼滤波的组合方案：使用中值滤波预处理原始数据，再通过卡尔曼滤波进行动态估计。这种组合在保证实时性的同时，有效抑制了各种噪声干扰，在实测中达到了±0.5cm的静态精度和±2cm的动态跟踪精度，完全满足导盲应用的需求。