多因素改进势场蚁群算法的路径规划

陈 勇，范平清，袁 涛

(上海工程技术大学 机械与汽车工程学院，上海 201620)

在移动机器人领域，自引导小车（Automated Guided Vehicle,AGV）的路径规划[1]问题是一个十分重要的研究方向，路径规划是指在两节点之间规划出一条综合指数最优的路径.由于机器人在各个领域的应用越来越广泛，机器人逐渐被应用在野外等路况较差的场景中[2]，路径的平缓度决定了机器人在寻路过程中是否会造成侧翻，这一指标成了路径规划中必须考虑的重要因素[3-5].现在的算法通常只考虑了路径是否最优，如经典的Dijkstra 算法[6]、人工势场法[7]、A-star 算法[8]、RRT 算法[9]以及智能遗传算法[10]、粒子群算法[11]等.

蚁群算法[12]在20 世纪90 年代末被提出，因算法相对简单，应用范围较广.但蚁群算法易出现局部最优，对目标点的可见度较低.文献[13]提出改进势场蚁群算法，利用人工势场法中的势场力构造出新的启发函数，减少了搜索的盲目性，提高了对终点的可见度，但算法缺乏全局搜索能力.文献[14]利用人工势场构造启发函数，借鉴了最大最小蚂蚁系统限制信息素范围以避免陷入局部最优，并在势场蚁群算法中引入三次B 样条曲线优化了路径，使得路径更加平滑.随后，文献[15]改进了势场蚁群算法，引入启发信息递减系数避免算法的局部最优问题，提出了自适应调节信息素挥发系数，提高了势场蚁群算法的全局搜索能力，避免出现停滞现象.文献[16]针对势场蚁群算法路径转折点较多的问题，提出了基于势场跳点的蚁群算法，算法提高了前期的搜索效率，使规划出的路径更加平滑.但上述路径搜索算法均只考虑了路径长度，因素较为单一.

后来一些学者提出利用3D 栅格建立地图模型以考虑地形因素，但此方法会增加计算机的运算时间，因此现多采用2D 栅格地图的高程图法[17-19]以提高算法效率.文献[20]在蚁群算法中结合多种因素，重新构造启发函数，引导蚂蚁走向最优路径.文献[21]在改进蚁群算法中引入多目标性能指标，将路径规划问题转化为一个多目标优化问题，在前人的基础上提出能耗[22-23]、安全度和路径长度这3 个评价指数，将这3 个带权值的评价指标作为多目标函数的约束条件，实现了路径规划的全局综合优化.文献[24]提出信息素阶梯分配原则，综合构建了路径安全性、颠簸性、平滑性以及路程最短性的多因子启发式函数，并运用动态切点法将多因素生成的路径平滑处理，提高了路径的综合质量.但其中的信息素阶梯分配原则考虑情况过于单一，在目标点变化时，搜索时盲目性仍旧较大.

基于上述研究，本文提出了一种多因素改进势场蚁群算法.首先，利用RGB-2D 栅格法模拟机器人寻路过程中颠簸的路面情况.该算法结合人工势场法，优化了蚁群算法中距离启发因子，克服了蚁群算法早期搜索的盲目性，并综合考虑了路径平缓性以及平滑性多因子启发式函数，在保证路径最短的同时，减少了最优路径中的转弯次数及平均高度.然后，引入启发信息递减系数，避免了蚁群陷入局部最优解，并运用最大最小蚂蚁策略提高了全局搜索能力.最后，利用动态切点法对生成的路径进行二次平滑，缩短了路径距离，提高了路线质量.

1 模型建立

1.1 多因素评价指标在传统的蚁群算法中，启发函数多是距离的倒数，这样虽然可以找出一条距离最优的路径，但在实际工作环境之中，其忽略了地面多种路况的不安全性以及机器人多次转弯的能耗时间问题，所以本文将路面平坦度和路径转弯次数同时作为路径最优的评价标准.由于这些因素指标会引起路径优化冲突，导致机器人在多种指标下不知如何选择，为了平衡各个因素指标，本文将启发函数定义为路径长度、平缓度（颠簸程度）和平滑度（转弯次数）的加权值，如下所示：

式中，Q为启发式函数，p为路径编号，a,b,c分别为路径长度L(p)，平缓度H(p)和平滑度T(p)的权重系数.其中，平缓度由机器人所经过的路径高度均方差表示，平滑度由路径的转弯次数表示.

1.2 多路况栅格地图建立地图建模的方法大体有栅格法、可视图法、拓扑法、自由空间法等.由于栅格法具有很好的鲁棒性，且能将实际地图按一定的比例切割成若干个面积相等的栅格，所以运用较为广泛.经典栅格地图之中，通常只用0 和1 两种数字来分别构造自由栅格和障碍物，本文添加hi值以表示不同地形栅格地图，则地图中栅格i的地形G(i)计算公式如下：

式中，当hi＞0 时，表示该栅格为隆起栅格，反之，hi＜0 时，该栅格表示沉陷栅格，hi的范围为(-1,1)，hi绝对值越大，表示该栅格沉陷或隆起的程度越大.利用绿色表示隆起地形的栅格，蓝色表示沉陷地形的栅格，如图1 所示.

图1 多地形栅格图Fig.1 The diagram of multi-terrain raster

2 基本算法

2.1 传统蚁群算法传统蚁群算法主要是依靠概率选择和信息素更新进行路径规划，蚂蚁初期会根据概率选择不同的路径，并在寻路的过程中释放信息素，剩下的蚂蚁倾向于选择信息素浓度较大的路径行走，逐渐地，最优路径上的信息素浓度越积越多.所以针对概率选择和信息素更新这两点，本文分别进行了改进.

2.1.1 概率选择 蚂蚁k在t时刻对下一个节点的选择是由节点i到节点j的信息素浓度τij(t)、距离启发因子ηij(t)的大小进行判定的，它是一种伪随机原则，如下所示：

式中，集合A中 是路径节点，α为信息素启发式因子，β为期望启发因子.当随机生成概率q≤q0时，则选择当前所有可移动节点中最大值的节点；否则，依据概率pijk(t)选择下一个节点，pijk(t)计算公式如下：

式中，dij是节点i和节点j之间的欧氏距离.

2.1.2 信息素更新 蚁量模型和蚁密模型多采用局部更新方式[25-26]，本文采用蚁周模型进行更新：任意时刻，蚂蚁经由节点i移动到节点j后会留下一定信息素，这就需要对信息素实时更新，信息素浓度的更新会使下一次迭代的蚂蚁倾向于走全局最优路径.更新信息素浓度公式如下所示：

式中，Δτi j(t,t+1)为蚂蚁在(t,t+1)时刻留在路径(i,j)上的信息素量，ρ (ρ ＜1)为信息素挥发系数，Lk为节点(i,j)的欧氏距离，Q为常数.设τi j(0)=C，C为常数，

2.2 人工势场法人工势场法通过引力和斥力解决避障问题，假设机器人当前节点为X=(x,y)，目标位置为Xg=(xg,yg)，则引力势场函数为：

式中，k为引力场常数，X为机器人节点量，Xg为目标节点量.引力公式为：

式中，-g(Ua)表示引力市场的页梯度.斥力势场函数定义为：

式中，m为 大于零的斥力场常数，d为机器人到障碍物的距离，d0为设定的安全距离.斥力公式为：

通过合力Fs的共同作用，机器人完成寻路并顺利避障.

3 多因素改进势场蚁群算法

在传统的蚁群算法中，通常以当前节点i与下一节点j的距离的倒数作为启发函数，该距离启发函数对终点的可见度较低，且并没有考虑实际多路况的情形.针对上述情况，本文首先引入人工势场法重新构造距离启发函数，克服了蚁群算法早期搜索的盲目性，然后综合考虑了路径长度，路径平缓性以及平滑性3 种因素，构建新的多因子启发式函数

式中，a、b、c为权重系数，Lij(t)为引入势场法的距离启发函数[27-28]，Hij(t)为平缓性启发函数，Tij(t)为平滑性启发函数，该启发式函数克服了传统路径规划中单以距离为指标的局限性.

3.1 改进启发函数

3.1.1 距离启发函数 蚁群算法中，传统的距离启发函数如式(4)～(5)所示，现利用人工势场法构造的合力Fs构造新的距离启发函数Lij(t)，由于人工势场对机器人有引导的作用，蚁群的收敛速度虽然更快，但是更容易得到局部最优解.针对蚁群算法易陷入局部最小，本文加入系数ξ 以改进势场力，如下所示：

式中，Nk为第k波蚂蚁的迭代次数，Nmax为总迭代次数，FA为改进势场合力.则优化后的距离启发函数为

式中，di j为节点i到节点j的距离，djg为节点j到目标点g的距离.当势场合力变为0 时，则依靠距离启发函数进行搜索，避免算法陷入“死锁”.

3.1.2 平缓性启发函数 在机器人实际工作环境中，过于颠簸的路面易造成机器人的侧翻，并且会影响机器人的移动速度和能耗.所以本文在启发式函数中加入平缓性启发因子Hi j(t)，引导机器人选择更为平缓的路径，从而提高路径的平缓性能，得到质量更高的路径.

式中，ai表示节点i附近所有可到达节点的集合，hmax表示当前节点i与周围相邻节点高度之差的最大值，hmin表示当前节点i与周围节点高度之差的最小值.h(i)-h(j)表示节点i到节点j的高度之差，Nmax为迭代最大次数，防止当hmax=hmin时分母为0 的情况，M和N为稳定修正参数.当蚂蚁选择高度差越小的路径时，平缓性启发因子Hi j(t)越大，越能得到更为平缓的路径.

印尼华语教学中断了30余年，实际上已不存在传统的华文教育。印尼华人的后代已被同化，融入印尼的主流社会。因此，我们必需更新对“华语”的认识，这样有利于解决印尼华语教学存在的问题。我们必须清醒地认识到，印尼语是印尼的国家官方语言，而华语或汉语是外语，所以我们应着力解决华语作为第二语言教学的问题。这样才能够使印尼华语教学在政治上确保正确，保证华语教学在实行单一语言政策的印尼可持续发展。

3.1.3 平滑性启发函数 传统蚁群算法在路径规划过程中会出现过多的拐点，这会导致机器人在拐点处重新调整位姿且进行不必要的加减速，行驶难度加大，机器人行驶时间变长.所以本文针对这一现象，在启发式函数中加入平滑性因子Tij(t)，机器人能够选择转弯次数更少的路径.

式中，δ(0≤δ ≤1)表示蚂蚁直行的重要程度，u表示机器人的灵活程度常量，Nmax为总迭代次数，σ为转弯修正常量，vi表示蚂蚁从起点到节点i的所有经过节点的集合，S(ai)表示集合中所有节点的个数，e-1表示集合中倒数第二个元素，即节点i的上一个节点m，dmi和di j分别表示蚂蚁从节点m到节点i和节点i到节点j的运动方向，当方向相同，即表示机器人不用转弯时启发函数更大，引导蚂蚁走向转弯次数更少的路径.

3.2 优化信息素更新

3.2.1 初始信息素分配原则 蚁群算法在路径搜索初期，会因为各个节点的信息素浓度差异较小使算法的正反馈效果不明显，导致算法初期搜索盲目性较大，收敛性差，搜索时间长等.为解决此问题，本文提出初始信息素不均匀分配方法，将算法所找到的起始点和目标点之间的所有节点重新分配初始信息素值，其余节点信息素值不变，从而提高算法前期的收敛速度和搜索时长.

式中，τi为新分配的初始信息素值，ω(ω ＞1)为初始信息素浓度增加系数，D为初始信息素浓度，P为起始点和目标点之间的所有节点集合.因为P中包含最优路径中的节点，所以有利于提高搜索的搜索速度.

3.2.2 多因子信息素更新 基于多目标的优化结果考虑，信息素的更新由路径平缓度、平滑度和路径长度共同决定.多因子信息素更新方式如下所示：

式中，当路径栅格i的信息素浓度 τi小于设定的最小信息素浓度τmin时，自动调整此时的信息素浓度为τmin，当此时信息素浓度大于设定的最大信息素浓度时τmax时，自动调整此时信息素浓度为τmax，从而解决路径搜索局部最优问题.

3.3 动态切点法处理栅格法的路径往往是从一个栅格的中心指向另一个栅格的中心，因此获得的路径并不平滑，为符合机器人对实际环境的性能要求，需要对规划出的路径进行平滑处理.本文采用动态切点法[29]对拐点进行平滑优化.

如图2 所示，机器人的起始点为A1，重点为An，算法从A1到An，沿着路径依次对拐点进行处理，具体步骤为：

图2 平滑优化示意图Fig.2 The schematic diagram of smoothing optimization

步骤 1比较并选择Ai-1Ai和AiAi+1两边中的较短边，以短边的端点P(xp,yp)作垂线与∠Ai-1AiAi+1的角平分线相交于点Oi-1(x0,y0)，相切圆的半径为

式中，k0为角平分线斜率，k01为短边斜率.相切圆的方程为

步骤 2判断相切圆是否与长边之间有交点S，若有，则转至步骤3，反之则转至步骤4.

步骤 3判断圆弧PS是否经过障碍点，若是，则转至步骤4；否则，用圆弧PS替代Ai-1AiAi+1，并转至步骤5.

步骤 4切点p(xp,yp)沿着所在线段移动到p2(xp2,yp2)，xp2为：

其中，λ根据实际情况设置，同时设置p2为初始切点，返回步骤1.

步骤 5判断机器人是否已遍历路径中的所有节点，若是，则返回步骤1，否则结束.

3.4 算法实现流程本文算法流程图如图3 所示.

图3 多因素势场蚁群算法流程图Fig.3 The flow chart of multi-factor potential field ant colony algorithm

本文算法的执行步骤如下：

步骤 1根据式(2)建立多路况栅格地图，并初始化算法的参数，将不均匀分配的信息素赋给每个栅格.

步骤 2根据式(13)计算启发式函数，通过概率选择要转移的网格.

步骤 3按式(22)更新路径中局部信息素，并按式(31)对信息素进行限制.

步骤 4判断当前节点是否为终点，如果是，转到步骤5，如果不是，返回步骤2，继续路径规划.

步骤 5计算该次迭代过程中路径的高度差，整体转弯次数和路径长度.

步骤 6寻找并输出本次迭代中的最优路径，并进行信息素的全局更新.

步骤 7判断是否达到最大迭代次数，如果是，转到步骤8，如果不是，重回步骤2.

步骤 8输出算法的最优路径，并通过动态切点法进行路径优化.

4 算法仿真分析

4.1 改进栅格地图机器人在野外恶劣环境中工作时，对安全性要求较高，因此提出在栅格地图中，即便是障碍物顶点也不能触碰到，从而保证机器人的绝对安全性，以得到一条质量更优的路径.现采用改进地图信息储存方式更新顶点防撞策略[30]，如图4 所示.

图4 栅格转向标号Fig.4 Grid steering label

本文规定偶数标号为机器人直行标号，奇数标号为机器人斜向标号，当只有斜向和与该方向垂直的两个直向栅格均无障碍物时，才可以进行斜向行进.如图4 中，若转向5 号栅格时，与5 号栅格相垂直的4 号和6 号两个方向的栅格均无障碍物时，机器人才会进行斜向转移.为了储存蚂蚁可行信息，建立转移距离矩阵D，如下所示：

式中，i1表示直行栅格，mod 为取余函数，i2是斜向栅格，i′和i′′分别表示斜向栅格相垂直的两方向的栅格，l为栅格的边长.

4.2 实验参数选取本文选择在Matlab 中进行仿真分析，因需要与文献[20]进行对比，因此设定算法参数与之相同.现设定算法的参数数据如表1 所示.

表1 实验参数数值表Tab.1 Experimental parameter values

4.3 算法仿真对比

4.3.1 20×20的障碍栅格地图 为了验证算法的有效性，在20×20的小栅格地图中进行仿真实验分析，并与文献[20]算法进行对比，仿真实验结果如表2 和图5 所示.为方便比较，本文引入综合指标S表示路径的质量，S越小，路径质量越高.S的计算公式为：

其中，H(t)表示路径的高度均方差，L(t)表示路径长度，L(t)表示路径的转弯次数，M为稳定迭代次数.

由表2 可知，本文算法在转弯次数、路径长度以及迭代次数上均优于文献[20]算法和传统蚁群算法，在高度均方差上优于传统蚁群算法，能够取得较好的仿真结果.由图5 可知，由于传统算法并未考虑路径的颠簸程度和平滑性，只追求了路径长度是否最短这一特性，所以当路径中存在坡度时，虽然本文算法的最短路径长于传统蚁群算法，但是在路径的平缓性和平滑性上有较好的效果.

表2 小栅格实验结果Tab.2 Experimental results of small grid

图5 小栅格机器人运动轨迹图Fig.5 Trajectory diagram of small raster robot

由图6 可知，相比于文献[20]中，本文由于加入了人工势场法和动态切点法，使得算法收敛速度变快，稳定迭代次数在5 次左右，避免了陷入局部最优问题且路径更为平滑，转弯次数更少，降低了16.67%的转弯次数，1.6%的路径长度.与其他两种算法相比，本文算法的综合指标更好.

图6 小栅格实验迭代图Fig.6 Iteration diagram of small grid experiment

4.3.2 30×30的障碍栅格地图 考虑到20×20栅格实验的局限性，在30×30的栅格地图中进行再次仿真.仿真结果如表3 和图7 所示.

由表3 可知，本文算法在高度均方差、转弯次数和迭代稳定次数上都优于传统算法和文献[20]算法，虽然在路径长度上比传统蚁群算法略差，但是综合指标最佳.由图7 可以看出，在30×30的地图中，本文算法寻得一条较为平坦的路径且在保证长度较短的同时，路径最为平滑.

图7 大栅格机器人运动轨迹图Fig.7 Trajectory diagram of large raster robot

表3 大栅格实验路径指标Tab.3 Experimental path index of large grid

由图8 可知，相比于传统蚁群算法，本文算法的路径长度虽然较长，但相比于文献[20]和传统蚁群算法中的算法，本文算法收敛性最佳，且减少了22.2%的转弯次数，9.2%的高度均方差且比文献[20]缩短了4.7%的路径长度.

图8 大栅格实验迭代图Fig.8 Iteration diagram of large grid experiment

4.3.3 多地形无障碍栅格地图 为验证本文算法对多地形地图的适应性，在多地形无障碍栅格地图中通过调整权重参数a、b、c、A、B、C进行实验对比分析.

实验1 中，根据式(1)，首先调整路径长度因素权重为a=1，路径高度权重和转弯次数权重b、c均为0，则式(26)～(28)中信息素更新权重的A、B、C分别为1、0、0，分析算法在只考虑长度的情况下，机器人寻得路径是否满足要求.

实验2 在综合考虑3 种因素的前提下，着重考虑路径长度因素，设置a、b、c分别为5、2、1，信息素更新权重的A、B、C均为1，查看在多地形无障碍地图中算法的适应性.

实验3 在不考虑路径长度的情况下，只考虑路径的高度均方差和转弯次数，设置a、b、c分别为5、2、1，信息素更新权重的A、B、C为0、1、1，查看算法在多地形地图中是否能找到一条最为安全的路径.实验综合指数参考式(38)如表4，实验结果如图9 所示.

表4 实验综合指数表Tab.4 Experimental comprehensive index table

由表4 可知，实验1 在不考虑路径高度和转弯次数因素的情况下综合指数最高，根据式（38）可知，综合指数越高，路径质量越差,实验1 结果如图9(a)所示，路径中只考虑了长度因素，由于路径中并没有障碍物.所以此次路径的转弯次数也为0，但路径中的高度均方差较大，得到的路径综合指数较高.由图9(b)可知，实验2 较实验1 可明显看出虽然路径的转弯次数增多，路径长度增加，但高度均方差减少，所得的综合指数也较低.图9(c)中，实验3 忽略路径长度这一因素，可得路径明显绕路，得到一条高度均方差较低的路径.在实际环境中，可以根据不同的工况，选择不同的路径因素权值，从而满足不同的需求.

图9 多地形无障碍实验图Fig.9 Experimental map of accessible multi-Terrain grid

5 结论

本文针对机器人无法应对复杂多变的路面环境等问题，综合考虑了路径长度因素，路径平缓因素和平滑因素，并通过引入人工势场法重新构造路径长度启发函数，加入势场力递减系数，从而解决蚁群算法迭代时间长且易陷入局部最优解的问题.算法提出初始化信息素不均匀分配方法，克服了蚁群算法早期搜索的盲目性.最后，利用动态切点法对生成的路径进行平滑处理，提高了路线质量.仿真实验表明，对于20×20栅格地图，改进算法减少了16.67%的转弯次数，1.6%的路径长度.对于30×30栅格地图，改进算法减少了22.2%的转弯次数，降低了9.2%的高度均方差且比文献[20]缩短了4.7%的路径长度，综合指数最佳.证明了算法具有较好的适应力，能够有效解决机器人路径规划问题.在后续的研究中，可以依据动态窗口法对动态的障碍物进行避障等.