超声波测距三大算法实测对比

前言

声波测距的数据包含很大噪声，即使障碍物（以纸板为例）静止，测量距离数据也上下跳变，需要通过数据滤波算法降低测量误差，主要滤波算法有平均值滤波和卡尔曼滤波。

在超声波测距中，无滤波、卡尔曼滤波和均值算法的核心区别在于对噪声的处理方式和对动态变化的适应能力。

无滤波算法： 直接输出原始测量值：每次测量直接返回超声波模块的原始数据（如时间差计算后的距离值）。 无抗干扰能力：完全暴露于环境噪声（如温度变化、多路径反射、电磁干扰等）。 实时性高：没有额外计算延迟。 精度低：数据波动大，可能出现异常跳变（如突然出现极大或极小值）。 稳定性差：在动态环境中（如移动物体或振动场景）容易产生误判。 适用场景：仅适用于对精度要求极低的简单检测（如粗略判断障碍物是否存在）。

卡尔曼滤波算法 基于系统模型的预测与更新：结合物理运动模型（如匀速假设）和测量值，动态调整估计。 动态适应性：通过状态转移矩阵（F）和噪声协方差（Q, R）建模系统不确定性。 抗噪声能力强：有效抑制高斯白噪声，平滑数据波动。 精度高：滤波后数据更接近真实值，减少随机误差。 动态响应好：在物体移动或环境变化时，能快速收敛到真实值。 参数敏感：需要合理设置过程噪声（Q）和测量噪声（R）协方差矩阵。 适用场景：适用于动态场景（如机器人避障、移动目标跟踪）。 Q（过程噪声）：表示系统模型的不确定性（如物体加速度变化）。值越大，滤波器越依赖新测量值。 R（测量噪声）：表示传感器本身的噪声水平。值越大，滤波器越信任模型预测。

均值算法（滑动平均滤波） 简单平滑噪声：对连续N次测量值取平均，抑制随机噪声。 无模型依赖：不考虑物理运动规律，仅通过统计学方法平滑数据。 实现简单：适合资源受限的嵌入式系统。 中等精度：能有效减少随机噪声，但无法处理系统性误差（如温度导致的声速变化）。 延迟明显：对快速变化的动态响应差（如移动物体突然靠近）。 适用场景：适用于静态或缓慢变化的场景（如液位监测、固定障碍物检测）。

一、无滤波算法

配置硬件： 根据树莓派的引脚图选择设置GPIO23和GPIO24管脚，即图中BCM23和BCM24的位置，分别为16号和18号针脚。 编写代码：

#导入 GPIO库

import RPi.GPIO as GPIO

import time

#设置 GPIO 模式为 BCM

GPIO.setmode(GPIO.BCM)

#定义 GPIO 引脚

GPIO_TRIGGER = 23

GPIO_ECHO = 24

#设置 GPIO 的工作方式 (IN / OUT)

GPIO.setup(GPIO_TRIGGER, GPIO.OUT)

GPIO.setup(GPIO_ECHO, GPIO.IN)

def distance():

# 发送高电平信号到 Trig 引脚

GPIO.output(GPIO_TRIGGER, True)

# 持续 10 us

time.sleep(0.00001)

GPIO.output(GPIO_TRIGGER, False)

start_time = time.time()

stop_time = time.time()

# 记录发送超声波的时刻1

while GPIO.input(GPIO_ECHO) == 0:

start_time = time.time()

# 记录接收到返回超声波的时刻2

while GPIO.input(GPIO_ECHO) == 1:

stop_time = time.time()

# 计算超声波的往返时间 = 时刻2 - 时刻1

time_elapsed = stop_time - start_time

# 声波的速度为 343m/s， 转化为 34300cm/s。

distance = (time_elapsed * 34300) / 2

return distance

if __name__ == '__main__':

try:

while True:

dist = distance()

print("Measured Distance = {:.2f} cm".format(dist))

time.sleep(1)

# Reset by pressing CTRL + C

except KeyboardInterrupt:

print("Measurement stopped by User")

GPIO.cleanup()

运行：

超声波测距原始算法

二、卡尔曼滤波算法

硬件搭建不变，仅将卡尔曼算法与原代码结合。代码如下：

import RPi.GPIO as GPIO

import time

GPIO.setmode(GPIO.BCM)

GPIO_TRIGGER = 23

GPIO_ECHO = 24

GPIO.setup(GPIO_TRIGGER, GPIO.OUT)

GPIO.setup(GPIO_ECHO, GPIO.IN)

# 卡尔曼滤波参数

Q = 0.001 # 过程噪声方差（系统模型误差）

R = 0.1 # 测量噪声方差（传感器误差）

x_est = 0.0 # 初始估计值

P = 1.0 # 初始估计误差协方差

def distance():

GPIO.output(GPIO_TRIGGER, True)

time.sleep(0.00001)

GPIO.output(GPIO_TRIGGER, False)

start_time = time.time()

while GPIO.input(GPIO_ECHO) == 0:

start_time = time.time()

stop_time = time.time()

while GPIO.input(GPIO_ECHO) == 1:

stop_time = time.time()

time_elapsed = stop_time - start_time

return (time_elapsed * 34300) / 2

def kalman_filter(z_measured):

global x_est, P

# 预测阶段

x_pred = x_est

P_pred = P + Q

# 更新阶段

K = P_pred / (P_pred + R)

x_est = x_pred + K * (z_measured - x_pred)

P = (1 - K) * P_pred

return x_est

if __name__ == '__main__':

try:

while True:

raw_dist = distance()

filtered_dist = kalman_filter(raw_dist)

print("Raw: {:.2f} cm, Filtered: {:.2f} cm".format(raw_dist, filtered_dist))

time.sleep(0.1)

except KeyboardInterrupt:

print("Measurement stopped by User")

GPIO.cleanup()

运行

超声波测距卡曼算法

三、均值滤波算法

硬件连接不变，仅修改代码算法。代码如下：

import RPi.GPIO as GPIO

import time

GPIO.setmode(GPIO.BCM)

GPIO_TRIGGER = 23

GPIO_ECHO = 24

GPIO.setup(GPIO_TRIGGER, GPIO.OUT)

GPIO.setup(GPIO_ECHO, GPIO.IN)

# 卡尔曼滤波参数

Q = 0.001 # 过程噪声方差（系统模型误差）

R = 0.1 # 测量噪声方差（传感器误差）

x_est = 0.0 # 初始估计值

P = 1.0 # 初始估计误差协方差

def distance():

GPIO.output(GPIO_TRIGGER, True)

time.sleep(0.00001)

GPIO.output(GPIO_TRIGGER, False)

start_time = time.time()

while GPIO.input(GPIO_ECHO) == 0:

start_time = time.time()

stop_time = time.time()

while GPIO.input(GPIO_ECHO) == 1:

stop_time = time.time()

time_elapsed = stop_time - start_time

return (time_elapsed * 34300) / 2

def kalman_filter(z_measured):

global x_est, P

# 预测阶段

x_pred = x_est

P_pred = P + Q

# 更新阶段

K = P_pred / (P_pred + R)

x_est = x_pred + K * (z_measured - x_pred)

P = (1 - K) * P_pred

return x_est

if __name__ == '__main__':

try:

while True:

raw_dist = distance()

filtered_dist = kalman_filter(raw_dist)

print("Raw: {:.2f} cm, Filtered: {:.2f} cm".format(raw_dist, filtered_dist))

time.sleep(0.1)

except KeyboardInterrupt:

print("Measurement stopped by User")

GPIO.cleanup()

wan@pi:/csb $ cat measure_avg.py

import RPi.GPIO as GPIO

import time

GPIO.setmode(GPIO.BCM)

GPIO_TRIGGER = 23

GPIO_ECHO = 24

GPIO.setup(GPIO_TRIGGER, GPIO.OUT)

GPIO.setup(GPIO_ECHO, GPIO.IN)

# 定义缓冲区大小

BUFFER_SIZE = 8

distance_buffer = [0.0] * BUFFER_SIZE

buffer_index = 0

def distance():

GPIO.output(GPIO_TRIGGER, True)

time.sleep(0.00001)

GPIO.output(GPIO_TRIGGER, False)

start_time = time.time()

while GPIO.input(GPIO_ECHO) == 0:

start_time = time.time()

stop_time = time.time()

while GPIO.input(GPIO_ECHO) == 1:

stop_time = time.time()

time_elapsed = stop_time - start_time

distance = (time_elapsed * 34300) / 2

return distance

def apply_moving_average(new_distance):

global buffer_index

# 更新缓冲区

distance_buffer[buffer_index] = new_distance

buffer_index = (buffer_index + 1) % BUFFER_SIZE

# 计算平均值

return sum(distance_buffer) / BUFFER_SIZE

if __name__ == '__main__':

try:

while True:

raw_dist = distance()

filtered_dist = apply_moving_average(raw_dist)

print("Raw: {:.2f} cm, Filtered: {:.2f} cm".format(raw_dist, filtered_dist))

time.sleep(0.1) # 提高采样频率以获取更稳定的数据

except KeyboardInterrupt:

print("Measurement stopped by User")

运行

超声波测距均值算法

四、读取超声波测距模块的ECHO和TRIG接口的波形

两者波形在时间上有所差异，TRIG波形快于ECHO波形，在波形形状上十分相似。

TRIG波形 ECHO波形