改进概率路标图算法
2021-12-23宁新杰徐照翔李兴广陈鹏宇
宁新杰，崔 炜，徐照翔，李兴广，陈鹏宇
(长春理工大学 电子信息工程学院，吉林 长春 130022)
0 引 言
目前，常用的路径规划算法有遗传算法[1]、人工势场法[2]、随机路标图(PRM)算法[3]等算法。当环境中的障碍物较为复杂，或在高维空间进行规划时，将导致规划算法计算量大大增加，算法效率大大降低。基于随机采样的PRM算法可以有效解决高维空间或多障碍物的复杂环境中的路径规划问题，因此，PRM算法广泛应用于在机械臂路径规划[4]、建筑物疏散路径规划、分支管路自动布局[5]等领域。
为了提高PRM算法中路线图的连通性，Mali等提出应用多重样本的节点和路线图的有效性[6]。Janso L等将随机采样序列替换为某种确定性采样序列，用以实现PRM算法搜索到路径的渐进最优[7]。杨岱川等对带有实际地图障碍物的多个目标点进行先后排序和路径搜寻，较好地解决了目前存在的移动处理器响应速度慢和不能一次规划多个目标点路径的问题[8]。李敏等提出一种定量计算自由空间障碍物数量稠密度的方法来解决窄通道问题[9]。SANTIAGO等将遗传算法与PRM算法进行结合，用来辨别PRM算法产生的随机点，但该混合算法规划的航迹收敛速度慢，只适用于特定场景[10]。然而，大部分学者的学者未将改进方法的计算量考虑在内，造成时间利用率降低，延长规划到可行路径的时间，本文使用边集优化方法，通过对随机点施加约束，对传统PRM算法进行改进，对搜索到的路径进行峰值节点提取，形成一条更利于机器人行走的路径，并通过仿真实验验证改进PRM算法的可行性。
1 传统PRM算法
PRM算法全称是概率路标图算法(probabilistic roadmap)，是一种基于图论的搜索算法，包含学习阶段和查询阶段两个阶段。
1.1 学习阶段
在学习阶段，路线图的数据结构是依赖概率的方式为给定场景构建的，路线图是一个无向图R=(N,E)，N代表在自由空间Cfree上所生成随机点集合，E代表连接任意两个节点的可行路径的集合。首先使用足够的随机点来提供一个相当均匀的自由空间Cfree覆盖，这些随机点构成点集N，然后从当前的N中选择多个节点ni(i=1,2,3,……)构成邻近集Nc。使用局部规划器计算ni与ni+1路径，如果路径与障碍物碰撞，则舍去，反之，则将路径(边)加入到E中。学习阶段构建路线图R供查询阶段使用。
1.2 查询阶段
在查询阶段，路线图被用来解决输入场景中的单个路径规划问题。尝试将源点s目标点g分别与N中的节点si,gi进行连接，如果连接成功，在路线图R=(N,E)中从E中搜索连接si,gi的边。通过重复计算符合要求的局部路径，并将它们连接起来，最终，利用搜索算法在起点、路线图R=(N,E)，PRM算法的算法效率取决于查询阶段的搜索速度，也就是说查询阶段的工作量决定算法的效率。然而，通过传统学习阶段构建的路线图较为复杂，使用寻路算法进行搜索会耗费大量时间，且生成的路径含有较多冗余节点。
2 PRM算法的局限性及相应的改进方法
2.1 PRM算法的局限性
传统路线图R(N,E)中的边集E是通过每一个随机点与其余一定范围内随机点连接后，经过可行性边检测后，除去与障碍物相交的边，最终得到的边集放入E中。然而，通过这种方式得到的边集，在查询阶段使用搜索算法搜索时耗费的时间较多，从而增加了PRM算法的运行时间[11]。某随机点n周围较小范围内可能存在多个随机点hi，将n与hi连接后构成边集E后，在一定程度上，增大了后续查询阶段寻路算法的计算量，延长了寻路所用时间[12]。由于这些随机点与该随机点的连接构成边集E的一部分，因此，在查询出的路径中可能在较短距离内发生转折，这会导致机器人在行驶过程中频繁的转向，不利于机器人的能耗节省。
2.2 边集优化方法
针对于某随机点周围较近范围内存在较多随机点会增加计算量且搜索出来的路径中很短距离发生转折的问题，采用对随机点进行约束的方法，对于存在于自由空间Cfree的采样点，以其位置p为圆心，半径为r1,r2分别绘制一个圆形区域，仅将圆心与圆环内随机点进行连接，并对连接的边进行障碍物检测，判断各边是否与障碍物相交，如果相交，则去掉相交的边，如果不发生碰撞，则将其加入到该点的边集中。具体如图1所示，白色区域代表自由空间Cfree，黑色的小圆代表随机点(将随机点作为圆心点后进行适当放大)，黑色的椭圆形区域代表障碍物，在500*500的障碍物地图中选取某随机点B(XB,YB)，并以其为圆心，以r1,r2为半径画圆，且仅将该圆心点与圆环内的随机点Xi进行连接，进行障碍物检测，去除碰撞边后加入到边集E中。该随机点B(XB,YB)的边集应满足以下条件：①点B与点Xi的欧氏距离小于半径r1，且大于r2(r1大于r2)；②点B，点Xi是位于自由空间Cfree中的点；③根据所需的机器人行走步长设计r1与r2的大小，r1与r2的差值应不小于30(500*500地图上)；④当约束外径一定时，随着约束内经的增大，算法的时间复杂度逐渐减小。当约束内经一定时，随着约束外径的增大，算法的时间复杂度逐渐增大。
图1 某圆心点的边集E
2.3 峰值节点提取
传统方法PRM算法查询出的路径容易在一定范围内发生多次转折，此外，边集优化方法也会产生一些冗余节点，这会导致路径节点增多，路径长度增加，机器人行驶时间延长的问题。针对这些问题，文中采用道格拉斯-普克抽稀算法[13]对查询阶段搜索到的路径提取关键节点进行二次优化，是用来对冗余节点进行压缩后提取必要节点。提取关键节点的步骤如下：连接起点、终点得到一条准线，之后计算path中除起始点和终点外，剩余结点到准线的欧式距离，选取欧式距离最大的峰值结点，比较其与阈值φ的大小，如果其小于阈值φ，则舍去起点和终点之间的所有点，如果最大距离大于φ，则保留峰值节点，并将原线分割成两部分，连接起点和峰值节点及峰值节点和终点，并对起点和峰值节点及峰值节点和终点之间的节点重复上述方法。抽稀结果中的节点数目随着选取阈值φ的增大而减小，使用时根据所需精度来选取合适的阈值，来获得最好的结果。
以图2中路径将道格拉斯-普克算法总结为以下几步：
(1)如图2(a)所示，使用虚线连接起点A(x1,y1)和终点H(x2,y2)，形成一条基准线AH，将式(1)和式(2)进行联立求出a和b的值(a、b值为设一次函数方程时参数，a、b为常数)使用式(3)找到距基准线最大距离Dmax的峰值节点C(x0,y0)。
(2)如图2(a)所示，过点C向基准线做垂线交AH于点M，使用式(3)求出CM长度为d，如果φ大于d，则舍去B,C,D,E,F等点。如果φ小于d，则用虚线连接AC，HC。
(3)对AC，HC这两条新的基准线重复步骤(1)，步骤(2)的操作。
(4)如图2(c)将路径起点、每次找到的峰值节点和终点连接起来，最终路径为ACEH
图2 道格拉斯-普克算法提取峰值节点
ax1+y1+b=0
(1)
ax2+y2+b=0
(2)
(3)
改进后的PRM算法，在学习阶段构建线图时，选择合适的约束半径r1=120，r2=50，仅将圆心处的随机点与半径为r1和半径为r2的两个圆形成的圆环内的随机点连接起来如图1所示，然后通过障碍物检测将无碰撞边加入到E中，通过这种方式建立好路线图后通过A*算法在路线图中搜索到一条连接起点和目标点的无碰撞路径，使用道格拉斯普克算法对无碰撞路径进行峰值节点提取操作，形成一条更优路径，易于满足机器人行驶需求。
改进后的PRM算法的优点：①在学习阶段使用约束圆环，会大幅度的简化查询阶段搜索集合E中边的数量，构造的路线图R较传统方法构建简单，一定程度上降低后续查询阶段搜索算法搜索到路径所用时间；②增加了查询阶段判断搜索是否会成功，决定PRM搜索是否成功在于起点和目标点能否在一个路线图中。增加这个判断是为了减少规划失败时浪费在查询阶段的时间；③使用道格拉斯-普克算法对PRM算法查询阶段得到的无碰撞路径进行峰值节点提取，有效去除冗余节点。
改进后的PRM算法流程如图3所示。
图3 改进后的PRM算法流程
改进后的PRM算法的伪代码：
Input:
n:the number of nodes to put in the roadmap
r1,r2: the radius of two circles
Output:
a roadmap R=(N,E)
(1)N←∅,E←∅
(2)while |N| < n do
(3)loop
(4)c← a randomly chosen free configuration
(5)if c is collision then
(6)b = RandomNode (c,r)
V←V ∪ { b }
(7)else V← V ∪ { c }
(8)end if
(9)end while
(10)for all c ∈ V do
(11)Nc← a set of candidate neighbors of c chosen
from N
for all v ∈ Nc， in order of increasing D (c,v ) do
(c,v) then
(13) if D(c,n)
(14) update Rs connected components %%更新
(15) end if
(16) end for
(17)end for
(18)R = (N,E)
(19)path=AStaP(E) %%搜索路径
(20) path=ARrecurisionF(pointsTab,path,Thres-hold) %%设定阈值，加入起点，目标点后进行峰值节点提取
(21)return path
3 仿真验证
仿真实验在电脑处理器为Intel(R) Core(TM) i5-5200U，安装内存为8 GB，64位操作系统Windows 8.1上的matlab2016b中进行。通过对比了传统的PRM算法和改进后的PRM算法在简单地图与复杂地图下的时间特性和对应的路径节点个数来验证改进方法的可行性和有效性。
在500*500的障碍物地图中，设定机器人起点为(10,10)，目标点为(490,490)，黑色椭圆图形代表障碍物，线条代表可行性边，白色空间代表自由空间Cfree，原点(0,0)在图像左上角，竖直向下为x轴正方向，水平向右为y轴正方向。随机选取100个随机点构成点集N，加入起点和终点，并将其与路线图在约束条件下连接起来，图4(a)中约束半径为210，图4(b)中采用边集优化方法设定约束外径为r1=210，约束内经r2=140。
图4 简单地图中传统PRM算法和 改进PRM算法路线对比
当随机点为100个或200个时，设定简单地图中约束内径为140，约束外径为210，复杂地图图中约束内经为80，约束外径为120。由图4可知，在障碍物已知环境中，使用边集约束方法对随机点的边集施加约束，较传统PRM算法得到的路线图，改进后的PRM算法得到的路线图更加简单，使得PRM算法在查询阶段用时更少。实验数据见表1，传统PRM算法和改进后的PRM算法在不同随机点数情况下在简单地图和复杂地图上的运行时间。实验结果表明，当随机点个数为100时，在简单地图和复杂地图中，改进后的PRM算法运行时间较传统PRM算法分别减少约10.64%、6.75%。当随机点个数为200时，在简单地图和复杂地图中，改进后的PRM算法运行时间较传统PRM算法分别减少约9.68%、7.17%。改进后的PRM算法能在已知障碍物的环境中更加快速规划出一条无碰撞路径，减少规划所用时间，对机器人的实时性有所提高。
为验证相同约束外径，不同约束内经时，改进PRM算法的时间特性。首先，设定相同的约束外径r1=150，同时设定约束内经r2从20至120逐渐变化，使用该边集约束方法对PRM算法进行改进后运行在简单地图上的运行时间数据见表2，运行时间数据是通过运行5次程序取中位数的方法得到的，从表2中数据可以看出，随着约束内径的不断增大，算法的运行时间在不断减少。这说明边集约束方法是切实可行的，对减少算法的运行时间是有效的。改进后 的PRM算法较传统的PRM算法运行时间大大降低，在保证一定路径搜索成功率的情况下，选择合适内外约束半径，表1中改进算法的时间特性还可以继续提升。
表1 成功规划到路径时间对比
表2 简单地图相同约束外径，不同约束内径运行时间对比(随机点为100个)
选取同一组随机点，且随机点数目为100个的简单地图上，图5(a)表示在传统的PRM学习阶段构建的路线搜索出来的无碰撞路径，图5(b)表示使用边集优化方法对PRM学习阶段构建的路线进行简化后，使用峰值节点提取对路径进行再优化后查询出一条无碰撞路径如图5(b)所示。从简单地图 中可以看出，采用传统方法查询出的路径中，有的节点之间在较短距离内发生多次转折，如果机器人沿着该路径进行前进会发生频繁转向，不利于机器人的控制，采用改进方法的PRM算法查询出的路径中节点数目较少，机器人行驶时转向次数较少，更利于机器人的控制及能量节省。
图5 简单地图传统PRM算法和改进PRM算法路径对比
选取同一组随机点，且随机点数目为100个的复杂地图上，图6(a)表示在传统的PRM学习阶段构建的路线搜索出来的无碰撞路径，图6(b)表示使用边集优化方法对PRM学习阶段构建的路线进行简化后，先查询出一条无碰撞路径，再使用峰值节点提取对路径进行再优化。从复杂图中可以看出，搜索到的无碰撞路径，改进方法较传统方法发生转折次数更少，且节点间在较短距离内不会发生转折，路线更加平滑，充分验证了峰值节点提取的必要性和有效性。
图6 复杂地图传统PRM算法和改进PRM算法路径对比
通过将边集约束方法应用到传统PRM算法成功查询出一条无碰撞路径后，设置合适的阈值φ，将生成的路径进行峰值节点提取，经过在简单地图和复杂地图仿真实验后，通过图5(a)、图5(b)、图6(a)、图6(b)得到的可行路径数据可以得出表3数据。在简单地图中，传统PRM算法初始路径的节点个数为7个，设置阈值为38后，改进PRM算法初始路径节点个数为4个，优化节点个数为3个。在复杂地图中，传统PRM算法初始路径的节点个数为17个，设置阈值为25后，改进PRM算法初始路径节点个数为10个，优化节点个数为7个。无论是简单地图还是复杂地图，较传统PRM算法查询出的无碰撞路径中节点数目，改进PRM算法后得出的路径节点更少，路径更加平滑，更利于机器人的行走。
表3 改进PRM初始路径与优化路径实验数据对比
4 结束语
针对于传统PRM算法查询路径所需时间较长及生成的初始路径中节点较多等问题，本文通过对点集N中的随机点采用内外径约束的边集优化方法，以减少生成的边集E的大小，从而减少查询阶段的计算量，最终将峰值节点提取方法应用于路径的再优化中，可以有效减少路径中的节点数目。仿真结果表明，相比于传统PRM算法，改进后的PRM算法具有计算量小，实时性好，路径中节点数目少等优点，对机器人使用PRM进行路径规划的后续开发有一定参考意义。
