基于改進RBPF算法的移動機器人SLAM方法研究

陳國軍,陳 巍

(南京工程學院工業中心，江蘇 南京 211167)

0 引言

自主導航是移動機器人研究的重點。為了實現自主導航，機器人需要理解運動場景的結構和方向。與此同時，定位和地圖構建(simultaneous location and map construction,SLAM)已經成為自主導航研究中的重要內容。機器人創建地圖，更新結構和定位自己，實現導航。基于粒子濾波的SLAM算法，例如FastSLAM算法和GMapping算法[1]，利用了粒子濾波非參數特性，能夠處理多模態分布，從而估計更接近正確值的狀態解。由于粒子濾波器SLAM的結果不一致、內存消耗大，以及馬爾可夫假設的誤差積累，使得粒子濾波器SLAM的研究遇到很多問題[2]。近年來，許多SLAM算法轉向使用位姿圖SLAM(oriented fast and rotated brief SLAM,ORBSLAM)算法。位姿圖SLAM算法包括兩個主要部分，前端用于從數據關聯中跟蹤機器人軌跡以創建姿態圖，后端用于優化姿態圖。但是位姿圖SLAM也存在很多問題。不良的位姿圖結構會破壞優化過程，使得結果陷入局部極小值[3]。本文針對非參數方法趨向于避免陷入局部位姿圖極小值的問題，對基于粒子濾波的方法進行了改進，利用修正梯度細化(corrective gradient refinement,CGR)算法對傳統的Rao-Backwellized粒子濾波SLAM(Rao-Backwellized particle filtering SLAM,RBPF SLAM)算法進行了擴充。使用柵格地圖結構和激光雷達傳感器作為提出的廣義SLAM算法，并將其擴展為位姿圖SLAM前端圖構建的基礎。

1 RBPF SLAM算法

在RBPF方法中，將狀態空間劃分為姿態狀態和地圖狀態。粒子濾波(particle filtering,PF)近似機器人的姿態置信度分布，而每個粒子包含表示環境模型的映射[4]。RBPF SLAM算法是使用PF來估計給定里程計測量序列U0:t-1和環境觀測Z0:t的機器人軌跡分布p(xt,m|Z0:t,U0:t-1)以及環境地圖m。采用該軌跡估計來估計地圖和軌跡上的期望分布：

p(xt,m|Z0:t,U0:t-1)=p(m|xt,Z0:t)p(xt|Z0:t,U0:t-1)

(1)

PF工作類似于蒙特卡洛定位，除了一個給定的地圖，每個粒子包含一個單獨的地圖。其思想是將機器人軌跡估計與地圖估計過程分開[5]。該分解允許首先估計機器人軌跡，然后計算給定軌跡地圖：

p(x1:t,m|Z1:t,U1:t-1)=p(m|x1:t,Z1:t)p(x1:t|Z1:t,U1:t-1)

(2)

由于x1:t和z1:t是已知的(以已知姿態映射)，因此可以有效地進行后驗過映射p(m|x1:t,Z1:t)的解析計算。后部p(x1:t|Z1:t,U1:t-1)的估計與定位問題相似，因為僅需要估計機器人軌跡。這是用PF進行增量處理觀察和測距讀數[6]。整個過程可以概括如下(對于每個粒子i)。

②重要度加權：根據最近的觀察、姿態估計和與該粒子相關聯的映射，為每個粒子分配單獨的重要度權重ω(i)。

(3)

③重采樣：具有低重要性權重的粒子ω通常被高重量的樣品所取代。此步驟是必要的，因為僅使用有限數量的粒子來近似連續分布。此外，重采樣允許在真實分布不同于提議的情況下應用PF。

(4)

基于掃描過程所傳遞的數據，計算粒子分布的高斯近似Ψ。

(5)

參數μ(i)和∑(i)分別是平均值和方差，分別通過每個粒子i進行確定。這些參數估計如下：

(6)

(7)

歸一化因子：

(8)

通過使用式(7)計算生成有效粒子，然后權重可以近似為xj，是接近相應掃描所指出的最大概率的點樣本)：

(9)

重新分布的粒子將保持在狀態表示中，而另一些則將被去除，這樣可以替換樣本，并且使得最終的根據狀態軌跡重新進行粒子分布。通過重新采樣，更新標準決定執行重采樣步驟，通過估計有效粒子數neff的方法是估計粒子方差。

(10)

(11)

2 改進RBPF SLAM算法

所提出的算法遵循連接圖路由算法(contact graph routing，CGR)局部化結構，并通過Rao-Backwellized粒子濾波器將其擴展到SLAM應用[8]。該方法是將CGR算法合并到RBPF中，以便改進軌跡p(x1:t|z1:t,u1:t-1)。通過四個CGR算法步驟(預測、精煉、接受、更新和重采樣)[9]，其算法可以總結如下。

①預測步驟：遵循CGR預測步驟，得到初始狀態建議分布集q0作為結果。

③驗收測試步驟：在產生最終分布集q的CGR算法，精確驗收測試步驟。

④更新和重采樣步驟：根據新的RBPF權重推導，通過廣義細化步驟的改進建議來更新權重。粒子集重采樣準則改變后，使用選擇性重采樣來防止粒子退化。

(12)

在實現過程中，為了降低復雜度、避免浮點誤差，忽略了歸一化常數，優選對數似然版本。D是對應距離dk的集合，將非歸一化的觀測對數似然定義為[11]：

(13)

傳統的Beam端點模型的實現需要對整個地圖的似然分數緩存進行預計算，這主要是為了進行定位。然而，這在SLAM情況下是不合適的。這是因為在計算觀測似然分數之前沒有關于地圖的信息。此外，通過使用匹配，不能保證每個觀測光束zk都能匹配其對應關系。因此，當未找到其對時，通過懲罰似然得分，產生了似然得分的最終形式。

(14)

式中：umull為一個恒定的懲罰距離，因為沒有找到對應關系-空似然得分。

3 試驗研究

3.1 仿真試驗

試驗測試結果如圖1所示。

圖1 試驗測試結果

本文在機器人操作系統(robot operating system,ROS)上實現了該算法，并進行試驗評估。先使用2個基準數據集進行仿真測試，由激光傳感器采集。數據集是在具有動態對象的無約束戶外環境中收集的。本文算法至少需要20個粒子來創建這個圖，第一個基準數據集(Intel試驗室數據集)是由一個差分驅動器配備LMS激光雷達，仿真場景大小近似為28 m×28 m。本文算法需要5個粒子來正確地創建一個具有5厘米柵格分辨率的閉環映射，如圖1(a)所示。在第二個數據集(Campus數據集)上進行測試，配備SICK LMS激光雷達，仿真場景大小近似為250 m×250 m，如圖1(b)所示。試驗表明，本文算法利用在實際的基準數據集中創建地圖，同時在所需的粒子方面保持效率。

3.2 室內場景試驗

本文使用從安裝有RPLIDAR A1激光傳感器的開源ROS移動機器人平臺收集自己的試驗數據集，在計算機上運行 Liunx(Ubuntu 16.04)的ROS系統，完成同時定位與地圖構建。為了測試改進RBPF SLAM算法提供的校正姿態的質量，需要確定適當的最小閾值分數以接受或拒絕姿態。由于得分根據傳感器和地圖上下文而不同，因此需要調優以獲得適當的地圖，將掃描匹配得分定義為：

(15)

式中：σsm為掃描匹配標準偏差。

本文將長度為30 m的試驗樓走廊設置為場景，試驗結果如圖2所示。

其中，圖2(a)、圖2(b)和圖2(c)通過根據上面的公式改變分數應用RBPF SLAM算法得到的結果，圖2(d)是根據本文的算法給出的結果。從結果來看，通過接受性測試與當前地圖最匹配的觀察可能性來概率地評估姿態，本文提出的方法能給出更少失真的地圖，使移動機器人定位精度得到了有效的提高，地圖構建的準確性也得到了有效的提升。試驗結果表明，該方法具有良好的系統魯棒性。

圖2 試驗結果

4 結束語

本文在RBRF SLAM算法的基礎上，通過將RBPF算法與CGR算法相結合，提出了基于CGR算法的改進RBPF SLAM方法。該方法有效地減少了所需粒子的數量，降低了在濾波預測階段機器人姿態估計的不確定性。本文提出的算法是在激光傳感器上實現的，在移動機器人試驗室環境下的試驗和結果分析。在LIDAR SLAM基準數據集上的測試表明，通過試驗分析，基于粒子濾波的RBPF SLAM算法能夠實時在線生成高精度柵格地圖，與實際地圖更加一致。