HC-SR04超声波测距模块:卡尔曼滤波练习

HC-SR04 是一款经典的超声波测距模块，测距范围 2cm ~ 400cm，精度约 3mm。这篇主要不是单纯讲模块怎么用，而是拿它来练一遍卡尔曼滤波，顺便把工作原理、接线方法、MicroPython 实现和中值滤波配合思路一起记下来。

一、工作原理

HC-SR04 模块通过发射超声波并接收回波来测量目标距离，工作频率约为 40 kHz。

模块前方两个探头常被称为“两只眼睛”，一个负责发射，一个负责接收：

• Trig（发射端）：发送超声波脉冲，遇到目标后被反射。
• Echo（接收端）：接收回波，并输出与距离对应的脉宽信号。

二、参数

三、测距过程

1. 触发：MCU 给 Trig 一个 ≥10 µs 的高电平触发脉冲。
2. 发射：模块内部发送约 8 个周期的 40 kHz 超声波包。
3. 计时：Echo 从低变高并保持，直到回波返回；其高电平持续时间 t 就是声波往返时间。
4. 计算：根据 Echo 的高电平持续时间换算距离，超时则视为无回波。
5. 节拍：最大有效距离 4 m 时，工程上通常设置 30 ms 超时、60 ~ 100 ms 测量周期，避免回波串扰。

MCU 给 Trig 一个 10us 高电平脉冲后，模块开始发射超声波。Echo 引脚输出高电平，MCU 根据高电平持续时间换算目标距离。

本质上就是：测时间，再换距离。

时序图

3.1 距离公式

20℃ 空气中声速约为 343m/s，换算后约为 0.0343 \, cm/\mu s。

这里除以 2，是因为测到的时间是超声波的往返时间，也就是“去 + 回”。

粗略温度补偿可写成：

影响测量精度的因素主要有三类：

1. 温度因素：温度升高，声速会增加，距离换算结果也会变化。
2. 材质因素：布、海绵等软质材料吸声较强，容易让回波变弱，读数偏小或直接超时。
3. 入射角因素：目标表面倾斜过大时，反射波可能不会返回接收头，导致测不到目标。

3.2 一次完整通信最长时间

最大测距为 4 m，声波往返距离相当于 8 m，因此最长回波脉宽大约为：

工程上通常会把超时时间设置为 30 ms，测量周期设置为 100 ms 左右，这样更稳，也能避免前一次回波对下一次测量造成干扰。

四、接线

接线图

备注：

• 本文 MicroPython 示例使用 trig=GPIO4、echo=GPIO5。
• 3.3V 分压示例：Echo 串 R1=1k，分点到 GND 接 R2=2k，分点电压约 3.3V。
• 调试串口（滤波示例中）：UART1 TX=Pin(0)、RX=Pin(1)，波特率 115200，用于输出原始值、中值滤波值和卡尔曼滤波值到 VOFA+ 上位机。

五、MicroPython 基础代码

from machine import Pin, time_pulse_us
import time

trig = Pin(4, Pin.OUT)
echo = Pin(5, Pin.IN)

def distance():
    # 清Trig
    trig.off()
    time.sleep_us(2)
    # 发送10us的触发脉冲
    trig.on()
    time.sleep_us(10)
    trig.off()
    # 计算脉宽
    duration = time_pulse_us(echo, 1, 30000)
    if duration < 0:
        return None
    # 换算厘米 343m/s = 343*100cm/1000000us 声速20°空气中约为343
    distance = (duration * 0.0343) / 2
    print(duration)
    return distance

while True:
    d = distance()
    if d is None:
        print("Out of range")
    else:
        print("Distance: %.2f cm" % d)
    # 一次最长测量时间约：23ms，所以定为100ms测量周期很安全
    time.sleep_ms(100)

从示波器上可以看到，代码里虽然写的是 10us 延时，但实际测出来大约是 14us。这也说明了 MicroPython 作为解释型语言，在实时性上会受到 解释器执行开销、GPIO 切换开销 和 sleep_us 精度 的影响。

示波器图

代码执行结果

可以看到，代码打印出来的回波时间与示波器测得的 Echo 持续时间是基本吻合的。

六、滤波

超声波测距的原始数据通常会有轻微抖动，所以实际使用时往往需要做滤波。下面按“先理解原理，再看代码”的顺序整理。

6.1 一维卡尔曼滤波

卡尔曼滤波可以简单理解为：先预测，再用测量值修正。

这里把目标距离看成一维状态，并假设相邻两次测量变化不大。

状态模型： x_k = x_{k-1} + w_k

测量模型： z_k = x_k + v_k

其中：

• x_k：第 k 次真实距离
• z_k：第 k 次测量值
• w_k：过程噪声，方差为 Q
• v_k：测量噪声，方差为 R

6.1.1 预测阶段

这里采用的是一维位置模型（常值模型），只关心“距离”这一个状态量，不单独估计速度和加速度。也可以理解为：在相邻两次采样间隔很短时，默认目标位置变化不大，等效近似为速度为 0，因此当前时刻的预测值直接取上一时刻的估计值。

预测方程：

预测误差协方差：

6.1.2 更新阶段

收到当前测量值后，先计算卡尔曼增益：

再用测量值修正预测值：

最后更新误差协方差：

6.1.3 调参直觉

• Q 大：响应更快，但结果更容易抖动。
• R 大：结果更平滑，但跟随会更慢。
• 对超声波测距这类场景，如果偶尔出现跳点，通常可以先做中值滤波，再做卡尔曼滤波。

6.2 只使用卡尔曼滤波

这个版本适合先观察卡尔曼滤波本身的效果：直接对原始测距值进行平滑，也更容易观察 Q 和 R 对结果的影响。

from machine import Pin, time_pulse_us,UART
import time

uart1 = UART(1, baudrate=115200, tx=Pin(0), rx=Pin(1))
trig = Pin(4, Pin.OUT)
echo = Pin(5, Pin.IN)

def distance():
    # 清Trig
    trig.off()
    time.sleep_us(2)
    # 发送10us的触发脉冲
    trig.on()
    time.sleep_us(10)
    trig.off()

    # 计算脉宽
    duration = time_pulse_us(echo,1, 30000)
    if duration < 0:
       return None
    # 换算厘米 343m/s = 343*100cm/1000000us 声速20°空气中约为343
    distance = (duration * 0.0343) / 2
    print(duration)
    return distance

# 卡尔曼参数
x_prev = 0
Q = 0.01
R = 0.2 #测量噪音
P_prev = 1

def klm(zk):
    global P_prev,x_prev
    # step1: 预测阶段
    xk_ = x_prev+0 # 预测状态
    Pk_ = P_prev+Q # 预测误差

    # step2: 更新阶段
    Kk = Pk_/(Pk_+R) # 卡尔曼增益
    xk = xk_+Kk*(zk-xk_) # 更新估计值
    Pk = (1-Kk)*Pk_  # 更新误差

    # 保存值供下次迭代
    P_prev = Pk
    x_prev = xk
    return xk

while True:
    dis = distance()

    if dis is None:
        print("Out of range")
    else:
        f = klm(dis)
        print("raw=%.2f  kalman=%.2f" % (dis, f))
        # 发给VOFA+
        uart1.write("%.2f,%.2f\r\n" % (dis, f))

    # 一次最长测量时间约：23ms，所以定为100ms测量周期很安全
    time.sleep_ms(100)

测量值和卡尔曼滤波后的值上位机对比图

从上位机曲线可以看到，卡尔曼滤波后的曲线明显更平滑，但当目标移动时，滤波值会比原始测量值略有滞后。

如果继续往前走，状态可以扩展为：

• x = 位置
• v = 速度

此时预测关系可写成：

这样目标移动时，滤波器会利用速度项提前预测，因此滞后会明显减小。

6.3 中值滤波 + 卡尔曼滤波

这一版更适合实际使用：

• 中值滤波先去掉偶发离群点。
• 卡尔曼滤波再做连续平滑。

也就是代码流程：先 median_filter(raw)，再 klm(mid)。

from machine import Pin, time_pulse_us, UART
import time

uart1 = UART(1, baudrate=115200, tx=Pin(0), rx=Pin(1))
trig = Pin(4, Pin.OUT)
echo = Pin(5, Pin.IN)

def distance():
    # 清Trig
    trig.off()
    time.sleep_us(2)
    # 发送10us的触发脉冲
    trig.on()
    time.sleep_us(10)
    trig.off()
    # 计算脉宽
    duration = time_pulse_us(echo, 1, 30000)
    if duration < 0:
        return None
    # 换算厘米 343m/s = 343*100cm/1000000us 声速20°空气中约为343
    distance = (duration * 0.0343) / 2
    print(duration)
    return distance

# -------------------
# 中值滤波：对连续多次测量结果取中值，消除偶然误差。
# -------------------
buf = []

def median_filter(x):
    global buf
    buf.append(x)
    if len(buf) > 5:
        # 当列表元素超过5，则移除buf索引0位置的元素
        buf.pop(0) # buf FIFO队列结构，尾部入队，头部出队
    temp = buf[:]  # 复制数组 浅拷贝
    temp.sort()    # 排序
    return temp[len(temp)//2]

# -------------------
# 卡尔曼参数
# -------------------
x_prev = 0
Q = 0.01
R = 0.2  # 测量噪音
P_prev = 1

def klm(zk):
    global P_prev, x_prev
    # step1: 预测阶段
    xk_ = x_prev + 0  # 预测状态
    Pk_ = P_prev + Q  # 预测误差
    # step2: 更新阶段
    Kk = Pk_ / (Pk_ + R)       # 卡尔曼增益
    xk = xk_ + Kk * (zk - xk_) # 更新估计值
    Pk = (1 - Kk) * Pk_        # 更新误差
    # 保存值供下次迭代
    P_prev = Pk
    x_prev = xk
    return xk

while True:
    raw = distance()
    if raw is not None:
        mid = median_filter(raw)       # 先中值
        kal = klm(mid)                 # 再卡尔曼
        print("raw=%.2f mid=%.2f kal=%.2f" % (raw, mid, kal))
        uart1.write("%.2f,%.2f,%.2f\r\n" % (raw, mid, kal))
    else:
        print("Out")
    # 一次最长测量时间约：23ms，所以定为100ms测量周期很安全
    time.sleep_ms(100)

测量值-中值滤波-卡尔曼滤波上位机曲线截图

七、总结

HC-SR04 的核心不复杂，关键点主要有四个：

1. Trig 负责触发，Echo 负责输出脉宽。
2. 距离计算本质是测回波时间，再按声速换算。
3. 工程上要注意 5V 电平、超时设置 和 测量周期。
4. 如果需要更稳的曲线，通常可以采用中值滤波 + 卡尔曼滤波的组合方案。