基于狀態切換的分布式多機器人編隊控制

衛恒， 呂強， 劉揚， 林輝燦， 梁冰

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

0 引言

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

多機器人編隊算法設計過程中，需要考慮機器人模型、外部環境干擾、算法控制精度以及不同隊形可控性等諸多問題[3]。傳統的多機器人編隊控制算法主要有Leader-Follower算法[4]、基于行為的算法[5]、虛擬結構法[6]、人工勢場法等。基于Leader-Follower算法的基本思想是在多機器人系統中選擇一個機器人作為Leader，其他機器人為Follower，通過調節Leader與Follower之間的角度和距離來控制編隊隊形。文獻[7]提出了距離- 角度與距離- 距離兩種編隊控制算法，距離- 角度算法通過控制Leader與Follower之間的距離和角度進行編隊控制，距離- 距離算法則通過Follower與相鄰機器人之間的距離來保持隊形。文獻[8]為了防止機器人之間出現碰撞，提出了基于虛擬引力的改進型距離- 角度算法與距離- 距離算法。Leader-Follower算法具有靈活的運動策略、良好的擴展性，但是算法中Leader與Follower之間不能形成穩定、有效的反饋，因此Follower的跟蹤誤差會隨著環境的干擾不斷擴大，且當Leader出現故障時整個多機器人系統也會無法工作。基于行為的算法其基本思想是為多機器人系統中的每個機器人設計基本的行為準則，例如避障、避碰、向目標運動、保持隊形等行為。該算法可以有效地降低整個多機器人系統編隊控制算法的復雜度，但是其對傳感器探測能力和機器人之間的通信能力要求較高，且在運行過程中無法對機器人的行為進行準確描述，難以保證系統的魯棒性。虛擬結構法最早由Lewis等[9]提出，其基本思想是將整個多機器人系統看成一個剛性整體，將各機器人看作是剛體上的某個點。該算法的優點是便于設計多機器人系統的編隊行為，隊形保持率高，但由于受到剛性結構的約束，其在避障、隊形變換等方面缺乏靈活性。人工勢場法的基本思想是在機器人周圍假設具有類似電磁場的引力和斥力，通過設定適當的勢能函數對多機器人進行編隊控制。該算法結構簡單、易于設計，能夠有效地完成避碰和避障，但在隊形保持時容易受到干擾，不易進行精確編隊控制，若進行隊形變換則需要重新設定勢能函數，缺乏靈活性[7]。

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

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

1 預先知識準備

1.1 3輪全向機器人建模

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

假設vx、vy為機器人在全局坐標系下x軸和y軸的速度分量，ω為機器人的自轉角速度，以機器人質心來代替機器人在全局坐標系下的位置，則機器人坐標為(x,y,θ)，二者之間關系可以表示為

(1)

全向機器人3個輪子的線速度可以表示為

(2)

聯立(1)式和(2)式，得3輪全向機器人的運動學方程為

(3)

假設機器人質量為m，機器人繞自身中心旋轉的轉動慣量為I，全向輪半徑為R，每個電機電壓為U1、U2、U3，電機的電樞電阻為Ra，電機轉矩常數為Kt. 令Fx為機器人在x軸方向上所受合力，Fy為機器人在y軸方向上所受合力，M為機器人所受力矩，由牛頓定律可得

(4)

由(4)式可得

(5)

(6)

聯立(5)式、(6)式，得3輪全向機器人的動力學方程為

(7)

1.2 圖論基本概念

機器人之間的通信拓撲結構通過有向圖G={V,E}完成，其中，V為節點集合，每一個機器人代表1個節點；E為邊集合，代表機器人之間的連接狀態。有向圖通過拉普拉斯矩陣表示，即L=D-A，其中D為度矩陣，表示每一個節點都有幾個節點與其相鄰；A為鄰接矩陣，表示節點之間的鄰接關系。設一共有n個機器人，i、j分別表示第i和第j個機器人，則拉普拉斯矩陣定位如下：

(8)

考慮到機器人之間通信拓撲結構會因環境不同而變化，本文設計了權重矩陣B=[b10,b20,…,bij]表示機器人之間的通信連接關系，定義矩陣H=L+B表示多機器人之間的通信拓撲關系。本文算法中，設定機器人之間的通信是雙向的，因此圖G是無向圖，B是對稱正定矩陣。由拉普拉斯矩陣性質可知，L也是對稱正定矩陣，因此矩陣H也是對稱正定矩陣。

2 基于狀態切換的分布式多機器人編隊控制策略

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

2.1 基于復合信息的狀態切換觸發策略

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

其次，考慮了多機器人編隊控制中，機器人因為不同的隊形，采用不同通信拓撲結構時的情況。拓撲結構主要分為2類：第1類是鏈狀拓撲結構，如圖3所示，此時采用Leader-Follower算法來進行編隊控制；第2類是環狀拓撲結構，如圖4所示，此時采用虛擬結構法來進行編隊控制。

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

控制策略設計中，采用基于任務驅動、自上而下的設計方案，首先確立切換系統模型，設計基于復合信息的狀態切換機制，然后以模塊化方式設計機器人控制部分。控制部分分為內環控制和外環控制。外環控制為3種編隊控制算法模塊：Leader-Follower算法模塊、虛擬結構算法模塊、基于行為的算法模塊。內環控制主要為移動機器人底層控制驅動算法。同時，因為在多機器人避碰方面，人工勢場法具有結構簡單、易于與其他編隊算法融合的特點，所以將人工勢場算法模塊也寫入機器人底層控制驅動算法中[7]。具體控制策略如下：

設切換系統為

(9)

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

基于復合信息的狀態切換機制，以多機器人系統編隊實時矩陣S′與機器人狀態為判斷依據，其中編隊實時矩陣由每個機器人與其他機器人之間的距離以及基于自身機體坐標系與其他機器人的夾角構成。由于構建的切換系統為事件驅動，機器人編隊矩陣S由任務發布或在運行中由環境突變觸發改變。

設有一個由N個機器人組成的多機器人系統，通信拓撲結構用H=L+B表示，qi=(xi,yi,θi,vi)T表示第i個機器人在t時刻的狀態，r表示機器人圓形鄰域的半徑，第i個機器人的鄰域Ni(t)={j|‖xj(t)-xi(t)‖≤r}，nr表示在半徑為r的鄰域內，共有nr個機器人與機器人i相鄰，則

(10)

依據1.2節分析，系統間連接關系由L確定，B為權重矩陣，因此機器人i的鄰接關系為

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

(11)

由此可得

(12)

從而得到差值矩陣為

(13)

設定指標權重矩陣k=(k1,k2,k3,k4)T，依據環境及小車狀態分別對di中的元素進行加權，從而對狀態切換機制進行調節。設定閾值矩陣為d0，當kdi>d0時進行狀態切換，當kdi≤d0時保持狀態不變。

2.2 基于UWB測距的隊形反饋機制

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

由圖5可得

(14)

(15)

(16)

l=bsinα,

(17)

(18)

由(17)式和(18)式即可得到多機器人系統中各個機器人之間的距離及夾角，從而對整個編隊隊形進行反饋。(14)式～(18)式是當前方機器人在后方機器人逆時針方向時解得的二者之間姿態角；當前方機器人在后方機器人順時針方向時，(16)式和(17)式可變為

(19)

l=asinβ.

(20)

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

2.3 分布式狀態估計算法

考慮到Leader-Follower算法、虛擬結構法及基于行為的算法都需要對跟隨點狀態進行估計，本節提出分布式狀態估計算法，依照通信拓撲結構，僅利用有限的局部信息對跟隨點的位置坐標、方向以及線速度等狀態量進行估計。

假設跟隨點的狀態變量q0為

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

(21)

第i個機器人對虛擬跟隨點狀態變量的估計值li為

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

(22)

機器人i與其相鄰機器人的狀態估計誤差為

(23)

設計如下估計率：

(24)

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

證明將(23)式對ei關于時間求導，得

(25)

(26)

3 試驗結果及分析

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

3.1 試驗1：多機器人編隊隊形變換試驗

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

UWB定位系統采集到的試驗1數據如圖9所示。

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

3.2 試驗2：多機器人編隊行進試驗

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

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

(27)

UWB定位系統采集到的試驗2數據如圖11所示。

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

3.3 試驗3：多機器人編隊避障試驗

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

UWB定位系統采集到的試驗3數據如圖13所示。

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

4 結論

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