基于双层协调体系的多移动机器人路径规划方法

钟佩思， 徐东方， 李东民， 梁中源， 刘 梅， 陈修龙

(1.山东科技大学先进制造技术研究中心， 青岛 266590； 2.山东科技大学机电工程系， 泰安 271000)

多移动机器人系统能够胜任单机器人所不能完成的复杂任务，因此，多移动机器人领域的研究受到了越来越多的关注[1-2]。其中，协调路径规划问题一直是多移动机器人研究的基础和重点[3]，一种灵活而有效的协调路径规划方法，能够进一步增强系统的可靠性和鲁棒性，并提高工作效率。

用于单移动机器人路径规划的方法有很多，如拓扑法、遗传算法[4]、人工势场及其改进算法[5]等，与单机器人不同，除长度、平滑度和避障能力等问题外，多移动机器人路径规划还要考虑机器人之间的协调避碰问题[6]。近年来，对多移动机器人路径规划的研究正在不断深入。孙树栋等[7]用遗传算法规划出能实现协调避碰的多机器人全局路径，但难以适应复杂的动态环境；周兰凤等[8]提出基于知识的遗传算法，提高了轨迹规划的效率；Kala[9]设计一种协同进化算法，规划出了能够有效协调避碰的多机器人全局路径组合，但局部协调处理较为复杂；景兴建等[10]提出用人工协调场进行多机器人路径规划以提高实时避碰能力，但其全局性能较差；吴晋等[11]通过改进人工协调场来应对协调规划中的运动抖动和“死锁”现象；Chandrasekhar等[12]采用改进的磷虾群算法进行协调路径规划，但未能验证在动态障碍物环境中算法的可行性。

可以看出，在目前的多移动机器人路径规划方法中，采用全局规划算法很难应对局部路径的实时协调，而倾向于局部规划的方法又难以形成优质的全局路径。针对这一问题，提出一种基于全局与局部双层协调的多移动机器人路径规划体系，结合改进的免疫协同进化算法和动态窗口法，通过相互协调，力求机器人在沿全局最优路径行驶时，仍能实现灵活地局部路径调整，从而进一步提高多移动机器人系统的全局规划与局部协调能力。

1 双层协调体系的构建

基于双层协调体系的多移动机器人路径规划方法主要由全局路径规划层和局部路径协调层构成，其体系结构如图1所示，采用混合式控制方式实现整体调控。路径规划前，首先要完成环境地图构建、各机器人初始位置和目标位置的确定以及优先级编号等前处理工作，其中，机器人的优先级顺序由所需完成任务的重要程度来决定。

图1 体系结构框图Fig.1 Architecture block diagram

全局路径规划层需要从各初始位置到目标位置为每个机器人分别规划出一条长度最短、转角最少、能有效避障并尽量减少机器人之间碰撞干涉的全局路径，该工作由改进的免疫协同进化算法来实现。通过免疫克隆机制能加快种群进化速度，为进一步适应多移动机器人的全局路径规划，在原算法的基础上引入路径组的概念进行改进。首先为n个机器人分别规划出多条较优的全局路径，并将每个机器人对应一条全局路径组成的集合X={X1,X2,…,Xn}称为一个路径组(Xi表示第i个机器人对应的一条全局路径)，全局路径规划的目标函数作为抗原，而在种群中匹配出的所有路径组均视为抗体，通过对每个抗体进行亲和度评价并反复迭代进化，最终能够规划出整体效果最优的全局路径组，机器人将沿着最优路径组中各自的全局路径行驶。

全局路径规划可以在一定程度上减少机器人之间的碰撞，但在工作过程中，难免出现动态或静态未知障碍物以及多个机器人相遇的情形，无法保证完全避免碰撞。因此仅靠全局路径规划难以应对复杂的工作环境，还需要通过局部路径协调来对机器人局部路径进行灵活调整以避免产生碰撞。

局部路径协调层的工作采用动态窗口法来实现，这是一种基于速度空间的局部避障方法，具有良好的动态处理能力。基于双层协调体系，可以利用全局路径信息辅助进行局部路径协调，以获得更好的规划效果。首先根据全局路径规划结果对最优路径组中的各条路径进行跟踪预测，当发现机器人之间存在干涉或碰撞趋势时，便提取碰撞点周围的环境信息以及各机器人在全局路径上的运动信息进行局部路径协调，从而避免碰撞发生；机器人个体对周围环境也进行实时探测和判断，当检测到可能产生碰撞的未知障碍物时，同样提取周围工作环境信息并进行局部路径调整避障。完成局部路径调整后，机器人重新回到各自的全局路径上继续行驶，直至顺利到达原定目标位置。

2 全局路径规划的实现

2.1 搭建工作环境

假设多移动机器人系统的工作空间S中有n个移动机器人和m个障碍物，且机器人个体在长度或宽度方向上的最大尺寸为d。构建地图时，把障碍物的边界向外扩展d/2，则在全局路径规划中机器人个体可视为质点。将机器人按优先级高低分别编号为R1, R2,…, Rn，执行的任务越重要，机器人的优先级就越高，相应的下标编号也就越小，而障碍物分别编号为O1, O2,…, Om。所有的障碍物共同组成障碍物空间SO，剩余的空间则称为自由空间SF。

将移动机器人Ri的起始位置定义为pi,1=(xi,1,yi,1)，目标位置定义为pi,l=(xi,l,yi,l)，l为路径点的个数，其全局路径Xi便可通过SF中的点序列{pi,1,pi,2,…,pi,l}来表示，其中pi,j=(xi,j,yi,j)，表示Ri的第j个路径点坐标值，由于不同机器人的行驶路径不同，所以路径点的个数l也会各不相同。为了便于扩展系统中机器人的个数，并减少后期实时路径跟踪和预测的计算量与计算时间，规定在全局路径上，所有移动机器人都以速度vq进行匀速行驶。

2.2 改进免疫协同进化算法

免疫协同进化算法流程如图1中的全局路径规划层所示，为更好地适应多移动机器人系统，在传统免疫协同进化算法步骤[13]的基础上进行了改进。

首先为每个机器人随机生成N条全局路径，这些路径共同构成一个n×N矩阵，称为抗体种群A。之后通过适应度函数对种群中的各条路径进行评价，选取适应度较高的n×K条路径构造记忆集Am，而剩下的n×L条路径则构成一般集Ag，定义适应度评价函数如下：

f(Xi)=α1len(Xi)+α2cor(Xi)+α3aov(Xi)

(1)

式(1)中：len(Xi)表示路径Xi的总长度，计算公式为

(2)

cor(Xi)表示路径Xi中连续折线间转角的总弧度，计算公式为

(3)

aov(Xi)表示路径Xi与工作空间中障碍物的相交次数；而α1、α2、α3则表示权重系数。

划分完Am和Ag后，从Am中再选取适应度较高的n×G条路径进行克隆以获得克隆集Ac，其中G≤K，其大小与Am的整体适应度成正比。为保证Am不遭到破坏，这里仅对Ac和Ag进行变异，并对Ac进行粒群进化处理[13]，之后对Am、Ag、Ac中的路径进行疫苗接种。如图1所示，疫苗主要分为删除、平滑、交换和避障4类，通过接种疫苗，可以有针对性地优化个体，从而加快收敛速度。

接种疫苗后，再次对Am、Ag、Ac中的路径进行适应度评价并更新记忆集，最后在更新后的记忆集A′m中通过排列组合匹配路径组，并通过亲和度函数对不同的路径组进行评价以判断是否达到规划要求，将路径组的亲和度评价函数定义为

(4)

式(4)中：C为一较大常数；X={X1,X2,…,Xn}表示从A′m中匹配出来的路径组；col(Xi)表示Xi与路径组中其他路径的相交次数，由于初始条件不确定，路径相交便存在碰撞的可能；cro(Xi)表示路径Xi在路径组中的拥挤程度，用Xi两侧规定范围D内其他路径的条数来表示；β1、β2为权重系数。

参照亲和度评价结果，若存在满足要求的最优路径组，则完成全局路径规划，机器人沿各自全局路径匀速行驶，并由系统进行跟踪预测，以及时判断可能产生碰撞的位置；若各路径组均不满足规划要求，则消亡掉当前种群中适应度低的路径，并更新种群以维护多样度不变，重复上述步骤继续迭代进化，直到获得满足要求的路径组为止。

2.3 全局规划性能验证

在传统免疫协同进化算法中，适应度评价被包含进亲和度函数中，直接由亲和度评价来划分Am和Ag，并且亲和度评价的对象是单条路径而非路径组，因此每次评价时，需要将待评路径与其他机器人所有路径的亲和度都计算一遍，计算量较大，致使迭代速度较慢，而且也不利于机器人个数的扩展。同时也因为评价对象是路径个体，所以最终获得的每条路径亲和度可能达到最高，但放一起却未必是整体亲和度最好的一组。

进行变异、进化和疫苗接种前，种群中各条路径的适应度都相对较低，此时进行亲和度评价不仅计算量大，且效果不佳，因此在改进的算法中，先通过适应度函数对Am和Ag进行划分，之后进行克隆、变异、进化和免疫处理，并再次利用适应度函数对路径进行评价，更新出适应度相对较好的记忆集A′m后，再对A′m中的路径进行亲和度评价。

在进行亲和度评价时，先通过排列组合将A′m中的路径匹配为不同的路径组，再利用亲和度函数分别评价，并通过反复迭代，寻找亲和度最高的路径组。亲和度评价对象是路径组这个整体概念而不是单条路径，所以进行评价时只考虑该路径组中n条路径间的亲和度即可，而不需要将待评路径与其他机器人所有路径的协作行为都计算一遍。

通过改进算法，能够将多机器人全局路径规划先简化为单个机器人的路径规划问题，当种群中的路径都达到较好的适应度后，再匹配路径组进行多机器人的路径亲和度评价，减少了很多不必要的协作度计算，从而加快了迭代速度。并且由于亲和度评价对象是路径组而非单条路径，所以能够保证最终规划出的n条路径可以取得最好的整体效果。在相同的软硬件环境和初始条件下，改进前后算法的迭代速度和迭代效果对比如图2所示。

从图2(a)中可以看出，改进后算法的迭代速度要优于原算法，且随着迭代次数的增加，这种优势会越来越明显，而图2(b)则显示改进后的算法在迭代至30次左右便逐渐趋于稳定，比改进前算法的收敛速度快，而且最终所获得的路径组亲和度也更高。这说明对算法的改进可行且有效，能实现更加快速、高质量的多移动机器人全局路径规划。

图2 算法性能对比Fig.2 Comparison of algorithm performance

3 局部路径协调规划

3.1 动态窗口法的构建

机器人沿各自全局路径以vq匀速行驶，根据初始位置和行驶时间可以预测出存在碰撞或干涉的路径点，当需要进行局部路径协调时，便启用动态窗口法进行调整。动态窗口法的主要思想[14]是在速度空间中采样多组速度并模拟机器人在这些速度下一定时间Δt内的轨迹，得到多组轨迹后，通过对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人进行路径协调。

(5)

进行速度采样时，虽然速度空间中存在无穷多组速度，但并不是每组速度都可取，需要根据机器人自身条件和工作环境的限制，将速度采样控制在一个速度动态窗口V内，该动态窗口可以表示为机器人最值速度窗口Vm、电机加速度性能窗口Ve和安全速度窗口Vs的交集：

(6)

图3 动态窗口采样示意图Fig.3 Dynamic window sampling diagram

基于动态窗口进行速度采样，并根据机器人运动学模型推算每组速度在时间Δt内对应的轨迹，进而完成对各条轨迹的评价，构造评价函数为

h(v,ω)=γ1hea(v,ω)+γ2dis(v,ω)+

γ3vel(v,ω)

(7)

式(7)中：hea(v,ω)用来评价在当前采样速度下，机器人到达模拟轨迹末端时的前进方向与目标方向之间的夹角δ，用π-δ表示，即δ越小评分越高；dis(v,ω)表示机器人在当前模拟轨迹上与障碍物之间的最小距离；vel(v,ω)表示当前模拟轨迹对应的线速度大小；γ1、γ2、γ3表示权重系数。

最后基于上述函数的评价结果，选择评分最高的轨迹对应的采样速度来驱动移动机器人运动。

3.2 局部路径协调处理

局部路径协调过程中，规定机器人的速度方向与大小均由动态窗口法的评价结果来确定，但协调的初始速度和终止速度仍然是全局路径上对应的速度，从而确保完成局部路径调整后，机器人回到全局路径上仍然能按原定速度匀速行驶。

当机器人之间存在碰撞可能时，需要比较这些机器人的优先级，由优先级低的机器人启用动态窗口法进行局部协调，而优先级最高的机器人将不受影响，继续沿自身路径行驶。相比之下，机器人的优先级越低，路径调整的次序就越靠前，相应的协调范围也越大，依次类推。位于全局路径或局部调整中的机器人，均可按此方法进行协调。而对于未知障碍物，则需要机器人自行检测并判断，若存在碰撞可能，便及时进行局部路径调整避障。

当协调避碰的对象为未知障碍物或处于局部调整中的机器人时，由于其运动信息不可预知，直接利用路径评价函数选择最合理的(v,ω)来驱动机器人进行协调避碰即可；而当避碰对象为全局路径上的机器人时，则可利用机器人在全局路径上的运动信息来辅助局部路径协调，从而进一步提高整体规划效果，仍以Rb的局部协调避碰为例，可通过Ra的全局路径运动信息完善对Rb的路径评价，在二者行驶轨迹相交前，引入新的路径评价项：

(8)

式(8)中：t′表示在采样速度(v,ω)驱使下，Rb的行驶轨迹与Ra全局路径相交时所需要的时间；(xa,t′，ya,t′)和(xb,t′，yb,t′)分别为行驶轨迹相交时，Ra和Rb所到达的位置坐标，该项权重系数取为γ4。

通过对行驶轨迹相交时机器人之间的位置关系进行评价，能够挑选出更加合理的运动轨迹，从而提高局部路径协调效率，待行驶轨迹相交后，再重新启用原路径评价函数进行评价调整。通过增加距离评价项，也可使该方法扩展至全局路径上多个机器人之间协调避碰的情况中。

4 仿真试验分析

利用MATLAB编程构建多机器人双层协调路径规划体系，随机生成二维栅格环境地图，栅格边长设置为1 m，系统中使用Pioneer3-DX款差分驱动机器人模型，d取为0.5 m，机器人个数设置为4，分别进行优先级编号并分配初始位置与目标位置坐标，在栅格地图上起点位置和目标位置分别用实心的圆形和方形来表示，由此进行仿真试验。路径规划过程中，各权重系数的取值如表1所示。

表1 权重系数取值Table 1 Calculation results of power

进行全局路径规划时，亲和度评价函数中常数C取为100，经过迭代进化，最终得到的规划结果如图4所示。规划过程中，种群内各路径的适应度值变化情况如图5所示，从A′m中匹配出的各路径组亲和度值变化则如图6所示。可以看出，迭代次数达到30次以后，便可获得趋于稳定规划结果，最终所获得的各路径适应度值较好，且路径组的亲和度也很高。改进后算法的收敛速度快，且全局路径规划的结果较为理想。

图4 全局路径规划结果Fig.4 Results of global path planning

图5 全局路径适应度值Fig.5 Fitness of global path

图6 全局路径亲和度Fig.6 Affinity of global path

设定系统的初始条件为机器人从各自起点位置同时出发，在全局路径上以vq=0.3 m/s匀速行驶，由此进行路径跟踪和预测。基于初始条件推算，机器人R1和R2在19.893 s会产生碰撞，此时R2处于图4中的a点位置，需要在a点附近对R2进行局部路径协调，R2的优先级比R1低一级，取协调范围半径为4vq，并将R1视为以d为半径的圆形动态障碍物，处理结果如图7(a)所示；路径调整致使R2的行驶时间延长，进而导致R2和R3在39.476 s时存在碰撞可能，此时R3处于图4中的b点位置，则在b点附近同样需要对R3进行局部路径协调，处理结果如图7(b)所示。

图7 局部路径协调处理Fig.7 Coordination of local paths

可以看出图4中R3和R4的全局路径也存在交点，但是根据初始条件推算，两个机器人之间不会发生干涉和碰撞，故不需要进行局部路径协调，对于地图中的未知障碍物c，需要由机器人自行探测并判断，以及时进行调整避让。

完成局部路径协调处理后，机器人重新回到原全局路径，以vq继续向前匀速行驶，若再次出现碰撞趋势，便重新开启动态窗口法进行局部路径调整，直至顺利到达目标位置以执行原定任务，基于双层协调体系的路径规划结果如图8所示。

从图8中可以看出，基于双层协调体系的多移动机器人路径规划方法所规划出的各条路径均具有较好的全局效果，并且可以实现机器人之间灵活有序的协调避碰，对于突然出现的未知障碍物也能做出及时的避障处理，能够实现预期目的。

图8 双层协调路径规划结果Fig.8 Results of bilevel coordinated path planning

5 结论

针对多移动机器人的路径规划问题，提出一种基于全局与局部双层协调的路径规划方法，通过仿真分析，得到以下结论。

(1)引入路径组亲和度评价方法来改进免疫协同进化算法，提高了全局路径规划的效率，为多移动机器人系统规划出了质量更高并能尽量减少机器人之间干涉碰撞的全局路径。

(2)利用动态窗口法并结合机器人的优先级机制和全局路径信息，及时对机器人的局部路径进行协调处理，有效避免了复杂环境下各机器人之间以及机器人与未知障碍物之间的碰撞。

基于双层协调体系的多移动机器人路径规划方法可以实现机器人在沿全局最优路径行驶时，仍能进行灵活有序的局部路径协调，能够应对动态复杂的工作环境，进一步提高了多移动机器人系统的作业能力。