一種多機(jī)器人編隊(duì)控制策略及實(shí)現(xiàn)

尚會(huì)超， 陳毛倩

(中原工學(xué)院， 鄭州 450007)

摘要：針對(duì)多機(jī)器人協(xié)作系統(tǒng)中機(jī)器人編隊(duì)控制問題，采用了一種基于運(yùn)動(dòng)學(xué)的領(lǐng)航-虛擬跟隨法的編隊(duì)方法，并在跟蹤控制方面對(duì)算法進(jìn)行了改進(jìn)和修正。MATLAB軟件仿真以及在HR-I型雙輪差速移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)上的實(shí)驗(yàn)證明，改進(jìn)算法能夠達(dá)到更好的編隊(duì)效果。

關(guān)鍵詞：多機(jī)器人；編隊(duì)；跟蹤控制；MATLAB仿真

中圖分類號(hào)：TP24

文獻(xiàn)標(biāo)志碼：A

DOI:10.3969/j.issn.1671-6906.2015.03.002

Abstract:Recently, in many research questions based on multi-robot cooperation system, formation problem is a widespread and relatively typical kind. In order to analyze the current problems within formation control, a formation method called “Leader-Virtual follower Method” is adopted in this paper that based on kinematics, it also is improved in tracking control and correction, then it is proved that the improved algorithm can achieve better result of formation through the MATLAB simulation and the HR-I two-wheel with different velocity mobile robot experimental platform validation.

收稿日期：2014-12-20

基金項(xiàng)目：國家自然科學(xué)基金項(xiàng)目(61305080)；河南省高?？萍紕?chuàng)新團(tuán)隊(duì)支持計(jì)劃(14IRTSTHN024)

作者簡介：王永林(1977-)，男，河南封丘人，講師，碩士，主要研究方向?yàn)槿斯ぶ悄?、電力電子與自動(dòng)化控制。

文章編號(hào)：1671-6906(2015)03-0013-06

現(xiàn)代社會(huì)生活環(huán)境出現(xiàn)了越來越多的限制人類活動(dòng)的極限狀況，如地震、臺(tái)風(fēng)等自然災(zāi)害之后的搜救，軍事上及安全部門的排雷追蹤等。在這些場合，如果采用機(jī)器人去替代人來完成復(fù)雜并且有一定危險(xiǎn)系數(shù)的操作，就能夠有效提高效率，減少人員傷亡。因此，如何更好地對(duì)機(jī)器人群體進(jìn)行組織，有效地處理多機(jī)器人之間的協(xié)作協(xié)調(diào)控制，怎樣最大限度地發(fā)揮出多機(jī)器人系統(tǒng)的能力和潛在能力，已經(jīng)成為近年來研究的一個(gè)熱門課題。

多機(jī)器人的編隊(duì)控制[1]是多個(gè)移動(dòng)機(jī)器人在到達(dá)目的地的過程中，能夠收斂于某種期望的隊(duì)形，保持相對(duì)位姿不變的同時(shí)也要適應(yīng)周邊環(huán)境存在的約束(例如進(jìn)行實(shí)時(shí)避障[2])的控制技術(shù)。近年來，隨著計(jì)算機(jī)技術(shù)和生物科學(xué)等先進(jìn)技術(shù)的發(fā)展，更多的智能算法被應(yīng)用到多機(jī)器人系統(tǒng)編隊(duì)控制中，比如文獻(xiàn)[3]基于生物刺激神經(jīng)網(wǎng)絡(luò)的多機(jī)器人編隊(duì)方法。有關(guān)隊(duì)形控制文獻(xiàn)多次提到的編隊(duì)方法主要有3種：領(lǐng)航-跟隨法(基于leader-referenced方法[4])、基于行為法和虛擬結(jié)構(gòu)法。其中，領(lǐng)航-跟隨法最為有效并且易于實(shí)施，因此被廣泛運(yùn)用。該方法的數(shù)學(xué)分析過程簡便，機(jī)器人易于保持隊(duì)形，具有高效的機(jī)器人群體運(yùn)動(dòng)效果。其基本思想是在多個(gè)機(jī)器人系統(tǒng)中選一個(gè)機(jī)器人作為整個(gè)群體的領(lǐng)導(dǎo)者(leader)，群體中的其他機(jī)器人作為該leader的隨從者(follower)。其控制過程是使follower機(jī)器人以預(yù)期的隊(duì)形結(jié)構(gòu)跟隨leader機(jī)器人動(dòng)作。文獻(xiàn)[5]所采用的基于行為的方法可實(shí)現(xiàn)分布式控制，但這個(gè)方法的缺點(diǎn)在于隊(duì)形控制的穩(wěn)定性較差。文獻(xiàn)[6]提出一種基于行為法與 leader-referenced 方法相結(jié)合的混合式編隊(duì)算法，使多機(jī)器人編隊(duì)效果更優(yōu)。然而，這些方法都沒有從根本上解決多機(jī)器人編隊(duì)控制中穩(wěn)定性和實(shí)用性這兩個(gè)難題。

虛擬結(jié)構(gòu)法[7]是將整個(gè)結(jié)構(gòu)隊(duì)形看作一個(gè)虛擬的剛體結(jié)構(gòu)，結(jié)構(gòu)中的每個(gè)機(jī)器人把剛體上的節(jié)點(diǎn)當(dāng)作跟蹤目標(biāo)。這種方法的優(yōu)點(diǎn)是行為容易描述，缺點(diǎn)是靈活性比較差。

此外，還有基于模糊原理和神經(jīng)網(wǎng)絡(luò)的智能避障算法[8]，以及考慮動(dòng)力學(xué)的魯棒控制器方面的領(lǐng)航者模式編隊(duì)方法[9]。該類算法的優(yōu)點(diǎn)是能夠快速編隊(duì)，進(jìn)行軌跡跟蹤，但在穩(wěn)定性、計(jì)算量控制方面還有很大的缺陷。

Balch T和Arkin R C最早提出領(lǐng)航-跟隨的控制方法[7]。1998年，Desai J P等首先提出了一種領(lǐng)航-跟隨隊(duì)形的控制框架[10]，并且用模型(l-φ)或(l-l)表示跟隨者相對(duì)于領(lǐng)航者的位姿。該方法隨后得到了進(jìn)一步的發(fā)展和演化[11]。Fujimori A等提出了一種改進(jìn)的跟隨領(lǐng)航法[12]，使得編隊(duì)控制變得比較簡單，但該方法在機(jī)器人掉隊(duì)后重新恢復(fù)編隊(duì)比較麻煩。

本文提出在一個(gè)隊(duì)形結(jié)構(gòu)中設(shè)置“虛擬領(lǐng)航者”(Virtual leader )的角色，將實(shí)際環(huán)境下多個(gè)跟隨者機(jī)器人直接跟隨領(lǐng)航者機(jī)器人的問題轉(zhuǎn)化為每個(gè)機(jī)器人分別跟隨其“虛擬領(lǐng)航者”的問題，使得編隊(duì)控制變得比較簡單，并且在文獻(xiàn)[13-14]的基礎(chǔ)上，針對(duì)跟隨者機(jī)器人RFollower設(shè)計(jì)了速度控制函數(shù)vfc=[vfcωfc]T。通過計(jì)算，在控制器公式中得到兩個(gè)新的附加項(xiàng)ηvfc和ηωfc。這兩個(gè)附加項(xiàng)的引入，能夠有效保證在領(lǐng)航者機(jī)器人角速度ωl≠0的情況下跟隨者機(jī)器人的隊(duì)形保持控制器的穩(wěn)定性。

1編隊(duì)控制策略

在多機(jī)器人系統(tǒng)控制中，編隊(duì)控制是一個(gè)研究熱點(diǎn)。多機(jī)器人系統(tǒng)的編隊(duì)控制可以轉(zhuǎn)化為兩個(gè)鄰近機(jī)器人存在局部領(lǐng)航與跟隨的關(guān)系，即跟隨機(jī)器人與其領(lǐng)航者保持一定距離與角度的關(guān)系。領(lǐng)航機(jī)器人的位姿確定后，跟隨機(jī)器人的目標(biāo)位姿也將被確定。

1.1機(jī)器人數(shù)學(xué)模型

考慮一組在全局坐標(biāo)系下的非完整移動(dòng)機(jī)器人，可假設(shè)系統(tǒng)中的每一個(gè)機(jī)器人都具有相同的運(yùn)動(dòng)模型(見圖1)，第i(1≤i≤n)個(gè)機(jī)器人在笛卡爾框架下的位姿被定義為Pi=[xiyiθi]T。其中，(xi,yi)是Ri的位置坐標(biāo)，θi是其航向角度。

圖1　單個(gè)非完整移動(dòng)機(jī)器人

1.2領(lǐng)航-跟隨結(jié)構(gòu)隊(duì)形控制

為了描述多機(jī)器人系統(tǒng)中各個(gè)機(jī)器人在隊(duì)形結(jié)構(gòu)中的位置，以兩個(gè)機(jī)器人為例來說明本文所采用的領(lǐng)航者-跟隨者隊(duì)形結(jié)構(gòu)的幾何模型。假設(shè)任意的跟隨者機(jī)器人RFollower，其跟隨的領(lǐng)航者機(jī)器人為RLeader，則本文采用的領(lǐng)航者-跟隨者隊(duì)形的結(jié)構(gòu)模型如圖2所示。其中，領(lǐng)航者機(jī)器人線速度用VLeader來表示，線速度與水平方向的夾角用θLeader來表示。

圖2　領(lǐng)航者-跟隨者機(jī)器人結(jié)構(gòu)隊(duì)形模型設(shè)計(jì)

系統(tǒng)中每個(gè)跟隨者機(jī)器人都有一個(gè)與之對(duì)應(yīng)的“虛擬跟隨者機(jī)器人”。本文介紹的多機(jī)器人合作編隊(duì)控制的方法是通過為RFollower設(shè)計(jì)適當(dāng)?shù)目刂扑惴ǎ蛊淇梢酝瓿蓪?shí)時(shí)跟蹤“虛擬跟隨者機(jī)器人”位姿qv，達(dá)到所期望的位姿，最終形成多機(jī)器人編隊(duì)系統(tǒng)所期望的結(jié)構(gòu)隊(duì)形。

2運(yùn)動(dòng)學(xué)控制器的設(shè)計(jì)

根據(jù)圖2給出的領(lǐng)航者-跟隨者結(jié)構(gòu)隊(duì)形控制模型，在領(lǐng)航者機(jī)器人RFollower和“虛擬跟隨者機(jī)器人”RVirtual之間建立期望的隊(duì)列結(jié)構(gòu)，將相對(duì)復(fù)雜的隊(duì)形控制問題轉(zhuǎn)化為簡單的跟蹤問題來研究。利用跟隨者機(jī)器人的實(shí)時(shí)位姿和相對(duì)應(yīng)的“虛擬跟隨者機(jī)器人”的位姿，建立隊(duì)形誤差方程，并在此基礎(chǔ)上設(shè)計(jì)運(yùn)動(dòng)學(xué)控制器，從而實(shí)現(xiàn)令隊(duì)形中的每個(gè)跟隨者機(jī)器人都收斂于其期望位姿，達(dá)到隊(duì)形控制的目的。

領(lǐng)航者機(jī)器人RLeader的數(shù)學(xué)模型設(shè)為：

(1)

其中，vl=[vlωl]T是領(lǐng)航者機(jī)器人RLeader的實(shí)時(shí)線速度和角速度，設(shè)任意時(shí)刻下均有vl≥0?？梢愿鶕?jù)領(lǐng)航者機(jī)器人RLeader的確定位姿和預(yù)期結(jié)構(gòu)隊(duì)形Fd，得到“虛擬跟隨者機(jī)器人”RVirtual的位姿:

(2)

領(lǐng)航者機(jī)器人RLeader與“虛擬跟隨者機(jī)器人”RVirtual之間的相對(duì)位姿誤差可定義為：

(3)

這里，[xfyfθf]T表示跟隨者機(jī)器人RVirtual的實(shí)際位置和方向，其公式為：

(4)

上述相對(duì)位姿誤差是在全局坐標(biāo)系下得到的，為了便于實(shí)際中對(duì)各機(jī)器人的控制，有必要對(duì)相對(duì)位姿誤差進(jìn)行坐標(biāo)轉(zhuǎn)換，得到在跟隨者機(jī)器人RFollower的坐標(biāo)系下的隊(duì)形跟蹤誤差。由圖2中二者的相對(duì)位姿投影關(guān)系可得：

(5)

Te是轉(zhuǎn)移矩陣。將式(2)、式(3)、式(4)代入式(5)，并利用三角函數(shù)公式計(jì)算和簡化，可得：

(6)

(7)

(8)

(9)

其中，kx、ky、kθ為正常量。與文獻(xiàn)[10]中所設(shè)計(jì)的跟蹤控制器相比，在上述控制器中可以觀察到兩個(gè)新引入的附加項(xiàng)ηvfc和ηωfc。相對(duì)于單個(gè)機(jī)器人運(yùn)動(dòng)控制中的目標(biāo)跟蹤控制來說，這兩個(gè)附加項(xiàng)的引入能夠有效地保證在領(lǐng)航者機(jī)器人RLeader角速度ωl≠0的情況下RFollower隊(duì)形保持控制器的穩(wěn)定性。

3仿真及實(shí)驗(yàn)驗(yàn)證

為了充分驗(yàn)證本文所提出的運(yùn)動(dòng)學(xué)控制器的有效性，采用MATLAB軟件實(shí)現(xiàn)了多機(jī)器人隊(duì)形形成的仿真實(shí)驗(yàn)。以柱形隊(duì)形和三角形隊(duì)形為期望隊(duì)形，分別令領(lǐng)航者機(jī)器人按照直線、圓弧及正弦曲線行進(jìn)；在實(shí)驗(yàn)方面，采用HR-I型雙輪差速移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)進(jìn)行驗(yàn)證。

3.1仿真驗(yàn)證

各機(jī)器人初始的位姿可任意指定。設(shè)定Leader機(jī)器人的初始位姿為qL=[000]T，F(xiàn)ollower1機(jī)器人和Follower2機(jī)器人的初始位姿為qF1=[-2-1π/6]T。式(8)和式(9)中的位置增益向量選定為：K=[kxkykθ]T=[13.53.5]T。圖3和圖4分別為柱形隊(duì)形和三角形對(duì)形的仿真結(jié)果。

(a)直線軌跡時(shí)柱形隊(duì)形形成　　　　　　　　(b)圓弧軌跡時(shí)柱形隊(duì)形形成

(c)正弦軌跡時(shí)柱形隊(duì)形形成 圖3　柱形隊(duì)形形成仿真過程

3.2實(shí)驗(yàn)驗(yàn)證

3.2.1領(lǐng)航者機(jī)器人Leader做直線運(yùn)動(dòng)

本文利用HR-I型雙輪差速移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)進(jìn)行了幾組實(shí)驗(yàn)調(diào)試。首先選取領(lǐng)航者機(jī)器人以直線作為移動(dòng)軌跡的情形進(jìn)行編隊(duì)實(shí)驗(yàn)，期望形成的隊(duì)形結(jié)構(gòu)則選取具有代表性的三角形隊(duì)形和柱狀隊(duì)形，對(duì)應(yīng)的視頻序列如圖5(a)和(b)所示。這兩種隊(duì)形對(duì)應(yīng)的位姿參數(shù)分別為：

該實(shí)驗(yàn)過程中領(lǐng)航者機(jī)器人的運(yùn)動(dòng)速度為：

(a)直線軌跡時(shí)三角形隊(duì)形形成　　　　　　　　(b)圓弧軌跡時(shí)三角形隊(duì)形形成

(c)正弦軌跡時(shí)三角形隊(duì)形形成 圖4　三角形隊(duì)形形成仿真過程

(a)等邊三角形編隊(duì)隊(duì)形形成實(shí)驗(yàn)過程

(b)柱形編隊(duì)隊(duì)形形成實(shí)驗(yàn)過程 圖5　HR-I移動(dòng)機(jī)器人平臺(tái)沿直線行走隊(duì)形形成的視頻畫面

兩個(gè)跟隨者機(jī)器人的運(yùn)動(dòng)速度約束為：

3.2.2領(lǐng)航者機(jī)器人Leader做圓周運(yùn)動(dòng)

為驗(yàn)證這種編隊(duì)控制方法對(duì)領(lǐng)航者機(jī)器人作復(fù)雜運(yùn)動(dòng)時(shí)的適應(yīng)能力，本文選取領(lǐng)航者機(jī)器人以圓弧運(yùn)動(dòng)作為移動(dòng)軌跡的情形進(jìn)行編隊(duì)實(shí)驗(yàn)。該實(shí)驗(yàn)視頻序列如圖6(a)和(b)所示。實(shí)驗(yàn)過程所對(duì)應(yīng)的位姿參數(shù)分別為：

該實(shí)驗(yàn)過程中領(lǐng)航者機(jī)器人的運(yùn)動(dòng)速度為：

兩個(gè)跟隨者機(jī)器人的運(yùn)動(dòng)速度約束為：

(a)形成并保持三角形隊(duì)形實(shí)驗(yàn)過程

(b)形成并保持柱狀隊(duì)形實(shí)驗(yàn)過程 圖6　HR-I移動(dòng)機(jī)器人平臺(tái)沿圓弧軌跡行走隊(duì)形形成的視頻畫面

4結(jié)語

(1)在MATLAB仿真軟件中，各個(gè)機(jī)器人可以從任意的初始狀態(tài)出發(fā)形成指定的期望結(jié)構(gòu)隊(duì)形，并且各機(jī)器人在隊(duì)形保持及位姿跟蹤方面具有很好的性能，驗(yàn)證了本文設(shè)計(jì)的編隊(duì)控制方法的有效性。

(2)在HR-I型機(jī)器人平臺(tái)上，領(lǐng)航者機(jī)器人分別沿直線和圓弧兩組不同運(yùn)動(dòng)軌跡的隊(duì)形形成實(shí)驗(yàn)，證明本算法對(duì)領(lǐng)航者機(jī)器人的運(yùn)動(dòng)軌跡具有很好的適應(yīng)性及實(shí)用性。

參考文獻(xiàn)：

[1]譚民, 王碩,曹志強(qiáng).多機(jī)器人系統(tǒng)[M].北京:清華大學(xué)出版社, 2005: 120-124.

[2]馮劉中,肖世德,司徒渝,等.基于雙移動(dòng)信標(biāo)的多機(jī)器人編隊(duì)控制算法[J].信息與控制, 2011, 40(2): 145-159.

[3]仰曉芳,倪建軍.基于生物刺激神經(jīng)網(wǎng)絡(luò)的多機(jī)器人編隊(duì)方法[J]. 計(jì)算機(jī)應(yīng)用, 2013, 33(5): 1298-1300,1304.

[4]Ghommam J, Mehrjerdi H, Saad M. Leader-follower Formation Control of Nonholonomic Robots with Fuzzy Logic Based Approach for Obstacle Avoidance[C]//International Conference on Intelligent Robots and Systems．Piscataway : IEEE, 2011: 2340-2345．

[5]Yuan J, Tang G Y. Formation Control for Mobile Multiple Robots Based on Hierarchical Virtual Structures[C]//IEEE International Conference on Control and Automation. Piscataway: IEEE, 2010: 393-398．

[6]張捍東,黃鸝,岑豫皖.改進(jìn)的多移動(dòng)機(jī)器人混合編隊(duì)方法[J].計(jì)算機(jī)應(yīng)用, 2012， 32(7): 1955 -1964.

[7]Balch T, Arkin R C.Behavior-based Formation Control for Multi-robot Teams[J].IEEE Transaction on Robotics and Automation, 1998, 14(6): 926-939.

[8]Das A K, Fierro R,Kumar V,et al. A Vision-based formation Control Framework[J].IEEE Transaction on Robotics and Automation, 2002, 18(5): 813-825.

[9]么洪飛,李艷東.多移動(dòng)機(jī)器人的領(lǐng)航者模式編隊(duì)控制[J].計(jì)算機(jī)仿真, 2014, 31(7): 330-333, 350.

[10]Desai J P,Ostrowski J P，Kumar V.Controlling Formations of Multiple Mobile Robot [C]//Proceeding of the International Conference on Robotics and Automation, Leuven, Belgium, USA: IEEE, 1998: 2864-2869.

[11]Desai J P, Ostrowski J P,Kumar V.Modeling and Control of Formations of Nonholonomic Mobile Robot[C] // Proceeding of the International Conference on Robotics and Automation, 2001, 17(6): 905-908.

[12]Fujimori A, Fujimoto T, Gabor B. Distributed Leader-follower Navigation of Mobile Robots[C]//Proceeding of the 5th International Conference on Control and Automation. Budapest: Institute of Electrical and Electronics Engineers Inc, 2005: 960-965.

[13]Dierks T, Jagannathan S. Control of Nonholonomic Mobile Robot Formations: Back-stepping Kinematics into Dynamics[C]//Proceeding of the IEEE International Conference on Control Application, Part of the IEEE Multi-conference on Systems and Control,USA: IEEE, 2007:94-99.

[14]Kanayama Y, Kimura Y， Miyazaki F， et al. A Stable Tracking Control Method for an Autonomous Mobile Robot [C]//Proceeding of the IEEE International Conference on Robotics and Automation. USA: IEEE, 1990:384-389.

(責(zé)任編輯：王長通)

A Sort of Control Strategy and Implementation

Based on Multi-robot Formation

SHANG Hui-chao, CHEN Mao-qian

(Zhongyuan University of Technology, Zhengzhou 450007, China)

Key words:multiple robots; formation; tracking control; MATLAB simulation