基于單目視覺的四旋翼飛行器目標(biāo)跟蹤算法研究

張立國, 李曉松, 肖 磊, 金 梅, 董 浩(. 燕山大學(xué) 河北省測試計量技術(shù)與儀器重點實驗室, 河北 秦皇島 066004;. 河北省自動化研究所, 河北 石家莊 050000)

1 引 言

無人飛行器(unmanned aerial vehicle,UAV)是由集成慣性導(dǎo)航系統(tǒng)(inertial navigation system,INS)、全球定位系統(tǒng)(global position system,GPS)、圖像采集系統(tǒng)、控制器等組成的復(fù)雜系統(tǒng)。四旋翼飛行器結(jié)構(gòu)簡單、機動能力強、可垂直升降,特別適合對室內(nèi)、隱蔽環(huán)境中移動目標(biāo)的監(jiān)視、偵查、跟蹤。國內(nèi)外已經(jīng)就無人機對移動目標(biāo)的跟蹤展開了大量的研究并提出多種跟蹤方法[1],包括Meanshift算法[2]、Camshift算法[3]、光流跟蹤算法[4]、HOG跟蹤算法[5]、TLD跟蹤算法[6]等。吳鳳和采用灰度閾值法進行圖像分割,實現(xiàn)具有單像素邊緣的圖像輪廓提取[7];黃桂平對單目視覺測量方法進行研究、分析[8];呂強利用四旋翼實現(xiàn)了室內(nèi)路徑識別與路徑跟蹤[9];Pestana采用TLD算法實現(xiàn)了人物識別和跟蹤[10]。本文提出一種新的跟蹤策略,目的是克服跟蹤過程中跟蹤目標(biāo)顏色與周圍環(huán)境顏色相近時的干擾情況。

本文側(cè)重實現(xiàn)對視頻中移動單目標(biāo)的跟蹤,實驗中使用的無人機為Parrot生產(chǎn)的AR.drone 2.0。該四旋翼無人機搭載前置單目攝像頭,地面站通過WIFI連接AR.drone,并采用跨平臺計算機視覺庫(open source computer vision library，OpenCV)結(jié)合交互多模型卡爾曼濾波[11]算法進行視頻中移動目標(biāo)位置的預(yù)測與修正,實現(xiàn)對移動目標(biāo)的跟蹤[12～15]。

2 視頻中移動目標(biāo)信息估計

2.1 卡爾曼濾波運動目標(biāo)跟蹤

卡爾曼濾波算法引入狀態(tài)空間的概念,可以對當(dāng)前、過去狀態(tài)進行估計,特別適用于變量多、時變系統(tǒng),是線性系統(tǒng)中最常用的一種最優(yōu)估計算法,可以較精確地估計視頻中目標(biāo)的位置與速度信息。

依據(jù)卡爾曼濾波原理,首先引入一個離散時間過程控制系統(tǒng),該系統(tǒng)可用線性差分方程表示為:

xk+1=Axk+Buk+wk

(1)

zk=Hxk+vk

(2)

式中:A,B,H分別為系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣、輸入增益矩陣、觀測矩陣;xk為k時刻的系統(tǒng)狀態(tài);uk為k時刻對系統(tǒng)的控制量;wk,vk分別為系統(tǒng)噪聲、觀測噪聲;zk為k時刻的測量值。

系統(tǒng)噪聲和觀測噪聲滿足:

E(wk)=0, cov(wk,wi)=Qkδki

(3)

E(vk)=0, cov(vk,vi)=Rkδki

(4)

cov(wk,vi)=0

(5)

式中:cov為協(xié)方差;E為期望;Qk為系統(tǒng)噪聲wk的白噪聲矩陣;Rk為觀測噪聲vk的白噪聲矩陣;i為不同于k的其它時刻;δki表示Kronecher函數(shù)。式(3)和式(4)中,白噪聲矩陣Q和R由協(xié)方差組成,因此也稱其為協(xié)方差矩陣。由式(5)可知,wk,vk是相互獨立的。

卡爾曼濾波算法大體上分為2個階段,即預(yù)測階段和修正階段。預(yù)測當(dāng)前時刻的系統(tǒng)狀態(tài),并結(jié)合當(dāng)前系統(tǒng)的觀測值預(yù)測當(dāng)前時刻的真實狀態(tài)。卡爾曼濾波遞推公式如下。

狀態(tài)預(yù)測方程:

(6)

誤差協(xié)方差預(yù)測:

Pk|k-1=APk-1AT+Q

(7)

卡爾曼增益計算:

Kk=Pk|k-1HT(HPk|k-1HT+R)-1

(8)

誤差協(xié)方差修正:

Pk=(I-KkH)Pk|k-1

(9)

狀態(tài)估計方程:

(10)

當(dāng)所研究的目標(biāo)為非機動目標(biāo)時,狀態(tài)方程可以簡化為:

(11)

將式(7)～式(11)組合成方程組,即為目標(biāo)運動狀態(tài)的卡爾曼濾波方程。

利用AR.drone四旋翼飛行器自帶的單目攝像頭采集得到的視頻序列,相鄰幀時間間隔為ΔT,以視頻圖像中移動目標(biāo)在X,Y軸的位置和速度作為狀態(tài)變量,以圖像中移動目標(biāo)的位置為觀測值進行目標(biāo)信息的估計,卡爾曼濾波的初值和噪聲參數(shù)等是經(jīng)過反復(fù)試驗后選定的,適合跟蹤做勻速運動的物體的狀態(tài)。轉(zhuǎn)移矩陣、觀測矩陣、協(xié)方差矩陣設(shè)置為:

(12)

(13)

(14)

(15)

常規(guī)的卡爾曼濾波器使用線性隨機差分方程來描述離散時間過程的狀態(tài)變量;對于線性高斯模型卡爾曼濾波器通過有限維統(tǒng)計量來精確計算后驗分布;當(dāng)應(yīng)用系統(tǒng)不是線性系統(tǒng)時可以采用擴展卡爾曼濾波器或者無跡卡爾曼濾波器。在卡爾曼濾波算法中的狀態(tài)轉(zhuǎn)移方程和觀測方程的被估計量都是隨時間變化的,僅采用基本的卡爾曼濾波算法往往不能夠完全覆蓋目標(biāo)的所有運動狀態(tài);交互多模型(IMM)算法使用兩個或者更多的模型描述工作過程的可能狀態(tài),并通過加權(quán)融合進行系統(tǒng)的狀態(tài)估計;采用合適的四旋翼解算算法可以提高解算精度;采用交互濾波器進行姿態(tài)解算能夠同時降低低頻和高頻干擾;梯度下降法需要對多向量函數(shù)求導(dǎo),利用梯度下降法可以求得一組姿態(tài)四元數(shù),但是梯度下降法的步長非動態(tài),當(dāng)步長過大時會導(dǎo)致估計不準(zhǔn)確,特別是當(dāng)高速運動時,容易導(dǎo)致延遲較大、姿態(tài)解算失敗;基于反步法的四旋翼軌跡跟蹤,將四旋翼控制系統(tǒng)分為前后、左右、上下、偏航4個子系統(tǒng),但是不能消除由于模型參數(shù)不確定性帶來的影響,飛行質(zhì)量、轉(zhuǎn)動慣量與飛行器所受的干擾往往是不確定的。

IMM算法可以根據(jù)實際的運動狀況實現(xiàn)模型間的轉(zhuǎn)換,因此本文采用IMM算法作為目標(biāo)跟蹤的算法。

假定目標(biāo)有r種運動狀態(tài),對應(yīng)著r個運動模型,則可以由目標(biāo)狀態(tài)方程表示的第j個模型為:

xj(k+1)=Fjxj(k)+Gj(k)wj(k)

(16)

式中:Gj(k)為驅(qū)動噪聲矩陣;wj(k)為均值為零、協(xié)方差矩陣為Fj的白噪聲序列。

測量方程見式(2)所示。

對無跡卡爾曼濾波算法(UKF)和擴展卡爾曼濾波算法(EKF)分別采用交互多模型卡爾曼濾波算法進行分析,2個模型分別為IMM-UKF和IMM-EKF。在非線性條件下對兩種算法的濾波效果進行仿真分析。

假如狀態(tài)方程為:

(17)

觀測方程為:

(18)

圖1中的仿真結(jié)果是IMM-UKF和IMM-EKF在各個時刻對真實狀態(tài)的估計結(jié)果,圖2對比了各個時刻的IMM-UKF和IMM-EKF估計偏差結(jié)果。從圖中結(jié)果可以看出IMM-UKF的估計結(jié)果比IMM-EKF估計結(jié)果要精確。

結(jié)合圖1和圖2的仿真結(jié)果可知,在非線性條件的大多數(shù)情況下,IMM-UKF的濾波誤差比IMM-EKF的濾波誤差要小,所以在狀態(tài)估計的準(zhǔn)確性上IMM-UKF要明顯優(yōu)于IMM-EKF,IMM-UKF具有更高的濾波精度。采用基于交互多模型的卡爾曼濾波算法,不僅對做勻速直線運動和圓周運動的目標(biāo)可實現(xiàn)跟蹤,而且當(dāng)運動模型發(fā)生變化時仍能夠穩(wěn)定地跟蹤。

圖1 2種狀態(tài)估計結(jié)果對比

圖2 各個時刻估計偏差對比

2.2 顏色空間選擇

計算機視覺最常用的顏色空間模型有RGB,YUV,HSV,HSI等模型。其中R,G,B為紅、綠、藍3個通道顏色的值。

在目標(biāo)跟蹤過程中,若背景亮度隨光照發(fā)生變化時,如出現(xiàn)目標(biāo)不跟隨的現(xiàn)象,本質(zhì)上是因為RGB對光照反映敏感的原因。HSV在計算機視覺、目標(biāo)跟蹤系統(tǒng)中經(jīng)常被使用。其中H、S、V為色調(diào)(hue)、飽和度(saturation)、明亮值(value)的值。HSV顏色空間的H的取值范圍是0°～360°,其中紅色是0°,綠色是120°,藍色是240°;飽和度S的取值范圍是0～1,值越大色彩越純,值越低色彩逐漸變灰;明亮值V的取值范圍是0～1,值越大色彩越亮。RGB模型到HSV模型的轉(zhuǎn)換公式為:

V=max(R,G,B)

(19)

(20)

(21)

如果H<0°,H=H+360°,將色度轉(zhuǎn)換到0°～360°之內(nèi)。利用OpenCV的CV_BGR2HSV_FULL顏色轉(zhuǎn)換模型將RGB色彩空間轉(zhuǎn)換到HSV色彩空間時，為了顯示深度圖像，將其規(guī)范化為像素值是0～255范圍的偽灰度圖像。

2.3 目標(biāo)識別

為了能夠使目標(biāo)能夠從背景中提取出來,需要對圖像進行去噪,經(jīng)過OpenCV形態(tài)學(xué)變換的閉運算消除小型黑洞連接一些缺口,使物體輪廓光滑達到去噪的效果。運動目標(biāo)物體的檢測算法有基于特征的算法、基于光流的算法、基于輪廓的算法,其中基于特征算法的特征提取和匹配過程中計算量較大;基于光流的算法運算量較大,無法保證實時性。本文采用基于目標(biāo)輪廓的算法。通過OpenCV的cvFindContours檢索所有目標(biāo)區(qū)域二圖像的輪廓并分為兩層,頂層為各部分的外部輪廓,第二層為空洞的邊界。

在cvFindContours中Image的輸入圖像為8位的單通道二值圖像,非零像素作為1處理,零像素保持不變。Storage用來存儲查找到的輪廓;first_contour指向第一個外部輪廓的地址;header_size用于比較序列的尺寸。Mode檢索決定了檢索的輪廓的方式。檢索方式共有4種,本文選擇的是CV_RETR_CCOMP,檢索所有的輪廓,并將他們組織為兩層。頂層為所有物體的外邊界,底層是孔的外部邊界。Method有5個值,其中本文用到的V_CHAIN_APPROX_SIMPLE:壓縮水平的、垂直的和斜的部分,只保留最后終點。輪廓可以使用圓形或者矩形,本文使用矩形來描述檢測的目標(biāo)輪廓。

采用自動閾值的方法對圖像的灰度進行相應(yīng)分析,選定一個初始值并結(jié)合算法不斷調(diào)整這個估計值,直到找到合適值為止。

步驟如下:選取跟蹤目標(biāo)的一個灰度值作為最初閾值D0;根據(jù)閾值D0把圖像分割成2個區(qū)域,并計算兩個區(qū)域灰度均值μ1和μ2,進而計算出新的閾值Di+1=0.5×(μ1+μ2);反復(fù)計算直到Di+1和Di的差值小于某個給定值。

比較感興趣區(qū)域中各個輪廓的面積大小,找到最大輪廓,然后對移動目標(biāo)進行分析,計算區(qū)域的質(zhì)心(x0,y0),在得到質(zhì)心值的基礎(chǔ)上通過卡爾曼濾波對其進行修正,即利用對當(dāng)前時刻的先驗估計及當(dāng)前實際觀測值的分析,得到當(dāng)前時刻狀態(tài)的后驗估計;第二步,進行目標(biāo)運動狀態(tài)的預(yù)測,指的是用當(dāng)前狀態(tài)及誤差協(xié)方差推算出下一時刻狀態(tài)的先驗估計;在完成修正和預(yù)測之后,使用后驗估計進行預(yù)測下一時刻的先驗估計。重復(fù)進行如上的步驟,直到收斂為止。質(zhì)心坐標(biāo)計算公式為:

(22)

其中零階矩為:

(23)

式中:I(x,y)為坐標(biāo)(x,y)的像素值。

一階矩為:

(24)

(25)

2.4 飛行速度與方向設(shè)置

AR.drone的最大飛行速度5 m/s,最大飛行速度下可飛行13 min,其中前置攝像頭的分辨力為640×320像素,可以獲取清晰的視頻圖像。

本文采用跟隨移動且顏色為紅色的物體,圖3為無人機的機身坐標(biāo)系。

圖3 AR.drone機身坐標(biāo)系

為了保持無人機能夠穩(wěn)定、持續(xù)地跟蹤,設(shè)置沿x軸方向的速度為0.1 m/s,同時計算四旋翼飛機的角速度。設(shè)置角速度為ω,ω的大小由圍繞它做勻速圓周運動的移動物體在圖像中的位置決定,如圖4所示。

圖4 跟蹤移動目標(biāo)預(yù)測位置與視頻中心差Δy

角速度ω為:

(26)

式中:w為跟蹤圖像的寬度;Δy為跟蹤目標(biāo)變化的水平距離。

3 實驗結(jié)果與分析

圖5展示的是跟蹤目標(biāo)去噪前后HSV顏色空間的圖像,其中圖5(a)為去噪前的圖像,圖5(b)為去噪后的圖像。表1給出了多種濾波算法濾波時間的對比結(jié)果。

圖5 去噪前后的圖像

表1 算法濾波時間統(tǒng)計 s

粒子濾波在計算過程中需要將粒子進行迭代計算,重采樣后加權(quán)得到結(jié)果。因此粒子濾波的計算時間明顯大于卡爾曼濾波?？柭鼮V波算法以及卡爾曼濾波算法的改進算法擴展卡爾曼、無跡卡爾曼均能在較短時間內(nèi)實現(xiàn)濾波,結(jié)合表1可以看出本文采用的交互卡爾曼濾波的濾波時間明顯快于粒子濾波,同卡爾曼濾波的改進算法的濾波時間相當(dāng)。滿足實時性、快速性跟蹤目標(biāo)的要求。

目標(biāo)跟蹤中,Camshift是利用顏色信息對移動目標(biāo)進行跟蹤的算法,圖6展示的是使用該算法跟蹤目標(biāo)的效果。圖7是采用本文算法跟蹤移動目標(biāo)的效果。

圖6 Camshift算法跟蹤移動目標(biāo)

圖7 本文算法跟蹤移動目標(biāo)

比較采用Camshift算法同本文算法跟蹤移動目標(biāo)的效果,可以看出:在第638,1150,1255幀,由于背景顏色與跟蹤目標(biāo)顏色比較接近,采用的Camshift算法出現(xiàn)了失跟現(xiàn)象。兩種算法的追蹤成功率比較如表2所示。

采用的OpenCV同交互多模型卡爾曼濾波結(jié)合的跟蹤算法,有效地檢測到了移動目標(biāo)的輪廓并利用卡爾曼濾波對移動目標(biāo)的移動狀態(tài)進行估計,當(dāng)系統(tǒng)處于非線性、非高斯性時能夠有效地保證跟蹤的準(zhǔn)確性、實時性和穩(wěn)定性,即使是在受到背景顏色干擾的情況下仍能夠持續(xù)、穩(wěn)定地對移動目標(biāo)進行跟蹤,從而較好地保持了跟蹤的魯棒性與實時性。

表2 算法在背景顏色相近下的追蹤成功率

4 結(jié) 論

常用的Camshift算法在背景光照發(fā)生變化或者跟蹤目標(biāo)與背景顏色接近時無法保證跟蹤的魯棒性,容易出現(xiàn)跟蹤目標(biāo)丟失的現(xiàn)象。本文提出了采用OpenCV對四旋翼AR.drone采集得到的視頻圖像進行空間變換、去噪,實現(xiàn)了對移動目標(biāo)的識別。利用交互多模型卡爾曼對移動目標(biāo)位置進行預(yù)測可以縮短圖像處理范圍的特點,設(shè)計了相應(yīng)的四旋翼跟隨移動目標(biāo)的跟蹤算法。通過地面站控制平臺控制AR.drone實現(xiàn)了對移動目標(biāo)的跟蹤,而且滿足了快速、穩(wěn)健跟蹤的要求,避免了由于背景顏色、周圍顏色與跟蹤目標(biāo)顏色相近導(dǎo)致出現(xiàn)的跟蹤失敗現(xiàn)象。下一步將研究在出現(xiàn)遮擋、干擾的情況下如何實現(xiàn)四旋翼飛行器對移動目標(biāo)的穩(wěn)健、快速跟蹤。

[參考文獻]

[1] 李培華. 序列圖像中運動目標(biāo)跟蹤方法[M]. 北京:科學(xué)出版社, 2010.

[2] Comaniciu D, Ramesh V, Meer P. Real-time tracking of non-rigid objects using Mean Shift[C]//Proceedings of IEEE Computer Society Conference on Computer Vision and Pattern Recognition,New York:IEEE Press, 2000, 2:142-149.

[3] Nouar O D, Ali G, Raphael C. Improved object tracking with Camshift algorithm[C]//Proceedings of the IEEE International Conference on Acoustics,Speech,and Signal Processing. Toulouse,France:IEEE,2006.2(II):657-660.

[4] Baker S, Matthews I. Lucas-Kanade 20 Years On:A Unifying Framework[J].InternationalJournalofComputerVision,2004,56(3):221-255.

[5] Dalal N,Triggs B. Histograms of oriented gradients forhuman detection[J].IEEEConferenceonComputerVision&PatternRecognition, 2005, 1(12):886-893.

[6] Kalal Z, Mikolajczyk K, Matas J. Tracking learningdetection[J].PatternAnalysisandMachineIntelligence,IEEETransactionson, 2012, 34(7):1409-1422.

[7] 吳風(fēng)和. 基于計算機視覺測量技術(shù)的圖像輪廓提取方法研究[J]. 計量學(xué)報, 2007, 28(1):18-22.

[8] 黃桂平, 李廣云, 王保豐. 單目視覺測量技術(shù)研究[J]. 計量學(xué)報, 2004, 25(4):314-317.

[9] 呂強, 倪佩佩, 劉峰. 基于視覺的四旋翼飛行器路徑跟蹤[J]. 四川兵工學(xué)報, 2014, 35(11):106-109.

[10] Pestana J, Sanchez-Lopez J, Saripalli S,etal. Computer vision based general object following for GPS-denied multirotor unmanned vehicles[C]// American Control Conference (ACC). Oregon,USA:IEEE,2014.

[11] Hajiyev C, Soken H E. Robust Adaptive Kalman Filter for estimation of UAV dynamics in the presence of sensor/actuator faults[J].AerospaceScienceandTechnology, 2013, 28(1):376-383.

[12] 權(quán)太范. 目標(biāo)跟蹤新理論[M]. 北京:國防工業(yè)出版社, 2009.

[13] 黃小平. 卡爾曼濾波原理及其應(yīng)用[M]. 北京:電子工業(yè)出版社, 2015.

[14] 田源,王洪濤. 基于量子核聚類算法的圖像邊緣特征提取研[J]. 計量學(xué)報,2016, 37(6): 582-586.

[15] 仝曉龍, 竇騰飛, 劉艷紅. 運動目標(biāo)跟蹤技術(shù)綜述[J]. 科技創(chuàng)新與應(yīng)用, 2014,(18):18-20.