鲁 飞, 鲁照权, 牛 晨, 孙伟业, 詹浩东
(合肥工业大学 电气与自动化工程学院,安徽 合肥 230009)
近年来,路径规划一直是机器人研究领域中的热点问题,其核心要求是按照特定的优化准则如距离最短、时间最短、能耗最低等,搜索出一条从起始点到目标点的最优安全路径。目前,国内外已经提出了很多算法用于机器人路径规划,如A*算法[1]、人工势场法等,但大部分研究都是基于二维环境,对于环境比较复杂的三维空间存在着不少局限性。随着研究的深入,有学者提出了智能仿生算法以及它们改进算法的应用,如蚁群算法[2]、遗传算法[3]、神经网络算法[4]等。其中蚁群算法由于具有较强的适应性和鲁棒性,又易与其他方法结合,在机器人路径规划领域得到了广泛应用,但蚁群算法也存在收敛速度慢、易出现停滞和局部收敛等缺点。文献[5]、文献[6]分别将蚁群算法和遗传算法、粒子群算法相结合,提高了算法寻优能力,具有一定的灵活性;文献[7]通过对初始信息素的差异化分配,并在概率转移函数中引入转角启发信息,提高了蚁群搜索效率,但没有应用于三维环境中;文献[8]在蚂蚁状态转移的过程中引入稀疏性约束进行改进,有效减少了蚁群的搜索时间,但算法的运行速度有待进一步提高。为使蚂蚁尽快找到最优解,
本文以路径的适应度值为优化标准,对蚁群算法进行了改进,并运用在三维空间的路径规划上,通过MATLAB仿真证明了改进后的蚁群算法有一定的优越性。
采用栅格法对三维路径规划空间进行建模。模型抽象方法如下:将三维地图左下角顶点作为坐标原点A,建立三维坐标系,以A为顶点,分别沿x轴、y轴和z轴方向取三维地图的最大长度AB,AD,AA′构造立方体区域ABCD-A′B′C′D,如图1所示。
图1 三维空间划分
利用平面对三维空间进行均匀划分,从中抽取出三维路径规划所需要的离散点,首先沿AB边将空间ABCD-A′B′C′D进行n等分,得到n+1个平面Πi(i=1,2,…n),再对这n+1个平面分别沿AD边进行m等分,沿AA′进行l等分,得到空间中的各个交点。平面划分如图2所示。
图2 平面划分
通过上述步骤,整个规划空间ABCD-A′B′C′D被分解成空间离散点的集合,这些离散点便构成蚁群算法搜索的各条路径。
为了降低路径规划的复杂程度,在算法中采取一种分层前进与栅格平面法相结合的搜索模式[9],如图3所示。
图3 搜索可视区域
其思想是: 设定x轴方向为机器人前进的主方向,在该方向上以单位距离划分层次[10],从当前节点建立可视域,在前向运动一定距离Lx,max情况下,设定机器人最大横向移动距离为Ly,max,最大纵向移动距离Lz,max。这样,当蚂蚁由当前节点转移到下一节点时,搜索范围被限制在可视区域内,从而简化了路径搜索的复杂度。
传统蚁群算法由于各路径初始信息素浓度相同,使蚂蚁在搜索过程中行走的随机性太强,收敛速度太慢,且信息素的载体为节点间的线段,大大增加了算法的空间复杂度。本文改进算法将信息素存储在离散点中,并在初始化时对各离散点信息素进行不均匀分配,目的在于减小算法复杂度,并提高算法初期的搜索效率。具体方法如下:以线段L连接起始点S和目标点D,由于最优路径多集中在L附近区域。因此,以L与各平面Πi(i=1,2,…,n)的交点为圆心,Lm为半径,构建较优离散点域,根据区域内各离散点到直线L的距离,计算各个离散点的初始信息素值,数学表达式如下
(1)
(2)
式中τ0为离散点初始信息素浓度;Ly,max和Lz,max分别为最大横向移动距离和最大纵向移动距离;len(Pa,L)为节点Pa到直线L的距离;λ为地图比例因子,根据地图具体参数来确定。
启发函数是蚁群算法中的重要组成部分,其作用是利用距离信息引导蚂蚁选择最短路径,直接影响到算法的收敛性、稳定性以及最优性[11]。但蚂蚁搜索节点时往往会忽视周围障碍物因素,优先选择与当前节点最近的离散点,从而陷入局部最优。针对此问题,本文增加可行性策略构造新的启发函数,同时引入动态夹角因素,使蚂蚁对最优路径的搜索更具有方向性。本文构造启发函数由以下四部分组成:
1)可行性因素[12]
根据三维空间内离散点的可行性,对规划空间中各节点的可视区域离散点计算可行性因素权值。数学表达式如下
(3)
式中s(ia+1,ja+1,ka+1)为可视域R(Pa)中各离散点的权值,可行点权值为1,否则为0。定义可行性因素S(ia,ja,ka)的计算公式如下
(4)
Num1=∑S(ia+1,ja+1,ka+1)
(5)
式中Num为在点(ia,ja,ka)上可视点的数量;Num1为可视点中可行离散点的数量。
启发函数中引入可行性因素虽然使得路径规划的安全性提高,但同时也增加了算法的复杂度,原因在于选择下一节点时,要对可视域中所有离散点进行扫描,计算出所有的可行离散点。为解决此问题,本文选择在建模阶段就计算出可视域中各离散点的可行性权值,在计算启发函数时直接调用相应权值即可,有效节省了计算时间。
2)相邻节点路径长度
D(ia,ja,ka)为两节点间路径长度,促使蚂蚁选择距离较近的点;D(ia,ja,ka)的计算公式如下
D(ia,ja,ka)=
(6)
式中 (ia,ja,ka)为当前节点的具体坐标,(ia+1,ja+1,ka+1)为下一点坐标。
3)距目标点路径长度
Q(ia,ja,ka)为下一点到目标点的路径长度,促使蚂蚁选择距离目标更近的点,Q(ia,ja,ka)的计算公式如下
Q(ia,ja,ka)=
(7)
其中,(iD,jD,kD)为目标节点坐标。
4)夹角因素
图4 夹角示意
(8)
夹角因素A(ia,ja,ka)定义为
(9)
式中δ为一个大于零的角度参数,防止θa(ia,ja,ka)为零。ω为角度因素系数,由式(8)可以看出,在当前节点Pa(ia,ja,ka)可视域内选择下一节点Pa+1(ia+1,ja+1,ka+1)时,若夹角θa(ia,ja,ka)趋向于一个较小的角度,则对应的路径是较优的。由于在路径规划初期可视区域距离目标点D较远,ω应取较小值以避免角度因素的盲目诱导,提高算法初期的路径多样性;在路径规划后期距离目标点较近时,ω的值应该相应增大来提高角度因素的影响。因此,ω在算法迭代运算过程中是一个动态参数,定义如下
(10)
式中ω0为夹角因素系数初值,len(Pa,S)为当前点与起始点的距离,len(S,D)为起始点和目标点的距离。结合上述因素,本文构造启发函数H(ia,ja,ka)如下
Γ=D(ia,ja,ka)·Q(ia,ja,ka)
(11)
H(ia,ja,ka)=Γ·S(ia,ja,ka)·A(ia,ja,ka)
(12)
经过分析,由于在迭代初期各离散点的信息素浓度不同,此时,信息素权重因子应在路径选择中占主导地位,即α的值应较大,β的值应较小。随着迭代的不断进行,最优路径上的信息素浓度逐渐远高于其他路径,为防止算法陷入局部最优,应逐渐减小信息素浓度在路径选择中的重要程度,即α值逐渐减小,β值相应增加,从而有助于找到全局最优解[13]。因此,本文将信息素因子α和启发函数因子β的值改为动态参数,随着迭代的进行而做出改变,为使α,β的取值变化更加平稳,分别采用余弦和正弦函数进行赋值,如式(13)、式(14)所示
(13)
(14)
式中Nmax为迭代总次数,N为当前迭代次数,则改进后的概率转移为
(15)
当蚁群完成一次路径搜索,以长度作为评价值,将所有蚂蚁的路径长度按照升序排列,只选择排在前面的部分蚂蚁进行信息素更新,更新规则如下
τijk=(1-ρ)τijk+ρΔτijk
(16)
(17)
式中len(k)为第k只蚂蚁经过的路径长度;rank(k)为蚂蚁k的排名;μ·M为要更新的蚂蚁数量;ρ为信息素挥发因子;Q为信息素常量。
为了更好地平衡全局寻优能力和局部寻优能力,本文根据文献[14]中的自适应方法调整ρ,ρ的初始值设置为0.9,当算法在连续v次迭代内没有更新最优值,ρ按照式(18)调整,计算公式如下
(18)
式中ρmin为预先设置的信息素挥发速率的最小值,以防止ρ过小而导致收敛速度太慢。
改进算法的具体步骤如下:
Step1 构建三维工作环境,确定起始点S和目标点D,并初始化相关参数。
Step2 信息素初始化,通过式(1)得到各节点的信息素初始值,对图中各节点进行信息素的不均匀分配。
Step3 搜索最优路径,在起始点S放置M只蚂蚁,将起始点S放入禁忌表[15]中,根据式(15)计算每只蚂蚁对下一平面中各节点的状态转移概率,并采用轮盘赌法选择下一节点。
Step4 更新禁忌表,并判断是否到达终点D,当所有蚂蚁完成一次搜索后,将路径长度按照从小到大的顺序排列,由式(17)确定要更新的蚂蚁数量,并采用式(16)更新相应路径上各节点的信息素值。
Step5 检验最优解的更新情况,若最优解在连续v次迭代中未发生变化,按式(18)调整信息素挥发因子。
Step6 判断是否达到最大迭代次数,若是,则输出全局最优路径长度;若否,则清空禁忌表,转到Step3继续执行。
为了验证文中改进算法是否有效,在MATLAB 2018a中进行仿真实验,并将结果与文献[10]进行对比。
规划空间为21 km×21 km×2 km,其中x轴,y轴方向每个节点间距1 km,z轴方向每个节点间距200 m。设置路径起点在规划空间的序号为(1,11,3),目标点序号(21,10,5)。
本文改进算法的各项参数如表1所示。
表1 实验参数取值
根据以上参数进行三维路径规划实验,通过设置相同参数,与文献[10]仿真结果图对比如图5。其中,三角形曲线为文献[10]算法所得到的最优路径,正方形曲线为本文改进算法得到的最优路径。
图5 三维路径规划结果对比
由图5的适应度变化曲线可以看出,改进后的算法在得到更优解的同时,迭代次数有了明显减小,证明了本文对蚁群算法的改进是有效的。为保证结果的准确性,分别运用本文改进算法与文献[10]算法进行多次实验,随机抽取4组实验结果,如表2所示。
表2 实验结果
为验证算法在复杂环境下的适应性,更换地图环境并改变起始点和目标点的坐标,设置起始点和目标点坐标序号分别为(1,17,3)和(21,15,6),再次进行多组实验,其中一组实验结果如图6。
图6 场景二实验结果对比
随机抽取4组实验结果绘制成表格如表3所示。
表3 场景二实验结果
从实验结果可以看出,引入夹角因素和可行性因素后,算法规划出的路径安全性与方向性都得到了增强,由此可以证明本文提出的改进策略使蚁群算法搜索最优解的能力得到了提高,能保证在快速收敛的情况下依然具有较强的全局搜索能力,有效克服了传统蚁群算法收敛速度慢,易陷入局部最优的缺点。
本文针对蚁群算法在三维路径规划中存在的搜索效率低,易陷入局部最优等问题,对蚁群算法的初始信息素分配,启发函数,权重因子以及信息素更新规则提出了改进,通过栅格法对三维环境进行抽象建模,仿真结果表明:改进后的蚁群算法能够快速有效地实现三维空间中的路径规划,同时具有较强的全局寻优能力,相比传统蚁群算法具有一定的优越性,不足之处在于算法的运行时间相对较长,后续会对如何提高算法运行速度展开进一步研究。