蚁群融合动态窗口法的分布式多机器人运动规划研究

王 倩,杨立炜,李俊丽,杨 振

(昆明理工大学 信息工程与自动化学院, 昆明 650504)

0 引 言

分布式多移动机器人系统可完成更复杂的任务,成为了目前的前沿研究热点[1]。运动规划作为其中的关键性技术,在具有先验信息的静态环境下为机器人规划安全行驶路径,在充满未知复杂的动态环境下机器人遵循全局路径的引导进行局部避障运动。

几十年来,众多专家学者提出了大量的路径规划算法,如A*算法[2]、蚁群算法、粒子群算法[3]、遗传算法[4]等,这些优化算法被广泛应用于车间调度[5]、多目标优化[6]、机器人路径规划[7]等方面。蚁群算法[8]具有正反馈、较好的适应性以及优化能力强等优点,但也存在前期搜索盲目性、搜索效率低、易陷入局部最优等问题。文献[9]受A*算法启发,采用该算法作为蚁群的启发信息以提高搜索效率,并加入弯曲抑制算子提高了路径平滑度。文献[10]引入最优最差蚂蚁系统,对全局信息素进行差别更新,加快了算法的收敛速度。文献[11]在转移概率中加入避障因子提高蚂蚁算法搜索能力,大大减少了复杂环境下蚂蚁个体的致死数量。上述改进都侧重于实现全局路径的最优,缺乏考虑局部未知环境下机器人的实时运动。动态窗口法(dynamic window algorithm,DWA)是一种实时避障算法,文献[12]提出了一种基于Q学习的改进DWA,修正了原有评价函数的不合理机制,扩展了两个新评价函数,使得机器人在复杂环境下拥有较好的导航运动效果。由于航向和速度评价函数之间的矛盾可能会导致机器人频繁换向、避障不及时等问题,文献[13]为此在DWA的路径评价函数中引入方向变化评价因子,抑制某些特定因素对评价函数的过度影响,减少机器人不必要的转向。

以上算法[9-13]在不同程度上解决了单移动机器人而未考虑多个机器人场景下的路径规划问题。文献[14]针对多移动机器人,以神经网络和滚动窗口法为基础,提出了一种未知环境下的混合路径规划算法,引入了动量梯度下降法与路径点检测机制,加快了算法的搜索效率。文献[15]采用分层路径规划来解决多移动机器人的运动问题,使用贝兹曲线融合遗传算法规划出单机器人的最优路径,区分多机器人的冲突类型,确定其碰撞区域从而选择合适的策略消除冲突。文献[16]受到多目标粒子群优化的启发,提出了一种多机器人路径规划新方法,其以路径短、安全性高和平滑性优为目标,寻找从起点到终点的最优路径,引入概率窗口的理念,结合机器人传感器获得的实时信息和先验信息,选择适应度更高的路径。

针对多移动机器人在运动过程中的避障和安全性协调问题,本文引入A*算法、转弯代价和转弯抑制因子改进启发式函数以增加路径平滑度;引入路径综合信息素更新概念,结合路径特性对信息素挥发系数进行调整提高全局路径综合性能以避免局部最优;考虑到机器人的运动特性,利用冗余点删除策略进行后期优化;基于DWA进行局部路径规划,提出机器人自适应导航方法提高全局路径的追踪精度。最后,本文将融合蚁群与DWA的单机器人运动导航策略运用于分布式多机器人系统,通过优先级策略协调多机器人系统存在的各种冲突。

1 蚁群算法

1.1 传统蚁群算法原理

蚁群算法是一种经典的启发式算法。人工蚂蚁依据路径信息从八邻域范围内选择合适的节点,迭代出一条最优的可行路径。在t次迭代中,蚂蚁k由节点i向节点j转移的概率为

(1)

(2)

当所有蚂蚁完成一次迭代以后,路径上的信息素根据(3)—(4)式进行更新。

τij(t+1)=(1-ρ)τij(t)+Δτij(t,t+1)

(3)

(4)

(3)—(4)式中:ρ是信息素挥发系数;Δτij(t,t+1)是蚂蚁寻路中的信息素增量;Q是初始信息素强度;L表示蚂蚁通过当前迭代的总路径长度。

1.2 改进蚁群算法

1.2.1 优化启发函数

传统启发式函数只考虑了当前节点到下一节点之间的欧氏距离,使得蚂蚁在搜索节点时更倾向于选择与当前节点距离较近的节点,路径的最优性得不到保证。为了提高蚁群搜索过程的启发效果和算法的收敛速度,本文在启发函数中引入A*算法思想,可表示为

f(n)=h(n)+g(n)

(5)

(6)

(7)

(5)—(7)式中:f(n)、g(n)和h(n)为代价函数;(xs,ys)为起始点;(xn,yn)为当前节点;(xg,yg)为目标点。本文将A*算法的实际代价函数g(n)和估计代价函数h(n)作为蚁群搜索路径的启发式信息,并添加转弯代价因子和转角抑制因子,以减少路径累积转向角度和转弯次数。改进的启发式函数可表示为

(8)

(9)

(10)

(8)—(10)式中,A、B、C为大于1的常数;Cbend为转弯代价;Rthita表示上一节点n-1、当前节点n和待选节点n+1之间的角度(弧度);Eturn为转角抑制因子,用于强化蚂蚁在路径搜索过程中沿着相同方向运动的趋势,以此减少冗余转折点的生成;t(J)表示蚂蚁从当前栅格向待选栅格的转向标号;t(J-1)表示蚂蚁从上一栅格到当前栅格的转向标号。

1.2.2 改进信息素更新规则

将蚂蚁的搜索行为集中到最优解的附近可以提高解的质量和收敛速度,但会导致算法“早熟”。为了充分利用当前最优解,本文每次迭代只对最优蚂蚁路径进行信息素更新。此外,本文综合考虑了路径的长度、转弯次数和转弯角度,改进信息素更新规则并将综合评分最高的最优解进行定义,表示为

(11)

(12)

(13)

(11)—(13)式中:F为当前综合评价下的路径指标;Q1为大于1 的常数;L为最优路径长度;T表示最优路径的转弯数;C表示最优路径的转弯角度总和(弧度);K1、K2、K3为权重系数。

1.2.3 冗余点删除策略

经过上述改进所得到的最优路径上依旧存在少量的冗余拐点。为此,本文进一步优化路径。

步骤1消除同一直线上的冗余点,具体删除策略如图1所示。原始路径包含节点P1→P2→P3→P4→P5,消除同一直线上的冗余点,直到生成仅包含起点、终点和转折点P1→P2→P3→P4的路径Mid_Route。

图1 步骤1冗余点删除策略Fig.1 Redundancy point deletion strategy step 1

步骤2消除整条路径上的冗余点,具体删除策略如图2所示。优先连接起点P1与终点P4,线段[P1,P4]的直线距离dis[P1,P4]满足dis[P1,P4]

图2 步骤2冗余点删除策略Fig.2 Redundancy point deletion strategy step 2

2 动态窗口法

DWA通过对多组速度进行采样分析来预测机器人在一段时间内的运动轨迹,然后根据评价函数选出最优速度组驱动机器人运动。

2.1 动态窗口法原理

2.1.1 机器人运动模型

DWA对窗口内机器人的线速度与角速度进行采样,假设机器人的轨迹可以分解成多个时间片,则在每个时间片Δt内,机器人的运动学模型可表示为

(14)

(14)式中:x(t)、y(t)、θ(t)分别表示机器人在t时刻x、y方向上的位置和运动方向角;v(t)和ω(t)分别表示线速度和角速度。

2.1.2 机器人速度约束

在移动机器人速度组空间中,DWA将机器人的避障描述为具有约束的优化问题,包括对微分机器人的不完全运动约束,对机器人结构的动力学约束和环境约束。

1)机器人受自身条件制约的最大最小速度约束。机器人速度约束范围可表示为

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

(15)

2)电机加减速约束。机器人受电机影响,存在加减速度约束。在动态窗口显示内,受到加减速影响的最大和最小速度范围为

(16)

3)制动距离约束。整个机器人轨迹可以细分为若干个直线或圆弧运动,为保证机器人的安全,在最大减速条件下机器人应在撞到障碍物之前减速至0。机器人速度必须满足的条件为

(17)

(17)式中,dist(v,ω)表示此速度下该轨迹与障碍物之间的最短距离。速度只有满足(17)式的条件,才能保证机器人运动的安全。

上述约束将机器人限制在一定的速度范围内,可将其表示为速度集

Vr=Vs∩Vd∩Va

(18)

2.1.3 评价函数

经过速度组(v,ω)采样后,DWA会生成若干条预测轨迹,然后根据评价函数进行轨迹评分,筛选出得分最佳的轨迹,机器人在此速度组的驱动下实现(14)式所示的运动。评价函数可以表示为

G(v,ω)=σ(a·heading(v,ω)+b·dist(v,ω)+c·velocity(v,ω))

(19)

(19)式中:σ表示归一化操作;G(v,ω)为模拟轨迹评价值,使其值最大的(v,ω)即为最优速度;heading(v,ω)为机器人导航函数,机器人运动方向与目标方向偏航越小,其值越大;dist(v,ω)为机器人与障碍物的最近距离,其值越大,发生碰撞的危险就越小;velocity(v,ω)用于评估机器人运动性能,速度越大,得分越高;a、b、c为对应评价函数的权重因子。

2.2 自适应追踪局部目标点

目前很多文献[17-20]基于全局路径规划算法融合DWA进行研究,将全局路径的拐点作为机器人运动的局部目标点。然而在具有未知障碍物的环境中,该方式存在不合理性。例如,在一段没有转折点的路径上往往存在大量未知静态障碍物,机器人存在的避障困难以及对路径跟踪精度的不够会导致其丢失导航方向。因此,本文提出了自适应追踪局部目标点的方法,表示为

Old_path(i-1),nds]

(20)

local_target(t+1)=

(21)

(20)—(21)式中:Remake函数是本文设计的路径生成函数;local_target(t+1)表示机器人在t+1时刻的导航信息点;d1表示机器人与当前导航点的距离;d2表示预测的最优模拟轨迹末端到当前导航点的距离;d3表示导航点到障碍物的最近距离(该项的作用是为了防止导航点生成在未知障碍物区域内,机器人不可达);l1,l2,l3是距离相关的参数。Remake函数按照一个期望节点间距nds从全局路径中重新生成包含大量节点的导航路径,操作流程如下:首先,基于蚁群算法得到全局路径Old_path;其次,将相邻节点Old_path(i)和Old_path(i-1)连接生成线段方程;然后,求解该线段上满足期望的节点间距nds的节点序列,保存在New_path中;最后,依次遍历完所有线段得到完整的机器人导航路径New_path。当机器人运动过程中满足3个条件(d1

改进局部目标点选取规则如图3所示。原始路径包含3个路径节点Old_path(i-1,i,i+1),通过(20)式生成具有大量节点信息的新路径,且每两个节点之间的距离为nds;由于未知障碍物(图3中红色障碍物)会影响机器人对局部目标点的选取,本文在(21)式中设置几个不同的距离保证机器人在具有未知障碍物环境下导航的正确性。当满足机器人与局部目标点的距离小于1.7 m,或者模拟轨迹末端到局部目标点的距离小于1.5 m,或者局部目标点到最近障碍物的距离小于0.7 m这3个条件之一时,可认定该点可取为局部目标点(也即机器人运动导航点),否则设置终点为局部目标点。

图3 局部目标点选取规则Fig.3 Schematic diagram of the our local target point tracking method

3 多机器人的优先级协调策略

多移动机器人的路径规划问题实则是由单移动机器人扩展出来的。DWA动态避障策略主要适用于静态环境或局部小规模动态环境,对于解决多个机器人之间的运动冲突效果不佳,本文基于IACO-IDWA策略构建了分布式多移动机器人系统。

优先级策略作为当前协调避碰的主流技术之一,旨在通过不同的任务指标预先为每个移动机器人设定相应的优先级别,当存在运动冲突时优先级低的机器人为优先级高的机器人让行。为此,本文引入了优先级机制,以此降低多机器人在求解路径冲突问题时的难度,下述以优先级较高的AGV1和优先级较低的AGV2进行阐述。

1)如果两个机器人的实际距离d12小于期望避障距离d,则存在潜在碰撞风险。

2)以优先级较低的机器人AGV2为中心建立局部坐标轴用于计算两机器人的连线与x轴正方向构成的夹角α,以及AGV1的运动方向角β。

3)判断|α-β|是否小于90°,如果满足条件,则碰撞风险可能性极高。AGV1将AGV2视为未知静态障碍物,通过DWA进行避障运动。当两个机器人之间的距离关系为d12≥d且|α-β|≥90°时,AGV2恢复运动。为保证多机器人运动的安全性,AGV2会在短时间内进行骤然减速运动,依据的规则为

(22)

本文的优先级避障策略在某些情况下会导致低优先级的机器人产生额外的等待时间,但机器人的安全性和全局最优性在极大程度上得到了保证。

4 仿真实验分析

4.1 改进蚁群算法实验分析

为验证常规环境中算法的优越性,在30 m×30 m的环境地图上将本文算法与文献[10-11]中的算法进行对比实验。文献[11]的路径是经过优化的,而文献[10]的路径是未经优化的,测试地图和数据均来自文献[11]。算法对比分析实验结果如表1所示;3种算法的路径规划结果及算法迭代曲线如图4所示。为体现本文改进蚁群算法的综合性能,参考文献[7]的方法,定义路径综合性能为

表1 算法对比分析

图4 路径规划结果图Fig.4 Path planning result diagram

F=xT+yL+zC

(23)

(23)式中:F为路径综合指标;T为最优路径转弯次数;L为最优路径长度;C为最优路径与障碍物边缘发生碰撞次数;x、y、z表示各个指标的权重,本文均取为1。

在路径安全性与平滑性方面,本文算法明显优于文献[10-11],但在路径长度方面文献[10-11]略有优势。由表1可看出,本文算法转弯次数、与障碍物边缘碰撞次数最少;迭代稳定次数为9,与文献[10]相比下降较多,略高于文献[11];运行时间明显优于其他两种算法。从路径综合指标来看,本文算法无论是优化前还是优化后都优于对比算法。本文与文献[11]算法都经历了后期优化,但本文的转弯次数、与障碍物边缘碰撞次数都优于文献[11],这样机器人沿本文路径运动时更为安全,且运动过程中对姿态的调整和避障能力要求更低。

4.2 多移动机器人路径规划实验

为了验证本文融合改进算法与优先级法在多移动机器人路径规划中解决冲突的能力,本文设计了具有已知先验信息的简单环境、含有未知静态和动态障碍物的复杂环境进行验证;设计了3个考虑运动学约束的移动机器人,优先级为AGV1>AGV2>AGV3。

4.2.1 简单静态环境

静态环境多移动机器人路径规划如图5所示。

图5 静态环境多移动机器人路径规划Fig.5 Multi-robot path planning in static environments

图5a为本文为每个机器人规划出的综合性能最优的全局路径,其中蓝线、红线、粉线分别为AGV1、AGV2、AGV3这3个机器人的路径情况;黑色网格是拥有先验信息的静态障碍物。图5b显示了机器人在规划过程中的实时变化。静态环境多移动机器人参数变化如图6所示。图6a和图6b分别为机器人的线速度和角速度变化曲线。

4.2.2 复杂动态环境

从图5b和图6可以看出,当AGV2、AGV3检测到与AGV1的碰撞风险时,分别在第41个运动控制节点和第50个运动节点开始减速。AGV2在第47个控制节点完全停止,AGV3在第51个节点完全停止。当优先级最高的AGV1与其他机器人有发生碰撞冲突风险时,AGV1把其他机器人的停止节点视为未知静态障碍物,进行避障后继续向目标点前进。AGV1离开与AGV2的冲突区域后,AGV2恢复运动,并将AGV3视为未知静态障碍物进行避障,直到AGV2离开冲突范围后AGV3才恢复运动。本文进行的多机器人运动实验用时101.7 s,AGV1、AGV2和AGV3的行驶的实际距离分别为12.70 m、12.63 m和11.45 m。

动态环境多移动机器人路径规划如图7所示。图7a显示了为每个机器人规划出的最优路径,图7b显示了机器人在避障过程中的实时变化。动态环境多移动机器人参数变化如图8所示。图8中,红色栅格是未知的静态障碍物,黄色栅格是未知的动态障碍物,黑色虚线是动态障碍物的运动轨迹。

图7 动态环境多移动机器人路径规划Fig.7 Multi-robot path planning in dynamic environments

图8 动态环境多移动机器人参数对比Fig.8 Multi-robot parameter comparison in dynamic environments

从图7b和图8可以看出,AGV1与AGV3完成了躲避动态障碍物的动作后,3个机器人相遇。AGV1与AGV3发生冲突,按照优先级的限制,AGV3减速至完全停止。AGV2也与AGV1发生冲突,AGV2减速至停止,AGV1进行动态避障后继续向目标点前进。AGV1离开冲突范围后,AGV3恢复运动。随着AGV1的远去,AGV2随即恢复运动并躲避动态障碍物,最终各机器人成功到达终点。整个多机器人运动实验用时188.34 s,AGV1、AGV2和AGV3的行驶距离分别为13.16 m、113.89 m和9.165 m。

5 结束语

针对多移动机器人在运动过程中避障与安全性难以协调的问题,本文提出了一种能适应复杂未知环境的多机器人融合避障算法。在启发式函数中综合考虑A*算法的启发信息、转弯代价因子和转角抑制因子并进行改进,增加路径的平滑性与安全性;提出了以综合路径指标来进行信息素更新的方法,提高了全局路径综合性能;通过冗余点优化策略进一步减少了全局路径的长度和转折点数量,进一步提高路径质量;基于DWA算法提出了自适应导航策略来引导机器人在复杂环境下的运动;将蚁群融合DWA的避障策略应用在分布式多移动机器人系统中,结合优先级策略解决多机器人之间的运动冲突问题。不同环境下的多机器人仿真实验表明,本文算法能够实现多移动机器人在未知环境中的协同避障。