基于機載單目視覺的無人機移動目標(biāo)實時跟蹤?

孫鑄劉永

（南京理工大學(xué)計算機科學(xué)與工程學(xué)院 南京 210094）

1 引言

近年來，隨著微小傳感器、嵌入式微處理器、新型控制理論等相關(guān)技術(shù)的發(fā)展，許多世界軍事強國在研發(fā)作戰(zhàn)式無人機和大型長航時無人機的同時，也在集中科研力量研究微小型無人機，其在軍事方面無人機可以搭載多種設(shè)備完成多種軍事任務(wù)，如通訊中繼、信號干擾、精確打擊、隱秘偵查、目標(biāo)定位、地形勘探等，同時在氣象探測、抗災(zāi)搶險、農(nóng)業(yè)植保、交通監(jiān)控、環(huán)境測繪、航拍攝影等民用領(lǐng)域市場應(yīng)用也很廣泛，前景廣闊［1～5］。而以上很多場景中都離不開無人機的自主跟蹤操作。

目前國內(nèi)外對無人機自主跟蹤移動目標(biāo)的問題中典型應(yīng)用包括目標(biāo)搜索定位、空中移動目標(biāo)跟蹤，地面移動目標(biāo)跟蹤等。其中文獻［8］中較早地對空中的飛行平臺目標(biāo)定位做了研究，文中利用飛艇自帶圖像傳感器估算地面車輛的位置，解決城市中GPS信號不準(zhǔn)接受不良等問題，實驗中假設(shè)飛艇懸停高度固定，且圖像中有多個已知的標(biāo)志點。而文獻［9］中是利用固定翼無人機已知的三維位置，姿態(tài)以及圖像傳感器的姿態(tài)角，后識別目標(biāo)在圖像中的像素位置，之后通過兩者之間的坐標(biāo)變換，得到地面目標(biāo)的位置。無人機移動目標(biāo)跟蹤場景中，平臺和目標(biāo)都是時刻移動的，被跟蹤目標(biāo)往往是機動目標(biāo)，且目標(biāo)會受到各種因素的干擾，在這樣的情況下，無人機要高精度地跟蹤目標(biāo)，就必須要研究無人機對運動目標(biāo)進行狀態(tài)估計及自身的控制方法問題。

2 系統(tǒng)建模與總體設(shè)計

2.1 系統(tǒng)坐標(biāo)系定義

本文世界坐標(biāo)系定義采用北東地（NED）坐標(biāo)系，OWXW軸指向北方，OWYW軸指向東方，OWZW軸指向重力方向，如圖1中所示世界坐標(biāo)系FW=OW，｛XW，YW，ZW｝，OW為坐標(biāo)系原點?？罩袡C器人定義機體坐標(biāo)系，F(xiàn)R=OR，｛XR，YR，ZR｝，OR為無人機的質(zhì)心，四個電機的對角交叉線構(gòu)成的平面垂直向下方向為ZR方向，1、2電機的角平分線方向為XR，2、3電機的角平分線方向為 YR，定義RR∈R3×3為機體坐標(biāo)系相對于世界坐標(biāo)系的旋轉(zhuǎn)變換。在視覺處理過程中，圖像平面坐標(biāo)系至關(guān)重要，定義像平面的左上角為原點OI，橫軸為u軸，縱軸為v軸，即FI=OI，｛u，v｝，與光軸的交點為圓點O，平行于縱軸v向下為Y軸，平行于橫軸u的為X軸，F(xiàn)=O，｛X，Y｝。定義攝像機坐標(biāo)系，以光心為圓點OC，對應(yīng)的光軸為ZC，另 XC、YC坐標(biāo)軸與圖像物理坐標(biāo)系平行，F(xiàn)C=OC，｛XC，YC，ZC｝。釋義圖如圖1所示。

圖1 坐標(biāo)系定義

2.2 四旋翼無人機動力學(xué)模型

為了更好地對四旋翼進行動力學(xué)分析，首先需要做一下假設(shè)需要對其建立數(shù)學(xué)模型：

1）四旋翼是一個質(zhì)量分布均勻，整體結(jié)構(gòu)完全對稱的剛體。

2）重力加速度為常數(shù)g。

3）忽略空氣擾動和阻力，槳葉產(chǎn)生的升力與其轉(zhuǎn)速平方成正比。

令ε=(x y z)T為無人機在世界坐標(biāo)系下相對于原點運動位移向量，η=(φ θ ψ)T為繞世界坐標(biāo)系的旋轉(zhuǎn)向量。定義Uz，Uφ，Uθ，Uψ分別表示四個電機的總升力和三個姿態(tài)角度的控制量。在“+”模式下，他們與四個電機的升力的關(guān)系可以如式（1）所示，其中l(wèi)為槳葉的質(zhì)心到機體質(zhì)心之間的距離，d為旋轉(zhuǎn)阻力系數(shù)。動力學(xué)方程如式（1）所示。

其中，Jr為旋翼的轉(zhuǎn)動慣量，Ωr=Ω2+Ω4-Ω1-分別是四個旋翼的角速度。

以上等式顯示了各個狀態(tài)量與輸入控制量Uz，Uφ，Uθ，Uψ之間的關(guān)系，方便后續(xù)設(shè)計控制器時的理論分析。

3 基于圖像的目標(biāo)檢測與跟蹤

在純圖像的目標(biāo)檢測與跟蹤中本文先計算場景全局運動，之后取仿射變換后的兩幀差值，獲取移動目標(biāo)的位置，將獲取的目標(biāo)選框交給TLD［7］目標(biāo)跟蹤算法完成實時跟蹤。為了減少全局運動補償時計算量，采用ORB［6］特征點的方式，提取目標(biāo)特征，該算法具有局部不變性且運算速度是SIFT［13］的100倍，SURF［14］的10倍。相機的全局運動參數(shù)模型有很多，可根據(jù)參數(shù)多少分為十二參數(shù)，八參數(shù)，六參數(shù)等模型，由于相機旋轉(zhuǎn)角度較大時，六參數(shù)模型不能較好地表示相機運動模型，八參數(shù)模型表示了相機可能的旋轉(zhuǎn)、平移、縮放、不規(guī)則伸展等運動，且參數(shù)量相比十二參數(shù)較少，本文使用相機的八參數(shù)模型，如式（2）。該模型下定義參數(shù)矩陣矢量：

要求解m0～m7的值可以根據(jù)上面的ORB特征中的匹配點，每對匹配點有兩個方程，ORB可以提供三對以上的特征匹配點，之后通過最小二程求解方程組。

上述得到的參數(shù)值，是在ORB的特征匹配點中誤匹配點沒有去除的情況下得到的，如果其中的誤匹配點較多，最小二乘所得到的值是不準(zhǔn)確的，因此須去除誤匹配點后再進行估計?？紤]本文中的系統(tǒng)嵌入式平臺資源寶貴，應(yīng)盡量減少計算時間，本文采用改進的RANSAC方法，RANSAC［15］中初始的匹配點是隨機的，不考慮匹配點的差異性，增加去除誤匹配的時間。而PROSAC［11］方法，該算法將點初始集匹配的結(jié)果作為排序的依據(jù)，使得在采樣時根據(jù)匹配結(jié)果由高到低的得分進行排序。利用式（2）將圖像由于相機自身運動導(dǎo)致的誤差補償?shù)?，背景做差后消除大部分的背景圖像，中值濾波和自適應(yīng)閾值分割幀，去除一些噪點，之后設(shè)置檢索連通域，使用形態(tài)學(xué)濾波的膨脹腐蝕，得到處理后的目標(biāo)圖像。

之后將獲取到的目標(biāo)選框傳遞給TLD目標(biāo)跟蹤算法，本文在使用TLD目標(biāo)跟蹤算法時結(jié)合硬件平臺的特性，追求運行的實時性，將跟蹤模塊部分的Forward-Backward跟蹤部分做并行優(yōu)化，使得充分利用硬件多核特性，提升了系統(tǒng)的實時性。

4 基于IBVS視覺伺服移動目標(biāo)跟蹤控制

將純視覺跟蹤取得的二維位置如何應(yīng)用在視覺伺服控制中，本文推導(dǎo)出改進的圖像視覺伺服控制，引入了新的伺服圖像特征，基于新特征推導(dǎo)出的圖像雅克比矩陣可以直接利用純圖像目標(biāo)跟蹤算法中得到的目標(biāo)選框。IBVS［10］無人機跟蹤控制的流程圖如圖2所示。

圖2 IBVS跟蹤控制流程圖

下面我們主要研究視覺伺服控制器中的關(guān)鍵部分圖像雅克比矩陣以及整個跟蹤控制器的設(shè)計。根據(jù)攝像機成像過程中的坐標(biāo)變換關(guān)系，以及機器人末端執(zhí)行器的運動原理，可得到如下等式：

此時已經(jīng)得到圖像二維像素位置特征與相機運動速度之間的關(guān)系，但是移動目標(biāo)視覺跟蹤算法中還有一個特征沒有使用，即目標(biāo)框的面積。這里假設(shè)圖像中的目標(biāo)框的面積為Sc，目標(biāo)的實際面積為Sw，其為常量，則根據(jù)相機透視投影模型，可得到如下公式：

兩邊求導(dǎo)并對結(jié)合式（2），可得：

視覺伺服控制器需要知道圖像特征變化率s?與相機運動速度VC之間的關(guān)系s?=LsVc，結(jié)合分析可得如下關(guān)系，其中Ls為所求的圖像雅克比矩陣。由于在視覺跟蹤移動目標(biāo)過程中，算法不對目標(biāo)的姿態(tài)變化做檢測，因此也可以假設(shè)相機相對目標(biāo)的旋轉(zhuǎn)運動完全是由相機自身運動產(chǎn)生的。同時無人機自身內(nèi)置的IMU傳感器可以實時檢測自身的姿態(tài)變化，旋轉(zhuǎn)角速度，因為相機相對于無人機是固定的，則該角速度就是相機的角速度，那么可以得出wx，wy，wz為IMU測量角速度值的負(fù)值，是可以直接得到的。

以上的伺服控制器需要結(jié)合飛行器底層位姿控制器，考慮到系統(tǒng)的穩(wěn)定性和抗干擾性，本文將速度控制器設(shè)計為PI控制，而姿態(tài)控制器使用串級PID［12］控制，使系統(tǒng)的姿態(tài)控制更加穩(wěn)定，其整個跟蹤控制結(jié)構(gòu)圖如圖3所示。

圖3 視覺伺服跟蹤控制系統(tǒng)

5 實驗

在實驗中我們主要分為兩個部分，一是視覺部分的實驗，二是跟蹤著陸部分的仿真實驗和在系統(tǒng)平臺上的跟蹤實驗。

5.1 跟蹤仿真實驗

使用Matlab Simulink環(huán)境下進行仿真，仿真模型如圖4所示。

其中，仿真系統(tǒng)中使用的參數(shù)取值如表1所示。

圖4 跟蹤系統(tǒng)仿真圖

表1 仿真模型中的參數(shù)取值

這里，我們使目標(biāo)先做橫向二次函數(shù)軌跡運動，之后保持靜止，使用視覺伺服控制無人機跟蹤目標(biāo)，觀察無人機與目標(biāo)的軌跡關(guān)系，如圖5所示，控制器能夠較好地跟蹤上目標(biāo)的軌跡，以及在跟蹤過程中伺服控制器，其值在零處來回震蕩，最終完成跟蹤控制。

圖5 目標(biāo)軌跡與無人機的變化與伺服的速度輸出

5.2 動態(tài)場景下的目標(biāo)識別跟蹤

實驗環(huán)境：實驗使用mvbluefox-200wc相機，捕捉圖像，處理平臺是Ubuntu14.04。

1）動態(tài)場景移動目標(biāo)識別與跟蹤

ORB特征點匹配，PROSAC精匹配，如圖6所示。

圖6 orb特征點匹配

前景運動補償后的前景運動區(qū)域，形態(tài)學(xué)濾波后的目標(biāo)，目標(biāo)檢測結(jié)果如圖7所示。

圖7 經(jīng)過前景運動補償濾波后的移動目標(biāo)和識別

識別到目標(biāo)框后，使用TLD進行后續(xù)的跟蹤，跟蹤速度每幀50ms，20Hz的頻率，實驗結(jié)果如圖8所示。

圖8 跟蹤中的目標(biāo)物體

5.3 四旋翼平臺跟蹤實驗

實驗飛行器如圖9所示。

圖9 無人機平臺

實驗過程中我們控制移動小車讓無人機已指定高度飛行在移動小車上方，為了驗證跟蹤效果，通過室內(nèi)固定的雙目視覺相機立體定位無人機的位置，圖10是跟蹤過程中兩者的位置變化關(guān)系。

圖10 跟蹤過程中的位置變化

從圖10中可以看出基本的跟蹤可以實現(xiàn)，但是在拐角的時候有跟蹤滯后效應(yīng)，這也是在使用視覺伺服輸出直接作為期望速度偏差量時容易產(chǎn)生的影響，實際伺服速度輸出如圖11所示。

圖11 伺服輸出的速度偏差

上面兩幅圖反應(yīng)了視覺伺服輸出的期望速度偏差值隨時間的變化，x軸最大輸出值60cm/s，y軸最大輸出值55cm/s，整個過程在零值上下震蕩，實驗證明視覺伺服控制器能基本滿足跟蹤要求。

6 結(jié)語

本文使用無標(biāo)記點的跟蹤方式，使用快速有效的ORB特征點提取結(jié)合全局運動補償?shù)姆椒ǎ梢钥焖僮R別動態(tài)場景下的移動目標(biāo)，同時使用并行的TLD目標(biāo)跟蹤算法能夠高效準(zhǔn)確跟蹤目標(biāo)，跟蹤算法只能輸出圖像平面內(nèi)的二維坐標(biāo)。最后引入新的圖像面積特征點，并計算基于此特征點，推導(dǎo)計算出IBVS中的圖像雅克比矩陣和它的偽逆并做了仿真和實驗驗證。