基于立體視覺(jué)的無(wú)人機(jī)動(dòng)態(tài)目標(biāo)實(shí)時(shí)跟蹤

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

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

0 引言

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

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

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

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

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

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

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

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

1.1.1 ED 算法

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

圖1 橢圓檢測(cè)總流程

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

1.1.2 CAED 算法

單純的ED 算法足以在圖像中可靠地檢測(cè)出橢圓物體，但是由于其是在全圖檢測(cè)并驗(yàn)證，計(jì)算耗時(shí)相對(duì)較長(zhǎng)，無(wú)法在微型計(jì)算機(jī)上達(dá)到60 fps 的實(shí)時(shí)性要求。

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

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

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

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

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

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

圖3 前后幀搜索窗口迭代過(guò)程

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

1.2 雙目測(cè)距

1.2.1 相機(jī)標(biāo)定

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

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

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

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

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

圖4 針孔投影模型

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

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

圖5 對(duì)極幾何約束

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

利用直接線(xiàn)性變換求得H，再對(duì)其作SVD 分解，得到外參R、t。

1.2.2 雙目測(cè)距

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

圖6 由三角測(cè)量計(jì)算地圖點(diǎn)深度

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

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

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

2 無(wú)人機(jī)動(dòng)態(tài)目標(biāo)跟蹤模型

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

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

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

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

2.1 位置串級(jí)PID 控制

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

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

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

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

圖8 平面受力分析

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

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

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

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

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

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

3 實(shí)驗(yàn)與結(jié)果分析

3.1 整體框架

四旋翼實(shí)驗(yàn)平臺(tái)(見(jiàn)圖10)的目標(biāo)跟蹤系統(tǒng)包括硬件與軟件兩部分。硬件部分由雙目相機(jī)、微型視覺(jué)計(jì)算機(jī)和飛控組成。

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

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

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

圖11 雙目相機(jī)

3.2 相機(jī)標(biāo)定結(jié)果與驗(yàn)證

3.2.1 雙目標(biāo)定實(shí)驗(yàn)

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

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

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

內(nèi)參：

畸變系數(shù)：

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

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

圖13 特征匹配

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

表1 對(duì)極約束誤差

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

3.2.2 雙目測(cè)距實(shí)驗(yàn)

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

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

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

表2 目標(biāo)解算位置與實(shí)測(cè)位置對(duì)比

3.3 目標(biāo)跟蹤實(shí)驗(yàn)

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

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

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

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

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

圖15 視覺(jué)追蹤總流程

4 結(jié)論

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

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