結(jié)合傳感器融合與退火參數(shù)改進(jìn)RBPF-SLAM算法

李 旦,王冠凌,豐宇航

(安徽工程大學(xué) a.電氣工程學(xué)院;b.高端裝備先進(jìn)感知與智能控制教育部重點(diǎn)實(shí)驗(yàn)室,安徽 蕪湖 241000)

0 引言

為確定移動(dòng)機(jī)器人在未知環(huán)境中的位置,需要對(duì)移動(dòng)機(jī)器人進(jìn)行定位,然而定位又建立在環(huán)境地圖的基礎(chǔ)上。最早Smith等[1]提出同時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)的概念,主要內(nèi)容為移動(dòng)機(jī)器人根據(jù)位姿估計(jì)與傳感器獲取的未知環(huán)境地圖信息,實(shí)現(xiàn)環(huán)境地圖的構(gòu)建。目前SLAM仍然是機(jī)器人等研究領(lǐng)域的熱點(diǎn)[2-3]。

早期主要是由擴(kuò)展卡爾曼濾波(extended kalman filter,EKF)和粒子濾波(particle filter,PF)解決SLAM問(wèn)題[4-5]。在非線性高斯系統(tǒng)中,EKF的精度并不高,尤其是在環(huán)境特征量較大時(shí),計(jì)算的復(fù)雜度高,并且EKF難以處理數(shù)據(jù)關(guān)聯(lián)的問(wèn)題[6]。粒子濾波是用粒子集近似后驗(yàn)概率密度分布,能夠有效處理非線性非高斯分布的問(wèn)題,因而粒子濾波在SLAM等研究領(lǐng)域應(yīng)用廣泛。

Doucet等[7]利用RBPF算法處理SLAM問(wèn)題,Montemerlo等[8]在此基礎(chǔ)上提出Fast-SLAM算法(RBPF-SLAM)。該算法將機(jī)器人定位和地圖更新分開(kāi)進(jìn)行,先對(duì)位姿進(jìn)行估計(jì),再根據(jù)位姿構(gòu)建環(huán)境地圖,同時(shí)每個(gè)粒子都包含一種運(yùn)動(dòng)軌跡以及一幅環(huán)境地圖。因此RBPF-SLAM算法相較于EKF和PF算法,實(shí)時(shí)性和效率更高,可以有效解決SLAM問(wèn)題。但該算法中所用的粒子數(shù)目較多,計(jì)算量大,此外頻繁地執(zhí)行重采樣可能會(huì)剔除有效粒子,造成粒子退化[9]。

Griseti等為改進(jìn)RBPF-SLAM算法提出了Gmapping算法[10]34。此算法在計(jì)算提議分布時(shí)不僅使用移動(dòng)機(jī)器人的運(yùn)動(dòng)模型,同時(shí)還考慮激光雷達(dá)傳感器的觀測(cè)信息,這樣使得提議分布更加精準(zhǔn),提高了算法性能。為減少重采樣的次數(shù)引入自適應(yīng)重采樣,從而重采樣只有在粒子數(shù)下降至閾值以下時(shí)才執(zhí)行,由權(quán)重高的粒子來(lái)代替權(quán)重低的粒子。Gmapping算法在閉環(huán)較多的環(huán)境地圖中仍需要較多粒子數(shù)目,并且構(gòu)建的柵格地圖效果不理想,其穩(wěn)定性有待提高。潘楊杰等[11-12]利用輪式里程計(jì)信息與IMU進(jìn)行融合,準(zhǔn)確地估計(jì)輪式機(jī)器人的位姿。孫弋等[13-14]利用退火參數(shù)優(yōu)化混合提議分布,使得優(yōu)化后的提議分布更加接近目標(biāo)分布。王依人等[15]引入自適應(yīng)重采樣,減輕頻繁執(zhí)行重采樣造成粒子退化的問(wèn)題。

目前RBPF-SLAM算法中利用里程計(jì)估計(jì)移動(dòng)機(jī)器人位姿,且僅用運(yùn)動(dòng)模型作為提議分布,從而影響構(gòu)建環(huán)境地圖精度。因此本研究提出融合輪式里程計(jì)信息和IMU信息提高位姿精度以及利用退火參數(shù)優(yōu)化混合提議分布,有效減少采樣粒子數(shù)目和計(jì)算量。

1 RBPF-SLAM算法

RBPF-SLAM算法主要是利用雷達(dá)傳感器觀測(cè)信息z1∶t=z1,z2,…，zt和輪式里程計(jì)信息u1∶t-1=u1,u2,…，ut-1來(lái)估計(jì)移動(dòng)機(jī)器人的運(yùn)動(dòng)軌跡x1∶t=x1,x2,…，xt和環(huán)境地圖m的后驗(yàn)概率p(x1∶t,m|z1∶t,u1∶t-1)。RBPF-SLAM算法將聯(lián)合后驗(yàn)概率分解為如下形式:

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

。

(1)

公式(1)中將p(x1∶t,m|z1∶t,u1∶t-1)分解為兩個(gè)獨(dú)立的后驗(yàn)概率乘積,先估計(jì)移動(dòng)機(jī)器人的運(yùn)動(dòng)軌跡,然后由運(yùn)動(dòng)軌跡結(jié)合觀測(cè)數(shù)據(jù)更新環(huán)境地圖。p(m|x1∶t,z1∶t,u1∶t-1)為地圖的后驗(yàn)概率,p(x1∶t|z1∶t,u1∶t-1)為運(yùn)動(dòng)軌跡的后驗(yàn)概率,需要粒子濾波器對(duì)潛在的運(yùn)動(dòng)軌跡進(jìn)行估算,同時(shí)每個(gè)粒子都具有一種運(yùn)動(dòng)軌跡。由這些粒子的運(yùn)動(dòng)軌跡和系統(tǒng)的觀察構(gòu)建最終的環(huán)境地圖。

RBPF-SLAM算法采用序列重要性重采樣濾波器(SIR)對(duì)移動(dòng)機(jī)器人進(jìn)行位姿估計(jì)和環(huán)境地圖的更新,其具體流程可以分為以下4個(gè)步驟:

(2)

由以上四個(gè)步驟得出,權(quán)重的計(jì)算需要根據(jù)上一時(shí)刻的數(shù)據(jù),導(dǎo)致計(jì)算量會(huì)變得越來(lái)越大。Doucet等提出限制提議分布q,得到計(jì)算重要性權(quán)重的遞歸公式[16]:

(3)

根據(jù)文獻(xiàn)[10]得到計(jì)算展開(kāi)公式[10]34為

(4)

由公式(2)、(3)、(4)可求得如下權(quán)重計(jì)算公式：

(5)

2 改進(jìn)RBPF-SLAM算法

2.1 里程計(jì)和IMU信息融合

應(yīng)用EKF算法主要估計(jì)移動(dòng)機(jī)器人的位姿信息,利用輪式里程計(jì)和IMU建立位姿融合的狀態(tài)模型。由于里程計(jì)和IMU數(shù)據(jù)中都有移動(dòng)機(jī)器人的轉(zhuǎn)向角度和速度的信息,故位姿狀態(tài)向量為

(6)

(7)

里程計(jì)的數(shù)據(jù)可以直接獲取,則得到其預(yù)測(cè)方程為

(8)

其中:Hodom為輪式里程計(jì)的預(yù)測(cè)矩陣It;eodom,t(d)為輪式里程計(jì)的誤差,且它為服從高斯分布N(0,Rodom(d))的協(xié)方差矩陣。

由于只是研究二維空間的環(huán)境地圖,因此對(duì)于IMU僅需要三軸中Z的軸數(shù)據(jù),則IMU的預(yù)測(cè)方程可表示為

(9)

其中:HIMU為IMU的預(yù)測(cè)矩陣;eIMU,t為IMU數(shù)據(jù)的誤差,且它為服從高斯分布N(0,RIMU)的協(xié)方差矩陣。

將輪式里程計(jì)和IMU的預(yù)測(cè)方程進(jìn)行聯(lián)合,從而得到系統(tǒng)的預(yù)測(cè)方程為

(10)

其中:系統(tǒng)的預(yù)測(cè)矩陣H為

(11)

系統(tǒng)的噪聲協(xié)方差矩陣R(d)為

(12)

將移動(dòng)機(jī)器人位姿信息的狀態(tài)轉(zhuǎn)移方程、預(yù)測(cè)方程和誤差模型代入如下擴(kuò)展卡爾曼濾波的公式(13)和公式(14)中就可實(shí)現(xiàn)移動(dòng)機(jī)器人的位姿估計(jì)。

擴(kuò)展卡爾曼濾波預(yù)測(cè)模型為

(13)

擴(kuò)展卡爾曼濾波更新模型為

(14)

2.2 混合提議分布

如圖1所示:實(shí)線為p(z|x)的概率分布,它是由激光雷達(dá)感知環(huán)境信息獲得的高斯分布;虛線為p(x|x′,u)的概率分布,它是由輪式里程計(jì)所獲得的高斯分布。

圖1 觀測(cè)模型和運(yùn)動(dòng)模型似然函數(shù)分布

由圖1可知觀測(cè)模型提供的信息準(zhǔn)確度相比運(yùn)動(dòng)模型的準(zhǔn)確度要高很多。RBPF-SLAM算法中運(yùn)動(dòng)模型相對(duì)于觀測(cè)模型的噪聲較大,僅使用運(yùn)動(dòng)模型會(huì)導(dǎo)致提議分布的精度低。為了解決上述問(wèn)題,將輪式里程計(jì)與IMU數(shù)據(jù)融合得到精準(zhǔn)的運(yùn)動(dòng)模型與激光雷達(dá)的聯(lián)合模型,作為混合提議分布,如式(15)所示,

(15)

加入觀測(cè)模型的粒子權(quán)重公式變成：

(16)

(17)

(18)

高斯分布構(gòu)建混合提議分布之后就能對(duì)下一時(shí)刻移動(dòng)機(jī)器人的位姿信息進(jìn)行采樣。此時(shí)粒子權(quán)重的計(jì)算公式為

(19)

加入精確的激光雷達(dá)傳感器觀測(cè)數(shù)據(jù),使得重要性權(quán)重方差小,但是積分計(jì)算相對(duì)比較復(fù)雜,當(dāng)觀測(cè)模型中出現(xiàn)峰值分布,會(huì)對(duì)采樣效率造成影響,致使濾波器的發(fā)散[17]。故采用退火參數(shù)α對(duì)混合提議分布中兩種模型的比重進(jìn)行調(diào)節(jié),得到結(jié)果如式(20)所示:

(20)

對(duì)應(yīng)的粒子權(quán)重計(jì)算公式為

(21)

其中退火參數(shù)范圍為0≤α≤1。調(diào)整退火參數(shù)的關(guān)鍵在于重要性采樣之后,對(duì)運(yùn)動(dòng)模型數(shù)據(jù)和觀測(cè)模型數(shù)據(jù)進(jìn)行比較,當(dāng)運(yùn)動(dòng)模型數(shù)據(jù)比觀測(cè)模型數(shù)據(jù)大時(shí),需減小重要性權(quán)重的占比,即將α數(shù)值減小。反之,則增大α數(shù)值。通過(guò)實(shí)驗(yàn)以及觀測(cè)數(shù)據(jù)與目標(biāo)分布之間的關(guān)系對(duì)比得出,當(dāng)提議分布中運(yùn)動(dòng)模型起主導(dǎo)作用時(shí),取α為0.6;當(dāng)傳感器的觀測(cè)模型接近目標(biāo)分布時(shí),取α為0.02,提高觀測(cè)模型的比例。

2.3 自適應(yīng)重采樣

重采樣步驟對(duì)粒子濾波具有重要影響,在重采樣期間,將權(quán)重高的粒子代替權(quán)重低的粒子。然而頻繁地執(zhí)行重采樣可能將有效粒子剔除,使得粒子退化[18]。故由Neff來(lái)表示粒子退化程度,其公式為

(22)

2.4 RBPF-SLAM算法改進(jìn)流程

2) 由式(20)求得混合提議分布。

3) 在混合提議分布中對(duì)粒子進(jìn)行采樣。

4) 根據(jù)式(21)計(jì)算并更新每個(gè)粒子權(quán)重。

5) 進(jìn)行自適應(yīng)重采樣操作。

3 實(shí)驗(yàn)結(jié)果和分析

3.1 改進(jìn)RBPF-SLAM算法的實(shí)現(xiàn)

為測(cè)試改進(jìn)RBPF-SLAM算法的性能,開(kāi)展驗(yàn)證實(shí)驗(yàn)。仿真實(shí)驗(yàn)在一臺(tái)操作系統(tǒng)為Ubuntu18.04的計(jì)算機(jī)(處理器:Intel Core i5-9400;RAM:8.00 GB)上運(yùn)行,機(jī)器人操作系統(tǒng)(robot operating system,ROS)使用melodic版本運(yùn)行RBPF-SLAM算法和改進(jìn)的RBPF-SLAM算法。通過(guò)ROS平臺(tái)的Gazebo軟件里搭建仿真環(huán)境,移動(dòng)機(jī)器人底盤(pán)使用兩輪差速移動(dòng),裝載輪式里程計(jì)、IMU和二維激光雷達(dá)。本實(shí)驗(yàn)搭建了兩組仿真環(huán)境,大小分別為15 m×13 m和22 m×21 m。

ROS還自帶一個(gè)三維可視化工具RVIZ,本研究中涉及的RVIZ用于顯示激光雷達(dá)點(diǎn)云以及實(shí)時(shí)繪制環(huán)境地圖。圖2中兩幅圖是由傳統(tǒng)RBPF-SLAM算法使用30個(gè)采樣粒子構(gòu)建出的二維柵格地圖,由于僅用輪式里程計(jì)作為提議分布,導(dǎo)致該算法的誤差較大,構(gòu)建的環(huán)境地圖部分出現(xiàn)了偏差。圖3中兩幅圖是由改進(jìn)的RBPF-SLAM算法使用15個(gè)采樣粒子構(gòu)建二維柵格地圖,從地圖的效果來(lái)看比傳統(tǒng)算法構(gòu)建的環(huán)境地圖更加清晰準(zhǔn)確。圖2和圖3中白色區(qū)域?yàn)榧す饫走_(dá)已掃描到的區(qū)域,黑色線條代表物體的輪廓,灰色區(qū)域則是激光雷達(dá)未能掃描到的地圖區(qū)域。

圖2 RBPF-SLAM算法構(gòu)建環(huán)境地圖

圖3 改進(jìn)RBPF-SLAM算法構(gòu)建環(huán)境地圖

從上述兩組對(duì)比實(shí)驗(yàn)來(lái)看,改進(jìn)的RBPF-SLAM算法通過(guò)使用較少的粒子數(shù)目可以構(gòu)建出更加精確的環(huán)境地圖。

3.2 實(shí)驗(yàn)結(jié)果分析

為了更加直觀地分析改進(jìn)的算法的位姿精度,在150 s內(nèi),將傳統(tǒng)RBPF-SLAM算法與改進(jìn)的算法在的環(huán)境地圖中對(duì)位姿狀態(tài)估計(jì)的精確性進(jìn)行比較。由圖4位姿狀態(tài)估計(jì)的仿真曲線可知,改進(jìn)的算法其均方根誤差要小于傳統(tǒng)算法。因此,改進(jìn)的RBPF-SLAM通過(guò)EKF融合輪式里程計(jì)信息和IMU信息的位姿估計(jì)的精度優(yōu)于RBPF-SLAM算法。

圖4 位姿狀態(tài)估計(jì)誤差對(duì)比曲線

表1中將兩種算法構(gòu)建相同環(huán)境地圖所用的粒子數(shù)目和時(shí)間做出比較。改進(jìn)后的算法使用的粒子數(shù)目要少于傳統(tǒng)算法使用的粒子數(shù)目,同時(shí)改進(jìn)后的算法縮短了構(gòu)建環(huán)境地圖所用的時(shí)間。改進(jìn)后的算法，即利用退火參數(shù)優(yōu)化混合提議分布的算法,減少采樣粒子數(shù)目,提高了系統(tǒng)的運(yùn)行效率。

表1 RBPF-SLAM算法與改進(jìn)的RBPF-SLAM算法構(gòu)建環(huán)境地圖結(jié)果比較

4 結(jié)論

本研究提出一種改進(jìn)的RBPF-SLAM算法,使用擴(kuò)展卡爾曼濾波融合傳感器信息提高移動(dòng)機(jī)器人的位姿,并利用退火參數(shù)優(yōu)化提議分布減少粒子數(shù)目,得到更精確的環(huán)境地圖。通過(guò)室內(nèi)環(huán)境的仿真實(shí)驗(yàn),驗(yàn)證了改進(jìn)算法的位姿估計(jì)精確性和地圖構(gòu)建的高效性。雖然改進(jìn)能夠減少重采樣次數(shù),但增大了計(jì)算代價(jià),在后續(xù)工作中將對(duì)重要性重采樣進(jìn)行優(yōu)化,降低粒子退化以及提高算法性能。