結合傳感器融合與退火參數改進RBPF-SLAM算法

李 旦,王冠凌,豐宇航

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

0 引言

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

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

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

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

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

1 RBPF-SLAM算法

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

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)分解為兩個獨立的后驗概率乘積,先估計移動機器人的運動軌跡,然后由運動軌跡結合觀測數據更新環境地圖。p(m|x1∶t,z1∶t,u1∶t-1)為地圖的后驗概率,p(x1∶t|z1∶t,u1∶t-1)為運動軌跡的后驗概率,需要粒子濾波器對潛在的運動軌跡進行估算,同時每個粒子都具有一種運動軌跡。由這些粒子的運動軌跡和系統的觀察構建最終的環境地圖。

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

(2)

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

(3)

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

(4)

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

(5)

2 改進RBPF-SLAM算法

2.1 里程計和IMU信息融合

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

(6)

(7)

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

(8)

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

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

(9)

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

將輪式里程計和IMU的預測方程進行聯合,從而得到系統的預測方程為

(10)

其中:系統的預測矩陣H為

(11)

系統的噪聲協方差矩陣R(d)為

(12)

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

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

(13)

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

(14)

2.2 混合提議分布

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

圖1 觀測模型和運動模型似然函數分布

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

(15)

加入觀測模型的粒子權重公式變成：

(16)

(17)

(18)

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

(19)

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

(20)

對應的粒子權重計算公式為

(21)

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

2.3 自適應重采樣

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

(22)

2.4 RBPF-SLAM算法改進流程

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

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

4) 根據式(21)計算并更新每個粒子權重。

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

3 實驗結果和分析

3.1 改進RBPF-SLAM算法的實現

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

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

圖2 RBPF-SLAM算法構建環境地圖

圖3 改進RBPF-SLAM算法構建環境地圖

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

3.2 實驗結果分析

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

圖4 位姿狀態估計誤差對比曲線

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

表1 RBPF-SLAM算法與改進的RBPF-SLAM算法構建環境地圖結果比較

4 結論

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