基于改進RBPF的激光SLAM算法研究

王馨閣，田宗強，陳文濤，張 雨，楊 光，王 偉，陳孟元*

(1.安徽工程大學(xué) 電氣工程學(xué)院，安徽 蕪湖 241000；2.安徽信息工程學(xué)院 電氣與電子工程學(xué)院，安徽 蕪湖 241000)

同步定位與地圖構(gòu)建(Simultaneous Location and Mapping,SLAM)是移動機器人研究中面臨的主要問題，是實現(xiàn)移動機器人智能化亟待解決的關(guān)鍵技術(shù)[1-2]。基于卡爾曼濾波器的SLAM算法的提出有效地解決了最初的SLAM問題，但由于卡爾曼濾波應(yīng)用范圍僅局限于線性系統(tǒng)中，在移動機器人建圖過程中遇到的大多是非線性非高斯系統(tǒng)，針對此種問題，基于卡爾曼濾波器的SLAM算法并不能很好地解決[3]。隨著粒子濾波的出現(xiàn)以及非線性系統(tǒng)中粒子濾波的精度逐漸接近最優(yōu)估計的事實，粒子濾波已經(jīng)被大多數(shù)學(xué)者用于SLAM。其中Montemerlo[4]和Paskin[5]等提出了一種可以有效地解決SLAM問題的RBPF算法。目前，移動機器人實現(xiàn)自主定位與導(dǎo)航的基礎(chǔ)就是依靠各種傳感器，而激光傳感器是最精確、最穩(wěn)定的傳感器之一,因此，激光SLAM成為最主流的自主定位導(dǎo)航方法。對于簡單的室內(nèi)環(huán)境來說，與視覺SLAM對比，激光SLAM在精度和穩(wěn)定性上有著明顯優(yōu)勢[6]。目前，基于粒子濾波器(Particle Filter,PF)，即PF-SLAM算法的激光SLAM算法，在實際應(yīng)用中，環(huán)境中存在的大量路標(biāo)會引起PF-SLAM算法狀態(tài)向量“維數(shù)災(zāi)難”，導(dǎo)致求解計算過程十分復(fù)雜。文獻[7]首次提出將Rao-Blackwellised粒子濾波器(Rao-Blackwellised Particle Filter,RBPF)，即RBPF-SLAM算法應(yīng)用于解決SLAM過程中狀態(tài)向量維數(shù)高問題。文獻[8-9]認為基于RBPF-SLAM的算法可以分解為定位和建圖兩大部分。RBPF-SLAM算法的提出使得定位與建圖過程中的計算量大為減小，為SLAM的發(fā)展奠定了理論基礎(chǔ)[10]。針對算法實時性效率問題，一種基于RBPF-SLAM的Gmapping算法被提出，該算法的主要內(nèi)容為利用高精度、高穩(wěn)定性的激光測量數(shù)據(jù)可以改善提議分布的求解過程這一特性，使得改進后的提議分布與實際后驗概率分布更為接近，進而提升算法的實時性[11]。另一方面，針對基于粒子濾波的SLAM算法的研究中相繼被提出的主要為FastSLAM算法和GMapping算法[12]，兩種算法在求解過程均利用了粒子濾波能夠?qū)Χ嗄B(tài)分布進行非參數(shù)處理的特性，獲得與正確值更接近的狀態(tài)解，從而提升建圖效果，降低定位誤差。但是，在求解過程中粒子濾波器SLAM的結(jié)果不一致，以及狀態(tài)向量的不斷擴張會導(dǎo)致內(nèi)存消耗大，在進行狀態(tài)估計時，由于后驗概率的計算也會引起馬爾科夫假設(shè)的誤差累積，使得粒子濾波器SLAM算法受到很多限制[13]。

傳統(tǒng)RBPF-SLAM算法在實際應(yīng)用中往往需要大量的粒子。其主要原因一方面是在針對建議分布的計算過程中只考慮運動模型數(shù)據(jù)而忽略實際測量數(shù)據(jù)，引起估計值與觀測值的誤差較大，從而需要更多的粒子對誤差進行擬合，但這一過程消耗了大量內(nèi)存，降低了算法效率[14]。另一方面，在粒子濾波階段，由于粒子樣本貧化問題，大量粒子的數(shù)據(jù)不可用，降低了SLAM定位精度和誤差累計現(xiàn)象。對于此，Moral[15-16]等提出了一種自適應(yīng)重采樣方法改進粒子濾波階段的重采樣貧化問題以及降低使用粒子的數(shù)量。

在此基礎(chǔ)上，利用高精度的激光雷達傳感器數(shù)據(jù)來改進建議分布函數(shù)和修正里程計讀數(shù)，提升有效粒子集合的粒子數(shù)量。同時，針對粒子退化問題，采用一種自適應(yīng)重采樣機制約束重采樣進程，避免粒子濾波過程中的粒子貧化問題。以機器人操作系統(tǒng)(ROS)為基礎(chǔ)模型，進行機器人平臺實驗和仿真實驗驗證所提算法的有效性。

1 算法研究

1.1 基本RBPF-SLAM算法原理

SLAM的關(guān)鍵思想是求解后驗概率p(x1∶t,m|z1∶t,u1∶t-1),其中z1∶t=z1,z2,…,zt表示機器人自主觀測數(shù)據(jù)信息；u1∶t-1=u1,u2,…,ut-1表示里程計數(shù)據(jù)信息；x1∶t表示移動機器人的估計運行軌跡；m表示環(huán)境地圖。RBPF-SLAM算法將SLAM問題劃分為定位和建圖兩大部分，以此來降低采樣過程中狀態(tài)空間維數(shù)擴張問題，達到簡化計算過程的目的[17-18]。其中，基于環(huán)境地圖和機器人運行軌跡x1∶t的聯(lián)合后驗概率分布，即

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

(1)

式中，右側(cè)為兩個獨立的后驗概率乘積，其中p(x1∶t|z1∶t,u1∶t-1)為機器人運行軌跡的估計后驗概率；p(m|x1∶t,z1∶t)為環(huán)境地圖的后驗概率；在解決SLAM問題中，這樣的概率分解可以先計算機器人的運行軌跡，之后在已知機器人運行軌跡的情況下計算并更新當(dāng)前的環(huán)境地圖。每一個粒子代表一條可能的機器人運行路徑。

RBPF-SLAM算法的運行步驟具體如下：

(3)計算權(quán)重：根據(jù)權(quán)重采樣原則，計算每個粒子分布單獨的重要度權(quán)重ω(i)，即

(2)

1.2 激光雷達掃描測量模型

二維激光雷達的測量為平面(這里不考慮其在z軸方向上的運動，即其掃描范圍僅為平面),激光雷達的旋轉(zhuǎn)中心設(shè)置在坐標(biāo)系的原點，Neato XV-11激光傳感器的正面被定義為坐標(biāo)系的X軸。坐標(biāo)系遵循左手法則。

激光雷達掃描測量模型如圖1所示。由圖1可知，P為觀測點；d是P點距離傳感器的距離；θi是方位角(即激光束的發(fā)射角度)。由此將坐標(biāo)系轉(zhuǎn)換為笛卡爾坐標(biāo)系來處理原始數(shù)據(jù)，然后收集激光掃描數(shù)據(jù)的集合為B={(xi,yi)|i=1,2,…,N}，坐標(biāo)變換如式(3)所示。

圖1 激光雷達掃描測量模型

(3)

2 改進RBPF-SLAM算法

2.1 改進建議分布函數(shù)

單獨將里程計運動模型作為提議分布時，可以簡化計算，但由于里程計在運動過程中易受外部環(huán)境干擾，在進行重采樣過程中會出現(xiàn)粒子退化現(xiàn)象，導(dǎo)致重要粒子的丟失，激光的分布相比里程計分布更接近目標(biāo)分布。因此，為了能夠充分覆蓋高觀測似然區(qū)域，引入較為精確的激光雷達數(shù)據(jù)對建議分布函數(shù)進行改進。通過在觀測概率峰值附近選取K個值來模擬建議分布，降低粒子數(shù)目減少計算量。利用掃描匹配篩選出概率大的區(qū)域后，對此區(qū)域進行重采樣，利用近似化正態(tài)分布實現(xiàn)K個粒子概率值模擬出的改進建議分布。

SLAM要解決的問題是由控制數(shù)據(jù)u1∶t和觀測數(shù)據(jù)z1∶t來求位姿和地圖的聯(lián)合分布p(x1∶t,m|z1∶t,u1∶t-1)，為了改進提議分布，研究使用最近一次的觀測值計算一個粒子的采樣數(shù)據(jù)。然后，利用該粒子在生成下一代粒子時負責(zé)觀測最近代數(shù)粒子的權(quán)值，從目標(biāo)分布中獲得機器人的位姿估計，通過應(yīng)用貝葉斯定律展開得到改進建議分布為：

(4)

掃描過程中的粒子分布可近似表示為高斯函數(shù)：

(5)

(6)

(7)

(8)

在掃描匹配后，粒子分布改變，需要重新計算粒子權(quán)重，選取最接近相應(yīng)掃描的最大概率樣本粒子作為近似xj，根據(jù)前后2幀激光雷達掃描數(shù)據(jù)進行匹配，對移動機器人的運動模型數(shù)據(jù)進行修正，粒子權(quán)重變?yōu)椋?/p>

(9)

改進完建議分布后需要進行重采樣，在重采樣之前，利用迭代最近點算法(Iterative Closed Point,ICP)對粒子進行掃描匹配，求解最小誤差，提高位姿估計解算精度，迭代公式表示為：

(10)

2.2 改進自適應(yīng)重采樣

在重采樣濾波階段，出現(xiàn)的粒子權(quán)重退化現(xiàn)象主要表現(xiàn)為：當(dāng)?shù)谝淮Ｗ咏?jīng)過N次迭代后，部分粒子的權(quán)重開始變小，甚至出現(xiàn)權(quán)重為零的現(xiàn)象，其中僅有少部分粒子的權(quán)重較大。因此，引入有效粒子數(shù)Neff[19-20]估計當(dāng)前粒子集對目標(biāo)函數(shù)的近似程度，伴隨著濾波采樣的進行，無效采樣粒子數(shù)目逐漸增加，將會浪費大量的計算資源和內(nèi)存消耗，使得估計時間增加，定位精度低。因此，引入有效粒子數(shù)Neff來衡量粒子權(quán)值的退化程度，即

(11)

(12)

式(11)表示權(quán)重大的粒子過多或權(quán)重小的粒子過多，粒子權(quán)重之間差距越大，表明在重采樣過程中粒子所占權(quán)值退化現(xiàn)象越嚴重，當(dāng)采集的有效粒子數(shù)低于預(yù)先設(shè)定的閾值時，保留權(quán)值大的粒子進行重采樣。

采用一種自適應(yīng)的重采樣方法來保持粒子的多樣性，防止粒子退化現(xiàn)象的發(fā)生，其主要內(nèi)容為在重采樣階段，保持對權(quán)重較高和權(quán)重較低的粒子進行重采樣，對中等權(quán)值的粒子進行保留，具體步驟如下：

步驟2 對重采樣過程中每個粒子被復(fù)制的次數(shù)ni進行計算統(tǒng)計，并按照從高權(quán)重粒子群到低權(quán)重粒子群的順序排列，中等權(quán)重粒子群直接保留。建議分布函數(shù)中每個粒子被選擇的概率為：

(13)

式中，rank(i)表示權(quán)重改變后重新排列后的粒子序號；arank,brank為常數(shù)系數(shù)。根據(jù)式(13)確定，arank=2-brank,1≤brank≤2，取值為：

(14)

(15)

根據(jù)以上所述過程執(zhí)行重采樣，如果當(dāng)前粒子被復(fù)制的總次數(shù)∑ni<(NH+NL)，則將權(quán)重由高到低的粒子被復(fù)制的次數(shù)進行增加，最終達到∑ni=(NH+NL)；如果∑ni=(NH+NL)，則減少權(quán)重由低到高的粒子被復(fù)制的次數(shù)，達到∑ni<(NH+NL)，保持粒子總數(shù)不變。改進提議分布和自適應(yīng)重采樣后的RBPF-SLAM算法流程圖如圖2所示。

圖2 改進RBPF-SLAM算法流程圖

3 實驗結(jié)果分析

3.1 實驗平臺

實驗硬件平臺為配置有里程計和激光雷達的“旅行者Ⅱ”移動機器人，同時搭載了一臺裝有Ubuntu 12.04操作系統(tǒng)的筆記本電腦。激光雷達RPLIDAR AI分布于機器人頂部四周，里程計則位于移動平臺內(nèi)部，移動機器人平臺如圖3所示。機器人以0.5 m/s的速度勻速移動，進行在線實時構(gòu)建地圖，并用RVIZ顯示地圖。

圖3 移動機器人平臺 圖4 Gazebo仿真環(huán)境界面

3.2 基本建圖

實驗環(huán)境地圖大小為10 m*10 m，Gazebo仿真界面如圖4所示。搭建的虛擬仿真環(huán)境效果圖如圖5所示。采用傳統(tǒng)RBPF-SLAM算法構(gòu)建的室內(nèi)地圖如圖6所示，改進算法構(gòu)建的室內(nèi)地圖如圖7所示。在圖7中3個缺口表示未探測區(qū)域，其中被深灰色線條包圍的區(qū)域表示掃描到的障礙物區(qū)域。經(jīng)過反復(fù)實驗，在算法濾波定位階段，樣本粒子數(shù)目設(shè)置為30，效率和建圖效果更佳。從對比效果來看，相對于傳統(tǒng)的RBPF-SLAM算法，在提出的改進算法中，可以根據(jù)激光雷達和里程計數(shù)據(jù)優(yōu)化提議分布，改進的算法使用更少的粒子數(shù)目可以創(chuàng)建出精確度更高、效果更好的環(huán)境地圖。

圖5 虛擬仿真環(huán)境 圖6 傳統(tǒng)RBPF-SLAM算法

圖7 改進RBPF-SLAM算法

3.3 算法對比實驗

在實際工作環(huán)境下，移動機器人所面臨的比搭建的理想化地圖更為復(fù)雜，所包含的具有顯著特征環(huán)境信息也較豐富，且在運行過程中會受到如行人、移動障礙物等的影響。對于此種情況，分別用傳統(tǒng)RBPF-SLAM方法和改進的算法進行定位和構(gòu)圖實驗，以此來分析改進算法的有效性。

利用傳統(tǒng)算法構(gòu)建的部分辦公區(qū)域柵格地圖如圖8所示。改進算法構(gòu)建的部分辦公區(qū)域的柵格地圖如圖9所示。在構(gòu)圖掃描匹配過程中，輪式機器人的運動軌跡如圖10所示。從實驗結(jié)果可知，研究改進RBPF估計軌跡和真實值一開始相差較大，但最終會跟隨在真實值附近，具有良好的跟隨性，而傳統(tǒng)算法一直出現(xiàn)漂移現(xiàn)象且誤差越來越大。相比于傳統(tǒng)RBPF-SLAM方法，在環(huán)境結(jié)構(gòu)復(fù)雜的場景中，改進的RBPF-SLAM方法構(gòu)建的走廊及辦公區(qū)域柵格地圖基本無失真且構(gòu)建的柵格化地圖較清晰，定位精度更高。

圖8 傳統(tǒng)RBPF-SLAM算法構(gòu)建辦公區(qū)域地圖 圖9 改進RBPF-SLAM算法構(gòu)建辦公區(qū)域地圖

圖10 辦公區(qū)域運行軌跡對比

3.4 結(jié)果分析

為驗證改進算法能降低運行軌跡的誤差且減少所需粒子數(shù)，在2D LIDAR數(shù)據(jù)采集中，對改進算法進行反復(fù)實驗，真實軌跡與測量軌跡的一致性地圖如圖11所示。輪式機器人運行誤差分析如圖12所示。從圖11中可以看出，初始運動時粒子的真實軌道和預(yù)測軌道相差較多，但運行一段時間后，預(yù)測值逐漸向真實值靠近，并在真實值附近跟隨，具有良好的預(yù)測效果。在最開始運行時，粒子群誤差和測量誤差的差值較大；隨著運行時間的增加，粒子群誤差逐漸向測量誤差靠近，兩者誤差值逐漸變小，且一直保持在較低水平。

圖11 粒子運行軌跡 圖12 運行誤差分析

分析比較改進算法與傳統(tǒng)算法在運行時間上的性能對比，在實驗過程中分別采用兩種算法進行了5次實驗，對實驗結(jié)果取平均值作為統(tǒng)計值，性能對比如表1所示。由表1可知，改進算法所需粒子數(shù)遠小于傳統(tǒng)RBPF-SLAM算法，構(gòu)建地圖所需時間也比傳統(tǒng)RBPF-SLAM算法有較明顯的縮短。

表1 創(chuàng)建地圖性能對比表

4 結(jié)論

研究對基于RBPF的激光雷達SLAM算法進行了研究和優(yōu)化。通過運動里程計與RPLIDAR激光測量相結(jié)合，校正基于里程計測量值的建議分布函數(shù)，調(diào)整粒子權(quán)重提高了有效粒子數(shù)目；同時，通過自適應(yīng)重采樣的方法重新估計粒子的權(quán)重，得到精確的柵格地圖，有效地緩解了由于重采樣帶來的粒子退化問題，改進了粒子濾波在地圖估計中的不足。仿真結(jié)果表明，通過將運動里程計信息和激光測量數(shù)據(jù)相結(jié)合并通過自適應(yīng)重采樣的方法，解決了粒子權(quán)重退化問題，優(yōu)化后的激光SLAM算法的建圖和定位精度均得到提高。