黃一凡，楊建業(yè)，戚國慶

（南京理工大學(xué)自動化學(xué)院，江蘇南京210094）

對室內(nèi)的運動目標(biāo)輔助定位，時長期以來人們研究的熱點。早在上個世紀(jì)70年代末，Jain等人在文獻(xiàn)[1]中提出了使用幀間差分的方法來提取運動目標(biāo)。Birchfield等人[2]提出一種基于顏色及空間的直方圖跟蹤算法，這種方法不僅利用顏色分布，還利用了像素之間的空間分布關(guān)系，提高了跟蹤效率。文獻(xiàn)[3-5]提出的跟蹤算法是基于邊緣的直方圖特征，這種算法相對于之前的算法可以提取目標(biāo)更多的信息，獲得更加理想的跟蹤效果。

目前廣泛應(yīng)用的無人機(jī)三維空間定位技術(shù)有兩種方式：一類是無人機(jī)通過攜帶的裝備，如GPS、慣性導(dǎo)航，同步定位與制圖（SLAM），獲得精確的位置信息，機(jī)載設(shè)備定位方式是集成在飛行控制系統(tǒng)中，不能獨立于無人機(jī)，靈活性較差。另一類是借助外設(shè)備提供精確的位置信息，如全球衛(wèi)星定位系統(tǒng)[6]，運動捕捉系統(tǒng)[7-8]，運動捕捉系統(tǒng)擁有高分辨率的攝像頭，可以亞毫米級地追蹤一個或多個無人機(jī)的位姿[9-10]。運動捕捉系統(tǒng)定位精度較高，但其造價較為昂貴，便攜性較差，更重要的是，系統(tǒng)需要在目標(biāo)上安裝標(biāo)記點，更換定位目標(biāo)時，也需要安裝相應(yīng)的標(biāo)記點，較為繁瑣。因此對于室內(nèi)目標(biāo)，設(shè)計一種高精度、低成本、便于攜帶的輔助定位方法，具有重要的需求[11-13]。

針對該問題，本文將目標(biāo)跟蹤與室內(nèi)定位技術(shù)相結(jié)合，設(shè)計了一套基于二自由度轉(zhuǎn)臺的雙目視覺跟蹤隨動系統(tǒng)。通過雙攝像頭交會定位提高了定位的精度，單片機(jī)控制云臺轉(zhuǎn)動提高了可跟蹤的范圍，擺脫了以往單個攝像頭在視頻跟蹤系統(tǒng)中的局限性。

1 攝像機(jī)標(biāo)定

1.1 坐標(biāo)轉(zhuǎn)換

要將相機(jī)坐標(biāo)系轉(zhuǎn)換成為世界坐標(biāo)系[14]，需要引入兩個矩陣，一個是旋轉(zhuǎn)矩陣R另一個是平移矩陣T。首先假設(shè)存在一個點P，它在兩個坐標(biāo)系下的坐標(biāo)分別標(biāo)示為那么可以得到以下關(guān)系：

上式中，R為正交陣，t為平移向量，是四階矩陣。

1.2 雙攝像機(jī)標(biāo)定原理

圖1 雙攝像機(jī)標(biāo)定原理圖

如圖1，假設(shè)空間存在一點P，分別從左右相機(jī)對P點進(jìn)行拍攝，再分別用單相機(jī)的標(biāo)定方法對P點進(jìn)行單獨標(biāo)定，得到左相機(jī)的外參和右相機(jī)的外參，那么C1（左）相機(jī)坐標(biāo)系的位置用R1和t1表示，C2（右）相機(jī)坐標(biāo)系的位置用R2和t2表示。那么假設(shè)任意一點P在C1相機(jī)坐標(biāo)系的坐標(biāo)是XC1，在C2相機(jī)坐標(biāo)系下的坐標(biāo)是XC2，在世界坐標(biāo)系下的坐標(biāo)是Xw，得到以下關(guān)系：

解（2）方程，消去兩邊共同的Xw，得：

左右相機(jī)的外參數(shù)如下表示：

最后得到兩個攝像機(jī)下的坐標(biāo)關(guān)系：

式（6）得到了以左相機(jī)為原點的坐標(biāo)系。

2 目標(biāo)跟蹤

2.1 KCF算法原理

KCF跟蹤算法的基本思想是不斷對嶺回歸分類器學(xué)習(xí)，它的跟蹤原理如圖2所示。

圖2 KCF原理圖

在訓(xùn)練期間，KCF算法會運用循環(huán)矩陣?yán)碚搶蛉〉哪繕?biāo)區(qū)域進(jìn)行采樣，得到正負(fù)樣本，從而提取出方向梯度直方圖特征，將采樣得到的所有樣本對嶺回歸分類器進(jìn)行訓(xùn)練。

2.2 嶺回歸分類器

首先，對線性嶺回歸分類器進(jìn)行訓(xùn)練的目標(biāo)就是一定要找到一個函數(shù)f(x)=wTx，使得損失函數(shù)最小，即:

式（7）中x=（x1，x2，…，xn），表示為樣本，其中λ為正項，目的是為了防止過擬合，w是所求的參數(shù)。通過上面的式（7）可以求出一個封閉解的模式：

式（8）中，矩陣X是基本組合的循環(huán)矩陣，y是每個樣本的標(biāo)簽值的集合。引入一個核函數(shù)理論來對樣本進(jìn)行分類，其思想是把低維空間里面線性不可分的模式通過核函數(shù)映射到高維空間實現(xiàn)線性可分，核函數(shù)的格式如下：

式（9）中k(x,z)為核函數(shù)，φ(x)和φ(z)為映射函數(shù)。當(dāng)使用核函數(shù)將樣本x映射為φ(x)時，所求的分類器f（x）=wTx中的系數(shù)w轉(zhuǎn)化為對偶空間中的a。由表示定理可知，系數(shù)ω是樣本x的線性組合，即：

由式（7）和式（11）可以得到：

式（12）中，K為映射之后的核矩陣，Ki，j=k（xi，xj）。在檢測階段，輸入的視頻經(jīng)過嶺回歸分類器的分類來確定最終的目標(biāo)位置信息，將得到的目標(biāo)位置信息繼續(xù)訓(xùn)練嶺回歸分類器。通過這種反復(fù)訓(xùn)練的方法對目標(biāo)完成跟蹤。

3 交會定位

如圖3所示，O1、O2表示左右相機(jī)坐標(biāo)系，P表示被跟蹤目標(biāo)，f表示相機(jī)焦距，T表示左右相機(jī)間距，d是物體分別在兩個相機(jī)所成的像之間的距離，所以需要求得d的大小，再根據(jù)相似三角形的原理求出Z[15]。

即

圖3 交會定位原理圖

4 云臺控制

4.1 云臺簡介

本實驗采用的云臺是飛宇mini云臺，搭載的相機(jī)是Gopro4以及Gopro3+。飛宇mini云臺是可以通過外部PWM信號進(jìn)行控制的，PWM波的有效信號范圍為：1 020～2 020 μs（1 520 μs是中間值）其中1 000～1 200 μs為鎖定模式，1 200～1 800 μs為航向和俯仰跟隨模式，1 800 μs以上為航向跟隨模式。

信號電平要求3.3 V（如果5 V需加100 Ω的限流電阻），在平衡狀態(tài)（鏡頭朝向正前方）時PWM信號脈寬要求 1 520 μs，最大值為 2 020 μs，最小值為1 020 μs。在位置模式下，最大值 2 020 μs對應(yīng)的位置是電機(jī)能轉(zhuǎn)動到的最大位置（鏡頭朝正下方或正上方）。

4.2 ARDUINO單片機(jī)對云臺控制

ARDUINO單片機(jī)可以生成指定脈寬的PWM波，而在位置模式下，可以控制云臺轉(zhuǎn)動指定角度。當(dāng)生成的PWM波為1020 μs時，對應(yīng)的角度為0度，當(dāng)PWM波為2020 μs時，對應(yīng)的角度為180度。

當(dāng)跟蹤的目標(biāo)距離相機(jī)的上、下、左、右邊界200萬像素時，通過單片機(jī)控制云臺，使云臺向著相應(yīng)的方向轉(zhuǎn)動5度，達(dá)到對目標(biāo)的有效跟蹤。

5 實驗結(jié)果展示

圖4為實驗硬件示意圖，雙攝像機(jī)分別置于二自由度云臺上，單片機(jī)通過對像素的判斷控制云臺轉(zhuǎn)動，實現(xiàn)跟蹤與定位。

圖4 實驗設(shè)備圖

使用Matlab的Stereo Camera Calibrator工具箱對相機(jī)標(biāo)定，得到結(jié)果如表1所示。

跟蹤和定位程序使用VS2013與OPENCV3.0編寫。兩個相機(jī)處于同一平面且相距15厘米放置，通過上述的標(biāo)定方法使之處于以左相機(jī)為原點的坐標(biāo)系下。實驗設(shè)備如圖5所示。

表1 雙攝像機(jī)標(biāo)定結(jié)果

圖5 硬件示意圖

選取跟蹤的目標(biāo)后，左右相機(jī)的畫面分別呈現(xiàn)如圖6，當(dāng)跟蹤目標(biāo)向上運動時，左右相機(jī)的畫面分別呈現(xiàn)如圖7所示。

圖6 飛行前跟蹤目標(biāo)

圖7 飛行中跟蹤目標(biāo)

可以看出當(dāng)目標(biāo)以一定速度向上運動時，KCF算法完全可以跟上框取的運動物體。

將目標(biāo)的位置數(shù)據(jù)導(dǎo)出，繪制成目標(biāo)的位置軌跡圖，如圖8所示。

通過程序得到其中某一時刻物體的三維坐標(biāo)分別為x坐標(biāo)-68.1063 mm，y坐標(biāo)537.593 mm，z坐標(biāo)2735.84 mm。

圖8 位置軌跡圖

由x、y、z3個坐標(biāo)計算出物體距離坐標(biāo)原點（即左相機(jī)）2.7 m，而用卷尺實際測量出來物體的距離為2.58 m，誤差為4.65%。由于相機(jī)的標(biāo)定本身存在不可避免的誤差，如果要減小誤差，可以增加標(biāo)定圖片的數(shù)量或者提高標(biāo)定板的精度。

6 結(jié)束語

本文針對室內(nèi)定位設(shè)計了一套基于二自由度轉(zhuǎn)臺的跟蹤隨動系統(tǒng)。通過KCF算法完成對無人機(jī)的跟蹤，用交會定位原理得到無人機(jī)的位置，通過單片機(jī)對二自由度轉(zhuǎn)臺進(jìn)行控制，提高了跟蹤范圍，實現(xiàn)了雙目交會定位，并且通過實驗驗證了該系統(tǒng)的有效性。