立体仓库多机器人协同路径规划与冲突消解

立体仓库多机器人路径规划问题是机器人仓储运输领域中的研究热点之一，提高多机器人之间协同响应能力是提高立体仓库运行效率的关键环节。

多机器人协同作业主要需解决路径规划和冲突消解2个方面的问题。按照系统路径规划的控制方式，运动规划方法可以分为完全集中规划[1]、不完全集中规划[2]、不完全分散规划[3],以及完全分散规划[4]。众多学者对此进行了相关研究[5,6,7],现有研究多以时间、距离等为优化目标，虽有涉及能耗[8],但大多只考虑单目标优化方法，较少涉及考虑时间与能耗的多目标优化研究。冲突消解是多机器人系统路径规划问题的重要技术之一，也是多机器人系统路径规划和多个单机器人路径规划的本质区别体现。多机器人系统协调避障问题除了要解决单个机器人自身路径规划问题，还必须解决多个机器人之间的碰撞、堵塞及死锁问题。目前，学者们提出的冲突消解策略主要有交通规则法[9]、优先级法[10]、分层地图法[11]、几何修正法[12],以及基于行为的避碰方法[13]等。其中，优先级法是一种在机器人路径规划中广泛使用的冲突消解方法，这种方法虽然节省了计算时间且易于实现，但是动态环境具有不确定性，不能保证初期分配的优先级序列能正常工作。

鉴于此，本文针对立体仓库多机器人协同路径规划问题，采用栅格法对结构化立体仓库环境进行建模，在考虑机器人面临的运动学约束、运动边界约束、协同安全性约束和协同时间窗约束的基础上，建立了具有最优作业时间和能耗的多目标路径规划模型。针对此模型特点，将多机器人路径规划问题分解为多个单机器人路径规划子问题，利用改进蚁群算法为单机器人规划出初始路径，通过时空协同约束处理解决了单机器人之间的协同问题，提出了多蚁群协同进化算法，以对问题进行寻优求解。为提高算法求解质量，将下一步移动潜在节点的数量作为最优路径选择的考虑因素，同时采用自适应调节挥发系数来提高算法的性能，一定程度上避免了局部最优，通过最优解交换机制和信息素关联机制加强了多种群之间的协作交流。针对路径规划可能出现的冲突问题，提出了动态优先级冲突消解策略，对发生冲突的机器人实时确定优先级，有效地解决了多机器人作业冲突问题。

本文涉及的参数及其含义如表1所示。

表1(续)

立体仓库多机器人协同作业是机器人仓储运输领域关键研究之一，主要由路径规划和冲突消解2层构成。路径规划层要求在复杂的环境约束下，对作业中的每个机器人从起始点至目标点之间规划出一条最优或近优路径；冲突消解层要求通过合理的避碰策略对相关机器人进行协同处理，得到较优组合路径，以保证机器人协同作业。

本文采用文献[14]提到的立体仓库环境模型，如图1所示。该模型具有易于创建、便于维护以及良好的可扩展性等优点。图1中用栅格地图表示立体仓库环境，由仓储货架、机器人和拣选台(站点)组成，每个仓储货架包含多个货位，每个货位存放一种货物，当机器人接到作业任务后，根据订单中货物对应的目标位置信息规划出满足任务要求的安全无冲突的路径，图1中箭头表示机器人运行的方向。

本文主要从高效率、低能耗2个方面对机器人运动路径进行优化，优化目标为时间和能耗。

在路径规划数学模型中，考虑到立体仓库系统中机器人在运动过程中的加速度，根据机器人运动速度能否达到额定速度vmax,将运动形式分为第Ⅰ类运动(l≤v2maxmax2/a)和第Ⅱ类运动(l>v2maxmax2/a)。

对于第Ⅰ类运动，由基本位移公式l=v0t+1/2at2得，运行时间为：

t=2l/a−−−−√         (1)t=2l/a         (1)

对于第Ⅱ类运动，由基本速度公式vt=v0+at和速度位移公式vt2-v02=2al得，运行时间为：

t=1vmax+vmaxa         (2)t=1vmax+vmaxa         (2)

当冲突不可避免时，机器人要重新规划出一条冲突等待时间twait最短的局部路径，即：

twait=TRA-TRB (3)

多机器人路径规划总的时间代价T为：

T=max(t1,t2,…,tk,…,tM)+twait (4)

式中：tk为机器人Rk的运行时间。

机器人运动过程中的总需求能量包括其在运动方向上变速和匀速运动时所需的能量、克服工作轨道摩擦力所需的能量，以及冲突等待时消耗的能量。根据动力学理论，在恒定加速场下(加速度不为0),单位质量的能耗等于加速度与运行距离的乘积。

对于第Ⅰ类运动，机器人能耗为：

e=mla+μmgl (5)

对于第Ⅱ类运动，机器人能耗为：

e=m(1−v2maxa)a=μmgl=mv2max         (6)e=m(1-vmax2a)a=μmgl=mvmax2         (6)

冲突等待时消耗的能量与等待时间成正比，即：

Ewait=ξtwait (7)

多机器人路径规划总的能耗代价E为：

E=∑k=1M(ek+Ewait)         (8)E=∑k=1Μ(ek+Ewait)         (8)

式中：ek为机器人Rk的能耗。

为达到最优作业时间与能耗，需同时考虑到总时间代价T最小、总能耗代价E最小。对于多目标优化问题的求解，加权求和法是一种常用的方法。本文应用加权求和法将多目标优化问题转化为单目标优化问题，对应的单目标优化函数为：

minF=min(ω1T+ω2E) (9)

ω1和ω2可以采用层次分析法、模糊评价法等来确定。ω的不同取值可以反映用户对不同路径的偏好。归一化处理后单目标优化函数为：

minF=min(ω1T−TminTmax−Tmin+ω2E−EminEmax−Emin)         (10)minF=min(ω1Τ-ΤminΤmax-Τmin+ω2E-EminEmax-Emin)         (10)

在路径规划过程中，机器人需满足运动学约束、运动边界约束、协同安全性约束和协同时间窗约束。

1)运动边界约束规定机器人只能进行平面运动，分别为向前、向后、向左、向右移动及原地暂停，每次运动时仅向某一方向运动。其中：

⎧⎩⎨⎪⎪Pk(0)=(x0k,y0k)Pk(tmaxk)=(xmaxk,ymaxk)x0k,xmaxk,y0k,ymaxk∈X  k∈[1,M]         (11){Ρk(0)=(x0k,y0k)Ρk(tmaxk)=(xmaxk,ymaxk)x0k,xmaxk,y0k,ymaxk∈X  k∈[1,Μ]         (11)

2)运动学约束要求机器人沿着路径运行时，其速度和加速度应该遵循给定的区间，以避免滑失，即：

{vk(t)≤vmaxkak(t)≤amaxkt∈[0,tmaxk]k∈[1,M]         (12){vk(t)≤vmaxkt∈[0,tmaxk]ak(t)≤amaxkk∈[1,Μ]         (12)

3)协同安全性约束可确保机器人在运动过程中，相互之间保持一定的安全距离，以规避机器人间发生安全事故。机器人之间的实际距离dkr可表示为：

dkr=(Rxk−Rxr)2+(Ryk−Ryr)2−−−−−−−−−−−−−−−−−−−−√         (13)dkr=(Rkx-Rrx)2+(Rky-Rry)2         (13)

ds为常数，其值由路径精度、机器人体积以及移动速度等因素共同决定。1≤k≤M,1≤r≤M,k≠r。由此，在X中机器人Rk、Rr应满足如下安全性约束：

dskr-dkr(t),∀k,r∈[1,M],k≠r,t∈[0,max(tmaxk,tmaxr)] (14)

4)协同时间窗约束可保证多机器人协同作业时，不同机器人经过每个栅格的时间不同，即：

Wk={wh=[tinhhin,touthhout]} (15)

为了保证仓储物流的安全性，需要对时间窗进行精确计算。设机器人到达栅格h的时间为th,则有：

tinhhin=th-tb-te (16)

设机器人长度为D,则机器人通过某一栅格时，有：

touth=th+v2k+2akD√−vkak         (17)thout=th+vk2+2akD-vkak         (17)

多机器人协同路径规划是一个复杂的多约束组合优化问题，直接解决非常困难。本文采用分治-协作策略，将复杂的多机器人协同路径规划问题分解为多个单机器人路径规划子问题，将针对单机器人规划出的路径作为初始路径集合，之后通过多机器人之间的协同约束处理，对多机器人路径规划问题进行完整求解。

机器人路径优化问题属于组合优化问题中的旅行商问题(Traveling Salesman Problem, TSP),该问题已被证明是非确定性多项式难题(Non-deterministic Polynomianal-hard, NP-hard),通常采用启发式优化算法进行求解。蚁群算法具有并行计算、无中心控制等优点，是求解复杂优化问题的一种常用方法；但由于搜索初期信息匮乏，导致搜索初期积累时间较长，求解速度慢，且容易陷入局部最优，因此，需对蚁群算法进行改进，以提高求解质量。

标准蚁群算法利用转移概率指导蚂蚁的移动方向，保证了蚂蚁搜索的随机性。机器人路径规划过程中，蚂蚁在探索周围4个节点时，相应的障碍节点不应纳入下一步移动节点的选择范围；因此，改进蚁群算法首先应去除不可到达的节点，将下一步移动潜在节点的数量作为最优路径选择的考虑因素，设计新的路径转移函数。在路径搜索过程中，当蚂蚁q在t时刻处于节点i时，按式(18)计算下一步要到达的节点j,即：

pqij(t)=argmax{[τij(t)]α⋅[ηij(t)]β⋅[1ni(t)]γ}         (18)pijq(t)=argmax{[τij(t)]α⋅[ηij(t)]β⋅[1ni(t)]γ}         (18)

在蚁群路径搜索的过程中，由于挥发系数ρ的存在，那些较少被搜索到甚至从未被搜索到的解上的信息素会减小甚至消失；当ρ过大时，随着信息素的增大，该路径被选择的可能性增大。以上2种情况都会降低该算法的全局搜索能力和解的多样性。若ρ减小，则算法的全局搜索能力会提高，但是收敛性能变差，所以本文采取自适应的挥发系数来调节。ρ的初始值取较大的值来保证收敛速度，随着迭代次数的增大，ρ减小。为确保算法的全局搜索能力，同时设定挥发系数的下限ρmin来确保算法的收敛性。

ρ(t+1)={φρ(t)ρminρ(t+1)≥ρminρ(t+1)<ρmin         (19)ρ(t+1)={φρ(t)ρ(t+1)≥ρminρminρ(t+1)<ρmin         (19)

式中：ρ(t)、ρ(t+1)分别为t、t+1时刻挥发系数。

通过上述设计的改进蚁群算法对多机器人系统中的每个机器人分别进行优化求解，便得到了单机器人初始路径集合。

在得到初始路径集合后，通过信息交流加强种群之间的合作，达到协同进化的目的，以进一步保证算法全程的收敛速度与全局搜索能力的协调与统一。本文通过最优解交换机制和信息素关联机制来进行信息交流。

多机器人之间的协同约束主要包括时间协同约束和空间协同约束，这是各机器人必须协同和满足的信息；因此，对单机器人路径规划的评价不仅要考虑目标函数的适应值，还要考虑多机器人协同约束的满足程度。

时间协同约束条件即要求不同机器人到达同一栅格的时间需要满足相对时间窗约束。时间协同约束处理方法如图2所示，各机器人将规划出的初始路径时间发送至时间协同约束处理层，时间协同约束处理层针对每个机器人发送过来的时间，按式(16)、式(17)计算出各机器人的协同时间窗口，并将协同信息传递到每个机器人的规划器中，各机器人规划出满足相应时间窗的路径，即可满足多机器人之间的时间协同约束条件。

空间协同约束条件要求不同机器人之间在作业过程中的任意时刻保持一定的安全距离。空间协同约束处理方法如图3所示，各机器人在t时刻将自身位置P(t)发送到空间协同约束处理层，空间协同约束处理层收集所有机器人的当前位置，并根据式(20)判断其当前空间位置Pk(t)是否满足安全距离约束，从而将当前位置是否可行的信息Infk传递给机器人Rk。Infk计算如下：

Infk={10dkr>ds,∀k,r=1,2,⋯,M,且r≠kdkr≤ds,∀k,r=1,2,⋯,M,且r≠k         (20)Ιnfk={1dkr>ds,∀k,r=1,2,⋯,Μ,且r≠k0dkr≤ds,∀k,r=1,2,⋯,Μ,且r≠k         (20)

最优解交换机制即设置若干个蚁群种群，各自同时进行运算，在每个种群全部蚂蚁完成路径访问后，用种群A的所有全局最优解中效果最好的若干个(通常取蚁群蚂蚁总数u的10 %)个体，替换掉种群B中当前所有全局最优解中效果最差的若干个个体，同时用种群B的效果最好的若干个全局最优解替换掉种群A中的效果最差的若干个全局最优解，完成信息交流。接着，2个种群的蚂蚁分别各自更新信息素浓度，继续运算求解。当满足终止条件时，输出所求的全局最优解，最优解交换示意图如图4所示。

改进后的算法在全部分组蚂蚁每完成一次全局访问后，都会进行信息交流与协同合作，保留若干组效果较好的解，同时去除相应数目的效果较差的解，从而提高了算法的收敛速度，也增强了蚁群的搜索能力，避免了算法出现停滞现象，使算法具备更高的适应力和灵活性。

在蚁群构造新解空间后进行信息素更新时，再次引入协同机制，使种群A和种群B的信息素更新相互影响，相互关联，相当于提高了下次访问的信息素浓度，有利于加快算法速度。

在改进算法中，信息素更新时，既有本种群全部蚂蚁访问路径上的信息素更新，又有另一种群最优解对应的路径上的信息素更新。

对于种群A来说，信息素更新浓度τA(i,j)为：

τA(i,j)=ρτA(i,j)+ΔτA(i,j)+ETBA (21)

ETBA=∑upi,j∈π∗BUBTmin(qB)         (22)EΤBA=∑pi,j∈πB*uUBΤmin(qB)         (22)

其中：

ΔτA(i,j)={QLq0若第q只蚂蚁选择了(i,j)其他         (23)ΔτA(i,j)={QLq若第q只蚂蚁选择了(i,j)0其他         (23)

同样，种群A中的最优路径上的信息素更新浓度也影响着种群B的全局信息素更新浓度，即：

τB(i,j)=ρτB(i,j)+ΔτB(i,j)+ETAB (24)

ETAB=∑upi,j∈π∗AUATmin(qA)         (25)EΤAB=∑pi,j∈πA*uUAΤmin(qA)         (25)

其中：

ΔτB(i,j)={QLq0若第q只蚂蚁选择了(i,j)其他         (26)ΔτB(i,j)={QLq若第q只蚂蚁选择了(i,j)0其他         (26)

文献[15]中采用事先给定每个机器人优先级的方法，虽然节省了计算时间，但并不能保证规划出的路径为系统的最优路径。为判定优先级移动序列，本文提出动态优先级策略，假设机器人Rk的优先级可表述为PRIk,当发生路径冲突时，机器人Rk与终点的距离为lk,机器人Rr与终点的距离为lr,当lk>lr时，则PRIk比PRIr低，即PRIk<PRIr,其中k≠r,k=1,2,…,M,r=1,2,…,M。

由动态优先级策略可知，机器人发生冲突时的优先级跟机器人与目标位置的距离有关。在立体仓库系统中，机器人典型的冲突类型如图5所示，图5中，圆圈代表冲突位置，箭头代表机器人的运行方向。

假设发生冲突时，机器人与目标点的距离大小为l1>l2>l3>l4,则对应的优先级序列为PRI4>PRI3>PRI2>PRI1。对于图5a)所示情况，机器人R2首先占用冲突位置，并在冲突位置上下2个栅格中随机选择一个自由栅格绕行，机器人R1等待机器人R2离开冲突位置时按原路径继续移动；对于图5b)所示情况，机器人R3按原路径先占用冲突位置，并按原方向继续移动，机器人R1等待机器人R3离开冲突位置后继续按原方向移动；对于图5c)所示情况，机器人R4首先占用冲突位置，并选择冲突位置右侧自由栅格绕行，机器人R3等待机器人R4离开冲突位置后按原方向移动，机器人R1等待机器人R3离开冲突位置后继续按原路径移动；对于图5d)所示情况，在冲突位置发生死锁，则采用回撤策略，通过比较发生冲突的机器人回撤至可躲避冲突的暂停栅格之间距离的大小，由回撤距离最近的机器人进行暂避操作，其他机器人按照优先级序列依次通过冲突区域。

多蚁群协同进化算法流程如图6所示，其主要求解步骤如下。

步骤1:初始化。初始化目标位置、算法参数，机器人集合R中的每个机器人对应一个蚁群，共M个子种群，每个种群保持自己的信息素结构。

步骤2:对每个子种群，用改进蚁群算法让蚂蚁开始寻找路径，求解机器人的规划路径。

步骤3:关联信息素。按式(21)～式(26)对种群之间的信息素进行关联，并完成一次信息素更新。

步骤4:选择代表个体组。当种群中所有蚂蚁都完成路径搜索时，选择若干路径作为当前种群的代表。

步骤5:协同信息交互操作。对代表个体组进行时间协同约束处理和空间协同约束处理，筛选出满足协同约束条件的路径。

步骤6:判断路径是否存在冲突。若存在，则采用冲突消解方法解决冲突；若不存在，则记录机器人的规划路径。

步骤7:如果满足循环终止条件，则输出优化结果，否则返回步骤2。

在MATLAB R2014b环境中进行多机器人路径规划仿真，运算平台配置为处理器Inter© Celeron© CPU 1005M@2.5 GHz和内存4 GB的个人计算机。利用图1提出的立体仓库环境模型，将多机器人运动空间划分为24×24个栅格，每个机器人一次搬运一个货物，以5个机器人为例，实验参数设置如表2所示，机器人初始位置及目标货位坐标如表3所示。

仿真实施情况如下。

1)比较有冲突消解策略和无冲突消解策略2种方法规划出的路径，验证冲突消解策略的可行性。

2)针对相同数目的机器人，比较使用2种不同的路径规划方法完成路径规划的情况，用于验证本文所提算法优越性。

针对上述情况，利用多蚁群协同进化算法进行求解，用符号[机器人代号，位置坐标，占用栅格时间]来表示机器人的位置信息。分别在无冲突消解策略和有冲突消解策略2种情况下对立体仓库多机器人路径进行规划仿真，其结果对比如图7所示。

由图7a)可知，机器人R1与机器人R3在12.75 s时在位置(13,11) m处发生碰撞，表明无冲突消解策略不适用于多机器人的路径规划；由图7b)可知，系统为避免机器人在冲突位置发生冲突，启用了动态优先级冲突消解策略，机器人R3首先占用栅格(13,11) m, 并在11.56 s时开始绕行，机器人R1等待机器人R3释放冲突位置后，维持原路径不变继续移动。

无冲突消解策略和有冲突消解策略时机器人运动过程中距离变化曲线如图8所示。由图8a)可知，系统在12 s时，机器人R1和机器人R3之间的距离小于安全距离1 m, 在12.75 s时机器人R1与机器人R3在位置(13,11) m处发生碰撞；而经过冲突消解之后，机器人在任意时刻两两之间的距离均大于安全距离，满足了协同安全性约束，如图8b)所示。

机器人运动过程中速度与加速度的变化曲线如图9所示。图9表明，各机器人在运动过程中均满足速度和加速度约束。

综上所述，本文所提出的动态优先级冲突消解方法适用于二维环境下多机器人的路径规划问题。

为验证本文算法的优越性，采用蚁群算法对上述5个机器人的路径规划问题进行求解，算法参数设置同表2,蚁群算法与多蚁群协同进化算法仿真结果对比如表4所示。

由表4可知，蚁群算法和多蚁群协同进化算法都能够求解多机器人路径规划问题。从时间角度看，与多蚁群协同进化算法相比，蚁群算法中机器人R1和机器人R5的运行时间较大，机器人R2和机器人R3的运行时间较小，原因在于蚁群算法规划出的路径存在较多冲突，发生冲突时，机器人R1和机器人R5的优先级较低，产生等待时间，导致总时间较大；从能耗角度看，2种算法中机器人R2、机器人R3和机器人R4的能耗相近，但蚁群算法中机器人R1和机器人R5的能耗大于多蚁群协同进化算法所求的能耗，导致总能耗较大；从整个多机器人系统角度看，多蚁群协同进化算法所求得的系统时间、能耗和冲突等待时间均优于蚁群算法。

由上述结果分析可知，本文所提出的多蚁群协同进化算法在求解立体仓库系统多机器人路径规划问题上具有优越性。

本文采用栅格法对结构化的立体仓库环境进行建模，在考虑机器人面临的运动学约束、运动边界约束、协同安全性约束和协同时间窗约束的基础上，建立了以最优作业时间和能耗为目标的路径规划模型。针对此模型的特点，将多机器人路径规划问题分解为多个单机器人路径规划子问题，设计了多蚁群协同进化算法，以对问题进行寻优求解，将下一步移动潜在节点的数量作为最优路径选择的考虑因素，提高了算法的求解质量。同时采用自适应调节挥发系数来提高算法的性能，一定程度上避免了局部最优，通过最优解交换机制和信息素关联机制加强了多种群之间的协作交流。针对路径规划可能出现的冲突问题，提出了动态优先级冲突消解策略，有效地解决了多机器人作业冲突问题。最后，通过仿真结果表明，本文方法可有效避免多机器人路径冲突问题，与蚁群算法得到的结果进行比较，验证了本文提出的算法在求解多机器人路径规划问题的有效性和优越性。本文研究能有效提高立体仓库中多机器人的协同响应能力和作业效率，为立体仓库中多机器人路径规划和冲突消解提供了一种技术思路，具有广泛的应用价值。