張立國, 李曉松, 肖 磊, 金 梅, 董 浩(. 燕山大學(xué) 河北省測試計(jì)量技術(shù)與儀器重點(diǎn)實(shí)驗(yàn)室, 河北 秦皇島 066004;. 河北省自動(dòng)化研究所, 河北 石家莊 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)簡單、機(jī)動(dòng)能力強(qiáng)、可垂直升降,特別適合對(duì)室內(nèi)、隱蔽環(huán)境中移動(dòng)目標(biāo)的監(jiān)視、偵查、跟蹤。國內(nèi)外已經(jīng)就無人機(jī)對(duì)移動(dòng)目標(biāo)的跟蹤展開了大量的研究并提出多種跟蹤方法[1],包括Meanshift算法[2]、Camshift算法[3]、光流跟蹤算法[4]、HOG跟蹤算法[5]、TLD跟蹤算法[6]等。吳鳳和采用灰度閾值法進(jìn)行圖像分割,實(shí)現(xiàn)具有單像素邊緣的圖像輪廓提取[7];黃桂平對(duì)單目視覺測量方法進(jìn)行研究、分析[8];呂強(qiáng)利用四旋翼實(shí)現(xiàn)了室內(nèi)路徑識(shí)別與路徑跟蹤[9];Pestana采用TLD算法實(shí)現(xiàn)了人物識(shí)別和跟蹤[10]。本文提出一種新的跟蹤策略,目的是克服跟蹤過程中跟蹤目標(biāo)顏色與周圍環(huán)境顏色相近時(shí)的干擾情況。

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

2 視頻中移動(dòng)目標(biāo)信息估計(jì)

2.1 卡爾曼濾波運(yùn)動(dòng)目標(biāo)跟蹤

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

依據(jù)卡爾曼濾波原理,首先引入一個(gè)離散時(shí)間過程控制系統(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時(shí)刻的系統(tǒng)狀態(tài);uk為k時(shí)刻對(duì)系統(tǒng)的控制量;wk,vk分別為系統(tǒng)噪聲、觀測噪聲;zk為k時(shí)刻的測量值。

系統(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的其它時(shí)刻;δki表示Kronecher函數(shù)。式(3)和式(4)中,白噪聲矩陣Q和R由協(xié)方差組成,因此也稱其為協(xié)方差矩陣。由式(5)可知,wk,vk是相互獨(dú)立的。

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

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

(6)

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

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

(7)

卡爾曼增益計(jì)算:

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

(8)

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

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

(9)

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

(10)

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

(11)

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

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

(12)

(13)

(14)

(15)

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

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

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

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

(16)

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

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

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

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

(17)

觀測方程為:

(18)

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

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

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

圖2 各個(gè)時(shí)刻估計(jì)偏差對(duì)比

2.2 顏色空間選擇

計(jì)算機(jī)視覺最常用的顏色空間模型有RGB,YUV,HSV,HSI等模型。其中R,G,B為紅、綠、藍(lán)3個(gè)通道顏色的值。

在目標(biāo)跟蹤過程中,若背景亮度隨光照發(fā)生變化時(shí),如出現(xiàn)目標(biāo)不跟隨的現(xiàn)象,本質(zhì)上是因?yàn)镽GB對(duì)光照反映敏感的原因。HSV在計(jì)算機(jī)視覺、目標(biāo)跟蹤系統(tǒng)中經(jīng)常被使用。其中H、S、V為色調(diào)(hue)、飽和度(saturation)、明亮值(value)的值。HSV顏色空間的H的取值范圍是0°～360°,其中紅色是0°,綠色是120°,藍(lán)色是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色彩空間時(shí)，為了顯示深度圖像，將其規(guī)范化為像素值是0～255范圍的偽灰度圖像。

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

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

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

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

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

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

(22)

其中零階矩為:

(23)

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

一階矩為:

(24)

(25)

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

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

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

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

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

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

角速度ω為:

(26)

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

3 實(shí)驗(yàn)結(jié)果與分析

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

圖5 去噪前后的圖像

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

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

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

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

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

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

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

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

4 結(jié) 論

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

[參考文獻(xiàn)]

[1] 李培華. 序列圖像中運(yùn)動(dòng)目標(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)和. 基于計(jì)算機(jī)視覺測量技術(shù)的圖像輪廓提取方法研究[J]. 計(jì)量學(xué)報(bào), 2007, 28(1):18-22.

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

[9] 呂強(qiáng), 倪佩佩, 劉峰. 基于視覺的四旋翼飛行器路徑跟蹤[J]. 四川兵工學(xué)報(bào), 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]. 計(jì)量學(xué)報(bào),2016, 37(6): 582-586.

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