SLAM系統一般分為五個模塊,系統框架如圖所示,包括:
傳感器數據:主要用於採集實際環境中的各類型原始數據。包括雷射掃描數據、視頻圖像數據、點雲數據等。
視覺裡程計:主要用於不同時刻間移動目標相對位置的估算。包括特徵匹配、直接配準等算法的應用。
後端:主要用於優化視覺裡程計帶來的累計誤差。包括濾波器、圖優化等算法應用。
建圖:用於三維地圖構建。
迴環檢測:主要用於空間累積誤差消除。
其工作流程大致為:
傳感器讀取數據後,視覺裡程計估計兩個時刻的相對運動(Ego-motion),後端處理視覺裡程計估計結果的累積誤差,建圖則根據前端與後端得到的運動軌跡來建立三維地圖,迴環檢測考慮了同一場景不同時刻的圖像,提供了空間上約束來消除累積誤差。
(一)依據傳感器的特點不同,SLAM技術也分為雷射和視覺兩個技術路線
1、雷射雷達傳感器
雷射雷達利用雷射在同一空間參考系下獲取物體表面每個採樣點的空間坐標,可得到一系列表達目標空間分布和目標表面特性的海量點的集合。由於雷射雷達距離測量比較準確,誤差模型簡單,在強光直射以外的環境中運行穩定,因此點雲的處理也比較容易。同時,點雲信息本身包含直接的幾何關係,使得機器人的路徑規劃和導航變得直觀。雷射SLAM理論研究也相對成熟,落地產品更豐富。
雷射雷達SLAM發展
基於雷射雷達的SLAM(Lidar SLAM)採用2D或3D雷射雷達(也叫單線或多線雷射雷達)。在室內機器人(如掃地機器人)上,一般使用2D雷射雷達,在無人駕駛領域,一般使用3D雷射雷達。
現任Udacity創始人CEO、前Google副總裁、谷歌無人車領導者Sebastian Thrun在他2005年的經典著作《ProbabilisticRobotics》一書中詳細闡述了利用2D雷射雷達基於概率方法進行地圖構建和定位的理論基礎,並闡述了基於RBPF(Rao-blackwellizedparticle filter)粒子濾波器的FastSLAM方法,成為後來2D雷射雷達建圖的標準方法之一GMapping的基礎,該算法也被集成到機器人作業系統(Robot Operation System,ROS)中。2013年,對ROS中的幾種2DSLAM的算法HectorSLAM,KartoSLAM,CoreSLAM,LagoSLAM和GMapping做了比較評估。2016年,Google開源其雷射雷達SLAM算法庫Cartographer,它與GMapping相比,能夠有效處理閉環,達到了較好的效果。
2、視覺傳感器
早期的視覺SLAM基於濾波理論,其非線性的誤差模型和巨大的計算量成為了它實用落地的障礙。近年來,隨著具有稀疏性的非線性優化理論(Bundle Adjustment)以及相機技術、計算性能的進步,實時運行的視覺SLAM已經不再是夢想。
2、視覺SLAM發展
現在主流的視覺SLAM系統的特性主要體現在前端和後端,如圖5所示。
前端對應視覺裡程計,主要用於數據關聯,研究幀與幀之間變換關係,完成實時的位姿跟蹤,對輸入的圖像進行處理,計算姿態變化。由於與後端還存在反饋,因此也檢測並處理閉環,當有慣性測量單元(IMU)信息時,還可以參與融合計算。
後端主要對前端的輸出結果進行優化,利用濾波理論,如擴展卡爾曼濾波(EKF)、粒子濾波(PF)等優化理論進行樹或圖的優化,得到最優的位姿估計和地圖。
2007年,第一個成功基於單目攝像頭的純視覺SLAM 系統MonoSLAM[3] 誕生。同年,Murray和Klein提出了實時SLAM系統PTAM(ParallelTracking and Mapping)[4], PTAM實現了架構創新,它將姿態跟蹤(Tracking)和建圖(Mapping)兩個線程分開並行進行,這在當時是一個創舉,第一次讓大家覺得對地圖的優化可以整合到實時計算中。這種設計為後來的實時SLAM(如ORB-SLAM)所效仿,成為了現代SLAM系統的標配。
圖 PTAM系統與EKF-SLAM系統產生結果對比
2015年,Mur-Artal 等提出了開源的單目ORB-SLAM[6],並於2016年拓展為支持雙目和RGBD傳感器的ORB-SLAM2[7],它是目前支持傳感器最全且性能最好的視覺SLAM系統之一,也是所有在KITTI數據集上提交結果的開源系統中排名最靠前的一個。
如圖7所示,ORB-SLAM主要分為三個線程進行,即跟蹤(Tracking)、建圖(LocalMapping)和閉環檢測(LoopClosing)。跟蹤部分是主要是從圖像中提取ORB特徵,根據上一幀進行姿態估計,或者進行通過全局重定位初始化位姿,然後跟蹤已經重建的局部地圖,優化位姿,再根據一些規則確定新的關鍵幀。建圖部分主要完成局部地圖構建。包括對關鍵幀的插入,驗證最近生成的地圖點並進行篩選,然後生成新的地圖點,使用局部捆集調整(Local BA),最後再對插入的關鍵幀進行篩選,去除多餘的關鍵幀。閉環檢測部分主要分為兩個過程,分別是閉環探測和閉環校正。閉環檢測先使用二進位詞典匹配檢測(WOB)進行探測,然後通過Sim3算法計算相似變換。閉環校正,主要是閉環融合和Essential Graph的圖優化。由於ORB-SLAM系統是基於特徵點的SLAM系統,故其能夠實時計算出相機的軌線,並生成場景的稀疏三維重建結果。
2016年,TUM機器視覺組的Engel等人又提出了DSO系統[9],該系統是一種新的基於直接法和稀疏法的視覺裡程計,它將最小化光度誤差模型和模型參數聯合優化方法相結合。為了滿足實時性,不對圖像進行光滑處理,而是對整個圖像均勻採樣(如圖8)。該系統在TUM monoVO、EuRoC MAV和ICL-NUIM三個數據集上進行了測試,達到了很高的跟蹤精度和魯棒性。
圖 DSO系統
2017年,香港科技大學的沈紹劼老師課題組提出了融合慣性測量單元(IMU)和視覺信息的VINS系統[10],同時開源手機和Linux兩個版本的代碼,這是首個直接開源手機平臺代碼的視覺IMU融合SLAM系統。這個系統可以運行在iOS設備上,為手機端的增強現實應用提供精確的定位功能,同時該系統也在應用在了無人機控制上,並取得了較好的效果。
SLAM前20年(1986-2004)可稱之為「古典年代」。古典年代時期,引入了SLAM概率論推導方法,包括基於擴展卡爾曼濾波、粒子濾波和最大似然估計;這裡的第一個挑戰是效率和數據關聯的魯棒性問題。
接下來的年代,我們稱之為「算法分析」年代(2004-2015),在算法分析的年代,有許多SLAM基本特性的研究,包括可觀測性,收斂性和一致性。在這一時期,研究者們理解了稀疏特徵在高效SLAM解決方案中的重要角色,開發了主要開源SLAM庫。
現在我們正進入SLAM的第三個時代,從魯棒性覺醒的時代,它具有以下特徵:
1.魯棒性能:這一時期SLAM系統在任意環境總運行的失效率較低;系統具有失效保護機制且具有自動調整的功能可以根據應用場景自動選擇系統參數;
2.高層次的理解力:SLAM系統已經除了基本的幾何重建還能夠在更高層次上理解環境信息(比如,語義、可供性、高層幾何意義、物理意義等);
3.資源敏感:SLAM系統可以進行裁剪以滿足現有傳感器和計算資源,也可以根據現有資源調整計算負載;
4.對任務驅動的判斷:SLAM系統可以自適應地表示地圖,其複雜度可以根據機器人執行的任務而改變。
地理信息在視頻監控中有著廣泛的應用,傳統的二維地理信息數據無法與平安城市監控系統的業務數據進行有效的集成和關聯,無法提供深層次數據服務,已經不能適應城市發展的需要與人民對優質生活的要求。若將SLAM三維稠密地圖構建用於視頻監控領域,可以更好地實現對監控攝像頭的搜索定位和視頻決策與分析。在三維地圖內模擬雲臺操作也可直接查看三維內效果和視頻效果,這便於在三維地圖內進行可視調整,調試出最佳監控點位,便於攝像頭的安裝及後期維護。
Visual Odometry,即視覺裡程計。它估計兩個時刻機器人的相對運動(Ego-motion)。在雷射SLAM中,我們可以將當前的觀測與全局地圖進行匹配,用ICP求解相對運動。而對於相機,它在歐氏空間裡運動,我們經常需要估計一個三維空間的變換矩陣——SE3或Sim3(單目情形)。求解這個矩陣是VO的核心問題,而求解的思路,則分為基於特徵的思路和不使用特徵的直接方法。
特徵匹配
基於特徵的方法是目前VO的主流方式。對於兩幅圖像,首先提取圖像中的特徵,然後根據兩幅圖的特徵匹配,計算相機的變換矩陣。最常用的是點特徵,例如Harris角點、SIFT、SURF、ORB。如果使用RGBD相機,利用已知深度的特徵點,就可以直接估計相機的運動。給定一組特徵點以及它們之間的配對關係,求解相機的姿態,該問題被稱為PnP問題(Perspective-N-Point)。PnP可以用非線性優化來求解,得到兩個幀之間的位置關係。
不使用特徵進行VO的方法稱為直接法。它直接把圖像中所有像素寫進一個位姿估計方程,求出幀間相對運動。例如,在RGBD SLAM中,可以用ICP(Iterative Closest Point,迭代最近鄰)求解兩個點雲之間的變換矩陣。對於單目SLAM,我們可以匹配兩個圖像間的像素,或者像圖像與一個全局的模型相匹配。直接法的典型例子是SVO和LSD-SLAM。它們在單目SLAM中使用直接法,取得了較好的效果。目前看來,直接法比特徵VO需要更多的計算量,而且對相機的圖像採集速率也有較高的要求。
在VO估計幀間運動之後,理論上就可以得到機器人的軌跡了。然而視覺裡程計和普通的裡程計一樣,存在累積誤差的問題(Drift)。直觀地說,在t1和t2時刻,估計的轉角比真實轉角少1度,那麼之後的軌跡就全部少掉了這1度。時間一長,建出的房間可能由方形變成了多邊形,估計出的軌跡亦會有嚴重的漂移。所以在SLAM中,還會把幀間相對運動放到一個稱之為後端的程序中進行加工處理。
早期的SLAM後端使用濾波器方式。由於那時還未形成前後端的概念,有時人們也稱研究濾波器的工作為研究SLAM。SLAM最早的提出者R. Smith等人就把SLAM建構成了一個EKF(Extended Kalman Filter,擴展卡爾曼濾波)問題。他們按照EKF的形式,把SLAM寫成了一個運動方程和觀測方式,以最小化這兩個方程中的噪聲項為目的,使用典型的濾波器思路來解決SLAM問題。
當一個幀到達時,我們能(通過碼盤或IMU)測出該幀與上一幀的相對運動,但是存在噪聲,是為運動方程。同時,通過傳感器對路標的觀測,我們測出了機器人與路標間的位姿關係,同樣也帶有噪聲,是為觀測方程。通過這兩者信息,我們可以預測出機器人在當前時刻的位置。同樣,根據以往記錄的路標點,我們又能計算出一個卡爾曼增益,以補償噪聲的影響。於是,對當前幀和路標的估計,即是這個預測與更新的不斷迭代的過程。
21世紀之後,SLAM研究者開始借鑑SfM(Structure from Motion)問題中的方法,把捆集優化(Bundle Adjustment)引入到SLAM中來。優化方法和濾波器方法有根本上的不同。它並不是一個迭代的過程,而是考慮過去所有幀中的信息。通過優化,把誤差平均分到每一次觀測當中。在SLAM中的Bundle Adjustment常常以圖的形式給出,所以研究者亦稱之為圖優化方法(Graph Optimization)。圖優化可以直觀地表示優化問題,可利用稀疏代數進行快速的求解,表達迴環也十分的方便,因而成為現今視覺SLAM中主流的優化方法。
迴環檢測,又稱閉環檢測(Loop closure detection),是指機器人識別曾到達場景的能力。如果檢測成功,可以顯著地減小累積誤差。迴環檢測實質上是一種檢測觀測數據相似性的算法。對於視覺SLAM,多數系統採用目前較為成熟的詞袋模型(Bag-of-Words, BoW)。詞袋模型把圖像中的視覺特徵(SIFT, SURF等)聚類,然後建立詞典,進而尋找每個圖中含有哪些「單詞」(word)。也有研究者使用傳統模式識別的方法,把迴環檢測建構成一個分類問題,訓練分類器進行分類。
迴環檢測的難點在於,錯誤的檢測結果可能使地圖變得很糟糕。這些錯誤分為兩類:1.假陽性(False Positive),又稱感知偏差(Perceptual Aliasing),指事實上不同的場景被當成了同一個;2.假陰性(False Negative),又稱感知變異(Perceptual Variability),指事實上同一個場景被當成了兩個。感知偏差會嚴重地影響地圖的結果,通常是希望避免的。一個好的迴環檢測算法應該能檢測出儘量多的真實迴環。研究者常常用準確率-召回率曲線來評價一個檢測算法的好壞。
目前,SLAM(即時定位與地圖構建)技術主要被運用於無人機、無人駕駛、機器人、AR、智能家居等領域,從各應用場景入手,促進消費升級。
VR/AR 方面:輔助增強視覺效果。SLAM技術能夠構建視覺效果更為真實的地圖,從而針對當前視角渲染虛擬物體的疊加效果,使之更真實沒有違和感。VR/AR代表性產品中微軟Hololens、谷歌ProjectTango以及MagicLeap都應用了SLAM作為視覺增強手段。
無人機領域及機器人定位導航領域:地圖建模。SLAM可以快速構建局部3D地圖,並與地理信息系統(GIS)、視覺對象識別技術相結合,可以輔助無人機識別路障並自動避障規劃路徑,還可以輔助機器人執行路徑規劃、自主探索、導航等任務。曾經刷爆美國朋友圈的Hover camera無人機,正是應用SLAM技術。國內的科沃斯、塔米掃地機通過用SLAM算法結合雷射雷達或者攝像頭的方法,讓掃地機可以高效繪製室內地圖,智能分析和規劃掃地環境,從而成功讓自己步入了智能導航的陣列。
無人駕駛領域:視覺裡程計。SLAM 技術可以提供視覺裡程計功能,並與GPS等其他定位方式相融合,從而滿足無人駕駛精準定位的需求。例如,應用了基於雷射雷達技術Lidar Slam的Google無人駕駛車以及牛津大學Mobile Robotics Group11年改裝的無人駕駛汽車野貓(Wildcat)均已成功路測。
機器人
雷射+SLAM是目前機器人自主定位導航所使用的主流技術。雷射測距相比較於圖像和超聲波測距,具有良好的指向性和高度聚焦性,是目前最可靠、穩定的定位技術。雷射雷達傳感器獲取地圖信息,構建地圖,實現路徑規劃與導航。
無人駕駛
無人駕駛是近年來較火的話題之一,Google、Uber、百度等企業都在加速研發無人駕駛相關技術,搶佔先機。
隨著城市物聯網和智能系統的完善,無人駕駛必是大勢所趨。無人駕駛利用雷射雷達傳感器(Velodyne、IBEO等)作為工具,獲取地圖數據,並構建地圖,規避路程中遇到的障礙物,實現路徑規劃。跟SLAM技術在機器人領域的應用類似,只是相比較於SLAM在機器人中的應用,無人駕駛的雷達要求和成本要明顯高於機器人。
無人機
無人機在飛行的過程中需要知道哪裡有障礙物,該怎麼規避,怎麼重新規劃路線。顯然,這是SLAM技術的應用。但無人機飛行的範圍較大,所以對精度的要求不高,市面上其他的一些光流、超聲波傳感器可以作為輔助。
AR
AR通過電腦技術,將虛擬的信息應用到真實世界,真實的環境和虛擬的物體實時地疊加到了同一個畫面或空間同時存在。這一畫面的實現,離不開SLAM技術的實時定位。雖然在AR行業有很多可代替技術,但是,SLAM技術是最理想的定位導航技術。
相較於SLAM在機器人、無人駕駛等領域的應用,在AR行業的應用則有很多不同點。
1、精度上:AR一般更關注於局部精度,要求恢復的相機運動避免出現漂移、抖動,這樣疊加的虛擬物體才能看起來與現實場景真實地融合在一起。但在機器人和無人駕駛領域則一般更關注全局精度,需要恢復的整條運動軌跡誤差累積不能太大,循環迴路要能閉合,而在某個局部的漂移、 抖動等問題往往對機器人應用來說影響不大。
2、效率上:AR需要在有限的計算資源下實時求解,人眼的刷新率為24幀,所以AR的計算效率通常需要到達30幀以上; 機器人本身運動就很慢,可以把幀率降低,所以對算法效率的要求相對較低。
3、配置上:AR對硬體的體積、功率、成本等問題比機器人更敏感,比如機器人上可以配置魚眼、雙目或深度攝像頭、高性能CPU等硬體來降低SLAM的難度,而AR應用更傾向於採用更為高效、魯邦的算法達到需求。
多傳感器融合、優化數據關聯與迴環檢測、與前端異構處理器集成、提升魯棒性和重定位精度都是SLAM技術接下來的發展方向,但這些都會隨著消費刺激和產業鏈的發展逐步解決。就像手機中的陀螺儀一樣,在不久的將來,也會飛入尋常百姓家,改變人類的生活。