水下机器人 SLAM 与三维重建技术路线

更新日期：2026-08-16

1. 项目目标

系统面向载人潜水器或水下机器人，覆盖两个主要场景：

1. 水下实时作业：在没有 GPS 信号的环境中实时估计机器人六自由度轨迹，并生成可用于观察、导航和覆盖检查的稠密点云。
2. 岸上离线重建：保存水下采集的完整原始数据，回到岸上后利用全局优化轨迹、TSDF 和 3D Gaussian Splatting（3DGS）等技术生成高质量三维模型。

目标不是依赖单一算法解决所有问题，而是把实时定位、传感器融合、可靠记录和高质量离线重建拆分成相互衔接的模块。

2. 总体架构

水下实时系统
ZED 双目图像与深度 ─┐
ZED IMU / 工业 INS ─┤
DVL 速度 ───────────┤
压力深度计 ─────────┤──> 时间同步与多传感器融合 ──> 实时位姿
声学定位（可选）────┘                              │
                                                   ├──> 实时轨迹
ZED 深度 ─────────────────────────────────────────┘
                                                   └──> 局部稠密点云

同步数据记录
左右原图 + 深度 + IMU/INS + DVL + 压力 + 标定 + 时间戳 + 实时位姿
                              │
                              v
岸上离线处理
回环检测与全局图优化 ──> 优化轨迹 ──> 点云/TSDF ──> 3DGS/表面重建

3. 第一阶段：水下实时 SLAM

3.1 视觉与深度前端

使用 ZED 2i 提供：

• 同步双目图像；
• 双目标定参数；
• GPU 深度图和点云；
• 内置 IMU 数据；
• ZED SDK 位置跟踪结果，可作为基线或辅助里程计。

ZED SDK 的官方 Spatial Mapping 示例采用以下流程：

双目图像 -> 深度估计 -> 视觉/惯性位置跟踪
         -> 将深度变换到世界坐标系 -> 融合 Mesh 或稠密点云

该方案数据链路短、显示稳定，适合作为实时稠密建图性能基线。但其回环、图优化和状态估计内部实现不透明，不应单独承担长航程生产定位。

3.2 SLAM 后端

当前验证阶段继续使用 RTAB-Map：

• 输入 ZED 彩色图、深度图和标定数据；
• 生成视觉里程计、轨迹和稠密点云；
• 通过回环检测及位姿图优化降低累计漂移；
• 使用数据库保存地图和关键帧，便于回放与分析。

应优化目前的 ROS 2 图像链路：

• 使用统一的 RGB-D 同步节点，只同步一次彩色图、深度图和 CameraInfo；
• 避免 rgbd_odometry、rtabmap 和监控界面重复订阅多路原始大图；
• 监控界面优先使用压缩图像；
• 实际建图优先选择稳定的 15 FPS，而不是不稳定的 30 FPS；
• 明确相机、SLAM 节点启动顺序，减少 DDS 发现和按需发布造成的启动同步超时。

3.3 多传感器融合

水下生产系统不能只依赖双目视觉。建议逐步融合：

• IMU/INS：提供高频姿态、角速度和短期运动约束；
• DVL：提供相对海底或水体的速度，是抑制水下位置漂移的重要传感器；
• 压力深度计：稳定约束垂直方向；
• 声学定位：USBL/LBL 等设备可提供稀疏绝对位置；
• GNSS：仅在水面或上岸后提供绝对位置和全局坐标对齐。

推荐使用基于 GTSAM、因子图或成熟滤波框架的融合层，而不是把所有传感器直接塞进视觉 SLAM。视觉里程计、DVL、压力、INS 和声学定位分别作为测量因子进入统一状态估计器。

4. 第二阶段：可靠数据记录

实时系统必须优先保证原始数据可恢复，即使实时建图暂时失败，岸上仍能重新计算。建议同步保存：

• 左、右原始图像及校正图像；
• ZED 深度图和置信度信息；
• ZED IMU、外部 INS 原始数据；
• DVL 速度与质量状态；
• 压力深度、温度等环境数据；
• 声学定位和水面 GNSS；
• 相机内参、双目外参、相机到机器人机体的外参；
• 所有传感器原始时间戳、同步状态和时钟偏差；
• 在线估计的轨迹、TF、诊断信息和算法配置。

ZED 建议同时录制 SVO，ROS 2 侧使用 rosbag2/MCAP 记录其他传感器和算法结果。任务开始前进行磁盘余量、写入速度和掉帧检测。

5. 第三阶段：岸上离线优化与重建

5.1 轨迹优化

离线阶段重新运行视觉特征匹配和回环检测，并融合完整的 INS、DVL、压力及可能的声学/GNSS约束，生成质量高于实时结果的全局一致轨迹。

5.2 几何重建

优先输出具有明确尺度和可测量性的几何成果：

• 优化后的融合点云；
• TSDF/Voxel Hashing 融合结果；
• 三角网格；
• 相机轨迹、覆盖范围和质量报告。

TSDF 或点云是工程测量与质量检查的基础。3DGS更适合高质量外观展示，不应替代可测量的几何模型。

5.3 3D Gaussian Splatting

3DGS处理链路建议为：

原始高质量图像
    +
校正后的相机模型
    +
离线全局优化轨迹
    +
深度/点云几何先验
    ↓
3DGS训练与优化
    ↓
可视化模型 + 与点云/Mesh对齐的工程成果

水下需要额外处理颜色衰减、浑浊散射、悬浮颗粒、人工照明变化和观察窗折射，否则这些变化可能被3DGS错误地建模为场景结构或浮点伪影。

6. pySLAM 的定位

pySLAM 是一个混合 Python/C++ 的研究型视觉 SLAM 框架，支持单目、双目和 RGB-D，并提供：

• 多种经典与现代局部特征；
• BoW、NetVLAD、CosPlace、Megaloc 等回环方案；
• g2o 和 GTSAM 优化；
• 稀疏地图、轨迹保存和重定位；
• TSDF、体素哈希及语义稠密融合；
• 增量 Gaussian Splatting；
• DUSt3R、MASt3R、VGGT 等多视图重建方法。

6.1 适合承担的工作

• 对比不同视觉特征与匹配器；
• 验证不同回环检测方法；
• 使用 ZED 数据测试双目/RGB-D SLAM；
• 研究 TSDF、语义地图及增量3DGS；
• 作为岸上离线重建和算法选型平台。

6.2 当前不适合作为生产主系统的原因

• 项目明确定位为研究框架和持续开发中的系统；
• IMU集成仍在 Roadmap，不是成熟视觉惯性管线；
• ROS 1/ROS 2支持仍在 Roadmap；
• 没有现成的 ZED SDK实时输入接口，需要自定义适配；
• 没有成熟的 DVL、压力计、INS 和声学定位融合；
• Python与大量研究依赖增加部署、版本管理和长期维护成本；
• 采用 GPLv3，闭源商业产品需提前进行许可证审查。

因此，pySLAM适合作为算法实验和离线重建平台，不建议当前直接替换 RTAB-Map或充当水下生产定位的唯一核心。

7. 各组件建议分工

组件	推荐定位
ZED SDK	相机驱动、双目深度、IMU读取、SVO记录、短程位置跟踪基线
ZED ROS 2 Wrapper	将 ZED 数据接入机器人 ROS 2 系统
RTAB-Map	当前阶段实时 RGB-D SLAM、回环、图优化、地图数据库
多传感器融合器	融合视觉、IMU/INS、DVL、压力、声学和GNSS
pySLAM	特征/回环算法研究、稠密融合、3DGS和离线实验
自研监控界面	展示视频、稠密点云、轨迹、覆盖率和设备状态

8. 推荐实施阶段

阶段 A：陆上基线验证

1. 固化 ZED SDK Spatial Mapping 的帧率、轨迹和点云基线；
2. 优化 RTAB-Map RGB-D同步和数据链路；
3. 对比 ZED轨迹、RTAB-Map轨迹及建图质量；
4. 建立可重复播放的 SVO + MCAP测试数据集。

阶段 B：pySLAM 离线验证

1. 在独立环境安装 pySLAM，避免污染 ROS 2环境；
2. 先用官方数据集跑通 stereo、RGB-D和回环；
3. 编写 ZED SVO/图像序列适配器；
4. 对相同采集序列比较 RTAB-Map与pySLAM轨迹；
5. 测试 TSDF和增量3DGS，记录速度、显存、模型质量和失败模式。

阶段 C：水池与近岸试验

1. 完成防水舱条件下的双目标定和折射验证；
2. 增加压力深度计、INS和DVL；
3. 测试低纹理、浑浊、悬浮物和不同灯光条件；
4. 评估实时轨迹连续性、漂移率、回环成功率和点云重影；
5. 验证原始数据记录的完整性和离线可恢复性。

阶段 D：生产化

1. 将关键实时路径迁移到稳定的 C++组件；
2. 建立进程看护、传感器健康检查和自动重启；
3. 建立配置版本、标定版本和任务元数据管理；
4. 设计实时降级策略，例如视觉失败时由 INS+DVL+压力维持状态；
5. 建立标准数据集、精度指标、回归测试和现场验收流程。

9. 关键风险

• 普通空气标定不能直接代表防水观察窗后的成像模型；
• 浑浊、反光、悬浮颗粒和动态生物会破坏特征与深度；
• 仅靠 IMU不能长期消除位置漂移，DVL和绝对约束很重要；
• 实时点云看起来正常不等于轨迹全局一致；
• 3DGS外观质量高不代表模型具有工程测量精度；
• ROS 2传输大量原始图像时，重复订阅与时间同步可能造成数据饥饿；
• GPLv3及第三方模型许可证可能影响商业闭源发布。

10. 当前建议结论

当前最合理的路线是：

以 ZED SDK负责稳定采集和深度，以 RTAB-Map完成当前实时 SLAM验证，以独立融合层逐步接入 INS、DVL和压力信息；完整记录原始数据，岸上通过全局优化获得高质量轨迹，再使用 TSDF和3DGS进行离线重建。pySLAM作为算法研究与离线重建平台引入，而不是直接作为生产实时定位核心。