1引言

    同步定位與地圖構(gòu)建(SimultaneousLocalizationandMapping，SLAM)是機(jī)器人領(lǐng)域的技術(shù)熱點(diǎn)與難點(diǎn)，其解決的定位和建圖問題被認(rèn)為是機(jī)器人實(shí)現(xiàn)自主導(dǎo)航的關(guān)鍵。該技術(shù)主要原理是通過機(jī)器人配備的多種傳感器來感知周圍環(huán)境，并計算出自己在當(dāng)前環(huán)境下的位置。SLAM被提出后，先后經(jīng)歷了多個研究階段。由于激光雷達(dá)精度高、范圍廣，早期的SLAM研究往往以激光雷達(dá)為主要的傳感器。另外，早期的SLAM采用擴(kuò)展卡爾曼濾波方法估計機(jī)器人的位姿[1]，但效果不好，對于某些強(qiáng)非線性系統(tǒng)，該方法會帶來更多的截斷誤差，從而導(dǎo)致無法精準(zhǔn)地實(shí)現(xiàn)定位與建圖。而后基于粒子濾波的SLAM逐漸成為主流，粒子采樣方法可有效避免非線性問題，但同樣引出了隨著粒子數(shù)增多，計算量增大的問題。

    一直到2007年，Grisetti等[2]提出了基于改進(jìn)粒子濾波的SLAM方法(Gmapping)，該方法通過改進(jìn)的建議分布及自適應(yīng)重采樣技術(shù)有效地提高了定位精度并降低了計算量，是激光SLAM的一個里程碑。Konolige等[3]在2010年提出了KartoSLAM，該方法采用圖優(yōu)化代替粒子濾波，并采用稀疏點(diǎn)調(diào)整來解決非線性優(yōu)化中矩陣直接求解難的問題。Kohlbrecher等[4]在2011年提出了HectorSLAM，該方法不需要里程計信息，通過利用高斯牛頓方法來解決掃描匹配問題，但對傳感器要求較高，需要高精度的激光雷達(dá)才能運(yùn)行。由谷歌于2016年提出的Cartographer[5]可以說是最新的激光SLAM方案。該方法將獲得的每一幀激光數(shù)據(jù)，利用掃描匹配在最佳估計位置處插入子圖(Submap)中，且掃描匹配只與當(dāng)前子圖有關(guān)。在生成一個子圖后，會進(jìn)行一次局部的回環(huán)(LoopClose)，而在所有子圖完成后，利用分支定位和預(yù)先計算的網(wǎng)格進(jìn)行全局的回環(huán)。相比于Gmapping和Hector，該方案具有累積誤差較低，且不需要高成本設(shè)備的優(yōu)點(diǎn)。視覺SLAM由于傳感器成本低、圖像信息豐富逐漸成為了SLAM的研究熱點(diǎn)。

    但與激光SLAM相比，視覺SLAM更加復(fù)雜。Davison等[6]在2007年首次提出的MonoSLAM被認(rèn)為是眾多視覺SLAM的“發(fā)源地”。該方法以擴(kuò)展卡爾曼濾波為后端，追蹤前端稀疏的特征點(diǎn)；利用概率密度函數(shù)來表示不確定性，從觀測模型和遞歸的計算，最終獲得后驗(yàn)概率分布的均值和方差。Sim等[7]利用粒子濾波實(shí)現(xiàn)了視覺SLAM，該方法避開了線性化的問題并且精度高，但需要使用大量的粒子，從而導(dǎo)致計算復(fù)雜度的提高。隨后，為了減少計算復(fù)雜度，提取關(guān)鍵幀變得極為重要，其中最具代表性的是2007年Klein和Murray提出的PTAM[8]。該方法提出了簡單有效地提取關(guān)鍵幀的技術(shù)，并且該方法最為關(guān)鍵的兩點(diǎn)是：(1)實(shí)現(xiàn)了跟蹤與建圖的并行化，雖然跟蹤部分需要實(shí)時響應(yīng)圖像數(shù)據(jù)，但后端優(yōu)化卻不需要實(shí)時計算。即后端優(yōu)化可在后臺慢慢運(yùn)行，在需要的時候?qū)蓚€線程同步即可。這也是首次提出了區(qū)分前后端的概念，引領(lǐng)了之后眾多SLAM方法的架構(gòu)設(shè)計。(2)第一次使用非線性優(yōu)化，而不是傳統(tǒng)的濾波器。從PTAM提出后，視覺SLAM研究逐漸轉(zhuǎn)向了以非線性優(yōu)化為主導(dǎo)的后端。

    2016年Mur-Artal和Tardos[9]提出了現(xiàn)代SLAM中非常著名的第二代基于具有帶方向性的加速分段測試特征(FAST)關(guān)鍵點(diǎn)以及帶旋轉(zhuǎn)不變性的二進(jìn)制魯棒獨(dú)立基本特征(BRIEF)描述子的特征點(diǎn)(OrientedFASTandRotatedBRIEF，ORB)[10]的視覺SLAM系統(tǒng)ORB-SLAM2，是現(xiàn)代SLAM中做得十分完善并且易用的系統(tǒng)之一。該方法不僅支持單目、雙目、深度攝像頭三種模式，并且將定位、地圖創(chuàng)建、閉環(huán)分為三個線程，且都使用ORB特征。ORB-SLAM2的閉環(huán)檢測是一大亮點(diǎn)，該方法采用了詞袋模型，有效地防止了累積誤差，并且能在丟失之后迅速找回。但ORB-SLAM2的缺點(diǎn)也十分明顯，圖像特征提取與匹配以及后端的優(yōu)化都需要計算資源，在嵌入式平臺上實(shí)現(xiàn)實(shí)時運(yùn)行存在一定的困難，并且構(gòu)建的稀疏特征點(diǎn)地圖雖然可以很好地滿足定位，但無法提供導(dǎo)航、避障等功能。相比于提取圖像中的特征點(diǎn)，根據(jù)圖像像素灰度信息來計算相機(jī)運(yùn)動的直接法從另一個方向?qū)崿F(xiàn)了定位和建圖。Stühmer等[11]提出的相機(jī)定位方法依賴圖像的每個像素點(diǎn)，即用稠密的圖像對準(zhǔn)來進(jìn)行自身定位，并構(gòu)建出稠密的三維地圖。Engel等[12]對當(dāng)前圖像構(gòu)建半稠密深度地圖，并使用稠密圖像配準(zhǔn)(DenseImageAlignment)法計算相機(jī)位姿。構(gòu)建半稠密地圖即估計圖像中梯度較大的所有像素的深度值，該深度值被表示為高斯分布，且當(dāng)新的圖像到來時，該深度值被更新。Engel等[13]提出了LSD-SLAM算法，其核心是將直接法應(yīng)用到半稠密的單目SLAM中，這在之前的直接法中很少見到。以往基于特征點(diǎn)只能構(gòu)建稀疏地圖，而稠密地圖又需要RGB-D這種可以提供深度信息的相機(jī)。

    Forster等[14]于2014年提出了半直接法單目視覺里程計(Semi-directMonocularVisualOdometry，SVO)，一種被稱為“稀疏直接法”的方法，該方法將特征點(diǎn)與直接法混合使用，跟蹤了一些關(guān)鍵點(diǎn)(如角點(diǎn)等)，然后按照直接法根據(jù)關(guān)鍵點(diǎn)周圍的信息估計相機(jī)運(yùn)動及位置。相比于其他方案，SVO既不用消耗大量資源去計算描述子，也不必處理過多的像素信息，因此該方法可廣泛用于無人機(jī)、手持增強(qiáng)現(xiàn)實(shí)(AugmentedReality，AR)等設(shè)備上。Newcombe等[15]提出了Kinect融合的方法，該方法通過Kinect獲取的深度圖像對每幀圖像中的每個像素進(jìn)行最小化距離測量而獲得相機(jī)位姿，且融合所有深度圖像，從而獲得全局地圖信息。Gokhool等[16]使用圖像像素點(diǎn)的光度信息和幾何信息來構(gòu)造誤差函數(shù)，通過最小化誤差函數(shù)而獲得相機(jī)位姿，且地圖問題被處理為位姿圖表示。Kerl等[17]提出了較好的直接RGB-DSLAM方法，該方法結(jié)合像素點(diǎn)的強(qiáng)度誤差與深度誤差作為誤差函數(shù)，通過最小化代價函數(shù)，從而求出最優(yōu)相機(jī)位姿，該過程由g2o實(shí)現(xiàn)，并提出了基于熵的關(guān)鍵幀提取及閉環(huán)檢測方法，從而大大降低了路徑的誤差。 

    在多傳感器融合方面，目前有視覺傳感器、激光雷達(dá)、慣性測量單元(InertialMeasurementUnit，IMU)及超聲波傳感器等多種傳感器。目前，主要融合方向?yàn)榧す饫走_(dá)結(jié)合視覺傳感器及IMU結(jié)合視覺傳感器。Chen等[18]通過視覺傳感器結(jié)合IMU來進(jìn)行機(jī)器人的精準(zhǔn)位姿估計，并在機(jī)器人上垂直安裝二維激光雷達(dá)采集點(diǎn)云數(shù)據(jù)實(shí)現(xiàn)三維建圖。Houben等[19]采用三維激光數(shù)據(jù)用于微型無人機(jī)定位，針對結(jié)構(gòu)相似的環(huán)境激光定位會出現(xiàn)模糊的問題，提出在環(huán)境中加入視覺標(biāo)記，提高定位精度，同時可用于微型無人機(jī)的重定位。王消為等[20]提出了一種雙目視覺信息和激光雷達(dá)數(shù)據(jù)融合的SLAM方法，該方法基于改進(jìn)的粒子濾波算法實(shí)現(xiàn)了在計算建議分布時觀測數(shù)據(jù)，其中同時包含視覺信息和激光雷達(dá)數(shù)據(jù)。相對于里程計運(yùn)動模型作為建議分布，該方法有效地提高了定位和建圖的精度。張杰和周軍[21]提出了一種激光雷達(dá)與視覺結(jié)合的SLAM方法，其中激光地圖用于導(dǎo)航，視覺地圖用于復(fù)原目標(biāo)場景；并提出一種改進(jìn)的迭代最近點(diǎn)法(IterativeClosestPoint，ICP)用于實(shí)現(xiàn)更快的點(diǎn)云拼接，同時采用圖優(yōu)化的方法降低了累積誤差，保證地圖精度。Shi等[22]在小型無人機(jī)上，利用視覺里程計為二維激光的ICP提供初值，在實(shí)時性和精確度方面都達(dá)到了較好的效果。

    Qin等[23]提出了視覺與IMU的緊耦合方案，將視覺構(gòu)造的殘差項(xiàng)和IMU構(gòu)造的殘差項(xiàng)放在一起構(gòu)成一個聯(lián)合優(yōu)化問題。Li等[24]和Lynen等[25]通過擴(kuò)展卡爾曼濾波器對視覺和IMU進(jìn)行融合以實(shí)時獲取狀態(tài)估計。相對而言，激光SLAM的效果仍然要優(yōu)于視覺SLAM，但激光SLAM由于其本身激光數(shù)據(jù)的特性導(dǎo)致其無法有效地進(jìn)行大范圍的閉環(huán)檢測。對于低價格的激光雷達(dá)，由于激光點(diǎn)不夠密集，構(gòu)建的地圖常常會出現(xiàn)回到原來的位置地圖，由此造成定位不準(zhǔn)確而出現(xiàn)偏差的情況，這是由累積誤差所導(dǎo)致的。同時在激光SLAM中，閉環(huán)檢測一直是一大難點(diǎn)：由于獲取的激光數(shù)據(jù)是二維點(diǎn)云數(shù)據(jù)，無明顯特征且相互之間十分相似，所以基于激光數(shù)據(jù)的閉環(huán)檢測往往效果不好。由于圖像中包含了豐富的信息，故視覺SLAM在閉環(huán)檢測方面存在著天然優(yōu)勢。ORB-SLAM2中提出的詞袋模型，采用了ORB特征配合詞袋的方法，具有很高的準(zhǔn)確率和速度，是當(dāng)前應(yīng)用最廣的閉環(huán)檢測手段。針對低成本激光SLAM噪聲大、精度低、難以閉環(huán)的問題，本文提出激光結(jié)合視覺進(jìn)行聯(lián)合優(yōu)化的方法來提高定位建圖精度，并通過視覺詞袋模型有效地解決激光閉環(huán)檢測難的問題。

2基于圖優(yōu)化的同步定位與地圖構(gòu)建框架

    激光SLAM主要通過相鄰幀激光的匹配來計算相鄰幀位姿變換，但由于激光數(shù)據(jù)并非完全無噪聲的數(shù)據(jù)，尤其對于低成本激光雷達(dá)，打出去的激光點(diǎn)較為稀疏，從而導(dǎo)致計算得到的相鄰幀位姿變換存在誤差，因此往往需要加入濾波或優(yōu)化的方法來使定位更加精準(zhǔn)。而視覺SLAM是通過特征點(diǎn)提取與匹配來反算位姿，但若其中一幀出現(xiàn)問題就會導(dǎo)致誤差累積并

不斷地增大。圖優(yōu)化是SLAM后端優(yōu)化的熱門方法，通過構(gòu)造節(jié)點(diǎn)和約束邊清晰地展示了一系列位姿和觀測量的關(guān)系，再通過非線性優(yōu)化的手段求出最優(yōu)變量從而得到精準(zhǔn)的位姿估計。該方法最早應(yīng)用于視覺SLAM中，起初由于圖像特征點(diǎn)數(shù)量過多，導(dǎo)致矩陣維數(shù)過大，求解困難，所以該方法一直未能成為主流方法。直到2011年海塞矩陣的稀疏性被發(fā)現(xiàn)，極大地提高了運(yùn)算速度，使得非線性優(yōu)化和圖優(yōu)化的思想成功應(yīng)用于視覺SLAM和激光SLAM中。而根據(jù)位姿估計和優(yōu)化的實(shí)時性要求不同，SLAM也被分為前端和后端兩部分，當(dāng)前主流的SLAM框架如圖1所示。

    前端主要通過傳感器數(shù)據(jù)估計機(jī)器人的位姿，但無論是圖像還是激光，觀測到的數(shù)據(jù)都含有不同程度的噪聲。相對而言，高精度激光雷達(dá)噪聲會小很多但成本過高，而通過低成本的激光雷達(dá)及相機(jī)采集圖像來進(jìn)行位姿計算都會導(dǎo)致定位與實(shí)際真值有累積誤差，并且累積誤差會隨著時間的增加而越來越大。后端優(yōu)化的主要作用就是通過濾波或優(yōu)化的方式提高定位及構(gòu)建的地圖精度，消除累積誤差。本文采用圖優(yōu)化作為后端，通過非線性優(yōu)化尋找下降梯度的方式來迭代實(shí)現(xiàn)誤差最小化。簡單來說，圖優(yōu)化是以圖的形式來描繪優(yōu)化問題。在SLAM中，圖的節(jié)點(diǎn)表示位姿，邊表示位姿之間以及位姿與觀測量的約束關(guān)系。機(jī)器人在導(dǎo)航建圖過程中，觀測量為激光數(shù)據(jù)及通過攝像頭不停地捕捉到的外部環(huán)境信息，生成大量機(jī)器人觀測到的ORB特征點(diǎn)對應(yīng)的三維空間點(diǎn)。將所有數(shù)據(jù)放入圖的框架中，如圖2所示。

    其中，X表示關(guān)鍵幀位姿；O表示觀測量，包含了特征點(diǎn)所對應(yīng)的三維空間點(diǎn)坐標(biāo)及二維激光數(shù)據(jù)。視覺誤差由重投影誤差(圖3)來表示，重投影誤差的計算需要給定相鄰幀所對應(yīng)的兩個相機(jī)位姿、匹配好的特征點(diǎn)在兩幅圖像中的二維坐標(biāo)及所對應(yīng)的三維空間點(diǎn)的三維坐標(biāo)。純視覺SLAM往往通過特征點(diǎn)提取和匹配，再使用EPnP等方法來求取相鄰幀位姿變換估計。但相比于相鄰幀的激光匹配，該方法誤差較大，因此本文使用激光掃描匹配得到的位姿估計作為后端優(yōu)化的初始值。而對于相鄰幀圖像中通過特征點(diǎn)匹配得到的特征點(diǎn)對p1、p2，本文采用的深度攝像頭可直接獲取得到前一幀圖像特征點(diǎn)p1所對應(yīng)的三維空間點(diǎn)P的坐標(biāo)，將點(diǎn)P重新投影到后一幀圖像上形成圖像中的特征點(diǎn)。由于位姿估計的誤差及深度攝像頭噪聲的存在，與p2并非完全重合，兩點(diǎn)之間的距離就是誤差。

圖3

圖4聯(lián)合優(yōu)化整體框架

重投影坐標(biāo)計算公式及過程如下：(1)通過世界坐標(biāo)系到相機(jī)坐標(biāo)系的變換關(guān)系(R,t)，計算出世界坐標(biāo)系下點(diǎn)P所對應(yīng)的相機(jī)坐標(biāo)系下點(diǎn)的三維坐標(biāo)。

  （1）

其中，R為旋轉(zhuǎn)矩陣；t為平移矩陣；T表示轉(zhuǎn)置。(2)將投至歸一化平面，并將歸為1得到歸一化坐標(biāo)Pc。

其中，(uc,vc)為歸一化平面上的二維坐標(biāo)。(3)根據(jù)相機(jī)內(nèi)參模型，像素坐標(biāo)系為歸一化平面橫軸縮放了fx倍，豎軸縮放了fy倍，并且原點(diǎn)平移了cx、cy個像素。因此，像素坐標(biāo)(us,vs)計算公式如下：

則該點(diǎn)誤差函數(shù)為。其中，p2為直接通過圖像得到的像素坐標(biāo)；為根據(jù)公式(1)～(3)計算得到的重投影坐標(biāo)。將上述誤差函      數(shù)擴(kuò)展到相鄰兩幀之間，則最小化代價函數(shù)為。通過非線性優(yōu)化算法可以得到誤差最小化時相應(yīng)的位姿變換(R,t)和三維空間點(diǎn)坐標(biāo)Pi。

    相對于視覺誤差，激光誤差的獲取更為簡單。激光SLAM往往需要通過掃描匹配來實(shí)現(xiàn)相鄰幀的位姿變換估計，而該估計值(R,t)無法保證前一幀所有激光數(shù)據(jù)經(jīng)過該位姿變換與后一幀激光數(shù)據(jù)完全重合。因此，激光的誤差定義如下：

  (4)

之后，通過非線性優(yōu)化來使誤差函數(shù)最小化，求得的位姿再返回到前端作為下一幀的參考幀位姿。

3.后端優(yōu)化與閉環(huán)檢測

    視覺SLAM構(gòu)建的地圖由特征點(diǎn)構(gòu)成，特征地圖最大的問題在于無法用于導(dǎo)航，僅適合用于定位。低成本激光雷達(dá)構(gòu)建的柵格地圖更適合用于導(dǎo)航，但存在著激光稀疏、噪聲大的問題。因此，本文提出視覺激光融合的方法，在提高定位精度的同時確保地圖更加精準(zhǔn)，還解決了激光SLAM構(gòu)建的柵格地圖難以回環(huán)的問題。而二維柵格地圖的構(gòu)建主要依賴于一系列位姿和激光測量數(shù)據(jù)，根據(jù)柵格被占用的概率來增量式的建圖。由于同一時刻觀測到的視覺信息和激光數(shù)據(jù)并非完全相互獨(dú)立，因此本文提出視覺激光聯(lián)合優(yōu)化來充分利用數(shù)據(jù)之間的約束。加入視覺信息的激光視覺聯(lián)合優(yōu)化的SLAM整體框架如圖4所示。

3.1誤差函數(shù)

    傳統(tǒng)視覺相鄰幀誤差函數(shù)已在第2節(jié)中給出具體形式，圖3圖4聯(lián)合優(yōu)化整體框架6263技術(shù)前沿技術(shù)前沿重投影坐標(biāo)與三維空間點(diǎn)、上一幀圖像特征點(diǎn)以及位姿變換的關(guān)系如下：

  （5）

其中，K為相機(jī)內(nèi)參；Z為三維點(diǎn)的深度值。則誤差函數(shù)如下：

位姿變換(R,t)可寫成對應(yīng)的李代數(shù)形式，李代數(shù)變換公式為：

  （7）

    其中，相鄰幀共有m個匹配特征點(diǎn)，n個激光數(shù)據(jù)點(diǎn)。實(shí)際計算中可將m、n限制在一定數(shù)量內(nèi)從而減少計算復(fù)雜度。單獨(dú)的視覺SLAM有其自有的計算相鄰幀位姿變換算法，但由于圖像數(shù)據(jù)的特點(diǎn)，計算得到的(R,t)往往沒有通過激光掃描匹配得到的位姿變換精準(zhǔn)。因此，采用激光掃描匹配得到的位姿變換來估計誤差函數(shù)中的位姿變換初始值。

3.2稀疏姿態(tài)調(diào)整

    根據(jù)觀測模型可以很容易判斷出誤差函數(shù)不是線性函數(shù)，所以本文采用非線性優(yōu)化來求解誤差函數(shù)最小值所對應(yīng)的變量。由于三維特征點(diǎn)和激光數(shù)據(jù)量過多，且隨著時間推移整體代價函數(shù)所包含的多項(xiàng)式會變得越來越多，故本文只優(yōu)化誤差函數(shù)中的位姿變量。同時，姿態(tài)圖的優(yōu)化也會隨著頂點(diǎn)約束的增加而變慢，所以本文采用稀疏姿態(tài)調(diào)整法，利用矩陣的稀疏性來提高優(yōu)化速率。由于機(jī)器人位姿是由變換矩陣不斷計算得到且一一對應(yīng)，即求得每兩相鄰幀之間的位姿變換，亦即可獲得機(jī)器人的當(dāng)前位姿。因此，將機(jī)器人位姿作為唯一變量并對其進(jìn)行優(yōu)化，視覺觀測量和激光點(diǎn)云數(shù)據(jù)作為位姿之間的約束。設(shè)機(jī)器人位姿與位姿變換之間的關(guān)系為：

則誤差函數(shù)可改寫為關(guān)于位姿x的函數(shù)。其中，x為位姿的集合，即待優(yōu)化變量為：

  （10）

其中，k為待優(yōu)化位姿個數(shù)。相應(yīng)地，是對整體自變量x的增量。因此，當(dāng)加入增量后，目標(biāo)函數(shù)為：

  （11）

    其中，J表示雅克比矩陣，為代價函數(shù)對自變量的偏導(dǎo)數(shù)；K為待優(yōu)化位姿個數(shù)，相鄰幀優(yōu)化時為2，全局優(yōu)化時為當(dāng)前幀到回環(huán)幀之間的位姿個數(shù)。位姿優(yōu)化可看作是最小二乘問題，而解決最小二乘問題的常用方法有梯度下降法、高斯牛頓法及LevenbergMarquadt(L-M)法。其中，L-M法是對梯度下降法和高斯牛頓法的綜合運(yùn)用，效果最佳，因此本文采用L-M法來求解上述最小二乘問題。通過加入拉格朗日乘子對誤差函數(shù)進(jìn)行改進(jìn)：

  （12）

    其中，參數(shù)表示近似模型與實(shí)際模型相似程度，越接近1時，越小，高斯牛頓法的近似效果越好；越小時近似效果越差，優(yōu)化方法更近似于梯度下降法。一般情況下，H矩陣維數(shù)很大，矩陣求逆的復(fù)雜度為O(n3)。但由于H矩陣內(nèi)部包含著各個頂點(diǎn)之間的約束，而只有相鄰頂點(diǎn)之間才具有直接約束，從而導(dǎo)致H矩陣大部分元素為0，具有稀疏性。因此，利用H矩陣特有的稀疏性可以大大提高運(yùn)算速度。求解公式(16)可得，沿著梯度下降方向循環(huán)迭代，最終獲得目標(biāo)函數(shù)最小時所對應(yīng)的自變量x，即機(jī)器人位姿。

3.3閉環(huán)檢測

    閉環(huán)檢測是SLAM中的一個核心問題，通過對曾經(jīng)走過的地點(diǎn)進(jìn)行識別，能有效地減小累積誤差，提高定位精度?；诩す饫走_(dá)的SLAM算法往往因?yàn)閿?shù)據(jù)單一而無法有效地進(jìn)行閉環(huán)檢測，而視覺圖像豐富的紋理特征則剛好可以彌補(bǔ)激光雷達(dá)的這一缺陷。本文采用視覺SLAM中最常用的詞袋模型(Bag-of-Words，BoW)，通過視覺特征來構(gòu)建關(guān)鍵幀所對應(yīng)的字典，在檢測到回環(huán)后通過回環(huán)幀與當(dāng)前幀的匹配來計算當(dāng)前位姿，并將這一約束加入到后端中來進(jìn)行回環(huán)幀與當(dāng)前幀之間的全局優(yōu)化，提高定位精度的同時，也防止了激光所構(gòu)建的柵格地圖常出現(xiàn)的無法閉合的情況。

表1位姿估計對比

    由于構(gòu)建室內(nèi)地圖所采集到的圖像數(shù)量過多，且相鄰圖像之間具有很高的重復(fù)性，因此首先需要進(jìn)行關(guān)鍵幀的提取。本文關(guān)鍵幀選取機(jī)制如下：(1)距離上一次全局重定位已經(jīng)過去了15幀；(2)距離上一次插入關(guān)鍵幀已經(jīng)過去了15幀；(3)關(guān)鍵幀必須已經(jīng)跟蹤到了至少50個三維特征點(diǎn)。其中，(1)、(2)是其獨(dú)特性的基礎(chǔ)，因?yàn)槎虝r間視野內(nèi)的特征不會發(fā)生明顯變化；(3)保證了其魯棒性，過少的地圖點(diǎn)會導(dǎo)致計算誤差的不均勻。相對而言，激光SLAM關(guān)鍵幀的選取要更為簡單且穩(wěn)定，但采用激光作為關(guān)鍵幀選取參量容易出現(xiàn)關(guān)鍵幀包含的圖片特征點(diǎn)不夠、圖像不連續(xù)等問題。因此，本文采用基于視覺的關(guān)鍵幀選取機(jī)制，既保證相鄰關(guān)鍵幀不會過于接近，又保證有足夠的信息進(jìn)行關(guān)鍵幀之間的匹配。常見的圖像特征有尺度不變特征變換(ScaleInvariantFeatureTransform，SIFT)[26]、加速穩(wěn)健特征(SpeededUpRobustFeatures，SURF)[27]和ORB等。其中，SIFT特征點(diǎn)具有旋轉(zhuǎn)不變性和尺度不變性，并且穩(wěn)定性高，不易受到光照及噪聲的影響，是最理想的選擇。然而，SIFT特征提取速度慢無法保證實(shí)時，故不適用于SLAM中。SURF效果與SIFT相近同樣存在計算時間過長的問題。因此，本文選擇ORB來構(gòu)建詞袋模型，ORB在保持了旋轉(zhuǎn)不變性和尺度不變性的同時，速度要明顯優(yōu)于SIFT和SURF。

    BoW通過比對圖像所包含的特征將當(dāng)前幀與每一個關(guān)鍵幀作相似度計算。首先當(dāng)相似度表明當(dāng)前幀與某一關(guān)鍵幀足夠相似時，則認(rèn)為機(jī)器人回到了該關(guān)鍵幀附近的位置，產(chǎn)生回環(huán)。然后，在回環(huán)后對當(dāng)前幀和該關(guān)鍵幀進(jìn)行激光點(diǎn)云的ICP匹配從而以該關(guān)鍵幀的位姿為基準(zhǔn)，計算出當(dāng)前機(jī)器人位姿，并將當(dāng)前幀位姿、當(dāng)前幀與回環(huán)幀之間的圖像特征點(diǎn)、激光點(diǎn)云作為約束加入到圖優(yōu)化框架中，從而對檢測到回環(huán)的關(guān)鍵幀與當(dāng)前關(guān)鍵幀之間的一系列位姿進(jìn)行全局優(yōu)化。最后，根據(jù)優(yōu)化后的位姿及每一幀位姿所攜帶的激光數(shù)據(jù)重新構(gòu)建地圖，消除地圖中無法閉合的部分。

4實(shí)驗(yàn)

    本文實(shí)驗(yàn)分為兩部分：第一部分在小范圍場景下進(jìn)行定點(diǎn)定位精度的對比實(shí)驗(yàn)，分別對傳統(tǒng)基于圖優(yōu)化的激光SLAM方法(即Karto)和本文提出的激光視覺相結(jié)合的方法進(jìn)行定位數(shù)據(jù)采集；第二部分為閉環(huán)實(shí)驗(yàn)，驗(yàn)證本文提出的方法是否可以有效地解決激光SLAM可能出現(xiàn)的地圖不閉合情況。

4.1實(shí)驗(yàn)平臺和環(huán)境

    本文實(shí)驗(yàn)在Turtlebot2上進(jìn)行，配備筆記本電腦以及激光雷達(dá)和深度攝像頭，電腦配置為IntelCorei5處理器、8G內(nèi)存，運(yùn)行ubuntu14.04＋ROSIndigo系統(tǒng)。二維激光雷達(dá)采用單線激光雷達(dá)RPLIDARA2，人為設(shè)置采樣頻率為5～15Hz，測量半徑為8m。深度相機(jī)采用奧比中光公司出品的Astra深度相機(jī)。其中，該相機(jī)深度測距的有效范圍為0.6～8m，精度達(dá)3mm；深度相機(jī)視角可以達(dá)到水平58°和垂直45.5°。

    本文實(shí)驗(yàn)在中國科學(xué)院深圳先進(jìn)技術(shù)研究院B棟辦公樓進(jìn)行，以機(jī)器人起始位置建立世界坐標(biāo)系，在B棟5樓B、C區(qū)電梯之間選擇5個標(biāo)志性位置，分別用Karto和本文提出的方法進(jìn)行位姿測量，實(shí)驗(yàn)過程如圖5所示。從0點(diǎn)出發(fā)，以0點(diǎn)為世界坐標(biāo)系原點(diǎn)，0到1方向?yàn)閤軸方向，2到3方向?yàn)椋瓂軸方向，依次沿1、2、3、4、5運(yùn)動，各點(diǎn)實(shí)際坐標(biāo)如表1所示。機(jī)器人實(shí)際位姿由時間和速度來控制。為保證視覺特征提取的穩(wěn)定性，機(jī)器人線速度保持勻速0.2m/s，角速度30(°)/s，即機(jī)器人只有0.2m/s勻速前進(jìn)、30(°)/s勻速右轉(zhuǎn)及停止三個狀態(tài)。其中，前進(jìn)15s為前進(jìn)3m；右轉(zhuǎn)3s為右轉(zhuǎn)90°。最終通過發(fā)布線速度和角速度指令并計時來控制機(jī)器人準(zhǔn)確到達(dá)5個標(biāo)志點(diǎn)。第二部分實(shí)驗(yàn)場景為中國科學(xué)院深圳先進(jìn)技術(shù)研究院B棟4樓B區(qū)工位及C區(qū)3樓。其中，4樓工位為小范圍閉環(huán)，C區(qū)3樓為大范圍回環(huán)。

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

    定位結(jié)果及誤差對比如表1所示。表1中數(shù)據(jù)說明僅依靠激光進(jìn)行SLAM時初始誤差較小，但隨著距離變遠(yuǎn)，測量值與實(shí)際位姿之間誤差逐漸增大。而實(shí)驗(yàn)過程中使用的激光成本較低，光束本身不夠密集，導(dǎo)致無法保證有足夠的可用數(shù)據(jù)過濾掉測量過程中測量值本身的誤差，從而產(chǎn)生累積誤差。本文提出的激光視覺聯(lián)合優(yōu)化位姿的方法雖然同樣存在累積誤差，但加入了視覺信息約束可有效地縮小累積誤差，可獲得更高的定位精度。圖6(a)～(d)分別是Karto[3]及本文提出的方法所構(gòu)建的柵格地圖；圖6(e)是ORB-SLAM2所構(gòu)建的C區(qū)3樓稀疏特征點(diǎn)地圖。其中，機(jī)器人位姿由一系列紅色箭頭表示。圖6(a)、(b)為工位環(huán)境的小范圍閉環(huán)，從中可以看到，圖6(a)白線圈處有不重合的現(xiàn)象，但由于場景較小，效果并不明顯。

    圖6(c)、6(d)為C區(qū)大范圍場景所構(gòu)建的地圖，由于場景較大，隨著時間推移產(chǎn)生的累積誤差會不斷增大。圖6(c)中可以明顯看到白線圈出的部分地圖并未重合，這是因?yàn)槔鄯e誤差影響了機(jī)器人的定位效果，而由于定位偏差的加大，根據(jù)機(jī)器人位姿及激光數(shù)據(jù)所構(gòu)建的地圖會出現(xiàn)不閉合的現(xiàn)象。相對而言，圖6(d)中機(jī)器人移動一圈回到起始位置時檢測到了閉環(huán)，根據(jù)起始位置計算出了當(dāng)前幀位姿，并將當(dāng)前幀位姿作為約束，進(jìn)行全局優(yōu)化，再通過優(yōu)化后的所有位姿及所攜帶的激光數(shù)據(jù)更新地圖信息，消除無法閉合的情況。實(shí)驗(yàn)結(jié)果表明，本文提出的方法可以有效地檢測回環(huán)，并優(yōu)化兩點(diǎn)之間的所有位姿，位姿優(yōu)化之后再根據(jù)每一幀位姿所攜帶的激光數(shù)據(jù)重新構(gòu)建地圖，成功消除了無法閉合的地圖部分。但由于加入了視覺信息，建圖過程中對機(jī)器人速度限制變大，同時對動態(tài)障礙物的抗干擾能力變小，只能在無人時進(jìn)行建圖，且需要避免攝像頭面向大面積白墻等無特征物體。

5.總結(jié)與展望

    本文研究了SLAM中多傳感器融合的問題，針對低成本激光雷達(dá)噪聲大、視覺構(gòu)建的稀疏特征地圖不適用于導(dǎo)航的問題，提出了激光視覺相結(jié)合定位建圖的方案。通過采用基于稀疏姿態(tài)調(diào)整的方法，將激光數(shù)據(jù)和圖像信息進(jìn)行聯(lián)合優(yōu)化，有效地提高了定位和建圖的精度，并通過視覺詞袋模型實(shí)現(xiàn)閉環(huán)檢測，解決了激光閉環(huán)檢測難實(shí)現(xiàn)的問題。但由于加入了視覺信息，系統(tǒng)魯棒性變差，且室內(nèi)環(huán)境光照變化往往較小，同時存在大量的動態(tài)障礙物，如攝像頭捕捉到移動的行人時會導(dǎo)致視覺約束的錯誤，從而影響整體的定位建圖效果。同時，當(dāng)遇到白墻等物體時無法提取足夠的特征，視覺約束失效從而導(dǎo)致建圖失敗。下一步工作將利用激光雷達(dá)和視覺各自的優(yōu)點(diǎn)，提高整體的魯棒性，保證建圖的效果。