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

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

0 引言

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

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

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

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

1 預(yù)先知識準(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點連線的夾角；φ為相鄰兩個輪子軸的夾角,φ=120°；L為質(zhì)心OB到輪子的距離；v1、v2、v3為3輪全向機(jī)器人輪子的線速度；f1、f2、f3為3個電機(jī)提供的驅(qū)動力。

假設(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個輪子的線速度可以表示為

(2)

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

(3)

假設(shè)機(jī)器人質(zhì)量為m，機(jī)器人繞自身中心旋轉(zhuǎn)的轉(zhuǎn)動慣量為I，全向輪半徑為R，每個電機(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ī)器人的動力學(xué)方程為

(7)

1.2 圖論基本概念

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

(8)

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

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

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

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

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

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

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

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

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

(9)

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

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

設(shè)有一個由N個機(jī)器人組成的多機(jī)器人系統(tǒng)，通信拓?fù)浣Y(jié)構(gòu)用H=L+B表示，qi=(xi,yi,θi,vi)T表示第i個機(jī)器人在t時刻的狀態(tài)，r表示機(jī)器人圓形鄰域的半徑，第i個機(jī)器人的鄰域Ni(t)={j|‖xj(t)-xi(t)‖≤r}，nr表示在半徑為r的鄰域內(nèi)，共有nr個機(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)分別對di中的元素進(jìn)行加權(quán)，從而對狀態(tài)切換機(jī)制進(jìn)行調(diào)節(jié)。設(shè)定閾值矩陣為d0，當(dāng)kdi>d0時進(jìn)行狀態(tài)切換，當(dāng)kdi≤d0時保持狀態(tài)不變。

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

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

由圖5可得

(14)

(15)

(16)

l=bsinα,

(17)

(18)

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

(19)

l=asinβ.

(20)

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

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

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

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

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

(21)

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

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

(22)

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

(23)

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

(24)

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

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

(25)

(26)

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

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

3.1 試驗1：多機(jī)器人編隊隊形變換試驗

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

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

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

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

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

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

(27)

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

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

3.3 試驗3：多機(jī)器人編隊避障試驗

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

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

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

4 結(jié)論

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