基于Rao-Blackwellized粒子濾波器移動機器人SLAM研究

黃 輝， 鄒安安， 胡 鵬， 鄒媛媛， 蔡慶榮

(五邑大學 智能制造學部，廣東 江門 529020)

隨著科學技術和人工智能技術的蓬勃發展，人們對科技產品的追求意愿也在愈漸增大。與此同時，匯集多種技術的移動機器人的發展進入到一個高潮。需求推動科技產品的發展，在具體應用環境中，如災難現場、地下電纜等惡劣環境需要使用移動機器人代替人工進行檢測和探索。移動機器人能完成這些工作的前提是能感知并認識環境，并能確定自身處于環境的位置。SLAM(Simultaneous Localization and Mapping，同時定位與地圖創建)[1]是移動機器人完成上述任務的基本前提，也是移動機器人實現自主的關鍵。

SLAM為移動機器人在部分已知或未知環境中利用載有的傳感器感知環境并同時建圖和定位，Smith和Chesseman最先于1987年提出這一概念[2],國內外研究者共同推動了它的發展。如今，主要的SLAM研究方法有基于圖優化的方法和基于概率估計的方法。但在研究SLAM問題時，由于機器人具有很多不確定性，研究者更習慣把SLAM當作一個狀態估計問題，產生了基于概率估計的SLAM流派。擴展卡爾曼濾波(Extended Kalman Filter,EKF)和粒子濾波(Particle Filter，PF)方法常被用于SLAM狀態估計中，由此產生了EKF-SLAM和PF-SLAM算法。對于服從高斯分布噪聲的系統，EKF是以高斯函數來表示系統的概率密度函數，對于非線性模型的系統，EKF將非線性模型泰勒展開近似為線性模型，因此存在線性化誤差，在強非線性系統中效果很差。而PF是以一定數量的樣本粒子來近似系統概率密度分布[3]。粒子集若具有足夠的粒子數，就能表示任何復雜的概率分布，完美適用于非線性、非高斯系統的概率估計。

Doucet和Mur-phy等于1999年提出Rao-Blackwellized粒子濾波SLAM(RBPF-SLAM)[4]，借鑒粒子濾波的思想，通過具有一定權重的粒子來估計機器人位姿和環境地圖特征。粒子估計值與真實值越接近，粒子權重就越大，在迭代中就容易被保留下來；反之，估計值與真實值相差太遠的粒子，權重越小，更易在迭代中被舍去。粒子數目越多，與真實位置接近的粒子也越多，RBPF-SLAM算法定位、構圖就越精確，但是粒子數量的增多會加大算法的計算量并影響實時性[5-8]。

本文首先基于kobuki底盤和RPLIDAR A2雷達搭建機器人系統，針對常規RBPF-SLAM算法建圖精度差的不足，改進其建議分布，采用高精度的掃描幀匹配算法，增加自適應采樣操作，在已搭建好的機器人平臺中進行建圖實驗，驗證了優化RBPF-SLAM算法的實用性。

1 移動機器人總體框架

1.1 硬件框架

基于分層控制的理念，搭建了差速型機器人平臺，將機器人硬件系統架構分為3層：底層，中層和上層。底層為運動控制層，包括kobuki底盤、STM32控制板，帶編碼器的直流電機，執行導航規劃層發送的控制指令，并將測得的機器人里程數據反饋給導航規劃層，提供定位信息；中層為導航與規劃層，包括裝有ROS(Robot Operating System)的TX2開發板和主傳感器RPLIDAR A2雷達，收集機器人本體傳感器數據和激光傳感器數據，并對這些數據進行融合處理，計算出機器人位姿信息并根據相應規劃任務向底層發出控制指令；上層為人機交互層，包括裝有ROS的PC機，實時顯示機器人運動環境信息和自身狀態信息。中層與底層通過串口通信，上層與中層通過無線局域網通信。

圖1 移動機器人硬件框架

1.2 ROS系統

在ubutu16.04系統中，搭建ROS軟件系統[9]，/Base_link是公共話題，節點在此進行信息交互。里程計節點、激光節點、機器人節點分別記錄里程計信息、激光信息和鍵盤控制信息，并將記錄的信息發布到/Base_link主題；定位節點、地圖構建節點訂閱/Base_link主題，獲取所需的里程計信息和激光測距信息，通過/Rosaria節點控制機器人完成環境地圖構建、定位等操作，通過Rviz可視化平臺實時顯示地圖和機器人位置信息。

圖2 機器人及ROS的主要節點

2 RBPF-SLAM算法原理

2.1 基于粒子濾波的RBPF-SLAM算法

RBPF-SLAM的目的是給出激光觀測數據p(zt|xt,m)和里程計數據p(xt|xt-1,ut-1)，估計機器人運動軌跡x1∶t和地圖m的聯合后驗概率p(x1∶t,m|z1∶t,u1∶t-1)。將聯合后驗概率分解為

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

(1)

從而將SLAM問題分解為兩個后驗概率的乘積。式(1)等號右側第一部分為粒子的后驗路徑，通過粒子濾波來估計后驗路徑p(x1∶t|z1∶t,u1∶t-1)。每個粒子都有一個潛在的運動軌跡，這代表了機器人一條可能的路徑，同時生成自己的地圖。

式(1)等號右側第二部分為粒子生成的后驗地圖，采用EKF估計地圖特征，每個粒子都有自己單獨的地圖。使用占據柵格地圖算法來生成二維平面占用柵格圖，地圖m被劃分為有限多個柵格單元mi，每一柵格單元有占據和空閑兩種狀態。柵格有障礙物用占據(Occupied狀態)表示；柵格無障礙物用空閑(Free狀態)表示。在占據柵格地圖中，用p(mi)=0表示柵格空閑狀態概率，p(mi)=1表示柵格占據狀態概率，兩者概率和為1。假設柵格地圖中每個矩形柵格相互獨立，因此全局地圖m的占用概率為各單元格占用概率之積。后驗地圖近似為柵格單元邊緣的乘積。

(2)

2.2 RBPF-SLAM算法流程

RBPF-SLAM算法步驟如下。

② 權重估計。為了更好地估計建議分布，減少與目標分布的誤差，需要計算每個粒子的重要性權重。

(3)

③ 重采樣。根據設定的閾值對系統粒子進行重采樣。重采樣后，所有粒子具有相同的權重。

3 RBPF-SLAM改進

3.1 改進提議分布

獲得下一時刻的粒子集，需要從建議分布π抽取樣本粒子。建議分布和目標分布越接近，算法估計的最終結果越好。傳統RBPF-SLAM算法采用里程計運動模型作為建議分布，該運動模型計算簡單，根據觀測模型計算重要性權重

(4)

因為地面不平、輪子打滑等影響，里程計數據的準確性較低，建議分布采用里程計模型采樣時樣本只能覆蓋狀態空間區域的一小部分，導致粒子之間權重差異大，得到的有效粒子數少，如圖3所示。傳統RBPF-SLAM算法執行時，由于有效粒子數少需多次重新采樣，導致粒子耗散且多樣性降低，影響算法性能。針對這些問題，考慮精度更高的激光傳感器數據，通過匹配前后兩幀激光數據獲取機器人的運動參數，修正機器人里程計運動模型。將其定位在一個更精確的似然區域，從而采樣更多有效粒子。

圖3 粒子似然區域

因此，將激光數據zt融合到建議分布中，采樣集中在觀測可能性更高的區域，能夠得到采樣粒子權重方差的最佳建議分布。由此產生的建議分布為

(5)

使用該建議分布時，權重計算公式為

(6)

選用迭代最近點算法(Iterative Closest Point,ICP)對激光點云數據進行掃描匹配。設當前幀點集為P={p1,p2,…,pm}，對應參考點集為Q={q1,q2,…,qn}，分別包含m個點和n個點，求解兩組點云之間的變換T(T={R,t})，使式(7)得到最優解。

(7)

式中，Qi、Pi分別為參考點集和當前點集在笛卡爾坐標系下第i對鄰近點的二維坐標；N為成功配對的點對數。

3.2 自適應重采樣

重采樣是RBPF-SLAM算法的重要步驟。在進行粒子重采樣時，用高權值的粒子替代低權值的粒子。但是，在重采樣過程中有可能會濾除效果不錯但權值不高的粒子，這會導致有效粒子數減少、粒子退化。因此，引入有效粒子數Neff[10]來決定是否有必要執行重采樣操作，它表示粒子集的退化程度，其值越大說明粒子集多樣性越好，越小則粒子退化越嚴重。有效粒子數計算公式為

(8)

圖4 優化的RBPF-SLAM算法流程框圖

4 仿真和實驗

4.1 建圖環境與結果

選取實驗環境為兩個不同大小區域，如圖5、圖6所示，在開源的ROS中進行RBPF-SLAM建圖實驗。首先PC機遠程連接處于同一局域網中的移動機器人機載工控機，啟動底盤和激光雷達，同時啟動手動控制，以恒定速度控制機器人掃描環境一周，掃描過程中利用激光數據和里程計數據更新環境地圖和機器人位姿信息。常規RBPF-SLAM算法和優化的RBPF-SLAM算法構建的地圖如圖7、圖8所示。

圖5 環境1

圖6 環境2

圖7 環境1地圖

圖8 環境2地圖

實驗環境1為10 m×5 m的辦公區域，常規的RBPF-SLAM算法構建的柵格地圖出現扭曲、失真的現象，而優化后的RBPF-SLAM算法構建的柵格地圖精度更高，地圖中障礙物輪廓更清晰，且地圖無扭曲失真現象。

實驗環境2為3 m×4 m的小房間，常規的RBPF-SLAM算法由于回環錯誤，構建的柵格地圖出現重疊的現象，而優化的RBPF-SLAM算法，則回環成功，地圖輪廓分明，能更好地表示環境。

4.2 結果分析

對比2種方法的運行時間和機器人在地圖中的定位誤差，分別在環境1、環境2各進行10次實驗，記錄運行時間并在Rviz中查看機器人在地圖中的定位誤差，如表1所示。改進的RBPF-SLAM算法的運行效率以及地圖平均誤差、最大誤差都要優于常規RBPF-SLAM算法，創建的地圖也更加清晰精確，輪廓分明，表明改進的RBPF-SLAM算法構建的地圖精度更優。

表1 不同算法定位誤差比較

5 結束語

利用激光雷達、kobuki底盤搭建了差速型機器人平臺，為機器人搭建了ROS軟件操作系統。從RBPF-SLAM的基本定義入手，研究了基于柵格地圖的RBPF-SLAM算法，在常規RBPF-SLAM算法的基礎上，改進其建議分布函數，對點云采用迭代最近點匹配，并增加自適應重采樣操作，得到最優粒子，構建了更加逼近環境的真實地圖。在ROS系統中基于現場環境實時建圖，對比運行時間和機器人定位誤差，結果表明優化后的RBPF-SLAM算法能以更高的運行速度構建精度更高的柵格地圖。移動機器人的分層設計理念能提高機器人各功能模塊的復用性，改進的RBPF-SLAM算法為移動機器人導航提供了條件。