作者：韩立 | 来源：3DCV
1、导读
协作同步定位和建图（C-SLAM）是在没有外部定位系统的环境（例如室内、地下或水下）中成功进行多机器人操作的重要组成部分。在本文中，我们介绍了Swarm-SLAM，这是一种开源C-SLAM系统，其设计具有可扩展性、灵活性、分散性和稀疏性，这些都是群体机器人技术的关键特性。我们的系统支持激光雷达、立体声和RGB-D传感，并且包含一种新颖的机器人间闭环优先级技术，可减少通信并加速收敛。我们在五个不同的数据集上评估了ROS2的实现，并在真实世界的实验中使用了三个通过自组织网络进行通信的机器人。
2、研究内容
我们提出了在一个完整的资源高效C-SLAM框架中组装的新技术，该框架符合这些关键的群体兼容性属性。我们的方法是完全去中心化的，支持不同类型的传感器（立体相机、RGB-D 相机和激光雷达），并且比以前的技术需要的通信要少得多。为了减少数据交换，我们引入了一种新颖的预算方法来基于代数连接最大化来选择候选机器人间闭环，其灵感来自于最近关于位姿图稀疏化的工作。这种位置识别匹配的预处理使我们能够更快地实现准确的C-SLAM估计，并使用更少的通信资源。此外，我们利用机器人软件工程的进步，使我们的框架与自组织网络兼容。
3、贡献
- 基于代数连通性最大化的通信约束下的稀疏预算机器人间闭环检测算法；
- 适用于零星的机器人间通信的分散式邻居管理和位姿图优化方法；
- 与群兼容的开源框架，支持激光雷达以及立体或 RGB-D 相机；
4、Swarm-SLAM
如图1所示，Swarm-SLAM由三个模块组成。首先，为了实现去中心化，邻居管理模块持续跟踪哪些机器人处于通信范围内（即可以可靠到达的邻居）以及已经交换了哪些数据。机器人以固定速率发布心跳消息，以便可以定期评估网络连接。为了使系统可扩展，其他模块查询邻居管理进程以确定哪些机器人（如果有）可用，并协调操作。
5、前端
与许多类似的机器人间闭环检测技术类似，我们采用两阶段方法，其中全局匹配生成候选位置识别匹配，并使用后者中的局部特征进行验证阶段，即本地匹配。
5.1、全局匹配
对于每个关键帧，从传感器数据中提取可以与相似性分数进行比较的紧凑描述符并将其广播给相邻的机器人。当两个机器人相遇时，我们进行簿记以确定哪些全局描述符已经被另一个机器人知道以及哪些需要传输。我们使用ScanContext作为激光雷达扫描的全局描述符，并使用最近基于CNN的CosPlace作为图像。我们使用基于余弦相似度的最近邻进行描述符匹配。一旦计算出匹配，Swarm-SLAM就会提供两种候选优先级机制：先前工作中使用的贪婪优先级算法和一种新颖的谱方法。为了执行候选优先级排序，我们将多机器人位姿图定义为：
在图2中，我们展示了我们的谱方法结果与标准贪婪方法获得的结果的比较。我们可以看到，使用我们的光谱技术选择的候选者沿着姿态图更均匀地分布，而贪婪的候选者主要集中在高相似性区域。因此，我们选择的候选者对于估计过程来说冗余较少。
5.2、本地匹配
一旦选择了机器人间闭环候选，下一步就是执行局部匹配（即几何验证）。此步骤根据传感器利用更大的局部特征、关键点或点云集合来计算候选两个顶点之间的3D相对姿态测量。为了避免计算相同的闭环两次并减少几何验证的通信负担，我们按照将顶点局部特征共享问题表述为顶点覆盖问题。当两个或多个机器人间闭环候选者共享一个共同顶点时，仅需要传输共同顶点即可有效计算所有相关的相对位姿测量。因此，通过计算最小顶点覆盖，对于二分图以及大约3个或更多机器人来说是最佳的，我们获得了避免冗余通信的交换策略。
5.3、机器人间通信
值得注意的是，无论是光谱匹配还是顶点交换策略，都需要在通信范围内的机器人中动态选举临时代理。然后，代理计算匹配并发送要传输顶点的请求。在我们当前的实现中，根据我们的邻居管理系统，代理只是范围内具有最低ID的机器人，但它可以通过不同的分散机制来选举（例如，基于每个机器人上的可用计算资源）。
6、后端
后端的作用是从姿势图中的前端收集里程计、机器人内部和机器人之间的闭环测量，然后根据这些噪声测量估计最可能的地图和姿势。如上所述，与其他基于分布式位姿图优化的最新系统不同，我们选择更简单的分散方法。与前端类似，通信范围内的机器人之间动态选举一个机器人来进行计算。其他机器人与选定的机器人共享其当前的姿态图估计，并在计算完成后接收更新的估计。重要的是，在机器人交会期间，可以通过协商临时选择任何机器人来执行位姿图优化。Swarm-SLAM使用分级非凸性求解器和稳健的截断最小二乘损失来执行位姿图优化。
为了确保在多次零星交会后收敛到单个全局定位估计而不强制执行中央权威，我们引入了锚选择过程来跟踪当前的全局参考系。在位姿图优化期间，锚点通常对应于先验，该先验为图的第一个位姿分配固定值。然后，该锚点成为结果估计的参考系。一开始，所有机器人都在它们自己的局部参考系内，其中原点对应于它们的第一个姿势。然后，当一些机器人第一次见面时，我们选择具有最低ID的机器人的第一个姿势作为锚点。因此，作为估计过程的结果，所涉及的机器人估计共享相同的参考系。在随后的交会中，基于具有最低ID的参考系来选择锚点。经过几次交会后，机器人会收敛到单个全局参考系，而不需要包括所有机器人的交会。这意味着Swarm-SLAM可以通过较小的机器人组之间的迭代估计来扩展到较大的机器人组。
7、实验结果
我们在来自五个不同数据集的七个序列上测试了Swarm-SLAM。为了展示我们框架的灵活性，我们使用 IMU、立体相机、激光雷达或组合作为输入。
机器人间闭环检测评估
在图3中，我们在代数连通性和绝对平移误差（ATE）方面比较了贪婪和谱机器人间闭环候选优先级划分技术。每种方法都用于根据预算对来自同一组候选者的循环闭合计算进行优先级排序，即一次选择一个循环闭合。我们根据候选集（x轴）内计算的闭环百分比绘制每个指标。我们连续执行优先级排序，直到选择了所有可能的匹配项。我们预计更好的优先级将在早期达到合理的准确性，并且只选择一小部分匹配。ATE 是根据包含所有可能的机器人间闭环的最终姿态图估计来计算的，因此构成了我们可以实现的最佳估计。在第一行，我们可以看到，正如预期的那样，我们的光谱优先级正确地最大化了位姿图的代数连通性。在第二行，正如预期的那样，我们可以看到我们的谱优先级比贪婪优先级更快地降低了误差。总的来说，我们的实验表明，尽早仔细选择候选者需要计算更少的机器人间闭环，以显着减少估计误差（ATE）。
分散式C-SLAM评估
在表2中，我们展示了在后端计算的所有序列的估计值，这些序列的GPS纬度和经度数据可作为地面实况。我们报告了AMD Ryzen 7 CPU上的计算时间以及所需的总通信量（以kB为单位）。使用相同的前端，我们将基于GNC的去中心化后端与两种最先进的分布式方法进行了比较：DGS位姿图优化与成对一致性最大化（PCM）相结合)用于中使用的异常值拒绝；以及基于 RCBD 求解器的分级非凸性（D-GNC）的分布式实现。我们选择的后端始终达到最高的准确度 (ATE)，而替代方法有时无法生成合理的估计。在所需的通信和计算时间方面，我们也始终优于其他方法。有趣的是，当在KITTI-360 09（我们的机器人数量最多的数据集）上进行测试时，与GNC相比，DGS+PCM和D-GNC所需的计算时间更少，但它们没有达到同等的精度，并且需要超过5倍的计算时间。数据传输量。虽然分布式方法受益于大型问题的分工，但需要更多的研究才能获得相同水平的准确性、鲁棒性和通信带宽。这证明了我们实际选择一种更简单的方法，在单个分散选举的机器人上计算后端，这种方法对通信故障更加鲁棒，并且更容易实现。
在图4中，我们显示了Swarm-SLAM对来自四个不同集合点的KITTI360 09序列的估计结果，集合点定义为机器人子集在彼此通信范围内的相遇。我们的锚点选择方案确保通过从具有最低参考系ID的机器人中选择当前的第一个姿态估计（即（a）机器人0、（b）机器人2、（c）机器人3的第一个姿态），我们可以传播机器人团队中的全局参考系。换句话说，我们能够通过机器人子集之间的连续估计来收敛到单个全局参考系，而无需强制执行连接维护或中央机构。这种分散的方法仅依赖于相邻机器人之间的本地交互，从而提高了系统的可扩展性。我们在图5中展示了剩余数据集序列上的Swarm-SLAM解决方案。
真实世界实验
为了评估Swarm-SLAM在资源受限平台上的可行性，我们将该系统部署在室内停车场，并收集有关计算时间和通信负载的统计数据。如图6所示，我们使用3个不同的机器人（Boston Dynamics Spot、Agilex Scout和Agilex Scout Mini）进行了在线真实演示，所有机器人都配备了NVIDIA Jetson AGX Xavier机载计算机、Intel Realsense D455摄像头、 Ouster激光雷达OS0-64、VectorNav VN100 IMU和用于ad hoc网络的GL-iNet GL-S1300 OpenWrt网关。我们使用激光雷达和IMU进行里程计，使用RGB-D相机进行机器人间环路闭合检测。
如表3所示，我们的机器人在实验过程中总共行驶了475米，总共产生了3103个关键帧，需要在搜索机器人间闭环时进行匹配和验证。该过程导致67个循环闭合，其中10个被GNC优化器拒绝。如此大量的异常值是由于停车场中有许多看起来相似的部分造成的。Swarm-SLAM实现了精确定位，机器人之间仅传输94.95MB的数据（不包括可视化）。通信负载主要归因于前端，因此取决于关键帧的数量。在表3中，我们还报告了平均稀疏化和位姿图优化时间。我们可以观察到稀疏化时间虽然不可忽略，但低于位姿图优化。为了缓解这个问题，我们在单独的线程中实现了稀疏化和优化。
8、结论
在本文中，我们提出了Swarm-SLAM，这是一种全面的资源高效型C-SLAM框架，旨在符合群体机器人的基本属性。在未来的工作中，我们的目标是研究地点识别中的协作域校准和/或不确定性估计，以减少机器人间闭环中测量异常值的普遍性，从而提高C-SLAM的整体准确性和弹性。总的来说，我们希望我们的开源框架能够作为研究和开发新方法和新技术的测试平台，包括位置识别、机器人间闭环检测、多机器人位姿图优化和其他开放问题。