戶曉玲，王健安

(太原科技大學(xué) 華科學(xué)院，山西 太原 030024)

0 引 言

自20世紀(jì)90年代開始，多機(jī)器人編隊(duì)問題一直被眾多機(jī)器人愛好者廣泛研究。E. Bahceci等[1]在2003年將多機(jī)器人編隊(duì)問題定義為：一群機(jī)器人通過相互之間的協(xié)調(diào)，形成并保持一種特定幾何形狀的隊(duì)形。多機(jī)器人編隊(duì)控制[2-5]是指多個(gè)機(jī)器人組成的群體在適應(yīng)環(huán)境約束的條件下保持特定的隊(duì)形搜索到指定目標(biāo)的一門控制技術(shù)。多機(jī)器人編隊(duì)在執(zhí)行如目標(biāo)搜索、地圖探測(cè)以及地震搜救等復(fù)雜任務(wù)中，編成特定隊(duì)形展開目標(biāo)搜索十分重要。因此，如何讓機(jī)器人根據(jù)指定的搜索目標(biāo)控制編隊(duì)迅速形成指定隊(duì)形具有重要的研究意義。

對(duì)多機(jī)器人編隊(duì)控制的研究最早有Reynolds提出的基于行為的編隊(duì)控制模型[6]、J. P. Desai等提出的領(lǐng)航跟隨法和圖論法[7]、Khatbi等提出的人工勢(shì)場(chǎng)法[8]，以及Ren等提出的虛擬結(jié)構(gòu)法[9]。典型的成果有英國的JAYDE和美國的VIJAY用控制矩陣和狀態(tài)轉(zhuǎn)換圖來實(shí)現(xiàn)編隊(duì)的變化[10-11]，國內(nèi)有沈陽自動(dòng)化設(shè)計(jì)的MARCAS系統(tǒng)和陳衛(wèi)東等提出的行為融合加權(quán)法[12-13]。隨著任務(wù)難度越來越大，基本的編隊(duì)算法已經(jīng)無法滿足目前的需求，一些新的編隊(duì)控制算法應(yīng)運(yùn)而生。

文中提出一種新的基于微粒群模型的機(jī)器人編隊(duì)控制算法，該算法通過構(gòu)造適應(yīng)值函數(shù)，并采用微粒群算法獲取適應(yīng)值函數(shù)的最優(yōu)解，將優(yōu)化過程中的最優(yōu)解作為機(jī)器人新的運(yùn)動(dòng)方向，并利用MATLAB軟件進(jìn)行仿真。

1 機(jī)器人編隊(duì)優(yōu)化模型

1.1 問題的描述和定義

定義一個(gè)二維連續(xù)空間表示物理環(huán)境：T2={(x,y),0≤x≤xmax,0≤y≤ymax}。

Ω為包含所有機(jī)器人的群體集合，Ω={S1,S2,…,Sn}；Si表示簡單智能的機(jī)器人，坐標(biāo)為PSi=(xi,yi)。

(1)線形編隊(duì)表示機(jī)器人與X軸橫坐標(biāo)的夾角為θ(θ∈(0～360°))，兩相鄰機(jī)器人之間的距離為常數(shù)d的隊(duì)形。

(2)圓形編隊(duì)表示機(jī)器人均勻排列成以極點(diǎn)為圓心，半徑為r的圓，其中θi為第i個(gè)機(jī)器人與X軸正半軸的夾角。

(3)三角形編隊(duì)表示以S1、S2和S3為三個(gè)頂點(diǎn)機(jī)器人排成的一個(gè)正三角形。假設(shè)機(jī)器人數(shù)量為n，定義函數(shù)round(X)表示將X四舍五入取整。

第一條邊上的機(jī)器人為:A1～Around((n+3)/3)；

第二條邊上的機(jī)器人為:Around((n+3)/3)～Around((n+round((n+3)/3))/2)；

第三條邊上的機(jī)器人為:Around((n+round((n+3)/3))/2)～An,A1。

(4)六邊形編隊(duì)表示六個(gè)機(jī)器人排成正六邊形。設(shè)機(jī)器人數(shù)量為n，每條邊上從起始機(jī)器人到終止機(jī)器人的確定方法為：用S1表示第一條邊的終點(diǎn)機(jī)器人編號(hào)，Si表示第i條邊的終點(diǎn)機(jī)器人編號(hào)，定義符號(hào)[ ]表示向下取整。則：

Si=Si-1+[(n-Si-1)+(7-i)/(7-i)]，

i=1,2,…,5，S0=0

采用分布式控制方式，將每個(gè)機(jī)器人初始化為一個(gè)微粒，每個(gè)機(jī)器人根據(jù)微粒群算法(PSO)[14]優(yōu)化目標(biāo)隊(duì)形(線形、三角形、圓形、六邊形)的位置，通過更新Pbest來獲得gbest，機(jī)器人向gbest的方向運(yùn)動(dòng)，實(shí)現(xiàn)多機(jī)器人編隊(duì)和隊(duì)形變換。

1.2 編隊(duì)優(yōu)化算法

編隊(duì)優(yōu)化算法的基本思路是構(gòu)造一個(gè)包含Ω中單個(gè)機(jī)器人位置信息的適應(yīng)值函數(shù)f，將f取極小值時(shí)的變量作為目標(biāo)隊(duì)形的最終位置。用PSO尋找f極值中的pg(微粒迭代過程中的最好位置)作為機(jī)器人Ω的運(yùn)動(dòng)方向。機(jī)器人的位置和速度迭代公式如下：

(1)

(2)

式1采用閉環(huán)控制，機(jī)器人的期望位置pgi和機(jī)器人所感知到的實(shí)際位置pAi之差為誤差信號(hào)?，即?=pgi-pAi。通過將機(jī)器人的運(yùn)行速度V設(shè)定為?的函數(shù)來驅(qū)動(dòng)機(jī)器人移動(dòng)到正確的位置?，F(xiàn)設(shè)V=ω?，ω為權(quán)值，有：

(3)

算法的總體流程如圖1所示。

圖1 算法總體流程

1.3 不同隊(duì)形適應(yīng)值函數(shù)的構(gòu)造

(1)線形隊(duì)形f如下：

f(x1,y1,x2,y2,…,xn,yn)=

(4)

其中，θ指直線與X軸正方向夾角；d指兩相鄰機(jī)器人之間的直線距離。

(2)三角隊(duì)形f見式5，設(shè)round((n+3)/3)=p，round((n+round((n+3)/3))/2)=q。

f(x1,y1,x2,y2,…,xn,yn)=

[d1sinθ1-(yi-yi+1)]2}+

[d2sinθ2-(yj-yj+1)]2}+

[d3sinθ3-(yk-yk+1)]2+

[(d3cosθ3-(xn-x1)]2+

[d3sinθ3-(yn-y1)]2}

(5)

其中，di為第i條邊上相鄰機(jī)器人之間的距離；θi為第i條邊與X軸正方向的夾角。通過調(diào)整f中的di，θi即可得到任意形狀的三角形。若為正三角形，則θi=θi-1+2π/3,i=2,3。

(3)圓形隊(duì)形是以極點(diǎn)為圓心，r為半徑的圓，適應(yīng)值函數(shù)為：

(4)六邊形適應(yīng)值函數(shù)如式7所示：

f(x1,y1,x2,y2,…,xn,yn)=

[d1sinθ1-(yi-yi+1)]2}+

[d2sinθ2-(yj-yj+1)]2}+

[d3sinθ3-(yk-yk+1)]2}+

[d4sinθ4-(yl-yl+1)]2}+

[d5sinθ5-(ym-ym+1)]2}+

[d6sinθ6-(yi-yi+1)]2+

[d6cosθ6-(xn-x1)]2+

[d6sinθ6-(yn-y1)]2}

(7)

其中，di指第i條邊上相鄰機(jī)器人之間的距離；θi指第i條邊與X軸正方向的夾角。通過調(diào)整f中的di和θi可以得到任意形狀的六邊形。正六邊形為θi=θi-1+π/3,i=2,3,4,5,6。

2 隊(duì)列變換策略

2.1 線形與三角形的變換

線形變?nèi)切危焊鶕?jù)機(jī)器人個(gè)數(shù)n計(jì)算出S1和S2，S1～S2的機(jī)器人位置不變；1～S1-1的機(jī)器人以S1為參考點(diǎn)排成直線，斜率θ1=θ+2π/3，θ為原直線的斜率；目標(biāo)函數(shù)如式8所示。編號(hào)S2+1～S1的機(jī)器人以S2為參考點(diǎn)排成直線，直線斜率θ2=θ+π/3，目標(biāo)函數(shù)如式9所示。

f(xs2+1,ys2+1,xs2+2,ys2+2,…,xs,ys)=

[(i-S2)×dsinθ2-(yi-yS2)]2}

(8)

f(x1,y1,x2,y2,…,xs1-1,ys1-1)=

[(s1-i)×dsinθ1-(yi-ys1)]2}

(9)

三角形變線形：S1～S2的機(jī)器人保持位置不變，以S1和S2所在直線為另外兩條邊的斜率形成直線形。使用前面提到的直線形編隊(duì)進(jìn)行優(yōu)化。

2.2 三角形與圓形的變換

三角形變圓形：將1、S1、S2三個(gè)機(jī)器人的重心o作為極點(diǎn)，圓的半徑r可根據(jù)需要選擇，取所有的機(jī)器人到重心的平均距離作為r。機(jī)器人的速度和位置迭代公式如式10和式11所示，其中di為機(jī)器人到重心的距離。

Vi+1=g×(Xi-O)×(r-di)

(10)

Xi+1=Xi+Vi+1×Δt

(11)

圓形變?nèi)切危焊鶕?jù)x=r×cosθ和y=r×sinθ將極坐標(biāo)系轉(zhuǎn)化為直角坐標(biāo)系，通過n計(jì)算S1和S2，則1、S1、S2成為三角形的三個(gè)頂點(diǎn)。再用式12和式13將兩頂點(diǎn)之間的機(jī)器人運(yùn)動(dòng)到兩頂點(diǎn)所在直線上，其中o1、o2為兩頂點(diǎn)的位置，n1為兩頂點(diǎn)間機(jī)器人的個(gè)數(shù)。

(12)

Xi+1=Xi+Vi+1×Δt

(13)

2.3 圓形與六邊形的變換

圓形變六邊形：根據(jù)n1計(jì)算出S1～S5，則1,S1～S5變成六邊形的六個(gè)頂點(diǎn)。用式14和式15將兩頂點(diǎn)之間的機(jī)器人運(yùn)動(dòng)到兩頂點(diǎn)所連成的線段上，其中o1、o2為兩頂點(diǎn)的位置。

(14)

Xi+1=Xi+Vi+1×Δt

(15)

六邊形變圓形：極點(diǎn)為1,S1～S5六個(gè)機(jī)器人的重心o，根據(jù)式16和式17更新機(jī)器人的速度和位置，其中圓心半徑r為所有機(jī)器人到重心的平均距離，di為機(jī)器人到重心的距離。

Vi+1=g×(Xi-o)×(r-di)

(16)

Xi+1=Xi+Vi+1×Δt

(17)

3 仿真實(shí)驗(yàn)結(jié)果

3.1 不同形狀編隊(duì)仿真

圖2(a)是線形仿真，n=7，θ1=5π/6，d=10時(shí)，迭代了528次。圖中星號(hào)表示初始位置時(shí)機(jī)器人的團(tuán)隊(duì)重心。圖2(b)是三角形仿真，n1=12，θ1=π/3，邊長為60時(shí)，迭代1 844次的結(jié)果。圖2(c)是圓形仿真，n=10，r=15時(shí)，迭代2 076次的結(jié)果。圖2(d)是六邊形仿真，n=20，邊長為40，θ1=π/3時(shí)，迭代4 101次的結(jié)果。

表1表示在不同機(jī)器人個(gè)數(shù)下對(duì)四種隊(duì)形編隊(duì)仿真時(shí)，算法執(zhí)行50次的平均迭代次數(shù)、最好迭代次數(shù)和最壞迭代次數(shù)，以及迭代失敗次數(shù)和迭代次數(shù)的方差。

圖2 線形、三角形、圓形、六邊形仿真

形狀算法執(zhí)行50次平均迭代次數(shù)50次中最好迭代次數(shù)50次中最壞迭代次數(shù)所有迭代次數(shù)的方差線形θ=4*π/3,n=7449.440 027860875.339 5θ=4*π/3,n=101.023 3e+38051 17376.937 6三角形θ1=π/3,n=8960.300 03301 491259.940 3θ1=π/6,n=121 8641 2342 379240.864 1圓形r=15,n=102.233 8e+31 9083 337266.249 4r=30,n=203.228 5e+33 0323 906163.555 5六邊形θ1=π/3,n=152.628 1e+32 0883 004208.547 5θ1=π/3,n=204.281 7e+33 8764 624167.311 4

3.2 四種形狀隊(duì)形變換仿真

圖3機(jī)器人數(shù)量為10，s1=4，s2=7。(c)、(d)的點(diǎn)為s1與s2之間的機(jī)器人。圖(a)的線形到圖(b)的三角形，迭代次數(shù)為95，運(yùn)動(dòng)軌跡為(c)。從圖(b)的三角形到圖(a)的線形，迭代次數(shù)為88，運(yùn)動(dòng)軌跡為圖(d)。

圖3 線形和三角形變換

圖4中的機(jī)器人數(shù)量為20個(gè)，迭代次數(shù)為100，仿真中機(jī)器人是直接運(yùn)動(dòng)到目標(biāo)點(diǎn)的。

圖4 三角形與圓形的變換

圖5中機(jī)器人數(shù)量為18個(gè)，固定迭代次數(shù)為100。圖5是圓形與六邊形的變換。

圖5 圓形與六邊形的變換

從仿真結(jié)果可以看出，利用該計(jì)算方法能快速地實(shí)現(xiàn)多機(jī)器人編隊(duì)和隊(duì)形變換，自組織編隊(duì)形成的目標(biāo)隊(duì)形重心與機(jī)器人初始化時(shí)的重心距離不遠(yuǎn)，從而體現(xiàn)了PSO算法優(yōu)化的魅力。

4 結(jié)束語

提出了一種基于PSO的分布式編隊(duì)控制算法。仿真結(jié)果表明，與集中式控制相比，分布式控制方式可以獨(dú)立考慮每個(gè)機(jī)器人的特性，具有較強(qiáng)的靈活性、魯棒性等優(yōu)點(diǎn)。而采用PSO算法可以很快地達(dá)到適應(yīng)值，收斂速度更快，迭代次數(shù)更少，有效提高了編隊(duì)效率。但是文中研究的多機(jī)器人編隊(duì)是靜態(tài)的，如何讓機(jī)器人動(dòng)態(tài)編隊(duì)完成跟蹤任務(wù)還需要進(jìn)一步研究；并且文中只考慮二維平面的機(jī)器人編隊(duì)，編隊(duì)過程中沒有考慮障礙物，因此以后可以加入障礙物并從二維擴(kuò)展到三維[15]進(jìn)行研究。