基于雙目視覺(jué)的無(wú)人飛行器目標(biāo)跟蹤與定位

張 梁，徐錦法

南京航空航天大學(xué) 直升機(jī)旋翼動(dòng)力學(xué)國(guó)家級(jí)重點(diǎn)實(shí)驗(yàn)室，南京 210016

基于雙目視覺(jué)的無(wú)人飛行器目標(biāo)跟蹤與定位

張 梁，徐錦法

南京航空航天大學(xué) 直升機(jī)旋翼動(dòng)力學(xué)國(guó)家級(jí)重點(diǎn)實(shí)驗(yàn)室，南京 210016

1 引言

近年來(lái)國(guó)內(nèi)外在攝像機(jī)目標(biāo)跟蹤領(lǐng)域展開(kāi)廣泛研究?；谏士臻g的目標(biāo)識(shí)別與跟蹤[1-4]針對(duì)光照相對(duì)恒定的室內(nèi)環(huán)境以及背景色彩相對(duì)單一的室外環(huán)境比較有利。Teuliere[5]等人使用四旋翼飛行器，搭載下視的單目攝像機(jī)，跟蹤實(shí)驗(yàn)室內(nèi)的紅色模型賽車(chē)，通過(guò)IMU得到飛行器高度，實(shí)現(xiàn)了目標(biāo)跟蹤與定位以及航向角鎖定。Miguel[6]等人同樣使用四旋翼飛行器，搭載前視的單目攝像機(jī)，跟蹤紅色的氣球，通過(guò)計(jì)算特征面積得到相對(duì)距離，使用模糊控制實(shí)現(xiàn)了目標(biāo)定位跟蹤。

Camshift[7]算法是基于顏色的特征跟蹤算法，是Meanshift[8]（均值偏移）算法的擴(kuò)展算法，將Meanshift算法擴(kuò)展到了實(shí)時(shí)連續(xù)圖像跟蹤，且跟蹤窗口可根據(jù)目標(biāo)實(shí)時(shí)縮放，因此具有較好的跟蹤與定位效果，目前應(yīng)用相對(duì)廣泛。

使用單目攝像機(jī)進(jìn)行目標(biāo)定位需借助輔助傳感器進(jìn)行測(cè)距，或者目標(biāo)大小信息已知，因此受限較多。采用雙目攝像機(jī)可避免增加其他方式傳感器，處理方式簡(jiǎn)便，且無(wú)需預(yù)知目標(biāo)尺寸等信息，適用范圍更廣[9-11]。

基于以上特點(diǎn)，本文提出了一種基于Camshift算法的目標(biāo)跟蹤方法，通過(guò)在六旋翼無(wú)人飛行器上搭載雙目視覺(jué)系統(tǒng)（如圖1所示）實(shí)現(xiàn)對(duì)目標(biāo)的測(cè)距與定位，采用卡爾曼濾波算法減小噪聲干擾，使測(cè)距與定位更穩(wěn)定。

圖1 六旋翼實(shí)驗(yàn)平臺(tái)

2 Camshift目標(biāo)跟蹤算法

Camshift（Continuously Adaptive Mean-Shift）算法是連續(xù)自適應(yīng)Meanshift算法的簡(jiǎn)稱(chēng)，以Meanshift算法為核心，實(shí)現(xiàn)圖像任一時(shí)刻的Meanshift迭代計(jì)算，再以此刻得到的目標(biāo)窗口為下一時(shí)刻Meanshift算法搜索窗口初始值。進(jìn)行Meanshift算法計(jì)算前，首先需要得到顏色概率分布圖，將目標(biāo)從原圖像中分離出來(lái)。

2.1 反向投影

得到顏色概率分布圖是進(jìn)行Meanshift算法計(jì)算的前提，由此可從背景中提取出跟蹤物體。通過(guò)對(duì)圖像進(jìn)行反向投影[12]（Back Projection）得到顏色概率分布，即反向投影圖。反向投影反映了原圖像中某像素點(diǎn)與目標(biāo)色彩直方圖模型的對(duì)應(yīng)關(guān)系，直方圖反映了顏色的概率分布，因此反向投影可得到目標(biāo)色彩在原圖像中的概率分布情況。

反向投影過(guò)程如下：

（1）對(duì)原圖像進(jìn)行顏色空間轉(zhuǎn)換，轉(zhuǎn)換至HSV（色度、飽和度、亮度）空間，并提取出H分量。HSV空間可消除亮度對(duì)色彩提取的干擾。

（2）選取目標(biāo)區(qū)域，對(duì)目標(biāo)區(qū)域像素點(diǎn)H分量進(jìn)行統(tǒng)計(jì)，得到色彩直方圖，并量化至0～255。

（3）選取原圖像中某一像素點(diǎn)，將H分量與目標(biāo)色彩直方圖進(jìn)行匹配，并將匹配所得直方圖中的數(shù)值作為反向投影圖中該像素點(diǎn)對(duì)應(yīng)坐標(biāo)下像素點(diǎn)的像素值。

（4）重復(fù)步驟（3），直至原圖像中所有像素點(diǎn)均經(jīng)過(guò)匹配，由此得到反映目標(biāo)顏色概率分布的反向投影圖。

反向投影結(jié)果圖2所示，選取綠色圓形圖案作為跟蹤目標(biāo)，對(duì)其進(jìn)行反向投影，結(jié)果如圖2右圖所示，目標(biāo)被很好地從背景中分離出來(lái)。

圖2 目標(biāo)反向投影結(jié)果

2.2 Meanshift算法

Meanshift算法是均值偏移算法，是跟蹤應(yīng)用領(lǐng)域中尋找數(shù)據(jù)密度分布局部極值的一種迭代方法。

Meanshift算法迭代收斂過(guò)程如圖3所示，其步驟可歸納為：

（1）選擇搜索窗口初始位置；

（2）計(jì)算窗口中像素點(diǎn)重心；

（3）將窗口中心移至重心處；

（4）重復(fù)（2）、（3）步驟直到算法收斂。

圖3 Meanshift算法迭代收斂過(guò)程

窗口中像素點(diǎn)重心可表示如下：

經(jīng)過(guò)Meanshift迭代收斂的窗口中心即為跟蹤目標(biāo)的中心，便可對(duì)目標(biāo)進(jìn)行定位跟蹤。

2.3 Camshift算法

Camshift算法的自適應(yīng)性主要體現(xiàn)在其跟蹤窗口大小可根據(jù)目標(biāo)變化而自動(dòng)調(diào)整。Camshift算法具體步驟如下：

（1）選取待跟蹤目標(biāo)區(qū)域，并計(jì)算此區(qū)域色彩直方圖，同時(shí)設(shè)定初始搜索區(qū)域。

（2）讀取當(dāng)前時(shí)刻原始圖像，經(jīng)過(guò)反向投影得到反向投影圖，從背景中分離出跟蹤目標(biāo)。

（3）使用Meanshift算法收斂搜索區(qū)域。

（4）將收斂區(qū)域集中，調(diào)整跟蹤窗口大小。

（5）重復(fù)（2）～（4）步驟。

當(dāng)目標(biāo)尺度發(fā)生變化時(shí)，Camshift算法可通過(guò)步驟（4）將收斂區(qū)域集中，用以調(diào)整跟蹤窗口大小，以便進(jìn)行自適應(yīng)跟蹤。

3 雙目相機(jī)特征點(diǎn)三維重建

使用雙目攝像機(jī)優(yōu)勢(shì)在于能夠提供對(duì)象的深度信息，實(shí)現(xiàn)像素點(diǎn)三維坐標(biāo)重建。本文所用雙目相機(jī)已進(jìn)行了相機(jī)標(biāo)定，標(biāo)定方法參見(jiàn)文獻(xiàn)[13]，所獲得的圖像也已進(jìn)行了立體校正，標(biāo)定與校正方式參見(jiàn)文獻(xiàn)[12]。經(jīng)過(guò)Bouguet方法校正后兩攝像機(jī)光軸平行并相交于無(wú)窮遠(yuǎn)，同時(shí)兩圖像實(shí)現(xiàn)行對(duì)準(zhǔn)，立體校正為圖像重新選擇圖像中心與邊界，使兩圖像疊加視圖面積最大化，校正后兩攝像機(jī)具有相同主點(diǎn)與焦距，即具有相同的內(nèi)參。

攝像機(jī)模型坐標(biāo)系對(duì)應(yīng)關(guān)系如圖4所示，為方便圖像平面與攝像機(jī)模型坐標(biāo)轉(zhuǎn)換，將攝像機(jī)坐標(biāo)系坐標(biāo)軸取為：X軸與圖像坐標(biāo)系u軸平行，Y軸與圖像坐標(biāo)系v軸平行，Z軸與光心重合。根據(jù)透視投影關(guān)系，基本針孔攝像機(jī)模型的空間任意一點(diǎn)P與圖像平面對(duì)應(yīng)關(guān)系可表示為：

式中zc是攝像機(jī)坐標(biāo)系下Z軸坐標(biāo)；u、v是圖像坐標(biāo)系下像素坐標(biāo)值；dx、dy是圖像坐標(biāo)系下每一個(gè)像素在x軸和y軸方向的物理尺寸，單位為mm；u0、v0是攝像機(jī)光軸與圖像平面的交點(diǎn)坐標(biāo)值；F是相機(jī)焦距；R、t分別是世界坐標(biāo)系相對(duì)于攝像機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣和平移向量；xw、yw、zw分別是該特征點(diǎn)在世界坐標(biāo)系下坐標(biāo)值；αx=F/dx、αy=F/dy；M 稱(chēng)為投影矩陣，而A和[R|t]分別稱(chēng)為攝像機(jī)的內(nèi)參和外參，上標(biāo)～表示齊次向量。

圖4 各坐標(biāo)系間關(guān)系

特征點(diǎn)三維重建時(shí)所用相機(jī)參考坐標(biāo)系為左相機(jī)坐標(biāo)系，即所求得的特征點(diǎn)三維信息是在左相機(jī)坐標(biāo)系下求得的。根據(jù)雙目相機(jī)三角測(cè)量法[14]，如圖5所示，假定兩攝像機(jī)內(nèi)參相同，且兩坐標(biāo)系僅在x軸相差一段距離B，即基線(xiàn)距離，公共視場(chǎng)空間點(diǎn)P(xc，yc，zc)在左右攝像機(jī)坐標(biāo)系下坐標(biāo)分別為P(xc，yc，zc)和P(xc-B，yc，zc)，兩圖像平面對(duì)應(yīng)特征點(diǎn)為 pL(uL，vL)、pR(uR，vR)，圖像已校正，即vL=vR。

圖5 雙目視覺(jué)圖像重建示意圖

由圖5可知，根據(jù)相似三角形定理以及攝像機(jī)投影方程公式（4），存在如下關(guān)系：

由左、右像平面的 pL和 pR坐標(biāo)計(jì)算得到空間點(diǎn)P在攝像機(jī)坐標(biāo)系內(nèi)三維坐標(biāo)值，實(shí)現(xiàn)了特征點(diǎn)三維重建。

4 目標(biāo)跟蹤與定位

Camshift算法可確定目標(biāo)中心在圖像平面中所處位置，對(duì)左右相機(jī)同一時(shí)刻圖像分別運(yùn)行Camshift算法以確定目標(biāo)的左、右中心，通過(guò)雙目三角測(cè)量法確定目標(biāo)在左相機(jī)坐標(biāo)系三維坐標(biāo)。考慮到旋翼飛行器體積不大，假定左相機(jī)坐標(biāo)系與飛行器機(jī)體坐標(biāo)系重合，算法流程圖如圖6所示。

圖6 目標(biāo)跟蹤定位算法流程

根據(jù)三角測(cè)量法可知，目標(biāo)在機(jī)體坐標(biāo)系三維坐標(biāo)可用式（6）表示：

式中，認(rèn)為相機(jī)αx與αy相等，f為相機(jī)焦距，單位為像素。

由圖4可知，通過(guò)相機(jī)光軸與目標(biāo)像素點(diǎn)的偏移可計(jì)算相機(jī)光心與目標(biāo)夾角，由此可計(jì)算得到相機(jī)與目標(biāo)間的相對(duì)偏航角與俯仰角。

利用求得的飛行器與目標(biāo)間的相對(duì)偏航角δψ可校正飛行器的偏航角，使其正視目標(biāo)飛行。機(jī)體坐標(biāo)系下深度信息表征飛行器與目標(biāo)間的相對(duì)距離，利用高度與相對(duì)偏航角可控制目標(biāo)處于飛行器視場(chǎng)內(nèi)。

實(shí)際傳感器測(cè)量存在噪聲干擾等影響，應(yīng)用卡爾曼濾波進(jìn)行數(shù)據(jù)濾波估計(jì)，能減少噪聲干擾，提高數(shù)據(jù)的穩(wěn)定性和測(cè)量精度。所用的線(xiàn)性卡爾曼濾波狀態(tài)方程和觀測(cè)方程為：

式中，狀態(tài)矢量為 X=[xbybzbδψ]T，輸出矢量為Z=[xbybzbδψ]T，狀態(tài)矩陣F及量測(cè)矩陣H均為4維單位矩陣I。狀態(tài)誤差矢量W和量測(cè)誤差矢量V均為高斯白噪聲。

將圖像處理得到的目標(biāo)三維信息作為飛行軌跡控制的負(fù)反饋輸入信號(hào)，可保證飛行器的航向與距離的穩(wěn)定與控制，實(shí)現(xiàn)飛行器自主跟蹤目標(biāo)。調(diào)節(jié)控制器參數(shù)能改善飛行器的操縱響應(yīng)，控制結(jié)構(gòu)框圖如圖7所示，飛行控制系統(tǒng)及控制算法參見(jiàn)文獻(xiàn)[15]，本文不再贅述。

圖7 無(wú)人機(jī)自主飛行跟蹤目標(biāo)控制框圖

5 測(cè)試結(jié)果

為驗(yàn)證所提出算法，應(yīng)用兩臺(tái)低成本USB相機(jī)作為圖像采集設(shè)備，相機(jī)鏡頭焦距8 mm，兩相機(jī)間基線(xiàn)長(zhǎng)度10 cm，圖像尺寸352×288。使用一臺(tái)筆記本電腦作為圖像處理計(jì)算機(jī)，CPU為Intel T7500，主頻達(dá)到2.2 GHz，內(nèi)存為2 GB。運(yùn)用OpenCV 2.4.7代碼庫(kù)，結(jié)合VS2010 C++實(shí)現(xiàn)程序編寫(xiě)，Debug模式下幀頻達(dá)到8 frame/s。六旋翼平臺(tái)采用MH550，飛控系統(tǒng)為自主研發(fā)的自適應(yīng)飛控系統(tǒng)。

如圖8所示為雙目視覺(jué)目標(biāo)跟蹤地面實(shí)驗(yàn)結(jié)果，方框區(qū)域?yàn)槟繕?biāo)區(qū)域，紅點(diǎn)為目標(biāo)左、右中心。圖9所示為一次跟蹤任務(wù)的三維顯示結(jié)果，顯示了目標(biāo)在飛行器坐標(biāo)系下經(jīng)卡爾曼濾波得到的三維位置估計(jì)。圖10所示為此次跟蹤的相對(duì)偏航角變化。圖示結(jié)果表明目標(biāo)運(yùn)動(dòng)軌跡清晰、三維位置估計(jì)值精確。

圖9 經(jīng)卡爾曼濾波的地面跟蹤結(jié)果三維顯示

圖10 跟蹤過(guò)程中相對(duì)偏航角變化

圖11所示為一距離測(cè)量結(jié)果，米尺測(cè)量數(shù)據(jù)與相機(jī)測(cè)量結(jié)果有2 cm誤差，即3.3%誤差，從曲線(xiàn)可以看出，經(jīng)過(guò)卡爾曼濾波后，數(shù)據(jù)更加平滑穩(wěn)定，減少了隨機(jī)噪聲對(duì)測(cè)量的影響，卡爾曼濾波計(jì)算中系統(tǒng)噪聲協(xié)方差矩陣與測(cè)量噪聲協(xié)方差矩陣為Q=0.15×I4×4，R= 1×I4×4。

圖11 測(cè)量過(guò)程穩(wěn)態(tài)誤差

圖12所示為相機(jī)移動(dòng)情況下相機(jī)距離正前方目標(biāo)之間的距離實(shí)測(cè)結(jié)果。圖中可見(jiàn)，相機(jī)測(cè)距跟蹤迅速，誤差控制在2 cm范圍內(nèi)，經(jīng)卡爾曼濾波距離估計(jì)值能滿(mǎn)足飛行器目標(biāo)跟蹤及定位任務(wù)要求。

圖12 距離測(cè)試實(shí)驗(yàn)

如圖13所示為飛行器目標(biāo)跟蹤測(cè)試情況，實(shí)現(xiàn)了六旋翼飛行器視覺(jué)跟蹤定位。實(shí)驗(yàn)時(shí)相機(jī)正視跟蹤目標(biāo)，即相對(duì)偏航角為0°，飛行器距離目標(biāo)2 m，距離地面1 m，機(jī)載前向與地向激光測(cè)距儀，測(cè)量Zb軸目標(biāo)相對(duì)飛行器距離以及飛行器飛行高度（Yb軸），由于目標(biāo)離地高度已知，地向激光測(cè)量值與已知目標(biāo)離地高度之差即為Yb軸目標(biāo)相對(duì)飛行器距離，并將Zb軸與Yb軸目標(biāo)相對(duì)飛行器的距離作為真值。真值與視覺(jué)估計(jì)值的差值即視覺(jué)估計(jì)誤差，Zb軸位置誤差如圖14所示，最大誤差±5 cm，Yb軸位置誤差如圖15所示，最大誤差±3 cm。實(shí)驗(yàn)表明本文算法穩(wěn)定可靠，估計(jì)誤差較小。

圖13 無(wú)人機(jī)視覺(jué)跟蹤測(cè)試

圖14 目標(biāo)跟蹤Zb軸位置誤差

圖15 目標(biāo)跟蹤Yb軸位置誤差

6 結(jié)束語(yǔ)

（1）Camshift算法計(jì)算穩(wěn)定性好，可實(shí)現(xiàn)對(duì)不同尺寸目標(biāo)的跟蹤，能有效擴(kuò)大跟蹤范圍。

（2）基于雙目視覺(jué)的目標(biāo)定位估計(jì)相比于紅外、超聲波等測(cè)距方式，具有較高的性?xún)r(jià)比，且可避免反射波被吸收使測(cè)量失敗等問(wèn)題。

（3）雙目視覺(jué)目標(biāo)跟蹤定位結(jié)合卡爾曼濾波能有效減小噪聲干擾，提高測(cè)量精度，可在雙目視覺(jué)有效距離內(nèi)將精度控制在±5 cm以?xún)?nèi)。

（4）地面和飛行跟蹤實(shí)驗(yàn)結(jié)果表明，本文所提出的目標(biāo)跟蹤定位算法穩(wěn)定可靠，精度較高。

[1]Mohammed A D，Morris T.A robust visual object tracking approach on a mobile device[M]//Information and Communication Technology.Berlin Heidelberg：Springer，2014：190-198.

[2]Hsia C，Liou Y，Chiang J.Directional prediction CamShift algorithm based on adaptive search pattern for moving object tracking[J].Journal of Real-Time Image Processing，2013.

[3]Zin N A M，Abdullah S N H S，Abdullah A.Improved CAMshift based on supervised learning[M]//Robot Intelligence Technology and Applications 2012.Berlin Heidelberg：Springer，2013：611-621.

[4]Hidayatullah P，Konik H.CAMSHIFT improvement on multi-hue object and multi-object tracking[C]//2011 3rd European Workshop on Visual Information Processing（EUVIP），2011：143-148.

[5]Teuliere C，Eck L，Marchand E.Chasing a moving target from a flying UAV[C]//2011 IEEE/RSJ International Conference on Intelligent Robots and Systems（IROS），2011：4929-4934.

[6]Olivares-Mendez M A，Mondragon I，Campoy Cervera P，et al.Aerial object following using visual fuzzy servoing[C]// Proceedings of the 1st Workshop on Research，Development and Education on Unmanned Aerial Systems（RED-UAS 2011），2011：61-70.

[7]Bradski G R.Computer vision face tracking for use in a perceptual user interface[J].Intel Technology Journal，1998.

[8]Comaniciu D，Meer P.Mean shift：a robust approach toward feature space analysis[J].IEEE Transactions on Pattern Analysis and Machine Intelligence，2002，24（5）：603-619.

[9]Stefanik K V，Gassaway J C，Kochersberger K，et al.UAV-based stereo vision for rapid aerial terrain mapping[J]. GIScience&Remote Sensing，2011，48（1）：24-49.

[10]Shabayek A E R，Demonceaux C，Morel O，et al.Vision based uav attitude estimation：progress and insights[J]. Journal of Intelligent&Robotic Systems，2012，65（1）：295-308.

[11]Kitt B，Geiger A，Lategahn H.Visual odometry based on stereo image sequences with ransac-based outlier rejection scheme[C]//Intelligent Vehicles Symposium（IV），2010：486-492.

[12]Bradski G，Kaehler A.Learning OpenCV：computer vision with the OpenCV library[M].Sebastopol：O’Reilly Media，Incorporated，2008：350-380.

[13]Zhang Z.A flexible new technique for camera calibration[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence，2000，22（11）：1330-1334.

[14]馬頌德，張正友.計(jì)算機(jī)視覺(jué)：計(jì)算理論與算法基礎(chǔ)[M].北京：科學(xué)出版社，1998：52-94.

[15]夏青元，徐錦法.三軸式無(wú)人旋翼飛行器及自適應(yīng)飛行控制系統(tǒng)設(shè)計(jì)[J].航空學(xué)報(bào)，2013，34（3）：495-508.

ZHANG Liang,XU Jinfa

National Key Laboratory of RotorcraftAeromechanics,Nanjing University ofAeronautics&Astronautics,Nanjing 210016,China

According to the problem of target tracking for UAV,a target tracking and locating algorithm based on Camshift algorithm and binocular vision is proposed.The left and right images from binocular camera are used to calculate the central interesting point of the target with Camshift algorithm.After the central interesting point is reconstructed,the relative position and yaw angle between UAV and target under the body coordinate system are got.Data is optimized with the kalman filter. The estimated data is used as the flight control system loopback input data to achieve the autonomous tracking of UAV. The result of the experiment shows that the error of the algorithm is little.This algorithm has strong stability and accuracy.

Unmanned Aerial Vehicle（UAV）;binocular stereo vision;target tracking;locating

針對(duì)小型無(wú)人飛行器跟蹤目標(biāo)的問(wèn)題，提出了一種基于雙目視覺(jué)和Camshift算法的無(wú)人飛行器目標(biāo)跟蹤以及定位算法。雙目相機(jī)得到的左右圖像通過(guò)Camshift算法處理可得到目標(biāo)中心特征點(diǎn)，對(duì)目標(biāo)中心特征點(diǎn)進(jìn)行三維重建，得到機(jī)體坐標(biāo)系下無(wú)人飛行器與目標(biāo)間的相對(duì)位置和偏航角，應(yīng)用卡爾曼濾波算法對(duì)測(cè)量值進(jìn)行了優(yōu)化，將所得估計(jì)值作為飛行控制系統(tǒng)的反饋輸入值，實(shí)現(xiàn)了無(wú)人飛行器自主跟蹤飛行。結(jié)果表明所提算法誤差較小，具有較高的穩(wěn)定性與精確性。

無(wú)人飛行器；雙目視覺(jué)；目標(biāo)跟蹤；定位

A

V249.1

10.3778/j.issn.1002-8331.1405-0263

ZHANG Liang,XU Jinfa.Target tracking and locating for UAV based on binocular stereo vision.Computer Engineering and Applications,2014,50（24）：27-31.

江蘇高校優(yōu)勢(shì)學(xué)科建設(shè)工程資助項(xiàng)目。

張梁（1984—），男，博士研究生，研究領(lǐng)域?yàn)闊o(wú)人飛行器視覺(jué)導(dǎo)引與組合導(dǎo)航；徐錦法（1963—），男，博士，教授，研究領(lǐng)域?yàn)闊o(wú)人飛行器飛行控制與組合導(dǎo)航。E-mail：xjfae@nuaa.edu.cn

2014-05-21

2014-07-14

1002-8331（2014）24-0027-05

CNKI網(wǎng)絡(luò)優(yōu)先出版：2014-08-19，http∶//www.cnki.net/kcms/doi/10.3778/j.issn.1002-8331.1405-0263.html