基于RBPF的激光SLAM算法優化設計

吳正越,張 超,林 巖

(北京航空航天大學 自動化科學與電氣工程學院,北京 100191)

0 概述

同時定位與建圖(Simultaneous Location and Mapping,SLAM)是機器人自主導航的基礎,也是實現機器人智能化的關鍵技術。激光SLAM是目前較穩定和主流的機器人定位導航方法,相比于視覺SLAM,激光SLAM的觀測精度更高,其在簡單的室內環境中具有明顯優勢[1]。目前,激光SLAM算法大都基于粒子濾波器(Particle Filter,PF),這種SLAM算法被稱為PF-SLAM算法。由于環境中可能存在大量路標,使得PF-SLAM算法狀態向量維數非常高,計算較復雜。為解決該問題,文獻[2]將Rao-Blackwellised粒子濾波器(Rao-Blackwellised Particle Filter,RBPF)應用于SLAM算法中,提出一種RBPF-SLAM算法。該算法將機器人定位和建圖進行分解[3-4],用粒子濾波器對機器人實現定位,用卡爾曼濾波器進行建圖。RBPF-SLAM使基于粒子濾波器的SLAM算法計算量大幅減小,促進了SLAM理論的發展[5]。文獻[6]基于RBPF-SLAM提出一種激光SLAM算法Gmapping,該算法將高精度的激光測量數據加入到提議分布求取中,使提議分布更接近實際后驗分布,從而提高了算法的效率。

RBPF-SLAM算法在粒子濾波過程中的簡單重采樣會導致樣本貧化問題,致使SLAM定位和建圖誤差過大。為此,多種改進的重采樣方法相繼被提出,其中,自適應重采樣方法成為研究熱點之一。文獻[7]對自適應重采樣方法進行詳細分析,文獻[8]提出利用自適應方法改進重采樣的SLAM算法。雖然這些算法能夠緩解樣本貧化問題,提高估計準確性,但計算量大幅增加,且只能達到有限精度。文獻[9]提出最小采樣方差(MSV)重采樣方法,該方法可使采樣方差最小化,保證重采樣前后粒子分布的一致性,使得權值較小但更接近真實值的粒子不被刪除。將最小方差重采樣方法引入到RBPF-SLAM算法中,可以提高SLAM的位姿估計和建圖精度。

基于RBPF的激光SLAM算法在計算提議分布的過程中需要建立激光測量模型[10]。波束模型作為測距儀的近似物理模型,存在光滑性差、計算復雜度高等問題。Gmapping采用似然域模型作為激光測量模型,似然域模型將傳感器掃描的終點映射到地圖的全局坐標空間,利用映射點與地圖中障礙物的距離計算概率[11]。似然域模型相比波束模型光滑性更好,計算更高效,但由于其假定環境是靜態的,因此不能對動態物體進行清晰建模。本文利用最小方差重采樣方法改進Gmapping算法的重采樣過程,以緩解樣本貧化問題。在Gmapping算法原有激光測量模型的基礎上加入意外物體的觀測概率,使算法在動態環境中建立準確的激光測量模型。

1 RBPF-SLAM算法

SLAM算法的關鍵是求解后驗概率p(x1:t,m|z1:t,u1:t-1),即由觀測值z1:t=z1,z2,…,zt和控制量u1:t-1=u1,u2,…,ut-1估計機器人的運行軌跡x1:t=x1,x2,…,xt和環境地圖m。

基于粒子濾波的PF-SLAM算法在求解后驗概率p(x1:t,m|z1:t,u1:t-1)的過程中,由于狀態空間維數擴張,計算復雜度非常高。RBPF-SLAM算法將SLAM問題分解為定位和建圖兩部分,降低了采樣時的狀態空間維數,從而達到簡化計算的目的[12-13]。RBPF-SLAM算法后驗概率計算如下:

p(x1:t,m|z1:t,u1:t-1)=

p(m|x1:t,z1:t)p(x1:t|z1:t,u1:t-1)

(1)

RBPF-SLAM算法具體步驟如下:

(2)

步驟3重采樣。按照粒子權值在整體粒子權值中的占比復制粒子,重采樣后粒子的權值都設置為1/N。

在RBPF-SLAM算法的粒子迭代更新過程中,部分粒子會出現粒子退化問題,這些粒子的權值減小到可以忽略的程度,但仍會占用計算資源[14]。粒子退化程度表示如下:

(3)

當Neff小于給定的一個閾值時,粒子將發生嚴重退化。為克服粒子退化以減少計算量,RBPF-SLAM算法在重采樣過程中丟棄權值小的粒子,只保留權值大的粒子。

2 RBPF-SLAM算法重采樣策略改進

2.1 MSV重采樣算法

傳統RBPF-SLAM算法重采樣策略是去除權值小的粒子,多復制權值大的粒子。這種簡單粗糙的重采樣方法會使得粒子的多樣性丟失,造成樣本貧化問題,導致定位和建圖失真[15-16]。

采樣方差(Sampling Variance,SV)可以度量粒子分布在重采樣前后的差別,進而衡量重采樣方法對粒子多樣性的破壞程度[17]。SV的計算公式如下:

(4)

本文引入MSV方法以改進原RBPF-SLAM算法中的重采樣策略。MSV方法可使SV最小化,使重采樣前后粒子分布保持一致,緩解樣本貧化現象。MSV方法具體步驟如下:

(5)

2.2 仿真驗證

本文在Matlab平臺上利用悉尼大學戶外機器人中心(ACFR)發布的SLAM算法仿真器進行實驗。原RBPF-SLAM算法與本文改進的RBPF-SLAM算法都用60個粒子實現仿真,結果如圖1所示。其中,五角星為真實路標,加號為SLAM算法估計的路標,曲線為機器人運行軌跡。從圖1可以看出,在相同粒子數目情況下,原RBPF-SLAM算法估計路標的位置與路標真實位置相差較大,改進的RBPF-SLAM算法估計路標的位置與真實路標位置則相對一致,即該算法建圖效果更好。

圖1 2種算法SLAM仿真結果對比

為比較2種重采樣方法的定位效果,記錄SLAM過程中機器人估計位置與實際位置的間距,結果如圖2所示。從圖2可以看出,MSV重采樣方法具有更高的定位精度。

圖2 2種方法在SLAM過程中的定位誤差對比

通過以上實驗結果對比可知,相對原RBPF-SLAM算法,改進的RBPF-SLAM算法定位和建圖精度更高,在重采樣過程中樣本貧化問題得到有效緩解。

3 激光SLAM算法測量模型優化

3.1 基于RBPF的激光SLAM算法

激光SLAM是一類以激光數據作為傳感器輸入的SLAM算法[18]。經典的激光SLAM算法Gmapping以RBPF-SLAM算法作為基礎[19]。

RBPF-SLAM算法的提議分布選取非常重要,提議分布越接近目標分布,采樣所需的粒子數目越少[20]。Gmapping將激光測量數據加入到提議分布求取中,可以獲得更精確的提議分布,融合激光測量的提議分布表示如下:

(6)

將式(6)代入RBPF-SLAM算法的權值計算中,得到新的權值更新公式為:

(7)

(8)

其中,歸一化因子η(i)為:

(9)

3.2 似然域模型改進

1)測量噪聲。真實環境和單束激光測量如圖3所示。

圖3 真實環境和單束激光測量

測量到的點與地圖m上第k(k=1,2,…,N)個物體之間的歐式距離為dk,傳感器測量到物體的概率phit(z|x,mk)用一個0均值的高斯函數表示:

phit(z|x,mk)=εσhitdk

(10)

整體測量概率為所有N個物體概率的疊加,圖3的整體測量概率如圖4所示。

圖4 由環境引起的測量概率

2)測量失敗。如果觀測值z為激光測量最大量程z=zmax,這在物理世界中表示測量失敗,似然域模型直接將觀測值丟棄。

3)隨機測量。用一個均勻分布prand對測量中的隨機噪聲進行建模,如式(11)所示:

(11)

似然域模型假定環境為靜態環境,但現實中的環境大多為動態環境。為使激光測量模型更真實地反映動態環境,本文在似然域模型中加入意外對象觀測的概率。假設意外對象在環境中隨機出現,則距離傳感器越近的對象被探測到的概率越大,本文用一個負指數函數作為意外物體觀測概率pshort(z|x,m),如圖5所示,pshort(z|x,m)的有效域為(0,z*),z*為地圖中激光測量光束方向上最近障礙間的距離。

圖5 動態物體測量概率模型

改進后的似然域模型p(z|m,x)如式(12)所示:

p(z|m,x)=λhitphit+λrandprand+λshortpshort

(12)

其中,λ是各部分概率的權重。

4 機器人平臺激光SLAM實驗

圖6所示為真實機器人平臺,該平臺采用四輪差速驅動的方式,機器人底盤部分向上位機反饋里程計信息,激光測量裝置采用國產RPLIDAR-A2激光雷達。上位機運行Linux(Ubuntu 16.04)的ROS系統,利用ROS分布式通信的特性實現機器人底盤控制、傳感器數據獲取以及SLAM算法。SLAM算法包括基于RBPF的激光SLAM算法(Gmapping)和改進后的激光SLAM算法,改進算法利用MSV方法并將改進的似然域模型作為激光測量模型。

圖6 機器人平臺

手柄控制機器人平臺在多人走動的辦公室進行建圖實驗,構建的柵格地圖如圖7所示,其中,圖7(a)、圖7(b)分別為原激光SLAM算法和改進SLAM算法構建的辦公室地圖。從圖7可以看出,原激光SLAM算法構建的地圖存在偏移歪斜的問題,而改進算法構建的地圖更整齊。

圖7 2種算法的辦公室環境建圖結果

在建圖過程中,手柄控制機器人走到指定的位置,記錄算法估計的機器人位姿與實際機器人位姿的誤差,結果如表1所示。從表1可以看出,改進的激光SLAM算法定位誤差更小,定位效果更好。

表1 2種算法的辦公室環境定位誤差對比

在雜物較多的倉庫中構建的柵格地圖如圖8所示,其中,圖8(a)、圖8(b)分別為原激光SLAM算法和改進激光SLAM算法構建的倉庫地圖。從圖8可以看出,原激光SLAM算法構建的地圖比較混亂,部分墻體出現重影,而改進算法構建的地圖更清晰。

圖8 2種算法的倉庫環境建圖結果

在倉庫環境中,2種算法的機器人定位誤差如表2所示,從表2可以看出,改進的激光SLAM算法定位精度更高。

表2 2種算法的倉庫環境定位誤差對比

5 結束語

本文對傳統RBPF-SLAM算法進行研究,針對RBPF-SLAM算法中簡單重采樣造成的樣本貧化問題,采用MSV重采樣方法進行改進。在基于RBPF的激光SLAM算法中,傳統的激光測量似然域模型沒有考慮動態物體的影響,為此,本文在測量模型中加入意外物體的觀測概率,使激光測量模型能更好地反映真實環境。通過以上2點對基于RBPF的激光SLAM算法進行改進,并在真實機器人平臺上實現室內激光SLAM實驗,結果表明,改進的激光SLAM算法在動態環境中取得了較好的建圖和定位效果。意外物體觀測概率密度的加入使得激光SLAM算法計算量有所增加,下一步將采用更簡單的一次函數來近似模擬動態物體測量概率模型,以降低算法的計算復雜度。