基于修正PRM算法的智能车辆路径规划研究

李琼琼，徐溢琪，布升强,2，杨家富*，陈勇

(1.南京林业大学 机械电子工程学院，南京210037; 2.恒生电子股份电子有限公司，杭州 310053)

0 引言

近年来，随着云计算、大数据等新兴技术的发展和5G建设的全面启动，智能车辆得到了快速发展，路径规划技术是智能车辆运动决策和定位导航的基础[1-3]，最具有代表性的路径规划算法有基于地图的路径规划算法、基于生物群体智能行为算法和基于采样的路径规划算法[4-7]。基于采样的路径规划算法有快速扩展随机树算法(Rapidly-Exploring Random Tree, RRT)[8]和概率地图算法(Probabilistic Road Map，PRM)。PRM算法是一种随机采样算法[9]，通过构造规划空间的路网拓扑关系地图，将连续空间内的路径规划问题转换为拓扑空间内的规划问题[10-11]，但其在规划路径时存在着采样点选取缺乏导向性、路标图复用率低和算法搜索效率低等缺点[12]，国内外研究学者针对这些不足进行了大量的改进研究。Kurniawati等[13]于障碍物边界采样，评估最优可行区域，优化PRM算法随机采样的分散性;邹善席等[14]在基本PRM算法基础上引入增加节点的改进策略以优化陷入障碍物的采样点，但是节点数量的增加使得计算成本较高，从而影响算法的实时性能;Ravankar等[15]采用分层混合PRM算法和人工势场方法(Artifical Potential Field，APF)进行全局路径规划，使用一种节点分布分解的方法将路标图划分为高电位和低电位区域，提高了算法搜索路径的效率;受到邻相关矩阵的启发，Esposito等[16]提出一种优化概率路线图的处理算法，简化处理自由空间内凸单元与节点数量所需的计算。

本研究针对PRM算法采样点冗余、路标图构建质量不稳定等缺点，基于均匀采样设计一种伪随机采样方法，采用双向增量式碰撞检测机制，设置路点之间的距离连接阈值，提取规划路径的关键路点进行平滑处理，改善了概率地图算法在规划路径时的不足。

1 修正PRM算法

1.1 PRM算法

PRM算法包括采样和查询阶段。

采样阶段：PRM算法在规划空间内随机采样，并通过局部规划器判断采样点的合理性，重复采样n次生成的有效路点构成集合V,遍历V，连接所有路点之间的可行路径扩展至整个规划空间，形成路标图G(V,E)，其中V={v1,v2,…,vn}表示路点集合，E={vi,vj|,vi,vj∈V}表示路点之间边的集合。

算法查询阶段:将起始点qinit与目标点qgoal接入路标图G(V,E)中，使用图搜索算法在路标图G(V,E)中寻找连接起始点qinit与目标点qgoal的无碰撞路径。

1.2 伪随机采样

在PRM算法中，生成的采样点数量随着规划空间的增大而增加，难以实现全局均匀分布，易造成采样点冗余。由于最短路径出现在起始点与目标点连线区域附近的概率较大，把该区域视作重点采样区域，简称为空间主轴线区域[17-18],如图1所示，s表示起点，g表示目标点。

图1 空间主轴线

构建空间主轴信息。令起始点s坐标为(xs,ys)、目标点g坐标为(xg,yg)，空间主轴线的长度L与偏角θ为

L=||g-s||2

。

(1)

(2)

将长度为L的空间主轴线n等分，得到纵向采样间距(Nd)为

(3)

参考随机采样方式，采样点对称分布在空间主轴线附近的扇形区域内，采样点Pi,j(x,y)的计算公式如下

x=xs+rd×cos(θ+φj)

。

(4)

y=ys+rd×sin(θ+φj)

。

(5)

rd=i×Nd,i=(1,2,…,n)

。

(6)

式中：xs、xs为智能车辆的起始点；rd为采样半径，采样半径以起始点为圆心；φj∈[-φm,φm]为采样点的偏向角度；φm为最大偏转角，用于控制扇形采样区域的角度，即横向采样的范围。

由图2(a)和图2(b)可知，采样点对称分布在空间主轴线的两侧，采样范围受最大偏转角φm的控制，随着φm的增大，采样点沿着空间主轴方向朝四周扩散。为了使采样点分布更均匀，沿着空间主轴线调整横向采样范围，采样范围调整增量为Δφ=φm/n，调整后采样点分布情况如图2(c)和2(d)所示。

图2 基于空间主轴的采样方式

依据均匀采样的特点，统计自由空间内采样点的个数p，定义横向采样层的有效采样率为R

(7)

式中：N为当前采样层的总采样次数；R为有效采样率，R越大，表示该采样层的连通性越好，若R过小，表示该采样层中大部分采样点落入障碍空间。

若后续采样层沿用相同的采样间距，采样点落入障碍空间的几率将增大，为了提高采样点的避障能力，引入随机增量Δr调整采样点的采样间隔，以图2(d)为基础，调节随机增量Δr的取值大小得到图3，随机增量Δr的取值越大，采样点越趋近于随机分布，Δr的取值越小，采样点趋近均匀分布。

图3 基于伪随机的采样方式

引入随机增量后Δr后采样半径如公式(8)所示。

(8)

由图4可知，空心圆点表示调整采样间距前的采样点，实心圆点表示调整采样间距后的采样点，红色标记代表落入障碍空间的采样点，黑色标记代表自由空间中的采样点，调整采样间距前，该采样层的有效采样率较低(R=0.3)，调整采样间距提高了有效采样率(R=0.8)，伪随机采样策略提高了采样点的生成质量。

图4 采样点调整示意图

1.3 双向增量式碰撞检测

在传统的PRM算法中，碰撞检测采取增量式检测策略，规划器按照固定的步长选取路点并检测该点是否落入障碍空间，为了提高碰撞检测执行效率，将增量式检测方法与二分法结合，提出一种双向增量式检测策略。

由图5(a)可知，首先判断首末端路点的合理性，若路点属于障碍空间，则结束检测；若路点属于自用空间，则沿着首末连接路点双向逐步选取测试点，并判断测试点的合理性；若所选的测试点属于障碍空间，则停止检测丢弃该路径，如图5(b)所示。通过碰撞检测的路点相互连接，最终形成路标图。

图5 双向增量式检测策略示意图

1.4 邻层连接策略

若选取较多的处于同一采样层的路点连接形成局部路径不利于缩短全局路径长度，考虑到纵向采样层的分布特点，设置纵向采样间距的连接阈值为LTH，使得路点由同一采样层连接转换为相邻采样层连接。

选取基于伪随机采样策略和碰撞检测策略生成的路点(N=20)，得到种连接策略驱动下构建的路标图，如图6所示。图6(a)为全连接策略生成的路标图，红色实线代表被筛选出的路径，图6(b)邻层连接策略生成的路标图。从耗时上分析，采用不同连接策略的构图用时分别为0.906 s 和0.437 s，邻层连接的连接方法使得构图效率优化了48.2%。

图6 路标图对比情况

2 路径平滑

经过算法搜索得到的路径可能会存在“锯齿”，需要经过平滑处理后才利于驱动底层控制器的实施，更为贴合智能车辆行驶的实际工况。贝塞尔曲线由于其简单易设计、可直接生成光滑轨迹等优点被应用于航迹规划中，n阶贝塞尔曲线表达式为

(9)

式中：Pi为贝塞尔曲线的第n+1个控制点；bi,n(t)为Bernstein基函数，该函数的取值情况如公式(10)所示。

(10)

本文选用4阶塞尔曲线对修正PRM算法的规划路径进行平滑处理，4阶塞尔曲线表达式为

B(t)=(1-t)4P0+4P1(1-t)3t+6P2(1-t)2t2+

4P3(1-t)t3+P4t4，t∈[0,1]。

(11)

贝塞尔曲线在任意一点的曲率κ(t)为

(12)

假定规划路径path={Pn}由一系列离散点组成(n≥5)，将离散点作为贝塞尔曲线的控制点Pi，根据公式(12)可以得到贝塞尔曲线的曲率κ(P)

(13)

起始点处的贝塞尔曲线的曲率κ(0)为

(14)

在具体实施过程中，提取修正PRM算法搜索路径的关键路点，对关键路点间的连线做离散化处理得到贝塞尔曲线的离散控制点Pi，经过公式(9)对离散点做插值拟合，实现路径的平滑处理。

3 仿真试验及分析

为验证修正PRM算法的构图效率和路径规划效率，以基本PRM算法为对比算法，搭建Matlab仿真试验平台和ROS小车试验平台对修正PRM算法的正确性进行验证分析，计算机参数为：Windows10，硬盘512 GB，内存为8 G。

3.1 算法构图效率对比

已知规划空间如图7和图8所示。基本PRM算法和修正PRM算法在采样阶段保持采样点总数N=m×n相同，其中，m、n分别代表算法的横、纵向采样点个数，重点关注规划路径长度和路标图构建时间，重复多次试验(记录10次)结果取均值见表1。

表1 算法对比结果

以采样点N=60为例，分析路标图构建结果图7(a)和图8(a)，在PRM算法中采样点分布较广，存在着许多冗余采样点，修正PRM算法构建的路标图7(a)，采样点的位置选择具有一定的导向性，主要沿着空间主轴线分布，冗余采样点较少。

图7 基本PRM算法规划结果(N=60)

图8 修正PRM算法规划结果(N=60)

表1可知，在采样点数量不多的情况下(N=30,N=60)，算法规划路径的长度和路标图的构建耗时差别不明显，随着采样点数量增加(N=90)，算法的规划路径长度结果和构图时间的差别逐渐增大。

由图9可知，在其他条件相同的情况下，随着采样点数量的增加，路径光滑程度逐渐改善，但路径总体趋势保持不变，说明修正PRM算法求解的路径解质量较为稳定。

图9 修正PRM算法规划结果对比

以图9(b)为基础进行路径平滑处理，得到图10，蓝色实线表示修正PRM算法规划路径，黑色空心圆形标识代表关键路点，将其作为贝塞尔曲线控制点，平滑处理后得到的路径(红线所示)更符合智能车辆行驶工况。

图10 路径平滑示意图

3.2 路径规划效率对比

为验证修正PRM算法的路径规划效率，以基本PRM算法为对比算法进行算例试验。其中，算例1为方形迷宫，算例2为窄通道。在采样点数量一致的情况下重复执行算例试验11次。算例1和算例2的仿真试验结果如图11所示，11次仿真试验规划路径长度和运行时间取均值，路径搜索成功次数占总次数的比例为成功率，见表2。

表2 算法效率对比结果

由图11可知，在算例1试验中，2种算法中落入障碍空间的采样点数量相当，但在PRM算法中自用空间内的采样点分布广泛，造成了冗余；在修正PRM算法中，采样点集中分布在空间主轴线的两侧，提高了采样点的利用率；在算例2试验中，PRM算法中大部分采样点落入障碍空间，自用空间内的采样点极少，影响了路径解的质量；修正PRM算法中采样点沿着空间主轴线分布，自用空间内的采样点数量较多为寻求更优质的路径解提供了可能。

图11 算法规划结果对比

由表2和图12可知，对于算例1试验，在采样点数量较低的情况下(N=30)，修正PRM算法不能成功规划路径；当采样点数量为60时，2种算法规划路径长度、运行耗时和成功率差别不大；当采样点增至90时，修正PRM算法规划路径长度和运行耗时明显优于基本PRM算法。对于算例2试验，在采样点数量较低的情况下，2种算法均不能成功求解路径；随着采样点数量的增加，修正PRM算法规划路径的成功率更高，且路径解的质量更为可靠。

图12 算法成功率对比

3.3 ROS仿真试验

为了进一步验证修正PRM算法的可实施性，基于ROS试验平台设计仿真试验。ROS小车的构成如图13所示。

图13 ROS小车组成

本文主要讨论二维平面环境下的智能车辆路径规划问题，使用ROS小车试验平台提供的功能包实现激光雷达构建地图的功能，试验场地如图14所示，SLAM建图效果如图15所示。基于该环境地图以ROS小车自身的定位结果为起始点，并指定目标点，执行修正PRM算法，路径规划结果如图16所示。

图14 场地情况

图15 SLAM建图结果

(a)路标图

从试验结果可知，修正PRM算法在SLAM构建的仿真的地图中，通过伪随机采样策略与双向增量式检测策略，成功建立了路标图，如图16(a)所示，并搜索出一条连通起点与目标点的可行规划路径，验证了算法的可行性。

4 结论

针对传统PRM算法应用于智能车辆路径规划存在的不足，提出两点改进策略。

(1)基于空间主轴线设计一种伪随机采样方式，引入随机增量调节采样点的波动范围，优化采样点的生成质量。

(2)采用双向增量式碰撞检测策略和邻层路点连接策略，减少碰撞检测的调用次数，提高路标图的构建速率。

使用4阶贝塞尔曲线对规划路径进行平滑处理，使其更加符合车辆行驶工况，最后利用Matlab、ROS搭建试验平台对修正PRM算法的正确性进行验证分析。试验结果表明，修正PRM算法在增强路标图稳定性、缩短规划路径长度和提高算法搜索速率方面具有明显的优势。但无论是修正PRM算法、A*算法等都是一种模型驱动，还存在着一定的局限性，结合数据驱动、云网融合技术来进行智能车辆的路径规划及避障，最终使智能车辆的技术更加成熟。