移动机器人的栅格概率路径图法路径规划

邱添，张志安，王海龙

(南京理工大学机械工程学院，江苏南京 210094)

0 前言

近年来，移动机器人被广泛应用于工业、娱乐、军事等领域，作为机器人研究领域核心问题的路径规划也成为研究热点。路径规划算法根据其搜索路径的方式不同，大体可以分为三类：基于图论的路径规划算法，基于采样的路径规划算法和智能路径规划算法。基于图论的经典算法包括Dijkstra算法[1]和A*算法[2-3]等。智能路径算法的代表包括人工势场法[4]、遗传算法[5]、蚁群算法[6]等。基于采样的路径规划算法主要为快速搜索随机树算法[7](Rapidly-Exploring Random Trees，RRT)和概率路径图算法(Probability Roadmap Method, PRM)。

概率路径图算法在地图中进行有限数量的采样，随后在这些点间建立连接，再使用基于图论的方法规划路径，极大地减少了运算量。但是PRM算法的缺陷在于：(1)算法很难在狭小的通道中进行采样，若是增加采样数量，算法计算量会呈指数形式增长；(2)PRM算法生成的路径常带有大量折角，不符合移动机器人在实际运动时的约束。

针对这些问题，BOOR等[8]提出了一种基于高斯分布的采样方法，增强了在障碍物附近的采样概率，提升了采样效率。但是该算法对采样点是“挑剔”的：只有当一个采样点本身无碰撞且周围有障碍物时，该采样点才会被添加到地图中，该方法在障碍物附近采样时的耗时为随机采样的4倍，在复杂地图上难以满足实时性要求。HSU等[9]提出一种桥型测试，增加将点放入狭窄通道中的概率以提升算法可靠性。孙凤池等[10]提出了智能概率图算法(SPRM)，在障碍物周围建立局部栅格地图，并且进行路径平滑，提升了路径质量，但是依靠障碍物建立栅格也需要额外的复杂度，因此算法实时性较差。RAVANKAR等[11]提出了HPPRM算法，该算法将人工势场法与PRM算法融合，提升了在狭窄通道的通过率，但是需要额外的时间计算势场力。宋宇、王志明[12]将角点检测加入了PRM算法，直接针对障碍物采样，提升了窄道通过率，但是角点检测需要对整个地图作卷积，这需要额外的时间复杂度，并且角点检测对圆形等没有角点的障碍物效果不明显。CAO等[13]改进了采样器，选择分布在障碍物内部的采样点，按距离均匀采样，从而增加狭窄通道内的采样点数量，但是该方法在简单地图上的优化效果较好，在复杂地图运算量较大。

本文作者提出一种栅格概率路径图算法(GridProbability Roadmap Method, GPRM)，将整个地图划分为不同区域，根据区域的不同使用不同的采样策略，提升采样的效率,减少安全区域大量无效的采样，增加狭窄通道内的采样，从而提升算法的实时性和可行性;在采样后，优化连接方式，减少了路径长度；提升路径平滑度，使路径符合移动机器人实际的运动约束，提升路径的安全程度。

1 GPRM算法原理

1.1 经典PRM算法

经典PRM算法分为两个阶段：学习阶段和查询阶段。在学习阶段，建立空的无向图G=(V,E)，通过采样器在给定的构型空间C上随机且均匀地采样n个点，采样点落在自由空间Cfree保留，将点加入V，落在障碍空间Cobs则重新采样，直至采样点数量达到n。采样结束后，连接器将遍历节点集V，建立点与点之间的连接。如果点与点之间没有障碍物，则认为两个点相通，计算并保存两点之间的距离，将这条边加入集合E。图1展示了学习阶段生成的采样图和连线图。

查询阶段使用图搜索算法搜索出一条最短路径，通常使用Dijkstra算法或A*算法，最后得到一条从起点到终点的路径。

其中，查询阶段花费时间较少，影响算法整体效率的部分主要在学习阶段。由于要遍历所有点，进行碰撞检测和距离计算，最耗时的部分在学习阶段连接器建立连接的过程，时间复杂度为O(n2)。因此，采样点的数量直接关系到算法的运行速度。

大多数情况下，相对少的采样点足以覆盖大多数可行空间，可以保持算法的实时性，但是随着环境变复杂，或者采样点分布不合理，算法可能出现无解的情况。如图2(a)所示，图中的采样点数量为50，由于采样点分布不合理，无法形成从起点到终点的连通路径。这种情况常常发生在狭窄的通道处，通过增加采样点可以解决办法，但是算法运行时间会呈指数形式上升。图2(b)中采样点数量为100，形成了连通终点到起点的连通路径。

图2 不同采样数量的路径图

1.2 GPRM算法原理

经典PRM算法采用随机采样的方式，在狭窄通道处采样非常困难，而大片无障碍的连通区域往往会堆积较多数量的采样点，这些采样点对优化路径的意义较小，并且会增加计算量。因此，GPRM算法主要进行了3点改进：(1)用栅格划分构型空间，根据栅格内障碍物的面积，划分栅格类型，并基于栅格的类型采取不同采样策略，增加在障碍物附近尤其是狭小通道附近的采样，减少连通区域的采样；(2)在查询阶段建立点与点的连接时，不再遍历所有节点，每个节点只和附近栅格的节点进行连接，减少运算量；(3)建立路径后，使用算法平滑、优化路径。

1.2.1 栅格划分方法

在学习阶段前，增加预处理阶段，假设构型空间C大小为ncol×nrow,设定栅格缩放系数k(k>1)，栅格的边长l通过公式(1)得到，栅格的数量n通过公式(2)得到，其中ceil()为向上取整运算。

(1)

(2)

划分栅格后，通过对栅格区域的数值求和计算每个栅格中障碍物占的比例。如果栅格完全处于自由空间Cfree中，将栅格划分为安全栅格；如果栅格完全处于障碍物Cobs中，则将栅格划分为威胁栅格；否则，将栅格划分为阻碍栅格，若栅格中的障碍物占比大于一半，记为威胁阻碍栅格，否则记为安全阻碍栅格。

1.2.2 采样策略

在学习阶段，采样将基于栅格类型进行。将构型空间C采样的总数mtotal平均分给每个栅格，每个栅格的采样数量mgrid=mtotal/n，当mgrid不为整数时，且障碍物小于栅格面积的一半时mgrid向上取整，否则向下取整。本文作者将一个栅格上、下、左、右4个栅格称为它的相通栅格，将一个栅格上、下、左、右、左上、左下、右上、右下的8个栅格称为它的相邻栅格，如图6所示。对于安全栅格，如果它的相通栅格均为安全栅格，则只在栅格的中心采样一次便开始下一个栅格的采样，否则在栅格内随机采样。这样可以避免在自由空间Cfree内的频繁采样，减少计算量。对于威胁栅格，将分配给该栅格的采样点随机分给相邻的阻碍栅格，如果它的相邻栅格均为阻碍栅格，则不采样。对于阻碍栅格，进行随机采样，得到采样点Ps(xs,ys)，Ps落在自由空间Cfree中则保留；落在障碍物Cobs中，若该栅格为安全阻碍栅格则重新采样，若是威胁阻碍栅格则利用障碍物内采样法进行重采样，保留重采样点Pd。

图6 可行解与最优解

阻碍栅格内的障碍物内采样方法步骤如下：首先在栅格内提取障碍物边缘点，得到障碍物边界的点集B，然后遍历点集B，得到离采样点最近的边缘点Pb(xb,yb),接着计算出Ps和Pb之间的欧氏距离d，沿着PsPb延长一个随机距离dr(0

图3 障碍物内采样示意

这种采样方法有利于在狭小的通道内进行采样，能充分利用采样失败时获得的数据，提升采样效率，从而有效提升算法成功率。图4展示了障碍物内采样的效果，此刻算法的采样点数为150，栅格缩放系数为8，在通道内生成了15个采样点。

图4 狭窄通道内采样

为了验证该采样方法在狭窄通道内的效率，本文作者用不同算法在该地图内分别进行50次仿真，表1记录了采样数目相同时，不同方法在通道内的采样点数目，其中，GPRM算法栅格缩放系数为8。由于4种方法采样时间均小于10 ms，对算法运行实时性的影响忽略不计，故未记录采样耗时。可以明显看出:GPRM算法的采样方法在狭窄通道中的采样效率相比其他算法有优势。

表1 狭窄通道内采样统计

1.2.3 连接策略

采样结束后，得到节点集E，此时，需要将能相通的节点连接起来。经典PRM算法遍历整个节点集E，逐个测试每个节点是否相通，时间复杂度为O(n2)，当节点数量增加时，算法运行时间呈指数形式增长，难以满足实时性的要求。在GPRM算法中，节点不需要与所有其他节点进行连接测试，只需要对节点所在栅格的相通栅格以及其相通栅格内的节点进行连接测试，其示意见图5(c)。并且，连接范围可以根据需求灵活调整。时间复杂度下降为O(mn)，其中m为起始点所在栅格的碰撞检测栅格(图5(c))内采样点的数量，且m

图5 不同栅格关系示意

1.2.4 路径优化与平滑

Dijkstra算法可以得到最优路径，但是在GPRM算法中，并非所有可连接的点都被连接，因此只能得到可行解而非最优解，可能出现如图6所示的情况，点P1为起点，点P3为终点，容易看出最短路径为P1P3而不是P1P2加上P2P3，但点P3不在点P1的检索范围内，因此两点没有连通。对于这种情况可以加大连接过程时的检索范围，但这样会增加算法复杂度，算法实时性受到影响，因此需要优化路径。

路径优化过程如下：(1)获得Dijkstra算法获得的路径点集P{Pi,i=1,2,…,n}，P1和Pn分别为起点和终点，建立优化路径点集N{P1}用来存放优化后的路径，初始只包括起点。(2)将P1作为测试点，逐个测试P中的点，若测试点与点Pm无法直接相连，则认为点Pm-1为转折点，点P2,…,Pm-2为冗余节点，将Pm-1放入优化路径点集N中，并将Pm-1作为新的测试点，逐个向后测试，直到测试到终点。(3)逐个连接优化路径点集N中的点，生成完整的路径。

优化结果如图7所示:优化前，路径总长度为943.9，节点数量为10；优化后，路径总长度为907.3，节点数量为7，路径长度和节点数量分别下降了3.8%和30%。

图7 路径优化示意

优化后，路径的质量有所提升，但其仍然是由数条线段相连组成的，不符合实际应用场景中移动机器人的运动约束，因此需要对路径进行平滑，去掉路径之间的折角，产生一条光滑连续的路径。

如图8所示，点P1、P2、P3、P4为一系列路径点，移动机器人最小转向半径为rmin，∠P1P2P3=α1, ∠P2P3P4=α2, 对于路径的第一部分P1P2P3,取l=min(lP1P2,lP2P3)/2，在P1P2和P2P3上找到与点P2距离为l的点Pl1和Pl2，那么可以得到半径r1=l/tan(α1/2)，圆心O1通过求线段P1P2和线段P2P3过Pl1Pl2的垂线的交点得到。得到圆弧后，检测圆弧上的每个点，如果圆弧在障碍物上，则将l减半，重复上述步骤，若半径r1

图8 路径平滑示意

图9 路径平滑效果展示

2 算法及结果分析原理

文中算法仿真部分将分为两个部分：第一部分将分析栅格缩放系数的选取对GPRM算法性能的影响；第二部分将在不同类型的地图上比较GPRM算法与经典PRM算法的各项性能。仿真设备CPU为Intel(R)Core(TM)i5-9300H 2.4GHz，RAM为8 GB，仿真软件为MATLAB 2018a。

2.1 栅格缩放系数分析与性能对比

GPRM算法的核心思想是用栅格划分整个地图，采样和连接都针对栅格进行。栅格的大小与数量都和栅格缩放系数k有直接的关系，因此，栅格缩放系数的选取非常重要。文中分别在大小为500×500的简单地图和大小为1 000×1 000复杂地图上选取不同的栅格缩放系数k和采样点数，分析其对性能的影响。两张测试地图如图10所示。

图10 测试地图

首先对简单地图进行测试，选取不同的采样点数量和栅格缩放系数k，每次测试运行100次程序，结果如表2所示。

表2 简单地图测试结果

对表1结果进行分析可知：采样点数量不变时，栅格缩放系数k越大，运行时间越短。当栅格缩放系数k≥5时，GPRM算法运行时间和通过率都优于经典PRM算法，尤其是采样点数量为100、栅格缩放系数为10时，运行时间的提升达到了84.5%。但当k取3时，GPRM算法的运行时间反而超过了经典算法。在分析算法不同部分的耗时后发现：算法的耗时主要集中在学习阶段的连接部分。经典PRM算法的连接部分需要遍历所有点，而GPRM算法只需遍历附近栅格内的点即可，因此能节省很多时间。但是当栅格缩放系数k较小时，GPRM算法也近乎遍历所有点进行连接测试，除此以外还需进行提取附近栅格采样点的操作，因此算法性能反而不如经典算法。

在简单地图上的仿真很难看出栅格缩放系数k的选取与算法成功率的关系，下面将会选取不同的采样点数目和系数k，在大小为1 000×1 000的复杂地图上进行仿真实验和对比，每种系数运行100次，结果如表3所示，图11为GPRM算法在复杂地图上的运行结果。分析可知：在采样数不变的情况下，增加栅格缩放系数k能够有效减少程序运行时间，但是k的增加可能导致生成路径成功率的下降。根据对失败时采样点分布信息的观察发现：在采样数量固定时，增加栅格缩放系数，会导致平均分在每个栅格内的点数减少，当分配到每个栅格内的采样点数小于1.5时，算法在复杂地图上的成功率会明显下降。

表3 复杂地图测试结果

图11 复杂地图运行结果

通过以上的仿真实验，可以得到栅格缩放系数k的取值原则：(1)如果没有特殊要求，k的取值必须大于3，否则算法效率会低于经典算法。(2)在采样点数不变的情况下，k的值越大，算法的运行速度越快。(3)总采样点数不变的情况下，每个栅格中的采样点数随着k的增大而减小，当栅格中采样点数小于3时，算法成功率会下降，因此，在复杂地图上，不能让k无限增大，尽量保持每个栅格中的采样点数大于等于2。在实际应用中，可以选择较多的采样点数和较大的栅格缩放系数k，同时兼顾算法的实时性和有效性。

2.2 算法性能测试

由于移动机器人的路径规划并没有标准的数据集供算法进行测试，本文作者将选取来自文献[1-11]中的32幅地图，以及8幅作者根据周围环境建立的地图作为数据集。经过统计得到地图上障碍物的平均占比为36.2%。为了统一数据，将所有地图的尺寸缩放为500×500，在每张地图上运行10次，40张地图共400次。除了GPRM算法外，选取了经典PRM算法、高斯采样法[8]和桥测试采样法[9]作为对照，由于高斯法和桥测试法的采样需要较多的采样点才能完成，因此这两种方法采样数均为100，而经典PRM和GPRM有采样数为50和100的测试。仿真结果如表4所示。

表4 综合测评结果

根据表4可知：GPRM算法相对于经典PRM算法，能节省60%以上的运行时间，地图通过率能提升20%以上；同时，算法性能也优于高斯采样法和桥测试采样法，极大地提升了PRM算法的可靠性和实时性，并且GPRM算法生成的路径平滑，符合移动机器人的运动约束。

3 结语

针对经典PRM算法实时性差，并且难以通过狭窄通道的问题，提出了栅格概率路径图法。利用栅格划分地图，根据栅格内障碍物面积将栅格化分为不同威胁等级，并根据栅格的威胁程度进行采样，提升了在狭窄通道内采样的概率。并且简化了建立采样点间连接的步骤，极大地提升了算法的效率；规划路径后，优化、平滑生成的路径，使之符合移动机器人的约束。还分析了栅格缩放系数k的选取对算法性能和结果的影响，k的取值使每个栅格内的采样点数量为2时可以兼顾实时性与可靠性。仿真结果表明，GPRM算法有较快的运行速度和较高的鲁棒性，并且生成的路径平滑，符合移动机器人的移动约束，有较高的推广应用价值。