0 摘要

本文介绍了一个名为Kimera-Multi的多机器人同时定位与建图（SLAM）系统。真实应用需要多机器人SLAM系统具有稳健性，能够识别和拒绝由感知混淆导致的错误的机器人间和机器人内部环路闭合，并且只依赖本地（点对点）通信来实现分布式定位和建图；此外，为了实现高级决策和空间人工智能，这些系统捕获语义信息也是必要的。Kimera-Multi通过一组配备视觉惯性传感器的机器人团队实现。每个机器人使用Kimera构建本地轨迹估计和本地网格。当通信可用时，机器人会启动基于分布式 graduated nonconvexity 算法的分布式地点识别和鲁棒姿态图优化协议。所提出的协议允许机器人通过利用机器人间的环路闭合来改善其本地轨迹估计，同时抵抗异常值的干扰。最后，每个机器人使用其改进的轨迹估计来使用网格变形技术纠正本地网格。Kimera-Multi在逼真的仿真、SLAM基准测试数据集和使用地面机器人收集的具有挑战性的户外数据集中进行了演示。实验显示，Kimera-Multi：1）在稳健性和准确性方面优于现有技术；2）在实现完全分布式的情况下达到可与集中式SLAM系统相媲美的估计误差；3）通信带宽小；4）生成准确的度量-语义3D网格；5）模块化且也可以用于标准应用。本文介绍的Kimera-Multi系统可以用于多机器人同时定位与建图（SLAM），具有稳健性和完全分布式的特点，并且能够捕获语义信息从而实现高级决策和空间人工智能。该系统通过一组配备视觉惯性传感器的机器人团队实现，每个机器人都可以构建本地轨迹估计和本地网格，并使用分布式算法实现鲁棒姿态图优化以及机器人间的环路闭合。最终，Kimera-Multi可以生成准确的度量-语义三维网格。此外，该系统也可以用于不带语义标签的三维重建或者仅用于轨迹估计。关键词：多机器人系统、同时定位与建图、机器人视觉系统。

1. 引言

多机器人协同同时定位和建图（SLAM）是机器人研究中的一个重要主题，因为它能够在长时间内提供大规模环境的情境感知。这种能力对于各种应用非常基本，例如工厂自动化、搜救、智能交通、星球探索以及军事和民用监视和监测。

本文通过开发一种系统，使得在通信带宽的现实约束下实现实时密集度量-语义三维网格模型的估计，推进了协同SLAM的最新技术。三维网格捕获了机器人所在环境的完整且密集的几何信息。此外，通过使用人类可理解的语义标签（例如“建筑物”，“道路”和“物体”）对网格进行注释，我们的系统提供了环境的高级抽象，这对于实现下一代空间感知或空间人工智能以及高级决策至关重要。在单机器人SLAM中，度量-语义模型已被用于开创性的工作，例如SLAM++和SemanticFusion。最近的工作包括使用多核CPU实时构建度量-语义三维模型的系统，例如Kimera和Voxblox++。在多机器人SLAM中，许多现有系统依赖于稀疏的地标（例如[6]和[7]）。尽管这些系统在协同定位方面表现出色，但它们不能提供密集映射的完整解决方案，而这对于关键导航任务（例如避碰和运动规划）是必需的。另一方面，最近的多机器人系统开始利用语义信息来帮助协同SLAM，但潜在的表示仍然是稀疏的（例如物体）。最近的工作采用了密集的语义分割，但该方法只能进行局部地图的成对匹配。总体而言，还没有一个完整的多机器人系统可以用于密集度量-语义SLAM，部分原因是建立这样的模型涉及到额外的通信和计算成本。本文通过开发一种协同度量-语义SLAM系统来弥补这个空白。此外，所提出的系统是完全分布式的，可以在真实的通信约束下运行。

此外，本文旨在提高协同SLAM在具有挑战性的现实环境中的稳健性。在实践中，由于相似的场景，感知混淆经常会导致错误的机器人间数据关联（即异常值环路闭合），进而导致标准估计后端的灾难性故障。在多机器人SLAM中，这个问题更加复杂，因为缺乏一个共同的参考框架和全局无异常值的里程碑骨干。虽然最近的工作针对协同SLAM提出了几种鲁棒估计技术，但它们要么过于依赖初始化，要么采用启发式搜索方法，这会导致低召回率（即错过正确的环路闭合）。本文通过开发基于graduated nonconvexity（GNC）（“Graduated nonconvexity”是指一种在优化中使用的技术，它以可控的方式使目标函数变为非凸函数，从而更好地探索解空间。这种技术通常用于深度学习中，因为目标函数高度非凸且难以优化。通过逐渐引入非凸性，优化过程可以摆脱局部最小值并找到更好的解。这种方法有时也被称为“课程学习”。）的鲁棒分布式后端来解决这个挑战。

贡献：本文的主要贡献是Kimera-Multi，一种用于多机器人密集度量-语义SLAM的完全分布式系统。我们的系统使一组机器人能够在实时协作中估计环境的语义注释的三维网格模型。每个机器人运行Kimera以处理机载视觉惯性传感器数据并获得本地轨迹和三维网格估计。当通信可用时，会触发完全分布式过程来执行机器人间的地点识别、相对位姿估计和鲁棒分布式轨迹估计。从联合优化的轨迹估计中，每个机器人执行实时局部网格变形，以纠正局部映射漂移并改善全局地图的一致性。Kimera-Multi的实现是模块化的，并允许禁用或替换不同的组件。图1展示了Kimera-Multi在美国马萨诸塞州Medfield采集的三个机器人协同SLAM数据集上的应用。

本文的第二个技术贡献是一种新的、抗异常值分布式姿态图优化（PGO）方法，它作为Kimera-Multi的分布式后端。第一阶段通过使用GNC来估计机器人之间的坐标系相对变换，在全局参考框架中初始化机器人的本地轨迹。这种方法对异常值环路闭合具有鲁棒性，并且非常高效，因为它不需要迭代通信。第二阶段解决了完整的鲁棒PGO问题。为此，我们提出了一个建立在最先进的Riemannian block-coordinate descent（RBCD 是一种优化算法，用于在黎曼流形上最小化目标函数。在此算法中，每次迭代只更新一个坐标轴（或者一组坐标轴），以避免计算整个梯度。这种分块更新的方法可以减少计算量和存储需求，并且可以处理大规模数据。黎曼流形是一种非欧几里得的空间，其中每个点都有一个切空间，用于描述该点的局部性质。RBCD算法利用了切空间的结构来优化目标函数。）求解器之上的GNC分布式扩展。与以前的技术相比，我们的方法实现了更加鲁棒和准确的轨迹估计，并且对参数调整不太敏感。

我们的第三个贡献是广泛的实验评估。我们对Kimera-Multi在一系列大规模逼真的仿真和SLAM基准数据集上进行了定量评估。此外，我们还使用自主地面机器人收集的挑战性实际数据集演示了Kimera-Multi的性能。我们的结果表明Kimera-Multi：1）与以往工作中采用的替代技术相比，提供了更加鲁棒和准确的分布式轨迹估计；2）在完全分布式的情况下实现了与集中式系统类似的估计精度；3）具有高效的通信，与基线集中式系统相比可实现高达70％的通信减少；4）构建了准确的度量-语义三维网格；5）是模块化的，并可以根据用户需求禁用或替换不同的功能，如网格重建和语义注释。

与以前的研究相比的创新之处：本文早期版本在[18]中提出了Kimera-Multi，但本文扩展了该工作，并增加了两个新贡献。首先，我们开发了一种抗异常值的完全分布式轨迹估计方法，基于GNC。在[18]中，我们使用了一种增量的、基于对数置信度的成对一致性最大化（PCM 它是一种用于通信和储存数字信号的编码方法。在这个上下文中，PCM可能被用来拒绝异常值环路闭合。异常值是指在数据集中与其他数据点明显不同的值。环路闭合是指在控制系统中出现的问题，其中输出信号被反馈回输入端，导致系统产生不稳定的行为。使用PCM可以拒绝这些异常值并避免环路闭合问题。）来拒绝异常值环路闭合。然而，PCM采用了图论形式，在实践中依赖启发式的最大团搜索，从而导致召回率低。在本文中，我们采用了一个更加高效和准确的GNC方法，其不需要迭代通信，并且能够保证最优解。第二个贡献是我们开发了一种分布式PGO算法，它在每个轮询步骤中只跨越一小部分变量，从而大大减少了通信负载。此外，我们还提出了一种新型的块坐标下降求解器，这是分布式Riemannian优化中最快的求解器之一。

另一个与以前的工作相比的创新点是我们首次将度量-语义三维网格捕获和注释引入到多机器人SLAM中。我们的系统能够纠正局部映射漂移，并生成密集和具有语义注释的全局地图，这对于许多关键应用程序非常重要。我们的系统不仅包含传统的几何信息，还包括语义标签，这使得环境的高级抽象成为可能。这些信息可以用于进一步的计算机视觉任务，如目标检测、语义场景理解和决策制定。

总之，本文提出了一种完全分布式的多机器人协同度量-语义SLAM系统Kimera-Multi，这是一种实时、高效、鲁棒的解决方案，可以在长时间内提供大规模环境的情境感知。我们开发了一种新型的抗异常值分布式PGO算法，并将度量-语义三维网格注释引入到多机器人SLAM中。Kimera-Multi在大规模仿真和实际数据集上进行了广泛的评估，并且表现出色。未来，我们计划将Kimera-Multi拓展到更多领域，并将其应用于更加挑战性和实际的场景中。

2. RELATED WORK

在单机器人SLAM领域，最近的研究趋势是将度量和语义信息结合起来构建地图。Kimera是一种提供准确实时视觉惯性里程计（VIO）和轻量级网格重建的框架，本文的多机器人度量-语义SLAM系统是基于Kimera构建的。

多机器人SLAM中，大多数方法都集中在稠密几何表示（如占用地图）或稀疏地标地图上。最近一些工作开始将稀疏对象或稠密语义信息纳入多机器人感知中。本文扩展了早期的Kimera-Multi，并增加了一个新的异常值鲁棒的分布式PGO算法和其他实验评估。

互相闭合的环路对于将机器人的轨迹对齐到公共参考系并改善其轨迹估计至关重要。传统的中央视觉SLAM系统通过将全局描述符和本地视觉特征发送到中心服务器来进行环路闭合检测。最近的工作开发了分布式和通信高效的方法来进行机器人之间的环路闭合检测。

PGO是现代SLAM系统中常用的估计方法。集中式PGO方法收集所有测量结果并在中心站点计算所有机器人的轨迹估计。同时，也有很多努力设计分布式PGO方法。标准的最小二乘PGO公式容易受到偏差的环路闭合的影响，所以必须对其进行改进，如RANSAC、M-estimation、IRLS等。本文提出了一种全新的分布式GNC方法，并证明其在鲁棒性和准确性上优于PCM。

3. SYSTEM OVERVIEW

在Kimera-Multi中，每个机器人都运行着图2所示的完全分散式度量-语义SLAM系统。该系统由四个主要模块组成：1）本地（单机器人）Kimera；2）分布式闭环检测；3）通过PGO进行鲁棒的分布式轨迹估计；和4）本地网格优化（LMO）。在这些模块中，分布式闭环检测和鲁棒的分布式PGO是涉及机器人之间通信的唯一模块。图3显示了这些模块之间的数据流。

Kimera [1] 在每个机器人上运行，并提供实时的本地轨迹和网格估计。特别地，Kimera-VIO [16] 作为VIO模块，在处理原始的立体图像和惯性测量单元（IMU）数据以获得机器人的里程轨迹估计方面发挥着作用。

Kimera-Semantics [16] 处理深度图像（可能是从RGB-D相机或通过立体匹配获得的）和2D语义分割[76]，并使用VIO姿态估计产生密集的度量-语义3D网格。此外，Kimera-VIO使用ORB特征和DBoW2 [37]计算每个关键帧的BoW表示，用于分布式闭环检测。有兴趣的读者可以参考[1]和[16]了解更多技术细节。

分布式闭环检测（见第IV节）在两个机器人α和β处于通信范围内时执行。机器人交换它们收集的关键帧的BoW描述符。当机器人找到一对匹配的描述符（通常对应于对同一地点的观察），它们使用标准GV技术执行相对姿态估计。相对姿态对应于一个假定的机器人间闭环，并在鲁棒的分布式轨迹估计过程中使用。

鲁棒的分布式轨迹估计（见第V节）通过使用来自Kimera-VIO和到目前为止检测到的所有假定闭环的里程测量，执行全局参考框架中所有机器人的最优轨迹估计，通过执行鲁棒的分布式PGO来实现。一开始，使用鲁棒的初始化方案找到机器人参考帧之间的粗略相对变换。然后，使用基于可分布式GNC [15]的鲁棒优化过程，使用RBCD求解器[17]同时选择内点闭环并恢复最优轨迹估计。与会议版本的Kimera-Multi [18]中使用的增量PCM技术[13]相比，我们的新方法使轨迹估计更加鲁棒准确，并且对参数调整的敏感度更低。

LMO（见第VI节）在鲁棒的分布式轨迹估计阶段之后执行。该模块执行本地处理步骤，以使每个机器人的网格变形以与分布式PGO的轨迹估计保持一致。

Kimera-Multi采用C++实现，并使用机器人操作系统[77]作为机器人和在每个机器人上执行的模块之间的通信层。该系统在线使用CPU运行，并且是模块化的，因此允许替换或删除模块。例如，如果没有语义标签可用，则该系统还可以生成密集的度量网格，或者如果用户不需要密集的重建，则仅生成优化后的轨迹。

IV. 分布式闭环检测

本节描述了Kimera-Multi的前端，该前端负责在两个机器人之间检测机器人之间的闭环。信息流程如图3所示。当通信变得可用时，机器人α通过将其自上次汇合以来的新关键帧的全局描述符发送给另一个机器人β来启动分布式闭环检测过程。我们使用DBoW2库[37]将这些描述符实现为BoW向量。收到BoW向量后，机器人β在其自己的关键帧中搜索候选匹配项，其标准化视觉相似性得分超过一个阈值（在我们的代码中≥0.1）。当确定潜在的闭环时，机器人执行标准的GV以估计两个匹配关键帧之间的相对变换。在我们的实现中，机器人β首先从机器人α请求匹配关键帧的三维关键点和关联描述符（见图3）。随后，机器人β使用OpenCV [78]中实现的最近邻搜索进行匹配两组特征描述符的假定对应关系。从假设的对应关系中，机器人β尝试使用Nistér的五点方法[79]和Arun的三点方法[80]结合RANSAC [56]来计算相对变换。这两种技术都在OpenGV库中实现。如果GV成功具有超过五个对应关系，则接受闭环并将其发送到鲁棒分布式轨迹估计模块。
V. 强韧的分布式轨迹估计

在Kimera-Multi中，机器人通过协作解决PGO问题来估计它们的轨迹，使用了整个团队的里程计测量和机器人之间的循环闭合。其中一些循环闭合可能是异常值（例如，由感知混淆引起），因此我们需要一种强韧的方法来解决PGO。在之前的Kimera-Multi版本[18]中，我们使用了PCM [13]的增量变体进行异常值排除，通过最大团计算来进行轨迹估计。然而，即使使用并行化[82]，精确的最大团搜索运行时间在具有700个循环闭合的图形中也超过10秒，这对于我们的应用程序来说并不实用。因此，在实践中，PCM必须依靠启发式的最大团算法，因此通常表现出较差的召回率，如第VII-A节所示。

在本文中，我们提出了一种基于GNC [15]的新型分布式强韧轨迹估计方法。GNC的主要思想是从鲁棒成本函数的凸逼近开始，并逐渐引入非凸性以防止收敛到虚假解。虽然一般而言，GNC不需要初始猜测[15]，但已经观察到在存在异常值的情况下，用于3D SLAM的全局求解器（例如SE-Sync [83]）变得太慢了[74]。因此，在[74]中，在GNC的每个迭代中执行本地优化（从无异常值的初始猜测开始），这种方法被证明非常有效。在单机器人SLAM中，可以通过将里程计测量连接起来获得无异常值的初始猜测。在多机器人情况下，不同机器人姿势之间没有里程计，因此挑战就是构建对异常值不敏感的初始猜测。

为了解决上述挑战，所提出的DGNC方法涉及两个阶段。在第一阶段中（参见第V-B节），我们使用一种异常值强韧且通信高效的方法，以全局参考框架初始化机器人轨迹。在第二阶段中（参见第V-C节），我们开发一个完全分布式的过程来执行GNC，使用RBCD分布式求解器作为子例程。算法1提供了D-GNC的伪代码。

A. 背景：GNC

我们首先简要回顾一下GNC [15]，[84]。与传统的M-estimation [85]，[86]相关的一个挑战是所使用的鲁棒成本函数ρ可能非常非凸，因此使得局部搜索技术对初始猜测敏感。GNC背后的关键思想是优化一系列更容易（即更少非凸）的替代成本函数，这些函数逐渐收敛到原始的鲁棒成本函数。每个替代问题采用与经典M-estimation相同的形式算法流程如下：

4. DISTRIBUTED LOOP CLOSURE DETECTION

原始参考论文第三节
A. Background: GNC
B. Robust Distributed Initialization
C. Robust Distributed PGO

Kimera-Multi是一个分布式多机器人协同SLAM系统，对于每个单独的机器人，它们通过视觉惯性传感器使用Kimera的Kimera-VIO和 Kimera-Semantics两个模块分别估计各自的局部位姿和局部mesh，当两个机器人可以互相通讯时，初始化基于分布式渐进式非凸性算法（distributed graduated nonconvexity algorithm）的分布式位置识别检测和位姿图优化功能，通过机器人之间的闭环检测实现对outliers的鲁棒，最后提高位姿估计的准确性和mesh重建的精度。
Kimera-Multi的核心算法流程是：分布式回环检测，分布式的轨迹优化 以及最后的 局部mesh地图优化；

分布式回环检测：当机器人之间可以互相通讯时，机器人会发送自上次通讯始到当前的所有关键帧的全局描述子，将描述子转换为BoW向量后进行关键帧内搜索匹配，匹配成功后计算两关键帧的相对变换关系；
分布式的轨迹优化：轨迹优化部分主要是使用PGO解决outliers的问题，这里作者基于GNC提出了一种分布式鲁棒轨迹估计方法，让代价函数从凸近似（convex approximation ）开始逐渐到非凸，从而避免伪解；
局部mesh地图优化：这里主要是利用上面分部署PGO的回环检测，实现对每个机器人mesh地图的优化；这里作者提出了一种提出了一种基于变形图（deformation graphs）的mesh优化方法。

Kimera-Multi在仿真数据集和真实场景中的效果；Kimera-Multi最终实现的效果如下：

1.在完全分布式的情况下提供了鲁棒和准确的轨迹估计；
2.与kimera相比提供了3D Meshs度量语义估计的准确性；
3.通讯效率的显著提升。

Kimera-Multi是2019年挂在arXiv上的，属于较早的工作，在Kimera-Multi之后是Luca组将多机器人协同SLAM系统的场景理解和3D Dynamic Scene Graph结合，又有了今年的一篇工作“Hydra-Multi: Collaborative Online Construction of 3D Scene Graphs with Multi-Robot Teams”。

4.X Kimera-Multi相关补充

Kimera
Kimera is a C++ library for real-time metric-semantic simultaneous localization and mapping, which uses camera images and inertial data to build a semantically annotated 3D mesh of the environment. Kimera is modular, ROS-enabled, and runs on a CPU.

Kimera comprises four modules:

A fast and accurate Visual Inertial Odometry (VIO) pipeline (Kimera-VIO)
A full SLAM implementation based on Robust Pose Graph Optimization (Kimera-RPGO)
A per-frame and multi-frame 3D mesh generator (Kimera-Mesher)
And a generator of semantically annotated 3D meshes (Kimera-Semantics)

Click on the following links to install Kimera’s modules and get started! It is very easy to install!

Kimera-VIO & Kimera-Mesher

Kimera-RPGO

Kimera-Semantics（Chart）

5. EXPERIMENTS

6. CONCLUSION

本文介绍了Kimera-Multi，这是一个分布式多机器人系统，用于稳健且密集的度量-语义SLAM。我们的系统通过估计能够捕捉环境的密集几何和语义信息的3D网格模型，推进了最先进的多机器人感知技术。Kimera-Multi完全分布式：每个机器人都可以独立进行导航，使用Kimera实时估计局部轨迹和网格。当通信变得可用时，机器人会进行本地通信来检测环路闭合并执行分布式轨迹估计。根据全局优化的轨迹估计，每个机器人执行LMO以优化其本地地图。我们还介绍了D-GNC，这是一种新颖的两阶段方法，用于强健的分布式PGO，它作为Kimera-Multi的估计骨干，并且胜过了之前的异常值拒绝方法。

我们使用照片真实性模拟、室内SLAM基准数据集和大规模室外数据集进行了广泛的Kimera-Multi评估。我们的结果表明，Kimera-Multi提供了稳健且精确的轨迹估计，同时完全分布式；比起Kimera输入，它可以更准确地估计带有度量-语义信息的3D网格；并且Kimera-Multi通信效率高，与基线集中式系统相比实现了显著的通信减少。

7. Reference

Hydra-Multi: Collaborative Online Construction of 3D Scene Graphs with Multi-Robot Teams

ICRA 2023: https://www.icra2023.org/

MIT Multirobot Mapping Sets New “Gold Standard” IEEE conference award-winner turns SLAM into collaborative prize hunt：https://spectrum.ieee.org/multi-robot-slam-icra2023

Kimera-Multi: Robust, Distributed, Dense Metric-Semantic SLAM for Multi-Robot Systems, TRO, 2022

Kimera: an open-source library for real-time metric-semantic localization and mapping, ICRA, 2020

3D scene graph: A structure for unified semantics, ICCV, 2019

3D dynamic scene graphs: Actionable spatial perception with places, objects, and humans, RSS, 2020

Kimera: From SLAM to spatial perception with 3D dynamic scene graphs, IJRR, 2021

Hydra: A Real-time Spatial Perception System for 3D Scene Graph Construction and Optimization, RSS, 2022

Hydra-Multi: Collaborative Online Construction of 3D Scene Graphs with Multi-Robot Teams, arXiv, 2023

Foundations of Spatial Perception for Robotics: Hierarchical Representations and Real-time Systems, arXiv, 2023

Stabilization of Complementarity Systems via Contact-Aware Controllers, 宾大

Autonomous Cave Surveying With an Aerial Robot, CMU

Prehensile Manipulation Planning: Modeling, Algorithms and Implementation, 法国图卢兹大学

Rock-and-Walk Manipulation: Object Locomotion by Passive Rolling Dynamics and Periodic Active Control, 港科

Origami-Inspired Soft Actuators for Stimulus Perception and Crawling Robot Applications, 上海大学

Kimera: an Open-Source Library for Real-Time Metric-Semantic Localization and Mapping