基于改进PRM算法的机器人路径规划

封 澳,杨锦宇,谢玉阳,孙延康,王璇之,肖 建*

(1.南京邮电大学 集成电路科学与工程学院,江苏 南京 210023;2.金陵中学 中美班,江苏 南京 210041;3.南京邮电大学 电子与光学工程学院、柔性电子(未来技术)学院,江苏 南京 210023)

0 引 言

机器人路径规划是机器人领域中十分关键的技术之一。现有的路径规划算法主要分为基于图搜索的算法[1-2]、基于采样的算法[3-4]、基于演化的算法[5]以及基于学习的算法[6-7]。其中,基于采样的算法主要包括快速探索随机树(Rapidly-exploring Random Tree,RRT)算法[8]和概率路线图(Probabilistic Roadmap,PRM)算法[9-10]。PRM算法由于其鲁棒性强、可扩展性好以及算法简单易实现等优点,被广泛应用在机器人导航、机械臂路径规划等领域。

近年来,研究人员对传统PRM算法进行了大量的改进研究。邱添等人通过将地图栅格化划分,根据每个栅格的威胁等级采用不同的采样策略,提高了在狭窄通道内的采样效率[11]。邹善席等人将落在障碍物中的节点重新采样,在保证采样点数目不变的条件下提高了节点的利用率[12];李琼琼等人提出了一种以空间主轴线为参照轴的伪随机采样策略,优化了采样点的生成方式,增强了采样点的导向性[13];薛阳等人使用局部敏感哈希算法构建无向路线图,加快了无向路线图的构建速度[14];程谦等人优化了采样点的连接方式,减少了无向路线图中的不合理路径[15];宁新杰等人对规划出的路径使用道格拉斯-普克算法进行峰值提取,去除了路径的冗余节点[16]。针对传统PRM算法采样点分布不均匀、路线图构建效率低以及路径冗余不平滑等缺点,该文引进Sobol序列优化采样策略,使采样点全局均匀分布;通过对节点进行分类并施加连接约束,优化路线图的构建和搜索效率;采用节点平移算法调整节点位置,使路径满足实际空间中最优路径;最后,使用贝塞尔曲线平滑路径,使其满足实际机器人的运动约束。

1 传统PRM算法

1.1 PRM算法定义

传统PRM算法的基本原理是利用随机采样策略生成一组采样点,去除落在障碍物中的采样点后,将相距满足连通距离的两个采样点相互连接建立无向路线图,通过图搜索算法在该图上寻找连通起始点到目标点的一条最短路径。传统PRM算法一般需要经过两个主要阶段:采样阶段和查询阶段。

采样阶段:PRM算法从状态空间C中随机采样N个点Xi,i=1,2,…,N,去除落在障碍物中的采样点后得到一组有效的采样点集合V;然后遍历集合中的采样点进行连通性分析,当d(Xi,Xj)≤R时,点Xi,Xj之间建立一条无向边,其中d(Xi,Xj)表示采样点Xi和Xj之间的距离,R是PRM算法的连通半径;最终PRM算法会得到一个路线图G(V,E),其中V={X1,X2,…,XM}表示有效采样点的集合且M≤N,E={Xi,Xj|Xi,Xj∈V}表示两个有效采样点之间边的集合。

查询阶段:将起始点qinit和目标点qgoal接入到路线图G(V,E)中,使用Dijkstar等图搜索算法在路线图G(V,E)中寻找连通起始点qinit和目标点qgoal之间的一条最短路径。其中,Dijkstra算法是典型基于图的最短路算法,用于计算一个节点到其他节点的最短路径[2]。

1.2 传统PRM算法存在的问题

PRM算法可以在复杂环境中进行路径规划,还可以处理多机器人或在多维空间中进行路径规划,因此在路径规划领域被广泛应用。但是传统的PRM算法通常具有以下问题:

(1)受采样点质量影响:传统PRM算法中,采样点位置通常由随机采样策略生成,这种采样策略完全随机且难以确保每个空间区域均有采样点,导致算法每次规划出的路径长度和路径质量均有所不同;并且由于不是全局均匀采样,当采样点数量较少时,会导致搜索空间不足,难以规划出有效路径;当采样点数量过多时,又会导致效率降低和计算复杂度增加,严重影响PRM算法的运行效率。

(2)路线图构图效率低下:在路线图构图阶段,对于任意有效的采样点,PRM算法均会与其周围一定距离内可相互连通的点进行连接,并作为路线图的一个边;而很多边对路径搜索并没有任何帮助,不仅占用系统计算资源,还严重影响路线图构图以及路径搜索的效率。

(3)路径冗余且不平滑:PRM算法规划出的路径由多个采样点直线连接组合而成,即使在路线图中搜索到最短路径,但在实际空间中并不一定是最优路径;并且路径上存在许多拐点,还需要对路径进行平滑处理以提升路径的行驶效果。

2 改进PRM算法

针对传统PRM算法存在的问题,该文分别从改进采样策略,改进路线图构图方式以及采用路径优化与平滑算法三方面进行改进。

2.1 改进采样策略

为了解决PRM算法采样点分布不均匀、采样质量低等问题,该文引进二维Sobol序列改进采样策略。

Sobol序列是一种基于二进制分割的低差异序列。其对于每个维度,首先将单位区间[0,1]等分为2k个子区间,并编号为0,1,…,2k-1。然后将每个子区间编号转化为k位二进制数。对于第i行二进制序列bi,1,bi,2,…,bi,k,通过将其转换为di=bi,1×2k-1+bi,2×2k-2+…+bi,k×20,计算出每行的权重系数ci=di/2k。对于第j个维度上的序列x1,j,x2,j,…,xn,j,根据权重系数可以计算出:

x1,j=cj,1
x2,j=cj,1⊕cj,2
x3,j=cj,1⊕cj,2⊕cj,3
…
xn,j=cj,1⊕cj,2⊕…⊕cj,n

(1)

其中,⊕表示按位异或运算符。由于Sobol序列中的各个维度之间可能存在相关性,为了提高序列的随机性,通常还需要对序列进行去相关处理并对该维度进行重排,从而保证生成随机数的质量。

使用随机采样策略和Sobol采样策略分别在空白地图中采样相同次数(N=16),采样结果如图1(a)和图1(b)所示。相比于随机采样策略,Sobol采样策略的采样点分布更加均匀,可以用较少的点覆盖更大的空间。将地图以横向、纵向和棋盘格等分的方式划分成多个子区域,划分结果分别如图1(c)、图1(d)、图1(e)所示。Sobol采样策略可以确保每个地图子区域中均有采样点,极大程度上解决了随机采样策略因采样点数量较少导致的搜索空间不足的问题。

图1 随机采样策略和Sobol采样策略对比

2.2 改进路线图

为了降低路线图的大小,提高路线图的构建、搜索以及算法的运行效率,该文对采样点分配邻域属性并施加连接约束。首先,将与起始点满足一定范围且具备连通条件的采样点称为第一邻域点,与第一邻域内的点满足一定范围且具备连通条件的采样点称为第二邻域点,依次类推,直至所有采样点均被分配一个邻域属性。规定起始点只与第一邻域内的点进行连接,其他采样点只能与其相邻的邻域点进行连接,同一邻域内的点不进行相互连接。在同一地图上采样相同次数(N=40),传统路线图和改进路线图构图结果如图2所示。

从图2可知,在相同采样点数量且不改变路线图连通性的前提下,改进路线图的边数明显低于传统路线图。经过测试,在相同采样点数量下,改进路线图的平均边数、平均构建耗时以及平均路径搜索耗时分别比传统路线图减少67.89%～73.09%,79.99%～80.73%和24.72%～33.84%。随着采样点数量的不断增加,两种路线图的平均边数、构建时间以及路径搜索耗时上的差距将越来越明显。

2.3 路径优化与平滑策略

针对PRM算法规划出的路径在实际空间中并非最优路径且存在较多拐点的情况,该文提出一种基于节点平移和贝塞尔曲线的路径优化与平滑策略。

首先,使用Dijkstra算法在路线图中搜索出连接起始点qinit和目标点qgoal的最短路径,路径上的点集用F={qinit,q1,q2,…,qn,qgoal}表示。对于q1到qn中的任意节点qm,将节点qm沿着qm和qm+1之间的连线平移,若节点qm与qm+1之间的连线紧切障碍物安全半径,则节点qm停止移动并将当前位置作为节点qm的最新位置更新点集F;若节点qm移动到qm+1,则删除qm节点,使节点qm-1与qm+1直接相连并更新点集F。遍历节点q1到qn,直至所有节点位置均更新完毕。

图3显示了路径优化的示意图,图3(b)中路径优化步骤如下:

图3 路径优化示意图

经过优化后的路径长度和质量均有所提升,但仍是由数个节点直线相连而成,存在着不少拐点,因此该文采用贝塞尔曲线平滑路径,使其符合实际机器人的运动约束。该文选用二阶贝塞尔曲线对优化后的路径进行平滑。路径平滑示意图如图3(c)所示。在具体实施过程中,以固定间隔对优化后的路径进行离散化,选取路径节点和距其最近的两个离散点作为贝塞尔曲线的离散控制点,经过公式2对离散点进行插值拟合,实现对路径拐点的平滑处理。

P(t)=(1-t)2P0+2P1(1-t)t+P2t2,t∈[0,1]

(2)

图3(d)展示了原始路径与优化路径的对比结果,经过节点平移和贝塞尔曲线优化后的路径更符合实际机器人的运动约束,且路径长度比原始路径减少11.71%。

2.4 改进PRM算法的步骤

改进PRM算法的步骤如下:

步骤1 PRM算法参数初始化:机器人起点、目标点、采样点数量N、连通半径R和路径离散间隔L。

步骤2 二维Sobol策略生成N个采样点,按照采样点生成顺序依次去除落入障碍物中的无效采样点,剩余有效采样点作为路线图的节点。

步骤3 对有效采样点分配邻域属性,对相邻邻域内的采样点进行距离和连通性分析,符合连通条件则进行连接作为路线图的一个边。

步骤4 使用Dijkstra算法在路线图中搜索连通起始点和目标点的最短路径。

步骤5 对搜索出的最短路径使用节点平移优化算法,将最短路径上的节点进行平移,找到符合实际空间中最优路径。

步骤6 按照固定间隔对路径进行离散化,选择优化后的节点和距其最近的两个离散点进行二阶贝塞尔曲线插值,实现路径的平滑处理。

改进PRM算法流程如图4所示。

图4 改进PRM算法流程

3 改进PRM算法的仿真与结果分析

为了验证改进PRM算法的有效性和可行性,对传统PRM算法、文中改进PRM算法和其他改进的PRM算法在Python环境中进行了仿真实验,所用计算机参数为Intel(R) Core(TM) i7-7700HQ CPU @ 2.80 GHz,内存16 GB,64位Window10操作系统。改进PRM算法初始化参数分别设置为:连通半径R=100,路径离散间隔L=10。

3.1 传统PRM算法比较结果分析

在320×240的状态空间中,随机设置机器人的起始点和目标点,传统PRM算法和改进PRM算法分别在状态空间中以不同采样数量进行采样,采样点和路径对比结果如图5和表1所示。

表1 不同采样点数量下的算法对比结果

图5 不同采样点数量下算法规划结果对比

从图5可以看出,当采样点较少时(N=30),传统PRM采样点分布不均匀,部分地区存在采样点密集或稀疏的情况,且规划出的路径存在很多曲折和拐点;随着采样点数量的增加(N=90),传统PRM算法的采样点分布逐渐改善且规划出的路径曲折程度和路径长度也有所优化。在不同采样点数量的情况下,改进PRM算法的采样点分布均匀,不存在采样点稀疏或密集的情况,且随着采样点数量的增加,改进PRM算法规划的路径长度及位置几乎不发生变化,说明改进PRM算法的采样点和规划的路径质量更为稳定。

从表1可知,在采样点数量分别为30,60和90时,改进PRM算法的耗时比传统PRM算法的耗时分别减少53.8%,72.7%和74.3%;且当采样点数量较少时,算法规划的路径长度差别明显,随着采样点数量的增加,算法规划的路径长度差别逐渐减少;说明改进PRM算法在运行效率上明显优于传统PRM算法,且在采样点数量较低时也能规划出合理路径。

3.2 改进PRM算法泛化性分析

为了验证改进PRM算法的泛化性,该文在不同测试的地图上以(30,30)为起点、(row-30,col-30)为目标点进行测试,其中(row,col)为测试地图的大小。在每个测试地图上分别运行100次,路径长度和运行时间取多次运行的均值,路径规划成功次数占总运行次数的比例为成功率。测试地图由图7可知。

由表2可知,在三个不同的测试地图中,当采样点数量较少时(N=30),两种算法的运行成功率随着地图面积的增加而逐渐减低,改进PRM算法在路径长度和运行时间上优于传统PRM算法;当采样点数量增加到90时,两种算法的运行成功率均明显提高,改进PRM算法在提升率上快于传统PRM算法;随着采样点数量的增加,改进PRM算法的路径长度几乎不变,传统PRM算法的路径长度减少,两种算法之间的路径差距降低;虽然运行时间均有所增加,但改进PRM算法在运行时间上明显优于传统PRM算法。

表2 不同地图下算法对比结果

3.3 其他改进PRM算法比较结果分析

为了进一步验证改进PRM算法的性能,与其他改进PRM算法在测试地图中以成功率、平均路径长度和平均运行时间为评价指标进行测试。其他改进PRM算法介绍如下:

改进PRM算法1:宁新杰等人提出的改进PRM算法[16],该算法在传统PRM算法中加入边集优化方法对采样点进行约束,避免距离较近的采样点相互连接,减少路线图构图的大小;并对规划出的路径使用道格拉斯-普克算法进行峰值点提取,去除冗余节点。

改进PRM算法2:李琼琼等人提出的修正PRM算法[13],该算法设计了一种以空间主轴线为参照轴的伪随机采样策略,采用邻层连接策略并使用双向递增方法进行碰撞检测,优化碰撞检测调用次数,最后对规划出的路径使用四阶贝塞尔曲线进行平滑处理。

将传统PRM算法和三种改进PRM算法在测试地图中分别以不同采样点总数测试100次,比较其成功次数,测试结果如图6所示。

(a)测试地图1 (b)测试地图2 (c)测试地图3

从图6可以看出,由于改进PRM算法1继续沿用了传统PRM的随机采样策略,因此,该算法在三种测试地图上的成功率与传统PRM算法接近;改进PRM算法2优化了采样点的生成方式,但这种采样策略只是在空间主轴线附近均匀采样,并非全局均匀采样,因此,该算法在大多数测试情况中的运行成功率比其他测试算法的低;该文改进的PRM算法采用二维Sobol序列改进采样策略,使采样点均匀分布在状态空间中,在运行成功率上优于其他测试算法。

将不同改进PRM算法以相同采样点数目(N=90)分别在测试地图1,2和3中测试100次,平均路径长度和平均运行时间对比结果如表3所示。为了更直观地展示不同改进PRM算法之间的对比结果,其中一次规划路径的结果如图7所示。

表3 不同改进PRM算法在测试地图下的对比结果

从表3和图7可以看出,改进PRM算法1使用道格拉斯-普克算法去除冗余节点,优化后的路径长度与文中改进的PRM算法的路径长度接近,但其并未对路径进行平滑处理,不符合实际机器人的运动约束;改进PRM算法2使用四阶贝塞尔曲线对路径进行平滑处理,但其并未改变路径节点的位置,导致规划出的路径长度并非最优;文中改进的PRM算法使用节点平移算法优化节点位置,并使用二阶贝塞尔曲线对路径拐点进行平滑处理,在保证路径平滑的同时维持了较短的路径长度。

传统PRM算法最耗时的部分为构建无向路线图,改进PRM算法1在构建无向路线图时加入了边集优化方法,避免距离较近的采样点相互连接,减少了构图的大小,但其优化性能有限,在三种优化算法中耗时最长;改进PRM算法2在构建无向路线图时采用邻层连接,并使用双向递增方法进行碰撞检测,进一步缩短了算法的运行时间;文中改进的PRM算法对所有采样点进行邻域分类,并规定相邻邻域采样点相互连接,相同邻域采样点不进行连接,减少了与路径规划无关的边被加入到无向路线图中的概率,算法的运行时间在三种优化算法中最短。

通过上述实验可以得到以下结论:

(1)采用二维Sobol序列优化采样策略,用较少的采样点覆盖更多区域,保证全局均匀采样,重复运行时算法规划出的路径长度波动更小,更为稳定;

(2)对全局采样点进行邻域分类并施加连接约束,在保证路线图连通性的前提下减少了构图及路径搜索的时间,提高了算法的运行效率;

(3)使用节点平移优化算法优化路径节点位置,并使用贝塞尔曲线平滑路径拐点,降低了规划路径受采样点质量的影响,保证了较短的路径长度,有效提升规划路径的质量;

(4)在测试地图上进行的大量实验结果表明,相比于传统PRM算法和其他改进PRM算法,文中改进的PRM算法在路径长度、运行时间以及成功率上拥有更好的表现。

4 结束语

针对传统PRM算法采样点分布不均匀、路线图构图效率低以及路径冗余不平滑等缺点,分别采用二维Sobol序列改进采样策略,优化采样点分布;通过对节点进行邻域分类并施加连接约束,提高路线图的构图和搜索效率;最后采用节点平移优化算法去除冗余路径并使用贝塞尔曲线平滑路径拐点,提高PRM算法规划路径的质量。在不同大小和布局的测试地图中进行的大量仿真实验表明,相比于传统PRM算法和其他改进PRM算法,文中改进的PRM算法在路径长度、运行时间以及成功率上具有明显优势。