融合人工势场蚁群算法的移动机器人路径规划

李志锟， 赵倩楠

(1.广东科技学院机电工程学院，广东 东莞 523000；2.高端装备先进感知与智能控制教育部重点实验室，安徽 芜湖 241000)

0 引言

路径规划是移动机器人依赖智能算法从起点到目标位置规划出一条最优路径。智能算法的应用可有效提高移动机器人路径规划效率，目前已有大量智能算法应用于移动机器人的路径规划，如D*算法[1]、蚁群算法[2-6]、人工势场法[7]、人工鱼群算法[8-10]等。每种算法都有自己的优点和缺点，利用不同算法的优点进行融合，为解决移动机器人路径规划提供了新的方法，如人工势场法与蚁群算法之间的结合、粒子群算法和A*算法之间的结合、模拟退火算法和改进人工鱼群算法之间的结合等。蚁群算法是通过研究蚂蚁觅食规律而得出的概率型算法，蚁群算法相比其他智能算法具有更强的鲁棒性和正反馈性；分布式搜索方法使得蚁群算法擅长解决组合优化问题，信息素的分泌增强蚁群选择局部最优路径的概率，当全局非最优路径上的信息素较多时，由于蚁群的正反馈性，将吸引越来越多的蚂蚁选择全局非最优路径，降低了移动机器人路径规划能力；而蚁群算法和人工势场算法的融合，能够提高算法收敛速度以及缩短全局最优路径长度。

人工势场法[11]通过设计一种人造力干预移动机器人的运动，有利区域对移动机器人的吸引作用，障碍物及不利区域对移动机器人的排斥作用，最后通过合力来引导移动机器人完成最优路径搜索，利用人工势场法规划出的路径基本可以避免死锁现象。作为分布式算法，传统蚁群算法虽然有足够强的鲁棒性，但是收敛速度较慢，且利用蚁群算法[5，12-13]搜索出的最优路径较长，甚至可能陷入局部最优。为解决传统蚁群算法存在的这些问题，本文融合人工势场算法，对传统蚁群算法进行改进，构造势场力启发函数，同时改进信息素更新策略，融合人工势场法的蚁群算法[14-17]相对传统蚁群算法，具有较强的全局路径搜索能力，能较快地完成起始点到终点的路径规划。

1 人工势场法的基本原理

人工势场法的基本原理为在移动机器人路径规划区域内引入模拟的势力场。当移动机器人从起点向终点移动时，受到模拟的吸引力或排斥力的作用，影响移动机器人路径的选择。如图1所示，终点产生一个吸引力，吸引移动机器人朝着终点移动，阻碍移动机器人通过的障碍物产生排斥力，避免移动机器人碰撞障碍物，多个排斥力和引力的综合作用下，移动机器人将沿着合力方向移动。移动机器人受到的排斥力的大小和移动机器人与障碍物之间的距离有关，距离越长，则移动机器人受到障碍物的排斥力就越小；同理，终点对移动机器人引力的大小和移动机器人与终点的距离成正比，距离越长，则终点对移动机器人的引力就越大。移动机器人在搜索路径时，目标点对它的引力保证移动机器人朝着终点方向移动，而障碍物对它的排斥力则避免移动机器人与障碍物发生碰撞。

图1 人工势场法路径规划图Fig.1 Path planning diagram of artificial potential field method

设移动机器人在二维空间中的当前位置X坐标为(x,y)，目标点Xd坐标为(xd,yd)，当移动机器人在模拟的势力场内移动时，引力势场函数为

(1)

式中,kp表示位置增益系数。引力势场对移动机器人产生的引力为引力势能的负梯度方向，即

Fa=-kp·[(x-xd),(y-yd)]。

(2)

由式(2)可以看出，移动机器人距离终点越近，终点对移动机器人的引力则越小。定义斥力场函数为

(3)

式中：m为斥力场的增益系数；P0为障碍物的影响范围；ρ(X,X0)为移动机器人和障碍物之间的最短直线距离；当移动机器人到障碍物的直线距离小于P0，障碍物对移动机器人产生的斥力有效。斥力势场对移动机器人产生的斥力为

Fr=-grad(Ur(X))

(4)

(5)

由式(5)可以看出，移动机器人距离障碍物越近，则产生的排斥力越大，因此，人工势场作用在移动机器人上的合力Ft为

Ft=Fa+Fr。

(6)

移动机器人将在引力及斥力的合力作用下移动，并最终到达终点Xd。

2 融合人工势场法的蚁群算法

2.1 改进斥力函数

当终点附近的障碍物较多时，由于移动机器人距离障碍物较近，受到障碍物的排斥力较大，而此时，移动机器人与目标点距离较小，目标点对移动机器人的吸引力也随之减小，可能导致移动机器人无法到达目标点。改进后的斥力场函数为

(7)

λ=(X-Xd)n

(8)

其中，n为正整数。从式(8)可以看出，当移动机器人靠近目标点时，同时也在靠近与目标点距离较近的障碍物，目标点距离影响因子λ的值也随之变小，有效避免移动机器人在目标点附近受到过大的斥力，相应的斥力算式为

(9)

(10)

(11)

其中，Fr1及Fr2两个分力表示的方向不同，Fr1是由障碍物指向移动机器人的矢量，Fr2是由移动机器人指向终点的矢量。

对改进后的人工势场算法进行仿真实验，移动机器人的运动轨迹如图2所示。图3所示为对目标点附近放大显示。图2、图3中，小圆圈代表障碍物，实心点组成了移动机器人路径规划轨迹，三角形代表目标点位置。图2、图3中坐标均无单位。移动机器人起始点坐标为(0,0),目标点坐标为(10,10)，移动机器人避开了所有的障碍物，斥力函数有利于移动机器人避开障碍物，但也可能导致移动机器人偏离目标点，因此，为了解决势场力的负面影响，引入了目标点距离影响因子λ，动态调整移动机器人受到的斥力，改善势场力对于移动机器人路径搜索的影响。即便在目标点附近放置较多障碍物，改进人工势场算法通过改进斥力场函数，避免移动机器人受到较大的斥力而无法规划出最优路径。

图2 改进人工势场算法机器人轨迹图Fig.2 Robot trajectory diagram of the improved artificial potential field algorithm

图3 轨迹放大图Fig.3 The enlarged trajectory

2.2 构造势场力启发函数

传统蚁群算法构造出的启发函数仅考虑节点之间的距离，即

(12)

式中，di j表示当前节点位置i与下一步可到达节点j之间的欧氏距离，位于节点i的蚂蚁k选择节点j作为下一步要到达的位置。位置选择概率为

(13)

式中：ak表示蚂蚁k下一步可选择到达位置的集合；τi j表示路径(i,j)上的信息素值；ηi j表示启发函数；α表示信息素启发因子；β表示启发函数ηi j的权重参数。di j的值越小，该启发函数的值就越大，则节点i选择节点j的概率就越大。

根据式(12)可知，传统蚁群算法的启发函数仅仅与移动机器人当前位置到下一节点位置的距离有关，当终点附近存在大量障碍物时，由于传统蚁群算法的启发函数考虑因素较少，可能使得传统蚁群算法陷入局部最优，由于蚁群算法的正反馈性，当初代蚂蚁未能寻找到最优路径，同时蚁群分泌大量的信息素吸引其他蚂蚁选择该路径，最终蚂蚁搜索出的路径可能较长，甚至规划出错误的路径。为了解决传统蚁群算法存在的问题，通过人工势场法搜索到的节点与终点的距离信息来改进启发函数，在移动机器人路径搜索的后期，为了避免势场蚁群算法陷入局部最优，降低启发函数对移动机器人路径搜索的影响，引入启发函数递减参数φ，移动机器人越接近终点，启发函数起的作用就越小。在考虑距离启发信息之外，当移动机器人在路径搜索时遇到复杂的障碍物环境，移动机器人要绕开障碍物，同时要搜索出最优路径，因此，也要考虑势场启发信息，改进后的启发函数为

(14)

式中：ηF(t)为势场启发函数；ηd(t)为距离启发函数；φ为大于0小于1的常数。为了保证移动机器人搜索到的路径足够短，在移动机器人的可选节点中，移动机器人优先考虑距离目标点更近的节点，用移动机器人下一步要选择的节点j到目标点e的距离dje替代传统蚁群算法的di j，改进后的距离启发信息为

(15)

势场启发函数为

ηF(t)=aFt·cos θ

(16)

式中：a是大于零的常数；θ为移动机器人在当前位置i指向下一个节点j的方向与势场合力Ft方向的夹角。人工势场力的存在，引导移动机器人避开障碍物，朝向终点移动，提高了算法的收敛速度，但是也更容易陷入局部最优解，融合人工势场的蚁群算法引入势场合力的衰减系数ξ，随着融合人工势场的蚁群算法不断迭代，势场合力逐渐衰减至零，则根据式(16)可知，势场启发函数ηF(t)趋向于数值1，势场合力的衰减系数ξ为

(17)

式中：Nmax为最大迭代次数；Nk为当下迭代次数。因此，改进后的启发函数为

(18)

式中，F1为改进后的势场合力，其算式为

(19)

2.3 信息素更新策略

传统蚁群算法在蚁群初始化时，分布均匀的信息素，移动机器人在路径规划前期，各栅格位置的信息素值差异不大，导致移动机器人盲目搜索。本文提出信息素不均匀分配方式，增加移动机器人起始点指向终点方向所在区域的信息素浓度，信息素浓度沿着垂直于移动机器人起始点到终点方向逐步衰减，如图4所示。

图4 初始信息素分配方式Fig.4 Initial pheromone distribution mode

图4中，移动机器人起始点到目标点连接的直线函数表达式为

y=-x+C

(20)

式中，C为正整数。当该直线遇到障碍物时，障碍物所在的栅格位置信息素浓度值设置为零，初始信息素分布方式为

(21)

式中：T为初始信息素调整参数；坐标系中y=-x+C代表的直线穿过所有非障碍物栅格的初始信息素浓度值为T，初始信息素的差异化分配对于提高算法的收敛速度具有明显作用。

当移动机器人完成从节点i到节点j的移动，需要对移动机器人行走过的路径进行局部信息素更新，局部信息素更新规则为

τi j=(1-ρ)τi j+ω·τ0

(22)

式中：ω为参数，ω在(0,1)中取值；ρ表示信息素挥发系数，且ρ在(0,1)中取值；τ0表示信息素初始值。在路径搜索过程中，移动机器人每经过路径(i,j)时，均按照式(22)进行局部信息素更新，当移动机器人完成一次路径迭代后，对所有路径上的信息素做全局更新，蚂蚁系统的全局信息素更新方式为

(23)

(24)

随着算法迭代次数的增加，当信息素集中于非最优路径时，由于蚁群算法的正反馈作用，移动机器人在路径搜索时更倾向于选择信息素多的路径，最终，算法收敛于非最优解，为解决这一问题，降低信息素的干扰，引入信息素调节系数，改进后的全局信息素更新方式为

(25)

式中，e1表示参数。随着Nk的增加，信息素调节系数逐渐减小，避免算法收敛于非最优解。

3 融合人工势场蚁群算法的实现流程

融合人工势场蚁群算法的实现步骤如下：

1) 初始化融合人工势场蚁群算法的所有参数，建立栅格地图环境，设置蚁群的起始点、终点，根据式(21)进行信息素不均匀分布；

2)m只蚂蚁从起始点开始搜索路径；

3) 将构造的势场力启发函数算式(18)、重新设计的信息素更新算式(25)代入状态转移概率算式(13)，选择蚂蚁要到达的节点位置；

4) 判断所有蚂蚁是否避开所有障碍物到达终点位置，若是，存储本次迭代产生的最优路径，若不是，返回3)；

5) 根据算式(13)对每条路径更新信息素浓度值，根据算式(18)更新势场力启发函数；

6) 判断是否完成所有迭代，如果达到最大迭代次数，存储并输出最优路径，否则返回2)。

4 算法仿真实验

4.1 实验1：20 m×20 m仿真环境

在20 m×20 m栅格仿真环境下，对本文提出的融合人工势场蚁群算法及文献[14]算法分别进行仿真实验。对比路径轨迹如图5所示。移动机器人起点和终点坐标分别为(0.5 m,19.5 m)，(19.5 m,0.5 m)。图5(a)为应用融合人工势场蚁群算法规划出的最优路径轨迹，最优路径长度为30.38 m，移动机器人在搜索路径时转折次数为12。图5(b)为采用文献[14]算法规划出的最优路径轨迹，最优路径轨迹长度为30.38 m，移动机器人在搜索路径时转折次数为14。显然，采用融合人工势场蚁群算法规划出的最优路径转折次数更少，路径更加平滑。

图5 两种算法的路径轨迹(实验1)Fig.5 Path trajectory diagram of two algorithms(Experiment 1)

图6为采用人工势场蚁群算法规划出的各代最优路径轨迹，可以看出，各代最优路径均避开所有障碍物，且各代轨迹均集中于起点指向终点的直线附近，实验数据如表1所示。

图6 融合人工势场蚁群算法各代最优路径轨迹(实验1)

表1 20 m×20 m栅格环境实验结果对比

两种算法收敛曲线如图7所示。由图7(a)可知,移动机器人在第6次搜索路径时达到稳定状态,由图7(b)可知,移动机器人在第30次搜索路径时达到稳定状态。采用人工势场蚁群算法在收敛速度方面比文献[14]算法提高了80%，由于人工势场蚁群算法采用全新的信息素不均匀分布策略，对距离启发信息及势场启发信息同时改进，构造了全新的势场力启发函数。因此，人工势场蚁群算法在迭代初期搜索出的最优路径长度已经比较接近全局最优路径长度；而文献[14]算法在迭代初期振荡较大，且收敛速度较慢，效率较低，不利于移动机器人路径规划。因此，采用人工势场蚁群算法规划路径速度更快，搜索效率更高。

图7 两种算法的收敛曲线Fig.7 Convergence curves of two algorithms

4.2 实验2：30 m×30 m仿真环境

为了验证融合人工势场蚁群算法在复杂环境下的路径规划能力，在30 m×30 m栅格仿真环境下，采用与文献[15]同样的栅格地图进行仿真实验及对比。移动机器人起点和终点坐标分别为(0.5 m,29.5 m)和(29.5 m,0.5 m)。图8为移动机器人采用融合人工势场蚁群算法规划出的最优路径轨迹，路径长度为43.355 3 m，最优路径转折次数为9;同一幅栅格地图下，文献[15]规划出的最优路径长度为44.526 9 m，最优路径转折次数为12。在最优路径长度方面，本文提出的融合人工势场蚁群算法相比文献[15]算法缩短了2.6%。在路径平滑度方面，本文算法相比文献[15]算法缩短了25%。图9为融合人工势场蚁群算法的收敛曲线，可以看出，第1代蚁群搜索出的最优路径长度为62.669 0 m,第2代蚁群搜索出的最优路径长度为49.598 0 m,第2代蚁群搜索出的最优路径长度相比第1代蚁群搜索出的最优路径长度大幅缩短，第3代蚁群搜索出的最优路径长度为43.355 3 m,达到全局最优路径长度。文献[15]算法的收敛次数为9，相比文献[15]算法，本文算法在收敛速度方面提高了66.7%，实验数据如表2所示。体现了本文提出的融合人工势场蚁群算法具有极强的路径搜索能力。

图8 融合人工势场蚁群算法路径轨迹(实验2)

图9 融合人工势场蚁群算法收敛曲线

图10为移动机器人采用融合人工势场蚁群算法规划出的各代最优路径轨迹，可以看出，从起点到终点存在多条路径实现最短路径长度，各代最优路径均集中于起点到终点方向的直线附近，表明本文算法的有效性及优越性。

图10 融合人工势场蚁群算法各代最优路径轨迹(实验2)

表2 30 m×30 m栅格环境实验结果对比

5 结束语

本文首先介绍了人工势场法的基本原理。为了解决势场力的负面影响，引入目标点距离影响因子，通过改进斥力场函数，避免移动机器人受到较大斥力而无法实现路径规划；为了降低启发函数对于移动机器人路径搜索的影响，引入启发函数递减参数，同时兼顾势场启发信息及距离启发信息的改进，来构造全新的势场力启发函数；为了避免融合势场蚁群算法陷入局部最优解，构造势场合力衰减系数，随着算法的不断迭代，势场合力逐渐衰减至零；采用全新的信息素不均匀分配策略，引入信息素调节系数，改进全局信息素更新方式，促进移动机器人朝着最优路径方向搜索，提高了移动机器人全局路径搜索能力。