一種SURF-FREAK特征誤差模型視覺里程計

劉 軍，柴宏旭，趙 威

(蘭州理工大學 機械電子工程學院，甘肅 蘭州 730050)

0 引 言

近年來，隨著自動化水平的提高，智能機器人已經成為生產生活中的一部分，如掃地機器人、無人機等。無人設備在未知環境下執行任務可通過自身攜帶的傳感器獲取環境信息從而完成構建導航地圖、路徑規劃等相關任務。而視覺傳感器不僅能獲得豐富的圖像信息，且具有質量輕、價格低、體積小等優勢，在機器人導航方面得到了廣泛應用。

視覺里程計(visual odometry，VO)是智能機器人同步定位與地圖構建(simultaneous localization and mapping，SLAM)的關鍵技術之一[1-2]，可實現移動機器人在未知環境下的自主定位，為環境地圖模型的建立與機器人導航提供位置信息。該技術起源于二十世紀八十年代，自2004年David Nister發表了一篇以視覺里程計為標題的論文[3]，并于同年成功應用于美國國家航空航天局(NASA)主導的火星漫游者勇氣號和機遇號上后，掀起了新一輪的研究高潮。經過眾多學者的努力，當前其主流技術主要發展為三大類：直接法[4-5]、特征點法[6-7]、半直接法[8]。較典型且已開源的A.J.Davison所提MonoSLAM方案的視覺里程計技術[5]即屬于直接法，其技術特點是直接通過計算像素灰度值的差異來確定相機運動。但直接法在過程中非常容易丟失特征點從而導致相機定位失敗。特征點法主要基于特征點進行圖像匹配，相比于直接法更穩定。較典型的如2015年Raul Mur-Artal所提出的ORB-SLAM[7]，以ORB特征點貫穿于系統的特征處理、匹配、回環檢測等方面，在全局優化基礎之上可以獲得較準確、全局一致的相機運動軌跡。半直接法將特征點法和直接法相結合，在關鍵幀中提取特征點，但在普通幀上通過計算灰度值差異進行特征跟蹤，如文獻[8]等。該技術雖具有較快的單幀處理速率以及良好的實時性，但在定位精度以及魯棒性上存在不足之處。

上述視覺里程計方案主要基于2D圖像模型，因此容易受到環境因素的影響，如相機旋轉、圖像模糊、弱紋理區域等等，易導致定位失敗，因而難以處理復雜環境或難以滿足復雜任務要求場景的建模要求。而深度相機近年來得到了廣泛關注與應用[9]，其除圖像信息之外也提供深度信息，可較好地彌補和解決上述問題[10]。目前基于深度相機的視覺里程計有兩種技術模型：一種為3D-2D點位姿求解，如PnP(perspective-n-point)模型，通過已知三維空間點和圖像中所對應投影點的位置關系來估計相機位姿；另一種為3D-3D點云位姿求解，即通過在不同坐標系下觀測到的同一三維空間點集信息來估計相機位姿，例如Besl、Mckay等人[11]提出的迭代最近點算法等，其算法是基于四元數來定義相機位置，從測量點確定其對應就近點的點集后，通過構建代價函數(cost function)進行迭代計算，直至達到允許誤差從而結束迭代過程。因此，為提高處理效率，許多學者結合深度相機的特點提出了諸多改進算法，例如，Siemiatkowska等人在2D圖像上提取特征點進行粗配準，然后將粗配準得到的變換矩陣作為精配準的初始值，通過改進算法初始值的方式減小迭代次數以提高效率[12]。Ji H等人提出了一種ColorICP算法，利用三維點的顏色信息，在尋找最近點的時候，只在顏色相近的區域尋找最近點作為匹配點，可有效減少計算量，提高匹配效率[13]。其他圍繞提高計算效率方面的努力如文獻[14-16]等，但上述改進要么效率提升有限，要么往往都以犧牲精度為代價，存在不同的技術缺陷，如光照變化影響大、空間旋轉尺度難以保證等等，很難兼顧精度與效率，在此不再一一贅述。

基于此，為兼顧精度與效率，受文獻[17]啟發，該文提出了一種新的視覺里程計技術。該技術將SURF特征點提取方式與FREAK描述子相結合，在粗配準確定特征點時，為保證精度，采納SURF特征點提取方式以保持空間尺度不變性，同時采用FREAK描述子，即以先行檢查前128位描述信息而非對全部描述信息進行檢查的方式提高檢查效率。同時在精配準時，在概率與統計學意義下，不同于傳統采納歐氏距離對特征點距離進行度量的方式，而是結合實際情況將所有特征點的位置誤差也引入到距離函數中，即提出了一種歐氏、馬氏距離相混合的距離函數對特征點距離進行度量，該種方式更符合實際情況，既有效度量了特征點的概率位置，也考慮了其位置方差的影響；并且，為進一步增強特征點空間位置準確性與降低視覺里程計累計誤差，在精匹配后采用卡爾曼濾波對特征點空間位置進行更新，以提高特征點位置與相機位姿精度。尤其不同于傳統視覺里程計技術僅給出相機概率位置期望，該文在上述改進的基礎上，給出了相機位置的方差計算公式。標準數據集測試實驗表明，該技術能有效兼顧實時性和準確性，并能較好地對相機運動軌跡進行估計。

1 視覺里程計問題描述

視覺里程計是通過視覺方式還原出相機運動軌跡。該文在考慮視覺深度信息誤差的基礎之上，提出了一種新型視覺里程計技術，以解決當前基于深度相機的視覺里程計技術在小型自主移動設備上使用時的精度與效率協調問題。通過對相機采集到的圖像進行處理，計算出相機每一時刻運動位姿，即公式(1)，從而構建出相機運動軌跡。

令世界坐標系(用上標w表示)是一個固定的參照坐標用來描述相機和物體的位置；相機坐標系(用上標c表示)是相機視角下建立的坐標系；像素坐標(u,v)是在相機采集的圖像上建立的二維坐標。相機初始位姿坐標定義為世界坐標，相機坐標后續不斷變化。相機間后續位姿變化用一個4×4的變換矩陣表示：

(1)

每一時刻在不同位姿處，空間特征點與相機特征點的轉換關系通過公式(2)呈現。設相機坐標系下的特征點為PC，采集圖像投影點為p，像素坐標記為ui，此坐標可由變換矩陣T轉換到世界坐標系下：

(2)

2 一種融合SURF-FREAK特征點誤差模型視覺里程計技術

傳統的視覺里程計建立圖像噪聲擾動模型，但未考慮Kinetic相機采集深度數據時存在噪聲，會導致深度值d存在誤差，對位姿估計的準確性有較大的影響，甚至會導致機器定位失敗。因此，受文獻[17]啟發，該文在其基礎上采取高斯誤差模型描述Kinect深度值的誤差；并且為了快速獲得準確的相機位姿，先行通過圖像配準環節進行粗配準，為后續精配準環節提供初始值，減少精配準目標函數迭代次數，提高效率。

2.1 基于SURF-FREAK圖像特征匹配

在粗配準環節，該文結合SURF特征點提取方式和FREAK描述信息的優勢，提高特征點準確性和效率。相較于傳統的FAST提取方式，因通過不同尺度的盒式濾波(box filter，BF)與圖像卷積構建出多尺度空間，在圖像中穩定的特征點進行檢測篩選；并且SURF特征點描述子在匹配時計算量大，結合FREAK描述子通過二進制描述符，以先行匹配128位描述信息而非對全部描述信息進行檢查的方式提高檢查效率。

2.1.1 SURF特征提取

SURF特征檢測是通過計算Hessian矩陣行列式，以局部曲率極大值來檢測特征點的區域，并通過盒式濾波(box filter)構建多尺度空間，對特征點和原始圖像做卷積，進行不同尺度空間的極值檢測，從而生成圖像穩定的邊緣點(突變點)，用于特征點提取，使其具備多尺度不變性。經過Hessian矩陣處理的每個像素點與其圖像域和尺度域的所有相鄰點進行比較尋找極值點，再通過盒式濾波器構建多尺度空間，濾除掉不具備多尺度不變性的極值點，篩選出最終穩定的特征點。傳統SURF特征點在匹配時，建立特征點主方向，通過在特征點周圍選取一個正方形框，框的邊長為20倍特征點像素大小。然后將該框平分為16個子區域，在每個子區域計算25個像素的水平方向和垂直方向的haar小波特征，并對水平方向和垂直方向計算絕對值之和，構造描述向量。以此作為圖像特征區域描述信息，對圖像進行匹配。

SURF特征點匹配如圖1所示。

圖1 SURF特征點匹配

SURF特征點可以通過提高閾值來提高匹配準確性，但SURF特征需要計算每個尺寸的haar小波響應，計算量較大；此外圖像配準時，通過計算特征點周圍描述向量間的歐氏距離，確定特征點匹配度。SURF特征點描述子雖然精度高，但存在計算量大、效率低等問題。基于此，該文通過FREAK二進制描述代替SURF特征描述的方式來提高效率。

2.1.2 FREAK描述子及匹配

圖像通過SURF完成特征提取，通過二進制描述符FREAK描述子代替SURF描述子，以降低計算量，提高效率。相較于SURF特征描述子，一方面通過二進制編碼來代替haar小波響應，以提高運算速度；另一方面FREAK描述子模擬人眼，根據采樣點與特征點的距離遠近，采用了不同大小的高斯核函數進行平滑處理，通過重疊的感受野，可以獲得更多的信息，使最終的描述符更具獨特性和可辨識性。

圖2 FREAK描述子

FREAK描述子[18]是通過模擬人眼視網膜對特征點周圍的圖像信息進行處理。如圖2(a)所示，人眼的視網膜根據感光細胞的密度分成了三個區域，即中央凹(fovea)、旁中央凹(parafoveal)、周邊區(perifoveal)。fovea區域感光細胞密度大，能夠獲得較高的分辨率，主要用于細節識別；perifoveal區域感光細胞密度較少可獲得低分辨率的圖像，可用于輪廓識別。FREAK描述子就是借鑒了人類視覺分區獲取信息而提出的一種算法，FREAK描述子采樣像素分布在多個同心圓，采樣像素的分布是按照指數形式的分布密度分布的，越接近同心圓的分布密度越大，如圖2(b)所示。

二進制編碼FREAK特征描述算法，編碼過程見公式(3):

(3)

最終，根據計算得到了512 bit的二進制描述符，該描述符的列是方差由高到低的排列，而高方差表征了模糊信息，低方差表征了細節信息。因此，選取前128 bit即16 bytes進行匹配，若兩個待匹配的特征點前16 bytes距離小于設定的閾值，則再用剩余的比特位進行匹配。第一步匹配可以剔除掉90%的不相關匹配點，很大程度上提高了匹配的速度。通過融合SURF-FREAK特征提取描述算法，對室內區域進行環境匹配，如圖3所示。

圖3 SURF-FREAK圖像匹配

至此相機通過圖像匹配技術完成粗匹配，結合圖像間對極幾何約束預估相機初始位姿，為后續精匹配環節提供了一個較好的初值，有效縮短了迭代次數，降低了計算復雜度。

2.2 基于高斯分布的特征點云幀間配準模型

傳統的視覺里程計只考慮圖像噪聲擾動模型，但未考慮RGB-D相機采集深度數據時存在噪聲，導致深度值d存在誤差，對位姿估計的準確性有較大的影響。因此，該文采取高斯誤差模型描述RGB-D相機深度采集圖像的誤差，以便更好地估計相機的位姿變化。由于深度相機采集的數據存在噪聲，會導致像素坐標(u,v)及深度值d存在誤差，現作如下假設：

(1)u、v相互獨立，且服從正態分布，即u～N(μu,σu)，v～N(μv,σv)；

(2)相機相鄰時刻觀測值相互獨立；

(3)同一時刻圖像提取不同特征點相互獨立；

像素(u,v)處z的均值和方差為：

i∈[μu-1,μu+1],j∈[μv-1,μv+1]

(4)

空間點P(x,y,z)的位置期望為：

μ=(μx,μy,μz)T

(5)

且μz滿足協方差矩陣，可表示為：

(6)

將圖像特征點誤差模型通過高斯表達，位置期望μ表示特征點位置理想情況，而協方差矩陣表達特征點位置在期望附近分布模型。在精配準環節，通過建立累積誤差模型，并對其優化降低協方差矩陣數值，提高位置準確性。

3 點云圖像幀間精配準模型

本節根據圖像局部特征匹配為精配準環節中ICP算法提供初值，結合RGB-D相機圖像高斯誤差模型，改進傳統ICP算法的距離函數，構建出新型的ICP算法目標函數，并推導出相機運動位置的期望和協方差。

3.1 ICP算法幀間配準計算

首先，將已配準特征點帶入ICP模型計算。使用特征描述向量x，匹配到新一幀與歷史模型集中的相似特征點，進行狀態估計。

該文結合2.2節世界坐標系下特征點位置方差的描述，引入Kinect相機位姿方差，使其局部特征點位置不確定性模型具有更加完整的描述：

(7)

然后，將相鄰關鍵幀的特征匹配點集帶入ICP算法進行配準。其中ICP匹配算法中的傳統歐氏距離不能體現特征點位置的空間方差，而馬氏距離是一種有效計算相互匹配的未知樣本集合之間的相似度計算方法。用馬氏距離作為ICP算法配準準則時，對微小的擾動非常敏感，會夸大擾動或誤匹配帶來的影響，且缺乏描述空間距離的幾何意義和直觀性。因此，ICP算法的配準特征點之間的距離函數為：

(8)

其中，

3.2 視覺里程計累計誤差優化模型

根據公式(8)，構造世界坐標系下相機位姿累計誤差模型，并求出最優的相機位置期望及協方差。

設當前場景和模型集中參與ICP配準的特征點一共為n對，其中k時刻的第i個特征點的位置期望和協方差矩陣分別用μPi,k、ΣPi,k表示。

根據ICP算法，相機位置期望μPi,k應滿足：

(9)

通過對上式用μtk求偏導，計算期望極值點：

(10)

整理可得，相機位置期望μtk應滿足：

(11)

根據協方差的定義，相機位置協方差陣Σtk應滿足：

Σtk=E[(tk-μtk)(tk-μtk)T]

(12)

(13)

3.3 基于EKF的特征點位置一致化更新

考慮到觀測噪聲的存在，采用卡爾曼濾波更新特征點空間位置的一致性，降低視覺里程計的累計誤差。首先，k-1時刻W坐標系下的坐標表示k時刻特征點位置的預測值，即：

(14)

(15)

當用以表示特征點空間位置誤差的協方差矩陣較小時，可以用式(16)對相機位姿的期望及協方差進行近似，相對于原式(11)、(13)一定程度上降低了算法的運算量：

(16)

至此，通過公式(16)可以得到相機當前時刻的位置期望及協方差，通過記錄每一時刻相機位置期望，可以得到相機運動軌跡。

4 實驗與分析

實驗軟硬件配置如下：

深度相機：Kinect v2；

計算機硬件配置：AMD A8處理器，3.0 GHz主頻，不使用GPU加速，4 G內存；

計算機系統為Ubuntu16.04，運行平臺ROS kinetic。

為了驗證算法的有效性和可對比性，采用TUM標準數據集，通過對辦公桌前相機簡單平行運動frg1_xyz數據集在保證精度的基礎上測試算法實時性；此外在復雜室內辦公室場景frg1_room數據集測試其準確性。同時作為比較，使用基于非線性PnP模型BA算法、基于SVD-ICP算法作為對比。

4.1 算法實時性測試

測試數據集frg1_xyz，主要用于程序實時性測試。基于此，該數據集將著重測試程序實時性，其中真實軌跡與估計軌跡對比見圖4，非線性PnP-BA算法、SVD-ICP算法與文中算法的實時性對比見圖5，表1記錄了不同算法下的誤差數據。

圖4 真實軌跡與估計軌跡對比

圖5 不同算法實時性圖像

表1 不同算法誤差 mm

從圖4可以看出真實軌跡與估計軌跡的誤差情況，相機在做直線運動的情況下，沒有產生誤差，但在旋轉過程中產生少許誤差。圖5所展示的是不同算法處理采集圖像所使用的時間，PNP-BA模型算法處理精度最高但時間最慢，原因是PNP-BA算法采用了全局非線性優化，通過構造全局誤差模型并對其優化，降低了整體誤差；文中算法通過圖像匹配技術結合對極幾何約束提供初值，相較于SVD-ICP算法不但提高了精度而且縮短了時間。可以代替ICP算法在輕量級設備上計算相機運動軌跡。表1中數據表明，文中方法誤差不超25 mm，雖然PNP-BA算法誤差很小，但是實時性較差，對處理器有較高的要求，不適用于小型化設備。

4.2 準確性對比實驗

使用測試數據集frg1_room，對文中算法進行測試。文中算法真實軌跡與估計軌跡對比見圖6，SVD-ICP算法真實軌跡與估計軌跡對比見圖7，表2記錄了不同算法模型下的誤差。

圖6 文中算法真實軌跡與估計軌跡對比

圖7 SVD-ICP算法真實軌跡與估計軌跡對比

表2 算法誤差對比 mm

用TUM的測試數據集frg1_room，測試室內環境下相機定位準確性。從圖6可以看出，在相機進行直線運動時產生的誤差較小，但當相機出現旋轉運動時會出現一定的誤差，因為旋轉運動時相機采集的圖像信息會發生劇烈的變化，所提取的特征點會有一部分失效，此時深度圖像估計運動軌跡比重增大，所以程序主要的誤差都是從相機旋轉運動中產生。

相較于SVD-ICP算法，圖7展示的真實軌跡與估計軌跡存在較大差異，一方面SVD-ICP算法未對其建立高斯分布的特征點誤差模型，會存在深度圖的噪音干擾；另一方面，文中設計的距離函數，既有效度量了特征點的概率位置，也考慮了其位置方差的影響，因而提高了軌跡精度。表2可以通過數據展示不同算法估計軌跡產生的誤差值，其中室內環境下相機定位誤差不超過30 mm。文中算法可以滿足相機定位精度和實時性需求，能夠較好地實現嵌入式機器視覺定位的需求。

5 結束語

視覺里程計作為SLAM重要環節之一，對減少機器運動軌跡誤差、提高地圖精度至關重要。該文提出一種基于FREAK-SURF特征點深度誤差視覺里程計算法，旨在輕量級設備滿足實時性要求的基礎上提高相機運動軌跡精度，為構造地圖提供準確的位置信息。首先，融合SURF特征提取算法和FREAK描述子完成特征匹配，提高匹配效率；其次，ICP配準環節設計出一種新型的距離代價函數，融合馬氏距離和歐氏距離的特點；然后，根據特征點和相機空間位置誤差模型，建立視覺里程計累計誤差并通過擴展卡爾曼濾波器優化。對比實驗表明，該算法在實時性上強于非線性PNP模型BA優化算法和SVD-ICP算法，在精度上高于傳統的SVD-ICP算法。