基于立體視覺的無人機動態目標實時跟蹤

唐 凱，張海濤，鄭建杰，孔維迪

(陸軍工程大學 國防工程學院，江蘇 南京 210000)

0 引言

對空中動態目標的跟蹤是實現無人機協同工作的基礎，也是當前無人機技術領域研究的前沿課題之一。單目相機無法提供目標深度，使用限制較多，雙目相機根據三角測量原理可以直接計算目標的空間位置，適用范圍更廣[1]。TEULIERE C[2]等人使用搭載俯視單目相機的四旋翼飛行器，跟蹤地面模型賽車，由IMU 計算飛行器高度，實現了對目標的航向角鎖定與跟蹤定位。MIGUEL[3]等人使用搭載前視單目相機的四旋翼飛行器，跟蹤紅色氣球，通過特征面積計算相對距離，使用模糊控制實現了目標定位跟蹤。

張梁等人[4]利用雙目相機測量目標的空間位置，實現了六旋翼飛行器對目標的跟蹤。

TEULIERE C[2]、MIGUEL[3]、張梁等人[4]實現的目標跟蹤是基于顏色的特征跟蹤Camshift[5]算法，該算法的跟蹤窗口可根據目標實時縮放，跟蹤定位效果較好，但該算法只適用于光照恒定的室內環境以及背景色彩單一的室外環境。

本文提出了一種基于輪廓的特征跟蹤方法，連續自適應橢圓檢測(Continuously Adaptive Ellipse Detector，CAED)算法，克服了特征跟蹤Camshift 算法過于依賴色彩的缺陷，降低了復雜室外環境下目標丟失的幾率，提高了目標檢測的適用范圍。文中提出的對空中動態目標跟蹤算法，通過在四旋翼飛行器上搭載雙目相機，并自行設計四旋翼飛行控制算法，實現對空中動態目標的平穩跟蹤。對于無人機協同工作以及無人機電力巡檢、除障等有一定的參考意義。

1 基于視覺的目標跟蹤

為了實現無人機基于雙目相機的動態目標跟蹤，主要步驟可分為：(1)四旋翼無人機在平穩起飛后，讀取相機圖像，利用CAED 算法識別目標；(2)利用雙目測距算法，計算目標的位置；(3)針對目標追蹤，建立四旋翼控制模型。

1.1 CAED 目標跟蹤算法

CAED 算法基于目標的輪廓，實現在每幀對目標的識別與跟蹤。CAED 以橢圓檢測（Ellipse-Detector，ED）算法為核心，實現圖像任一時刻的ED 迭代計算，再以此幀得到的目標窗口為下一時刻ED 算法搜索窗口的初始值，可以在每一幀都快速檢測到目標物體的像素中心。

1.1.1 ED 算法

ED 算法是FORNACIARI M[6]等人提出的一種快速橢圓檢測算法，可以在嵌入式設備實時運行。該算法是一種基于邊界聚類的橢圓檢測方法，結合使用了基于弧段的方法和最小二乘法。從邊界圖提取圓弧，再經過過濾、聚類，最終用最小二乘法擬合出橢圓。總體程序如圖1 所示。該方法能有效應對多個橢圓、橢圓相互遮擋和橢圓部分缺損等復雜情況。圖2 顯示了ED 算法的橢圓檢測結果(圓心與外形皆已標出)。

圖1 橢圓檢測總流程

圖2 橢圓物體檢測結果

1.1.2 CAED 算法

單純的ED 算法足以在圖像中可靠地檢測出橢圓物體，但是由于其是在全圖檢測并驗證，計算耗時相對較長，無法在微型計算機上達到60 fps 的實時性要求。

受到Camshift[5]算法的啟發，CAED 算法充分利用前后多幀與左右兩幀圖像橢圓目標的像素中心之間的約束關系，從而設置合適橢圓搜索窗口，既提高了目標檢測的速度，又提高了雙目測距的可靠性和實時性。CAED 算法的迭代計算過程如下：

(1)初始化。針對目標大小，設定檢測橢圓像素半徑閾值，左目相機從全圖搜索橢圓目標，如果檢測到目標，標記其中心位置和輪廓，設定搜索窗口為同心矩形，邊長為橢圓長徑的3 倍。

(2)預估相鄰幀搜索窗口。假設相鄰3 幀目標與相機的距離變化率、位置變化率方向都不變，由左幀中目標的位置，推算右幀和下一左幀的初始搜索窗口。

(3)檢測目標。在(2)預估的窗口(圖3 虛線框)中檢測橢圓目標，計算中心和半徑。

(4)重置搜索窗口。使用(3)的結果，重新設置窗口中心和大小(圖3 實線框，箭頭為目標在圖像中的運動方向)，并作為下一幀的初始搜索窗口。

(5)重復步驟(2)～(4)，如果丟失目標，就再次執行步驟(1)。

圖3 前后幀搜索窗口迭代過程

在高幀率相機的配合下，CAED 可以對目標軌跡作合理預測，并根據目標在視野中的位置與大小自動調整搜索窗口，保證了實時性。

1.2 雙目測距

1.2.1 相機標定

雙目測距算法根據目標在左右相機的像素坐標，利用三角測量原理計算目標的空間位置。由于出廠時相機的參數一般是未知的，因此，需要事先標定雙目相機。

標定后可以得到左右相機的內參數K1和K2，以及外參數(左右相機的相對位姿)R、t。本文采用具有較好的魯棒性和實用性的張氏標定法[5]計算內參K1和K2，在此基礎上，利用對極幾何和單應矩陣H 分解得到外參R、t。

(1)內參標定

設相機坐標系下的空間點p=(x，y，z)T(見圖4)，投影點像素坐標puv=(u，v，l)T可表示為：

其中K 為相機的內參數矩陣。

圖4 針孔投影模型

(2)外參標定

外參標定本質上是計算左右相機投影平面的相對位姿，根據對極幾何約束(圖5)，由至少8 對正確匹配的像素點(p1，i，p2，i)可以推斷相對位姿。

圖5 對極幾何約束

特別注明，當像素點p1，i共面和p2，i共面時(如棋盤格角點共面)，可以利用單應矩陣H 描述像素點的匹配關系：

利用直接線性變換求得H，再對其作SVD 分解，得到外參R、t。

1.2.2 雙目測距

雙目相機的內外參數確定后，就可以由三角測量(圖6)原理求得目標的空間位置。O1和O2為光心，p1和p2為匹配的像素點。理論上，直線O1p1與O2p2會相交于點p，該點即為相應的空間點。

圖6 由三角測量計算地圖點深度

由對極幾何的定義，p1和p2應滿足以下關系：

等式兩邊左乘(K-12p2)^，得：

從而可以求出像素深度z1，得到空間點坐標p=。

2 無人機動態目標跟蹤模型

無人機動態目標跟蹤模型根據目標的位置信息，改變電機的轉速，從而控制無人機的位置與姿態，實現對動態目標的平穩跟蹤[7]。

為了簡化計算，使機體坐標系{B}與左相機坐標系重合(x 軸向右，y 軸向上，z 軸向前)，無人機起飛至高度為1 m 并穩定后，定義此時的機體坐標系為大地坐標系{W}。

在大地坐標系{W}下，定義無人機俯仰角θ，滾轉角φ，航向角ψ。在機體坐標系{B}中，設目標的期望位置為p*=(x*，y*，z*)T，實際位置為p=(x，y，z)T。

追蹤目標時僅改變無人機在大地坐標系{W}下x、y、z 三軸的位置，不改變航向角ψ。由于無人機在低速狀態下，θ ≈0,φ ≈0，故認為{B}與{W}僅相差一個平移[8]，從而{W}與{B}之間的位移、速度變換是線性的。為確保追蹤過程中機體的穩定，減少振蕩，分別設計位置串級PID 控制和姿態串級PID控制。

2.1 位置串級PID 控制

如圖7 所示，位置控制由兩個閉環組成，選擇雙目相機作為信號反饋源。外環為位置控制，由反饋的位置p，根據期望位置p*，通過P 控制方法計算控制輸入量，并作為速度環的期望速度v*。內環為速度控制，由反饋的無人機相對于目標的速度v，并根據期望速度v*，通過PID 控制方法計算控制輸入量，作為期望加速度a*。

圖7 位置控制系統結構圖

如圖8 所示，通過動力學擴展，又可將期望加速度a*用期望升力、期望姿態角θ*、φ*表示：

從而，期望的四個電機產生的總拉力為：

圖8 平面受力分析

2.2 姿態串級PID 控制

如圖9 所示，姿態控制由兩個閉環組成，IMU 作為信號反饋源。外環為角度控制環，由反饋的角度Ψ=(θ，φ)T，根據上一節位置控制推導的期望角度Ψ*，采用P 控制計算控制輸入量，作為內環的期望角速度ω*，再由反饋的角速度ω=(ωθ，ωφ)T，采用PID控制計算控制輸入量，即期望角加速度。再由動力學，映射到扭矩：

圖9 姿態控制系統結構圖

2.3 電機轉速控制

本文無人機的電機布局為“X”形，由總拉力T和扭矩τ，便可計算出達到期望狀態所需的電機轉速：

其中k 為槳葉升力系數，l 為相鄰電機間距。給定各控制環合適的PID 系數后，控制模型通過雙目相機的位置反饋和IMU 的姿態反饋，實現了由目標位置到電機轉速的控制映射：p(t)→ω2(t)。

3 實驗與結果分析

3.1 整體框架

四旋翼實驗平臺(見圖10)的目標跟蹤系統包括硬件與軟件兩部分。硬件部分由雙目相機、微型視覺計算機和飛控組成。

圖10 四旋翼實驗平臺

雙目相機獲取視頻信息并傳輸到微型計算機中，計算機在視頻中逐幀識別提取出目標，將目標的圖像坐標解算為機體坐標系下的空間坐標，同時將坐標信息發送到飛控。飛控接收計算機的目標位置反饋，運行四旋翼目標追蹤控制算法，通過調節各電機的轉速，控制機體追蹤目標。

雙目相機選擇全瑞視訊高清雙目相機(圖11)，60 fps，雙目分辨率為2 560×960，視角為70°。微型計算機選擇Nvidia TX2，飛控選擇基于STM32 芯片的開源飛控Pixhawk 4。

圖11 雙目相機

3.2 相機標定結果與驗證

3.2.1 雙目標定實驗

以一張8×6 棋盤格(圖12)作為標定物，分別對左右單目進行標定，定標板每個棋盤格大小為60 mm×60 mm，試驗分別選取了18 幅不同距離與角度下的有效圖像。

圖12 標定棋盤格

標定結果如下，

內參：

畸變系數：

根據多對左右圖像的棋盤格角點計算單應矩陣H，進而得到R、t。

標定結果顯示左右兩個相機投影面基本平行，平移量t 大致符合59.5 mm 的測量數據。再次對一張雙目圖像(圖13)提取匹配特征點p1，i和p2，i，驗證對極約束：

圖13 特征匹配

理想情況下e=0，驗證結果如表1 所示。

表1 對極約束誤差

由表1 中數據，誤差e 基本接近0，標定結果符合對極約束的精度要求。

3.2.2 雙目測距實驗

由雙目圖像檢測出的圓心像素坐標puv1，puv2，從而計算出目標在左相機坐標系下的位置。

圖14 為雙目相機拍攝的不同距離不同角度下的目標圖片(僅顯示左圖)，解算的目標位置信息和距離與實際測量距離以及誤差如表2 所示，發現誤差不超過100 mm。

圖14 不同角度距離下的目標位置

表2 目標解算位置與實測位置對比

3.3 目標跟蹤實驗

為了提高追蹤的準確性和有效性，所選擇的追蹤目標為橢圓物體。設定目標的期望位置為p*=[0，0，2 000]T，在實驗中，空中目標以約300 mm/s 的速度做定速無規則運動，四旋翼無人機需要對空中的物體進行鎖定以及實時追蹤，具體流程見圖15。

圖16 是對靜止空中目標的追蹤實驗截圖。選取了追蹤具有代表性的12 幀左相機的圖像，展示了無人機懸停→追蹤→預定位置懸停的狀態過渡。對靜止目標的追蹤效果良好，但是存在一定程度的超調，因為設置PID 參數時假定了目標是定速運動。

圖17 是對動態空中目標的追蹤實驗截圖，選取具有代表性的12 幀左相機的圖像，展示了無人機懸停→追蹤→相對靜止的狀態過渡。對于運動方向變化平緩的目標，無人機可以實現較穩定的跟蹤，而如果目標的方向發生大的變動，則無人機在追蹤過程中會可能在期望位置來回搖擺，尤其是目標速度在左右方向變動時，甚至可能會跟蹤丟失。

圖16 目標追蹤(靜止目標)

圖17 目標追蹤(動態目標)

圖15 視覺追蹤總流程

4 結論

本文將雙目視覺測距和目標追蹤相結合，利用改進的橢圓檢測算法以及三角測量原理，利用目標的有限信息實現了對無人機位姿和目標位置速度的有效估計，針對運動目標跟蹤任務，設計并實現了基于串級PID 的四旋翼位姿控制率。將圖像處理算法與四旋翼控制相結合，進行了測試實驗，驗證了本文所提出的目標追蹤飛行控制方法。

當無人機與目標橫向相對速度小于1.2 m/s，距離小于4 m 時，跟蹤效果良好，如在該方案基礎上進行相應改動可以實現避障與導航，這對小型無人機在復雜室外環境的應用具有很大的意義。