最新开源DCL-SLAM:一种用于机器人群体的分布式协作激光雷达 SLAM 框架

文章目录

摘要
一、介绍
二、相关工作
- [A. 多机器人回环闭合](#A. 多机器人回环闭合)
- [B. 异常值剔除](#B. 异常值剔除)
- [C. 多机器人PGO](#C. 多机器人PGO)
[三. DCL-SLAM框架](#三. DCL-SLAM框架)
- [A. 单机器人前端](#A. 单机器人前端)
- [B. 分布式回环闭合](#B. 分布式回环闭合)
- [C. 分布式后端](#C. 分布式后端)
四、实验

摘要

代码：github

原文：原文

为了在未知环境中执行协作任务，机器人群体需要建立一个全局参考框架，并在共享的环境理解中定位自身。然而，在现实世界中，这面临许多挑战，例如环境的先验信息缺失以及团队成员之间的通信不畅。本文提出了DCL-SLAM，这是一种与前端无关的完全分布式协作激光雷达（LiDAR）SLAM框架，可在信息交换量较低的情况下实现未知环境中的协同定位。基于点对点通信，DCL-SLAM采用轻量级的LiDAR-Iris描述子进行地点识别，不需要全队的完全连接。DCL-SLAM包含三个主要部分：可替换的单机器人前端LiDAR里程计 、用于检测机器人之间重叠的分布式回环闭合模块 ，以及结合排除伪回环测量的分布式后端模块，该模块适应分布式位姿图优化器。我们将该框架与多种开源LiDAR里程计集成，以展示其多样性。该系统在不同规模和环境的基准数据集和实地实验中进行了广泛评估。实验结果表明，DCL-SLAM在精度和带宽方面均优于其他最先进的多机器人LiDARSLAM系统。

一、介绍

在SLAM领域中，同时定位与地图构建（SLAM）是机器人导航中的一项基本能力，尤其在未知和无GPS环境中。已有大量文献专注于单机器人SLAM的先进方法。然而，与单机器人系统相比，多机器人系统在时间敏感的应用中，如工厂自动化、不安全区域探测、智能交通和军民搜索与救援等，表现出更显著的能力和工作效率。因此，基于单机器人SLAM，过去十年中已经研究了多种机器人群体的协作SLAM（C-SLAM）方法。C-SLAM的目标是整合各机器人的数据，建立相对位姿变换，并在机器人之间创建全局地图。

然而，在机器人群体的实际应用中，C-SLAM仍是一个未解决的问题，目前的研究面临若干挑战，因为机器人间的通信可能缓慢（带宽限制）或不可行（通信范围有限）。由于激光雷达（LiDAR）具备高分辨率点云、对各种天气和光照条件的鲁棒性等多种优势，已有许多LiDAR C-SLAM方法被提出。然而，大多数系统并不适用于机器人群体，而更适合于机器人团队，特别是在具有通信约束的大规模场景中。此外，许多C-SLAM研究通过将数据集拆分成多个部分进行系统评估，这无法代表机器人群体的实际协作。因此，在真实场景中从多个平台收集合成数据进行系统评估是必要的。尽管C-SLAM是一个相对年轻的研究领域，但它前景广阔。

基于此，本文提出了DCL-SLAM，一种完全分布式的协作激光雷达SLAM框架，面向机器人群体。DCL-SLAM专注于数据高效和鲁棒的机器人间通信，使用部分位姿测量和间接数据关联实现一致的轨迹估计。该框架兼容各种基于LiDAR里程计的要求和场景，无需额外努力。每个机器人独立执行单机器人的前端，并在队友处于通信范围内时实现协同定位。为满足通信约束，集成了轻量级全局描述子LiDAR-Iris，用于描述激光扫描和检测回环闭合，无需交换额外的原始数据。此外，我们设计了三阶段的数据高效分布式回环闭合方法，以获取机器人间的相对位姿变换。随后，基于方法进行的异常值剔除用于移除伪机器人间回环闭合，以提高鲁棒性。最后，所有收集到的测量值通过两阶段的分布式Gauss--Seidel（DGS）方法进行联合优化，所需的信息交换最小化。

我们使用多个多机器人数据集对框架进行评估，包括公开数据集（如KITTI）和使用三台无人地面车辆（UGVs）采集的校园数据集。图1直观展示了这些UGVs的轨迹和重建地图。此外，还展示了在线现场测试和可扩展性实验。

图 1. 由 DCL-SLAM 框架在前端使用 LIO-SAM 和三台 UGV 重建的地图和轨迹，分别以绿色、红色和蓝色显示，地点为中山大学东校区图书馆。

总结来说，本研究的贡献如下：

提出了一个与前端无关、完全分布式的协作LiDAR SLAM框架，支持快速迁移到不同的LiDAR传感器、平台和场景。
提出了一种高度集成的数据高效的机器人间回环闭合检测方法，通过三阶段通信流程实现高精度和鲁棒性。
进行了广泛的实验评估，包括性能、可扩展性和真实场景测试，以验证所提出的系统。该系统的自定义数据集和源代码已向公众开放。

本文其余部分的组织结构如下：第II节回顾了相关的机器人间回环闭合、异常值剔除以及多机器人位姿图优化（PGO）的背景文献。第III节介绍了所提出的DCL-SLAM系统框架。第IV节提供了实验结果，包括性能、精度和通信情况。第V节给出了结论。

图 2. 提出的完全分布式协作 LiDAR SLAM，DCL-SLAM，由可替换的单机器人前端模块组成，用于生成里程计因子和机器人内部回环因子；分布式回环闭合模块，用于检测机器人之间的回环闭合；分布式 PGO 模块，用于在排除错误回环测量的情况下估计机器人群的全局轨迹；以及点对点通信模块，用于交换必要的数据（描述符、测量值、位姿估计等）

二、相关工作

A. 多机器人回环闭合

通过检测机器人间的回环闭合来估计机器人之间的相对位姿，是在无外部定位基础设施的情况下将机器人轨迹合并到公共框架中的一种高效方法。此外，这对于补偿前端里程计漂移和提高轨迹估计的准确性至关重要。第一个问题是如何有效检测机器人间的回环闭合。在 $16$ 中，团队中的机器人使用扩展卡尔曼滤波器（EKF）通过对范围和方位进行两两估计来估算相对变换，这需要额外的传感器。在 $7$ 、 $8$ 和 $17$ 中，每个机器人生成的局部激光地图被直接传输到服务器进行集中式地图融合，这需要服务器具有高计算资源并依赖稳定的通信性能。许多基于特征的视觉C-SLAM，如CVI-SLAM $18$ 、CCM-SLAM $19$ 、DOOR-SLAM $20$ 、Kimera-Multi $21$ 、Omni-Swarm $22$ 和COVINS $23$ ，共享关键帧的二进制描述符（如定向快速旋转简要描述符ORB和二进制鲁棒独立基础特征BRIEF），并使用词袋（BoW）方法进行多机器人回环闭合。此外，Shan等人 $24$ 使用投影的LiDAR强度图提取ORB特征，通过BoW方法进行位置识别。引入BoW的关键帧全局描述符可以实现快速、鲁棒的回环闭合搜索，同时减少系统的带宽占用和后端优化的计算量，对LiDAR C-SLAM具有一定的参考意义。通常，点云的描述符提取可以分为局部、全局和基于学习的描述符。局部描述符 $25$ 、 $26$ 是从点云中关键点的局部邻域提取的。尽管局部描述符方法由于缺乏描述能力而不适用于机器人间的回环闭合检测场景，但它们可以用于验证假设的回环闭合并在几何验证中估计相对位姿。从整个点集合中提取全局描述符可以缓解描述能力不足的问题，如GRSD $27$ 、M2DP $28$ 、Scan-Context $29$ 、DELIGHT $30$ 、CoSPAIR $31$ 和LiDAR-Iris $13$ 。DiSCo-SLAM $11$ 引入了Scan-Context $29$ 用于机器人间回环闭合，并提出了一个两阶段的局部和全局分布式优化框架。Swarm-SLAM $32$ 是最近的开源多机器人SLAM系统，巧妙地结合了视觉和LiDAR传感器，利用Scan-Context和BoW进行高效回环闭合。该系统通过去中心化架构和位姿图稀疏化减少通信需求，但稀疏化可能会不经意地省略对定位至关重要的关键信息。与DiSCo-SLAM相比，考虑到带宽限制和通信范围，我们的方法提出了一个带有三阶段通信流程的分布式回环闭合框架，避免了交换所有原始或特征点云。其次，我们进行了广泛的综合实验验证所提系统，并在一个包含九个机器人的大规模团队中应用了我们的系统，而DiSCo-SLAM中只有三个机器人。最近，卷积神经网络已被用于训练特征描述符 $33$ 、 $34$ 、 $35$ 。SegMap $9$ 提出了一个用于LiDAR的基于数据驱动的描述符，用于提取有意义的特征来进行回环检测和地图重建。FPET-Net $36$ 引入了一个特征点提取模块，以减少点云大小并保留数据特征，同时通过点变换模块提取全局描述符。然而，这些基于学习的方法需要大量的训练数据。在我们的工作中，我们采用了轻量级全局描述符LiDAR-Iris，用于快速、鲁棒的分布式回环闭合检测。它对每个分区的高度信息进行编码，提取不需预训练的区分性特征，并且在无需强制匹配的情况下具有旋转不变性。回环闭合的相对位姿获取是第二个问题。在LiDAR SLAM中，通常采用3D-3D匹配方法，如ICP $37$ 和GICP $38$ 。在分布式系统中，由于通信范围和带宽限制，如何交换每个机器人上的描述符是一个关键问题。Cieslewski和Scaramuzza $39$ 预先为每个机器人分配词汇中的单词，并将整个查询发送给候选机器人以检测回环闭合。Cieslewski等人 $40$ 在一个完全连接的团队中利用NetVLAD描述符进行了高效的数据关联进行分布式回环闭合。这些方法需要较少的数据交换量，并且在团队机器人数量增加时具有良好的扩展性，但设计用于全连接系统。Lajoie等人 $20$ 、Choudhary等人 $41$ 和Wang等人 $42$ 在机器人会合时高效地交换数据，考虑到可用的通信和计算资源。这些方法更适合于大规模场景中通信范围有限的机器人群体。

B. 异常值剔除

异常值剔除对于鲁棒的PGO至关重要，因为错误测量可能由感知混淆引起。随机抽样一致性（RANSAC）算法 $43$ 在LiDAR和视觉SLAM中被广泛使用，能够应对大量异常值。然而，它根据给定模型区分内点和外点。Do等人 $44$ 通过考虑测量数据的相似性和回环闭合之间的一致性来剔除不准确的回环闭合。逐步非凸性（GNC） $45$ 被提出用于优化一系列递增的非凸代理成本函数。Mangelson等人 $14$ 提出了成对一致性测量（PCM）集合最大化方法，用于检查每对机器人间回环闭合的相容性，以实现多机器人地图的鲁棒合并。此方法被多个多机器人SLAM系统 $11$ 、 $20$ 、 $21$ 、 $22$ 采用用于异常值剔除。本文结合了RANSAC和PCM，使系统更加鲁棒。

C. 多机器人PGO

PGO是后端优化位姿的流行且有效的方法。集中式PGO方法 $17$ 、 $46$ 在服务器或机器人处收集所有信息，并在全局位姿图上估计所有机器人的轨迹。然而，由于带宽限制和通信范围有限，分布式方法 $15$ 、 $47$ 更适合多机器人系统。DDF-SAM $47$ 提出了一种基于高斯消去的基于地标的分布式轨迹优化方法。Tron等人 $48$ 提出了一种多阶段分布式黎曼共识协议，用于分布式黎曼梯度下降的执行。Choudhary等人 $15$ 提出了一种两阶段DGS方法，避免了复杂的记录管理和双重计数。Fan和Murphey $49$ 提出了一种求解分布式PGO的主化-最小化方法。Tian等人 $50$ 提出了求解PGO问题的秩受限松弛的黎曼块坐标下降（RBCD）方法，在局部网络中性能优于DGS。分布式C-SLAM解决方案 $20$ 、 $40$ 采用了 $15$ ，因其适用于有限的通信资源。在本工作中，我们也采用DGS方法作为后端。

图 3. 从机器人 α 的视角（使用 VLP-16）展示的分布式回环闭合模块示意图。

三. DCL-SLAM框架

我们提出的框架依赖于通过临时无线网络进行点对点通信。当两台机器人会合时，它们共享环境信息，以建立全局参考框架并执行分布式SLAM。此外，当通信范围内没有队友时，每台机器人执行单机器人SLAM，以确保每个机器人的独立性。

在一些现有的C-SLAM系统中 $9$ ，前端和后端模块紧密耦合，使得很难以低成本替换或更新某个改进的模块（例如，里程计和机器人间回环闭合）。因此，我们的框架将其模块化为三个主要部分：单机器人前端、分布式回环闭合和分布式后端。图2展示了我们提出的DCL-SLAM框架概述。每台机器人收集来自LiDAR和惯性测量单元（IMU）的原始数据，并本地运行单机器人前端模块（第III-A节），以估计其轨迹。在分布式回环闭合模块（第III-B节）中，选择一个来自里程计位姿的关键帧扫描，并将其描述为全局描述符（LiDAR-Iris $13$ ），以实现机器人间的地点识别。每台机器人将该描述符发送给通信范围内的其他机器人，以进行机器人间回环闭合检测。输出的回环闭合候选项随后通过ICP方法和RANSAC算法使用原始点云或特征点云进行验证，从而获得机器人间回环闭合测量值。之后，分布式后端模块（第III-C节）收集所有里程计测量值、机器人内部回环测量值和机器人间回环测量值，以计算最大集合的PCM $14$ 并过滤掉异常值。通过使用 $15$ 中提出的两阶段DGS方法来求解机器人群体的最优轨迹估计，从而达成位姿图配置的一致性。

A. 单机器人前端

DCL-SLAM框架支持与各种LiDAR里程计的单机器人前端接口。它与不同的LiDAR配置（例如，固态和机械LiDAR）、平台（例如，地面和空中车辆）以及场景（例如，室内和室外环境）兼容。在我们的实现中，LIO-SAM $2$ 被采用作为地面车辆里程计的典型示例，FAST-LIO2 $4$ 用于空中车辆，其他LiDAR里程计是替代选择。一旦里程计估计被优化，就可以构建LiDAR里程计因子 F odom r ( r ∈ { α , β , γ , ... } ) F_{\text{odom}}^{r}(r\in\{\alpha,\beta,\gamma,\ldots\}) Fodomr(r∈{α,β,γ,...}) 并发送到分布式后端模块。相关联的点云扫描用于机器人内部和机器人之间的回环闭合。为了提高单机器人前端模块的精度，需要一个机器人内部回环闭合子模块来消除里程计的漂移。在一些现有的LiDAR里程计中（例如，LIO-SAM），已经包含了机器人内部回环闭合子模块。因此，很容易提取结果来构建用于姿态分布式图优化的机器人内部回环闭合因子 F intra r F_{\text{intra}}^{r} Fintrar。此外，我们还为仅熟悉以下机器人间回环闭合模块的里程计系统（例如，FAST-LIO2）提供了一个内置的机器人内部回环闭合子模块，而无需与其他机器人通信。

B. 分布式回环闭合

在我们的框架中，分布式回环闭合模块在机器人群之间执行位置识别，以生成相对姿态变换，并在图3中显示。此外，检测由自己或其他群中机器人创建的局部地图之间的重叠部分对于减少其累积漂移并将所有机器人融合到一个一致的全局地图中至关重要。我们的分布式回环闭合模块由四个步骤组成：关键帧选择、描述、搜索和验证。为了提高计算和通信效率，我们首先在选择关键帧时，当位置和旋转与前一个相比超过阈值时进行选择。在我们的工作中，位置和旋转变化的阈值分别是1米和0.2弧度。此外，我们执行了一个三阶段的分布式回环闭合检测方法，包括共享每个关键帧的全局描述符，如果在候选中找到，则共享过滤后的点云和姿态，以及如果验证通过则共享匹配结果。它允许数据高效的通信，并且不需要机器人群之间的完全连接。

描述：LiDAR-Iris $13$ ，一个最先进的全局LiDAR描述符，被用于多机器人位置识别。如图4所示，激光扫描S被投影到其鸟瞰图上，并根据角度和径向分辨率被划分为 N r N_{r} Nr（径向方向） × N a \times N_{a} ×Na（角度方向）个箱子，由 S = ⋃ S i j , i ∈ $1 , 2 , ... , N r$ , j ∈ $1 , 2 , ... , N a$ S=\bigcup S_{ij}, i\in $1,2,\\ldots, N_{r}$ , j\in $1,2,\\ldots, N_{a}$ S=⋃Sij,i∈ $1,2,...,Nr$ ,j∈ $1,2,...,Na$ 定义。每个箱子 S i j S_{ij} Sij 将高度信息编码为8位二进制代码 c i j c_{ij} cij 作为图像 I 的像素强度

I = ( c i j ) ∈ R N r × N a . ( 1 ) I=\left(c_{ij}\right)\in \mathbb{R}^{N_{r}\times N_{a}}.\qquad(1) I=(cij)∈RNr×Na.(1)

四个一维 L o G − G a b o r LoG-Gabor LoG−Gabor滤波器被用来与图像I的每一行进行卷积，以提取图像特征。一维 L o G − G a b o r LoG-Gabor LoG−Gabor滤波器在频率域中的形式如下：

G ( ω ) = exp ⁡ ( − ln ⁡ 2 ( ω / ω 0 ) 2 ln ⁡ 2 ( σ / ω 0 ) ) ( 2 ) G(\omega)=\exp\left(-\frac{\ln^2\left(\omega/\omega_0\right)}{2\ln^2\left(\sigma/\omega_0\right)}\right) \qquad(2) G(ω)=exp(−2ln2(σ/ω0)ln2(ω/ω0))(2)

其中 ω 0 \omega_{0} ω0 是滤波器的中心频率， σ \sigma σ 是滤波器带宽的参数。通过对卷积响应应用简单的阈值操作，获得一个区分性的二值特征图像。这个描述过程不需要计算每个箱中的点数和预训练模型。

在本文中，对于Velodyne HDL-64E(KITTI数据集)和VLP-16(我们的数据集)， N r N_{r} Nr 是80， N a N_{a} Na 是360。在每个机器人中，扫描将被本地描述为一个二值特征图像。如图3所示，一旦机器人 β \beta β 处于机器人 α \alpha α 的通信范围内，机器人 β \beta β 的二值特征图像将被发送给机器人 α \alpha α 单独使用行密钥（每个关键帧约 29 k B 29~kB 29 kB）。因此，机器人 α \alpha α 可以获得所有机器人的二值特征图像的历史数据库。

搜索：接收机器人（机器人 α \alpha α）搜索共享二值特征图像的邻居。为了处理视点变化（例如，反向重访），采用傅里叶变换来估计两幅图像之间的旋转。假设图像 I α I_{\alpha} Iα 和 I β I_{\beta} Iβ 有一个偏移 ( Δ m , Δ n ) (\Delta m,\Delta n) (Δm,Δn)，那么它的傅里叶变换 F α ( u , v ) F_{\alpha}(u, v) Fα(u,v) 具有以下属性：

F α ( u , v ) = F { I α ( m , n ) ) } = F { I β ( m − Δ m , n − Δ n ) ) } = exp ⁡ ( − i ( u Δ x + v Δ y ) F β ( u , v ) ( 3 ) \begin{align*} F_\alpha(u, v)&=F\left\{I_\alpha(m, n)\right)\}\\ &=F\left\{I_\beta(m-\Delta m, n-\Delta n)\right)\}\\ &=\exp(-i(u\Delta x+v\Delta y) F_\beta(u, v)\end{align*} \qquad(3) Fα(u,v)=F{Iα(m,n))}=F{Iβ(m−Δm,n−Δn))}=exp(−i(uΔx+vΔy)Fβ(u,v)(3)

这意味着

( Δ m , Δ n ) = arg ⁡ max ⁡ m , n exp ⁡ ( − i ( u Δ m + v Δ n ) = arg ⁡ max ⁡ m , n F β ( u , v ) F α ( u , v ) . ( 4 ) \begin{align*}(\Delta m,\Delta n)&=\underset{m, n}{\arg\max}\exp(-i(u\Delta m+v\Delta n)\\ &=\underset{m, n}{\arg\max}\frac{F_\beta(u, v)}{F_\alpha(u, v)}.\end{align*} \qquad(4) (Δm,Δn)=m,nargmaxexp(−i(uΔm+vΔn)=m,nargmaxFα(u,v)Fβ(u,v).(4)

由于在历史数据库中对所有二值特征图像进行距离计算是繁重的，我们通过引入行密钥使用分层最近邻搜索来实现快速搜索。每个机器人进一步将图像简化为一个 N r × 1 N_{r}\times 1 Nr×1 向量作为查询行密钥K，通过将每行图像编码成单个值。

K = ( k i ) ∈ R N r K=\left(k_i\right)\in \mathbb{R}^{N_r} K=(ki)∈RNr

k i = ( ∑ j = 1 N a ∣ c i j ∣ 2 ) 1 2 ( 5 ) k_{i}=\left(\sum_{j=1}^{N_{a}}\left|c_{i j}\right|^{2}\right)^{\frac{1}{2}} \qquad(5) ki=(j=1∑Na∣cij∣2)21(5)

其中 c i j ∈ I c_{i j}\in I cij∈I。行密钥 K 是旋转不变的，因为编码函数与视点无关。在搜索阶段，构建了一个行密钥 k-d 树用于循环闭合候选搜索，邻居的二值特征图像使用汉明距离与查询二值特征图像进行比较

d = ∑ N r ∑ N a b i j α ⊕ b i j β < η ( 6 ) d=\sum^{N_{r}}\sum^{N_{a}} b_{ij}^{\alpha}\oplus b_{ij}^{\beta}<\eta\qquad(6) d=∑Nr∑Nabijα⊕bijβ<η(6)

验证：在验证阶段，候选匹配项随后通过在机器人 α 的过滤扫描和机器人 β 的周围子图上执行扫描到地图匹配来验证，使用 RANSAC 算法。如果内点集足够大，它就认为相应的循环闭合成功。循环闭合候选者的相对姿态变换使用 ICP 方法进行评估。之后，机器人 β 将机器人间循环闭合的相对姿态变换反馈给机器人 α（每个关键帧大约 104B）。一旦找到并共享了机器人间循环因子 Finter，两个机器人就启动了第 III-C 节中描述的分布式 PGO。

C. 分布式后端

在我们的 D C L − S L A M DCL-SLAM DCL−SLAM 框架中，分布式后端模块包括两个子模块。第一个子模块，异常值剔除，移除由机器人间循环闭合引起的错误匹配。第二个子模块， P G O R PGOR PGOR（基于图的优化），构建了一个包含所有因子的全局位姿图，以估计机器人群的全局轨迹。在后端 P G O PGO PGO中考虑了三种类型的因子，包括里程计因子 F odom r F_{\text{odom}}^{r} Fodomr，机器人内循环因子 F intra r F_{\text{intra}}^{r} Fintrar，和机器人间循环因子 F inter r F_{\text{inter}}^{r} Finterr。这些因子的一般似然项为

ϕ ( x ) = ψ ( z α i β j ∣ x ) ( 7 ) \phi(x)=\psi\left(z_{\alpha_i\beta_j}\mid x\right)\qquad(7) ϕ(x)=ψ(zαiβj∣x)(7)

其中 x = $x α , x β , x γ , ...$ x=\left $x_{\\alpha}, x_{\\beta}, x_{\\gamma},\\ldots\\right$ x= $xα,xβ,xγ,...$ 是一组机器人轨迹的姿态集合， z α i β j z_{\alpha_{i}\beta_{j}} zαiβj 是一个与机器人 α \alpha α 在时间 i 的姿态变换和机器人 β \beta β 在时间 j 的姿态变换相关的一般测量模型。

异常值剔除：分布式异常值剔除子模块剔除了在循环查找过程中仅通过匹配特征引起的错误阳性机器人间循环闭合。这种感知别名可能会导致机器人轨迹估计中的相当大的漂移。因此，PCMs（问题补充材料）最大化设置

在文献 $14$ 中引入了异常值剔除方法，并作为分布式方法实现。PCM（问题补充材料）通过以下指标检查机器人间循环闭合测量对 z α j β m z_{\alpha_{j}\beta_{m}} zαjβm 和 z α i β n z_{\alpha_{i}\beta_{n}} zαiβn 的一致性：

∥ ( z α i α j ⋅ z α j β m ⋅ z β m β n ) ⋅ z α i β n − 1 ∥ 2 2 < ϵ ( 8 ) \left\|\left(z_{\alpha_i\alpha_j}\cdot z_{\alpha_j\beta_m}\cdot z_{\beta_m\beta_n}\right)\cdot z_{\alpha_i\beta_n}^{-1}\right\|_2^2<\epsilon\qquad(8) (zαiαj⋅zαjβm⋅zβmβn)⋅zαiβn−1 22<ϵ(8)

其中 z α i α j z_{\alpha_{i}\alpha_{j}} zαiαj 是与机器人 α \alpha α 中的时间 i 和 j 相关的机器人内姿态变换， z α j β m z_{\alpha_{j}\beta_{m}} zαjβm 是与机器人 α \alpha α 中的时间 j 和机器人 β \beta β 中的 m 相关的机器人间姿态变换， ϵ \epsilon ϵ 是似然阈值。在实验中， ϵ \epsilon ϵ 设为 5.348。完成成对一致性检查后，通过寻找最大团来找到最大的PCM集合。最后，最大集合中的循环闭合传递给分布式PGOR进行进一步的联合优化。

位姿图优化：PGO子模块使用里程计测量和循环闭合测量（机器人内和机器人间）来估计轨迹的

x ∗ = arg ⁡ max ⁡ x ∏ ϕ ( x ) . ( 9 ) x^*=\underset{x}{\arg\max}\prod\phi(x).\qquad(9) x∗=xargmax∏ϕ(x).(9)

在我们的实现中，采用了文献 $15$ 中的方法，其中每个机器人使用两阶段DGS来解决其位姿图。分布式协作定位过程在算法1中进行了总结，该算法在每个机器人内部定期执行。每个机器人根据位姿图的连通性（基于机器人间循环闭合的数量）计算其优化顺序，并识别通信范围内的连接机器人和邻近机器人（第1-5行）。然后，所有邻近机器人同步初始化PGOR（第6-11行），随后进行异常值剔除以移除误报的循环闭合（第12和13行）。如果机器人中途离开或发生优化错误，则优化将中止（第14-16行，20-22行，以及25-27行）。在两阶段DGS方法的每次迭代中，必要的旋转和姿态估计将被传输到指定的连接机器人；然后，每个机器人将根据优化顺序依次进行优化（第18，19，23和24行）。通过DGS方法，群体中的机器人可以交换最少的信息，以避免复杂的记账和双重计数，并就最优轨迹估计达成共识。

四、实验

图 5. (a) 我们在中山大学东校区采集的数据集的传感器配置。请注意，附加的摄像头未在后续实验中使用。 (b) Agilex SCOUTMINI 机器人团队。

图 7. 在KITTI序列05 $从(a)到(e)$ 和我们学院数据集 $从(f)到(j)$ 上使用不同方法的轨迹结果。未能与其他轨迹合并的轨迹仅与真实值的起点对齐。(a) LIO-SAM。(b) FAST-LIO2。© DiSCo-SLAM。(d) DCL-LIO-SAM。(e) DCL-FAST-LIO2。(f) LIO-SAM。(g) FAST-LIO2。(h) DiSCo-SLAM。(i) DCL-LIO-SAM。(j) DCL-FAST-LIO2。

图8. 在严重感知别名的环境中，带有和不带有异常值剔除的DCL-LIO-SAM的轨迹结果，其中机器人 α / β / γ \alpha/\beta/\gamma α/β/γ 分别用绿色/红色/蓝色表示。(a) 没有异常值剔除。(b) 带有异常值剔除。

图9. 在大规模实验模拟中，机器人 α / β / γ / δ / ϵ / ζ / η / θ / ι \alpha/\beta/\gamma/\delta/\epsilon/\zeta/\eta/\theta/\iota α/β/γ/δ/ϵ/ζ/η/θ/ι 的地图分别用绿色/红色/黑色/棕色/紫色/蓝色/深绿色/橙色/深棕色表示。(a) 轨迹。(b) 地图。