作者：龐巧遇
來源：公眾號@3D視覺工坊
鏈接：同時定位與地圖創建綜述
SLAM包含兩個主要任務，定位和建圖。這是移動機器人自主完成作業任務需要解決的基本問題，特別是在未知環境的情況下，移動機器人既要確定自身在環境中的位姿，又要根據確定的位姿來創建所處環境的地圖，這是一個相輔相成、不斷迭代的過程。因此，SLAM問題是一個復雜的耦合問題，也可以被看作是先有雞還是雞蛋的問題[1]。SLAM問題的概念最初是在1986年在加利福尼亞州舊金山舉行的IEEE機器人和自動化會議上提出來的[2-3]。1987年，Prter Chesseman首次提出使用EKF(擴展卡爾曼濾波器)來估計機器人的位置和姿態[4]。1995年，Hugh Durrant-Whyte 等人在國際機器人研討會上對 SLAM 問題的理論框架進行闡述，驗證了 SLAM 問題的收斂性[5]。1986-2004年，由於概率方法的廣泛應用，SLAM 問題的研究得到了快速的發展，這一時期也被稱作SLAM問題的“經典時期”(classic age)，主要研究方法包括擴展卡爾曼濾波(Extended Kalman Filters, EKF)、粒子濾波(Rao-Blackwellized)和最大似然估計等[6]。這些方法遇到的最大瓶頸是計算的復雜度，受限於當時的計算水平，難以滿足構建大規模地圖的要求。2004-2015年，SLAM 問題的研究進入“算法分析時期”(algorithm-analysis age)，Dissanayake等人從狀態的可觀測性、狀態估計的收斂性、一致性和算法計算效率等角度對這一時期的部分工作做了綜述[7]，很好的描述了SLAM 基本特性研究的進展。同時期，Cesar等人對SLAM 的研究做了系統性的綜述，包括算法的魯棒性、應用的可擴展性、地圖表示形式（度量地圖和語義地圖）等問題，並且分別針對這些方面的研究提出了一些有待解決的問題[8]。2015年-至今，是“魯棒性-預測性時代”(Robust-Predictive age)，主要研究魯棒性、高級別的場景理解，計算資源優化，任務驅動的環境感知。本文的組織結構如下:第一節將闡述激光SLAM，包括激光傳感器、開源激光SLAM系統以及激光SLAM未來的挑戰。第二節重點介紹了視覺SLAM，包括攝像機傳感器、開源視覺SLAM系統、視覺SLAM未來的發展以及深度學習在視覺SLAM中的研究進展。第三節將展示激光和視覺融合的SLAM。最后，本文討論了SLAM未來的研究方向，以及對SLAM的發展作出展望。
1 激光SLAM
激光SLAM 所需要的傳感器一般有激光雷達（Lidar）、慣性測量單元（IMU）、里程計（Odometry）。通常室內采用二維激光雷達，室外采用三維激光雷達，里程計采用輪式里程計。由於IMU 具有較高的角速度測量精度，里程計具有較高的局部位置測量精度，一般用IMU 計算角度信息，里程計計算位置信息，再配合激光雷達進行同時定位與建圖。
1.1 激光雷達傳感器
激光雷達傳感器可分為二維激光雷達和三維激光雷達，它們由激光雷達光束的數量來定義。就生產工藝而言，激光雷達也可分為機械激光雷達、微機電等混合固態激光雷達和固態激光雷達。固態激光雷達主要應用相控陣和Flash技術。在自動駕駛快速發展的趨勢下，小型化和輕型化的固態激光雷達將逐步占據市場，並滿足大多數應用。目前生產激光雷達知名公司有Velodyne，其主要產品是機械激光雷達，有VLP-16, HDL-32E和HDL-64E三種類型的產品。SLAMTEC主要生產低成本的激光雷達，其產品有RPLIDAR A1、A2和R3三種類型。Ouster主要生產機械激光雷達，有16到128個頻道不同的類型激光雷達。Quanergy發布了世界上第一個的固態激光雷達—S3。S3是一種微型固態激光雷達。
1.2 激光SLAM系統
當前，激光SLAM 框架一般分為前端掃描匹配、后端優化、閉環檢測、地圖構建四個關鍵模塊。前端掃描匹配是激光SLAM 的核心步驟，工作內容是已知前一幀位姿並利用相鄰幀之間的關系估計當前幀的位姿；前端掃描匹配能給出短時間內的位姿和地圖，但由於不可避免的誤差累積，后端優化正是當長時間增量式掃描匹配后優化里程計及地圖信息；閉環檢測負責通過檢測閉環而減少全局地圖的漂移現象，以便生成全局一致性地圖；地圖構建模塊負責生成和維護全局地圖。基於激光的SLAM開源方法有Gmapping、HectorSlam、LagoSLAM、Cartographer、Loam、Lego-Loam等。Gmapping是基於粒子濾波的方法，增加了掃描匹配方法來估計機器人的位置[9]。HectorSlam利用掃描匹配技術和慣性傳感器(Inertial Measurement Unit，IMU)進行同時定位與建圖[10]。LagoSLAM是基於圖優化的SLAM，利用非線性使得非凸成本的最小化[11]。Cartographer是谷歌發布的一個SLAM算法，它采用子映射和回環檢測來獲得更好的性能[12]。Loam是一種使用三維激光雷達進行同時定位建圖的方法[13]。Lego-Loam是將VLP-16激光雷達的點雲數據和IMU數據作為輸入,實時輸出姿態估計，並進行全局優化和回環檢測[14]。
1.3挑戰與未來
1.3.1成本與適應性
激光雷達的優點是它能提供三維信息，不受光照變化的影響。此外，視角相對較大，可以達到360度。但是激光雷達的技術門檻很高，導致開發周期長，成本高。未來，小型化、成本合理、固態化以及實現高可靠性和適應性是激光雷達的發展趨勢。
1.3.2 低紋理和動態環境
大部分SLAM系統只能在固定的環境中工作，但實際環境是在不斷變化的。此外，走廊、管道等低紋理環境也會給激光SLAM帶來很大困難。利用慣性測量單元可以解決上述問題[15]。此外，將時間維度結合到繪圖過程中，以使機器人在動態環境中能夠具有精確的地圖[16]。如何使激光SLAM對低紋理和動態環境更加魯棒，以及如何保持地圖的精確更新是未來需要解決的問題。
2 視覺SLAM
隨着中央處理器和圖形處理器的發展，圖形處理能力越來越強。相機傳感器越來越便宜，越來越輕，同時也越來越通用。在過去的十年里，視覺SLAM發展迅速。相比於激光SLAM，使用相機的視覺SLAM系統具有更便宜、更輕巧等特性。現在，視覺SLAM系統可以在微型計算機和嵌入式設備上運行[17]，甚至在移動設備上也能使用，比如智能手機[18]。
2.1 視覺傳感器
視覺SLAM最常用的傳感器是相機。具體來說，攝像機可以分為單目相機、立體相機、RGB-D相機、事件相機等。單目相機無法獲得真正的深度，真實的軌跡與地圖有一定的比例，這稱為尺度模糊[19]。基於單目相機的SLAM必須初始化，並存在漂移問題。立體相機是兩個單目相機的組合，兩個單目攝像機之間的基線距離是已知的，所以深度可以基於校准、校正、匹配和計算得到。深度相機通過立體、結構光和飛行時間等技術可以直接輸出像素深度。結構光是紅外激光器向物體表面發射一些具有結構特征的圖案。然后紅外照相機將收集由於表面深度不同而引起的圖案變化。飛行時間將測量激光飛行的時間來計算距離。事件相機不是以固定的速率捕捉圖像，而是異步測量每個像素的亮度變化[20]。事件攝像機具有很高的動態性范圍、高時間分辨率、低功耗等特性，並且不會受到運動模糊的影響。因此，事件攝像機在高速和高動態范圍內的性能優於傳統攝像機[21]。
2.2 視覺SLAM系統
自2002 年起，視覺SLAM 算法的研究開始引起關注，但重大的進展是從2007 年Klein 等人[22]提出的並行跟蹤與建圖( parallel tracking and mapping，PTAM) 算法開始的。他們提出一個可以將地圖創建和位姿估計過程放在兩個並發線程中運行的非濾波器算法框架，並取得了實時運行效果。其后，本領域的大部分研究成果都延續了PTAM 框架中的思想。ORB-SLAM使用三個線程，包括跟蹤、基於對極約束的局部優化和基於姿態圖的全局優化，並且支持單目相機、立體相機和RGB-D相機[23]。Pro-SLAM是一個輕量級視覺SLAM系統，易於理解。它是一種基於特征跟蹤的方法，可以有效地匹配一個或多個視頻序列之間的特征點對應關系[24]。LSD-SLAM提出了一種新的基於李代數和直接法的同時定位與建圖方法，該方法支持立體相機[25]。RGBD-SLAM是基於深度相機的，可以在沒有其他傳感器的幫助下重建三維場景地圖[26]。EVO是一種基於事件的視覺里程計算法，該算法不受運動的影響，在強烈光照變化的動態場景中，可以良好運行[27]。
2.3 挑戰與未來
2.3.1 魯棒性和可移植性
視覺SLAM仍然面臨着光照變化、動態環境、快速運動、劇烈旋轉和低紋理環境等重要問題。解決思路如下：首先，事件相機每秒能夠產生多達一百萬個事件，足以在高速和高動態范圍內進行非常快速的運動，可以精確的對相機進行姿態估計。其次，使用邊緣、平面、表面等特征，可以減少特征的相關性，進行有效地特征跟蹤。視覺SLAM在未來的應用，一個是基於智能手機和無人機等嵌入式平台的SLAM，另一個是三維場景重建和場景理解。在動態的、非結構化的大規模環境中需要平衡實時性和准確性是一個至關重要的問題[28]。
2.3.2 語義SLAM
隨着深度學習在計算機視覺領域的快速發展，研究者對深度學習在視覺SLAM中的應用有很大的興趣。視覺SLAM中的特征匹配、閉環檢測等模塊都可以通過深度學習來獲得更優的結果。在視覺SLAM中應用深度學習可以實現目標識別和分割，有助於SLAM系統更好地感知周圍環境。語義SLAM也有助於全局優化、回環檢測和重定位[29]。傳統的同時定位和建圖方法依賴於點、線等幾何特征來推斷環境結構的平面。語義SLAM可以實現大規模場景中高精度同時定位與建圖。
3 激光和視覺融合的SLAM
激光和視覺傳感器在SLAM應用中都有其局限性，基於激光和視覺傳感器融合的SLAM方法能夠有效的利用各個傳感器的優勢，彌補傳感器在某些特殊環境下的劣勢，成為當前研究的熱點之一。
3.1 激光和視覺傳感器數據的外部標定激光和視覺傳感器數據融合的前提條件是不同傳感器對同一目標在同一時刻的描述。因此，不同傳感器數據之間的自動標定以及不同傳感器的數據融合是激光和視覺融合SLAM需要解決的關鍵問題。傳感器的標定方法可以分為兩種，第一種是按照給定的相對變換關系安裝傳感器；第二種則是根據不同傳感器數據之間的約束關系來計算兩個傳感器之間的相對變換關系。當傳感器發生故障后，第一種方法需要重新校正，而且移動機器人運動過程中的振動會使得相對誤差逐漸增大，因此更多的采用第二種標定方法，這種標定方法類似於單目相機的內參標定，通過給定的標定板，利用幾何約束關系構建坐標轉換系數矩陣方程，從而確定相機坐標系和激光雷達坐標系之間的轉換關系[30]。而且，針對未標定過或者標定誤差較大的單目相機，該方法還可以采用全局優化的方法同步優化相機內部標定和外部標定。Lidar-Camera提出了一種新的管道和實驗裝置，以找到精確的剛體變換，用於利用3D-3D點對應關系對激光雷達和相機進行外部校准[31]。
3.2 激光和視覺傳感器的融合傳感器數據融合層次一般分為三種：數據層融合、特征層融合和決策層融合。激光和視覺傳感器是異質的，因此數據無法在數據層進行融合，而決策層融合預處理代價高，而且融合的結果相對而言最不准確，因此激光和視覺傳感器數據融合主要是特征層的數據融合。一種較為簡單、直觀的融合方法是基於估計理論數據融合方法主要包括卡爾曼濾波方法、協方差融合方法、最小二乘法等。這種方法為不同的傳感器數據建立狀態空間模型，然后對其進行狀態估計，從而實現數據融合[32]。另一種是基於推理數據的融合方法，主要包括貝葉斯估計法和Dempster-Shafer(D-S)證據推理法等。貝葉斯估計法屬於靜態環境信息融合方法，信息描述為概率分布，適應於具有可加高斯噪聲的不確定性信息處理。多貝葉斯估計把每個傳感器作為貝葉斯估計，將環境中各個物體的關聯概率分布結合成聯合的后驗概率分布函數，通過使聯合分布函數的似然函數為最大，提供最終融合值，D-S證據推理法是貝葉斯估計法的擴展方法[33]。V-Loam提出了一個將視覺里程計和激光雷達結合的通用框架，以在線的方法從視覺里程計和基於匹配的激光雷達里程計兩個方面入手，同時改進了運動估計和點雲配准[34]。
3.3 挑戰與未來
3.3.1 數據關聯
SLAM的未來必須集成多個傳感器。但不同的傳感器有不同的數據類型、時間戳和坐標系表達式，需要統一處理。此外，還應考慮多傳感器之間的物理模型建立、狀態估計和優化。
3.3.2 硬件集成
目前，還沒有合適的芯片和硬件來集成SLAM技術，使其成為產品。另一方面，如果傳感器的精度由於故障或老化而降低，傳感器測量的噪聲與模型不匹配。前端傳感器應具備處理數據的能力，並能從硬件層向算法層傳遞高質量的數據。
3.3.3 抵御風險和故障恢復的能力
SLAM系統應該是具有抵御風險和故障恢復的能力，這里不是重新定位或回環檢測的問題，而是SLAM系統必須有能力應對風險或故障。同時，SLAM系統應該能夠在不同的平台上運行，不管平台的計算約束如何。如何平衡准確性、魯棒性和有限的資源是一個具有挑戰性的問題。
4 發展趨勢
4.1 大規模復雜環境的適應性和魯棒性的提高
大規模復雜環境的適應性和魯棒性是SLAM 系統實用化的必然要求，其趨勢在於從當前的室內或簡單室外環境轉向大范圍的、動態的、復雜的室外環境研究。適應動態環境、抗光照變化、一些低紋理環境下的SLAM 技術、無須初始化的SLAM 技術也正在發展。此外，模式識別和機器學習理論的發展也正在使得閉環檢測效果更加精確[35]。
4.2 提高計算效率並優化精度
在提高計算效率方面，一些基於嵌入式、低功耗設備上SLAM 技術的研究也逐漸受到技術企業的關注，例如VR 和AR 頭盔乃至手機設備都能提供較好的使用體驗，同時擺脫對笨重計算設備的依賴[36]。
4.3 多機器人和多傳感器協同SLAM
在多機器人和多傳感器系統SLAM 方面，多機器人協同建圖、基於多傳感器融合的SLAM 系統、visual-inertial SLAM等研究也逐漸興起。KIT 和CMU 等大學將自有地面無人車輛采集的數據打包發布供研究者測試和對比各自算法在真實環境中的效果，這些數據不僅包括單目相機的數據，還有如慣性導航、GPS、雙目視覺、激光雷達數據等數據[37]。因此，未來基於多傳感器融合的SLAM 算法將是一個熱門的研究方向。
5 總結本文分析了三種類型的同時定位與地圖創建方法的各個基本組件，並對比了近年來重要算法的設計思路。綜合近年來的重要成果不難發現，同時定位與地圖創建算法的發展正在朝着越來越注重准確性、實時運行和具備較強的故障恢復能力等方向發展，這一趨勢也為此類技術在各種環境下的實用性奠定了基礎。SLAM 技術會從實驗室引入生活和工業應用場景中，使得生活環境越來越智能化。
參考文獻:
[1] John J, Hugh F. Simultaneous map buildingand localization for an autonomous mobile robot[J]. IEEE/RSJ International Workshop on Intelligent Robots ,1991, 7(6): 1442–1447.
[2] Durrant H, Bailey T. Simultaneous localization and mapping(slam):Part i[J]. IEEE Robotics & Automation Magazine, 2006, 13(2): 99-110.
[3] Bailey T, Durran H. Simultaneous localization and mapping(slam):Part ii[J]. IEEE Robotics & Automation Magazine, 2006, 13(3): 108-117.
[4] Randall S, Matthew S. Estimating uncertain spatial relationships in robotics[J]. Autonomous robot vehicles, 1987, 13(3): 167–193.
[5] Durrant H, Rye D. Localization of autonomous guided vehicles[J]. Robotics Research.1996, 12(1): 613-625.
[6] Aulinas J, Petillot Y, Salvi Joaquim, et al. The slam problem: A survey [J]. Artificial Intelligence Research and Developmen, 2008, 11(2): 363-371.
[7] Dissanayake G, Huang S, Wang Z, et al. Areview of recent developments in simultaneous localization and mapping[J]. IEEE International Conference on Industrial and Information Systems. 2011,5(2): 477-482.
[8] Candena C, Carlone L, Carrillo H, et al. Past, present, and future of simultaneous localization and mapping：Towards the Robust-Perception Age[J]. IEEE Transactions on Robotics, 2016, 32(6): 1309-1332.
[9] Giorgio G, Cyrill S, Wolfram B, et al. Improved techniques for grid mapping with rao-black wellized particle filters[J]. IEEE transactions on Robotics, 2007,23(1): 34-50.
[10] Michael M, Sebastian Thrun, Daphne K, Ben W, et al. Fast-slam 2.0: An improved particle filtering algorithm for simultaneous localization and mapping that provably converges[J]. 2003, 32(6): 1151–1156.
[11] Luca C, Rosario A. A linear approximation for graph-based simultaneous localization and mapping[J]. Robotics: Science and Systems VII, 2012, 12(5): 41–48.
[12] Wolfgang H, Damon K, Holger R, and Daniel A. Realtime loop closure in 2d lidar slam[J]. IEEE International Conference on Robotics and Automation, 2016, 15(5): 1271–1278.
[13] Ji Z, Sanjiv S. Loam: Lidar odometry and mapping in real-time[J]. Science and Systems, 2014, 15(5): 9-25.
[14] Tixiao S, Brendan E. Lego-loam: Lightweight and ground optimized lidar odometry and mapping on variable terrain[J]. International Conference on Intelligent Robots and Systems, 2018,14(5): 4758–4765.
[15] Zhong W, Yan C, Yue M. IMU-Assisted 2d slam method for low-texture and dynamic environments[J]. Applied Sciences, 2018, 8(12): 25-34.
[16] Aisha W, Michael K, Hordur J. Dynamic pose graph slam: Long-term mapping in low dynamic environments[J]. International Conference on Intelligent Robots and Systems, 2012, 5(5): 1871–1878.
[17] Raul M, Jose M. Orb-slam: a versatile and accurate monocular slam system[J]. IEEE transactions on robotics, 2015, 31(5):1147–1163.[18] Tong Q, Peiliang L, and Shaojie S. Vins-mono: A robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4):1004–1020.
[19] Simon L, Torsten S, Michael B. Get out of my lab: Large-scale, real time visual-inertial localization[J]. In Robotics: Science and Systems, 2015, 18(5): 10-25.
[20] Guillermo G, Tobi D, Garrick O, et al. Event-based vision: A survey[J]. IEEE Transactions on Robotics, 2019, 34(4):1004–1020.
[21] Patrick L, Christoph P, and Tobi D. A 128x128120db 15us latency asynchronous temporal contrast vision sensor[J]. IEEE journal of solid-state circuits, 2008, 43(2):566–576.
[22] Klein G，Murray D． Parallel tracking and mapping for small AR workspaces[J]. Proc of IEEE and ACM International Symposium on Mixed and Augmented Reality. 2007, 10(5): 1-10．
[23] Ethan R, Vincent R, Kurt K, and Gary R. Orb: An efficient alternative to sift or surf[J]. ICCV, 2011,10(5): 2-15.
[24] Guofeng Z, Haomin L, et al. Efficient non-consecutive feature tracking for robust structure-from-motion[J]. IEEE Transactions on Image Processing, 2016, 25(12):5957–5970.
[25] Jakob E, Thomas S, and Daniel C. Lsd-slam: Largescale direct monocular slam[J]. European conference on computer ision, 2014, 12(5): 834–849.
[26] Felix E, Hess J, Daniel C. 3-d mapping with an rgb-d camera[J]. IEEE transactions on robotics, 2014, 30(1):177–187.[27] Henri R, Timo H, Guillermo G. Evo: A geometric approach to event-based 6-dof parallel tracking and mapping in real time[J]. IEEE Robotics and Automation Letters, 2016, 2(2):593–600.
[28] Muhammad S, Gon W. Simultaneous localization and mapping in the epoch of semantics: A survey[J]. International Journal of Control, Automation and Systems, 2019, 17(3): 729–742.
[29] Nikolay A, Sean L, Kostas D. A unifying view of geometry, semantics, and data association in slam[J]. IJCAI, 2018, 12(4): 5204–5208.
[30] Jesse L, Sebastian T. Automatic online calibration of cameras and lasers[J]. Robotics: Science and Systems, 2013,10(5): 2-16.
[31] Dhall A, Chelani, V, Radhakrishnan M. Camera calibration using 3D-3D point correspondences[J]. ArXiv eprints, 2017, 11(4): 24-36.[32] Wen S, Othman K, Rad A, et al. Indoor slam using laser and camera with closed-loop controller for nao hunmanoid robot[J]. Abstract and Applied Analysis,2014, 12(5): 1-8.
[33]Chang H, Lee C, Lu Y. P-SLAM: Simultaneous localization and mapping with environmental-structure prediction[J]. IEEE Transactions on Robotics, 2007, 23(2): 281-293.
[34] Ji Z and Sanjiv S. Visual-lidar odometry and mapping: Low-drift, robust, and fast[J]. IEEE International Conference on Robotics and Automation, 2015, 10(5): 2174–2181.
[35] Stefan M, Georg A, Christian Witt. Visual slam for automated driving: exploring the applications of deep learning[J]. IEEE Conference on Computer Vision and Pattern Recognition , 2018, 10(5): 360–370.
[36] Oscar G, Grasa1 J. EKF Monocular slam 3D modeling, measuring and augmented reality from endoscope image sequences[J]. IEEE International Conference on Robotics and Automation , 2009, 10(1): 174–185.
[37] Urtasun R，Lenz P，Geiger A． Are we ready for autonomous driving? The KITTI vision benchmark suite[C]. IEEE Conference on Computer Vision and Pattern Recognition． Washington DC: IEEE Computer Society，2012: 3354-3361．
本文僅做學術分享，如有侵權，請聯系刪文。
下載1在「3D視覺工坊」公眾號后台回復：3D視覺，即可下載 3D視覺相關資料干貨，涉及相機標定、三維重建、立體視覺、SLAM、深度學習、點雲后處理、多視圖幾何等方向。
下載2在「3D視覺工坊」公眾號后台回復：3D視覺優質源碼，即可下載包括結構光、標定源碼、缺陷檢測源碼、深度估計與深度補全源碼、點雲處理相關源碼、立體匹配源碼、單目、雙目3D檢測、基於點雲的3D檢測、6D姿態估計源碼匯總等。
下載3在「3D視覺工坊」公眾號后台回復：相機標定，即可下載獨家相機標定學習課件與視頻網址；后台回復：立體匹配，即可下載獨家立體匹配學習課件與視頻網址。