基于一致性改进模型的多机器人编队运动规划方法

李 珣，何晶晶，赵征凡，严 冬，王晓华

(1.西安工程大学 电子信息学院，陕西 西安 710048；2.工业和信息化部 电子第五研究所，广东 广州 510610)

0 引 言

将多机器人系统(multi-robot system, MRS)应用到车间生产过程中是当前智能化车间升级的主要方式[1]。但是，当大量的移动机器人出现在有限的车间环境中时，特别是车间内存在固定工位，则留给机器人运动的空间将会非常有限，针对单个机器人的路径规划方法给予各个机器人的最优或次优路径将会极为相似，甚至重合。这样的情况使机器人运动过程中的相互扰动不可避免，这种扰动会因为机器人数量的增加而增加，降低了MRS运行效率。因此，如何将机器人个体间的信息与资源进行充分利用，对MRS的运动路径进行优化[2]，降低多机器人的运动成本，提升系统的鲁棒性和容错能力[3-5],是当前多机器人编队研究的重点内容之一。

编队控制多个机器人，按照特定的要求保持个体之间的几何关系，对比单独控制机器人这种方式能够高效地控制MRS躲避静或动态障碍物到达目的地并且由于机器人的编队行进，其路径选择较多条机器人规划路径简单、行进过程的动作统一可以简化协同控制的方案[6-8]。因此，近年对于MRS的控制研究层出不穷，对于多机器人的编队控制方向的研究按照采用的方法可以分为：人工势场法、基于行为的启发式方法、领航-跟随方法以及基于图论的方法等。基于人工势场法的机器人编队控制，SONG等设计了3种势函数，目标是在实现编队一致性控制同时保证编队中机器人行进中能够顺利的避碰、避障[9]，但是该方法对于环境的动态变化，难以应用于存在动态障碍物的环境中；ARRICHIELLO团队则提出了一种基于零空间行为控制方法，该方法属于基于行为的启发式编队控制方法，其目标在于保证高优先级任务的优先执行，同时兼顾完成部分低优先级的任务[10]，较SONG所提方法对于环境变化的动态约束较小，但是该方法需要根据机器人数量或空间环境提供较多的启发式条件。但上述2种方法均未对有限空间环境中出现的机器人运动冲突问题进行讨论。SOLEYMANI等则针对三维空间中编队控制问题进行了研究，其在基于行为方法的基础上结合双环控制系统和李雅普诺夫直接法，实现了机器人个体间冲突的消解和协同，继而促使多机器人在运动过程按照队形要求保持运动状态[11]，但是该方法的问题在于空间多个机器人根据统一控制保持编队，但缺少明确的队形保持约束造成个体的行为难以预测，且系统计算量随机器人数量增多成级数倍增大。为了能够将编队的避障、运动效率与队形控制相结合，学者们还提出了许多新颖的方法：LI和SPONG提出用几何分解的方法来解决多机器人系统中速度和位置的耦合控制问题[12]；JAFARI等讨论了多领航者的系统收敛性，并利用图论分析了多智能体结构可控性条件[13]；GUO等提出双领航者的控制策略，仅领航者对运动进行规划，其余机器人只与2个相邻机器人组成三角形队形，对三角形的组成规则进行了约束，适用于大规模编队[14]。这些研究对编队一致性控制与编队的静态避障问题进行了较为深入的研究，但是除已陈述的问题外，当机器人数量增加，对于场景中机器人运动路径之间形成的机器人互扰问题、避障效率与队形保持问题，均尚未有较明确的解决方案。

综上，针对3～5个机器人组成的MRS编队所未考虑的多移动机器人协同优化控制问题，本文将分析当编队中机器人之间的干扰特点，并将除领航机器人外的其他机器人之间关系解释为相互的动态障碍物[15]，利用提出的图论法与领航-跟随法结合的方法，实现多机器人编队中各个机器人运动的一致性，保证运动过程中的队形保持；引入虚拟弹簧映射表达机器人与障碍物以及与其他机器人之间的位置关系，使机器人能够有效进行冲突的消解；为使机器人在编队避障过程中保持与队伍行进的一致性，改进了速度限制方法[16]利用动态速度限制防止虚拟弹簧力过大机器人严重偏离编队队形；最后，根据实际的生产车间环境构建了仿真环境进行了5～20个机器人编队的仿真实验。

1 改进领航算法的编队模型

1.1 多机器人模型

协同控制策略必须以交换局部信息为基础[17]。因此，利用图论基本概念对MRS定义[18]：对于n个机器人集合R= {r1,r2,…,rn}，定义δ=(G，E，O)为n阶的加权有向图，节点集合O={o1,o2,…,on}，有向的边E和加权邻接矩阵的集合G=(gij)n×n。该网络中的有向边缘Eij由有序节点对(oi，oj)表示。从节点oi到节点oj在g中的有向路径是有向网络中的一个边缘序列(oi，oj)。

为描述节点与边之间的联系，引入邻接矩阵A，表示节点之间(或机器人与其相邻机器人之间)的信息连通性，可表示为

A=[aij=1, (oi,oj)∈E, 0, 其他]

(1)

L=D-A表示拓扑图的拉普拉斯矩阵，该矩阵特征值0出现的个数就是图连通区域的个数，获得矩阵L=[lij]n×n，lij可表示为

(2)

D∈Rm是一个对角矩阵，该矩阵由邻接矩阵A转换获得，将矩阵A的每列元素相加，并将每列元素对应的和值放在矩阵的对角线对应位置上得到程度矩阵D。包含n个机器人的编队系统，机器人i的二阶动力学模型[13]可表示为

(3)

式中：t与(t+1)为当前和下一时刻的时间；pi(t)、pi(t+1)分别为机器人i在二维空间的位置；vi(t)、vi(t+1)为机器人i的二维速度；ui(t)为机器人i的输入控制量；T为采样周期。

为形成领航-跟随思想[19]，在常规编队一致性控制率上加入跟随者与领航者的预期相对间隔误差，使得跟随者跟随领航者运动编队达成预期的队形运动。与式(3)参数意义相同，根据式(1)获得机器人i的输入控制量可表示为

ui(t)=-(Lij⊗In)[(pi(t) -pj(t))-

(Ei(t) -Ej(t))+(vi(t)-vj(t))]

(4)

式中：In为n维的单位矩阵；Ei为编队各机器人的预期相对间隔误差;⊗为卷积。为了使编队机器人i达到期望位置与速度，所有机器人能够维持预期队形，有

(5)

式中：pl(t)、vl(t)分别为领航者的t时刻的位置和速度。

图1为本文采用运算量较低的通信拓扑结构，以编队存在5个机器人为例，领航者标记为leader，跟随机器人标记为1、2、3、4。这种结构保证在有限数量的多机器人集合中，计算量较小，每个跟随者的位置、速度与领航者或旁侧跟随机器人相统一，有助于保持队形。

图 1 编队机器人为5时的通信 拓扑结构图Fig.1 Communication topology of formation robots with five

编队领航者(t+1)时刻的速度，在速度限制以内，由其与目标点t时刻的距离dl_obj(t)(角标obj表示目标)和角度θ决定，根据角度θ获得领航者在x，y轴速度可表示为

(6)

式中：α系数受机器人加速度、电量、跟随者合力等影响；领航者初始速度vl(0)=vmax，vmax为机器人运动的最大速度限制值。

1.2 基于编队一致性的弹簧避障模型

静态障碍物的空间位置不变，在编队向目标点运动过程中，将根据与障碍物之间的距离改变编队的形态、速度等。本文对弹簧力模型[20]根据编队进行改进，并加入速度限制，进一步提高队形保持度、避障精度、运动速度等。

(7)

式中：dl_obs(t)为t时刻领航者与障碍物之间的距离(角标obs表示障碍物位置)；dij(t)为t时刻机器人之间的距离；(dl_obs(t))3与(dij(t))3中幂的取值表示弹性系数，实验发现取3次幂效果最佳；f的上角标x,y分别表示该值在x,y轴上的分量。

2) 领航者避障。编队在运动过程中，首先是领航者避障，领航者的避障速度与弹簧虚拟力的大小和初始速度有关，在x，y方向的运行速度可表示为

(8)

3) 跟随者避障。在编队运动过程中，跟随者跟随领航者向目标点运动，但本文的编队不是直线编队，跟随者直接跟随领航者运动时不能完全安全避障，所以跟随者在领航者运动趋势的同时，也需要进行自主的避障。在跟随者避障过程中，需要考虑与领航者的一致性、虚拟弹簧力的影响、领航者的速度等机器人i在x，y方向的运行速度可表示为

(9)

4) 编队一致性。为保证在编队运动过程中的一致性，跟随者根据机器人之间的位置误差改变速度使得跟随机器人i与领航者趋于一致[21-23]，模型可表示为

(10)

(11)

5) 动态速度限制函数。机器人速度、加速度过高容易导致机器人出现偏离编队的情况，甚至难以回归，但是如果对速度、加速度进行定值限制，容易降低编队和个体机器人的灵活性，为了平衡2种情况，在编队运动中加入动态速度限制函数，有

(12)

vi(t+1)=min[vi(t)+Δvi(t+1),vmax]

(13)

式中：Δvi(t+1)为机器人i在t+1时刻的加速度，与该机器人距离误差增加或减小的趋势相关。如距离误差增加，则ε为正值且与增加的趋势正相关，加速度在下一时刻增加，反之ε为负值与距离误差减小趋势正相关。式(13)保证了机器人在不超过速度限制的情况下与距离误差变化趋势同步变化，并以缩小距离误差为指引。

2 数值模拟与结果分析

2.1 实验构造及数据初始化

实验物理环境为有限的二维区域，大小为30 m×30 m。分别构造存在静态、动态障碍物的仿真环境，在此环境下构造实验和数据初始化。仿真实验中，定义机器人是同种类型，具备基本的导航、避障、通信等功能，领航者有且仅有1个。

数据初始化编队中的机器人数量，n=5、10、15、20，机器人初始位置随机获得，数据初始化结果如表1所示。

表 1 不同实验环境中参数初始化表Tab.1 Parameter initialization table in different experimental environments

跟随者i，i=[1,19]分别根据实验中不同机器人数量编号，其中障碍物长7 m，宽1.6 m，障碍物之间间隔4.5 m，左下角的障碍物与原点间隔为4.5 m，动态障碍物起点为(25,0)、(25,15)，终点为(5,15)、(10,3)。

2.2 实验结果及分析

编队系统的机器人数量超过20后，数据特征不明显，所以实验以编队机器人数量为5、10、15、20个分别在无冲突、有冲突机器人仿真环境中进行实验并分析结果。

2.2.1 无动态障碍物环境 图2是编队系统在无动态障碍物环境的运动轨迹，图2(a)、(b)、(c)、(d)分别为编队机器人为5、10、15、20个的运动轨迹。

(a) n=5 (b) n=10

(c) n=15 (d) n=20图 2 无动态障碍物环境改进模型编队运动轨迹图Fig.2 Formation trajectory of without dynamic obstacle environment improvement model

图2中，当编队中机器人数量增加后，领航者的路径没有发生变化。图2(a)中，5个机器人编队机器人数量少，在编队运行中跟随者都和领航者选择了相同的通道到达终点，图2(b)、(c)、(d)中，10个机器人、15个机器人、20个机器人编队由于机器人数量的增加以及车间过道较窄，所以编队在运动过程中，明显存在机器人选择其他不与领航者相同的路径，做出了动态的改变，且是在保证路径长度增加最少的情况下，完成避障后形成预期队形到达终点。

图3是编队系统在无动态障碍物环境运动时跟随者与领航者的位置误差，图3(a)、(b)、(c)、(d)分别为编队机器人为5、10、15、20个的运动轨迹。

(a) n=5 (b) n=10

图3中，跟随者与领航者的位置误差会随编队机器人数量增加而增加：10个机器人最大误差比5个机器人的增加44.35%；15个机器人最大误差较10个机器人编队增加44.25%；20个机器人时最大误差较15个机器人编队减少9.42%。从图3数据变化可以看出，出现最大误差时间随编队机器人数量增长而提前，说明在有限空间内机器人数量对于误差的产生有较大影响。同时说明本文方法有较好的调节作用；最大误差值同样跟随编队机器人数量而增加，模拟的车间中较窄的通道，导致为保持到达目的地的效率，存在跟随者选择与领航者不一致但选择最近的分岔路径情况。编队最优路径或最优路径的分支在一致性的约束下存在一个平面包线，当机器人数量增加，速度控制下的编队机器人个体的速度保持是以扩大路径包线为代价的，路径包线的扩大必然带来位置误差的增加。

在无动态障碍物环境中对编队为5个、10个、15个、20个机器人分别进行10次实验，对编队中领航者路径长度、跟随者路径长度的均值、最大值、最小值进行了统计，结果如表2所示。

表 2 无动态障碍物编队路径长度

续表 2 无动态障碍物编队路径长度

从表2可以看出，在相同环境且编队机器人数量相同的条件下进行10次仿真时，编队中领航者的路径长度变化较小，波动值约小于0.63%，说明领航者在不同的数量的编队中稳定性满足控制要求；跟随者路径长度均值、跟随者路径长度最大值与最小值的变化都不超过5.00%，在编队运行周期中表现出了很好的稳定性，将编队机器人的运动路径控制在可接受的范围内。但同时发现，上述3个值的增加值跟随编队机器人数量的增加而增加，且这个增加值是单调递增的，因此可以预见在有限空间或路径区域内，编队机器人数量存在瓶颈可能。

2.2.2 存在动态障碍物环境 在模拟环境中，增加2个随机运动的同类机器人作为动态障碍物，分别对5、10、15、20个机器人组成的编队进行动态避障实验。编队系统在动态障碍物环境的运动轨迹如图4所示，编队中跟随者与领航者的位置误差如图5所示。

(a) n=5 (b) n=10

(c) n=15 (d) n=20图 4 动态障碍物环境改进模型编队运动轨迹图Fig.4 Formation trajectory of dynamic obstacle environment improvement model

比较图2与图4，当编队机器人数量为5时，障碍物机器人的出现基本不影响编队的运动。当机器人数量增多，冲突消解算法提前改变了编队机器人的速度与方向，编队的运动轨迹没有出现较大的变化，也没有出现车间过道堵塞现象。图4中，在不遇到障碍物时，跟随者基本与领航者趋于一致，当出现障碍物时，编队的队形会被影响，但是当编队与冲突错过时，编队会自行调整位置，跟随者会快速调整与领航者趋于一致，最后安全到达终点。在有冲突机器人环境中，编队也会因为机器人数量的增加而出现机器人路径分岔的现象，这说明即使在动态障碍物环境中，编队系统也会因为需要快速到达目的地而做出及时调整。

(a) n=5 (b) n=10

(c) n=15 (d) n=20图 5 有动态障碍物环境改进模型编队运动领航者与跟随者位置误差Fig.5 Position error of leader and follower in formation motion with dynamic obstacle environment improvement model

对比图4与图5中，在有动态障碍物环境中，动态障碍物对编队运动会有影响，但由于较早的信息共享，机器人编队可以避免与动态障碍物发生正面冲突，但是由于编队机器人改变了原本的轨迹，所以跟随者与领航者误差增加。在机器人数量增加的情况编队跟随者会因为车间过道较窄分岔，位置误差会因为分岔而出现增加的情况，所以10个机器人、15个机器人、20个机器人最大误差较大。结合图5中各个机器人位置误差曲线，在编队即将到达终点时，误差都在减小状态且编队随机器人数量增加到达终点的时间都接近，证明了系统在动态障碍物环境的动态调节能力。

为了验证动态障碍物条件下本文算法具有较好的稳定性，分别对有0～3个冲突机器人的环境下，进行10次实验，再对编队中领航者路径长度、跟随者路径长度的均值、最大值、最小值进行了统计，获得结果如表3所示。

表 3 有动态障碍物编队路径长度Tab.3 Path length of formation with dynamic obstacles

续表 3 有动态障碍物编队路径长度Tab.3 Path length of formation with dynamic obstacles

从表3可以看出，领航者的路径长度均值随动态障碍物的增加而增加，但可以发现，增加的绝对值较小，长度变化不超过1.00%；重点分析跟随者路径长度均值的变化趋势，并没有明显表现出随着冲突机器人增加而增加，特别是在编队机器人数量为20时，路径均值并没有明显的上升趋势，说明领航者、动态障碍物运动信息的交互，能够为机器人编队的控制提供动态的支持。对比跟随者路径最大与最小值：在2个冲突障碍物时，5个机器人编队时差值比例为2.76%；10个机器人时，差值比例为6.62%；15个机器人，参照图3，存在跟随者机器人与领航者路径出现分叉现象，导致跟随者路径的最大值与最小值，差值比例16.27%；但是当编队机器人20个时，差值比例降低到了7.56%。较多机器人数量的编队最大、最小长度路径差值说明运动数据充分分享可以使编队中机器人的运动规划更加稳定，将编队机器人的运动路径控制在了可接受的范围内。

2.2.3 不同方法对比结果 本文将图论法，行为法结合，再提出弹簧力避障，改进动态速度限制法。为了证明本文改进算法的性能，因此设定仿真环境相同、编队为5个机器人，以机器人路径长度作为评价标准，本文算法分别与图论与领航结合算法、虚拟领航法比较，结果如表4所示。

表 4 不同方法编队路径长度Tab.4 Formation path length in different methods

从表4可以看出，由于虚拟领航法中的领航者不需要避障，所以领航者路径最短，除此之外本文算法编队机器人路径最短，冲突机器人增加时，本文算法的领航者路径相对图论与领航结合算法至少减少2.28%,跟随者路径均值、最大值、最小值至少减少2.41%，对比虚拟领航法，本文算法的跟随者路径均值、最大值、最小值至少减少3.91%。综合比较，本文算法较传统算法在机器人路径长度，编队一致性控制方面都有所改进。

3 结 语

本文分析了机器人数量增多对编队运行产生的影响，在前人研究的基础上，提出基于弹簧力映射的静态障碍物编队避障模型且在编队中增加领航机器人，建立了基于位置误差的动态速度限制函数，用于保证跟随者与领航者的队形一致性，同时，领航者的加入较好地分享了避障信息，从而提高了编队的避障速度和任务完成速度，机器人的运动效率得以提高。仿真实验结果表明，改进后的编队避障方法能够给予编队机器人较好地避障指引，同时，随机器人数量增长位置误差、平均路径长度等并未出现同比例的增加，表明了文中方法应用于较多数量的多机器人编队避障地可行性。