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

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

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

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

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

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

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

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

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

1.1 特征提取

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

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

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

圖1 FAST 角點(diǎn)示意Fig.1 Diagram of FAST corner

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

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

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

1.2 特征匹配與PnP 求解

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

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

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

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

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

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

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

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

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

1.3 回環(huán)檢測(cè)

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

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

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

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

SqueezeNet 通過(guò)Fire module 來(lái)減少網(wǎng)絡(luò)的參數(shù)量和Flops，每個(gè)Fire module 由squeeze 和expand模塊構(gòu)成.squeeze 模塊使用e1個(gè)1×1 的卷積核對(duì)特征圖降維，經(jīng)Relu 激活后特征圖為H×W×e1.經(jīng)expand 的兩個(gè)分路（卷積核大小分別為1×1，3×3）分別計(jì)算后特征圖為H×W×e2，然后通過(guò)concat 層將兩個(gè)分路的特征圖拼接在一起生成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 圖像時(shí)，網(wǎng)絡(luò)的計(jì)算量?jī)H為837 MFlops，因此，該網(wǎng)絡(luò)結(jié)構(gòu)比較適合應(yīng)用于智能移動(dòng)機(jī)器人等輕量級(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）.可以通過(guò)這兩幅圖像的特征向量的余弦相似度來(lái)判斷這兩個(gè)圖像是否相似，如式（4）.

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

1.4 全局優(yōu)化

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

圖5 位姿圖Fig.5 Pose graph

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

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

連接的兩個(gè)節(jié)點(diǎn)k和j之間的誤差為

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

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

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

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

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

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

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

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

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

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

圖6 軌跡估計(jì)對(duì)比Fig.6 Comparison results for trajectory estimation

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

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

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

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

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

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

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

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

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

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

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

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

2.3 建圖精度

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

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

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

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

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

表4 建圖誤差對(duì)比Tab.4 Comparison of mapping errors

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

3 結(jié) 論

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

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

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