面向关隘地形的分层调度多机器人路径规划

张凯翔,毛剑琳,宣志玮,向凤红,付丽霞

(1.昆明理工大学 机电工程学院,云南 昆明 650500;2.昆明理工大学 信息工程与自动化学院,云南 昆明 650500)

0 引言

近年来,随着智能机器人的广泛应用,多机器人路径规划(Multi-Robot Path Planning,MRPP)作为多智能体协作领域的一项关键技术越来越受到国内外专家学者的广泛关注[1-2]。MRPP的主要任务是为群体中每一个机器人找到1条从它们的起点到终点且彼此间无冲突的路径[3]。随着技术的高速发展和需求的不断驱动,MRPP技术在仓储系统、交通控制、机场运营等各领域中受到越来越多的关注和支持[4-5]。

MRPP问题是典型的NP Hard问题[6]。当前,对该问题的求解常用的有基于简化模型和基于搜索等方法[7]。其中,基于简化模型的方法将MRPP问题简化为广泛研究的组合优化问题,再采用传统方法进行求解,YU等[8]将MRPP问题简化为一个时间域扩展的多商品流(multi-commodity flow)模型,并采用线性规划求解器Gurobi进行求解。SURYNEK[9]在时间扩展图的基础上,将MRPP问题降解为命题可满足(propositional Satisfiability,SAT)问题,并采用SAT求解器进行求解。上述算法虽然对模型做了简化,但由于以离散时间作为扩展轴,模型规模随着时间步的增加而不断扩大从而导致求解效率急剧下降。类似的问题也反映在基于搜索的A*算法上[10],A*算法被广泛地应用于求解单机器人路径规划问题[11],且具有完备性和最优性的特点[12],但若将其直接扩展到MRPP问题,随着机器人数量的增加将出现解空间爆炸现象[13]。MRPP问题的解空间规模随着时间步呈指数级增长,且与机器人的数量成指数关系,采用传统基于搜索的耦合型算法将造成求解效率低,占用内存量大等突出问题[14]。为降低解空间的搜索复杂度,STANDLEY[15]采用算子分解(Operator Decomposition,OD)的方法,在基于搜索的耦合型算法基础上通过剪枝很大程度上减少了A*算法的搜索空间。但随着机器人数量的进一步增加,其在限定时间内的求解成功率将在很大程度上被削弱。

基于搜索的解耦型算法被用于求解更多数量机器人的场景中,SHARON等[16]提出基于冲突的搜索(Conflict-Based Search,CBS)算法,将一个整体的MRPP问题在某种程度上降维到了多个独立机器人的单机规划问题,通过分层将MRPP问题中的路径规划和冲突避免两个子问题相互独立开来,由于上下层均使用了最优搜索,CBS算法具有最优性保证[17]。LI等[18]针对CBS算法在顶层冲突树扩展时可能出现的子节点重复问题,提出不相交分割(disjoint splitting)方法对冲突树进行剪枝从而避免了大量重复搜索工作。为提高MRPP问题中机器人的协作性,GRESHLER等[19]引入协作CBS(Cooperative CBS,Co-CBS),在多机协作框架的基础上集成CBS算法,实现了多个机器人的协作任务分配及其路径规划方案。上述算法通过问题解耦和分层处理的方式,在很大程度上提高了较大数量规模MRPP问题的求解效率,然而由于分层算法在顶层对冲突双方添加约束并重新规划过程中,没有考虑到其他机器人的影响,当在窄道、关隘等高耦合的问题场景中时,容易陷入解决完一个冲突又生成新的冲突的反复循环的情况,从而导致该方法难以得到可行解。

另一类基于搜索的解耦型算法通常在单机规划的同时即考虑冲突的避免问题。SILVER[20]提出的分层协作A*(Hierarchical Cooperative A*,HCA*)算法在对MRPP问题进行解耦的基础上,按优先级顺序依次为每个机器人做单机路径规划,并把前面规划的路径信息作为带时限的障碍约束添加到后面的机器人规划当中来进行冲突避免。在此基础上,SILVER[20]进一步提出带时间窗的HCA*(Windowed HCA*,WHCA*)算法,将MRPP问题通过固定的时间窗进行分步后再采用HCA*算法进行求解,在很大程度上加速了问题的求解。上述算法由于在规划时就考虑对其他机器人的冲突避免,具有较高的求解效率[21],但无法保证最优性,且算法的求解成功率和路径耗时对机器人的调度次序较为敏感[22]。WU等[23]在分布式系统中分析了路径的同调类(homology classes)数量并以备选路径的多少来安排机器人的调度次序,起到了平衡路径总耗时与最大完工时间的作用,但该过程未涉及终点封堵的类型。李昆鹏等[24]针对多AGV系统的路径规划,在HCA*的思想基础上采用两阶段算法结合多种避碰方式使得物料运输任务的路径总耗时较等待避碰方式获得1%～5%的改善。LI等[25]在优先级规划(Prioritized Planning,PP)中采用D*Lite作为底层算法,在提高搜索效率的同时优化了路径质量。然而,随着地形复杂度的增加,尤其是在具有关隘环境的复杂地形中,机器人路径间的耦合程度将急剧上升,仅依靠固定的规则和调整策略来改善调度次序往往不具灵活性,难以应对复杂环境的动态变化。

基于上述分析,本文针对关隘地形下高耦合的多机器人路径规划问题,通过专设优化层来灵活调整和优化调度次序以提高算法的求解性能。构建MRPP问题的分层求解框架,包含优化层和规划层。其中,优化层负责对机器人的调度次序进行调整和优化;规划层采用HCA*等通用求解算法进行路径求解。同时,为避免关隘地形中搜索过程陷入死循环状态,对HCA*算法进行改进并设置了熔断机制使搜索环节适时结束并跳出。通过多层级的配合与迭代进化,以提高在复杂环境中的求解成功率,并在获得可行解的基础上逐步降低路径耗时。

1 问题描述

1.1 多机器人路径规划

MRPP问题定义为由K个机器人组成的机器人群体A,如图1a所示每个机器人ak∈A,k=1,2,…,K有各自的起点sk和终点gk[26]。系统需为各机器人ak规划出一条从起点到终点的路径pk,且任意机器人ai与aj的路径pi与pj间不能发生冲突。在无冲突基础上,令机器人群体的路径总耗时最少或最大完工时间最短是本问题的优化目标[27-28]。

图1 多机器人路径规划问题描述

本文考虑匀速场景,冲突类型主要分为点冲突类型和边冲突类型两类[29]。其中,点冲突类型指两个机器人在相同的时刻占据相同的位置,如图1b的路径中2号机器人与3号机器人将在t=2时共同占据地图点v=(2,2)的位置。而边冲突类型指两个机器人在前后时间步内发生相向移动,如图1b中点vi(2,3)与点vj(2,4)构成的边e(vi,vj)上,1号机器人与3号机器人将在t=2～3时发生相向移动,从而造成两者的对撞。

1.2 关隘地形

关隘地形是一类特殊但常见的地形,在该地形环境中路径耦合度将急剧上升,从而限制了现有算法的求解性能。如图2所示,地图中存在某些特殊的关口将地图分割开,成为两边区域沟通的唯一通道,并且一次只容许一个机器人通过。在该环境中,若有机器人占据了两侧出入口甚至以关隘内部通道为终点时,将对其他机器人的通行造成影响。图2给出了关隘地形下的3种机器人冲突情形,均为以关隘为终点的堵塞模型。

图2 关隘地形阻塞类型示意

随着机器人数量增多,路径间的耦合性也相应增强。在高耦合度的场景中(如图2a),分层解耦型算法如CBS,Co-CBS等算法将耗费大量时间在顶层冲突节点的选择和消解过程,且消解完一个冲突极容易导致另一个新的冲突产生,从而使得求解效率低下甚至难以求解。

综上可见,关隘地形在增加路径耦合度的同时使得MRPP问题的求解变得更为困难。HCA*等算法虽然解耦效率较高,但该类算法在求解过程中的机器人调度次序对算法的求解成功率和求解质量均有重要影响。由此,在现有算法基础上,针对关隘地形高度耦合的多机器人调度次序优化和路径求解是本文的研究目标。

2 调度次序优化及带熔断机制的改进HCA*算法

2.1 HCA*算法基础

2.1.1 基本思想

HCA*算法由协作A*(Cooperative A*,CA*)和反向回溯A*(Reverse Resumable A*,RRA*)算法组合而成。其中,CA*[20]在求解思路上将MRPP问题从一个整体问题分解为了有前后联系的单机器人路径规划问题,按顺序依次对机器人a1,a2,…,aK做单机器人的路径规划,并将前面规划的机器人路径信息作为障碍约束添加到后面的机器人规划当中。即在A*搜索过程中额外设立了一个预约表,当完成第i个机器人ai的路径规划后,便将该机器人包含离散时间点的路径信息pi放入该预约表并与前面已有的列表项共同组成下一个机器人ai+1的带时限障碍物,依次对每个机器人执行上述操作,直至完成全部机器人的路径规划方案。在CA*基础上,SILVER[20]对A*搜索过程中的启发函数做了进一步改进,在CA*算法下增设了启发函数计算层并使用RRA*算法对目标点到当前点进行反向A*搜索以获得当前点到目标点的实际距离,再将该距离作为启发函数,以进一步提高算法的搜索质量。

2.1.2 冲突处理策略

为降低解空间计算方程[30]的底数,以减小MRPP问题的解空间规模,本文在HCA*执行单机A*搜索的过程中采用4邻域+原地等待的方式进行扩展。如图3所示,从当前状态点分别往上下左右4个方向及原地进行扩展,并根据障碍物判断该扩展点是否有效。所有扩展点的路径耗时(cost)值等于当前点的cost值增加1个时间单位。

图3 open表节点扩展方式

在上述扩展方式基础上,设置HCA*在路径搜索过程中的冲突处理策略。针对可能出现的点冲突和边冲突两种冲突类型,采用原地等待或绕道的方式进行冲突避免[31]。如图4所示,原地等待方式适用于点冲突类型,该方式选择冲突的一方令其等待,即扩展等待点为下一步的状态点,待另一方通过后再正常前进。绕道方式适用于两种冲突类型,令冲突一方绕开另一方的行进道路,待另一方离开后再返回原路继续前进。

图4 冲突处理方式

2.2 遗传优化与带熔断机制的IHCA*算法

2.2.1 多机器人路径优化求解框架

在MRPP问题中,针对调度次序分配和路径求解两个子任务,设计分层优化求解框架如图5所示。图中,上层的优化算法负责生成各机器人的调度次序,并根据适应度对调度次序进行迭代和进化;下层带熔断机制的改进HCA*(Improved HCA*,IHCA*)算法则根据优化层制定的调度次序进行各机器人的路径规划,并将求解结果返回上层以对该次序的适应度进行更新。其中,上层采用遗传算法(Genetic Algorithm,GA)[32]作为优化算法;下层结构中的带熔断机制CA*算法为各机器人进行路径节点的搜索和规划,RRA*算法则负责为CA*算法计算实际距离的启发函数值以提高搜索质量。各层级间相互协作,使得问题的求解成功率得到提高,并在获得可行解的基础上逐步对路径总耗时指标进行优化。

图5 MRPP问题分层优化求解框架

2.2.2 基于GA的优化层设计

(1)适应度函数

在机器人调度次序的进化过程中,适应度起到了一个重要的引导作用,基于适应度进行的遗传算子操作使得整体种群向着适应度更佳的方向进化。结合MRPP问题的两项重要性能:求解成功率和路径耗时,构造适应度函数如下:

(1)

式中:K为机器人总数,Ks为求解出的机器人个数,costi为机器人i的路径移动耗时。适应度函数由前后两项构成,其中前者表示在该调度次序下所有能够求解的机器人总耗时。而后者则作为惩罚项将未能求解的机器人路径耗时均以最大耗时赋值,并加入到总耗时中获得最终的综合指标作为该调度次序的适应度函数。

(2)编码与解码

改进算法的主要目标是通过改善多机器人的调度次序以提高路径求解成功率和求解质量。因此以调度次序为目标对象对其进行实数编码,生成数量等于机器人数K的顺序实数[1,2,3,…,K],并随机匹配到各个机器人ak上。解码时,按照各机器人所匹配的实数值,从小到大排列获得各个机器人的调度次序。该编码送入路径规划层,由IHCA*算法进行路径搜索和求解,并得到以路径耗时表达的适应度值。

(3)遗传算子

按照自然界优胜劣汰原则,根据适应度对种群PopPrio(i)中的n个个体即各个调度次序Prio进行排序。在设定的选择概率ps基础上,从小到大挑选出优秀父代个体,再按照一定的幸存概率pv对剩余的父代个体进行随机挑选,以此形成ns个父代存活组合。在父代存活组合的基础上,采用随机生成的方法再产生n-ns个个体,将两者合并一起进入下一步的交叉与变异操作。其中,交叉和变异操作按照实数编码的遗传算法常规操作进行,本文中采用片段交叉方式和多片段互换的变异方式进行,并在该过程中采用精英保留策略将当前最好的个体直接复制到下一代。最终生成下一代种群PopPrio(i+1)。

2.2.3 带搜索熔断机制的IHCA*算法

原始HCA*算法在路径搜索过程中易陷入死循环状态。如图6所示,若某关隘被前面的机器人封堵,则后面的机器人由于无法通过该关隘,将返回到地图的其他位置,并以时间步为区分进行反复搜索和扩展,从而造成无效的资源浪费。若规划层的算法陷于死循环状态,将使得整体算法停滞不前而导致求解失败。因此,需要为下层HCA*算法设置一定的熔断机制,当HCA*在搜索过程中出现上述情况时使其退出本次任务,并进入下一个机器人的规划任务中。

图6 关隘死循环

以HCA*算法在搜索过程中的close表和RRA*算法所获得的close_rra表中节点的数量关系为参考设置熔断机制。其中,close表中存放HCA*算法在当前机器人路径规划过程中已经扩展过的节点,该表中位置重复的节点以时间步作为区分。close_rra表则由RRA*在进行反向A*搜索时生成,记录了从当前点到目标点的一系列路径距离信息且随着搜索过程动态更新,该表中节点位置不重复。因此,HCA*算法搜索过程中所实际扩展的close表中节点将由close_rra表中的部分节点以时间轴扩展的方式组合而成。在正常没有阻塞的情况下,close表中的节点数量小于close_rra表中的节点数量,而当在路径发生部分拥堵的情况下,close表中的节点将随着时间步的扩展而激增。因此,通过对比close表和close_rra表中的节点数量关系从而在HCA*算法中设置熔断机制如式(2)所示:

(2)

式中:C为1时表示达到熔断阈值,A*算法将停止本次搜索任务并进入下一个任务;C为0时A*算法将继续本次搜索任务直至求解完毕或到达熔断阈值。Nc为close表中的节点数量,Nr为close_rra表中的节点数量,ω为拥堵缓冲系数,用来缓冲暂时性的拥堵以减小误判的概率。

2.2.4 G-IHCA*算法

将上述内容加入优化框架,形成遗传优化及带搜索熔断机制的G-IHCA*多机器人路径规划算法,如算法1所示。算法整体由内部的IHCA*算法和外部的GA算法构成,其中,Prio表示一组调度次序,PopPrio表示由n组调度次序组成的调度次序种群,C(PopPrio(i))用于判断第i代种群是否达到收敛条件,paths为Prio次序对应的MRPP问题解。

算法1G-IHCA*算法。

1:initialize:PopPrio(1)={Prio(1),Prio(2),…,Prio(n)}

2:calculate fitness:F(1)={f(Prio(1)),f(Prio(2)),…,f(Prio(n))}

3:while C(PopPrio(i)) ≠ Ture do

4: for all prio(j) ∈ PopPrio(i) do

5: reservation table T = ∅,paths = ∅

6: for each robotakaccording to prio(j) do

7: pathpk= ∅

8: close = ∅,Nc= length(close)

9: close_rra←rra*,Nr= length(close_rra)

10: while open ≠ ∅ andNc≤ωNrdo

11: P←pop(open) by f value

12: close←P

13: if P is goal node then

14: return pkby trace close

15: end if

16: expend next node Q by P

17: open←Q if Q not conflict with T and not in close

18: update close_rra

19: Nc= length(close),Nr= length(close_rra)

20: end while

21: return pk=∅

22: add pkto paths

23: add pkto T

24: end for

25: calculate fitness f(Prio(j)) to F(i)

26: end for

27: select:PopPrio’(i)=s(PopPrio(i),F(i),ps,pv)

28: crossover:PopPrio’’(i)=c(PopPrio’(i),pc)

29: mutate:PopPrio’’’(i)=m(PopPrio’’(i),pm)

30: generate new population:PopPrio(i+1)= PopPrio’’’(i); i=i+1

31:end while

3 仿真实验结果

3.1 仿真设置

为验证所提算法的有效性,采用文献[15]和文献[33]中的典型地图算例进行仿真。其中,地图类型I采用8×8地图[15],地图类型II选用地图样例集[33]中的32×32地图,如图7所示。分别采用OD[15]、HCA*[20]、CBS[16]、PP[25]算法和本文的G-IHCA*算法对算例进行求解。其中,G-IHCA*的种群数量n设置为20,并在初始种群中保留一个与HCA*、PP算法一致的原始次序,最大迭代次数限制为50。所有算法均在Intel core i5 4590 3.3 GHz,内存为8 G的PC机上以Python程序运行。算法求解时间限制为5 min,所有算法需在限制时间内完成对问题的求解,超出限制时间则视为求解失败。

图7 测试地图类型

3.2 固定任务的路径规划结果

采用文献[15]中所设置的10个机器人,并规定其起点、终点,如图8所示。在该算例中,可以明显发现2号机器人占据了地图的关隘位置,阻挡其他机器人的通过。同时,8号机器人和10号机器人将占据一个通道并对其他机器人的通过造成一定的阻碍,在它们对道路进行封堵之后将使图中(4,2)、(7,1)等位点成为地图中的新增关隘,对通行造成进一步拥堵和限制。

图8 地图I及固定任务

令G-IHCA*的拥堵缓冲系数ω=6,各算法的求解结果如表1所示,其中N表示算法无法完成或在限制时间内无法完成对该算例的求解,则总耗时以NA表示。Y表示算法在限制时间内完成求解任务,并获得实际的总耗时指标。

表1 地图I固定任务求解结果

由表1可知,由于地图I的固定任务构造了多个关隘形成复杂的地形结构,除G-IHCA*外其他算法均无法在限定时间内完成路径求解。在路径耗时上,G-IHCA*通过对调度次序的调整和优化,在获得可行解的基础上逐步获得了更优的路径总耗时,该求解过程的适应度曲线如图9所示,各适应度值对应的调度次序如表2所示。

图9 地图I固定任务求解过程适应度

在图9和表2所示的G-IHCA*的迭代求解过程中,一开始的总耗时较大。随着遗传迭代,调度次序不断进化,总耗时也在慢慢缩短,直至迭代到第10代时进入了收敛阶段。表2中各调度次序所得的机器人路径规划分布图如图10所示。图中相同颜色线条由一个机器人的起点、途经点和终点以栅格为精度单位连接而成。此外,为避免图中同一栅格内的路径重叠显示,对各路径做了偏移处理,每个机器人路径在栅格内均有固定且彼此不重叠的显示位置。

图10 不同任务耗时路径图

对该算例求解过程及结果的分析表明,本文算法可有效实现预期目标:在其他算法无法有效求解的案例中通过调整机器人的调度次序,增大了求解成功的可能性,并且可以进一步通过不断调整调度次序,使得规划方案的总耗时不断缩短。此外,上述迭代求解过程充分表明在原始HCA*陷于搜索死循环无法跳出时,本文设置的熔断机制及时地进行了熔断操作并进入到下一次规划任务中,有效地避免了计算资源和时间的浪费。

3.3 不同数量机器人随机任务的路径规划结果

为进一步验证算法的有效性和稳定性,通过两类地图不同数量的随机任务求解以对改进算法进行更全面的测试与分析。每类地图分别设置6种机器人数量,每种数量均测试20次随机任务即所有机器人的起点和终点均由随机方式生成。此外,由于本文的分层优化框架同样适用于HCA*算法的变体WHCA*等算法并构建G-IWHCA*算法,本环节中将加入该算法以体现所提算法框架适用性广的优点。其中,G-IHCA*与G-IWHCA*算法中两类地图的拥堵缓冲系数ω设置一致,均取值ω1=6和ω2=10,G-IWHCA*算法的时间窗W取值16。所有算法的求解成功率如图11所示。

图11 不同数量随机任务测试结果

由图11可知,在机器人数量较少的情况下,几种算法都有较高的求解成功率,但随着数量的增多,求解成功率明显下降,尤其是耦合型的OD算法,在数量超过10的情况下就很难在规定时间内完成求解了。解耦型算法中,在机器人数量较多的高耦合情况下,分层解耦型的CBS算法求解效率迅速下降,相较而言HCA*、WHCA*与PP算法在高耦合场景下仍具备一定的求解效率。整体来看,G-IHCA*与G-IWHCA*算法在所有数量段测试中均有较好表现,前5个数量段中能够维持80%以上的求解成功率,最后一个数量段的表现相较于其他算法有较大幅度提升。此外,从两类地图的对比中可以发现,地图Ⅰ中,WHCA*与G-IWHCA*相较于对应的HCA*与G-IHCA*在成功率和稳定性上表现更好,地图Ⅱ中则与之相反。该情况是由于WHCA*算法在加入固定时间窗后虽提高了求解速度,但当遇到因路口封堵而需要的绕道距离大于固定值W时,将陷入在封堵点附近反复震荡的搜索状况,地图Ⅱ中的地形更容易引起WHCA*震荡现象的出现,从而导致求解失效。这同时也表明,在加入本文的优化框架后,通过调度次序的优化,G-IWHCA*能够在一定程度上减少震荡现象的发生并提高求解成功率。

图12和图13给出两类地图下G-IHCA*算法对较大数量机器人的路径分布结果。可见,由于关隘的限制和机器人数量的增多,关隘附近的路径耦合度随之上升,具体表现为关隘口附近的路径密集程度明显增加。通过合理制定调度次序再使用IHCA*算法进行求解,各机器人逐个通过了关隘并到达指定位置,验证了所提算法在高耦合复杂环境下的求解效果。

图12 地图类型Ⅰ下G-IHCA*路径图

图13 地图类型Ⅱ下G-IHCA*路径图

在此基础上,进一步对各算法所规划方案的路径耗时进行分析。由于算法之间求解成功率的差异,平均路径总耗时的指标需对公共算例进行分析。公共算例即每个数量段测试的20个算例中,所有待比较算法均可以求解的一部分算例。

由表3可知,在获得可行解的基础上通过对调度次序的优化,G-IHCA*与G-IWHCA*算法规划方案的总耗时相较于HCA*、WHCA*和PP等同类型算法均有获得一定程度的优化,且结果接近OD、CBS等算法所求得的问题最优结果。在与各自的基础算法对比中,G-IHCA*和G-IWHCA*算法的优化效果更为显著,并且随着机器人数量的增加,改善效果也越明显,表明本文所提算法的优化层起到了降低路径总耗时的改进作用。

表3 算法平均路径总耗时分析

4 结束语

本文在具有关隘环境的复杂地形MRPP问题求解中,采用调度次序优化的思想,在HCA*基础上搭建了分层的MRPP问题优化求解框架。通过分层将MRPP整体问题被分解为了调度次序优化和路径规划两个子问题,上层只针对规划过程的调度次序问题,且独立于下层的实际地图环境,适用于各类复杂场景的求解。为避免HCA*在关隘地形的搜索过程中由于高优先级机器的阻挡而陷于反复无效搜索的状态,提出了带熔断机制的IHCA*算法对搜索过程进行限制,可在很大程度上减少HCA*算法的平均无效求解时间。研究结果表明,本文提出的G-IHCA*算法及拓展的G-IWHCA*算法在对复杂关隘地形的高耦合算例测试中,较现有算法在求解成功率上有较大改善,在最终方案的总耗时上较同类型算法耗费更少,且在某些数量段上接近CBS等算法所得问题最优解的水平,从而验证了算法的有效性。对于大规模的任务地形,本文方法的优先级调整过程仍然耗时较大,未来研究将考虑采用博弈的方式进行优先级调整。