遞階式虛擬結(jié)構(gòu)非完整機(jī)器人的編隊(duì)控制

袁健，張文霞

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

遞階式虛擬結(jié)構(gòu)非完整機(jī)器人的編隊(duì)控制

袁健1，張文霞2

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

提出一種遞階虛擬結(jié)構(gòu)編隊(duì)控制方法，將機(jī)器人編隊(duì)按空間分布劃分為簇，并定義某段時(shí)間內(nèi)該簇（虛擬結(jié)構(gòu)）的參考軌跡，將虛擬結(jié)構(gòu)的運(yùn)動(dòng)轉(zhuǎn)化為各機(jī)器人的期望軌跡，然后基于反步思想，并通過設(shè)計(jì)移動(dòng)機(jī)器人誤差跟蹤系統(tǒng)的Lyapunov函數(shù)來設(shè)計(jì)動(dòng)態(tài)反饋控制器，實(shí)現(xiàn)了對(duì)參考軌跡的全局漸近跟蹤.最后通過對(duì)給定直線和圓軌跡的編隊(duì)跟蹤試驗(yàn)，驗(yàn)證了該控制策略的有效性.

移動(dòng)機(jī)器人；編隊(duì)控制；遞階式虛擬結(jié)構(gòu)；跟蹤控制

0 引言

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

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

1 問題描述

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

1.1 遞階式虛擬結(jié)構(gòu)

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

圖1 遞階式虛擬結(jié)構(gòu)

第j簇機(jī)器人編隊(duì)性能指標(biāo)為：

其中Nj為第j簇編隊(duì)機(jī)器人數(shù)目，wji為第j簇中第i個(gè)機(jī)器人的性能權(quán)重.則整個(gè)編隊(duì)的性能指標(biāo)可表示為：

其中M為編隊(duì)的簇?cái)?shù)目，Wj為第j簇權(quán)重.根據(jù)各機(jī)器人控制器模塊性能指標(biāo)Ji，計(jì)算各自的性能指標(biāo)，并向上發(fā)送到編隊(duì)控制模塊Fi.Fi計(jì)算整個(gè)編隊(duì)性能指標(biāo)ζ（pe，t），并根據(jù)協(xié)調(diào)變量ξi來判斷是否達(dá)到期望軌跡而選取性能閾值δ，若不滿足期望性能指標(biāo)則需重新發(fā)送協(xié)調(diào)變量ξi，即ζ（pe，t）＞δ，發(fā)送ξi；ζ（pe，t）≤δ，不發(fā)送ξi.

1.2 非完整移動(dòng)機(jī)器人軌跡跟蹤控制

移動(dòng)機(jī)器人是非完整性系統(tǒng)，非完整性約束表現(xiàn)在車輪只能在垂直于驅(qū)動(dòng)軸的方向滾動(dòng)而不能產(chǎn)生側(cè)滑.設(shè)其具有n個(gè)廣義坐標(biāo)（p1，p2，…，pn），則動(dòng)力學(xué)方程可以描述為：

其中M（p）∈Rn×n為對(duì)稱正定慣性矩陣，V（p，p˙）∈Rn×n為離心力和科里奧利力矩陣，F(xiàn)（p˙）∈Rn表示表面摩擦力向量，G（p）∈Rn為引力向量，qd表示未知有界擾動(dòng)，B（p）∈Rn×r為輸入轉(zhuǎn)換矩陣，q∈Rr為控制量，A（p）∈Rm×n為約束矩陣，λ∈Rm為約束力向量.對(duì)于在平面內(nèi)運(yùn)動(dòng)的具有單前輪的移動(dòng)機(jī)器人系統(tǒng)，其運(yùn)動(dòng)學(xué)方程約束可以表示為A（p）Tp˙=0，其中p=[x，y，θ]T表示機(jī)器人的運(yùn)動(dòng)狀態(tài)，q=[υ，ω]T表示機(jī)器人方程的控制輸入.（x，y）為機(jī)器人在慣性坐標(biāo)系下的位置坐標(biāo)，θ=arctan（y˙（t）/x˙（t））為駛向角，υ，ω分別為機(jī)器人的線速度和角速度，約束方程為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，為其運(yùn)動(dòng)學(xué)模型.

在移動(dòng)機(jī)器人工作平面內(nèi)建立兩個(gè)坐標(biāo)系，分別為慣性坐標(biāo)系x-o-y和機(jī)器人本體坐標(biāo)系x′-o′-y′.如圖2所示，機(jī)器人狀態(tài)由其兩驅(qū)動(dòng)輪的中點(diǎn)在慣性坐標(biāo)系下的位置及駛向角來表示.

機(jī)器人運(yùn)動(dòng)學(xué)方程為：

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

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

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

在機(jī)器人本體坐標(biāo)系下的位姿誤差定義為[22]：

圖2 移動(dòng)機(jī)器人的位姿及誤差

其中Te為從慣性坐標(biāo)系到機(jī)器人本體坐標(biāo)系的轉(zhuǎn)換矩陣.

對(duì)式（2）求時(shí)間導(dǎo)數(shù)，所以誤差的動(dòng)力學(xué)方程可以寫為：

可得：

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

圖3 跟蹤控制框

2 編隊(duì)控制器設(shè)計(jì)

針對(duì)非完整機(jī)器人編隊(duì)形成問題，上節(jié)提出了一種遞階式虛擬結(jié)構(gòu)的編隊(duì)形成策略，并將編隊(duì)問題轉(zhuǎn)化為機(jī)器人的軌跡跟蹤問題，本節(jié)將討論軌跡跟蹤控制器的設(shè)計(jì)問題.

定理1假設(shè)υd和ωd在[0，∞）上一致連續(xù)并有界（υd和ωd不同時(shí)收斂到0），可以設(shè)計(jì)控制律υ和ω，使得系統(tǒng)（3）全局漸近穩(wěn)定，即，其中，

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

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

3 仿真算例

例1橫向編隊(duì)并實(shí)現(xiàn)直線軌跡跟蹤控制問題.設(shè)3個(gè)機(jī)器人Robot1、Robot2、Robot3的初始位置狀態(tài)分別為：

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

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

圖4 三機(jī)器人橫向編隊(duì)

圖5 各機(jī)器人性能指標(biāo)

表1 橫向編隊(duì)跟蹤誤差指標(biāo)

例2三角形編隊(duì)并實(shí)現(xiàn)圓形軌跡跟蹤控制問題.3個(gè)機(jī)器人Robot1、Robot2、Robot3的初始位置狀態(tài)分別為：

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

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

圖6 三角形編隊(duì)實(shí)現(xiàn)圓軌跡跟蹤

圖7 各機(jī)器人性能指標(biāo)

表2 三角編隊(duì)跟蹤誤差指標(biāo)

4 結(jié)語

本文研究了跟蹤軌跡為直線和圓兩種情形的多移動(dòng)機(jī)器人編隊(duì)形成和控制問題，通過構(gòu)造Lyapunov函數(shù)使其導(dǎo)數(shù)負(fù)定來構(gòu)造反步控制器，并基于分布式虛擬結(jié)構(gòu)方法實(shí)現(xiàn)了多機(jī)器人的平行編隊(duì)和三角編隊(duì)，使其能夠在無障礙情況下保持隊(duì)形并快速收斂到期望軌跡.通過仿真驗(yàn)證了該方法的有效性.

[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，F(xiàn)iore 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，F(xiàn)arrell 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

袁?。?980-），男，山東榮成人，博士.研究方向：計(jì)算機(jī)控制及仿真技術(shù).E-mail：jyuangjian@ouc.edu.cn