,,,

(1.浙江工業(yè)大學 信息工程學院，浙江 杭州 310023;2.南開大學 機器人與信息自動化研究所，天津 300071)

針對處在復雜環(huán)境中的機器人，利用自身所攜帶的視覺傳感器獲取機器人所在環(huán)境的三維空間模型和機器人的運動軌跡，這是視覺SLAM(Simultaneous localization and mapping,即時定位與地圖構建)所要完成的工作，也是實現移動機器人任務規(guī)劃、導航和控制等自主運動的前提之一[1]。近幾年，流行的深度攝像機不僅可以獲得環(huán)境的二維圖像信息，還可以準確得到對應環(huán)境的深度信息。因此，利用深度攝像機來做視覺SLAM成為研究的熱點。現行的RGB-D SLAM算法主要由前端和后端兩部分組成[2]。前端部分利用得到的彩色圖和深度圖計算提取各幀間的空間位置關系，而后端部分對所得到的這些移動機器人位姿進行優(yōu)化，來減少其中存在的非線性誤差。Henry等[3]提出將連續(xù)的彩色圖間的匹配作為ICP(Iterative closest point迭代最近點)的初始化，通過位置識別提高了回環(huán)檢測效率，最后采用稀疏捆優(yōu)化(Sparse bundle adjustment)對移動機器人位姿進行優(yōu)化。其采用的FAST角點檢測方法代替原來的SIFT算法加快了算法的運算速度，但FAST本身缺乏尺度不變性以及旋轉不變性，因此魯棒性較差。Dryanovski等[4]采用Shi-Tomasi角點檢測算法得到RGB圖像的特征點,運用ICP算法得到當前幀與環(huán)境模型的旋轉平移矩陣，然后采用卡爾曼濾波方法對環(huán)境特征模型進行更新。由于只采用了傳統(tǒng)的濾波器方法，因此存在一定的累積誤差，精度相對較低。Endres等[5]基于Kinect實現了三維地圖的構建，由于采用的SIFT算法很耗時，導致實時性不強；在后端采用了比較簡單的閉環(huán)檢測方法,容易遺漏可能存在的有效閉環(huán)。Mur-Artal等[6]提出了一種基于ORB(Oriented brief)的單目視覺SLAM系統(tǒng)，取得了較好的效果。

針對RGB-D SLAM中所存在的上述問題，提出一種基于前后端圖優(yōu)化的RGB-D SLAM算法。主要工作包括將圖優(yōu)化[7]方案應用于SLAM算法的前端和后端[8]，提出一種基于圖優(yōu)化的Bundle Adjustment方法來獲取移動機器人相對位姿[9]，既提高了準確性，也能有效提高計算效率。在后端算法中利用基于詞袋[10]的回環(huán)檢測算法檢測得到局部回環(huán)和全局大回環(huán)[11]，利用圖優(yōu)化的方法進行優(yōu)化來減小累積誤差，得到移動機器人位姿的全局最優(yōu)解。從而得到移動機器人精確的運動軌跡，并構建出準確的三維空間模型[12]。

1 SLAM系統(tǒng)前端

1.1 特征選擇

綜合考慮計算效率和特征匹配準確度，采用ORB[13]算法對特征點進行檢測與描述。ORB具有方向的FAST興趣點檢測和具有旋轉不變的BRIEF興趣點描述子兩部分特征提取。

1.2 基于圖優(yōu)化的PnP問題求解

dipi=K(RPi+t)

(1)

根據式(1)計算得到的理論二維像素坐標pi與真實二維像素坐標，可得誤差函數ei為

(2)

式(2)的最小化誤差T*為

(3)

式中：pi為二維像素坐標；K為相機內參；R為旋轉矩陣；t為平移向量；di為深度信息。

采用深度傳感器，可以根據深度信息得到匹配點的三維位置。因此，在這里要處理的是已知三維到二維匹配點，求解移動機器人位姿的問題，具體步驟如下：

1) 首先采用EPnP[14]的求解方式求解得到移動機器人位姿的初始值。

2) 然后將當前移動機器人位姿設為圖優(yōu)化中的頂點。

3) 重投影誤差作為圖優(yōu)化中的誤差項。

4) 利用g2o求解得到移動機器人位姿的最優(yōu)解。

1.3 提取關鍵幀

相鄰幀之間的距離往往很近，沒有明顯的差別，將每一幀都添加到地圖中，將導致地圖的頻繁更新，浪費計算時間與空間、降低了效率。為此，采用提取關鍵幀的方法，當機器人運動超過一定距離時，就增加一個“關鍵幀”，最后將得到的關鍵幀拼接起來就得到了三維點云地圖。

提取關鍵幀的具體方法：對于新得到的一幀I，采用1.2節(jié)所描述的方法計算I與上一關鍵幀的相對運動，并判定該運動的大小，若E>Eerror(Eerror為設定的最大運動閾值)，說明運動太大，估計是計算錯誤，將該幀舍棄。若E

2 SLAM系統(tǒng)后端

2.1 基于DBoW2算法的回環(huán)檢測簡述

在進行全局回環(huán)檢測前，要先進行局部回環(huán)檢測，將新加進來的與相鄰的前幾個關鍵幀進行匹配，滿足相似度標準便可建立局部的小回環(huán)，通過這個約束關系來減少當下產生的累計誤差。

DBow2(Bags of binary words for fast place recognition in image sequence)是一種高效的回環(huán)檢測算法，該算法使用Fast算法檢測特征點，使用ORB描述子作為特征描述子。

2.2 Bag of Words字典建立

1) 從訓練圖像中離線抽取特征。

2) 將抽取的特征用k-means++算法聚類，將描述子空間劃分成k類。

3) 將劃分的每個子空間，繼續(xù)利用k-means++算法聚類做聚類。

4) 按照上述循環(huán)，將描述子建立樹形結構，如圖1所示。

圖1 字典樹Fig.1 Vocabulary tree

字典樹在建立過程中，每個葉子也就是每個 word 記錄了該 word 在所有的訓練圖像中出現的頻率，出現的頻率越高，表示這個 word 的區(qū)分度越小，因此權重越低。權重的計算公式為

(4)

式中：N為訓練圖像的數量；ni為word在這些訓練圖像中出現的次數。

2.3 在線更新字典樹

當需要在字典樹中插入一幅新圖像It，將在圖像中提取的描述符按照Hamming距離從字典樹的根部開始逐級向下到達葉子節(jié)點，計算每個葉子節(jié)點即每個word在圖像It中出現的頻率，計算公式為

(5)

式中：niIt為word在圖像中出現的次數；nIt為圖像中描述子的總數。在樹構建的過程中每個葉子節(jié)點存儲了inverse index，存儲了到達葉子節(jié)點的圖像It的ID和word在圖像It描述vector中第i維的值，即

(6)

對于一幅圖像所有的描述子，進行以上操作，可以得到每個word的值，將這些值構成圖像的描述向量vt。

2.4 相似度計算

將新得到的關鍵幀與具有一定數量間隔的所有關鍵幀逐一徑向相似度計算，兩幅圖相似度計算公式為

(7)

將相似度得分超過一定閾值的先前關鍵幀都加入到候選隊列中。再將候選隊列中的每一幀與當前關鍵幀進行特征匹配，當匹配得到的內點數超過一定閾值時則說明成功檢測到閉環(huán)。

2.5 圖優(yōu)化的數學建模

采用位姿約束進行SLAM的優(yōu)化問題建模而不用特征約束，這樣做既直觀而且減少了很多計算量，增強了算法的實時性。其數學建模為

(8)

式中：x為機器人位姿(包含三維位置與三個角度姿態(tài)，采用姿態(tài)矩陣來描述)；R為旋轉矩陣(正交矩陣)；xt為平移向量，即

xi=Tijxj

(9)

式中Tij為第i幀與第j幀間經過的運動(是通過這兩幀的圖像匹配所得到的位姿約束)。

將這個優(yōu)化問題表示成如圖2所示的優(yōu)化圖，移動機器人位姿xi作為圖2中的節(jié)點，幀間約束Tij作為圖2中的邊。

圖2 優(yōu)化圖Fig.2 Optimal graph

由于實際上兩個估計值之間存在誤差eij，所以存在

eij=(xi-Tijxj)

(10)

由此可推得每條邊都存在誤差，對這些誤差定義一個二范數，并將這些誤差相加，就得到了要求解的優(yōu)化問題的目標函數，即

(11)

對這個優(yōu)化問題進行求解，其實這就是一個非線性最小二乘問題。設f(x)=eij，將目標函數進行泰勒級數展開，可得到

(12)

于是便可以采用Levenberg-Marquardt算法來求解這個非線性問題，其中該算法可描述為

1) 給定初始值x0, 以級初始化半徑u。

2) 對于第k次迭代，求解得

(13)

s.t. ‖DΔxk‖2≤uρ

(14)

3) 計算ρ，得

(15)

6) 如果ρ大于某閾值，認為近似可行，令xk+1=xk+Δxk。

7) 判斷算法是否收斂，如果不收斂則返回2)，否則結束。

信賴域內的優(yōu)化，利用Lagrange乘子轉化為無約束，即

(16)

增量方程為

(H+λDTD)Δx=g

(17)

2.6 基于g2o的位姿求解

將前端算法得到的移動機器人位姿作為需要求解的優(yōu)化變量，將相鄰關鍵幀間的運動約束以及檢測到回環(huán)的兩幀之間的約束作為邊加入到圖中。由此，便得到了g2o所需的節(jié)點(優(yōu)化變量)和邊(誤差項)，g2o優(yōu)化的具體流程如圖3所示。

圖3 g2o流程圖Fig.3 g2o flow diagram

這樣便可優(yōu)化求解得到想要的優(yōu)化變量即移動機器人的位姿?；谇昂蠖藞D優(yōu)化算法可描述為

1) 初始化：讀取彩色圖與深度圖像幀。

2) 提取ORB特征并進行特征匹配。

3) EPnP求解初始位姿x。

4) ifEkey

5) 將當前幀作為關鍵幀。

6) 利用g2o及式(1～3)優(yōu)化局部位姿。

7) end if。

8) 由式(4～7)計算得到兩幀圖像的相似度。

9) ifs(v1,v2)>sw(sw為閾值)。

10) 說明成功檢測到閉環(huán)。

11) 利用高g2o及式(10～17)優(yōu)化全局位姿。

12) end if。

根據最優(yōu)位姿計算得到三維地圖及運動軌跡。

3 實驗結果與分析

設計了兩組實驗，其中第一組采用的是Computer vision group提供的數據集Freibury1_floor(簡稱“floor”)，第二組采用的是自己構建的數據集(簡稱“自建”)，拍攝的環(huán)境是作者的實驗室環(huán)境,本實驗運行在配置為Intel(R) Core(TM) i3 CPU 4 GB RAM 的 Ubuntu 14.04平臺上。實驗中分別采用前后端圖優(yōu)化的方法，以及在前端只采用opencv中solvePnPRansac函數求解移動機器人位姿的方法對這兩組數據進行測試(簡稱“solvePnPRansac方法”)，基于前后端圖優(yōu)化方法求解floor數據集得到的運動軌跡如圖4所示。

圖4 圖優(yōu)化解floor數據集軌跡Fig.4 Trajectories of floor data sets based on graph optimization

基于solvePnPRansac方法求解floor數據集得到的實驗結果如圖5所示。

圖5 solvePnPRansac解floor數據集軌跡Fig.5 Trajectories of floor data sets based on solvePnPRansac

分別采用前后端圖優(yōu)化方法和solvePnPRansac方法求解自建數據集得到的軌跡圖如圖6，7所示。由圖4與圖5，圖6與圖7對比可得：采用基于前后端圖優(yōu)化的方法相比于在前端只采用solvePnPRansac方法所得到的軌跡更為準確，而且精度有較大的提高。

圖6 圖優(yōu)化解自建數據集軌跡Fig.6 Trajectories of self built data sets based on graph optimization

圖7 solvePnPRansac解自建數據集軌跡Fig.7 Trajectories of self built data sets based on solvePnPRansac

采用前后端圖優(yōu)化方法求解floor數據集和自建數據集得到的三維點云地圖如圖8，9所示。通過這2幅三維點云地圖，可以看到以前后端圖優(yōu)化方法為基礎拼接得到的三維地圖模型也十分精確，進一步表明了前后端圖優(yōu)化方法對于構建三維地圖的良好適用性。

圖8 圖優(yōu)化解floor數據集地圖Fig.8 Map of floor data sets based on graph optimization

圖9 圖優(yōu)化解自建數據集地圖Fig.9 Map of self built data sets based on graph optimization

實驗采用的數據集信息如表1所示，表2清楚地展現出基于前后端圖優(yōu)化的方法相比與solvePnPRansac方法在運行速度幾乎相等的情況下，在精度上有明顯的優(yōu)勢，且擁有較好的實時性。

表1 實驗數據集信息Table 1 Experimental data set information

表2 實驗結果分析Table 2 Experimental result analysis

4 結 論

在圖優(yōu)化方法的基礎上，提出了在視覺SLAM前后端都使用圖優(yōu)化方法來優(yōu)化移動機器人位姿的方案。該方法利用深度相機作為傳感器，通過ORB算子在較短時間內完成RGB圖像的特征點提取與匹配，運用圖優(yōu)化的方法計算得到相機的初步位姿，采用DBoW2算法進行回環(huán)檢測，基于圖優(yōu)化的方法進行后端優(yōu)化，并利用優(yōu)化求解得到的移動機器人位姿構建出移動機器人運動軌跡以及三維場景地圖，通過基于floor數據集和自建數據集的兩組實驗驗證了本算法的準確性和高效性。