论文：《Graph-based Subterranean Exploration Path Planning using Aerial and Legged Robots》
1. 介绍
机器人技术的集体进步使得自主机器人探索和映射任务的潜力扩展到了越来越多的应用中，无论是在民用还是军事领域。目前，空中和地面机器人已经被用于大量的搜索和救援（Balta等人，2017；Delmerico等人，2019；Michael等人，2012；Tomic等人，2012）、工业检查（Balaguer等人，2000；Bircher等人，2015；Bircher, Kamel, Alexis, Burri等人，2016；Caprari等人，2012；Gehring等人，2019；Hutter等人，2018；Kolvenbach等人，2019；Sawada等人，1991）、监视（Grocholsky等人，2006）和商业应用（Rao等人，2016）。尽管在系统和方法上都取得了显著的进步，但仍有许多关键环境对于机器人的接入和弹性自主性特别具有挑战性。这尤其与地下环境有关，其自主探索仍然特别困难，需要新的贡献。这一事实反映在围绕国防高级研究计划局（DARPA）地下挑战组织的社区的协调努力中。地下世界呈现出一系列特性，使得机器人自主性，无论是地面还是空中，都变得困难，包括它们通常非常长且大规模，特别狭窄和封闭，多分支和多层次，自相似，感知降级，和通信匮乏。此外，它们通常具有粗糙和动态的地形，包括泥浆，瓦砾和楼梯。尽管存在这些挑战，但地下机器人自主性的重要性及其在多个应用领域的相关优势呼吁加速相关研究。用于矿山救援、地下城市基础设施（例如，地铁和下水道）的检查，以及在地球和太空的洞穴和熔岩管内的探索和科学任务的机器人都是典型的领域。
在这项研究中，我们探讨了解决自主地下探索挑战的潜在方法，如图1所示，这些方法涉及飞行和腿部机器人。特别地，本文强调了以认识地下环境特性的方式来解决探索路径规划问题，并进一步关注在多个地下矿山和掩体中进行广泛的现场评估。我们提出了一种新的全面的基于图的探索路径规划器（GBPlanner），它建立在一个针对地下领域定制的分叉的本地和全局规划架构之上。本地规划器在当前机器人位置周围的有界体积内高效地运行。它采用了一种快速探索随机图采样核心，用于评估机器人路径，这些路径最大化了与探索更多未映射体积相关的信息增益，同时确保了碰撞避免（对于飞行或行走系统）并尊重适用的可行性约束（行走机器人）。选择最佳路径是为了最大化信息增益，并随后进行优化以增强其“安全”属性。然而，在复杂和大规模的地下环境中，本地规划器可能会报告无法识别出提供足够信息增益的可接受路径（例如，当机器人到达死胡同，如矿山进口）。在这种情况下，将启动全局规划步骤，并通过搜索全局但稀疏且逐步构建的图，确保识别出通向探索空间之前感知的边缘（例如，未访问的矿山分支）的路径，从而有效地重新定位机器人以继续其任务。此外，全局规划器还持续更新返回家的路径，当机器人的剩余耐力接近其限制时，该路径将被触发。这种分叉的基于采样的方案被认为能够确保大规模、狭窄、长、通常像隧道一样的多分支地下环境的有效和稳健的探索。它还考虑了可能的机器人可行性约束，并提供了在地下环境中经常适用的方向性概念。这种针对地下环境的探索路径规划解决方案已经通过在模拟中首先使用腿部和飞行机器人进行测试，然后在多个现场实验中进行测试，这些实验包括在美国和瑞士的活动和非活动的长且狭窄的“长壁”金矿和铁矿，以及“房间和柱”煤矿和地下掩体。结果还包括在DARPA地下挑战隧道电路框架下的现场部署，并附带开源代码和公开发布给社区的相关数据集。在扩展我们在Dang, Mascarich, Khattak, Nguyen等人（2019）和Dang, Mascarich, Khattak, Papachristos等人（2019）的先前努力方面，本研究做出了以下额外的贡献：
• 通过新的路径优化步骤，改进了最大化机器人安全性的本地图搜索。 • 一种新颖的全局规划方法，该方法优化全局图以增强其拓扑结构，并提供改进的路径，以将机器人重新定位到探索空间的边界或返回家。 • 明确检测和标注边界，然后由全局规划阶段使用，以重新定位机器人，使其能够有效地继续探索。 • 通过整合可遍历性分析，将规划器扩展到腿部系统。 • 对本地和全局规划阶段的所有步骤进行详细的复杂性分析。
同时，本文强调以现场实验为重点的全面评估。为了最好地评估我们的贡献并实现研究的可重复性，本文进一步贡献:
• 一套涉及飞行和行走机器人在不同类型和拓扑的地下矿山以及地下掩体中的新的广泛实验研究。 • 与当前最先进方法的比较研究，结果表明GBPlanner超越了现有的成熟方法。 • 开源发布了基于图的探索路径规划方法（ARL，2020），以及一个广泛的数据集（ARL & RSL，2019）。
最后，我们努力使这篇文章尽可能地独立，从而使感兴趣的读者能够完全理解该方法，其性能和可能的限制。 本文的其余部分结构如下：第2部分介绍了相关工作，然后在第3部分中陈述了问题。提出的方法在第4部分详细介绍，而评估研究在第5部分和第6部分中呈现。最后，在第7部分得出结论。
2. 相关工作
这段文字是对探索路径规划的一般问题的研究工作的概述。它首先提到了一些早期的工作，如“下一个最佳视图”的采样和基于边界的探索。然后，它提到了一些更近期的研究，如收缩视野多目标规划，多机器人方法，以及大量的现场工作。然而，这些现有的规划方法大多设计用于处理户外区域或相对较小的结构化设施，并没有反映出非常大规模的，类似隧道的，多分支的，狭窄的，广泛复杂的地下环境的挑战。
为了应对这些挑战，一些研究者已经研究了针对地下探索问题的定制解决方案。例如，Baker等人和Silver等人的工作提出了基于边缘探索和交叉点检测的地下环境的拓扑探索方法。这些方法已经在Groundhog系统的地面平台上进行了验证，展示了开创性的探索能力。还有一些其他的工作强调了在几何约束环境中的运动规划，尽管并非特定于地下领域。此外，还有一些工作关注了地下环境中的定位挑战。
从系统的角度来看，Morris等人和Novák等人的工作概述了地面和潜水地下机器人的创新，而Tardioli等人的论文提供了地下使用地面机器人的概述。此外，Bakambu和Polotski以及Lösch等人的方法概述了地下机器人漫游者的导航解决方案。
这段文字是关于地下探索路径规划领域的快速发展的描述，这个发展是由DARPA地下挑战的启动所推动的。这段文字提到了一些相关的工作，例如使用有腿系统的Miller等人的工作，关于相关工件分类的数据集，关于结合接触和惯性数据进行狭窄和降级飞行机器人导航的方法，以及Lajoie等人关于机器人定位的贡献，Huang等人的轻于空气设计，以及我们团队在Bjelonic等人，Dang, Mascarich, Khattak, Nguyen等人，Dang, Mascarich, Khattak, Papachristos等人，Khattak等人，和Papachristos等人的贡献。
受这个挑战的激励，本文提出的贡献旨在提供一种本质上通用的，但也在设计上优化的，用于自主地下探索的路径规划方法。这些目标是通过一种分叉的方法来实现的，这种方法包括高效和富有资源的本地规划策略，这种策略针对体积增益进行了优化，并结合了一个全局规划层，这个层负责确保在机器人到达死胡同（例如，矿井入口）后的自主归航和机器人重新定位。该方法适用于各种机器人配置，并通过飞行和行走系统进行了演示。
3. 问题陈述
这段文字描述了地下环境中的探索路径规划问题，这是体积探索问题的一个实例，其目标是自主地构建一个完整的未知空间地图。一般来说，地下环境是一个封闭的、有界的、连通的点集，可能的例外是从地面上的接入点（例如，矿井入口或地铁入口），并且由大规模的复杂网络的分支、交叉点、多层和开口（例如，房间或洞穴）组成。它可能包含一系列的障碍物，并可能呈现出严重的可行性约束。因此，这些类型的环境（例如，地下矿山、隧道、地铁基础设施和洞穴网络）由于其长度和拓扑的独特特性，引入了多个挑战，这些挑战必须在规划器中考虑。
首先，环境的规模禁止在环境边界的全配置空间中以计算有效的方式进行规划。然而，地下环境中常见的特殊几何属性提供了一个显著的特征，规划器可以利用这个特征；迭代地探索局部子空间可以产生大量的探索奖励，而不需要穷尽地搜索整个空间。同时，随着机器人通过局部探索进行，已探索空间的明显边界和关键点，如地下矿山的分支点和交叉点出现。这个洞察让我们能够将探索路径规划问题重新定义为两个分叉的阶段，即，负责在当前机器人姿态周围的滑动空间窗口内进行有效探索的局部探索规划器，以及负责（a）当局部规划器报告其无法进展时，将机器人重新定位到以前未探索的区域，或者（b）当剩余的耐力不足以继续时，返回到预定义的家庭位置的全局探索规划器。鉴于这种两层方法，我们定义了“局部完成”和“全局完成”的概念。
4.1 局部路径规划
提出的探索架构的前端——局部探索规划器，是设计来提供针对地下环境几何形状的快速探索行为，它基于快速探索随机图算法。给定占用地图\(\mathbb{M}\)，当前机器人的配置 \(ξ_0\)，一个以当前机器人位置为中心的局部空间，其维度为 \(D_L\)，“禁止通行” \(\mathbb{M}_{NG}\) 区域捕获进一步的可通行性约束（如果适用），以及与机器人物理尺寸相关的边界框 \(D_R\)，规划器在局部体积 \(V_{D_L}\) 内随机采样一个无碰撞配置 \(\mathbb{M}_R(ξ_{rand}) ∈ \mathbb{M}_{free}\) 。从这个可行的样本出发，规划器使用最近邻搜索（Moore, 1991）在图中找到其最近的顶点\(ξ_{nearest}\) 。如果一条直线路径连接这两个$[ξ_{rand}, ξ_{nearest} ] $ 通过了碰撞检查，那么新的样本和直线路径都被添加到图中，分别作为一个新的顶点和新的边。接下来，规划器试图从这个新添加的顶点连接到在定义的半径 \(δ\) 内的邻居顶点的更密集的无碰撞边。这个构建图的步骤继续创建局部图\(\mathbb{G}_L\)，直到它超过设定的最大顶点数\(N_{\mathbb{V},max}\)或边数\(N_{\mathbb{E},max}\)。
在构建了局部图\({\mathbb{G}_L}\)之后，使用Dijkstra算法（Cormen等人，2009）来找到从根顶点\(ξ_0\)到局部图中所有其他顶点的最短路径集\(Σ_L\)。这一步旨在通过在图中只使用最小长度的路径来提高探索率，同时也有助于避免在狭窄空间（如地下隧道）中不希望出现的麻烦的锯齿形运动。在主导方向上的锯齿形模式在相对长且狭窄的拓扑结构中对探索几乎没有或没有任何好处，但由于它们的额外长度会降低探索率。考虑到足够的采样密度，由于锯齿形运动会增加路径长度，使用Dijkstra倾向于消除它们。然后，规划器计算每个顶点的VolumetricGain，这是预期的累积未映射体积，即机载范围传感器\(\mathbb{S}\)在每个顶点的机器人配置下可能感知的体积。选择这个信息度量的动机是它能够以相对较小的计算成本产生有效的行为，而信息理论的替代方法可以被视为有效的替代方法（Charrow等人，2015；Tabib等人，2003）。所提出的体积增益计算依赖于光线投射，如图3所示，它是一种灵活的方法，可以扩展到多个深度传感器。例如，很容易添加另一个从向上的深度传感器得到的项来映射更宽空间（如洞穴网络）的天花板。体积增益，以及与距离和方向相关的其他权重函数，被用来计算\(Σ_L\)中每个最短路径的\(\text{ExplorationGain}\)。特别地，给定一条路径 \(σ_i ∈Σ_L,i = 1, \cdots , n\) ，路径上有一组顶点 \(ν_j^i ∈ σ_i, j=… , 1,\cdots,m_i\)， \(\text{ExplorationGain}(σ_i)\) 的计算如下：
\[ e^{-\gamma_{\mathcal{S}}^{\mathcal{S}(\sigma_i,\sigma_{exp})}}\sum^{m_i}_{j=1}{\text{VolumetricGain}(ν^i_j)}e^{-\gamma_\mathcal{D}^{\mathcal{D}(ν_1^i,ν_j^i)}} \]
其中\(e^{-\gamma_{\mathcal{S}}^{\mathcal{S}(\sigma_i,\sigma_{exp})}}\)为转向惩罚，\(\text{VolumetricGain}(ν^i_j)\)为体积增益，\(e^{-\gamma_\mathcal{D}^{\mathcal{D}(ν_1^i,ν_j^i)}}\)为欧式距离增益
在机器人接近环境的分支点时，拦截分支边缘附近的顶点可能会获得向局部遮挡区域的非常高的体积增益，这鼓励在希望最大化探索率的交叉点上来回切换分支路径。类似的情况可能会发生在经常创建高收益期望的小遮挡区域。然而，这种行为在实践中通常是不希望的，因为它会导致机器人探索方向的突然和不必要的改变。为了在某些情况下惩罚这种行为，引入了一个相似性函数( , σ σ ) q i exp，以不利于大大偏离当前估计探索方向的路径。相似性度量在这种情况下是使用动态时间规整（DTW）方法开发的（Bachrach，2013），该方法计算计划路径\(σ_i\)和具有相同长度的伪直线探索路径\(σ_{exp}\)之间的累积欧几里得距离。\(σ_{exp}\)的方向是通过机器人姿态的时间窗口平均得到的。然后选择并优化最大化ExplorationGain的路径，机器人随后执行该路径。每次规划器运行后，整个过程会迭代重复。整个过程在图4和图5中进行了可视化，其伪代码在算法1和算法2中提供。请注意，局部完成的正式推断需要明确知道剩余空间\(V_{D_L ,\text{res}}\)。然而，在实践中，当没有发现探索增益超过小阈值\(g_ϵ>0\)的路径时，这种情况会被检测到。
4.2 全局路径规划
全局规划器在当前已探索的环境全局空间上进行搜索，并提供两个关键功能，以确保在大型地下环境中探索自主性的可扩展性。首先，当局部规划器报告由于局部完成或存在过窄的通道无法继续时，它会搜索通向未探索区域的替代路径。其次，一旦剩余时间接近其允许的预算（例如，电池寿命或用户限制的时间预算），它负责关键的归航程序。为了应对涉及多个交叉点和循环路线的大规模地下环境的这些挑战，我们提出了一种方法，从迭代派生的局部探索图中逐步构建并持续维护一个轻量级的无向图。
具体来说，全局规划器进行两步来增加和扩展其从每个采样的局部图中得到的图。在第一步，全局图通过引入新的顶点和沿着这些路径的边来添加最佳路径\(σ_{L,best}\)。它还通过从每个新添加的顶点添加额外的无碰撞边到其现有的邻居来使图变得密集。这旨在确保全局规划器总是可以找到一个可行的和短的路径，而不是完全依赖于一个简单的回溯方法。随后，全局规划器试图从包含具有高体积增益的顶点的局部图的最短路径列表中添加额外的路径。通过利用那些未访问但高增益的顶点，全局图可以维护一个短但选择性的潜在顶点列表，这些顶点隐式地编码了“边界”的位置和朝向环境未探索分支的方向。
为了进一步减少来自局部图的潜在路径的数量，使用DTW相似度度量来聚类所有的高增益路径，并且只有每个聚类中的“主要”路径（最长的路径）被添加到全局图中。这些主要路径的叶顶点被标记为潜在的“边界”。周期性地，重新评估每个边界顶点的体积增益，以维护一个更短的首选边界列表。总的来说，这个过程实现了关键的全局边界重新定位功能，即使在大规模和多分支的拓扑结构内也能实现有效的探索。当局部规划器无法派生任何有信息的路径时，会偶尔触发它来重新定位机器人。为了执行这个任务，全局规划器运行Dijkstra的算法两次，（a）找到从当前位置到所有潜在边界的最短路径，和（b）找到从所有边界到家庭位置的最短路径。同样，全局规划器根据在全局图上运行Dijkstra的算法，持续地确定一个返回家庭的路径。当返回家庭的时间接近剩余的耐力时，它会指定这条路径。随后，我们将提供关于确定未来探索的最佳路径的过程的更详细的分析。
设\(ν_{\text{G},\text{cur}}\)为全局图中表示当前机器人配置的顶点，\(ν = {ν_i}\)为全局图中更新的潜在前沿集合。在这种情况下，选择一条路径以最大化预期的探索增益并将机器人重新定位到未探索空间的前沿的问题尤其复杂。这主要是因为随着机器人在探索过程中前进，这一决策发生在一个越来越大的搜索空间中。此外，潜在的未探索分支是基于从传感器有限视野收集的不完整体积信息来识别的。例如，当机器人在地下矿井中并经过多个交叉口时，这些交叉口的多个未访问分支可能成为“前沿”，预测哪个候选者将被证明是最好的非常困难。因此，本研究中提出的全局规划器是基于两个基本原则开发的。首先，它在考虑任何可行路径时考虑时间预算，这反过来允许机器人能够初步访问一个前沿，然后至少有足够的耐力执行归航程序（最坏情况）。其次，规划器应优先考虑需要较短时间到达的高增益区域。全局规划器的探索增益表示为
\[ \textbf{GlobalExplorationGain}_{G}\left(\nu_{G,i}^{\mathcal{F}}\right)=\mathcal{T}\left(\nu_{G,\mathrm{cur}},\nu_{G,i}^{\mathcal{F}}\right)\text{VolumetricGain}\left(\nu_{G,i}^{\mathcal{F}}\right)e^{-\varepsilon_{\mathcal{D}}\mathcal{D}\left(\nu_{G,\mathrm{cur}},\nu_{G,i}^{\mathcal{F}}\right)},\tag{2} \]
其中，\(\mathcal{T}\left(\nu_{G,\mathrm{cur}},\nu_{G,i}^{\mathcal{F}}\right)\) 是如果规划器选择前沿 \(\nu_{G,i}^{\mathcal{F}}\) 时估计的剩余探索时间。该参数通过机器人的剩余耐力时间（RET）近似计算，然后减去从当前顶点到指定顶点以及从那里到归航位置的预计到达时间（ETA）（方程3）。上述内容的形式如下：
\[ \mathcal{T}(\nu_{\mathrm{G,cur}},\nu_{\mathrm{G,i}}^\mathcal{F})=\mathbf{RET}-\mathbf{ETA}(\nu_{\mathrm{G,cur}},\nu_{\mathrm{G,i}}^F)-\mathbf{ETA}(\nu_{\mathrm{G,i}}^\mathcal{F},\nu_{\mathrm{G,home}}),\tag{3} \]
其中，\(\mathcal{D}\left(\nu_{G,\mathrm{cur}},\nu_{G,i}^{\mathcal{F}}\right)\) 是从当前位置到前沿的最短路径长度，而可调参数 \(\varepsilon_{\mathcal{D}}\) 用于惩罚较长的路径。从概念上讲，这可以被认为是机器人在选择该前沿进行探索时，在时间 \(\mathcal{T}\left(\nu_{G,\mathrm{cur}},\nu_{G,i}^{\mathcal{F}}\right)\) 内可能获得的“暂定体积增益”。该算法在图6中进一步可视化，其伪代码在算法3和算法4中提供。
4.3 可通行性感知规划
所提出的探索管道具有多功能性，可用于空中和足式平台。然而，对于足式机器人，采样空间相对于机器人的当前高度限制在xy平面上，以反映其在局部2.5D上行走的事实，并且一个适当的运动控制器负责整个身体系统的控制。此外，由于来自映射框架的不可避免的不确定性，主要使用占用图（Hornung等，2013；Oleynikova等，2017）进行机器人的碰撞检测，而使用高程图（Fankhauser等，2014，2018）创建靠近机器人的地形的2.5D表示，这样可以在更精细的分辨率上对其腿部进行可通行性分析。
首先，来自前置深度摄像头的深度信息被转换为高程图，这是一个基于网格的概率地形估计图，包括上下置信区间。获得的地图被限制在机器人周围，并反映了通过机器人运动聚合的姿态不确定性（以机器人为中心的映射）。然后使用该地图进行可通行性分析。
为了执行此任务，采用了Wermelinger等人（2016）的工作。该方法利用滤波器提取地形特征，如坡度、粗糙度和台阶。然后通过加权求和将这些特征结合起来，得出可通行性度量，同时对每个特征应用总阈值和个体阈值，从而将路径分类为足式机器人及其足迹的可通行或不可通行路径。自然，这需要在所提出的规划器的算法设计中进行增强，而不仅仅是执行碰撞检测。原则上，可通行性分析可以确定机器人可以接受哪些路径，但得出的可通行性地图只能覆盖与占用图相比更近的区域。
这是因为可通行性地图需要密集测量（在所展示的实验中使用了2厘米的网格大小）以确保对机器人的所有四条腿进行可靠检查。然而，仅在机器人周围的短距离内进行规划必然会导致相对低效的探索行为。为了克服这个问题但仍然遵守可通行性约束，采用了一种工程技术，根据该技术，高程图之外的区域被假定为可通行，从而允许规划更长的高增益路径。基于机器人的速度方向和大小，递归调用额外的可通行性测试以在线方式验证轨迹上的每个航点。如果机器人接近不可通行区域，规划器会重新触发以提供替代路径。为了表示和实现效率，所提出的规划器接口可通行性分析，并将不可通行区域表示为“禁行”区。
4.4 路径优化
给定来自规划器的最佳探索路径 \(\sigma_{best}\)，无论是局部的还是全局的，一个细化步骤被考虑用来调整路径以进一步提高机器人的安全性。主要目的是修改路径上的所有顶点，使其远离障碍物。在这种假设下，这种修改是允许的，因为探索增益的相关变化通常可以忽略不计。这一假设已经在仿真和实践中得到了验证，尤其是在地下环境的隧道状拓扑结构中，这一假设非常成立。观察到的增益差异不大于由于体积增益计算过程中的噪声引入的不可避免的误差。细化过程如下所述，并在图7中进行了可视化展示。
首先，路径会被迭代修剪以移除由于随机采样过程而产生的短边（例如，<0.5米）。然后，每个顶点会使用一种受之前工作（Deits & Tedrake, 2015; Liu et al., 2017）启发的方法在其可行多边形内进行调整。更具体地，对于每条边 \([\nu_i, \nu_{i+1}] \in \sigma_{best}\)，通过逐渐膨胀一个沿该边中心的椭球来估计一组超平面；然后将顶点 \(\nu_{i+1}\) 移动到由计算出的超平面和垂直于该顶点处边的平面形成的多边形的中心。只有在调整后的路径仍然无碰撞的情况下，才允许进行这种修改。这个步骤会对路径的所有顶点重复执行。结果是，修改后的路径与给定路径相似但更安全，因为其所有顶点都被推离了周围的障碍物。