移動機器人SLAM關鍵問題和解決方法綜述①

楊雪夢, 姚敏茹, 曹 凱

(西安工業大學 電子信息工程學院, 西安 710021)

同時定位與地圖構建(Simultaneous Localization And Mapping, SLAM), 是搭載特定傳感器的主體在沒有環境先驗信息的情況下(即未知環境下), 在運動過程中建立環境的模型, 并同時估計自身的運動[1]. 如今SLAM已經成為移動機器人領域研究的熱門問題.

移動機器人對未知環境的探索一直是其自主化研究的熱點和難點, 而SLAM則是其中一個重要的基本問題, 是實現移動機器人自主定位、自主導航、路徑規劃、目標識別及目標跟蹤等任務的基礎和前提, 并應用在地面機器人[2,3]、水下機器人[4,5]、無人機[6,7]等平臺上. SLAM技術被認為是移動機器人是否能夠實現自主化的關鍵. 而隨著人類社會的不斷進步和發展,人們在生產生活中對智能移動產品的需求與日俱增,移動機器人的應用環境因此更趨復雜化和非結構化,SLAM問題已成為自主移動機器人面臨的急需解決的重要基礎性難題. 而在互聯網與科技創新的今天,SLAM技術在物流、教育、醫療、服務、娛樂等行業有著十分廣闊的應用前景.

很多機器人對環境的構建都是在環境地圖已知的情況下完成的, 但是通常情況下卻很難事先獲取環境地圖, 而SLAM正是提供了一種未知環境下同時定位與地圖構建的方法. SLAM主要包含機器人狀態估計和環境地圖構建兩大部分, 機器人的狀態估計通過其位置和方向來描述, 環境地圖則描述了機器人的運動環境, 該地圖的構建通過地標和障礙的位置來完成. 該地圖的構建不僅是為了支持額外的任務如路徑規劃、導航等, 也是為了能夠修正狀態估計的定位誤差, 在沒有地圖信息時能夠減小漂移帶來的誤差, 在有地圖時通過重新訪問已知區域來“重置”誤差(也稱閉環)[8–10].所以說, SLAM就是在移動機器人不斷運動過程中, 通過自身傳感器對自身位置進行估計的同時構建周圍環境地圖的過程. 因此同時定位與地圖構建是移動機器人導航研究的基礎, 也是移動機器人實現自主性的最重要條件之一.

1 SLAM 發展現狀

上世紀80年代時的SLAM問題是一個狀態估計問題, 最早是由 Smith, Self和 Cheeseman 提出來的[11],當時被稱為“空間狀態不確定性的估計”. 由此開始的三十年, 前一階段的二十年來主要研究了基于概率估計的SLAM[12,13], 例如擴展卡爾曼濾波、粒子濾波和最大似然估計, 這一階段面臨著數據關聯穩定性和算法有效性的挑戰; 之后的十年則是算法的分析, 主要包含算法的可觀測性、收斂性、一致性問題, 濾波算法變得日趨成熟.

隨著計算機視覺的發展, 在2006年視覺SLAM作為一個新的分支被提出[14]并受到研究者的關注, 直到2012年視覺SLAM成為機器人領域的熱點問題. 而在這個過程中, SLAM逐漸被劃分為兩個部分, 首先是前端, 通過傳感器的觀測來獲取相關信息, 主要涉及計算機視覺及信號處理相關理論, 如圖像的特征提取與匹配等; 其次是后端, 對獲取的信息篩選優化并得到有效信息, 其中幾何、圖論、優化、概率估計等都是所涵蓋的研究內容, 主要涉及濾波及非線性優化, 如回環檢測、位姿圖優化. 表1對基于概率估計的和視覺SLAM方法作了詳盡的比較. 隨著SLAM技術的不斷發展, 基于多機器人的SLAM[15–17]也成為很多研究者關注的方向.

基于此, 本文從不確定性、數據關聯、環境地圖表達三大關鍵問題出發, 詳細闡述了移動機器人SLAM實現方法的研究現狀, 并討論了目前視覺SLAM存在的問題及面臨的挑戰, 給出未來SLAM發展趨勢.

表1 基于概率估計的和視覺SLAM 方法比較

2 SLAM 關鍵問題

2.1 不確定性問題

SLAM的不確定[11,18]主要來源于機器人系統和非系統誤差. 前者主要是機器人或傳感器本身存在的參數不準確, 后者則是由于外界環境所導致的誤差. 其不確定主要體現在以下3方面:

(1) 移動機器人輪子的打滑、未知外力的影響都會造成一定的不確定性;

(2) 移動機器人傳感器對環境特征觀測的不確定性, 主要是傳感器本身參數(如分辨率、測量精度等)以及噪聲的影響;

(3) 由于觀測噪聲等的影響, 觀測數據與地圖數據對應匹配時可能會導致數據關聯錯誤.

由于移動機器人SLAM不確定性, 導致難以建立準確的模型. 就傳感器觀測數據的不確定性來說, 可采用高分辨率或者多傳感器數據融合的辦法來解決, 最大程度減小誤差積累. 通過多傳感器數據融合可提高SLAM準確性, 如文獻[7]利用視覺和慣性傳感器(如IMU), 充分結合圖像和測量信息(加速度和角速度)進行綜合處理, 減少客觀存在的不確定性引起的誤差.

2.2 數據關聯問題

數據關聯問題指建立當前傳感器觀測數據與地圖中已觀測數據間的對應關系, 確定是否都來源于同一特征. 數據關聯在一定程度上體現了SLAM方法的準確定, 其主要受以下三點因素的影響:

(1) 由于錯誤的數據關聯會導致對環境地圖更新以及位姿估計的錯誤, 所以對于環境特征的提取和描述有著很高的穩定性要求.

(2) 傳感器的測量噪聲、動態環境特征等不確定因素也會影響數據關聯的準確性.

(3) 數據關聯算法的選擇也非常重要, 這直接影響著機器人SLAM的準確性和魯棒性. 現在已有的一些算法有最近鄰數據關聯法 (Nearest Neighbor, NN)[19]、基于概率的數據關聯 (Probability Data Association,PDA)[20]、基于幾何關系的數據關聯[21]等算法.

在視覺SLAM上把這種數據關聯也稱為回環檢測[22,23], 其實質上就是判斷經過的地方是否為曾經經過的同一地方, 是一種對圖像相似性的檢測方法. 隨著時間的推移, 誤差累積逐漸增加. 如圖1 橢圓圈所示, 可以直觀的看出由于積累誤差導致的漂移, 這使得構建的地圖邊界不一致. 所以它關系到我們估計的軌跡和地圖邊界在一段時間內的準確性.

圖1 積累誤差導致的地圖不準確示意圖

2.3 環境地圖的表達問題

移動機器人通過傳感器來感知周圍環境, 最終建立自己的環境地圖. 研究者對于地圖存在不同需求, 構建地圖主要是服務于定位, 則需要建立與任務要求對應的地圖. 構建的地圖需滿足以下三點要求:

(1) 地圖要能準確描述環境特征;

(2) 在有噪聲干擾等不確定信息的存在下準確估計機器人的位姿;

(3) 地圖的構建能夠充分展示環境的特征信息, 根據不同的任務需求建立相應的地圖模型, 并保證SLAM的精度. 在保證精度的同時, 盡量減少地圖創建過程中的數據量, 來提高SLAM算法的實時性.

常用的一些環境地圖的構建方法有[24](如圖2所示): 2D柵格地圖、2D拓撲地圖、3D點云地圖、3D網格地圖, 以及近兩年剛興起的八叉樹[25], 該方法占用存儲空間小且能夠實現動態建模, 在實時性上優于3D點云地圖.

圖2 四種建圖方法 (①柵格地圖; ②拓撲地圖; ③點云地圖;④八叉樹)

3 SLAM 研究方法

由于SLAM問題涉及未知且不確定的環境信息和干擾噪聲, 一般通過概率方法來描述該問題. 卡爾曼濾波方法是多種SLAM算法的基礎, 但是其計算量大和對數據關聯不敏感的缺點也十分明顯, 隨后提出的基于粒子濾波的算法在這兩方面有所改進. 而視覺SLAM則是在地圖構建上有了新的突破.

3.1 基于卡爾曼濾波的算法研究

在基于路標的移動機器人SLAM中, 通常把機器人位姿r和地圖信息m存儲在一個狀態向量X=[r m]T中, 并假設系統的觀測模型和運動模型都是服從高斯分布的[26]. 該方法的基本思想就是通過機器人位姿與環境特征之間的關系以及其狀態協方差所構成的不確定信息, 完成對機器人位姿和地圖構建的一個循環迭代的估計校正過程. 卡爾曼濾波是一種滿足高斯分布的最優的最小方差貝葉斯估計, 但該方法只能適用于線性估計問題, 而一般的問題都是涉及非線性問題,所以基于非線性模型的擴展卡爾曼濾波在SLAM上的應用更為廣泛. 該方法不僅易于實現, 且在處理不確定信息方面有著很多優點, 已經用于許多SLAM的研究中. EKF用一階泰勒展開式近似表示非線性, 這樣不可避免地會引入線性化誤差[27,28], 通過預測和更新難以達到最優.

該方法簡單容易實現, 但是也存在著很多的問題.系統協方差能夠很好的處理不確定信息, 協方差矩陣維持機器人與特征標志之間以及各標志之間的相關關系, 對算法的收斂性起著重要的作用. 但其更新導致繁重的計算量, 復雜度可達到O(n2)[29], n為地圖特征標志數.

3.1.1 不確定性

針對處理不確定信息時存在的計算量問題, 研究者提出了一些改進算法. 主要總結為以下兩方面:

(1)從大規模環境方面來說, 文獻[30]引入局部子地圖, 在各子地圖中分別實現SLAM, 有效的減小了計算量. 文獻[31]提出一種改進的算法 KLM-EKF, 用已知路標的信息對機器人位姿和協方差矩陣進行修正,并創建輔助系數矩陣修正已構建地圖, 最終實現路標的全局更新.

(2)從協方差的角度來說, 文獻[32]提出一種平方根容積卡爾曼濾波 (Square-Root Cubature Kalman Filter, SCKF)算法, 利用傳遞平方根因子代替協方差矩陣, 計算中避免了耗費時間的Cholesky分解, 一定程度上減小了線性化誤差, 提高了算法效率. 文獻[33]引入稀疏矩陣概念, 忽略取值較小的元素, 文獻[34]提出的擴展信息濾波器 (Extended Information Filter, EIF)算法利用稀疏矩陣概念, 采用協方差矩陣的逆矩陣表示不確定信息來進行局部信息更新, 有效降低時間復雜度.

3.1.2 數據關聯

針對數據關聯問題, 由于機器人位置和地圖特征標志位置是未知的, 不確定性參數符合高斯分布, 隨著運動距離的增大位置誤差不斷增大, 都會導致數據關聯不準確, 這也被稱為一致性問題[35,36]. 雖然國內對數據關聯方面進行了一定研究, 但對于機器人定位估計的一致性一直缺乏完善的理論分析.

大多數的文獻都在計算量和數據關聯上進行了改進, 而忽略了對算法一致性的重視. 在為數不多的相關研究中, 文獻[37]提出是由于非線性誤差積累導致算法不一致性. 運動模型和觀測模型必須要進行線性化才能保證算法的收斂性, 線性化雖然簡化了問題但使得系統產生了截斷誤差, 使得估計的狀態值和協方差與真實的狀態值和協方差不匹配, 對其精度產生影響. 文獻[38]提出的迭代卡爾曼濾波算法, 通過多次迭代估計提高了估計精度, 減小了估計誤差. 但其實噪聲不完全服從高斯分布, 以及當在大規模環境下時都會導致不一致的可能.

有研究表明認為不一致的本質原因在于機器人姿態角的誤差和不確定性. 近年來國外有文獻提出, 不一致是由于全局參考坐標起點和方向的不可觀測性引起,并提出了 IEKF-SLAM (Invariant-Extended Kalman Filter)算法[39], 證明了基于不變量擴展卡爾曼濾波SLAM算法的收斂性和一致性. 文獻[40]進一步證明了3D不變量擴展卡爾曼濾波SLAM算法的一致性.

綜上所述, 基于卡爾曼濾波的SLAM在非線性系統中有著廣泛的應用, 且實現容易, 能夠很好地處理不確定信息. 隨后的研究在減小非線性化誤差, 提高算法精度上也有著很大貢獻, 但在高維復雜環境下對數據關聯的不敏感對SLAM的性能還是有很大影響.

3.2 基于粒子濾波的算法研究

EKF要求系統需要滿足高斯分布, 而粒子濾波算法作為次優的濾波方法在解決非高斯非線性系統具有明顯的優勢, 其基本思想是用一組相關權重的隨機樣本集合, 以及基于這些樣本的估計來近似表征后驗概率密度函數, 采用樣本均值代替積分運算來獲得狀態最小方差估計的過程[41].

3.2.1 不確定性

常用的基于Rao-Blackwellised粒子濾波器(RBPF)解決了SLAM的理論問題[42], 該問題分解成機器人的位姿估計和地圖估計, 大量的粒子數通過不斷的采樣-重采樣, 使得每個粒子所估計的“粒子地圖”更接近其真實值.

(1) 為了減少粒子數量, 文獻[43]使用 Kullback-Leibler Distance (KLD)方法自適應確定粒子數量. 文獻[44]朱磊等將人工魚群算法引入到RBPF SLAM中,使粒子分布在重采樣之前就更加接近真實情況, 然后利用定向重采樣方法使新產生的粒子更加接近于真實運動情況. 文獻[45]提出在計算提議分布時將機器人里程計信息和激光傳感器采集的距離信息進行融合, 有效地減少了所需粒子的數量.

(2)對粒子的反復篩選會使得權重大的粒子多次被重復選擇, 其余大量的粒子被舍棄, 導致粒子耗盡.對于該問題, 文獻[46]對重采樣中權值較小的粒子進行遺傳變異操作, 緩解粒子枯竭現象, 提高機器人位姿估計的一致性, 并維持粒子集的多樣性. 再一種思路就是用自適應重采樣技術[47], 給定一個權重方差的閾值, 當粒子權重方差低于該閾值是進行重采樣, 達到既維護了合理的粒子權重方差又減少重采樣次數的目的, 保證了粒子的多樣性.

基于Rao-Blackwellised的粒子濾波方法引入自適應重采樣技術減少了粒子耗盡問題, 計算粒子分布的時候不單單僅依靠機器人里程計信息, 同時考慮當前傳感器的觀測信息, 一定程度減少了機器人位置在粒子濾波步驟中的不確定性.

3.2.2 數據關聯

RBPF在特征地圖和柵格地圖的SLAM中都得到比較成功的應用. 與 EKF SLAM 相比, RBPF 對數據關聯不敏感, 容許錯誤的數據關聯, 這意味著當發生數據關聯錯誤時, EKF SLAM 會得到比 RBPF SLAM 更好的結果. 而Montomerlo提出的基于Rao-Blackwellised的替代方法FastSLAM[48,49], 解決了數據關聯對SLAM的影響.

FastSLAM利用在機器人路徑已知的情況下各路標之間位置相互獨立的特點(在已知數據關聯情況下),將粒子濾波表征為機器人的軌跡濾波, 則每個路標通過獨立的EKF進行估計, 很好的融合了新的觀測信息,使得采樣觀測序列的影響有更好的魯棒性.

相比于EKF的復雜度(主要是協方差的更新)和數據關聯問題, FastSLAM計算復雜度可從O(nk)降低為 O(klogn)[50], k 為粒子數, n 為路標數量, 適用于路標多的大環境問題, 由于粒子的獨立性, 大大減小了數據關聯的影響.

隨后的研究中, 文獻[51,52]將粒子群優化思想引入到FastSLAM中, 提出了一種基于粒子群優化的同時定位與建圖方法. 通過粒子群優化方法對FastSLAM中預估粒子進行更新, 調整粒子的提議分布, 使得預測采樣粒子集中于機器人的真實位姿附近. 該方法能有效提高SLAM的精度, 并減少所使用的粒子數以及計算的時間復雜度. 文獻[53]通過混合里程計信息和距離傳感器信息構造新的采樣提議分布, 使得粒子采樣更加準確, 并結合自適應的重采樣方法, 有效地減少重采樣次數, 避免頻繁重采樣導致的粒子耗盡問題, 進一步提高了算法的性能. 文獻[54]對于解決粒子群退化問題時帶來的貧化問題, 一改傳統的優等粒子代替劣等粒子, 提出讓次等離子向優等粒子方向移動的一種趨優重采樣算法, 更好的維護了粒子群的多樣性.

綜上所述, FastSLAM算法的研究中, 研究者集中在建議分布、重采樣策略和加入自適應機制做出了改進[55–57], 并實現了預期的效果. 受 FastSLAM 算法由來的啟發, 粒子濾波算法與其他智能算法的結合也取得重要進展[58,59]. 由于FastSLAM數據關聯的獨立性, 使其在算法的快速性和抗干擾性上有著很好的優勢, 在復雜大環境下應用廣泛.

3.3 視覺SLAM

基于概率估計的SLAM主要的應用環境、地圖表示主要是在二維空間, 而在三維空間的擴展有所局限.對于該問題, 一方面需要提高移動機器人對環境的感知能力, 另一方面也需要在現有感知能力限制下, 設計更好的SLAM系統, 以提高系統的實用性. 逐漸地視覺傳感器實現機器人SLAM開始被提出, 視覺SLAM作為一個專有名詞, 從2006年開始受到國內外學者的關注, 并發表一系列研究文獻[60–63].

3.3.1 外部傳感器

視覺SLAM最開始使用的外部傳感器主要有聲納和激光雷達[64,65], 具有分辨率高、抗有源干擾能力強等優點, 但其工作受到了環境的約束, 如GPS信號. 由于SLAM主要在未知環境下完成, 我們無從獲知環境信息, 而相機能夠獲取精準直觀的環境信息且成本低、功耗小.

隨著計算機視覺的廣泛應用, 利用相機作為外部傳感器成為了視覺SLAM研究的主要方向. 根據其工作方式的不同, 可分為以下三種:

(1) 單目相機: 只能反映出三維場景的二維圖像,并沒有體現出物體到相機之間的距離信息. 由于單目SLAM無法僅憑圖像確定真實尺度, 尺度不確定性是其主要特點, 也是誤差的主要來源. 人們開始使用雙目和深度相機.

(2) 雙目相機: 通過左右眼圖像的差異來判斷場景中物體的遠近, 能從直接提取完整的特征數據. 它既能應用于室內也能應用于室外. 但是像素點的深度需要大量的計算才能得到, 且配置與標定復雜, 所以計算量是雙目的主要問題之一.

(3) RGB-D相機: 可同時獲取圖像彩色信息和深度信息. 微軟公司2010年推出的Kinect相機為三維SLAM問題的解決提出了新思路, 由于Kinect價格便宜、簡單方便, 且能快速獲取環境的彩色信息和深度信息, 不受光譜的影響[66], 這使基于RGB-D的SLAM得到了迅速的發展[67–71].

3.3.2 標志性研究成果

視覺SLAM的先驅Davison在2007年提出了MonoSLAM[72], 是第一個基于EKF的實時單目視覺系統, 以EKF為后端來追蹤前端非常稀疏的特征點. 隨后 Klein等人提出了 PTAM[73](Parallel Tracking Mapping), 是第一個將跟蹤和建圖分為兩個單獨任務并在兩個平行的線程進行處理, 該研究奠定了視覺SLAM后端處理以非線性優化為主導而不是使用傳統的濾波器作為后端. 繼PTAM的雙線程結構后, Tardos又提出的ORB-SLAM三線程結構[74](即特征點的實時跟蹤、地圖創建及局部優化、地圖全局優化)實現稀疏地圖的構建, 同時支持單目、雙目、RGB-D三種模式, 并得到了研究者的認同. 而 LSD-SLAM 是 J. Engle等人2014年提出的直接法應用在半稠密的單目SLAM中[75], 也就是直接對像素點進行處理并構建出大規模的三維環境地圖. 類比于 ORB-SLAM, LSD-SLAM 則標志著單目直接法在SLAM中的成功應用, 以及地圖從稀疏到半稠密的轉變. 同時2014年提出的RTABMAP[76]是RGB-D中經典的一個方法, 能夠實現基于特征的視覺里程計、基于詞袋的回環檢測、后端的位姿圖優化、點云圖和三角網絡地圖, 但由于其集成度較高, 在其基礎上的開發變得比較困難.

隨著開源方案的增多, 還有一些算法也逐漸普及,如DTAM、DVO、RGBD-SLAM-V2等. 表2列出了視覺SLAM的幾種典型方法的特點及優缺點. 視覺傳感器很好地利用了豐富的環境信息, 實現了從早期二維地圖到三維地圖的轉化, 豐富了地圖信息, 擴展了應用領域, 有著很大的實用價值. 但在現實環境下還存在很大的魯棒性和高適應能力技術挑戰.

表2 視覺 SLAM 五種典型方法的比較

3.3.3 視覺 SLAM 存在的問題

視覺SLAM近年來雖然有著很多的研究成果, 但是僅用相機作為唯一外部傳感器進行同時定位地圖構建還是存在著很多問題及挑戰.

(1)機器人在不同時刻觀測到的環境地圖會受到各種噪聲的干擾, 導致視覺定位誤差和對環境結構的錯誤表達. 例如: 動態環境、相機觀測角度、相機噪聲、光照變化都可能導致視覺觀測的不準確, 從而影響到地圖結構的正確性. 對于RGB-D相機來說, 噪聲的干擾使得相機不能得到準確的深度信息, 導致不能進行準確的定位.

(2)動態環境下信息的丟失也是一大問題, 導致該問題的原因有很多, 例如: 運動的不連續、環境特征點少等. 這些問題不僅會影響關鍵幀匹配和回環檢測的效果, 也會降低環境地圖的質量, 這對于視覺導航是不利的. 因此如何有效地降低各種噪聲產生的負面影響、減少誤差的積累以增強視覺SLAM的魯棒性、實時性是非常值得研究的課題.

4 SLAM 未來發展趨勢

根據現有SLAM技術應用的場合, 如運動、導航、娛樂等, 我們希望SLAM能夠在嵌入式或者手機等便攜的小型設備上使用, 更好地為上層應用服務, 或者也能利用高性能計算設備實現精密的三維重建等.為此, 我們把SLAM未來發展方向分為以下三個方向:

(1)從硬件條件來說: 結合多種傳感器(例如慣性傳感器、光流計、里程計等). 例如慣性傳感器IMU與相機的結合, 能夠提供相機快速運動下較好的估計. 雖然這種方法只是處于試驗階段, 但是這樣的結合為SLAM提供了非常有效的方法. 我們希望SLAM能夠在小型化低成本的低端硬件上實現, 例如手機等移動終端上, 從更加實際的角度出發讓算法更加實用, 能夠應用在復雜的現實場景下.

(2)從軟件算法上來說: 結合深度學習. 隨著深度學習在人工智能領域的發展, 通過與SLAM的結合我們能夠實現對圖像更加準確的識別、檢測, 使得機器人能夠更有效的識別環境信息[77,78], 更加準確的估計機器人位姿狀態. 雖然這些方法有著很好的準確性, 但是還不成熟. 所以深度學習與SLAM結合來處理圖像是未來研究趨勢.

(3)從應用層面來說: 地圖的呈現形式能夠更加包羅萬象, 更好地應用于實際. 針對不同的任務需求以及場景環境都能對應相應的地圖形式, 更好地服務于人類. 所以在現有地圖形式基礎上開發新型的地圖模型是未來需要不斷探索的方向.

5 結束語

SLAM在研究的近三十年來發展十分迅速, 基于概率估計的SLAM方法一定程度上解決了不確定性問題以及數據關聯問題, 但其地圖構建僅局限于二維地圖形式, 實時性和適應環境能力遠遠達不到實際應用的條件, 而基于視覺信息的SLAM方法具有很好的感知環境的能力使其在適應性與實時性上有一定優勢,并能實現三維環境地圖的構建, 更好地提高SLAM實用性. 所以在復雜多變的環境下, 有效的融合多種傳感器信息, 并結合先進的人工智能算法是SLAM的發展趨勢.