基于RBPF的激光SLAM算法優(yōu)化設(shè)計(jì)

吳正越,張 超,林 巖

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

0 概述

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

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

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

1 RBPF-SLAM算法

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

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

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重采樣。按照粒子權(quán)值在整體粒子權(quán)值中的占比復(fù)制粒子,重采樣后粒子的權(quán)值都設(shè)置為1/N。

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

(3)

當(dāng)Neff小于給定的一個(gè)閾值時(shí),粒子將發(fā)生嚴(yán)重退化。為克服粒子退化以減少計(jì)算量,RBPF-SLAM算法在重采樣過程中丟棄權(quán)值小的粒子,只保留權(quán)值大的粒子。

2 RBPF-SLAM算法重采樣策略改進(jìn)

2.1 MSV重采樣算法

傳統(tǒng)RBPF-SLAM算法重采樣策略是去除權(quán)值小的粒子,多復(fù)制權(quán)值大的粒子。這種簡(jiǎn)單粗糙的重采樣方法會(huì)使得粒子的多樣性丟失,造成樣本貧化問題,導(dǎo)致定位和建圖失真[15-16]。

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

(4)

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

(5)

2.2 仿真驗(yàn)證

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

圖1 2種算法SLAM仿真結(jié)果對(duì)比

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

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

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

3 激光SLAM算法測(cè)量模型優(yōu)化

3.1 基于RBPF的激光SLAM算法

激光SLAM是一類以激光數(shù)據(jù)作為傳感器輸入的SLAM算法[18]。經(jīng)典的激光SLAM算法Gmapping以RBPF-SLAM算法作為基礎(chǔ)[19]。

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

(6)

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

(7)

(8)

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

(9)

3.2 似然域模型改進(jìn)

1)測(cè)量噪聲。真實(shí)環(huán)境和單束激光測(cè)量如圖3所示。

圖3 真實(shí)環(huán)境和單束激光測(cè)量

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

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

(10)

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

圖4 由環(huán)境引起的測(cè)量概率

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

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

(11)

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

圖5 動(dòng)態(tài)物體測(cè)量概率模型

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

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

(12)

其中,λ是各部分概率的權(quán)重。

4 機(jī)器人平臺(tái)激光SLAM實(shí)驗(yàn)

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

圖6 機(jī)器人平臺(tái)

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

圖7 2種算法的辦公室環(huán)境建圖結(jié)果

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

表1 2種算法的辦公室環(huán)境定位誤差對(duì)比

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

圖8 2種算法的倉(cāng)庫(kù)環(huán)境建圖結(jié)果

在倉(cāng)庫(kù)環(huán)境中,2種算法的機(jī)器人定位誤差如表2所示,從表2可以看出,改進(jìn)的激光SLAM算法定位精度更高。

表2 2種算法的倉(cāng)庫(kù)環(huán)境定位誤差對(duì)比

5 結(jié)束語(yǔ)

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