面向群組機器人自組裝的Voronoi圖邊界求交細分路徑規(guī)劃方法

侯向輝,盧 濤,張美玉,簡琤峰

(浙江工業(yè)大學 計算機學院數(shù)字媒體技術研究所,杭州 310023)

1 引 言

群組機器人是由一群無差別的微型機器人組成的群體,具有典型分布式系統(tǒng)的特征,通過這些有限個的能力局限的機器人交互、協(xié)調(diào)等來完成復雜的任務.與傳統(tǒng)智能機器人相比,群組機器人在靈活性、成本控制、穩(wěn)定性等方面有著絕對的優(yōu)勢.文獻[1]將群組機器人用于水下通信方面,文獻[2]將其用于部署無線網(wǎng)狀網(wǎng)絡(WMN),群組機器人也被應用于圖形展示[3]以及工業(yè)紡織[4]等方面,可見群組機器人在各個領域正發(fā)揮獨特的作用.群組機器人的研究領域中,路徑規(guī)劃是關鍵一步,其目的是為了在機器人能接受的能量消耗成本下并避免碰撞以實現(xiàn)目標配置.當前已有相關研究解決群體機器人路徑規(guī)劃問題,如領導者-跟隨者策略,行為和規(guī)則方法,虛擬結(jié)構方式、基于壓縮voronoi圖實現(xiàn)自組裝的方法等[5,6].領導者跟隨策略方面,Peng等人提出的方法[7]是通過選出領導機器人,其它機器人并保持一定距離的跟隨來實現(xiàn)的,R.Haghighi等人為了更好地解決機器人數(shù)量較多的問題而提出的多組協(xié)調(diào)控制方法也是基于領導者-跟隨者策略.Xu、L.Pitonakova等人提出的[8,9]基于行為和規(guī)則方法通過模擬生物活動規(guī)律來實現(xiàn)機器人的路徑規(guī)劃,A.Kolling則將人類群體交互(HSI)的概念引入機器人的路徑規(guī)劃當中[10].L.Pitonakova等人提出的信息-成本-獎勵(ICR)框架,該框架涉及機器人獲取和共享信息與群體利用信息的能力,以便在特定任務和環(huán)境的情況下有效通過獎勵機制合理規(guī)劃機器人的運動.虛擬結(jié)構方面還有Ren等人提出的虛擬結(jié)構在車編隊中的分散[11].由Wei等人提出的基于質(zhì)心Voronoi鑲嵌[12]的智能控制算法[13]為解決L.Bayindir提出的中央?yún)f(xié)調(diào)思想提供了可行方案,Wei等實現(xiàn)的同步協(xié)作調(diào)度策略CVT(Centroidal Voronoi Tessellation)算法在時間和路程損耗上都有了很大程度上的優(yōu)化,然而這一通過壓縮來驅(qū)使機器人移動的方案仍然存在這一些問題:1)由于采用了被動的壓縮方案,僅通過限定可活動區(qū)域來趨使機器人到達目標點會造成一定的誤差;2)CVT算法的死鎖(deadlock)現(xiàn)象需要花費大量的時間去判斷;3)由于采用矩形組合的方式實現(xiàn)目標配置,所以無法形成帶有弧度的目標配置,目標配置類型較少;4)CVT算法要求初始分布為均勻分布,但是當機器人堆積在一處時,會為矩陣劃分帶來困難,方案適用性方面較差;5)由于并未為機器人設置目標地,在迭代的過程中,機器人的運動缺少目的性,能量消耗過多；6)死鎖(deadlock)現(xiàn)象在高維頻發(fā),造成CVT算法在高維求解能力上表現(xiàn)較差.

本文提出的VBIT(Voronoi′s Boundary Intersection Tessellation)算法是一種主動性同步協(xié)作調(diào)度策略,首先確定目標配置的目標點(TP),采用Hungarian算法[14,15]為每個機器人分配TP,確定TP與起始點(DP)的連線與根據(jù)Voronoi圖劃分算法確定的多邊形區(qū)域(cell)之間的位置關系[16]并將兩者的交點作為機器人下一步運動起始點的方法,來完成機器人主動完成自組裝任務的目的,通過matlab仿真對比實驗證明了VBIT算法在時間消耗和路徑長度上的改進效果,并展示了幾種復雜的目標配置以及高維時的求解能力.

2 CVT算法

CVT算法的核心思想是利用Centroidal Voronoi的特點來實現(xiàn)機器人之間的碰撞避免和群組機器人的全局路徑規(guī)劃,通過壓縮機器人的可活動區(qū)域來改變每一個cell的質(zhì)心位置,以該點作為下一次迭代時機器人的目標位置,如此地反復操作,將所有機器人驅(qū)使到目標位置,過程中發(fā)生的死鎖情況,通過粒子擾動來打破.

時間方面:由于傳統(tǒng)CVT算法在機器人的移動問題上始終是被動的,所以造成了每次的壓縮過程中部分機器人的移動緩慢.在迭代后期,由于可行區(qū)域壓縮的程度并不大,質(zhì)心的移動距離小,所以后期群組機器人整體的移動也十分緩慢,造成了無故的時間消耗,同時死鎖的判斷也需要耗費大量的時間.在復雜的目標配置以及機器人數(shù)量較多的情況下,CVT算法發(fā)生死鎖的概率大大增加,造成其在復雜目標配置及高維情況下求解的時間大幅增長或無法得出目標配置.

路徑方面:傳統(tǒng)CVT算法通過將壓縮后形成的質(zhì)心位置作為下一次機器人運動的目標點,這一策略雖能避免機器人之間的碰撞,但由于質(zhì)心的位置通常與機器人到目標點的路線有一定的偏差,且每次的質(zhì)心位置僅與當前的壓縮程度有關,與目標配置并無直接關系.

方案適用性方面:傳統(tǒng)CVT算法通過將機器人包含在不同的矩形區(qū)域中,通過壓縮這些區(qū)域,來實現(xiàn)不同形狀的目標配置,該方案在較簡單的目標配置下表現(xiàn)出了較好的適用性,但是對于包含任意曲線或更為復雜的2D目標配置則無法實現(xiàn).

3 VBIT算法

以上分析可見,CVT算法的缺陷在于缺少主動性,僅依靠被動壓縮可活動區(qū)域來達成目標配置的方法在自組裝過程中會出現(xiàn)很多的不可控情況,其在目標配置的精度和時間上的表現(xiàn)仍有較大的改進空間.針對傳統(tǒng)CVT算法的缺陷,VBIT算法進行了如下改進來解決相應問題.VBIT算法不再通過壓縮可行區(qū)域的方案來驅(qū)使機器人到達目標配置,而是將目標配置具象化為若干的坐標點,由這些目標配置點來描述目標配置.匈牙利算法在此基礎上分配機器人和目標配置點之間的對應關系,當機器人與目標配置點之間的對應關系確定后,分別做機器人與其對應目標配置點之間的連線,(為方便描述,在此定義機器人以及起始點的個數(shù)均為n)相應地這條直線會與每個機器人所在的cell有一個交點P={Pi,i∈1,2,…,n}或TP已經(jīng)位于cell內(nèi),如有交點,則這個交點將被作為該機器人下一次運動的目標點,否則直接移動到TP.如此迭代,使得每個機器人都運動到目標位置.目標配置具象化可以有效解決目標配置精度問題、實現(xiàn)了算法的主動性.匈牙利分配算法能夠有效地解決機器人與起始點的分配關系,減少了路徑長度以及時間損耗,下面對VBIT算法的細節(jié)處進行描述，流程圖如圖1所示.

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

VBIT算法的整體流程:

1.得到目標配置;

2.根據(jù)目標配置求得相應目標點的坐標位置,另外還有機器人初始位置,可活動區(qū)域等參數(shù);

3.判斷是否所有機器人均到達目標點,如果是,則算法結(jié)束,否則順序執(zhí)行;

4.利用匈牙利算法為每個機器人分配目標點;

5.根據(jù)當前機器人位置在可行區(qū)域內(nèi)生成Voronoi圖;

6.首先判斷每個目標點是否在對應機器人所在cell內(nèi),如果是,則直接將機器人移動到目標點.否則,求得該機器人到對應起始點連線與本cell的交點,將機器人移動到該點.當所有機器人移動完后,返回到步驟3.

在上述過程中,TP坐標位置、匈牙利算法與分配目標點問題的結(jié)合、判斷TP是否在cell內(nèi)、求解交點坐標這些問題在后續(xù)會進行描述.

3.1 基于匈牙利算法的匹配

首先要解決機器人目標配置分配問題的標準化并建立標準化模型.要求解的是路徑最小和問題,每一個機器人與起始點是惟一的一對一關系.可以歸類為指派問題的數(shù)學模型,該模型的數(shù)學公式表達為:

目標函數(shù):

(1)

Zij為成本函數(shù),記錄第i個機器人到j起始點的路徑距離,MinY為總成本.

i,j=1,2,…,n

(2)

(3)

約束條件

(4)

在本文的應用環(huán)境中,成本函數(shù)矩陣Z由每個機器人對應所有起始點的距離所組成,且Z為方陣.如表1所示.

表1 成本函數(shù)矩陣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,如果最少直線的數(shù)量等于n,轉(zhuǎn)入步驟6,否則轉(zhuǎn)入步驟 5.

步驟5.矩陣z3中所有未被直線覆蓋的元素減去未被覆蓋元素中最小的元素,同時在直線相交點加上該最小元素得到z4,令z2=z4,轉(zhuǎn)步驟4.

步驟6.從零元素最少的行或列開始指派,直到所有任務都指派完畢,得到最優(yōu)指派方案P.

計算過程中涉及到的同行加減一個常數(shù)后的矩陣Z是否會發(fā)生變化,以下公式證明了前后求得的最優(yōu)解是相同的.

定理1.設矩陣Z的第i行對應的常數(shù)為di

(5)

(6)

f與c僅是兩個常數(shù),所以兩目標在相同約束條件下,最優(yōu)解是相同的.

MinY=sum(P.*Z0)

(7)

目標函數(shù)MinY即為所求最小路徑成本.

3.2 機器人和對應目標點連線與機器人所在cell的交點問題

首先需要判斷起始點是否在機器人所在cell內(nèi),如果不在,則位置關系如圖2所示,否則,位置關系如圖3所示.

從圖2,圖3中可以看出利用匈牙利算法為每個機器人分配的起始點和機器人當前位置的連線與機器人所在cell的交點即為機器人下次運動的起始點,這樣在最大程度上保證了機器人移動的路徑優(yōu)化程度.

圖2 目標點位于cell外Fig.2 Target point is outside the cell

圖3 目標點位于cell內(nèi)Fig.3 Target point in the cell

求交的實現(xiàn)方法為PNPLOY算法,首先判斷起始點是否在cell內(nèi),如果待測點在多邊形內(nèi),從起始點引出一條射線必會與多邊形有至少一個交點.交點為奇數(shù)個時表示該點位于多邊形內(nèi)部,反之則位于外部.偽代碼表示為:

int count=0;

//以起始點P為端點從右向左引一條射線L

for(cell的一條邊S)//遍歷cell的每一條邊

if(P在邊S上)

P位于cell內(nèi);

end

if(S不是水平的)

if(S的一個端點在L上)

if(該端點是S的較大端點)

count++;

end

else if(S與L相交)

count++;

end

end

end

if(count%2==0)

P不在cell內(nèi);

else

P位于cell內(nèi);

end

由此可得到起始點與cell之間的位置關系,如果位于cell內(nèi),則將機器人直接移動到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年提出了置換不變路徑規(guī)劃算法來控制每個機器人的方向.上述算法不適用于此,因為Sambots的導航過程在不同的階段有不同的管理機制.在自組裝過程中,每個Sambot的運動都是自發(fā)的,在到達目標位置之前的這個階段,Sambot采用了基于行為的控制方法,即控制器由一系列行為組成,每個行為用于實現(xiàn)特定的功能.每一種行為都包括一系列的運動方案,包括(a)避免碰撞,(b)自轉(zhuǎn),(c)前進/后退直線運動和(d)前進/后退弧運動.當Sambot到達?？繀^(qū)時,僅僅通過自主移動的行為控制方法來同時滿足線性和角位移約束是非常困難的.因此,我們采用基于動力學的兩步路徑規(guī)劃算法來實現(xiàn)每個Sambot的定向控制.根據(jù)該算法,對接區(qū)域的導航過程分為兩個步驟:首先滿足角位移約束,然后滿足線性位移條件.這樣就減輕了約束條件,有效地降低了控制難度.

3.4 Sambots自組裝路徑規(guī)劃控制的假設

對于自組裝環(huán)境,目前的工作我們只考慮2D平面的情況,以下采取5種假設.

1)開始時,Sambots在2D矩形區(qū)域內(nèi)具有近似均勻的分布.

2)Sambots能夠確定他們的位置和方向.

3)Sambots有足夠的力量完成他們的動作,通信,對接和對接后的整體運動.

4)Sambots完全啟動,受非完整約束.

5)忽略運動中的碰撞和避障問題.

根據(jù)上述假設,群體中的代理機器人不僅知道所有機器人的狀態(tài),而且知道全局最終的期望狀態(tài).這指向一個集中式算法,而不是分布式算法.這里采用集中式算法是為了保證自組裝的實現(xiàn).

3.5 VBIT目標配置模型

通過自組裝,多個Sambots可以形成具有多種配置的機器人聚集體.在這里,我們只考慮二維空間中Sambots的自組裝問題.VBIT算法采用的模型包括了CVT算法建立在一個平面上的三種典型配置,包括直線,十字和H形.除這些開鏈模型外,還有閉鏈形式的環(huán)型配置.

圖5展示了由9個Sambot組成的直線型目標配置,圖6展示了由11個機器人組成的H型目標配置.其中×表示DP,為TP,實線邊框表示形成的目標配置形狀,圓圈表示機器人當前位置.值得注意的是,對于任何的目標配置,只需要考慮目標配置的點坐標即可.同理,可以增加機器人的數(shù)量來達到相同的效果,與CVT算法所采用的矩陣壓縮再拼接的方法相比,CVT算法不僅存在死鎖情況,而且每次壓縮之前要通過一定的比例關系求出對應目標配置的邊框坐標位置,對于稍微復雜的cross型以及H型配置要事先為每個矩形分配所包含的機器人個數(shù),過程繁復且效率不高,得到的機器人路徑有過多的冗余,后期的迭代會較為遲緩,在時間上的表現(xiàn)也有所瑕疵.VBIT算法為主動性的路徑規(guī)劃方案,且每個機器人都有自己對應的目標點,因此對比CVT算法,無論是在消耗時間還是在機器人運動路徑長度上,VBIT算法都有明顯的優(yōu)勢.從圖5-圖6中可以發(fā)現(xiàn),VBIT為每個機器人規(guī)劃的路徑均為直線,最大程度上減少了無謂的路徑損耗,每一個機器人在自己的cell內(nèi)向目標點的運動,都是最大程度上接近目標點的,所以迭代次數(shù)會大幅減少.文獻[6]中提到的三個問題:如何有效地減少死鎖的可能性,如何準確判斷死鎖狀態(tài)以及如何及時對死鎖進行判斷.這三個問題集中在死鎖上,死鎖發(fā)生的原因為機器人的運動由壓縮可活動區(qū)域來實現(xiàn),機器人的運動沒有主動性,造成如圖4最右端所展現(xiàn)的死鎖情況(兩個機器人上下重疊,沒有達到我們預期的一字排開的要求),這些問題在VBIT算法下都能得到有效地解決,并且隨著粒子的增加,VBIT算法在時間以及路徑上都表現(xiàn)得一樣優(yōu)秀、成功地尋找到了最優(yōu)的全局規(guī)劃方案,在本文的第四章會對上述說明給出證明.

圖4 CVT算法的死鎖現(xiàn)象圖5 VBIT算法實現(xiàn)的line型目標配置圖6 VBIT算法實現(xiàn)的H型目標配置Fig.4 DeadlockofCVTalgorithmFig.5 VBITalgorithm'sline-typetargetFig.6 VBITalgorithm'sH-typetargetconfigurationconfiguration

最后,需要提一下的是,由于每次的迭代過程中,機器人始終是運動到cell的邊界上的,所以為了避免機器人之間的碰撞,可以按照機器人當前的運動軌跡作一定的回溯操作,使機器人退回到自己所在的cell內(nèi).

4 仿真結(jié)果與對比分析

4.1 仿真結(jié)果

通過Matlab仿真和實驗對比驗證了新算法的有效性.直線型、十字型、H型在Sambot的工程上都有廣泛的應用,線型的可以用來模仿蛇等無足動物,十字型和H型的則可以組合搭配模仿生成多足類爬行動物等.下面對這三種典型的配置模型進行對比試驗,由于CVT算法中并未涉及到環(huán)型配置,則僅展示環(huán)型目標配置形成的過程和時間消耗.傳統(tǒng)CVT算法無法給出具體的目標配置點坐標,遂按照一定的比例來為CVT構造出一個目標配置與VBIT算法相同的目標配置.由于CVT算法在H型目標配置、高維情況下有頻繁的死鎖情況發(fā)生,導致CVT算法無法得到目標配置,本文僅對較為復雜的H型進行高維實驗,其它目標配置的高維情況本文不展開描述.

表2以9機器人line型、9機器人十字型、11機器人H型、12機器人circle型為例說明,其中Crs表示圍成目標配置的點坐標,Point表示與之對應的目標點.

實驗參數(shù)如表2所示.

相應地增加機器人數(shù)量則按照一定的比例分別為CVT以及VBIT改變Crs坐標以及Point坐標.由于傳統(tǒng)CVT算法會產(chǎn)生一定的誤差,為了后續(xù)的對比試驗描述,在此先做以下定義:已知Sambot的規(guī)格為80mm×102mm,為了在坐標系中轉(zhuǎn)化更加方便,將規(guī)格近似成為100mm×100mm.本文設定允許的誤差容限分別為1mm,5mm,10mm.100mm在本實驗中對應的大小為1,轉(zhuǎn)換后的誤差容限對應在方陣中的大小為0.01、0.05、0.1.這一誤差容限的意義為所有的機器人與對應目標點的距離都應小于這一數(shù)值,記為ε>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]

本次對比實驗是在同一臺電腦上進行的,為了保證實驗的公平性,對比試驗中所有的參數(shù)都相同,包括機器人初始位置、地圖大小、目標配置位置等,比較不同數(shù)量機器人下的時間損耗和路徑長度,對比試驗中將VBIT算法與CVT算法在三個誤差容限ε=(0.01,0.05,0.1)內(nèi)的表現(xiàn)進行對比.在line型目標配置組下,隨著粒子的增加,僅在x軸上對可移動范圍做延伸.Cross以及H型配置下,可活動區(qū)域按比例擴大.圖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在不同誤差容限中的時間消耗均顯示出近似指數(shù)增長的趨勢,而VBIT算法的時間消耗的增長幅度很小,始終保持在一個較低的水平.在H型目標配置下,CVT在不同誤差容限下呈現(xiàn)出相較于LINE與CROSS型幅度更大的增長趨勢,是因為H型目標配置相對更復雜,更容易陷入死鎖.VBIT算法則保持在一個較低的水平,且增幅很小.路徑長度方面,VBIT的平均路徑長度在絕大多數(shù)情況下都要小于CVT所規(guī)劃出的路徑長度,由于機器人初始位置是隨機的,所以路徑長度并沒有一個比較清晰的規(guī)律.從每個算法自身來看,在三種目標配置下,CVT算法在機器人數(shù)量增加后,所消耗的時間隨著誤差容限的增加而減少,其中不同誤差容限內(nèi),H型的差距最為明顯,CROSS型次之,LINE型最不明顯,這也是由于目標配置復雜度造成的.CVT算法在不同誤差容限下的路徑長度也基本隨著誤差容限的增加而減少.而VBIT算法無論是在什么樣的目標配置下,其時間消耗始終保持在一個較低的水平,與CVT算法拉開了比較大的差距.在路徑長度方面,VBIT算法在三種目標配置下也基本保持著增長的趨勢,但增長幅度較小,整體基本小于CVT算法.

4.2 結(jié)果分析

通過VBIT與CVT不同誤差容限的對比情況來看,無論是在時間損耗還是路徑長度上,VBIT算法的耗時更短、平均路徑更短,并實現(xiàn)了CVT算法所不能實現(xiàn)的更為復雜的目標配置,VBIT算法在高維情況下的表現(xiàn)同樣優(yōu)秀,時間和路徑長度上并沒有因為機器人數(shù)目的增加而出現(xiàn)大幅度的增長.總體而言,VBIT算法達到了群組機器人路徑規(guī)劃的要求,并與CVT算法相比體現(xiàn)出了其優(yōu)越性,實現(xiàn)了對群組機器人自組裝過程所消耗的時間縮減和路徑縮短(能量消耗).VBIT算法在高維情況下所表現(xiàn)出來的自組裝能力達到預期要求,解決了CVT算法在高維求解能力上的不足.

5 結(jié) 論

本文提出的基于voronoi圖劃分的VBIT算法,并將其應用于群組機器人的自組裝路徑規(guī)劃方面,通過matlab仿真實現(xiàn)了4種典型的目標裝配,并與傳統(tǒng)CVT算法在耗時以及路徑長度上,分別在不同的機器人個數(shù)下進行了對比試驗,VBIT算法是具有主動性、全局性、通用性等特點的改進算法,在自組裝的過程中,各個機器人沒有優(yōu)先級關系,VBIT算法將為它們統(tǒng)一規(guī)劃路徑,由于采用了邊界點作為機器人下一步的運動位置,在保證了不碰撞的前提下,每個機器人在自己的cell內(nèi)做最大程度的有目的性移動.經(jīng)過VBIT規(guī)劃后的路徑均為直線,且一一分配了機器人與起始點之間的對應關系,因此每個機器人從初始位置到起始點的距離都是較小的.由于做了最大程度上的移動,也減少了算法的迭代次數(shù),實現(xiàn)了時間以及路徑長度上的雙贏.

相比于傳統(tǒng)CVT算法,VBIT算法不存在死鎖情況,本文中僅給出了典型的4種目標配置,實際中VBIT算法 可以實現(xiàn)更多2D空間內(nèi)的任意形狀,今后工作在此算法基礎上進行3D方面以及避障方面的拓展研究.