陳國軍,陳 巍

(南京工程學(xué)院工業(yè)中心，江蘇 南京 211167)

0 引言

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

1 RBPF SLAM算法

在RBPF方法中，將狀態(tài)空間劃分為姿態(tài)狀態(tài)和地圖狀態(tài)。粒子濾波(particle filtering,PF)近似機器人的姿態(tài)置信度分布，而每個粒子包含表示環(huán)境模型的映射[4]。RBPF SLAM算法是使用PF來估計給定里程計測量序列U0:t-1和環(huán)境觀測Z0:t的機器人軌跡分布p(xt,m|Z0:t,U0:t-1)以及環(huán)境地圖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是已知的(以已知姿態(tài)映射)，因此可以有效地進行后驗過映射p(m|x1:t,Z1:t)的解析計算。后部p(x1:t|Z1:t,U1:t-1)的估計與定位問題相似，因為僅需要估計機器人軌跡。這是用PF進行增量處理觀察和測距讀數(shù)[6]。整個過程可以概括如下(對于每個粒子i)。

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

(3)

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

(4)

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

(5)

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

(6)

(7)

歸一化因子：

(8)

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

(9)

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

(10)

(11)

2 改進RBPF SLAM算法

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

①預(yù)測步驟：遵循CGR預(yù)測步驟，得到初始狀態(tài)建議分布集q0作為結(jié)果。

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

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

(12)

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

(13)

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

(14)

式中：umull為一個恒定的懲罰距離，因為沒有找到對應(yīng)關(guān)系-空似然得分。

3 試驗研究

3.1 仿真試驗

試驗測試結(jié)果如圖1所示。

圖1 試驗測試結(jié)果

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

3.2 室內(nèi)場景試驗

本文使用從安裝有RPLIDAR A1激光傳感器的開源ROS移動機器人平臺收集自己的試驗數(shù)據(jù)集，在計算機上運行 Liunx(Ubuntu 16.04)的ROS系統(tǒng)，完成同時定位與地圖構(gòu)建。為了測試改進RBPF SLAM算法提供的校正姿態(tài)的質(zhì)量，需要確定適當?shù)淖钚￠撝捣謹?shù)以接受或拒絕姿態(tài)。由于得分根據(jù)傳感器和地圖上下文而不同，因此需要調(diào)優(yōu)以獲得適當?shù)牡貓D，將掃描匹配得分定義為：

(15)

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

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

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

圖2 試驗結(jié)果

4 結(jié)束語

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