智能小车避障实战：用HC-SR04+STM32实现厘米级测距（附滤波算法）

在机器人开发领域，让机器“看见”并理解周围环境是自主移动的第一步。对于许多创客、机器人爱好者以及嵌入式开发者而言，超声波传感器是实现这一基础感知功能最经济、最直接的方案之一。其中，HC-SR04模块以其低廉的价格和简单的接口，成为了入门和中级项目的常客。然而，直接将模块读取的原始数据用于智能小车的实时避障，往往会遭遇数据跳动、环境干扰、测量盲区等一系列挑战，导致小车行为“神经质”或不稳定。

这篇文章将带你超越简单的“读取-计算”层面，深入一个完整的工程化实现。我们将以STM32微控制器为核心，不仅讲解如何驱动HC-SR04，更聚焦于如何通过软件算法和系统设计，将原始的、充满噪声的距离信息，提炼成稳定、可靠的决策依据。你会看到如何利用STM32CubeMX快速搭建项目框架，如何设计移动均值滤波与温度补偿算法来提升精度，并最终将这套感知系统封装成一个独立的、可复用的软件模块，甚至探讨其与更高级系统（如ROS）集成的可能性。我们的目标是，让你手中的智能小车，能像经验丰富的司机一样，对前方的障碍做出平滑、准确的反应。

1. 从原理到实践：HC-SR04与STM32的深度握手

HC-SR04的工作原理看似简单：触发、发射、接收、计时。但其时序精度和信号完整性直接决定了测距的准确性。与Arduino等平台常用的pulseIn函数不同，在STM32上，我们需要更精细地控制时间，并处理可能的中断冲突。

1.1 硬件连接与CubeMX基础配置

首先，确保你的硬件连接正确。HC-SR04需要5V供电（VCC），STM32的IO口通常是3.3V电平，但HC-SR04的TRIG触发引脚可以接受3.3V高电平，而ECHO回响引脚输出的5V高电平则需要注意：直接连接到STM32的3.3V容忍引脚（大部分STM32的IO口都是5V容忍的，但最好查阅具体型号的数据手册确认）。一个更稳妥的方案是使用一个简单的电阻分压电路，将ECHO的5V输出降至3.3V左右再接入MCU。

注意：在进行任何连接前，务必断开电源。带电插拔传感器模块可能导致瞬间电流冲击，损坏模块或MCU的IO口。

接下来，我们使用STM32CubeMX进行初始化。假设我们使用STM32F103C8T6（Blue Pill板）和TIM2进行高精度计时。

1. 系统核心与时钟：在RCC中，将高速外部时钟（HSE）设置为Crystal/Ceramic Resonator。在Clock Configuration标签页，配置系统时钟为72MHz。
2. GPIO配置：
   • 选择一个GPIO引脚（如PA1）作为TRIG输出，初始状态设为低电平，模式为Output Push Pull。
   • 选择一个GPIO引脚（如PA0）作为ECHO输入，模式为Input，不上拉也不下拉（因为外部电路可能已有处理）。
3. 定时器配置：启用一个通用定时器，如TIM2。将其时钟源设置为内部时钟，预分频器（PSC）设置为71（72MHz/(71+1) = 1MHz，即每个计数1微秒）。计数器周期（ARR）设置为最大值65535（对于测距足够）。这样，定时器每计数一次代表1微秒。
4. 生成代码：设置好项目名称、路径和IDE（如Keil或STM32CubeIDE），然后生成代码。

1.2 精准时序驱动与原始数据捕获

CubeMX生成代码后，我们需要编写驱动函数。核心在于精确产生10us以上的TRIG脉冲，并高精度地测量ECHO高电平的持续时间。

// ultrasonic.h
typedef struct {
    GPIO_TypeDef* trig_port;
    uint16_t trig_pin;
    GPIO_TypeDef* echo_port;
    uint16_t echo_pin;
    TIM_HandleTypeDef* htim;
    float distance_cm; // 经过处理后的距离值
    float temperature_c; // 环境温度，用于声速补偿
} Ultrasonic_HandleTypeDef;

void ULTRASONIC_Init(Ultrasonic_HandleTypeDef *hus);
float ULTRASONIC_Measure(Ultrasonic_HandleTypeDef *hus);

// ultrasonic.c
#include "ultrasonic.h"

void ULTRASONIC_Init(Ultrasonic_HandleTypeDef *hus) {
    // 确保TRIG引脚初始为低电平
    HAL_GPIO_WritePin(hus->trig_port, hus->trig_pin, GPIO_PIN_RESET);
    // 启动用于计时的定时器
    HAL_TIM_Base_Start(hus->htim);
    hus->temperature_c = 25.0; // 默认温度25摄氏度
}

float ULTRASONIC_Measure(Ultrasonic_HandleTypeDef *hus) {
    uint32_t start_time = 0, end_time = 0, pulse_width = 0;
    float distance = 0.0f;

    // 1. 发送10us的TRIG高脉冲
    HAL_GPIO_WritePin(hus->trig_port, hus->trig_pin, GPIO_PIN_SET);
    // 精确延时10微秒。这里使用DWT周期计数器或简单的NOP循环。
    // 为简化示例，使用HAL_Delay，但实际应用需用更精确的微秒延时函数。
    // 例如：for(int i=0; i<72; i++) { __NOP(); } @72MHz 约1us
    delay_us(10);
    HAL_GPIO_WritePin(hus->trig_port, hus->trig_pin, GPIO_PIN_RESET);

    // 2. 等待ECHO变为高电平（回波开始）
    while(HAL_GPIO_ReadPin(hus->echo_port, hus->echo_pin) == GPIO_PIN_RESET);
    start_time = __HAL_TIM_GET_COUNTER(hus->htim); // 获取定时器当前值

    // 3. 等待ECHO变为低电平（回波结束）
    while(HAL_GPIO_ReadPin(hus->echo_port, hus->echo_pin) == GPIO_PIN_SET);
    end_time = __HAL_TIM_GET_COUNTER(hus->htim);

    // 4. 处理定时器溢出
    if (end_time >= start_time) {
        pulse_width = end_time - start_time;
    } else {
        // 定时器溢出，计算溢出后的值
        pulse_width = (hus->htim->Instance->ARR - start_time) + end_time;
    }

    // 5. 计算距离（基础公式，未补偿）
    // 声速在25°C干燥空气中约为346 m/s，即34600 cm/s
    // 时间单位是微秒(us), 1秒=10^6微秒
    // 距离(cm) = 时间(us) * 声速(cm/us) / 2
    // 声速(cm/us) = 34600 / 10^6 = 0.0346
    // 所以：距离 = pulse_width * 0.0346 / 2 = pulse_width * 0.0173
    distance = pulse_width * 0.0173f;

    // 如果测量超时或无效（例如超过400cm），返回一个错误值，如-1
    if (pulse_width > 23200) { // 对应约400cm
        return -1.0f;
    }

    return distance;
}

这个基础测量函数返回的是单次、未经处理的原始距离值。在实际动态环境中，这个值会存在显著的波动。

2. 从噪声中提取信号：滤波算法的艺术

原始超声波数据就像未经打磨的玉石，夹杂着环境噪声、多径反射（声波经多次反射后返回）以及电路本身的干扰。直接使用这样的数据控制小车，会导致电机频繁启停、转向抖动。滤波算法的任务就是平滑这些波动，保留真实的距离变化趋势。

2.1 移动均值滤波：简单而有效的一线方案

移动均值滤波（Moving Average Filter）是处理此类随机噪声最常用的方法之一。其思想是维护一个固定长度的数据队列，每次新的测量值到来时，将其加入队列，并剔除最旧的值，然后计算队列中所有数据的平均值作为本次输出。

// filter.h
#define MOVING_AVG_WINDOW_SIZE 5

typedef struct {
    float buffer[MOVING_AVG_WINDOW_SIZE];
    uint8_t index;
    uint8_t count; // 实际已填充的数据个数
} MovingAverageFilter_HandleTypeDef;

void MovingAverage_Init(MovingAverageFilter_HandleTypeDef *filter);
float MovingAverage_Update(MovingAverageFilter_HandleTypeDef *filter, float new_value);

// filter.c
#include "filter.h"

void MovingAverage_Init(MovingAverageFilter_HandleTypeDef *filter) {
    for(int i=0; i<MOVING_AVG_WINDOW_SIZE; i++) {
        filter->buffer[i] = 0.0f;
    }
    filter->index = 0;
    filter->count = 0;
}

float MovingAverage_Update(MovingAverageFilter_HandleTypeDef *filter, float new_value) {
    float sum = 0.0f;

    // 将新值放入缓冲区
    filter->buffer[filter->index] = new_value;
    filter->index = (filter->index + 1) % MOVING_AVG_WINDOW_SIZE;

    // 更新有效数据计数，直到缓冲区被首次填满
    if(filter->count < MOVING_AVG_WINDOW_SIZE) {
        filter->count++;
    }

    // 计算平均值
    for(uint8_t i=0; i<filter->count; i++) {
        sum += filter->buffer[i];
    }
    return sum / filter->count;
}

窗口大小的选择是一门平衡艺术：

• 窗口太小（如3）：滤波效果弱，数据仍可能跳动。
• 窗口太大（如10）：滤波效果强，但会引入明显的滞后（延迟），当小车快速接近障碍物时，系统反应会变慢，可能导致碰撞。

对于以中等速度（如0.3-0.5米/秒）移动的智能小车，窗口大小设为5是一个不错的起点。你可以在实际路面测试中调整这个参数。

2.2 递推平均滤波与限幅滤波的组合拳

单纯的移动平均对偶尔出现的野值（明显偏离正常范围的错误数据）处理能力有限。一个野值会影响其后连续多个周期的输出。我们可以结合限幅滤波来增强鲁棒性。

限幅滤波的思想很简单：如果本次测量值与上次滤波输出值的差值超过一个预设的合理最大变化量，则认为本次测量可能是野值，予以剔除或进行特殊处理（例如用上次输出值代替，或等待下一次测量）。

#define MAX_DELTA_CM 20.0f // 假设两次有效测量间距离最大变化20cm（根据小车速度设定）

float Combined_Filter_Update(MovingAverageFilter_HandleTypeDef *ma_filter, float last_filtered_value, float new_raw_value) {
    float filtered_value;

    // 第一步：限幅判断
    if(fabs(new_raw_value - last_filtered_value) > MAX_DELTA_CM && last_filtered_value > 0) {
        // 差值过大，疑似野值。策略：丢弃，用上一次滤波值参与移动平均（或直接返回上一次滤波值）
        // 这里选择用上一次滤波值代替新值输入移动平均，保持数据流连续但抑制突变。
        filtered_value = MovingAverage_Update(ma_filter, last_filtered_value);
    } else {
        // 差值合理，正常进行移动平均滤波
        filtered_value = MovingAverage_Update(ma_filter, new_raw_value);
    }
    return filtered_value;
}

这种组合策略能有效应对因传感器短暂失锁、强声学干扰等产生的突发错误数据，使输出曲线更加平滑可靠。

3. 提升精度：不容忽视的温度补偿

我们一直使用的声速（~340 m/s）是一个在标准条件（20°C干燥空气）下的近似值。实际上，声速v与空气温度T（摄氏度）密切相关，其关系可以近似为： v = 331.4 + 0.606 * T (m/s)

在0°C到40°C的常见环境温度范围内，声速变化范围可达约331 m/s 到 353 m/s，差异超过6%。对于一个测量值为100cm的距离，如果不进行温度补偿，在最坏情况下可能引入超过6cm的系统误差！这对于要求厘米级精度的避障应用是不可接受的。

实现温度补偿有两种思路：

1. 使用独立温度传感器：如DS18B20、DHT11等，实时读取环境温度。
2. 使用固定补偿值：如果应用环境温度相对稳定（例如室内恒温），可以测量一次环境温度，然后在代码中设定一个固定的补偿系数。

我们采用第一种更通用的方法。假设我们已经通过某种方式（如I2C、单总线）获取了当前温度temperature_c。

// 在计算距离的函数中加入温度补偿
float ULTRASONIC_Measure_With_TempComp(Ultrasonic_HandleTypeDef *hus) {
    uint32_t pulse_width;
    float distance_cm;
    float speed_of_sound_cm_per_us; // 声速，单位 cm/微秒

    // ... (获取 pulse_width 的代码与之前相同) ...

    // 动态计算当前温度下的声速
    // v = 331.4 + 0.606 * T (m/s)
    // 转换为 cm/微秒: v_cm_per_us = (v_m_per_s * 100) / 1e6
    float speed_m_per_s = 331.4f + 0.606f * hus->temperature_c;
    speed_of_sound_cm_per_us = (speed_m_per_s * 100.0f) / 1000000.0f;

    // 计算距离
    distance_cm = pulse_width * speed_of_sound_cm_per_us / 2.0f;

    return distance_cm;
}

将温度补偿集成到你的滤波流程中，你会发现，在不同季节或早晚温差大的环境下，小车的测距一致性得到了显著提升。

4. 工程化整合与ROS节点封装思路

至此，我们拥有了驱动、滤波和补偿的核心组件。接下来，我们需要将这些组件整合成一个稳定、可维护的软件模块，并考虑其在更大系统（如机器人操作系统ROS）中的应用。

4.1 构建一个健壮的超声波传感器任务

在基于RTOS（如FreeRTOS）的智能小车项目中，通常会将传感器数据采集放在一个独立的任务中。这个任务以固定的频率（例如50Hz，即20ms一次）进行测量、滤波和更新。

// ultrasonic_task.c
#include "ultrasonic.h"
#include "filter.h"
#include "cmsis_os.h"

extern Ultrasonic_HandleTypeDef hultrasonic_front;
extern MovingAverageFilter_HandleTypeDef filter_front;
extern float g_filtered_distance_front_cm;

void UltrasonicTask(void const * argument) {
    float raw_distance, filtered_distance;
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xFrequency = pdMS_TO_TICKS(20); // 20ms周期

    ULTRASONIC_Init(&hultrasonic_front);
    MovingAverage_Init(&filter_front);

    for(;;) {
        // 1. 获取带温度补偿的原始距离
        raw_distance = ULTRASONIC_Measure_With_TempComp(&hultrasonic_front);

        // 2. 应用组合滤波（假设上次滤波值已保存在全局变量中）
        filtered_distance = Combined_Filter_Update(&filter_front, g_filtered_distance_front_cm, raw_distance);

        // 3. 更新全局距离变量（需考虑互斥锁保护）
        // osMutexWait(distance_mutex, osWaitForever);
        g_filtered_distance_front_cm = filtered_distance;
        // osMutexRelease(distance_mutex);

        // 4. 可选：通过串口或其他方式输出调试信息
        // printf("Raw: %.2f cm, Filtered: %.2f cm\n", raw_distance, filtered_distance);

        // 5. 精确延时，保证固定频率执行
        vTaskDelayUntil(&xLastWakeTime, xFrequency);
    }
}

4.2 面向ROS的节点封装

如果你的智能小车上层使用ROS（Robot Operating System），那么将STM32上的超声波驱动封装成一个ROS节点是非常有价值的。STM32作为下位机，可以通过串口（UART）或USB与运行ROS的上位机（如树莓派）通信。

一个典型的架构是：STM32负责所有的底层硬件驱动、实时滤波和补偿计算，然后以固定的频率将处理后的、稳定的距离数据打包发送给上位机。上位机的ROS节点接收数据，并将其发布到/ultrasonic/distance这样的ROS话题中，供导航、避障等节点订阅。

STM32端（数据发送）：

// 在UltrasonicTask中，将滤波后的数据通过串口发送
// 定义简单的协议，例如： "US1,123.45\n" 表示1号超声波传感器距离123.45cm
char tx_buffer[32];
sprintf(tx_buffer, "US1,%.2f\n", g_filtered_distance_front_cm);
HAL_UART_Transmit(&huart2, (uint8_t*)tx_buffer, strlen(tx_buffer), HAL_MAX_DELAY);

ROS端（Python节点示例）：

#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import Range
import serial

def ultrasonic_node():
    rospy.init_node('ultrasonic_driver', anonymous=True)
    pub = rospy.Publisher('/ultrasonic/front', Range, queue_size=10)
    ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=1) # 根据实际串口修改

    range_msg = Range()
    range_msg.radiation_type = Range.ULTRASOUND
    range_msg.field_of_view = 0.26 # 约15度，HC-SR04的典型值
    range_msg.min_range = 0.02 # 2cm
    range_msg.max_range = 4.0  # 4m

    while not rospy.is_shutdown():
        line = ser.readline().decode('ascii', errors='ignore').strip()
        if line.startswith('US1,'):
            try:
                distance = float(line.split(',')[1]) / 100.0 # 转换为米
                range_msg.range = distance
                range_msg.header.stamp = rospy.Time.now()
                pub.publish(range_msg)
            except ValueError:
                pass

if __name__ == '__main__':
    try:
        ultrasonic_node()
    except rospy.ROSInterruptException:
        pass

通过这种封装，超声波传感器就成为了ROS传感器生态中的一个标准组件，可以方便地与move_base、teb_local_planner等导航包集成，实现基于激光雷达/深度相机类似的代价地图更新，虽然精度和分辨率不及，但对于低成本的避障应用已经足够。

5. 路面实测与参数调优：理论到实践的最后一公里

所有的算法和代码都需要在实际的智能小车和真实环境中进行验证和调整。搭建一个简单的测试场地，设置不同材质（木板、纸箱、金属、窗帘）和角度的障碍物。

测试与记录：让小车静止，在固定距离（如30cm, 100cm）放置障碍物，记录原始数据和滤波后数据，观察波动情况。然后让小车以恒定速度向障碍物行驶，记录距离变化曲线，检查滤波算法是否引入了不可接受的延迟。

关键参数调优表：

参数	描述	调优建议与影响
测量周期	两次触发测量之间的时间间隔。	HC-SR04手册建议大于60ms。太短会导致上次回波干扰下次测量。在RTOS任务中，设置为60-100ms是安全的。
移动平均窗口大小	滤波队列的长度。	从5开始测试。静态测试时观察平滑度，动态测试（小车运动）时观察滞后。在滞后可接受的前提下，选择能有效平滑波动的最大窗口。
限幅滤波阈值 (MAX_DELTA_CM)	判断野值的最大距离变化量。	根据小车的最大速度设定。例如小车最大速度0.5m/s，测量周期0.1s，则最大理论位置变化为5cm。可将阈值设为理论值的2-3倍（如10-15cm），以容忍测量噪声和加速过程。
有效测量范围	判定为有效数据的距离范围。	HC-SR04标称2cm-400cm。但近端（<5cm）可能因回波过强而失真，远端（>300cm）回波可能过弱。可根据实际应用限定一个更可靠的范围，如5cm-250cm，此范围外的数据直接丢弃或标记为无效。
温度更新频率	更新声速补偿值的频率。	环境温度变化缓慢。不需要每次测距都读取温度。可以单独设置一个温度采样任务，每5-10秒更新一次即可，避免频繁的I2C/单总线通信影响测距时序。

在调试过程中，串口打印日志是你的最佳伙伴。同时绘制数据曲线（可以用Excel、Python的Matplotlib，或串口绘图工具）能直观地对比滤波前后的效果。

最终，当你看到智能小车在复杂的室内环境中，面对突然出现的椅子腿、半开的门或是倾斜的墙面，都能流畅地减速、绕行，而不是急停或撞上去时，你会感受到这些看似繁琐的滤波和补偿算法所带来的巨大价值。它们将一款廉价的传感器，变成了一个可靠的“感知器官”，这正是嵌入式系统开发的魅力所在——用软件和算法赋予硬件超越其本身规格的能力。