基于狀態(tài)切換的分布式多機(jī)器人編隊(duì)控制

衛(wèi)恒， 呂強(qiáng)， 劉揚(yáng)， 林輝燦， 梁冰

(1.陸軍裝甲兵學(xué)院 兵器與控制系, 北京 100072; 2.空軍航空大學(xué) 信息系, 吉林 長春 130022;3.江西理工大學(xué) 信息工程學(xué)院, 江西 贛州 341000)

0 引言

近年來，隨著計(jì)算機(jī)應(yīng)用技術(shù)和數(shù)字信號(hào)處理技術(shù)的不斷發(fā)展，使得微處理芯片的功能越來越強(qiáng)、處理速度越來越快，同時(shí)自動(dòng)控制技術(shù)、網(wǎng)絡(luò)通信技術(shù)的日趨發(fā)展使得微型自主移動(dòng)機(jī)器人受到廣泛關(guān)注[1]。多移動(dòng)機(jī)器人系統(tǒng)比單個(gè)移動(dòng)機(jī)器人具有更好的穩(wěn)定性、更強(qiáng)的容錯(cuò)性和更高的工作效率，在偵查、巡邏、救援和環(huán)境勘測等領(lǐng)域都有很好的應(yīng)用前景和研究價(jià)值。多移動(dòng)機(jī)器人編隊(duì)控制是多移動(dòng)機(jī)器人系統(tǒng)的基礎(chǔ)，已經(jīng)成為目前機(jī)器人領(lǐng)域研究的熱點(diǎn)[2]。

多機(jī)器人編隊(duì)算法設(shè)計(jì)過程中，需要考慮機(jī)器人模型、外部環(huán)境干擾、算法控制精度以及不同隊(duì)形可控性等諸多問題[3]。傳統(tǒng)的多機(jī)器人編隊(duì)控制算法主要有Leader-Follower算法[4]、基于行為的算法[5]、虛擬結(jié)構(gòu)法[6]、人工勢(shì)場法等?；贚eader-Follower算法的基本思想是在多機(jī)器人系統(tǒng)中選擇一個(gè)機(jī)器人作為Leader，其他機(jī)器人為Follower，通過調(diào)節(jié)Leader與Follower之間的角度和距離來控制編隊(duì)隊(duì)形。文獻(xiàn)[7]提出了距離- 角度與距離- 距離兩種編隊(duì)控制算法，距離- 角度算法通過控制Leader與Follower之間的距離和角度進(jìn)行編隊(duì)控制，距離- 距離算法則通過Follower與相鄰機(jī)器人之間的距離來保持隊(duì)形。文獻(xiàn)[8]為了防止機(jī)器人之間出現(xiàn)碰撞，提出了基于虛擬引力的改進(jìn)型距離- 角度算法與距離- 距離算法。Leader-Follower算法具有靈活的運(yùn)動(dòng)策略、良好的擴(kuò)展性，但是算法中Leader與Follower之間不能形成穩(wěn)定、有效的反饋，因此Follower的跟蹤誤差會(huì)隨著環(huán)境的干擾不斷擴(kuò)大，且當(dāng)Leader出現(xiàn)故障時(shí)整個(gè)多機(jī)器人系統(tǒng)也會(huì)無法工作?；谛袨榈乃惴ㄆ浠舅枷胧菫槎鄼C(jī)器人系統(tǒng)中的每個(gè)機(jī)器人設(shè)計(jì)基本的行為準(zhǔn)則，例如避障、避碰、向目標(biāo)運(yùn)動(dòng)、保持隊(duì)形等行為。該算法可以有效地降低整個(gè)多機(jī)器人系統(tǒng)編隊(duì)控制算法的復(fù)雜度，但是其對(duì)傳感器探測能力和機(jī)器人之間的通信能力要求較高，且在運(yùn)行過程中無法對(duì)機(jī)器人的行為進(jìn)行準(zhǔn)確描述，難以保證系統(tǒng)的魯棒性。虛擬結(jié)構(gòu)法最早由Lewis等[9]提出，其基本思想是將整個(gè)多機(jī)器人系統(tǒng)看成一個(gè)剛性整體，將各機(jī)器人看作是剛體上的某個(gè)點(diǎn)。該算法的優(yōu)點(diǎn)是便于設(shè)計(jì)多機(jī)器人系統(tǒng)的編隊(duì)行為，隊(duì)形保持率高，但由于受到剛性結(jié)構(gòu)的約束，其在避障、隊(duì)形變換等方面缺乏靈活性。人工勢(shì)場法的基本思想是在機(jī)器人周圍假設(shè)具有類似電磁場的引力和斥力，通過設(shè)定適當(dāng)?shù)膭?shì)能函數(shù)對(duì)多機(jī)器人進(jìn)行編隊(duì)控制。該算法結(jié)構(gòu)簡單、易于設(shè)計(jì)，能夠有效地完成避碰和避障，但在隊(duì)形保持時(shí)容易受到干擾，不易進(jìn)行精確編隊(duì)控制，若進(jìn)行隊(duì)形變換則需要重新設(shè)定勢(shì)能函數(shù)，缺乏靈活性[7]。

近年來，切換系統(tǒng)逐漸受到學(xué)者們的關(guān)注，該系統(tǒng)是指一類具有模態(tài)切換且在指定范圍內(nèi)能夠按照隨機(jī)性或確定性規(guī)則改變系統(tǒng)動(dòng)態(tài)模式的一類系統(tǒng)[11]。由于許多工程系統(tǒng)在實(shí)際運(yùn)行中，需要根據(jù)環(huán)境變化而表現(xiàn)出不同的模態(tài)，切換系統(tǒng)在工程實(shí)踐中具有十分重要的作用[112-12]。同時(shí)，隨著多機(jī)器人系統(tǒng)機(jī)器人個(gè)數(shù)的增加以及單個(gè)機(jī)器人數(shù)據(jù)處理能力的不斷提升，多機(jī)器人系統(tǒng)的協(xié)同控制算法受到廣泛關(guān)注，學(xué)者們研究了多種分布式控制算法來解決從狀態(tài)估計(jì)、環(huán)境監(jiān)測到集體材料處理等各種各樣的任務(wù)。分布式多機(jī)器人系統(tǒng)能充分利用機(jī)器人的數(shù)據(jù)處理資源，分擔(dān)中央處理機(jī)的壓力，在靈活性、容錯(cuò)性方面具有很大優(yōu)勢(shì)[13]。

本文基于上述傳統(tǒng)編隊(duì)控制算法的優(yōu)缺點(diǎn)及發(fā)展趨勢(shì)，考慮在不改變算法基本框架的前提下將各個(gè)算法的優(yōu)點(diǎn)有效結(jié)合，從而增加編隊(duì)控制算法的靈活性和適應(yīng)性，將切換系統(tǒng)的思想引入多機(jī)器人編隊(duì)控制中，提出了基于狀態(tài)切換的分布式編隊(duì)控制策略。同時(shí)，基于超寬帶(UWB)測距構(gòu)建了多機(jī)器人系統(tǒng)之間的隊(duì)形反饋機(jī)制，在加強(qiáng)機(jī)器人之間聯(lián)系的同時(shí)降低了編隊(duì)控制算法的復(fù)雜度和對(duì)機(jī)器人數(shù)據(jù)處理能力的要求。最后依托試驗(yàn)室搭建的多機(jī)器人平臺(tái)進(jìn)行了試驗(yàn)，驗(yàn)證了編隊(duì)控制策略的有效性和穩(wěn)定性。

1 預(yù)先知識(shí)準(zhǔn)備

1.1 3輪全向機(jī)器人建模

本文采用的機(jī)器人為3輪全向機(jī)器人，其坐標(biāo)系如圖1所示。圖1中，Oxy為全局坐標(biāo)系；OBxByB為機(jī)體坐標(biāo)系，OB為機(jī)器人質(zhì)心，xB為機(jī)器人正面朝向，yB朝向垂直于xB，θ為機(jī)體坐標(biāo)系與全局坐標(biāo)系的夾角；β為機(jī)器人xB與機(jī)器人質(zhì)心OB與O點(diǎn)連線的夾角；φ為相鄰兩個(gè)輪子軸的夾角,φ=120°；L為質(zhì)心OB到輪子的距離；v1、v2、v3為3輪全向機(jī)器人輪子的線速度；f1、f2、f3為3個(gè)電機(jī)提供的驅(qū)動(dòng)力。

假設(shè)vx、vy為機(jī)器人在全局坐標(biāo)系下x軸和y軸的速度分量，ω為機(jī)器人的自轉(zhuǎn)角速度，以機(jī)器人質(zhì)心來代替機(jī)器人在全局坐標(biāo)系下的位置，則機(jī)器人坐標(biāo)為(x,y,θ)，二者之間關(guān)系可以表示為

(1)

全向機(jī)器人3個(gè)輪子的線速度可以表示為

(2)

聯(lián)立(1)式和(2)式，得3輪全向機(jī)器人的運(yùn)動(dòng)學(xué)方程為

(3)

假設(shè)機(jī)器人質(zhì)量為m，機(jī)器人繞自身中心旋轉(zhuǎn)的轉(zhuǎn)動(dòng)慣量為I，全向輪半徑為R，每個(gè)電機(jī)電壓為U1、U2、U3，電機(jī)的電樞電阻為Ra，電機(jī)轉(zhuǎn)矩常數(shù)為Kt. 令Fx為機(jī)器人在x軸方向上所受合力，F(xiàn)y為機(jī)器人在y軸方向上所受合力，M為機(jī)器人所受力矩，由牛頓定律可得

(4)

由(4)式可得

(5)

(6)

聯(lián)立(5)式、(6)式，得3輪全向機(jī)器人的動(dòng)力學(xué)方程為

(7)

1.2 圖論基本概念

機(jī)器人之間的通信拓?fù)浣Y(jié)構(gòu)通過有向圖G={V,E}完成，其中，V為節(jié)點(diǎn)集合，每一個(gè)機(jī)器人代表1個(gè)節(jié)點(diǎn)；E為邊集合，代表機(jī)器人之間的連接狀態(tài)。有向圖通過拉普拉斯矩陣表示，即L=D-A，其中D為度矩陣，表示每一個(gè)節(jié)點(diǎn)都有幾個(gè)節(jié)點(diǎn)與其相鄰；A為鄰接矩陣，表示節(jié)點(diǎn)之間的鄰接關(guān)系。設(shè)一共有n個(gè)機(jī)器人，i、j分別表示第i和第j個(gè)機(jī)器人，則拉普拉斯矩陣定位如下：

(8)

考慮到機(jī)器人之間通信拓?fù)浣Y(jié)構(gòu)會(huì)因環(huán)境不同而變化，本文設(shè)計(jì)了權(quán)重矩陣B=[b10,b20,…,bij]表示機(jī)器人之間的通信連接關(guān)系，定義矩陣H=L+B表示多機(jī)器人之間的通信拓?fù)潢P(guān)系。本文算法中，設(shè)定機(jī)器人之間的通信是雙向的，因此圖G是無向圖，B是對(duì)稱正定矩陣。由拉普拉斯矩陣性質(zhì)可知，L也是對(duì)稱正定矩陣，因此矩陣H也是對(duì)稱正定矩陣。

2 基于狀態(tài)切換的分布式多機(jī)器人編隊(duì)控制策略

多機(jī)器人編隊(duì)控制中，學(xué)者們已經(jīng)提出了大量基于不同環(huán)境下對(duì)各種算法的改進(jìn)方法，但是并沒有任何一種算法能較好地適應(yīng)各種環(huán)境。本文將切換系統(tǒng)的思想引入多機(jī)器人編隊(duì)控制算法中，針對(duì)不同的任務(wù)需要和機(jī)器人狀態(tài)，采用相應(yīng)的多機(jī)器人編隊(duì)控制算法，在不影響穩(wěn)定性的前提下，增加了系統(tǒng)的靈活性和適應(yīng)性。首先分析了多機(jī)器人編隊(duì)控制中機(jī)器人的狀態(tài)和可能遇到的環(huán)境，設(shè)計(jì)了基于復(fù)合信息的狀態(tài)切換觸發(fā)策略；其次設(shè)計(jì)了基于UWB測距的隊(duì)形反饋機(jī)制，通過其保持系統(tǒng)隊(duì)形，并為觸發(fā)機(jī)制實(shí)時(shí)提供系統(tǒng)隊(duì)形狀態(tài)信息；最后提出了分布式狀態(tài)估計(jì)算法，充分利用機(jī)器人自身的數(shù)據(jù)處理能力，降低中央處理器的負(fù)擔(dān)，并為觸發(fā)機(jī)制提供機(jī)器人的運(yùn)行狀態(tài)?？傮w設(shè)計(jì)框圖如圖2所示。

2.1 基于復(fù)合信息的狀態(tài)切換觸發(fā)策略

首先，考慮了多機(jī)器人編隊(duì)控制中針對(duì)任務(wù)需要的不同，機(jī)器人采取不同隊(duì)形和運(yùn)行狀態(tài)時(shí)的情況。依據(jù)第1節(jié)所述各算法的優(yōu)缺點(diǎn)匹配各狀態(tài)所需算法，編隊(duì)行為主要分為4種：1)組建編隊(duì)，是指機(jī)器人從任意位姿按照任務(wù)需要組建成期望的隊(duì)形，為了提高組建編隊(duì)的效率，一般采用基于行為的算法來完成編隊(duì)組建。2)編隊(duì)行進(jìn)，是指多機(jī)器人系統(tǒng)依據(jù)任務(wù)目標(biāo)按照組建的編隊(duì)行進(jìn)，由于編隊(duì)行進(jìn)過程中機(jī)器人之間的拓?fù)浣Y(jié)構(gòu)已經(jīng)固定，為了提高編隊(duì)行進(jìn)的精度，一般采用虛擬結(jié)構(gòu)法或Leader-Follower算法進(jìn)行編隊(duì)控制。3)隊(duì)形變換，是指為了適應(yīng)環(huán)境或根據(jù)任務(wù)需要對(duì)多機(jī)器人編隊(duì)隊(duì)形進(jìn)行動(dòng)態(tài)變換，由于虛擬結(jié)構(gòu)法相對(duì)于其他算法在編隊(duì)變換時(shí)只需要?jiǎng)討B(tài)設(shè)計(jì)虛擬質(zhì)點(diǎn)矩陣，因此采用該算法進(jìn)行編隊(duì)控制。4)編隊(duì)避障，是指在編隊(duì)行進(jìn)過程中，在行進(jìn)路線上出現(xiàn)障礙物時(shí)多機(jī)器人系統(tǒng)采取的避讓行為。本文將避障情況分為2類：第1類是強(qiáng)制保持隊(duì)形避障，由于某些任務(wù)需要，多機(jī)器人系統(tǒng)在避障時(shí)仍然需要保持隊(duì)形，此時(shí)采用虛擬結(jié)構(gòu)法或Leader-Follower保持隊(duì)形，采用人工勢(shì)場法進(jìn)行避障；第2類是采用最有效率的方式完成避障，此時(shí)采用基于行為的方法進(jìn)行避障，避障后再重新完成編隊(duì)組建。

其次，考慮了多機(jī)器人編隊(duì)控制中，機(jī)器人因?yàn)椴煌年?duì)形，采用不同通信拓?fù)浣Y(jié)構(gòu)時(shí)的情況。拓?fù)浣Y(jié)構(gòu)主要分為2類：第1類是鏈狀拓?fù)浣Y(jié)構(gòu)，如圖3所示，此時(shí)采用Leader-Follower算法來進(jìn)行編隊(duì)控制；第2類是環(huán)狀拓?fù)浣Y(jié)構(gòu)，如圖4所示，此時(shí)采用虛擬結(jié)構(gòu)法來進(jìn)行編隊(duì)控制。

最后，考慮了環(huán)境對(duì)編隊(duì)控制的影響，所處環(huán)境主要分為2類：第1類是在編隊(duì)行進(jìn)中出現(xiàn)障礙物，一般采用基于行為的算法進(jìn)行編隊(duì)避障；第2類主要是針對(duì)特定環(huán)境下的編隊(duì)隊(duì)形，例如采用虛擬結(jié)構(gòu)法行進(jìn)遇到狹長的通道，此時(shí)需要進(jìn)行兩次狀態(tài)切換：第1次是隊(duì)形變換，組建適用于該環(huán)境的狹長隊(duì)形；第2次是依據(jù)隊(duì)形重新組建通信拓?fù)浣Y(jié)構(gòu)，將編隊(duì)控制算法由虛擬結(jié)構(gòu)法切換為Leader-Follower算法。

控制策略設(shè)計(jì)中，采用基于任務(wù)驅(qū)動(dòng)、自上而下的設(shè)計(jì)方案，首先確立切換系統(tǒng)模型，設(shè)計(jì)基于復(fù)合信息的狀態(tài)切換機(jī)制，然后以模塊化方式設(shè)計(jì)機(jī)器人控制部分。控制部分分為內(nèi)環(huán)控制和外環(huán)控制。外環(huán)控制為3種編隊(duì)控制算法模塊：Leader-Follower算法模塊、虛擬結(jié)構(gòu)算法模塊、基于行為的算法模塊。內(nèi)環(huán)控制主要為移動(dòng)機(jī)器人底層控制驅(qū)動(dòng)算法。同時(shí)，因?yàn)樵诙鄼C(jī)器人避碰方面，人工勢(shì)場法具有結(jié)構(gòu)簡單、易于與其他編隊(duì)算法融合的特點(diǎn)，所以將人工勢(shì)場算法模塊也寫入機(jī)器人底層控制驅(qū)動(dòng)算法中[7]。具體控制策略如下：

設(shè)切換系統(tǒng)為

(9)

式中：x(t)為狀態(tài)變量；算法中選用常值切換規(guī)則，σ(t){1,2,3}；p1、p2、p3分別代表3種算法模塊下的子系統(tǒng)。因?yàn)樵谇袚Q系統(tǒng)中采用模塊化設(shè)計(jì)各個(gè)子系統(tǒng)，且已經(jīng)將人工勢(shì)場法嵌入底層驅(qū)動(dòng)程序，用于編隊(duì)中各個(gè)機(jī)器人之間的實(shí)時(shí)避碰，不需要考慮狀態(tài)切換時(shí)可能對(duì)系統(tǒng)產(chǎn)生的不穩(wěn)定情況，所以采用階躍式切換策略，避免對(duì)各子系統(tǒng)之間連接的復(fù)雜設(shè)計(jì)，即采用常值切換規(guī)則。

基于復(fù)合信息的狀態(tài)切換機(jī)制，以多機(jī)器人系統(tǒng)編隊(duì)實(shí)時(shí)矩陣S′與機(jī)器人狀態(tài)為判斷依據(jù)，其中編隊(duì)實(shí)時(shí)矩陣由每個(gè)機(jī)器人與其他機(jī)器人之間的距離以及基于自身機(jī)體坐標(biāo)系與其他機(jī)器人的夾角構(gòu)成。由于構(gòu)建的切換系統(tǒng)為事件驅(qū)動(dòng)，機(jī)器人編隊(duì)矩陣S由任務(wù)發(fā)布或在運(yùn)行中由環(huán)境突變觸發(fā)改變。

設(shè)有一個(gè)由N個(gè)機(jī)器人組成的多機(jī)器人系統(tǒng)，通信拓?fù)浣Y(jié)構(gòu)用H=L+B表示，qi=(xi,yi,θi,vi)T表示第i個(gè)機(jī)器人在t時(shí)刻的狀態(tài)，r表示機(jī)器人圓形鄰域的半徑，第i個(gè)機(jī)器人的鄰域Ni(t)={j|‖xj(t)-xi(t)‖≤r}，nr表示在半徑為r的鄰域內(nèi)，共有nr個(gè)機(jī)器人與機(jī)器人i相鄰，則

(10)

依據(jù)1.2節(jié)分析，系統(tǒng)間連接關(guān)系由L確定，B為權(quán)重矩陣，因此機(jī)器人i的鄰接關(guān)系為

F=I+D-1(I+L)，

(11)

由此可得

(12)

從而得到差值矩陣為

(13)

設(shè)定指標(biāo)權(quán)重矩陣k=(k1,k2,k3,k4)T，依據(jù)環(huán)境及小車狀態(tài)分別對(duì)di中的元素進(jìn)行加權(quán)，從而對(duì)狀態(tài)切換機(jī)制進(jìn)行調(diào)節(jié)。設(shè)定閾值矩陣為d0，當(dāng)kdi>d0時(shí)進(jìn)行狀態(tài)切換，當(dāng)kdi≤d0時(shí)保持狀態(tài)不變。

2.2 基于UWB測距的隊(duì)形反饋機(jī)制

基于復(fù)合信息的狀態(tài)切換機(jī)制中，編隊(duì)實(shí)時(shí)矩陣S′由基于UWB測距的隊(duì)形反饋機(jī)制給出，通過與任務(wù)發(fā)布的編隊(duì)矩陣S進(jìn)行比較，對(duì)多機(jī)器人系統(tǒng)狀態(tài)進(jìn)行切換觸發(fā)?？紤]到UWB測距系統(tǒng)性價(jià)比高、能耗低，且UWB數(shù)據(jù)結(jié)構(gòu)簡單，利于在微型移動(dòng)機(jī)器人上進(jìn)行實(shí)時(shí)處理，同時(shí)依托試驗(yàn)室在UWB室內(nèi)定位方面已經(jīng)做的工作，搭建了基于UWB測距的多移動(dòng)機(jī)器人平臺(tái)，因此本文設(shè)計(jì)了基于UWB測距的隊(duì)形反饋機(jī)制。算法原理如圖5所示。圖5中，A、B兩點(diǎn)為1號(hào)機(jī)器人上的兩個(gè)UWB模塊，D為機(jī)器人的質(zhì)心，C為其他機(jī)器人上的UWB模塊，l0為基線，l為1號(hào)機(jī)器人到2號(hào)機(jī)器人垂直距離，θ0為1號(hào)機(jī)器人與2號(hào)機(jī)器人朝向之間的夾角，θ1、θ2、l1、l2為計(jì)算過程的中間變量。

由圖5可得

(14)

(15)

(16)

l=bsinα,

(17)

(18)

由(17)式和(18)式即可得到多機(jī)器人系統(tǒng)中各個(gè)機(jī)器人之間的距離及夾角，從而對(duì)整個(gè)編隊(duì)隊(duì)形進(jìn)行反饋。(14)式～(18)式是當(dāng)前方機(jī)器人在后方機(jī)器人逆時(shí)針方向時(shí)解得的二者之間姿態(tài)角；當(dāng)前方機(jī)器人在后方機(jī)器人順時(shí)針方向時(shí)，(16)式和(17)式可變?yōu)?/p>

(19)

l=asinβ.

(20)

(14)式～(20)式為基于UWB測距的隊(duì)形反饋機(jī)制的數(shù)字推導(dǎo)過程，當(dāng)UWB模塊≥3時(shí)，構(gòu)建的多機(jī)器人系統(tǒng)即可完成協(xié)同定位，每個(gè)機(jī)器人都可以將自身位置相對(duì)于其他機(jī)器人位置解算出來，從而組建編隊(duì)隊(duì)形，并對(duì)整個(gè)編隊(duì)隊(duì)形起到保持、反饋?zhàn)饔?。算法流程圖如圖6所示。

2.3 分布式狀態(tài)估計(jì)算法

考慮到Leader-Follower算法、虛擬結(jié)構(gòu)法及基于行為的算法都需要對(duì)跟隨點(diǎn)狀態(tài)進(jìn)行估計(jì)，本節(jié)提出分布式狀態(tài)估計(jì)算法，依照通信拓?fù)浣Y(jié)構(gòu)，僅利用有限的局部信息對(duì)跟隨點(diǎn)的位置坐標(biāo)、方向以及線速度等狀態(tài)量進(jìn)行估計(jì)。

假設(shè)跟隨點(diǎn)的狀態(tài)變量q0為

q0=(x0,y0,θ0,v0)T，

(21)

第i個(gè)機(jī)器人對(duì)虛擬跟隨點(diǎn)狀態(tài)變量的估計(jì)值li為

li=(xi0,yi0,θi0,vi0)T，

(22)

機(jī)器人i與其相鄰機(jī)器人的狀態(tài)估計(jì)誤差為

(23)

設(shè)計(jì)如下估計(jì)率：

(24)

定理1當(dāng)至少有一個(gè)機(jī)器人可以直接由中央處理機(jī)得到其虛擬跟隨點(diǎn)狀態(tài)，即bi0>0時(shí)，li收斂于q0.

證明將(23)式對(duì)ei關(guān)于時(shí)間求導(dǎo)，得

(25)

(26)

3 試驗(yàn)結(jié)果及分析

為了對(duì)編隊(duì)控制算法進(jìn)行驗(yàn)證，基于試驗(yàn)室構(gòu)建的多機(jī)器人網(wǎng)絡(luò)架構(gòu)，以自行設(shè)計(jì)的3輪全向機(jī)器人和UWB測距模塊為載體，構(gòu)建了多移動(dòng)機(jī)器人研究平臺(tái)[15-16](見圖7)，兩片UWB測距模塊之間的測距誤差為±7 cm. 在長4 m×寬5 m的室內(nèi)環(huán)境分別進(jìn)行多機(jī)器人編隊(duì)隊(duì)形變換、編隊(duì)行進(jìn)、編隊(duì)避障試驗(yàn)，驗(yàn)證算法的有效性和穩(wěn)定性。

3.1 試驗(yàn)1：多機(jī)器人編隊(duì)隊(duì)形變換試驗(yàn)

將4輛全向機(jī)器人置于任意位姿。試驗(yàn)步驟如下：1)通過人工勢(shì)場法組建第1個(gè)隊(duì)形；2)通過中央處理機(jī)下達(dá)隊(duì)形變換命令，觸發(fā)狀態(tài)切換機(jī)制，基于虛擬結(jié)構(gòu)法完成第2個(gè)和第3個(gè)隊(duì)形變換。試驗(yàn)中設(shè)定編隊(duì)距離為30 cm，避碰距離為15 cm，試驗(yàn)結(jié)果如圖8、圖9所示，試驗(yàn)視頻地址為https:∥v.youku.com/v_show/id_XMzY2MjkxMTk0MA==.html?spm=a2hzp.8244740.0.0.

UWB定位系統(tǒng)采集到的試驗(yàn)1數(shù)據(jù)如圖9所示。

由試驗(yàn)1的結(jié)果可知，在多機(jī)器人編隊(duì)隊(duì)形變換試驗(yàn)中，隊(duì)形內(nèi)每個(gè)機(jī)器人的誤差在±10 cm范圍，機(jī)器人能夠按照任務(wù)需要完成編隊(duì)隊(duì)形變換，同時(shí)在隊(duì)形變換中實(shí)時(shí)避碰，且在避碰后能繼續(xù)完成相應(yīng)任務(wù)，驗(yàn)證了基于復(fù)合信息的狀態(tài)切換機(jī)制和基于UWB測距的隊(duì)形反饋機(jī)制的有效性和穩(wěn)定性。

3.2 試驗(yàn)2：多機(jī)器人編隊(duì)行進(jìn)試驗(yàn)

共完成兩組試驗(yàn)：1)首先將4輛全向機(jī)器人置于任意位姿，然后基于人工勢(shì)場法組建方形編隊(duì)隊(duì)形，最后通過狀態(tài)切換，基于Leader-Follower算法完成編隊(duì)跟蹤方形軌跡；試驗(yàn)中設(shè)定編隊(duì)距離為50 cm，避碰距離為10 cm，試驗(yàn)結(jié)果如圖10所示。2)在3輛全向機(jī)器人基于人工勢(shì)場法組建一字形編隊(duì)隊(duì)形后，通過觸發(fā)狀態(tài)切換機(jī)制，由虛擬結(jié)構(gòu)法完成編隊(duì)跟蹤sin函數(shù)軌跡；試驗(yàn)中設(shè)定編隊(duì)距離為40 cm，避碰距離為20 cm，虛擬質(zhì)點(diǎn)的期望軌跡為

y=1.25+1.25×sin (2πx/2.5).

(27)

UWB定位系統(tǒng)采集到的試驗(yàn)2數(shù)據(jù)如圖11所示。

由試驗(yàn)2的結(jié)果可知，相對(duì)于試驗(yàn)1，試驗(yàn)2不僅完成了編隊(duì)組建和隊(duì)形變換，還進(jìn)行了編隊(duì)行進(jìn)，分別驗(yàn)證了人工勢(shì)場法與虛擬結(jié)構(gòu)法和Leader-Follower算法之間狀態(tài)切換機(jī)制的有效性和穩(wěn)定性。同時(shí)，與其他算法僅在編隊(duì)控制的某個(gè)環(huán)節(jié)有較好的效果相比，試驗(yàn)2中的兩組試驗(yàn)，也證明了本文所提算法在保證有效性的前提下，增加了系統(tǒng)的靈活性和適應(yīng)性，同時(shí)降低了算法的復(fù)雜度。試驗(yàn)中，多機(jī)器人編隊(duì)軌跡跟蹤狀態(tài)良好、編隊(duì)隊(duì)形保持穩(wěn)定，驗(yàn)證了分布式狀態(tài)估計(jì)算法在編隊(duì)行進(jìn)時(shí)的有效性和穩(wěn)定性。

3.3 試驗(yàn)3：多機(jī)器人編隊(duì)避障試驗(yàn)

通過對(duì)狀態(tài)切換機(jī)制中觸發(fā)條件的權(quán)重進(jìn)行修改，將試驗(yàn)分為兩組：1)強(qiáng)制保持隊(duì)形進(jìn)行編隊(duì)避障；2)采用最有效率方式進(jìn)行編隊(duì)避障。為驗(yàn)證機(jī)器人在編隊(duì)行進(jìn)中遇到障礙物時(shí)，能否觸發(fā)狀態(tài)切換并有效避障，在1號(hào)機(jī)器人上安裝了RoboPeak公司生產(chǎn)的Rplidar A1激光雷達(dá)(見圖12)，用來感知周圍環(huán)境，將數(shù)據(jù)傳輸給中央處理機(jī)，并在編隊(duì)行進(jìn)的路徑上放置長35 cm×寬50 cm的障礙物。試驗(yàn)中，首先將3輛全向機(jī)器人置于任意位姿，然后基于人工勢(shì)場法組建一字型隊(duì)形；其次通過狀態(tài)切換機(jī)制，由虛擬結(jié)構(gòu)法保持編隊(duì)行進(jìn)，當(dāng)遇到障礙物時(shí)，觸發(fā)狀態(tài)切換機(jī)制分別由虛擬結(jié)構(gòu)法強(qiáng)制保持隊(duì)形進(jìn)行避障、由基于行為的算法進(jìn)行編隊(duì)避障；最后在避障后重新組建編隊(duì)并保持行進(jìn)。試驗(yàn)中設(shè)定編隊(duì)距離為30 cm，避碰距離為10 cm.

UWB定位系統(tǒng)采集到的試驗(yàn)3數(shù)據(jù)如圖13所示。

由試驗(yàn)3的結(jié)果可知：第1組試驗(yàn)結(jié)果為基于虛擬結(jié)構(gòu)法強(qiáng)制保持隊(duì)形進(jìn)行避障，第2組試驗(yàn)結(jié)果為基于行為的算法進(jìn)行編隊(duì)避障；通過改變狀態(tài)切換的閾值矩陣，可以方便、有效地對(duì)多機(jī)器人編隊(duì)狀態(tài)進(jìn)行改變，從而增加系統(tǒng)的靈活性和適應(yīng)性。試驗(yàn)結(jié)果證明了本文所提算法能夠有效地完成編隊(duì)避障并實(shí)時(shí)避碰，且在避障完成后能重新組建隊(duì)形并保持隊(duì)形前進(jìn)。相對(duì)于試驗(yàn)1和試驗(yàn)2，在完成編隊(duì)組建、編隊(duì)行進(jìn)后，進(jìn)一步考察了編隊(duì)避障，驗(yàn)證了基于狀態(tài)切換的分布式多機(jī)器人編隊(duì)控制策略在實(shí)際編隊(duì)控制中的有效性和穩(wěn)定性。

4 結(jié)論

本文基于UWB測距，將切換系統(tǒng)的思想引入編隊(duì)控制中，提出了基于狀態(tài)切換的編隊(duì)控制策略。在充分發(fā)揮各傳統(tǒng)編隊(duì)控制算法優(yōu)點(diǎn)的同時(shí)，最大化摒棄各算法的缺點(diǎn)，有效降低了編隊(duì)控制算法的復(fù)雜度。提出了分布式狀態(tài)估計(jì)算法，降低了對(duì)機(jī)器人數(shù)據(jù)處理能力的要求。最后通過搭建的多機(jī)器人平臺(tái)，完成了編隊(duì)隊(duì)形變換、編隊(duì)行進(jìn)、編隊(duì)避障等試驗(yàn)，實(shí)驗(yàn)結(jié)果表明，所提策略能夠在多任務(wù)條件下有效完成編隊(duì)控制。