劉祥

（上海海事大學(xué)信息工程學(xué)院,上海 201306）

0 引言

視覺里程計(jì)一直是機(jī)器視覺中研究的熱點(diǎn)問題。視覺傳感器，例如數(shù)碼相機(jī)，已經(jīng)成為機(jī)器人導(dǎo)航系統(tǒng)中不可缺少的組成部分，通過相機(jī)獲取并處理圖像，獲得機(jī)器人當(dāng)前的位置、姿態(tài)和距離等信息，簡(jiǎn)稱視覺里程計(jì)。它僅僅通過機(jī)器人攜帶的相機(jī)獲取到的圖像，來追蹤和判斷機(jī)器人當(dāng)前的位置和姿態(tài)，為機(jī)器人導(dǎo)航提供有效依據(jù)。為了估計(jì)機(jī)器人的運(yùn)動(dòng)情況，傳統(tǒng)方法采用激光傳感器，利用迭代最近點(diǎn)法對(duì)激光點(diǎn)云數(shù)據(jù)進(jìn)行匹配，從而求解機(jī)器人的位姿。它的算法思想是在兩次掃描點(diǎn)云數(shù)據(jù)之間重復(fù)進(jìn)行選擇對(duì)應(yīng)關(guān)系點(diǎn)對(duì)，計(jì)算最優(yōu)的剛體變換，直到滿足正確匹配的收斂精度要求。在實(shí)際應(yīng)用中，由于初始值的選擇不確定極易出現(xiàn)局部最優(yōu)解的情況，這在一定程度上限制了它的應(yīng)用。相反的，視覺里程計(jì)法成為當(dāng)下的主流方法，它通過對(duì)圖像進(jìn)行特征提取、特征匹配、位姿估計(jì)以及優(yōu)化等完成室內(nèi)定位任務(wù)。

視覺里程計(jì)的發(fā)展已經(jīng)有20多年了，在國(guó)內(nèi)外廣泛應(yīng)用于商業(yè)和軍事導(dǎo)航系統(tǒng)中。將視覺里程計(jì)技術(shù)應(yīng)用于機(jī)器人中，機(jī)器人可以追蹤自己的位置，進(jìn)行定位，同時(shí)可以構(gòu)建周圍環(huán)境的地圖。隨著圖像處理技術(shù)的發(fā)展，也因視覺傳感器方便攜帶，功耗低，無需GPS等優(yōu)點(diǎn)，視覺里程計(jì)技術(shù)在室內(nèi)導(dǎo)航和定位得到了廣泛的應(yīng)用。使用普通的相機(jī)一直是視覺里程計(jì)的經(jīng)典方法，而近幾年推出的商業(yè)傳感器可以同時(shí)捕獲彩色圖像和深度圖像，即RGB-D傳感器，它無需三角測(cè)量等算法便可得到圖像深度數(shù)據(jù)，在三維重構(gòu)和里程計(jì)方面有著巨大的應(yīng)用價(jià)值。

考慮到實(shí)時(shí)性和實(shí)用性，在對(duì)圖像序列進(jìn)行特征提取時(shí)，并沒有采用效果最好的SIFT特征，而是選用了ORB特征，該特征具有較好的尺度不變性和旋轉(zhuǎn)不變性，同時(shí)能達(dá)到工程實(shí)用的目的。首先，根據(jù)特征描述子對(duì)相鄰時(shí)間序列的圖像進(jìn)行匹配，采用隨機(jī)抽樣一致法進(jìn)行反復(fù)迭代，剔除誤匹配，求出最優(yōu)的本質(zhì)矩陣。然后通過矩陣分解，得到旋轉(zhuǎn)量和平移量，從而恢復(fù)出機(jī)器人的運(yùn)動(dòng)軌跡和姿態(tài)。最后采用光束法平差優(yōu)化，對(duì)局部位姿和周圍的點(diǎn)云地圖進(jìn)行局部和全局優(yōu)化，從而減小里程計(jì)累積漂移誤差并提高定位精度。

1 視覺里程計(jì)算法

1.1 特征提取與匹配

特征點(diǎn)的提取和選擇，首先要考慮到處理每幀圖像不能耗時(shí)過多，同時(shí)不能占用過多的CPU資源，為此我們選用ORB特征。它是一種提取圖像視覺特征的算法，包括特征點(diǎn)的檢測(cè)和特征點(diǎn)的描述兩部分，它采用FAST角點(diǎn)檢測(cè)作為關(guān)鍵點(diǎn)，采用BRIEF描述子做特征描述[1]。FAST角點(diǎn)具有運(yùn)算速度非?？斓膬?yōu)點(diǎn)，同時(shí)可以檢測(cè)出豐富的角點(diǎn)特征，但是不具有方向性。ORB算法在其基礎(chǔ)上進(jìn)行改進(jìn)，通過定義形心，規(guī)定了特征點(diǎn)的主方向，使其具有了旋轉(zhuǎn)不變性。同時(shí)，通過圖像金字塔，在每層圖像上進(jìn)行角點(diǎn)檢測(cè)，使其具有較好的尺度不變性。FAST特征點(diǎn)如圖1所示：

圖1 FAST特征點(diǎn)

FAST特征點(diǎn)的檢測(cè)和判定原則：通過對(duì)比中心像素點(diǎn)與周圍像素點(diǎn)的像素值的灰度差異，超過某個(gè)閾值時(shí)即判定該像素為特征點(diǎn)。通過（1）式角點(diǎn)響應(yīng)函數(shù)可以判斷FAST特征點(diǎn)。

式中：I（x）表示待測(cè)點(diǎn)周圍內(nèi)可選的隨機(jī)點(diǎn)的灰度值。

ORB算法首先計(jì)算特征點(diǎn)周圍的灰度的形心，再將特征點(diǎn)到該特征點(diǎn)周圍的圖形形心的方向作為該特征點(diǎn)的主方向[2]，局部區(qū)域矩的公式如下：

則計(jì)算出特征點(diǎn)周圍的區(qū)域內(nèi)的圖像的灰度形心為：

此時(shí)則可以判定FAST特征點(diǎn)的主方向?yàn)椋?/p>

ORB特征點(diǎn)的描述采用BRIEF描述子，它是由二進(jìn)制位組成的一個(gè)向量，每個(gè)二進(jìn)制位的0和1代表了特征點(diǎn)周圍像素的大小關(guān)系。我們用特征點(diǎn)周圍的128個(gè)像素點(diǎn)比較大小關(guān)系后得到128維的向量來描述該特征點(diǎn)[3]。通過幀與幀，或者幀與地圖之間的特征描述子的匹配，我們可以估計(jì)圖像之間的變換關(guān)系并進(jìn)行位姿優(yōu)化等。由于我們采用了BRIEF描述子，所以需要采用漢明距離來衡量描述子的相似程度，然后確立特征點(diǎn)的對(duì)應(yīng)關(guān)系。

由于圖像特征比較豐富，且需要匹配當(dāng)前幀和地圖之間的關(guān)系，若采用暴力匹配法則所需時(shí)間過久，所以采用快速近似最近鄰算法，該算法適合于匹配點(diǎn)數(shù)量極多的情況。該方法是根據(jù)K均值或KD-Tree搜索操作所實(shí)現(xiàn)的，可以根據(jù)數(shù)據(jù)集的分布特點(diǎn),對(duì)映射精度和空間資源消耗的要求來推薦索引類型和檢索的參數(shù)，在高位空間最近鄰查找不受局部敏感哈希影響。快速近似最近鄰算法模型的特征空間一般是n維的實(shí)數(shù)的向量空間，它的關(guān)鍵在于使用歐氏距離找到特征點(diǎn)的鄰居。圖2是特征提取和匹配的結(jié)果：

圖2 特征提取與匹配

1.2 剔除誤匹配

特征匹配后我們可以看到，并不是每一個(gè)特征點(diǎn)都能正確匹配，存在一定的誤匹配。因此我們選用RANSAC算法來剔除誤匹配[4]，該算法的核心思想是根據(jù)兩幀圖像之間的對(duì)極約束關(guān)系，并通過多次反復(fù)迭代來剔除誤匹配，并根據(jù)最優(yōu)的匹配對(duì)計(jì)算出本質(zhì)矩陣。該算法的具體做法是：

①先設(shè)定采樣的次數(shù)N，有效匹配點(diǎn)與對(duì)極線間的距離閾值M；同時(shí)將相鄰兩幀圖像的所有特征點(diǎn)的像素坐標(biāo)做歸一化處理，分別得到歸一化的變換矩陣。

②在匹配點(diǎn)集中隨機(jī)選取八個(gè)點(diǎn)計(jì)算本質(zhì)矩陣。

③利用本質(zhì)矩陣分別與其余的匹配點(diǎn)計(jì)算匹配點(diǎn)與對(duì)極線之間的距離。

④找出尚未匹配的點(diǎn)集中，對(duì)極線距離滿足小于閾值M的匹配點(diǎn)。

⑤統(tǒng)計(jì)滿足上述條件的特征點(diǎn)的數(shù)ni和特征點(diǎn)的索引號(hào)。

⑥反復(fù)執(zhí)行上述步驟，直到誤差函數(shù)取值達(dá)到最小，并且標(biāo)記對(duì)應(yīng)的索引號(hào)，則我們認(rèn)為此時(shí)的匹配滿足最優(yōu)的匹配。

⑦最后我們用最佳的匹配點(diǎn)集計(jì)算出本質(zhì)矩陣。

該算法是通過反復(fù)迭代，并不斷更新基本矩陣的值，找到最佳匹配關(guān)系[5]。采樣次數(shù)N設(shè)置過大，會(huì)增加處理每幀圖像的時(shí)長(zhǎng)，設(shè)置過小則不能找到最佳的匹配對(duì)，算法失效，起不到剔除誤匹配的效果[6]。下圖是經(jīng)過RANSAC算法后，誤匹配已經(jīng)明顯減少。

圖3 剔除誤匹配后

1.3 位姿估計(jì)

相同一個(gè)攝像機(jī)采集的兩幀圖像之間是有很強(qiáng)的約束關(guān)系的，它們之間滿足下式：

其中X'和X分別代表匹配圖像上的一對(duì)匹配特征點(diǎn)的像素坐標(biāo)，F(xiàn)是基本矩陣，表明了兩幀圖像之間的變換關(guān)系，它是一個(gè)秩為2的3×3的齊次矩陣。

采用歸一化8點(diǎn)法計(jì)算基本矩陣，它的重要思想是在求取基本矩陣之前對(duì)數(shù)據(jù)進(jìn)行歸一化，這樣做并不會(huì)顯著增加計(jì)算量，并且可以容易的求取矩陣[7]。下面給出8點(diǎn)算法的具體步驟：

①歸一化：根據(jù)X,TX變換圖像的像素坐標(biāo)，其中T為歸一化的矩陣變換，包括平移和縮放兩部分。

②由最小奇異值的奇異矢量確定基本矩陣F，即得到線性解。也可以由其行列式的值為零得到強(qiáng)迫約束解。

(三)新會(huì)計(jì)制度中增加預(yù)算會(huì)計(jì)的概念。使得單位會(huì)計(jì)要素包括財(cái)務(wù)會(huì)計(jì)要素和預(yù)算會(huì)計(jì)要素，實(shí)行財(cái)務(wù)會(huì)計(jì)和預(yù)算會(huì)計(jì)平行記賬法，單位對(duì)于納入部門預(yù)算管理的現(xiàn)金收支業(yè)務(wù)，在采用財(cái)務(wù)會(huì)計(jì)核算的同時(shí)又進(jìn)行預(yù)算會(huì)計(jì)核算，增加了預(yù)算收支、預(yù)算結(jié)余的賬務(wù)處理，不僅清晰反映了財(cái)務(wù)主體的運(yùn)行狀況還有效反映和監(jiān)督了會(huì)計(jì)主體預(yù)算收支執(zhí)行情況。

③解除歸一化，得到基本矩陣F。

得到基本矩陣后，并在保持投影點(diǎn)不變的情況下，對(duì)基本矩陣分解得到四個(gè)可能的解。如圖4所示。

圖4 四種可能情況的解

我們根據(jù)P點(diǎn)應(yīng)該在兩幅視圖中都具有正的深度即可篩選出正確的解。分解基本矩陣后得到R和T，R是一個(gè)正交旋轉(zhuǎn)矩陣，T是一個(gè)平移矩陣。求解出R和T，即完成相機(jī)運(yùn)動(dòng)的估計(jì)，得到相機(jī)的運(yùn)動(dòng)軌跡和姿態(tài)[8]。

1.4 光束法平差優(yōu)化算法

估計(jì)相機(jī)的位姿和3D點(diǎn)的空間位置一直是計(jì)算機(jī)視覺的經(jīng)典問題，優(yōu)化相機(jī)位姿和圖像中的特征點(diǎn)，在視覺里程計(jì)中是很重要的一個(gè)部分[9]。采用光束法平差對(duì)路標(biāo)點(diǎn)和相機(jī)位姿進(jìn)行優(yōu)化，在給定相同場(chǎng)景不同圖像當(dāng)中的多個(gè)2D點(diǎn)時(shí)，則光束法平差可以表示為一個(gè)非線性最小二乘問題：

求解這個(gè)方程的最小值，需要獲得較多的觀察圖像，才能得到一個(gè)較好的解。在實(shí)際操作中，通過迭代的方式來求解。通常采用L-M梯度下降策略來求取最優(yōu)位姿,在求解過程中如果能提供良好的初始值，則算法會(huì)快速收斂[10]。在此之前，可用8點(diǎn)法求解初值。其次是步長(zhǎng)的控制，選擇一個(gè)合理的步長(zhǎng)算法會(huì)立即收斂，否則算法可能會(huì)在幾個(gè)谷底之間跳轉(zhuǎn)，出現(xiàn)不收斂的情況。

使用局部的光束法平差對(duì)關(guān)鍵幀和路標(biāo)點(diǎn)進(jìn)行優(yōu)化，這樣有利于提高局部的定位精度，同時(shí)也可以減少累積誤差。在構(gòu)建好地圖之后，采用全局的光束法平差對(duì)整體的位姿和地圖中的路標(biāo)點(diǎn)進(jìn)行優(yōu)化調(diào)整。光束法平差的應(yīng)用，有利于減少噪聲對(duì)系統(tǒng)帶來的影響，它同時(shí)調(diào)整圖像的觀測(cè)值和估計(jì)值，使整個(gè)系統(tǒng)得到持續(xù)的優(yōu)化。

2 算法實(shí)現(xiàn)及結(jié)果分析

算法在Ubuntu14.04操作系統(tǒng)實(shí)現(xiàn)，使用了開源的計(jì)算機(jī)視覺庫(kù)OpenCV，并且采用C++11新標(biāo)準(zhǔn)編程，提高了系統(tǒng)整體的穩(wěn)定性。在程序設(shè)計(jì)過程中，一個(gè)幀通常包括了一個(gè)圖像和特征點(diǎn)、位姿、內(nèi)參等信息，由于相機(jī)采集的數(shù)據(jù)很多，所以我們用關(guān)鍵幀來代表整個(gè)相機(jī)的運(yùn)動(dòng)軌跡，同時(shí)利用關(guān)鍵幀恢復(fù)出運(yùn)動(dòng)和三維的點(diǎn)云地圖和空間結(jié)構(gòu)。此外，地圖點(diǎn)對(duì)于定位來講，可以提供豐富的位置信息，我們還可以將新的幀與路標(biāo)點(diǎn)進(jìn)行匹配，從而估計(jì)運(yùn)動(dòng)信息[11]。對(duì)于基于特點(diǎn)的視覺里程計(jì)來講，最關(guān)鍵的是相鄰兩幀圖像之間的運(yùn)動(dòng)估計(jì)，即當(dāng)前幀與參考幀的匹配關(guān)系，下面給出它的操作步驟：

①對(duì)采集到的新的當(dāng)前幀，提取關(guān)鍵點(diǎn)和描述子。

②如果系統(tǒng)沒有初始化，則以該幀為參考幀，并且根據(jù)對(duì)應(yīng)的深度圖來計(jì)算特征點(diǎn)的3D位置信息，然后返回①。

④判斷上述估計(jì)是否成功。

⑤如果算法成功，則把當(dāng)前幀作為新的參考幀，返回①。

⑥如果失敗，我們統(tǒng)計(jì)連續(xù)丟失的幀數(shù)，當(dāng)連續(xù)丟失的幀數(shù)過大，則設(shè)置里程計(jì)的狀態(tài)為丟失，算法結(jié)束。若未超過，返回①。

本算法最終在廣泛使用的TUM數(shù)據(jù)集上進(jìn)行測(cè)試，TUM的數(shù)據(jù)集通過外部設(shè)備和GPS獲取了標(biāo)準(zhǔn)軌跡，同時(shí)提供了一些比較工具和腳本程序，適合做研究。本次試驗(yàn)從三維重構(gòu)結(jié)果、定位精度、旋轉(zhuǎn)誤差和平移誤差來對(duì)算法進(jìn)行衡量。三維重構(gòu)結(jié)果如圖5所示，其中黃色線代表的是相機(jī)的運(yùn)動(dòng)軌跡，從三維重構(gòu)的結(jié)果來看，空間點(diǎn)的3D位置還是比較準(zhǔn)確的，同時(shí)也基本恢復(fù)了場(chǎng)景的結(jié)構(gòu)。

圖5 場(chǎng)景的三維重構(gòu)結(jié)果

圖6的定位結(jié)果中，紅色線代表的是估計(jì)軌跡與標(biāo)準(zhǔn)軌跡的差值，可以看到在數(shù)據(jù)集的多個(gè)場(chǎng)景下，算法表現(xiàn)優(yōu)秀，特別是在長(zhǎng)時(shí)間運(yùn)行的（d）數(shù)據(jù)集中，系統(tǒng)的累積漂移誤差很小。接下來，對(duì)比光束法平差算法和普通視覺里程計(jì)的旋轉(zhuǎn)誤差和平移誤差，在不同數(shù)據(jù)集的實(shí)驗(yàn)結(jié)果見表1。

表1 旋轉(zhuǎn)和平移誤差結(jié)果

通過對(duì)比研究，我們可以明顯的看到使用光束法平差優(yōu)化后，算法能有效減小幀與幀之間的旋轉(zhuǎn)和平移誤差，由此提高了里程計(jì)的定位精度。

3 結(jié)語

通過加入光束法平差優(yōu)化算法與普通視覺里程計(jì)進(jìn)行對(duì)比研究，驗(yàn)證了算法的有效性。從實(shí)驗(yàn)結(jié)果看，該算法可以有效地減少平移誤差和旋轉(zhuǎn)誤差，提高定位精度，并且在一定程度上，減小了里程計(jì)的累積誤差。

圖6 定位結(jié)果分析