基于卷積神經(jīng)網(wǎng)絡(luò)優(yōu)化回環(huán)檢測的視覺SLAM 算法

郭 烈 ，葛平淑 ，王 肖 ，王東興

（1.大連理工大學(xué)汽車工程學(xué)院，遼寧 大連 116024；2.大連民族大學(xué)機(jī)電工程學(xué)院，遼寧 大連 116600；3.大連理工大學(xué)控制科學(xué)與工程學(xué)院，遼寧 大連 116024）

即時定位與建圖（simultaneous localization and mapping，SLAM）是智能機(jī)器人和無人駕駛汽車實現(xiàn)自主導(dǎo)航與運動控制的關(guān)鍵技術(shù)[1].室外環(huán)境下，機(jī)器人可依靠GPS 和高精地圖實現(xiàn)自身定位與導(dǎo)航，但是在室內(nèi)環(huán)境下GPS 信號較弱導(dǎo)致定位偏差較大[2]，因此，有必要研究SLAM 技術(shù).早期SLAM技術(shù)主要依賴于激光傳感器，可獲得比較精確的距離信息，但其成本較高導(dǎo)致推廣應(yīng)用困難.計算機(jī)視覺的發(fā)展促使視覺SLAM 技術(shù)得到廣泛關(guān)注，特別是深度相機(jī)的出現(xiàn)，使SLAM 技術(shù)得到進(jìn)一步發(fā)展.深度相機(jī)可提供像素點的深度信息，解決尺度問題同時，也可減少大量的空間點的深度計算，使得視覺SLAM 的實時性得到較大程度的改善[3].

2007 年，Davison 等[4]提出了MonoSLAM 算法，是第一個可以實時運行的單目視覺SLAM 算法.MonoSLAM 以擴(kuò)展卡爾曼濾波為后端，追蹤前端非常稀疏的特征點，以相機(jī)的當(dāng)前狀態(tài)和所有路標(biāo)點為狀態(tài)量，更新其均值和協(xié)方差.同年，Klein 等[5]提出了PTAM (parallel tracking and mapping)算法.PTAM 提出前端和后端的概念，即局部地圖建立和對于地圖的優(yōu)化，在之后的SLAM 算法中得到廣泛應(yīng)用.與傳統(tǒng)的濾波方法不同，PTAM 還是第一個使用非線性優(yōu)化的方法，在以前人們一直認(rèn)為非線性優(yōu)化方法無法處理數(shù)據(jù)量龐大的視覺SLAM 問題，但是后來研究發(fā)現(xiàn)后端非線性優(yōu)化具有很大的稀疏性，對于其重投影誤差函數(shù)的二階偏導(dǎo)矩陣是可以比較快速求出的.因此，非線性優(yōu)化方法開始在SLAM算法中占據(jù)主導(dǎo)地位.

ORB-SLAM (oriented fast and rotated brief SLAM)是現(xiàn)代SLAM 算法典型代表[6]，該算法圍繞ORB 算子進(jìn)行計算，包括視覺里程計與回環(huán)檢測的ORB 字典，體現(xiàn)ORB 特征是現(xiàn)階段計算平臺的一種優(yōu)秀的效率與精度之間的折中方式.ORB 比SIFT(scale-invariant feature transform)或SURF (speeded up robust features)省時，可實時計算，相比Harris 等簡單角點特征，又具有良好的旋轉(zhuǎn)和縮放不變性，并且ORB提供描述子使大范圍運動時能夠進(jìn)行回環(huán)檢測和重定位[7].

在SLAM 建圖的過程中，由于各種噪聲的影響將不可避免地導(dǎo)致誤差的出現(xiàn)，隨著機(jī)器人的運動累積誤差逐漸增大，建立的地圖與真實場景的差距也將逐漸增大.為了減小累積誤差的影響，一些SLAM算法引入了回環(huán)檢測機(jī)制，即檢測機(jī)器人是否運動到之前經(jīng)過的地方，若有回環(huán)出現(xiàn)便對估計的運動軌跡增加約束并進(jìn)行一次全局的軌跡優(yōu)化，這樣便可以消除累積的運動誤差，目前大多數(shù)視覺SLAM算法都采用詞袋法來進(jìn)行回環(huán)檢測.由于利用圖像特征點信息來進(jìn)行回環(huán)檢測的詞袋法的計算量比較大，所以不太適合用于輕量級的計算設(shè)備上，而且基于特征信息進(jìn)行回環(huán)檢測的準(zhǔn)確率也比較低[8].

針對上述問題，本文通過Kinect 深度相機(jī)獲取環(huán)境信息，提出一種利用訓(xùn)練好的卷積神經(jīng)網(wǎng)絡(luò)（CNN）提取圖像特征并進(jìn)行回環(huán)檢測的視覺SLAM算法，使得回環(huán)檢測的準(zhǔn)確率與速度都能得到改善.

1 基本原理與實現(xiàn)方法

算法流程如下，對于Kinect 相機(jī)獲得的彩色圖像進(jìn)行特征提取并與前一幀進(jìn)行特征匹配，根據(jù)正確匹配的特征信息結(jié)合深度圖像的距離信息進(jìn)行PnP（perspective-n-point）求解從而得到兩幀圖像間的位姿變換.通過設(shè)定合理的運動范圍來篩選出關(guān)鍵幀，然后將關(guān)鍵幀以及相鄰關(guān)鍵幀之間的位姿變換信息進(jìn)行儲存.將當(dāng)前關(guān)鍵幀與之前所有關(guān)鍵幀進(jìn)行回環(huán)檢測，若出現(xiàn)回環(huán)則將兩個關(guān)鍵幀進(jìn)行特征提取、特征匹配和PnP 求解，然后將所有的位姿信息利用圖優(yōu)化工具進(jìn)行全局優(yōu)化并根據(jù)優(yōu)化后的信息拼接點云地圖.

1.1 特征提取

視覺里程計建立的主要問題是如何通過圖像來計算相機(jī)的位姿變換，因此，希望能在圖像中找到一些比較特殊的點.傳統(tǒng)的視覺SLAM 算法通常采用SIFT 或SURF 特征，這些特征的不變性都很好，但是其計算都非常耗時，不能滿足SLAM 的實時性需求.2011 年Rublee 等[9]提出了ORB 算子，其計算速度比SURF 和SIFT 要快且具有良好的不變性.為此，本文采用ORB 算子實現(xiàn)特征提取.

ORB 算子由關(guān)鍵點和描述子兩部分組成：關(guān)鍵點稱為“Oriented FAST（features from accelarated segment test）”，是一種改進(jìn)的FAST 角點；描述子稱為BRIEF（binary robust independent elementary features）.

FAST 角點是指圖像中灰度梯度變化較大的地方，如圖1 所示，其中f1為水平方向的階數(shù).首先，取圖像中的一個像素點，然后對比此點的灰度值與以此點為圓心半徑為3 的圓上的16 個像素的灰度值，若有連續(xù)N個點大于或者小于設(shè)定的閾值則認(rèn)為此點為角點.

圖1 FAST 角點示意Fig.1 Diagram of FAST corner

為了解決角點方向性的問題，增加了尺度和旋轉(zhuǎn)特性描述[10].尺度不變性通過構(gòu)建圖像金字塔并對其中每一層的圖像進(jìn)行特征點檢測，然后將共同檢測到的特征點認(rèn)定為正確檢測結(jié)果.旋轉(zhuǎn)特性由灰度質(zhì)心法來描述，灰度質(zhì)心法即由灰度值來確定圖像的質(zhì)心[11].在一個圖像塊M中定義圖像矩為

圖像塊質(zhì)心C的坐標(biāo)為，再連接圖像塊的幾何中心O與質(zhì)心C便可得到方向向量.

BRIEF 是一種二進(jìn)制描述子，其描述向量由多個0 和1 組成，編碼了關(guān)鍵點附近兩個像素的大小關(guān)系.如果取128 個關(guān)鍵點，則可得到一個由0 和1 組成的128 維向量.結(jié)合FAST 角點的旋轉(zhuǎn)特性便得到具有旋轉(zhuǎn)特性的BRIEF 描述子.

1.2 特征匹配與PnP 求解

圖像的特征提取結(jié)束后，采用快速最鄰近搜索算法進(jìn)行特征點匹配.通過建立隨機(jī)K-D 樹來找到圖像中某個特征點在另一張圖像中的對應(yīng)點，然后計算兩個特征點BRIEF 描述子的漢明距離.將圖像中匹配出距離最小的點的距離記做D，取該距離的4 倍為閾值作為篩選標(biāo)準(zhǔn)，對所有的配對點進(jìn)行篩選，將小于該距離的點認(rèn)為正確匹配點.最后對另一幅圖像做同樣操作，將兩者共同匹配到的點作為最終匹配點.

PnP 是求解3D 到2D 點對運動的方法[12]，如果兩幅圖像中一幅圖像特征點的空間三維坐標(biāo)已知，最少僅需3 對點就可以估計相機(jī)的運動了.特征點的空間三維坐標(biāo)可由相機(jī)的深度圖確定，PnP 問題的求解方法有很多種，例如P3P、直接線性變換、非線性優(yōu)化等，本文采取非線性優(yōu)化的方法，將PnP問題轉(zhuǎn)換為一個非線性最小二乘問題.

如圖2 所示，由于觀測噪聲的存在，對于空間中某一個特征點P，p1為重投影點，其觀測位置（像素坐標(biāo)）和該點按照當(dāng)前相機(jī)位姿計算得到的投影位置p2之間是有一定偏差，該偏差即為重投影誤差，e為節(jié)點間誤差.

圖2 重投影誤差Fig.2 Error because of reprojcetion

假設(shè)空間點Pi=(Xi，Yi，Zi)，i=1，2，···，n.相機(jī)位姿的旋轉(zhuǎn)矩陣為R、位移向量為t，將PnP 問題構(gòu)建成一個定義于李代數(shù)上最小化重投影誤差的非線性最小二乘問題，用ξ表示R和t的李代數(shù)，Pi在此位姿下重投影點pi的像素坐標(biāo)為(ui，vi)，則

式中：si為第i點的深度；K為轉(zhuǎn)換系數(shù)；ξ∧為李代數(shù)反對稱矩陣.

若有n對匹配特征，根據(jù)上面提到的重投影誤差可建立最小二乘問題，如式（3）.

式中：ξ*為李代數(shù)重投影.

該最小二乘問題的求解可用Gauss-Newton 法或Levernberg-Marquardt 法進(jìn)行求解.在SLAM 中，通過調(diào)整相機(jī)位姿和路標(biāo)點坐標(biāo)來使誤差達(dá)到最小的過程稱為BA（bundle adjustment）.

1.3 回環(huán)檢測

如果視覺里程計的建立只考慮相鄰幀間的運動，可能會導(dǎo)致誤差累積，如果能檢測出相機(jī)經(jīng)過相同的位置，那么便可以為估計的相機(jī)運動軌跡增添約束，使其更加接近真實軌跡[13].

卷積神經(jīng)網(wǎng)絡(luò)是一種前饋神經(jīng)網(wǎng)絡(luò)，主要由卷積層和池化層組成[14].相比于傳統(tǒng)計算機(jī)視覺中人工設(shè)計的圖像特征，利用CNN 提取的圖像特征更能反應(yīng)圖像的真實特性，在圖像分類與識別領(lǐng)域使用效果要遠(yuǎn)強(qiáng)于傳統(tǒng)計算機(jī)視覺方法.因此，本文采用CNN 代替?zhèn)鹘y(tǒng)詞袋法來進(jìn)行回環(huán)檢測.

SqueezeNet 是由UC Berkeley 和Stanford 研究人員一起設(shè)計的，其設(shè)計的初衷并不是為了達(dá)到最佳的CNN 識別精度，而是希望簡化網(wǎng)絡(luò)復(fù)雜度，同時達(dá)到public 網(wǎng)絡(luò)的識別精度.SqueezeNet 核心構(gòu)成 為Fire module，SqueezeNet 包 含fire2～fire9 共8 個Fire module，其中fire2 結(jié)構(gòu)如圖3 所示，其中H、W分別為特征圖的高度和寬度；ei為通道數(shù).

圖3 fire2 結(jié)構(gòu)Fig.3 Structure of fire2

SqueezeNet 通過Fire module 來減少網(wǎng)絡(luò)的參數(shù)量和Flops，每個Fire module 由squeeze 和expand模塊構(gòu)成.squeeze 模塊使用e1個1×1 的卷積核對特征圖降維，經(jīng)Relu 激活后特征圖為H×W×e1.經(jīng)expand 的兩個分路（卷積核大小分別為1×1，3×3）分別計算后特征圖為H×W×e2，然后通過concat 層將兩個分路的特征圖拼接在一起生成H×W×e3.其中e2=4e1，e3=2e2.由于fire 網(wǎng)絡(luò)卷積核大小完全相同，只有通道數(shù)不同.圖4 網(wǎng)絡(luò)結(jié)構(gòu)圖中fire 結(jié)構(gòu)只標(biāo)注fire 結(jié)構(gòu)輸出通道數(shù)e3.

當(dāng)網(wǎng)絡(luò)輸入為224×224×3 的RGB 圖像時，網(wǎng)絡(luò)的計算量僅為837 MFlops，因此，該網(wǎng)絡(luò)結(jié)構(gòu)比較適合應(yīng)用于智能移動機(jī)器人等輕量級設(shè)備上.本文使用的SqueezeNet 的網(wǎng)絡(luò)結(jié)構(gòu)如圖4 所示[15].

圖4 SqueezeNet 網(wǎng)絡(luò)結(jié)構(gòu)Fig.4 Network structure of SqueezeNet

將圖像輸入SqueezeNet 網(wǎng)絡(luò)，輸入圖像的格式為224 × 224 的三通道RGB 圖像，提取網(wǎng)絡(luò)輸出的1 000 維數(shù)組A=（a1，a2，a3，···，a1000）作為此圖像的特征向量.假設(shè)另一幅圖像的特征向量為B=（b1，b2，b3，···，b1000）.可以通過這兩幅圖像的特征向量的余弦相似度來判斷這兩個圖像是否相似，如式（4）.

若余弦相似度大于設(shè)定的閾值則認(rèn)為出現(xiàn)了回環(huán)，否則跳入下一幀圖像，再與輸入圖像的特征向量進(jìn)行比較，直至出現(xiàn)回環(huán).

1.4 全局優(yōu)化

在SLAM 過程中，需要考慮后端的全局優(yōu)化.對于相機(jī)的運動可以用位姿圖來表示，如圖5 所示，其中每個節(jié)點代表每一個關(guān)鍵幀對應(yīng)的相機(jī)位姿，節(jié)點間的邊代表兩關(guān)鍵幀之間的相機(jī)位姿變換.

圖5 位姿圖Fig.5 Pose graph

相機(jī)的位姿有6 個自由度，分別是3 個軸向位置和繞3 個軸的轉(zhuǎn)矩，用wk表示，即wk=[xk yk zkγkκk?k],k=1,2,···,j.

兩個位姿間的變換關(guān)系為

連接的兩個節(jié)點k和j之間的誤差為

將所有的節(jié)點間誤差相加起來并標(biāo)量化，可以得到總誤差為

式中：F為節(jié)點集合；Ωk,j為xk和xj之間的信息矩陣.

求解最小二乘問題便可得到優(yōu)化后的位姿.本文采用通用圖形優(yōu)化g2o 來得到優(yōu)化后的位姿[16].定義頂點和邊的類型，添加節(jié)點和邊的信息，選擇合適的優(yōu)化算法的求解器和迭代算法.g2o 提供了3 種不同的線性求解器：CSparse、基于喬里斯基分解Cholmod 和基于預(yù)處理共軛梯度算法PCG（preconditioned conjugate gradient），迭代算法包括高斯牛頓法、Levernberg-Marquardt 法和Powell’s dogleg.

2 試驗結(jié)果與分析

試驗在一個安裝了Kinect V1 深度相機(jī)和Nvidia TX2 開發(fā)板的Turtlebot 二代機(jī)器人平臺上進(jìn)行.通過Ubuntu14.04 系統(tǒng)運行算法，涉及軟件包括ROS機(jī)器人操作系統(tǒng)和Caffe、OpenCV、Eigen、PCL、g2o等開源庫[17].

試驗數(shù)據(jù)集包括TUM（technical university of munich）數(shù)據(jù)集、nyuv2 數(shù)據(jù)集和利用Turtlebot2 自行采集的數(shù)據(jù)集，如表1.其中，TUM 數(shù)據(jù)集是由德國慕尼黑工業(yè)大學(xué)計算機(jī)視覺組采集制作的[18]，通過在深度相機(jī)上加裝高精運動傳感器使得在獲取彩色圖像和深度圖像的同時可以得到相機(jī)的真實運動軌跡.TUM 數(shù)據(jù)集中還提出了一種軌跡誤差的計算方法，可以計算得到SLAM 算法的估計運動軌跡與高精運動傳感器獲取的標(biāo)準(zhǔn)軌跡之間的誤差.

表1 實驗采用的數(shù)據(jù)集Tab.1 Data sets for testing

2.1 與無回環(huán)檢測的算法進(jìn)行對比

利用Turtlebot2 在室內(nèi)采集的數(shù)據(jù)集，包括968 幀分辨率為640 × 480 像素的彩色圖像和對應(yīng)的深度圖像，采集速度為30 幀/s.

有、無回環(huán)檢測軌跡估計對比結(jié)果如圖6 所示，當(dāng)采用無回環(huán)檢測的算法進(jìn)行建圖時，其軌跡如圖6（a）所示，由于誤差的累積導(dǎo)致相機(jī)運動軌跡出現(xiàn)漂移而無法閉合.而采用本文算法進(jìn)行建圖時，如圖6（b）所示，由于成功檢測到回環(huán)因而在回環(huán)處添加了運動約束從而使運動軌跡趨于閉合.

圖6 軌跡估計對比Fig.6 Comparison results for trajectory estimation

圖7 是采用無回環(huán)檢測與本文回環(huán)檢測算法進(jìn)行建圖時獲得的點云圖.

從圖7（b）中可以看出有回環(huán)檢測時，在出現(xiàn)回環(huán)的位置點云地圖基本重合，表明估計的軌跡與真實軌跡有較大誤差.由此可見：本文算法可成功檢測出回環(huán)并為全局軌跡優(yōu)化增添約束，使得估計的運動和建立的地圖更接近真實情況.

圖7 點云圖對比（紅圈為出現(xiàn)回環(huán)部分）Fig.7 Comparison results for points nephogram(the red circle is the loop)

2.2 與詞袋法進(jìn)行對比

通過算法來判定是否出現(xiàn)回環(huán)與事實可能是不同的，大概會出現(xiàn)4 種不同情況，如表2 所示.

表2 回環(huán)檢測可能出現(xiàn)的結(jié)果Tab.2 Possible results for loop detection

采用準(zhǔn)確率Ta和召回率Tr來衡量回環(huán)檢測結(jié)果的好壞，如式（8）、（9）.

式中：T1、T2、T3分別為回環(huán)檢測結(jié)果是真陽性、假陽性和假陰性的數(shù)量.

分別將本文提出的回環(huán)檢測算法和詞袋法在New college 數(shù)據(jù)集和自己制作的數(shù)據(jù)集上進(jìn)行測試，試驗得到準(zhǔn)確率和召回率對比結(jié)果如圖8 所示，具體數(shù)據(jù)見表3 所示.

表3 本文算法與詞袋法對比Tab.3 Comparison between algorithm and word bag method in this paper

圖8 本文算法與詞袋法的準(zhǔn)確性和召回率曲線對比Fig.8 Comparison of the accuracy and recall curves between the algorithm in this paper and the word bag method

由圖8 可以看出，在準(zhǔn)確率相同的情況下本文算法的召回率明顯高于詞袋法的召回率.因為在回環(huán)檢測算法中準(zhǔn)確率的重要性是遠(yuǎn)高于召回率的，一旦出現(xiàn)假陽性結(jié)果會對最終的地圖建立有極為不好的影響，因此，一般都要保證準(zhǔn)確率為1.當(dāng)準(zhǔn)確率為1 時本文算法的召回率為0.4，而詞袋法的召回率為0.33，本文算法相對于詞袋法提高了約21%，對兩張圖片進(jìn)行相似度計算所花時間減少了74%.

2.3 建圖精度

德國慕尼黑工業(yè)大學(xué)計算機(jī)視覺組在TUM 數(shù)據(jù)集中提供了一種軌跡誤差的測量方法——絕對軌跡誤差（absolute trajectory error，ATE）.ATE 通過計算估計的軌跡和標(biāo)準(zhǔn)軌跡的均方根誤差來獲得.假設(shè)相機(jī)的標(biāo)準(zhǔn)軌跡為X={X1,X2,···,Xn}，相機(jī)的估計軌跡為，則絕對軌跡誤差的均方根為

式中：trans(·)為獲取位姿平移向量的函數(shù).

圖9 為本文算法的估計運動軌跡與通過高精運動傳感器獲取的標(biāo)準(zhǔn)軌跡.由于TUM 數(shù)據(jù)集中采用的高精運動傳感器的采集頻率非常高，所以獲取的軌跡信息更加豐富.

圖9 標(biāo)準(zhǔn)軌跡、估計軌跡和軌跡誤差Fig.9 Standard trajectory，estimated trajectory and trajectory error

表4 對算法的建圖精度進(jìn)行了對比，所使用的TUM fr3_stf 數(shù)據(jù)集采集時長共31.55 s、標(biāo)準(zhǔn)軌跡總長5.884 m、相機(jī)平均移動速度0.193 m/s，計算與本文算法估計軌跡的絕對軌跡誤差的均方根值為0.050 0 m.同樣使用RGB-D SLAM[19]進(jìn)行建圖得到絕對軌跡誤差的均方根值為0.049 0 m.由圖9（a）可見TUM fr3_stf 中無回環(huán)出現(xiàn)，本文算法與RGB-D S LAM建圖誤差基本相近.

表4 建圖誤差對比Tab.4 Comparison of mapping errors

所使用的TUM fr3_xyz 數(shù)據(jù)集采集時長共30.09 s、標(biāo)準(zhǔn)軌跡總長7.112 m、相機(jī)平均移動速度0.244 m/s，計算與本文算法估計軌跡的絕對軌跡誤差的均方根值為0.010 4 m.同樣使用RGB-D SLAM進(jìn)行建圖得到絕對軌跡誤差的均方根值為0.013 5 m.由圖9（b）可見TUM fr3_xyz 中有較多回環(huán)出現(xiàn)，本文算法與RGB-D SLAM 相比，建圖誤差降低了29%.

3 結(jié) 論

1）提出一種利用CNN 進(jìn)行回環(huán)檢測的視覺SLAM 算法.前端采用特征點法建立視覺里程計，追蹤ORB 特征并結(jié)合深度相機(jī)獲得的距離信息來估計相機(jī)運動，后端利用CNN 進(jìn)行回環(huán)檢測并利用g2o 圖優(yōu)化工具進(jìn)行全局優(yōu)化.

2）分別對有、無回環(huán)檢測在軌跡估計誤差、點云圖、建圖精度等方面進(jìn)行試驗.相比與無回環(huán)檢測算法，本文算法在出現(xiàn)回環(huán)情況下可以使估計的運動軌跡趨于閉合從而使建立的地圖更加接近真實場景；數(shù)據(jù)集上測試表明，當(dāng)有較多回環(huán)出現(xiàn)時，本文算法相比較于RGB-D SLAM 算法在建圖準(zhǔn)確度上有所提高；在回環(huán)檢測方面，相比于詞袋法，本文算法召回率可提高21%且計算耗時減少74%.

3）本文算法在魯棒性方面并未做具體改進(jìn)，在面對特征點丟失的情況可能會表現(xiàn)不佳，以后需要展開更加深入的研究.