李 旦,王冠凌,豐宇航

(安徽工程大學 a.電氣工程學院;b.高端裝備先進感知與智能控制教育部重點實驗室,安徽 蕪湖 241000)

0 引言

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

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

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

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

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

1 RBPF-SLAM算法

RBPF-SLAM算法主要是利用雷達傳感器觀測信息z1∶t=z1,z2,…，zt和輪式里程計信息u1∶t-1=u1,u2,…，ut-1來估計移動機器人的運動軌跡x1∶t=x1,x2,…，xt和環(huán)境地圖m的后驗概率p(x1∶t,m|z1∶t,u1∶t-1)。RBPF-SLAM算法將聯(liá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)分解為兩個獨立的后驗概率乘積,先估計移動機器人的運動軌跡,然后由運動軌跡結(jié)合觀測數(shù)據(jù)更新環(huán)境地圖。p(m|x1∶t,z1∶t,u1∶t-1)為地圖的后驗概率,p(x1∶t|z1∶t,u1∶t-1)為運動軌跡的后驗概率,需要粒子濾波器對潛在的運動軌跡進行估算,同時每個粒子都具有一種運動軌跡。由這些粒子的運動軌跡和系統(tǒng)的觀察構(gòu)建最終的環(huán)境地圖。

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

(2)

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

(3)

根據(jù)文獻[10]得到計算展開公式[10]34為

(4)

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

(5)

2 改進RBPF-SLAM算法

2.1 里程計和IMU信息融合

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

(6)

(7)

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

(8)

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

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

(9)

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

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

(10)

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

(11)

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

(12)

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

擴展卡爾曼濾波預測模型為

(13)

擴展卡爾曼濾波更新模型為

(14)

2.2 混合提議分布

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

圖1 觀測模型和運動模型似然函數(shù)分布

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

(15)

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

(16)

(17)

(18)

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

(19)

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

(20)

對應的粒子權(quán)重計算公式為

(21)

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

2.3 自適應重采樣

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

(22)

2.4 RBPF-SLAM算法改進流程

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

3) 在混合提議分布中對粒子進行采樣。

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

5) 進行自適應重采樣操作。

3 實驗結(jié)果和分析

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

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

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

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

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

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

3.2 實驗結(jié)果分析

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

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

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

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

4 結(jié)論

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