張奇松，胡玉蘭

（沈陽(yáng)理工大學(xué) 信息科學(xué)與工程學(xué)院，遼寧 沈陽(yáng) 110168）

在機(jī)器人協(xié)調(diào)協(xié)作中，多機(jī)器人圍捕任務(wù)可以看作多機(jī)器人隊(duì)形控制的另一種形式。在傳統(tǒng)隊(duì)形控制中，有跟隨領(lǐng)航者法（Leader-follower）、基于行為法（Behavior-based）、虛擬結(jié)構(gòu)法（Virtual structure）。其中跟隨領(lǐng)航者法即基于Leaderollower法和虛擬結(jié)構(gòu)法的隊(duì)形控制方法，側(cè)重于傳統(tǒng)的控制理論，可以利用動(dòng)態(tài)系統(tǒng)理論對(duì)編隊(duì)系統(tǒng)的穩(wěn)定性進(jìn)行分析，但是缺乏基于行為控制方法中的隊(duì)形反饋信息。為了增加基于Leader-follower方法在進(jìn)行編隊(duì)控制時(shí)的實(shí)用性，這里提出一種基于有限狀態(tài)機(jī)的多機(jī)器人圍捕方案，引入隊(duì)形條件反饋信息，建立基于條件反饋機(jī)制和Leader-follower相結(jié)合的多機(jī)器人隊(duì)形控制模型實(shí)現(xiàn)了多機(jī)器人系統(tǒng)的隊(duì)形控制，并給出有限狀態(tài)機(jī)原理，定義圍捕各階段的不同狀態(tài)。

1 改進(jìn)式Leader-follower隊(duì)形控制

1.1 Leader-follower基本思想

基于Leader-follower法是由Leader給定Follower的運(yùn)行參數(shù)，具體的運(yùn)動(dòng)控制由Follower自行計(jì)算，該方法具有很好的系統(tǒng)可擴(kuò)展性、運(yùn)動(dòng)策略靈活性和計(jì)算并行性。但是Leader與 Follower相對(duì)獨(dú)立，Leader不容易得到 Follower的跟蹤誤差，如果Leader運(yùn)動(dòng)過(guò)快可能導(dǎo)致落后的機(jī)器人離隊(duì)，而且整個(gè)系統(tǒng)對(duì)Leader機(jī)器人依賴性較大，如果Leader機(jī)器人發(fā)生故障會(huì)導(dǎo)致整個(gè)系統(tǒng)癱瘓。針對(duì)上述缺點(diǎn)，一些文獻(xiàn)提出相應(yīng)的解決辦法。文獻(xiàn)[1]利用反饋線性化得到非完整約束機(jī)器人的軌跡跟蹤控制法則；文獻(xiàn)[2]平滑時(shí)變反饋控制法則構(gòu)成一個(gè)開(kāi)環(huán)鏈?zhǔn)降年?duì)形；文獻(xiàn)[3]引入合作方法，即Follower的運(yùn)動(dòng)不僅由自身決定，還受到其他機(jī)器人的影響。

在利用傳統(tǒng)的Leader-follower方法控制隊(duì)形過(guò)程中，為了在避障和保持隊(duì)形之間進(jìn)行協(xié)調(diào)，常常引入“虛擬機(jī)器人”概念，并將機(jī)器人的運(yùn)行狀態(tài)分為正常運(yùn)行模式和避障模式。當(dāng)某個(gè)機(jī)器人處于避障模式時(shí)，其他機(jī)器人參考替代它的“虛擬機(jī)器人”以保證正常的運(yùn)行，但如果Leader根據(jù)隊(duì)形反饋進(jìn)行的調(diào)節(jié)過(guò)于頻繁，會(huì)導(dǎo)致整體運(yùn)動(dòng)不順暢，甚至可能當(dāng)多個(gè)Follower同時(shí)處于避障狀態(tài)，由于Leader隊(duì)形反饋停止運(yùn)動(dòng)，導(dǎo)致系統(tǒng)處于死鎖狀態(tài)。對(duì)此，這里引入了隊(duì)形的條件反饋機(jī)制，如圖1所示。

圖1 隊(duì)形條件反饋的Leader-follower控制結(jié)構(gòu)Fig.1 Leader-follower control structure of formation conditions feedback

1.2 條件反饋隊(duì)形控制的基本原理

條件隊(duì)形反饋的基本原理為：

1）初始時(shí)Leader接受任務(wù)T，然后根據(jù)環(huán)境信息E進(jìn)行運(yùn)動(dòng)規(guī)劃。

2）Leader將自身的運(yùn)行狀態(tài) S=（XL，YL，VL，WL）和任務(wù)信息 T 通過(guò)公共變量區(qū)通知 Robot i（i=1，2，...，n）。

3）每個(gè)follower根據(jù)任務(wù)T計(jì)算自己在隊(duì)形中的期望位置 Pdi=（xdi，ydi）（i=1，2，...，n），結(jié)合環(huán)境信息 E，根據(jù)控制策略產(chǎn)生控制量，即（υi，wi）。 隊(duì)形信息 P=（P1，P2，...，Pn）被反饋給Leader，其中P1為Follower1當(dāng)前位置。在正常運(yùn)動(dòng)時(shí)Leader不將隊(duì)形反饋信息作為自己運(yùn)動(dòng)的依據(jù)，依靠Follower自行調(diào)整速度來(lái)提高隊(duì)形保持率。

4）如果當(dāng)個(gè)別機(jī)器人處于通信范圍邊緣時(shí)，Leader降低運(yùn)動(dòng)速度，使得落后機(jī)器人能夠趕上，防止Follower掉隊(duì)。在特殊情況下（等待較長(zhǎng)時(shí)間而最遠(yuǎn)的Follower與Leader之間的距離沒(méi)有縮短），Leader會(huì)放棄故障機(jī)器人，重新進(jìn)行隊(duì)形調(diào)整。

2 多機(jī)器人圍捕策略設(shè)計(jì)

2.1 開(kāi)闊隊(duì)形偵查策略

在機(jī)器人行進(jìn)過(guò)程中，為了保證系統(tǒng)具有強(qiáng)的防御性能，獲得詳細(xì)的環(huán)境信息和保持近距離的通信，選擇具有向心性的隊(duì)形（像圓形、菱形）。線形隊(duì)形（縱隊(duì)和橫隊(duì)）中機(jī)器人位置非常開(kāi)闊，偵查范圍廣、重復(fù)偵查區(qū)域少，所以這里使用線形隊(duì)形作為偵查隊(duì)形。

2.2 半圓形隊(duì)形預(yù)包圍策略

在開(kāi)闊地帶執(zhí)行圍捕任務(wù)時(shí)，圓形包圍隊(duì)形難以直接形成，因?yàn)楫?dāng)invader發(fā)現(xiàn)圍捕機(jī)器人后會(huì)改變運(yùn)動(dòng)軌跡，多機(jī)器人系統(tǒng)預(yù)先分配的包圍點(diǎn)便失去了作用。為此，將最后的圍捕分為兩個(gè)階段。

第1階段為扇形展開(kāi)半圓形預(yù)包圍圈，如圖2所示。在圍捕任務(wù)中，機(jī)器人一般具有略強(qiáng)于Invader的偵查能力，所以機(jī)器人會(huì)先于Invader發(fā)現(xiàn)對(duì)方。最早發(fā)現(xiàn)Invader的機(jī)器人自動(dòng)成為圍捕階段的Leader，然后其根據(jù)擬合出的Invader軌跡確定出預(yù)包圍點(diǎn)，并根據(jù)機(jī)器人之間的位置關(guān)系分配半圓形預(yù)包圍隊(duì)形的隊(duì)形點(diǎn)。因?yàn)榇藭r(shí)重要的不再是機(jī)器人到達(dá)隊(duì)形點(diǎn)的先后時(shí)間，而是能夠有效地堵截Invader的逃脫方向。當(dāng)Invader發(fā)現(xiàn)機(jī)器人后會(huì)首選轉(zhuǎn)向逃脫，此時(shí)機(jī)器人若能夠?qū)⑵湔胺?80°的路徑全部封堵，則Invader只能反向逃脫，這樣可以為接下來(lái)圓形包圍隊(duì)形的形成爭(zhēng)取有利條件。

圖2 扇形展開(kāi)半圓形預(yù)包圍隊(duì)形Fig.2 Sector semicircle beforehand surrounded formation

第2階段為收縮隊(duì)形并形成圓形包圍圈。為了防止Invader過(guò)早發(fā)現(xiàn)機(jī)器人，本文將預(yù)包圍隊(duì)形的半徑設(shè)的比較大，如果直接形成運(yùn)行圓形包圍隊(duì)形，早會(huì)造成相鄰機(jī)器人之間的空隙過(guò)大，Invader可以從空隙中逃脫，導(dǎo)致圍捕的失敗。所以，本文設(shè)計(jì)機(jī)器人預(yù)包圍隊(duì)形首先向Invader方向收縮，然后形成圓形包圍隊(duì)形。

2.3 圍捕任務(wù)退出策略

在動(dòng)態(tài)圍捕任務(wù)中，因?yàn)椴荒艽_保機(jī)器人每一次都能偵查到動(dòng)態(tài)目標(biāo)，所以設(shè)定兩種退出方式：1）按照偵查隊(duì)形前進(jìn)N步?jīng)]有發(fā)現(xiàn)目標(biāo)，則系統(tǒng)退出圍捕任務(wù)；2）發(fā)現(xiàn)目標(biāo)后，在圍捕過(guò)程中，以要求隊(duì)形包圍Invader，并且Invader靜止，則圍捕任務(wù)完成。

3 基于有限狀態(tài)機(jī)的多機(jī)器人圍捕

3.1 圍捕有限狀態(tài)機(jī)設(shè)計(jì)

設(shè)定有限狀態(tài)機(jī)為一個(gè)三元組M=（E，Qst，φ），各變量具體含義如下：Qst={q1，q2，...，qm}為離散狀態(tài)的有限集合；E={e1，e2，...，en}為L(zhǎng)eader指令的有限集合；φ為從一個(gè)狀態(tài)到另一個(gè)狀態(tài)的映射，可表達(dá)為 φ（qs，E）=qk∈Qst。

在系統(tǒng)模型中 Qst包含6種狀態(tài)：初始化（q1）、保持隊(duì)形運(yùn)行（q2）、偵查（q3）、半圓隊(duì)形預(yù)圍捕（q4）、圓形隊(duì)形捕捉（q5）和退出（q6），其中在 q1、q3、q4狀態(tài)的初始時(shí)刻要完成新隊(duì)形點(diǎn)的分配及Leader更換。

初始時(shí)刻，機(jī)器人群體處于無(wú)序排列的狀態(tài)，Leader機(jī)器人發(fā)出啟動(dòng)指令（e1），機(jī)器人首先組成指定隊(duì)形，然后向指定區(qū)域運(yùn)動(dòng)。在保持隊(duì)形運(yùn)動(dòng)過(guò)程，機(jī)器人主要任務(wù)是快速運(yùn)動(dòng)，此外要低頻率的進(jìn)行Invader偵查。如果發(fā)現(xiàn)環(huán)境復(fù)雜，機(jī)器人會(huì)自行進(jìn)行蛇形隊(duì)形的轉(zhuǎn)換。如果在這個(gè)階段發(fā)現(xiàn)Invader（e2），機(jī)器人則直接進(jìn)入捕捉狀態(tài)。機(jī)器人到達(dá)指定區(qū)域（e3）后，開(kāi)始轉(zhuǎn)換為比較開(kāi)闊的偵查隊(duì)形。由于環(huán)境的限制，本文將捕捉結(jié)果分為 2種：1）發(fā)現(xiàn) Invader（e2），則機(jī)器人進(jìn)行圍捕，捕捉到 Invader后退出（e8）；2）到達(dá)偵查限制（e6）而沒(méi)有發(fā)現(xiàn)Invader，則系統(tǒng)直接退出（e8）。在圍捕的過(guò)程中可能發(fā)生Invader逃脫（e5）的情況，這是多機(jī)器人系統(tǒng)重新進(jìn)入偵查狀態(tài)。在機(jī)器人進(jìn)行捕捉過(guò)程中，最先發(fā)現(xiàn)Invader的機(jī)器人自動(dòng)成為新的Leader，整個(gè)捕捉過(guò)程由新Leader控制。為了加快捕捉任務(wù)完成的速度，系統(tǒng)首先組成半圓形隊(duì)形（e4）進(jìn)行預(yù)捕捉。Leader根據(jù)Invader的運(yùn)動(dòng)方向和運(yùn)動(dòng)速度預(yù)測(cè)出捕捉點(diǎn)，然后以捕捉點(diǎn)為圓心，在Invader的運(yùn)動(dòng)方向上形成較大半徑的半圓形包圍隊(duì)形，從而阻擋住Invader的逃脫，而且能夠較晚被Invader發(fā)現(xiàn)。形成半圓包圍隊(duì)形后，Leader發(fā)出圓形包圍指令（e7）。首先收縮隊(duì)形，各機(jī)器人向Invader的位置移動(dòng)，Invader會(huì)選擇逃脫方向，這時(shí)各機(jī)器人在Invader周圍形成圓形包圍圈，從而徹底阻擋住Invader的逃脫路線，圍捕任務(wù)完成。

根據(jù)上述描述，機(jī)器人狀態(tài)轉(zhuǎn)換如下：φ（q1，e1）=q2，φ（q2，e2）=q4，φ（q2，e3）=q3，φ（q3，e2）=q4，φ（q4，e7）=q5，φ（q4，e6）=q6，φ（q4，e5）=q3，φ（q5，e5）=q3。

3.2 程序流程

圖3為機(jī)器人動(dòng)態(tài)，圍捕程序流程圖。

從該流程圖可以看出，機(jī)器人的運(yùn)行場(chǎng)景定為不同的狀態(tài)，在每種狀態(tài)中，機(jī)器人的功能模塊的組合順序和獲得的參數(shù)存在差別，通過(guò)有限狀態(tài)機(jī)來(lái)決定行為的映射規(guī)則。執(zhí)行層由執(zhí)行機(jī)構(gòu)和探測(cè)模塊組成，執(zhí)行機(jī)構(gòu)的主要功能是將行為層的控制參數(shù)轉(zhuǎn)化為實(shí)際的物理控制量并通過(guò)硬件實(shí)施，探測(cè)模塊的主要功能是利用各種傳感器探測(cè)周圍環(huán)境并將環(huán)境信息轉(zhuǎn)化為信息處理模塊可使用的信息量。

4 仿真實(shí)例

圖3 動(dòng)態(tài)圍捕流程圖Fig.3 Flow chart of dynamic seizing

仿真系統(tǒng)采用Visual C++語(yǔ)言開(kāi)發(fā)，在仿真中機(jī)器人的半徑為2 cm，步長(zhǎng)為0.5 cm，間隔時(shí)間為100 ms，本文規(guī)定隊(duì)形中Follower的位置誤差為2 cm。圖4給出了圍捕實(shí)驗(yàn)中7種仿真圖。

在圍捕實(shí)驗(yàn)中，Invader采用跟圍捕機(jī)器人相同的功能設(shè)置，只是在傳感器感知范圍上略微存在差別。當(dāng)沒(méi)有探測(cè)到任何障礙物時(shí)，Invader為直線運(yùn)動(dòng)；否則，它朝向安全的方向運(yùn)動(dòng)。

圖4 7種仿真圖Fig.4 Seven kinds of simulation diagrams

從以上仿真圖可以看出本文根據(jù)有限狀態(tài)機(jī)原理將機(jī)器人分為 6種狀態(tài)，其中圖 4（a）～圖 4（f）分別對(duì)應(yīng)初始化，保持隊(duì)形初始化、保持隊(duì)形運(yùn)行、偵查、半圓隊(duì)形預(yù)圍捕、圓形隊(duì)形捕捉和退出。而圖4（g）為追捕機(jī)器人將入侵機(jī)器人圍在其中心，根據(jù)避碰原則（即入侵機(jī)器人將其他機(jī)器人視為動(dòng)態(tài)障礙物）入侵機(jī)器人沒(méi)有躲避的空間，不能再移動(dòng)，根據(jù)退出條件，圍捕成功，系統(tǒng)結(jié)束運(yùn)作。

5 結(jié) 論

本文主要研究了多機(jī)器人的協(xié)作圍捕問(wèn)題。為了實(shí)現(xiàn)圍捕任務(wù)，提出了一種基于條件反饋機(jī)制的Leader－followers隊(duì)形控制方法，同時(shí)采用有限狀態(tài)機(jī)在不同狀態(tài)之間進(jìn)行切換。每種狀態(tài)的具體實(shí)現(xiàn)過(guò)程，都是由隊(duì)形形成和隊(duì)形保持組成，通過(guò)這種狀態(tài)轉(zhuǎn)換帶動(dòng)隊(duì)形轉(zhuǎn)換的形式，將協(xié)調(diào)編隊(duì)與圍捕任務(wù)有效地結(jié)合在一起。最后通過(guò)仿真驗(yàn)證了算法的有效性。

[1]Desai J P， Ostrowski J， Kumar V.Controlling formations of multiple mobile robots[C]//IEEE International conference on Robotics and Automation Belgium，1998:2864－2869.

[2]Yamaguchi H，Burdick J W.Asymptotic stabilization of multiple nonholonomic mobile robots forming group formations［C］//IEEE Int Conf on Robotics and Automation，Belgium，1998：3573－3580.

[3]Pereira G A S，Das A K，Kumar V，et a1.Formation control with configuration space constraints[C]//Proc of the IEEE/RJS Int Conf on Intelligent Robots attd Systems，Las Vegas，2003：2755－2760.

[4]程磊.多移動(dòng)機(jī)器人協(xié)調(diào)控制系統(tǒng)的研究與實(shí)現(xiàn) [D].武漢：華中理工大學(xué)，2005：37－42.

[5]劉海波，顧國(guó)昌，張國(guó)印.智能機(jī)器人體系結(jié)構(gòu)分類研究[J].哈爾濱工程大學(xué)學(xué)報(bào)， 2003， 24 （6）:57－60.LIU Hai-bo，GU Guo-chang，ZHANG Guo-yin.Intelligent robot structure system classification research[J].Journal of Habin Engineering University，2003，24（6）:57－60.

[6]譚民，王碩，曹志強(qiáng).多機(jī)器人系統(tǒng)[M].北京：清華大學(xué)出版社，2005.