師五喜，王 健

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

近年來，移動(dòng)機(jī)器人技術(shù)得到了很大發(fā)展，其控制技術(shù)趨于成熟。單機(jī)器人功能單一，受信息獲取能力、個(gè)體工作能力以及系統(tǒng)穩(wěn)定性的約束，人們受鳥類集群、魚群、蜂群等生物種群合作行為的啟發(fā)，對(duì)多機(jī)器人編隊(duì)系統(tǒng)進(jìn)行研究。多機(jī)器人編隊(duì)系統(tǒng)是利用局部信息交換，使得系統(tǒng)內(nèi)各個(gè)機(jī)器人間彼此競(jìng)爭(zhēng)、相互協(xié)同，執(zhí)行單機(jī)器人難以完成的、大量而復(fù)雜的任務(wù)。目前多機(jī)器人編隊(duì)方法主要有：領(lǐng)航-跟隨法[1-4]、基于行為的方法[5-6]、人工勢(shì)場(chǎng)法[7-8]、虛擬結(jié)構(gòu)法[9-10]和圖論法[11-12]。

由于領(lǐng)航-跟隨方法具有分散結(jié)構(gòu)，可將多個(gè)機(jī)器人編隊(duì)問題簡(jiǎn)化為若干個(gè)機(jī)器人的軌跡跟蹤控制問題，這樣可以減輕機(jī)器人之間的通信壓力，且計(jì)算量較小，易于工程實(shí)現(xiàn)。該方法中的控制器設(shè)計(jì)主要有神經(jīng)網(wǎng)絡(luò)控制[13-14]、滑?？刂芠15-16]、預(yù)測(cè)控制[17]、反步法控制[18-19]等。文獻(xiàn)[13]對(duì)多移動(dòng)機(jī)器人編隊(duì)控制問題設(shè)計(jì)了基于徑向基函數(shù)神經(jīng)網(wǎng)絡(luò)的控制器，文獻(xiàn)[14]在文獻(xiàn)[13]的基礎(chǔ)上考慮了在線估計(jì)跟隨者不確定部分的上界，簡(jiǎn)化了控制器的設(shè)計(jì)過程。但以上控制器設(shè)計(jì)過程由于引入了移動(dòng)機(jī)器人的動(dòng)力學(xué)模型，其控制方案比較復(fù)雜，不易于實(shí)際應(yīng)用。文獻(xiàn)[15]針對(duì)多移動(dòng)機(jī)器人協(xié)同行駛機(jī)制，運(yùn)用滑?？刂品椒ㄔO(shè)計(jì)了跟蹤控制器來實(shí)現(xiàn)群車的編隊(duì)控制，然而滑?？刂浦杏捎诙墩竦拇嬖?，在系統(tǒng)運(yùn)行時(shí)跟隨者軌跡會(huì)出現(xiàn)搖擺現(xiàn)象。文獻(xiàn)[17]針對(duì)多移動(dòng)機(jī)器人編隊(duì)控制時(shí)存在的時(shí)滯現(xiàn)象，提出一種基于預(yù)測(cè)控制的脈沖控制方法，利用一致性思想將編隊(duì)控制轉(zhuǎn)換為系統(tǒng)的穩(wěn)定性問題。但該方法控制精度偏低，應(yīng)用于實(shí)際跟蹤效果較差。文獻(xiàn)[18]針對(duì)車式移動(dòng)機(jī)器人的編隊(duì)問題，將機(jī)器人運(yùn)動(dòng)學(xué)模型轉(zhuǎn)化為鏈?zhǔn)较到y(tǒng)，運(yùn)用反步法構(gòu)造編隊(duì)控制器。文獻(xiàn)[19]在文獻(xiàn)[18]的基礎(chǔ)上，考慮了實(shí)際運(yùn)行過程中移動(dòng)機(jī)器人對(duì)起伏地形等環(huán)境的適應(yīng)問題，利用反步法設(shè)計(jì)控制器，實(shí)現(xiàn)了受外界干擾的車式移動(dòng)機(jī)器人的編隊(duì)控制，但文獻(xiàn)[18]和文獻(xiàn)[19]的設(shè)計(jì)過程比較復(fù)雜，在仿真過程中速度達(dá)到穩(wěn)態(tài)時(shí)會(huì)發(fā)生跳變現(xiàn)象，此方案很難通過實(shí)物進(jìn)行驗(yàn)證。

因此本文提出一種新的編隊(duì)控制方法，由領(lǐng)航者引導(dǎo)運(yùn)動(dòng)方向被視為主軌跡，參考軌跡由虛擬機(jī)器人生成，通過虛擬機(jī)器人與實(shí)際跟隨者構(gòu)造跟蹤誤差系統(tǒng)。針對(duì)該誤差系統(tǒng)設(shè)計(jì)編隊(duì)控制器，通過Lyapunov理論證明了跟蹤誤差將收斂到原點(diǎn)的小鄰域內(nèi)。文中分別對(duì)直線軌跡和圓形軌跡編隊(duì)進(jìn)行了仿真與實(shí)驗(yàn)，各個(gè)機(jī)器人可以從任意的初始狀態(tài)出發(fā)形成指定的期望結(jié)構(gòu)隊(duì)形并保持編隊(duì)效果，仿真與實(shí)驗(yàn)結(jié)果驗(yàn)證了所提算法的有效性。

1 系統(tǒng)模型

研究對(duì)象是典型的三輪結(jié)構(gòu)移動(dòng)機(jī)器人，前輪為從動(dòng)輪，兩后輪為驅(qū)動(dòng)輪。假設(shè)坐標(biāo)系為固定參考坐標(biāo)系，選取移動(dòng)機(jī)器人的兩個(gè)驅(qū)動(dòng)輪連線中點(diǎn)為參考點(diǎn)。移動(dòng)機(jī)器人位置與姿態(tài)表示為[x，y，θ]T，其中（x，y）表示移動(dòng)機(jī)器人參考點(diǎn)在xoy 坐標(biāo)系中的位置，θ 表示移動(dòng)機(jī)器人朝向與 x 軸的偏轉(zhuǎn)角度，v（t）與 w（t）分別為移動(dòng)機(jī)器人的線速度與角速度。則移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型可以表示為：

為了描述多移動(dòng)機(jī)器人系統(tǒng)中各個(gè)移動(dòng)機(jī)器人與領(lǐng)航者的位姿關(guān)系，采用圖1 的領(lǐng)航-跟隨結(jié)構(gòu)的幾何模型。對(duì)于一個(gè)編隊(duì)系統(tǒng)，編隊(duì)的主軌跡通常由領(lǐng)航者RL決定，跟隨者RF的參考軌跡由領(lǐng)航者RL和結(jié)構(gòu)參數(shù)生成的虛擬機(jī)器人RV軌跡決定。當(dāng)領(lǐng)航者RL的位姿發(fā)生變化后，由它確定的跟隨者RF的位姿要相應(yīng)的改變到虛擬機(jī)器人RV的位置上，所以整個(gè)系統(tǒng)的實(shí)質(zhì)是跟隨者RF不斷的跟蹤虛擬機(jī)器人RV的一個(gè)過程。

圖1 領(lǐng)航-跟隨者模型Fig.1 Leader-follower model

圖1 中，領(lǐng)航者 RL的坐標(biāo)為[xL，yL，θL]T，跟隨者 RF的坐標(biāo)為[xF，yF，θF]T，虛擬機(jī)器人 RV的坐標(biāo)為[xV，yV，θV]T。其中領(lǐng)航者的線速度用vL來表示，角速度用wL來表示。跟隨者的線速度用vF來表示，角速度用wF來表示。表示虛擬機(jī)器人RV相對(duì)于領(lǐng)航者RL的距離與連線方向之間形成的夾角。

假設(shè)1：虛擬機(jī)器人RV的姿態(tài)角與領(lǐng)航者RL的姿態(tài)角相同，即 θV= θL。

假設(shè)2：初始姿態(tài)角偏差有界，即|θe（0）|<π。

圖1 中虛擬機(jī)器人RV與領(lǐng)航者RL之間的幾何位置關(guān)系可以描述為：

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

由式（1）知領(lǐng)航者的運(yùn)動(dòng)學(xué)模型為：

對(duì)式（2）求導(dǎo)，結(jié)合式（4）得到：

由式（1）得到跟隨者運(yùn)動(dòng)學(xué)模型為：

對(duì)式（3）求導(dǎo)，結(jié)合式（5）、（6），得到跟蹤誤差的動(dòng)態(tài)方程：

至此，將多機(jī)器人系統(tǒng)的編隊(duì)問題轉(zhuǎn)化為跟隨者RF對(duì)虛擬機(jī)器人RV的軌跡跟蹤控制問題，本文的目標(biāo)是尋找合適的控制輸入（vF，wF）T，使得跟隨者與虛擬機(jī)器人的跟蹤誤差（xe，ye，θe）收斂到原點(diǎn)的小鄰域內(nèi)。

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

為設(shè)計(jì)控制器，取

式中：c1>a1；c2>a2；a1、a2為設(shè)計(jì)而取定的正常數(shù)。

由式（8）和式（9）可得：

對(duì)式（11）求導(dǎo)，可得：

定理1由多個(gè)輪式移動(dòng)機(jī)器人構(gòu)成的編隊(duì)系統(tǒng)，如果 vF、wF有界且 vF>0 初始姿態(tài)角偏差|θe（0）|<π，針對(duì)式（7）描述的誤差動(dòng)態(tài)方程，若跟隨機(jī)器人采用控制律為式（10）和式（12），則當(dāng) t→∞ 時(shí)，xe，ye，θe均收斂到原點(diǎn)小鄰域內(nèi)。

證明：將式（8）、（9）代入式（7）中，得：

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

對(duì)式（15）求導(dǎo)并代入（13）和（14）式得：

通過適當(dāng)數(shù)學(xué)運(yùn)算，（16）式可進(jìn)一步寫成

對(duì)式（17）積分可得：

當(dāng) t→∞，e-αt→0。因此對(duì)于任意的存在時(shí)刻 T>0，當(dāng) t>T 時(shí)，V≤μ，進(jìn)一步得到因此選擇適當(dāng)?shù)腸1、c2的值，可使 μ 變得任意小，從而使（xe，ye）收斂到原點(diǎn)的小鄰域內(nèi)。

對(duì)式（11）變形可得：

當(dāng)（xe，ye）任意小時(shí)，sin（θF- θL）也任意小，由此可知θF-θL任意小，從而θe收斂到原點(diǎn)的小鄰域內(nèi)，證畢。

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

3.1 仿真

為驗(yàn)證所提方法的有效性，在Matlab/Simulink 中進(jìn)行仿真研究。選取1 臺(tái)領(lǐng)航者與2 臺(tái)跟隨者來實(shí)現(xiàn)編隊(duì)隊(duì)形，分別對(duì)直線軌跡編隊(duì)和圓形軌跡編隊(duì)進(jìn)行仿真。仿真中a1與a2都取值0.1。

仿真1：3 臺(tái)移動(dòng)機(jī)器人從起始位置出發(fā)沿直線運(yùn)動(dòng)并組成三角形編隊(duì)前進(jìn)。領(lǐng)航者的初始線速度與角速度設(shè)置為vL= 0.1 m/s，wL= 0 rad/s 初始位姿為（2，2，π/4），2 臺(tái)跟隨者初始位姿（1，0，π/4）和（0，1，π/4）。2 臺(tái)虛擬機(jī)器人與領(lǐng)航者的期望距離與相對(duì)轉(zhuǎn)角分別為-5π/6。跟隨者1 與跟隨者2 的控制器參數(shù)分別為：c1=0.8，c2=0.8 和 c3=1.1，c4=1.1。3 臺(tái)移動(dòng)機(jī)器人在二維平面上的運(yùn)行軌跡如圖2 所示，其中領(lǐng)航者為紅色實(shí)線，跟隨者1 為綠色虛線，跟隨者2 為藍(lán)色虛線。2 臺(tái)跟隨者的速度曲線如圖3 所示，其中藍(lán)色虛線為跟隨者的線速度，紅色實(shí)線為跟隨者的角速度。

圖2 直線運(yùn)動(dòng)軌跡Fig.2 Trajectory of linear motion

圖3 直線運(yùn)動(dòng)跟隨者速度曲線Fig.3 Velocity curve of follower with rectilinear motion

仿真2：3 臺(tái)移動(dòng)機(jī)器人從初始位置出發(fā)沿曲線運(yùn)動(dòng)并組成直線編隊(duì)。領(lǐng)航者的初始線速度與角速度設(shè)置為 vL=0.1 m/s，wL=0 rad/s，初始位姿為（2，2，0），2 臺(tái)跟隨者初始位姿（1，2.5，0），（1，1，0）。2 臺(tái)虛擬機(jī)器人與領(lǐng)航者的期望距離與相對(duì)轉(zhuǎn)角分別設(shè)計(jì)為跟隨者1 與跟隨者2 的控制器參數(shù)分別為：c1=0.8，c2=0.8 和c3=1.1，c4=1.1。3 臺(tái)移動(dòng)機(jī)器人在二維平面上的運(yùn)行軌跡如圖4 所示，2 臺(tái)跟隨者速度曲線如圖5所示。

由仿真結(jié)果可知，雖然2 組仿真的跟隨者初始位置距期望位置較遠(yuǎn)，存在較大的編隊(duì)初始誤差，使得跟隨者的線速度與角速度變化較大，但當(dāng)編隊(duì)達(dá)到理想狀態(tài)時(shí)，跟隨者的跟蹤誤差、速度均達(dá)到穩(wěn)定。

圖4 圓周運(yùn)動(dòng)軌跡Fig.4 Trajectory of circular motion

圖5 圓周運(yùn)動(dòng)跟隨者速度曲線Fig.5 Velocity curve of follower with circular motion

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

為驗(yàn)證所提方法的實(shí)用性，采用多移動(dòng)機(jī)器人協(xié)同編隊(duì)系統(tǒng)進(jìn)行實(shí)物實(shí)驗(yàn)。實(shí)驗(yàn)采用的輪式機(jī)器人具體如圖6 所示。主體部分是長方形的塑料基板，驅(qū)動(dòng)輪與定向輪在基板底部形成穩(wěn)固的三角。車底部還有電機(jī)、編碼器，車體上方安裝電源模塊、無線模塊、控制模塊、電壓轉(zhuǎn)換模塊。

圖6 實(shí)驗(yàn)平臺(tái)Fig.6 Experimental platform

本文進(jìn)行了2 組編隊(duì)實(shí)驗(yàn)，實(shí)驗(yàn)中a1與a2都取值為0.1。

實(shí)驗(yàn)1：領(lǐng)航者與兩臺(tái)跟隨者的初始位姿，相對(duì)轉(zhuǎn)角與距離，初始速度都跟仿真1 相同，控制器參數(shù)設(shè)置為 c1=0.5，c2=0.5 和 c3=0.5，c4=0.5。3 臺(tái)機(jī)器人在二維平面上的運(yùn)行軌跡如圖7 所示，速度曲線如圖8 所示。

圖7 三角形編隊(duì)Fig.7 Triangle formation

圖8 三角隊(duì)形跟隨者輸入Fig.8 Velocity curve of triangle follower input

實(shí)驗(yàn)2：領(lǐng)航者與2 臺(tái)跟隨者的初始位姿，相對(duì)轉(zhuǎn)角與距離，初始速度都跟仿真2 相同，控制器參數(shù)設(shè)置為 c1=0.5，c2=0.5 和 c3=0.5，c4=0.5。3 臺(tái)機(jī)器人在二維平面上的運(yùn)行軌跡如圖9 所示，速度曲線如圖10 所示。

上述的實(shí)物實(shí)驗(yàn)中，領(lǐng)航者分別以直線和圓周作為移動(dòng)軌跡，2 臺(tái)跟隨者從給定的初始位置開始運(yùn)動(dòng)，能夠跟隨領(lǐng)航者分別形成三角與直線編隊(duì)結(jié)構(gòu)。速度信號(hào)前期經(jīng)過一個(gè)短暫的調(diào)整過程便進(jìn)入穩(wěn)定狀態(tài)，但是由于電機(jī)飽和約束和信號(hào)延遲等硬件因素的影響，導(dǎo)致機(jī)器人在運(yùn)動(dòng)過程中存在擾動(dòng)，速度曲線不是非常光滑。

圖9 直線形編隊(duì)Fig.9 Linear formation

圖10 直線形跟隨者輸入Fig.10 Velocity curve of linear follower input

4 結(jié) 論

文中提出一種針對(duì)領(lǐng)航-跟隨型的編隊(duì)控制方法。首先，根據(jù)領(lǐng)航者和期望隊(duì)形確定虛擬機(jī)器人的位姿參數(shù)，將領(lǐng)航-跟隨法的編隊(duì)問題轉(zhuǎn)化為跟隨者對(duì)虛擬機(jī)器人的軌跡跟蹤問題；然后，針對(duì)運(yùn)動(dòng)學(xué)模型特點(diǎn)設(shè)計(jì)編隊(duì)控制器，并證明了跟蹤誤差收斂到原點(diǎn)的小鄰域內(nèi)；最后，進(jìn)行了仿真與實(shí)物實(shí)驗(yàn)，結(jié)果表明：

（1）多移動(dòng)機(jī)器人系統(tǒng)能形成編隊(duì)，并以較小的誤差保持隊(duì)形做編隊(duì)運(yùn)動(dòng)，即位姿誤差（xe，ye，θe）將收斂到原點(diǎn)的鄰域內(nèi)。

（2）通過設(shè)定參數(shù)a1、a2的值，從而改變位姿誤差（xe，ye，θe）的收斂范圍。

（3）期望編隊(duì)隊(duì)形在有限時(shí)間內(nèi)形成，控制器設(shè)計(jì)時(shí)可以融入快速終端滑模的思想。