登山第二十梯：无人机实时自主探索——我是一只小小小鸟

1）摘要

        自主探索是无人机 （UAV） 各种应用的基本问题。最近，基于 LiDAR 的探索因其能够生成大规模环境的高精度点云地图而受到广泛关注。虽然点云本身就为导航提供了信息，但许多现有的勘探方法仍然依赖于额外的（通常是昂贵的）环境表示。这种依赖源于两个主要原因：需要边界检测或信息增益计算，这通常取决于内存密集型占用网格地图，以及直接在点云上进行路径规划的高计算复杂性，主要是由于昂贵的碰撞检查。为了解决这些限制，我们提出了 EPIC，这是一个基于 LiDAR 的轻量级无人机探索框架，可直接利用点云数据来探索大规模环境。EPIC 引入了一种直接源自点云质量的新型观测地图，无需全局占用网格图，同时保留了全面的探索功能。我们还提出了一种直接在点云上运行的增量拓扑图构建方法，可以在大规模环境中进行实时路径规划。利用这些组件，我们构建了一个分层规划框架，该框架可生成敏捷且节能的轨迹，与大多数现有方法相比，显著降低了内存消耗和计算时间。广泛的模拟和实际实验表明，与最先进的方法相比，EPIC 实现了更快的探索，同时显著降低了内存消耗。

2）创新点

①一种直接利用点云数据进行大规模探索的新型框架，无需额外的昂贵环境表示，同时确保高效和全局的勘探。

②直接来自点云的观测图，有效指导勘探。

③增量构建的拓扑图，支持直接在点云上进行实时路径规划，有助于在大规模环境中进行高效探索。

④广泛的仿真和实际测试验证了所提出的方法，在内存消耗、探索效率和计算速度方面表现出卓越的性能。

3）算法结构

        这篇文章中解决的问题是使用自主飞行器 （UAV） ∈ R 3 探索大规模、未知、有界的 3D 空间来生成密集的点云地图。

        提出的探索框架由基于观察地图的环境表示和实时分层规划框架组成。在收到新的 LiDAR 点云扫描后，记录表面观测质量的观测地图会逐渐更新。基于观测地图检测前沿 并对其进行聚类。为了实现大范围环境中的高效路径规划，在点云上逐步构建拓扑图 G。视点是根据拓扑可达性战略性选择的。随后，计算从当前位置开始并穿过所有视点的全局引导路径。基于全局路径，生成搞笑、敏捷的局部轨迹，引导无人机按顺序 （V-C） 访问这些视点。如果不存在边界，则认为探索已完成。

A 基于观测地图探索的环境表达

        在本节中，我们提出了一种新的基于观测的无人机探索环境表示方法，该方法包括两个关键组成部分：观测地图Mobs 和前沿集群列表Lclusters。前者记录表面观测的质量，该质量由 LiDAR 的观察距离和视图方向决定。根据质量评估，观察到的表面被标记为观察良好或观察不佳。前沿定义为观察良好的表面和观察不佳的表面之间的边界，被聚类并存储在 前沿集群列表中。当收到新的点云帧时，Mobs 和 Lclusters 都会以增量方式更新。

a1 观测地图构建

        为了方便对地表观测质量的高效查询，避免过多的内存消耗，我们采用了空间哈希映射来存储地表观测的质量。将其定义为观测地图 Mobs，内部的每个元素都是表示小表面斑块的体素，并根据观测质量标记为观察良好或观察不佳。与内存密集型占用网格地图不同，这种选择性存储确保只关注地表周围的区域，从而减少内存消耗，同时保留足够的信息进行边界检测。

        每个表面面片的观测质量由距离约束和视点方向约束决定。考虑一个以 ps 为中心的表面贴片，并且 LiDAR 位于 pl ，如果满足以下条件，则认为满足距离约束：

        如上图（a） 所示，鉴于随着 LiDAR 光束变得更加垂直于表面贴片，观察质量会提高，对于 2D 情况，视向质量与 |90◦ − θ|呈负相关。假设 l2 ≥ l1，θ 可以通过以下方式计算：

其中 δ 表示两个相邻 LiDAR 光束之间的恒定角度间隔，该值由 LiDAR 的机械配置决定。根据方程，|90◦ − θ|和 L1/L2 呈负相关。因此，我们可以使用 l1/l2 来评估视图方向的质量。我们认为当 l1/l2 超过阈值 T 时，视图方向约束得到满足。

        对于 3D 情况，如上图（b） 所示，LiDAR 的感应区域由许多小的金字塔形体积组成。考虑一个以 pi 为中心的表面patch，落在金字塔形的体积 fi 内。如果组成 fi 的四条光线都满足上述条件，则我们认为满足视图方向约束。当收到新的点云帧时，与点云相交的生物体素将被识别并附加到更新队列 Q 中。我们随后迭代队列。对于每个未标记或之前标记为观察不佳的体素，如果它同时满足距离和方向约束，我们会将其分类更新为观察良好。否则，我们会将其标记为观察不佳。

a2 前沿检测和聚类

        为了实现实时前沿边界更新，我们提出了一种增量前沿检测和聚类方法。我们首先迭代所有在 Q 中标记为观察不佳的体素，如果体素有一个标记为观察良好的相邻体素，则将其分类更新为 frontier。请注意，frontier 集合是 poorly集合的子集。

在迭代过程中，同时计算从 frontier 变为 well-observed 的体素以及从 Poorly-observed 过渡到 frontier 的体素的轴对齐边界框 （AABB） Bupdate。除了更新 Mob 之外，当前帧中新生成的 frontier 也会被附加到一个集合的 Fcurrent 中。

        对于点云数据的第一帧，我们基于欧几里得距离和表面法线对 Fcurrent 应用聚类方法。考虑两个边界体素 fi ， fj 分别具有中心位置 pi 、 pj 和法线向量 ni 、 nj 。如果满足以下条件，我们将这两个体素视为相邻体素：

        聚类过程首先从 Fcurrent 中选择第一个元素。从体素开始，我们采用 Breadth First Search （BFS） 算法来扩展集群，识别并合满足指定距离和法线相似性标准的相邻体素。如果 BFS 自然完成或集群的 AABB 大小超过预定义的阈值，则扩展将终止。在此过程中，添加到集群的体素会同时从 Fcurrent 中删除。集群扩展结束后，该过程将使用更新的 Fcurrent 的第一个元素重新初始化。迭代将继续，直到 Fcurrent 为空。之后，每个集群的 AABB、前沿体素及其相应的法线向量都存储在 Lclusters 中以供进一步处理。

        对于点云的第二帧或后续帧，我们首先识别所有与 Bupdate 相交的集群，将它们从 Lclusters 中删除，并将它们的边界体素添加到 Fcurrent 中。然后，我们在 Fcurrent 上执行新的聚类过程，并根据聚类结果更新 Lclusters。

B 分层探索规划

        分层探索规划模块由三个关键组件组成：增量构建的拓扑图 G、全局引导路径规划器和局部轨迹规划器。

        为了在大规模环境中实现高效的路径搜索，我们提出了一种直接在点云上构建的增量拓扑图。利用拓扑图 G，Global Planner 计算引导路径。随后，当地规划者根据这些引导路径生成安全、平稳和可行的轨迹，然后无人机可以有效地覆盖边境。

b1 增量拓扑图构建

        拓扑图广泛用于大规模环境探索，以实现更快的路径搜索。但是，大多数现有方法都基于内存密集型的全局占用网格图构建拓扑图。为了解决这个问题，我们提出了一种直接在点云上运行的增量拓扑图构建方法。在我们的方法中，拓扑图表示为 G = （V， E），其中 V 表示独立自由空间中的代表性顶点，E 表示显示这些自由空间之间连通性的边。E 中的每条边都与无碰撞路径相关联。

        我们将整个探索空间分解为长方体区域 R，这些区域由空间哈希映射管理。拓扑图 G 的顶点 V 存储在其相应的区域内。在收到新的区域中心无碰撞球体区域子区域代表顶点障碍物观测面 （a） （b） （c） （d） 点云帧后，与激光雷达当前观测区域相交的区域被添加到更新列表 Lregion 中。此外，对于在整个探索过程中首次添加到 Lregion 的区域，其相应的顶点集将初始化为 ∅。

（区域中拓扑图顶点更新过程的图示。（a） 和 （b）：使用无碰撞球体的自由空间覆盖。（c）：基于连通性的聚类球体。（d）：选择代表性顶点。）

        1） 顶点更新：对于 Lregion 中的每个区域 ri，我们使用多个无碰撞球体来覆盖其自由空间。如图（a） 和图（b） 所示，初始无碰撞球体以 ri 为中心，其半径由到点云图中最近点的距离决定。如果此球体完全包含该区域，则 ri 的覆盖操作结束。否则，该过程将根据球体的半径继续。如果半径超过预定义的安全距离，则该区域将由对应于球体内切立方体的六个面的平面细分为更小的子区域。否则，球体将被丢弃，然后该区域被三个正交平面通过球体的中心分成八个子区域，平行于 xy、yz 和 zx 平面。

        对于每个子区域，以递归方式重复球体生成和区域细分的过程，直到它完全被无碰撞的球体覆盖或其大小小于安全半径。随后，我们采用联合查找算法，根据它们的连通性对这些球体进行聚类，如图（c） 所示。如果两个球体的交集大小大于安全半径，则这两个球体被视为同一群集的一部分。

        对于每个生成的群集，将选择一个代表性顶点。顶点的位置设置为簇内最靠近区域中心的球体中心，如图（d） 所示。此外，我们在进程开始之前将存储在 Lregion 中的所有顶点记录为 Vpre，并在 Lregion 更新后记录为 Vnew。这两个集合都保留用于后续算法步骤。

        2） Edge Updating：边集 E 初始化为 ∅。随着 G 顶点的更新，连接它们的边将从通过 A* 搜索算法获得的路径派生。如上图所示，基于 Vpre 和 Vnew，我们将顶点分为三个不同的集合：

其中 Vremove 表示 Vpre 中存在但在 Vnew 中不存在的顶点，Vremain 表示在 Vpre 和 Vnew 之间保持不变的顶点，Vinsert 表示出现在 Vnew 中但不在 Vpre 中的新顶点。

        首先，我们遍历 Vremove，从 G 中消除所有相关的边。接下来，对于 Vremain 中的顶点，我们检查与连接到它们的边对应的所有无碰撞路径。对于每个路径，我们验证它在更新的点云地图中是否保持无碰撞。如果路径不再无冲突，我们将其相应的顶点对添加到队列 Qedge 中，并从 G 中删除关联的边。然后，对于每个 v ∈ Vinsert，我们在 v 和相邻区域中的节点之间构造顶点对，并将这些对添加到 Qedge。

        随后，对 Qedge 中的每个顶点对执行 A* 搜索算法。如果找到无碰撞路径，则将相应的边添加到 G 中，并存储无碰撞路径。此外，为了加速搜索过程并避免冗余边缘，我们将 A* 算法的搜索范围限制在一个小区域。此区域定义为轴对齐边界框 （AABB），它紧紧地包围了起点和目标点，并带有等于一个区域大小的附加填充。最后，我们更新 vodom，即代表无人机当前位置的顶点，调整其位置，删除以前的边缘，并建立新的连接。

b2 全局路径规划

        该模块旨在确定 Lclusters 中前沿集群的有效访问序列，然后为本地轨迹规划器生成引导路径。

        鉴于全局最优访问序列通常优先考虑附近的集群，而不是远处的集群，并且考虑到飞行过程中的高频重新规划，在下一个重新规划周期之前只执行一小部分引导路径。所以，远程集群排序的影响会减小。因此，我们最初使用粗略距离估计对 Lclusters 中的所有前沿集群进行排名，从而产生粗略的初步访问序列。然后，我们对该序列的前 K 个元素进行更精细的调整，相应地生成全局引导路径。这种两阶段策略将计算资源集中在近端集群上，有效地平衡了路径最优性和计算效率，使其特别适合大规模场景下的实时探索。

        1） 前沿集群排序：在探索过程中，不断生成前沿，每个前沿都可以在生成时通过无人机位置的直线路径到达，因为它们当时从无人机上可以看到。受这一观察结果的启发，我们引入了一个回溯距离指标来确定 Frontier 集群的优先级。对于以 ci 为中心的集群，让 pi 表示无人机在集群产生时的位置，回溯距离可以定义为：

        回溯距离 di 通过追溯其历史路径来量化无人机到达 ci 的总距离。通过维护无人机在每个集群生成时的飞行距离及其总飞行距离的记录，我们可以计算 O（1） 时间复杂度的 di。这种高效的计算使我们能够在 Lclusters 中快速对集群进行排名，从而产生一个粗略的全局访问序列。

        2） 引导路径规划：对于序列中的前 K 个集群，我们根据后续3）中描述的方法为每个集群生成一个视点，并将这些视点作为顶点连接到图 G。然后，我们对 G 执行 A* 搜索以计算视点对之间的距离。随后，我们将其表述为非对称旅行推销员问题 （ATSP） 并解决它。该解决方案产生从当前车辆位置开始并穿越所有选定视点的最短路径，作为我们的全局引导路径。

        3） 视点生成：对于前沿集群 Cf，候选视点集合是在以集群中心为中心的圆柱坐标系中均匀采样得到，如下图（a） 所示。然后，我们计算从每个候选视点到点云地图中最近点的距离。距离低于安全阈值的视点将被消除。其余候选视点每个对应于一个无碰撞球体。然后，我们根据它们相关球体的连通性将它们聚类到几个视点集群 Ccv 中。这个过程如下图（b） 所示，其中候选视点被分为两个视点集群。

        对于 Ccv 中的每个集群，其中的所有元素都可以相互访问。所以我们选择一个代表元素，并将其作为顶点 vrepresent 添加到 G 中，以检查它是否可以从 vodom 到达。如果是这样，则集群中的所有元素都被视为可访问。在此评估之后，将从 G 中删除 vrepresent 及其关联的边缘，以确保图形在未来的规划周期中保持高效和清晰。然后，我们从 Cf 的可访问候选项中选择覆盖率得分最高的视点。视点的覆盖率分数定义为 Cf 中可以很好地观察到的边界数量。如果到视点的距离小于 D，将边界体素连接到视点 lvf 的线畅通无阻，并且 lvf 与边界的法线向量之间的角度低于阈值，则认为边界体素观察良好。对于每个可到达的视点，候选偏航角在 360 度上均匀采样，对于每个采样的偏航角，评估 LiDAR 视场 （FOV） 内观察到的边界数量。选择最大化观察良好边界数量的偏航角，此计数将记录为相应视点的覆盖分数。然后，选择得分最高的视点并将其连接到拓扑图。

b3 局部轨迹生成

        获得全局引导路径后，我们直接在点云地图上生成平滑的局部轨迹。采用分段多项式 MINCO 分别表示位置轨迹 p（t） = [x（t）， y（t）， z（t）]T 和偏航轨迹 ψ（t）。为了构建安全惩罚，我们使用 FIRI 沿引导路径生成多面体安全飞行走廊。为了主动观察未知空间，偏航被限制在查看未来的航路点。此外，我们对最大速度、倾斜角度、推力和本体速施加约束，以确保动力学的可行性。还纳入了最低时间惩罚以实现敏捷飞行。利用 MINCO 的特性，我们通过联合时空优化生成高效且敏捷的本地轨迹。

4）实验

A 实验设置

a1 对比方法： FUEL, ERRT, and M3 using the MARSIM simulator。

a2 大场景：a cave [224×330×41]m3 , a garage [192×156×4]m3 , and a campus [160×128×40]m3

a3 测试方法：每种方法每个场景执行四次，最大速度为 4.0m/s，地图分辨率为 0.4m（M3 除外），时间限制为 1500 秒。

B 实验结果

        从上图可是，在以垂直落差、狭窄隧道和死胡同为特征的洞穴场景中，以及以不同的建筑高度和广阔的开放区域为特色的校园场景中，我们的方法是唯一实现完整探索的方法。随着勘探区域的扩大，路径搜索的计算负担增加，FUEL 超时，ERRT 因信息获取不足而提前终止，M3 由于其粗略的前沿更新策略而提前结束勘探。在车库场景中，一个更结构化的环境，我们的方法和 FUEL 都完成了探索，尽管 FUEL 需要两倍的时间。ERRT 和 M3 实现了相对较高的表面覆盖率，但没有完成任务。

        如上表所示，每种方法都表现出不同的权衡： M3 和 ERRT 都采用贪婪策略来实现稳定的响应时间，但牺牲了全局规划最优性。请注意，尽管 M3 在大多数场景中实现了最低的平均计算时间，但由于表示过于简化，它无法完成探索。另一方面，FUEL 优先考虑全局规划最优性，导致高计算负担（在校园场景中高达 16.72 秒）。我们的算法平衡了这些方法，具有有竞争力的计算时间（平均 0.05 − 0.09 秒），实现了完整的探索和近乎最优的路径规划（甚至比车库场景中的 FUEL 还要短）。这导致更高的平均速度（平均 2.5 − 3.1m/s）和更快的整体探索。

        如上表所示，我们的环境表示方法在所有测试场景中实现了最低的内存开销。

        此外，进行了各种真实世界的实验以进一步验证我们的方法。我们利用了一个基于 LiDAR 的四旋翼平台，配备了 Intel NUC 机载计算机、NxtPx4 自动驾驶仪 [22] 和安装在 40◦ 俯仰角的 Livox MID360 LiDAR。FAST-LIO2 [23] 的修改版本被改编用于定位和映射。为确保安全，最高速度限制为 3.5m/s。首先，我们探索了一个大型校园建筑场景。勘探覆盖了 [77 × 74 × 3]m3 的区域，耗时 181 秒，穿越了 530.4m。图 1 显示了示例点云图和飞行轨迹。其次，为了在自然环境中验证我们的方法，我们在 [50 × 84 × 3] m3 杂乱的森林场景中进行了勘探测试，耗时 152.6 秒，穿越 328.5 米。点云地图和轨迹如图 9 所示。最后，我们在 [77 × 24 × 3] m3 室内走廊场景中测试了我们的方法，飞行时间为 94.7 秒，飞行长度为 194.8m。补充视频中提供了更详细的实验结果和所有场景的可视化。

5）结论

        在这封信中，我们介绍了 EPIC，这是一种基于轻量级 LiDAR 的无人机探索框架，适用于大规模环境。它使用直接源自点云的基于观测的环境表示，消除了内存密集型全局占用网格地图，同时保留了足够的信息以进行全面探索。我们还开发了一种直接在点云上运行的增量拓扑图构建方法，从而实现高效的路径规划。利用这些组件，我们构建了一个分层规划框架，与现有方法相比，显著降低了内存消耗和计算时间。