改进RRT 的复杂障碍物环境路径规划算法研究

牛旭，张志安

（南京理工大学机械工程学院，江苏南京 210094）

路径规划，即根据规划空间中的信息，规划出一条最优路径[1]。对于复杂障碍物环境下或者高维空间下的路径规划问题，常见的算法有RRT 算法和PRM 算法[2-4]。而对于多维度空间或复杂环境的路径规划，RRT 算法不需要规划空间，其泛用性更好[5-6]。但是，基本的RRT 算法在规划路径时也存在一些缺点，由于采样过程的随机性，搜索路径存在具有扩展随机性大，冗余节点多，收敛速度慢，导致搜索效率低等问题。

在后续研究中，文献[7]提出了渐进最优RRT*算法，通过重新选择父节点和重布线思想来优化路径。文献[8]提出了Quick-RRT*算法，通过扩大RRT*算法动态调整父节点的范围，提高其搜索效率。文献[9]在复杂环境下利用A*作为引导域限制RRT 的无效搜索提高其采样效率。文献[10]提出了一种Informed-RRT*的变体，利用目标偏置引导策略搜索目标点，利用椭球形子集细化路径，从而找到最优路径。文献[11]引入了权重系数，增加了树向目标点方向的扩展分量，文献[12]等在RRT 中引入了引力系数，增加树目标点的扩展分量，引导扩展偏向目标点，提高其迭代效率。

但对于树扩展随机性问题的改进研究，大都单一考虑了如何让树偏向目标点方向进行扩展，而不考虑环境障碍所带来的影响。对此，该文提出了一种改进的RRT 算法：引入自适应步长目标偏置，结合势场力引导随机树向目标点的生长，借助区域定性降低障碍物附近的搜索区域，再对路径关键点剪枝，利用B 样条曲线完成对路径平滑优化，得到一条最优路径。

1 算法原理

1.1 RRT算法原理

路径规划通常被视为在状态空间Χ中搜索从初始状态xinit到目标区域xgoal∈X或目标状态xgoal的连续路径[13]。该算法采用随机扩展的方法在状态空间中构造从初始节点到目标节点的树。以状态空间的起点xinit作为根节点，在搜索空间中随机选取一个点xrand。如果这个点在可行区域内，它将遍历T，在这个随机点上寻找最接近的节点n。通过碰撞检测，当xrand与xnear之间的距离比设定的扩展步长小时，xrand将作为叶节点xnear加入到随机树中；而当xrand与xnear之间的距离大于设定的扩展步长时，则在xrand与xnear之间的线段上，以xnear扩展设定步长的点xnew，将其加入到随机展开树中，将与xnew最近的节点称为xnew的父节点。重复上述迭代过程，直到目标节点变为叶节点或超过设定的迭代次数时则搜索结束。从目标节点回溯到起点，即可获得规划的路径。节点扩展的原理如图1 所示。

图1 RRT算法节点扩展原理图

传统RRT 算法伪代码如下所示：

1.2 人工势场法（APF）

APF 是一种局部路径规划算法[14-15]。假设机器人x在目标引力分量和障碍物斥力分量的共同作用下运动。其总势场U(x)可以表示为:

式中，Utarget(x)是目标点作用于机器人x的引力势场，Uobstacle(x)是障碍物对机器人的斥力势场。

其势场力F(x)可以表示为:

同样地，在式（2）中，Ftarget(x)是目标点对于机器人x的引力作用力，Fobstacle(x)是障碍物对机器人x的斥力作用力，而F(x)则是其合力。

但是人工势场法在路径规划中存在以下的缺点：

1）当运动空间障碍物较多时，路径规划的成功率会降低；

2）当障碍物与目标的距离小于一定阈值时，容易发生振荡，使其无法到达目标点；

3）当F(x)所受合力在势场中为0 时，将会陷入局部最小值，陷入局部振荡阶段。

2 改进RRT路径规划算法

2.1 自适应步长目标偏置

由于恒定步长的设定，无效搜索路径冗余繁多，使得算法的收敛速度慢；对狭窄区域或存在复杂障碍物区域的探索能力不足的情况，由此引入改进的自适应步长策略。当距离障碍物较远时，以大步长扩展，增加对所处未知环境的包容性，而当其进入障碍物影响域内，为了增加其对复杂环境的探索能力，选择以小步长扩展，如式（3）所示：

引入自适应步长来消除远离目标区域的无用搜索区域，引导随机树可能向目标点生长，大大减少了基本RRT 算法的迭代次数和节点数。

特殊情况下，针对xnear位于目标点邻域λ内时，步长，以目标点偏置导向，高效迭代。

2.2 引入引力场与斥力场

2.2.1 引入引力分量

在自适应步长目标偏置算法中引入人工势场的概念，增加随机节点的重力分量，控制随机树向目标点生长，降低随机性。原理图如图2 所示。其主要的核心思想是在每个新生成的节点中加入引力分量。

图2 添加引力分量构造

在构造时，引力函数G(n)在每个新节点n中引入，以改变生长方向的合力。它可以表示为:

式中，F(n)代表随机树在搜索过程中确定新节点的函数，Rd(n)代表最新节点的随机生长函数，G(n)代表添加的目标引力函数。

xnew节点的随机生长函数如式（5）所示:

目标引力函数G(n)如式（6）所示：

式（5）、（6）中，ρ是步长，g为引力增益系数，xgoal为目标位置矢量，|xgoal-xnear|表示节点间集合距离的绝对值。

2.2.2 引入斥力分量

在自适应目标偏置RRT 算法中引入APF 中障碍物斥力场的概念，引导局部随机树朝向远离障碍物生长。RRT 算法增加斥力分量的展开图如图3 所示。其主要的核心思想是对局部随机树搜索展开过程中产生的每个新节点引入斥力分量。

图3 添加斥力分量构造

在构造时，障碍物排斥函数T(n)，在生成的新节点n处引入，可以表示为：

式中，F(n)代表随机树在搜索过程中确定新节点n的函数，Rd(n)代表最新节点的随机生长函数，和T(n)是添加的障碍物斥力函数。

障碍物斥力函数T(n)如下：

式中，krep为斥力增益系数，p(x)为随机节点到障碍物的最短距离，r0为节点上障碍物的影响范围，xobstacle为障碍物的斥力矢量。

2.3 改进人工势场法

传统的势场为了充分利用势场的优势，其斥力系数远远大于引力系数，会出现在目标位置附近由于障碍物的斥力过大而无法到达目标位置的问题。针对上述问题，如图4 所示，利用多区域的划分，重新构造目标斥力函数为：

式中，rsta为障碍物的安全距离（障碍物膨胀后）。

既要保障斥力势场的效果明显，又要兼顾附近目标位置的需求，利用划分定性，在R0(R0=2r0)内的点所受斥力作用为无穷大，而当其处于Rsta(Rsta=2rsta)临界区时，斥力由斥力系数定性。

2.4 复杂障碍物环境下逃离局部最小值

机器人在整个势场中的运动是随机的，当其处于复杂障碍物环境下时，由于引入势场的概念，势场引导会出现合势场为0 的点，使得节点扩展处于缓慢，振荡循环的状态；通过连续五次相邻路径节点间的角度变化均处于±5°误差范围内，判定其处于局部振荡阶段；

由图5 可知，处于复杂环境（三个近距离障碍物1，2，3）的势场，具体实现：从点A扩展到点I结束，当计算第i个夹角时，需要利用i-1 和i+1 的节点坐标，用余弦定理进行角度计算；求出点A、B和点C之间的角度，以及点B、C和点D之间的角度，以此类推，其计算公式如式（10）所示：

为了引导其逃离局部最小值，该文引入局部定向剪枝概念，对路径关键点进一步择优剪枝选取。如图5 所示，当机器人从点A开始陷入局部振荡时，则连接点A与点B、C、D、E、F、G、H、I之间的线段进行碰撞判定，直到完成路径迭代：A→H→I。

2.5 B样条曲线的拟合优化

B 样条是样条曲线的一种特殊表现形式，同时也是贝塞尔曲线的一般形式，其具有凸包性、几何不变性、局部支撑性、非负性等优点[16]。n次B 样条曲线表达式为：

式中，Pi为第i段控制点的方程，Fi,n为n次B 样条的基函数，其公式为：

而常见的B 样条曲线的是二次和三次B 样条曲线，因为二次B 样条曲线同样不需要经过首尾的节点，所以B 样条曲线基函数选择为三次样条曲线。

为了保证沿路径运动时的速度平稳性，减少突变处，对图5 进行三次样条拟合优化，从a到目标点的平滑曲线如图6 所示。

图6 三次样条曲线拟合优化

3 仿真实验与结果分析

为了验证改进后的算法能否在空间的起始点更高效地搜索目标点并生成路径，利用Matlab 搭建仿真环境，完成改进RRT 算法的仿真实验。在该研究中，只考虑二维空间中的情况，模拟地图的尺寸为1 000×1 000（无量纲）。为了方便比较，统一了各算法的仿真参数。具体参数如下：起始节点坐标为(0,0)，目标节点坐标为(999,999)，起始点和目标点分别用不同形状标记，扩展步长为30。

3.1 标准RRT 与自适应步长（AS-RRT）算法实验结果分析

标准RRT 与自适应步长(AS-RRT)算法实验结果分别如图7、8 所示。

图7 标准RRT实验结果图

图8 自适应步长目标偏置RRT实验结果图

从图7-8 可以看出，标准RRT 算法在搜索过程中没有自适应步长偏置引导，路径是随机生成的。而对于改进的自适应步长偏置算法，引入大步长加大在无障碍物影响的搜索区域的范围，而引导随机树在多障碍物影响域引入小步长增加环境遍历包容性；大大减少了基本RRT 算法在多障碍物域内搜索的迭代次数和扩展的节点数。

3.2 APF-RRT和APF-ASRRT算法实验结果分析

APE-RRT 和APF-ASRRT 算法实验结果分别如图9、10 所示。观察两图发现，通过引入引力系数kp，其值为0.000 01，路径可以更严格地限制在起始节点和目标节点之间的区域内，与自适应步长RRT 算法相比，减小了搜索面积。由于障碍物斥力场的存在（斥力系数krep为200 000），对距离障碍物不同距离范围内的斥力定性，保障了障碍物附近有效点的搜索面积。因此，可以发现多障碍物之间的路径几乎是一条线，不仅使搜索路径与障碍物保持适当的距离，而且保证多障碍物环境定性区域搜索的完备性；但是由于势场的缺陷，路径会出现局部振荡阶段。

图9 APF-RRT实验结果图

图10 APF-ASRRT实验结果图

3.3 剪枝优化和三次样条曲线结果分析

APF-ASRRT 算法的剪枝优化和三次样条拟合实验结果分别如图11、12 所示。

图11 APF-ASRRT算法剪枝优化实验结果图

在图11-12 两幅图中，从图11 看出针对局部振荡阶段的震荡缺陷，剪枝择优选取路径关键点，绿色表示算法在二维空间中剪枝后的路径。图12 中黑色路径即是该三次样条曲线拟合出最优可行路径。从这两幅图中可以看出，剪枝优化与曲线拟合路径保障环境的实施可行性。

图12 APF-ASRRT算法三次样条拟合实验结果图

3.4 复杂环境RRT算法和改进RRT算法实验结果分析

复杂环境标准RRT 算法和改进RRT 算法实验结果分别如图13、14 所示。

图13 复杂环境标准RRT实验结果图

为了增加对复杂障碍物环境的对比，将算法部署到多障碍物复杂环境，从图13 和图14 中可以看出，该算法仍能保证对路径的搜索稳定性。

图14 复杂环境改进RRT结果图

3.5 RRT算法改进实验系列对比分析

研究中，改进的AS-RRT 算法、改进的APF RRT算法和基本算法在相同系数的前提下，对50 次仿真的迭代次数、搜索时间和路径长度进行了对比，计算模拟实际的平均搜索时间、平均路径长度和平均迭代次数，如表1 所示。

表1 仿真实验结果对比

从表1 中可以看出，AS-RRT 算法与标准RRT 算法相比，路径搜索时间减少了10.16%，路径长度减少了9.15%。而改进的APF-RRT 算法进一步减少了52.1%的迭代次数和92.1%的计算时间。在路径长度上，由于斥力场的限制，APF-RRT 改进算法的路径长度并没有减少。在多障碍物环境下，由改进的APF-ASRRT 算法结果可以看出，该算法在保证了对同一路径搜索的同时，搜索时间和迭代次数分别显著减少68.5%和20%。

4 结论

该文研究基于RRT 算法结合人工势场的概念改进算法，提出一种多障碍物路径寻优的策略。仿真实验证明，该混合算法明显改善了基本RRT 算法的不足。通过引入自适应步长偏置策略和人工势场概念，有效地减少了路径搜索过程中的迭代次数和运行时间，减少了路径的随机性和偏差，并且对初步规划出的路径进行剪枝择优和三次样条B 曲线平滑处理。该算法对RRT 进行多处改进，对于多障碍物环境的路径规划，具有一定的参考意义。

虽然在对多障碍物的表面做规则量化处理，但在实际的环境中，往往是不规则的多边形，因此未来的研究可针对不规则的障碍物进行研究。