水下机器人ROV/AUV在水下作业时遇到的核心问题往往是“看不见、测不准、躲不开”。普通光学摄像头在水质浑浊的环境中可用性极差而 GPS 在水下又会失效这时候避障声呐就成了最可靠的感知方式之一。实际项目中避障声呐不仅要解决“有没有障碍物”的问题还要把各方向的障碍物距离实时显示出来在接近危险时自动规划躲避动作。本文围绕“避障声呐 测距显示 雷达表盘 自主避障控制”这套完整链路讲解从声呐测距原理、多方向数据采集与融合到雷达表盘可视化、分级警报和差速避障控制的具体实现并提供一套可运行的 Python 示例代码方便你在没有水下硬件的情况下先用模拟数据完成联调。1. 背景与核心概念1.1 水下机器人为什么需要避障声呐水下机器人的工作环境通常比较复杂水坝检测、管道巡检、水产养殖监控、水下搜救等场景中障碍物可能来自岩壁、桥墩、渔网、缆绳、水生植物甚至是一起作业的另一台潜水器。视觉传感器虽然能提供丰富的纹理和颜色信息但在低能见度水域中往往只能看到几十厘米甚至更近的距离。相比之下声波在水中传播衰减小声呐可以在浑浊水体中实现数米到数十米的探测距离是对水下机器人最友好的环境感知手段之一。避障声呐的核心任务是持续输出某个方向上的障碍物距离。多个声呐按不同角度布置后机器人就能知道自己周围一圈的障碍分布情况相当于给水下机器人装上了“全向感知皮肤”。这些距离信息一方面送给雷达表盘做可视化另一方面送给避障控制模块在距离过低时自动触发减速、转向甚至紧急停车。1.2 声呐测距的基本原理避障声呐大多采用飞行时间法Time of FlightToF测距。声呐换能器向目标方向发射一束声脉冲声波遇到障碍物后反射回来由接收换能器采集回波。假设声波在水中单趟传播时间为t水中声速为c那么障碍物距离d可以表示为d (c * t) / 2这里除以 2 是因为声波走的是“发射到障碍物再从障碍物返回”的双程距离。水中声速并不是固定的它受温度、盐度和静压力影响。通常浅水环境下可以近似取 1500 m/s但如果你需要高精度测量建议加入温度传感器做声速校准公式可以参考常见的 Mackenzie 声速经验公式。空气中常见的超声波测距模块如 HC-SR04使用 40 kHz 左右的超声波而在水中使用的避障声呐频率往往更高常见的有 200 kHz、500 kHz 甚至更高频率。频率越高角度分辨率通常越好但探测距离会变短频率越低探测距离更远但波束角可能更宽。项目中需要根据机器人的尺寸、避障距离和作业场景来综合选择。1.3 单波束、多波束与全方位声呐布局单波束声呐一次只能测量一个方向的距离结构简单、成本低但需要机械旋转或安装多个探头才能实现全方位覆盖。多波束声呐通过相控阵或阵列处理一次发射可以同时获得多个角度的回波信息价格较高但非常适合前视避障。在小型水下机器人项目中比较务实的方案是“多探头拼接”在机器人周围按固定角度安装 6 到 16 个单波束避障声呐形成一个环形阵列。控制器按照顺序或并行方式读取每个探头的距离值经过数据融合后生成一张 360° 范围的障碍物距离分布图。雷达表盘显示的正是这张分布图。这种方案的好处是成本可控每个探头独立更换和维修。数据更新频率直观每个方向的距离值是独立的。逻辑简单适合从样机到小批量产品迭代。1.4 雷达表盘在系统中的作用雷达表盘是一种极坐标显示界面横轴方向表示方位角纵轴方向表示距离。声呐测得哪个方向有障碍物雷达表盘就在对应角度、对应距离处画出一个点或一块扇形区域。操作员在甲板监控端一眼就能看出哪个方向有障碍障碍在多少米障碍是否已经进入危险区域。同时雷达表盘也可以叠加航向、自身姿态和推进器状态方便远程遥控员快速判断局势。需要说明的是雷达表盘既可以在岸基上位机上显示也可以运行在机器人自带的防水触控屏上显示形式保持一致只是部署位置不同。2. 系统整体架构设计2.1 系统分层架构一个完整的避障声呐水下机器人测距显示与自主避障系统可以拆成四层层级职责典型模块感知层获取外部环境距离信息环形声呐阵列、姿态传感器、深度计决策层数据滤波、融合、避障决策主控程序、避障算法模块执行层根据决策控制推进器左右推进器、垂直推进器、舵机交互层数据显示和远程监控雷达表盘、日志、报警声音感知层负责把物理世界的距离转化为数字信号决策层负责把多路距离数据变成“下一步怎么走”的控制指令执行层把指令转化为推进器的转速和方向交互层把整个过程展示给操作员。分层的好处是每一层可以独立测试先验证声呐数据是否准确再验证雷达表盘显示是否正常最后才接入推进器做闭环控制。2.2 数据流与控制流系统的数据流可以描述为一条单向链路环形声呐阵列 → 主控串口/I2C读取 → 距离数据预处理滤波、剔除异常值 → 角度-距离数据融合 → 雷达表盘显示 → 避障决策危险方向判断、转向控制量计算 → 推进器执行控制流上有两个并行任务。一个任务是定时刷新雷达表盘通常 5 Hz 到 10 Hz 即可满足人眼观察需求另一个任务是避障控制频率需要更高一些例如 20 Hz 到 50 Hz因为控制频率太低会导致机器人撞上障碍物之后才反应过来。2.3 关键模块划分在实际工程中不建议把所有代码都塞进一个文件。至少应该拆成这样几个模块sonar.py声呐数据读取接口支持模拟数据和真实串口数据。fusion.py多路声呐数据的融合与特征提取。radar_display.py雷达表盘界面的绘制与刷新。avoidance.py分级警报和避障控制逻辑。config.py声呐数量、角度布局、阈值等参数的集中配置。main.py主程序入口负责组装各模块。这样的模块划分既方便单元测试也方便后续替换硬件。比如你现在使用模拟声呐调试后面买了真实声呐只需要替换sonar.py中的数据源其余模块不需要大改。3. 环境准备与版本说明3.1 硬件环境本文示例以小型水下机器人样机为参考你可以根据实际情况调整。典型硬件清单如下部件作用说明主控板数据采集、控制计算推荐带 Python 环境的 Jetson Nano、树莓派 4B或带 MicroPython 的 STM32 开发板避障声呐测量障碍物距离至少 6 个建议 8 个或 12 个环绕安装推进器提供前进、后退、转向动力水平方向至少 2 个差速控制实现转向电子调速器驱动推进器根据 PPM/PWM/串口信号控制转速电源模块给主控和推进器供电注意模拟信号与动力电源隔离防水舱保护电子设备声呐探头需要露出舱体或使用专用透声窗如果暂时没有声呐硬件可以先用软件模拟数据完成整个流程的联调这也是本文示例采用的方式。3.2 软件环境版本说明如下实际使用时请根据你的环境调整操作系统Ubuntu 20.04/22.04、Windows 10/11、macOS 均可。Python3.8 及以上。主要 Python 库numpy数值计算。matplotlib雷达表盘绘制。pyserial串口读取真实声呐数据如果使用模拟数据可以暂不安装。安装命令pip install numpy matplotlib pyserial如果使用树莓派/Jetson 这类嵌入式主控建议用系统自带 Python 环境或者用venv创建独立虚拟环境避免和系统包冲突。3.3 示例项目结构robot_navigation/ ├── config.py ├── sonar.py ├── fusion.py ├── avoidance.py ├── radar_display.py └── main.py每个文件的职责会在后续章节逐一展开。这样组织项目即使在没有真实硬件的情况下也可以完整地验证“多路声呐数据 - 数据融合 - 雷达表盘 - 避障控制”的整套逻辑。4. 声呐数据采集与预处理4.1 声呐数据结构每路避障声呐在系统中对应一个固定的安装方位角。假设机器人头部为正前方 0°顺时针方向为正那么 8 路声呐可以均匀分布在 0°、45°、90°、135°、180°、225°、270°、315° 的位置。为了方便后续处理我们把角度和声呐编号映射关系写入配置文件中。# 文件路径config.py # 水中声速单位米/秒浅水环境可近似取 1500 SOUND_SPEED 1500.0 # 声呐数量 SONAR_COUNT 8 # 各路声呐的安装角度单位度0 度表示正前方顺时针为正 SONAR_ANGLES [0, 45, 90, 135, 180, 225, 270, 315] # 最大量程单位米超过该值视为无有效回波或超出测距范围 SONAR_MAX_RANGE 5.0 # 最小量程单位米低于该值认为声呐进入盲区 SONAR_MIN_RANGE 0.2 # 安全距离单位米 SAFE_DISTANCE 2.0 # 警戒距离单位米 WARNING_DISTANCE 1.2 # 危险距离单位米 DANGER_DISTANCE 0.6声呐返回的数据本质上是一个距离值。如果声呐模块本身支持数字输出一般通过串口发送类似R045:1.25这样的字符串表示编号 45 的声呐测量距离为 1.25 米。也有模块使用 I2C 寄存器方式读取。本文不绑定具体型号而是抽象出一个读取接口这样换模块时只需要改动底层读取函数。4.2 模拟声呐数据源没有真实硬件时可以用模拟数据验证上位机和控制逻辑。模拟数据的关键点是距离值不能完全随机否则无法模拟出真实的障碍物趋势。合理的模拟方式是让某个方向附近存在一个“障碍目标”距离随目标相对运动缓慢变化。这里我们实现一个简单的SimulatedSonar类# 文件路径sonar.py import time import math import random class SimulatedSonar: 模拟声呐数据源。 在没有真实声呐硬件时用于联调雷达表盘和避障控制逻辑。 def __init__(self, angles, max_range5.0, seedNone): self.angles angles self.max_range max_range if seed is not None: random.seed(seed) # 创建一个模拟障碍物角度、距离、角速度、径向速度 self.obstacle_angle random.uniform(30, 150) self.obstacle_distance random.uniform(1.5, 3.5) self.angle_speed random.uniform(-10, 10) self.radius_speed random.uniform(-0.2, 0.2) self.last_time time.time() def _update_obstacle(self): now time.time() dt now - self.last_time self.last_time now self.obstacle_angle self.angle_speed * dt self.obstacle_distance self.radius_speed * dt if self.obstacle_angle 360: self.obstacle_angle - 360 if self.obstacle_angle 0: self.obstacle_angle 360 if self.obstacle_distance 0.8 or self.obstacle_distance 4.5: self.radius_speed -self.radius_speed def read_distances(self): 模拟一次声呐扫描返回与 angles 对应的距离列表。 每个方向根据与障碍物的夹角做衰减夹角越大越可能发现障碍。 self._update_obstacle() distances [] for angle in self.angles: angle_diff abs(angle - self.obstacle_angle) if angle_diff 180: angle_diff 360 - angle_diff if angle_diff 45: # 障碍物在该波束附近返回一个带波束宽度衰减的距离值 noise random.gauss(0, 0.05) measured self.obstacle_distance noise distances.append(max(0.15, measured)) else: # 不在波束范围内返回一个随机远距离值 distances.append(random.uniform(3.0, self.max_range)) return distances这个模拟器虽然简单但能产生连续变化的障碍物距离非常适合测试雷达表盘刷新和避障逻辑。后面我们还可以通过调整障碍物的角速度和径向速度模拟机器人靠近障碍物时避障系统应该如何反应。4.3 串口真实声呐读取如果使用真实的串口声呐SonarSerial类可以按行读取数据。不同厂家模块的协议不同下面是通用思路# 文件路径sonar.py import serial class SerialSonar: 串口声呐数据源。 协议示例每行数据格式为 角度:距离例如 45:1.25 实际使用请按你的设备协议修改解析函数。 def __init__(self, port/dev/ttyUSB0, baudrate115200, anglesNone): self.angles angles or [] self.ser serial.Serial(port, baudrate, timeout0.1) def read_distances(self): distances [self.max_range] * len(self.angles) lines [] try: while self.ser.in_waiting: line self.ser.readline().decode(utf-8, errorsignore).strip() if line: lines.append(line) except Exception: pass for line in lines: parts line.split(:) if len(parts) ! 2: continue try: angle float(parts[0]) distance float(parts[1]) except ValueError: continue # 根据角度映射到数组下标 if angle in self.angles: idx self.angles.index(angle) distances[idx] distance return distances真实使用中要注意串口数据是异步到达的主控循环读取时可能同时收到多帧数据。建议在定时器或者独立线程中读取串口把最新的距离结果放到共享变量中避免主线程被阻塞。另外有些声呐回传的是原始飞行时间单位微秒你需要根据声速换算成米。例如回传t_us 2000微秒距离为(t_us / 1e6) * SOUND_SPEED / 2.0。4.4 距离数据滤波水下环境存在气泡、悬浮物和噪声声呐单次测量容易产生异常跳变。直接把这些数据送到雷达表盘和避障控制会带来显示抖动和控制振荡。常用的处理方式是“中值滤波 滑动平均”组合。中值滤波的思路是取最近 N 次测量值的中位数能有效去除脉冲噪声滑动平均则进一步平滑连续变化。代码实现如下# 文件路径sonar.py from collections import deque class DistanceFilter: 对单路声呐距离做中值滤波和滑动平均。 def __init__(self, window_size5): self.window_size window_size self.buffer deque(maxlenwindow_size) def update(self, value): self.buffer.append(value) if len(self.buffer) self.window_size: return value sorted_values sorted(self.buffer) median sorted_values[len(sorted_values) // 2] avg sum(self.buffer) / len(self.buffer) # 中值和滑动平均做加权融合权重可调 return 0.6 * median 0.4 * avg每个声呐维护一个独立滤波器避免多路数据互相干扰。在声呐读数连续缺失时滤波器应返回上一次的有效值而不是 0。距离为 0 通常表示“没有有效回波”或“超出量程”需要和真实距离 0 区分开。5. 全方位声呐数据融合5.1 角度与距离映射多路声呐读取完毕后我们得到的是“按声呐编号排列的距离列表”。为了方便雷达表盘绘制和避障决策需要把它转换成“角度 - 距离”的字典或者直接在极坐标中使用。# 文件路径fusion.py class SonarFusion: def __init__(self, angles, max_range5.0): self.angles angles self.max_range max_range self.filters {} for angle in angles: self.filters[angle] DistanceFilter(window_size5) def update(self, distances): distances: 与 angles 顺序对应的原始距离列表 返回: {角度: 滤波后的距离} result {} for angle, distance in zip(self.angles, distances): if distance 0 or distance self.max_range: result[angle] self.max_range else: result[angle] self.filters[angle].update(distance) return result这里把无效距离统一设为最大量程表示当前方向没有检测到有效障碍物。雷达表盘绘制时最大量程处的点会显示在最外圈或者直接不显示避免误导操作员。5.2 最近障碍物方向提取避障控制需要知道“最危险的障碍在哪个方向”以及“距离是多少”。我们可以从融合后的数据中找出最小距离并记录对应的角度。另外如果多个方向都存在近距离障碍物说明机器人可能被包围需要触发更保守的避障策略。# 文件路径fusion.py def get_nearest_obstacle(self, fused_data): nearest_angle None nearest_distance self.max_range for angle, distance in fused_data.items(): if distance nearest_distance: nearest_distance distance nearest_angle angle return nearest_angle, nearest_distance如果声呐布局不是均匀的例如前方 0° 和左右 45° 各有一个声呐后方只有一个 180° 声呐那么在求“最近障碍物方向”时需要按每个声呐实际覆盖的扇形区域做加权。更精细的做法是先把角度插值成连续的 360° 分布但这会增加计算量。对于大多数小型水下机器人均匀环形布局已经足够。5.3 多方向障碍分布计算避障时只靠一个最近障碍方向容易导致抖动。比如障碍物从左侧靠近系统猛向右转结果右侧又出现一个障碍机器人就会左右摇摆。更稳妥的方法是计算前方扇形区域内各方向的障碍物距离并选择“最安全”的转向角度。这里我们定义一个简单的前向感知函数# 文件路径fusion.py def get_sector_min_distance(self, fused_data, center_angle, half_width30): 计算以 center_angle 为中心、宽度为 half_width*2 的扇形区域内的最小障碍距离。 min_distance self.max_range for angle, distance in fused_data.items(): diff abs(angle - center_angle) if diff 180: diff 360 - diff if diff half_width: if distance min_distance: min_distance distance return min_distance通过计算前方、左前、右前三个区域的最小障碍距离避障控制就能判断应该减速、左转还是右转。6. 雷达表盘显示设计6.1 极坐标雷达图基础雷达表盘是避障系统最直观的呈现界面。使用matplotlib的极坐标投影可以快速实现一个专业风格的雷达表盘。基本思路是极径半径表示距离默认最大量程 5 米。极角角度表示方位角0° 在正上方或正右方按顺时针增长。每个声呐测得一个距离就在对应角度、对应半径处画一个点。把危险距离区域用红色扇形标出警戒区域用黄色扇形标出。最简单的雷达表盘绘制函数如下# 文件路径radar_display.py import matplotlib.pyplot as plt import numpy as np class RadarDisplay: def __init__(self, angles, max_range5.0, warning1.2, danger0.6): self.angles angles self.max_range max_range self.warning warning self.danger danger self.fig plt.figure(figsize(6, 6)) self.ax self.fig.add_subplot(111, projectionpolar) self.ax.set_theta_zero_location(N) self.ax.set_theta_direction(-1) self.ax.set_ylim(0, self.max_range) self.ax.set_yticks(np.arange(0, self.max_range 1, 1.0)) self.ax.set_xlabel(距离单位: m) def draw(self, fused_data): self.ax.clear() self.ax.set_theta_zero_location(N) self.ax.set_theta_direction(-1) self.ax.set_ylim(0, self.max_range) self.ax.set_yticks(np.arange(0, self.max_range 1, 1.0)) # 画危险区域和警戒区域 theta np.linspace(0, 2 * np.pi, 200) self.ax.fill_between(theta, 0, self.danger, colorred, alpha0.15, labeldanger) self.ax.fill_between(theta, self.danger, self.warning, coloryellow, alpha0.15, labelwarning) # 画声呐点 for angle, distance in fused_data.items(): angle_rad np.deg2rad(angle) if distance self.danger: color red elif distance self.warning: color orange else: color green self.ax.plot(angle_rad, distance, o, colorcolor, markersize12) # 在点旁边标注距离读数 offset 0.3 distance_text f{distance:.1f}m self.ax.text(angle_rad, distance offset, distance_text, hacenter, vacenter, fontsize9, colorwhite, bboxdict(boxstyleround, fcblack, alpha0.5)) self.ax.legend(locupper right, bbox_to_anchor(1.2, 1.1)) plt.pause(0.01)这段代码有几个关键点set_theta_zero_location(N)让 0° 指向正上方符合雷达习惯。set_theta_direction(-1)让角度顺时针增长。距离小于危险阈值时用红色显示小于警戒阈值时用橙色显示否则用绿色。每个点的旁边都绘制了距离读数方便直接读出具体数字。6.2 实时刷新机制雷达表盘需要持续刷新。最简单的方式是使用matplotlib的交互模式和plt.pause()在主循环中反复调用draw()。这种方式代码简单适合原型验证。如果想要更高刷新率推荐使用matplotlib.animation.FuncAnimation或者pyqtgraph。FuncAnimation的方式更适合作为独立窗口运行# 文件路径radar_display.py from matplotlib.animation import FuncAnimation class RadarDisplayAnimation(RadarDisplay): def __init__(self, angles, max_range5.0, warning1.2, danger0.6): super().__init__(angles, max_range, warning, danger) self.latest_data None self.anim FuncAnimation(self.fig, self._animate, interval100) def update_data(self, fused_data): self.latest_data fused_data def _animate(self, frame): if self.latest_data is not None: self.draw(self.latest_data)刷新间隔interval100表示每 100 毫秒刷新一次即 10 Hz。人眼观察雷达表盘时这个帧率已经足够流畅。如果你还要在同一个窗口显示水下机器人的航向角、深度、电量等信息可以考虑使用自定义 GridSpec 布局把雷达图和其他数据面板放在同一个Figure中。6.3 距离读数与报警提示雷达表盘上的“距离读数”除了显示在声呐点旁边还可以在全景图中标注最近障碍物。可以在绘制函数中加入一个“最近障碍物”提示框显示类似“最近障碍: 左前方 45°, 距离 0.8 米”的文字。这样的设计让操作员不用逐个读角度直接看到关键告警信息。报警提示还可以结合声音或光效。主控可以通过 GPIO 点亮红色警示灯或者播放蜂鸣器声音。在上位机端也可以调用系统提示音。不过需要注意水下机器人本体与岸基上位机之间通信延迟较高警报的决策必须在机器人端完成不能等上位机提醒后才避障。7. 接近警报与自主避障控制7.1 分级警报策略避障控制不能只看单一距离而应该按照距离分级处理。这里定义三档警报级别距离范围系统行为安全 警戒距离按目标方向正常航行警戒危险距离 ~ 警戒距离减速持续观察准备转向危险 危险距离立即执行避障转向必要时紧急停车危险距离的选取与机器人的制动距离有关。水下机器人惯性大、推进器响应慢如果危险距离设置过小即使立刻反转推进器也可能撞上障碍。建议根据实测制动距离把危险距离设定在制动距离的 1.5 倍以上。7.2 差速驱动的避障策略小型水下机器人水平运动通常采用左右两个水平推进器的差速控制。前进、后退和转向都通过左右推进器的不同转速组合实现。避障转向控制可以用如下伪逻辑描述如果前方安全保持目标航向。如果前方危险且左侧比右侧安全左转。如果前方危险且右侧比左侧安全右转。如果左右都不安全原地旋转远离最近障碍方向。具体到代码实现我们需要根据最近障碍物角度和前方区域最小距离来计算转向量。下面是一个简化的控制函数# 文件路径avoidance.py import math class AvoidanceController: def __init__(self, safe2.0, warning1.2, danger0.6): self.safe safe self.warning warning self.danger danger self.base_speed 0.5 # 基础前进速度范围 0~1 self.turn_gain 1.2 # 转向增益 def compute_control(self, fused_data, nearest_angle, nearest_distance): 输入融合后的距离数据、最近障碍物角度和距离。 返回 (left_speed, right_speed)范围 -1 ~ 1。 left_speed self.base_speed right_speed self.base_speed # 如果没有任何障碍直行 if nearest_angle is None or nearest_distance self.safe: return left_speed, right_speed # 前方扇形区域最小距离 front_distance self._sector_min_distance(fused_data, center_angle0, half_width40) left_distance self._sector_min_distance(fused_data, center_angle60, half_width40) right_distance self._sector_min_distance(fused_data, center_angle-60, half_width40) min_front min(front_distance, nearest_distance) if min_front self.danger: # 危险立即减速并转向 left_speed self.base_speed * 0.3 right_speed self.base_speed * 0.3 if left_distance right_distance: left_speed -0.4 right_speed 0.6 else: left_speed 0.6 right_speed -0.4 elif min_front self.warning: # 警戒减速并轻微转向 left_speed self.base_speed * 0.6 right_speed self.base_speed * 0.6 if nearest_angle 0: # 障碍在右侧向左转 left_speed - 0.2 right_speed 0.2 else: left_speed 0.2 right_speed - 0.2 else: # 安全正常航行可加入轻微的避障预判 pass # 限制输出范围 left_speed max(-1.0, min(1.0, left_speed)) right_speed max(-1.0, min(1.0, right_speed)) return left_speed, right_speed def _sector_min_distance(self, fused_data, center_angle, half_width): min_distance self.safe for angle, distance in fused_data.items(): diff abs(angle - center_angle) if diff 180: diff 360 - diff if diff half_width: min_distance min(min_distance, distance) return min_distance需要说明的是这是一个工程上的简化控制策略不是最优控制。实际项目中可以在left_speed和right_speed之间加入低通滤波避免控制量突变导致推进器抖动。也可以引入模糊控制把障碍距离和障碍角度作为输入左右推进器速度作为输出通过模糊规则表来平滑控制。7.3 紧急停止与人工接管自主避障控制永远不能完全替代人工判断。实际运行中必须保留远程急停指令当操作员发现异常时可以一键切换到手动模式。在代码层面主循环应该持续监听遥控指令收到“手动接管”信号后避障逻辑暂停改由操作员直接控制推进器。紧急停止的优先级应该是系统中最高优先级任何其他逻辑都不能阻止急停信号的执行。这一点在真实水下机器人中至关重要因为推进器故障、声呐被渔网缠绕、控制算法异常等突发情况都可能让机器人处于危险状态。8. 系统集成与运行验证8.1 主程序集成将声呐读取、数据融合、雷达表盘显示和避障控制整合到main.py中。为了便于以后接入真实声呐我们把数据源放在config.py中用参数控制# 文件路径config.py USE_SIMULATOR True # True 使用模拟声呐False 使用串口声呐 SERIAL_PORT /dev/ttyUSB0 SERIAL_BAUDRATE 115200 # 控制周期单位秒 CONTROL_INTERVAL 0.02 # 50Hz避障控制频率 DISPLAY_INTERVAL 0.1 # 10Hz雷达表盘刷新频率main.py主循环结构如下# 文件路径main.py import time from config import * from sonar import SimulatedSonar, SerialSonar, DistanceFilter from fusion import SonarFusion from radar_display import RadarDisplayAnimation from avoidance import AvoidanceController def main(): # 初始化数据源 if USE_SIMULATOR: sonar SimulatedSonar(SONAR_ANGLES, SONAR_MAX_RANGE, seed42) else: sonar SerialSonar(SERIAL_PORT, SERIAL_BAUDRATE, SONAR_ANGLES) fusion SonarFusion(SONAR_ANGLES, SONAR_MAX_RANGE) controller AvoidanceController( safeSAFE_DISTANCE, warningWARNING_DISTANCE, dangerDANGER_DISTANCE ) radar RadarDisplayAnimation(SONAR_ANGLES, SONAR_MAX_RANGE, warningWARNING_DISTANCE, dangerDANGER_DISTANCE) print(系统启动完成开始运行...) last_control_time time.time() last_display_time time.time() try: while True: now time.time() # 读取声呐数据 distances sonar.read_distances() fused_data fusion.update(distances) # 确定最近障碍 nearest_angle, nearest_distance fusion.get_nearest_obstacle(fused_data) # 避障控制以较高频率执行 if now - last_control_time CONTROL_INTERVAL: left_speed, right_speed controller.compute_control( fused_data, nearest_angle, nearest_distance ) # 实际项目中将 left_speed 和 right_speed 发送到推进器 # send_to_thrusters(left_speed, right_speed) print(f[控制] 最近障碍: 角度{nearest_angle}, 距离{nearest_distance:.2f}m, f左推进器{left_speed:.2f}, 右推进器{right_speed:.2f}) last_control_time now # 雷达表盘显示以较低频率刷新 if now - last_display_time DISPLAY_INTERVAL: radar.update_data(fused_data) last_display_time now time.sleep(0.005) except KeyboardInterrupt: print(用户中断系统退出) finally: # 如果有推进器这里应该发送停车指令 print(系统已停止) if __name__ __main__: main()运行程序后你会看到一个极坐标雷达窗口不断刷新每个方向上的声呐点颜色会随着障碍物距离变化终端则会打印最近障碍物角度、距离和左右推进器控制量。由于这里使用的是模拟声呐障碍物会缓慢移动你能够直观看到控制策略的变化过程。8.2 预期运行效果模拟器的默认种子会让一个障碍物从右前方约 60° 左右位置开始运动。程序启动后你会看到雷达表盘中 60° 方向附近出现一个橙黄色或红色的声呐点距离读数逐渐变化。终端打印的控制指令在最近距离大于安全距离时保持直行进入警戒距离后开始出现减速和转向进入危险距离后左右推进器速度差明显增大模拟机器人避开障碍的动作。如果你把SONAR_COUNT调整为 12那么声呐角度数组也需要对应修改雷达表盘上会出现更密的探测点覆盖的盲区更小。8.3 对接真实推进器的接口实际应用中避障控制器输出的左右推进器速度需要转换为推进器 PWM 或串口指令。一般函数可以写成def send_to_thrusters(left_speed, right_speed): # 将 -1~1 映射为 1100~1900 的 PWM 信号 left_pwm int(1500 left_speed * 400) right_pwm int(1500 right_speed * 400) left_pwm max(1100, min(1900, left_pwm)) right_pwm max(1100, min(1900, right_pwm)) # 通过串口或PWM板卡发送 pass这里把 -1 到 1 的抽象速度映射到常见航模电调的 1100~1900 微秒 PWM 范围。不同电调的信号范围可能存在差异务必以你的设备手册为准并在水池测试中先做小油门验证。9. 常见问题与排查思路9.1 声呐读数频繁跳变问题现象常见原因解决思路声呐距离值偶尔跳到很大或很小水中有气泡、悬浮物或多径效应增加中值滤波窗口增大异常值剔除阈值同一障碍物在不同角度读数差异大声呐波束角过宽测到非目标反射物减小波束角或对每个声呐做角度补偿近距离读数始终为 0声呐进入盲区回波与发射脉冲混叠增加最小量程限制把盲区数据标记为无效建议在实际水域中记录一段原始数据和滤波后数据的日志对比波形来确认滤波参数是否合理。9.2 雷达表盘刷新卡顿问题现象常见原因解决思路雷达窗口拖动时画面卡顿主循环中大量打印或阻塞式读取串口把显示刷新放到独立线程或使用 FuncAnimation声呐点数增多后刷新变慢每帧重新创建大量绘图对象复用绘图对象或改用scatter更新数据多个窗口相互影响交互模式开启多个 Figure只保留一个 Figure用子图布局9.3 避障控制抖动或振荡问题现象常见原因解决思路机器人左右反复转向控制频率低或者最近障碍物方向频繁切换提高控制频率增加转向控制量低通滤波前方无障碍但机器人仍转向声呐盲区被误判为障碍检查声呐盲区设置增加最小距离限制转速突变猛烈控制输出未做限幅对左右推进器速度做限幅或加入加速度限制9.4 串口通信丢包问题现象常见原因解决思路部分声呐数据长期收不到串口波特率不匹配或线缆接触不良检查波特率设置确认接线数据乱码设备供电不足导致电平不稳独立供电减少共地干扰数据读取延迟高一次readline()等待时间过长设置超时时间使用缓冲区积累后统一解析10. 最佳实践与工程建议10.1 声呐布局要减少盲区声呐的波束角通常在 20° 到 60° 之间两个相邻声呐的波束要有一定重叠避免出现探测盲区。均匀布局时8 个声呐的间隔是 45°如果单个声呐波束角只有 30°那么相邻波束之间存在 15° 的间隙。这个间隙里的障碍物可能直到很近才被发现。建议根据声呐实际波束角计算覆盖范围必要时增加声呐数量或选择波束角更宽的探头。10.2 数据必须带时间戳在水下机器人系统中传感器数据、控制指令和显示数据最好都带上统一的时间戳。声呐数据经过滤波后如果还要和其他传感器如姿态传感器、深度计融合时间同步就非常重要。最简单的方式是在主循环开始处记录now time.time()然后让所有数据都挂在这个时间戳上。10.3 控制周期和显示周期要分离避障控制频率建议保持在 20 Hz 以上而雷达表盘显示 10 Hz 就够了。如果控制逻辑和显示逻辑共用同一个主循环可能会出现显示刷新阻塞控制的情况。比较稳妥的做法是控制逻辑在主循环中严格按照固定周期执行。显示逻辑使用独立线程或者使用matplotlib的FuncAnimation定时器。声呐数据读取如果使用串口也建议放到独立线程把最新数据写入共享变量。10.4 紧急停车优先级最高自主避障中的任何转向决策都不应该影响紧急停车命令的执行。真实水下机器人必须有一个独立于避障算法的急停链路可以是通过遥控器直接切断推进器电源也可以是主控上优先级最高的中断任务。在软件架构中建议把急停指令放到独立线程中监听一旦收到急停信号立即执行停车指令并禁止避障逻辑恢复输出。10.5 先水池测试再开放水域水下机器人的避障参数不能直接搬到真实水域中。不同水域的温度、盐度、浊度、水流速度相差很大。建议先在一个尺寸可控的水池中布置已知障碍物验证声呐测距精度、雷达显示效果和避障控制逻辑记录实际制动距离再调整安全距离、警戒距离和危险距离。开放水域测试时务必保证有安全员和回收绳。10.6 日志与回放机制水下机器人调试过程很容易出现“问题无法复现”的情况。建议把每次运行过程中的原始声呐数据、滤波后数据、控制指令都写入文件可以通过csv或jsonl格式保存。这样出现异常时可以把日志重新喂给雷达显示和避障算法离线分析问题原因。11. 总结与后续学习方向从项目落地的角度看本文完成了三个关键闭环。第一个是声呐数据采集闭环理解了避障声呐基于飞行时间法的测距原理掌握了多路声呐的距离数据读取、滤波和无效值处理。第二个是雷达表盘显示闭环基于极坐标实现了全方位距离读数可视化能够区分安全、警戒、危险三档状态并在界面上直接标注距离。第三个是自主避障控制闭环根据多方向障碍距离计算最近障碍物通过差速控制策略输出左右推进器速度并预留了手动接管和紧急停车接口。这套方案的优点在于不依赖昂贵硬件也能完成全流程开发。使用模拟声呐时你可以在办公室把雷达表盘、数据融合、避障控制逻辑全部联调完毕后续采购真实声呐后只需替换sonar.py中的数据源并在水池中重新标定距离阈值。如果你要继续深入可以从三个方向入手。一是传感器融合方向把避障声呐和姿态传感器、深度计、DVL多普勒测速仪结合起来构建更好的水下环境感知模型。二是建图与定位方向把多波束声呐数据用于水下 SLAM让机器人在未知水域中实时构建障碍物地图。三是智能决策方向尝试用深度强化学习或模糊控制替代手工状态机使机器人在复杂水流和随机障碍物环境中具备更平滑的避障能力。实际项目里永远不要只看某一个模块的调试结果一定要把“感知 - 显示 - 决策 - 执行”看成一个整体。避障声呐数据再准如果雷达表盘没有正确显示控制算法再聪明如果推进器响应不及时整个系统仍然会失效。先把每个环节的最小闭环跑通再逐步增加声呐数量、提高控制频率、优化转向平滑度你的水下机器人避障系统就能一步步工程化落地。