基于人工势场法和启发式采样的最优路径收敛方法

李 伟，金世俊

（东南大学仪器科学与工程学院，南京 210096）

0 引言

路径规划是移动机器人导航的关键问题之一，目前移动机器人的路径规划算法主要分为两类：基于地图的全局路径规划和基于传感器未知环境的局部路径规划。

全局路径规划有A*算法、D*算法、快速搜索随机树（Rapidly exploring Random Tree，RRT）算法等。A*算法、D*算法基于栅格地图环境，可以获取最优路径，但是算法的表现受地图分辨率影响：地图分辨率较小时，算法速度较快，但是对于环境信息无法完全表现；地图分辨率较大时，对环境的模拟效果提高，但是算法运行时间极大增加。

局部路径规划有传统的人工势场（Artificial Potential Field，APF）法、粒子群算法、蚁群算法和遗传算法等。人工势场法容易实现，计算量小，路径规划实时性高，但存在局部最小值；粒子群算法智能的精度高、收敛快但易于陷入局部最优；蚁群算法和遗传算法易于获取最优路径，不易陷入局部最优但受初始条件影响较大。

RRT 算法［1］是众多路径规划算法中一种基于采样的算法，相较于A*算法、D*算法基于栅格地图且地图分辨率对于算法表现有较大的影响，它无需对环境进行结构化建模，同时RRT 算法在高维空间路径规划也有出色的表现［2-3］。RRT 算法具有概率完备性的特征，无论在多么复杂的环境下，只要有路径存在，在不限制采样次数的条件下，RRT算法最终都会找到一条可行路径。

RRT 将起点作为扩展树的根节点，利用随机采样的方式扩展树，一旦随机树触及目标点或者随机树的某一叶子节点进入目标点一定范围，则随机树立刻停止扩展，返回由起点到目标点的一条路径。RRT-Connect［4］算法用于提高RRT 算法的搜索速度，采用由起点和目标点各建立一棵搜索树，一旦两棵树相遇，则停止扩展，返回路径。该类算法能够在较短的时间内找到一条无碰撞的可行路径，但是由于采样次数往往较少，所获得的路径具有较大的代价。

为了解决RRT类算法最终获得路径的非最优性，Esposito等［5］和Karaman 等［6］基于快速搜索随机树的概率完备特征，提出了具有渐进最优性的快速搜索随机树（Rapidly exploring Random Tree star，RRT*）算法，在RRT算法的基础上加入了重新选择父节点以及调整现有节点父节点的策略，使其具有渐近最优的特点。RRT*-Connect 算法［7-9］同样用于提高RRT*算法的搜索速度，采用起点和终点两棵树同时搜索的策略，提高算法的收敛速度。此类算法虽然可以获得渐近最优的路径，但是其建立在对整个环境空间的随机均匀采样上，往往需要足够多的采样次数才能获取较优的路径，实时性较差。

为了优化算法的实时性，一些基于优化采样策略的算法被提出。P-RRT*（Potential RRT*）算法［10］在获得随机点时融入了人工势场法，使得随机点趋向目标点直到与障碍物碰撞或者到达目标点，以获取新的采样点，提升每次随机采样点对算法整体的效果，降低随机点为冗余点的几率，提高算法效率；但是最终生成的路径往往沿着障碍物边缘，无法获取全局最优路径。国内一些学者同样也在RRT*算法的采样过程中加入人工势场的思想：文献［11］提出了一种改进的RRT 路径规划算法，引入人工势场法，改进随机树的生长角度，缩短路径规划时间；文献［12］提出了PRRT-Connect（Potential RRTConnect）算法，该算法结合了人工势场法和RRT-Connect 算法，优化采样和扩展策略，提高算法效率，很好地解决了复杂环境下路径规划问题；文献［13］提出了一种改进RRT*路径规划算法，快速瞄准包含起点和终点的连通区域，采样过程中加入人工势场法的策略，实现偏向目标采样，加速RRT*收敛。但是这些算法都只将人工势场法与采样过程结合，并没有避免对整个环境空间均匀采样。

启发式RRT*（Informed RRT*，Informed-RRT*）算法［14-15］利用RRT 算法获取一条非最优的路径，然后在这条路径的基础上采用启发集合（Informed Set）采样的方式缩小采样范围，提高采样效率，但是由于RRT 算法的随机性，获取初始路径的时间代价和路径代价稳定性都不高，基于其构成的启发集合较冗余，效率不高。

本文提出了一种结合人工势场法和启发采样策略的快速获取最优路径的RRT*（Potential Informed-RRT*，PI-RRT*）算法，该算法基于人工势场法快速获取初始路径，基于启发集合采样缩小随机均匀采样范围，该算法通过稳定性较好的人工势场法获取原始的可行路径，再以原始路径构建初始启发集合，通过限定在启发集合内进行均匀采样，并在算法进行过程中动态调整启发集合范围，提高采样效率，降低冗余采样次数。基于Matlab对PI-RRT*算法和已有算法进行仿真对比，实验结果表明本文算法在采样点数以及算法运行时间方面，较已有算法都有较明显的改进。

1 背景知识

1.1 问题背景

将整个状态空间定义为X=(0，1)d，Xobs表示障碍物空间，Xfree表示为整个状态空间除去障碍物空间后剩下的无障碍空间，记为Xfree=XXobs。(Xfree，xinit，Xgoal)定义了一个路径规划问题，其中xinit∈Xfree表示路径规划问题中的初始状态，Xgoal⊂Xfree表示目标集合。令连续函数f：[0，1]↦X表示从起点到目标点的一条路径，则对于∀τ∈[0，1]有f(τ) ∈Xfree。

针对以上路径规划问题，有以下两个规定：

1）如果某路径规划算法，可以生成一条连续的路径f，f与障碍物无碰撞，且满足：

则称此路径为可行路径，产生此路径的算法为可行的路径规划算法。

2）令c(f)表示经路径f由起点xinit到目标点xgoal的代价函数（本文将路径的欧几里得距离作为路径的代价），记f={x1，x2，…，xi，…，xn}，其中xi表示路径上的第i个节点，所以有，使得代价函数c(f)取得最小值时的路径f*为路径规划问题的最优路径。

1.2 基本的RRT*算法

RRT*算法的随机树扩展过程如图1所示。

图1 RRT*路径规划算法的扩展过程Fig.1 Expansion process of RRT*path planning algorithm

RRT*算法详细步骤如下：

步骤1 初始化随机树G，包括随机树节点集合V，随机树边集合E，并将路径规划起点xinit加入随机树节点集合V。

步骤2 判断采样次数是否达到上限n：如果达到，输出最优路径，结束循环；如果没有达到，进行步骤3。

步骤3 由随机采样函数Sample（）在状态空间X中进行均匀随机采样获得随机点xrand。

步骤4 由函数Nearest（）获取当前随机树G中离xrand距离最小的点xnearest。

步骤5 函数Steer（）由xnearest开始向随机点xrand扩展步长ε，生成新的节点xnew。

步骤6 利用Collision（）函数判断xnearest到xnew的路径是否与障碍物或者环境边界发生碰撞：如果碰撞发生，则返回步骤2进行循环迭代；如果新的路径没有发生碰撞，执行下一步。

步骤7 用函数Neighbors（）生成随机树G的节点集合V中位于新节点xnew一定范围邻域内的节点集合Xnear。

步骤8 在Xnear内利用chooseParent（）函数获取离xnew路径代价最小的节点作为xnew的新的父节点xparent。

步骤9 更新随机树G，将xnew加入随机树节点集合V，将xparent到xnew的路径加入随机树边集合E。

步骤10 在xnew邻域Xnear内重新遍历所有节点，调整加入xnew后Xnear内节点的最优路径，使得其中的节点路径代价最小。

步骤11 返回步骤2 进行循环迭代，直到达到最大采样次数，输出全局最优路径。

RRT*路径规划算法的伪代码如下所示：

2 PI-RRT*算法

RRT*路径规划算法在RRT 算法的基础上降低了最终路径的代价，但是往往实时性较差，原因在于其采样是基于状态空间X完全意义上的均匀采样，所以状态空间X中任意一个点被选中的概率都是相等的；然而移动机器人使其可行路径往往只占空间中的很小一部分，这就导致大量冗余采样点的存在，降低算法的效率。本文通过人工势场法获取环境中一条初始路径，并利用这条路径建立启发集合，通过启发集合内的均匀采样减少冗余采样点，提高了RRT*路径规划算法的效率。

2.1 基于人工势场法建立初始扩展路径

人工势场法的基本原理是将环境地图建立为一个二维的虚拟力场，目标点xgoal产生引力场Uat，吸引机器人向目标点运动；同时障碍物Xobs在其一定范围内产生斥力场Ure，阻碍机器人向障碍物移动。引力场Uat和斥力场Ure共同形成整个环境中的虚拟力场Ures，虚拟力场作用于机器人上表现为机器人受到一个虚拟力Fres的驱动作用，Fres的方向沿着机器人所在位置力场梯度下降方向，驱动机器人朝目标点运动［16-17］，大致过程如图2所示。

图2 人工势场法下的路径规划Fig.2 Path planning under artificial potential field

综上所述，人工势场法具有目的性强的特点，且无需对环境进行建模，同时人工势场法具有和RRT 算法相似的路径扩展特征，因此本文选用人工势场法生成初始路径。在生成初始路径的过程中，将人工势场法的节点和路径边作为RRT 搜索树的节点和路径边，由此构成初始的RRT 搜索树，有效地融合了人工势场法和RRT 算法。另一方面，Informed-RRT*算法采用RRT 算法构建初始路径，RRT 算法存在随机性强、获取初始路径所需采样点数较多、所需时间及路径代价不稳定、鲁棒性较差的缺点。因此，利用人工势场法代替RRT 算法构建初始路径，稳定性强，可以明显地缩短Informed-RRT*算法生成初始路径的时间以及减小初始路径代价，缩小初始启发采样集合。

相较于传统的分段人工势场引力方程，本文采用单一距离平方正比关系的引力方程，主要是由于利用人工势场算法直接进行机器人路径规划时，为了防止出现目标不可达的情况，人为地将目标点附近的引力减小；而本文仅仅利用人工势场算法生成一条初始路径，一旦路径点进入目标点范围，则直接结束算法。传统的人工势场法在路径生成的过程中会出现路径震荡的问题，本文为了避免此问题的产生，利用RRT*中路径重写的思想，在人工势场法生成路径的过程中优化路径，解决初始路径震荡的问题。

目标点xgoal产生引力场Uat公式如下：

其中：Ka表示引力系数；d(x-xgoal)表示当前位置x到目标点xgoal的距离。

障碍物Xobs产生的斥力场Ure公式如下：

其中：Kr表示斥力系数；dmin表示机器人距离障碍物最短的距离；x′表示Xobs中距离当前位置最近的点；表示障碍物的斥力作用范围。如果机器人和障碍物之间的距离超过，障碍物对机器人没有斥力作用。

由F=-∇U可以得到：

其中：

综上，通过Fres=Fat+Fre计算出环境中某一位置所受到的虚拟势场力大小。

基于人工势场生成初始扩展路径的算法Get-Potential-Path（）伪代码如下所示。

由RRT和人工势场法构建的初始路径如图3所示。

图3 两种算法的初始路径对比Fig.3 Comparison of initial paths of two algorithms

显然，人工势场法建立初始路径所需的采样点数以及最终的路径长度明显优于RRT建立的初始路径。

当算法结束时，该算法返回初始路径总代价ccurr，ccurr将作为初始参数用于建立启发采样集合。

2.2 建立动态启发采样集合

显然，当环境中存在一条路径fcurr，则∀τ∈[0，1]，有fcurr(τ) ∈Xfree，且满足：

则称fcurr为一条可行路径。当fcurr非最优路径f*时，一定∃x∈Xfree，使得：

Xfree状态空间中满足式（7）的所有x的集合记作Ximp，Ximp∈Xfree，若限制RRT*算法在Ximp内采样，并通过父节点重选和路径重写，则能够保证路径趋向于最优路径。由于避免了对无效范围的采样，可以提高RRT*算法收敛到最优路径的速度，提高规划效率。

在规划过程中，由于环境中未探索点路径代价无法提前获取，所以Ximp仅仅是理想的采样集合，在算法实现过程中做不到严格的Ximp内采样。与此同时，在二维平面中一定有式（8）成立，所以环境中某点x如果可以使得路径代价减小，则一定满足式（9）：

为Ximp的估计采样集合，一定有，即满足式（7）的所有采样点一定属于为启发采样集合。

显然，满足式（9）的所有状态点构成的集合是平面中的椭圆区域，分别以xinit和xgoal作为椭圆区域的左右焦点，长轴长度为当前路径代价c(fcurr)，短轴长度为：

其中cmin是xinit和xgoal之间的直线距离。

基于前文人工势场法的初始路径，建立初始启发采样集合，如图4 所示，随着路径规划算法的进行，当前路径代价c(fcurr)逐渐减小，椭圆区域在保持焦点不变的情况下，长短轴长度都会减小，最终导致椭圆区域收敛于一个极限状态，即基于最优路径f*所构建的启发采样集合。

图4 启发式采样集合Fig.4 Informed sampling set

2.3 启发采样集合内的均匀采样

RRT*算法的概率完备性是建立在对采样区域均匀采样的基础上，环境中各点作为采样点的机会都是相同的。为了保持算法的理论有效性，需要对启发采样集合进行均匀采样。

基于启发采样集合的均匀采样方式主要有两种：一是拒绝式采样（Rejection Sampling），通过全环境的均匀采样获取随机点，然后判断随机点是否在椭圆形启发采样集合内，再做是否保留当前随机点的决定；二是通过在单位圆内的均匀随机采样［18］，再利用坐标变换转化为椭圆区域内随机点的方式，获取随机点。

拒绝式采样的采样方式仍然为全环境内均匀采样，在采样后需要对获取的随机点加以判断，判断其是否在启发采样集合的范围内，然后取舍。由于启发采样集合只占整个环境的一小部分且随着算法进行动态缩小，所以拒绝采样方式下随机点落于启发集合内的概率取决于启发采样集合占据整个环境的比例。显然随着算法的进行，想要获取落于启发采样集合内的随机点则需要重复多次采样。通过单位圆内均匀采样后转化为启发采样集合内的随机点的方式则没有多次重复采样后取舍的问题，可以提高算法效率。所以本文采用第二种采样策略。

设xcircle为以原点为圆心的单位圆内的一个均匀采样随机点，即xcircle∼U(Xcircle)，其中Xcircle={x∈X|‖x‖2≤1}，则有：

其中：xellipse∼xcenter=(xinit+xgoal)/2 表示分别以xinit和xgoal作为左右焦点的椭圆形启发采样区域的中心；L表示由单位圆到椭圆采样区域的线性变换矩阵；C表示由以启发采样区域两焦点连线为横轴的相对坐标系到世界坐标系的坐标变换矩阵。

取xcircle为单位圆Xcircle内某点，令为单位圆到椭圆的线性变换，则有：

由式（11）得到：

再有xellipse=为椭圆启发采样区域的相对坐标系到绝对坐标系的坐标变换，其中式（14）为坐标变换矩阵：

其中：θ∈为xinit和xgoal所在直线和绝对坐标系横轴的夹角。

综合式（10）、（13）和（14），单位圆内均匀采样可以由式（15）转化为椭圆形启发采样区域内的均匀采样。

启发集合内均匀采样算法Informed-Set-Sampling（）的伪代码如下所示：

3 仿真验证与结果分析

根据上述算法设计，本文在Inter Core i5-9300H 2.40 GHz主频PC 上采用Matlab 2020a 进行算法编程仿真测试。仿真基于40×40的地图环境，模拟的是非结构化环境，采用不规则多边形模拟障碍物。在两种不同的环境下，分别采用RRT*、Informed-RRT*以及本文提出的PI-RRT*算法获取路径。环境地图以及获取相同路径代价时的规划路径如图5～6所示。

图5 仿真环境一中的算法表现Fig.5 Algorithm performance in the first simulation environment

两种环境下起点坐标都为(-15，-15)，终点坐标都为（15，15），RRT*、Informed-RRT*以及本文提出的PI-RRT*算法在保证路径代价相近的情况时，实验结果如图5～6 所示。两种环境中获取的路径代价以及获取对应路径所消耗的时间和所需采样点数分别如表1～2所示。

图6 仿真环境二中的算法表现Fig.6 Algorithm performance in the second simulation environment

表1 仿真环境一中的实验结果Tab.1 Experimental results in the first simulation environment

表2 仿真环境二中的实验结果Tab.2 Experimental results in the second simulation environment

由各算法的实验结果可知，在不同环境中，分别获取相同路径代价所消耗的采样次数以及算法执行时间，PI-RRT*相较于RRT*，采样点数减少约67%，算法运行时间平均缩短约74.5%；相较于Informed-RRT*，采样点数减少约40%～50%，算法运行时间平均缩短约62.5%。PI-RRT*算法表现明显优于RRT*以及Informed-RRT*算法。PI-RRT*算法由于采用了目标导向型的人工势场算法生成初始路径的策略，使得相对于Informed-RRT*算法由RRT 生成的初始路径，具有较小的路径代价，提高了收敛到最优路径的速度。

由图7 分析可知，在相同的采样次数下，本文的PI-RRT*路径规划算法得到的路径相较于RRT*和Informed-RRT*算法获取的路径具有明显小的路径代价。

图7 不同仿真环境各算法路径代价随采样次数的变化曲线Fig.7 Curves of path costs of algorithms in different simulation environments varying with sampling number

图8表明，当算法运行时间相同时，RRT*算法和Informed-RRT*在算法开始的前一段时间表现较为接近，之后由于Informed-RRT*算法的局部采样策略，使得路径收敛到最优路径的速度较RRT*算法加快；而PI-RRT*算法又优于Informed-RRT*算法的初始路径，使得路径可以更加快速地收敛到最优路径。

图8 不同仿真环境各算法路径代价随时间变化的曲线Fig.8 Curves of path costs of algorithms in different simulation environments varying with time

4 结语

本文提出了基于人工势场法和启发采样的最优路径收敛方法。该方法利用人工势场法构建初始路径，继而生成初始启发采样集合，限制RRT*算法于启发式采样集合内进行均匀采样，同时在算法运行的过程中计算现有最优路径的代价，调整启发采样集合的范围，极大减少冗余采样次数，加速路径收敛。通过仿真实验验证，本文提出的PI-RRT*算法较RRT*算法以及Informed-RRT*算法性能上有很大的提升，可以提高最优路径的生成速度。

PI-RRT*路径规划算法也有不足的地方，虽然对于非结构化道路具有良好的路径收敛速度，但是在迷宫类的环境下，由于路径拐点较多，最优路径代价本身较大，效果并不明显，未来仍有改进的空间。