前文講述自動駕駛的環(huán)境感知和實時定位時提到了SLAM，今天就具體學習一下SLAM的基本原理和技術方向。

SLAM是Simultaneous Localization and Mapping的縮寫，意為“同時定位與建圖 （同步定位與地圖構建）”，是業(yè)界公認視覺領域空間定位技術的前沿方向。它是指運動物體自身位置不確定的條件下，根據傳感器的信息，一邊計算自身位置，一邊構建環(huán)境地圖的過程,同時利用地圖進行自主定位和導航。它主要用于解決機器人在未知環(huán)境運動時，如何通過對環(huán)境的觀測確定自身的定位和運動軌跡，同時構建出周圍環(huán)境的地圖。



機器人從一個未知的位置出發(fā), 在一個分布著人造或者自然路標(或稱為特征) 的未知環(huán)境中運動。機器人根據己知的運動學模型或者里程計信息推定自己的位置(Dead Reckoning),同時利用自身安裝的傳感器測量其附近的路標相對于自己的位置,同時建造增量式地圖。SLAM 技術的目標就是在實現(xiàn)機器人全局定位的同時, 根據觀測到的環(huán)境路標遞增地建立機器人的導航地圖。



目前，SLAM的應用領域主要有機器人、汽車自動駕駛、虛擬現(xiàn)實和增強現(xiàn)實。其用途包括傳感器自身的定位，以及后續(xù)的路徑規(guī)劃、場景理解。

那么為什么SLAM技術這么重要呢？無論是一個機器人也好，還是無人駕駛汽車也好，要在一個未知環(huán)境中，實現(xiàn)智能化行動，其中最核心，最根本的問題有四個，定位技術（Localization）, 第二個是建圖(Mapping)， 第三個是導航（Navigation）包括路徑規(guī)劃技術（Path Planning）和跟蹤技術（Tracking）, 第四個就是控制執(zhí)行技術（Controlling）。 而這四個問題中的前三個，SLAM都扮演了最核心的功能。 其意義，打個比方，就像移動互聯(lián)網時代中，WIFI和3/4G數(shù)據傳輸技術對手機的意義，是一樣的。扮演一個最核心，也是最不可或缺的角色。

一個成熟可用的SLAM系統(tǒng)， 必須包括四個基本方面：
1. 如何進行環(huán)境描述，即環(huán)境地圖的表示方法；比如dense和sparse都是它的不同表達方式，這個需要根據實際場景需求去抉擇。
2. 怎樣獲得環(huán)境信息，機器人在環(huán)境中漫游并記錄傳感器的感知數(shù)據，這涉及到機器人的定位與環(huán)境特征提取問題；需要考慮如何全面的感知這個環(huán)境，RGBD攝像頭FOV通常比較小，但激光雷達比較大。
3. 怎樣表示獲得的環(huán)境信息，并根據環(huán)境信息更新地圖，這需要解決對不確定信息的描述和處理方法；不同的sensor的數(shù)據類型、時間戳、坐標系表達方式各有不同，需要統(tǒng)一處理。
4. 發(fā)展穩(wěn)定、可靠的SLAM方法。這里面涉及到很多數(shù)學問題，物理模型建立，狀態(tài)估計和優(yōu)化。

針對環(huán)境的描述（地圖），目前各國研究者已經提出了多種表示法，大致可分為三類：柵格表示、幾何信息表示和拓撲圖表示，每種方法都有自己的優(yōu)缺點。

柵格地圖表示法即將整個環(huán)境分為若干相同大小的柵格，對于每個柵格，各自指出其中是否存在障礙物。這種方法最早由Elfes和Moravec提出，而后Elfes進行了進一步的研究。優(yōu)點是：(1)創(chuàng)建和維護容易， (2) 盡量保留了整個環(huán)境的各種信息， (3)借助于該地圖，可以方便地進行自定位和路徑規(guī)劃。而缺點在于：當柵格數(shù)量增大時(在大規(guī)模環(huán)境或對環(huán)境劃分比較詳細時)，對地圖的維護行為將變得困難，同時定位過程中搜索空間很大，如果沒有較好的簡化算法，實現(xiàn)實時應用比較困難。

幾何信息地圖表示法是指機器人收集對環(huán)境的感知信息，從中提取更為抽象的幾何特征，例如線段或曲線，使用這些幾何信息描述環(huán)境。該表示法更為緊湊，且便于位置估計和目標識別。幾何方法利用卡爾曼濾波在局部區(qū)域內可獲得較高精度，且計算量小，但在廣域環(huán)境中卻難以維持精確的坐標信息。幾何信息的提取需要對感知信息作額外處理，且需要一定數(shù)量的感知數(shù)據才能得到結果。

拓撲地圖抽象度高，特別在環(huán)境大而簡單時。這種方法將環(huán)境表示為一張拓撲意義中的圖(Graph)，圖中的節(jié)點對應于環(huán)境中的一個特征狀態(tài)、地點。如果節(jié)點間存在直接連接的路徑則相當于圖中連接節(jié)點的弧。優(yōu)點是：(1)有利于進一步的路徑和任務規(guī)劃，(2)存儲和搜索空間都比較小，計算效率高，(3)可以使用很多現(xiàn)有成熟、高效的搜索和推理算法。缺點在于對拓撲圖的使用是建立在對拓撲節(jié)點的識別匹配基礎上的，如當環(huán)境中存在兩個很相似的地方時，拓撲圖方法將很難確定這是否為同一點。

SLAM中定位與環(huán)境特征提?。阂苿訖C器人自定位與環(huán)境建模問題是緊密相關的。環(huán)境模型的準確性依賴于定位精度，而定位的實現(xiàn)又離不開環(huán)境模型。在未知環(huán)境中，機器人沒有什么參照物，只能依靠自己并不十分準確的傳感器來獲取外界信息，如同一個盲人在一個陌生環(huán)境中摸索的情況。這種情況下，定位是比較困難的。有地圖的定位和有定位的地圖創(chuàng)建都是容易解決的，但無地圖的定位和未解決定位的地圖創(chuàng)建如同"雞--蛋"問題，無從下手。已有的研究中對這類問題的解決方法可分為兩類：一類利用自身攜帶的多種內部傳感器(包括里程儀、羅盤、加速度計等)，通過多種傳感信息的融合減少定位的誤差，使用的融合算法多為基于卡爾曼濾波的方法。

這類方法由于沒有參考外部信息，在長時間的漫游后誤差的積累會比較大。另一類方法在依靠內部傳感器估計自身運動的同時，使用外部傳感器(如激光測距儀、視覺等)感知環(huán)境，對獲得的信息進行分析提取環(huán)境特征并保存，在下一步通過對環(huán)境特征的比較對自身位置進行校正。但這種方法依賴于能夠取得環(huán)境特征。

環(huán)境特征提取的方法有：
Hough transform，是一類基于灰度圖探察直線和其他曲線的方法。該方法需要一簇能被搜索的預準備的特定曲線，并根據顯示的灰度圖中一簇曲線產生曲線參數(shù)。

Clustering分析是一種數(shù)據探測工具，對于未分類樣例是有效的，同時，它的目標就是把所針對對象分組成自然類別或基于相似性或距離的簇類。在被提取對象類別未知的情況中，簇技術是一類比Hough Transform更有效的技術。簇類應是以"凝聚"為中心，而不是支離破碎的、不相交的。而環(huán)境特征有時是很難提取出的，例如：
·環(huán)境特征不夠明顯時；
·傳感器信息比較少，難以從一次感知信息中獲得環(huán)境特征。
在未知環(huán)境中，環(huán)境信息的不確定性尤為明顯。對于不確定信息的描述和處理方法，研究人員已經提出了多種用來處理不確定性的度量方法，如模糊度量、概率度量、信任度量、可能性度量、證據理論度量等等。目前在SLAM中使用較多的主要是模糊度量和概率度量的方法。

以概率描述信息的不確定性，其優(yōu)點在于：適應不確定模型，對于性能差的傳感器也適用，在實際應用中魯棒性較好，在很多實現(xiàn)中得到應用。缺點是計算量較大，算法建立在一定假設條件上，而假設的合理性有爭議。因此，有些學者認為使用模糊度的描述方法更適合于該任務。他們認為模糊邏輯(fuzzy logic)提供了一個關于處理不確定信息更自然的框架，是一個更為魯棒且有效的工具(尤其對于聲納感知過程引起的不確定性)。同時，在對多種不確定性信息進行建模和融合時有更多的操作符供選擇。



SLAM研究自1988年提出以來，已經過了近三十年。早期SLAM研究側重于使用濾波器理論。目前SLAM方法大致可分為兩類：一類為基于概率模型的方法，另一類為非概率模型方法。許多基于卡爾曼濾波的SLAM方法如完全SLAM、壓縮濾波、Fast SLAM就屬于概率模型方法。非概率模型方法有SM-SLAM、掃描匹配、數(shù)據融合(data association)、基于模糊邏輯等。
下圖是一個典型的卡爾曼濾波的SLAM方法的框圖：



狀態(tài)估計是卡爾曼濾波的重要組成部分。一般來說，根據觀測數(shù)據對隨機量進行定量推斷就是估計問題，特別是對動態(tài)行為的狀態(tài)估計，它能實現(xiàn)實時運行狀態(tài)的估計和預測功能。比如對飛行器狀態(tài)估計。狀態(tài)估計對于了解和控制一個系統(tǒng)具有重要意義，所應用的方法屬于統(tǒng)計學中的估計理論。最常用的是最小二乘估計，線性最小方差估計、最小方差估計、遞推最小二乘估計等。其他如風險準則的貝葉斯估計、最大似然估計、隨機逼近等方法也都有應用。

受噪聲干擾的狀態(tài)量是個隨機量，不可能測得精確值，但可對它進行一系列觀測，并依據一組觀測值，按某種統(tǒng)計觀點對它進行估計。使估計值盡可能準確地接近真實值，這就是最優(yōu)估計。真實值與估計值之差稱為估計誤差。若估計值的數(shù)學期望與真實值相等，這種估計稱為無偏估計?？柭岢龅倪f推最優(yōu)估計理論，采用狀態(tài)空間描述法，在算法采用遞推形式，卡爾曼濾波能處理多維和非平穩(wěn)的隨機過程。

卡爾曼濾波理論的提出，克服了威納濾波理論的局限性使其在工程上得到了廣泛的應用，尤其在控制、制導、導航、通訊等現(xiàn)代工程方面。



SLAM 技術涵蓋的范圍非常廣，按照不同的傳感器、應用場景、核心算法，SLAM 有很多種分類方法。如果按照傳感器的不同，則可以分為基于激光雷達的 2D/3D SLAM、基于深度相機的 RGBD SLAM、基于視覺傳感器的 visual SLAM（以下簡稱 vSLAM）、基于視覺傳感器和慣性單元的 visual inertial odometry（以下簡稱 VIO）。

基于激光雷達的 2D SLAM 相對成熟，早在 2005 年，Sebastian Thrun 等人的經典著作《概率機器人學》將 2D SLAM 研究和總結得非常透徹，基本確定了激光雷達 SLAM 的框架。目前常用的 Grid Mapping 方法也已經有 10 余年的歷史。2016 年，Google 開源了激光雷達 SLAM 程序 Cartographer，可以融合 IMU 信息，統(tǒng)一處理 2D 與 3D SLAM 。目前 2D SLAM 已經成功地應用于掃地機器人中。
下圖是2006 年使用激光雷達生成的某實驗室地圖：



基于深度相機的 RGBD SLAM 過去幾年也發(fā)展迅速。自微軟的 Kinect 推出以來，掀起了一波 RGBD SLAM 的研究熱潮，短短幾年時間內相繼出現(xiàn)了幾種重要算法，例如 Kinect Fusion、Kintinuous、Voxel Hashing、Dynamic Fusion 等。微軟的 Hololens 應該集成了 RGBD SLAM，在深度傳感器可以工作的場合，它可以達到非常好的效果。

視覺傳感器包括單目相機、雙目相機、魚眼相機等。由于視覺傳感器價格便宜，在室內室外均可以使用，因此 vSLAM 是研究的一大熱點。早期的 vSLAM 如 monoSLAM 更多的是延續(xù)機器人領域的濾波方法?，F(xiàn)在使用更多的是計算機視覺領域的優(yōu)化方法，具體來說，是運動恢復結構（structure-from-motion）中的光束法平差（bundle adjustment）。在 vSLAM 中，按照視覺特征的提取方式，又可以分為特征法、直接法。當前 vSLAM 的代表算法有 ORB-SLAM、SVO、DSO 等。

視覺傳感器對于無紋理的區(qū)域是沒有辦法工作的。慣性測量單元（IMU）通過內置的陀螺儀和加速度計可以測量角速度和加速度，進而推算相機的姿態(tài)，不過推算的姿態(tài)存在累計誤差。視覺傳感器和 IMU 存在很大的互補性，因此將二者測量信息進行融合的 VIO 也是一個研究熱點。按照信息融合方式的不同，VIO 又可以分為基于濾波的方法、基于優(yōu)化的方法。VIO 的代表算法有 EKF、MSCKF、preintegration、OKVIS 等。Google 的 Tango 平板就實現(xiàn)了效果不錯 VIO。

激光雷達優(yōu)點是可視范圍廣，但是缺點性價比低，低成本的雷達角分辨率不夠高，影響到建模精度。vSLAM的話，缺點就是FOV通常不大，只有50-60 degree，這樣高速旋轉時就容易丟，解決方案就是，做vSLAM跟雷達還有IMU的組合。總的來說，相比于基于激光雷達和基于深度相機的 SLAM，基于視覺傳感器的 vSLAM 和 VIO 還不夠成熟，操作比較難，通常需要融合其他傳感器或者在一些受控的環(huán)境中使用。

我們通過分析傳感器的測量信息做個定性的分析。激光雷達或者 RGBD 相機可以直接獲取環(huán)境的點云。對于點云中的一個點，它告訴我們在某個方位和距離上存在一個障礙點。而視覺傳感器獲取的是灰度圖像或者彩色圖像。對于圖像中的一個像素，它只能告訴我們在某個方位有障礙點、障礙點周圍的表觀（local appearance）如何，但它不能告訴我們這個障礙點的距離。要想計算該點的距離，需要把相機挪動一個位置再對它觀察一次，然后按照三角測量的原理進行推算。

原理上很清晰，實際做起來并不簡單。首先需要在兩幅圖像中尋找點的對應，這涉及到特征點的提取和匹配、或者準稠密點之間的匹配。計算機視覺發(fā)展到今天，其實還不存在性能和速度上很好滿足 vSLAM 的特征提取和匹配算法。常見的特征點提取算法，性能上大致可以認為SIFT>SURF>ORB>FAST，效率上可以認為FAST>ORB>SURF>SIFT（大于號左邊代表更優(yōu)。性能主要包括匹配精度、特征點的數(shù)量和空間分布等）。為了在性能和效率上取得折中，通常采用 FAST 或者 ORB，只能舍棄性能更好的 SIFT、SURF 等。

其次，匹配點的圖像坐標與空間坐標之間的關系是非線性的，例如 2D-2D 點的對應滿足對極幾何、2D-3D 點的對應滿足 PnP 約束。這些匹配數(shù)量較多，前后兩幀圖像中一般有幾十至數(shù)百的匹配。這些匹配會引入眾多約束關系，使得待估計變量的關系錯綜復雜。為了得到一個較優(yōu)的估計，通常需要建立優(yōu)化問題，整體優(yōu)化多個變量。說起來這無非是一個非線性最小二乘優(yōu)化問題，但實現(xiàn)起來并不簡單，因為存在非線性約束、約束數(shù)量很多、存在誤差和野值點，并且要將計算時間控制在允許范圍。目前廣泛采用關鍵幀技術，并且通過很多方法來控制問題規(guī)模、保持問題的稀疏性等。

非線性優(yōu)化問題可以用下圖來形象闡述。圓餅代表待優(yōu)化的變量（相機姿態(tài)、特征點的空間坐標），桿子代表約束（對線幾何、PnP 等）。



前面分析了 vSLAM 的兩個困難。前者導致了前端的特征跟蹤不易，后者導致了后端的優(yōu)化不易。想做出一個高效率、魯棒的 vSLAM 系統(tǒng)還是一個非常有挑戰(zhàn)的任務。效率方面，SLAM 必須是實時運行的。如果不能做到實時，就不能稱作 SLAM。不考慮實時性，采用從運動恢復結構（structure-from-motion）效果會更好。魯棒性方面，一個脆弱的系統(tǒng)會導致用戶體驗很差，功能有限。
下圖是使用structure-from-motion 對玲瓏塔進行三維重建的示例圖：



vSLAM 的核心算法在預備階段，主要包括傳感器的選型和各種標定。Visual SLAM 自 PTAM 算法以來，框架基本趨于固定。通常包括 3 個線程，前端 tracking 線程、后端 mapping 優(yōu)化線程、閉環(huán)檢測（loop closure）線程。前端 tracking 線程主要涉及到：特征的提取、特征的匹配；多視圖幾何的知識，包括對極幾何、PnP、剛體運動、李代數(shù)等。而后端優(yōu)化線程則涉及到非線性最小二乘優(yōu)化，屬于數(shù)值優(yōu)化的內容。閉環(huán)檢測線程涉及到地點識別，本質上是圖像檢索問題。對于 VIO，還涉及到濾波算法、狀態(tài)估計等內容。

將 SLAM 算法拆解了看，用到的技術是偏傳統(tǒng)的。與當前大熱的深度學習「黑箱模型」不同，SLAM 的各個環(huán)節(jié)基本都是白箱，能夠解釋得非常清楚。但 SLAM 算法并不是上述各種算法的簡單疊加，而是一個系統(tǒng)工程，里面有很多 tradeoff。如果僅僅跑跑開源程序，沒有什么核心競爭力。不論是做產品還是做學術研究，都應當熟悉各種技術，才能有所創(chuàng)造。

VSLAM 的發(fā)展目前來看，感覺是中規(guī)中矩，各個環(huán)節(jié)在前人的基礎上一點點優(yōu)化，同時不斷吸收其他方向的最新成果。短期內肯定會在現(xiàn)有框架下不停地改進。至于長遠一些的趨勢，IEEE TRO 2016 有一篇綜述文章 Past, present,and future of SLAM: Towards the robust-perception age。幾位有聲望的學者在文中對 SLAM 的趨勢做了非常好的總結。

新型傳感器的出現(xiàn)會不停地為 SLAM 注入活力。如果我們能夠直接獲取高質量的原始信息，SLAM 的運算壓力就可以減輕很多。舉例來說，近幾年在 SLAM 中逐漸有使用低功耗、高幀率的 event camera（又稱 dynamic vision system, DVS）。如果這類傳感器的成本能降下來，會給 SLAM 的技術格局帶來許多變化。

自從深度學習在諸多領域所向披靡，不少研究者試圖用深度學習中 end-to-end 的思想重構 SLAM 的流程。目前有些工作試圖把 SLAM 的某些環(huán)節(jié)用深度學習代替。不過這些方法沒有體現(xiàn)出壓倒性優(yōu)勢，傳統(tǒng)的幾何方法依然是主流。在深度學習的熱潮之下，SLAM 涉及的各個環(huán)節(jié)應該會逐漸吸收深度學習的成果，精度和魯棒性也會因此提升。也許將來 SLAM 的某些環(huán)節(jié)會整體被深度學習取代，形成一個新的框架。

SLAM 原本只關注環(huán)境的幾何信息，未來跟語義信息應該有更多的結合。借助于深度學習技術，當前的物體檢測、語義分割的技術發(fā)展很快，可以從圖像中可以獲得豐富的語義信息。這些語義信息是可以輔助推斷幾何信息的，例如已知物體的尺寸就是一個重要的幾何線索。

因為計算機視覺技術的快速發(fā)展，攝像頭和激光雷達成本的降低，SLAM正在AR、機器人、無人機、無人駕駛等領域火速入侵，用夸張點的說法，就像移動互聯(lián)網時代的手機地圖定位一樣，只要是跟位置相關的應用，誰拋棄了這張地圖，誰將拋棄了自己的未來。
1. 無人駕駛汽車
因為Google無人駕駛車的科普，很多人都知道了基于激光雷達技術的Lidar Slam。Lidar Slam是指利用激光雷達作為外部傳感器，獲取地圖數(shù)據，使機器人實現(xiàn)同步定位與地圖構建。雖然成本高昂，但目前為止是最穩(wěn)定、最可靠、高性能的SLAM方式。



自動（無人）駕駛一般用組合定位，首先本體感受傳感器如里程計（Odometry）、陀螺儀(Gyroscopes)等，通過給定初始位姿，來測量相對于機器人初始位姿的距離和方向來確定當前機器人的位姿，也叫做航跡推測。然后用激光雷達或視覺感知環(huán)境，用主動或被動標識、地圖匹配、GPS、或導航信標進行定位。位置的計算方法包括有三角測量法、三邊測量法和模型匹配算法等。從這個角度而言，IMU也是自動駕駛必備的部件。

2. 室內機器人
掃地機要算機器人里最早用到SLAM技術這一批了，國內的科沃斯、塔米掃地機通過用SLAM算法結合激光雷達或者攝像頭的方法，讓掃地機可以高效繪制室內地圖，智能分析和規(guī)劃掃地環(huán)境，從而成功讓自己步入了智能導航的陣列。



3. AR/VR
目前基于SLAM技術開發(fā)的代表性產品有微軟的Hololens，谷歌的Project Tango以及同樣有名的Magic Leap，很多VR應用需要用到SLAM技術，定位只是一個feature，路徑記錄、3D重構、地圖構建都可以是SLAM技術的輸出。根據 SLAM 得到地圖和當前視角對疊加虛擬物體做相應渲染，這樣做可以使得疊加的虛擬物體看起來比較真實，沒有違和感。

4. 無人機
國內大家非常熟悉的大疆精靈四避障用的雙目視覺+超聲波。P4里面呈現(xiàn)的主動避障功能就是一種非常非常典型的Slam的弱應用，可以使用 SLAM 構建局部地圖，輔助無人機進行自主避障、規(guī)劃路徑。無人機只需要知道障礙物在哪，就可以進行Planning，并且繞開障礙物。當然Slam能做的事情遠遠不止這些，包括災區(qū)救援，包括探洞，包括人機配合甚至集群，所有的關于無人機的夢想都建立在Slam之上，這是無人機能飛（具有定位，姿態(tài)確定以后）的時代以后，無人機最核心的技術。