遞階式虛擬結構非完整機器人的編隊控制

袁健，張文霞

（1.中國海洋大學信息科學與工程學院，山東青島266100；2.青島理工大學琴島學院機電系，山東青島266016）

遞階式虛擬結構非完整機器人的編隊控制

袁健1，張文霞2

（1.中國海洋大學信息科學與工程學院，山東青島266100；2.青島理工大學琴島學院機電系，山東青島266016）

提出一種遞階虛擬結構編隊控制方法，將機器人編隊按空間分布劃分為簇，并定義某段時間內該簇（虛擬結構）的參考軌跡，將虛擬結構的運動轉化為各機器人的期望軌跡，然后基于反步思想，并通過設計移動機器人誤差跟蹤系統的Lyapunov函數來設計動態反饋控制器，實現了對參考軌跡的全局漸近跟蹤.最后通過對給定直線和圓軌跡的編隊跟蹤試驗，驗證了該控制策略的有效性.

移動機器人；編隊控制；遞階式虛擬結構；跟蹤控制

0 引言

多移動機器人編隊控制是多機器人協作的典型問題，是要求各機器人在執行任務時，整個機器人團隊建立并保持預先決定的幾何形狀（如一字形、三角形等），同時又要適應環境變化的一種控制技術.多移動機器人編隊控制思想可以廣泛應用于陸地自治機器人(AGV)[1-12]、無人駕駛飛行器(UAV)[13-15]、水下自治航行器(AUV)[16-19]以及人造衛星[20-21]等的隊形控制問題中，可以完成單個機器人無法實現的控制任務.基于虛擬結構的編隊控制方法要求機器人以剛體上相應點的位置和姿態信息作為各自的跟蹤目標，這樣就可以形成一定的隊形[2，4，11，20-21]，與其它編隊控制方法相比，其控制思想更容易理解和實現，缺點是采用集中式，無法實現靈活的編隊切換，并缺乏對障礙的適應性.文獻［4］提出一種分散式編隊控制框架，實現了多機器人的編隊控制，但其僅針對同類型的機器人，而不能擴展到異類型機器人的編隊控制中.文獻［11］將協商算法和領航者-跟隨者思想應用到機器人的編隊控制中，通過即時局部通訊實現了動態的協商控制，但一旦通訊失效則編隊失敗.文獻［20］將主從式、行為式以及虛擬機構式編隊思想應用到多深空探測器的編隊控制中，并提出一種集中式的協調控制框架，利用多智能體控制思想實現了探測器的編隊控制.文獻［21］改進了文獻［20］的集中式編隊框架，并結合虛擬結構思想，實現了航天器的編隊控制.編隊控制不僅要考慮位置的協調，在實際環境中還需考慮時間上的協調，與路徑跟蹤方法相比，軌跡跟蹤方法更適用于對時間有嚴格要求的編隊控制.文獻［22］對非完整移動機器人系統軌跡跟蹤控制采取了反步控制方法，對滿足一定條件的參考軌跡實現全局指數跟蹤，此法要求參考平移速度不趨于零，或參考平移速度趨于零但參考角速度不趨于零.

本文提出一種遞階式虛擬結構編隊控制方法，結合leader-follower和virtual structure編隊思想，將機器人編隊分為若干簇，對簇和簇內機器人的控制采用反步控制來實現具有遞階式虛擬的多機器人編隊控制.首先將機器人編隊按空間分布劃分為簇，并定義某段時間內該簇（虛擬結構）的期望動力學特性，再將虛擬結構的運動轉化為各機器人的期望運動（軌跡），然后基于反步思想，通過設計移動機器人誤差跟蹤系統的Lyapunov函數，來設計動態反饋控制器，通過具有障礙檢測能力的機器人（此時為leader）和其它機器人之間的通訊來實現實時的隊形切換和反饋，實現了一定性能指標下對參考軌跡的全局漸近跟蹤，最后通過對給定直線和圓軌跡的編隊跟蹤試驗，驗證了該控制策略的有效性.

1 問題描述

機器人編隊需要根據環境信息（障礙）進行相應靈活的隊形切換操作，也就是我們要預先指定某種形狀的隊列以適應周圍環境（比如避碰、避障等）.虛擬結構法可以很容易地指定機器人群體的行為，并可以進行隊形反饋，機器人之間不涉及復雜的通訊協議，就可以實現各機器人的完全自主運動.一旦選擇了某種隊形后，機器人之間的相對位置保持不變，即在剛體參考坐標系下坐標不變的情況下，以一定的自由度來改變自己的姿態；多移動機器人以剛體上的相應點作為各自的跟蹤目標，就可以形成一定的隊形，從而編隊控制問題就轉化為機器人的軌跡跟蹤問題.

1.1 遞階式虛擬結構

本文提出一種遞階式虛擬結構協調框架，用以解決由于通訊范圍和帶寬約束所導致的整個編隊跟蹤性能下降的問題.首先將整個編隊在空間上分為若干簇，每簇選定一個通訊能力強的機器人作為該簇的領航者，只有領航者之間進行通信，獲取期望軌跡后協調每簇的行為.簇間及簇中每個機器人只是按照虛擬結構進行編隊控制，這樣就可以控制大規模的機器人編隊，如圖1（a）所示.圖1（b）為每簇的控制結構，其中最高層Gi為離散事件監測器，可以演化出一系列的編隊模式，其輸出為當前編隊模式；Fi為編隊控制模塊，產生并以廣播通信的方式協調變量ξi；Ki為機器人個體的局部控制器模塊，其輸入為機器人的輸出向量yi和協調變量ξi，并將ξi轉化為要求的機器人位置和指向，然后控制機器人跟蹤該要求狀態，輸出為控制向量ui和性能值zi；最底層的Si代表機器人系統，機器人的輸入就是其控制器的輸出向量ui，機器人的輸出向量yi代表機器人的位姿.為確保跟蹤性能，定義各個機器人性能指標：

圖1 遞階式虛擬結構

第j簇機器人編隊性能指標為：

其中Nj為第j簇編隊機器人數目，wji為第j簇中第i個機器人的性能權重.則整個編隊的性能指標可表示為：

其中M為編隊的簇數目，Wj為第j簇權重.根據各機器人控制器模塊性能指標Ji，計算各自的性能指標，并向上發送到編隊控制模塊Fi.Fi計算整個編隊性能指標ζ（pe，t），并根據協調變量ξi來判斷是否達到期望軌跡而選取性能閾值δ，若不滿足期望性能指標則需重新發送協調變量ξi，即ζ（pe，t）＞δ，發送ξi；ζ（pe，t）≤δ，不發送ξi.

1.2 非完整移動機器人軌跡跟蹤控制

移動機器人是非完整性系統，非完整性約束表現在車輪只能在垂直于驅動軸的方向滾動而不能產生側滑.設其具有n個廣義坐標（p1，p2，…，pn），則動力學方程可以描述為：

其中M（p）∈Rn×n為對稱正定慣性矩陣，V（p，p˙）∈Rn×n為離心力和科里奧利力矩陣，F（p˙）∈Rn表示表面摩擦力向量，G（p）∈Rn為引力向量，qd表示未知有界擾動，B（p）∈Rn×r為輸入轉換矩陣，q∈Rr為控制量，A（p）∈Rm×n為約束矩陣，λ∈Rm為約束力向量.對于在平面內運動的具有單前輪的移動機器人系統，其運動學方程約束可以表示為A（p）Tp˙=0，其中p=[x，y，θ]T表示機器人的運動狀態，q=[υ，ω]T表示機器人方程的控制輸入.（x，y）為機器人在慣性坐標系下的位置坐標，θ=arctan（y˙（t）/x˙（t））為駛向角，υ，ω分別為機器人的線速度和角速度，約束方程為y˙cosθ-x˙sinθ=0，約束矩陣可寫為A（p）=[sinθ，-cosθ，0]T，所以A（p）Tp˙=0，又由矩陣為約束矩陣A（p）的零空間，即A（p）TS（p）=0.因此p˙=S（p）q，為其運動學模型.

在移動機器人工作平面內建立兩個坐標系，分別為慣性坐標系x-o-y和機器人本體坐標系x′-o′-y′.如圖2所示，機器人狀態由其兩驅動輪的中點在慣性坐標系下的位置及駛向角來表示.

機器人運動學方程為：

并具有速度非完整約束x˙sin θ=y˙cos θ.

軌跡跟蹤問題就是要找到υ和ω的

控制律，使機器人跟蹤參考軌跡[xd，yd，θd]T和參考速度輸入υd和ωd，如圖2所示.

在機器人本體坐標系下的位姿誤差定義為[22]：

圖2 移動機器人的位姿及誤差

其中Te為從慣性坐標系到機器人本體坐標系的轉換矩陣.

對式（2）求時間導數，所以誤差的動力學方程可以寫為：

可得：

其中υ，ω分別可以表示為：υ=υ（xe，ye，θe，υd，ωd），ω=ω（xe，ye，θe，υd，ωd）.移動機器人運動學模型的全局軌跡跟蹤問題，即通過控制輸入q，使得對任意初始誤差[xe（0），ye（0），θe（0）]T，方程（3）的閉環軌跡和控制υ，ω一致有界且.跟蹤控制系統框如圖3所示，參考軌跡pd是參考輸入υ通過參考系統得到的系統輸出，其中，NC為非線性控制器，Σ1為受控機器人系統（1），Σd為參考系統的運動學方程：

圖3 跟蹤控制框

2 編隊控制器設計

針對非完整機器人編隊形成問題，上節提出了一種遞階式虛擬結構的編隊形成策略，并將編隊問題轉化為機器人的軌跡跟蹤問題，本節將討論軌跡跟蹤控制器的設計問題.

定理1假設υd和ωd在[0，∞）上一致連續并有界（υd和ωd不同時收斂到0），可以設計控制律υ和ω，使得系統（3）全局漸近穩定，即，其中，

證明若令xe=c1ωye，θe=0，則可使式（3）中的ye→0.

其中α=ωye+υdcos θe-c1ye-c1ω（-ωxe+υdsin θe）.

3 仿真算例

例1橫向編隊并實現直線軌跡跟蹤控制問題.設3個機器人Robot1、Robot2、Robot3的初始位置狀態分別為：

Robot1、Robot2、Robot3預先指定的跟蹤軌跡的位置初值分別為：

三者間保持0.2 m的橫向距離，直線軌跡初始斜率為k=1，當t=400 s時斜率變為0.選取ωd=0 rad/s，υd=0.2 m/s，c1=3，c2=3，c3=10，w11=w12=w13=1，仿真步長τ＝100 ms，仿真時間T=1 000 s.當t=0 s，100 s，300 s，500 s，600 s，1 001 s時的位姿如圖4所示.各個機器人的性能指標Ji（pe，t）、總性能指標隨時間變化而變化的曲線如圖5所示.取性能閾值δ＝1.0，從表1中可以看出，t=100 s時滿足性能要求；t=400 s軌跡發生改變，導致性能指標變大，但又很快收斂.

圖4 三機器人橫向編隊

圖5 各機器人性能指標

表1 橫向編隊跟蹤誤差指標

例2三角形編隊并實現圓形軌跡跟蹤控制問題.3個機器人Robot1、Robot2、Robot3的初始位置狀態分別為：

Robot1、Robot2、Robot3預先指定的跟蹤軌跡的位置初值為：

選取ωd=0.05 rad/s，υd=0.05 m/s，w11=w12=w13=1，仿真步長τ＝100 ms.c1=1，c2=122，c3=19，初始軌跡圓半徑為r1=1.8 m，在t=400 s時，期望軌跡發生改變，圓半徑為r2=2 m，仿真時間T=1 000 s.圖6中分別標注了3個機器人在t=0 s，25 s，300 s，400 s，500 s，600 s，1 001 s時的位姿.性能指標Ji（pe，t）、總性能指標ζ（pe，t）＝隨時間變化而變化的曲線如圖7所示.取性能閾值δ＝1.0，從表2中可以看出，t=25 s時滿足性能要求；t=400 s軌跡發生改變，導致性能指標變大，但又很快收斂.

圖6 三角形編隊實現圓軌跡跟蹤

圖7 各機器人性能指標

表2 三角編隊跟蹤誤差指標

4 結語

本文研究了跟蹤軌跡為直線和圓兩種情形的多移動機器人編隊形成和控制問題，通過構造Lyapunov函數使其導數負定來構造反步控制器，并基于分布式虛擬結構方法實現了多機器人的平行編隊和三角編隊，使其能夠在無障礙情況下保持隊形并快速收斂到期望軌跡.通過仿真驗證了該方法的有效性.

[1] Wang P K C.Navigation strategies for multiple autonomous mobile robot s moving in formation [J].Journal of Robotic Systems，1991，8（2）：177-195.

[2] Lewis M A，Tan K H.High precision format ion cont rol of mobilerobot s using virt ual structure approach[J].Autonomous Robots，1997（4）：387-403.

[3] Balch T，Arkin R C.Behavior-based formation control for multi-robot teams[J].IEEE Transactions on Roboti cs and Automation，1998（14）：926-939.

[4] Lawton A R，Young B J，Be ard R W.A decent ralized approach to eleme ntary format ion maneuvers[C]//IEEE Internat ional Conference on Robot ics and Automat ion.San Francisco：IEEE Press，2000（3）：2728-2 733.

[5] Leonard N E，Fiore lli E.Virt ual leader，artif icial pote ntials and coordinate d cont rol of groups[C]//IEEE Conferenceon Decision and Cont rol.Orlando：IEEE Press，2001（3）：2 968-2 973.

[6] Dunba W B，Murray R M.Model predictive control of coordinate d multi-vehicle formations[C]//IEEE Conference on Decision and Cont rol.Las Ve gas：IEEE Press，2002（4）：4 631-4 636.

[7] Desai J P.A graph theoretic approach f or modeling mobile robotteam formations [J].Journal of Robotic Systems，2002，19（11）：511-525.

[8] Lawton J R，Beard R W，Young B J.A decentralized approach to formation maneuvers[J].IEEE Transactions on Robotics and Automation，2003，19（6）：933-941.

[9] Do K D.Formation tracking control of unicycle-type mobile robots with limited sensing ranges[J].IEEE Transactions on Control Systems Technology，2008，16（3）：527-538.

[10] De La Cruz C,Carelli R.Dynamic model based formation control and obstacle avoidance of multirobot systems[J].Robotica,2008,26（3）：345-356.

[11] RenW,Sorensen N.Distributed coordination architecture for multi-robot formation control[J].Robotics and Autonomous Systems,2008,56（4）：324-333.

[12] Dong Wenjie，Farrell J A.Decentralized cooperative control of multiple nonholonomic dynamic systems with uncertainty[J].Automatica，2009（45）：706-710.

[13] Ryan A，Zennaro M，Howell A，et al.An overview of emerging results in cooperative UAV control[C]//IEEE Conference on Decision and Control.Paradise Island：IEEE Press，2004（1）：602-607.

[14] Galzi D,Shtessel Y.UAV Formations control using high order sliding modes[C]//American Control Conference.Minnesota：IEEE Press，2006：4 249-4 255.

[15] Seungkeun K，Youdan K.Three dimensional optimum controllers for multiple UAV formation flight using behavior-based decentralized approach[C]//International Conference on Control.Seoul：IEEE Press，2007：1 387-1 392.

[16]Tangirala S，Kumar R，Bhattacharyya S，et al.Hybrid-model based hierarchical mission control architecture f or aut onomous underwat er vehicles[C]//American Cont rol Conference.Oregan：IEEE Press，2005（1）：668-673.

[17] Okamoto A，Edwards D B，Anderson M J.Robustcont rol of a plat oon of underwat er autonomous vehicles [J].Sea Technology，2005，46（12）：10-13.

[18] Yang E F，Gu D B.Nonlinear formation-keeping and mooring cont rol of mult iple autonomous unde rwater vehicles[J].IEEE/ASME Transact ions on Mechatronics，2007,2（2）：164-178.

[19] Kalantar S，Zimmer U R.Distributed shape cont rol of homogeneous swarms of autonomous underwat er vehicles[ J].Autonomous Robots，2007，22（1）：37-53.

[20] Beard R W，Lawton J，Hadacgh F Y.A coordination architecture for spacecraft format ion control[J].IEEE Transactions on Cont rol Systems Technology,2001,9（6）：777-790.

[21] Ren W,Beard R W.Decentralized scheme for spacecraft formation flying via the virtual structure approach[J].Journal of Guidance，Control and Dynamics，2004，27（1）：73-82.

[22] Tiang Z P，Nijmeujer H.Tracking control of mobile robot s：A case st udy in backst epping [J].Automatica，1997，33（7）：1 393-1 399.

Formation Control for Mobile Nonholonomic Multi-Robots with Hierarchical Virtual Structure

YUAN Jian1，ZHANG Wen-xia2
（1.College of Information Science&Engineering,Ocean University of China,Qingdao 266100,Shandong,China;2.Mechanical&Electrical Engineering Department,Qindao College,Qingdao 266016,Shandong,China）

s：Theformationcontrol problem ofmobilenonholonomicmulti-robots is investigated.A formation control structure with hierarchical Virtual Structure is proposed.The mobile robots are divided into some clusters according to their distributions in space and motion trajectory of every cluster（virtual structure）.The motion of virtual structure is transformed to desired trajectory of every robot.A Lyapunov function of the error tracking system is constructed based on the backstepping method to design dynamic feedback tracking controller.The tracking controller can make the trajectory of every mobile robot globally and asymptotically track a desired trajectory.Finally，formation simulations considering line and circle shape are carried out and the numerical simulation results show the effectiveness of the proposed control scheme.

nonholonomic；mobile robot；formation control；hierarchical virtual structure；trackingcontrol

TP 273

A

1001-4217（2010）02-0042-09

2009-11-18

袁健（1980-），男，山東榮成人，博士.研究方向：計算機控制及仿真技術.E-mail：jyuangjian@ouc.edu.cn