[2020]FUEL: Fast UAV Exploration using Incremental ...

maofaw

Abstract
自主探索是无人机各种应用的基本问题。然而，由于缺乏有效的全球覆盖、保守的运动计划和较低的决策频率，现有方法被证明勘探速度不足。在本文中，我们提出了FUEL，这是一种可以在复杂未知环境中支持快速无人机展开的分层框架。我们通过前沿信息结构（FIS）在勘探规划所需的整个空间中维护关键信息，当空间被勘探时，这些信息可以逐步更新。在FIS的支持下，层次规划器分三步规划勘探运动，找到有效的全局覆盖路径，细化局部视点集，并按顺序生成最短时间轨迹。我们提供了广泛的基准测试和真实世界测试，与最先进的方法相比，我们的方法以前所未有的效率（3-8倍的速度）完成了勘探任务。我们的方法将开源。
code：https://github.com/HKUST-Aerial-Robotics/FUEL
I. INTRODUCTION
无人驾驶飞行器，特别是四旋翼飞行器在各种应用中获得了广泛的普及，例如检查、精确农业和搜索救援。在这些任务中，自主探索是一个基本的组成部分，即车辆探索未知环境并绘制其地图以收集信息。
近年来，人们提出了各种勘探规划方法，并进行了一些实际实验[1]–[4]。然而，它们中的大多数显示出低/中等勘探率，这对于许多大规模的实际应用来说是不令人满意的。首先，许多现有的规划者计划以贪婪的方式探索运动，例如最大限度地获得即时信息，或导航到最近的未知区域。贪婪策略忽略了全局最优性，因此导致整体效率低下。此外，大多数方法都会产生相当保守的运动，以确保在先前未知的环境中同时获得信息和安全。低速勘探，然而，不允许四驱车充分利用其动态能力来完成任务。最后但并非最不重要的一点是，许多方法都有很高的计算开销，无法快速、频繁地响应环境变化。然而，为了实现更快的探索，只要有新的环境信息可用，就需要立即重新规划新的运动。
基于上述事实，本文提出了FUEL，一种支持复杂环境中快速无人机扩展的分层框架。我们引入了前沿信息结构（FIS），它包含勘探规划所需的整个空间中的基本信息。当收集到新的信息时，结构可以高效、递增地更新，因此它能够支持高频规划。基于FIS，分层规划器以三个粗略到精细的步骤生成探索运动。它首先寻找在积累的环境信息背景下最佳的全球探索之旅。然后，对旅游的当地部分进行改进，进一步提高勘探速度。
最后，安全、动态生成了完全可行的最短时间轨迹。规划人员不仅制定了有效的全球覆盖路径，还制定了安全灵活的局部机动。此外，无论何时探索未访问的区域，都会触发规划器，以便四驱车始终对任何环境变化作出迅速响应，从而实现持续快速的探索。
我们将我们的方法与经典和最先进的模拟方法进行了比较。结果表明，在所有情况下，我们的方法都能在更短的时间内完成探索（速度快3-8倍）。此外，我们在各种具有挑战性的环境中进行全方位的船上现实世界探索。仿真和实际测试都表明，与最先进的方法相比，我们的方法具有前所未有的性能。为了造福社区，我们将公开源代码。本文的贡献总结如下：
1） 增量更新的FIS，可捕获整个探索空间的基本信息，并促进高频率的探索规划。
2） 一种分层规划方法，可生成高效的全球覆盖路径，并为高速勘探提供安全灵活的局部机动。
3） 广泛的仿真和实况测试验证了所提方法。我们系统的源代码将公开。
II. RELATED WORK
A. Exploration Path Planning
使用移动机器人绘制未知环境地图的机器人探索已经研究了多年。正如本文所做的，一些作品侧重于快速探索空间[1,5]。同时，其他方法更注重精确重建[2,6]。在提出的各种方法中，基于前沿的方法是一种经典方法。这些方法首先在[7]中介绍，然后在[8]中进行了更全面的评估。为了检测三维空间中的边界，在[9]中提出了一种基于随机微分方程的方法。在原始方法[7]中，选择最近的边界作为下一个目标。[1] 提出了不同的方案。在每一个决策中，它都会选择FOV内的边界，以最小化速度变化，从而保持一贯的高飞行速度。该方案的性能优于经典方法[7]。在[10]中，引入了基于边界的可微信息增益度量，允许使用梯度信息优化路径。
基于采样的探索，作为另一种主要方法，随机生成视点来探索空间。这些方法与次优视图（NBV）[11]的概念密切相关，后者反复计算覆盖视图以获得场景的完整模型。[12] NBV首次用于3D勘探，在3D勘探中，它使用可访问的空间扩展RRT，并以后退的地平线方式执行信息增益最高的边缘。该方法得到了扩展，以考虑定位的不确定性[13]、不同物体的视觉重要性[14]和随后的检查任务[15]。为了避免直接丢弃扩展的树，[16，17]中构建了路线图，以重用先前的知识。[2] 使用重新布线持续维护和优化单个树设计灵感来自RRT*。为了实现更快的飞行，[5]直接对安全且动态可行的运动基元进行采样，并执行信息量最大的基元。
还有一些方法结合了前沿方法和抽样方法的优点。[4，18]规划通向边界的全局路径，并在本地采样路径。[18] 提出了一种基于梯度的局部路径优化方法。[3] 对边界周围的视点进行采样，并找到经过这些视点的全球最短路线。[6] 使用基于采样的算法生成完全覆盖边界的检查路径。
现有的大多数方法都是贪婪地做出决策，并且没有考虑到四驱车的动力学，这导致了低效的全球巡游和保守的机动。相比之下，我们计划的旅行能够有效覆盖整个环境，并生成动态可行的最短时间轨迹，从而实现敏捷飞行。
B. Quadrotor Trajectory Planning
四驱机器人的轨迹规划已经得到了广泛的研究，主要分为硬约束和软约束两类。前者是由最小捕捉轨迹[19]开创的，其闭合形式的解决方案随后被提出[20]。基于[19]，[21]-[23]提取凸安全区域以生成安全轨迹。为了获得更合理的时间分配，提出了快速行进[22]、运动动力学搜索[23]和基于混合整数的方法[24]。[22]还介绍了一种基于B´ezier曲线的高效方法，以确保可行性。
软约束方法通常会制定一个折衷多个目标的非线性优化。最近[25]-[30]将其应用于当地重新规划，展示了其诱人的表现。[31]恢复了这些方法，随后扩展到连续时间轨迹[25]。为了缓解局部极小问题，[26]使用无冲突路径初始化优化。[27]引入了用于重新规划的均匀B样条曲线。最近，[28]进一步利用了B样条曲线，并在现场测试中证明了快速飞行。[29,30]中的拓扑引导路径和感知意识进一步改进了[28]。
在本文中，我们以[28]为基础进行轨迹规划，但将其扩展到优化B样条曲线的所有参数。这样，总轨迹时间可以最小化，以便以更高的导航速度探索未知空间。
III. SYSTEM OVERVIEW


提出的框架基于体素栅格图。如图2所示，它由FIS的增量更新（第四节）和分层勘探规划方法（第五节）组成。每当使用传感器测量更新地图时，都会检查是否有任何前沿集群受到影响。如果是，则移除受影响簇的FIS，同时提取新边界及其FIS（第四节）。然后，触发勘探规划，发现全局勘探路线，细化局部视点，并依次生成到选定视点的轨迹（第五节）。如果不存在边界，则认为勘探已完成。
IV. INCREMENTAL FRONTIER INFORMATION STRUCTURE
如经典前沿勘探[7]所示，frontiers被定义为紧邻未知体素的已知自由体素，它们被分组为集群以指导导航。传统上，提取的信息过于粗糙，无法进行细粒度决策。此外，通过处理整个地图来检索边界，这对于大型场景和高规划频率来说是不可缩放的。在这项工作中，我们从边界中提取更丰富的信息，以实现更精细的规划，并开发一种增量方法来检测本地更新地图中的边界。
A. Frontier Information Structure
当创建新的前沿集群 F_i 时，计算前沿信息结构 FI_i 。它存储属于集群的所有cell C_i 和 C_i 的平均位置 p_{avg,i} 。还计算了集群的轴对齐边界框（AABB） B_i ，为了加快发现边界变化（第IV-B节）。为了服务勘探规划（第五节），围绕集群生成候选视点 VP_i 。此外，双链表 L_{cost,i} 包含 F_i 和所有其他集群之间的连接成本。表I列出了FIS存储的数据。


B. Incremental Frontier Detection and Clustering
如图3所示，每次通过传感器测量更新地图时，也会记录更新区域 B_m 的AABB，其中删除过时的前沿簇并搜索新簇。它首先遍历所有簇，只返回AABB（ B_i ）与 B_m 相交的簇。然后，对返回的簇进行精确检查，其中包含不再是边界的单元的簇被删除。这两个过程受到宽/窄相位碰撞检测算法[32]的启发，该算法可以快速消除大多数未受影响的簇，并显著减少昂贵的精确检查次数。


去除后，通过区域增长算法搜索新的边界并聚类成组，类似于经典的基于边界的方法。在这些组中，忽略了通常由噪声传感器观测产生的具有少量单元的组。然而，其余的群体可能包含大型集群，不利于区分独特的未知区域和作出精细的决策。因此，如果最大特征值超过阈值，我们对每个簇执行主成分分析（PCA），并沿第一个主轴将其分成两个统一的簇。拆分是递归进行的，以便将所有大簇划分为小簇。
C. Viewpoint Generation and Cost Update
直觉上，一个前沿集群意味着一个探索太空的潜在目的地。然而，与以前只导航到集群中心的方法不同，我们需要更精细的决策。为此，当创建集群Fi时，我们生成一组丰富的视点 VP_i=\left\{ \mathbb x_{i,1}, \mathbb x_{i,2},\cdot\cdot\cdot,\mathbb x_{i,n_i} \right\} covering it，其中 \mathbb x_{i,j}=\left( \mathbb p_{i,j},\xi_{i,j} \right) ， VP_i 是通过圆柱坐标系中的均匀采样点找到的，其原点位于集群的中心，如图4所示。对于位于自由空间内的每个采样点 p ，通过使用类似于[16]的偏航优化方法，将偏航角确定为最大化传感器对集群覆盖的角度。覆盖率被评估为符合传感器模型且不被占用体素遮挡的前沿单元的数量。然后，覆盖率高于阈值的视点被保留，并按覆盖率的降序排序。我们在 VP_i\left( n_i\leq N_{view} \right) 中最多保留Nview视点，以使局部视点细化（第五节）易于处理。



图4.为前沿集群生成候选视点。在以簇平均位置为中心的圆柱坐标系内，均匀采样点

要执行勘探旅行的全球规划（第五节），需要每对集群 \left( F_{k_1},F_{k_2} \right) 之间的连接成本。让 t_{1b}\left( \mathbb x_{k_1,j_1},\mathbb x_{k_2,j_2}\right) 表示在两个视点 \mathbb x_{k_1,j_1} 和 \mathbb x_{k2,j2} 之间移动时的时间下限，其计算公式为：
\begin{gathered} t_{\mathrm{lb}}\left(\mathbf{x}_{k_1, j_1}, \mathbf{x}_{k_2, j_1}\right)=\max \left\{\frac{\operatorname{length}\left(P\left(\mathbf{p}_{k_1, j_1}, \mathbf{p}_{k_2, j_2}\right)\right)}{v_{\max }}\right. \\ \left.\frac{\min \left(\left|\xi_{k_1, j_1}-\xi_{k_2, j_2}\right|, 2 \pi-\left|\xi_{k_1, j_1}-\xi_{k_2, j_2}\right|\right)}{\dot{\xi}_{\max }}\right\} \end{gathered}
其中 P\left( \mathbb p_{k1,j1},\mathbb p_{k2,j2}\right) 表示  \mathbb p_{k_1,j_1} 和  \mathbb p_{k_2,j_2} 之间的无碰撞路径；通过路径搜索算法找到 v_{max} 和 \xi_{max} 是偏航速度和角速度的极限。对于每对 \left( F_{k_1},F_{k_2} \right) ，我们选择覆盖率最高的视点，并将成本估计为 t_{1b}\left( \mathbb x_{k_1,1},\mathbb x_{k_2,1}\right) ，其中使用A*算法在体素栅格地图上搜索 P\left( \mathbb p_{k_1,1},\mathbb p_{k_2,1}\right) 。
请注意，从头开始计算所有 N_{cls} 集群对之间的连接成本需要 O\left(  N_{cls}^2 \right) A*搜索，这相当昂贵。幸运的是，成本也可以以增量方式计算。当删除任何过时集群时（第IV-B节）， L_{cost,i} 中的相关成本项目；所有剩余FIS中的 L_{cost,i} 被删除。然后，计算每个新集群到所有其他集群的连接成本，并将其插入 L_{cost,i} 、 假设k_{new}每个边界检测中都有已知的新簇，则上述更新方案需要 O\left( k_{new}\cdot N_{cls} \right) 时间。实际上，已知值 k_{new} 很小，可以视为一个常数，导致连接成本的线性时间更新。
V. HIERARCHICAL EXPLORATION PLANNING
我们没有采用贪婪的探索策略或产生保守的机动，而是制定了有效覆盖边界的全球路径，并为更快的飞行制定了安全灵活的行动计划。我们的规划师从最近的分层四轮规划范式中获得灵感[21、22、28]，并分三个阶段做出决策，如图5所示。



图5：以三个粗略到精细的步骤生成勘探运动：（1）发现了整个环境中覆盖边境集群的最短路线。（2） 全球巡演的一个本地部分得到了改进。（3） 生成一条安全的最短时间轨迹，到达精巡视的第一个视点。

A. Global Exploration Tour Planning
我们的勘探计划从寻找有效覆盖现有前沿集群的全球旅游开始。受[3]的启发，我们将其表述为旅行推销员问题（TSP）的一个变体，该问题从当前视点和所有簇上的传递视点。我们通过适当设计参与成本矩阵 M_{tsp} ，将该变量简化为标准非对称TSP（ATSP），可通过可用算法快速求解。
假设总共有 N_{cls} 簇，M_{tsp}对应于N_{cls}+1维方阵。主要部分是由每对前沿集群之间的连接成本组成的N_{cls}\times N_{cls} block，计算如下：
\begin{aligned} &\mathbf{M}_{\mathrm{tsp}}\left(k_1, k_2\right)=\mathbf{M}_{\mathrm{tsp}}\left(k_2, k_1\right) \\ &=t_{\mathrm{lb}}\left(\mathbf{x}_{k_1, 1}, \mathbf{x}_{k_2, 1}\right), k_1, k_2 \in\left\{1,2, \cdots, N_{\mathrm{cls}}\right\} \end{aligned}
如第IV-C节所述，当检测到边界时，此信息将保持不变。因此，可以填充 N_{cls}\times N_{cls} 块，而无需额外的开销。
M_{tsp} 的第一行和第一列与当前视点 x_0=\left( p_0,\xi_0 \right) 和 N_{cls} 簇相关联。从 x_0 开始，第 k 个集群的成本评估方法为：
\begin{array}{r} \mathbf{M}_{\mathrm{tsp}}(0, k)=t_{\mathrm{lb}}\left(\mathbf{x}_0, \mathbf{x}_{k, 1}\right)+w_{\mathrm{c}} \cdot c_{\mathrm{c}}\left(\mathbf{x}_{k, 1}\right) \\ k \in\left\{1,2, \cdots, N_{\mathrm{cls}}\right\} \end{array}
这里引入了运动一致性代价 c_c\left( \mathbb x_{k,1} \right) ，其通常计算为：
c_{\mathrm{c}}\left(\mathbf{x}_{k, j}\right)=\cos ^{-1} \frac{\left(\mathbf{p}_{k, j}-\mathbf{p}_0\right) \cdot \mathbf{v}_0}{\left\|\mathbf{p}_{k, j}-\mathbf{p}_0\right\|\left\|\mathbf{v}_0\right\|}
其中 \mathbb v_0 是当前速度。在某些情况下，多次tours具有可比的时间下限，因此可能会在连续的规划步骤中产生来回机动，从而减缓进度。我们消除了与 c_c\left( \mathbb x_{k,1} \right) 的这种不一致，后者会对飞行方向的大变化造成不利影响。
我们的问题不同于标准TSP，后者的解决方案是一个闭环回路。然而，我们可以通过将其他集群的零连接成本分配给当前视点，将其减少为ATSP：
\mathbf{M}_{\text {tsp }}(k, 0)=0, k \in\left\{0,1,2, \cdots, N_{\text {cls }}\right\}
这样，在任何闭环tours中回到当前的视角都不会产生额外的成本，因此每个闭环tours总是包含一个具有相同成本的开环tours。因此，我们可以通过寻找最优闭环回路并检索其等成本的开环回路来获得最优开环回路。
B. Local Viewpoint Refinement
global tour规划找到了访问所有集群的良好顺序。尽管如此，它只涉及每个集群的单个视点，不一定是所有视点中的最佳组合。
为此，通过使用图搜索方法（如图6所示），考虑在全球巡视的截断段上设置更丰富的视点集，以进一步提高勘探速度。我们发现了连续的集群，其全球巡视上的视点比 R_{rf} 更接近当前位置。为了简化符号，假设 F_i,1\leq i \leq N_{rf} 是考虑的簇。我们为其视点 VP_i 和当前视点 \mathbb x_0 创建图形节点。然后，每个节点都连接到与下一个具有有向边的簇相关联的其他节点，这些有向非循环图将捕获截断巡更的可能变化。我们使用Dijkstra算法来搜索最优的局部tour \Xi=\left\{\mathbf{x}_{1, j_1}, \mathbf{x}_{2, j_2}, \cdots, \mathbf{x}_{N_{\mathrm{rf}}, j_{N_{\mathrm{rf}}}}\right\} 使成本最小化：
\begin{aligned} c_{\mathrm{rf}}(\Xi) &=t_{\mathrm{lb}}\left(\mathbf{x}_0, \mathbf{x}_{1, j_1}\right)+w_{\mathrm{c}} \cdot c_{\mathrm{c}}\left(\mathbf{x}_{1, j_1}\right) \\ &+t_{\mathrm{lb}}\left(\mathbf{x}_{N_{\mathrm{rf}}, j_{N_{\mathrm{rf}}}}, \mathbf{x}_{N_{\mathrm{rf}}+1,1}\right)+\sum_{k=1}^{N_{\mathrm{rf}}-1} t_{\mathrm{lb}}\left(\mathbf{x}_{k, j_k}, \mathbf{x}_{k+1, j_{k+1}}\right) \end{aligned}



图6.使用图形搜索方法局部优化视点。沿着全球巡视的一个截断段，考虑每个访问集群的多个视点，以选择最佳视点集

它还包括时间下限和运动一致性。请注意，将信息增益[4，12]合并到Equ是很简单的。然而，评估多个视点的信息增益是昂贵的。实际上，我们发现，简单地根据覆盖范围采用观点要快得多，并会导致一致的满意结果。
C. Minimum-time B-spline Trajectory
考虑到离散的视点，平滑导航需要连续的轨迹。我们的四驱轨迹规划基于一种方法[28]，该方法可以生成平滑、安全和动态可行的B样条轨迹。我们进一步优化B样条曲线的所有参数，使总轨迹时间最小化，从而使四驱车能够充分利用其动态性能。
由于quadrotor动力学是差分平坦的[19]，我们计划平坦输出的轨迹 x\in\left( x,y,z,\xi \right) ，让 X_{cb}=\left\{ \mathbb x_{c,0}, \mathbb x_{c,1}, \cdot\cdot\cdot, \mathbb x_{c,N_b} \right\} ，其中 \mathbb x_{c,i} = \left( \mathbb p_{c,i},\xi_{c,i} \right) 是 p_b degree uniform B样条的 N_b+1 控制点， \Delta t_b 是knot span。
我们找到了平衡平滑度和总轨迹时间，并满足安全性、动态可行性和边界状态约束的B样条曲线。它可以表述为以下优化问题：
\underset{\mathbf{X}_{\mathrm{c}, b}, \Delta t_b}{\arg \min } f_{\mathrm{s}}+w_{\mathrm{t}} T+\lambda_{\mathrm{c}} f_c+\lambda_{\mathrm{d}}\left(f_{\mathrm{v}}+f_{\mathrm{a}}\right)+\lambda_{\mathrm{bs}} f_{\mathrm{bs}}
与[28]类似， f_s 是弹性带平滑度成本：
f_{\mathrm{s}}=\sum_{i=0}^{N_b-2} \mathbf{s}_i^{\mathrm{T}} \mathbf{R}_{\mathrm{s}} \mathbf{s}_i, \quad \mathbf{s}_i=\mathbf{x}_{\mathrm{c}, i+2}-2 \mathbf{x}_{\mathrm{c}, i+1}+\mathbf{x}_{\mathrm{c}, i}
其中 R_s 是惩罚矩阵：
\mathbf{R}_{\mathrm{s}}=\left[\begin{array}{cc} w_{\mathrm{s}, \mathrm{p}} \mathbf{I}_3 & 0 \\ 0^{\mathrm{T}} & w_{\mathrm{s}, \xi} \end{array}\right]
T 是总轨迹时间，取决于 \Delta t_b 和B样条线段的数量：
T=\left(N_b+1-p_b\right) \cdot \Delta t_b
f_c 、 f_v 和 f_a 是确保安全性和动态可行性的惩罚措施。给定以下函数：
\mathcal{P}\left(\tau_1, \tau_2\right)=\left\{\begin{array}{cl} \left(\tau_1-\tau_2\right)^2 & \tau_1 \leq \tau_2 \\ 0 & \text { else } \end{array}\right.
f_c 评估为：
f_{\mathrm{c}}=\sum_{i=0}^{N_b} \mathcal{P}\left(d\left(\mathbf{p}_{\mathrm{c}, i}\right), d_{\min }\right)
其中 d\left( \mathbb p_{c,i} \right) 是点 \mathbb p_{c,i} 到最近的障碍物的距离，可以从映射模块维护的欧氏带符号距离场（ESDF）中获得。实际上，大于四驱车半径的间隙（通常为0.5米）可以确保在复杂场景中的安全。 f_v 和 f_a 惩罚不可行的速度和加速度：
\begin{aligned} &f_{\mathrm{v}}=\sum_{i=0}^{N_b-1}\left\{\sum_{\mu \in\{x, y, z\}} \mathcal{P}\left(v_{\mathrm{max}},\left|\dot{p}_{\mathrm{c}, i, \mu}\right|\right)+\mathcal{P}\left(\dot{\xi}_{\mathrm{max}},\left|\dot{\xi}_{\mathrm{c}, i}\right|\right)\right\}_{}\\ &f_{\mathrm{a}}=\sum_{i=0}^{N_b-2}\left\{\sum_{\mu \in\{x, y, z\}} \mathcal{P}\left(a_{\max },\left|\ddot{p}_{\mathrm{c}, i, \mu}\right|\right)+\mathcal{P}\left(\ddot{\xi}_{\max },\left|\ddot{\xi}_{\mathrm{c}, i}\right|\right)\right\} \end{aligned}
其中利用了衍生工具的控制点：
\begin{aligned} &\dot{\mathrm{x}}_{\mathrm{c}, i}=\left[\dot{p}_{\mathrm{c}, i, x}, \dot{p}_{\mathrm{c}, i, y}, \dot{p}_{\mathrm{c}, i, z}, \dot{\xi}_{\mathrm{c}, i}\right]^{\mathrm{T}}=\frac{\mathbf{x}_{\mathrm{c}, i+1}-\mathbf{x}_{\mathrm{c}, i}}{\Delta t_b} \\ &\ddot{\mathrm{x}}_{\mathrm{c}, i}=\left[\ddot{p}_{\mathrm{c}, i, x}, \ddot{p}_{\mathrm{c}, i, y}, \ddot{p}_{\mathrm{c}, i, z}, \ddot{\xi}_{\mathrm{c}, i}\right]^{\mathrm{T}}=\frac{\mathbf{x}_{\mathrm{c}, i+2}-2 \mathbf{x}_{\mathrm{c}, i+1}+\mathbf{x}_{\mathrm{c}, i}}{\Delta t_b^2} \end{aligned}
在Equ。12、13和14，利用B样条曲线的凸壳特性，有效地保证了算法的可行性。为了简洁起见，我们请读者参考[28]了解更多细节。
最后，我们将开始时的0到2阶导数设置为瞬时状态 \left(\mathrm{x}_0, \dot{\mathrm{x}}_0, \ddot{\mathrm{x}}_0\right) ，以实现平滑运动。末尾的0阶导数也由下一个要访问的视点 \mathbb x_{next} 。在实施中，我们使用三次B样条曲线，因此相关成本为：
\begin{aligned} &f_{\mathrm{bs}}=\left\|\frac{\mathbf{x}_{\mathrm{c}, 0}+4 \mathbf{x}_{\mathrm{c}, 1}+\mathrm{x}_{\mathrm{c}, 2}}{6}-\mathrm{x}_0\right\|^2+\left\|\frac{\dot{\mathrm{x}}_{\mathrm{c}, 0}+\dot{\mathrm{x}}_{\mathrm{c}, 1}}{2}-\dot{\mathrm{x}}_0\right\|_{}^2 \\ &+\left\|\ddot{\mathrm{x}}_{\mathrm{c}, 0}-\ddot{\mathrm{x}}_0\right\|^2+\left\|\frac{\mathrm{x}_{\mathrm{c}, N_b-2}+4 \mathrm{x}_{\mathrm{c}, N_b-1}+\mathrm{x}_{\mathrm{c}, N_b}}{6}-\mathbf{x}_{\mathrm{next}}\right\|^2 \end{aligned}
VI. RESULTS
A. Implementation Details
我们在公式3和6在global tour规划中设置 w_c=1.5 ，使用Lin Kernighan Helsgaun启发式求解器求解ATSP[33]。在局部视点优化中，我们在每个FIS中最多保留 N_{view}=15 个视点，并在半径 R_{rf}=5.0 内截断全局巡视。对于轨迹优化，我们使用 w_{\mathrm{s}, \mathrm{p}}=5.0, w_{\mathrm{s} . \xi}=2.5, w_{\mathrm{t}}=1.0, \lambda_{\mathrm{c}}=\lambda_{\mathrm{bs}}=10.0, \lambda_{\mathrm{d}}=2.0,d_{min}=0.4 ，并使用通用非线性优化求解器 NLopt^2 求解该问题。使用三次B样条（ p_b=3 ）作为轨迹表示。
为了实现快速探索，高效的绘图框架至关重要。在我们的工作中，我们使用了一个体积图[34]，它已成功应用于复杂场景中的快速自主飞行[28，30]。与广泛应用于勘探的[35]类似，[34]构建了空间的占用网格表示。同时，它还不断维护ESDF，以促进轨迹规划。为了简洁起见，我们请感兴趣的读者参阅[34]，以了解有关我们的映射框架的更多详细信息。
在所有现场实验中，我们通过视觉惯性状态估计器定位四驱[36]。我们使用几何控制器[37]跟踪 \left( x,y,z,\xi \right) 轨迹。我们为定制的四驱平台配备了Intel RealSense深度相机D435i。以上所有模块均在Intel Core i7-8550U CPU上运行。
B. Benchmark and Analysis
我们在仿真中测试了我们提出的框架。我们在桥梁场景和大型迷宫场景中对其进行基准测试。比较了三种方法：NBVP[12]、经典前沿法[7]和快速前沿法[1]。注意，[1]没有可用的开源代码，所以我们使用我们的实现。在所有测试中，所有方法的动态极限均设置为 v_{max}=2.0m/s 和 \xi_{max}=0.9rad/s 。传感器的FOV设置为 \left[ 80\times60 \right] 度，最大范围为 4.5m 。在两种情况下，每个方法都以相同的初始配置运行3次。四种方法的统计和勘探进展分别见表二和表三列出了我们方法的每个组成部分的计算时间。




1） Bridge Scenario：首先，我们在包含桥梁的 10\times 20 \times 5 m^3 空间中比较了四种方法，如图7所示。结果表明，我们实现了更短的勘探时间和更小的时间方差。我们方法的总体探索路径明显更短，主要是因为我们计划在全球范围内进行旅游。执行的路径更平滑，因为我们局部细化运动并生成平滑的轨迹。此外，由于最短时间轨迹规划，我们能够以更高的飞行速度导航。



图7：在包含桥梁的三维空间中，建议方法、经典前沿方法[7]、快速前沿方法[1]和NBVP[12]的基准比较。整体勘探路径分别显示为蓝色、红色、绿色和粉红色。

2) Large Maze Scenario: 我们还对图9所示的大型迷宫环境中的方法进行了定量比较。探索空间为 20\times 80 \times 3 m^3 。在这种情况下，由于场景的复杂性，所有基准方法都需要很长时间才能达到完全覆盖。相比之下，我们的方法完成勘探的速度平均快4倍以上。完成后四种方法执行的路径如图9所示。值得注意的是，我们的方法以更合理的顺序探索迷宫，而无需经常重访同一地点。因此，它产生了更短的覆盖路径和近似线性的勘探速度（图8）。这种行为是由于全球计划，如果没有全球计划，可能会多次重访已知区域，并像基准方法那样减缓进度。还要注意，大型迷宫中的计算时间更长，主要是因为场景更大，自然会涉及更多的边界簇。





图8.四种方法在桥梁（顶部）和大迷宫（底部）场景中的探索进展





图9.由建议方法（蓝色）、经典前沿方法[7]（红色）、快速前沿方法[1]（绿色）和NBVP[12]（粉红色）生成的路径。

C. Field Exploration Tests
为了进一步验证所提出的方法，我们在室内和室外环境中进行了广泛的现场实验.在所有测试中，我们将动力学极限设置为 v_{max}=1.5m/s ， a_{max}=0.8m/s 和 \xi_{max}=0.9rad/s 。请注意，我们不使用任何外部设备进行定位，只依赖车载状态估计器。
首先，我们在两个室内场景中进行快速探索测试。第一个场景如图1所示，我们在其中部署了几十个障碍物，四驱车应执行3D机动，以绘制未知空间并同时避开障碍物。我们用一个 10\times6\times2m^3 立方米的盒子将要探索的空间围起来。图1显示了一个示例地图和飞行轨迹。第二个室内场景是一个较大的环境，包括两个房间，其中一个房间与场景1相似，另一个房间是包含桌椅的办公室的一部分。该空间由一个 15\times11\times2m^3 的盒子限定。四驱车从探索大房间开始，然后进入小房间。第二个场景，在线生成的地图和轨迹如图10所示。请注意，在这两个场景中，四驱车开始于能见度较低的地点，因此它开始时只映射了环境的一小部分。最后，为了在自然环境中验证我们的方法，我们在森林中进行了勘探测试。待勘探区域的面积为 11\times10\times2m^3 。实验环境和相关结果如图11所示。







图10：在由两个房间组成的室内场景中进行的实验：一个有桌椅的小房间（左上角），一个满是障碍物的大房间（右上角）。





图11：在森林中进行的探索实验。

上述实验证明了我们的方法在复杂的现实场景中的能力。它们还显示了我们自主四驱系统的优点，其中状态估计[36]和映射模块[34]对于完成现实世界的任务也至关重要。所有实验都有视频演示（图1），我们请读者参考它了解更多细节。
VII. CONCLUSIONS
在本文中，我们提出了一个快速自主四驱探索的分层框架。引入了增量维护的FIS，为勘探规划提供基本信息。基于FIS规划人员分三个步骤规划勘探运动，找到有效的全局巡视，选择一组局部最优视点，并生成最短时间的局部轨迹。该方法以高频率做出决策，以快速响应环境变化。基准测试和真实世界测试都表明了我们方法的能力。
我们的方法的一个局限性是像大多数方法一样，假设状态估计是完美的。我们在地面真值定位的仿真中评估了我们的方法，但未考虑姿态漂移。然而，状态估计误差普遍存在，不容忽视。未来，我们计划在我们的方法中考虑状态估计的不确定性，并评估其在姿态漂移下的性能。