涡流人工势场引导下的RRT*移动机器人路径规划

曹 凯，陈阳泉，高 嵩，高佳佳

1.西安工业大学机电工程学院，西安 710021

2.西安工业大学电子信息工程学院，西安 710021

3.加州大学默塞德分校工程学院，美国默塞德95343

近年来智能移动机器人在军事国防、航空航天以及星球探索方面起到了重要的作用[1-4]，因而成为学术界研究和关注的热点问题。移动机器人在任务空间自由活动的过程中，首先需要解决路径规划问题。路径规划是在静态和动态环境、模型约束以及不确定性的情况下，自动生成到达预定目标点的可行和最佳路径的过程。目前已有许多路径规划算法用于解决静态和动态环境中的运动规划问题[5]，例如人工势场[6-7]、基于网格的方法[8-9]、神经网络[10]、进化仿生的方法[11-13]和基于采样的规划[14-15]。

人工势场（artificial potential fields，APF）包括目标点周围的吸引势场和障碍物周围的排斥势场。吸引势场使机器人朝向目标移动，斥力势场使机器人远离障碍物，最终机器人将朝着吸引和排斥合力的方向移动。虽然APF 具有简单的数学表达式和较高的计算效率，但也存在一些固有问题：

（1）容易陷入局部最小；

（2）障碍物密集且间隔较小的环境中找不到可行路径；

（3）在狭窄通道和目标点附近存在障碍物导致无法到达的情况下出现机器人振荡。

以往的研究大多假设障碍物远离目标点，在这种情况下，当机器人接近目标位置时，由于障碍物引起的排斥力可以忽略不计，机器人会被吸引力吸引到目标位置。然而，在实际环境中目标位置或许会非常接近障碍物，导致当机器人靠近目标时，也会接近附近的障碍物，而排斥力将远大于吸引力，并且目标位置不是总势能的全局最小值。因此，由于附近的障碍物，机器人无法到达目标。针对这一问题，考虑到机器人与目标之间的相对距离，对路径规划中的排斥势函数进行了修正，从而确保总势在目标位置有一个全局最小值，机器人最终会达到目标[16]。

基于采样的规划（sampling-based planning，SBP）算法对配置空间进行纯粹的探索，以保证算法的概率完备性。其中Kavraki 等人于1994 年提出的概率路线图（probabilistic roadmap，PRM）[17]和Lavalle 于1998 年提出的快速随机探索树（rapidly-exploring random tree，RRT）[18]是最有效的SBP 算法。然而，PRM对障碍物信息的依赖使得它只适用于静态环境。相比之下，RRT 更适合动态环境和未知场景，并且可以包含非完整约束条件。

RRT 算法是从根（起始点）开始向未探索的区域随机生成一棵可行轨迹树，当其中一条无碰撞轨迹到达目标区域时产生一条可行路径。而路径间隙、路径长度以及规划时间都取决于节点产生的随机性。因此，RRT 算法主要问题有：

（1）在初始迭代期间，目标区域附近的有用样本无法最快做出贡献，增加了规划时间；

（2）在整个探索空间中进行探索采样，产生较多节点，扩大了内存需求；

（3）寻找到的路径往往不是最优的，路径成本较高，且向最优路径收敛缓慢；

（4）车辆与障碍物之间的距离，会改变生成路径的间隙，从而影响路径安全。

目前，有大量文献关于RRT 算法以及RRT*算法存在的问题提出了一些解决方案，例如文献[19]提出RRT-Connect 方法，它从起始点和目标点生长两棵不同的树，同时不断尝试用贪婪的启发式方法连接它们，以改善规划时间。在文献[20]中，限制节点扩展的最大数量，并通过删除无效节点，添加高性能节点，保证了完整节点重新布线后的路径长度最短，但是收敛速度比RRT*慢。RRT*算法作为RRT 的一种重要变体。文献[21]中的RRT*算法通过配置路径成本函数来连接树中的节点，从而降低路径长度。文献[22]和文献[23]提出在找到可行路径之后，对路径再进行优化以得到较短的路径。文献[24]使用启发式滑动窗口采样减少B-RRT*算法随机采样所带来的误差，并将车辆运动学约束加入到重选父节点和重布随机树的过程，使用贝塞尔曲线对所规划的路径进行平滑处理。文献[25]采用目标偏向策略和气味扩散法来改善扩展节点的选取，使得随机树的生长趋向于目标点，并提出一种基于三次B 样条曲线的路径平滑方法，极大地提升了搜索效率和路径质量。

本研究在APF 中加入涡流的引导，并与RRT*采样的方法相结合来解决RRT*存在的一些问题。该方法利用涡流人工势场（vortex-APF，VAPF）引导探索更有前途的配置空间，减少了得到最优路径的迭代次数，从而降低执行时间，加快收敛速度。在轨迹树中拒绝插入无效节点和高成本节点，以降低内存需求。然后对可行路径进行修剪和平滑处理，在无障碍区域连接尽可能少的节点得到一条长度较短、更为平滑的最优或次优路径。仿真结果表明，该方法在内存需求、执行时间和路径复杂性方面均具有有效性。

1 相关工作

1.1 RRT*算法

RRT 算法是通过随机探索自由空间，并构建一棵从起始点（zstart）开始寻找朝向目标状态的可行路径的树。在迭代的过程中，创建随机点并搜索距离该点最近的顶点，然后以固定步长得到该方向上的新节点，并检查其是否属于自由空间，直至达到目标（zgoal）。

RRT*算法使用了三个新的概念对RRT 进行了改进：第一个是新增节点的本地邻域；第二个是成本函数，它计算了从初始状态到树中节点的总成本；第三个是重新布线，它利用路径成本函数在新增节点的本地邻域中重新布线，从而降低路径成本。RRT*算法的流程与RRT 算法类似，在迭代的过程中，创建随机采样节点，搜索到该点的最近节点znearest，根据固定步长连接到最近的节点，就形成探索树中的新节点。新节点领域中的点znear由新节点为中心半径为r的区域来确定，如果通过新节点的路径比通过当前父节点的路径具有更低的成本并且两点之间是无障碍区域，那么新节点被设置为最佳父节点以保持树的结构，如图1 所示。

重复上述过程来维护节点树的增长，直到达到目标。如果目标状态位于障碍物空间中，则迭代过程被中止或者寻找路径失败，称为陷阱状态。RRT*算法的基本流程如下所示。

Fig.1 RRT*node extension process图1 RRT*节点扩展过程

算法1T=(V,E)←RRT*(zstart,zgoal)

1.2 人工势场法

1986 年，Khatib 提出利用势场进行避障和路径规划的思想[26]，即人工势场法APF。它来自物理学中场的概念，它将物体的运动视为两种力相互作用的结果。因此在该方法中，机器人被建模为受吸引力和排斥力影响的粒子。障碍物表示为斥力场Urep，目标则是吸引场Uatt。在吸引力和排斥力的共同作用下，机器人沿着合力的方向移动，安全地到达目标区域，即不发生任何碰撞。目标点对于机器人的引力场和斥力场如式（1）和式（2）所示，目标对于机器人的吸引力Fatt和排斥力Frep分别为吸引势场和排斥势场的负梯度，如式（3）和式（4）所示。吸引势场和排斥势场的矢量和是势场的总和Ftotal，其计算公式如式（5）所示。

式（1）～（5）中，x是机器人当前的位置，xg为目标点，机器人与障碍物之间的最短距离用ρ表示，ρ0表示势场的最小影响距离，Ka和Kr分别表示吸引场和斥力场的缩放因子。

2 路径规划混合优化方法

2.1 改进的RRT*算法（Improved RRT*）

在RRT*算法中远离生成路径的扩展节点重复连线，通常这些节点不仅没有降低路径成本，还因为无效节点间的重新布线增加了时间成本。因此，在生成路径的节点附近进行有意义的重新布线操作，可以有效地改善路径成本。

改进RRT*算法仅在有前途的区域中探索，生成更有前景的树T，从而以更少的时间和更小的内存需求找到最佳路径。它以zstart为根来初始化探索树T，先确定zstart和zgoal之间的偏向区域BRegion，然后随机选择BRegion中的节点进行采样，找到可行的初始路径。同时对初始路径使用节点拒绝技术，去除高成本节点和无效节点，然后进行原始路径修剪，获得一条长度更小，节点更少的优化路径，如图2 所示。

首先，确定探索区域，使用偏向采样探索区域，并对采样产生的节点频繁地进行重新布线操作，这有利于迅速地降低当前探索路径的成本。当发现目标节点或目标区域内（最小步长范围内）的节点时，找到一条从起始位置到目标位置的初始路径，如图2（a）所示。如果当前区域探索完成后未找到可行路径，则重新改变区域的大小，直到找到目标位置，如图2（c）所示。

倾向得分匹配法可以控制处理组与对照组之间不可观测且不随时间变化的组间差异（Heckman，LaLonde & Smith，1999）。此方法的核心逻辑是针对样本数据中不存在实施过境免签政策地区在没有实施此项政策情况下的入境旅游绩效的“反事实情形”，采用半参数估计方法估计实施地区未实施该项政策的倾向得分值，进而为实施地区选择匹配特征尽可能接近的未实施地区，保证处理组与对照组在政策实施前的发展轨迹基本“平行”。

Fig.2 Improved RRT*processing flow图2 改进的RRT*算法处理流程

探索区域的确定主要是利用起始点和目标点之间的连线的斜率，计算出延长线与地图边界之间的交点P和Q，然后对P和Q分别加减100 单位（可根据地图比例调节），得到与地图相交的新交点P1、P2、Q1 和Q2，并连接P1 和Q1，P2 和Q2，得到新的直线方程。如果这些新交点超出地图范围，则以地图顶点和直线方程重新计算得到交点P3 和Q3，P3、P2、Q1 和Q3 形成连通区域，如图3 所示。

Fig.3 Determining biased region图3 偏向区域确定的方法

其次，节点拒绝的工作是去除高成本节点和无效节点。如果新的节点和目标之间的距离成本大于当前路径的距离成本，那么新采样的节点就被拒绝插入树中。此外，如果某一个节点不能被扩展（没有子节点或者仅有一个子节点），这也意味着它不在达到目标状态的路径上，这样的节点也会从树杈中被删除掉。因此，只通过有用节点来扩展树，不仅可以维持树的成本，还可以快速地寻找到一条路径成本较小、收敛时间最短、内存需求最少的可行路径，如图2（c）所示。

最后，对先前得到的路径进行修剪（类似于树木生长过程中需要将多余的树枝修剪掉，以保证树主干的营养吸收）。因为RRT*算法是概率完备的，它倾向于随机点方向构建可行轨迹树，所以大多数解决方案不是最优的。因此，需要采用修剪算法以获得路程更短，噪声更少的路径，如图2（d）所示。

路径修剪的原理如图4 所示，获得初始路径后，从起点开始遍历每一个路径节点并相互连线，如果起点和其中的一个节点Zp1之间存在障碍物（蓝色虚线），则退回Zp1的上一个节点Zp1-1，而起点到Zp1-1之间的线段就是一段平滑路径。同时，Zp1-1作为新的起点遍历剩余的路径节点。以此类推，直到遍历到目标点形成一条优化后的低成本路径（橙色虚线）。

Fig.4 Path pruning图4 路径修剪

2.2 改进的APF 方法

APF 方法将障碍物作为移动机器人的斥力场，斥力场是以障碍物为中心产生向外发散的排斥力，从而使机器人远离障碍物，如图5 所示。图5 中的红色线条表示3 个圆形障碍物向外产生的发散的排斥力。虽然在吸引力的作用下可以到达目标点，往往难以遵循能量最小、时间和路径最优等优化准则。

Fig.5 Repulsive field distribution图5 斥力场分布

本研究在APF 的基础上将涡流引入势场，从而形成涡流场。涡流场可以产生切向梯度，利用迫使机器人绕过每个障碍物的动作来代替原本的排斥动作，从而引导机器人避开指向自然运动方向的有害或不利梯度，如图6 所示。

通过对机器人与障碍物之间位置关系进行反切运算可以得到涡流场的方向。同时，在原有的斥力场计算中加入了梯度信息和机器人与障碍物之间的距离可以得到涡流斥力场。涡流人工势场（VAPF）的引入，能够使机器人的路径更加平滑。

Fig.6 Vortex repulsive field distribution图6 涡流斥力场分布

2.3 VAPF 与改进的RRT*混合优化方法

首先利用RRT*算法在偏向区域中进行随机采样，然后根据机器人的位置和障碍物的影响范围，计算了作用于机器人的涡流场，并利用随机吸引场梯度下降对随机采样点zrand进行改造，得到一个引导采样点zprand，同时利用节点拒绝技术将路径中无效的节点（不在主路径中的节点）进行剔除，通过节点扩展的迭代得到一条初始路径，然后进行路径修剪和路径平滑处理，最终可以得到一条最优路径。VAPFRRT*混合优化方法的基本流程如下所示：

算法2T=(V,E)←VAPF-RRT*(zstart,zgoal)

算法第6 行中的随机梯度下降（randomized gradient descent，RGD）子函数表示随机吸引场梯度下降，它用吸引势场对随机采样点zrand进行扩展，得到一个改进的采样点xprand，并将xprand作为后续算法的随机采样点。算法流程伪代码如下所示：

函数1RGD(zrand)

3 算法仿真与结果分析

本研究在Matlab2016 中设计仿真场景，分别使用RRT、改进RRT*和VAPF-RRT*方法进行路径规划，考虑到RRT 类算法具有概率随机性，因此每种方法进行了32 次实验，取平均路径长度、执行时间和采样节点用来分析路径规划方法的性能。为了将执行时间限制在合理范围，迭代次数设定为2 000 次。

仿真场景由多个不同半径的圆形障碍物组成，使用RRT 和改进RRT*方法完成路径规划的效果图，如图7 所示。使用VAPF-RRT*方法对障碍物产生的涡流场和路径规划效果图，如图8 所示。

使用3 种算法在上述场景分别进行32 次仿真实验，表1 给出了每种算法用于达到最优路径解的平均执行时间、采样节点和路径成本。

Fig.7 RRT and improved RRT*path planning图7 RRT 与改进的RRT*方法路径规划

Fig.8 VAPF-RRT*vortex and path planning图8 VAPF-RRT*方法产生的涡流场和路径规划

Table 1 Simulation results for calculating optimal or approximate optimal path表1 计算最优或近似最优路径的仿真结果

从图7 中可以直观地看出，改进RRT*方法不论从扩展节点数量还是路径成本都要优于RRT 算法，而图8 显示了VAPF-RRT*算法没有扩展多余的节点，形成的路径更加平滑，而且斥力场的存在使机器人与障碍物之间保持相对距离，从而保证了机器人在运动过程中的安全性。通过表1 可以得到，改进RRT*算法的平均执行时间和路径成本是3 种方法中最优的，相对于RRT 算法和VAPF-RRT*算法，执行时间分别减少了12.1%和33.1%，路径成本分别缩短了21.1%和10.3%，说明偏向区域搜索可以明确搜索方向，加快收敛速度，降低执行时间；路径修剪可以修剪多余节点，达到优化路径的效果。在扩展节点方面，VAPF-RRT*方法的节点数量要优于另外两种方法，因为VAPF 的涡流场引导RRT*的随机采样节点，并将引导节点作为路径形成的导航点，从而降低扩展节点的数量。改进RRT*方法在偏向区域中随机采样再进行节点拒绝，收敛速度要优于使用涡流场来引导采样节点。虽然路径修剪使路径成本有明显的下降，但是容易出现优化路径贴着障碍物的情况，而且折线路径在实际应用中也存在较多问题。

从子函数RGD 流程中可以看出，障碍物势场的影响距离ρ0会影响引导节点的选取，从而影响执行时间、扩展节点和路径成本。本研究在实验过程中分别选取圆形障碍物半径的1.0、1.5、2.0 和3.0 倍作为障碍物势场的影响距离，图9 显示了选取不同影响距离对于最终路径形成产生的影响，其中黑色圆圈为障碍物，包围障碍物的青色圆圈为最小影响距离，绿色圆圈和深蓝色圆圈分别为起始点和目标点。

Fig.9 Path planning under different influence radii图9 不同影响距离下的路径规划

从图9（a）和图9（b）中可以看出，选取1.0 倍和1.5 倍障碍物半径作为势场的影响距离，通过VAPFRRT*算法形成的路径与障碍物之间保持的距离较小，甚至出现了贴着障碍物运动的情况，这不仅增加了执行时间和路径成本，也无法保证机器人在运动过程中的安全性。而对2.0 倍半径的影响距离而言，规划出的路径不仅相对平滑，而且可以和障碍物之间保持相对安全的距离。对比图9（c）和图9（d）可以发现，路径与障碍物之间的距离虽然稍有扩大，但后半段的路径却出现了波动。通过仿真实验发现，影响距离在大于2.0 倍障碍物半径后，逐渐增加影响距离对路径规划已无明显的改变。

综上所述，仿真结果证明了VAPF-RRT*算法在执行时间、采样节点、路径成本方面优化的有效性，如：

（1）在偏向区域内探索，加快了收敛到可行轨迹的速度，减少了规划时间；

（2）VAPF 算法的涡流场效应使节点产生的方向性增强，从而生长轨迹更为集中的扩展树；

（3）降低了内存需求，使RRT*算法也可以应用于内存有限的嵌入式系统；

（4）路径修剪和路径平滑，可以得到路径长度相对较短且平滑的路径；

（5）RRT*算法的随机性可以克服APF 算法容易陷入局部最优的问题。

4 结束语

在本研究中，提出了一种涡流人工势场引导下的RRT*算法来解决路径规划中存在的一些问题。通过多次仿真实验，其仿真结果可以定量分析改进算法的优越性、高效性和实用性。实验结果表明：本文方法可以有效地减少采样节点，降低规划时间并优化得到一条相对较短且平滑的路径。同时，RRT*算法的随机性又克服了APF 算法容易陷入局部最优的问题。

重复进行实验后发现，虽然本研究中的方法可以使用较少的时间和较小的内存，但是得到的路径并非全局最优路径。此外，也没有考虑实际场景中工业机器人和移动机器人的运动学和动力学约束条件下的安全问题。未来研究工作中希望可以根据搜索的初始路径拟合空间贝塞尔曲线，并通过优化曲线控制点使行驶轨迹满足各类约束的同时，提高路径的平顺性与安全性。最后希望本研究将可以应用于自主无人车辆和工业生产加工的高维运动规划研究领域。