FAR Planner Fast, Attemptable Route Planner using Dynamic Visibility Update

FAR规划器：基于动态可视域更新的快速可尝试路径规划系统

摘要——未知环境中的路径规划问题仍是一个具有挑战性的难题。由于在导航过程中环境被逐步观测，底层规划器必须及时持续地更新环境表征并重新规划，以适应新的观测数据。本文提出一种基于可见性图的规划框架，能够处理已知与未知环境中的导航任务。该规划器采用多边形环境表征方法，通过提取障碍物边缘点形成闭合多边形来构建环境模型。基于此，我们采用双层数据结构动态更新全局可见性图：随着导航进程扩展可见边，同时移除被新观测障碍物遮挡的边。在未知环境中导航时，该方法能实时获取环境布局、更新可见性图，并根据新观测环境快速重新规划以探索通往目标的路径。我们在仿真和真实场景中对方法进行了评估。实验表明，该方法在未知环境中具有探索导航能力：相比基于搜索的方法（A*、D* Lite）可减少12-47%的行程时间，较基于采样的方法（RRT*、BIT*和SPARS）则能缩短24-35%以上。

1 介绍

基于可见性图的路径规划方法虽经学术界研究，却未获得广泛普及。该方法面临的主要困境在于其对多边形世界模型的严苛要求[1]。在构建可见性图时往往需要耗费大量计算资源[2]，尤其在处理复杂三维(3D)环境时更为显著。本文重新审视可见性图在路径规划问题中的应用，并论证其在快速重规划与处理未知/部分已知环境方面的优势。我们的方法充分利用了可见性边连接障碍物这一特性：在未知环境中导航时，未探索区域通常障碍物稀少，仅需处理少量可见性边，因此调整可见性图的计算开销极低——这种调整会随着导航过程中新区域的发现而反复发生。

我们的方法采用双层框架实现实时可视性图构建。在局部层面，该方法在每帧数据上构建可视性图。通过从测距传感器获取的数据，系统会提取障碍物周围的边缘点，并将边缘点转换为一系列闭合多边形。随后，在多边形之间连接可见性边以形成可见性图，该图被合并至全局图层并在全局尺度上维护。这种增量式图构建方式实现了较低的计算成本，仅需少量处理（约占用单核i7 CPU线程的20%）。针对三维环境中的飞行器路径规划，该方法进一步扩展为融合多层多边形表征体系。

在动态构建和维护可见性图的基础上，该方法以低延迟（约10毫秒内）搜索图中的最短路径，从而实现接收目标后的快速响应。当在未知环境中导航时，该方法持续以顺序方式尝试多条路线，引导车辆最终抵达目标。若环境中存在动态障碍物，该方法会消除被动态障碍物阻挡的可见性边，并在重新获得可见性后恢复这些边的连接。

我们通过地面与空中载具，在仿真和现实实验中评估本方法。地面载具仿真环境包括中等复杂度的室内环境、中等规模的户外校园环境，以及大规模高复杂度的隧道网络环境。我们以最先进的路径规划器在未知或部分已知环境中执行导航任务的表现为基准，并与我们的方法进行对比。我们得出结论：在规模庞大、高度复杂的环境中，本方法在规划时间和抵达目标耗时方面均优于当前最先进的规划器。

本文的主要贡献可概括如下：
• 一种用于从障碍物中提取多边形并以低计算成本增量构建可见性图的双层算法框架。
• 该框架能够动态调整可见性图，以在未知环境中进行可尝试的导航并处理动态障碍物。
• 在未知和部分已知环境中对最先进规划器性能进行基准测试。

FAR Planner开源软件已集成至我们的开源自主探索开发环境[3]中，以推动导航自主性研究。这两个代码库共同构成了地面车辆导航的全套规划算法体系。

二、相关工作

路径规划问题已从多个角度得到解决。然而在未知环境中的导航与规划仍具挑战性，特别是在现实场景中。本文所述方法基于随机采样规划器、搜索规划器与学习规划器的关键成果，能够同时解决已知与未知环境中的规划问题。

基于随机采样的规划器：快速扩展随机树(RRT)家族中的经典方法包括原始RRT及其变体，如RRT*[5]、RRT-Connect[6]、Informed RRT*[7]和BIT*[8]。这些方法与基于概率路线图(PRM)的方法[9]（如Lazy PRM[10]）以及最新能生成稀疏子图以实现快速查询解析的SPARS[11]共同擅长探索环境中的自由空间。然而在缺乏先验地图的情况下，这些方法需要从自由空间和未知空间同时采样，导致规划时间过长。为此开发了[12][13]等方法通过目标偏置采样或重用历史样本[14]来减少未知空间的冗余采样。这些方法要么采用贪婪引导策略易陷入局部极小值，要么需要高昂计算成本来维护迭代过程中的树/图结构以适应新观测环境。

基于搜索的规划器：这类方法包括迪杰斯特拉算法[15]、A算法[16]、D算法[17]和D* Lite算法[18]。迪杰斯特拉算法和A算法在离散化网格上进行搜索。由于需要在每个规划周期重新初始化传播过程，这些方法往往需要较长的规划时间。此外，在未知环境中，这些方法通常需要预定义离散化区域且缺乏可扩展性。为此提出的A算法增量版本——D和D Lite算法，能够有效处理未知环境导航中的环境变化问题地形。这些方法通过复用前一个规划周期的结果并仅调整局部不一致状态，从而减少规划时间。然而最新研究[19][20]表明，当车辆驶入死胡同且需要采用与先前规划截然不同的路径脱离死局时，规划时间可能大幅增加，甚至会超过非增量式规划方案的耗时。

基于学习的规划器：这类方法[21]–[23]需要通过监督式训练或使用真实数据进行训练。训练过程本质上将地图信息编码到内部表征中（例如深度网络）。在测试阶段，这些方法能处理与训练环境配置相似的场景。本质上，基于学习的规划器是数据驱动的，其适用范围可能受限于训练数据中包含的环境类型。

本研究重点探讨一种基于度量的动态可见性图更新方法。尽管利用可见性图进行机器人导航的研究已有文献记载[1][24]-[28]，但由于其计算复杂度高且需要明确定义的多边形几何结构，该方法在实际应用中尚未得到充分运用。我们的工作对基于可见性图的方法进行了适应性改进，充分利用其视线检测优势和相对稀疏的可见性边特性，从而实现在未知及部分已知环境中的快速路径规划。

III 方法

定义Q⊂R³为机器人导航的工作空间。设S⊂Q为来自障碍物的传感器数据点集合。我们的方法从S构建一个可见性图（v-graph），记作G。给定机器人位置probot∈Q与目标点pgoal∈Q，即可在probot与pgoal之间搜索路径。

A 障碍物多边形提取与配准

我们描述了将传感器数据点S转换为多边形集合{Plocal k ⊂ Q | k ∈ Z +}的处理流程。对于地面车辆而言，系统中通常会运行地形可通行性分析模块来评估地形特征。该模块接收激光雷达或深度相机等测距设备的测量数据，并输出代表障碍物的S集合。

多边形提取过程采用图像处理技术。设I为二值图像，其中黑色像素对应可通行空间的点，白色像素对应障碍物上的点，图像I以机器人位置probot为中心。我们首先将S投影到I上，同时根据车辆尺寸对S中的点进行膨胀处理。接着使用均值滤波器对图像进行模糊化处理，生成灰度图像Iblur，在图2(b)中。为帮助读者理解流程，我们将多边形提取算法记录在算法1中。。

双层V型图动态更新

本文采用的v-graph G包含两层结构：环绕机器人的局部层（记为Llocal）和覆盖观测环境的全局层（记为Lglobal）。在每帧数据中，Llocal由传感器数据点S生成后与Lglobal进行融合。已知构建v-graph的计算复杂度为O(n² log n)[31]，其中n表示v-graph的顶点数量。由于我们的v-graph仅在局部Llocal上构建，其计算成本得到显著控制。换言之，本方法通过仅更新车辆邻近区域的方式，以增量式更新v-graph将计算量均匀分配至各数据帧。

构建局部层：回顾传感器数据点S被转换为多边形集合{Plocal k}。基于{Plocal k}，我们在局部层Llocal上构建部分简化的可视性图。设Elocal为Llocal层上的可视边集合。具体而言，对于长度超过阈值的Elocal边，我们舍弃那些指向单个或两个连接多边形的不必要边，保留”绕行”边（见图3）。而短于阈值的Elocal边则全部保留不做”简化”。这是因为较短边的顶点位置噪声效应会放大，导致方向误差更为显著。边缘的变化使得难以判断这些边缘是否朝向多边形内部。具体而言，构成{Plocal k}中多边形的边缘也会保留在Elocal中，因为它们”环绕”着多边形。在实际观察中，我们发现环境中的可通行空间通常受限，大多数可见边缘会被附近的多边形阻挡，最终导致Elocal中连接的边缘数量相对较少。

更新全局层：在Llocal上构建局部vgraph后，我们将Llocal与Lglobal合并，以更新与Llocal重叠区域的Lglobal。定义{Pglobal l ⊂ Q | l ∈ Z+}为多边形集合，Eglobal为Lglobal上的可见性边集合。该过程始于关联{Plocal k}与{Pglobal l}之间的顶点。仅当两者互为最近顶点且欧氏距离小于阈值时，{Plocal k}中的顶点才会与{Pglobal l}中的顶点建立关联。随后更新已关联{Pglobal l}顶点的位置。此处采用鲁棒拟合[32]进一步剔除异常关联。给定{Pglobal l}中某顶点，通过迭代过程筛选多帧数据中的对应顶点：初始将所有顶点视为内点，每次迭代重新计算内点均值与协方差矩阵，据此重新划分顶点中的内点与离群点。若连续两次迭代内点未变化或达到最大迭代次数，则终止迭代，并将{Pglobal l}中该顶点更新为内点均值。对于未关联的{Pglobal l}顶点，根据多帧数据中连续未关联次数的投票结果进行移除。未关联的{Plocal k}顶点则作为新顶点加入{Pglobal l}。最后将Elocal中的边合并至Eglobal：若边已存在于Eglobal则更新，否则作为新边添加。v-graph更新的整体流程如算法2所示。

当机器人在环境中导航时，{Pglobal l}中与机器人建立非阻塞可视边的顶点构成自由空间，其余顶点则构成未知空间。导航完成后，保存的v-graph会为每个顶点标注空间类型。在后续运行时，该v-graph可作为先验地图加载至路径规划器。在通过顶点搜索路径时，我们提供两种选项：在自由空间与未知空间的组合区域进行可尝试规划，或仅在自由空间进行不可尝试规划。

D 扩展至三维多层V型图
我们针对飞行器规划的扩展三维方法将环境建模为多个水平切片，并提取多层多边形。可见性边在不同多边形层之间进行连接。需注意，部分简化机制仅适用于单个多边形层上的可见性边。对于连接不同多边形层的可见性边，我们保留了所有非阻挡边。此外，对于横跨三个或更多多边形图层的可见性边，碰撞检查会考虑该边穿过的中间层上多边形的阻挡情况。图4展示了一个三维可见性图的示例以及在该图上搜索的路径。

图4. 三维多层可视性图表示例。多层多边形（红色）从三维环境中提取。可视性边（青色线条）横跨多个多边形层。此外，在车辆（坐标系）与目标点（红点）之间的三维多层可视性图上搜索出一条路径（蓝色）。

IV实验

如图5所示，我们的地面车辆平台与仿真飞行器平台均配备了Velodyne Puck激光雷达作为导航规划的测距传感器。地面车辆搭载640×360分辨率的摄像头和基于MEMS的惯性测量单元（IMU），与激光雷达协同实现状态估计[33]。该自主系统整合了开发环境中的导航模块——包括地形分析、基于局部规划器生成的动力学可行轨迹的航点追踪[34]等基础导航模块，并在系统顶层运行FAR规划器。

在实验中，我们将本方法与两种基于搜索的方法（A* [16]、D* Lite [18]）以及三种基于采样的方法（RRT* [5]、BIT* [8]、SPARS [11]）进行了比较。其中，BIT被视为RRT系列中最先进的方法，而RRT是该系列的经典方法；SPARS则被认为是PRM系列中最先进的方法。所有实验均在一台4.1GHz的i7计算机上运行。我们将FAR规划器的v-graph更新频率设置为2.5Hz，并在每次v-graph更新时执行路径搜索以进行重新规划。该规划器使用0.2米/像素分辨率的图像提取边缘点来构建多边形。v-graph上的局部层是一个40米×40米的区域，车辆位于该区域中心。

A 地面车辆模拟

模拟实验采用了与图5(a)中真实地面车辆平台相同的车辆及传感器配置，车速设定为2米/秒。实验分别在中等复杂度的室内环境、中等规模的户外校园环境以及大规模高复杂度的隧道网络环境中进行。图6至图8分别展示了三种环境下的实验结果。所有实验场景中，车辆均被设定为沿一系列航点行进。每组实验包含两种模式：完全未知环境与部分已知环境。在完全未知环境模式下，每当车辆抵达一个航点后，路径规划器会被重置，使车辆在向下一个航点行进时处于环境信息未知状态；而在部分已知环境模式下，系统则保留已探索的环境信息。不是重置规划器，而是让其在整个运行过程中持续积累环境观测数据，即随着车辆在环境中导航，该环境会逐渐转变为部分已知环境

图6-8(a)展示了在未知环境设置下，我们方法与其他方法的轨迹对比。FAR规划器能够生成与A和D Lite算法相似的高效轨迹，而BIT*、SPARS和RRT*算法则容易受随机性影响，常在轨迹上产生往复模式。图6-8(b)-©展示了各点通行时间及规划器重置时的搜索耗时。图6-8(d)-(e)呈现了在部分已知环境设置下的相同指标，该环境中观测数据会随导航过程不断累积。表I-III分别汇总了全程总通行时间、平均路径搜索时间（规划耗时）以及单CPU线程在整个运行过程中的占用率所表征的处理负载百分比。对于FAR规划器而言，处理负载包括多边形提取、v图更新和路径搜索等所有任务的计算工作。

在基于搜索的路径规划方法中，A算法与D Lite算法以分辨率完备性著称，能够找到最优路径。然而这些方法难以扩展，当环境规模增大且复杂度提升时，其计算成本会显著增加。漫长的规划时间会导致响应迟缓与行程耗时延长。本研究中，A与D算法的地图分辨率均设置为0.2米。表二与表三显示，A算法的搜索时间从室内环境到校园环境几乎翻倍，而从校园环境转入隧道环境时，其耗时增长约达三倍。处理负载从中等规模室内环境的16.5%上升至大规模复杂隧道环境的89.9%。另一方面，D Lite算法通过复用上一轮规划的状态值来减少规划时间。但当车辆遇到死胡同时，大量状态值会变得不一致，重新规划需要多次迭代才能使状态值收敛。D* Lite的搜索时间从室内环境的28.6毫秒增至校园环境的462.7毫秒，其处理负载也从15.7%攀升至97.2%。

对于基于采样的方法（如BIT*、SPARS和RRT*），其搜索时间存在高度不稳定性且易受随机性影响，常会生成沿轨迹来回折返的路径模式，从而增加总体行进时间。在未知环境中寻找路径时，采样范围不仅需覆盖自由空间，还必须包含未知空间。当环境转为部分已知状态时，搜索时间会显著增加——因为未知空间与自由空间的连接通路常被已观测到的障碍物阻断，导致难以在未知空间中找到通往目标的可行路径。如表II和III所示，这三种基于采样的方法（BIT*、SPARS和RRT*）从完全未知环境到部分已知环境时，搜索时间和处理负荷均呈现上升趋势。特别是在隧道环境中，BIT*的搜索时间从16.8毫秒激增至392.3毫秒，处理负荷从9.6%攀升至92.1%，最终导致部分已知环境设定下的总体行进时间大幅延长，比未知环境设置长50.7%。

如图6-8(a)所示，FAR规划器能够在图上搜索最优路径，并生成穿越未知环境抵达目标的高效轨迹。表I显示，在室内和隧道环境中，FAR规划器较A算法减少高达12.0%的行程时间，较D Lite算法减少高达47.0%的行程时间。在户外校园环境中，A算法在行程时间上略微优于FAR规划器。但FAR规划器仅消耗约一半的运算负荷，且重新规划速度比A算法快近两个数量级。

与基于采样的规划器相比，FAR规划器在不同环境设置下的行进时间、规划时间一致性及处理负载方面均超越了BIT*、SPARS和RRT算法。在隧道环境中，FAR规划器完成运行的速度比BIT快23.8%以上，比SPARS快34.7%以上，比RRT*快26.0%以上。此外，通过双层vgraph框架，FAR规划器在未知和部分已知环境设置中均保持较低处理负载，其平均处理负载不足单CPU线程的20%（详见表III）。值得注意的是，FAR规划器在所有实验中均展现出快速重规划能力，平均搜索时间低于10毫秒（见表II）。

B 地面车辆物理实验

物理实验采用图5(a)所示的地面车辆平台，速度设定为1.5米/秒。如图9所示，车辆从建筑物内部出发，导航至外部，最终抵达车库建筑内的目标点。实验环境为事先未知状态。车辆途中四次遭遇死胡同后重新规划路线。一辆手推车初始阻挡了车辆去路，迫使其选择另一条导致死胡同的路径。当车辆从死胡同折返时，手推车已被移开，使车辆得以通行。室外区域存在行人作为动态障碍物。FAR规划器会断开被人群阻挡的可视边，待行人离开后重新连接这些边。图10(b)展示了全程搜索耗时情况（平均7.32毫秒）。车辆在406秒内行驶了388米。

图9. 地面车辆物理实验。(a)图中蓝色曲线表示车辆从蓝点出发至红点结束的行驶轨迹。该车辆从建筑物内部出发，导航至外部环境，最终抵达车库建筑内的目标位置。实验环境完全未知。青色线段表示导航过程中生成的可见边。底部小图按轨迹标注从A至F点拍摄：其中B、D、E、F为四个死胡同位置，车辆在此重新规划路线。A点处走廊的手推车迫使车辆改道进入B点死胡同；返回途中手推车已被移走，车辆得以穿过走廊抵达外部。C点周边区域存在动态障碍物。(b)图显示全程搜索耗时情况。

卡内基梅隆大学-俄亥俄州立大学联合团队采用FAR规划器作为参加DARPA地下挑战赛的主路径规划系统。在决赛中，该团队凭借全场最完整的勘探测绘表现（28个区域中完成26个），荣获”最佳区域勘探奖”。

C飞行器仿真

我们使用图5(b)中模拟的空中平台，速度设置为4米/秒。FAR规划器的三维版本采用多层多边形结构，多边形层间的垂直分辨率为1米。图10展示了基于大学校园环境的仿真初步结果：飞行器遵循一系列航点，在未知环境中实现自主导航。该图同时呈现了目标点与飞行轨迹。总体而言，飞行器在57秒内完成了210米航迹的飞行。

图10. 校园环境中飞行器模拟的初步结果。与地面车辆模拟实验类似，飞行器被设置为从起点按递增顺序导航至可通行点3，其轨迹如图所示。飞行器抵达每个航点后，路径规划器将重新初始化。

v 结论

我们提出了一种基于可见性图的路径规划框架。该方法能够高效处理未知和部分已知环境，其核心流程包括将障碍物数据注册为多边形，并采用双层数据结构动态更新可见性图以降低计算成本。路径搜索通过可见性图传播实现，能以极低延迟发现路径。我们对比测试了多种先进算法的性能：基于采样的BIT*、SPARS和经典RRT算法，以及基于搜索的A算法以及D* Lite算法在未知和部分已知环境中的表现。此外，我们通过地面车辆在仿真和真实环境中对本方法进行了评估，并利用多层三维V型图对飞行器进行了初步测试。对比结果表明：本方法在未知与部分已知环境中均能保持较低且稳定的运算负荷，与现有技术相比显著减少了路径规划时间。