1引言

同步定位與地圖構(gòu)建(SimultaneousLocalizationandMapping，SLAM)是機(jī)器人領(lǐng)域的技術(shù)熱點(diǎn)與難點(diǎn)，其解決的定位和建圖問(wèn)題被認(rèn)為是機(jī)器人實(shí)現(xiàn)自主導(dǎo)航的關(guān)鍵。該技術(shù)主要原理是通過(guò)機(jī)器人配備的多種傳感器來(lái)感知周圍環(huán)境，并計(jì)算出自己在當(dāng)前環(huán)境下的位置。SLAM被提出后，先后經(jīng)歷了多個(gè)研究階段。由于激光雷達(dá)精度高、范圍廣，早期的SLAM研究往往以激光雷達(dá)為主要的傳感器。另外，早期的SLAM采用擴(kuò)展卡爾曼濾波方法估計(jì)機(jī)器人的位姿[1]，但效果不好，對(duì)于某些強(qiáng)非線性系統(tǒng)，該方法會(huì)帶來(lái)更多的截?cái)嗾`差，從而導(dǎo)致無(wú)法精準(zhǔn)地實(shí)現(xiàn)定位與建圖。而后基于粒子濾波的SLAM逐漸成為主流，粒子采樣方法可有效避免非線性問(wèn)題，但同樣引出了隨著粒子數(shù)增多，計(jì)算量增大的問(wèn)題。一直到2007年，Grisetti等[2]提出了基于改進(jìn)粒子濾波的SLAM方法(Gmapping)，該方法通過(guò)改進(jìn)的建議分布及自適應(yīng)重采樣技術(shù)有效地提高了定位精度并降低了計(jì)算量，是激光SLAM的一個(gè)里程碑。Konolige等[3]在2010年提出了KartoSLAM，該方法采用圖優(yōu)化代替粒子濾波，并采用稀疏點(diǎn)調(diào)整來(lái)解決非線性優(yōu)化中矩陣直接求解難的問(wèn)題。Kohlbrecher等[4]在2011年提出了HectorSLAM，該方法不需要里程計(jì)信息，通過(guò)利用高斯牛頓方法來(lái)解決掃描匹配問(wèn)題，但對(duì)傳感器要求較高，需要高精度的激光雷達(dá)才能運(yùn)行。由谷歌于2016年提出的Cartographer[5]可以說(shuō)是最新的激光SLAM方案。該方法將獲得的每一幀激光數(shù)據(jù)，利用掃描匹配在最佳估計(jì)位置處插入子圖(Submap)中，且掃描匹配只與當(dāng)前子圖有關(guān)。在生成一個(gè)子圖后，會(huì)進(jìn)行一次局部的回環(huán)(LoopClose)，而在所有子圖完成后，利用分支定位和預(yù)先計(jì)算的網(wǎng)格進(jìn)行全局的回環(huán)。相比于Gmapping和Hector，該方案具有累積誤差較低，且不需要高成本設(shè)備的優(yōu)點(diǎn)。視覺(jué)SLAM由于傳感器成本低、圖像信息豐富逐漸成為了SLAM的研究熱點(diǎn)。但與激光SLAM相比，視覺(jué)SLAM更加復(fù)雜。Davison等[6]在2007年首次提出的MonoSLAM被認(rèn)為是眾多視覺(jué)SLAM的“發(fā)源地”。該方法以擴(kuò)展卡爾曼濾波為后端，追蹤前端稀疏的特征點(diǎn)；利用概率密度函數(shù)來(lái)表示不確定性，從觀測(cè)模型和遞歸的計(jì)算，最終獲得后驗(yàn)概率分布的均值和方差。Sim等[7]利用粒子濾波實(shí)現(xiàn)了視覺(jué)SLAM，該方法避開了線性化的問(wèn)題并且精度高，但需要使用大量的粒子，從而導(dǎo)致計(jì)算復(fù)雜度的提高。隨后，為了減少計(jì)算復(fù)雜度，提取關(guān)鍵幀變得極為重要，其中最具代表性的是2007年Klein和Murray提出的PTAM[8]。該方法提出了簡(jiǎn)單有效地提取關(guān)鍵幀的技術(shù)，并且該方法最為關(guān)鍵的兩點(diǎn)是：(1)實(shí)現(xiàn)了跟蹤與建圖的并行化，雖然跟蹤部分需要實(shí)時(shí)響應(yīng)圖像數(shù)據(jù)，但后端優(yōu)化卻不需要實(shí)時(shí)計(jì)算。即后端優(yōu)化可在后臺(tái)慢慢運(yùn)行，在需要的時(shí)候?qū)蓚€(gè)線程同步即可。這也是首次提出了區(qū)分前后端的概念，引領(lǐng)了之后眾多SLAM方法的架構(gòu)設(shè)計(jì)。(2)第一次使用非線性優(yōu)化，而不是傳統(tǒng)的濾波器。從PTAM提出后，視覺(jué)SLAM研究逐漸轉(zhuǎn)向了以非線性優(yōu)化為主導(dǎo)的后端。2016年Mur-Artal和Tardos[9]提出了現(xiàn)代SLAM中非常著名的第二代基于具有帶方向性的加速分段測(cè)試特征(FAST)關(guān)鍵點(diǎn)以及帶旋轉(zhuǎn)不變性的二進(jìn)制魯棒獨(dú)立基本特征(BRIEF)描述子的特征點(diǎn)(OrientedFASTandRotatedBRIEF，ORB)[10]的視覺(jué)SLAM系統(tǒng)ORB-SLAM2，是現(xiàn)代SLAM中做得十分完善并且易用的系統(tǒng)之一。該方法不僅支持單目、雙目、深度攝像頭三種模式，并且將定位、地圖創(chuàng)建、閉環(huán)分為三個(gè)線程，且都使用ORB特征。ORB-SLAM2的閉環(huán)檢測(cè)是一大亮點(diǎn)，該方法采用了詞袋模型，有效地防止了累積誤差，并且能在丟失之后迅速找回。但ORB-SLAM2的缺點(diǎn)也十分明顯，圖像特征提取與匹配以及后端的優(yōu)化都需要計(jì)算資源，在嵌入式平臺(tái)上實(shí)現(xiàn)實(shí)時(shí)運(yùn)行存在一定的困難，并且構(gòu)建的稀疏特征點(diǎn)地圖雖然可以很好地滿足定位，但無(wú)法提供導(dǎo)航、避障等功能。相比于提取圖像中的特征點(diǎn)，根據(jù)圖像像素灰度信息來(lái)計(jì)算相機(jī)運(yùn)動(dòng)的直接法從另一個(gè)方向?qū)崿F(xiàn)了定位和建圖。Stühmer等[11]提出的相機(jī)定位方法依賴圖像的每個(gè)像素點(diǎn)，即用稠密的圖像對(duì)準(zhǔn)來(lái)進(jìn)行自身定位，并構(gòu)建出稠密的三維地圖。Engel等[12]對(duì)當(dāng)前圖像構(gòu)建半稠密深度地圖，并使用稠密圖像配準(zhǔn)(DenseImageAlignment)法計(jì)算相機(jī)位姿。構(gòu)建半稠密地圖即估計(jì)圖像中梯度較大的所有像素的深度值，該深度值被表示為高斯分布，且當(dāng)新的圖像到來(lái)時(shí)，該深度值被更新。Engel等[13]提出了LSD-SLAM算法，其核心是將直接法應(yīng)用到半稠密的單目SLAM中，這在之前的直接法中很少見到。以往基于特征點(diǎn)只能構(gòu)建稀疏地圖，而稠密地圖又需要RGB-D這種可以提供深度信息的相機(jī)。Forster等[14]于2014年提出了半直接法單目視覺(jué)里程計(jì)(Semi-directMonocularVisualOdometry，SVO)，一種被稱為“稀疏直接法”的方法，該方法將特征點(diǎn)與直接法混合使用，跟蹤了一些關(guān)鍵點(diǎn)(如角點(diǎn)等)，然后按照直接法根據(jù)關(guān)鍵點(diǎn)周圍的信息估計(jì)相機(jī)運(yùn)動(dòng)及位置。相比于其他方案，SVO既不用消耗大量資源去計(jì)算描述子，也不必處理過(guò)多的像素信息，因此該方法可廣泛用于無(wú)人機(jī)、手持增強(qiáng)現(xiàn)實(shí)(AugmentedReality，AR)等設(shè)備上。Newcombe等[15]提出了Kinect融合的方法，該方法通過(guò)Kinect獲取的深度圖像對(duì)每幀圖像中的每個(gè)像素進(jìn)行最小化距離測(cè)量而獲得相機(jī)位姿，且融合所有深度圖像，從而獲得全局地圖信息。Gokhool等[16]使用圖像像素點(diǎn)的光度信息和幾何信息來(lái)構(gòu)造誤差函數(shù)，通過(guò)最小化誤差函數(shù)而獲得相機(jī)位姿，且地圖問(wèn)題被處理為位姿圖表示。Kerl等[17]提出了較好的直接RGB-DSLAM方法，該方法結(jié)合像素點(diǎn)的強(qiáng)度誤差與深度誤差作為誤差函數(shù)，通過(guò)最小化代價(jià)函數(shù)，從而求出最優(yōu)相機(jī)位姿，該過(guò)程由g2o實(shí)現(xiàn)，并提出了基于熵的關(guān)鍵幀提取及閉環(huán)檢測(cè)方法，從而大大降低了路徑的誤差。在多傳感器融合方面，目前有視覺(jué)傳感器、激光雷達(dá)、慣性測(cè)量單元(InertialMeasurementUnit，IMU)及超聲波傳感器等多種傳感器。目前，主要融合方向?yàn)榧す饫走_(dá)結(jié)合視覺(jué)傳感器及IMU結(jié)合視覺(jué)傳感器。Chen等[18]通過(guò)視覺(jué)傳感器結(jié)合IMU來(lái)進(jìn)行機(jī)器人的精準(zhǔn)位姿估計(jì)，并在機(jī)器人上垂直安裝二維激光雷達(dá)采集點(diǎn)云數(shù)據(jù)實(shí)現(xiàn)三維建圖。Houben等[19]采用三維激光數(shù)據(jù)用于微型無(wú)人機(jī)定位，針對(duì)結(jié)構(gòu)相似的環(huán)境激光定位會(huì)出現(xiàn)模糊的問(wèn)題，提出在環(huán)境中加入視覺(jué)標(biāo)記，提高定位精度，同時(shí)可用于微型無(wú)人機(jī)的重定位。王消為等[20]提出了一種雙目視覺(jué)信息和激光雷達(dá)數(shù)據(jù)融合的SLAM方法，該方法基于改進(jìn)的粒子濾波算法實(shí)現(xiàn)了在計(jì)算建議分布時(shí)觀測(cè)數(shù)據(jù)，其中同時(shí)包含視覺(jué)信息和激光雷達(dá)數(shù)據(jù)。相對(duì)于里程計(jì)運(yùn)動(dòng)模型作為建議分布，該方法有效地提高了定位和建圖的精度。張杰和周軍[21]提出了一種激光雷達(dá)與視覺(jué)結(jié)合的SLAM方法，其中激光地圖用于導(dǎo)航，視覺(jué)地圖用于復(fù)原目標(biāo)場(chǎng)景；并提出一種改進(jìn)的迭代最近點(diǎn)法(IterativeClosestPoint，ICP)用于實(shí)現(xiàn)更快的點(diǎn)云拼接，同時(shí)采用圖優(yōu)化的方法降低了累積誤差，保證地圖精度。Shi等[22]在小型無(wú)人機(jī)上，利用視覺(jué)里程計(jì)為二維激光的ICP提供初值，在實(shí)時(shí)性和精確度方

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

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

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

不斷地增大。圖優(yōu)化是SLAM后端優(yōu)化的熱門方法，通過(guò)構(gòu)造節(jié)點(diǎn)和約束邊清晰地展示了一系列位姿和觀測(cè)量的關(guān)系，再通過(guò)非線性優(yōu)化的手段求出最優(yōu)變量從而得到精準(zhǔn)的位姿估

計(jì)。該方法最早應(yīng)用于視覺(jué)SLAM中，起初由于圖像特征點(diǎn)數(shù)量過(guò)多，導(dǎo)致矩陣維數(shù)過(guò)大，求解困難，所以該方法一直未能成為主流方法。直到2011年海塞矩陣的稀疏性被發(fā)現(xiàn)，極大地提高了運(yùn)算速度，使得非線性優(yōu)化和圖優(yōu)化的思想成功應(yīng)用于視覺(jué)SLAM和激光SLAM中。而根據(jù)位姿估計(jì)和優(yōu)化的實(shí)時(shí)性要求不同，SLAM也被分為前端和后端兩部分，當(dāng)前主流的SLAM框架如圖1所示。

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

人觀測(cè)到的ORB特征點(diǎn)對(duì)應(yīng)的三維空間點(diǎn)。將所有數(shù)據(jù)放入圖的框架中，如圖2所示。

其中，X表示關(guān)鍵幀位姿；O表示觀測(cè)量，包含了特征點(diǎn)所對(duì)應(yīng)的三維空間點(diǎn)坐標(biāo)及二維激光數(shù)據(jù)。視覺(jué)誤差由重投影誤差(圖3)來(lái)表示，重投影誤差的計(jì)算需要給定相鄰幀所

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

圖3

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

重投影坐標(biāo)計(jì)算公式及過(guò)程如下：(1)通過(guò)世界坐標(biāo)系到相機(jī)坐標(biāo)系的變換關(guān)系(R,t)，計(jì)算出世界坐標(biāo)系下點(diǎn)P所對(duì)應(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個(gè)像素。因此，像素坐標(biāo)(us,vs)計(jì)算公式如下：

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

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

  (4)

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

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

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

3.1誤差函數(shù)

傳統(tǒng)視覺(jué)相鄰幀誤差函數(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)可寫成對(duì)應(yīng)的李代數(shù)形式，李代數(shù)變換公式為：

  （7）

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

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

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

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

  （10）

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

  （11）

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

  （12）

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

3.3閉環(huán)檢測(cè)

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

表1位姿估計(jì)對(duì)比

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

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

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

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

4.1實(shí)驗(yàn)平臺(tái)和環(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，測(cè)量半徑為8m。深度相機(jī)采用奧比中光公司出品的Astra深度相機(jī)。其中，該相機(jī)深度測(cè)距的有效范圍為0.6～8m，精度達(dá)3mm；深度相機(jī)視角可以達(dá)到水平58°和垂直45.5°。

本文實(shí)驗(yàn)在中國(guó)科學(xué)院深圳先進(jìn)技術(shù)研究院B棟辦公樓進(jìn)行，以機(jī)器人起始位置建立世界坐標(biāo)系，在B棟5樓B、C區(qū)電梯之間選擇5個(gè)標(biāo)志性位置，分別用Karto和本文提出的方法進(jìn)行位姿測(cè)量，實(shí)驗(yàn)過(guò)程如圖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)動(dòng)，各點(diǎn)實(shí)際坐標(biāo)如表1所示。機(jī)器人實(shí)際位姿由時(shí)間和速度來(lái)控制。為保證視覺(jué)特征提取的穩(wěn)定性，機(jī)器人線速度保持勻速0.2m/s，角速度30(°)/s，即機(jī)器人只有0.2m/s勻速前進(jìn)、30(°)/s勻速右轉(zhuǎn)及停止三個(gè)狀態(tài)。其中，前進(jìn)15s為前進(jìn)3m；右轉(zhuǎn)3s為右轉(zhuǎn)90°。最終通過(guò)發(fā)布線速度和角速度指令并計(jì)時(shí)來(lái)控制機(jī)器人準(zhǔn)確到達(dá)5個(gè)標(biāo)志點(diǎn)。第二部分實(shí)驗(yàn)場(chǎng)景為中國(guó)科學(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é)果及誤差對(duì)比如表1所示。表1中數(shù)據(jù)說(shuō)明僅依靠激光進(jìn)行SLAM時(shí)初始誤差較小，但隨著距離變遠(yuǎn)，測(cè)量值與實(shí)際位姿之間誤差逐漸增大。而實(shí)驗(yàn)過(guò)程中使用的激光成本較低，光束本身不夠密集，導(dǎo)致無(wú)法保證有足夠的可用數(shù)據(jù)過(guò)濾掉測(cè)量過(guò)程中測(cè)量值本身的誤差，從而產(chǎn)生累積誤差。本文提出的激光視覺(jué)聯(lián)合優(yōu)化位姿的方法雖然同樣存在累積誤差，但加入了視覺(jué)信息約束可有效地縮小累積誤差，可獲得更高的定位精度。圖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)象，但由于場(chǎng)景較小，效果并不明顯。

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

5.總結(jié)與展望

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

聲明：本文內(nèi)容由企業(yè)提供，如涉及版權(quán)、機(jī)密問(wèn)題，請(qǐng)及時(shí)聯(lián)系我們刪除（QQ: 2737591964），不便之處，敬請(qǐng)諒解！