基于雙目視覺的無人飛行器目標跟蹤與定位

張 梁，徐錦法

南京航空航天大學 直升機旋翼動力學國家級重點實驗室，南京 210016

基于雙目視覺的無人飛行器目標跟蹤與定位

張 梁，徐錦法

南京航空航天大學 直升機旋翼動力學國家級重點實驗室，南京 210016

1 引言

近年來國內外在攝像機目標跟蹤領域展開廣泛研究。基于色彩空間的目標識別與跟蹤[1-4]針對光照相對恒定的室內環境以及背景色彩相對單一的室外環境比較有利。Teuliere[5]等人使用四旋翼飛行器，搭載下視的單目攝像機，跟蹤實驗室內的紅色模型賽車，通過IMU得到飛行器高度，實現了目標跟蹤與定位以及航向角鎖定。Miguel[6]等人同樣使用四旋翼飛行器，搭載前視的單目攝像機，跟蹤紅色的氣球，通過計算特征面積得到相對距離，使用模糊控制實現了目標定位跟蹤。

Camshift[7]算法是基于顏色的特征跟蹤算法，是Meanshift[8]（均值偏移）算法的擴展算法，將Meanshift算法擴展到了實時連續圖像跟蹤，且跟蹤窗口可根據目標實時縮放，因此具有較好的跟蹤與定位效果，目前應用相對廣泛。

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

基于以上特點，本文提出了一種基于Camshift算法的目標跟蹤方法，通過在六旋翼無人飛行器上搭載雙目視覺系統（如圖1所示）實現對目標的測距與定位，采用卡爾曼濾波算法減小噪聲干擾，使測距與定位更穩定。

圖1 六旋翼實驗平臺

2 Camshift目標跟蹤算法

Camshift（Continuously Adaptive Mean-Shift）算法是連續自適應Meanshift算法的簡稱，以Meanshift算法為核心，實現圖像任一時刻的Meanshift迭代計算，再以此刻得到的目標窗口為下一時刻Meanshift算法搜索窗口初始值。進行Meanshift算法計算前，首先需要得到顏色概率分布圖，將目標從原圖像中分離出來。

2.1 反向投影

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

反向投影過程如下：

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

（2）選取目標區域，對目標區域像素點H分量進行統計，得到色彩直方圖，并量化至0～255。

（3）選取原圖像中某一像素點，將H分量與目標色彩直方圖進行匹配，并將匹配所得直方圖中的數值作為反向投影圖中該像素點對應坐標下像素點的像素值。

（4）重復步驟（3），直至原圖像中所有像素點均經過匹配，由此得到反映目標顏色概率分布的反向投影圖。

反向投影結果圖2所示，選取綠色圓形圖案作為跟蹤目標，對其進行反向投影，結果如圖2右圖所示，目標被很好地從背景中分離出來。

圖2 目標反向投影結果

2.2 Meanshift算法

Meanshift算法是均值偏移算法，是跟蹤應用領域中尋找數據密度分布局部極值的一種迭代方法。

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

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

（2）計算窗口中像素點重心；

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

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

圖3 Meanshift算法迭代收斂過程

窗口中像素點重心可表示如下：

經過Meanshift迭代收斂的窗口中心即為跟蹤目標的中心，便可對目標進行定位跟蹤。

2.3 Camshift算法

Camshift算法的自適應性主要體現在其跟蹤窗口大小可根據目標變化而自動調整。Camshift算法具體步驟如下：

（1）選取待跟蹤目標區域，并計算此區域色彩直方圖，同時設定初始搜索區域。

（2）讀取當前時刻原始圖像，經過反向投影得到反向投影圖，從背景中分離出跟蹤目標。

（3）使用Meanshift算法收斂搜索區域。

（4）將收斂區域集中，調整跟蹤窗口大小。

（5）重復（2）～（4）步驟。

當目標尺度發生變化時，Camshift算法可通過步驟（4）將收斂區域集中，用以調整跟蹤窗口大小，以便進行自適應跟蹤。

3 雙目相機特征點三維重建

使用雙目攝像機優勢在于能夠提供對象的深度信息，實現像素點三維坐標重建。本文所用雙目相機已進行了相機標定，標定方法參見文獻[13]，所獲得的圖像也已進行了立體校正，標定與校正方式參見文獻[12]。經過Bouguet方法校正后兩攝像機光軸平行并相交于無窮遠，同時兩圖像實現行對準，立體校正為圖像重新選擇圖像中心與邊界，使兩圖像疊加視圖面積最大化，校正后兩攝像機具有相同主點與焦距，即具有相同的內參。

攝像機模型坐標系對應關系如圖4所示，為方便圖像平面與攝像機模型坐標轉換，將攝像機坐標系坐標軸取為：X軸與圖像坐標系u軸平行，Y軸與圖像坐標系v軸平行，Z軸與光心重合。根據透視投影關系，基本針孔攝像機模型的空間任意一點P與圖像平面對應關系可表示為：

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

圖4 各坐標系間關系

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

圖5 雙目視覺圖像重建示意圖

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

由左、右像平面的 pL和 pR坐標計算得到空間點P在攝像機坐標系內三維坐標值，實現了特征點三維重建。

4 目標跟蹤與定位

Camshift算法可確定目標中心在圖像平面中所處位置，對左右相機同一時刻圖像分別運行Camshift算法以確定目標的左、右中心，通過雙目三角測量法確定目標在左相機坐標系三維坐標。考慮到旋翼飛行器體積不大，假定左相機坐標系與飛行器機體坐標系重合，算法流程圖如圖6所示。

圖6 目標跟蹤定位算法流程

根據三角測量法可知，目標在機體坐標系三維坐標可用式（6）表示：

式中，認為相機αx與αy相等，f為相機焦距，單位為像素。

由圖4可知，通過相機光軸與目標像素點的偏移可計算相機光心與目標夾角，由此可計算得到相機與目標間的相對偏航角與俯仰角。

利用求得的飛行器與目標間的相對偏航角δψ可校正飛行器的偏航角，使其正視目標飛行。機體坐標系下深度信息表征飛行器與目標間的相對距離，利用高度與相對偏航角可控制目標處于飛行器視場內。

實際傳感器測量存在噪聲干擾等影響，應用卡爾曼濾波進行數據濾波估計，能減少噪聲干擾，提高數據的穩定性和測量精度。所用的線性卡爾曼濾波狀態方程和觀測方程為：

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

將圖像處理得到的目標三維信息作為飛行軌跡控制的負反饋輸入信號，可保證飛行器的航向與距離的穩定與控制，實現飛行器自主跟蹤目標。調節控制器參數能改善飛行器的操縱響應，控制結構框圖如圖7所示，飛行控制系統及控制算法參見文獻[15]，本文不再贅述。

圖7 無人機自主飛行跟蹤目標控制框圖

5 測試結果

為驗證所提出算法，應用兩臺低成本USB相機作為圖像采集設備，相機鏡頭焦距8 mm，兩相機間基線長度10 cm，圖像尺寸352×288。使用一臺筆記本電腦作為圖像處理計算機，CPU為Intel T7500，主頻達到2.2 GHz，內存為2 GB。運用OpenCV 2.4.7代碼庫，結合VS2010 C++實現程序編寫，Debug模式下幀頻達到8 frame/s。六旋翼平臺采用MH550，飛控系統為自主研發的自適應飛控系統。

如圖8所示為雙目視覺目標跟蹤地面實驗結果，方框區域為目標區域，紅點為目標左、右中心。圖9所示為一次跟蹤任務的三維顯示結果，顯示了目標在飛行器坐標系下經卡爾曼濾波得到的三維位置估計。圖10所示為此次跟蹤的相對偏航角變化。圖示結果表明目標運動軌跡清晰、三維位置估計值精確。

圖9 經卡爾曼濾波的地面跟蹤結果三維顯示

圖10 跟蹤過程中相對偏航角變化

圖11所示為一距離測量結果，米尺測量數據與相機測量結果有2 cm誤差，即3.3%誤差，從曲線可以看出，經過卡爾曼濾波后，數據更加平滑穩定，減少了隨機噪聲對測量的影響，卡爾曼濾波計算中系統噪聲協方差矩陣與測量噪聲協方差矩陣為Q=0.15×I4×4，R= 1×I4×4。

圖11 測量過程穩態誤差

圖12所示為相機移動情況下相機距離正前方目標之間的距離實測結果。圖中可見，相機測距跟蹤迅速，誤差控制在2 cm范圍內，經卡爾曼濾波距離估計值能滿足飛行器目標跟蹤及定位任務要求。

圖12 距離測試實驗

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

圖13 無人機視覺跟蹤測試

圖14 目標跟蹤Zb軸位置誤差

圖15 目標跟蹤Yb軸位置誤差

6 結束語

（1）Camshift算法計算穩定性好，可實現對不同尺寸目標的跟蹤，能有效擴大跟蹤范圍。

（2）基于雙目視覺的目標定位估計相比于紅外、超聲波等測距方式，具有較高的性價比，且可避免反射波被吸收使測量失敗等問題。

（3）雙目視覺目標跟蹤定位結合卡爾曼濾波能有效減小噪聲干擾，提高測量精度，可在雙目視覺有效距離內將精度控制在±5 cm以內。

（4）地面和飛行跟蹤實驗結果表明，本文所提出的目標跟蹤定位算法穩定可靠，精度較高。

[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]馬頌德，張正友.計算機視覺：計算理論與算法基礎[M].北京：科學出版社，1998：52-94.

[15]夏青元，徐錦法.三軸式無人旋翼飛行器及自適應飛行控制系統設計[J].航空學報，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

針對小型無人飛行器跟蹤目標的問題，提出了一種基于雙目視覺和Camshift算法的無人飛行器目標跟蹤以及定位算法。雙目相機得到的左右圖像通過Camshift算法處理可得到目標中心特征點，對目標中心特征點進行三維重建，得到機體坐標系下無人飛行器與目標間的相對位置和偏航角，應用卡爾曼濾波算法對測量值進行了優化，將所得估計值作為飛行控制系統的反饋輸入值，實現了無人飛行器自主跟蹤飛行。結果表明所提算法誤差較小，具有較高的穩定性與精確性。

無人飛行器；雙目視覺；目標跟蹤；定位

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.

江蘇高校優勢學科建設工程資助項目。

張梁（1984—），男，博士研究生，研究領域為無人飛行器視覺導引與組合導航；徐錦法（1963—），男，博士，教授，研究領域為無人飛行器飛行控制與組合導航。E-mail：xjfae@nuaa.edu.cn

2014-05-21

2014-07-14

1002-8331（2014）24-0027-05

CNKI網絡優先出版：2014-08-19，http∶//www.cnki.net/kcms/doi/10.3778/j.issn.1002-8331.1405-0263.html