多個(gè)輪式移動(dòng)機(jī)器人沿同一軌跡曲線運(yùn)動(dòng)的編隊(duì)控制研究

劉昕宇,周宇生

(貴州大學(xué) 數(shù)學(xué)與統(tǒng)計(jì)學(xué)院, 貴陽(yáng) 550025)

0 引言

控制輸入個(gè)數(shù)少于系統(tǒng)自由度數(shù)目的欠驅(qū)動(dòng)系統(tǒng)普遍存在于實(shí)際生產(chǎn)生活中。目前關(guān)于各種欠驅(qū)動(dòng)模型及其相關(guān)的控制理論研究成果相當(dāng)豐富[1-3]。其中,兩輪式移動(dòng)機(jī)器人就是一種典型的欠驅(qū)動(dòng)系統(tǒng)[4-6],通過(guò)在兩側(cè)車輪安裝驅(qū)動(dòng)馬達(dá),分別控制車輪的轉(zhuǎn)速,使輪式移動(dòng)機(jī)器人實(shí)現(xiàn)直行或轉(zhuǎn)向運(yùn)動(dòng)。在一些實(shí)際問(wèn)題中,可能需要采用多個(gè)輪式移動(dòng)機(jī)器人進(jìn)行協(xié)調(diào)作業(yè),以完成復(fù)雜的運(yùn)動(dòng)任務(wù),如工農(nóng)業(yè)生產(chǎn)、環(huán)境監(jiān)測(cè)、國(guó)防軍事等[7-9]。與單個(gè)輪式移動(dòng)機(jī)器人相比,多個(gè)輪式移動(dòng)機(jī)器人在協(xié)調(diào)作業(yè)過(guò)程中需要一直保持一定的隊(duì)形結(jié)構(gòu)。為此,許多研究者提出了一系列的方法解決多個(gè)機(jī)器人編隊(duì)的運(yùn)動(dòng)控制問(wèn)題,包括領(lǐng)航跟隨法[10-13]與虛擬結(jié)構(gòu)法[17]等。

領(lǐng)航跟隨法是一組編隊(duì)機(jī)器人中的一個(gè)或幾個(gè)機(jī)器人被選為領(lǐng)導(dǎo)者,它們決定整個(gè)編隊(duì)的運(yùn)動(dòng)軌跡,其余機(jī)器人作為跟隨者,與領(lǐng)航者之間保持一定的角度和距離[15]。這個(gè)理論簡(jiǎn)單可靠,受到越來(lái)越多學(xué)者的重視,他們?cè)诖嘶A(chǔ)上提出了許多改進(jìn)的編隊(duì)控制方法。具體來(lái)說(shuō),李詠華等[10]提出了一種領(lǐng)航-跟隨型多移動(dòng)小車分布式編隊(duì)控制方法,該方法能夠快速收斂至期望隊(duì)形并具有很好的抗干擾能力。Wang等[11]考慮到不是所有個(gè)體都能跟蹤期望路徑,在多四旋翼飛行器的編隊(duì)中提出了一種分布式控制算法實(shí)現(xiàn)了領(lǐng)航-跟隨的編隊(duì)跟蹤策略。Gonzalez等[12]提出了移動(dòng)機(jī)器人和四旋翼飛機(jī)在領(lǐng)航-跟隨方案下基于距離和方向的動(dòng)力學(xué)模型,設(shè)計(jì)了一種魯棒控制策略解決編隊(duì)控制問(wèn)題。Moorthy等[13]為每個(gè)跟隨者開(kāi)發(fā)了一個(gè)分布式估計(jì)器估計(jì)領(lǐng)導(dǎo)者狀態(tài),同時(shí)為了解決速度跳躍問(wèn)題,設(shè)計(jì)了一種基于生物神經(jīng)動(dòng)力學(xué)的反步控制器管理編隊(duì)。此外,在多機(jī)器人編隊(duì)控制問(wèn)題中,基于障礙李雅普諾夫函數(shù)的方法具有處理約束的能力。Jin[14]提出了一種新型欠驅(qū)動(dòng)自主水面艦船的容錯(cuò)領(lǐng)航-跟隨編隊(duì)控制方法,解決了視距角度的約束以及有限時(shí)間收斂的問(wèn)題。Dai等[16]研究了通信約束下非完整移動(dòng)機(jī)器人的編隊(duì)跟蹤控制問(wèn)題。此外,Zhang等[17]通過(guò)改進(jìn)傳統(tǒng)的虛擬結(jié)構(gòu)方法,結(jié)合個(gè)體路徑跟蹤控制設(shè)計(jì),解決了網(wǎng)絡(luò)非完整移動(dòng)車輛團(tuán)隊(duì)協(xié)同路徑跟蹤問(wèn)題。

可以看到,在上述的研究工作中,幾乎沒(méi)有考慮領(lǐng)航者和跟隨者沿相同軌跡運(yùn)動(dòng)的問(wèn)題。他們大多是基于領(lǐng)航者與跟隨者之間的距離和角度展開(kāi)研究,在這種情況下,他們之間的相對(duì)位置可以很容易地確定下來(lái)。然而,當(dāng)領(lǐng)航者和跟隨者需要沿同一軌跡運(yùn)動(dòng)時(shí),他們之間的相對(duì)位置關(guān)系需要結(jié)合目標(biāo)軌跡曲線才能被進(jìn)一步確定。Yuan等[5]在多轉(zhuǎn)向拖掛車移動(dòng)機(jī)器人中利用反步法實(shí)現(xiàn)了拖車和掛車跟蹤相同的目標(biāo)路徑。Zhou等[18]引入被動(dòng)轉(zhuǎn)向角及齒輪轉(zhuǎn)向機(jī)制,使拖車和掛車都能在低速下精確跟蹤同一目標(biāo)軌跡曲線。在編隊(duì)控制問(wèn)題中考慮領(lǐng)航者與跟隨者沿同一軌跡運(yùn)動(dòng)同樣具有現(xiàn)實(shí)意義。如在軍事演習(xí)中[19],沿同一軌跡運(yùn)動(dòng)可以豐富編隊(duì)隊(duì)形。另外,當(dāng)一個(gè)編隊(duì)在狹長(zhǎng)的路段運(yùn)行,此時(shí)需要所有個(gè)體都跟隨領(lǐng)航者沿同一軌跡運(yùn)行。這樣減少了所需的車道寬度,也大大提高了系統(tǒng)的機(jī)動(dòng)性與靈活性[20]。同時(shí),由于編隊(duì)系統(tǒng)處在同一軌跡上完成任務(wù),那么現(xiàn)有的單機(jī)器人路徑規(guī)劃算法也能夠被使用[5]。綜上所述,本文以兩輪式移動(dòng)機(jī)器人為例,考慮了領(lǐng)航者和跟隨者沿相同路徑運(yùn)動(dòng)且保持一定距離的編隊(duì)控制問(wèn)題,具體貢獻(xiàn)如下:

1) 對(duì)簡(jiǎn)單的圓形軌跡進(jìn)行幾何分析,得到領(lǐng)航者和跟隨者之間的關(guān)鍵位置關(guān)系。在此基礎(chǔ)上,進(jìn)一步得到了軌跡是一般光滑曲線時(shí)的位置關(guān)系,將該關(guān)系式結(jié)合到控制器的設(shè)計(jì)中,實(shí)現(xiàn)整個(gè)編隊(duì)沿同一軌跡運(yùn)動(dòng)的目標(biāo)。

2) 針對(duì)帶有輸出約束的編隊(duì)系統(tǒng),為了保證編隊(duì)跟蹤誤差的瞬態(tài)和穩(wěn)態(tài)性能,我們將障礙李雅普諾夫函數(shù)引入到編隊(duì)控制設(shè)計(jì)中,將系統(tǒng)誤差限定在一定的邊界范圍內(nèi),所提出的編隊(duì)控制策略能夠保證領(lǐng)航者與跟隨者之間避免碰撞,有利于編隊(duì)控制方案的實(shí)施。

本文的其余部分安排如下。第2節(jié)給出了兩輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué),動(dòng)力學(xué)模型和任務(wù)控制目標(biāo)。第3節(jié)詳細(xì)描述了領(lǐng)航者和跟隨者形成同一軌跡的過(guò)程和控制器的設(shè)計(jì)。第4節(jié)利用Matlab仿真驗(yàn)證了所提方法的有效性。最后,第5節(jié)對(duì)本文結(jié)果進(jìn)行了總結(jié)。

1 問(wèn)題描述

本節(jié)將簡(jiǎn)述兩輪式移動(dòng)機(jī)器人的結(jié)構(gòu)以及其運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型,并結(jié)合模型介紹相應(yīng)的控制任務(wù)。

1.1 兩輪式移動(dòng)機(jī)器人模型

如圖1所示,兩輪式移動(dòng)機(jī)器人主要由兩車輪與一個(gè)剛性連桿連接,在輪軸兩側(cè)的車輪上分別安裝有驅(qū)動(dòng)馬達(dá),通過(guò)驅(qū)動(dòng)車輪轉(zhuǎn)動(dòng)實(shí)現(xiàn)兩輪式移動(dòng)機(jī)器人前向和轉(zhuǎn)向運(yùn)動(dòng)。此外,假設(shè)每個(gè)車輪只向前滾動(dòng)而不產(chǎn)生橫向滑動(dòng)和空轉(zhuǎn)。文中所用符號(hào)和公式的定義見(jiàn)表1。

圖1 兩輪式移動(dòng)機(jī)器人示意圖

表1 兩輪式移動(dòng)機(jī)器人各參數(shù)及變量

1.2 運(yùn)動(dòng)學(xué)方程與動(dòng)力學(xué)方程

兩輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型如下所示:

其中

兩輪式移動(dòng)機(jī)器人動(dòng)力學(xué)方程如下所示:

(1)

其中:

a11=a22=2Mw+MB

式(1)還可以表示為如下形式:

(2)

其中:

u1=Tl+Tr,u2=Tl-Tr

1.3 控制目標(biāo)

本文的控制任務(wù)是為多個(gè)兩輪式移動(dòng)機(jī)器人的編隊(duì)問(wèn)題設(shè)計(jì)跟蹤控制器,以實(shí)現(xiàn)以下目標(biāo):

1) 基于動(dòng)態(tài)跟蹤目標(biāo),目標(biāo)軌跡曲線能夠被領(lǐng)航者精確追蹤。

2) 領(lǐng)航者和跟隨者能夠沿同一軌跡運(yùn)動(dòng)。

3) 在運(yùn)動(dòng)過(guò)程中,領(lǐng)航者與跟隨者能夠始終保持預(yù)設(shè)的距離。

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

本節(jié)將分別介紹領(lǐng)航者與跟隨者的控制器設(shè)計(jì),并給出相應(yīng)的穩(wěn)定性分析。

2.1 動(dòng)態(tài)跟蹤目標(biāo)

(3)

進(jìn)一步,式(3)可改寫(xiě)為速度目標(biāo)形式

(4)

(5)

一般來(lái)說(shuō),φ(t)可設(shè)計(jì)為如下[21]

φ(t)=-(βt+1)lβ2texp(-βt)

式中:l為目標(biāo)軌跡曲線的長(zhǎng)度;β為待定參數(shù)。

在實(shí)際應(yīng)用中,初始時(shí)刻輪式移動(dòng)機(jī)器人是靜止不動(dòng)的,可以調(diào)整前向速度目標(biāo)使初始速度誤差為零,并可以根據(jù)實(shí)際需要調(diào)節(jié)參數(shù)β,使整個(gè)過(guò)程跟蹤控制的效果最好。

2.2 領(lǐng)航者的跟蹤控制設(shè)計(jì)

在本節(jié)中,通過(guò)動(dòng)態(tài)跟蹤目標(biāo)為領(lǐng)航者設(shè)計(jì)控制器,以便于領(lǐng)航者能夠精確跟蹤目標(biāo)軌跡曲線。

2.2.1前向速度控制器設(shè)計(jì)

注意到控制式(2)是解耦的,因此可以先設(shè)計(jì)u1,然后基于實(shí)際的前向速度設(shè)計(jì)u2跟蹤偏航轉(zhuǎn)速目標(biāo)。結(jié)合式(5),前向速度方程可以寫(xiě)為

(6)

(7)

在此,利用反步法設(shè)計(jì)前向速度控制器。將e2視為虛擬控制量,引入如下變換:

e2=-e1=η(e1)

(8)

選擇李雅普諾夫函數(shù)V1

對(duì)V1兩邊關(guān)于時(shí)間求導(dǎo),并將式(8)代入可得

在此,引入如下變量代換

z=e2-η(e1)=e2+e1

(9)

對(duì)式(9)求導(dǎo)并結(jié)合式(7),整理后可得:

(10)

類似的,選取李雅普諾夫函數(shù)V2

對(duì)V2兩邊關(guān)于時(shí)間求導(dǎo),并將式(10)代入可得

(11)

此時(shí)可將前向速度控制器u1設(shè)計(jì)為如下表達(dá)式

(12)

聯(lián)立式(11)及式(12),可得

最后,將式(9)代入式(12),可得最終的前向速度控制器u1,表達(dá)式如下所示:

2.2.2偏航轉(zhuǎn)速控制器設(shè)計(jì)

設(shè)計(jì)的積分滑模面為:

(13)

式中G1>0,是合適的常數(shù)。

最終設(shè)計(jì)的偏航轉(zhuǎn)速控制器u2為如下表達(dá)式:

式中:k、G2、K均為大于零的常數(shù)。

2.3 跟隨者的位置分析

2.3.1特殊曲線軌跡

如圖2所示,我們所研究編隊(duì)問(wèn)題的控制目標(biāo)是領(lǐng)航者和跟隨者沿同一軌跡運(yùn)動(dòng),并且兩者之間始終保持一定的距離。首先考慮一種簡(jiǎn)單的情況,領(lǐng)航者的目標(biāo)軌跡曲線是如圖2所示的圓軌跡曲線。圖中的3個(gè)簡(jiǎn)單幾何關(guān)系如下所示:

(14)

式中:Φ、β、γ如圖2所示。θb和θv分別表示領(lǐng)航者和跟隨者的偏航角。

圖2 圓軌跡曲線編隊(duì)示意圖

整理上式,可以得到領(lǐng)航者與跟隨者偏航角的關(guān)系:

θv=θb-2β

基于式(14)中的幾何關(guān)系,跟隨者和領(lǐng)航者的相對(duì)位置關(guān)系可表示為:

其中:(xb,yb),(xv,yv)分別表示領(lǐng)航者和跟隨者的位置。

2.3.2一般曲線軌跡

在本節(jié)中,考慮領(lǐng)航者的期望軌跡為一般光滑軌跡曲線。由于一般軌跡的未知性,很難直接確定領(lǐng)航者與跟隨者之間的相對(duì)位置關(guān)系,因此做出如下假設(shè)。

假設(shè)1將領(lǐng)航者與跟隨者之間的軌跡弧視為圓弧近似處理。

假設(shè)2圓弧的半徑比兩車之間的距離要大得多。

如圖3所示,當(dāng)領(lǐng)航者的軌跡是一般光滑曲線時(shí),R不再是一個(gè)常量,而是一個(gè)隨β變化而變化的變量。記近似處理之后的半徑和角度分別為Rb。此時(shí)根據(jù)假設(shè)條件,上一節(jié)的幾何關(guān)系依舊成立。在此基礎(chǔ)上做進(jìn)一步的調(diào)整,由于領(lǐng)航者與跟隨者沿同一軌跡運(yùn)動(dòng),可以得到

相應(yīng)可得

圖3 一般軌跡曲線編隊(duì)示意圖

故領(lǐng)航者的期望軌跡是一般光滑軌跡曲線時(shí),它們之間的相對(duì)位置關(guān)系如下所示:

(15)

2.4 跟隨者的編隊(duì)控制器設(shè)計(jì)

為了達(dá)到控制目標(biāo),本節(jié)對(duì)跟隨者采用雙閉環(huán)的控制結(jié)構(gòu)。

2.4.1外環(huán)速度控制器設(shè)計(jì)

運(yùn)動(dòng)學(xué)誤差系統(tǒng)定義如下:

式中,(xf,yf,θf(wàn))T是跟隨者的位置響應(yīng);(xv-xf,yv-yf,θv-θf(wàn))T是位姿誤差;(xe,ye,θe)T是坐標(biāo)變換后的位姿誤差。

對(duì)上式誤差方程關(guān)于時(shí)間求導(dǎo),整理后得:

(16)

式中:ωf和vf分別表示跟隨機(jī)器人的偏航轉(zhuǎn)速和前向速度。

可以看到,編隊(duì)問(wèn)題已經(jīng)轉(zhuǎn)化為了軌跡跟蹤問(wèn)題,設(shè)置如下控制律可使誤差系統(tǒng)漸近穩(wěn)定。

(17)

式中:Kx、Kθ、Ky、a、b為增益系數(shù),均為大于零的常數(shù)。

證明：考慮如下李雅普諾夫函數(shù)

顯然,函數(shù)V是正定的。對(duì)V關(guān)于時(shí)間求導(dǎo),并將式(17)代入,整理可得

vv(a+b)sinθe(ye+Kθθe)]-

如果a+b=1,則

2.4.2內(nèi)環(huán)姿態(tài)控制器的設(shè)計(jì)

在雙閉環(huán)控制系統(tǒng)中,基于直接李雅普諾夫函數(shù)法設(shè)計(jì)的速度控制器在外環(huán)可以實(shí)現(xiàn)跟蹤誤差的收斂,與兩輪式機(jī)器人的姿態(tài)變量相關(guān)。在內(nèi)環(huán)上,需要引入力矩控制器來(lái)實(shí)現(xiàn)對(duì)期望速度的跟蹤。

定義速度的跟蹤誤差為

(18)

對(duì)上式求導(dǎo)可得

(19)

構(gòu)造如下對(duì)稱障礙李雅普諾夫函數(shù)

式中:kb>0為約束邊界;誤差初始值z(mì)1(0),z2(0)滿足|z1(0)|

對(duì)Vz1、Vz2求導(dǎo)并結(jié)合兩輪式移動(dòng)機(jī)器人動(dòng)力學(xué)方程,整理后可得

(20)

引入如下控制律u10和u20,

其中k1、k2、κ為大于零的常數(shù)。

將控制律u10和u20代入式(20),整理可得

3 仿真結(jié)果與分析

在仿真中,編隊(duì)系統(tǒng)的參數(shù)設(shè)置如下:

下面對(duì)固定的參數(shù)值進(jìn)行數(shù)值模擬,以分析本文方法的有效性。

如圖4所示,實(shí)際軌跡與目標(biāo)軌跡之間的偏差非常小,說(shuō)明了采用動(dòng)態(tài)跟蹤目標(biāo)跟蹤目標(biāo)曲線的曲率,通過(guò)偏航速度的時(shí)時(shí)動(dòng)態(tài)調(diào)整,確實(shí)能夠極大地減少運(yùn)動(dòng)中的累積位置誤差[21]。

圖4 領(lǐng)航機(jī)器人軌跡跟蹤曲線

如圖5所示,2個(gè)跟隨者都能夠精確地與領(lǐng)航者沿同一軌跡運(yùn)動(dòng)。再結(jié)合圖6和圖7,跟隨者們大約能在1 s左右收斂到預(yù)定的距離,從而形成一個(gè)同軌跡運(yùn)動(dòng)的編隊(duì)隊(duì)形。這說(shuō)明充分利用領(lǐng)航者和跟隨者之間位置的幾何關(guān)系所設(shè)計(jì)的控制器具有控制精確和響應(yīng)速度快的優(yōu)點(diǎn)。

圖5 實(shí)際的編隊(duì)運(yùn)動(dòng)軌跡曲線

圖6 跟隨者1與領(lǐng)航者保持的距離

圖7 跟隨者2與領(lǐng)航者保持的距離

事實(shí)上,編隊(duì)系統(tǒng)在運(yùn)動(dòng)過(guò)程中會(huì)受到未知擾動(dòng)的影響。接下來(lái),為了檢驗(yàn)其抗干擾性能,下面通過(guò)仿真驗(yàn)證4種類型干擾(d1、d2、d3、d4)對(duì)于距離的影響情況,如圖8所示。

從圖8(a)—(d)可以看出,在存在持續(xù)外部干擾的情況下,繼續(xù)采用控制器u10和u20,輪式移動(dòng)機(jī)器人之間仍然能夠保持非常接近的預(yù)設(shè)距離。這很好地驗(yàn)證了采用動(dòng)態(tài)跟蹤目標(biāo)和充分利用幾何位置關(guān)系所設(shè)計(jì)的控制器具有非常好的魯棒性。

圖8 4類干擾情況下跟隨者2與領(lǐng)航者的距離

如圖9所示,本文提出的方法能夠保證跟蹤誤差始終約束在±kb之內(nèi),這驗(yàn)證了所設(shè)計(jì)的障礙李雅普諾夫函數(shù)的有效性。

為了進(jìn)一步地驗(yàn)證本文提出的控制方法的有效性和優(yōu)越性,我們選取常用的積分滑模控制方法與本文方法進(jìn)行比較,其軌跡如圖10所示。

為了保證對(duì)比的公平性,2種方法的系統(tǒng)參數(shù)均保持一樣,仿真效果如圖11所示。顯然,相比于積分滑?？刂品椒?本文方法所設(shè)計(jì)的控制器有著更快的收斂速度以及更小的超調(diào)量。

圖9 速度跟蹤誤差曲線

圖10 2種方法運(yùn)動(dòng)軌跡曲線

圖11 2種方法保持距離曲線

4 結(jié)論

對(duì)于多機(jī)器人編隊(duì)控制問(wèn)題,提出了一種沿同一軌跡的編隊(duì)隊(duì)形方法,建立了兩輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)方程。針對(duì)領(lǐng)航者,結(jié)合動(dòng)態(tài)跟蹤目標(biāo)的思想設(shè)計(jì)了控制器。同時(shí),將領(lǐng)航者與跟隨者之間的幾何關(guān)系,應(yīng)用于力矩控制器的設(shè)計(jì),最終實(shí)現(xiàn)了領(lǐng)航者和跟隨者沿相同目標(biāo)軌跡運(yùn)動(dòng)的目標(biāo)。