SLAM綜述性特別是原理講述比較淺顯易懂的的資料比較少,相對比較知名的是《SLAM for Dummies》,但中文資料相對較少,這里就簡單概述一下《SLAM for Dummies》的核心內容。
(一) SLAM for Dummies中SLAM的基本模塊
SLAM的基本組成包括:地標(Landmark)抽取、數據關聯、系統狀態變量估計、基於觀測值得系統狀態變量更新,以及地標更新。SLAM for Dummies主要描述了2D場景下的地圖構建與機器人定位,這里的狀態變量主要是指機器人x,y坐標位置與方向角以及地標x,y坐標位置,而系統狀態變量估計、基於觀測值的系統狀態變量更新主要使用EKF算法。
(二) SLAM for Dummies中SLAM硬件組成
機器人采用了輪式室內機器人Evolution Robotics ER1 robot,使用里程計(odometry )預測下一時刻機器人的系統狀態變量,里程計的誤差要求是每移動1米的誤差小於2cm,每轉動45度的誤差小於2度。測距模塊采用Sick公司的激光雷達,其參數為角度分辨率為0.25度或0.5度,掃描范圍100度或180度,測距誤差正負5厘米。
(三) SLAM for Dummies中SLAM流程
SLAM流程如下圖所示,基於里程計的變化使用EKF更新系統狀態變量,基於激光掃描數據抽取地標,並進行數據關聯完成本次觀測的地標與以前觀察到地標的關聯,根據數據關聯的結果,即觀測過程,使用EKF更新和修訂基於里程計預測的系統狀態變量,同時更新地標信息,增加以前沒有觀測到地標。
(四) SLAM for Dummies中地標的抽取
地標的選取原則:(1)地標容易重新觀測到;(2)地標相互之間區分度高;(3)環境中地標數量充足;(4)地標是穩定不動的。
基於激光雷達觀測數據進行地標抽取的算法:
(1)Spike地標:基於激光雷達測距結果,抽取出“點”型地標。如果2個相鄰的掃描點的測距距離大於一個距離門限,認為是一個Spike地標。在平滑的環境中,Spike無法正常工作,也無法處理像人這樣在環境中移動的情況
(2)RANSAC地標:基於雷達測距結果,抽取出“線”型地標。通過隨機采樣角度差在D度以內的激光測距結果,並通過最小均方LS近似擬合成一條直線,如果距離該直線很近的激光測距結果的樣點數量大於一個指定的門限,則認為該直線為一個地標。
在EKF算法中,地標有一個相對機器人位置的距離和方向角,對於“線”型地標,通常以機器人位置到“線”型地標的垂直線長度作為地標相對機器人的距離,垂直線的夾角作為方向角。
相對而言,以上兩種基於地標的算法在准確性和穩定性方面差於基於scan匹配的算法。
(五) SLAM for Dummies中地標的關聯
地標關聯是指完成兩個不同掃描觀測到的地標之間的匹配。在《SLAM for Dummies》中,使用數據庫維護以前觀測到的地標及其被觀測到的次數。只有該地標被觀測到N次,才認為是合格的地標。 對於新的掃描,基於最近鄰居規則,與數據庫中的一個地標關聯,並計算兩者之間的歐式距離或者Mahalanobis距離,當距離小於一個門限(該門限是EKF不確定參數的函數)時,認為兩者是同一個地標,否則認為是新地標,被觀測到的次數設為1。
(六) SLAM for Dummies中核心算法EKF的設計
EKF的主要流程如下:(1)使用里程計數據更新當前系統狀態;(2)基於地標關聯或者說是地標的重觀察修訂更新的系統狀態;(3)在當前系統狀態中增加新的地標。
EKF算法的核心是系統狀態變量(通常是由機器人位置的X,Y坐標、方向角,各個地標位置的X,Y坐標等3+2N個元素組成的列向量,N為合格地標的數量)、系統狀態協方差((3+2N)*(3+2N)矩陣,初始化時,因為沒有地標信息,也沒有里程計數據,協方差矩陣僅為3*3的對角矩陣,對角矩陣的值為接近0 的一個非0值)的維護和更新,以及Kalman增益的計算。
(1)基於里程計數據更新當前系統狀態
(a)更新由機器人位置的X,Y坐標、方向角,即系統狀態變量的前三個元素
(b)更新預測模型(方程)的雅克布矩陣
(c)更新預測模型(方程)的高斯噪聲方差
(d)計算系統狀態協方差
(2)基於地標關聯修訂系統狀態
針對每一個匹配的地標,執行以下處理過程:
(a)計算觀測模型(方程)的雅克布矩陣
(b)更新觀測模型(方程)的高斯噪聲方差
(c)基於觀測模型高斯噪聲方差、觀測模型(方程)的雅克布矩陣,系統狀態協方差矩陣,計算Kalman增益
(d)基於Kalman增益,更新系統狀態變量
(3)在當前系統狀態中增加新的地標
針對每一個匹配的地標,執行以下處理過程:
(a)在系統狀態變量中增加新的地標,元素個數從3+2N拓展到3+2N+2M,M為新增地標個數
(b)在系統狀態變量協方差矩陣中增加新地標對應的項