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

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

引 言

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

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

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

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

1.1 稀疏光流原理

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

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

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

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

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

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

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

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

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

解得：

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

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

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

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

ArUco marker 算法的主要步驟為：

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

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

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

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

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

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

2 基于光流法的無(wú)人機(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ù)，通過(guò)張正友相機(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í)際的投影中，通過(guò)標(biāo)定完畢的相機(jī)內(nèi)參進(jìn)行計(jì)算得到的結(jié)果很難實(shí)現(xiàn)兩個(gè)點(diǎn)完全重合，這種情況下可以取交線的一個(gè)中點(diǎn)，如圖3 所示。根據(jù)式(7)，對(duì)左、右相機(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 個(gè)行向量；X 表示世界坐標(biāo)系坐標(biāo)。對(duì)等式兩邊同時(shí)叉乘x1有：

從而有：

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

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

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

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

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

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

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

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

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

通過(guò)雙目相機(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和圖像中對(duì)應(yīng)的點(diǎn)xi，其中i 為匹配點(diǎn)集合的索引，最小化3D 空間點(diǎn)投影誤差公式為：

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

對(duì)于式(16)表示的凸函數(shù)，其局部最優(yōu)解即為全局最優(yōu)解，因此初始化矩陣R,t 可以取任意值?？梢杂?jì)算出靶標(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，基線長(zhǎng)度為120 mm，焦距為3.9 mm，深度工作距離為0.49 ～10 m。采用張正友相機(jī)標(biāo)定法，最終標(biāo)定結(jié)果如表1 所示。

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

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

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

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

表2 測(cè)量結(jié)果

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

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

圖6 動(dòng)態(tài)目標(biāo)識(shí)別與跟蹤

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

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

4 結(jié)束語(yǔ)

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