基于势场-蚁群算法的平滑机器人路径规划

彭 湘，向凤红，毛剑琳

(昆明理工大学信息工程与自动化学院，云南，昆明 650500)

1 引言

随着机器人智能化的不断发展，移动机器人逐渐从最初仅应用于工业产业到如今广泛应用在教育、娱乐、医疗等环境中[1]。因此人们对机器人的性能要求也越来越高，机器人在环境中的行走精确度要求也逐渐提高。移动机器人路径规划是指机器人在已知或未知环境中，规划出一条从起始点到目标点的无碰撞路径[2-3]，此路径可以存在一个或多个约束条件，例如时间最优，路径最优或次优等。路径规划主要分为全局路径规划和局部路径规划[3]，全局路径规划主要针对静态环境，环境中障碍物是静止的且位置确定；局部路径规划主要靠机器人携带的传感器获取周围环境信息，主要应用于障碍物不断变化的动态环境中。

针对移动机器人的路径规划，国内外学者提出了一系列方法[5]，常用的包括一些传统算法，如Dijkstra算法、模拟退火算法，以及近年来新兴的智能仿生算法[6]，如遗传算法、蜂群算法、鱼群算法等。不同的算法均根据不同的性能指标有着不同的优缺点。蚁群算法最早是由Marco Dorigo等人于1991年提出[7]，是一种通过模拟蚂蚁的觅食行为，在给定空间进行路径规划的智能仿生算法，因其具有良好的鲁棒性、正反馈性以及易与其它算法结合的优点，广泛应用于机器人的路径规划中。文献[8]针对复杂环境下的路径规划，改进了传统蚁群算法，但规划的最优路径存在尖峰，算法的全局能力也有待提高；文献[9]利用聚类算法对环境的识别能力，提出了一种自适应搜索半径蚁群路径规划算法，解决了蚁群全局收敛速度较慢问题，但算法并未考虑蚁群搜索时陷入局部最优的情况；文献[10]对栅格环境下机器人的规划路径进行平滑处理，减少了路径的转折次数和转折角度，但文中平滑方法并未达到完全消除路径拐角的作用，优化后的路径仍然存在拐角。

鉴于以上特点，本文提出一种势场-蚁群优化(Improve Ant Colony Optimization with Artificial Potential Field，IACO-APF)平滑机器人路径规划方法，主要思想是：考虑机器人所受的势场合力和下一节点到目标点的距离，综合构造启发信息，不仅提高了算法的全局性和收敛速度，多种因素共同作用还能避免蚁群陷入“死锁”；将信息素限制在一定范围内，防止机器人在搜索过程中因信息素过高陷入局部极小值；最后引入三次B样条曲线对最优路径做平滑处理，消除了路径拐角，机器人的行走也更加流畅。

2 建模

2.1 问题假设

为使移动机器人更精准、有效地规划出一条符合实际工作环境的路径，这里对机器人的工作环境作如下假设：①机器人仅静止和匀速运动两种状态，且可在两种状态间自由切换；②机器人自身携带传感器以获取周围环境信息，机器人在工作时间内均能检测到附近障碍物位置；③忽略环境中除障碍物以外的力对机器人的影响；④机器人工作空间为二维有限空间。

2.2 环境建模

首先建立移动机器人的路径规划模型。假设机器人工作空间是一个二维平面，已知障碍物静止且位置确定，为了更好的模拟实际工作环境，本文采用栅格法[11]来划分移动机器人的工作环境。栅格法是机器人建模中的常用方法，但栅格法只能近似的模拟机器人的工作环境，因此这里采用膨胀法对障碍物进行处理，即对栅格内不足一个栅格的障碍物按一个栅格处理。

图1 膨胀法处理障碍物

其中白色栅格表示机器人可自由通过的区域，黑色栅格表示障碍物，机器人无法通过此区域。栅格大小为R。以栅格图左下角为原点建立笛卡尔坐标系，从左至右为x轴，从下至上为y轴，对建立的栅格图从左至右，从上至下编码，依次为1、2、3…..。

图2 栅格图编码

3 融合算法

单一算法本身往往存在各种不同的缺陷，而算法之间根据算法特性互相融合可以弥补算法的缺陷，得到更优的效果。蚁群在觅食时的搜索机制与移动机器人进行路径规划的相似性使得蚁群算法在机器人路径规划中有较好的应用，但传统蚁群算法在前期搜索时由于信息素浓度较低，启发信息仅受下一可能前往的节点作用等因素，导致搜索存在“盲目性”，收敛速度较慢，并且容易陷入“死锁”，从而停滞搜索。在规划过程中，随着迭代次数增加，信息素浓度逐渐升高，蚁群又极易出现“早熟”现象。因此本文提出势场-蚁群算法，利用人工势场力的作用，使得蚁群在搜索前期，主要靠加入势场合力的启发信息作用，能够加快蚁群前期的搜索速度，并在信息素更新时限定信息素范围，避免蚁群因过快收敛而陷入局部最优，提高路径规划的效率。

3.1 人工势场法

根据人工势场法的思想，移动机器人在工作环境中受虚拟势场的影响，机器人受到目标点引力和障碍物斥力的作用，从而规划出一条无碰撞的路径。在任一位置处，机器人受到的引力势场和斥力势场的和为

Usum=Uatt+Urep

(1)

其中，引力势场表示为

(2)

其中：ξ是尺度因子，d2(P，G)表示物体当前位置Pi到目标点G的距离。对引力势场求导即可得到引力：

Fatt=-ΔUatt(P)=ξd(P，G)

(3)

斥力势场表示为

(4)

其中：d表示移动机器人到障碍物的距离，d0表示障碍物斥力场的作用范围。同理，斥力表示为

Frep(P)=-∇Urep(P)

(5)

机器人受到的势场合力表示为

Fsum=Fatt+Frep

(6)

图3 人工势场受力图

3.2 改进势场-蚁群算法

3.2.1 构造启发信息函数

传统蚁群算法启发信息函数表示为：

(7)

由于传统蚁群算法在构造启发信息函数仅考虑蚂蚁当前节点i到下一节点j的距离dij，导致蚂蚁的搜索全局性大大下降，故本文从算法的全局性出发，在启发信息函数中考虑人工势场合力、目标点在全局产生的引力势场对机器人产生的吸引力，以及下一节点j与目标点G间的距离djG，综合构造新的启发信息函数为

(8)

3.2.2 信息素更新

蚁群在完成一次迭代，所有的蚂蚁都到达目标点以后，需要对全局信息素进行更新。为了避免蚂蚁在搜索过程中因为某路径信息素过高而陷入搜索停滞状态，本文受最大最小蚂蚁系统(MMAS)[12-13]启发，在信息素更新时，将信息素限定在一定范围，用公式表示为

(9)

其中：τmin为规定最小信息素浓度，τmax为规定最大信息素浓度。当信息素小于最小信息时速浓度或大于最大信息素浓度时，信息素就会被按照式(9)重置。

4 平滑路径规划实现

由于栅格法的局限性，移动机器人只能从一个栅格的中心点移向另一个栅格的中心点，因此规划出的最优路径往往是一条带有尖峰的不平滑路径，若存在路径拐角过大或拐角过多带来的外力作用可能会造成机器人不必要的能量损失，不符合实际环境对机器人的性能要求。为了更好的模拟实际环境，得到一条平滑且优于最优路径的结果，本文引入三次B样条曲线对最优路径拐点尖峰作平滑处理。

4.1 三次B样条曲线

B样条曲线具有可微性、凸包含性以及局部可修改性等特点，能够满足曲线节点间的状态约束[14]。其中最广泛使用的是三次B样条曲线，三次B样条曲线在节点具有二阶连续性，满足机器人在运动时速度与加速度连续，且曲线落在控制点所形成的三角形内[15]，因此可将三次B样条曲线用于路径规划。三次B样条曲线特性如图4所示。

图4 三次B样条曲线特性

由n+1个控制点Pi构成的k次B样条曲线方程的数学表达式为：

(10)

其中：Ni，k(u)表示第i个k阶的B样条基函数，表示为

(11)

当k为3时，此时即为三次B样条曲线，基函数表示为

(12)

因此，三次B样条曲线方程为：

0≤u≤1

(13)

4.2 路径平滑实现

三次B样条曲线平滑路径流程图如图5所示。

图5 三次B样条曲线平滑路径

5 仿真

5.1 算法步骤

Step1：栅格法建模，设置移动机器人的起始点S、目标点G；初始化势场-蚁群算法各参数；设置三次B样条曲线参数；

Step3：判断蚂蚁是否到达目标点G：若是，则一次迭代结束，否则，继续搜索；

Step4：完成一次迭代后，更新全局信息素；

Step5：判断当前迭代次数Nc是否达到最大迭代次数Ncmax，若是，则输出最优路径，否则，转到Step2，直到Nc=Ncmax；

Step6：对输出的最优路径作平滑处理，输出最终优化结果。

5.2 仿真及分析

本文采用MATLAB 2014a为仿真工具，移动机器人工作环境设置为mxn的栅格地图，起始点栅格序号为1，终止点栅格序号为mxn。设定仿真参数为：Ncmax=200次，蚂蚁个数m=50只，信息素因子α=1，启发因子β=7，信息素挥发系数ρ=0.3，信息素增强系数Q=1。为了验证算法的可行性，首先在10x10的栅格地图环境下进行仿真，仿真结果如图6所示。

图6 势场-蚁群算法(10x10)

可以看到，在仿真结果图6中，算法在10x10栅格环境下运行成功且找到一条从起始点到目标点的有效路径，路径长度为14.7279，验证了本文提出的算法是有效可行的。

为了进一步验证算法的性能，实验继续在20x20的栅格环境下对比传统蚁群算法和本文提出的势场-蚁群算法。仿真结果如图7、图8所示。

图7 传统蚁群算法(20x20)

图8 势场-蚁群算法(20x20)

对比图7、8仿真结果，传统蚁群算法收敛图曲线在找到最优路径后仍旧出现了几次波动，此后才趋于平稳，最终迭代28次后进入收敛状态，而改进势场-蚁群算法仅用6次迭代就已收敛，迭代次数上减少了78.6%。在路径长度方面，传统蚁群算法路径较改进势场-蚁群算法长1.1715，并且传统蚁群算法规划的路径拐点较多，假设将此应用在实际环境中，拐点过多会导致机器人在行走过程中耗能较多，而本文算法规划的路径拐点数仅为4，较传统蚁群减少了69.2%。仿真结果表明，改进势场-蚁群算法在搜索效率和收敛速度上较传统蚁群算法都有较大改进，同时改进势场-蚁群算法规划出的路径更短，路径拐点也大大减少。实验在20x20栅格环境下的运行情况对比见表1。

表1 算法结果对比

针对算法规划的最优路径，采用三次B样条曲线优化的最终结果如图8所示，可以看出，针对拐点处的尖峰路径，通过设置三次B样条曲线的控制点，可以使平滑后的路径成功落在控制点与拐点包围的三角形内，并且路径长度比原最优路径减少了9.0603，对比文献[16]使用的平滑方法，采用三次B样条曲线处理路径能够完全消除路径上任意拐点，达到100%平滑路径的目的。

图9 平滑优化结果

6 结束语

针对机器人在静态环境下的全局路径规划，本文在传统蚁群算法的基础上提出势场-蚁群融合算法。实验结果表明，相较于传统蚁群算法而言，势场-蚁群算法在收敛速度、全局性等问题上都有较大的改进，同时算法还解决了传统蚁群算法存在的易陷入“死锁”和局部最优解的问题。在规划出最优路径后，采取三次B样条曲线对路径进行优化处理，完全平滑了路径尖峰，还缩短了路径长度。整个算法在优化后运行效率和运算结果都得到了大大的提升。在今后的后续工作中，将针对参数变化和动态环境对规划过程带来的影响等问题做进一步研究。