面向群组机器人自组装的Voronoi图边界求交细分路径规划方法

侯向辉,卢 涛,张美玉,简琤峰

(浙江工业大学 计算机学院数字媒体技术研究所,杭州 310023)

1 引 言

群组机器人是由一群无差别的微型机器人组成的群体,具有典型分布式系统的特征,通过这些有限个的能力局限的机器人交互、协调等来完成复杂的任务.与传统智能机器人相比,群组机器人在灵活性、成本控制、稳定性等方面有着绝对的优势.文献[1]将群组机器人用于水下通信方面,文献[2]将其用于部署无线网状网络(WMN),群组机器人也被应用于图形展示[3]以及工业纺织[4]等方面,可见群组机器人在各个领域正发挥独特的作用.群组机器人的研究领域中,路径规划是关键一步,其目的是为了在机器人能接受的能量消耗成本下并避免碰撞以实现目标配置.当前已有相关研究解决群体机器人路径规划问题,如领导者-跟随者策略,行为和规则方法,虚拟结构方式、基于压缩voronoi图实现自组装的方法等[5,6].领导者跟随策略方面,Peng等人提出的方法[7]是通过选出领导机器人,其它机器人并保持一定距离的跟随来实现的,R.Haghighi等人为了更好地解决机器人数量较多的问题而提出的多组协调控制方法也是基于领导者-跟随者策略.Xu、L.Pitonakova等人提出的[8,9]基于行为和规则方法通过模拟生物活动规律来实现机器人的路径规划,A.Kolling则将人类群体交互(HSI)的概念引入机器人的路径规划当中[10].L.Pitonakova等人提出的信息-成本-奖励(ICR)框架,该框架涉及机器人获取和共享信息与群体利用信息的能力,以便在特定任务和环境的情况下有效通过奖励机制合理规划机器人的运动.虚拟结构方面还有Ren等人提出的虚拟结构在车编队中的分散[11].由Wei等人提出的基于质心Voronoi镶嵌[12]的智能控制算法[13]为解决L.Bayindir提出的中央协调思想提供了可行方案,Wei等实现的同步协作调度策略CVT(Centroidal Voronoi Tessellation)算法在时间和路程损耗上都有了很大程度上的优化,然而这一通过压缩来驱使机器人移动的方案仍然存在这一些问题:1)由于采用了被动的压缩方案,仅通过限定可活动区域来趋使机器人到达目标点会造成一定的误差;2)CVT算法的死锁(deadlock)现象需要花费大量的时间去判断;3)由于采用矩形组合的方式实现目标配置,所以无法形成带有弧度的目标配置,目标配置类型较少;4)CVT算法要求初始分布为均匀分布,但是当机器人堆积在一处时,会为矩阵划分带来困难,方案适用性方面较差;5)由于并未为机器人设置目标地,在迭代的过程中,机器人的运动缺少目的性,能量消耗过多；6)死锁(deadlock)现象在高维频发,造成CVT算法在高维求解能力上表现较差.

本文提出的VBIT(Voronoi′s Boundary Intersection Tessellation)算法是一种主动性同步协作调度策略,首先确定目标配置的目标点(TP),采用Hungarian算法[14,15]为每个机器人分配TP,确定TP与起始点(DP)的连线与根据Voronoi图划分算法确定的多边形区域(cell)之间的位置关系[16]并将两者的交点作为机器人下一步运动起始点的方法,来完成机器人主动完成自组装任务的目的,通过matlab仿真对比实验证明了VBIT算法在时间消耗和路径长度上的改进效果,并展示了几种复杂的目标配置以及高维时的求解能力.

2 CVT算法

CVT算法的核心思想是利用Centroidal Voronoi的特点来实现机器人之间的碰撞避免和群组机器人的全局路径规划,通过压缩机器人的可活动区域来改变每一个cell的质心位置,以该点作为下一次迭代时机器人的目标位置,如此地反复操作,将所有机器人驱使到目标位置,过程中发生的死锁情况,通过粒子扰动来打破.

时间方面:由于传统CVT算法在机器人的移动问题上始终是被动的,所以造成了每次的压缩过程中部分机器人的移动缓慢.在迭代后期,由于可行区域压缩的程度并不大,质心的移动距离小,所以后期群组机器人整体的移动也十分缓慢,造成了无故的时间消耗,同时死锁的判断也需要耗费大量的时间.在复杂的目标配置以及机器人数量较多的情况下,CVT算法发生死锁的概率大大增加,造成其在复杂目标配置及高维情况下求解的时间大幅增长或无法得出目标配置.

路径方面:传统CVT算法通过将压缩后形成的质心位置作为下一次机器人运动的目标点,这一策略虽能避免机器人之间的碰撞,但由于质心的位置通常与机器人到目标点的路线有一定的偏差,且每次的质心位置仅与当前的压缩程度有关,与目标配置并无直接关系.

方案适用性方面:传统CVT算法通过将机器人包含在不同的矩形区域中,通过压缩这些区域,来实现不同形状的目标配置,该方案在较简单的目标配置下表现出了较好的适用性,但是对于包含任意曲线或更为复杂的2D目标配置则无法实现.

3 VBIT算法

以上分析可见,CVT算法的缺陷在于缺少主动性,仅依靠被动压缩可活动区域来达成目标配置的方法在自组装过程中会出现很多的不可控情况,其在目标配置的精度和时间上的表现仍有较大的改进空间.针对传统CVT算法的缺陷,VBIT算法进行了如下改进来解决相应问题.VBIT算法不再通过压缩可行区域的方案来驱使机器人到达目标配置,而是将目标配置具象化为若干的坐标点,由这些目标配置点来描述目标配置.匈牙利算法在此基础上分配机器人和目标配置点之间的对应关系,当机器人与目标配置点之间的对应关系确定后,分别做机器人与其对应目标配置点之间的连线,(为方便描述,在此定义机器人以及起始点的个数均为n)相应地这条直线会与每个机器人所在的cell有一个交点P={Pi,i∈1,2,…,n}或TP已经位于cell内,如有交点,则这个交点将被作为该机器人下一次运动的目标点,否则直接移动到TP.如此迭代,使得每个机器人都运动到目标位置.目标配置具象化可以有效解决目标配置精度问题、实现了算法的主动性.匈牙利分配算法能够有效地解决机器人与起始点的分配关系,减少了路径长度以及时间损耗,下面对VBIT算法的细节处进行描述，流程图如图1所示.

图1 VBIT算法流程图Fig.1 VBIT algorithm flow chart

VBIT算法的整体流程:

1.得到目标配置;

2.根据目标配置求得相应目标点的坐标位置,另外还有机器人初始位置,可活动区域等参数;

3.判断是否所有机器人均到达目标点,如果是,则算法结束,否则顺序执行;

4.利用匈牙利算法为每个机器人分配目标点;

5.根据当前机器人位置在可行区域内生成Voronoi图;

6.首先判断每个目标点是否在对应机器人所在cell内,如果是,则直接将机器人移动到目标点.否则,求得该机器人到对应起始点连线与本cell的交点,将机器人移动到该点.当所有机器人移动完后,返回到步骤3.

在上述过程中,TP坐标位置、匈牙利算法与分配目标点问题的结合、判断TP是否在cell内、求解交点坐标这些问题在后续会进行描述.

3.1 基于匈牙利算法的匹配

首先要解决机器人目标配置分配问题的标准化并建立标准化模型.要求解的是路径最小和问题,每一个机器人与起始点是惟一的一对一关系.可以归类为指派问题的数学模型,该模型的数学公式表达为:

目标函数:

(1)

Zij为成本函数,记录第i个机器人到j起始点的路径距离,MinY为总成本.

i,j=1,2,…,n

(2)

(3)

约束条件

(4)

在本文的应用环境中,成本函数矩阵Z由每个机器人对应所有起始点的距离所组成,且Z为方阵.如表1所示.

表1 成本函数矩阵Z
Table 1 Cost function matrix Z

TPRBj=1j=2j=3……i=1z11z12z13……i=2z21z22z23……i=3z31z32z33…………………………zij

算法步骤:

步骤1.建立资源分配问题的效益矩阵z0(n*n).

步骤2.从效益矩阵z0每行减去该行最小的元素,使得每行都有一个零元素,得到z1.

步骤3.从z1每列减去该列最小的元素,使得每列都有一个零元素,得到z2.

步骤4.用最少的直线覆盖z2中的零元素得到z3,如果最少直线的数量等于n,转入步骤6,否则转入步骤 5.

步骤5.矩阵z3中所有未被直线覆盖的元素减去未被覆盖元素中最小的元素,同时在直线相交点加上该最小元素得到z4,令z2=z4,转步骤4.

步骤6.从零元素最少的行或列开始指派,直到所有任务都指派完毕,得到最优指派方案P.

计算过程中涉及到的同行加减一个常数后的矩阵Z是否会发生变化,以下公式证明了前后求得的最优解是相同的.

定理1.设矩阵Z的第i行对应的常数为di

(5)

(6)

f与c仅是两个常数,所以两目标在相同约束条件下,最优解是相同的.

MinY=sum(P.*Z0)

(7)

目标函数MinY即为所求最小路径成本.

3.2 机器人和对应目标点连线与机器人所在cell的交点问题

首先需要判断起始点是否在机器人所在cell内,如果不在,则位置关系如图2所示,否则,位置关系如图3所示.

从图2,图3中可以看出利用匈牙利算法为每个机器人分配的起始点和机器人当前位置的连线与机器人所在cell的交点即为机器人下次运动的起始点,这样在最大程度上保证了机器人移动的路径优化程度.

图2 目标点位于cell外Fig.2 Target point is outside the cell

图3 目标点位于cell内Fig.3 Target point in the cell

求交的实现方法为PNPLOY算法,首先判断起始点是否在cell内,如果待测点在多边形内,从起始点引出一条射线必会与多边形有至少一个交点.交点为奇数个时表示该点位于多边形内部,反之则位于外部.伪代码表示为:

int count=0;

//以起始点P为端点从右向左引一条射线L

for(cell的一条边S)//遍历cell的每一条边

if(P在边S上)

P位于cell内;

end

if(S不是水平的)

if(S的一个端点在L上)

if(该端点是S的较大端点)

count++;

end

else if(S与L相交)

count++;

end

end

end

if(count%2==0)

P不在cell内;

else

P位于cell内;

end

由此可得到起始点与cell之间的位置关系,如果位于cell内,则将机器人直接移动到TP,如果位于cell外,那么交点坐标(Xb,Yb)可以表示为:

(8)

(9)

其中(x1,y1),(x2,y2)为第一条线段的端点坐标,(x3,y3),(x4,y4)为第二条线段的端点坐标.

将机器人移动到得到的交点坐标(Xb,Yb)上.

3.3 Sambot方向的控制

在自组装的过程中,角度控制也是关键问题,它一直是研究的热门话题.例如,当目标配置已知时,van den Berg 等人于2008年提出了实时多主体导航角度控制的“互惠速度障碍”的概念.Yu和Lavalle在2012年、Turpin等在2013和2012年提出了置换不变路径规划算法来控制每个机器人的方向.上述算法不适用于此,因为Sambots的导航过程在不同的阶段有不同的管理机制.在自组装过程中,每个Sambot的运动都是自发的,在到达目标位置之前的这个阶段,Sambot采用了基于行为的控制方法,即控制器由一系列行为组成,每个行为用于实现特定的功能.每一种行为都包括一系列的运动方案,包括(a)避免碰撞,(b)自转,(c)前进/后退直线运动和(d)前进/后退弧运动.当Sambot到达停靠区时,仅仅通过自主移动的行为控制方法来同时满足线性和角位移约束是非常困难的.因此,我们采用基于动力学的两步路径规划算法来实现每个Sambot的定向控制.根据该算法,对接区域的导航过程分为两个步骤:首先满足角位移约束,然后满足线性位移条件.这样就减轻了约束条件,有效地降低了控制难度.

3.4 Sambots自组装路径规划控制的假设

对于自组装环境,目前的工作我们只考虑2D平面的情况,以下采取5种假设.

1)开始时,Sambots在2D矩形区域内具有近似均匀的分布.

2)Sambots能够确定他们的位置和方向.

3)Sambots有足够的力量完成他们的动作,通信,对接和对接后的整体运动.

4)Sambots完全启动,受非完整约束.

5)忽略运动中的碰撞和避障问题.

根据上述假设,群体中的代理机器人不仅知道所有机器人的状态,而且知道全局最终的期望状态.这指向一个集中式算法,而不是分布式算法.这里采用集中式算法是为了保证自组装的实现.

3.5 VBIT目标配置模型

通过自组装,多个Sambots可以形成具有多种配置的机器人聚集体.在这里,我们只考虑二维空间中Sambots的自组装问题.VBIT算法采用的模型包括了CVT算法建立在一个平面上的三种典型配置,包括直线,十字和H形.除这些开链模型外,还有闭链形式的环型配置.

图5展示了由9个Sambot组成的直线型目标配置,图6展示了由11个机器人组成的H型目标配置.其中×表示DP,为TP,实线边框表示形成的目标配置形状,圆圈表示机器人当前位置.值得注意的是,对于任何的目标配置,只需要考虑目标配置的点坐标即可.同理,可以增加机器人的数量来达到相同的效果,与CVT算法所采用的矩阵压缩再拼接的方法相比,CVT算法不仅存在死锁情况,而且每次压缩之前要通过一定的比例关系求出对应目标配置的边框坐标位置,对于稍微复杂的cross型以及H型配置要事先为每个矩形分配所包含的机器人个数,过程繁复且效率不高,得到的机器人路径有过多的冗余,后期的迭代会较为迟缓,在时间上的表现也有所瑕疵.VBIT算法为主动性的路径规划方案,且每个机器人都有自己对应的目标点,因此对比CVT算法,无论是在消耗时间还是在机器人运动路径长度上,VBIT算法都有明显的优势.从图5-图6中可以发现,VBIT为每个机器人规划的路径均为直线,最大程度上减少了无谓的路径损耗,每一个机器人在自己的cell内向目标点的运动,都是最大程度上接近目标点的,所以迭代次数会大幅减少.文献[6]中提到的三个问题:如何有效地减少死锁的可能性,如何准确判断死锁状态以及如何及时对死锁进行判断.这三个问题集中在死锁上,死锁发生的原因为机器人的运动由压缩可活动区域来实现,机器人的运动没有主动性,造成如图4最右端所展现的死锁情况(两个机器人上下重叠,没有达到我们预期的一字排开的要求),这些问题在VBIT算法下都能得到有效地解决,并且随着粒子的增加,VBIT算法在时间以及路径上都表现得一样优秀、成功地寻找到了最优的全局规划方案,在本文的第四章会对上述说明给出证明.

图4 CVT算法的死锁现象图5 VBIT算法实现的line型目标配置图6 VBIT算法实现的H型目标配置Fig.4 DeadlockofCVTalgorithmFig.5 VBITalgorithm'sline-typetargetFig.6 VBITalgorithm'sH-typetargetconfigurationconfiguration

最后,需要提一下的是,由于每次的迭代过程中,机器人始终是运动到cell的边界上的,所以为了避免机器人之间的碰撞,可以按照机器人当前的运动轨迹作一定的回溯操作,使机器人退回到自己所在的cell内.

4 仿真结果与对比分析

4.1 仿真结果

通过Matlab仿真和实验对比验证了新算法的有效性.直线型、十字型、H型在Sambot的工程上都有广泛的应用,线型的可以用来模仿蛇等无足动物,十字型和H型的则可以组合搭配模仿生成多足类爬行动物等.下面对这三种典型的配置模型进行对比试验,由于CVT算法中并未涉及到环型配置,则仅展示环型目标配置形成的过程和时间消耗.传统CVT算法无法给出具体的目标配置点坐标,遂按照一定的比例来为CVT构造出一个目标配置与VBIT算法相同的目标配置.由于CVT算法在H型目标配置、高维情况下有频繁的死锁情况发生,导致CVT算法无法得到目标配置,本文仅对较为复杂的H型进行高维实验,其它目标配置的高维情况本文不展开描述.

表2以9机器人line型、9机器人十字型、11机器人H型、12机器人circle型为例说明,其中Crs表示围成目标配置的点坐标,Point表示与之对应的目标点.

实验参数如表2所示.

相应地增加机器人数量则按照一定的比例分别为CVT以及VBIT改变Crs坐标以及Point坐标.由于传统CVT算法会产生一定的误差,为了后续的对比试验描述,在此先做以下定义:已知Sambot的规格为80mm×102mm,为了在坐标系中转化更加方便,将规格近似成为100mm×100mm.本文设定允许的误差容限分别为1mm,5mm,10mm.100mm在本实验中对应的大小为1,转换后的误差容限对应在方阵中的大小为0.01、0.05、0.1.这一误差容限的意义为所有的机器人与对应目标点的距离都应小于这一数值,记为ε>max(Z)(Z为前文所提及的成本矩阵).

表2 目标配置对照表

Table 2 Target configuration comparison table

CVTVBITLINE_TYPECrs=[0.5,4.5;0.5,5.5;9.5,5.5;9.5,4.5]Point=[1,5;2,5;3,5;4,5;5,5;6,5;7,5;8,5;9,5]CROSS_TYPECrs=[2.5,4.5;2.5,5.5;4.5,5.5;4.5,7.5;5.5,7.5;5.5,5.5;7.5,5.5;7.5,4.5;5.5,4.5;5.5,24.5,2.5;4.5,4.5]Point=[3,5;4,5;5,5;6,5;7,5;5,7;5,6;5,4;5,3]

本次对比实验是在同一台电脑上进行的,为了保证实验的公平性,对比试验中所有的参数都相同,包括机器人初始位置、地图大小、目标配置位置等,比较不同数量机器人下的时间损耗和路径长度,对比试验中将VBIT算法与CVT算法在三个误差容限ε=(0.01,0.05,0.1)内的表现进行对比.在line型目标配置组下,随着粒子的增加,仅在x轴上对可移动范围做延伸.Cross以及H型配置下,可活动区域按比例扩大.图7-图10是时间以及路径长度对比图.

图7 LINE型时间消耗图8 LINE型平均路径长度图9 H型时间消耗图10 H型平均路径长度对比图对比图对比图对比图Fig.7 LINE-type'stimeFig.8 LINE-type'saveragepathFig.9 H-type'stimeFig.10 H-type'saveragepathconsumptioncomparisonchartlengthcomparisonchartconsumptioncomparisonchartlengthcomparisonchart

从三组对比试验中可以看出,时间消耗方面:在LINE型以及CROSS型下,CVT在不同误差容限中的时间消耗均显示出近似指数增长的趋势,而VBIT算法的时间消耗的增长幅度很小,始终保持在一个较低的水平.在H型目标配置下,CVT在不同误差容限下呈现出相较于LINE与CROSS型幅度更大的增长趋势,是因为H型目标配置相对更复杂,更容易陷入死锁.VBIT算法则保持在一个较低的水平,且增幅很小.路径长度方面,VBIT的平均路径长度在绝大多数情况下都要小于CVT所规划出的路径长度,由于机器人初始位置是随机的,所以路径长度并没有一个比较清晰的规律.从每个算法自身来看,在三种目标配置下,CVT算法在机器人数量增加后,所消耗的时间随着误差容限的增加而减少,其中不同误差容限内,H型的差距最为明显,CROSS型次之,LINE型最不明显,这也是由于目标配置复杂度造成的.CVT算法在不同误差容限下的路径长度也基本随着误差容限的增加而减少.而VBIT算法无论是在什么样的目标配置下,其时间消耗始终保持在一个较低的水平,与CVT算法拉开了比较大的差距.在路径长度方面,VBIT算法在三种目标配置下也基本保持着增长的趋势,但增长幅度较小,整体基本小于CVT算法.

4.2 结果分析

通过VBIT与CVT不同误差容限的对比情况来看,无论是在时间损耗还是路径长度上,VBIT算法的耗时更短、平均路径更短,并实现了CVT算法所不能实现的更为复杂的目标配置,VBIT算法在高维情况下的表现同样优秀,时间和路径长度上并没有因为机器人数目的增加而出现大幅度的增长.总体而言,VBIT算法达到了群组机器人路径规划的要求,并与CVT算法相比体现出了其优越性,实现了对群组机器人自组装过程所消耗的时间缩减和路径缩短(能量消耗).VBIT算法在高维情况下所表现出来的自组装能力达到预期要求,解决了CVT算法在高维求解能力上的不足.

5 结 论

本文提出的基于voronoi图划分的VBIT算法,并将其应用于群组机器人的自组装路径规划方面,通过matlab仿真实现了4种典型的目标装配,并与传统CVT算法在耗时以及路径长度上,分别在不同的机器人个数下进行了对比试验,VBIT算法是具有主动性、全局性、通用性等特点的改进算法,在自组装的过程中,各个机器人没有优先级关系,VBIT算法将为它们统一规划路径,由于采用了边界点作为机器人下一步的运动位置,在保证了不碰撞的前提下,每个机器人在自己的cell内做最大程度的有目的性移动.经过VBIT规划后的路径均为直线,且一一分配了机器人与起始点之间的对应关系,因此每个机器人从初始位置到起始点的距离都是较小的.由于做了最大程度上的移动,也减少了算法的迭代次数,实现了时间以及路径长度上的双赢.

相比于传统CVT算法,VBIT算法不存在死锁情况,本文中仅给出了典型的4种目标配置,实际中VBIT算法 可以实现更多2D空间内的任意形状,今后工作在此算法基础上进行3D方面以及避障方面的拓展研究.