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

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

0 引言

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

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

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

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

1 基于視覺的目標(biāo)跟蹤

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

1.1 CAED 目標(biāo)跟蹤算法

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

1.1.1 ED 算法

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

圖1 橢圓檢測總流程

圖2 橢圓物體檢測結(jié)果

1.1.2 CAED 算法

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

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

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

(2)預(yù)估相鄰幀搜索窗口。假設(shè)相鄰3 幀目標(biāo)與相機的距離變化率、位置變化率方向都不變，由左幀中目標(biāo)的位置，推算右?guī)拖乱蛔髱某跏妓阉鞔翱凇?/p>

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

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

(5)重復(fù)步驟(2)～(4)，如果丟失目標(biāo)，就再次執(zhí)行步驟(1)。

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

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

1.2 雙目測距

1.2.1 相機標(biāo)定

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

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

(1)內(nèi)參標(biāo)定

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

其中K 為相機的內(nèi)參數(shù)矩陣。

圖4 針孔投影模型

(2)外參標(biāo)定

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

圖5 對極幾何約束

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

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

1.2.2 雙目測距

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

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

由對極幾何的定義，p1和p2應(yīng)滿足以下關(guān)系：

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

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

2 無人機動態(tài)目標(biāo)跟蹤模型

無人機動態(tài)目標(biāo)跟蹤模型根據(jù)目標(biāo)的位置信息，改變電機的轉(zhuǎn)速，從而控制無人機的位置與姿態(tài)，實現(xiàn)對動態(tài)目標(biāo)的平穩(wěn)跟蹤[7]。

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

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

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

2.1 位置串級PID 控制

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

圖7 位置控制系統(tǒng)結(jié)構(gòu)圖

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

從而，期望的四個電機產(chǎn)生的總拉力為：

圖8 平面受力分析

2.2 姿態(tài)串級PID 控制

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

圖9 姿態(tài)控制系統(tǒng)結(jié)構(gòu)圖

2.3 電機轉(zhuǎn)速控制

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

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

3 實驗與結(jié)果分析

3.1 整體框架

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

圖10 四旋翼實驗平臺

雙目相機獲取視頻信息并傳輸?shù)轿⑿陀嬎銠C中，計算機在視頻中逐幀識別提取出目標(biāo)，將目標(biāo)的圖像坐標(biāo)解算為機體坐標(biāo)系下的空間坐標(biāo)，同時將坐標(biāo)信息發(fā)送到飛控。飛控接收計算機的目標(biāo)位置反饋，運行四旋翼目標(biāo)追蹤控制算法，通過調(diào)節(jié)各電機的轉(zhuǎn)速，控制機體追蹤目標(biāo)。

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

圖11 雙目相機

3.2 相機標(biāo)定結(jié)果與驗證

3.2.1 雙目標(biāo)定實驗

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

圖12 標(biāo)定棋盤格

標(biāo)定結(jié)果如下，

內(nèi)參：

畸變系數(shù)：

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

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

圖13 特征匹配

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

表1 對極約束誤差

由表1 中數(shù)據(jù)，誤差e 基本接近0，標(biāo)定結(jié)果符合對極約束的精度要求。

3.2.2 雙目測距實驗

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

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

圖14 不同角度距離下的目標(biāo)位置

表2 目標(biāo)解算位置與實測位置對比

3.3 目標(biāo)跟蹤實驗

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

圖16 是對靜止空中目標(biāo)的追蹤實驗截圖。選取了追蹤具有代表性的12 幀左相機的圖像，展示了無人機懸?！粉櫋A(yù)定位置懸停的狀態(tài)過渡。對靜止目標(biāo)的追蹤效果良好，但是存在一定程度的超調(diào)，因為設(shè)置PID 參數(shù)時假定了目標(biāo)是定速運動。

圖17 是對動態(tài)空中目標(biāo)的追蹤實驗截圖，選取具有代表性的12 幀左相機的圖像，展示了無人機懸?！粉櫋鄬o止的狀態(tài)過渡。對于運動方向變化平緩的目標(biāo)，無人機可以實現(xiàn)較穩(wěn)定的跟蹤，而如果目標(biāo)的方向發(fā)生大的變動，則無人機在追蹤過程中會可能在期望位置來回?fù)u擺，尤其是目標(biāo)速度在左右方向變動時，甚至可能會跟蹤丟失。

圖16 目標(biāo)追蹤(靜止目標(biāo))

圖17 目標(biāo)追蹤(動態(tài)目標(biāo))

圖15 視覺追蹤總流程

4 結(jié)論

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

當(dāng)無人機與目標(biāo)橫向相對速度小于1.2 m/s，距離小于4 m 時，跟蹤效果良好，如在該方案基礎(chǔ)上進行相應(yīng)改動可以實現(xiàn)避障與導(dǎo)航，這對小型無人機在復(fù)雜室外環(huán)境的應(yīng)用具有很大的意義。