劉明芹，張曉光 ，徐桂云，李宗周

1.中國礦業(yè)大學(xué) 機電學(xué)院，江蘇 徐州 221116

2.江蘇海洋大學(xué) 機械與海洋工程學(xué)院，江蘇 連云港 222001

1 引言

SLAM（Simultaneous Localization And Mapping）技術(shù)首次提出是用來估算對象之間位置和方向關(guān)系，以及估算與關(guān)系相關(guān)的不確定性。經(jīng)過國內(nèi)外眾多學(xué)者多年的研究，目前該技術(shù)被廣泛應(yīng)用于自主移動（無人機、機器人、無人駕駛汽車等）及AR 等領(lǐng)域[1-3]。SLAM技術(shù)在機器人領(lǐng)域應(yīng)用較廣泛，本文主要綜述其在機器人領(lǐng)域的應(yīng)用。

SLAM在機器人領(lǐng)域主要用來解決我在哪、我要去哪的問題。也就是解決機器人自身定位和對環(huán)境的認(rèn)知問題，進(jìn)而解決路徑規(guī)劃問題。

2 SLAM算法的主要發(fā)展歷程

2.1 基于距離傳感器的主流SLAM技術(shù)

早期SLAM 技術(shù)從提出開始，學(xué)者們在應(yīng)用中逐漸針對存在的缺點進(jìn)行改進(jìn)，使SLAM 技術(shù)在不斷地修正中持續(xù)發(fā)展?；诟怕式y(tǒng)計的擴展卡爾曼濾波器最早應(yīng)用于SLAM 上，由于EKF 擴展卡爾曼濾波器計算量大，計算速度慢，后續(xù)學(xué)者對EKF-SLAM提出改進(jìn)，Castellanos等[4]對一致性進(jìn)行改進(jìn)，2002年Montemerlo等針對EKF計算量大的缺點進(jìn)行改進(jìn)，提出Fast SLAM1.0[5]和 Fast SLAM2.0[6]，該 方 法 使 用 EKF 和 RBPF（Rao-Blackwellized Particle Filtering）聯(lián)合起來，估計位置和軌跡，使計算量大大減小，運算速度提升。

2.2 基于視覺傳感器的主流SLAM技術(shù)

隨著視覺傳感器和圖像傳輸、處理技術(shù)的提高，2010年以后，越來越多的SLAM系統(tǒng)采用視覺傳感器為主要輸入信號，從此SLAM 的研究主要集中在視覺SLAM技術(shù)上，即VSLAM（Visual SLAM）。VSLAM在計算機視覺、機器人和AR 領(lǐng)域被廣泛使用。在視覺SLAM技術(shù)上，國內(nèi)外眾多學(xué)者進(jìn)行研究。從現(xiàn)有研究成果中看，所使用的視覺傳感器主要有單目、雙目（雙目有普通雙目視覺和Kinect RGB-D 深度視覺兩種）和全景視覺傳感器三種。不同結(jié)構(gòu)的傳感器其相應(yīng)的后續(xù)處理方法也不同：如單目相機無法得到環(huán)境深度信息，所以研究的算法中要包含深度預(yù)測環(huán)節(jié)；而雙目視覺（普通雙目視覺和Kinect 深度視覺）可以由傳感器獲得深度信息，算法中就不需要包含深度預(yù)測環(huán)節(jié)。

本文將現(xiàn)有研究視覺SLAM 的技術(shù)按是否提取特征點然后通過匹配進(jìn)行運算，分為特征法和直接法。由于RGB-D 相機獲取環(huán)境的信息與普通視覺傳感器不同，所以它后續(xù)處理技術(shù)也不同，本文將基于RGB-D傳感器的SLAM單獨列出。

2.2.1 基于特征法的SLAM

早期VSLAM的研究主要基于單目相機視覺輸入，使用基于跟蹤和特征點建圖，這被稱為“基于特征的方法”。Harris 最早提出以角點為特征點，稱為Harris 角點，由于Harris 角點對亮度和對比度的變化魯棒性弱，且不具備尺度不變性，在角點檢測時清晰度不高。

利用特征點檢測器及描述符比較流行的方法主要有FAST、SIFT[7]、SURF[8]和ORB[9-10]。其中SIFT和SURF檢測器包含特征點方向，涉及梯度計算的直方圖，ORB特征點由FAST角點檢測器檢測，加入了方向和旋轉(zhuǎn)不變性特征描述，使特征檢測實時性和描述的魯棒性更強。

點特征是VSLAM算法的主流特征，也有基于線特征，但沒有被廣泛應(yīng)用。

基于特征點法一直是VSLAM的主流方法，解決方案比較成熟。但從眾多研究者的研究中發(fā)現(xiàn)，特征點法存在以下幾個缺點：

（1）關(guān)鍵點的提取與描述子的計算非常耗時。實踐中發(fā)現(xiàn)，SIFT在CPU上是無法實時計算的，ORB也需要近20 ms的計算。如果整個SLAM以每幀30 ms的速度運行，那么一大半時間都將花在計算特征點上。

（2）使用特征點時，忽略了除特征點以外的所有信息。一幅圖像有幾十萬個像素，而特征點只有幾百個。只使用特征點丟棄了大部分可能有用的圖像信息。

（3）相機有時會運動到特征缺失的地方，這些地方往往沒有明顯的紋理信息。例如有時會面對一堵白墻，或者一個空蕩蕩的走廊。這些場景下特征點數(shù)量會明顯減少，可能找不到足夠的匹配點來計算相機運動。

2.2.2 基于直接法的SLAM

為克服特征點法的上述缺點，提出既不計算關(guān)鍵點也不計算描述子，而是根據(jù)像素灰度的差異，直接計算相機運動的方法，稱為直接法（Direct Method）。Silveira[11]在2008 年最早提出了直接法的概念。在直接法中，并不需要知道點與點之間的對應(yīng)關(guān)系，而是通過最小化光度誤差（Photometric error）來計算。

在直接法中，完成相機位姿估計的視覺里程計，是保證算法準(zhǔn)確性和快速性的關(guān)鍵、是準(zhǔn)確建圖的基礎(chǔ)，也是算法的主要核心部分。眾多學(xué)者在視覺里程計上進(jìn)行研究，F(xiàn)orster 在 2014 年提出SVO（Fast Semi-direct monocular Visual Odometry）[12]，通過特征點匹配完成跟蹤，通過直接方法完成建圖。通過特征描述子和LK光流（Lucas-Kanade tracker）用于查找匹配點，通過最小化特征點周圍的光度誤差來估計相機運動。SVO 系統(tǒng)對稀疏的特征塊使用直接法配準(zhǔn)，獲取相機位姿，隨后根據(jù)灰度不變假設(shè)構(gòu)造優(yōu)化方程，對預(yù)測的特征位置進(jìn)行優(yōu)化，最后對位姿和結(jié)構(gòu)進(jìn)行優(yōu)化，由于SVO是開源的，被該領(lǐng)域?qū)W者廣泛采用。2016年Engel等提出DSO（Direct Sparse Odometry）直接稀疏里程計[13]，DSO 是一種完全直接的方法，它擬合了完全直接的概率模型（最小光度誤差）一致性，聯(lián)合優(yōu)化所有模型參數(shù)，包括幾何參數(shù)（作為參考幀中的逆深度）和攝像機運動。省略了其他直接方法中使用的前期平滑濾波，取而代之的是在整個圖像中均勻地采樣像素，因此其在實時性上非常成功。不依賴于關(guān)鍵點探測器或描述符，它可以自然地從所有區(qū)域采集像素，包括光度梯度變化的圖像區(qū)、邊緣，或是類似白色墻壁等那些光度平滑變化的區(qū)域。該算法模型集成了一個完整的光度校準(zhǔn)、計算曝光時間、鏡頭漸暈和非線性響應(yīng)函數(shù)。實驗表明，無論是在跟蹤精度上還是在魯棒性方面，該方法在各種現(xiàn)實環(huán)境中都優(yōu)于目前最先進(jìn)其他視覺里程計方法。

在2011 年，Newcombe[14]提出了基于密度追蹤的實時地圖創(chuàng)建算法（Dense Tracking And Mapping，DTAM），開啟了直接法在SLAM 中的嘗試。DTAM 采用逆深度方式表達(dá)深度，為每個像素都恢復(fù)稠密的深度圖，并且采用全局優(yōu)化，計算量大但能保證在特征缺失、圖像模糊等情況下穩(wěn)定地直接跟蹤和創(chuàng)建地圖，取得較好的效果。2014 年Engel 等人[15]提出了同樣基于直接法的LSD-SLAM，并于2015年擴展為支持雙目相機[16]和全景相機的版本[17]。該算法允許構(gòu)建大規(guī)模、一致的環(huán)境圖。除了基于直接圖像對齊的高精度姿勢估計之外，還可以實時重建3D環(huán)境為具有相關(guān)的半稠密深度圖的關(guān)鍵幀的姿勢圖。在sim(3)上運行具有尺度漂移意識的直接跟蹤方法，并可將深度值的噪聲影響納入跟蹤。與DTAM相比，LSD-SLAM僅恢復(fù)半稠密深度圖且每個像素深度獨立計算，因此能達(dá)到很高的計算效率。

2.2.3 基于RGB-D的SLAM

在2011 年Newcombe 等人最早提出基于深度的VSLAM Kinect Fution[18]。Kinect Fusion在GPU上運行以實現(xiàn)實時處理。Kahler 等人[19]在建圖過程中使用體素塊散列在移動設(shè)備上實現(xiàn)Kinect Fusion的實時處理。

為了獲得幾何一致的映射，在RGB-D VSLAM 算法中也使用位姿圖優(yōu)化和形變圖優(yōu)化。Kerl 等人[20]使用位姿圖優(yōu)化來減少累積誤差，這種位姿圖優(yōu)化幾乎與單目VSLAM 算法中的回環(huán)相同。Whelan 等人[21]分別使用位姿圖優(yōu)化來改進(jìn)相機運動估計和形變圖優(yōu)化來改進(jìn)地圖，估計的地圖得到了改進(jìn)。形變圖優(yōu)化通過RGB-D圖像和重建模型之間的匹配來估計相機運動。

基于結(jié)構(gòu)光的RGB-D相機，如Microsoft Kinect，變得便宜且小巧。由于這些相機能實時提供3D 信息，因此這些相機也用于VSLAM算法。應(yīng)該注意的是，大多數(shù)面向消費者的深度相機是為室內(nèi)使用而開發(fā)的。他們將紅外光（IR）投射到環(huán)境中以測量深度信息，在室外環(huán)境中難以檢測發(fā)射的紅外光。此外，深度測量范圍存在限制，使得RGB-D 傳感器只可以捕獲從1～4 m 的環(huán)境。RGB-D SLAM API 會提供給消費者成套設(shè)備，例如 Google Tango 和 Structure Sensor。特別是，Google Tango通過整合內(nèi)部傳感器信息給消費者提供穩(wěn)定的估算結(jié)果，使后續(xù)使用變得簡單方便。

3 基于距離傳感器的SLAM算法及研究內(nèi)容

3.1 基于距離傳感器的SLAM研究框架

根據(jù)國內(nèi)外學(xué)者的研究文獻(xiàn)可見，基于距離傳感器進(jìn)行SLAM的機器人，攜帶感知外部環(huán)境的距離傳感器常見的有激光雷達(dá)和聲吶測距、激光測距傳感器，另外少數(shù)用紅外、微波、Kinect等，用外部傳感器獲得環(huán)境中障礙物距離機器人的距離，根據(jù)距離信息對周圍環(huán)境特征進(jìn)行抽象描述。這些機器人同時也會攜帶如里程計、速度傳感器、加速度傳感器、陀螺儀等可以檢測機器人自身狀態(tài)的內(nèi)部傳感器。將內(nèi)部傳感器和外部傳感器的信息融合，基于SLAM 框架對機器人定位，并構(gòu)建增量式環(huán)境地圖，同時消除航跡誤差和位姿偏差。

基于概率的方法仍然是解決基于距離傳感器SLAM 問題的主要方法。其基本思想是將SLAM 過程視為一個Bayesian評估問題。

Xt表示0到t時刻機器人的狀態(tài)，Zt表示0到t時刻傳感器的觀測值，ut是0到t時刻給定機器人的控制量（或叫里程計數(shù)值）。

在SLAM 算法中，機器人的狀態(tài)Xt采用創(chuàng)建的地圖Xn(t)和機器人當(dāng)前的位姿Xr(t)聯(lián)合概率分布來表示，即p(Xr(t),Xn(t)|Zt,ut),Xn(t)是環(huán)境地圖，Zt,ut為機器人序列的觀測值和運動值。

由此，可以將 SLAM 問題分解成兩個相對獨立的問題：機器人定位問題和環(huán)境特征估計問題，或者是基于觀測確定位姿和基于機器人位姿對地圖更新問題。

3.2 SLAM算法及研究的具體內(nèi)容

基于距離傳感器SLAM在機器人定位問題上，主要采用濾波器EKF 和PF，在環(huán)境特征估計上，主要采用EKF等方法。

3.2.1 EKF-SLAM

在EKF-SLAM 中，系統(tǒng)的狀態(tài)變量包括機器人位姿和環(huán)境特征的位置，假設(shè)所有狀態(tài)變量均服從正態(tài)分布，狀態(tài)變量之間的不確定關(guān)系由狀態(tài)的協(xié)方差矩陣表示，而整個EKF-SLAM 的核心問題就是計算和更新協(xié)方差矩陣。圖1給出了EKF-SLAM的過程。

圖1 EKF-SLAM過程

EKF 算法的本質(zhì)就是在“最佳點”處對非線性模型進(jìn)行線性化。非線性模型的線性化是通過Taylor 展開完成的。通過對運動模型和觀測模型的線性化處理，得到 EKF的預(yù)測和更新的狀態(tài)估計過程：

在第K步預(yù)測下一步的狀態(tài)、協(xié)方差以及觀測值如下：

第k+1 步獲得觀測值Z(k+1)，利用觀測值進(jìn)行如下更新：

EKF-SLAM 的主要過程就是利用式（2）～（6）遞推估計由機器人位姿和路標(biāo)位置組成的狀態(tài)及相應(yīng)的協(xié)方差矩陣。

EKF-SLAM 的優(yōu)點是，小環(huán)境預(yù)測時計算量小，易于實現(xiàn)。缺點是概率密度函數(shù)滿足高斯分布，線性化處理誤差大，對錯誤數(shù)據(jù)關(guān)聯(lián)敏感，對大環(huán)境預(yù)測時計算量大。

3.2.2 PF-SLAM

基于RB粒子濾波器的PF-SLAM，在條件概率的基礎(chǔ)上，因式分解：

nt為地圖上路標(biāo)標(biāo)記集合，nt={n(1),n(2),…,n(k)}。

將機器人狀態(tài)估計分為運動軌跡估計和地圖估計兩部分。粒子濾波幾乎能夠適用于任意傳感器特性、動力學(xué)、噪音分布等，可以用于描述非線性、非高斯分布模型。采用通用的概率近似法，和以前的參數(shù)方法相比較，減小了在后驗概率密度形狀上的假設(shè)限制。通過控制采樣點的數(shù)目，粒子濾波能夠隨計算資源進(jìn)行調(diào)節(jié)。相同的代碼可以在計算機上按照不同的速度執(zhí)行不需要修改，能用于解決機器人“綁架”（Robot Kidnapping）問題。與EKF-SLAM 比降低了計算的復(fù)雜度，又具有較好的魯棒性。但其局限性是假定環(huán)境特征的觀測只與機器人當(dāng)前位置有關(guān)。

3.2.3 Fast SLAM

Fast SLAM中機器人的運動軌跡p(|Zt,ut,nt)的后驗概率用一個粒子濾波器來計算，每個粒子采用N個EKF 濾波器，來估計地圖中N個路標(biāo)位置p(Xn(t)|,Zt,ut,nt)的后驗概率分布。如果有M個粒子的話，將需要NM個EKF 濾波器，計算量比EKF-SLAM 方法少很多，且在每次采樣中估算機器人的運動軌跡時間是恒定的，這給機器人控制提供了方便。

基于粒子濾波的Fast SLAM 算法采用加權(quán)采樣的思想，算法框架如圖2 所示，主要由以下四個迭代步驟組成：首先，針對每個粒子，利用運動學(xué)模型預(yù)測p(Xr(t)|u(t),Xr(t-1))機器人新的位姿；然后，在每個粒子中，利用觀測信息基于 EKF 更新地圖；其次，利用前面假定的機器人運動軌跡和估計的概率分布，計算每個粒子的權(quán)重；最后，利用上一步的權(quán)重進(jìn)行加權(quán)采樣，得到無權(quán)重的新粒子集。

圖2 Fast SLAM框架

Fast SLAM 算法優(yōu)點：采用非線性模型，將高維運算降為低維運算，在計算復(fù)雜度、數(shù)據(jù)關(guān)聯(lián)等方面具有優(yōu)勢；缺點是樣本退化、樣本貧化嚴(yán)重。

另外，在基于距離傳感器的SLAM 中，還有應(yīng)用SVD（矩陣奇異值分解）的SLAM 方法。通過激光雷達(dá)獲得機器人與障礙物的方向、距離信息，利用SVD獲得路標(biāo)在全局坐標(biāo)系和局部坐標(biāo)系下的關(guān)系，完成地圖的更新。將SLAM 問題看成距離信息矩陣SVD 的過程，通過把距離矩陣SVD得到包含有路標(biāo)信息和機器人位姿的2 個矩陣[22]。該方法優(yōu)點是不需要線性化模型，能降維處理，具有計算量小、跟蹤性能好，避免了運動模型的偏差和“綁架”問題；缺點是依據(jù)實際情況不同會出現(xiàn)病態(tài)矩陣。

3.3 地圖的表達(dá)方式

基于距離傳感器的SLAM地圖表達(dá)方式，典型模式有基于特征的地圖和基于網(wǎng)格的地圖。

基于特征的地圖，表示機器人移動時觀測到的路標(biāo)和應(yīng)用路標(biāo)定位的概率，一般基于路標(biāo)定位的EKFSLAM建立該特征的地圖比較多。圖3是特征地圖。

圖3 特征地圖

網(wǎng)格地圖表示地圖是否被占用，或者反應(yīng)的被占用的相關(guān)概率。網(wǎng)格地圖比特征地圖更清楚反應(yīng)環(huán)境的實際特征，圖4是2D網(wǎng)格地圖。

圖4 2D網(wǎng)格地圖

3.4 基于距離傳感器SLAM方法性能比較分析

幾種比較典型的基于距離傳感器的SLAM 方法性能比較分析如表1所示。

基于距離傳感器的SLAM方法，是在建立模型的前提下使用概率估計和濾波的方法對機器人位置和運行軌跡估計，這個過程存在事先假定條件，如果假定條件出現(xiàn)偏差或不滿足，將會導(dǎo)致后續(xù)估計出現(xiàn)誤差。所以，基于距離傳感器的方法大多計算量大，使用過程中存在局限性，因此導(dǎo)致誤差較大?；诰嚯x傳感器建立的地圖是特征圖或網(wǎng)格圖，對環(huán)境表達(dá)不直觀，與人類對環(huán)境的理解有差距。

表1 基于距離傳感器SLAM方法性能比較

4 基于視覺的SLAM方法

視覺SLAM的處理過程由傳感器信息讀取、視覺里程計（也叫前端）、后端優(yōu)化（非線性優(yōu)化）、回環(huán)檢測和建圖五個過程組成。圖5是視覺SLAM的流程圖。

圖5 視覺SLAM流程圖

（1）傳感器信息讀取。在視覺SLAM中主要為相機圖像信息的讀取和預(yù)處理。如果是在機器人中，還可能有碼盤、慣性傳感器等信息的讀取和同步。

（2）視覺里程計（Visual Odometry，VO）。視覺里程計的任務(wù)是估算相鄰圖像間相機的運動，以及局部地圖的樣子，VO又稱為前端（Front End）。

（3）回環(huán)檢測（Loop Closing）。回環(huán)檢測判斷機器人是否到達(dá)過先前的位置。如果檢測到回環(huán)，它會把信息提供給后端進(jìn)行處理。

（4）后端優(yōu)化（Back End Optimization）。后端接受不同時刻視覺里程計測量的相機位姿，以及回環(huán)檢測的信息，對它們進(jìn)行優(yōu)化，得到全局一致的軌跡和地圖。由于接在VO之后，所以稱為后端（Back End）。

（5）建圖（Mapping）。它根據(jù)估計的軌跡，建立與任務(wù)要求對應(yīng)的地圖。

視覺SLAM在執(zhí)行上述五個過程中，主要依據(jù)關(guān)鍵幀技術(shù)，采用與關(guān)鍵幀的誤差最小的優(yōu)化方法尋找系統(tǒng)最優(yōu)的參數(shù)值。把目前文獻(xiàn)按在此過程中采用技術(shù)的特點，分為三大類：特征點SLAM（或間接法）、直接SLAM 和基于深度學(xué)習(xí)的SLAM。下面對以上三類中比較典型的幾種方法進(jìn)行分析。

4.1 特征點SLAM

基于特征點的視覺SLAM利用關(guān)鍵幀圖像，每當(dāng)系統(tǒng)獲取到一幀圖像，首先提取圖像中的特征點，計算每個特征點的描述子，然后通過與前一幀圖像中的特征點進(jìn)行特征匹配建立起特征點之間的一一對應(yīng)關(guān)系，最后利用匹配的特征點在不同的圖像中的投影結(jié)果最小化重投影誤差（Reprojection error）來估計相機的運動。特征點計算幀間相機位姿的原理如圖6 所示，Tk,k-1是k和k-1 時刻兩幀之間的位姿變換。

圖6 特征點計算幀間相機位姿的原理

2015 年，Mur-Artal 等提出了開源的單目ORBSLAM，并于2016年拓展為支持雙目和RGBD傳感器的ORB-SLAM2，它是目前支持傳感器最全且性能最好的視覺SLAM系統(tǒng)之一，也是所有在KITTI數(shù)據(jù)集上運行結(jié)果排名最靠前的一個。ORB-SLAM 延續(xù)了PTAM（Parallel Tracking And Mapping）的算法框架[23]，增加了單獨的回環(huán)檢測線程，采用詞袋（Bag of Words）法進(jìn)行回環(huán)檢測和重定位。

基于詞袋的閉環(huán)檢測由四個步驟組成，如圖7所示。

圖7 基于詞袋的閉環(huán)檢測流程

（1）數(shù)據(jù)庫查詢

當(dāng)獲取一個圖像It時，它將轉(zhuǎn)換為詞袋矢量vt。在數(shù)據(jù)庫中搜索vt，生成匹配候選對象＜vt,vt1＞，＜vt,vt2＞,…,＜vt,vtj＞ 及與其相關(guān)的s(vt,vtj)列表。

然后，用在向量vt的序列中期望獲得的最佳分?jǐn)?shù)對每次獲得的分?jǐn)?shù)進(jìn)行歸一化，從而獲得一個歸一化的相似性度η：

選擇一個較小的閾值α，對η(vt,vtj)＜α的匹配圖像不予選擇。

（2）匹配分組

為了防止在查詢數(shù)據(jù)庫時時間比較近的圖像之間發(fā)生競爭，將這樣的圖像分為一組，并將它們視為一個匹配項。使用符號Ti表示有一定間隔的連續(xù)時間組tn1,tn2,…,tnm和用VTi表示一個匹配組，其包含匹配項vn1,vn2,…,vnm。因此，如果tn1,tn2,…,tnm中連續(xù)時間之間的間隔很小，則將一組匹配項轉(zhuǎn)換為單個匹配項計算分值H：

選擇得分最高的組作為匹配項，然后進(jìn)行后續(xù)時間一致性檢測。

（3）時間一致性檢測

在獲得最佳匹配VT′之后，檢查它與前序查詢的時間一致性。

匹配＜vt,VT′＞必須與K個先前的匹配＜vt-Δt,VT1＞,＜vt-2Δt,VT2＞,…,＜vt-kΔt,VTk＞ 保持一致，使得Tj和Tj+1間隔接近交疊。如果通過了時間約束，只保留使得分η最大的匹配 ＜vt,vt′＞(t′∈T′)，認(rèn)為它是一個閉環(huán)候選，然后由幾何一致性檢查決定是否最終接受。

（4）幾何一致性檢查

在閉環(huán)候選的任何一對圖像之間應(yīng)用幾何檢查，用RANSAC查找It和It′之間的基本矩陣進(jìn)行驗證。

ORB SLAM建立了關(guān)鍵幀圖以及關(guān)鍵幀之間的可見關(guān)系圖。如果兩個關(guān)鍵幀之間共享相同地圖點（至少15個），則兩個關(guān)鍵幀之間存在一條邊，該邊的權(quán)重θ為公共地圖點數(shù)。每個節(jié)點都是一個關(guān)鍵幀，關(guān)鍵幀之間的可見性信息，表示為無方向的加權(quán)圖。關(guān)鍵幀、地圖點和可見性圖的表示見圖8[9]。

圖8 地圖表示形式（關(guān)鍵幀、地圖點、可見性圖和基礎(chǔ)圖）

在支持雙目和RGBD 傳感器的ORB-SLAM2[10]中，使用g2o 里的 LM（Levenberg-Marquardt）算法，優(yōu)化相機的運動參數(shù)R和t，從而使世界坐標(biāo)系中已匹配的3D點Xi∈R3和關(guān)鍵點之間的重投影誤差最小。

其中ρ是Huber代價函數(shù)，而Σ是與關(guān)鍵點規(guī)模（scale）相關(guān)的協(xié)方差矩陣。π(?)是投影函數(shù)。

BA（Bundle Adjustment，光束法平差）也是應(yīng)用比較廣泛的圖優(yōu)化方法，在ORB SLAM 中采用BA 結(jié)合g2o，在圖優(yōu)化中有效地閉合回路，執(zhí)行位姿圖優(yōu)化，該位姿圖沿圖分配了閉合誤差。在優(yōu)化之后，根據(jù)能觀察到該點的其中一個關(guān)鍵幀的校正來變換每個地圖點。

基于最小二乘的圖優(yōu)化法g2o，是機器人視覺SLAM 中較常用的圖優(yōu)化方法。它較其他圖優(yōu)化方法如性能全面，并且還可以輕松擴展解決其他新問題。

ORB-SLAM 對PTAM 框架中的大部分組件都做了改進(jìn)，歸納起來主要有以下幾點：（1）ORB-SLAM追蹤、建圖、重定位和回環(huán)檢測各個環(huán)節(jié)都使用了統(tǒng)一的ORB 特征，使得建立的地圖可以保存載入重復(fù)利用；（2）得益于可見圖（convisibility graph）的使用，將跟蹤和建圖操作集中在一個局部互見區(qū)域中，使其能夠不依賴于整體地圖的大小，能夠?qū)崿F(xiàn)大范圍場景的實時操作；（3）采用統(tǒng)一的BoW詞袋模型進(jìn)行重定位和閉環(huán)檢測，并且建立索引來提高檢測速度；（4）改進(jìn)了PTAM只能手工選擇從平面場景初始化的不足，提出基于模型選擇的新的自動魯棒的系統(tǒng)初始化策略，允許從平面或非平面場景可靠地自動初始化。

4.2 直接SLAM

在2011 年，Newcombe[14]提出了基于密度追蹤的實時地圖創(chuàng)建算法DTAM（Dense Tracking And Mapping），開啟了直接法在SLAM中的嘗試。

直接法與特征法不同，直接法不需要進(jìn)行特征的提取，而是通過最小化兩幀圖像之間的光度誤差（Photometric Error）來估計圖像之間的運動，如圖9 所示。直接法的思路源自圖像光流（Optical Flow）的計算，基本假設(shè)是圖像灰度一致性，即同一個空間點在各個圖像中的像素灰度值不變。因此，通過估計得到的位姿變換和深度值可以將圖像投影（Warp）到另一幀圖像的坐標(biāo)系下，計算此時投影的圖像與原圖差值即為兩幀圖像的光度誤差，通過最小化兩幀圖像之間的光度誤差來估計圖像之間的運動。

其中，D為當(dāng)前幀對應(yīng)的深度圖，T2,1為當(dāng)前幀和相鄰幀之間的位姿變換估計。

圖9 直接法計算幀間位姿示意圖

DTAM 對特征缺失、圖像模糊有很好的魯棒性，但由于DTAM為每個像素都恢復(fù)稠密的深度圖，并且采用全局優(yōu)化，因此計算量很大，即使采用GPU 加速，模型的擴展效率仍然較低。DTAM 為每個像素都恢復(fù)稠密的深度圖，如圖10所示。

2013年，TUM機器視覺組的Engel等人提出了一套同樣也是基于直接法的視覺里程計（Visual Odometry，VO）系統(tǒng)，該系統(tǒng)2014年擴展為視覺SLAM系統(tǒng)LSD-SLAM[15]。

Engel 等在文獻(xiàn)[15]中采用基于深度估計的半密集視覺里程計，通過高斯-牛頓最小化光度誤差來對齊兩個圖像：給出對ξ的最大似然估計量。

圖10 稠密的深度圖

從現(xiàn)有關(guān)鍵幀Ki=(Ii,Di,Vi)開始，通過最小化光度誤差的歸一化方差來計算新圖像Ij的相對3D 姿態(tài)ξij∈se(3)，即

其中：

其中，‖ ‖δ是Huber范數(shù)

用于歸一化殘差。

LSD SLAM 中采用sim（3）上直接圖像對齊，使用場景深度和跟蹤精度之間的固有相關(guān)性來解決在長距離跟蹤上，導(dǎo)致尺度漂移產(chǎn)生的誤差?？s放每個創(chuàng)建的關(guān)鍵幀的深度圖，以使其平均逆深度為1，建立最小化總誤差函數(shù)為：

在將新的關(guān)鍵幀Ki添加到地圖后，將收集許多可能的循環(huán)閉合關(guān)鍵幀Kj1,Kj2,…,Kjn，使用最接近的十個關(guān)鍵幀，檢測大規(guī)模的循環(huán)閉合。為了避免錯誤插入或錯誤跟蹤閉環(huán)，采用相互跟蹤檢查，即對于每個候選幀Kjk，獨立估計相對于關(guān)鍵幀Ki的攝像機位姿ξjki和ξijk，僅當(dāng)兩個估算值在統(tǒng)計上相似時，即如果

足夠小時，才將它們添加到全局地圖中，作為閉環(huán)候選。

LSD SLAM 中由位姿圖描述相機位置及移動軌跡?？赏ㄟ^位姿圖優(yōu)化（pose graph optimization）來對閉環(huán)進(jìn)行優(yōu)化。如圖11 所示[15]藍(lán)色標(biāo)記是一個個相機位姿，一般由錐形四面體表示，頂點是所采集圖像時相機在世界坐標(biāo)系中位置，四邊形底面表示此時相機的視平面，相機位姿之間連線用綠色表示。

圖11 描述相機的位姿圖

LSD-SLAM 中是由一組關(guān)鍵幀和跟蹤的sim（3）約束組成的地圖，使用最小化的誤差函數(shù)對其進(jìn)行圖優(yōu)化，最小化的誤差函數(shù)為（W定義為世界坐標(biāo)）：

ξji是關(guān)鍵幀i和j的相對位姿。

LSD-SLAM 隨機從位姿圖中選擇一個具有兩個以上相鄰關(guān)鍵幀的關(guān)鍵幀，并試圖將當(dāng)前幀與它進(jìn)行匹配，如果外點/內(nèi)點比率較大，那么丟棄該關(guān)鍵幀，重新隨機選擇；否則接著測試所有與它相鄰的關(guān)鍵幀，如果相鄰的關(guān)鍵幀中內(nèi)點/外點比率較大的關(guān)鍵幀數(shù)多于外點/內(nèi)點比率較大的關(guān)鍵幀數(shù)，或者存在多于五個的內(nèi)點/外點比率較大的關(guān)鍵幀，那么選擇內(nèi)點/外點比率最大的關(guān)鍵幀進(jìn)行跟蹤，重定位成功。

該算法允許構(gòu)建大規(guī)模、一致的環(huán)境圖。除了基于直接圖像對齊的高精度姿勢估計之外，還可以實時重建3D 環(huán)境為具有相關(guān)的半稠密深度圖的關(guān)鍵幀的位姿圖。在sim（3）上運行具有尺度漂移意識的直接跟蹤方法，并可將深度值的噪聲影響納入跟蹤。與DTAM 相比，LSD-SLAM僅恢復(fù)半稠密深度圖且每個像素深度獨立計算，因此能達(dá)到很高的計算效率。

4.3 典型視覺SLAM方法性能比較分析

基于視覺SLAM的幾種典型方法工作機制、優(yōu)缺點和適用場合等比較，如表2所示。隨著計算機處理速度的提升，基于視覺傳感器的SLAM實時性和準(zhǔn)確性大大提高，目前已達(dá)到無人車城市道路實時應(yīng)用的水平。由于視覺傳感器采集信息豐富、使用方便、價格便宜，構(gòu)建的環(huán)境地圖接近人眼所見的真實環(huán)境，所以視覺SLAM將是未來的主流應(yīng)用技術(shù)。

4.4 基于深度學(xué)習(xí)的視覺SLAM

隨著計算機數(shù)據(jù)處理量的提升以及深度學(xué)習(xí)技術(shù)的發(fā)展，國內(nèi)外學(xué)者發(fā)現(xiàn)將深度學(xué)習(xí)技術(shù)應(yīng)用到SLAM中，將會給SLAM技術(shù)帶來巨大革命。目前在SLAM中可以利用深度學(xué)習(xí)技術(shù)閉環(huán)檢測、實時估計相機位姿、特征點檢測和描述、構(gòu)建語義地圖等。

4.4.1 深度學(xué)習(xí)在閉環(huán)檢測上的應(yīng)用

目前深度學(xué)習(xí)在閉環(huán)檢測上的應(yīng)用可分為兩大類，一類是利用深度學(xué)習(xí)模型提取圖像特征進(jìn)行閉環(huán)檢測；另一類是提取環(huán)境語義信息，利用語義節(jié)點匹配。

（1）利用深度學(xué)習(xí)模型提取圖像特征進(jìn)行閉環(huán)檢測

深度學(xué)習(xí)在閉環(huán)檢測上的應(yīng)用，主要是通過使用深度學(xué)習(xí)模型提取圖像特征，并使用相似度矩陣檢測閉環(huán)。經(jīng)過特征提取后得到的特征向量在經(jīng)過相似度計算后可以判定為擁有相同的圖像特征，來證明兩幅圖像擁有相似的內(nèi)容。

Chen等人[24]首次提出在閉環(huán)檢測上基于CNN的位置識別技術(shù)。Hou 等人[25]利用Caffe 框架下的AlexNet模型提取特征，Sunderhauf 等人[26]利用 Image Net 數(shù)據(jù)庫對基于Caffe框架的AlexNet模型進(jìn)行預(yù)訓(xùn)練，提高其在閉環(huán)檢測方面的優(yōu)勢，Li等人[27]提出了基于雅可比神經(jīng)網(wǎng)絡(luò)（fast Neural Network，NN）的Fast SLAM 算法來處理觀測誤差和SLAM線性化誤差，都取得了較好的效果。

表2 典型視覺SLAM方法性能比較

文獻(xiàn)[28]提出基于無監(jiān)督棧式卷積自編碼（CAEs）模型的特征提取方法，運用訓(xùn)練好的CAEs 卷積神經(jīng)網(wǎng)絡(luò)對輸入圖像進(jìn)行學(xué)習(xí)，將輸出的特征應(yīng)用于閉環(huán)檢測。與傳統(tǒng)的BoW 方法相比，能夠有效降低圖像特征的維數(shù)并改善特征描述的效果，可以在機器人SLAM閉環(huán)檢測環(huán)節(jié)獲得更好的精確性和魯棒性。

（2）構(gòu)建語義節(jié)點特征匹配

文獻(xiàn)[29]利用YOLOv3 目標(biāo)檢測算法獲取場景中的語義信息，以基于密度的帶有噪聲的稀疏聚類DBSCAN（Density-Based Spatial Clustering of Application with Noise）算法修正錯誤檢測和遺漏檢測，構(gòu)建語義節(jié)點，對關(guān)鍵幀形成局部語義拓?fù)鋱D。利用圖像特征和目標(biāo)類別信息進(jìn)行語義節(jié)點匹配，計算不同語義拓?fù)鋱D中對應(yīng)邊的變換關(guān)系，得到關(guān)鍵幀之間的相似度，并根據(jù)連續(xù)關(guān)鍵幀的相似度變化情況進(jìn)行閉環(huán)的判斷。目標(biāo)聚類有效地提高了室內(nèi)場景下的閉環(huán)檢測準(zhǔn)確性，與單純利用傳統(tǒng)視覺特征的算法相比，能夠獲得更加準(zhǔn)確的閉環(huán)檢測結(jié)果。

4.4.2 深度學(xué)習(xí)估計相機位姿

文獻(xiàn)[30]根據(jù)單目視覺傳感器采集的圖像，實時估計相機位姿，實現(xiàn)了單目相機實時六自由度重新定位。系統(tǒng)訓(xùn)練一個卷積神經(jīng)網(wǎng)絡(luò)，以端到端的方式從單個RGB 圖像中回歸六自由度相機的姿態(tài)，而無需其他工程或圖形優(yōu)化，該算法可以在室內(nèi)和室外實時運行。

4.4.3 深度學(xué)習(xí)應(yīng)用于特征點檢測全過程

文獻(xiàn)[31]采用深度網(wǎng)絡(luò)實現(xiàn)了完整的特征點檢測、方向估計和特征描述全部處理過程。以往的研究工作只能成功地單獨解決一個問題，該文獻(xiàn)展示了如何在保持端到端的差異性的同時以統(tǒng)一的方式完成這三個問題，與傳統(tǒng)方法比較取得了較好的效果。

隨著深度學(xué)習(xí)[32]、語義理解[33]和SLAM 技術(shù)的發(fā)展[34]，未來三者結(jié)合基于語義地圖指導(dǎo)機器人進(jìn)行定位和自主運行將是視覺SLAM的發(fā)展方向。

5 SLAM技術(shù)實現(xiàn)的難點和未來發(fā)展的方向

5.1 SLAM技術(shù)實現(xiàn)的難點

SLAM 是多學(xué)科多算法組合，它包含圖像處理技術(shù)、幾何學(xué)、圖理論、優(yōu)化和概率估計等學(xué)科的知識，需要扎實的矩陣、微積分、數(shù)值計算知識，SLAM與使用的傳感器和硬件平臺也有關(guān)系，研究者需要具備一定的硬件知識，了解所使用的傳感器的硬件特性。所以，根據(jù)不同的應(yīng)用場景，SLAM研究者和工程師必須處理從傳感器模型構(gòu)建到系統(tǒng)集成的各種實踐問題[35]。從前面章節(jié)的分析可以看出，SLAM的各個環(huán)節(jié)用到的技術(shù)是偏傳統(tǒng)的。與當(dāng)前大熱的深度學(xué)習(xí)“黑箱模型”不同，SLAM 的各個環(huán)節(jié)基本都是白箱，能夠解釋得非常清楚。但SLAM卻并不是上述各種算法的簡單疊加，而是一個系統(tǒng)工程。

比如SLAM需要平衡實時性和準(zhǔn)確性，SLAM一般是多線程并發(fā)執(zhí)行，資源的分配、讀寫的協(xié)調(diào)、地圖數(shù)據(jù)的管理、優(yōu)化和準(zhǔn)確性、關(guān)鍵參數(shù)和變量的不確定性以及高速高精度的姿態(tài)跟蹤等，都是需要解決的問題。

SLAM還需要考慮硬件的適配，SLAM的數(shù)據(jù)來源于傳感器，有時是多個傳感器融合，傳感器的質(zhì)量對SLAM的效果影響很大。例如，如果SLAM使用的相機圖像噪點非常多，那么就會對姿態(tài)跟蹤產(chǎn)生不好的影響，因為特征點提取會很不一致；再比如在VIO系統(tǒng)中，如果相機和IMU 的時間戳不一致（至少毫秒級），也會影響算法精度甚至算法失敗。多個傳感器的分別校準(zhǔn)和互相校準(zhǔn)，乃至整個系統(tǒng)眾多參數(shù)的調(diào)整，都是非常耗費時間的工程問題。

由于產(chǎn)品和硬件高度差異化，而SLAM相關(guān)技術(shù)的整合和優(yōu)化又很復(fù)雜，導(dǎo)致算法和軟件高度碎片化，所以市場上目前還沒有一套通用普適的解決方案。

5.2 視覺SLAM主要發(fā)展趨勢

SLAM 未來的發(fā)展趨勢有兩大類：一是朝輕量級、小型化方向發(fā)展，讓SLAM能夠在嵌入式或手機等小型設(shè)備上良好運行，然后考慮以它為底層功能的應(yīng)用，比如手機端的AR和無人機SLAM等。在這些應(yīng)用中，不希望SLAM占用所有計算資源，所以對SLAM的小型化和輕量化有非常強烈的需求。另一方面則是利用高性能計算設(shè)備，實現(xiàn)精密的三維重建、場景理解等功能。在這些應(yīng)用中，本文目的是完美地重建場景，而對于計算資源和設(shè)備的便攜性則沒有多大限制，由于可以利用GPU，這個方向和深度學(xué)習(xí)也有結(jié)合點。

5.3 SLAM技術(shù)未來的研究熱點

（1）多傳感器融合SLAM

相機能夠捕捉場景的豐富細(xì)節(jié)，而慣性測量單元（Inertial Measurement Unit，IMU）有高的幀率且相對小地能夠獲得準(zhǔn)確的短時間估計，這兩個傳感器能夠相互互補，從而一起使用能夠獲得更好的結(jié)果。

最初的視覺與IMU結(jié)合的位姿估計是用濾波方法解決的，用IMU的測量值作為預(yù)測值，視覺的測量值用于更新。

上述文獻(xiàn)分析已證明在準(zhǔn)確性上基于優(yōu)化的視覺SLAM優(yōu)于基于濾波的SLAM方法。

實際的機器人和硬件設(shè)備，通常都不會只攜帶一種傳感器，往往是多種傳感器的融合。比如機器人除了視覺傳感器，還通常具有激光雷達(dá)、里程計、IMU 等，手機除了攝像頭，也帶有IMU、磁力計等傳感器。融合多種傳感器的信息對于提高SLAM 系統(tǒng)的精度和魯棒性有著重要的意義。比如目前手機上的VIO的研究，它將視覺信息和IMU 信息融合，實現(xiàn)了兩種傳感器的優(yōu)勢互補，為SLAM的小型化與低成本化提供了非常有效解決方案，取得了良好的效果。

（2）語義SLAM

SLAM的另一個方向就是和深度學(xué)習(xí)技術(shù)結(jié)合[36-37]。到目前為止，SLAM的方案都處于特征點或者像素的層級。人們對于這些特征點或像素到底來自于什么東西一無所知。這使得計算機視覺中的SLAM 與人類的做法不怎么相似，至少人類自己從來看不到特征點，也不會去根據(jù)特征點判斷自身的運動方向?？吹降氖且粋€個物體，通過左右眼判斷它們的遠(yuǎn)近，然后基于它們在圖像當(dāng)中的運動推測相機的移動。SLAM 和語義的結(jié)合點主要有兩個方面：

一是語義幫助SLAM。如果有了物體識別的語義信息，就能得到一個帶有標(biāo)簽的地圖，物體信息可為回環(huán)檢測、BA優(yōu)化帶來更多的條件。

二是SLAM 幫助語義。物體識別和分割都需要大量的訓(xùn)練數(shù)據(jù)。要讓分類器識別各個角度的物體，需要從不同視角采集該物體的數(shù)據(jù)，然后進(jìn)行人工標(biāo)定，非常辛苦。而SLAM 中，由于可以估計相機的運動，可以自動地計算物體在圖像中的位置，節(jié)省人工標(biāo)定的成本。如果有自動生成的帶高質(zhì)量標(biāo)注的樣本數(shù)據(jù)，能夠很大程度上加速分類器的訓(xùn)練過程。

（3）SLAM算法與硬件的結(jié)合

此外，未來的SLAM技術(shù)將會越來越注重算法和硬件的深度結(jié)合，專用處理器（如HoloLens HPU）和一體化功能模組（如Tango模組）也是未來的發(fā)展方向，這將會大大降低現(xiàn)有硬件平臺的計算能力瓶頸和算法調(diào)試門檻，帶給用戶更流暢的體驗。

（4）SLAM算法與深度學(xué)習(xí)的結(jié)合

隨著深度學(xué)習(xí)在計算機視覺領(lǐng)域的大成功，大家對深度學(xué)習(xí)在機器人領(lǐng)域的應(yīng)用有很大的興趣。SLAM是一個大系統(tǒng)，里面有很多子模塊，例如閉環(huán)檢測、立體匹配等，都可通過深度學(xué)習(xí)的使用來獲得更優(yōu)的結(jié)果。

6 結(jié)論

本文從SLAM產(chǎn)生到目前最新的前沿主流技術(shù)，按機器人SLAM 技術(shù)常用的距離傳感器和視覺傳感器兩大類進(jìn)行綜述。分別對這兩類輸入信號對應(yīng)的SLAM主流技術(shù)，從技術(shù)框架、主要研究內(nèi)容、優(yōu)缺點及適應(yīng)的場景等性能做了分析，并對目前最新技術(shù)、未來的研究熱點做了介紹，預(yù)示了SLAM與深度學(xué)習(xí)和語義結(jié)合的未來發(fā)展方向。