CMU FAR Planner 在已知和未知环境中进行高效的导航任务算法详解

FAR Planner: Fast, Attemptable Route Planner using Dynamic Visibility Update

本文将介绍一种基于可视图的路径规划框架FAR Planner,它能够在已知和未知环境中进行高效的导航任务。

摘要

论文提出了一种能够处理已知和未知环境中导航任务的基于可视图的规划框架。该规划器采用多边形来表示环境,通过提取障碍物周围的边缘点形成封闭的多边形。在此基础上,该方法使用两层数据结构动态更新全局可视图,随着导航的进行扩展可视边,并删除被新观察到的障碍物遮挡的边。在未知环境中导航时,该方法能够即时获取环境布局,更新可视图,并根据新观察到的环境快速重新规划,从而尝试发现到达目标的路径。作者在仿真和真实世界中评估了该方法。结果表明,该方法能够在未知环境中尝试并导航,与基于搜索的方法(A*、D* Lite)相比,行驶时间减少了12-47%,与基于采样的方法(RRT*、BIT*、SPARS)相比,减少了24-35%以上。

1. 引言

可视图规划已经被研究界所研究,但尚未获得广泛的普及。可视图规划的主要困难在于它对多边形世界的要求。通常需要大量的计算来构建可视图,特别是如果环境复杂且是三维(3D)的。本文重新考虑了可视图来解决路径规划问题,并展示了其在快速重规划和处理未知及部分已知环境方面的优势。该方法受益于可视边连接障碍物之间的事实。当在未知环境中导航时,看不见的区域包含很少的障碍物,涉及少量的可视边,因此调整可视图的计算成本很低——这种调整通常随着观察到更多区域而反复发生。

2. 相关工作

可见性图在机器人导航中已经被研究界广泛研究,但还没有在实际应用中得到广泛应用。主要困难在于构建可见性图需要多边形化的环境表示,计算复杂度高,尤其在复杂的三维环境中。本文重新考虑了可见性图在路径规划问题中的应用,展示了其在快速重规划以及应对未知和部分已知环境方面的优势。作者受益于可见性边仅连接障碍物之间的特性,当在未知环境中导航时,未观测区域包含的障碍物较少,涉及的可见性边数量少,因而调整可见性图的计算成本很低。这种调整通常伴随着观测区域的增加而反复发生。

随机采样方法:经典的RRT(Rapidly-exploring Random Tree)系列方法包括基本的RRT、RRT*、RRT-Connect、Informed RRT和BIT等。与PRM(Probabilistic Roadmap)方法(如Lazy PRM)以及最新的SPARS方法(生成稀疏子图加快查询速度)一起,这类方法擅长于在环境中探索自由空间。然而,在缺乏先验地图的情况下,这些方法需要在未知区域进行采样,导致了较长的规划时间。一些方法通过引导采样点偏向目标,或者重复利用之前的采样点来减少冗余采样,但这类策略容易陷入局部最小值,并且需要较大计算量来维护数据结构。

基于搜索的方法:主要包括Dijkstra算法、A*、D和D Lite。Dijkstra和A在网格化的环境表示上进行搜索,对于未知环境需要预先划分搜索区域,难以应用于大规模环境。D和D* Lite通过增量式更新前一次搜索的状态来提高重规划效率,但研究表明当机器人遇到死胡同需要重新规划时,它们的规划时间也会大幅上升,甚至超过非增量式方法。

基于学习的方法:这类方法需要使用监督学习或者借助先验地图进行训练,本质上是将环境编码进神经网络的权重中。测试时,它们能够应用于与训练环境相似的新环境。这类方法依赖数据驱动,泛化能力受到训练数据集的限制。

本文聚焦于一种基于距离的度量方法,通过动态更新可见性图来进行路径规划。尽管使用可见性图进行机器人导航的思想已有较多研究,但在实际应用中还很少见,主要原因是构建可见性图的计算复杂度高,需要精确的多边形化环境表示。本文对经典可见性图方法进行了改进,通过增量式地构建局部可见性图,降低了计算复杂度,利用可见性图稀疏的特性,实现了在未知和部分已知环境下的快速路径规划。

3. 方法

A. 障碍物多边形提取和注册

本节描述了将传感器数据点集合S转换为一组多边形{P_k^local ⊂ Q | k ∈ Z⁺}的过程。

首先,定义工作空间Q ⊂ ℝ³,机器人在其中导航。设S ⊂ Q为来自障碍物的传感器数据点集合。

对于地面车辆,通常会在系统中运行地形可通行性分析模块,以分析地形特征。该模块接收来自激光雷达或深度相机等的距离测量值,并输出代表障碍物的数据点集合S。

多边形提取过程使用图像处理技术:

1. 创建一个二值图像I,其中黑色像素对应可通行空间中的点,白色像素对应障碍物上的点,I以机器人位置p_robot为中心。

2. 将S中的点投影到I上,同时使用车辆尺寸对S中的点进行膨胀。设车辆尺寸为l_vehicle,则S中每个点的膨胀半径为l_vehicle/2。

# 将传感器数据点投影到二值图像并膨胀
for point in S:
    x, y = transform_to_image_coord(point)  # 将点转换到图像坐标
    cv2.circle(I, (x, y), int(l_vehicle/2), 255, -1)  # 在图像上绘制膨胀后的点

3. 用平均滤波器模糊二值图像I,创建灰度图像I_blur。平均滤波器的大小为k_blur × k_blur。

# 对二值图像进行平均滤波
k_blur = 5
I_blur = cv2.blur(I, (k_blur, k_blur))

4. 提取I_blur中的边缘点,并使用文献[29]中的方法分析边缘点的拓扑分布。这将得到一组具有沿轮廓密集顶点的封闭多边形,记为{P_k^contour ⊂ Q | k ∈ Z⁺}。

# 提取边缘点
contours, _ = cv2.findContours(I_blur, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

# 分析边缘点拓扑结构,得到封闭多边形 
polygons_contour = []
for contour in contours:
    polygon = analyze_topology(contour)  # 根据文献[29]分析拓扑结构
    polygons_contour.append(polygon)

5. 对于每个P_k^contour,使用文献[30]中的Douglas-Peucker算法缩小顶点数量。

# 对每个多边形应用Douglas-Peucker算法缩小顶点数
polygons_simplified = []
for polygon in polygons_contour:
    simplified_polygon = douglas_peucker(polygon, epsilon)  # 应用Douglas-Peucker算法
    polygons_simplified.append(simplified_polygon)

6. 进一步检查每个顶点连接的两条边之间的内角,以推断障碍物的局部曲率。内角小于阈值ζ的顶点被消除。

设多边形P_k^contour的顶点集合为V_k = {v_i | i = 1, 2, …, n_k},其中v_i = (x_i, y_i)表示顶点坐标。对于每个顶点v_i,计算其连接的两条边e_i-1 = (v_i-1, v_i)和e_i = (v_i, v_i+1)之间的内角θ_i:

# 计算顶点连接边的内角
for polygon in polygons_simplified:
    vertices = polygon.vertices
    n = len(vertices)
    
    for i in range(n):
        v_prev = vertices[i-1]
        v_curr = vertices[i]
        v_next = vertices[(i+1) % n]
        
        edge1 = v_curr - v_prev
        edge2 = v_next - v_curr
        
        # 计算内角
        cos_angle = np.dot(edge1, edge2) / (np.linalg.norm(edge1) * np.linalg.norm(edge2))
        angle = np.arccos(cos_angle)
        
        if angle < threshold_angle:
            # 如果内角小于阈值,删除该顶点
            polygon.remove_vertex(v_curr)

7. 使用保留的顶点形成最终提取的多边形P_k^local。

# 使用保留的顶点形成最终多边形
polygons_local = polygons_simplified

算法1给出了多边形提取和注册的完整过程:

def polygon_extraction(S):
    # 创建二值图像I
    I = create_binary_image(S)
    
    # 对S中的点进行膨胀
    I = inflate_points(I, S, l_vehicle)
    
    # 对I进行平均滤波得到I_blur
    I_blur = cv2.blur(I, (k_blur, k_blur))
    
    # 提取I_blur中的边缘点
    contours = find_contours(I_blur)
    
    polygons_local = []
    for contour in contours:
        # 分析边缘点拓扑结构,得到封闭多边形
        polygon_contour = analyze_topology(contour)
        
        # 对多边形应用Douglas-Peucker算法缩小顶点数
        polygon_simplified = douglas_peucker(polygon_contour, epsilon)
        
        # 检查顶点连接边的内角,删除小于阈值的顶点
        polygon_final = remove_small_angle_vertices(polygon_simplified, threshold_angle)
        
        polygons_local.append(polygon_final)
    
    return polygons_local

B. 双层可视图动态更新

本节介绍使用双层数据结构动态更新可视图的过程。可视图G包含两层:围绕机器人的局部层L_local和覆盖观察环境的全局层L_global。

在每个数据帧上,从传感器数据点集合S生成局部层L_local,然后与全局层L_global合并。

构建局部层L_local的过程如下:

1. 从传感器数据点集合S中提取多边形{P_k^local},得到局部多边形集合。

2. 在局部多边形集合{P_k^local}上构建部分简化的可视图,得到局部可视边集合E_local。

设局部多边形集合为{P_k^local | k = 1, 2, …, m},每个多边形P_k^local的顶点集合为V_k^local = {v_i^k | i = 1, 2, …, n_k}。

对于每对多边形P_k^local和P_l^local (k ≠ l),检查它们的顶点之间的可见性:

# 构建局部可视图
E_local = []

for k in range(m):
    for l in range(k+1, m):
        polygon1 = polygons_local[k]
        polygon2 = polygons_local[l]
        
        for v1 in polygon1.vertices:
            for v2 in polygon2.vertices:
                # 检查顶点v1和v2之间的可见性
                if is_visible(v1, v2, polygons_local):
                    # 如果可见,添加可视边
                    E_local.append((v1, v2))

其中,is_visible(v1, v2, polygons_local)函数检查顶点v1和v2之间是否存在无遮挡的直线连接。可以使用简单的几何计算实现,例如检查连接v1和v2的线段是否与任何多边形相交。

3. 对于E_local中长度大于阈值l_max的边,忽略指向一个或两个相连多边形内部的不必要边,而保留"绕过"多边形的边。

# 简化局部可视图
simplified_E_local = []

for edge in E_local:
    v1, v2 = edge
    length = np.linalg.norm(v2 - v1)
    
    if length > l_max:
        # 检查边是否指向多边形内部
        if not is_pointing_inside(edge, polygons_local):
            simplified_E_local.append(edge)
    else:
        simplified_E_local.append(edge)

E_local = simplified_E_local

其中,is_pointing_inside(edge, polygons_local)函数检查边是否指向任何多边形的内部。可以通过计算边的方向向量与多边形边界的法向量之间的夹角来实现。

4. 将构成{P_k^local}中多边形的边添加到E_local中。

# 添加多边形边界边到局部可视图
for polygon in polygons_local:
    vertices = polygon.vertices
    n = len(vertices)
    
    for i in range(n):
        v1 = vertices[i]
        v2 = vertices[(i+1) % n]
        E_local.append((v1, v2))

在构建局部可视图之后,将其与全局层L_global合并以更新全局可视图。

设全局多边形集合为{P_l^global | l = 1, 2, …, n},每个多边形P_l^global的顶点集合为V_l^global = {v_i^l | i = 1, 2, …, n_l}。全局可视边集合为E_global。

合并过程如下:

1. 将局部多边形{P_k^local}的顶点与全局多边形{P_l^global}的顶点进行关联。

对于每个局部多边形顶点v_i^k ∈ V_k^local,找到与其最近的全局多边形顶点v_j^l ∈ V_l^global,如果它们之间的欧几里得距离小于阈值d_max,则将它们关联起来。

# 关联局部多边形顶点与全局多边形顶点
associations = {}

for polygon_local in polygons_local:
    for v_local in polygon_local.vertices:
        min_dist = float('inf')
        nearest_v_global = None
        
        for polygon_global in polygons_global:
            for v_global in polygon_global.vertices:
                dist = np.linalg.norm(v_local - v_global)
                if dist < min_dist:
                    min_dist = dist
                    nearest_v_global = v_global
        
        if min_dist < d_max:
            associations[v_local] = nearest_v_global

2. 对于已关联的全局多边形顶点,使用鲁棒拟合方法更新其位置。

对于每个全局多边形顶点v_j^l,收集其在过去m个数据帧中关联的局部多边形顶点{v_i^k(t) | t = 1, 2, …, m},使用鲁棒拟合方法更新v_j^l的位置:

# 使用鲁棒拟合更新全局多边形顶点位置
for v_global in polygons_global.vertices:
    associated_vertices = []
    
    for t in range(m):
        v_local = associations[t].get(v_global)
        if v_local is not None:
            associated_vertices.append(v_local)
    
    if len(associated_vertices) > 0:
        v_global_updated = robust_fitting(associated_vertices)
        v_global.update_position(v_global_updated)

其中,robust_fitting(associated_vertices)函数使用鲁棒拟合方法(如RANSAC)估计关联顶点的鲁棒位置。

3. 对于未关联的全局多边形顶点,根据投票结果将其删除。

对于每个未关联的全局多边形顶点v_j^l,统计其在过去m个数据帧中未被关联的次数,如果未关联次数超过阈值c_max,则将其从全局多边形中删除。

# 删除未关联的全局多边形顶点
for polygon_global in polygons_global:
    for v_global in polygon_global.vertices:
        if v_global not in associations.values():
            v_global.unassociated_count += 1
            
            if v_global.unassociated_count > c_max:
                polygon_global.remove_vertex(v_global)

4. 对于未关联的局部多边形顶点,将其添加到全局多边形中作为新顶点。

# 添加未关联的局部多边形顶点到全局多边形
for v_local in associations.keys():
    if associations[v_local] is None:
        v_global_new = v_local.copy()
        polygons_global.add_vertex(v_global_new)

5. 将局部可视边E_local合并到全局可视边E_global中。如果边已存在于E_global中,则更新;否则将其作为新边添加。

# 合并局部可视边到全局可视图
for edge_local in E_local:
    v1_local, v2_local = edge_local
    v1_global = associations[v1_local]
    v2_global = associations[v2_local]
    
    if (v1_global, v2_global) in E_global:
        E_global.update_edge(v1_global, v2_global)
    else:
        E_global.add_edge(v1_global, v2_global)

算法2总结了可视图的动态更新过程:

def dynamic_visibility_update(S, G):
    polygons_local = polygon_extraction(S)
    
    # 构建局部可视图
    E_local = build_local_visibility_graph(polygons_local)
    
    # 简化局部可视图
    E_local = simplify_local_visibility_graph(E_local, polygons_local, l_max)
    
    # 关联局部多边形顶点与全局多边形顶点
    associations = associate_vertices(polygons_local, polygons_global, d_max)
    
    # 更新已关联的全局多边形顶点位置
    update_associated_vertices(polygons_global, associations)
    
    # 删除未关联的全局多边形顶点
    remove_unassociated_vertices(polygons_global, associations, c_max)
    
    # 添加未关联的局部多边形顶点到全局多边形
    add_unassociated_vertices(polygons_global, associations)
    
    # 合并局部可视边到全局可视图
    merge_local_edges(E_local, E_global, associations)
    
    return G

C. 在可视图上规划

给定机器人位置p_robot和目标位置p_goal,在可视图G上搜索两点之间的最短路径。

首先,将p_robot和p_goal添加到全局可视图的顶点集合V_global中:

# 添加机器人位置和目标位置到全局可视图
V_global.add_vertex(p_robot)
V_global.add_vertex(p_goal)

然后,将p_robot和p_goal与V_global中的其他顶点进行可视性检查,添加无遮挡的可视边到E_global中:

# 连接机器人位置和目标位置到全局可视图
for v in V_global:
    if is_visible(p_robot, v, polygons_global):
        E_global.add_edge(p_robot, v)
    
    if is_visible(p_goal, v, polygons_global):
        E_global.add_edge(p_goal, v)

接下来,使用广度优先搜索(BFS)算法在全局可视图G上搜索从p_robot到p_goal的最短路径:

# 使用BFS搜索最短路径
path = bfs_shortest_path(G, p_robot, p_goal)

其中,bfs_shortest_path(G, start, goal)函数实现了BFS算法,返回从起点start到目标goal的最短路径。

在导航过程中,机器人可以利用可视图来判断环境中的空间类型:

• 与机器人位置p_robot有直接可视边相连的顶点属于可达的自由空间。
• 其他未与p_robot直接相连的顶点属于未知空间。

可以将顶点的空间类型信息存储在可视图中,用于后续的导航任务。

# 更新顶点的空间类型信息
for v in V_global:
    if (p_robot, v) in E_global:
        v.space_type = 'free'
    else:
        v.space_type = 'unknown'

导航完成后,可以将带有空间类型标签的可视图保存下来,作为环境的先验地图供未来使用。加载先验地图后,可以根据需求选择在自由空间中搜索路径(不探索),或者在自由和未知空间的组合中搜索路径(尝试探索)。

# 加载先验地图
G_prior = load_prior_map()

# 在自由空间中搜索路径
path_free = search_path(G_prior, p_robot, p_goal, search_space='free')

# 在自由和未知空间组合中搜索路径
path_combined = search_path(G_prior, p_robot, p_goal, search_space='combined')

D. 扩展到3D多层可视图

为了适应3D环境,可以将可视图扩展到多层结构。每一层对应环境中的一个水平切片,提取该切片高度范围内的障碍物多边形。

在提取多边形时,需要考虑障碍物在垂直方向上的高度范围。假设第i层对应的高度为h_i,障碍物在垂直方向上的高度范围为[h_min, h_max],则在第i层提取的多边形需要满足:

h_min ≤ h_i ≤ h_max

提取出每一层的多边形后,可以在层与层之间建立垂直方向上的可视边,连接不同层之间的多边形顶点。

与单层可视图类似,在每一层内部也可以应用部分简化机制,忽略一些不必要的可视边。

对于连接不同层的可视边,需要进行碰撞检查,以确保边没有与中间层的障碍物相交。假设一条连接第i层顶点v_i和第j层顶点v_j的可视边e_ij,中间经过了第k层(i < k < j),则需要检查e_ij是否与第k层的任何多边形相交:

# 检查跨层可视边的碰撞
for polygon in layer[k].polygons:
    if intersect(e_ij, polygon):
        return False

return True

如果e_ij与任何中间层多边形相交,则说明该边无法通过,需要将其从多层可视图中移除。

在多层可视图上进行路径规划时,需要同时考虑每一层内部的最短路径以及不同层之间的最短路径。可以使用分层搜索的策略,先在每一层内部搜索最短路径,然后再在层与层之间进行搜索,找到最终的全局最短路径。

4. 实验

在实验部分,作者在仿真和真实环境中对算法进行了评估,并与其他几种主流方法进行了比较。

仿真实验

作者使用与真实机器人平台相同的传感器配置在三个仿真环境中进行了实验:

1. 中等复杂度的室内环境
2. 中等规模的室外校园环境
3. 大规模高度复杂的隧道网络环境

在每个环境中,机器人需要导航通过一系列目标点。实验分为两种设置:未知环境和部分已知环境。在未知环境设置中,每到达一个目标点,就重置规划器,模拟在完全未知的环境中导航。在部分已知环境设置中,规划器会累积之前的环境观测信息,模拟在逐步探索的过程中导航。

作者对比了以下方法的性能:

• 基于搜索的方法:A*、D* Lite
• 基于采样的方法:RRT*、BIT*、SPARS
• FAR Planner

实验结果表明,FAR Planner能够在未知和部分已知环境中快速有效地找到可行路径,到达目标点的时间比其他方法更短,尤其在大规模复杂环境中优势更加明显。

真实环境实验

作者在实际的室内外环境中使用真实机器人平台对FAR Planner进行了测试。机器人需要从建筑内部导航到外部,最终到达另一个建筑的目标点。环境中存在静态障碍物和动态障碍物(行人),且事先完全未知。

实验结果表明,FAR Planner能够实时更新可视图,动态适应环境变化,避开障碍物并找到可行路径,成功到达目标点。即便在复杂的真实环境中,FAR Planner的平均路径搜索时间也小于10毫秒,展现出了高效的实时性能。

5. 结论

本文提出了一种基于可视图的路径规划方法FAR Planner,用于在未知和部分已知环境中进行高效导航。该方法的主要贡献包括:

1. 一种从传感器数据提取多边形并在局部范围内增量构建可视图的算法框架,具有低计算成本。

2. 该框架能够处理未知环境,通过在线更新可视图并重新规划路径来应对环境变化和动态障碍物。

3. 在未知和部分已知环境中对多种主流方法进行了性能基准测试。

实验结果表明,FAR Planner在各种复杂环境中都能保持稳定的低计算负荷,相比其他方法具有更短的规划时间和导航时间,展现出了优越的性能。

一些关于FAR Planner的思考和讨论:

1. FAR Planner通过增量式地构建和更新局部可视图,再与全局可视图合并的方式,有效地平衡了计算效率和全局信息的利用。这种分层的数据结构设计是该方法的一个亮点,使其能够在复杂环境中实现实时导航。

2. 在提取多边形和构建可视图的过程中,FAR Planner采用了一些图像处理和计算几何的技术,如二值化、膨胀、边缘提取、多边形简化等。这些技术的合理应用,有助于从原始传感器数据中提取出紧凑而准确的环境表示,为后续的路径规划奠定了基础。

3. 与其他主流方法相比,FAR Planner在未知环境和动态环境中表现出了明显的优势。传统的基于搜索的方法(如A和D Lite)在大规模复杂环境中计算效率较低,而基于采样的方法(如RRT和BIT)在未知环境中容易产生冗余采样,导致路径质量下降。FAR Planner通过动态更新可视图,在保证实时性的同时,能够较好地适应环境变化,生成高质量的路径。

4. FAR Planner目前还主要针对二维平面环境,虽然论文中提出了扩展到三维多层可视图的思路,但具体实现和评估还有待进一步研究。在复杂的三维环境中,如何高效地表达和搜索多层可视图,以及如何处理更多的环境约束(如地形起伏、障碍物高度等),都是值得探讨的问题。

5. 在实际应用中,FAR Planner还可以与其他模块(如定位、感知、控制等)相结合,形成完整的自主导航系统。例如,可以将视觉里程计或SLAM模块提供的定位信息,用于校正和更新可视图;也可以将局部路径规划的结果,输入到运动控制模块中,生成平滑、安全的执行轨迹。这些都是未来进一步研究和应用的方向。

6. FAR Planner的开源实现为相关研究提供了一个很好的基础,社区可以在此基础上进行改进和扩展。例如,可以探索更高效的多边形提取算法、更鲁棒的数据关联方法、更智能的路径搜索策略等,进一步提升算法的性能和适用性。同时,还可以将FAR Planner与其他传感器(如视觉、雷达等)相结合,扩大其应用场景。

总之,FAR Planner是一种高效、鲁棒的路径规划方法,特别适用于未知和动态环境下的自主导航任务。它巧妙地利用了可视图这一经典的环境表示方式,通过分层构建和增量更新的策略,实现了实时的路径规划。同时,它还为未来的研究和应用提供了很好的基础和启发。相信通过进一步的探索和完善,基于可视图的路径规划方法能够在更广泛的场景中得到应用,为机器人的自主导航能力带来新的突破。