本篇綜述是本人與知乎的一位大佬博主華神共同完成的。在這里感謝華神的幫助,同時(shí)希望本篇能為在SLAM道路上探索的同僚們盡些綿薄之力,歡迎各位一同探討和進(jìn)步。
01? 引言:SLAM概念與結(jié)構(gòu)
1.1? SLAM概述
首先,本人認(rèn)為需要思考一個(gè)問題:為什么要做SLAM?SLAM技術(shù)可以解決什么問題?簡(jiǎn)單的引入。 這個(gè)問題的回答可以是:1.大部分的路徑規(guī)劃需要基礎(chǔ)地圖;2.定位時(shí),為消除IMU、Wheel Odometry等里程計(jì)模型的“誤差累加”特性引入觀測(cè)參考;3. 提供一個(gè)全局表征(frame、origin)的參考,等。 再回到SLAM概念的定義:SLAM(Simultaneous Localization and Mapping)是以定位和建圖兩大技術(shù)為目標(biāo)的一個(gè)研究領(lǐng)域。目前主流的slam技術(shù)為激光slam(基于激光雷達(dá))和視覺slam(基于單/雙目攝像頭),實(shí)現(xiàn)上主要分為基于濾波 (Filter-Based) 的SLAM和基于圖優(yōu)化(Graph-Based)的SLAM。歸結(jié)其本質(zhì),其實(shí)就是——State Estimation in Robotics
1.2 SLAM結(jié)構(gòu)
▲?此處以cartographer框架為例
1.2.1 數(shù)據(jù)預(yù)處理
Sensor data的的接收;
多傳感器數(shù)據(jù)同步;
傳感器數(shù)據(jù)預(yù)處理(無效值去除、序列檢查、點(diǎn)云遮擋點(diǎn)與平行點(diǎn)去除、坐標(biāo)系處理,etc);
點(diǎn)云去畸變處理(運(yùn)動(dòng)畸變,重力對(duì)齊,etc);
1.2.2 前端LIO
基于sensor-tracker形式的pose-extrapolator;
點(diǎn)云注冊(cè)&配準(zhǔn);
e,g, CSM:Correlative Scan Matcher & Ceres Scan Matcher;
1.2.3 后端Refinement
基于前端LIO選擇的key nodes形成位姿圖;
利用多傳感器數(shù)據(jù)計(jì)算約束實(shí)現(xiàn)位姿圖的優(yōu)化(e,g, 基于Google Ceres構(gòu)建Pose Graph,利用node-submap constraints & submap-submap constraints & node-node constraints作為edges,scan-pose為node);
基于SPA加速的后端優(yōu)化;
1.2.4 閉環(huán)檢測(cè)
根據(jù)key node搜索neighbor key nodes,嘗試進(jìn)行點(diǎn)云配準(zhǔn)(scan2scan or scan2map);
e,g, LoopClosure:Correlative Scan Matcher & Branch and Bound;
1.2.5 地圖構(gòu)建保存與顯示
pbstream文件:建圖軌跡信息保存與加載;
根據(jù)優(yōu)化后的key nodes和對(duì)應(yīng)的point cloud生成點(diǎn)云地圖;
基于RangefindersRayCasting的概率柵格地圖的構(gòu)建、更新和擴(kuò)展。
1.3 基于濾波的傳感器融合算法方案
歸結(jié)基于濾波的定位算法的核心技術(shù),其實(shí)就是貝葉斯濾波或者其衍生算法。整體上的流程大抵都是基于上一時(shí)刻的狀態(tài)量,通過控制量輸入和運(yùn)動(dòng)方程的推演獲取預(yù)測(cè)的狀態(tài)量,再由相關(guān)傳感器的觀測(cè)對(duì)預(yù)測(cè)進(jìn)行融合“補(bǔ)償”。
目前常見的應(yīng)用在SLAM系統(tǒng)中的濾波算法主要有以下幾種:
Bayesian Filter;
Kalman Filter;
Particle Filter(adaptive Monte Carlo Localization);
Extended Kalman Filter;
Iterated Extended Kalman Filter;
Error-State Kalman Filter;
Multi-State Constraint Kalman Filter;
1.4 基于優(yōu)化的傳感器融合算法方案
歸結(jié)基于優(yōu)化的定位算法的核心技術(shù),其實(shí)就是最小二乘,但應(yīng)用在SLAM技術(shù)中,其關(guān)鍵的地方在于:如何設(shè)計(jì)與建立SLAM的problem structure:
?
? 也就是上述的目標(biāo)函數(shù)是由哪些部分構(gòu)成的?殘差項(xiàng)有哪些?還是以cartographer為例,殘差項(xiàng)可以包含關(guān)鍵幀之間的odom約束,關(guān)鍵幀與submap之間的約束(intra和inter),landmark引入產(chǎn)生的約束,等等。以上一起由通過不同的殘差項(xiàng)權(quán)重整合在一起,共同組成后端優(yōu)化的目標(biāo)函數(shù)。 那么除去這些呢,還可以引入哪些約束來優(yōu)化整體的優(yōu)化結(jié)構(gòu)?或者又有什么方式可以優(yōu)化優(yōu)化的處理過程?
▲?lio-sam中的因子圖結(jié)構(gòu) 目前常見的應(yīng)用在SLAM系統(tǒng)中的優(yōu)化庫主要有以下幾種:
G2o;
Ceres;
Gtsam;
Isam;
各個(gè)優(yōu)化庫的使用和特點(diǎn)就不在此處進(jìn)行說明了,感興趣的小伙伴可以自行網(wǎng)上查閱資料~
1.5 地圖構(gòu)建的基本原理
地圖可以根據(jù)具體的應(yīng)用場(chǎng)景和功能特性簡(jiǎn)單地分為幾種:
語義地圖;
基于概率更新的占柵格地圖(黑白灰三色圖);
3D點(diǎn)云地圖(稀疏、稠密);
基于TSDF的占柵格地圖(或點(diǎn)云地圖?)
? 2D柵格地圖的基本原理大抵會(huì)涉及以下幾個(gè)技術(shù)點(diǎn):
柵格地圖的生成與維護(hù);
概率更新算法;
Bresenham畫線算法;
而與柵格地圖特性相對(duì)應(yīng)的是基于TSDF(Truncated Signed Distance Function)原理生成的柵格地圖,TSDF生成的地圖最大的特性就是生成的地圖邊界是可以保證單像素級(jí)別的,而基于概率更新的柵格地圖的邊界往往都會(huì)由多個(gè)像素“混合”在一起。
▲?TSDF示意圖
▲?TSDF定義與柵格生成方法
02? 激光SLAM主流方案
2.1 Gmapping
簡(jiǎn)介:基于粒子濾波框架的激光SLAM,RBpf粒子濾波算法,即將定位和建圖過程分離,先進(jìn)行定位再進(jìn)行建圖,結(jié)合里程計(jì)和激光信息,每個(gè)粒子都攜帶一個(gè)地圖,構(gòu)建小場(chǎng)景地圖所需的計(jì)算量較小,精度較高。但在高分辨率建圖時(shí),在靜止?fàn)顟B(tài)下更新不好,存在震蕩并noise過多。
Github鏈接: https://github.com/ros-perception/openslam_gmapping https://github.com/ros-perception/slam_gmapping 相關(guān)論文: Giorgio Grisetti, Cyrill Stachniss, and Wolfram Burgard: Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters, IEEE Transactions on Robotics, Volume 23, pages 34-46, 2007. Giorgio Grisetti, Cyrill Stachniss, and Wolfram Burgard: Improving Grid-based SLAM with Rao-Blackwellized Particle Filters by Adaptive Proposals and Selective Resampling, In Proc. of the IEEE International Conference on Robotics and Automation (ICRA), 2005.
2.2?Hector_SLAM
簡(jiǎn)介:利用優(yōu)化方法進(jìn)行幀間匹配的激光slam算法,search space不大,不需要里程計(jì)信息,代碼較短。且只處理scan的endpoints(不利用laser scan structure),故而可利用IMU調(diào)整laser scan attitude。缺點(diǎn)是在雷達(dá)頻率不夠的設(shè)備上效果不佳,快速轉(zhuǎn)向時(shí)容易錯(cuò)誤匹配;沒有l(wèi)oop closure故而若累計(jì)誤差過大,HectorSLAM沒有地圖調(diào)整能力,且?guī)g優(yōu)化匹配時(shí)也是全局地圖上進(jìn)行,cpu資源消耗大。 Github鏈接: https://github.com/tu-darmstadt-ros-pkg/hector_slam 相關(guān)論文: Kohlbrecher, Stefan , et al. “A flexible and scalable slam system with full 3d motion estimation.” 2011 IEEE International Symposium on Safety, Security, and Rescue Robotics IEEE, 2011.
2.3 Karto
簡(jiǎn)介:基于圖優(yōu)化的SLAM,包含回環(huán)檢測(cè),適用于大面積建圖。前端幀匹配采取的的是correlative scan matcher的方式進(jìn)行粗、精兩次匹配, 回環(huán)檢測(cè)上karto沒有submap的概念,全部以keyScan的形式存儲(chǔ)在Mapper-sensor-Manager中,keyScan的插入依據(jù)pose的距離窗口生成localMap進(jìn)行匹配。local與gloal的loop closure依據(jù)graph的結(jié)構(gòu)和Mapper-sensor-Managerss順序存儲(chǔ)分配的ID信息,選擇candidates生成localMap進(jìn)行匹配,依據(jù)score進(jìn)一步確定閉環(huán)。 Github鏈接: https://github.com/ros-perception/open_karto https://github.com/ros-perception/slam_karto 相關(guān)論文: Olson, E. B. . “Real-time correlative scan matching.” Robotics and Automation, 2009. ICRA '09. IEEE International Conference on IEEE, 2009. Konolige, Kurt , et al. “Efficient sparse pose adjustment for 2D mapping.” 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems IEEE, 2010.
2.4 Cartographer
簡(jiǎn)介:基于圖優(yōu)化的SLAM,源自谷歌,代碼優(yōu)美,是非常完整的激光slam系統(tǒng),包含相對(duì)魯棒的前端,基于submap和node約束獨(dú)立的pose graph后端及各類評(píng)測(cè)工具。模塊化、系統(tǒng)化、工程化程度很高, 封裝很完善??蓪⑵湟暈樯?jí)版本的Karto的頂配豪華版本,搭載了傳感器同步、位姿外推器、激光數(shù)據(jù)預(yù)處理(去畸變、重復(fù)點(diǎn)云刪除)功能,前端幀匹配上融合使用了karto的暴力匹配csm和hector的基于LM的梯度優(yōu)化方法匹配,由于引入了submap的概念,后端回環(huán)檢測(cè)上使用分支定界的方法進(jìn)行快速搜索,后端優(yōu)化過程基于SPA完成。同時(shí)carto還開放了landmark、GPS等數(shù)據(jù)融合的接口,提供了地圖續(xù)掃和定位的功能。 Github鏈接: https://github.com/cartographer-project/cartographer 相關(guān)論文: Hess, Wolfgang , et al. “Real-Time Loop Closure in 2D LIDAR SLAM.” 2016 IEEE International Conference on Robotics and Automation (ICRA) IEEE, 2016.
2.5 LOAM方案
簡(jiǎn)介:經(jīng)典制作,長期霸榜kitti odometry門類第一,也衍生很多相關(guān)SLAM算法,如A-LOAM、LeGO-LOAM等。其主要思想是通過兩個(gè)算法:一個(gè)高頻激光里程計(jì)進(jìn)行低精度的運(yùn)動(dòng)估計(jì),即使用激光雷達(dá)做里程計(jì)計(jì)算兩次掃描之間的位姿變換;另一個(gè)是執(zhí)行低頻但是高精度的建圖與校正里程計(jì),利用多次掃描的結(jié)果構(gòu)建地圖,細(xì)化位姿軌跡。在點(diǎn)云匹配與特征提取時(shí),由于scan-to-scan匹配精度低但速度快, map-to-map匹配精度高但是速度慢,創(chuàng)新性使用scan-to-map來兼具精度與速度,這種思路給后續(xù)很多基于激光里程計(jì)或多傳感器融合框架提供思路。原本的LOAM沒有IMU輔助,不帶有回環(huán)檢測(cè),因而不可避免引起漂移。沿著此思路,產(chǎn)生了很多算法如LeGO-LOAM, LINS, LIO-Mapping, LIO-SAM等。 Github鏈接: LOAM中文注解版:https://github.com/cuitaixiang/LOAM_NOTED 相關(guān)論文: Ji Zhang and Sanjiv Singh. “LOAM: Lidar Odometry and Mapping in Real-time.” Proceedings of Robotics: Science and Systems Conference, 2014.
2.6 LeGo-LOAM方案
簡(jiǎn)介:在LOAM的基礎(chǔ)上細(xì)化了特征提取與優(yōu)化,帶有回環(huán)功能。可以在低功耗嵌入式系統(tǒng)上實(shí)現(xiàn)實(shí)時(shí)姿態(tài)估計(jì)。主要也是由一個(gè)高頻低精度激光里程計(jì)與一個(gè)低頻高精度建圖回環(huán)檢測(cè)組成。區(qū)別在于LeGO-LOAM在LOAM基礎(chǔ)上采用地面分割方式將點(diǎn)云分為地面點(diǎn)與非地面點(diǎn),進(jìn)一步縮小特征提取范圍,以加快計(jì)算速度,能夠在嵌入式設(shè)備上實(shí)時(shí)運(yùn)行;在建圖模塊添加了位姿圖與回環(huán)檢測(cè),解決了LOAM沒有后端優(yōu)化的弊端,提供建圖效率,其效果對(duì)比如下圖。 Github鏈接: LeGo-LOAM中文注解版:https://github.com/wykxwyc/LeGO-LOAM_NOTED/tree/master/src 相關(guān)論文: T. Shan and B. Englot, “LeGO-LOAM: Lightweight and Groundoptimized Lidar Odometry and Mapping on Variable Terrain,”IEEE/RSJ International Conference on Intelligent Robots and Systems,pp. 4758-4765, 2018.
2.7 LIO-SAM方案
簡(jiǎn)介:這是LeGO-LOAM作者新作,通過平滑和匹配的緊密耦合激光雷達(dá)慣性SLAM框架。LIO-SAM在因子圖(factor graph)上方制定了激光雷達(dá)慣性里程表,從而可以將不同來源的大量相對(duì)和絕對(duì)測(cè)量值(包括回路閉合)作為因子合并到系統(tǒng)中,通過慣性測(cè)量單元(IMU)預(yù)積分的估計(jì)運(yùn)動(dòng)使點(diǎn)云偏斜,并為激光雷達(dá)里程計(jì)優(yōu)化提供了初始預(yù)測(cè)。為了確保實(shí)時(shí)高性能,將之前的激光雷達(dá)掃描邊緣化以進(jìn)行姿勢(shì)優(yōu)化,而不是將激光雷達(dá)掃描與全局地圖匹配。選擇性地引入關(guān)鍵幀以及以有效的滑動(dòng)窗口將新的關(guān)鍵幀注冊(cè)到固定大小的先驗(yàn)“子關(guān)鍵幀”集合中,在局部范圍而不是全局范圍內(nèi)進(jìn)行掃描匹配。 算法可以添加不同的模塊,組合不同的方案。當(dāng)同時(shí)禁用GPS和閉環(huán)檢測(cè)時(shí),此方法稱為LIO-odom,它僅利用IMU預(yù)積分和激光雷達(dá)里程計(jì)因子;當(dāng)添加GPS因子時(shí),則將此方法稱為LIO-GPS;若使用IMU、激光雷達(dá)里程計(jì)、GPS、回環(huán)檢測(cè)時(shí)則稱之為LIO-SAM方法。 Github鏈接: https://github.com/TixiaoShan/LIO-SAM 相關(guān)論文: T. Shan and B. Englot, “LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping,”IEEE/RSJ International Conference on Intelligent Robots and Systems”,2020. 性能介紹:在論文作者錄制的數(shù)據(jù)集上,對(duì)比了LOAM、LIOM與自身幾種組合方案的建圖效果如下左圖所示,也比較了幾種算法在平移誤差及處理單幀數(shù)據(jù)時(shí)所耗時(shí)間如右圖所示,由此可見LIO-SAM處理速度較LOAM提高很多,精度上也有一定提高。
?
2.8 hdl_graph_slam方案
簡(jiǎn)介:這是一個(gè)基于圖優(yōu)化的3D激光SLAM框架,主要由激光里程計(jì)、回環(huán)檢測(cè)以及后端圖優(yōu)化構(gòu)成,同時(shí)融合了IMU、GPS以及地面檢測(cè)的信息作為圖的額外約束。算法首先讀入激光雷達(dá)的點(diǎn)云數(shù)據(jù),然后將原始的點(diǎn)云數(shù)據(jù)進(jìn)行預(yù)濾波,經(jīng)過濾波后的數(shù)據(jù)分別給到點(diǎn)云匹配里程計(jì)以及地面檢測(cè)節(jié)點(diǎn),兩個(gè)節(jié)點(diǎn)分別計(jì)算連續(xù)兩幀的相對(duì)運(yùn)動(dòng)和檢測(cè)到的地面的參數(shù),并將這兩種消息送到hdl_graph_slam節(jié)點(diǎn)進(jìn)行位姿圖(pose graph)的更新以及回環(huán)檢測(cè),并發(fā)布地圖的點(diǎn)云數(shù)據(jù)。hdl_graph_slam在資源消耗、代碼復(fù)雜度等方面具有優(yōu)勢(shì),且?guī)в卸ㄎ荒K,右圖顯示了帶GPS的室外建圖結(jié)果。 Github鏈接: https://github.com/koide3/hdl_graph_slam 相關(guān)論文: Kenji Koide, Jun Miura, and Emanuele Menegatti, “A Portable 3D LIDAR-based System for Long-term and Wide-area People Behavior Measurement”, Advanced Robotic Systems, 2019
?
2.9 SegMap方案
原理介紹:這是一種基于3D點(diǎn)云中線段提取的方式去建圖與定位的解決方案。將三維點(diǎn)云進(jìn)行分割,把不同的環(huán)境目標(biāo)劃分為不同的段(segment),從而提出了一種新的地圖表達(dá)方法:Segmap。相比于現(xiàn)有僅用來做定位的特征提取器,Segmap利用了數(shù)據(jù)驅(qū)動(dòng)(深度學(xué)習(xí))的描述子提取了語義特征信息。在語義信息層進(jìn)行數(shù)據(jù)處理大大減小了計(jì)算量,較小維度的語義特征描述子解決了單機(jī)器人與多機(jī)器人系統(tǒng)的實(shí)時(shí)數(shù)據(jù)壓縮問題。這種利用CNN語義信息的3D激光SLAM框架,在多機(jī)器人全局路徑規(guī)劃場(chǎng)景時(shí)比傳統(tǒng)SLAM更具有優(yōu)勢(shì)。 Github鏈接: https://github.com/ethz-asl/segmap 相關(guān)論文: R. Dubé, A. Cramariuc, D. Dugas, J. Nieto, R. Siegwart, and C. Cadena. “SegMap: 3D Segment Mapping using Data-Driven Descriptors.” Robotics: Science and Systems (RSS), 2018. R. Dubé, MG. Gollub, H. Sommer, I. Gilitschenski, R. Siegwart, C. Cadena and , J. Nieto. “Incremental Segment-Based Localization in 3D Point Clouds.” IEEE Robotics and Automation Letters, 2018. R. Dubé, D. Dugas, E. Stumm, J. Nieto, R. Siegwart, and C. Cadena. “SegMatch: Segment Based Place Recognition in 3D Point Clouds.” IEEE International Conference on Robotics and Automation, 2017.
2.10 SuMa方案
原理介紹:此算法使用Surfel地圖去實(shí)現(xiàn)前端里程計(jì)和閉環(huán)檢測(cè),此前Surfel地圖曾被用在RGBD-SLAM中,第一次被用在在室外大場(chǎng)景三維SLAM中。Surfel地圖最早是用在基于RGB-D相機(jī)的三維重建任務(wù)中的。SuMa的整體流程就是先處理點(diǎn)云把點(diǎn)云從三維展開成二維,然后生成局部地圖用來給當(dāng)前幀做匹配,接著通過ICP方法進(jìn)行位姿更新,然后更新surfel地圖,最后做閉環(huán)檢測(cè)與后端優(yōu)化。 Github鏈接: https://github.com/jbehley/SuMa 相關(guān)論文: J. Behley, C. Stachniss. Efficient Surfel-Based SLAM using 3D Laser Range Data in Urban Environments, Proc. of Robotics: Science and Systems (RSS), 2018.
2.11 SuMa++
原理介紹:這是一種新的基于語義信息的激光雷達(dá)SLAM系統(tǒng),可以更好地解決真實(shí)環(huán)境中的定位與建圖問題。該系統(tǒng)通過語義分割激光雷達(dá)點(diǎn)云來獲取點(diǎn)云級(jí)的密集語義信息,并將該語義信息集成到激光雷達(dá)SLAM中來提高激光雷達(dá)的定位與建圖精度。通過基于深度學(xué)習(xí)的卷積神經(jīng)網(wǎng)絡(luò),可以十分高效地在激光雷達(dá)“范圍圖(range image)”上進(jìn)行語義分割,并對(duì)整個(gè)激光雷達(dá)點(diǎn)云進(jìn)行語義標(biāo)記。通過結(jié)合幾何深度信息,進(jìn)一步提升語義分割的精度。基于帶語義標(biāo)記的激光雷達(dá)點(diǎn)云,此方法能夠構(gòu)建帶有語義信息且全局一致的密集“面元(surfel)”語義地圖。基于該語義地圖,也能夠可靠地過濾移除動(dòng)態(tài)物體,而且還可以通過語義約束來進(jìn)一步提高投影匹配ICP的位姿估計(jì)精度。 Github鏈接: https://github.com/PRBonn/semantic_suma 相關(guān)論文: Chen, Xieyuanli & Milioto, Andres & Palazzolo, Emanuele & Giguère, Philippe & Behley, Jens & Stachniss, Cyrill. SuMa++: Efficient LiDAR-based Semantic SLAM. 4530-4537. (IROS 2019).
?
?
03? 視覺SLAM主流方案
以視覺傳感器作為主要感知方式的SLAM稱為視覺SLAM 。按照建圖稀疏程度來分,視覺SLAM技術(shù)可以分為稀疏SLAM、半稠密SLAM和稠密SLAM。雖然同為SLAM系統(tǒng),但它們的側(cè)重點(diǎn)并不完全一樣。 SLAM 系統(tǒng)最初的設(shè)想是為機(jī)器人提供在未知環(huán)境中探索時(shí)的定位和導(dǎo)航能力,其核心在于實(shí)時(shí)定位。以定位為目的,需要建立周圍環(huán)境的路標(biāo)點(diǎn)地圖,進(jìn)而確定機(jī)器人相對(duì)路標(biāo)點(diǎn)的位置,這里的路標(biāo)點(diǎn)地圖即稀疏地圖,地圖服務(wù)于定位。 但隨著算法和算力的進(jìn)步,SLAM逐漸被用于對(duì)環(huán)境的重建,也即把所有看到的部分都完整的重建出來,此時(shí),SLAM所建立的地圖必須是稠密的,而SLAM系統(tǒng)的首要任務(wù)也從定位轉(zhuǎn)變?yōu)榱私h(huán)境的精確稠密地圖。這種首要任務(wù)的差異最終會(huì)反映在SLAM系統(tǒng)的技術(shù)方案上,稠密SLAM系統(tǒng)對(duì)精度的評(píng)價(jià)也從“定位精度”轉(zhuǎn)變?yōu)椤敖▓D精度”。此外,相比于稀疏SLAM系統(tǒng),稠密SLAM的建圖部分要消耗大得多的算力,通常都需要GPU加速來達(dá)到實(shí)時(shí)性。
3.1 LSD-SLAM 方案
LSD-SLAM 即 Large-Scale Direct SLAM,兼容單目相機(jī)和雙目相機(jī)。LSD-SLAM是一種基于光流跟蹤的直接法SLAM,但是實(shí)現(xiàn)了半稠密建圖,建圖規(guī)模大,可以在線實(shí)時(shí)運(yùn)行。作者有創(chuàng)見地提出了像素梯度與直接法的關(guān)系,并利用這種關(guān)系實(shí)現(xiàn)了簡(jiǎn)單直接法所無法實(shí)現(xiàn)的半稠密重建。作為一種基于關(guān)鍵幀的SLAM系統(tǒng),LSD-SLAM的主要處理流程為:
通過直接法對(duì)相機(jī)位姿進(jìn)行追蹤,當(dāng)當(dāng)前幀所包含的信息與最后一個(gè)關(guān)鍵幀有足夠差別時(shí),建立新的關(guān)鍵幀。
對(duì)當(dāng)前關(guān)鍵幀的深度信息進(jìn)行估計(jì),對(duì)于雙目LSD-SLAM,首先根據(jù)左右圖像的視差估計(jì)深度,然后結(jié)合不同時(shí)序的幀優(yōu)化深度。
對(duì)全局的關(guān)鍵幀進(jìn)行位姿圖優(yōu)化,獲得全局一致的地圖。
? LSD-SLAM 在 CPU 上實(shí)現(xiàn)了半稠密場(chǎng)景的重建,這在當(dāng)時(shí)(2014)是非常有創(chuàng)見的工作,是作者(TUM 計(jì)算機(jī)視覺組)多年對(duì)直接法進(jìn)行研究的成果。LSD-SLAM 的半稠密追蹤使用了一些精妙的手段,來保證追蹤的實(shí)時(shí)性與穩(wěn)定性。 另一方面,由于 LSD-SLAM 使用了直接法進(jìn)行跟蹤,所以它既有直接法的優(yōu)點(diǎn)(對(duì)特征缺失區(qū)域不敏感),也繼承了直接法的缺點(diǎn)。例如,LSD-SLAM 對(duì)相機(jī)內(nèi)參和曝光非常敏感,并且在相機(jī)快速運(yùn)動(dòng)時(shí)容易丟失。
3.2 ORB-SLAM2 方案
ORB-SLAM 系列是最為經(jīng)典的SLAM 方案之一,ORB-SLAM2 發(fā)表于2017年,是ORB-SLAM的升級(jí)版,同時(shí)支持單目、雙目、RGB-D相機(jī)。ORB-SLAM2的算法流程與ORB-SLAM幾乎相同,共包含三個(gè)模塊(線程),如下所示。
需要注意的是,重定位和回環(huán)檢測(cè)都是基于DBoW2詞袋實(shí)現(xiàn)的,而詞袋需要預(yù)先建立好,通??梢酝ㄟ^一個(gè)大型圖片數(shù)據(jù)集離線建立起詞袋,數(shù)據(jù)集數(shù)據(jù)越廣泛,建立的詞袋越實(shí)用。
3.3 ORB-SLAM3 方案
? 在前作的基礎(chǔ)上,西班牙Zaragoza大學(xué)于2020年7月最新開源了ORB-SLAM3的論文和源碼。ORB-SLAM3支持的設(shè)備和功能更多,支持單目、雙目、RGB-D相機(jī),針孔、魚眼,視覺慣性里程計(jì),多地圖SLAM等,幾乎全覆蓋了視覺SLAM各個(gè)分支??傮w來說,ORB-SLAM3 基本框架、代碼結(jié)構(gòu)都是ORB-SLAM2的延伸,但是加入了很多新的方法,實(shí)現(xiàn)了更好的效果,體現(xiàn)在以下三個(gè)方面:
構(gòu)建了基于特征的高度集成視覺-慣導(dǎo)SLAM系統(tǒng),更加魯棒,適應(yīng)于室內(nèi)/外的大/小場(chǎng)景,精度提升2~5倍。
是一個(gè)多地圖系統(tǒng),VO丟失時(shí),會(huì)建立新的地圖,當(dāng)找回場(chǎng)景后,會(huì)與之前的地圖自動(dòng)融合。
系統(tǒng)魯棒性與state-of-the-art相當(dāng),但精度更高,在無人機(jī)數(shù)據(jù)集上平均精度3.6cm,室內(nèi)數(shù)據(jù)集平均精度9mm。
3.4 DSM 方案:Direct Sparse Mapping
DSM 方案由西班牙的學(xué)者(作者之一同為ORB系列的作者)于2019年發(fā)表,這是一個(gè)基于直接法的完整單目SLAM系統(tǒng)。直接法SLAM系統(tǒng)通常采用光度誤差進(jìn)行BA優(yōu)化,但直接法不能依賴自身解決相同場(chǎng)景的 reobservation問題和相同點(diǎn)的融合問題(因此直接法的回環(huán)檢測(cè)多依賴特征點(diǎn)解決,如LSD-SLAM);DSM則解決了這一問題,是首個(gè)完全基于直接法實(shí)現(xiàn)回環(huán)檢測(cè)和地圖重用的SLAM系統(tǒng),并在 EuRoC 數(shù)據(jù)集上取得了直接法SLAM中最高的精度。 DSM設(shè)計(jì)了獨(dú)特的“局部窗口選擇策略”來選擇共視點(diǎn),通過 coarse-to-fine 的策略來實(shí)現(xiàn) reobservation,并以此實(shí)現(xiàn)回環(huán)檢測(cè);在優(yōu)化方面,仍然基于光度誤差做BA優(yōu)化。
? 基于以上特點(diǎn),DSM 具有較快的運(yùn)行速度(無需計(jì)算特征點(diǎn)描述子),精度很高;除了直接法自身的缺點(diǎn)之外,其缺點(diǎn)還在于單目無法恢復(fù)出尺度信息。
3.5 VINS-Fusion 方案
VINS系列由港科大沈劭劼課題組發(fā)表和公開,其中VINS-Fusion 是繼 VINS-Mono (單目視覺慣導(dǎo) SLAM 方案)后的雙目視覺慣導(dǎo) SLAM 方案,VINS-Fusion 是一種基于優(yōu)化的多傳感器狀態(tài)估計(jì)器,可實(shí)現(xiàn)自主應(yīng)用(無人機(jī),無人車, AR / VR)的精確自定位。VINS-Fusion 是 VINS-Mono 的擴(kuò)展,支持多種視覺慣性傳感器類型(單目相機(jī)+ IMU,雙目相機(jī)+ IMU,雙目相機(jī)-only),但兩者的技術(shù)框架是相同的,如下圖所示。其主要技術(shù)模塊有:
輕量級(jí)前端(視覺光流跟蹤+IMU預(yù)積分)
傳感器在線標(biāo)定+視覺慣導(dǎo)聯(lián)合初始化
緊耦合滑窗優(yōu)化(同時(shí)具有快速重定位功能)
回環(huán)檢測(cè)
全局四自由度位姿優(yōu)化
(注:紅色加粗表示核心工作)
? VINS方案本身是基于特征點(diǎn)的,因此只能用于稀疏建圖。VINS的優(yōu)點(diǎn)在于實(shí)現(xiàn)了很高的定位精度,這種精度來源于視覺+IMU融合以及緊耦合滑窗優(yōu)化算法。VINS對(duì)計(jì)算資源的消耗比較小,在普通CPU上既可以實(shí)現(xiàn)在線實(shí)時(shí)運(yùn)行,適合于對(duì)實(shí)時(shí)性有要求的移動(dòng)設(shè)備。
3.6 ElasticFusion 方案
ElasticFusion (2016) 是由帝國理工發(fā)表的一項(xiàng)優(yōu)秀RGB-D SLAM系統(tǒng),具有稠密建圖、在線實(shí)時(shí)運(yùn)行、輕量級(jí)等顯著特點(diǎn)。如前文所述,ElasticFusion 以稠密建圖為主要目標(biāo)(而非定位),建圖的精度和質(zhì)量是主要指標(biāo)。ElasticFusion 的技術(shù)特點(diǎn)如下:
基于 RGB-D 的稠密三維重建一般使用網(wǎng)格模型融合點(diǎn)云,ElasticFusion 是為數(shù)不多使用 surfel 模型的方案。
傳統(tǒng)的 SLAM 算法一般通過優(yōu)化位姿或者路標(biāo)點(diǎn)來提高精度,而 ElasticFusion 采用優(yōu)化 deformation graph 的方式。
融合了重定位算法(當(dāng)相機(jī)跟丟時(shí),重新計(jì)算相機(jī)的位姿)。
ElasticFusion 算法融合了 RGB信息(顏色一致性約束) 和深度信息(ICP 算法)進(jìn)行位姿估計(jì)。
ElasticFusion首先根據(jù)RGB-D圖像配準(zhǔn)估算位姿,根據(jù)位姿誤差決定進(jìn)行重定位還是回環(huán)檢測(cè);若存在回環(huán),則首先優(yōu)化Deformation graph 然后優(yōu)化 surfel地圖;若不存在,則更新和融合全局地圖,并估算當(dāng)前視角下的模型,用于下一幀圖像配準(zhǔn)??傮w來說,ElasticFusion具有較高的重建精度,在重建房間大小的場(chǎng)景時(shí)效果很好;但沒有對(duì)代碼做特別的優(yōu)化,在大場(chǎng)景重建時(shí)效果不佳。
?
3.7?InfiniTAM 方案
InfiniTAM 是牛津大學(xué)于2016年發(fā)表的稠密 SLAM 方案,該方案基于 KinectFusion 和 體素塊哈希表(voxel block hashing)發(fā)展而來。首先,InfiniTAM 方案在建圖部分利用 TSDF 模型(截?cái)喾?hào)距離場(chǎng))進(jìn)行建模,只是在建模的時(shí)候,不是對(duì)整個(gè)空間都劃分等大小的網(wǎng)格,而是只在場(chǎng)景表面的周圍劃分網(wǎng)格(voxel blocks),且只為待重建的表面上的Voxel block分配顯存,并使用哈希表這一結(jié)構(gòu)來管理GPU對(duì)Voxel block的內(nèi)存分配和數(shù)據(jù)訪問。通過這樣的方法,InfiniTAM 大大減小了稠密建圖對(duì) GPU 的內(nèi)存消耗,提升了算法效率。 InfiniTAM 的?算法流程?如右圖所示,InfiniTAM 系統(tǒng)的前端與KinectFusion較為相似,主要分為三個(gè)階段:
跟蹤階段:對(duì)新輸入的圖像進(jìn)行定位,估算對(duì)應(yīng)的相機(jī)位姿;
融合階段:用于將新數(shù)據(jù)集成到現(xiàn)有的3D世界模型當(dāng)中(更新voxel中存儲(chǔ)的SDF值);
渲染階段:利用光線投影算法(Raycasting)從世界模型中提取與下一個(gè)跟蹤步驟相關(guān)的模型區(qū)域,用于下一幀的位姿估算;
在后端部分,InfiniTAM 維護(hù)一個(gè) Active Submaps List(基于Voxel block + Hashing),從而實(shí)現(xiàn)輕量級(jí)的后端優(yōu)化。
? InfiniTAM 是一個(gè)完整的稠密SLAM系統(tǒng),包括前端位姿估計(jì),回環(huán)檢測(cè),重定位,后端優(yōu)化等全部SLAM功能。InfiniTAM使用哈希表來管理體素(voxel),大大節(jié)約了GPU內(nèi)存占用,因此可以構(gòu)建更大規(guī)模場(chǎng)景的地圖。
?
3.8?BundleFusion 方案
斯坦福大學(xué)2017年提出的BundleFusion技術(shù),被認(rèn)為可能是目前基于RGB-D相機(jī)進(jìn)行稠密三維重建效果最好的方法。BundleFusion的整體流程是比較清晰的:輸入的color+depth的數(shù)據(jù)流首先需要做幀與幀之間的匹配信息搜索,然后基于“稀疏特征+稠密特征”融合匹配以求得精確位姿變化;把連續(xù)相鄰幀聚合為“幀段”并在幀段內(nèi)做局部位姿優(yōu)化,獲得幀段地圖/Chunk;然后基于“幀段”做全局位姿優(yōu)化,將整體的漂移矯正;融合相鄰Chunk,得到全局位姿和地圖。整個(gè)過程持續(xù)動(dòng)態(tài)更新。
? 【融合匹配】在匹配方面,論文使用的是一種sparse-then-dense的并行全局優(yōu)化方法。也就是說,先使用稀疏的SIFT特征點(diǎn)來進(jìn)行比較粗糙的配準(zhǔn),因?yàn)橄∈杼卣鼽c(diǎn)本身就可以用來做loop closure檢測(cè)和relocalization。然后使用稠密的幾何和光度連續(xù)性進(jìn)行更加細(xì)致的配準(zhǔn)。圖1展示了sparse+dense這種方式和單純sparse的對(duì)比結(jié)果。 【優(yōu)化】在位姿優(yōu)化方面,論文使用了一種分層的 local-to-global 優(yōu)化方法,如圖2所示??偣卜譃閮蓪?,在第一層,每連續(xù)10幀組成一個(gè)chunk,第一幀作為關(guān)鍵幀,然后對(duì)chunk內(nèi)所有幀做局部位姿優(yōu)化。在第二層,只使用所有的chunk的關(guān)鍵幀進(jìn)行互相關(guān)聯(lián)然后進(jìn)行全局優(yōu)化。通過這種分層處理技巧,一方面可以剝離出關(guān)鍵幀,減少存儲(chǔ)和待處理的數(shù)據(jù);另一方面,分層優(yōu)化方法減少了每次優(yōu)化時(shí)的未知量,保證該方法可擴(kuò)展到大場(chǎng)景而漂移很小。
▲?local-to-global 優(yōu)化策略
▲?稀疏匹配與融合匹配效果對(duì)比 BundleFusion 方案在算法上具有明顯的優(yōu)點(diǎn):
使用持續(xù)的local to global分層優(yōu)化,去除了時(shí)域跟蹤的依賴。
不需要任何顯示的loop closure檢測(cè)。因?yàn)槊恳粠己蜌v史幀相關(guān),所以其實(shí)包含了持續(xù)的隱式的loop closure。
支持在GPU上實(shí)時(shí)魯棒跟蹤,可以在跟蹤失敗時(shí)移動(dòng)到重建成功的地方進(jìn)行relocalization,匹配上后繼續(xù)跟蹤。
實(shí)驗(yàn)表明,BundleFusion的重建效果確實(shí)是目前該領(lǐng)域效果最好的方法,下圖是和其他方法的對(duì)比,重建優(yōu)勢(shì)明顯。
? BundleFusion 的良好效果除了算法的功勞,還必須有強(qiáng)大硬件算力的支撐。當(dāng)使用兩塊GPU(GTX Titan X + GTX Titan Black)時(shí),BundleFusion在各大數(shù)據(jù)集上可以達(dá)到 36 fps 左右的處理深度;當(dāng)使用一塊 GTX Titan X 時(shí),處理速度則是 20 fps 左右。也即:目前算法需要兩塊GPU才能實(shí)時(shí)運(yùn)行,因此算法的優(yōu)化和加速仍是可改進(jìn)的地方。
?
?
04? 激光SLAM中的挑戰(zhàn)和階段分析
?
4.1 非結(jié)構(gòu)化道路的SLAM問題?抑或者激光SLAM的退化特性?
在廣場(chǎng)、機(jī)場(chǎng)等開闊區(qū)域,即使是多線激光,也只能看到幾圈地面上的點(diǎn)云。僅使用地面點(diǎn)云進(jìn)行匹配,很可能在水平面上發(fā)生隨機(jī)移動(dòng)。
在長隧道、單側(cè)墻、橋梁等場(chǎng)地中,激光匹配會(huì)存在一個(gè)方向上的額外自由度。也就是說,沿著隧道前進(jìn)時(shí),獲取到的激光點(diǎn)云是一樣的,使得匹配算法無法準(zhǔn)確估計(jì)這個(gè)方向上的運(yùn)動(dòng)。類似地,如果機(jī)器繞著一個(gè)圈柱形物體運(yùn)動(dòng)時(shí),也會(huì)發(fā)生這種情況。
在一些異形建筑面前,激光可能發(fā)生意想不到的失效情形。
解決思路:建圖算法適當(dāng)降低激光軌跡的權(quán)重,利用其他軌跡(或者數(shù)據(jù)約束)來補(bǔ)償激光的失效。
?
4.2 地圖表達(dá)與實(shí)際環(huán)境的差異?
機(jī)器人用的柵格地圖,主要表達(dá)何處有障礙物,何處是可通行的區(qū)域,此外就沒有了。它具有基礎(chǔ)的導(dǎo)航與定位功能,精度也不錯(cuò)(厘米級(jí)),制作起來十分簡(jiǎn)單,基本可以讓機(jī)器人自動(dòng)生成。
但除了通行區(qū)域,還有什么是需要機(jī)器人從地圖中獲取的環(huán)境信息?
?
4.3 占柵格地圖和高精地圖有哪些差別?
在室內(nèi),機(jī)器人可以去任意可以通過的地方,不會(huì)有太多阻攔,而對(duì)于自動(dòng)駕駛來說,每條路都有對(duì)應(yīng)的交通規(guī)則:有些地方只能靠右行駛,有些地方不能停車,十字路口還有復(fù)雜的通行規(guī)則。
室內(nèi)機(jī)器人可以利用柵格地圖進(jìn)行導(dǎo)航,但在室外可不能在十字路口上橫沖直撞。所以,在導(dǎo)航層面,室內(nèi)與室外的機(jī)器人出現(xiàn)了明顯的區(qū)別。室內(nèi)的導(dǎo)航可以基于柵格來實(shí)現(xiàn)諸如A*那樣的算法,但室外基本要依賴事先畫好的車道。
但自動(dòng)化的高精地圖標(biāo)注該如何進(jìn)行和實(shí)現(xiàn)工程落地?
目前SLAM程序的流程大抵上都一樣,基本的流程都是:
對(duì)陌生環(huán)境進(jìn)行一次掃描,建立地圖;
保存地圖;
以后運(yùn)行時(shí),打開這張地圖進(jìn)行定位。
而在第一和第三流程階段,回到文章的開頭,其本質(zhì)就是State Estimation in Robotics。而由此進(jìn)行分析的話,受到一致認(rèn)可的方向大抵有以下三類:
精度:傳感器選型、傳感器布局和傳感器標(biāo)定,但若相同的傳感器“條件”,精度本質(zhì)上還是由硬件決定了上限;
魯棒性:多傳感器融合,傳感器差異特性互補(bǔ);
效率:點(diǎn)云配準(zhǔn) & 優(yōu)化計(jì)算的效率提升
05??總結(jié)
以上內(nèi)容都是經(jīng)過網(wǎng)上各位大佬的經(jīng)驗(yàn)分享和個(gè)人簡(jiǎn)單的整理,匯整而成,本人也是十分感激能站在巨人們的肩膀上看整個(gè)SLAM技術(shù)大體樣貌。望與各位技術(shù)同僚共勉,一同在SLAM技術(shù)道路上高歌猛進(jìn)。 審核編輯:黃飛
?
評(píng)論