论文：《Autonomous Teamed Exploration of Subterranean Environments using Legged and Aerial Robots》
I. 介绍
机器人系统的集体进步使得它们能够在多种自主探索和绘图任务中得到应用。现在，航空和地面机器人被用于多种搜索和救援[1-4]，工业检查[5-12]，监视[13]，行星探索[14-19]和其他任务场景。尽管取得了进步，但仍有一些环境对机器人的自主探索特别具有挑战性，并挑战了机器人导航和自主性的最新技术。其中，地下环境如地下矿山，地铁基础设施和洞穴网络尤其艰难。受到DARPA地下挑战[20]的目标和愿景的激励，这项工作在自主探索路径规划的一系列发展中达到了高潮，这些发展使得CERBERUS团队的机器人能够探索地下矿山，多层电厂，隧道，下水道，洞穴，熔岩管等。
具体来说，我们提出了两个协同的贡献，即 a) “GBPlanner2”，这是我们的基于图的地下探索路径规划器[21]的修订和增强版本，用于统一指导腿部和航空机器人（在https://s.ntnu.no/gbplanner上开源），以及 b) 新开发的用于异构机器人团队合作（COHORT）框架，该框架促进了大规模，多分支和多样化地下环境的协同探索，这些环境既有狭窄的部分也有宽阔的部分。这两种方法协同工作，使得腿部和飞行系统能够协调他们的探索，同时每个系统保持完整的个体探索能力，从而实现复杂地下领域的自主合作探索和绘图。
为了验证团队探索路径规划的提出解决方案，我们提出了一系列评估，包括在瑞士Hagerbach地下矿山（图1）的部署，使用腿部和航空机器人分别探索了37分钟和8分钟，以及大规模的模拟研究。我们展示了由于GBPlanner2和COHORT的个体属性以及它们的协同作用，机器人展示了有效的自主探索行为，这些行为能够抵御由环境的大规模，具有挑战性的地形和复杂几何形状，以及建立可靠通信的困难所引入的复杂性。结果页面维护在https://s.ntnu.no/exploration。
在剩余的论文中，第二部分介绍了相关工作，然后是第三部分的问题陈述。提出的方法在第四部分详细介绍，第五部分是评估研究，第六部分是结论。
II. 相关工作
大量的研究工作集中在自主单机器人和多机器人探索的问题上[21-35]。早期的单机器人探索工作包括“下一最佳视图”的采样[25]，和边界的检测[26]，而最近的努力集中在强大的规划技术上，如随机树和图可能结合体积计算[22, 23, 36-39]，后退视野技术[23]，多目标优化[36, 37]，信息理论方案[40]，基于学习的方法[41]，以及考虑累积定位漂移可能性的方法[22, 42]。在多机器人探索中，[30]的开创性工作提出了一种利用网格地图和规划策略的多机器人协调策略，该策略试图通过考虑到达某个边界单元的成本和每个这样的单元的“探索效用”（作为移动到该单元的机器人数量的函数）来最小化集体探索时间。最近的努力包括基于分布式推理的方案[35]，体积增益驱动的多机器人探索[32]，地面和航空系统协作的方法[43]，考虑有限通信范围和耐力效果的技术[29, 31]，关于机器人如何共同清除阻塞路径的概念[44]等等。社区最近关注了地下探索的特定问题，因为DARPA地下挑战的加速效应。作为回应，世界各地的团队提出了针对单机器人和多机器人探索的新方法。这包括四足机器人的技术[28]，针对使用航空平台进行快速探索的方法[27]，适用于腿部和飞行系统的方案[21]，利用密集的本地和稀疏的全球信息的分层框架[45]，多机器人探索策略[34]，利用地下矿山和洞穴拓扑的方法[46, 47]，以及大量的现场机器人工作[48-51]。由于自主探索的重要性，并针对腿部和飞行机器人在地下环境中的团队部署，这项工作贡献了两种方法，即团队探索协调和单机器人规划，使得在无法与机器人进行通信时，能够进行弹性的多机器人团队协作和可靠的单机器人操作，能够处理具有挑战性的地形，并能够绘制多样化和大规模的几何形状。
III. 问题陈述
这项工作考虑的总体问题是，给定机器人的运动约束模型，以及他们的机载深度传感器\(\{\mathbb{S}^i\}\)，具有水平和垂直视场(FoV) \([F_H^i,F_V^i]\)，和有效范围d i max，用一个异构的机器人团队自主地探索一个封闭的地下环境的有界体积\(V\subset \mathbb{R}^3\)。由于地下环境，如地下矿山，地铁基础设施和洞穴，通常由复杂的分支网络，多个层次，大型房间，垂直结构，陡峭的斜坡和异常地形组成，我们专注于腿部和飞行系统的团队合作，利用他们的协同效应，同时保持高度的个体自主性。
总体探索问题是全局设定的，指的是确定最初未映射的空间\(V_{unm} \overset{init}{=} V\)中哪些部分是自由的 \(V_{free} ⊂ V\)或被占据的 \(V_{occ} ⊂ V\) 。环境被表示为一个占用地图 \(\mathcal{M}\) ，将体积离散化为（自由，占用或未知）的立方体体素 \(m ∈ \mathcal{M}\)，边长为 \(r_V\)。由于大多数深度感测方式不能穿过物体，环境可能包含无法探索的空心，狭窄或更一般的遮挡部分：
定义1（剩余体积）设 \(Ξ\) 为无碰撞配置的简单连通集，\(\bar{V}_m ⊆ Ξ\) 为所有可以通过深度传感器S感知到体素\(m\)的配置集。那么剩余体积定义为 \(V_{res} = m∈ \mathcal{M} (m| \bar{V} m = ∅)\) 。
然后定义多机器人探索问题为： 问题1（多机器人体积探索）给定一个有界体积 \(V\) 和一个由 \(N_R\) 个异构机器人组成的团队，找到一组 \(N_R\) 个无碰撞路径$ σ_i，i = 1, …, N_R$ ，从初始配置 \(ξ_{init} ∈ Ξ\) 开始，导致识别出自由和占用部分 \(V_{free}\)和 \(V_{occ}\) ，使得不存在任何无碰撞配置，从该配置可以感知到 \(V \setminus \{V_{free} , V_{occ} \}\) 的任何部分（\(V _{free} ∪ V_{occ} = V \setminus V_{res}\) ）。这个问题的可行路径 \(σ_i\) 受到传感器的有限视场（FoV）、其模型范围和适用的机器人运动约束的限制。
然而，需要注意的是，这项工作的核心原则是，多机器人探索是由展示出高度个体自主性的机器人进行的，因为地下环境呈现的拓扑和材料可能禁止连续的网络连接。相应地，我们定义了单机器人探索问题，给定在时间实例\(t_k\)和持续时间 \(T_k^i\)下，一个体积 \(V^{S_i^k} ⊂ V\) 被分配给机器人 \(i\) 。
问题2（单机器人体积探索）给定一个有界体积 \(V^{S^k_i}\) 和一个机器人 \(i\)，找到一个无碰撞路径 \(σ_{S_i^k}\)，从初始配置 \(ξ_{init,i}^k\) 开始，识别出自由和占用部分 \(V_{free}^{S_i^k}\) 和 \(V_{occ}^{S_i^k}\)，使得不存在任何可达的无碰撞配置，从该配置可以感知到 \(V^{{S_i^k}} \setminus {V_{free}^{S_i^k} , V_{occ}^{S_i^k}}\) 的任何部分。因此，\(V_{free}^{S_i^k} ∪ V_{occ}^{S_i^k} = V^{S_i^k} \setminus V_{res}\)。可行路径必须尊重传感器模型，并适用于运动，即最大偏航率，对于腿部机器人，需要遍历限制，要求 \(M\) 在每个路径上提供支持地面，其倾斜度是可负担的。
IV. 提出的方法
我们提出的贡献概述了一种策略，用于多机器人地面和飞行探索主要是地下环境，该策略利用基于全面探索规划器的单个机器人的自主性增加。我们详细介绍了新的单机器人基于图的探索规划器（GBPlanner 2.0）和异构机器人团队合作（COHORT）规划方法。
A. 重新审视基于图的地下探索
我们提出的自主地下探索策略的核心是一种用于单机器人自主探索分配的、最初未映射的地下体积 \(V^{S_i^k}\) 的路径规划器。该方法建立在我们之前关于基于图的地下探索的开源工作之上[21]，我们将其称为“GBPlanner1”。本文中介绍的新的GBPlanner版本，“GBPlanner2”，已经大大改进，以最好地处理具有挑战性的地下几何形状和陡峭的斜坡，并提供改进的计算性能。GBPlanner2保留了最初提出的分叉的本地/全球规划架构，其中本地规划器负责识别尊重机器人运动和感知约束的有效探索路径，而全球规划器被触发以重新定位机器人到探索空间的先前感知的边界，当本地层报告无法找到具有显著探索收益的路径，或者确保机器人在其耐力限制内返回家。图2提供了GBPlanner2的图形概述。像GBPlanner1一样，新方法考虑了探索收益，作为机器人 \(i\) 沿着某条路径 \(σ_{L,best}\) 移动时，如果其具有视场 \([F_{H}^i , F_{V}^i]\) 和建模范围 \(d^i_{max}\) 的深度传感器 \(\mathbb{S}^i\) ，将观察到的新体积（算法1）。
与GBPlanner1相比，大多数变化都在局部探索步骤中。对于地面机器人，该方法现在明确处理（陡峭的）正或负斜坡、垂直障碍物和沟渠的情况，从而为地面系统提供真正的3D探索能力。利用基于Voxblox的体积地图表示[52]，该方法在已知的自由空间内随机采样顶点，这些顶点位于自适应计算的局部边界框内，以最佳适应局部探索的几何形状。为了识别最佳适应的边界框DL，对局部聚合的点云执行主成分分析（PCA），并将相应的归一化特征向量\([v_x, v_y, v_z]\)按比例缩放\(µ_V > 0\)（可调）。因此，可以构建一个随机局部图\(\mathbb{G}_L\)。对于飞行机器人，两个顶点之间的可接受边连接只是无碰撞的直线段。
相反，当为地面机器人规划时，该方法通过以下步骤检查两个顶点之间的每条边： a) 将沿边缘的点投影到地面上，并验证地面存在且已被映射，从而能够支持机器人的行走； b) 验证路径的倾斜度在给定机器人的可行范围内（算法2）。值得注意的是，投影还会查询最短点到地面周围圆上的点，因为地面体素有时可能会被错误地映射为“自由”（例如，由于LiDAR入射角高、水坑等）。这一关键特性还允许将顶点组织为那些具有支持地面的顶点和那些在自由空间但其下方没有任何地面（占用）体素的顶点，称为“悬挂”顶点。它们可以通过遵守边缘倾斜限制的边连接到\(\mathbb{G}_L\)中的其他顶点，并用于体积增益目的，但它们的增益按\(e^{-\gamma_H},\gamma_H > 0\)缩放，并且不允许将这些边命令给机器人。
在进行这些修改后，该方法会像在GBPlanner1中一样构建一个局部随机图。一旦图构建完成，使用Dijkstra算法从机器人的位置到所有顶点导出最短路径。沿着这些路径\(\Sigma_L\)，在每个顶点上计算体积探索增益，并累积以得出路径的总增益，如[21]所述。由于这个过程是该方法中计算要求最高的部分之一，GBPlanner2提供了仅在Dijkstra路径的叶子顶点上计算增益的选项，并使用半径\(\rho > 0\)对这些顶点进行聚类，从而允许基于附近顶点的计算增益来近似某些顶点的增益。
这种修改是针对在非常宽、长和高的环境中运行的计算资源受限的微型飞行机器人而定制的，因为射线投射用于识别从新顶点位置由深度传感器观察到的未知体素数量，这可能非常耗费计算资源。算法1-3概述了GBPlanner2中修订的局部规划。最佳路径\(\sigma_{L,\text{best}}\)由机器人导出并执行。除了利用局部层的修订边和其他小改进外，GBPlanner2的全局规划阶段与GBPlanner1在[21]中的全局规划阶段相同。
B. 异构机器人团队的合作
COHORT通过异构机器人系统的团队操作实现，每个机器人都基于GBPlanner2具备单代理探索自主能力。完成此任务的前提是所有机器人的姿态和地图都在相同的惯性框架IC中表达，这一能力由[48,53,54]中详细介绍的全球多模态、多机器人映射（M3RM）和Voxgraph框架提供。M3RM要求软件既在机器人上运行，也在集中计算中心（C2H）上运行，机器人迭代地发送选择性和压缩的局部地图数据，C2H改进并对齐机器人的地图估计，因此需要一个（可能间歇可用的）通信链接。COHORT如图3所示操作，以支持由NR个机器人团队对未知体积\(V\)（和地图\(\mathcal{M}\)）的探索。在任何时候，每个机器人\(i(i = 1, ..., N_R)\)构建一个局部“子地图”\(\mathcal{M}^l_i\)，这是通过Voxblox [52]开发的分辨率为\(r_C\)的预定义窗口内的环境体积表示。子地图窗口基于固定时间段（此处为45秒）和从开始新子地图点的最大机器人位移（此处为20米）定义，当任何一个阈值被超过时，启动一个新子地图。
一旦子地图被接收并使用Voxgraph [54]与全局地图估计\(M^C\)对齐，它们就会被COHORT处理。在接收到新子地图后，进行波前传播以识别地图前沿。波前定义为该子地图中邻近已访问体素的未访问体素列表。波前传播利用广度优先搜索（BFS），结果是获取形成公共前沿的相邻体素，这些前沿定义为邻近自由映射体素的未知体素的集合（前沿点的质心）。使用上述已访问体素的极限，每个子地图派生一个局部边界框\(D_{M^l_i}\)。在\(D_{M^l_i}\)内，该方法在该子地图的局部框架\(B^l_i\)中随机采样固定数量的顶点，并检查它们是否位于自由空间中。使用子地图中采样的自由点集（\([x, y, z]^T\)），构建一个具有\(N_C\)个顶点的图\(\mathbb{G}_{M^l_i}\)（称为“子图”），并在每个采样点的半径\(\rho_C > 0\)内包含所有无碰撞边，以及在\(M^l_i\)内沿机器人轨迹采样的点。最后，构建的\(\mathbb{G}_{M^l_i}\)被添加到COHORT的所有机器人全局图\(\mathbb{G}_C\)中，随着子图的整合，\(\mathbb{G}_C\)不断扩展到所有机器人探索的空间。
当添加新子地图或由于更新的多机器人地图对齐导致子地图的变换发生变化时，每个前沿点都会在全局地图\(M^C\)中检查占用情况。如果某个子地图的前沿点位于另一个子地图的已知体素中，则该前沿点会被删除。然后，通过使用26连通性[55]的广度优先搜索（BFS）识别所有连接组件，将前沿点聚类成前沿。小于设定最小尺寸\(f_{\text{min}} > 0\)（此处\(f_{\text{min}} = 250\)）的前沿会被丢弃，其前沿点会被删除。一旦每个前沿重新聚类后，检查全局图以识别形成前沿的点的几何质心附近的\(\mathbb{G}_C\)点。这些点与前沿相连，促进多机器人探索协调。
具体来说，COHORT中的多机器人探索通过利用上述数据结构组织和GBPlanner2的服务进行。特别是，团队中的任何机器人都通过使用GBPlanner2探索环境，但会定期根据COHORT更新其探索位置，通过重新定位到机器人团队探索的地图\(\mathcal{M}^C\)的前沿。具体来说，只要机器人在C2H的通信范围内（可能通过其他机器人或如[48]所述的面包屑通信节点进行多跳网络通信），每隔\(T_C\)秒的探索时间会执行以下步骤：
首先，使用Dijkstra算法计算所有机器人到COHORT全局图\(\mathbb{G}_C\)上所有点的距离，以及从起始位置（\(\xi_{\text{init}}\)）到所有\(\mathbb{G}_C\)顶点的距离。然后，将所有前沿点表达到全局框架\(\mathcal{I}_C\)中，并重新聚类，以便不同子地图的重叠前沿不会被单独处理，并得出每个前沿的准确大小估计。重新聚类的前沿被排序，并得出最好的\(n_C\%\)。在前\(n_C\%\)的前沿中，找到离请求重新定位的机器人最近的一个，并使用\(\mathbb{G}_C\)找到机器人要行进的路径，路径以机器人的坐标系表示，并命令系统执行。COHORT还为机器人提供一个新的（固定大小的）边界框\(D^{S^k_i}\)，更新GBPlanner2的探索目标体积\(V^{S^k_i}\)，使其现在围绕目标前沿区域。一旦到达选定的前沿，GBPlanner2将使用新的目标体积\(V^{S^k_i}\)（\(D^{S^k_i}\)）重新触发进行探索。经过\(T_C\)秒的GBPlanner2探索后，COHORT将再次被调用以更新机器人将要探索的位置，而该过程在该机器人上迭代进行，并在所有机器人上以相同方式工作。值得注意的是，当机器人与COHORT交互时，它还会在机载存储其在该交互期间的位置。如果在随后的尝试中与COHORT的通信通道不可用，系统将检查是否已探索完分配的整个\(V^{S^k_i}\)，如果是，它将返回到上次提供通信链接的点（或在需要时迭代返回到之前的此类点）。如果\(V^{S^k_i}\)尚未完全探索，机器人将使用GBPlanner2继续探索额外的\(T^e_C\)秒，然后回溯到之前的通信点。