孟　靜，陳　罡，高曉丁

(1.浙江紡織服裝學(xué)院 機(jī)電學(xué)院，浙江 寧波　315200； 2.西安工程大學(xué) 機(jī)電工程學(xué)院，西安　710000)

?

多移動(dòng)機(jī)器人的目標(biāo)跟蹤研究

孟靜1，2，陳罡1，高曉丁2

(1.浙江紡織服裝學(xué)院 機(jī)電學(xué)院，浙江 寧波315200； 2.西安工程大學(xué) 機(jī)電工程學(xué)院，西安710000)

針對(duì)多移動(dòng)機(jī)器人對(duì)固定目標(biāo)和動(dòng)態(tài)目標(biāo)跟蹤問(wèn)題進(jìn)行了研究；首先基于l-φ閉環(huán)控率對(duì)多移動(dòng)機(jī)器人進(jìn)行剛性編隊(duì)，建立運(yùn)動(dòng)學(xué)模型，采用leader-follower協(xié)調(diào)策略算法，實(shí)現(xiàn)了多移動(dòng)機(jī)器人的協(xié)調(diào)合作；然后采用SURF算法識(shí)別目標(biāo)，通過(guò)路徑規(guī)劃實(shí)現(xiàn)對(duì)固定目標(biāo)的跟蹤以及利用卡爾曼濾波器實(shí)現(xiàn)對(duì)運(yùn)動(dòng)目標(biāo)的跟蹤，設(shè)計(jì)了基于Backstepping方法的控制器，使多移動(dòng)機(jī)器人能穩(wěn)定跟蹤目標(biāo)；最后用MATLAB進(jìn)行仿真；仿真結(jié)果表明，所設(shè)計(jì)的控制器和算法能使多移動(dòng)機(jī)器人的跟蹤誤差快速收斂于零，適用于多移動(dòng)機(jī)器人對(duì)目標(biāo)的跟蹤。

多移動(dòng)機(jī)器人；目標(biāo)跟蹤；路徑規(guī)劃；卡爾曼濾波器；Backstepping

0　前言

目標(biāo)跟蹤是機(jī)器人研究領(lǐng)域的一個(gè)重點(diǎn)，在人機(jī)交互、太空探索以及災(zāi)區(qū)救援等方面已有廣泛的應(yīng)用[1]。在固定目標(biāo)跟蹤中，需先對(duì)機(jī)器人的路徑進(jìn)行規(guī)劃，對(duì)路徑規(guī)劃問(wèn)題，主要有PRM(probabilistic roadmap method)算法、模擬退火算法以及人工勢(shì)場(chǎng)法和蟻群優(yōu)化算法相結(jié)合。Yilmaz等提出了一種基于互聯(lián)網(wǎng)的移動(dòng)機(jī)器人平臺(tái)，平臺(tái)采用單個(gè)網(wǎng)絡(luò)攝像頭，用戶可以通過(guò)網(wǎng)絡(luò)遠(yuǎn)程控制機(jī)器人實(shí)時(shí)跟蹤路線[2]。對(duì)動(dòng)態(tài)目標(biāo)點(diǎn)跟蹤主要采用幀間差分法、自適應(yīng)背景消除法、匹配塊法以及光流法等。Suluh等同時(shí)使用線性視覺(jué)軌跡算法用于跟蹤攔截在三維空間內(nèi)運(yùn)動(dòng)的氣球。隨著氣球上升，機(jī)器人利用自適應(yīng)控制算法自動(dòng)調(diào)整攝像頭轉(zhuǎn)動(dòng)角度，保證氣球質(zhì)心始終出現(xiàn)在圖像中心[3-4]。文獻(xiàn)[5]提出了一種擴(kuò)展式卡爾曼濾波的估計(jì)算法，解決了機(jī)器人在未知環(huán)境下的動(dòng)態(tài)目標(biāo)追蹤問(wèn)題，提高了目標(biāo)狀態(tài)估計(jì)的準(zhǔn)確性；文獻(xiàn)[6]將CMUcam3控制算法應(yīng)用于多種機(jī)動(dòng)模式的移動(dòng)機(jī)器人目標(biāo)跟蹤控制的研究，保證了機(jī)器人在運(yùn)動(dòng)過(guò)程中保持低功耗。但以上的目標(biāo)跟蹤多是基于單機(jī)器人系統(tǒng)及少數(shù)多機(jī)器人系統(tǒng)對(duì)固定目標(biāo)的跟蹤，多機(jī)器人系統(tǒng)對(duì)運(yùn)動(dòng)目標(biāo)跟蹤的研究情況仍處于萌芽階段，而多機(jī)器人系統(tǒng)由于其時(shí)間、空間、功能、信息和資源的分布性特點(diǎn)，能夠完成單機(jī)器人系統(tǒng)無(wú)法實(shí)現(xiàn)的復(fù)雜任務(wù)，所以，對(duì)于多移動(dòng)機(jī)器人對(duì)運(yùn)動(dòng)目標(biāo)的跟蹤問(wèn)題很有必要進(jìn)行詳細(xì)的研究。本文就多機(jī)器人系統(tǒng)對(duì)固定目標(biāo)和運(yùn)動(dòng)目標(biāo)的跟蹤進(jìn)行了研究，通過(guò)對(duì)協(xié)調(diào)策略和目標(biāo)跟蹤算法進(jìn)行設(shè)計(jì)，最后通過(guò)仿真進(jìn)行驗(yàn)證，結(jié)果表明多機(jī)器人系統(tǒng)能夠?qū)崿F(xiàn)對(duì)目標(biāo)的穩(wěn)定跟蹤。

1　運(yùn)動(dòng)學(xué)模型

本文在跟隨領(lǐng)航者法[7]思想的基礎(chǔ)上建立機(jī)器人的運(yùn)動(dòng)學(xué)模型。當(dāng)一個(gè)機(jī)器人群體由多個(gè)機(jī)器人組成時(shí)，需指定某個(gè)機(jī)器人為領(lǐng)航機(jī)器人，即 leader，其余機(jī)器人則作為它的跟隨者，也就是followers，要使跟隨機(jī)器人能準(zhǔn)確地跟蹤領(lǐng)航機(jī)器人的位置和方向，則需對(duì)領(lǐng)航機(jī)器人和跟隨機(jī)器人之間的距離和相對(duì)位置角度進(jìn)行合理的計(jì)算，跟隨機(jī)器人通過(guò)得到的距離和角度跟蹤到領(lǐng)航機(jī)器人，根據(jù)領(lǐng)航機(jī)器人與跟隨機(jī)器人之間的相對(duì)位置關(guān)系，可以形成所需隊(duì)形。其運(yùn)動(dòng)學(xué)模型如圖1所示。

圖1　運(yùn)動(dòng)學(xué)模型

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

(1)

其中：vi,wi分別表示機(jī)器人的線速度與角速度。

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

(2)

其中:

對(duì)于 follower機(jī)器人的控制輸出(v2,w2)為：

(3)

其中:

則l-φ的閉環(huán)控制率可以表示為:

(4)

其中:a1,a2為比例控制系數(shù)。

2　協(xié)調(diào)控制策略和算法

在得到多移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型后，還需要設(shè)計(jì)合適的協(xié)調(diào)控制策略和算法來(lái)實(shí)現(xiàn)對(duì)多移動(dòng)機(jī)器人的隊(duì)形控制。本文采用Leader-follower編隊(duì)方法對(duì)多移動(dòng)機(jī)器人的隊(duì)形進(jìn)行控制[8-9]。Leader的運(yùn)動(dòng)遵循基于勢(shì)場(chǎng)的目標(biāo)吸引力原則，引力勢(shì)場(chǎng)為：

(5)

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

(6)

Leader機(jī)器人受到目標(biāo)的引力勢(shì)場(chǎng)向目標(biāo)位置移動(dòng)。

ri=(xi,yi)為機(jī)器人i的位置矢量。

(7)

式中，Ld為機(jī)器人j和機(jī)器人i之間的期望距離，A為常量參數(shù)。

對(duì)(7)式求導(dǎo)，得到關(guān)于l的勢(shì)場(chǎng)力函數(shù)：

(8)

(9)

對(duì)式(9)求導(dǎo)，得到φ的勢(shì)場(chǎng)力函數(shù)為：

(10)

式中，Φd為機(jī)器人j和機(jī)器人i之間的期望夾角,B為常量參數(shù)。

由式(7)、式(9)得到關(guān)于隊(duì)形控制的總勢(shì)場(chǎng)函數(shù)為：

(11)

對(duì)式(11)求導(dǎo)，得出隊(duì)形控制總勢(shì)場(chǎng)力函數(shù)為：

(12)

3　控制器的設(shè)計(jì)

為了使多機(jī)器人系統(tǒng)中的leader快速而準(zhǔn)確地跟蹤確定目標(biāo)，本文采用基于Backstepping控制方法[10]設(shè)計(jì)的控制器。移動(dòng)機(jī)器人對(duì)確定目標(biāo)的跟蹤可描述為尋求一個(gè)控制矩陣K。

(13)

使得v(t)和w(t)的控制：

(14)

對(duì)位姿誤差方程pe求微分，可得移動(dòng)機(jī)器人位姿誤差模型為:

(15)

(16)

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

xe(wye-v+vrcosθe)+ye(-wxe+

(17)

設(shè)計(jì)控制律為：

(18)

將式(18)代入式(17)得：

(19)

4　目標(biāo)跟蹤

4.1目標(biāo)識(shí)別

在SURF特征點(diǎn)的匹配過(guò)程中，采用歐氏距離對(duì)兩個(gè)特征點(diǎn)的相似性度量進(jìn)行計(jì)算，對(duì)于Ms中的任一特征點(diǎn)bi，先求出Fs中與bi的歐氏距離最小的特征點(diǎn)aj,j∈[1,p]，則對(duì)應(yīng)的歐氏距離為：

對(duì)選出的所有匹配特征點(diǎn)求平均得：

(20)

則式(20)就是所要求的目標(biāo)位置。

4.2固定目標(biāo)跟蹤

當(dāng)移動(dòng)機(jī)器人對(duì)固定目標(biāo)進(jìn)行跟蹤時(shí),需先進(jìn)行路徑規(guī)劃,找到到固定目標(biāo)的最優(yōu)路徑,再進(jìn)行目標(biāo)跟蹤，本文采用遺傳算法進(jìn)行路徑規(guī)劃，假設(shè)在路徑規(guī)劃過(guò)程中不存在障礙物。設(shè)路徑點(diǎn)的坐標(biāo)為(xi,yi),路徑最短的適應(yīng)度函數(shù)[13]為：

(21)

其算法的具體步驟為：

1)對(duì)于初始種群的形成，可隨機(jī)地在工作空間中垂直于x1,x2,...,xn的垂線上選擇，本文中將種群規(guī)模取為40；

2)按式(21)可得到群體內(nèi)各個(gè)體的適應(yīng)度，得到相應(yīng)的累計(jì)值為Si，最后一個(gè)累計(jì)值記為Sn；

3)在區(qū)間[0,Sn]內(nèi)產(chǎn)生均勻分布的隨機(jī)數(shù)R；

4)依次用Si與R相比較，當(dāng)出現(xiàn)第一個(gè)Si大于或等于R的個(gè)體i將會(huì)被選為復(fù)制對(duì)象；

5) 重復(fù)3)、4)，直至滿足需要的個(gè)體數(shù)目；

6)對(duì)于復(fù)制得到的個(gè)體，首先進(jìn)行兩兩隨機(jī)配對(duì)，然后按雜交概率Pc，采用雙點(diǎn)交叉對(duì)匹配的個(gè)體進(jìn)行交叉繁殖，產(chǎn)生一對(duì)新的個(gè)體；

7)對(duì)得到的新的個(gè)體加入隨機(jī)擾動(dòng)，本文取[-0.15,0.15]范圍內(nèi)的零均值高斯白噪聲作為擾動(dòng)，找出最優(yōu)解；

8) 終止條件。

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

卡爾曼濾波器[14-15]能快速而準(zhǔn)確地估計(jì)運(yùn)動(dòng)目標(biāo)的運(yùn)動(dòng)參數(shù)。系統(tǒng)的狀態(tài)方程為：

(22)

觀測(cè)方程為:

(23)

Ak為狀態(tài)轉(zhuǎn)移矩陣，ξk為系統(tǒng)動(dòng)態(tài)噪聲矢量，Xk為系統(tǒng)狀態(tài)矢量; Ck為觀測(cè)系數(shù)矩陣,ηk+1為系統(tǒng)觀測(cè)噪聲矢量，Vk+1為系統(tǒng)觀測(cè)矢量,ηk+1和ξk是零均值高斯白噪聲序列，且彼此相互獨(dú)立，協(xié)方差矩陣分別為Q,R。

根據(jù)已知條件，預(yù)測(cè)下一刻系統(tǒng)的狀態(tài)，狀態(tài)向量預(yù)測(cè)方程為：

(24)

狀態(tài)向量協(xié)方差矩陣為：

(25)

其中：Pk和Pk+1|k分別為Xk和Xk+1|k的協(xié)方差矩陣。

根據(jù)預(yù)測(cè)值和測(cè)量值可得現(xiàn)在狀態(tài)的最優(yōu)估計(jì)值為：

(26)

(27)

Kgk為卡爾曼增益。

(28)

其中:I是單位矩陣。

通過(guò)以上公式不斷進(jìn)行遞推運(yùn)算，即可得出卡爾曼濾波的最優(yōu)估計(jì)值。

當(dāng)移動(dòng)機(jī)器人跟蹤運(yùn)動(dòng)目標(biāo)時(shí),假設(shè)運(yùn)動(dòng)目標(biāo)作勻速運(yùn)動(dòng)，因此，運(yùn)動(dòng)目標(biāo)的運(yùn)動(dòng)狀態(tài)可以用其某時(shí)刻在圖像中的位置和速度來(lái)表示。先對(duì)X軸方向的濾波過(guò)程進(jìn)行討論，Y軸方向同理，目標(biāo)物體運(yùn)動(dòng)狀態(tài)方程為:

(29)

式(29)中,xk為運(yùn)動(dòng)目標(biāo)在t=k時(shí)刻時(shí)X軸方向上的位置；vk為運(yùn)動(dòng)目標(biāo)在t=k時(shí)刻X軸方向上的速度；ak為運(yùn)動(dòng)目標(biāo)在t=k時(shí)刻X軸方向上的加速度;T為運(yùn)動(dòng)目標(biāo)在第k幀圖像和k+1幀圖像之間的時(shí)間間隔，akT和vkT為白噪聲序列。將式(29)寫(xiě)為矩陣形式為：

(30)

則系統(tǒng)的狀態(tài)矢量為：

(31)

狀態(tài)轉(zhuǎn)移矩陣為:

(32)

系統(tǒng)的動(dòng)態(tài)噪聲矢量為:

(33)

由于上述的觀測(cè)值是目標(biāo)在圖像中的像素位置,即:

(34)

則卡爾曼濾波器系統(tǒng)觀測(cè)矢量為:

(35)

系統(tǒng)的觀測(cè)系數(shù)矩陣為:

(36)

通過(guò)上述系統(tǒng)建立的狀態(tài)方程和觀測(cè)方程，經(jīng)過(guò)遞推,就可以不斷預(yù)測(cè)目標(biāo)在下一幀圖像中的位置。

在t=k時(shí)刻,利用SURF算法識(shí)別出運(yùn)動(dòng)目標(biāo)在第k幀圖像中的位置記為xk，當(dāng)運(yùn)動(dòng)目標(biāo)第一次出現(xiàn)時(shí),根據(jù)此時(shí)運(yùn)動(dòng)目標(biāo)的觀測(cè)位置x0初始化濾波器:

(37)

在對(duì)角線上對(duì)系統(tǒng)初始狀態(tài)向量協(xié)方差矩陣取較大值，取值根據(jù)實(shí)際測(cè)量情況來(lái)獲得,本文取為:

(38)

系統(tǒng)的動(dòng)態(tài)噪聲協(xié)方差矩陣N取為:

(39)

通過(guò)計(jì)算得到運(yùn)動(dòng)目標(biāo)在下一幀圖像中的預(yù)測(cè)位置X(1|0)。在該位置附近，繼續(xù)對(duì)運(yùn)動(dòng)目標(biāo)在下一幀圖像中的位置進(jìn)行局部搜索,識(shí)別出的運(yùn)動(dòng)目標(biāo)質(zhì)心位置即為X(1|1)。同理，運(yùn)動(dòng)目標(biāo)在第k+1幀圖像中的預(yù)測(cè)位置位為X(k+1|k),在該預(yù)測(cè)位置附近進(jìn)行局部搜索,通過(guò)SURF算法識(shí)別出的目標(biāo)位置坐標(biāo)為X(k+1)。

5　仿真結(jié)果

圖2　固定目標(biāo)跟蹤　　　　　　　　圖3　位姿誤差

圖4～圖6為多移動(dòng)機(jī)器人對(duì)運(yùn)動(dòng)目標(biāo)的跟蹤。假設(shè)動(dòng)態(tài)目標(biāo)的初始位置為(-2 000，500)，運(yùn)動(dòng)速度為v=10m/s,掃描周期T=5s，且運(yùn)動(dòng)軌跡為一條直線，則由圖4可以看出，移動(dòng)機(jī)器人根據(jù)卡爾曼濾波器得出的估計(jì)軌跡跟蹤動(dòng)態(tài)目標(biāo)，由圖5和圖6可以看出根據(jù)所設(shè)計(jì)的控制器位移誤差逐漸趨近于零，最終穩(wěn)定地跟蹤動(dòng)態(tài)目標(biāo)。

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

圖5　橫坐標(biāo)位姿誤差

圖6　縱坐標(biāo)位姿誤差

6　結(jié)語(yǔ)

本文討論了多移動(dòng)機(jī)器人對(duì)固定目標(biāo)和運(yùn)動(dòng)目標(biāo)的跟蹤問(wèn)題，利用SURF算法識(shí)別目標(biāo)，對(duì)固定目標(biāo)的跟蹤，先進(jìn)行路徑規(guī)劃，找出最優(yōu)路徑，機(jī)器人根據(jù)找出的最優(yōu)路徑跟蹤固定目標(biāo)。對(duì)運(yùn)動(dòng)目標(biāo)的跟蹤則采用卡爾曼濾波器得出運(yùn)動(dòng)目標(biāo)的最優(yōu)估計(jì)值，移動(dòng)機(jī)器人根據(jù)得出的估計(jì)值進(jìn)行路徑規(guī)劃，跟蹤動(dòng)態(tài)目標(biāo)，最后通過(guò)MATLAB仿真，仿真結(jié)果表明，通過(guò)所設(shè)計(jì)的控制器和算法能實(shí)現(xiàn)多移動(dòng)機(jī)器人對(duì)固定目標(biāo)與運(yùn)動(dòng)目標(biāo)的跟蹤。該研究對(duì)多移動(dòng)機(jī)器人對(duì)運(yùn)動(dòng)目標(biāo)的跟蹤具有一定的參考價(jià)值，以及為實(shí)際的救援工作提供了模擬平臺(tái)，在后續(xù)的工作中，將對(duì)多移動(dòng)機(jī)器人協(xié)調(diào)的穩(wěn)定性做進(jìn)一步的研究。

[1]ChwaD.Sliding-modetrackingcontrolofnonholonomicwheeledmobilerobotsinpolarcoordinates[J].IEEETransactionsonControlSystemsTechnology，2004，12(4)：637-644.

[2] 戴博，肖曉明，蔡自興.移動(dòng)機(jī)器人路徑規(guī)劃技術(shù)的研究現(xiàn)狀與展望[J].控制工程，2005，12(3)：198-202.

[3]LeeTL,LaiLC，WuCJ.Afuzzyalgorithmfornavigationofmobilerobotsinunknownenvironments[A].ProceedingsoftheIEEEInternationalSymposiumonCircuitsandSystems，Piscataway，NJ,USA：IEEE，2005：3039-3042.

[4] 許偉. 基于激光雷達(dá)環(huán)境信息處理的機(jī)器人定位導(dǎo)航技術(shù)研究[D]. 南京：南京理工大學(xué)，2006.

[5] 伍明，孫繼銀.基于擴(kuò)展式卡爾曼濾波的移動(dòng)機(jī)器人未知環(huán)境下動(dòng)態(tài)目標(biāo)跟蹤[J].機(jī)器人，2010，32(3).

[6] 沈義平.多機(jī)動(dòng)模式下移動(dòng)機(jī)器人目標(biāo)跟蹤控制研究[D].哈爾濱：哈爾濱工程大學(xué)，2013.

[7] 彭林.多移動(dòng)機(jī)器人群集運(yùn)動(dòng)控制研究[D].武漢：華中科技大學(xué)，2006.

[8] 胡瑋韜.多機(jī)器人編隊(duì)及運(yùn)動(dòng)控制研究[D].西安:西安電子科技大學(xué),2010.

[9] 王佳，吳曉蓓，徐志良.一種基于勢(shì)能函數(shù)的多智能體編隊(duì)控制新方法[J].信息與控制，2008,37(3):263-268.

[10] 王家軍.非完整控制系統(tǒng)的非線性控制策略研究[D].天津:天津大學(xué)，2003.

[11] 賈偉，張清，席慶彪，等.基于SURF的UAV快速目標(biāo)識(shí)別算法[J].計(jì)算機(jī)工程與應(yīng)用，2013,49(23)：132-136.

[12] 李鳳英.移動(dòng)機(jī)器人目標(biāo)識(shí)別及跟蹤搬運(yùn)策略研究[D].大連：大連海事大學(xué)，2009.

[13] 劉玲.基于智能計(jì)算的移動(dòng)機(jī)器人路徑規(guī)劃方法研究[D].湖南：湖南大學(xué)，2007.

[14] 劉士榮，孫凱，張波濤，等.基于改進(jìn)Camshift算法的移動(dòng)機(jī)器人運(yùn)動(dòng)目標(biāo)跟蹤[J].華中科技大學(xué)學(xué)報(bào)(自然科學(xué)版)，2011,39(2)：224-226.

[15]ChuiCK,ChenGR. 卡爾曼濾波及其實(shí)時(shí)應(yīng)用[M]. 第4版.北京：清華大學(xué)出版社，2013.

Research on Target Tracking of Multiple Mobile Robots

Meng Jing1,2，Chen Gang1，Gao Xiaoding2

(1.Institute of Electromechanics,Zhejiang Textile Vocational Technology College,Ningbo315200，China; 2.Institute of Electrical and Mechanical Engineering,Xi′an Polytechnic University,Xi′an710000，China)

Aiming at the fixed target and dynamic target tracking problem of the multiple mobile robots is studied. Firstly,the rigid formation and kinematics model of the multiple mobile robots is established that is based on the l-φ closed loop control,the coordination of the multiple mobile robots is achieved under the leader - followers coordination strategy algorithm; And then the target is identified by using SURF algorithm,through the path planning and Kalman filter,the fixed target and dynamic target can be tracked. The controller is designed based on Backstepping,the target can be tracked stably by the multiple mobile robots. Finally,Matlab is used to simulate,the results shows that the tracking errors of the multiple mobile robots is converged to zero quickly under the designed controller and the algorithm,which can be used for the target tracking of the multiple mobile robots.

multiple mobile robots；target tracking；path planning；Kalman filter；Backstepping

1671-4598(2016)04-0141-05DOI：10.16526/j.cnki.11-4762/tp.2016.04.042

TP242

A

2015-09-29；

2015-11-02。

國(guó)家自然科學(xué)基金(61203068);寧波市重大科技攻關(guān)項(xiàng)目(2013B10045);智能紡織機(jī)電技術(shù)研發(fā)(2015B11009)。

孟靜(1988-)，女，陜西榆林人，碩士研究生，主要從事機(jī)器人控制方向的研究。

陳罡(1974-)，男，浙江寧波人，教授，主要從事智能控制方向的研究。