基于改進(jìn)RBPF算法的移動(dòng)機(jī)器人SLAM方法研究

陳國(guó)軍,陳 巍

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

0 引言

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

1 RBPF SLAM算法

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

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

(1)

PF工作類(lèi)似于蒙特卡洛定位，除了一個(gè)給定的地圖，每個(gè)粒子包含一個(gè)單獨(dú)的地圖。其思想是將機(jī)器人軌跡估計(jì)與地圖估計(jì)過(guò)程分開(kāi)[5]。該分解允許首先估計(jì)機(jī)器人軌跡，然后計(jì)算給定軌跡地圖：

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)映射)，因此可以有效地進(jìn)行后驗(yàn)過(guò)映射p(m|x1:t,Z1:t)的解析計(jì)算。后部p(x1:t|Z1:t,U1:t-1)的估計(jì)與定位問(wèn)題相似，因?yàn)閮H需要估計(jì)機(jī)器人軌跡。這是用PF進(jìn)行增量處理觀察和測(cè)距讀數(shù)[6]。整個(gè)過(guò)程可以概括如下(對(duì)于每個(gè)粒子i)。

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

(3)

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

(4)

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

(5)

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

(6)

(7)

歸一化因子：

(8)

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

(9)

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

(10)

(11)

2 改進(jìn)RBPF SLAM算法

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

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

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

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

(12)

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

(13)

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

(14)

式中：umull為一個(gè)恒定的懲罰距離，因?yàn)闆](méi)有找到對(duì)應(yīng)關(guān)系-空似然得分。

3 試驗(yàn)研究

3.1 仿真試驗(yàn)

試驗(yàn)測(cè)試結(jié)果如圖1所示。

圖1 試驗(yàn)測(cè)試結(jié)果

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

3.2 室內(nèi)場(chǎng)景試驗(yàn)

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

(15)

式中：σsm為掃描匹配標(biāo)準(zhǔn)偏差。

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

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

圖2 試驗(yàn)結(jié)果

4 結(jié)束語(yǔ)

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