基于前后端圖優(yōu)化的RGB-D三維SLAM

,,,

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

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

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

1 SLAM系統(tǒng)前端

1.1 特征選擇

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

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

dipi=K(RPi+t)

(1)

根據(jù)式(1)計(jì)算得到的理論二維像素坐標(biāo)pi與真實(shí)二維像素坐標(biāo)，可得誤差函數(shù)ei為

(2)

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

(3)

式中：pi為二維像素坐標(biāo)；K為相機(jī)內(nèi)參；R為旋轉(zhuǎn)矩陣；t為平移向量；di為深度信息。

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

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

2) 然后將當(dāng)前移動(dòng)機(jī)器人位姿設(shè)為圖優(yōu)化中的頂點(diǎn)。

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

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

1.3 提取關(guān)鍵幀

相鄰幀之間的距離往往很近，沒有明顯的差別，將每一幀都添加到地圖中，將導(dǎo)致地圖的頻繁更新，浪費(fèi)計(jì)算時(shí)間與空間、降低了效率。為此，采用提取關(guān)鍵幀的方法，當(dāng)機(jī)器人運(yùn)動(dòng)超過一定距離時(shí)，就增加一個(gè)“關(guān)鍵幀”，最后將得到的關(guān)鍵幀拼接起來就得到了三維點(diǎn)云地圖。

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

2 SLAM系統(tǒng)后端

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

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

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

2.2 Bag of Words字典建立

1) 從訓(xùn)練圖像中離線抽取特征。

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

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

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

圖1 字典樹Fig.1 Vocabulary tree

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

(4)

式中：N為訓(xùn)練圖像的數(shù)量；ni為word在這些訓(xùn)練圖像中出現(xiàn)的次數(shù)。

2.3 在線更新字典樹

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

(5)

式中：niIt為word在圖像中出現(xiàn)的次數(shù)；nIt為圖像中描述子的總數(shù)。在樹構(gòu)建的過程中每個(gè)葉子節(jié)點(diǎn)存儲(chǔ)了inverse index，存儲(chǔ)了到達(dá)葉子節(jié)點(diǎn)的圖像It的ID和word在圖像It描述vector中第i維的值，即

(6)

對于一幅圖像所有的描述子，進(jìn)行以上操作，可以得到每個(gè)word的值，將這些值構(gòu)成圖像的描述向量vt。

2.4 相似度計(jì)算

將新得到的關(guān)鍵幀與具有一定數(shù)量間隔的所有關(guān)鍵幀逐一徑向相似度計(jì)算，兩幅圖相似度計(jì)算公式為

(7)

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

2.5 圖優(yōu)化的數(shù)學(xué)建模

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

(8)

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

xi=Tijxj

(9)

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

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

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

由于實(shí)際上兩個(gè)估計(jì)值之間存在誤差eij，所以存在

eij=(xi-Tijxj)

(10)

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

(11)

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

(12)

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

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

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

(13)

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

(14)

3) 計(jì)算ρ，得

(15)

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

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

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

(16)

增量方程為

(H+λDTD)Δx=g

(17)

2.6 基于g2o的位姿求解

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

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

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

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

2) 提取ORB特征并進(jìn)行特征匹配。

3) EPnP求解初始位姿x。

4) ifEkey

5) 將當(dāng)前幀作為關(guān)鍵幀。

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

7) end if。

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

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

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

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

12) end if。

根據(jù)最優(yōu)位姿計(jì)算得到三維地圖及運(yùn)動(dòng)軌跡。

3 實(shí)驗(yàn)結(jié)果與分析

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

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

基于solvePnPRansac方法求解floor數(shù)據(jù)集得到的實(shí)驗(yàn)結(jié)果如圖5所示。

圖5 solvePnPRansac解floor數(shù)據(jù)集軌跡Fig.5 Trajectories of floor data sets based on solvePnPRansac

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

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

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

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

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

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

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

表1 實(shí)驗(yàn)數(shù)據(jù)集信息Table 1 Experimental data set information

表2 實(shí)驗(yàn)結(jié)果分析Table 2 Experimental result analysis

4 結(jié) 論

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