來源：我想我思

SLAM(Simultaneous Localization and Mapping)同步定位與地圖構建,一種同時實現設備自身定位和環境地圖構建的技術。

原理是使用相機、激光雷達、慣性測量單元等傳感器，來收集環境信息，然后用算法將這些信息融合起來，以確定設備在未知環境中的位置，并構建一張環境地圖。

通俗解釋為：

用眼睛觀察周圍地標如建筑、大樹、花壇等，并記住他們的特征（特征提取）

在自己的腦海中，根據雙目獲得的信息，把特征地標在三維地圖中重建出來（三維重建）

當自己在行走時，不斷獲取新的特征地標，并且校正自己頭腦中的地圖模型（bundle adjustment or EKF）

根據自己前一段時間行走獲得的特征地標，確定自己的位置（trajectory）

當無意中走了很長一段路的時候，和腦海中的以往地標進行匹配，看一看是否走回了原路（loop-closure detection）。

SLAM包括以下幾個方面

傳感器：激光雷達、攝像頭、慣性測量單元（IMU）、里程計等。這些傳感器可以提供設備在運動過程中所需要的信息，如距離、角度、速度、方向等。

設備運動模型：運動模型用于估計設備的位姿，例如里程計模型、速度模型、加速度模型等。

視覺算法：視覺算法主要用于設備在環境中的感知和定位，如特征提取、特征匹配、圖像配準等。

濾波算法：濾波算法用于估計設備的位姿和地圖中不同位置的概率分布，如卡爾曼濾波器、粒子濾波器等。

優化算法：優化算法用于優化設備的位姿和地圖，如擴展信息濾波器、位姿圖優化等。

地圖表示：地圖表示方式可以有多種，如柵格地圖、拓撲地圖、點云地圖等。

常用的SLAM傳感器

激光雷達：激光雷達是一種通過發射激光束來探測目標并測量距離的傳感器。它可以提供高精度、高分辨率的距離和角度信息，被廣泛應用于SLAM中。

攝像頭：攝像頭可以通過拍攝環境中的圖像來獲取場景的信息，如特征點、輪廓等。在SLAM中，攝像頭可以用于實現視覺SLAM或者輔助其他傳感器進行定位。攝像頭種類有單目、雙目、單目結構光、雙目結構光、ToF。

慣性測量單元（IMU）：IMU可以測量機器人的加速度和角速度等信息，通過積分可以得到機器人的位姿信息。IMU通常與其他傳感器結合使用，用于實現多傳感器融合。

里程計：里程計是一種通過測量輪子旋轉的信息來推算機器人位移的傳感器。雖然里程計的精度較低，但在SLAM中仍然是一個非常重要的傳感器。

此外，還有其他一些傳感器，如GPS、磁力計等，可以用于SLAM中。不同的傳感器有不同的特點和適用場景，根據具體的應用需求選擇合適的傳感器非常重要。

常用的兩種地圖類型

覆蓋柵格地圖（Occupany Grid Map）

一種常見的地圖表示方式，主要用于描述環境中每個位置的占據狀態。在覆蓋柵格地圖中，環境被分成一個個網格單元，每個網格單元表示一個位置，并記錄該位置的占據狀態，一般用二進制值表示，例如0表示空閑，1表示占據。

覆蓋柵格地圖可以通過激光雷達等傳感器獲得環境中障礙物的位置信息，將障礙物所在的網格單元標記為占據狀態。同時，通過機器人的位姿信息，可以將機器人所在的網格單元標記為占據狀態，并在地圖中繪制出機器人的軌跡。

覆蓋柵格地圖的優點在于簡單、易于實現和可視化，可以用于快速構建地圖和進行導航。但是，由于覆蓋柵格地圖要求環境被劃分為網格單元，因此地圖的精度和分辨率可能會受到限制。同時，覆蓋柵格地圖還需要消耗大量的存儲空間和計算資源，因為需要維護大量的網格單元。

需要注意的是，覆蓋柵格地圖只記錄了占據狀態，并沒有記錄障礙物的形狀和大小等信息。如果需要更精確的地圖表示方式，可以使用其他地圖表示方法，如點云地圖、邊緣地圖等。

點云地圖（Point Cloud Map）

主要用于描述環境中物體的三維形狀和位置信息。在點云地圖中，環境被表示為一組三維點云，每個點表示一個位置，并記錄該位置的屬性信息，如顏色、方向量、反射率等。

點云地圖可以通過激光雷達、攝像頭等傳感器獲得環境中物體的位置和形狀信息，通過點云配準和濾波等算法，可以將多個點云融合成一個完整的地圖，用于機器人的定位、導航和避障等任務。

相對于覆蓋柵格地圖等二維地圖表示方式，點云地圖的優點在于可以提供更加真實的環境信息，可以描述物體的三維形狀和位置信息，同時也可以用于識別和分類物體。但是，點云地圖的缺點在于數據量較大，需要消耗大量的存儲空間和計算資源，同時對點云數據的處理和配準等算法也比較復雜。

需要注意的是，點云地圖的精度和分辨率取決于傳感器的精度和采樣率等因素，而點云地圖的質量對機器人的定位和導航等任務影響很大，因此需要對點云地圖進行精細的處理和優化。

SLAM工作原理

不同的SLAM算法，實現的具體細節會有所不同，但一般都包含前端和后端。

前端：

從傳感器中獲取原始數據，并將這些數據與已有地圖進行關聯，從而確定機器人軌跡的過程。

數據采集：通過傳感器獲取機器人周圍環境的數據，如激光點云數據、圖像數據等。

數據時空同步：將從不同傳感器或不同時間戳接收到的數據進行同步，以便后續配準。

特征提取：從采集的數據中提取用于建圖的特征點，如關鍵點、特征描述等。

數據融合：將不同傳感器獲取的數據融合起來，提高建圖的準確性和穩定性。

數據關聯：將當前幀的特征與之前的地圖，或者其他幀之間的特征進行匹配，以確定機器人的運動軌跡。

運動估計：通過數據關聯得到機器人的運動軌跡，可以是平移、旋轉等運動。

后端：

根據前端獲取的運動軌跡和地圖信息，對機器人的狀態、地圖和傳感器誤差等進行估計和優化的過程。

非線性優化：通過非線性最小二乘法等，對機器人姿態和地圖進行優化，使得機器人的位置和地圖更加準確。

回環檢測：識別機器人經過的相似位置，避免累積誤差的產生。可以有效降低機器人的定位誤差，提高SLAM算法的精度和魯棒性。

根據傳感器的不同，SLAM算法可以分為二維激光SLAM、三維激光SLAM，以及視覺SLAM。

二維激光SLAM常用的有Cartographer、Karto，三維激光SLAM較流行的是LIO-SAM和LOAM系列，視覺SLAM主流的方案為ORB-SLAM3、VINS-Fusion

激光SLAM

激光SLAM發展已相對較為成熟，它的研究內容主要包括：傳感器數據處理、前端配準方法、回環檢測、后端優化、圖優化、已知定位的建圖、3D激光與視覺融合、多傳感器融合。

光探測與測距（激光雷達）方法主要使用激光傳感器（或距離傳感器）。對比相機、ToF 和其他傳感器，激光可以使精確度大大提高，常用于自動駕駛汽車和無人機等高速移動運載設備的相關應用。激光傳感器的輸出值一般是二維 (x, y) 或三維 (x, y, z) 點云數據。激光傳感器點云提供了高精確度距離測度數據，特別適用于 SLAM 建圖。一般來說，首先通過點云匹配來連續估計移動。然后，使用計算得出的移動數據（移動距離）進行車輛定位。對于激光點云匹配，會使用迭代最近點 (ICP) 和正態分布變換 (NDT) 等配準算法。二維或三維點云地圖可以用柵格地圖或體素地圖表示。但就密度而言，點云不及圖像精細，因此并不總能提供充足的特征來進行匹配。例如，在障礙物較少的地方，將難以進行點云匹配，因此可能導致跟丟車輛。此外，點云匹配通常需要高處理能力，因此必須優化流程來提高速度。鑒于存在這些挑戰，自動駕駛汽車定位可能需要融合輪式測距、全球導航衛星系統 (GNSS) 和 IMU 數據等其他測量結果。倉儲機器人等應用場景通常采用二維激光雷達 SLAM，而三維激光雷達點云 SLAM 則可用于無人機和自動駕駛。

視覺SLAM

使用從相機和其他圖像傳感器采集的圖像。視覺 SLAM 可以使用普通相機（廣角、魚眼和球形相機）、復眼相機（立體相機和多相機）和 RGB-D 相機（深度相機和 ToF 相機）。視覺 SLAM 所需的相機價格相對低廉，因此實現成本較低。此外，相機可以提供大量信息，因此還可以用來檢測路標（即之前測量過的位置）。路標檢測還可以與基于圖的優化結合使用，這有助于靈活實現 SLAM。使用單個相機作為唯一傳感器的 vSLAM 稱為單目 SLAM，此時難以定義深度。這個問題可以通過以下方式解決：檢測待定位圖像中的 AR 標記、棋盤格或其他已知目標，或者將相機信息與其他傳感器信息融合，例如測量速度和方向等物理量的慣性測量單元 (IMU) 信息。vSLAM 相關的技術包括運動重建 (SfM)、視覺測距和捆綁調整。視覺 SLAM 算法可以大致分為兩類。稀疏方法：匹配圖像的特征點并使用 PTAM 和 ORB-SLAM 等算法。稠密方法：使用圖像的總體亮度以及 DTAM、LSD-SLAM、DSO 和 SVO 等算法。

視覺SLAM整體流程大致可以概括為五步：

（1）傳感器信息讀取

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

（2）視覺里程計 (Visual Odometry,VO)

視覺里程計的任務是估算相鄰圖像間相機的運動，以及局部地圖的樣子。VO又稱為前端(FrontEnd)。

（3）后端優化(Optimization）

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

（4）回環檢測(Loop Closing)

回環檢測判斷機器人是否到達過先前的位置。如果檢測到回環，它會把信息提供給后端進行處理。

（5）建圖(Mapping)

它根據估計的軌跡，建立與任務要求對應的地圖。

其整體流程圖大致如下

SLAM算法實現4要素

地圖表示，比如dense和sparse都是它的不同表達方式，這個需要根據實際場景需求去抉擇

信息感知，需要考慮如何全面的感知這個環境，RGBD攝像頭FOV通常比較小，但激光雷達比較大

數據關聯，不同的sensor的數據類型、時間戳、坐標系表達方式各有不同，需要統一處理

定位與構圖，就是指怎么實現位姿估計和建模，這里面涉及到很多數學問題，物理模型建立，狀態估計和優化

其他的還有回環檢測問題，探索問題（exploration），以及綁架問題（kidnapping）。

6大主流SLAM算法

Cartographer

由谷歌開發的一款基于激光雷達和RGB-D相機數據的SLAM算法。可以跨平臺使用，支持Lidar、IMU、Odemetry、GPS、Landmark等多種傳感器配置，被廣泛用于機器人導航、自動駕駛等領域。

Cartographer系統架構圖

Cartographer算法在前端完成占據柵格地圖的構建，得出激光雷達掃描幀的最佳位姿后，將掃描幀插入到子地圖Submap中，得到局部優化的子地圖并記錄位姿。

后端根據掃描幀間的位姿關系進行全局的地圖優化，并使用分支定界法加速求解，進而得出閉環掃描幀在全局地圖中的最佳位姿。

Karto

一種基于位姿圖優化的SLAM方法，使用了高度優化和非迭代的cholesky矩陣對系統進行解耦并求解。適用于各種室內環境，可以處理靜態和動態障礙物。

Karto系統架構圖

Karto使用圖論的標準形式表示地圖，其中每個節點代表了移動機器人運行軌跡上的一個位姿點，以及當前位姿下傳感器返回的感知信息。

節點之間的邊，代表了相鄰機器人位姿之間的位移矢量。對每一個新的位姿點定位，需要節點間匹配關系和邊帶來的約束，保持定位估計誤差的前后一致。

LIO-SAM

一種新型的激光慣性導航系統，結合了激光雷達和慣性測量單元數據，可以實現機器人的高精度定位和運動軌跡的建圖。

LIO-SAM系統架構圖

前端在傳統的LIDAR-SLAM基礎上，利用卡爾曼濾波和因子圖優化算法，將激光雷達和IMU數據融合，進一步提高機器人的定位精度和建圖效果。后端加入優化算法，使得機器人的定位精度和建圖精度都得到極大提升。

LOAM系列

一種成熟的基于激光雷達的SLAM算法，包括LOAM、LOAM-Velodyne、LOAM-LiDAR等。

其中LOAM使用3D激光雷達的數據來進行建圖和定位，利用點云數據的特征，如空間聚類、連續性約束等來估計位姿。

LOAM系統架構圖

LOAM-Velodyne則使用Velodyne激光雷達的3D點云數據，以實現更高精度的地圖重建和定位；LOAM-LiDAR采用LiDAR傳感器來進行建圖和定位，可以高效獲取目標物體的三維坐標信息，在一些機器人應用場景具有很大的優勢。

基于LOAM系統的地圖構建局部

ORB-SLAM3

當前最優秀的基于特征點的視覺SLAM系統之一，支持單目、雙目、RGB-D等多種相機模式，在特征提取、關鍵幀選取、地圖維護、位姿優化等方面進行了優化，并能建立短期、中期和長期的數據關聯，使得該系統兼具精度和魯棒性。

ORB-SLAM3系統架構圖

前端視覺里程計基于ORB特征，建立圖像幀之間特征點的數據關聯，以及圖像特征點和地圖點之間3D到2D的數據關聯，具有較好的魯棒性和提取效率，回環檢測和重定位也基于ORB特征實現。

VINS-Fusion

一種基于視覺慣性傳感器的視覺SLAM算法。它將視覺和慣性信息進行融合，來提高機器人在未知環境下的定位和導航能力。

VINS-Fusion系統架構圖

前端和后端之間通過一個狀態傳遞機制來交換數據。前端將IMU和圖像數據合并，生成一組狀態量，然后將其傳遞給后端進行融合和優化；后端將優化后的狀態量傳遞回前端，以便更新機器人的運動估計。

SLAM方案選擇

1. 二維SLAM就適合在二維平面上運動的機器人，如掃地機、配送機器人、迎賓機器人等。

具體來看，Cartographer的總體表現最為優秀，定位精度和建圖質量都很高，同時魯棒性也很強，能承受一定的噪聲和漂移干擾，可以應用于大范圍建圖；Karto適合建圖面積不是太大的低成本應用場景，可以在一定程度上滿足建圖需求，同時工程實現難度也不太高。

2. 三維SLAM則適用于機器人需要在三維空間中進行建模和定位的場景，比如無人機在室外飛行、機器人在戶外環境中進行探測等。

其中，LIO-SAM的魯棒性和實時性表現相對優秀，適用于建圖面積和硬件資源有限的小型機器人應用場景；在建圖面積和魯棒性方面比較弱的LOAM方案，適用于小而快速的機器人及小型場景的定位、建圖任務。

3.視覺SLAM通過攝像頭等可視傳感器來獲取機器人與環境之間的相對位置關系，不需要其他傳感器，更為輕量化。但對于光照變化、運動模糊等情況的處理仍存在挑戰。

定位精度和實時性都比較高的ORB-SLAM3，面對場景變化能夠快速適應，適用于多種場景的建圖。與之相比，VINS-Fusion適用于對定位精度要求較高，且建圖面積相對較小的機器人應用場景。

總的來說，這些算法各有優劣，如何選擇最合適的算法，需要根據具體的應用場景和實際需求來綜合考慮。

SLAM應用場景

隨著計算機處理速度顯著提升，且相機和激光測距儀等低成本傳感器大為普及，SLAM技術廣泛應用于機器人、AR、VR、無人機、自動駕駛等領域。

使用SLAM的掃地機器人可以使用滾輪轉數以及來自相機和其他成像傳感器的數據來確定移動位置。機器人還可以同步使用相機和其他傳感器創建其周圍障礙物的地圖，避免同一區域清潔兩次。即實現了定位和構圖，從而保證了機器人可以按軌跡運動，同時也降低了設備功耗。

SLAM 還用于許多其他應用場景：

利用一個小攝像頭實現VR頭顯空間定位；

讓一隊移動機器人在倉庫中移動并整理貨架；

讓自動駕駛汽車停泊到空車位；

讓無人機在未知環境中完成送貨、無人機空中陣列表演等等。

SLAM面臨的挑戰

雖然 SLAM 已在某些場景下投入實際應用，但是仍面臨諸多技術挑戰

（1）定位誤差累積，導致與實際值產生偏差

SLAM會估計連續移動，并容許一定的誤差。但是誤差會隨著時間累積，導致與實際值產生明顯偏差。誤差還會導致地圖數據瓦解或失真，讓后續搜索變得困難。我們來看一個繞正方形通道行駛的例子。隨著誤差累積，機器人的起點和終點對不上了。這稱為閉環問題。這類位姿估計誤差不可避免。我們必須設法檢測到閉環，并確定如何修正或抵消累積的誤差。對策之一是記住之前到過的某處的某些特征，將其作為路標，從而最小化定位誤差。構建位姿圖有助于修正誤差。將誤差最小化問題視為優化問題進行求解，以生成更準確的地圖數據。這種優化在視覺 SLAM 中稱為捆綁調整。

（2）定位失敗，地圖上的位置丟失

圖像和點云建圖不考慮機器人的移動特征。在某些情況下，這種方法會生成不連續的位置估計。例如，可能會有計算結果顯示，以 1 米/秒速度移動的機器人突然向前“瞬移”了 10 米。避免這種定位失敗的辦法有兩種：一是使用恢復算法；二是將運動模型與多個傳感器融合，以基于傳感器數據計算。有多種方法可以實現運動模型的傳感器融合。一種常見方法是使用卡爾曼濾波進行定位。由于大部分差速驅動機器人和四輪車輛一般都使用非線性運動模型，因此通常會使用擴展卡爾曼濾波器和粒子濾波器（蒙特卡羅定位）。某些情況下，也可以使用無跡卡爾曼濾波器等更加靈活的貝葉斯濾波器。一些常用傳感器是慣性測量裝置，例如慣性測量單元 (IMU)、航姿參考系統 (AHRS)、慣性導航系統 (INS)、加速度計傳感器、陀螺儀傳感器和磁力傳感器。安裝到車輛的輪式編碼器通常用于測距。定位失敗時，一種恢復對策是記住之前經過的某個位置的關鍵幀，將其作為路標。搜索路標時，會以特定方法進行特征提取以便高速掃描。有些方法基于圖像特征，例如特征袋 (BoF) 和視覺詞袋 (BoVW)。近年來，人們也使用深度學習來比較特征距離。

（3）圖像處理、點云處理和優化帶來高計算成本

在車輛硬件上實現 SLAM 時，計算成本是個問題。計算通常在處理能力有限的緊湊型低功耗嵌入式微處理器上執行。為了實現準確定位，必須高頻率執行圖像處理和點云匹配。此外，閉環等優化計算都是高成本計算流程。此處的挑戰在于如何在嵌入式微處理器上執行這種高成本處理。對策之一是并行運行多個不同流程。例如，用于匹配流程前處理的特征提取就相對適合并行運行。使用多核 CPU 進行處理時，單指令多數據 (SIMD) 計算和嵌入式 GPU 在某些情況下可以進一步提升速度。而且，由于位姿圖優化可以在相對長的周期里執行，降低其優先級并以規律間隔執行也能提高性能。

審核編輯：湯梓紅