快速室內視覺同步定位與建圖研究

王洪旭，席志紅

哈爾濱工程大學 信息與通信工程學院，黑龍江 哈爾濱 150001

面對家居服務機器人和醫(yī)療機器人技術的發(fā)展，室內同步定位和繪圖(Simultaneous Localization and Mapping，SLAM)這種實現(xiàn)機器人定位與構建環(huán)境地圖的技術也在快速發(fā)展，在實際應用中如何實現(xiàn)SLAM系統(tǒng)快速準確定位和構建豐富環(huán)境信息的地圖也逐漸成為視覺SLAM研究的熱點問題。

SLAM技術的發(fā)展歷程可分為以下3個階段[1]。傳統(tǒng)時代(1986—2004年)：通過求解系統(tǒng)隨時間變化的狀態(tài)估計模型來解決SLAM問題[2]。算法分析時代(2004—2015年)：研究SLAM的基本特性，包括觀測性、收斂性和一致性[2]。魯棒性?預測性時代(2015年至今)：SLAM需要在不同環(huán)境保持良好的魯棒性，能獲取更準確的場景信息。視覺SLAM根據(jù)其應用的圖像特征可以分為直接法和特征點法[3]。單目SLAM系統(tǒng)[4]采用激光方法使用擴展卡爾曼濾波框架成功構建完整的視覺SLAM系統(tǒng)，在此基礎上建立的MonoSLAM系統(tǒng)將三維重建方法應用到SLAM系統(tǒng)來構建點云地圖[5]。PTAM(parallel tracking and mapping)系統(tǒng)中開創(chuàng)性地提出了SLAM的多線程框架，將系統(tǒng)分為跟蹤和建圖線程，并在系統(tǒng)中廣泛采用Bundle Adjustment (BA)算法進行非線性優(yōu)化，正式取代了傳統(tǒng)的EKF(extended kalman filter)框架[6]。ORBSLAM2[7]沿用了PTAM框架，該系統(tǒng)適用于單目、雙目和RGB-D相機，并且可應用于室內和室外的大小規(guī)模場景，在保持高精度的基礎上表現(xiàn)出良好的魯棒性。整個系統(tǒng)采用ORB(oriented fast and rotated Brief)進行所有的關于特征的計算，引入回環(huán)檢測線程使得在PTAM基礎上增加對全局位姿的優(yōu)化，有效地減少了系統(tǒng)的累計誤差。

本文在ORB-SLAM2系統(tǒng)基礎上進行改進，針對ORB特征點提取耗時影響系統(tǒng)處理速度問題，引入關鍵點替代ORB特征點，設計了新的關鍵幀提取策略，并添加稠密點云地圖構建線程和八叉樹地圖構建線程，實現(xiàn)視覺SLAM系統(tǒng)的快速準確定位與地圖構建。

1 關鍵點信息獲取

1.1 Fast角點檢測

Fast特征可以定義為：若某個像素點在其周圍一定范圍內與其他像素點存在一定差異，則將中心像素稱為角點。為避免局部角點集中影響后續(xù)匹配結果，采用非極大值抑制方法對密集角點進行剔除，結果如圖1所示。

圖1 極大值抑制前后角點信息對比

1.2 角點的旋轉不變性與尺度不變性

角點雖然獲取較快的計算速度，但是如果相機位置發(fā)生旋轉變化，會造成匹配的大量丟失和誤匹配現(xiàn)象，所以使角點保持旋轉不變性和尺度不變性。

角點獲得旋轉不變性是基于灰度質心法實現(xiàn)的，灰度質心指的是以某一圖形塊灰度值作為權重的中心。具體實現(xiàn)步驟如下。

1)在以一個Fast特征P為圓心、半徑為3像素的圖像塊B中，定義圖像塊的矩為

2)通過矩可以找到圖形塊的灰度質心

3)連接圖像塊的幾何中心P與灰度質心C，得到一個方向向量，于是特征點的方向(單位：°)就可以表示為

本文引入圖像金字塔池化模型來獲取不同尺度圖像的角點信息，使得獲得的角點具有尺度不變性。將具有旋轉不變性和尺度不變性的角點作為關鍵點。

1.3 關鍵點與ORB特征點提取速度對比實驗

基于改進Fast角點作為關鍵點在提取速度上較ORB特征點明顯更快，節(jié)約特征提取時間，為定量分析兩者的差別，現(xiàn)對500張慕尼黑工業(yè)大學(Technische Universit?t München，TUM)數(shù)據(jù)集fr2_desk結果進行基于2種方法的特征提取實驗，每張圖片提取特征點數(shù)目為600，在實驗中對提取時間進行統(tǒng)計，統(tǒng)計結果如圖2所示。

圖2 特征提取時間對比

由圖2可以看出，本文對于關鍵點的提取速度明顯快于基于ORB特征點的提取方法，因為本文方法沒有提取特征點的描述子，所以耗時較短。

2 關鍵點的幀間匹配

2.1 基于關鍵點的幀間匹配

在LK(Lucas–Kanade)光流中將圖像看作時間的函數(shù)，那么在t時刻位于處的像素值可以表示為[8]。為使得計算準確，引入光流法的2個假設條件：亮度恒定和空間一致性。

亮度恒定：對于時刻t位于(x,y)處的像素點，假設其在時刻，該點新的像素點坐標值為，根據(jù)亮度恒定假設有

因為假設亮度不變，所以下一時刻的灰度值等于前一時刻灰度值，從而

式中dx/dt和dy/dt是像素在x和y軸方向運動速度，記為u，v。

通過式(1)計算像素運動u、v，式(1)是具有2個變量的一次方程，無法求2個變量解，所以引入額外約束即空間一致性假設：相鄰像素具有相似運動，即可得到像素在圖像間的運動速度u、v。因為引入最小二乘法求解像素運動即光流，所以可以把光流看作優(yōu)化問題：通過最小化灰度誤差來估計最優(yōu)的像素偏移。主要解決如下問題：

單層圖像可能由于相機快速運動引起圖像差異較大而在優(yōu)化時到達局部最小值，無法繼續(xù)優(yōu)化，使得最后優(yōu)化效果較差。對原始圖像進行一定比例縮放得到不同分辨率圖像，以原始圖像作為底層(共4層)，每向上一層進行0.6倍縮小得到圖像金字塔。單層LK光流和多層LK光流跟蹤結果如圖3所示。

圖3 光流跟蹤結果

參考幀與當前幀計算光流，結果在當前幀上用綠色點進行標注。如圖4所示，相機在參考幀和當前幀間進行明顯的右移，可以看出多層光流結果偏移方向更趨于一致，單層光流結果偏移方向表現(xiàn)散亂，所以多層光流結果表現(xiàn)優(yōu)于單層光流結果。

圖4 幀間直接匹配與誤匹配剔除后對比

2.2 關鍵點的誤匹配剔除

每幀圖像上獲取對應的關鍵點經過LK光流法進行基于關鍵點的幀間匹配過程會出現(xiàn)誤匹配。所以首先利用前文提取的關鍵點方向信息來進行初步的誤匹配結果剔除，根據(jù)參考幀和當前幀圖像的LK跟蹤結果對匹配關鍵點做差值，每30°表示一個區(qū)間，匹配點基本保持在第一二個區(qū)間內，所以本文保留初次匹配結果的第一二區(qū)間的關鍵點作為正確匹配關鍵點。

經過上述簡單誤匹配剔除后，匹配結果中仍存在誤匹配現(xiàn)象，繼續(xù)采用隨機抽樣一致算法計算變換矩陣，通過變換矩陣判斷關鍵點是內點(正確匹配點)還是外點(錯誤匹配點)。計算2幀圖片變化的變換矩陣H。

RANSAC算法的目標是找到一個H矩陣使得其能包含最多的數(shù)據(jù)點，即通過變換后2幀間的關鍵點盡可能多的進行匹配，變換公式可以寫為

式中：(x,y)和(x′,y′)為對應2幀的關鍵點像素坐標；s為尺度因子。

因為H矩陣可以通過令h33=1進行歸一化處理，然后計算滿足該變換矩陣的內點，代價函數(shù)為

具體步驟為：

1)從所有樣本數(shù)據(jù)集中隨機選取不共線的4對點計算H矩陣，記為模型M。

2)計算數(shù)據(jù)集中所有數(shù)據(jù)與模型M的投影誤差，若誤差小于閾值，加入內點集I。

3)如當前內點集I元素個數(shù)大于最優(yōu)內點集I_best,則更新I_best=I，同時更新迭代次數(shù)k。

4)如果迭代次數(shù)大于k，則退出;否則迭代次數(shù)加1，并重復上述步驟。

經過2次誤匹配剔除后，圖4(a)表示LK光流法直接匹配結果，圖4(b)表示2次誤匹配剔除后的匹配結果，可以看出匹配結果中誤匹配對明顯減少，說明本文方法能獲取較好的效果。

3 關鍵幀檢測

通過跟蹤線程確定的關鍵幀可以作為后續(xù)幀的參考幀，并且檢測到的關鍵幀會進入到后續(xù)的建圖和優(yōu)化線程，具體方法如下。

1)根據(jù)以上內容獲取當前幀的相機位姿，轉化為旋轉矩陣R和偏移量t，根據(jù)R和t來評價當前幀與參考關鍵幀的運動差異來獲取候選關鍵幀。

旋轉矩陣R和t分別為

無法直接比較不同幀圖像對應的旋轉矩陣大小，所以將旋轉矩陣轉化為歐拉角來描述旋轉，可以稱為通過“偏航?俯仰?滾轉(yaw-pitch-roll)”3個角度描述一個旋轉，求解3個旋轉角：

將旋轉矩陣和平移矩陣歸一化處理：

通過計算d的大小來度量參考幀與當前幀的運動幅值大小，αβ是相機運動時旋轉矩陣和平移矩陣對于d的貢獻權重。

2)在上述判斷的基礎上引入參考關鍵幀，將當前幀與參考關鍵幀的關鍵點匹配個數(shù)作為判斷指標。如果匹配點對個數(shù)小于當前幀提取關鍵點個數(shù)的0.75倍，那么將當前幀作為候選關鍵幀傳入下一線程處理。

4 實驗設計與結果分析

4.1 實驗平臺與誤差評價指標

視覺SLAM系統(tǒng)2個常用的評價指標是絕對軌跡誤差(absolute trajectory error,ATE)和相對位姿誤差(relative pose error,RPE)[9]。本文實驗基于PC端進行，系統(tǒng)為Ubuntu16，內存16 G，CPU i7。

4.2 實驗結果軌跡分析

首先采用TUM公開數(shù)據(jù)集進行相關分析實驗，因為是否存在回環(huán)對整個視覺SLAM系統(tǒng)的后端優(yōu)化有很大影響，所以采用數(shù)據(jù)集中的fr2_large_with_loop帶有回環(huán)的數(shù)據(jù)進行相關實驗，實驗結果的三維軌跡對比如圖5所示。其中誤差尺度在右側用不同顏色進行區(qū)分，其中參考軌跡(reference)為數(shù)據(jù)集實際軌跡?？梢钥闯鰣D5(a)與圖5(b)表示fr2_loop的軌跡誤差圖誤差基本一致，證明本文在大幅提高ORB-SLAM2系統(tǒng)的基礎上保持了良好魯棒性。

圖5 fr2_loop數(shù)據(jù)集軌跡對比

4.3 定位實驗結果分析

為了使對比實驗有更加廣泛的價值，在本文SLAM系統(tǒng)與ORB-SLAM2系統(tǒng)基礎上，采用另一個視覺SLAM成熟方法RGBD-SLAM2系統(tǒng)進行相關對比實驗。在TUM數(shù)據(jù)集進行相關對比實驗，下面只顯示fr1_room結果，其他對比實驗結果在表1中具體記錄。

表1 TUM數(shù)據(jù)集定位實驗RMSE誤差 m

對相對位姿誤差的值進行統(tǒng)計繪圖如圖6所示。從圖6可以看出3個SLAM系統(tǒng)在相對位姿誤差分布上，本文SLAM系統(tǒng)與ORB-SLAM2系統(tǒng)在更小誤差值上具有更多的分布數(shù)據(jù)，而誤差值越小表明該SLAM系統(tǒng)的相對位姿誤差越小，系統(tǒng)越穩(wěn)定，本文SLAM系統(tǒng)和ORB-SLAM2系統(tǒng)、RGBD-SLAM2系統(tǒng)相比較，具有更好的魯棒性。

圖6 RPE誤差對比

一般數(shù)據(jù)的最大值(MAX)、最小值(MIN)、算術均值(Arthmetic MEAN)、中值(MEDIAN)、均方根誤差(Root Mean Squard Error,RMSE)和標準差(Standard Deviation,STD)，這些數(shù)據(jù)對比如圖7所示。

圖7 絕對軌跡誤差對比

從圖7可以看出，在上述幾項誤差評價指標中，本文SLAM系統(tǒng)都保持幾乎所有誤差項的最低值。

TUM數(shù)據(jù)集的各項定位實驗數(shù)據(jù)如表1所示，所有數(shù)據(jù)集進行5次實驗，取誤差平均值進行計算。通過比較3個SLAM系統(tǒng)定位誤差的RMSE值可以看出，本文系統(tǒng)在其中幾項上表現(xiàn)好于ORB-SLAM2，其他幾項上與ORB-SLAM2基本保持一致。

4.4 地圖構建實驗

地圖構建是室內同步定位與建圖要解決的另一個重要問題，因為環(huán)境地圖的好壞直接影響導航結果與后續(xù)的處理過程，所以本文添加稠密點云地圖構建線程[10]，并為減少空間要求采用八叉樹地圖來存儲地圖，在滿足導航要求基礎上減少了地圖體積[11]。因為建圖效果類似，下面只列出fr3_long_office的建圖結果。

從圖8可以看出，稠密點云地圖對數(shù)據(jù)集中的桌面各種物體清晰度較高，可以清晰看出椅子、玩具等，而且根據(jù)本文方法將稠密點云地圖轉化為八叉樹地圖時，稠密點云地圖200 M左右大小，而八叉樹地圖26 M左右。對TUM數(shù)據(jù)集其他的數(shù)據(jù)進行相同的建圖實驗，統(tǒng)計其稠密點云地圖與八叉樹地圖大小如表2所示。

圖8 地圖構建結果

表2 TUM數(shù)據(jù)集稠密點云地圖與八叉樹地圖存儲空間占用對比 MB

結合圖8與表2可以看出，本文構建的八叉樹地圖在保留地圖細節(jié)基礎上大幅縮小了地圖體積，并且可以進一步應用于機器人導航，對于室內機器人的計算和存儲空間優(yōu)化具有重要應用價值。

5 結論

本文首先針對ORB特征點提取耗時過多而影響系統(tǒng)速度問題，應用LK光流法來處理非關鍵幀的跟蹤問題。通過基于旋轉矩陣和平移矩陣的歸一化處理及參考關鍵幀與當前幀的關鍵點匹配點閾值來判斷新的關鍵幀。實驗表明，本文方法在保持較高定位精度基礎上數(shù)據(jù)處理速度約為40 f/s，是ORB-SLAM2系統(tǒng)的1.4倍左右。此外本文針對ORB-SLAM2系統(tǒng)只有稀疏點云地圖的不足[12]，添加了稠密點云的構建與八叉樹地圖的構建環(huán)節(jié)。

在下一步的工作中，將研究如何構建語義地圖與利用語義地圖進行定位的優(yōu)化。