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

大家好，欢迎来到IT知识分享网。

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

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

1. PRM算法流程

1.1 预处理

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

1.2 搜索

2. PRM算法案例

这里是参考代码的GitHub地址

2.1 构型采样

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

2.2 邻域计算

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

2.3 图搜索（A*搜索）

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

3. 采样数量的影响

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 基于凸形识别的混合采样采样

4.5 几种采样策略的对比

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

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

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

4.6 渐进采样

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

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

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

5. PRM算法特点

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

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

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

参考文献

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

声明

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