基于固定滯后Ｇｉｂｂｓ采樣粒子濾波的移動(dòng)機(jī)器人ＳＬＡＭ

(1.華東交通大學(xué) 信息工程學(xué)院， 南昌 330013； 2.中南大學(xué) 信息科學(xué)與工程學(xué)院， 長(zhǎng)沙 410075)



摘要：針對(duì)采用Rao-Blackwellized粒子濾波器的移動(dòng)機(jī)器人同步定位與地圖構(gòu)建算法（RBPF-SLAM）所面臨的粒子退化問(wèn)題，提出了一種改進(jìn)的采樣方法。該方法在原有采樣方法的基礎(chǔ)上，加入一個(gè)用Gibbs采樣實(shí)現(xiàn)的向后MCMC（Markov chain Monte Carlo）移動(dòng)步驟，利用當(dāng)前新獲取的信息對(duì)機(jī)器人路徑樣本的最后一段進(jìn)行調(diào)整，從而降低了樣本退化的可能性。對(duì)比仿真實(shí)驗(yàn)驗(yàn)證了該方法的有效性。

關(guān)鍵詞：同步定位與地圖構(gòu)建； Rao-Blackwellized粒子濾波器； MCMC移動(dòng)； Gibbs采樣

中圖分類號(hào)：TP24文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1001-3695(2008)11-3292-04



Mobile robot simultaneous localization and mapping

based on particle filtering with fixed-lag Gibbs sampling



ZHANG Heng1，2， LIU Yan-li1， FAN Xiao-ping2， QU Zhi-hua2



(1. School of Information Engineering， East China Jiaotong University， Nanchang 330013， China； 2. School of Information Science Engineering， Central South University， Changsha 410075， China)



Abstract:For the particle degeneracy problem of mobile robot simultaneous localization and mapping algorithm using Rao-Blackwellized particle filter (RBPF-SLAM)， this paper proposed an improved sampling strategy. After the usual RBPF sampling step was completed， it incorporated a post Markov chain Monte Carlo( MCMC) move step to perturb the trajectory of each particle over a fixed lag time. The added step exploited the new information to improve the filter’s estimation for previous time steps， and reduced the probability of degeneracy. Comparing with the usual RBPF-SLAM algorithm， experimental results show the efficiency of the proposed method.

Key words：SLAM; Rao-Blackwellized particle filter; MCMC move; Gibbs sampling



0引言

在眾多移動(dòng)機(jī)器人的工作任務(wù)中，機(jī)器人對(duì)工作空間的信息并不是預(yù)先就知道的，或只有部分不確定的信息。其中一些任務(wù)可能需要機(jī)器人對(duì)環(huán)境進(jìn)行有效的探測(cè)并構(gòu)建出環(huán)境地圖，或者邊構(gòu)建地圖邊執(zhí)行任務(wù)，這就帶來(lái)了機(jī)器人環(huán)境探測(cè)和地圖構(gòu)建的問(wèn)題。地圖構(gòu)建就是建立機(jī)器人周圍環(huán)境的空間模型，并指導(dǎo)機(jī)器人導(dǎo)航或定位。為了得到地圖，機(jī)器人必須擁有外部傳感器來(lái)感知外界環(huán)境，但這些傳感器都存在測(cè)量誤差，且一般視野有限，使得機(jī)器人在構(gòu)建地圖的同時(shí)必須移動(dòng)。對(duì)機(jī)器人的運(yùn)動(dòng)控制為地圖構(gòu)建提供了豐富的信息，因?yàn)樗鼈儼藱C(jī)器人在每次進(jìn)行觀測(cè)時(shí)所處的位置信息。機(jī)器人運(yùn)動(dòng)測(cè)量同樣有誤差，僅僅依靠運(yùn)動(dòng)控制信息(包括里程計(jì))不足以決定機(jī)器人的位姿，需要融合外部傳感器的信息進(jìn)行定位。于是，地圖構(gòu)建過(guò)程依賴于精確的位姿信息，而機(jī)器人定位過(guò)程反過(guò)來(lái)又依賴于精確的地圖信息，兩者既矛盾又相關(guān)，必須同時(shí)加以考慮。對(duì)該問(wèn)題的系統(tǒng)研究最早出現(xiàn)在Smith等人^[1]撰寫的文章中，他們將該問(wèn)題稱為移動(dòng)機(jī)器人同步定位與地圖構(gòu)建(simultaneous localization and mapping，SLAM)。

自從Montemerlo等人^[2]提出了基于Rao-Blackwellized粒子濾波器的移動(dòng)機(jī)器人同步定位與地圖構(gòu)建算法(RBPF-SLAM)以來(lái)，該算法得到了廣泛的關(guān)注。最近的研究^[3]表明RBPF-SLAM算法容易產(chǎn)生不一致的地圖。其原因在于當(dāng)粒子權(quán)值分布的方差較大時(shí)出現(xiàn)粒子退化現(xiàn)象，在這種情況下重采樣步驟對(duì)一些有較高權(quán)值的粒子多次復(fù)制而拋棄權(quán)值較小的粒子。隨著重采樣操作的反復(fù)運(yùn)用，對(duì)機(jī)器人運(yùn)動(dòng)軌跡的過(guò)去部分的采樣趨于退化，導(dǎo)致其不足以表達(dá)估計(jì)的不確定性。最明顯的例子是機(jī)器人做環(huán)形運(yùn)動(dòng)后，往往不能得到一致地圖。

本文提出一種改進(jìn)的采用柵格地圖的RBPF-SLAM算法。該算法在現(xiàn)有的改進(jìn)型柵格RBPF-SLAM算法^[4]的基礎(chǔ)上，加入一個(gè)用Gibbs采樣實(shí)現(xiàn)的向后MCMC(Markov chain Monte Carlo)移動(dòng)步驟，利用當(dāng)前新獲取的信息對(duì)機(jī)器人路徑樣本的最后一段進(jìn)行調(diào)整，從而降低了樣本退化的可能性。

1RBPF-SLAM算法框架

采用Rao-Blackwellized粒子濾波器解SLAM問(wèn)題的關(guān)鍵思想是：給定觀測(cè)信息z1:t和控制輸入信息u1:t的條件下，估計(jì)機(jī)器人運(yùn)動(dòng)軌跡ξ1:t和環(huán)境地圖m的聯(lián)合后驗(yàn)分布p(ξ1:t，m｜u1:t，z1:t)，該思想最早由Murphy^[5]提出。根據(jù)貝葉斯公式，有

p(ξ1:t，m｜u1:t，z1:t)=p(m｜ξ1:t，z1:t)p(ξ1:t｜z1:t，u1:t)(1)

這樣，SLAM問(wèn)題就可以分為兩步解決：首先僅估計(jì)機(jī)器人的運(yùn)動(dòng)軌跡，然后基于估計(jì)的軌跡計(jì)算環(huán)境模型。式(1)中的p(ξ1:t｜z1:t，u1:t)部分類似于機(jī)器人定位問(wèn)題，當(dāng)觀測(cè)和控制輸入信息到達(dá)時(shí)，采用粒子濾波器遞增地進(jìn)行估計(jì);p(m｜ξ1:t，z1:t)部分類似于已知定位條件下的地圖構(gòu)建問(wèn)題，可以非常方便地解決。于是可以采用Rao-Blackwellized粒子濾波器來(lái)實(shí)現(xiàn)SLAM算法。在此濾波器中，每個(gè)粒子代表一條機(jī)器人的可能路徑，每個(gè)粒子與一個(gè)獨(dú)立的完整地圖相關(guān)聯(lián)，且基于粒子中的機(jī)器人軌跡的估計(jì)對(duì)相應(yīng)的地圖進(jìn)行更新。 

最常用的粒子濾波算法是采樣重要性重采樣(sampling importance resampling，SIR)濾波器，Rao-Blackwellized SIR濾波器可以遞推地方式處理觀測(cè)信息和控制輸入信息來(lái)構(gòu)建環(huán)境地圖。這個(gè)過(guò)程可以概括為以下四步： 

a)采樣?；谇耙粫r(shí)刻的樣本集{ξ(i)t-1}，從提議分布(proposal distribution)π抽取樣本{ξ(i)t}用于描述當(dāng)前時(shí)刻機(jī)器人的位姿。通常，用機(jī)器人的概率運(yùn)動(dòng)模型作為提議分布。 

b)重要性權(quán)值計(jì)算。給每個(gè)新樣本指定一權(quán)值，其計(jì)算方法是根據(jù)如下的重要性采樣原理計(jì)算：

w(i)t=p(ξ(i)1:t｜z1:t，u1:t)/π(ξ(i)1:t｜z1:t，u1:t)(2)

重要性權(quán)值用來(lái)補(bǔ)償目標(biāo)分布(target distribution)與提議分布之間的差別。 

c)重采樣。依據(jù)重要性權(quán)值{w(i)t}對(duì)樣本集{ξ(i)t}進(jìn)行重采樣。由于只能用有限的樣本模擬連續(xù)分布，重采樣步驟是必要的，通過(guò)該步驟除去權(quán)值較小的樣本，使計(jì)算集中在權(quán)值較大的樣本上，從而在一定程度上避免樣本退化現(xiàn)象的發(fā)生。另外，重采樣使粒子濾波算法可以應(yīng)用于提議分布與實(shí)際分布不同的情況，重采樣后，各樣本具有相同的重要性權(quán)值。 

d)地圖估計(jì)。對(duì)于描述機(jī)器人軌跡的每個(gè)樣本ξ(i)1:t，其相應(yīng)的地圖估計(jì)m(i)1:t可以利用p(m(i)1:t｜ξ(i)1:t，z1:t)計(jì)算。 

在這樣實(shí)現(xiàn)框架中，當(dāng)一次新的觀測(cè)信息獲得時(shí)，需要批量地計(jì)算機(jī)器人軌跡的重要性權(quán)值。由于機(jī)器人軌跡的長(zhǎng)度隨時(shí)間不斷增長(zhǎng)，這種計(jì)算方式使得該算法的效率越來(lái)越低。為了用遞推的方式計(jì)算重要性權(quán)值，選取提議分布滿足以下分解公式： 

π(ξ1:t｜z1:t，u1:t)=π(ξt｜ξ1:t-1，z1:t，u1:t)π(ξ1:t-1｜z1:t-1，u1:t-1)(3)

基于式(2)和(3)，重要性權(quán)值計(jì)算方式如下： 

w(i)t=[p(zt｜ξ(i)1:t，z1:t-1，u1:t)p(ξ(i)1:t｜z1:t-1，u1:t)]/

[p(zt｜z1:t-1，u1:t)π(ξ(i)t｜ξ(i)1:t-1，z1:t，u1:t)π(ξ(i)1:t-1｜z1:t-1，u1:t-1)]=

[ηp(zt｜ξ(i)1:t，z1:t-1)p(ξ(i)t｜ξ(i)1:t-1，ut)]/π(ξ(i)t｜ξ(i)1:t-1，z1:t，u1:t)×

p(ξ(i)1:t-1｜z1:t-1，u1:t-1)/π(ξ(i)1:t-1｜z1:t-1，u1:t-1)w(i)i-1∝w(i)t-1×

p(zt｜m(i)1:t-1，ξ(i)t)p(ξ(i)t｜ξ(i)1:t-1，ut)/π(ξ(i)t｜ξ(i)1:t-1，z1:t，u1:t)incremental weight(4)

其中：η=1/p(zt｜z1:t-1，u1:t)為歸一化系數(shù)，對(duì)于每個(gè)粒子均相等。 

至此，形成了一個(gè)完整的RBPF-SLAM算法框架，但實(shí)現(xiàn)時(shí)還要解決一些具體的問(wèn)題，如何設(shè)計(jì)地圖的表達(dá)方式及存取算法，使之方便于RBPF計(jì)算過(guò)程；如何選擇合理的提議分布并能方便地抽取樣本；重采樣如何進(jìn)行及什么時(shí)候進(jìn)行重采樣。本文將從后面兩個(gè)方面深入研究RBPF-SLAM算法的改進(jìn)方法。 

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

21融入當(dāng)前觀測(cè)信息采樣

為減輕重要性權(quán)值的退化，一個(gè)關(guān)鍵的步驟是重要性分布函數(shù)的選擇。Doucet等人^[6]證明，在粒子重要性權(quán)值方差最小意義下的最優(yōu)提議分布函數(shù)為

p(ξt｜ξ(i)1:t-1，z1:t，ut)=p(ξt｜m(i)1:t-1，ξ(i)t-1，zt，ut)=[p(zt｜m(i)1:t-1，ξt)p(ξt｜ξ(i)i-1，ut)]/p(zt｜m(i)1:t-1，ξ(i)t-1，ut)(5)

對(duì)應(yīng)的重要性權(quán)值計(jì)算公式為

w(i)t=w(i)t-1 [(ηp(zt｜m(i)1:t-1，ξ(i)t)p(ξ(i)t｜ξ(i)t-1，ut)]/

p(ξt｜m(i)1:t-1，ξ(i)t-1，zt，ut)∝w(i)t-1 [p(zt｜m(i)1:t-1，ξ(i)t)p(ξ(i)t｜ξ(i)t-1，ut)×p(zt｜m(i)1:t-1，ξ(i)t-1，ut)]/[p(zt｜m(i)1:t-1，ξt)p(ξt｜ξ(i)t-1，ut)]=w(i)t-1p(zt｜m(i)1:t-1，ξ(i)t-1，ut)=w(i)t-1∫p(zt｜m(t)1:t-1，ξ′)p(ξ′｜ξ(i)t-1，ut)dξ′ (6)

在一些粒子濾波器的應(yīng)用中，通常用運(yùn)動(dòng)模型p(ξt｜ξt-1，ut)作為提議分布函數(shù)。然而，若觀測(cè)模型相比運(yùn)動(dòng)模型更為精確(如本文中采用的激光掃描模型)，則似然函數(shù)p(zt｜m(i)1:t-1，ξt)對(duì)乘積p(zt｜m(i)1:t-1，ξt)p(ξt｜ξ(i)t-1，ut)的結(jié)果起主要作用，導(dǎo)致提取的樣本集中只有極少數(shù)樣本的重要性權(quán)值很大，而其他絕大多數(shù)樣本的重要性權(quán)值接近于0，這樣就容易出現(xiàn)粒子貧化問(wèn)題。 

文獻(xiàn)[4]設(shè)計(jì)了具體的融入當(dāng)前觀測(cè)信息采樣RBPF-SLAM算法的計(jì)算方法，即將最優(yōu)提議分布函數(shù)近似化為一個(gè)高斯分布，從而得到一種閉合的計(jì)算形式，可以方便地進(jìn)行采樣。

p(ξt｜m(i)1:t-1，ξ(i)t-1，zt，ut)≈N(μ(i)t，(i)t)(7)

對(duì)于每一個(gè)粒子i，參數(shù)μ(i)t和(i)t可以通過(guò)測(cè)試在區(qū)間R(i)提取的K個(gè)樣本點(diǎn){ξj}Kj=1的值來(lái)計(jì)算：

R(i)={ξ｜p(zt｜m(i)t-1，ξ)＞ε}(8)

μ(i)t=(1/η(i))∑Kj=1ξjp(zt｜m(i)1:t-1，ξj)p(ξj｜ξ(i)t-1，ut)(9)

(i)t=(1/η(i))∑Kj=1p(zt｜m(i)1:t-1，ξj)×p(ξj｜ξ(i)t-1，ut)(ξj-μ(i)t)(ξj-μ(i)t)T

(10)

其中：η(i)=∑Kj=1p(zt｜m(i)1:t-1，ξj)p(ξj｜ξ(i)t-1，ut)為歸一化系數(shù)，ξj∈{ξt｜p(ξt｜ξt-1，ut)＞λ}，對(duì)應(yīng)的重要性權(quán)值計(jì)算公式為

w(i)t≈w(i)t-1∑Kj=1p(zt｜m(i)1:t-1，ξj)p(ξj｜ξ(i)t-1，ut)=w(i)t-1η(i)(11)

具體的融入當(dāng)前觀測(cè)信息采樣過(guò)程為算法1。

算法1融入當(dāng)前觀測(cè)信息的RBPF-SLAM采樣過(guò)程

輸入：t-1時(shí)刻的i號(hào)粒子〈ξ(i)1:t-1，w(i)t-1〉，控制輸入ut，截止到t時(shí)刻的環(huán)境觀測(cè)集合z1:t。

輸出：t時(shí)刻i號(hào)粒子的重要性采樣ξ′(i)t及對(duì)應(yīng)的未歸一化權(quán)值w′(i)t。

//掃描匹配

22固定滯后Gibbs采樣

傳感器t時(shí)刻的觀測(cè)信息zt不僅給對(duì)ξt的估計(jì)提供信息，而且還通過(guò)地圖傳播給ξt-1，ξt-2，…，給它們的估計(jì)也提供信息。理想的粒子濾波器應(yīng)該更新機(jī)器人路徑中的所有位姿，而這是不現(xiàn)實(shí)的。在一般的RBPF-SLAM算法框架中，i號(hào)粒子的先前路徑ξ(i)1:t-1在t時(shí)刻濾波后并不改變，向后傳播觀測(cè)信息是通過(guò)計(jì)算其重要性權(quán)值來(lái)實(shí)現(xiàn)的，使得重要性權(quán)值的方差不斷變大，需要頻繁地采用重采樣操作。

考慮到傳感器觀測(cè)范圍有限，可以考慮只對(duì)最后一段時(shí)間的位姿作出更新，也就是對(duì)ξt-L+1:t采樣，并相應(yīng)地更新各粒子對(duì)應(yīng)地圖。有兩個(gè)思路來(lái)解決該問(wèn)題：a)對(duì)先前已經(jīng)采樣的各粒子路徑的最后L個(gè)位姿進(jìn)行修正，在本文中采用滯后Gibbs采樣實(shí)現(xiàn)；b)重新依據(jù)已有的所有信息提取粒子路徑的最后L個(gè)位姿樣本。

固定滯后Gibbs采樣的基本思想是：引入一個(gè)MCMC移動(dòng)操作，在上一節(jié)介紹的融入當(dāng)前觀測(cè)信息采樣的改進(jìn)RBPF-SLAM算法的采樣步驟完成之后，對(duì)粒子路徑最近的L時(shí)間段進(jìn)行移動(dòng)。用如下方式抽取樣本ξ′(i)t-L+1:t：

這樣，采樣后新的i號(hào)樣本為{ξ(i)1:t-L，ξ′(i)t-L+1:t}。

MCMC移動(dòng)之后，加權(quán)粒子集仍然近似地服從期望的驗(yàn)后分布，因此，粒子的重要性權(quán)值不變。該操作降低了在最后L時(shí)段上的退化程度。同時(shí)，利用了當(dāng)前的觀測(cè)信息對(duì)先前的位姿樣本抽取新的值。MCMC移動(dòng)可以反復(fù)進(jìn)行來(lái)得到更好的采樣，在本文中只采用一次，效果也非常明顯。

Gibbs采樣方法能有效地從聯(lián)合MCMC核q(ξt-L+1:t)進(jìn)行采樣，其基本方法是：輪換地從基于給定其他元素值的條件概率分布，對(duì)ξt-L+1:t中的各元素采樣。具體地，按下面的方式對(duì)i號(hào)粒子采樣：

介紹的最優(yōu)提議分布。其他式子有如下的一般形式：

ξ′(i)k~p(ξk｜ξ(i)1:k-1，k+1:t，u1:t，z1:t)(17)

接下來(lái)，考慮如何將式（17）變換成可以方便采樣的形式：

為歸一化系數(shù)。

具體的基于固定滯后Gibbs采樣的改進(jìn)RBPF-SLAM算法為算法2。

算法2基于固定滯后Gibbs采樣的改進(jìn)RBPF-SLAM算法

輸入：t-1時(shí)刻的帶權(quán)粒子集St-1，截止到t時(shí)刻的控制輸入集合u1:t，截止到t時(shí)刻的環(huán)境觀測(cè)集合z1:t，滯后長(zhǎng)度L。

輸出：t時(shí)刻的帶權(quán)粒子集St。

St置空; wtotal=0; 

3實(shí)驗(yàn)結(jié)果

在開(kāi)放系統(tǒng)^[8]的基礎(chǔ)上筆者開(kāi)發(fā)了一個(gè)RBPF-SLAM算法實(shí)驗(yàn)平臺(tái)。其運(yùn)行界面如圖1所示。為了驗(yàn)證本文提出的算法，分別對(duì)開(kāi)放數(shù)據(jù)庫(kù)Radish^[9]中提供的兩組不同數(shù)據(jù)記錄利用算法1和2進(jìn)行了處理，然后對(duì)各算法的結(jié)果進(jìn)行了對(duì)比。在實(shí)驗(yàn)過(guò)程中，算法1和2的所有參數(shù)設(shè)置相同。其中粒子數(shù)目均為20，算法2的固定滯后長(zhǎng)度為5。實(shí)驗(yàn)結(jié)果分別如圖2和3所示。

從實(shí)驗(yàn)結(jié)果來(lái)看，在相同粒子數(shù)目的條件下，進(jìn)一步改進(jìn)后的算法2比原算法1有明顯的改進(jìn)：a)環(huán)路閉合更加準(zhǔn)確；b)在運(yùn)行過(guò)程中有效粒子數(shù)目有所改善。但是，算法2也存在著一定的不足，雖然在空間復(fù)雜度方面沒(méi)有什么增加，但是時(shí)間復(fù)雜度增加了不少，這也是進(jìn)一步研究的方向之一。

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

本文對(duì)采用柵格地圖表示法的Rao-Blackwellized粒子濾波SLAM算法進(jìn)行了改進(jìn)，分析了采樣和重采樣操作對(duì)RBPF-SLAM算法估計(jì)一致性的影響，設(shè)計(jì)了一種基于滯后采樣策略——固定滯后Gibbs采樣的改進(jìn)RBPF-SLAM算法。對(duì)比實(shí)驗(yàn)表明，這種滯后采樣策略都能在一定程度上改善RBPF-SLAM算法的綜合性能：在采用相同粒子數(shù)目的情況下，重采樣次數(shù)顯著減少，地圖估計(jì)一致性也顯著提高。但是該算法存在著時(shí)間復(fù)雜度偏高的缺點(diǎn)，當(dāng)L的取值較大時(shí)不一定能滿足實(shí)時(shí)的要求，需進(jìn)一步對(duì)算法優(yōu)化。

參考文獻(xiàn)：

［1］SMITH R， SELF M， CHEESEMAN P. A stochastic map for uncertain spatial relationships[C]//Proc of the 4th International Symposium on Robotic Research. Cambridge: MIT Press， 1987:467-474. 

[2]MONTEMERLO M， THRUN S， KOLLER D. FastSLAM: a factored solution to the simultaneous localization and mapping problem[C]//Proc ofNational Conference on Artificial Intelligence. Edmonton， Canada: AAAI Press， 2002:593-598. 

[3]BAILEY T， NIETO J， NEBOT E. Consistency of the FastSLAM algorithm[C]//Proc of IEEE International Conference on Robotics and Automation. San Francisco: IEEE Press， 2006:424-429. 

[4]GRISETTI G， STACHNISS C， BURGARD W. Improved techniques for grid mapping with Rao-Blackwellized particle filters[J].IEEE Trans on Robotics， 2007，23(1):34-46. 

[5]MURPHY K P. Bayesian map learning in dynamic environments[C]//Advances in Neural Information Processing Systems. Cambridge: MIT Press， 2000: 1015-1021.

[6]DOUCET A， GODSILL S， ANDRIEU C. On sequential Monte Carlo sampling methods for Bayesian filtering[J].Statistics and Computing， 2000，10(3): 197-208.

[7]LIU J. Metropolized independent sampling with comparison to rejection sampling and importance sampling[J].Statistics and Computing， 1996，6(2):113-119.

[8]GRISETTI G， STACHNISS C， BURGARD W. GMapping[EB/OL].(2007). https://svn.openslam.org/data/svn/gmapping.

[9]HOWARD A， ROY N. The robotics data set repository[EB/OL].(2003). http://radish.sourceforge.net.