SLAM技術(shù)的發(fā)展推動了定位、跟蹤以及路徑規(guī)劃技術(shù)的發(fā)展

SLAM：Simultaneous Localization and Mapping，即時定位與地圖構(gòu)建技術(shù)。它指的是：機器人從未知環(huán)境的未知地點出發(fā)，在運動過程中通過重復(fù)觀測到的環(huán)境特征定位自身位置和姿態(tài)，再根據(jù)自身位置構(gòu)建周圍環(huán)境的增量式地圖，從而達(dá)到同時定位和地圖構(gòu)建的目的。

一、SLAM概述

SLAM：Simultaneous Localization and Mapping，即時定位與地圖構(gòu)建技術(shù)。利用SLAM技術(shù)，可以讓機器人在未知的環(huán)境里實時地知道自己在哪里，并同步繪制環(huán)境地圖。在機器人的定位、跟蹤和路徑規(guī)劃技術(shù)中，SLAM都扮演了核心角色。沒有SLAM的機器人就好比在移動互聯(lián)網(wǎng)時代不能上網(wǎng)的智能手機，發(fā)揮不了重要作用。非要向不了解SLAM的人簡單地描述SLAM的一個應(yīng)用場景的話，掃地機器人應(yīng)該就是用來解釋的最佳神器。

從圖中我們可以看出，掃地機器人能將其所經(jīng)過的區(qū)域繪制成地圖，規(guī)劃清掃路徑，從而進(jìn)行清掃。這種即時定位與地圖構(gòu)建技術(shù)，就是我們所說的SLAM技術(shù)。要利用SLAM技術(shù)，首先需要機器人配置至少一個測距裝置，通過測距裝置獲取機器人周圍環(huán)境的信息。目前比較常見的測距裝置主要有激光測距、超聲波測距和圖像測距。

由表可見，使用激光測距方法進(jìn)行距離測量，可獲取較高的空間定位精度，且運算要求較低。這樣的特性使得激光測距可以很容易地應(yīng)用到SLAM中。為方便大家理解，接下來我們將以二維空間的激光定位技術(shù)來解釋SLAM技術(shù)應(yīng)用的具體過程。

SLAM技術(shù)的發(fā)展推動了定位（Localization）、跟蹤（Tracking）以及路徑規(guī)劃（Path Planning）技術(shù)的發(fā)展，進(jìn)而對無人機、無人駕駛、機器人等熱門研究領(lǐng)域產(chǎn)生重大影響。

我們來看看SLAM技術(shù)都在哪些領(lǐng)域發(fā)揮作用吧。

圖1 SLAM應(yīng)用領(lǐng)域

VR/AR 方面：輔助增強視覺效果。SLAM技術(shù)能夠構(gòu)建視覺效果更為真實的地圖，從而針對當(dāng)前視角渲染虛擬物體的疊加效果，使之更真實沒有違和感。VR/AR代表性產(chǎn)品中微軟Hololens、谷歌ProjectTango以及MagicLeap都應(yīng)用了SLAM作為視覺增強手段。

無人機領(lǐng)域及機器人定位導(dǎo)航領(lǐng)域：地圖建模。SLAM可以快速構(gòu)建局部3D地圖，并與地理信息系統(tǒng)（GIS）、視覺對象識別技術(shù)相結(jié)合，可以輔助無人機識別路障并自動避障規(guī)劃路徑，還可以輔助機器人執(zhí)行路徑規(guī)劃、自主探索、導(dǎo)航等任務(wù)。曾經(jīng)刷爆美國朋友圈的Hover camera無人機，正是應(yīng)用SLAM技術(shù)。國內(nèi)的科沃斯、塔米掃地機通過用SLAM算法結(jié)合激光雷達(dá)或者攝像頭的方法，讓掃地機可以高效繪制室內(nèi)地圖，智能分析和規(guī)劃掃地環(huán)境，從而成功讓自己步入了智能導(dǎo)航的陣列。

無人駕駛領(lǐng)域：視覺里程計。SLAM 技術(shù)可以提供視覺里程計功能，并與GPS等其他定位方式相融合，從而滿足無人駕駛精準(zhǔn)定位的需求。例如，應(yīng)用了基于激光雷達(dá)技術(shù)Lidar Slam的Google無人駕駛車以及牛津大學(xué)Mobile Robotics Group11年改裝的無人駕駛汽車野貓（Wildcat）均已成功路測。

二、SLAM框架

自從上世紀(jì)80年代SLAM概念的提出到現(xiàn)在，SLAM技術(shù)已經(jīng)走過了30多年的歷史。SLAM技術(shù)采用的硬件、算法也在“與時俱進(jìn)”，SLAM技術(shù)框架逐漸豐富。在硬件方面，SLAM系統(tǒng)使用的傳感器從早期的聲吶，逐漸演變到后來的2D/3D激光雷達(dá)，再到單目、雙目攝像機，RGBD、ToF等深度攝像機，以及與慣性測量單元（IMU）等傳感器的融合。在軟件算法方面，也從開始的基于濾波器的方法，如擴展卡爾曼濾波器（EKF）、粒子濾波器（PF）等，向基于優(yōu)化的方法轉(zhuǎn)變。

SLAM系統(tǒng)一般分為五個模塊，系統(tǒng)框架如圖2所示，包括：

傳感器數(shù)據(jù)：主要用于采集實際環(huán)境中的各類型原始數(shù)據(jù)。包括激光掃描數(shù)據(jù)、視頻圖像數(shù)據(jù)、點云數(shù)據(jù)等。

視覺里程計：主要用于不同時刻間移動目標(biāo)相對位置的估算。包括特征匹配、直接配準(zhǔn)等算法的應(yīng)用。

后端：主要用于優(yōu)化視覺里程計帶來的累計誤差。包括濾波器、圖優(yōu)化等算法應(yīng)用。

建圖：用于三維地圖構(gòu)建。

回環(huán)檢測：主要用于空間累積誤差消除。

圖2 SLAM系統(tǒng)框架

其工作流程大致為：

傳感器讀取數(shù)據(jù)后，視覺里程計估計兩個時刻的相對運動（Ego-motion），后端處理視覺里程計估計結(jié)果的累積誤差，建圖則根據(jù)前端與后端得到的運動軌跡來建立三維地圖，回環(huán)檢測考慮了同一場景不同時刻的圖像，提供了空間上約束來消除累積誤差。

三、SLAM技術(shù)的發(fā)展

依據(jù)傳感器的特點不同，SLAM技術(shù)也分為激光和視覺兩個技術(shù)路線，其特點如圖4所示。

激光雷達(dá)傳感器

激光雷達(dá)利用激光在同一空間參考系下獲取物體表面每個采樣點的空間坐標(biāo)，可得到一系列表達(dá)目標(biāo)空間分布和目標(biāo)表面特性的海量點的集合。由于激光雷達(dá)距離測量比較準(zhǔn)確，誤差模型簡單，在強光直射以外的環(huán)境中運行穩(wěn)定，因此點云的處理也比較容易。同時，點云信息本身包含直接的幾何關(guān)系，使得機器人的路徑規(guī)劃和導(dǎo)航變得直觀。激光SLAM理論研究也相對成熟，落地產(chǎn)品更豐富。

視覺傳感器

早期的視覺SLAM基于濾波理論，其非線性的誤差模型和巨大的計算量成為了它實用落地的障礙。近年來，隨著具有稀疏性的非線性優(yōu)化理論（Bundle Adjustment）以及相機技術(shù)、計算性能的進(jìn)步，實時運行的視覺SLAM已經(jīng)不再是夢想。

SLAM兩大類傳感器

SLAM兩大傳感器特點

激光雷達(dá)SLAM發(fā)展

基于激光雷達(dá)的SLAM（Lidar SLAM）采用2D或3D激光雷達(dá)（也叫單線或多線激光雷達(dá)）。在室內(nèi)機器人（如掃地機器人）上，一般使用2D激光雷達(dá)，在無人駕駛領(lǐng)域，一般使用3D激光雷達(dá)。

現(xiàn)任Udacity創(chuàng)始人CEO、前Google副總裁、谷歌無人車領(lǐng)導(dǎo)者Sebastian Thrun在他2005年的經(jīng)典著作《ProbabilisticRobotics》一書中詳細(xì)闡述了利用2D激光雷達(dá)基于概率方法進(jìn)行地圖構(gòu)建和定位的理論基礎(chǔ)，并闡述了基于RBPF（Rao-blackwellizedparticle filter）粒子濾波器的FastSLAM方法，成為后來2D激光雷達(dá)建圖的標(biāo)準(zhǔn)方法之一GMapping的基礎(chǔ)，該算法也被集成到機器人操作系統(tǒng)（Robot Operation System，ROS）中。2013年，對ROS中的幾種2DSLAM的算法HectorSLAM，KartoSLAM，CoreSLAM，LagoSLAM和GMapping做了比較評估。2016年，Google開源其激光雷達(dá)SLAM算法庫Cartographer，它與GMapping相比，能夠有效處理閉環(huán)，達(dá)到了較好的效果。

視覺SLAM發(fā)展

現(xiàn)在主流的視覺SLAM系統(tǒng)的特性主要體現(xiàn)在前端和后端，如圖5所示。

前端對應(yīng)視覺里程計，主要用于數(shù)據(jù)關(guān)聯(lián)，研究幀與幀之間變換關(guān)系，完成實時的位姿跟蹤，對輸入的圖像進(jìn)行處理，計算姿態(tài)變化。由于與后端還存在反饋，因此也檢測并處理閉環(huán)，當(dāng)有慣性測量單元（IMU）信息時，還可以參與融合計算。

后端主要對前端的輸出結(jié)果進(jìn)行優(yōu)化，利用濾波理論，如擴展卡爾曼濾波（EKF）、粒子濾波（PF）等優(yōu)化理論進(jìn)行樹或圖的優(yōu)化，得到最優(yōu)的位姿估計和地圖。

典型SLAM的前端和后端

2007年，第一個成功基于單目攝像頭的純視覺SLAM 系統(tǒng)MonoSLAM誕生。同年，Murray和Klein提出了實時SLAM系統(tǒng)PTAM（ParallelTracking and Mapping），PTAM實現(xiàn)了架構(gòu)創(chuàng)新，它將姿態(tài)跟蹤（Tracking）和建圖（Mapping）兩個線程分開并行進(jìn)行，這在當(dāng)時是一個創(chuàng)舉，第一次讓大家覺得對地圖的優(yōu)化可以整合到實時計算中。這種設(shè)計為后來的實時SLAM（如ORB-SLAM）所效仿，成為了現(xiàn)代SLAM系統(tǒng)的標(biāo)配。

PTAM系統(tǒng)與EKF-SLAM系統(tǒng)產(chǎn)生結(jié)果對比

2015年，Mur-Artal 等提出了開源的單目ORB-SLAM，并于2016年拓展為支持雙目和RGBD傳感器的ORB-SLAM2，它是目前支持傳感器最全且性能最好的視覺SLAM系統(tǒng)之一，也是所有在KITTI數(shù)據(jù)集上提交結(jié)果的開源系統(tǒng)中排名最靠前的一個。

如圖7所示，ORB-SLAM主要分為三個線程進(jìn)行，即跟蹤（Tracking）、建圖（LocalMapping）和閉環(huán)檢測（LoopClosing）。跟蹤部分是主要是從圖像中提取ORB特征，根據(jù)上一幀進(jìn)行姿態(tài)估計，或者進(jìn)行通過全局重定位初始化位姿，然后跟蹤已經(jīng)重建的局部地圖，優(yōu)化位姿，再根據(jù)一些規(guī)則確定新的關(guān)鍵幀。建圖部分主要完成局部地圖構(gòu)建。包括對關(guān)鍵幀的插入，驗證最近生成的地圖點并進(jìn)行篩選，然后生成新的地圖點，使用局部捆集調(diào)整（Local BA），最后再對插入的關(guān)鍵幀進(jìn)行篩選，去除多余的關(guān)鍵幀。閉環(huán)檢測部分主要分為兩個過程，分別是閉環(huán)探測和閉環(huán)校正。閉環(huán)檢測先使用二進(jìn)制詞典匹配檢測（WOB）進(jìn)行探測，然后通過Sim3算法計算相似變換。閉環(huán)校正，主要是閉環(huán)融合和Essential Graph的圖優(yōu)化。由于ORB-SLAM系統(tǒng)是基于特征點的SLAM系統(tǒng)，故其能夠?qū)崟r計算出相機的軌線，并生成場景的稀疏三維重建結(jié)果。

ORB-SLAM2的三線程結(jié)構(gòu)

2016年，TUM機器視覺組的Engel等人又提出了DSO系統(tǒng)，該系統(tǒng)是一種新的基于直接法和稀疏法的視覺里程計，它將最小化光度誤差模型和模型參數(shù)聯(lián)合優(yōu)化方法相結(jié)合。為了滿足實時性，不對圖像進(jìn)行光滑處理，而是對整個圖像均勻采樣（如圖8）。該系統(tǒng)在TUM monoVO、EuRoC MAV和ICL-NUIM三個數(shù)據(jù)集上進(jìn)行了測試，達(dá)到了很高的跟蹤精度和魯棒性。

DSO系統(tǒng)

2017年，香港科技大學(xué)的沈紹劼老師課題組提出了融合慣性測量單元（IMU）和視覺信息的VINS系統(tǒng)，同時開源手機和Linux兩個版本的代碼，這是首個直接開源手機平臺代碼的視覺IMU融合SLAM系統(tǒng)。這個系統(tǒng)可以運行在iOS設(shè)備上，為手機端的增強現(xiàn)實應(yīng)用提供精確的定位功能，同時該系統(tǒng)也在應(yīng)用在了無人機控制上，并取得了較好的效果。