多機(jī)器人領(lǐng)航-跟隨型編隊(duì)控制

師五喜，王棟偉，李寶全

（天津工業(yè)大學(xué)電氣工程與自動化學(xué)院，天津 300387）

在實(shí)際工程應(yīng)用中，由于單個(gè)智能體自身能力的局限性，在處理復(fù)雜任務(wù)或處于復(fù)雜環(huán)境下僅靠個(gè)體行為往往無法解決.人們受到自然界中魚群、鳥群等生物種群協(xié)作完成捕食、遷移等生物集群協(xié)作行為的啟發(fā)，將這種生物集群編隊(duì)協(xié)作的特點(diǎn)運(yùn)用到多智能體系統(tǒng)中.目前，編隊(duì)運(yùn)動問題已在很多領(lǐng)域得到應(yīng)用，例如在工業(yè)領(lǐng)域，人們控制多個(gè)移動機(jī)器人以特定隊(duì)形搬運(yùn)大的物體；在軍事上，多個(gè)自主式小車被用于編隊(duì)巡邏或偵察；在警務(wù)領(lǐng)域，人們控制多個(gè)移動機(jī)器人組成弧形包圍或捕獲入侵者等.

多機(jī)器人編隊(duì)控制是目前國內(nèi)外研究的熱門課題之一.多移動機(jī)器人的編隊(duì)運(yùn)動，要求多個(gè)移動機(jī)器人作為一個(gè)編隊(duì)同時(shí)運(yùn)動到目標(biāo)區(qū)域，并在運(yùn)動過程中始終保持給定隊(duì)形，同時(shí)能安全地避開可能出現(xiàn)的障礙物.這種群體行為控制是解決許多機(jī)器人協(xié)作問題的基礎(chǔ)，它對實(shí)現(xiàn)多機(jī)器人在分布式空間環(huán)境中協(xié)同執(zhí)行任務(wù)具有重要意義.編隊(duì)控制方法有領(lǐng)航-跟隨（leader-follower）法[1-5]、虛擬結(jié)構(gòu)（virtual structure）法[6]、基于行為（behavior-based）的方法[7]、人工勢場法[8]等.文獻(xiàn)[1]提出了適用于2個(gè)機(jī)器人的距離-方位-方向控制方法和適用于3個(gè)機(jī)器人的距離-距離-方向控制方法.文獻(xiàn)[2]用自適應(yīng)控制方法，研究了有兩個(gè)領(lǐng)導(dǎo)者，其余均為跟隨者的編隊(duì)控制方法.文獻(xiàn)[3]提出了一種新的針對領(lǐng)航-跟隨系統(tǒng)的運(yùn)動學(xué)模型，該模型基于笛卡爾坐標(biāo).文獻(xiàn)[4]針對車式移動機(jī)器人設(shè)計(jì)了一種領(lǐng)航跟隨型編隊(duì)控制算法.文獻(xiàn)[5]通過設(shè)計(jì)模糊控制器，采用領(lǐng)航-跟隨法實(shí)現(xiàn)了多機(jī)器人的編隊(duì)控制.文獻(xiàn)[6]結(jié)合人工勢場和虛擬結(jié)構(gòu)法，提出了一種針對多AUV的編隊(duì)控制算法.文獻(xiàn)[7]提出了一種基于行為法的機(jī)器人編隊(duì)控制方法.文獻(xiàn)[8]所采用的基于行為的方法可實(shí)現(xiàn)分布式控制，但此方法使隊(duì)形控制的穩(wěn)定性較差.文獻(xiàn)[9]提出了一種基于行為法和領(lǐng)航-跟隨法相結(jié)合的混合式編隊(duì)算法，實(shí)現(xiàn)了對機(jī)器人的編隊(duì)控制.文獻(xiàn)[10]提出了一種基于滑?？刂频臋C(jī)器人編隊(duì)算法.文獻(xiàn)[11]綜合虛擬結(jié)構(gòu)法和基于行為的方法，提出了一種帶隊(duì)形反饋的分布式編隊(duì)控制算法.文獻(xiàn)[12]設(shè)計(jì)了一套基于機(jī)器人動力學(xué)模型的編隊(duì)控制算法.

以上算法雖然實(shí)現(xiàn)了編隊(duì)控制的目的，但在編隊(duì)運(yùn)動過程中，對機(jī)器人速度和加速度大小沒有限制.而在實(shí)際應(yīng)用中，機(jī)器人本身對速度和加速度的大小有一定的限制，當(dāng)速度和加速度大小超過機(jī)器人的截?cái)嗨俣葧r(shí)，機(jī)器人只會以自己允許的最大速度運(yùn)行，這樣在編隊(duì)運(yùn)動過程中，跟隨機(jī)器人便不能準(zhǔn)確地跟隨領(lǐng)航者機(jī)器人，完成編隊(duì)任務(wù).文獻(xiàn)[13]設(shè)計(jì)了一種具有速度與加速度限制的軌跡跟蹤控制算法，該算法通過在原軌跡跟蹤算法的基礎(chǔ)上，增加了速度反饋，從而保證了機(jī)器人輸入速度和加速度的有界性，但是該算法只是在單個(gè)機(jī)器人上實(shí)現(xiàn)了速度與加速度的限制，在機(jī)器人編隊(duì)控制系統(tǒng)中，不僅僅要考慮到對領(lǐng)航者機(jī)器人的速度和加速度限制，而且對于跟隨者機(jī)器人的速度和加速度大小也需要考慮在內(nèi).本文在文獻(xiàn)[13]的基礎(chǔ)上提出了具有速度和加速度限制的編隊(duì)控制算法.通過引入2個(gè)一階濾波器作為編隊(duì)控制器的反饋信號，使得機(jī)器人的輸入速度和加速度大小均有界.由于該反饋信號及其時(shí)間導(dǎo)數(shù)的大小完全受限于2個(gè)濾波器參數(shù)，因此可以通過調(diào)整2個(gè)濾波器參數(shù)的大小來調(diào)整機(jī)器人輸入速度和加速度的大小.文中通過李雅普諾夫理論證明了閉環(huán)系統(tǒng)的穩(wěn)定性，且通過仿真和在3個(gè)輪式移動機(jī)器人組成的編隊(duì)平臺上驗(yàn)證了本文方法的有效性.

1 機(jī)器人模型及問題描述

采用3個(gè)輪式移動機(jī)器人為研究對象，其中1個(gè)為領(lǐng)航者，另外2個(gè)為跟隨者.對于領(lǐng)航者的移動機(jī)器人，其運(yùn)動學(xué)模型只根據(jù)自身的位姿信息確定，而對于作為跟隨者的移動機(jī)器人，其運(yùn)動學(xué)模型建立在與領(lǐng)航者的位姿信息基礎(chǔ)之上，通過與領(lǐng)航者的相對位置和角度信息來間接描述其自身的位姿信息.

1.1 領(lǐng)航者運(yùn)動學(xué)模型

領(lǐng)航者機(jī)器人模型如圖1所示.

圖1 領(lǐng)航者機(jī)器人模型Fig.1 Leader robot model

假設(shè)領(lǐng)航者機(jī)器人在世界坐標(biāo)系中的位姿為（x，y，θ），其中：（x，y）為領(lǐng)航者機(jī)器人的位置坐標(biāo)；θ為機(jī)器人在世界坐標(biāo)系中的方向角；v（t）與w（t）分別為機(jī)器人的線速度與角速度，則機(jī)器人的運(yùn)動學(xué)模型為：

1.2 跟隨者運(yùn)動學(xué)模型

為了描述多機(jī)器人系統(tǒng)中各個(gè)機(jī)器人在隊(duì)形結(jié)構(gòu)中的位置，以2個(gè)機(jī)器人為例來說明本文所采用的領(lǐng)航-跟隨者隊(duì)形結(jié)構(gòu)的幾何模型.假設(shè)任意的跟隨者機(jī)器人RF，其跟隨的領(lǐng)航者機(jī)器人為RL，則本文采用的領(lǐng)航-跟隨者隊(duì)形結(jié)構(gòu)模型如圖2所示.

圖2 領(lǐng)航-跟隨者隊(duì)形結(jié)構(gòu)模型Fig.2 Pilot follower formation structure model

圖2中，領(lǐng)航者機(jī)器人的線速度用vL來表示，角速度用wL來表示，線速度與水平方向的夾角用θL來表示.跟隨者機(jī)器人的線速度用vF來表示，角速度用wF來表示，線速度與水平方向的夾角用θF來表示.以機(jī)器人兩輪軸心連線的中點(diǎn)為參考點(diǎn)，則2個(gè)機(jī)器人參考點(diǎn)之間的距離為λL-F，領(lǐng)航者機(jī)器人前進(jìn)方向與2個(gè)機(jī)器人參考點(diǎn)連線的夾角為φL-F.

對于一個(gè)領(lǐng)航-跟隨編隊(duì)系統(tǒng)，編隊(duì)的主軌跡通常由領(lǐng)航者機(jī)器人決定，跟隨機(jī)器人的參考軌跡由領(lǐng)航機(jī)器人生成的虛擬機(jī)器人軌跡決定[4].假設(shè)跟隨者機(jī)器人與領(lǐng)航者機(jī)器人最終達(dá)到的距離與角度分別為和，那么虛擬機(jī)器人與領(lǐng)航者機(jī)器人之間的距離和角度分別為和.在世界坐標(biāo)系X-Y中，虛擬機(jī)器人與領(lǐng)航者之間的位置關(guān)系為：

跟隨者與領(lǐng)航者之間的位置關(guān)系為：

虛擬機(jī)器人與跟隨者之間的表達(dá)式為：

通過轉(zhuǎn)移矩陣，將其轉(zhuǎn)換到跟隨者機(jī)器人RF自身的坐標(biāo)系xF-yF下的誤差表達(dá)式為：

將式（3）和式（4）帶入到式（5）中可得

對式（6）兩邊求導(dǎo)可得[12]：

至此，機(jī)器人編隊(duì)控制問題轉(zhuǎn)化為跟隨機(jī)器人RF對虛擬機(jī)器人RV的軌跡跟蹤問題，即尋找合適的控制律（vFωF）使得式（7）描述的閉環(huán)系統(tǒng)漸近穩(wěn)定，且

式中：vc、wc、av、aw均為正的常量.

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

文獻(xiàn)[13]通過在原軌跡跟蹤算法的基礎(chǔ)上，添加了2個(gè)一階濾波器，作為輸入速度的反饋信號，從而使得機(jī)器人的輸入速度和加速度有界，且其界限可以通過調(diào)整濾波器參數(shù)來調(diào)整.本文通過借鑒文獻(xiàn)[13]的設(shè)計(jì)方法，通過對機(jī)器人編隊(duì)控制算法進(jìn)行調(diào)整，設(shè)計(jì)控制器如下：

其中k為正反饋參數(shù)，

并且φ1和φ2分別為一階濾波器產(chǎn)生的反饋函數(shù)：

其中α1和α2均為正濾波參數(shù)，且φ1（0）=φ2（0）=0.注1：與文獻(xiàn)[13]所設(shè)計(jì)的針對單個(gè)機(jī)器人的軌跡跟蹤算法相比，上述控制器引入了2個(gè)附加項(xiàng)γvF和γwF.對于基于運(yùn)動學(xué)的多機(jī)器人編隊(duì)控制算法，這2個(gè)附加項(xiàng)的引入保證了編隊(duì)控制算法的穩(wěn)定性.

定理1對于式（7）描述誤差動態(tài)系統(tǒng)，設(shè)計(jì)控制律如式（9）和（10），自適應(yīng)律采用式（13），則當(dāng) t→∞時(shí)，ex，ey，eθ均收斂于 0，并且控制律（vFωF）滿足式（8）.

為證明以上定理，本文引入如下引理：

引理1[13]：如果一個(gè)有界的標(biāo)量函數(shù)z（t）滿足當(dāng)t→∞ 時(shí)，z→0并且，其中 （ft）是有界的并且一致連續(xù)的，那么當(dāng)t→∞時(shí)，并且f（t）→0.

證明：選取李雅普諾夫函數(shù)為：

由式（7）、（9）、（10）、（11）、（13）可得

將式（12）代入式（15）可得：

由式（7）和式（10）可得

所以當(dāng)ey≥ 0，

當(dāng)ey＜ 0，

同理可得，當(dāng) t→∞，ey→0.

當(dāng) t→∞ 時(shí)，sin eθ→0，cos eθ可能收斂于 1 或者-1.由于 V（t）是非增的，假設(shè) t→∞ 時(shí)，cos eθ→-1，則V（∞）→2/k.由于cos eθ（0）≠-1.所以當(dāng)k取足夠小時(shí)，總能滿足 V（0）＜ 2/k，即 V（0）＜ V（∞）.這與 V（t）是非增的矛盾.因此當(dāng)t→∞時(shí)，cos eθ→1.綜上所述，當(dāng) t→∞ 時(shí)，ex，ey，eθ將收斂于 0.

下面先證明機(jī)器人輸入速度和加速度的有界性.

由式（13）可知

由式（13）、式（19）、式（20）可知：

所以φ1和φ2是有界并且一致連續(xù)的.

由式（9）和式（19）可得

由式（10）和式（20）可得

由式（9）得

由（17）得

將式（25）代入到式（24）中得

由式（10）得

當(dāng)ey≠ 0時(shí)，式（27）可以寫為：

由于

其中由式（8）和式（23）可得

將式（29）代入式（28）可得

當(dāng)ey=0時(shí)

綜上所述

所以控制律（vFωF）滿足式（8）.

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

3.1 仿真

仿真中以3個(gè)機(jī)器人組成三角陣型隊(duì)列為例進(jìn)行驗(yàn)證，根據(jù)領(lǐng)航者走出的2種不同軌跡，分別進(jìn)行了仿真，并且在仿真實(shí)驗(yàn)1中進(jìn)行了速度與加速度有界性驗(yàn)證.仿真實(shí)驗(yàn)中，設(shè)定

vc=0.65 m/s，wc=1.2 r/s

ac=1.12 m/s2，aw=0.71 r/s2

2個(gè)跟隨機(jī)器人與領(lǐng)航機(jī)器人最終達(dá)到的距離和角度分別為

仿真1：3個(gè)機(jī)器人從初始位置先完成三角陣型編隊(duì)，然后保持編隊(duì)做橢圓形運(yùn)動.設(shè)領(lǐng)航者機(jī)器人的速度為（vL，wL）=（0.1，0.1），通過式（22）、（23）、（26）、（32）可以算得 α1=0.45，α2=0.5，k=3.0.3 個(gè)機(jī)器人的運(yùn)動軌跡如圖3所示，圖3中跟隨者1的運(yùn)動軌跡為綠色虛線，跟隨者2的運(yùn)動軌跡為藍(lán)色虛線.

圖4所示為跟隨者2的線速度、角速度、線加速度和角加速度.

圖4 跟隨者2的速度和加速度Fig.4 Speed and acceleration of follower 2

由圖4可知，其速度和加速度大小小于設(shè)定的速度與加速度上限.

仿真2：3個(gè)機(jī)器人從初始位置先完成三角陣型編隊(duì)，然后保持編隊(duì)做直線運(yùn)動.設(shè)領(lǐng)航者機(jī)器人的速度為（vL，wL）=（0.1，0），根據(jù)式（22）、式（23）、式（26）、式（32）選定參數(shù) α1=0.55，α2=0.1，k=1.0，運(yùn)動軌跡如圖5所示.

圖5 直線形編隊(duì)軌跡Fig.5 Linear formation trajectory

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

以3個(gè)機(jī)器人組成三角形編隊(duì)為例，根據(jù)領(lǐng)航者機(jī)器人運(yùn)行的軌跡不同，進(jìn)行了2次驗(yàn)證.

本文的實(shí)驗(yàn)平臺為3輛小型輪式移動機(jī)器人，如圖6所示.

圖6 實(shí)驗(yàn)平臺Fig.6 Experiment platform

小車以stm32f103zet6為處理器，2個(gè)直流電機(jī)為驅(qū)動輪，用1個(gè)萬向輪來保持小車的平衡，3輛小車通過Zigbee模塊實(shí)現(xiàn)通信，使得跟隨者機(jī)器人能夠?qū)崟r(shí)的獲得領(lǐng)航者的位姿信息，從而實(shí)現(xiàn)實(shí)時(shí)跟蹤.

對于小車的電機(jī)控制，一方面實(shí)驗(yàn)要求能夠精確的測出小車的運(yùn)行速度.因此，需要通過編碼器測出電機(jī)的轉(zhuǎn)速.另一方面，電機(jī)要能夠精確的實(shí)現(xiàn)給定的速度值，因此，小車的電機(jī)控制部分采用雙閉環(huán)PID調(diào)速控制.

本次實(shí)驗(yàn)需要領(lǐng)航者將位姿信息實(shí)時(shí)地傳遞給2個(gè)跟隨者，因此需要采用通信模塊.通信采用Zigbee透傳功能，設(shè)定領(lǐng)航者的Zigbee模塊為協(xié)調(diào)器，跟隨者的Zigbee模塊為路由器，協(xié)調(diào)器只負(fù)責(zé)發(fā)送信息，路由器只負(fù)責(zé)接受協(xié)調(diào)器發(fā)送的信息，因此，2個(gè)跟隨者只能接收領(lǐng)航者發(fā)送的信息，并且相互之間沒有影響.使得跟隨者能夠準(zhǔn)確地，實(shí)時(shí)地接收領(lǐng)航者的位姿信息.

實(shí)驗(yàn)1：3個(gè)機(jī)器人從初始位置先完成三角陣型編隊(duì)，然后保持編隊(duì)做橢圓形運(yùn)動.設(shè)領(lǐng)航者機(jī)器人的運(yùn)行速度，初始位姿以及與跟隨者之間期望的位姿都跟仿真1相同.通過串口采集機(jī)器人的運(yùn)動數(shù)據(jù)，則3個(gè)編隊(duì)機(jī)器人的運(yùn)行軌跡如圖7所示.最后，2個(gè)跟隨者機(jī)器人與領(lǐng)航者機(jī)器人組成三角陣型，并且保持三角陣型做橢圓形運(yùn)動.

圖7 組成三角陣型并且走出橢圓形軌跡Fig.7 Triangle formation and elliptical trajectory

實(shí)驗(yàn)2：3個(gè)機(jī)器人從初始位置先完成三角陣型編隊(duì)，然后保持編隊(duì)做直線運(yùn)動.設(shè)領(lǐng)航者機(jī)器人的運(yùn)動速度，初始位姿以及與跟隨者之間的期望的位姿與仿真2相同.則3個(gè)機(jī)器人的運(yùn)行軌跡如圖8所示，3個(gè)機(jī)器人組成三角陣型并且走出直線軌跡.

圖8 組成三角陣型并且走出直線軌跡Fig.8 Triangle formation and straight trajectory

4 結(jié)論

本文提出了一種具有速度和加速度限制的編隊(duì)控制算法，通過加入2個(gè)一階濾波器，在編隊(duì)運(yùn)動控制器的基礎(chǔ)上加入了有界的反饋信號.研究結(jié)果表明：

（1）通過設(shè)定濾波器參數(shù)α1，α2的大小，可以設(shè)定編隊(duì)機(jī)器人運(yùn)行速度和加速度的最大值vm和am.

（2）文中證明了閉環(huán)系統(tǒng)的穩(wěn)定性，仿真和實(shí)驗(yàn)結(jié)果也表明了.編隊(duì)機(jī)器人可以穩(wěn)定的組成設(shè)定的隊(duì)形并且保持隊(duì)形運(yùn)動，且其運(yùn)行速度和加速度均在設(shè)定的范圍內(nèi)，即當(dāng) t→∞ 時(shí)，位姿誤差 ex，ey，eθ將收斂于0.且v＜ vm，a＜ am.

實(shí)驗(yàn)中以3個(gè)機(jī)器人組成的編隊(duì)系統(tǒng)為研究對象，而實(shí)際應(yīng)用中，經(jīng)常需要更多的機(jī)器人來完成編隊(duì).本文研究成果不僅可以應(yīng)用于3個(gè)機(jī)器人組成的編隊(duì)系統(tǒng)，對于更多機(jī)器人組成的編隊(duì)系統(tǒng)也同樣適用.

參考文獻(xiàn)：

[1]CHEN J，SUN D，YANG J，et al.Leader-follower formation control of multiple non-holomoic mobile robots incorporating a receding-horizon scheme[J].International Journal of Robotics Research，2010，29（6）：727-747.

[2]GUO J，LIN Z，CAO M，et al.Adaptive leader-follower formationcontrolforautonomousmobilerobots[C]//Baltimore，MD，USA：AmericanControlConference.IEEEXplore，2010：6822-6827.

[3]LI X，XIAO J，CAI Z.Backstepping based multiple mobile robots formation control[C]//Edmonton，Alta，Canada：IEEE/RSJ International Conference on Intelligent Robots and Systems.IEEEXplore，2005：887-892.

[4]王保防，張瑞雷，李勝，等.基于軌跡跟蹤車式移動機(jī)器人編隊(duì)控制[J].控制與決策，2015（1）：176-180.

WANG B F，ZHANG R L，LI S，et al.Formation control of vehicle based mobile robot based on trajectory tracking[J].Control and Decision，2015（1）：176-180（in Chinese）.

[5]吳垠，劉忠信，陳增強(qiáng)，等.一種基于模糊方法的領(lǐng)導(dǎo)-跟隨型多機(jī)器人編隊(duì)控制[J].智能系統(tǒng)學(xué)報(bào)，2015（4）：533-540.

WU Y，LIU Z X，CHEN Z Q，et al.A leader follower multi robot formation control method based on fuzzy method[J].Journal of Intelligent Systems，2015（4）：533-540（in Chinese）.

[6]潘無為，姜大鵬，龐永杰，等.人工勢場和虛擬結(jié)構(gòu)相結(jié)合的多水下機(jī)器人編隊(duì)控制[J].兵工學(xué)報(bào)，2017，38（2）：326-334.

PANG W W，JIANG D P，PANG Y J，et al.Formation control of multi underwater robots based on combination of artificial potential field and virtual structure[J].Acta Armamentarii，2017，38（2）：326-334（in Chinese）.

[7]BALCH T，ARKIN R C.Behavior-based formation control for multirobot teams[J].IEEE Transactions on Robotics&Automation，1999，14（6）：926-939.

[8]YUAN J，TANG G Y.Formation control for mobile multiple robots based on hierarchical virtual structures[C]//IEEE International Conference on Control and Automation.Xiamen：IEEEXplore，2010：393-398.

[9]張捍東，黃鸝，岑豫皖.改進(jìn)的多移動機(jī)器人混合編隊(duì)方法[J].計(jì)算機(jī)應(yīng)用，2012，32（7）：1955-1957.ZHANG H D，HUANG L，CEN Y W.Improved hybrid formation method for multiple mobile robots[J].Computer Applications，2012，32（7）：1955-1957（in Chinese）.

[10]SANCHEZ J，F(xiàn)IERRO R.Sliding mode control for robot formations[C]//IEEE International Symposium on Intelligent Control.Houston：IEEE，2003：438-443.

[11]REN W，BEARD R.Decentralized scheme for spacecraft formation flying via the virtual structure approach[J].Journal of Guidance Control&Dynamics，2012，2（1）：1746-1751.

[12]DIERKS T，JAGANNATHAN S.Control of nonholonomic mobile robot formations：Backstepping kinematics into dynamics[C]//IEEE International Conference on Control Applications.Singapore：IEEE，2007：94-99.

[13]CHEN X，JIA Y，MATSUNO F.Tracking control of nonholonomic mobile robots with velocity and acceleration constraints[C]//AmericanControlConference.Portland：IEEE，2014：880-884.

[14]朱大奇，杜青.基于領(lǐng)航位置信息的AUV三維編隊(duì)控制方法[J].系統(tǒng)仿真技術(shù)，2013，9（3）：193-198.ZHU D Q，DU Q.Three dimensional formation control method of AUV based on navigation position information [J].System Simulation Technology，2013，9（3）：193-198（in Chinese）.

[15]DIERKS T，JAGANNATHAN S.Control of nonholonomic mobile robot formations：Backstepping kinematics into dynamics[C]//IEEE International Conference on Control Applications.Singapore：IEEE，2007：94-99.

[16]王中林，劉忠信，陳增強(qiáng)，等.一種多智能體領(lǐng)航跟隨編隊(duì)新型控制器的設(shè)計(jì)[J].智能系統(tǒng)學(xué)報(bào)，2014（3）：298-306.WANG Z L，LIU Z X，CHEN Z Q，et al.Design of a new multi-agent navigation following formation controller[J].Journal of Intelligent Systems，2014（3）：298-306（in Chinese）.