多机器人协同在线构建三维场景图

来自：深蓝AI

编辑：东岸因为@一点人工一点智能

原文：多机器人协同在线构建三维场景图

图片

多机器人系统是近年来机器人研究领域的热门话题之一。由于其在大规模环境中感知和行动的能力，多机器人系统在工厂自动化、智能交通、灾难响应和环境监控等应用中变得越来越受欢迎。

本文将介绍一种名为“Hydra-Multi”的多机器人协同在线构建三维场景图的技术。在多机器人系统中，如何使一组机器人在大环境中获得态势感知是一个重要问题。

特别是，在室内环境中，如何描述空间和语义信息是一个挑战性问题。传统的地图表示方法往往只能描述二维平面上的信息，而无法准确地描述三维空间信息。因此，三维场景图作为一种高级地图表示方法被提出。

01  背景

三维场景图将三维环境描述为分层图，其中节点在多个抽象层次上表示空间概念（例如物体、房间、建筑物）和边表示概念之间的关系（例如包含或邻接）。

这种表达方式最近被提出作为三维环境的高级表示，并已成功地用于分层规划和对象搜索。三维场景图特别适合于多机器人操作，因为它们能够进行快速规划和决策。

02  Hydra-Multi系统

Hydra-Multi是第一个多机器人空间感知系统，能够根据团队中机器人收集的传感器数据在线构建多机器人三维场景图。该系统采用了一种集中式的方法，能够通过从多个机器人获取增量输入，有效地找到机器人帧之间的相对变换，并结合闭环检测来正确地协调来自不同机器人的场景图节点来构建关节三维场景图。

Hydra-Multi系统的架构如下图所示：

图片

其中，每个机器人都配备了传感器套件，包括激光雷达、RGB-D相机和惯性测量单元（IMU）。这些传感器可以提供丰富的环境信息，例如深度图像、点云和姿态信息。每个机器人都有一个本地地图表示，用于存储其周围环境的信息。当多个机器人在同一区域内时，它们可以通过无线网络将本地地图发送给中央控制节点。

图片

图2 Hydra-Multi 系统由多机器人前端和多机器人后端组成。前端负责处理单个机器人输入并检测机器人间回路闭合。后端执行我们的对齐、优化和协调框架

中央控制节点负责将所有机器人的本地地图融合成一个全局场景图。为了实现这一目标，中央控制节点需要解决以下问题：

如何将不同机器人的本地坐标系转换为全局坐标系？如何处理传感器数据之间的不一致性？如何处理传感器数据之间的噪声？如何处理闭环检测？下面我们将逐一介绍这些问题的解决方案。

2.1 坐标系转换

在多机器人系统中，每个机器人都有自己的本地坐标系。为了将不同机器人的本地地图融合成一个全局场景图，需要将它们的本地坐标系转换为全局坐标系。具体来说，需要计算每个机器人的相对位姿，即旋转和平移矩阵，以将其本地坐标系与全局坐标系对齐。

在Hydra-Multi系统中，我们采用了一种基于特征点匹配的方法来计算机器人之间的相对位姿。具体来说，我们使用ORB-SLAM2算法来提取每个机器人的特征点，并将它们与其他机器人的特征点进行匹配。通过这些匹配点，我们可以计算出每个机器人之间的相对位姿。

2.2 数据不一致性处理

由于不同传感器套件之间的差异和噪声等因素，传感器数据之间可能存在不一致性。为了解决这个问题，在系统中，我们采用了一种基于加权最小二乘法（WLS）的方法来处理传感器数据之间的不一致性。

具体来说，在WLS方法中，我们首先根据传感器类型和测量误差等因素为每个传感器分配一个权重值。然后，我们使用这些权重值来调整每个传感器测量值的贡献度，并通过最小化加权残差平方和来估计全局场景图中每个节点的位置和姿态。

2.3 可能存在噪声

数据噪声处理传感器数据中可能存在噪声，这会影响全局场景图的精度。为了解决这个问题，在Hydra-Multi系统中，我们采用了一种基于高斯过程回归（GPR）的方法来处理传感器数据中的噪声。

具体来说，在GPR方法中，我们首先将传感器数据转换为高斯过程模型，并使用该模型来估计每个节点的位置和姿态。然后，我们使用高斯过程回归算法来对估计值进行平滑处理，以减少噪声的影响。

2.4 闭环检测

在多机器人系统中，由于机器人之间的相对位姿可能会发生漂移，因此需要进行闭环检测以纠正这种漂移。在Hydra-Multi系统中，我们采用了一种基于图优化的方法来进行闭环检测。

具体来说，在图优化方法中，我们将全局场景图表示为一个无向图，并将每个节点表示为一个变量。然后，我们使用约束条件来描述节点之间的关系，并通过最小化代价函数来优化所有变量。通过这种方式，我们可以找到最优的节点位置和姿态，并纠正机器人之间的相对位姿漂移。

图片

图3（a）Hydra-Multi 前端检测回路闭合，（b）初始对准步骤使用检测到的机器人间回路闭合和稳健的姿势平均来估计机器人之间的相对姿势；

在该阶段，还提出候选节点合并，(c）使用嵌入变形图方法执行全场景图的优化，如 [4]； 候选合并作为稳健因子添加到优化中（d）基于优化的结果，基于场景图优化的解决方案更新完整的场景图。

03  实验结果

为了评估Hydra-Multi系统的性能，我们在模拟和真实场景中进行了实验。在模拟场景中，我们使用Gazebo仿真器构建了一个包含四个机器人和多个障碍物的室内环境。在真实场景中，我们使用四个TurtleBot3机器人在实验室中进行了测试。

在实验中，我们比较了Hydra-Multi系统与其他三种方法的性能：单机器人SLAM、多机器人SLAM和多机器人协同建图。结果表明，Hydra-Multi系统能够在模拟和真实场景中实现高精度的三维场景图构建，并且能够有效地处理传感器数据之间的不一致性和噪声。

此外，Hydra-Multi系统还能够通过闭环检测来纠正机器人之间的相对位姿漂移，从而提高全局场景图的精度。

图片

图4 来自异构机器人的 Hydra-Multi 融合地图：一个配备有视觉惯性传感器的机器人产生语义注释地图（场景图的右侧），另一个配备有激光雷达的机器人产生纯几何三维重建（左侧）。Hydra-Multi 能够将两者结合成一个统一的多机器人场景图（上图）

图片

图片

图5 SMI 数据集中的 Hydra-Multi

图片

图片

04  总结 

本文介绍了一种名为“Hydra-Multi”的多机器人协同在线构建三维场景图的技术。

该技术采用了一种集中式的方法，能够通过从多个机器人获取增量输入，有效地找到机器人帧之间的相对变换，并结合闭环检测来正确地协调来自不同机器人的场景图节点来构建关节三维场景图。

实验结果表明，Hydra-Multi-多系统能够在模拟和真实场景中实现高精度的三维场景图构建，并且能够有效地处理传感器数据之间的不一致性和噪声。