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

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

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

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

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

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

1 算法研究

1.1 基本RBPF-SLAM算法原理

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

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

(1)

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

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

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

(2)

1.2 激光雷達掃描測量模型

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

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

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

(3)

2 改進RBPF-SLAM算法

2.1 改進建議分布函數

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

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

(4)

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

(5)

(6)

(7)

(8)

在掃描匹配后，粒子分布改變，需要重新計算粒子權重，選取最接近相應掃描的最大概率樣本粒子作為近似xj，根據前后2幀激光雷達掃描數據進行匹配，對移動機器人的運動模型數據進行修正，粒子權重變為：

(9)

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

(10)

2.2 改進自適應重采樣

在重采樣濾波階段，出現的粒子權重退化現象主要表現為：當第一代粒子經過N次迭代后，部分粒子的權重開始變小，甚至出現權重為零的現象，其中僅有少部分粒子的權重較大。因此，引入有效粒子數Neff[19-20]估計當前粒子集對目標函數的近似程度，伴隨著濾波采樣的進行，無效采樣粒子數目逐漸增加，將會浪費大量的計算資源和內存消耗，使得估計時間增加，定位精度低。因此，引入有效粒子數Neff來衡量粒子權值的退化程度，即

(11)

(12)

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

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

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

(13)

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

(14)

(15)

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

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

3 實驗結果分析

3.1 實驗平臺

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

圖3 移動機器人平臺 圖4 Gazebo仿真環境界面

3.2 基本建圖

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

圖5 虛擬仿真環境 圖6 傳統RBPF-SLAM算法

圖7 改進RBPF-SLAM算法

3.3 算法對比實驗

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

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

圖8 傳統RBPF-SLAM算法構建辦公區域地圖 圖9 改進RBPF-SLAM算法構建辦公區域地圖

圖10 辦公區域運行軌跡對比

3.4 結果分析

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

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

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

表1 創建地圖性能對比表

4 結論

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