三维环境中机器人路径规划算法改进

杨小月，李宏伟，秦雨露，姜懿芮，王步云

(1.郑州大学 计算机与人工智能学院，河南 郑州 450001；2.郑州大学 地球科学与技术学院，河南 郑州 450001)

0 引 言

针对RRT*算法在复杂环境中搜索存在的问题，Wang X等[3]提出一种自适应扩展双向RRT*(AEB-RRT*)算法，该算法能成功用于六自由度弧焊机器人路径规划问题。在双向快速探测随机树BI-RRT算法的基础上，Wang等[4]提出一种运动约束双向快速探测随机树(KB-RRT*)算法，通过引入运动学约束，从而避免树的不必要增长。为了实现机器人的安全高效导航，Kwon等[5]提出一种基于双树RRT的仿车机器人轨迹规划(CDT-RRT*)算法，在机器人运动学约束下快速产生生长轨迹，但无法在极其狭窄和混乱的环境中产生可行轨迹。针对蚁群算法在移动机器人路径规划领域存在的易陷入局部最优且收敛速度较慢等问题，李理等[6]和Luo等[7]通过加入新的影响因素去获得信息素初始值，从而得到非均匀分布的信息素，以此来加快算法收敛速度。Jiao等[8]和江明等[9]则将传统状态转移规则改为参数自适应转移，以此来提升蚁群算法的性能。赵天亮等[10]提出一种将A*算法和蚁群进行融合的算法，Chen等[11]提出一种多策略融合蚁群算法的系统。这两种方法均能提高算法的寻优能力但结构较为复杂。

本文首先对RRT*算法进行改进，再将改进后的RRT*算法与蚁群算法进行融合，引入双向搜索策略，和B样条曲线平滑策略对所得路径进一步优化，使得最终生成路径更加光滑，长度更短，效率更高。

1 算法基础

1.1 RRT*算法

RRT*算法相较于RRT算法主要进行以下两方面的改进，分别是：为Xnew重选父节点和为搜索树重布线的过程。具体过程如下[12]。

如图1(a)所示，该图为某刻RRT*算法在路径搜索过程中的树状结构图，图中线段上的数字代表节点之间的路径代价值。由图可知，该时刻算法运行过程中所产生的新节点Xnew为10号节点，在为该节点进行重选父节点的过程中，以它为中心在设定搜索范围内去寻找近邻节点，为5、6、9号节点。由图可知，到达10号节点的初始路径代价为17；该节点的近邻节点与之连线的路径代价依次为15、12、13。若想路径代价最小，需将10号节点的父节点由节点7改为节点6，重新连线生成的随机树如图1(b)所示。

图1 RRT*重选父节点过程

上述过程结束之后，再进一步对该随机树进行重布线。如图2(a)所示，该时刻算法运行过程中所产生的新节点Xnew为10号节点，近邻节点分别为5、7、9号节点，由起点Xstart到近邻节点连线的路径代价依次为10、15、9。若将5号节点的父节点换为10号节点，则到达节点5的路径代价变为15，大于原有代价10，因此不改变其父节点。同理，按照此方式去处理其它节点。最终重新连线生成的随机树如图2(b)所示。

图2 RRT*重布线过程

上述两个过程为RRT*算法在经典RRT算法基础上所做出的重要改进。该算法使得随机树在扩展过程中新产生节点的路径代价尽量小，路径更优[13]。

1.2 蚁群算法

蚁群算法中每只蚂蚁选择路径的依据被定义为“信息素浓度”，信息素浓度较高的路径会大概率被选择。同时，蚂蚁会释放定量信息素，以此形成正反馈，从而促使大多数蚂蚁个体集中至一条相对较优的路径上[14]。该算法的具体过程如下。

(1)

蚁群算法的信息素更新规则为

τij(t+1)=(1-ρ)τij(t)

(2)

(3)

(4)

式(2)表示节点的信息素浓度衰减过程，其中τij(t) 表示节点在时刻t的信息素浓度值，ρ(0<ρ<1) 为信息素浓度衰减系数。式(3)表示三维环境中离散点的信息素浓度。式(4)中的length(m) 表示由m只蚂蚁搜索的路径长度集合。上述搜索路径过程不断迭代即可寻出最优路径。

2 ACO-RRT*算法设计

2.1 RRT*算法改进

为了适应精细化三维建模环境和解决地面起伏不平坦等问题，本文首先对传统RRT*算法进行改进；并将改进后的RRT*算法与蚁群算法融合，设计出适用于三维环境的ACO-RRT*路径规划算法。本文对RRT*算法改进了以下几个方面，以提升算法运行的效率和性能。

RRT*是一种基于采样的路径规划算法，但是目前将其用于栅格地图中进行路径搜索的研究相对较少。本文针对三维环境中路径起伏不平坦的问题，将RRT*算法中的随机采样设置为正整数采样，以此来适应精细化的栅格建模环境，保证机器人以特定的步长去采样，计算公式为

(5)

式中：(x1，y1) 和 (x2，y2) 表示机器人在路径选择过程中路径节点的坐标。

传统的RRT*算法以一定的步长由Xrand向Xnear的连线方向扩展，以此得到新节点Xnew。但在三维环境中，地面起伏不平坦和环境中存在障碍物这种情况是不可避免的。因此在扩展新节点Xnew的过程中加入高度判断，即判断Xrand与Xnear连线中的所有路径节点是否高于或低于所对应障碍物的高度。若低于障碍物的高度，即发生碰撞，会出现路径穿过障碍物的情况，放弃此次生长，并重新进行路径节点的选取；否则，代表未发生碰撞，即该点可被扩展为新节点。

与此同时，在一定范围内遍历树中Xnew的所有近邻节点，将Xnew与近邻节点依次连线，进行碰撞检测，并寻找路径代价最小的近邻节点。本文节点之间的路径代价计算方法为三维欧氏距离，公式为

(6)

若发生碰撞，则将路径代价记为无穷大；否则，判断其连线是否超过障碍物高度，若没有超过，则找到路径代价最小的近邻节点，更新父节点，并将该点加入到树中；若超过该高度，则搜索新的节点。

本文改进的RRT*算法核心伪代码如下。

(1)V←{XA};ε←φ;TA,B←φ;Cpath←∞ //初始化各项参数

(2)fori=1,…,ndo//扩展新节点

(3)Xrand←CeilFree;

(4)Xnear←Near(Xrand,V);

(5)Xnew←Steer(Xnear,Xrand);

(6)ifLine(Xnew,Xnear)

(7)ifCollisionFree(T(Xnear,Xnew))then

(8)V←V∪{Xnew};

(9)Xmin←Xnear;

(10)minCost←Cost(Xnew,G)+Cost(T(Xnear,Xnew));
//记录当前路径代价

(11)forj=1,…,mdo

(12)Xnear←Near(Xnew,V);

//遍历近邻节点, 寻找最低代价节点

(13)Cnew←Cost(Xnear,G)+Cost(T(Xnear,Xnew));
//记录最新路径代价

(14)ifLine(Xnew,Xnear)

//判断所选新节点的连线是否与障碍物碰撞

(15)ifCollisionFree(T(Xnear,Xnew))&Cnew

(16)Xmin←Xnear;

(17)V←V∪{Xnew}; //将该节点加入到树中

(18)ε∪(T(Xnear,Xnew));

(19)returnTA,B(G)

2.2 ACO-RRT*算法设计

RRT*算法能够收敛到最优路径，且易与其它算法相结合；但是它节点计算量大，内存消耗大。蚁群算法在运算过程中，采用多个体同时进行的方式，很大程度上改进了算法在对大规模复杂问题方面的计算问题；但是它易陷入局部最优。本文设计的ACO-RRT*算法融合了两种算法的优点，对路径长度和运行时间进行优化。ACO-RRT*算法采用双向搜索策略，在起点处运行改进后的RRT*算法，同时在终点处运行蚁群算法，运行过程中，当找到第一个重合的路径节点时，则停止路径搜索，输出路径。上述过程的步骤如下。

步骤1 初始化算法各项参数，输入机器人三维环境模型。

步骤2

(1)从起点处开始运行改进后的RRT*算法：在搜索空间内取整采样，产生节点Xrand；之后进行改进后的采样和扩展新节点过程；再将Xnew与一定范围内的近邻节点依次连线，并判断新选择的路径代价是否比原存储的路径代价小，该判断如图3所示。由图3可得，阴影部分为模拟障碍物，图中线段上的数字代表节点之间的路径代价值，Xnew的父节点为Xparent，X1和X2为指定范围内的近邻节点。原存储路径代价为起点Xstart到Xnew的三维欧氏距离，由图可知为30，记作C0。若选择X1作为Xnew的新父节点，由图可知新的路径代价为27，记作Cnew1；若选择X2作为Xnew的新父节点，由图可知新的路径代价为24，记作Cnew2。由于Cnew2的值最小，即路径代价最低，故更新Xnew的父节点，同时进行碰撞检测。由图3(a)可知选择节点X2的这条路径发生碰撞，故将Cnew2记为无穷大。最终选择X1作为Xnew的新父节点，更新该节点路径代价并将X1加入树中，重新连线后如图3(b)所示。之后再判断Xnew与目标点之间的距离是否小于步长h。若小于，则停止搜索；否则重新进行采样。

图3 路径代价判断

(2)同时从终点开始运行蚁群算法。先为每只蚂蚁构建解空间，为其概率选择下一移动位置，直至所有蚂蚁访问完所有节点；再将蚂蚁途径路径的距离和目前迭代过程中的最短路径记录下来，并更新路径上的信息素浓度值。若尚未达到迭代次数上限，则将每只蚂蚁的路径记录表清空，并重新进行采样和扩展新节点过程；否则结束运算。

步骤3 在蚁群算法中每只蚂蚁以“逐层”寻找下一转移节点的方式来移动前进，将该方式对照到三维环境模型中，可表示为沿着X方向每次前进一层到达下一层所在平面的某一可选路径节点，再在下一平面的Y方向和Z方向进行路径点搜索，并将每只蚂蚁最终所选择的路径节点加入到相应的蚂蚁个体节点记录列表中，以此方式不断运行计算，直至趋向目标点，获得最终路径。在RRT*算法子节点的扩展过程中，将其父节点邻近的16个节点作为备选的子节点，即在前后左右两个栅格的距离范围内，对蚁群算法的路径记录表进行处理。

步骤4 剔除起点和终点，若RRT*算法扩展的新节点与蚁群算法中某个体的节点记录列表产生重合，即当第一个重合的路径节点被找到时，停止路径搜索，输出路径。

ACO-RRT*算法的核心代码如下。

(1)V←{XA};ε←φ;TA,B←φ;Cpath←∞;G(V,E) //初始化各项参数

(2)fori=1,…,ndo//扩展新节点

(3)Xnew←Steer(Xnear,Xrand);

(4)ifLine(Xnew,Xnear)

(5)ifCollisionFree(T(Xnear,Xnew))then

(6)V←V∪{Xnew};

(7)forj=1,…,mdo

(8)Xnear←Near(Xnew,V);

(9)ifLine(Xnew,Xnear)

(10)ifCollisionFree(T(Xnear,Xnew))&Cnew

(11)Xmin←Xnear; //更新父节点

(12)V←V∪{Xnew}; //将该点加入到树中

(13)ε∪(T(Xnear,Xnew)); //重布线

(14)fork∈mdo//为蚂蚁k构建解空间

(15)Jk(rk1)←V- -{rk1};

(16)rk←sk;

(17) add edge (rk,sk) toTourk;

(18)procedureUpdate_pheromones; //更新信息素

(19)Tr,s←T0;

(20)ηr,s←1/Cr,s;

(21)ifTA,B(G)=G(V,E)then//判断路径节点是否重合

(22) coutpath

2.3 B样条曲线平滑策略

针对ACO-RRT*算法所获得的初始路径，引入三次均匀B样条曲线平滑策略，对路径进行平滑优化。

由公式

(7)

可知，k=3时B样条曲线数学表达式为

(8)

(9)

当三次B样条曲线各节点矢量间插值为常数时，为三次均匀B样条曲线，第i段三次均匀B样条曲线数学表达式为[15]

(10)

由式(7)、式(9)、式(10)可得三次均匀B样条曲线的基函数数学表达式为

(11)

将式(11)代入式(7)可得

(12)

式(12)为三次均匀B样条曲线数学表达式。

如图4所示为本文引入B样条曲线平滑策略之后的路径仿真示意图，图中Start处为起始点，Goal处为目标点，黑色阴影部分代表机器人搜索空间中的障碍物。两点之间的这部分曲折实线段为未平滑前路径，引入平滑策略之后，路径中拐点周围的折线段被曲线所代替，图中用虚线段表示，以此获得更短路径。

图4 B样条曲线平滑路径仿真

2.4 设计路径平滑度函数

本文设计了路径平滑度函数Qk来计算算法在运行过程中路径的总体平滑度[16]，该函数表示为

(13)

式(13)中，D1、D2、D3表示任意3个相邻路径节点之间的距离，表达式为

(14)

(15)

(16)

式(3)中所得Qk值越大，则代表相邻路径节点之间所存夹角越多，即路径越曲折；反之，代表路径越平滑。

3 仿真实验结果与分析

为了验证算法在三维环境中的有效性，将本文算法进行编程，并基于Matlab2018a平台进行仿真实验验证。首先将不同三维环境模型数据通过Matlab中的“load”函数在ACO-RRT*算法的主程序中进行加载，完成不同三维环境模型的呈现。通过使用Matlab中的“scatter”和“plot”绘图函数，绘制算法在三维环境中通过搜索得到的路径。最后编写评估函数程序来测试算法的性能，从路径长度、运行时间和路径平滑度这3个维度来分析ACO-RRT*算法的有效性，并与RRT*算法和蚁群算法进行对比研究。

3.1 仿真实验环境

为了验证本文算法在不同三维环境下的适应性，选择了两种复杂程度不同的三维环境模型来模拟机器人真实的工作空间，两种模型的大小均为21km*21km*2000m。其中每个模型中的障碍物数量、形状及道路宽度都有所差别。

本文分别对两种环境模型进行“栅格化”，将机器人的工作空间沿X、Y、Z方向分解为多个大小相同的网格，每个网格即为一个单元[17]。其中曲面表面为机器人环境轮廓，凸出部分和曲面以下部分为机器人工作空间中的障碍物。将RRT*算法、蚁群算法和ACO-RRT*算法都用于三维环境模型中，搜索一条从起点到终点避开所有障碍物的路径，并分别设置不同的起始点和终点坐标。仿真环境描述见表1。

表1 仿真环境描述

3.2 仿真实验结果

不失一般性，蚁群算法中信息素启发因子为α∈[0，5]、 期望启发因子为β∈[0，5]、 局部信息素挥发率为ξ∈[0.1，0.99]、 全局信息素挥发率为ρ∈[0.1，0.99]。 根据本文所使用的三维环境模型和融合算法的特性，在经过反复测试实验之后，调试出相对适合本文算法的参数值，其中一些关键参数设置见表2。

表2 算法参数

本文在第一个三维环境模型中进行机器人路径规划仿真测试，3种算法的路径规划结果示意图如图5所示。由图5(a)可得RRT*算法初期虽然朝着目标点方向进行搜索，但由于该算法随机扩展节点，导致在起点处遇到障碍物时所生成的路径较为曲折；后续则一直朝着目标方向顺利搜索。整体生成路径的效果良好，但不够平滑，拐点较多。由图5(b)可得蚁群算法生成的路径整体较为曲折，这是由于启发式信息导致蚂蚁朝着终点方向搜索，直至遇到环境中的障碍物时，才改变搜索方向，因此花费较多时间，收敛速度较慢，整体路径效果一般。由图5(c)可得本文算法融合了改进的RRT*算法和蚁群算法的优点。该算法在寻找初始可行解时，所需扩展节点更少，减少了整体路径搜索时间；不仅能够很好地避开障碍物，且拐点较少；经过路径简化与平滑方法处理后，路径更光滑，且长度更短，符合预期效果。

图5 环境1中3种算法路径规划结果

本文在第二个三维环境模型中进行机器人路径规划仿真测试，3种算法的路径规划结果示意图如图6所示。由图可知，该模型比第一个模型更为复杂，障碍物分布不均，地面凹凸不平，增加了路径规划的难度。由图6(a)可得RRT*算法早期搜索路径时，因避开障碍物而浪费过多时间，整体路径效果一般。由图6(b)可得蚁群算法在更为复杂的环境中适应性能一般，路径较为曲折，且拐点数目较多。由图6(c)可得本文算法在复杂环境中生成的路径较优，由于双向搜索策略的引导，避免了前期路径搜索的盲目性，快速锁定终点方向，很大程度上提高了路径规划效率，在避开障碍物的同时又保证生成最短路径。

图6 环境2中3种算法路径规划结果

3.3 仿真实验对比分析

针对路径规划算法所具有的随机特性，本文在两种不同环境下分别进行10次实验，并从路径长度、运行时间和路径平滑度这3个维度对RRT*算法、蚁群算法和本文设计的ACO-RRT*算法进行对比分析。

(1)路径长度

在两种不同三维环境模型中，3种算法的多组实验路径长度对比见表3。

表3 3种算法多实验组路径长度对比

由表3可得，在环境模型1中，本文设计的ACO-RRT*算法与RRT*算法相比，平均路径长度减少了15.9%；与蚁群算法相比，平均路径长度减少了17.6%。在比环境模型1更复杂的环境模型2中，本文设计的ACO-RRT*算法与RRT*算法相比，平均路径长度减少了12.7%；与蚁群算法相比，平均路径长度减少了18.5%。由此可得，ACO-RRT*算法在复杂化三维环境中同样具备搜索较优路径的能力。根据方差可进一步得出，ACO-RRT*算法搜索路径的稳定性要优于另外两种算法，鲁棒性更强。

(2)运行时间

在两种不同三维环境模型中，3种算法的多组实验运行时间平均值对比如图7所示。

图7 3种算法多实验组运行时间对比

由图7可知，在环境模型1中，本文设计的ACO-RRT*算法与RRT*算法相比，平均运行时间减少了16.8%；与蚁群算法相比，平均运行时间减少了22.5%。在环境模型2中，本文设计的ACO-RRT*算法与RRT*算法相比，平均运行时间减少了16.9%；与蚁群算法相比，平均运行时间减少了25.1%。ACO-RRT*算法搜索速度提升的主要原因是引入了双向搜索策略和平滑策略，才使得该算法在三维环境中既能避开障碍物，又能保证路径的高效搜索。

(3)路径平滑度

在环境1中，3种算法的多组实验平滑度对比如图8所示。

图8 环境1中3种算法多实验组平滑度对比

在环境2中，3种算法的多组实验平滑度对比如图9所示。

图9 环境2中3种算法多实验组平滑度对比

由图8可得，在环境模型1中，本文设计的ACO-RRT*算法与RRT*算法相比，路径平滑度平均提高了26.2%；与蚁群算法相比，路径平滑度平均提高了20.5%。由图9可得，在环境模型2中，本文设计的ACO-RRT*算法与RRT*算法相比，路径平滑度平均提高了11.2%；与蚁群算法相比，路径平滑度平均提高了20.8%。由此可得，ACO-RRT*算法在复杂三维环境中搜索的可行路径质量较高，路径更平滑且耗费时间更少。

4 结束语

针对RRT*算法和蚁群算法在三维环境中进行路径规划时收敛速度慢和搜索具有盲目性等问题，本文提出了一种融合算法ACO-RRT*。首先对RRT*算法进行改进，以此来适应精细化三维建模环境和解决三维环境中地面起伏不平坦的问题。之后采用双向搜索策略，进行算法融合，在起点和终点同时运行改进后的RRT*算法和蚁群算法，相向而行。最后引入B样条曲线平滑策略，对路径进行平滑优化。将本文设计的ACO-RRT*算法用于不同三维环境中，并与RRT*算法和蚁群算法进行对比分析。仿真实验结果表明，ACO-RRT*算法能够在短时间内规划出一条长度更短、更平滑且更加稳定的路径，可有效用于三维环境中的路径规划。