基于蚁群与DWA融合的移动机器人动态路径规划*

张晓倩,黄 磊,石雨婷,汪建华,祁良剑

(南京林业大学机械电子工程学院,南京 210037)

0 引言

路径规划作为实现移动机器人完成自主导航的重难点技术之一,一直是该领域研究的热点[1],其作用是使移动机器人在未知环境下快速且安全地从起点移动到终点[2]。路径规划主要分为全局路径规划和局部避障规划,全局路径规划是指在已知地图下,使移动机器人自主规划出一条从起始点到目标点的最优路径,局部避障规划是指移动机器人在行驶过程中,依据激光扫描仪、运动传感器等工具对未知障碍物进行检测,使移动机器人能安全避障。其中,全局路径规划算法包含传统算法(A*算法[3]、Dijkstra算法[4])和智能算法(蚁群算法[5]、粒子群算法[6]、RRT算法[7]、PRM算法[8]、遗传算法[9]和强化学习算法[10-11]等),局部避障规划算法主要有DWA算法[12]。

蚁群算法作为一种基于自然界蚁群觅食行为的启发式仿生算法,在全局路径规划中具有较好的性能[13]。李理等[14]在启发函数中引入修正距离函数、平滑性启发函数,来提高所规划路径的综合性能;毛文平等[15]提出一种奖励惩罚机制更新信息素增量,提高了算法收敛速度;JIAO等[16]采用自适应调整状态转移概率计算公式的方法,来平衡不同时期下启发函数和信息素浓度的相对重要性,有效避免搜索过程中局部最优问题。鲁飞等[17]对信息素和启发信息的权重系数以及信息素蒸发系数采取动态调整的方法,解决传统蚁群算法收敛速度慢的问题。

DWA算法的模型较简单,且具有良好的局部避障能力。MAI等[18]在评价函数中加入障碍物密度因子,使得移动机器人通过提前感知障碍物的分布情况来提高避障效率;迟旭等[19]将改进的安全A*算法与DWA算法进行融合,解决了DWA算法易陷入局部最优的问题,但A*算法的搜索节点较多,会影响整个移动机器人的运行效率。YANG等[20]将蚁群与DWA算法融合,提高了机器人的避障效率,但是未考虑所规划路径的平滑性问题。

针对上述问题,本文提出了一种基于改进蚁群与DWA融合的动态路径规划算法,满足规划路径全局性能的同时也具有实时局部避障的能力。算法首先在传统蚁群算法中引入自适应机制改进状态转移概率和信息素更新规则,避免蚁群易陷入局部最优的同时也提高了路径搜索的效率;其次,对所规划的路径进行二次优化,提高全局路径的平滑性;然后在DWA的评价函数中添加全局路径融合子函数和障碍物安全阈值来提高移动机器人在运行时的动态避障能力。故该算法不仅能保证全局路径规划的最优性,也能成功躲避未知静态、动态障碍物,使得移动机器人可以安全高效地执行任务。

1 改进蚁群算法

蚁群算法具有较强的鲁棒性和适应性,但是其存在着搜索效率低和易陷入局部最优的问题。蚁群算法的核心内容为互相影响的状态转移概率和信息素的更新机制,其中状态转移概率是蚂蚁从一个点出发判断下一个目标点的选择过程,信息素作为蚁群间的沟通协调机制,是蚂蚁选择下一路径的依据。本文改进状态转移概率计算公式来提高算法的搜索效率;动态调整信息素的挥发系数来改进信息素更新机制,避免蚂蚁出现陷入局部最优的问题;引入路径优化策略来提高全局路径的平滑性。

1.1 改进状态转移概率

在蚁群算法中,蚂蚁的下一步行进方向是由状态转移概率决定的,而状态转移概率主要通过路径上的信息素和启发函数确定。算法前期,路径之间的信息素浓度较小,严重影响蚂蚁对路径的选择能力,从而导致算法收敛速度慢。因此本文采用伪随机状态转移规则[15],使得蚂蚁根据式(1)选择下一目标栅格。

(1)

式中:q0为选择比例,为[0,1]的随机数;δ为比例系数,Nc为当前迭代次数。前期q0较大,有利于蚂蚁根据全局路径信息来选择有利路径,可以提高算法前期的收敛速度;后期q0较小,促使蚂蚁进行随机搜索,确保算法的全局搜索优越性。

(2)

式中:τij(t)为路径(i,j)的信息素,α(t)为信息素权重系数,ηij(t)为路径(i,j)的启发函数,β(t)为启发函数权重系数,ΨiE(t)为目标影响函数,为当前节点i到目标点E欧氏距离的倒数,γ为目标影响函数的权重系数。

传统蚁群算法中,信息素权重系数和启发函数权重系数均为固定常数,易使蚂蚁进行盲目搜索。因此,本文采取文献[17]的思路,对两个权重系数进行自适应调整。

(3)

式(3)中的α(0)和β(0)的取值根据众多学者大量对比实验所得。改进后的蚁群算法在前期搜索时,启发函数的权重较高,有利于避免蚂蚁陷入局部最优;后期搜索时,信息素对蚂蚁的指导性提高,有利于蚂蚁选择最优路径。

1.2 改进信息素挥发系数

信息素越多的路径被蚂蚁选择的概率越大,但信息素会随着时间不断挥发。传统蚁群算法中,信息素挥发系数为固定常数,易使蚁群陷入局部最优,进而影响算法的收敛速度,因此本文采用动态调整信息素挥发因子的方法,使得信息素挥发因子ρ随迭代次数的增加而递减,在保证路径多样性的同时提高收敛速度,如式(4)所示。

(4)

式中:λ1为信息素挥发因子的调整系数,Nc为当前迭代次数,m为蚂蚁数量。

为了避免蚁群算法因信息素浓度太大或太小而引起算法停滞,设置信息素浓度的阈值如式(5)所示。

(5)

综上,本文改进后的信息素更新公式如式(6)所示。

(6)

式中:Δτij(t)为t时刻下所有蚂蚁在路径(i,j)上遗留的信息素增量,k表示第k只蚂蚁,m为蚂蚁总量,Q为固定的常数,表示信息素增量的初始强度;Lk为第k只蚂蚁在t时刻搜索路径的总长度。

1.3 路径优化策略

为了减少传统蚁群算法规划路径中的冗余节点,本文添加排除冗余节点机制,来减少规划路径的长度。排除冗余节点示意图如图1所示。

图1 排除冗余节点示意图

如图1所示,S为其起始点,E为目标点,由传统蚁群规划的初始路径为S→p1→p2→p3→p4→p5→E,采用路径优化策略进行检测,发现Sp1、p1p2之间未经过障碍物,所以节点p1为冗余节点,给予删除;同理,p2p5之间未经过障碍物,则节点p3、p4为冗余节点,给予排除。最终删除冗余节点后的优化路径为S→p2→p5→E。

2 改进DWA算法

2.1 传统DWA算法

DWA算法是一种基于预测控制的局部路径规划算法,通过对机器人运动学模型中多个速度空间进行采样,在特定时间内模拟出移动机器人的轨迹,最后基于评价函数从多组运行轨迹中选取最优轨迹[20]。

移动机器人运动学模型如式(7)所示:

(7)

式中:x(t)、y(t)、θ(t)分别表示t时刻下移动机器人在x、y方向的位姿信息,v(t)、ω(t)分别为移动机器人的线速度和角速度。

在速度空间中,DWA算法描述了具有约束条件下的机器人避障优化问题:

(1)机器人本身最大最小速度的限制。受机器人本身硬件方面的限制,其最大最小速度约束条件如式(8)所示。

Vs={(v,ω)|v∈[vmax,vmin]∩ω∈[ωmax,ωmin]}

(8)

(2)所用电机的性能存在最大加减速的限制,其约束条件如式(9)所示。

(9)

式中:vc、ωc为当前速度,va、ωa表示最大加速度,vb、ωb表示最大减速度。

(3)保障安全的制动距离。移动机器人遇到障碍物时,需要保证一定的安全距离,避免在速度阈值内与障碍物发生碰撞,其约束条件如式(10)所示。

(10)

常规的评价函数如式(11)所示。

G(v,ω)=σ[λ2heading(v,ω)+λ3dist(v,ω)+λ4vel(v,ω)]

(11)

式中:heading(v,ω)为方位角评价子函数,用于评价在当前航速下模拟轨迹末端方向与目标的方位角偏差;dist(v,ω)为距离评价子函数,用来计算轨迹到已知障碍物距离;vel(v,ω)为当前速度评价子函数,λ2、λ3、λ4分别为3个子函数的权重系数,σ为3个子函数的归一化参数。

2.2 改进评价函数

2.2.1 引入全局路径融合子函数

为了解决传统DWA算法易陷入局部最优解的问题,本文引入全局路径融合子函数globdist(v,ω),使得最终路径是在所得最优全局路径的基础上,进行局部路径规划,避免机器人造成额外的能量耗损。全局路径融合子函数globdist(v,ω)公式如式(12)所示。

(12)

式中:xglobal、yglobal是由改进蚁群算法规划的全局路径的节点坐标,xpart、ypart是DWA算法预测的局部路径的末梢坐标。其改进示意图如图2所示。

(a) 未添加全局路径融合子函数 (b) 添加全局路径融合子函数图2 添加全局路径融合子函数改进效果

2.2.2 引入障碍物安全阈值

机器人运行时,通过激光扫描仪及运动传感器不断检测未知障碍物,为避免移动机器人运行时与未知障碍物发生碰撞,本文引入障碍物安全阈值来提高所规划路径的安全性。障碍物安全阈值L_dist(v,ω)如式(13)所示。

(13)

式中:di、dj为第i、j个障碍物与移动机器人之间的距离,θ为机器人与第i、j障碍物间的连线夹角。当障碍物安全阈值大于未知障碍物扩展距离时,移动机器人可以安全通过。

改进后的评价函数如式(14)所示。

(14)

式中:λ5是障碍物安全阈值L_dist(v,ω)的权重系数,λ6是全局路径融合子函数globdist(v,ω)的权重系数。

3 改进蚁群与DWA融合算法流程

本文改进蚁群算法能够在复杂静态地图环境中,规划出最优全局路径,但是无法在存在未知障碍物的地图中运行,因此本文将其与改进的DWA算法进行融合,使得该融合算法能够规划出成功躲避障碍物且全局性最优的路径。其流程图如图3所示。

图3 改进蚁群与DWA融合算法流程图

4 融合算法仿真实验及分析

本文算法运行环境为windows10、64位系统、主频2.90 GHz、MATLAB2018b软件。

4.1 改进蚁群仿真实验及分析

为验证本文改进蚁群算法的有效性,建立30×30规模、障碍物覆盖率为33%的静态地图,与传统蚁群算法、文献[14]、文献[15]算法进行对比仿真实验。将蚂蚁个数设置为50,最大迭代次数设置为100,目标影响函数的权重系数γ设为1,信息素挥发因子的调整系数λ1设置为1.82。仿真结果如图4所示。

(a) 路线规划效果对比图 (b) 收敛曲线效果对比图图4 4种算法仿真实验对比图

其中,1号线为传统蚁群算法规划的路径,2号线为文献[14]算法规划的路径,3号线为文献[15]算法规划的路径,带有*标记的4号线为本文改进蚁群算法规划的路径。

结合图4和表1可知,传统蚁群算法收敛速度最慢,迭代100次后尚未稳定,规划的路径也不够平滑。文献[14]算法虽减少了路径的冗余节点个数,但是路径长度最长。文献[15]算法路径冗余节点较少,路径长度较短,但经过7次收敛才得到最优解。本文改进蚁群算法经过状态转移概率及信息素更新规则的改进,只需迭代3次后收敛就得到最优路径,大大提高了蚁群算法的收敛速度,且本文改进蚁群算法采取路径优化策略后,最优路径长度为42.77、拐点数为4,充分减少了路径上的冗余节点,进而减少了最优路径的长度。

表1 4种算法实验结果对比

4.2 混合算法仿真实验及分析

本文融合算法的参数设置如下:改进蚁群算法的参数设置不变;改进DWA算法相关参数的设置参考文献[20]设置:移动机器人最大速度为1.5 m/s,最大角速度为20 rad/s,最大加速度为0.2 m/s2,最大角加速度为50 rad/s,速度分辨率为0.02 m/s,角速度分辨率为1 rad/s,评价函数中λ2为0.05、λ3为0.2、λ4为0.3、λ5为0.02,预测周期为3 s,安全半径设为0.6 m,动态障碍物的移动速度为0.26 m/s,初始方向角度为45°。

4.2.1 含有未知静态障碍物的地图环境

首先建立50×50规模、静态障碍物覆盖率为31%的复杂地图,并在全局路径的关键位置上添加多个静态障碍物。为验证本文融合算法的有效性,与文献[19]改进算法进行对比仿真,实验结果如图5和图6所示。

(a) 规划路径 (b) 机器人运行的角速度、线速度图5 文献[19]算法在4个随机静态障碍物下的路径规划及机器人运行时角速度、线速度图

图5a和图6a中虚线分别表示文献[19]算法和本文算法所规划的全局路径,其中文献[19]算法所规划的全局路径长度为75.3,拐角次数为11;本文算法所规划的全局路径长度为79,拐角次数为8,本文较文献[19]最优路径长度虽较长,但是拐角次数上减少了27%。

图5a和图6a中实线分别表示文献[19]算法和本文算法所规划的避障路径。由图5a中可以看出文献[19]算法中,机器人靠近第一个静态障碍物时,未及时采取避障策略,有碰撞的危险,且机器人运行的角速度和线速度在130～250控制点之间波动很大。由图6a中可以看到,本文算法通过设定障碍物安全阈值,使得机器人在运行中可以提前探测未知障碍物,提供更安全且运行平稳的避障路线。

4.2.2 含有未知动态障碍物的复杂地图环境

上述实验验证了本文融合算法在复杂静态环境下的有效性,为进一步验证本文融合算法在含有未知动态障碍物的复杂地图环境下的可行性,在上述地图环境中,添加一个匀速运动的动态障碍物,使其运行方向与移动机器人运动方向相反,且运行轨迹与全局规划路径存在交叉。仿真结果如图7所示。

图7 本文算法在复杂动态环境下的运行过程

图7记录了机器人使用本文算法进行动态避障的全过程。由图7a可以看出,机器人在进行躲避动态障碍物之前,按照最优全局路径前行;通过本文引入的障碍物安全阈值,机器人在前行时提前检测动态障碍物;检测到障碍物后,采用改进的DWA算法进行局部避障,机器人成功躲避动态障碍物后,返回全局最优路径继续前行。由图7b可以看出,机器人采用本文算法可以成功且安全的躲避未知静态障碍物,随后返回全局路径,继续向目标点移动。由图7c可以看出,本文算法最终所得规划路径,不仅考虑了全局路径的最优性,也保证了局部避障的最优性。由图7d可以看出,本文算法通过将改进蚁群算法和改进DWA算法融合,使得机器人在运行过程中始终以稳定的速度前行,能满足移动机器人实际运行时的安全要求。

5 结论

本文利用蚁群算法搜索路径的全局搜索性及DWA算法的局部避障性,设计了一种新的混合动态路径规划方法。首先,为了提高传统蚁群算法在复杂环境下的全局搜索性能,本文改进了状态转移概率计算公式,来提高蚁群算法的收敛速度;动态调整信息素挥发系数,来避免蚂蚁陷入局部最优;采用路径优化策略,减少初步规划路径的冗余节点,更贴合移动机器人在实际运行时对于路线平滑度的要求。其次,为了提高传统DWA算法全局性差和避障性能低的问题,本文添加全局路径融合子函数,将改进蚁群算法所得的全局路径作为移动机器人的导航参考,并设置障碍物安全阈值,提高DWA算法的避障性能及运行稳定性。仿真实验结果表明,本文改进的蚁群算法能够在复杂环境下,高效且安全地规划出最优全局路径;改进蚁群与DWA融合的方法,能够在复杂动态环境下,规划出更高效的路径规划。