水下机器人避障声呐测距显示与自主避障系统实现

水下机器人（ROV/AUV）在水下作业时，遇到的核心问题往往是“看不见、测不准、躲不开”。普通光学摄像头在水质浑浊的环境中可用性极差，而 GPS 在水下又会失效，这时候避障声呐就成了最可靠的感知方式之一。实际项目中，避障声呐不仅要解决“有没有障碍物”的问题，还要把各方向的障碍物距离实时显示出来，在接近危险时自动规划躲避动作。本文围绕“避障声呐 + 测距显示 + 雷达表盘 + 自主避障控制”这套完整链路，讲解从声呐测距原理、多方向数据采集与融合，到雷达表盘可视化、分级警报和差速避障控制的具体实现，并提供一套可运行的 Python 示例代码，方便你在没有水下硬件的情况下先用模拟数据完成联调。

1. 背景与核心概念

1.1 水下机器人为什么需要避障声呐

水下机器人的工作环境通常比较复杂：水坝检测、管道巡检、水产养殖监控、水下搜救等场景中，障碍物可能来自岩壁、桥墩、渔网、缆绳、水生植物，甚至是一起作业的另一台潜水器。视觉传感器虽然能提供丰富的纹理和颜色信息，但在低能见度水域中往往只能看到几十厘米甚至更近的距离。相比之下，声波在水中传播衰减小，声呐可以在浑浊水体中实现数米到数十米的探测距离，是对水下机器人最友好的环境感知手段之一。

避障声呐的核心任务是持续输出某个方向上的障碍物距离。多个声呐按不同角度布置后，机器人就能知道自己周围一圈的障碍分布情况，相当于给水下机器人装上了“全向感知皮肤”。这些距离信息一方面送给雷达表盘做可视化，另一方面送给避障控制模块，在距离过低时自动触发减速、转向甚至紧急停车。

1.2 声呐测距的基本原理

避障声呐大多采用飞行时间法（Time of Flight，ToF）测距。声呐换能器向目标方向发射一束声脉冲，声波遇到障碍物后反射回来，由接收换能器采集回波。假设声波在水中单趟传播时间为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 硬件环境

本文示例以小型水下机器人样机为参考，你可以根据实际情况调整。典型硬件清单如下：

如果暂时没有声呐硬件，可以先用软件模拟数据完成整个流程的联调，这也是本文示例采用的方式。

3.2 软件环境

版本说明如下，实际使用时请根据你的环境调整：

• 操作系统：Ubuntu 20.04/22.04、Windows 10/11、macOS 均可。
• Python：3.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_range=5.0, seed=None): 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", baudrate=115200, angles=None): self.angles = angles or [] self.ser = serial.Serial(port, baudrate, timeout=0.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", errors="ignore").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_size=5): self.window_size = window_size self.buffer = deque(maxlen=window_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_range=5.0): self.angles = angles self.max_range = max_range self.filters = {} for angle in angles: self.filters[angle] = DistanceFilter(window_size=5) 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_width=30): """ 计算以 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_range=5.0, warning=1.2, danger=0.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, projection='polar') 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, color='red', alpha=0.15, label='danger') self.ax.fill_between(theta, self.danger, self.warning, color='yellow', alpha=0.15, label='warning') # 画声呐点 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', color=color, markersize=12) # 在点旁边标注距离读数 offset = 0.3 distance_text = f"{distance:.1f}m" self.ax.text(angle_rad, distance + offset, distance_text, ha='center', va='center', fontsize=9, color='white', bbox=dict(boxstyle='round', fc='black', alpha=0.5)) self.ax.legend(loc='upper 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_range=5.0, warning=1.2, danger=0.6): super().__init__(angles, max_range, warning, danger) self.latest_data = None self.anim = FuncAnimation(self.fig, self._animate, interval=100) 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)

刷新间隔interval=100表示每 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, safe=2.0, warning=1.2, danger=0.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_angle=0, half_width=40) left_distance = self._sector_min_distance(fused_data, center_angle=60, half_width=40) right_distance = self._sector_min_distance(fused_data, center_angle=-60, half_width=40) 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, seed=42) else: sonar = SerialSonar(SERIAL_PORT, SERIAL_BAUDRATE, SONAR_ANGLES) fusion = SonarFusion(SONAR_ANGLES, SONAR_MAX_RANGE) controller = AvoidanceController( safe=SAFE_DISTANCE, warning=WARNING_DISTANCE, danger=DANGER_DISTANCE ) radar = RadarDisplayAnimation(SONAR_ANGLES, SONAR_MAX_RANGE, warning=WARNING_DISTANCE, danger=DANGER_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 声呐读数频繁跳变

建议在实际水域中记录一段原始数据和滤波后数据的日志，对比波形来确认滤波参数是否合理。

9.2 雷达表盘刷新卡顿

9.3 避障控制抖动或振荡

9.4 串口通信丢包

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，让机器人在未知水域中实时构建障碍物地图。三是智能决策方向，尝试用深度强化学习或模糊控制替代手工状态机，使机器人在复杂水流和随机障碍物环境中具备更平滑的避障能力。

实际项目里，永远不要只看某一个模块的调试结果，一定要把“感知 -> 显示 -> 决策 -> 执行”看成一个整体。避障声呐数据再准，如果雷达表盘没有正确显示，控制算法再聪明，如果推进器响应不及时，整个系统仍然会失效。先把每个环节的最小闭环跑通，再逐步增加声呐数量、提高控制频率、优化转向平滑度，你的水下机器人避障系统就能一步步工程化落地。