基于PRM改进的路径规划算法*

邹善席,王 品,韩 旭

(1.中国科学院大学，北京 100049；2.中国科学院沈阳计算技术研究所高档数控国家工程研究中心，沈阳 110168；3.沈阳高精数控技术有限公司，沈阳 110168)

0 引言

路径规划是机器人完成各种任务的基础，是机器人研究领域的核心问题。所谓的路径规划是指按照一定的性能指标，机器人从所处的环境中搜索出一条从起点位置到终点位置，可以避开障碍物的最优或次优路径[1]。20世纪90年代M H Overmars等提出的Probabilistic Roadmap Method[2]。此法是基于采样算法的一种，很好的解决了在高维空间中构造出有效路径图的问题，其复杂度主要来源于搜索路径图的难度，场景的大小和空间的维数对其影响不大，具有计算量小，实时性好的优点。

传统的PRM算法[3]采用完全随机的采样策略，当采样点数量一定时，一些采样点落在障碍物空间内，导致自由空间中的采样点数量减少，可能无法完成路径的规划，传统的PRM算法要想解决这个问题，普遍重新采样，增加空间中的采样点，重新构建路径图，但是，采样点增多，会使路径规划的运算量成指数级增长，所用规划时间大幅度上升，这与路径规划对实时性的要求相违背，且完全随机的采样策略会导致规划出的路径与理想路径差异较大。针对传统PRM算法上述缺点，文中引入随机节点生成函数和改进的节点增强法，随机节点生成函数生成新节点取代落在障碍物中的节点，改进的节点增强法优化原始路径[4]。改进后的PRM算法，极大提高了采样点的利用率，减少了原始路径上节点数目，缩短路径长度，有效提高了路径的平滑度。

1 传统PRM算法

1.1 离线学习阶段

传统PRM算法[5]分为两个阶段，离线学习阶段和查询阶段 。离线学习阶段主要内容为，采集一定数量的节点，就是在规划空间内均匀选取N个节点，之后连接各节点，并去除与障碍物接触的连线，由此得到一个无向路径网络图G=(V,E)，其中V代表节点集，E代表所有可能的两点之间的路径集。

1.2 查询阶段

查询阶段主要任务是根据学习阶段构造的无向网络路径图，采用某种搜索算法,如A*算法，广度优先搜索算法等等，在起点和终点之间搜索出无碰撞的路径。

2 改进的PRM算法

从上述PRM算法描述中，采样的随机点对构建无向网络路径图的质量起着决定性作用，针对在路径图中节点分布点较少时，不能规划出路径的问题，提出一种改进后的PRM算法，其与传统PRM算法不同之处有以下两点：

① 在离线学习阶段，对空间内的采样点位置进行碰撞检测，若采样点落在障碍物内，则舍弃，反之，加入到无向路径网络图中。

② 在查询阶段，对规划出来的路径采用改进的节点增强法进行优化，减少原始路径中节点数目。

2.1 随机节点生成函数

对于落在障碍物空间Cobs内的采样点，使用一个随机节点生成函数RandomNode(p,r)，生成新的节点替换障碍物中的节点，p代表节点位置，r代表半径[6]，具体如图1所示，灰色区域代表障碍物，以障碍物中的节点A为圆心，根据机器人所处的环境，采用合适的半径r做虚线圆，则该虚线圆上每一个属于自由空间内的节点，都可能成为A的替换节点，节点B由随机节点生成函数RandomNode(p,r)随机生成，且节点B(xB,yB)满足以下条件:

② 节点B是自由空间内的节点。

图1 随机节点生成方法图

2.2 基于改进的节点增强法路径优化

基本节点增强法[7]即采用新增节点依次替换原路径节点，减少原路径节点数目，达到缩短路径长度的目的。该方法具体如图2所示。

图2 基本节点增强法示意图

对线段AB进行碰撞检测，若发生碰撞，采用二分法，记录AB的中点C为碰撞点，然后以C为垂足做AB的垂线，在AB两边的垂线上轮流以h的长度，寻找第一个节点D，能使AD和BD满足碰撞检测。

为了减少基础节点增强法在寻找D点时的难度和计算量，采用以下改进，具体如图3所示。

图3 改进节点增强法示意图

改进后的节点增强法，以发生碰撞的线段AB为直径做圆,使用随机节点生成函数RandomNode(p,r)，生成C节点，使AC和BC满足碰撞检测，此法增大了可选择的C点范围，避免了反复以h长度计算C点，减少了计算量。使用改进后的节点增强法对路径进行优化，具体实现如图4所示。

图4 改进后的节点增强法路径优化

图4中虚线为优化前的路径，实线为优化后的路径，从右向左，7个节点依次为A1、A2、A3、A4、A5、A6、A7，采用改进后的节点增强法，从A1开始向后搜索，进行碰撞检测，线段A1A4发生碰撞，调用RandomNode(p,r)生成随机节点B2(A1记为B1)，取代A2、A3，以B2为端点继续向后搜索，线段B2A7发生碰撞，同上，生成随机节点B3，取代原路径中A4、A5、A6、A7记做B4，则优化后的路径为B1B2B3B4，可以发现优化后路径节点数量减少，路径更接近最优路径。改进后PRM算法伪代码如下：

Input:

n:number of nodes toput in the roadmap

r:a radius of the circle

Output:

a roadmap G=(V,E)

①V←Ø，E←Ø

②while|V|

③loop

④ c← a randomly chosen free configuration

⑤ if c is conllision then

⑥b=RandomNode(c,r)

V←V∪{b}

⑦ else V←V∪{c}

⑧ end if

⑨end while

⑩for all c∈V do

from V

for all v∈Vc ,inorder of increasing D(c,v) do

(c,v) then

3 仿真结果及分析

为了验证改进PRM算法的正确性和有效性，采用了MATLAB构建的实验环境[8]，环境设置为70×70的带障碍物[9]区域，分别对传统PRM算法[10]和改进PRM算法进行了仿真，设置起始点坐标为(0,0),目标点坐标为(70,70)，规划一条从起始点到目标点的有效路径，随机采样60个节点，半径r取值10，具体仿真结果如图5所示。

图5 仿真路径图

图5中虚线路径为传统PRM算法生成，实线路径为改进PRM算法生成，从图5中可以看出改进PRM算法生成的路径更优，相对于虚线路径更短，拐点也更少，较接近最优路径[11]。

表1为机器人在上述环境中使用传统PRM[12]算法和改进PRM算法分别进行50次路径规划，记录每次机器人到达目的地所使用时间以及本次规划的路径长度，50次规划结束后，求出平均路径长度和平均规划时间(s)。

表1 50次仿真实验数据

由表1中数据我们可以得出，在采样节点较少时，改进PRM算法提高了路径规划的成功率，大幅度减少了路径上节点个数，降低了路径长度，所用规划时间有较大幅度降低，证明改进PRM算法具有较好的有效性。

4 结论

文中以改进传统PRM算法采用随机采样策略造成采样节点分布不均匀以及不容易规划出路径为目的，采用了基于PRM的改进算法。该方法通过引入随机节点生成函数RandomNode(p,r)，有效的解决了节点分布不均匀造成的有效节点较少的问题，然后再引入改进的节点增强法，有效解决了原始路径节点过多，路径过长的问题。相比传统PRM算法，改进后的PRM算法不仅具有计算量小，实时性好的优点，还能在采样点较少的情况下，规划出次优路径，所以在移动机器人路径规划方面具有一定的应用价值。