4、超声波传感器驱动：Range消息格式、I2C/GPIO通信、数据滤波处理
超声波传感器，说白了就是给机器人装上一对「蝙蝠耳朵」。它通过发射声波、等待回波，算出前方障碍物的距离。听起来简单，但真要在ROS 2里把它用好，坑还真不少。我最早做扫地机器人原型时，就被超声波的数据抖动折腾得够呛——明明前面是堵墙，读数却像心跳图一样上下乱跳。
这一节，我们重点搞定三件事：Range消息怎么发、I2C和GPIO怎么选、数据怎么滤波才能用。
4.1 Range消息格式：ROS 2里的「距离语言」
ROS 2里超声波传感器的标准消息类型是 sensor_msgs/msg/Range。你想想看，如果每个厂家都用自己的格式，那导航模块就得写一堆适配代码，多累啊。
Range消息的核心字段如下：
| 字段 | 类型 | 说明 |
|---|---|---|
header |
std_msgs/Header | 时间戳和坐标系ID |
radiation_type |
uint8 | 0=超声波，1=红外 |
field_of_view |
float32 | 波束角（弧度） |
min_range |
float32 | 最小量程（米） |
max_range |
float32 | 最大量程（米） |
range |
float32 | 当前测量距离（米） |
重点提醒：radiation_type 一定要设成 0（超声波）。我见过有人忘了设，结果导航模块按红外传感器的特性去处理，数据全偏了。
发布Range消息的代码其实很简洁。下面是我常用的一个发布节点示例：
# ultrasonic_publisher.py
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Range
class UltrasonicPublisher(Node):
def __init__(self):
super().__init__('ultrasonic_publisher')
self.publisher_ = self.create_publisher(Range, 'ultrasonic_range', 10)
timer_period = 0.05 # 50Hz，够快了吧？
self.timer = self.create_timer(timer_period, self.timer_callback)
def timer_callback(self):
msg = Range()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = 'ultrasonic_link'
msg.radiation_type = Range.ULTRASOUND
msg.field_of_view = 0.26 # 约15度波束角
msg.min_range = 0.02
msg.max_range = 4.0
msg.range = self.read_sensor() # 实际读取函数
self.publisher_.publish(msg)
个人习惯：我一般把发布频率设在30~50Hz。太低的话，移动机器人急停时会反应迟钝；太高了，I2C总线容易堵车。
4.2 I2C vs GPIO通信：选对路子，少走弯路
市面上常见的超声波模块，通信方式主要分两种：I2C 和 GPIO（脉冲宽度测量）。怎么选？我直接说结论：
- I2C：适合多传感器组网，一根总线上挂4~8个模块没问题。数据直接读寄存器，不用自己算时间。
- GPIO：适合单传感器、对实时性要求高的场景。但需要自己用
time模块测脉宽，精度受系统调度影响。
我曾经在一个项目里同时用了6个I2C超声波，结果发现地址冲突——有些便宜模块地址是硬编码的，没法改。后来只能加了一个I2C多路复用器（TCA9548A）才搞定。
4.2.1 I2C驱动示例（以HC-SR04 I2C版为例）
# i2c_ultrasonic.py
import smbus
import time
class I2CUltrasonic:
def __init__(self, address=0x57):
self.bus = smbus.SMBus(1)
self.address = address
def read_distance(self):
# 发送测量命令
self.bus.write_byte(self.address, 0x01)
time.sleep(0.06) # 等待测量完成
# 读取2字节数据
data = self.bus.read_i2c_block_data(self.address, 0x02, 2)
distance = (data[0] << 8) | data[1]
return distance / 1000.0 # 转成米
避坑指南：我曾经遇到过I2C读取偶尔返回0的情况。后来发现是模块测量还没完成就读了。加一个 time.sleep(0.06) 就稳定了。不同模块的等待时间不一样，建议看数据手册。
4.2.2 GPIO脉冲测量（以原始HC-SR04为例）
# gpio_ultrasonic.py
import RPi.GPIO as GPIO
import time
class GPIOSonar:
def __init__(self, trig_pin, echo_pin):
self.trig = trig_pin
self.echo = echo_pin
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.trig, GPIO.OUT)
GPIO.setup(self.echo, GPIO.IN)
def read_distance(self):
# 发送10us高电平触发
GPIO.output(self.trig, True)
time.sleep(0.00001)
GPIO.output(self.trig, False)
# 等待回波上升沿
while GPIO.input(self.echo) == 0:
pulse_start = time.time()
while GPIO.input(self.echo) == 1:
pulse_end = time.time()
pulse_duration = pulse_end - pulse_start
distance = pulse_duration * 343.0 / 2.0 # 声速343m/s
return distance
注意：GPIO方式有个致命问题——while 循环会阻塞CPU。如果系统负载高，测量时间会漂移。我建议在实时线程里跑，或者干脆用I2C版本。
4.3 数据滤波处理：让超声波「冷静」下来
原始超声波数据有多「疯」？我举个例子：静止对着30cm远的墙，读数可能在28cm到35cm之间来回跳。为什么？声波反射有干涉，加上温度、湿度影响。直接拿这个数据去导航，机器人会像喝醉了一样。
常用的滤波方法有三种，我按推荐程度排个序：
- 中值滤波：取最近N个读数的中位数。抗突发干扰效果最好。
- 滑动平均：取最近N个读数的平均值。平滑效果好，但对突变反应慢。
- 限幅滤波：如果当前读数跟上一次差太多，就丢弃。适合剔除明显错误值。
我个人习惯把限幅滤波和中值滤波结合起来用。先扔掉离谱的值，再取中位数，效果很稳。
4.3.1 组合滤波实现
# ultrasonic_filter.py
from collections import deque
class UltrasonicFilter:
def __init__(self, window_size=5, max_delta=0.1):
self.buffer = deque(maxlen=window_size)
self.last_valid = 0.0
self.max_delta = max_delta # 最大允许变化（米）
def filter(self, raw_distance):
# 第一步：限幅滤波
if abs(raw_distance - self.last_valid) > self.max_delta:
return self.last_valid # 丢弃突变值
# 第二步：加入缓冲区
self.buffer.append(raw_distance)
self.last_valid = raw_distance
# 第三步：中值滤波
sorted_vals = sorted(self.buffer)
return sorted_vals[len(sorted_vals) // 2]
经验之谈：窗口大小选5~7最合适。太小了滤波效果差，太大了延迟高。你想想看，如果窗口是20，机器人撞到墙了，滤波后的数据还在报「前面还有50cm」，那就真撞上去了。
4.4 知识体系总览
下面这张图，把超声波传感器驱动的核心脉络画清楚了。从硬件通信到ROS消息发布，再到数据滤波，每一步都环环相扣。
嗯，到这里，超声波传感器驱动这块的核心内容就讲完了。从Range消息格式，到I2C和GPIO两种通信方式的选型，再到数据滤波的实战技巧，每一步都是我在项目里踩过坑、填过坑之后总结出来的。你照着这个思路去做，至少能少走一半弯路。
一句话总结：Range消息是标准接口，I2C适合多传感器组网，GPIO适合单传感器快速响应，滤波一定要组合使用——限幅保安全，中值保平滑。
可可精品资料库