自动驾驶路径规划——基于概率采样的路径规划算法（PRM）

     路径规划的基本任务可以描述为：从开始位置到目标位置的一条曲线或离散点。
    这一任务通常涉及到两项基本问题：

1. 如何躲避构型空间中出现的障碍物（几何路径规划）;
2. 如何满足机器人本身在机械、传感方面的速度、加速度等限制（不确定性、反馈、微分约束等）。

    上世纪90年代，基于概率采样的算法被提出。
    这类算法不必对构型空间显示建模，取而代之的是用随机采样的点去近似潜在的构型空间，然后在这些采样点中进行搜索得到可行路径。
    在高维机器人路径规划问题中非常高效，有很大的优越性。
    基于抽样的运动规划算法是用于解决避障问题的重要方法，其核心思想为：先对机器人的单个构型进行碰撞检测，建立无碰撞构型的数据库，再对不同的构型进行采样以生成无碰撞路径。
    该算法的主要优点在于具有通用性，只需要针对不同的机器人运动规划问题进行合理的参数调整。
    典型的抽样规划方法有综合查询方法和单一查询方法两类。
    前者首先构建路线图，先通过采样和碰撞检测建立完整的无向图，以得到构型空间的完整连接属性。再通过图搜索即可得到可行的路径。
    后者则从特定的初始构型出发局部建立路线图，在构型空间中延伸树型数据结构，最终使它们相连。
    概率路线图（Probabilistic Roadmap，PRM）属于综合查询方法。

1. PRM算法流程

1.1 预处理

• 初始化。设G(V,E)为一个无向图，其中顶点集 V代表无碰撞的顶点集，连线集E代表无碰撞路径。初始状态为空。
• 构型采样。从构型空间中采样一个无碰撞的点a(i)并加入到顶点集 V 中。
• 领域计算。定义距离 ρ，对于已经存在于顶点集V 中的点，如果它与a(i)的距离小于 ρ，则将其称作点a(i)的邻域点。
• 边线连接。将点a(i)与其领域点相连，生成连线τ 。
• 碰撞检测。检测连线 τ 是否与障碍物发生碰撞，如果无碰撞，则将其加入到连线集 E 中。
• 结束条件。当所有采样点（满足采样数量要求）均已完成上述步骤后结束，否则重复2-5。

1.2 搜索

    采用图搜索算法对无向图G进行搜索，如果能找到起始点A到终点B的路线，说明存在可行的路径规划方案。

2. PRM算法案例

这里是参考代码的GitHub地址

2.1 构型采样

    构型采样。在图中随机采样一定数量（如750个）的无碰撞点(图中显示为紫色的点)。需要进行碰撞检测，抛弃与障碍物发生碰撞的点。（碰撞检测使用的地图是二值化地图）
注意：左下角（绿色）和右上角（红色）两个点是规划的起点和终点

2.2 邻域计算

    邻域计算，边线连接与碰撞检测。对每一个点，取其领域内（例如直线距离在12以内）的所有点进行连线，对连线进行碰撞检测，将结果存放在邻接矩阵中。

2.3 图搜索（A*搜索）

    Astar搜索。采用Astar搜索算法对上图进行搜索，找到从起点到终点的最短路径，即为可行的路径规划方案。    图中红色为最短路线，绿色为第二短的路线，蓝色为第三短的路线，其他颜色为保留线路。最后对路线处理(路径修剪)，得到最后最短的规划路线。

3. 采样数量的影响

    对同一地图，采样点的数量越多，找到合理路径以及更优路径的概率就越大。但同时，采样点数量越多，计算与搜索时间也会更长。
    如果只设置20个采样点，邻域20。可以看到，并不能找到可行路径，这也说明了抽样规划算法存在的完备性弱的问题。    若设置500和2000个采样点，虽然都能找到可行路径，但耗时却存在差异。500个点2000个点
    概率路线图(PRM)方法适用范围很广，对于不同的应用场景，只需要调整相应的参数即可。
    但是，抽样方法的完备性很弱，即使空间中存在合理的路径，由于抽样参数的设置问题，也可能无法找到路径；
    另外，由于抽样过程的随机性，该方法的稳定性也不好，前后两次的解往往不一样，因此在严格要求稳定性的场合并不适用。

4. 采样策略

4.1 基于障碍物的采样

    该采样策略得到的路标点分布在障碍物边缘，极大地提高了狭窄通道的采样效率。如下图伪代码所示：在每次采样时，首先在C_obs中采样节点q₁，然后在C_free 中采样节点q₂ ， q₂向q₁方向插值直到与障碍物碰撞，取碰撞前的节点 q 为路标点，完成一次采样。
    基于障碍物的采样策略关注障碍物边缘的路标点，得到的路标图覆盖狭窄通道概率更高，可提高搜索成功率。
    但每个采样点需反复对插值点进行碰撞检测得到，采样过程复杂且不稳定。
    此外，生成的路标点存在大量冗余，且得到的规划路径靠近障碍物，不易于机器人安全平稳控制。

4.2 高斯采样

    与基于障碍物采样类似，在障碍物边缘附近密集采样路标点。
    高斯采样方法思想为：在构型空间 C 中均匀采样节点q₁ ，然后以q₁为中心定义一个高斯分布 G。由高斯分布 G 生成节点q₂ ，如果这两个节点分别位于C_obs 和 C_free ，则返回位于 C_free中的节点为路标点，采样成功，否则采样失败，重复上述过程。
    与基于障碍物采样相比，高斯采样中节点q₂ 由节点q₁ 的高斯分布 G 生成，避免了在 C 空间中的多次插值和碰撞检测，提高了采样效率。
    高斯分布中方差σ的选择至关重要，太大的σ难以对狭窄通道采样，太小的σ采样效率不高，且得到的采样点距离障碍物太近，移动机器人容易和障碍物发生碰撞。
    合理的σ需根据机器人和状态空间的特性综合考量，既要保证机器人与障碍物有适当距离，同时能在狭窄通道中成功采样。
    与基于障碍物采样方法相似，高斯采样得到的路标点集中分布在障碍物边缘，可获取地图中的连通信息。但同样存在大量冗余，如凹陷、障碍物转角区域的路标点。

4.3 桥测试采样

    桥测试采样是专为狭窄通道问题提出的一种采样方法，该方法充分考虑了狭窄通道特性，将采样直接聚焦在包含关键连通信息的狭窄通道区域。
    相比基于障碍物采样和高斯采样方法，桥测试采样方法得到的路标点分布在狭窄通道内，而不是障碍物边缘。
    桥测试法通过对局部环境的简单测试，实现对狭窄通道的识别，以提高狭窄通道的采样密度。
    它基于对环境的如下观测，即对于一个 n维构型空间中的狭窄通道，机器人至少在一个方向 v 上行动非常受限，在 v 方向上细微的运动可能与障碍物发生碰撞，机器人若要在狭窄通道不发生碰撞，只能沿着垂直于 v 的方向行动。
    如图所示，可先得到一条两个端点均在障碍物内的路线 s ，如果 s 的中点 q 不在障碍物内，则可以判定 q 在狭窄通道内，路线 s 称之为“桥”。    桥测试的具体步骤为：首先在障碍物空间C_obs中采样节点q1 ，然后以q₁为中心生成高斯分布 G，通过 G 采样节点q₂ ，如果q2位于C_obs ，进一步验证q₁和q₂的中点q 所处位置，如果位于自由空间C_free ，则采样成功，将 q 加入路标图。

    桥测试采样的路标点集中在狭窄通道，能获取更加充分的连通信息，但也存在一些冗余采样点，如在一些障碍物转角、凹陷区域也有分布。

4.4 基于凸形识别的混合采样采样

    基于障碍物采样、高斯采样、桥测试采样均提高了路标质量，但在一些凹陷、平滑表面、障碍物转角等区域仍存在大量冗余路标点，这些路标点误导了路径的扩展，增加了计算量，不利于 PRM 查询阶段对最优路径的搜索。
    在构型空间中，自由空间被障碍物隔开，障碍物的突出部分刻画了障碍物的有效轮廓，具有丰富的自由空间连通信息。
    可视图法利用了这个特性，通过连接障碍物的这些突出边缘构建图结构，较为直观。
    如图所示，虚线部分包含了大量的连通信息，高质量的路径经过这些区域的概率很大。
    为了在障碍物凸出部分密集采样，利用凸出部分的几何特性，在采样点附近合理设置辅点（辅助采样点），实现对这些区域的智能识别。
    具体步骤为：利用高斯采样得到两个分别在障碍物空间 C_obs和自由空间 C_free 中的采样点 A 和 B，以 AB 为直径得到图中虚线所示圆弧，在圆弧上等距设置 C、D、E、F、G、H 辅点，然后依次对这 6 个点进行碰撞检测，如果在自由空间中的点超过一半，则可判断 B 点大概率位于障碍物的凸形边缘附近，将 B 点加入路标点集。

    给定高斯采样的两个节点 A、B，其中 A 位于障碍物空间，B 位于自由空间，基于凸形识别的采样伪代码如图所示。
    凸形识别采样方法能利用凸形区域的几何先验，通过合理设置辅助点，实现对凸形边缘路标点的采样。
    但完成一个路标点的采样过程，基于凸形识别的采样方法需进行将近 8 次碰撞检测（平均次数小于 8），显然更为耗时。
    可将凸形识别采样和均匀采样混合使用，利用凸形识别采样识别出凸形边缘路标点，然后在其附近随机扩展若干路标点，同时混合均匀采样在空旷区域配置少量路标点，从而使路标在空间中合理分布。

4.5 几种采样策略的对比

• 均匀采样法的路标点在环境中均匀分布，在狭窄通道较稀疏；
• 高斯采样法的路标点分布在障碍物边缘，存在很多冗余；
• 桥测试法的路标点分布在狭窄通道，在一些凹陷区域存在冗余，且通道口分布稀疏；
• 凸形识别采样的路标点密集分布在障碍物凸形边缘区域，包含大量环境中的连通信息。

    从表中可以看出，稍显不足的是，凸形识别由于每次采样需要对至少 6 个点进行碰撞检测，采样效率较低。

    为了测试该混合采样方法对 PRM 算法整体性能的影响，对比了基于不同采样方法的 PRM算法性能表现，每种采样器各运行 30 次，取平均得到仿真结果。
    对比结果可以看出，均匀采样由于需大量采样使狭窄通道连通，路标图复杂，规划最为耗时。
    高斯采样路标点分布在障碍物边缘，路标图沿着障碍物周边构建，路径最长。
    桥测试法在狭窄通道连通性好，可较快得到可行路径，但缺乏空旷区域的连通信息，路径相对较长。
    凸形识别采样路标点富含连通信息，路径最短，但由于采样效率低，规划较为耗时。
    混合采样在凸形区域局部扩展，提高了采样效率，在空旷区域均匀采样，改善了采样分布，规划性能提升明显。

4.6 渐进采样

    在基于采样优化的算法中,高斯采样、桥测试等方法在获得较好的障碍物、窄道等区域信息后,还要结合在整个空间随机分布的采样策略如均勻采样来生成最终的路标图。    与此相反,渐进采样的执行步骤是先进行类似均匀采样的操作,然后进行采样测试增加节点。
    渐进采样方法中有两步主要的操作：

• 初步采样。该步骤是为了获取大概的位形空间信息,可以选用均衡采样、伪随机采样等己有的采样策略。
• 节点增加。在获得大概的空间信息后,进行区域分析和判定,然后对可能含有较多连通信息的区域进行采样试验并增加节点。

    经过渐进采样之后,直接得到了可以使用的路标图,无需再结合均匀采样等方法,因为第一步的初步采样己经在空旷自由空间分布了采样点。

    初步采样后,得到了位形空间中的一组采样点pi和一组采样点状态O(pi)。
    由于具有连通信息的关键区域在障碍物附近,而所有O(pi)=1的采样点均在障碍物内部,因此可以有目的性的在采样点pi(这里称为基准点)附近通过采样点测试增加釆样点。
    在初步采样中使用均匀分布,节点增加步骤中的测试条件是落在自由空间内则保留,目的是达到类似高斯采样的效果。左图为高斯采样,右图为渐进采样。

5. PRM算法特点

    它与传统路径规划方法的不同之处在于,地图在位姿空间中不是以确定的方式来构造,而是使用某种概率技术来构造的,因此在路径规划时可以避免陷入局部最小值。
    另一个优点在于,其复杂度主要取决于寻找路径的难度,跟整个规划环境的复杂度和位姿空间的维数无关。
    基于路径规划器虽然不是完全的,有时可能会找不到存在的路径,但如果路径确实存在,则随着搜索时间的增加其成功的概率迅速向1收敛,从这个意义上讲,其具备了概率的完备性。
    传统的PRM算法也有其不足之处：

• 预处理阶段花费90%以上的时间用于碰撞检测计算；
• 对全部的局部路径进行碰撞检测,然而大多数局部路径不是最佳路径；
• 在没有碰撞时的检测最为耗时,因为碰撞检测会重复进行直到达到某一阈值为止。

    由此可见,传统的PRM规划算法在需要大量时间进行碰撞检测计算,因此只适合处理环境状态不变的场合。为减少规划时间,许多思路被相继提出：
    优化采样策略。比如：直接用均匀分布形式对位姿点进行采样,并且在关键区域如狭窄区域增加采样密度,以改善这些区域的连通性。此后又出现了一些采样策略,其目的都是为了确保采样密度和路径规划成功率之间的平衡。
    优化碰撞检测速度。采用分层包围盒碰撞检测算法,用分层的包围盒来代替环境中的实物,提高了碰撞检测的速度
    延迟碰撞检测。无碰撞位姿点的采样可以在路径规划时同时进行。位姿点间局部路径的碰撞检测可以延迟到找到最佳路径后再进行,这样可以减少很多无谓计算。
    基于上述思想学者提出了SBL-PRM算法,即同时从两个基本位姿点出发,采用启发式搜索方法双向搜索可能路径,找到最短路径后再对连接路径的各节点间线段进行碰撞检测,如果发现碰撞,则将该线段删除,重新搜索最短路径,直到找到无碰撞路径或无解返回。

参考文献

[1] 杨硕. 动态环境下自主移动机器人路径规划研究[D]. 广东:广东工业大学,2014. DOI:10.7666/d.Y2581874.

声明

本人所有文章仅作为自己的学习记录，若有侵权，联系立删。