鄧文堯，段學(xué)超，靖明陽

（西安電子科技大學(xué)電子裝備結(jié)構(gòu)設(shè)計教育部重點(diǎn)實(shí)驗(yàn)室，陜西西安710071）

引 言

基于視覺的動態(tài)目標(biāo)檢測與跟蹤已逐漸滲透到現(xiàn)代生產(chǎn)、生活的各個領(lǐng)域，例如視頻監(jiān)控、虛擬現(xiàn)實(shí)、人機(jī)交互、行星探測等場景。在目標(biāo)檢測算法研究領(lǐng)域，文獻(xiàn)[1]將積分圖用于圖像的特征表達(dá)，采用級聯(lián)分類器實(shí)現(xiàn)了目標(biāo)的魯棒性實(shí)時檢測。文獻(xiàn)[2]通過獲取圖像的關(guān)鍵點(diǎn)來描述物體的特征，提出了尺度不變特征變換（Scale Invariant Feature Transform, SIFT），但是該方法對場景中非目標(biāo)物體的運(yùn)動干擾較為敏感。文獻(xiàn)[3]提出了梯度直方圖（Histogram of Oriented Gradient, HOG）特征，將其應(yīng)用于靜態(tài)圖像中的行人檢測。文獻(xiàn)[4]將HOG 與支持向量機(jī)（Support Vector Machine, SVM）相結(jié)合提出了可變形部件模型（Deformable Part Model,DPM）。近年來，隨著深度學(xué)習(xí)的發(fā)展，文獻(xiàn)[5]于2015 年提出的Fast-R-CNN 被廣泛應(yīng)用在目標(biāo)識別方面。以上這些目標(biāo)跟蹤方法都是在無任何靶標(biāo)的情況下根據(jù)運(yùn)動物體的自身特征進(jìn)行檢測定位，具有較強(qiáng)的普遍性，但是難以滿足高精度定位的要求，尤其在多目標(biāo)運(yùn)動的情況下更難實(shí)現(xiàn)精準(zhǔn)定位和跟蹤。

ArUco marker是文獻(xiàn)[6]于2014年提出的一種用于增強(qiáng)現(xiàn)實(shí)和機(jī)器人定位等領(lǐng)域的相機(jī)姿態(tài)估計基準(zhǔn)標(biāo)記系統(tǒng)。該類marker 具有較高的魯棒性，將其標(biāo)記在物體的表面，可以實(shí)現(xiàn)物體的精確、快速定位。文獻(xiàn)[7]提出的稀疏光流法主要根據(jù)兩幀圖像對應(yīng)的像素點(diǎn)的信息進(jìn)行計算，從而得到光流矢量，實(shí)現(xiàn)對運(yùn)動物體的跟蹤。相比之下，傳統(tǒng)的光流則是利用SIFT 或SURF（Speeded-Up Robust Feature）[8]特征進(jìn)行跟蹤，但是由于兩幀圖像的特征點(diǎn)有時存在誤匹配，因此存在顯著誤差。本文采用ArUco marker特征點(diǎn)代替運(yùn)動物體的SIFT 特征，以期實(shí)現(xiàn)更高精度的定位。

另一方面，多旋翼無人機(jī)的應(yīng)用近年來得到了大范圍普及，但是在各類應(yīng)用中無人機(jī)的安全回收環(huán)節(jié)仍是一個眾所周知的問題。為了保證多旋翼無人機(jī)的安全降落，需要其在降落的過程中與收納平面保持水平。本文將ArUco marker 與光流法相結(jié)合應(yīng)用于無人機(jī)的實(shí)時位姿計算與動態(tài)跟蹤，從而為無人機(jī)抓捕收納提供支持。

1 稀疏光流跟蹤目標(biāo)的檢測原理

1.1 稀疏光流原理

光流是一種運(yùn)動模式，它是空間運(yùn)動物體在成像平面上對應(yīng)像素的瞬時速度。光流法是利用圖像序列中像素在時間域上的變化以及相鄰幀之間的相關(guān)性來確定上一幀與當(dāng)前幀之間存在的對應(yīng)關(guān)系，從而計算出相鄰幀之間物體運(yùn)動信息的一種方法。光流法的基本假設(shè)是圖像像素和亮度的守恒，并且同一物體在前后兩幀之間沒有明顯的形狀差別。

LK（Lucas-Kanade）算法[7]基于以下3 個基本假設(shè)：

1）亮度恒定。圖像中目標(biāo)像素點(diǎn)在前后幀的外觀保持不變，即目標(biāo)像素點(diǎn)在前后幀的亮度恒定，如式(1)所示：

式中，I(t)為t 時刻的圖像。

2）時間持續(xù)性或微小波動。即圖像的運(yùn)動相對于時間的變化比較緩慢，如式(2)所示：

式中：Ix,Iy表示圖像坐標(biāo)系中x,y 方向的差分；u,v為x,y 方向的速度；It為對應(yīng)點(diǎn)在兩幅圖像上的差分。

3）空間一致性。同一場景中同一表面上的點(diǎn)具有相似的運(yùn)動，其在圖像表面的成像也是在臨近的區(qū)域內(nèi)。在LK 光流法中選取5×5 的區(qū)域作為臨近點(diǎn)，每個點(diǎn)建立1 個方程，可以組成25 個方程的方程組，其矩陣形式如式(3)所示：

上式可以進(jìn)一步簡寫成：

利用最小二乘法求解該方程：

解得：

由此便可以得到每一個特征點(diǎn)的速度。LK 光流法一般適用于低速運(yùn)動的物體，它的改進(jìn)算法是金字塔LK 光流法[9]。金字塔LK 光流法的一般流程是首先在兩幅圖像的最高層計算LK 光流，金字塔下一層以上一層估計的結(jié)果作為起始點(diǎn)，從金字塔的頂層到底層不斷地重復(fù)該過程，實(shí)現(xiàn)由粗到細(xì)的光流跟蹤，因而對更快目標(biāo)的跟蹤具有更強(qiáng)的魯棒性。

1.2 稀疏光流和ArUco marker 的結(jié)合

ArUco marker 是Garrido-Jurado，R. Mu?oz-Salinas 等人在2014 年提出的一種生成可配置標(biāo)記字典（大小和比特數(shù)）的算法。該算法遵循同一個準(zhǔn)則來最大化標(biāo)記間的距離和比特轉(zhuǎn)換數(shù)，從而提高標(biāo)記點(diǎn)識別的魯棒性和準(zhǔn)確性。

使用ArUco marker 的主要目的是獲取準(zhǔn)確的標(biāo)記點(diǎn)，然后對標(biāo)記點(diǎn)采用稀疏光流法進(jìn)行檢測，避免直接使用SIFT，SURF，ORB（Oriented FAST and Rotated BRIEF）等特征檢測方法可能帶來的錯誤匹配的問題。就算法效率而言，ArUco marker 的檢測方法更快，但ArUco marker 一般需要人為附加標(biāo)記點(diǎn)，不能用于沒有標(biāo)記的運(yùn)動目標(biāo)。

ArUco marker 算法的主要步驟為：

1）將圖像轉(zhuǎn)成灰度圖，采用自適應(yīng)閾值的方法進(jìn)行圖像分割；

2）使用Suzuki 算法[10]進(jìn)行輪廓提取，并進(jìn)行四邊形擬合，對于距離相近的輪廓選取最外圍的四邊形；

3）計算單應(yīng)性矩陣，進(jìn)行閾值分割得到二值圖像，通過對黑色邊界的檢測剔除無關(guān)輪廓；

4）提取輪廓內(nèi)的marker 信息，得到庫中匹配的marker。

每個marker 都有唯一的ID，因此可以通過ID 匹配的方式尋找上一幀和下一幀圖像對應(yīng)的匹配點(diǎn)并使用稀疏光流，從而實(shí)現(xiàn)對運(yùn)動物體的跟蹤，并且計算出相應(yīng)的速度。靶標(biāo)的布置如圖1 所示。

圖1 ArUco marker 在無人機(jī)上的布局

2 基于光流法的無人機(jī)定位和跟蹤

2.1 相機(jī)的雙目標(biāo)定和手眼標(biāo)定

相機(jī)的成像模型可以看成是小孔成像模型。圖2描述了從空間三維坐標(biāo)到平面像素二維坐標(biāo)的轉(zhuǎn)換關(guān)系。

圖2 小孔成像模型

圖中：xw,yw,zw表示世界坐標(biāo)系坐標(biāo)；xc,yc,zc表示相機(jī)坐標(biāo)系坐標(biāo)；x,y 為圖像坐標(biāo)系坐標(biāo)；u,v為像素坐標(biāo)系坐標(biāo)；f 為焦距。

如圖2 所示，從世界坐標(biāo)系到像素坐標(biāo)系的轉(zhuǎn)換關(guān)系為：

式中：s 為比例系數(shù)；K 為相機(jī)的內(nèi)參矩陣；[r1r2r3t]為從世界坐標(biāo)系到相機(jī)坐標(biāo)系的齊次變換矩陣；H 為相機(jī)單應(yīng)性矩陣；fx,fy是在u,v 方向的放大系數(shù)；u0,v0表示圖像中心點(diǎn)的坐標(biāo)；μ表示相機(jī)在軸向的畸變系數(shù)，通過張正友相機(jī)標(biāo)定算法[11] 獲取。

本文使用直接線性變換（Direct Linear Transform, DLT）方法獲得目標(biāo)點(diǎn)在左相機(jī)坐標(biāo)系下的坐標(biāo)。如圖3 所示，假設(shè)世界坐標(biāo)系下有1 點(diǎn)P，坐標(biāo)為[X Y Z 1]T，P 在左、右相機(jī)下的投影點(diǎn)分別為pL,pR。

圖3 雙目視覺系統(tǒng)模型

在實(shí)際的投影中，通過標(biāo)定完畢的相機(jī)內(nèi)參進(jìn)行計算得到的結(jié)果很難實(shí)現(xiàn)兩個點(diǎn)完全重合，這種情況下可以取交線的一個中點(diǎn)，如圖3 所示。根據(jù)式(7)，對左、右相機(jī)分別建立從相機(jī)坐標(biāo)系到世界坐標(biāo)系的轉(zhuǎn)換關(guān)系如下：

式中：u1,v1表示P 點(diǎn)在左相機(jī)像素坐標(biāo)系下的坐標(biāo)；KL為左相機(jī)的內(nèi)參矩陣；P1是內(nèi)參矩陣和外參矩陣相乘的結(jié)果；P11,P12,P13為P1的3 個行向量；X 表示世界坐標(biāo)系坐標(biāo)。對等式兩邊同時叉乘x1有：

從而有：

通過觀察式(11)可以得到兩個線性方程。同理，右相機(jī)下具有相同的結(jié)果。將最終結(jié)果合并可以得到式(12)：

假設(shè)世界坐標(biāo)系和左相機(jī)坐標(biāo)系重合，則左相機(jī)的旋轉(zhuǎn)矩陣為單位矩陣，平移向量為0 向量。右相機(jī)到左相機(jī)的位姿轉(zhuǎn)換關(guān)系可以通過標(biāo)定的方法獲取。因此，式(12)僅有3 個未知數(shù)而具有4 個等式，可以采用最小二乘法進(jìn)行求解。在求解之前，先對向量X 做歸一化處理，即：

構(gòu)造最小二乘的目標(biāo)函數(shù)如下：

該優(yōu)化問題的解可以通過奇異值分解（Singular Value Decomposition, SVD）得到A 的對角化矩陣，其中最小特征值對應(yīng)的特征向量即為所求的結(jié)果。由此得到點(diǎn)P 在左相機(jī)坐標(biāo)系下的坐標(biāo)為：

2.2 基于光流法的無人機(jī)定位和跟蹤

采用已進(jìn)行標(biāo)定的雙目相機(jī)對無人機(jī)進(jìn)行跟蹤。首先在無人機(jī)的底面貼上5 個ArUco marker 標(biāo)記，分別位于無人機(jī)的底面中心和4 個邊角處。利用相機(jī)尋找無人機(jī)，定位到無人機(jī)后采用單個相機(jī)獲取ArUco marker 的光流，從而獲得無人機(jī)的運(yùn)動狀態(tài)。如果滿足抓取要求，則分析雙目相機(jī)獲得的深度圖，找到標(biāo)記點(diǎn)的位置坐標(biāo)，然后通過手眼標(biāo)定得到的轉(zhuǎn)換矩陣獲取無人機(jī)位置。算法流程如圖4 所示。

圖4 無人機(jī)定位抓取流程圖

當(dāng)無人機(jī)出現(xiàn)在相機(jī)視野中時，通過并聯(lián)機(jī)器人調(diào)整上平臺位姿或者控制無人機(jī)使其落到圖像的中心點(diǎn)。同時分析深度圖，得到5 個靶標(biāo)的坐標(biāo)，每個靶標(biāo)4 個角點(diǎn)，一共20 個點(diǎn)。5 個靶標(biāo)都位于無人機(jī)平面，可以采用最小二乘法擬合該平面：

式中：z =ax+by+c 為需要擬合的平面方程；Zi為實(shí)際測量的坐標(biāo)；靶標(biāo)點(diǎn)數(shù)n=20。

通過雙目相機(jī)獲取的靶標(biāo)坐標(biāo)是左相機(jī)坐標(biāo)系坐標(biāo)，從相機(jī)坐標(biāo)系到機(jī)器人坐標(biāo)系的轉(zhuǎn)換還需要手眼標(biāo)定。假設(shè)從相機(jī)坐標(biāo)系到世界坐標(biāo)系的旋轉(zhuǎn)矩陣為R，空間坐標(biāo)t ∈R3?，F(xiàn)有世界坐標(biāo)系下的3D 空間點(diǎn)Xi∈R3和圖像中對應(yīng)的點(diǎn)xi，其中i 為匹配點(diǎn)集合的索引，最小化3D 空間點(diǎn)投影誤差公式為：

式中，F(xiàn)(·)表示投影函數(shù)，可以寫成：

對于式(16)表示的凸函數(shù)，其局部最優(yōu)解即為全局最優(yōu)解，因此初始化矩陣R,t 可以取任意值?？梢杂嬎愠霭袠?biāo)點(diǎn)在機(jī)器人坐標(biāo)系中的位置向量為：

式中，TCtoR為標(biāo)定的齊次變換矩陣。進(jìn)一步擬合所有點(diǎn)，即可得到擬合的平面方程。

3 實(shí)驗(yàn)和結(jié)果

本文采用小覓雙目深度D1000-50 相機(jī)，分辨率為1 280×720，像素尺寸為3.75 μm×3.75 μm，基線長度為120 mm，焦距為3.9 mm，深度工作距離為0.49 ～10 m。采用張正友相機(jī)標(biāo)定法，最終標(biāo)定結(jié)果如表1 所示。

表1 相機(jī)內(nèi)參表

從標(biāo)定的結(jié)果來看，相機(jī)的基線距離為120.1 mm，左相機(jī)焦距為3.87 mm，右相機(jī)焦距為3.87 mm，這和實(shí)際相機(jī)的標(biāo)稱參數(shù)吻合。利用標(biāo)定好的相機(jī)對目標(biāo)進(jìn)行檢測，如圖5 所示。

圖5 雙目實(shí)際測量結(jié)果

最終得到4 個角點(diǎn)的左相機(jī)坐標(biāo)系坐標(biāo)。為了衡量相對精度，計算了角點(diǎn)兩兩之間的距離，并和實(shí)際的距離進(jìn)行對比，如表2 所示。

表2 測量結(jié)果

從表2 可以看出，任意兩點(diǎn)之間的距離約為90 mm，和實(shí)際距離一致，而角點(diǎn)的坐標(biāo)Z 值變化較大，這是因?yàn)閷?shí)際拍攝過程中marker 和相機(jī)之間存在一個傾角。因此，在距離相機(jī)30 ～90 mm 的范圍內(nèi)，相機(jī)的相對精度能控制在1 ～2 mm 以內(nèi)，滿足無人機(jī)跟蹤要求。

最后，為了驗(yàn)證本文提出的算法，采用ArUco marker 進(jìn)行了一次初步檢測實(shí)驗(yàn)。圖6(a)中每個靶標(biāo)角點(diǎn)處粗線標(biāo)識線代表marker 點(diǎn)的運(yùn)動方向，線的長度表示速度的大小。圖6(b)是動態(tài)跟蹤的目標(biāo)在白色背景下繪制出來的軌跡圖。將圖6(b)和實(shí)際情況對比，動態(tài)跟蹤的結(jié)果和實(shí)際運(yùn)動相符。最后，通過相機(jī)的雙目標(biāo)定、手眼標(biāo)定衡量檢測的誤差，并通過一定的誤差補(bǔ)償對最終結(jié)果進(jìn)行校正。為了驗(yàn)證本文提供的方案，設(shè)計并加工了原理樣機(jī)，展開了無人機(jī)抓捕實(shí)驗(yàn)。

圖6 動態(tài)目標(biāo)識別與跟蹤

實(shí)驗(yàn)過程中，首先控制粘貼有靶標(biāo)圖案的無人機(jī)飛抵抓捕與收納機(jī)器人的工作空間，并利用無人機(jī)的定點(diǎn)懸停功能懸停在機(jī)器人的工作空間內(nèi)。機(jī)器人視覺系統(tǒng)獲取無人機(jī)靶標(biāo)的圖像信息，并計算得到無人機(jī)的位姿信息。接下來控制系統(tǒng)根據(jù)相機(jī)反饋的位姿信息發(fā)出控制指令，驅(qū)動抓捕平臺快速向目標(biāo)移動。在滿足抓捕條件時，控制系統(tǒng)向抓手發(fā)出抓取指令，完成對無人機(jī)的抓取。最后，機(jī)器人系統(tǒng)收回電動缸并鎖緊電動缸與抓手，完成對無人機(jī)的收納，如圖7所示。

圖7 無人機(jī)抓捕實(shí)驗(yàn)

4 結(jié)束語

本文將ArUco marker 和稀疏光流相結(jié)合，實(shí)現(xiàn)了對無人機(jī)的精準(zhǔn)動態(tài)跟蹤，避免了特征點(diǎn)誤匹配帶來的誤差。通過雙目視覺系統(tǒng)，測量目標(biāo)點(diǎn)的相機(jī)坐標(biāo)系坐標(biāo)，在0.3 ～2 m 的距離范圍內(nèi)，測量相對精度能夠控制在毫米量級，在不采用任何圖形處理器（Graphics Processing Unit, GPU）加速的情況下能夠控制在15 ～25 幀的速率；然后通過手眼標(biāo)定將相機(jī)坐標(biāo)系轉(zhuǎn)換到并聯(lián)機(jī)器人的上平臺坐標(biāo)系；最后，對所有的標(biāo)記點(diǎn)進(jìn)行平面擬合，計算出動態(tài)目標(biāo)的實(shí)時位姿。本文通過實(shí)驗(yàn)驗(yàn)證了采用ArUco marker 和稀疏光流實(shí)現(xiàn)動態(tài)跟蹤的有效性，并衡量了雙目測量技術(shù)和設(shè)備的精度。