改進(jìn)3D SLAM算法在移動(dòng)機(jī)器人上的應(yīng)用

于志鵬，蔣 林

（武漢科技大學(xué)機(jī)械學(xué)院自動(dòng)化學(xué)院，湖北 武漢 430081）

1 引言

移動(dòng)機(jī)器人是最早出現(xiàn)的智能機(jī)器人之一，多個(gè)領(lǐng)域有廣泛的使用，這些任務(wù)都需要移動(dòng)機(jī)器人進(jìn)行實(shí)時(shí)的定位于建圖（SLAM）。一般移動(dòng)機(jī)器人需要通過多種傳感器設(shè)備，如激光雷達(dá)、相機(jī)、GPS、陀螺儀、高精度碼盤、紅外和超聲傳感器等，用于獲取自身以及周圍環(huán)境信息。雖然搭載這些傳感器設(shè)備可以提高系統(tǒng)的精確性星和魯棒性，但也會(huì)造成系統(tǒng)過去復(fù)雜，同時(shí)造價(jià)高昂，為大規(guī)模的推廣應(yīng)用設(shè)置了較高的技術(shù)門檻。

一種RGB-DSLAM算法在2010年由文獻(xiàn)[1]于2010年首次提出，方法應(yīng)用于移動(dòng)機(jī)器人的實(shí)時(shí)定位與建圖（SLAM）。在2012年，一種RGB-D SLAM方法由文獻(xiàn)[2]提出，該方法通過Octomap library創(chuàng)建環(huán)境的體素地圖并采用g2o[3]進(jìn)行圖優(yōu)化。傳統(tǒng)的三維建模的方法是通過激光掃描或者可探深度攝像頭來提供稠密的環(huán)境點(diǎn)云，或者將物體表面分割，建立匹配關(guān)系，分析構(gòu)造出光滑的表面模型[4]。新型的RGB-D傳感器，如Microsoft Kinect，可以直接提供密集的深度圖像和彩色圖像。然而，由于大量的內(nèi)存消耗，最初的3D SLAM的速度和效率都不是很好。

主要從四個(gè)方面對(duì)傳統(tǒng)RGB-D SLAM進(jìn)行改進(jìn)。（1）使用主成分分析法對(duì)SURF生成的特征點(diǎn)描述子進(jìn)行降維處理。（2）將圖像分割成若干個(gè)小區(qū)域，在每個(gè)區(qū)域內(nèi)提取改進(jìn)的SURF特征點(diǎn)并且限制每個(gè)區(qū)域內(nèi)的最大特征點(diǎn)數(shù)，對(duì)候選點(diǎn)的搜索區(qū)域進(jìn)行限制。（3）利用RATB-Map方案[5]，對(duì)全局地圖進(jìn)行三維重建，提高三維SLAM的實(shí)時(shí)性。

2 基于主成分分析的改進(jìn)特征點(diǎn)檢測(cè)

所提出的改進(jìn)SURF算法與傳統(tǒng)SURF算法在亞像素位置與主方向方面的描述相同，不同的是在描述子的生成，在提取到特征描述子之后，使用主成分分析法將特征點(diǎn)描述子進(jìn)行降維處理。雖然此方法比傳統(tǒng)的SURF特征檢測(cè)步驟多了一個(gè)PCA降維過程，但在后續(xù)的特征點(diǎn)匹配部分運(yùn)算速度更快。

基于主成分分析的SURF特征點(diǎn)檢測(cè)主要有5個(gè)步驟：（1）建立 Hession 矩陣；（2）構(gòu)建尺度空間；（3）特征點(diǎn)定位；（4）特征點(diǎn)主方向匹配；（5）使用傳統(tǒng)的SURF算法生成特征點(diǎn)描述子；（6）使用主成分分析法對(duì)特征點(diǎn)描述子降維。

圖像中某個(gè)像素點(diǎn)的Hession矩陣的定義為：

式中：Lxx（x，y，σ），Lxy（x，y，σ），Lxx（x，y，σ）—高斯二階偏導(dǎo)數(shù)與該點(diǎn)的卷積；σ—二階高斯濾波器的尺度因子。即：

式中：I（x，y）—圖像函數(shù)；σ—尺度因子且σ=1.2；g（σ）—二維高斯函數(shù)。

為了提高運(yùn)算速度，當(dāng)使用盒式濾波器時(shí)利用可以利用積分圖像加速運(yùn)算。

因?yàn)楹惺綖V波器是近似代替二階高斯濾波。所以通過引入權(quán)值來平衡準(zhǔn)確值與近似值之間的誤差，所以：

式中：Lxy，Lxx—高斯二階偏導(dǎo)數(shù)與該點(diǎn)的卷積；Dxx，Dxy—盒式濾波器與該點(diǎn)的卷積。

因此Hessian矩陣的判別式為：

式中：det H—Hessian矩陣的判別式；Dxx，Dxy—盒式濾波器與該點(diǎn)的卷積。

根據(jù)判別式求得的值來判斷該點(diǎn)是否是極值點(diǎn)。此步驟與SURF提取特征點(diǎn)的步驟相同。不同的是計(jì)算出相對(duì)于主方向的垂直與水平方向的harr小波特征，水平方向之和，水平方向絕對(duì)值之和，垂直方向之和，垂直方向絕對(duì)值之和構(gòu)成一個(gè)4維向量[6]。把這4個(gè)值作為每個(gè)子塊區(qū)域的特征向量，所以一共有4×4×4=64維向量作為SURF特征的描述子。

使用主成分分析法對(duì)特征點(diǎn)描述子降維的步驟為：

（1）將n個(gè)特征點(diǎn)的64維描述子組成64行n列的矩陣X，X=［x1，x2，…xn］；

（2）計(jì)算n個(gè)特征向量的平均值

（3）將X的每一行進(jìn)行零均值化，得到零均值向量di=xi-x，i=1，2，…n；

（5）計(jì)算出Q所對(duì)應(yīng)的64個(gè)特征值λi和特征向量ei；

（6）將特征向量按對(duì)應(yīng)特征值大小從大到小按行排列成矩陣，取前k行組成矩陣P；

（7）Y=PX即為降維到k維都的數(shù)據(jù)。

如果k等于18，那么矩陣P的大小為18×64，xi的大小為64×1，所以 P×xi為 18×1 的矩陣，即 yi的維度為 18，把 64 維的傳統(tǒng)SURF特征描述符降維到了18維[7]。

3 區(qū)域分塊匹配的特征點(diǎn)匹配

首先將圖像分割成若干個(gè)小區(qū)域，然后在每個(gè)區(qū)域內(nèi)分別提取改進(jìn)后的SURF特征點(diǎn)且限制每個(gè)區(qū)域內(nèi)的最大特征點(diǎn)數(shù)，例如 Kinect獲得的（640×480）像素的圖像可以以（80×80）像素為子區(qū)域大小分割為48個(gè)區(qū)域，在每個(gè)區(qū)域內(nèi)只保留最大25個(gè)強(qiáng)度（response）最高的特征點(diǎn)。這種方法能夠在圖像上提取到分布更加均勻的特征。目前移動(dòng)機(jī)器人實(shí)時(shí)定位與建圖的過程比較緩慢，平移和旋轉(zhuǎn)時(shí)產(chǎn)生的位移和角度都比較小，所以Kinect采集到的前后兩幀圖像的偏移量都較小，在匹配階段也對(duì)候選點(diǎn)的搜索區(qū)域進(jìn)行限制。假設(shè)目標(biāo)幀及源幀圖像分別為It、Is、pt為It上一特征點(diǎn)且其圖像坐標(biāo)為It（x，y），則定義pt在上Is的搜索區(qū)域?yàn)椋阂訧s（x，y）為中心，nσ為半徑的圓（σ為圖像分割尺度，n為大于0的整數(shù)，這里取n=3）。這種限制搜索范圍的策略不但節(jié)省了匹配所需的時(shí)間，同時(shí)也避免了一些位置差異較大的誤匹配。此外，采用FLANN（Fast Approximate Nearest Neighbor Search）算法進(jìn)行雙向匹配[8]。

4 高效的3D SLAM

RTAB-MAP是一個(gè)與時(shí)間無關(guān)的基于外觀的定位與建圖建圖方案。當(dāng)一個(gè)閉環(huán)被采用時(shí)，一個(gè)新的約束被添加到地圖的圖優(yōu)化中，圖優(yōu)化器就會(huì)將地圖中的錯(cuò)誤最小化。內(nèi)存管理算法用于限制閉環(huán)檢測(cè)和圖形優(yōu)化的位置和數(shù)量。在該方案中，地圖的結(jié)構(gòu)是一個(gè)具有節(jié)點(diǎn)和鏈接的圖。地圖中的每個(gè)位姿由節(jié)點(diǎn)保存。在節(jié)點(diǎn)中包含了用于閉環(huán)檢測(cè)的RGB圖像與深度圖像。記憶由STM（短期記憶），WM（工作記憶）和LTM（長(zhǎng)期記憶），如圖1所示。

圖1 RTAB-Map的內(nèi)存管理模型Fig.1 RTAB’s Memory Management Model

當(dāng)獲得新數(shù)據(jù)時(shí)，STM是添加到圖中的新節(jié)點(diǎn)的入口點(diǎn)，它是一個(gè)固定大小的S。STM中的節(jié)點(diǎn)不考慮環(huán)路閉合檢測(cè)。當(dāng)STM大小到達(dá)S節(jié)點(diǎn)時(shí)，儲(chǔ)存時(shí)間最長(zhǎng)的節(jié)點(diǎn)被移動(dòng)到WM。WM的大小間接確定為一個(gè)固定的時(shí)間限制T。當(dāng)新的數(shù)據(jù)需要處理的時(shí)間到達(dá)T時(shí)，圖中的某些節(jié)點(diǎn)從WM轉(zhuǎn)移到LTM，從而是WM保持幾乎恒定。

5 SLAM后端

基于g2o框架優(yōu)化姿態(tài)圖，創(chuàng)建全局一致的軌跡。g2o[3]框架是一個(gè)易于擴(kuò)展的圖形優(yōu)化器，可以應(yīng)用于廣泛的問題，包括多種SLAM和光束平差法的變體。

（5）中描述了非線性誤差函數(shù)的最小化。

通過后端的非線性誤差函數(shù)e（xi，xjzij）計(jì)算成對(duì)變換，使得xi和xj的位姿滿足約束zij。

當(dāng)重新訪問到地圖的已知部分時(shí)，全局優(yōu)化在閉環(huán)的情況下特別有利。因?yàn)閳D優(yōu)化中的閉環(huán)可以減少累計(jì)誤差。

6 實(shí)驗(yàn)結(jié)果及分析

實(shí)驗(yàn)使用的移動(dòng)機(jī)器人平臺(tái)為旅行家II號(hào)機(jī)器人，RGB-D相機(jī)為微軟Kinect相機(jī)。系統(tǒng)環(huán)境為：英特爾I7 4590 CPU，DDR3 8GRAM的Ubuntu14.04平臺(tái)上。本實(shí)驗(yàn)用到的旅行家II號(hào)機(jī)器人與Kinect，如圖2所示。提出的改進(jìn)SURF特征點(diǎn)檢測(cè)與匹配算法與傳統(tǒng)SURF算法所提取到的圖像的實(shí)驗(yàn)結(jié)果，如圖3、圖4所示。實(shí)驗(yàn)比較可知，傳統(tǒng)SURF檢測(cè)到的特征點(diǎn)數(shù)量為75個(gè)，檢測(cè)用時(shí)0.310s，匹配用時(shí)0.195s，準(zhǔn)確率為92.7%，總共用時(shí)0.505s。改進(jìn)SURF檢測(cè)28個(gè)特征點(diǎn)，消耗時(shí)間0.375s，匹配用時(shí)為0.027s，準(zhǔn)確率為：92.8%，總的消耗時(shí)間為0.402s，相比于傳統(tǒng)SURF，改進(jìn)SUFR算法能夠降低特征點(diǎn)檢測(cè)與匹配的總時(shí)間，并且匹配的準(zhǔn)確率與傳統(tǒng)的SURF的準(zhǔn)確率相等。

圖2 本實(shí)驗(yàn)用到的旅行家II號(hào)機(jī)器人與KinectFig.2 The Traveler II Robot and Kinect Used in This Experiment

圖3 傳統(tǒng)SURF特征點(diǎn)檢測(cè)與匹配結(jié)果Fig.3 Traditional SURF Feature Point Detection and Matching Results

圖4 改進(jìn)SURF特征點(diǎn)檢測(cè)與匹配結(jié)果Fig.4 Improved SURF Feature Point Detection and Matching Results is Proposed in This Paper

為了對(duì)所提出的算法進(jìn)行測(cè)試，對(duì)從Kinect提取到的30組圖像進(jìn)行特征點(diǎn)檢測(cè)與匹配，實(shí)驗(yàn)結(jié)果，如圖5所示。根據(jù)圖5可知進(jìn)行區(qū)域分塊后，雖然匹配的對(duì)數(shù)減少，但是匹配正確率與分塊前的準(zhǔn)確率相等，并且據(jù)實(shí)驗(yàn)統(tǒng)計(jì)，特征點(diǎn)匹配階段所消耗的時(shí)間減少了約20%。

圖5 區(qū)域分塊搜索前后的比較Fig.5 Comparison of Region Block Search Before and After

特征點(diǎn)檢測(cè)與匹配的平均時(shí)間進(jìn)行測(cè)試的結(jié)果，如表1所示。由表1可知，因?yàn)橐肓酥鞒煞址治龇椒ǎ≒CA），使得在生成特征點(diǎn)描述子的時(shí)候耗時(shí)比傳統(tǒng)SURF要長(zhǎng)，但是因?yàn)樵谔卣鼽c(diǎn)匹配時(shí)的特征描述子的維度降低以及特征點(diǎn)匹配階段采用了區(qū)域塊分割，所以在特征匹配階段用時(shí)更少，總體的耗時(shí)更少。雖然使用ORB的特征提取比使用SURF與SIFT描述符的特征提取快得多。但是在誤差上比其他兩種更高；利用SURF和SIFT特征點(diǎn)實(shí)現(xiàn)的RBG-D SLAM誤差相當(dāng)，均優(yōu)于ORB方法，但是SURF方法相比于SIFT方法更快些。所以綜合考慮，這里的方法更優(yōu)。

表1 特征檢測(cè)與提取的時(shí)間分析Tab.1 Time Analysis of Feature Detection and Extraction

為了驗(yàn)證測(cè)試所提出的RGB-D SLAM的RTAB-MAP方法所建立的地圖是否具有全局一致性，采用NYUv2中的數(shù)據(jù)集作為測(cè)試數(shù)據(jù)。傳統(tǒng)RGB-D SLAM生成的點(diǎn)云圖，如圖6所示。采用這里方法所得出的點(diǎn)云地圖，如圖7所示。相比而言這里所提出的RGB-D SLAM方法生成的點(diǎn)云信息更加清晰且全局一致性更好，冗余地圖結(jié)構(gòu)更少。因?yàn)樵摂?shù)據(jù)集中有人物的移動(dòng)，會(huì)對(duì)生成的點(diǎn)云地圖產(chǎn)生較大影響，采用這里的優(yōu)化算法可以使所生成的點(diǎn)云圖中的移動(dòng)物體具有清晰的輪廓，如圖8所示，此結(jié)果可以為后續(xù)的動(dòng)態(tài)物體識(shí)別提供先決條件。為驗(yàn)證所提出的3D SLAM系統(tǒng)框架的實(shí)時(shí)性與魯棒性，采用慕尼黑工業(yè)大學(xué)計(jì)算機(jī)視覺團(tuán)隊(duì)的RGB-D SLAM的測(cè)試數(shù)據(jù)集進(jìn)行實(shí)驗(yàn)。這里的方法與傳統(tǒng)方法的對(duì)比結(jié)果，如表2所示。從表2中可以看出這里的RGB-D SLAM方法在ATE誤差與RPE誤差兩項(xiàng)數(shù)據(jù)均小于傳統(tǒng)RGB-D SLAM方法。結(jié)果表明，這里的RBG-D SLAM相比于傳統(tǒng)得RBG-D SLAM方法在實(shí)時(shí)性與魯棒性以及地圖的全局一致性具有更優(yōu)越的表現(xiàn)。

圖6 傳統(tǒng)RGB-D SLAM生成的點(diǎn)云地圖Fig.6 Traditional RGB-D SLAM Generates Point Cloud Maps

圖7 所提出的RGB-D SLAM方法生成的點(diǎn)云地圖Fig.7 The Point Cloud Map Generated by the RGB-D SLAM Method Proposed

圖8 優(yōu)化算法對(duì)動(dòng)態(tài)物體影像圖Fig.8 Optimization Algorithm for Dynamic Object Image Proposed in This Paper

表2 對(duì)比傳統(tǒng)RGB-D SLAM方法在不同數(shù)據(jù)集上的ATE與RPE誤差Tab.2 Comparison of ATE and RPE Errors for Traditional RGB-D SLAM Methods on Different Data Sets

7 結(jié)語(yǔ)

提出了一種應(yīng)用于移動(dòng)機(jī)器人的高效的RGB-D SLAM算法，提高了機(jī)器人在實(shí)時(shí)定位與建圖過程的實(shí)時(shí)性與魯棒性。利用主成分分析法（PCA）對(duì)特征描述子進(jìn)行降維處理，同時(shí)將圖像區(qū)域分塊匹配，限制所劃分的區(qū)域內(nèi)的特征點(diǎn)數(shù)目。使用RTABMAP方案來實(shí)現(xiàn)RGB-D SLAM的建圖效率，在后端，g2o用于機(jī)器人的軌跡和全局地圖優(yōu)化。實(shí)驗(yàn)結(jié)果表明，這里所提出的改進(jìn)SURF算法可以加快RGB-D SLAM的特征提取速度，相比于傳統(tǒng)RGB-D SLAM的實(shí)時(shí)性與魯棒性以及全局一致性更高。