文章目录
- 摘要
- 一、介绍
- 二、相关工作
- [三. 系统描述](#三. 系统描述)
-
- [A. 点云预过滤](#A. 点云预过滤)
- [B. 扫描匹配里程计](#B. 扫描匹配里程计)
- [C. 基于图的SLAM](#C. 基于图的SLAM)
- [D. 多机器人基于图的SLAM](#D. 多机器人基于图的SLAM)
- 四、实验
摘要
原文：原文
代码：代码
摘要---同时定位与地图构建（SLAM）是机器人领域中自主系统的关键任务。在多机器人系统中，去中心化方法因其无需中央基站并促进可扩展性而受到了广泛关注。本文提出了一种去中心化的多机器人图优化 SLAM 方法，称为 mrg slam，该方法利用了成熟的单机器人 SLAM 框架 hdl graph slam。在我们的方案中，每个机器人独立运行基于激光雷达（LIDAR）的图优化 SLAM 算法，实现了高效地探索未知环境。机器人可以交换独特的图节点和边，并将共享的信息纳入各自的 SLAM 解算中。具体而言，每个机器人将交换到的图节点、相关的点云数据以及其他机器人提供的边整合进自己的图中，从而推动每个机器人构建全局一致的地图。我们通过在仿真环境和实际实验中的多机器人 SLAM 评估，验证了该算法的有效性。
一、介绍
摘要---同时定位与地图构建（SLAM）是移动机器人和自主系统领域的一个基础性问题，过去已有大量研究[1]。它使机器人能够构建一致的周围环境地图，同时利用这些地图来改善自我定位。SLAM 在诸如搜救任务、人类无法到达的地方以及太空探索等场景中发挥着至关重要的作用。多机器人协作可以显著提高整体 SLAM 系统的效率，正如早期多机器人探索研究所展示的那样[2]。这种合作体现在两个关键方面：首先，使用多个机器人可以更快速地执行环境探索；其次，具备不同效用和机动性特性的机器人可以执行异构任务，或探索某些机器人群体无法到达的区域。例如，无人机可能探索户外环境，而漫游车则在室内进行地图构建和导航。我们的主要应用场景集中在静态环境的探索，这是 Valles Marineris Explorer 项目中的一个核心任务[3]。该项目中，异构机器人群体被部署在类似火星的环境中进行导航与探索，并执行其中的各项任务。该机器人群体的一个重要组成部分是地面漫游车和爬行器，它们协同探索感兴趣区域。通过这些地面单位之间的信息交换，环境映射效率得到了极大提升，从而构建出一致的地图。因此，我们的研究重点是为 Valles Marineris Explorer 项目的具体探索需求，开发一套多机器人图优化 SLAM 系统。图优化 SLAM 是通过将环境表示为图结构，其中节点表示关键位置或姿态，边表示节点之间的空间关系来实现的[4]。节点是待优化的变量，而边则作为约束，空间上将节点相互联系。通过迭代图优化，计算一个使优化后的节点最大程度满足约束的解。在本文中，我们将单机器人 SLAM hdl graph slam[5] 扩展至多机器人应用场景。具体来说，我们设计了一种去中心化的图优化 SLAM，专门为装备有激光雷达（LIDAR）传感器的机器人量身定制。机器人通过对连续激光雷达扫描进行扫描匹配来计算里程计，并在移动一定距离时逐步添加节点和边。每个节点关联一个点云扫描，用于机器人内部和机器人间的回环检测。机器人内部的回环检测是指同一机器人对同一地点的识别，而机器人间的回环检测则涉及两个不同的机器人。当通信可用时，机器人在探索环境时可以交换图信息。每个机器人在其自己的参考坐标系中构建一致的地图。因此，机器人的初始姿态需要预先大致知道。没有专门的基站来收集所有 SLAM 信息作为机器人群体的中央管理者。然而，所提出的算法实例可以在固定计算平台上运行，并通过接收所有收集到的图节点和边，执行与中央 SLAM 管理者相同的任务。本文的主要贡献可以总结如下：
- 将一个成熟的单机器人 SLAM 扩展到多机器人场景，其中每个机器人只需要配备一个激光雷达，
- 提出了一种去中心化的多机器人 SLAM 解决方案，促进了系统的鲁棒性和可扩展性，
- 基于 ROS2 的多机器人 SLAM 系统的开源代码，专为多机器人系统设计，与其前身相比有所改进，
- 在仿真和现实场景中对 SLAM 系统进行了评估。
图 1：第二个机器人的 Kitti 05 地图结果。完整地图由两台机器人共享，而每台机器人只遍历一半的轨迹。红色和橙色线条表示机器人的 SLAM 路径。绿色和粉色点分别表示机器人内部和机器人间的回环检测。
二、相关工作
多机器人 SLAM 的综合概述可以参见 [6]。在以下内容中，我们将重点介绍使用激光雷达（LIDAR）作为主要传感器的 SLAM 方法。Dubé 等人 [7] 提出了一个基于 3D 激光雷达的在线多机器人 SLAM 方法，该方法能够从任意起始位置开始。当成功识别机器人间回环时，集中计算单元执行机器人轨迹对齐和地图合并。里程计模块使用多传感器融合，而我们的方案则专注于仅使用激光雷达的解决方案。在 [8] 中，作者提出了一个基于 F-LOAM [9] 的集中式 SLAM 系统，F-LOAM 是一种基于激光雷达的里程计和地图构建方法。每个机器人单独运行 F-LOAM，然后在随后的步骤中，由中央控制单元合并地图输出。为了让机器人使用合并后的地图，还需要额外的工作。DiSCo-SLAM [10] 是一个分布式的基于激光雷达的多机器人 SLAM 算法，使用 Scan Context [11] 描述符。3D 点云被投影到 2D 环形部分，其中 z 值表示匹配 2D 部分的强度。这种表示方法减少了带宽需求，并在机器人之间交换，用于执行位置识别。然而，当机器人的运动呈现较大滚转角和俯仰角时，Scan Context 对误差较为敏感，这使其在完全 3D 探索时效果不佳。在 [12] 中，提出了一种去中心化的 SLAM 方法，称为 Swarm-SLAM，支持来自激光雷达、相机和 RGB-D 相机的点云数据。在机器人会合过程中，传输稀疏的回环信息。回环检测通过使用紧凑的描述符进行位置识别计算。通过利用更多的局部特征集合来进行几何验证。
我们的去中心化 SLAM 方法相对于集中式系统有几个优势。如果集中式 SLAM 中的单一处理单元失败，整个地图构建系统可能会受到影响。而且，合作探索的整体鲁棒性得到了提高，因为当某个机器人或通信链路失败时，其他机器人仍可继续进行地图构建。去中心化系统在可扩展性方面更加灵活，因为可以通过增加更多的机器人来减少中央单元的计算负载。此外，机器人可以在没有与基站通信的情况下行驶更远的距离。然而，去中心化系统可能会导致更高的通信开销，尤其是在机器人群体之间多次共享信息时。此外，在没有中央管理者强制一致性的情况下，实现一致的地图表示可能会更复杂。然而，在我们的方法中，每个机器人都在自己的参考坐标系中构建一致的地图。
另一个需要考虑的点是，所提出的 [7]、[8]、[10] 方法都基于机器人操作系统（ROS1）。ROS1 通过中央 roscore 处理所有机器人间的通信，而该架构并未专为多系统通信设计。因此，在多机器人场景中使用 ROS1 算法需要付出相当大的努力。另一方面，ROS2 [13] 设计时明确允许并促进多系统通信。因此，我们将单机器人 SLAM 的代码，从 ROS1 移植到 ROS2，并以此为基础开发了我们的多机器人系统。
三. 系统描述
在本节中，我们概述了基于图的 SLAM 关键步骤，并介绍了我们的多机器人 SLAM 方法，整体框架如图 2 所示。我们首先总结了单机器人 SLAM 流程的主要处理步骤，该流程基于 [5]。这些步骤包括对点云进行预过滤、执行扫描匹配里程计计算，并最终利用这些步骤为机器人构建专属的图优化 SLAM。接下来，我们介绍了我们提出的多机器人扩展，特别关注图信息交换和机器人间回环检测。
A. 点云预过滤
在处理点云数据时，对其进行均化处理有助于后续的处理步骤。当进行扫描匹配并需要获取点对应关系时，类似的点云特别有用，这在里程计模块以及回环检测中都会用到。点云通过体素网格滤波器（具有固定分辨率）进行降采样。此外，通过应用半径滤波器去除离群点，当点云中某个点在给定半径内没有一定数量的邻居时，该点被视为离群点。最后，去除非常靠近或非常远的点。预过滤阶段输出的点云将被输入到里程计模块和图优化 SLAM 模块中。
B. 扫描匹配里程计
扫描匹配是在预过滤的点云上执行的，它估计两个连续扫描之间的6自由度（DOF）变换（即，一个姿态）。通过这些姿态的连接，可以计算出机器人的里程计。输出频率由LIDAR执行完整扫描所需的时间决定。扫描匹配算法是广义迭代最近点（GICP）[14]、[15]的一个变体，它通过使用多线程来优化速度。GICP旨在通过对齐两个3D点云a、b，找到变换 T ∈ S E ( 3 ) T \in SE(3) T∈SE(3)，以最小化两个扫描中对应点之间的距离。
d i = p b , i − T p a , i ( 1 ) d_i = p_{b, i} - T p_{a, i} \quad (1) di=pb,i−Tpa,i(1)
其中 p ∘ , i p_{\circ, i} p∘,i 是相应点云的第i个点。GICP算法找到最佳变换T，使两个点云对齐：
T = arg min T ∑ i d i ⊤ ( C i B + T C i A T ⊤ ) − 1 d i ( 2 ) T = \underset{T}{\arg\min}\sum_i d_i^{\top}\left(C_i^B + T C_i^A T^{\top}\right)^{-1} d_i \quad (2) T=Targmini∑di⊤(CiB+TCiAT⊤)−1di(2)
其中 C i A C_{i}^{A} CiA 和 C i B C_{i}^{B} CiB 表示由采样点的表面建模的协方差，遵循正态-正态分布，即： p a , i ∼ N ( p ^ a , i , C i A ) , p b , i ∼ N ( p ^ b , i , C i B ) p_{a, i} \sim \mathcal{N}(\hat{p}{a, i}, C{i}^{A}), p_{b, i} \sim \mathcal{N}(\hat{p}{b, i}, C{i}^{B}) pa,i∼N(p^a,i,CiA),pb,i∼N(p^b,i,CiB)。点之间的平面到平面距离被用作估计扫描之间变换的误差指标。在我们的情况下，初始猜测是机器人的前一个姿态。扫描匹配里程计的输出被转发到图SLAM。
C. 基于图的SLAM
在3D SLAM场景[4]中，优化目标是节点，由6自由度姿态表示。对于基于LIDAR的SLAM，每个节点都与相应的点云关联。类似于基于相机的算法，这样的节点被称为关键帧。我们交替使用"节点"和"关键帧"这两个术语。虽然节点主要指的是图和姿态，但关键帧强调了与每个节点相关联的点云所起的关键作用。点云用于构建环境的一致性地图，并检测关键帧之间的回环闭合。地图的原点由一个固定的锚节点表示，没有相应的点云。节点之间的边，即空间约束，也是连接图的两个节点的6自由度姿态。我们区分三种类型的边：锚点、里程计和回环闭合边。尽管它们都是由一个6自由度姿态表示，但它们服务于不同的目的。锚边是起始姿态和全局地图框架之间的变换，这在初始化期间设置。里程计边连接两个关键帧，并在机器人通过空间移动时收集。为了减少计算成本并不向图中添加冗余信息，只有在漫游车行进了一定距离或旋转了一定角度后才添加新节点。最终的边的来源是通过在节点和它们对应的点云之间寻找回环闭合来计算的。单机器人SLAM后端基于[4]，并使用图优化框架 g 2 o g^{2}o g2o[16]。SLAM后端的一个核心方面是围绕迭代地寻找非线性代价函数的最小值：
F ( x ) = ∑ ⟨ i , j ⟩ ∈ C e ( x i , x j , z i j ) ⊤ Ω i j e ( x i , x j , z i j ) , ( 3 ) x ∗ = arg min x F ( x ) , ( 4 ) \begin{align*} F(x)&=\sum_{\langle i, j\rangle\in\mathcal{C}} e\left(x_i, x_j, z_{i j}\right)^{\top}\Omega_{i j} e\left(x_i, x_j, z_{i j}\right),(3) \\ x^*&=\arg\min_{x} F(x),(4)\end{align*} F(x)x∗=⟨i,j⟩∈C∑e(xi,xj,zij)⊤Ωije(xi,xj,zij),(3)=argxminF(x),(4)
其中 x i , x j x_{i}, x_{j} xi,xj 代表受边 z i j z_{i j} zij 约束的图节点和信息矩阵 Ω i j \Omega_{i j} Ωij。误差项 e ( x i , x j , z i j ) e\left(x_{i}, x_{j}, z_{i j}\right) e(xi,xj,zij) 是一个向量函数，用于衡量参数满足约束的程度。目标是找到满足约束并最小化代价函数的参数集。回环闭合是一种位置识别技术，它能够补偿里程计输出的累积漂移，并构建全局一致的地图。每当注册新的关鍵帧时，在将它们添加到图之前，会测试它们与有效候选关键帧之间的回环闭合。为了防止与同一机器人最近添加的关键帧重复回环闭合，需要行进一定距离后才考虑过去的关鍵帧作为候选。此外，在为新关键帧注册回环闭合后，不会考虑与回环闭合节点太近的关键帧。通常，当它们与最近的图节点相距太远时，不会考虑关键帧作为回环闭合候选。一旦收集了所有候选帧，就在新的关键帧和候选关键帧的点云之间执行扫描匹配。当扫描匹配收敛，并且匹配点之间的欧几里得距离
s = ∑ i ∣ ∣ p b , i − T p a , i ∣ ∣ 2 ( 5 ) s = \sum_i || p_{b, i} - T p_{a, i} ||_2 \qquad(5) s=i∑∣∣pb,i−Tpa,i∣∣2(5)
低于某个阈值时，我们会根据[17]执行额外的回环闭合一致性检查，这不是原始单机器人SLAM的一部分。成功回环闭合的一致性检查在图3中以示例图节点和边进行了可视化。 x n x_{n} xn 是插入到图中的新关键帧， x k x_{k} xk 是候选关键帧。这个检查只能在节点具有前一个或下一个里程计边时执行。以下变换链之一需要接近于恒等变换。
T prev = T n k − 1 − 1 T n k T k k − 1 ≈ ! I , or ( 6 ) T_{\text{prev}} = T_{n k-1}^{-1} T_{n k} T_{k k-1} \stackrel{!}{\approx} I \text{, or} \quad(6) Tprev=Tnk−1−1TnkTkk−1≈!I, or(6)
T next = T n k − 1 T n k + 1 T k + 1 k ≈ ! I ( 7 ) T_{\text{next}} = T_{n k}^{-1} T_{n k+1} T_{k+1 k} \stackrel{!}{\approx} I \quad(7) Tnext=Tnk−1Tnk+1Tk+1k≈!I(7)
在实践中，平移的范数 ∣ ∣ t prev,next ∣ ∣ 2 ||t_{\text{prev,next}||2} ∣∣tprev,next∣∣2 需要低于某个阈值。对于相应的角度 ∣ θ ∣ = arccos ( tr ( ( R prev,next − 1 ) / 2 ) ) |\theta| = \arccos(\text{tr}((R{\text{prev,next}} - 1)/2)) ∣θ∣=arccos(tr((Rprev,next−1)/2))，其中 tr ( ⋅ ) \text{tr}(\cdot) tr(⋅) 是旋转矩阵 R ∈ S O ( 3 ) R \in SO(3) R∈SO(3) 的迹，也需要低于最大允许的角度偏差。这种形式的回环闭合检查减少了错误位置识别的可能性，这可能对整体SLAM解决方案有害。
D. 多机器人基于图的SLAM
基于图的SLAM后端对图节点和边的来源是不可知的。在执行多机器人SLAM时，必须确保节点和边的唯一性。为此，我们使用boost库的通用唯一标识符（UUID）来标记单机器人SLAM生成的所有关键帧和边。这确保了机器人只向本地图中添加一次唯一的节点和边。每个机器人发布最近的SLAM姿态，所有参与协作探索的机器人都会订阅这个姿态，并用于触发图节点和边的交换。我们实现了两个机器人协作通信的触发器。根据网络设置，图信息的交换只有在接近距离时才可能。在这种情况下，当一个机器人的位置在另一个机器人的某个半径内时，会请求图。如果在整个探索过程中通信始终可用，那么当任何机器人的过去位置在请求机器人的前路径的某个半径内时，机器人会请求另一个机器人的图。这种触发图交换的方式确保了在可能存在潜在回环闭合时进行信息交换。图请求包含请求机器人已经处理过的所有关键帧和边的UUID。随后，响应将只包含不是请求机器人图的一部分的节点和边。关键帧还包含与相应图节点相关联的点云，这主要影响了网络带宽需求。当整合交换的图信息时，不同的机器人可能检测到相同的回环闭合。因此，我们在将其添加到图之前，还会额外检查这两个节点是否已经包含回环闭合边。在鲁棒性方面
图 3：新关键帧 x n x_n xn 及其候选帧 x k x_k xk 的回环闭合一致性检查。需要橙色或粉红色的变换链接近于恒等变换。
图 4：示例性的多机器人图优化 SLAM，展示了相关的节点和边。
在鲁棒性方面，这种交换图信息的方法增强了系统的弹性，例如在系统重启的情况下。机器人可以作为整体SLAM解决方案的中继站，允许一个已经重启并丢失了其SLAM图的机器人通过其他探索中的机器人重建其图。这些机器人包含了之前SLAM图中的节点和边，有助于恢复丢失的信息。除了对点云进行预过滤外，在执行多机器人SLAM时还会移除额外的点。在其他机器人估计位置周围的半径内的点被排除在LIDAR扫描之外。这确保了在静态环境中不会将潜在的动态点包含在全球SLAM地图中，这可能会妨碍在相应关键帧处的回环闭合检测。为了避免与其他机器人发生碰撞，使用了一个单独的以机器人为中心的地图，该地图也包含动态障碍物，并在LIDAR频率下更新。然而，这超出了本文的范围。
图4描述了一个多机器人图的相关节点和边。x和z表示第一个机器人的节点和边， x ′ x^{\prime} x′表示第二个机器人的节点， z ′ z^{\prime} z′表示两个机器人节点之间的机器人间回环闭合边。锚节点zanchor只为第一个机器人命名。边 z i , j z_{i,j} zi,j（其中j=i+1）对应于里程计边（红色），而 z 41 z_{41} z41对应于机器人内部的回环闭合边（绿色）。第二个机器人的里程计边（橙色）没有命名，但是 z 22 ′ z_{22}^{\prime} z22′和 z 32 ′ z_{32}^{\prime} z32′是两个机器人之间的回环闭合边（粉红色）。
四、实验
图 5：由两台机器人探索的砾石坑仿真环境。3D 模型由布伦瑞克工业大学飞行控制研究所提供。
