基于改进PRM算法的无人车路径规划研究

(南京林业大学机械电子工程学院，江苏 南京 210037)

随着无人驾驶技术的快速发展，作为无人车驾驶关键技术之一的路径规划得到了广泛的关注和研究。路径规划是无人车完成各种任务的基础，也是无人驾驶研究的核心问题。无人车路径规划是指在一定的环境模型基础上，给定起始点和目标点，按照性能指标规划出一条无碰撞、能安全到达目标点的有效路径。近年来，研究人员提出了各种有效的算法：如基于图像检测的算法[1]，基于节点搜索的Dijkstra算法[2]和A*算法[3]，基于启发式搜索的蚁群算法[4-5]和粒子群算法[6]，基于地图搜索的快速扩展随机树算法[7]和概率图算法[8]。

其中，以概率路图法(Probabilistic RoadMap，PRM)为代表的基于地图搜索的路径规划技术在实践中表现出较好的性能，在工作空间中的通道狭窄时，由于均匀采样的使用，所得的多数采样点将落在自由空间内，窄通道采样点数目的不足将导致概率路线图连通性降低，最终导致路径规划失败。Boor等[9]提出了高斯采样策略，通过对窄通道采样点密度的提高来增加构成连通图的概率，但采样点数目过多会降低算法的效率，Hsu等[10]提出了一种桥测试法，通过测试中点是否在自由空间内来确定其是否位于窄通道中，该方法得到的采样点位置更精确，但每次要检测三个点的位置，PARK等[11]提出了一种自适应采样方法，通过对边界点的移动，使窄通道中增加一些采样点，但采样点数量要求还是很难满足。

基于以上各方法的特点，本文提出一种改进的PRM算法，运用人工势场法优化其采样方式，克服均匀采样的弊端，同时使落在障碍物中的点移动到自由空间内，达到增加自由空间内节点数的效果，再利用A*算法搜索路径，从而提高算法的效率。

1 PRM算法

PRM算法一般可分为两个阶段：学习阶段和查询阶段。

(1)学习阶段：随机采样定量节点，即均匀选取N个节点，为每个节点搜索邻居节点并建立连接，除去与障碍物接触的连线，构建出一个无向路径网络图G=(V，E)，其中V为节点集，E为所有可能的两点之间的路径集。PRM算法流程图如图1所示。

图1 PRM算法流程图

(2)查询阶段：根据起始点、目标点、路标地图信息，采用启发式搜索算法从路标图中搜索出一条可行路径。

2 PRM算法的改进

2.1 优化采样点

传统的PRM算法运用在构型空间中均匀撒点的采样策略，采样点数目与空间的大小成正比，整个空间中采样的概率处处相等，而落在空间中障碍物上的采样点会被直接舍弃。为了满足构建路标地图时的连通性要求，往往需要增加采样次数，导致落在自由空间中的采样点增多，降低了算法效率。

如果能对落在障碍物上的采样点进行二次利用，就可以避免因多次采样导致的算法效率降低。针对传统PRM算法在采样方式上的不足，本文通过引入人工势场法对采样点进行优化。利用人工势场法可以使采样点在势场中运动这一思想，把构型空间看成一个虚拟力场，虚拟力场是由自由空间对采样点的引力场和障碍物内部对采样点的斥力场组成。

U(q)=Uatt(q)+Urep(q)

(1)

式中：q为采样点；Uatt(q)为引力场；Urep(q)为斥力场；U(q)为势场和。

当采样点落在障碍物内时，因为受到障碍物内部的斥力作用而向障碍物外移动；当采样点移动到障碍物外时，由于自由空间引力场的作用，采样点的移动速度会逐渐减小至零并停止运动。在引力和斥力的合力下，采样点在虚拟势场中的动能公式如下：

(2)

(3)

式中：Ep为采样点的动能；F(r)为采样点受到的合力；C为采样点经过的轨迹；x(t)、y(t)为轨迹C在i，j方向的分量。

在构型空间中，已知势场分布和初始采样点的位置，利用式(3)就可以得出采样点在势场作用下的停止位置。这样，原本落在障碍物内的采样点就会变成自由空间内的点，既保证了采样点的数量，也增加了窄通道的采样密度，从而提高了算法效率。

2.2 不同障碍物的势场规划

在考虑势场规划问题时，认为自由空间中的点不受势场影响，不考虑其在势场中的运动；认为落在障碍物内的点只受其所在障碍物内的势场和障碍物周围势场的影响，忽略其他障碍物的影响。对于复杂的障碍物，将其分解成多个构型简单的障碍物进行考虑，避免出现局部极小值的现象。

而在实际环境中进行势场规划时，可以将空间中的障碍物分为两种类型：①静态障碍物，即固定原地不动的障碍物，如建筑物等；②动态障碍物，即随时可能移动的障碍物，如行人、车辆等。

对于静态障碍物，可以采用均匀的势场：

(4)

式中：k为障碍物内势力场强度；-1为障碍物外势力场强度。

对于动态障碍物，假定其运动轨迹已知，不考虑突发障碍物，利用采样点到障碍物中心的径向距离来进行计算，势场函数如下：

(5)

式中：d(r-r0)为采样点到障碍物中心的径向距离。

通过改变障碍物内的势力场强度，采样点移动距离的大小就得到改善。当k值较大时，采样点的移动距离较大，在障碍物周围的分布范围较广，采样点分布更均匀；当k值较小时，采样点的移动距离较小，在障碍物周围的分布范围较小，更适用于在有窄通道的构型空间进行势场规划。

当空间中出现突发障碍物时，若该障碍物与已规划路径相交，则需要重新生成路径。对落在突发障碍物内的点再次施加势场力，使其移动到自由空间中，如果该点能与其他路径点相连则不需要重新施加势场力，否则继续施加势场力，直至能与其他路径点相连。

2.3 人工势场法仿真试验

针对人工势场(APF)的构建过程，传统的做法是通过构建包裹障碍物的外接圆，进而生成斥力场，该做法需要知道外接圆的圆心。对于不规则的非圆形障碍物，搜索外接圆圆心将会消耗部分算力，不利于提高算法的执行效率。本文期望通过障碍物的边缘区域去构建斥力场，获得规划地图后，通过对于规划地图进行图形学滤波，以获取障碍物边缘的信息。依据式(1)～(3)，构建该障碍物对于当前位置的斥力场。整体算法流程图如图2所示。

图2 算法流程图

为验证基于障碍物边缘构建的斥力场对采样点分布的调节作用，假设规划仿真地图中存在三处不规则障碍物，经过图形学滤波处理后得到障碍物边缘如图3所示。算法在选定采样点的生成位置时，将受到基于障碍物边缘的斥力场与目标点构建引力场的综合作用，如图4所示，当障碍物中存在采样点时，采样点受合力场的作用将沿着合力方向进行移动。设定合力场的调节步长为20 rpx，迭代5次，采样点沿着合力方向运动至合力为零的位置，或者满足设定迭代次数，该采样点停止动作，下一个落入障碍物的采样点重复上述动作，直至障碍物中无采样点，经由APF作用后的采样点分布情况如图5所示。参照图5，其中，“⨁”表示自由空间中的点，“+”表示落在障碍物内的点，“○”表示从障碍物中移到自由空间中的点。自由空间中的点不受势场影响，不考虑其在势场中的运动。通过调整障碍物内势力场作用，使障碍物内的点在引力和斥力作用下移动到自由空间。结果表明，落入障碍内的采样点(标记为“+”)均有效移出障碍物内，体现了算法思想的可行性。对于算法的学习阶段，在采样点的数量同一量级下，通过改善采样点分布情况提高采样点的生成质量，有效增加了算法对于窄通道的描述细节，进而提高规划路径的生成质量。

图3 含边缘障碍物

图4 合力场作用

图5 人工势场法作用

3 仿真试验与结果分析

为了检验改进算法的有效性，在硬件环境最低配置为CPU：Intel Core i5，主频：2.4 GHz，内存：4 G，硬盘：120 GB，利用MATLAB 2018b 对改进PRM算法进行仿真。首先建立一个含有障碍物的500*500的地图，为便于仿真，将无人车视为一个质点。设定无人车的起始点坐标为(10，10)，目标点坐标为(490，490)。障碍物和边界位置固定，在采样地图相同条件下，传统PRM 算法仿真试验如图6所示，改进后的 PRM 算法仿真试验如图7所示，两种算法试验结果对比见表1。其中，start表示起始节点，goal表示目标节点，连线为仿真试验规划出的路径；保证起始点和目标点位置不变，在同样的地图环境中，对传统 PRM算法和改进后的PRM算法分别进行25次仿真试验，两者时间对比结果如图8所示；选取其中10次仿真试验，将两种算法路径长度对比结果列入表2。

表2 两种算法仿真试验数据

图6 传统PRM算法仿真试验

图7 改进PRM算法仿真试验

图8 时间对比

由图 3和图4可知，相对于传统PRM算法，改进PRM算法通过引入人工势场法使落在障碍物内的采样点变成自由空间内的点，对落在障碍物上的采样点进行了二次利用，保证了采样点的数量，增加了狭窄区域的采样密度，从而提高了狭窄区的采样效率。由表1可知，改进后的PRM算法仿真试验所得到的路径轨迹转折点较少，路径轨迹更加平滑。路径长度的缩短减少了路径规划所需的时间，无人车能顺利到达目标位置，提高了搜索效率，实现了路径优化目标。

表1 两种算法仿真试验结果

由图7和表2多次重复试验结果可知，改进后的PRM算法转折点个数均少于传统PRM算法，实现了路径平滑，通过比较时间结果方差和路径长度结果方差可知，改进后的PRM算法所用时间和路径长度的波动范围比传统的小。该仿真试验表明，相比于传统PRM算法，改进后的PRM算法在路径转折点、搜索效率和路径长度方面都有明显的优化，且更具稳定性。

4 结束语

针对无人车路径规划问题，以传统PRM算法为基础，引入人工势场，通过基于障碍物边缘的斥力场与目标点构建的引力场的综合作用，使落在障碍物内的点移动到自由空间内，保证了采样点的数量，再利用A*算法搜索路径。试验表明改进的PRM算法不但减少了搜索路径中转折点个数，而且有效缩短了路径规划时间和路径长度，进一步满足了无人车路径规划的实时性要求。