基于固定滯后Ｇｉｂｂｓ采樣粒子濾波的移動機器人ＳＬＡＭ
2008-12-31 00:00:00張恒劉艷麗樊曉平瞿志華
計算機應用研究 2008年11期
(1.華東交通大學 信息工程學院， 南昌 330013； 2.中南大學 信息科學與工程學院， 長沙 410075)
摘要：針對采用Rao-Blackwellized粒子濾波器的移動機器人同步定位與地圖構建算法（RBPF-SLAM）所面臨的粒子退化問題，提出了一種改進的采樣方法。該方法在原有采樣方法的基礎上，加入一個用Gibbs采樣實現的向后MCMC（Markov chain Monte Carlo）移動步驟，利用當前新獲取的信息對機器人路徑樣本的最后一段進行調整，從而降低了樣本退化的可能性。對比仿真實驗驗證了該方法的有效性。
關鍵詞：同步定位與地圖構建； Rao-Blackwellized粒子濾波器； MCMC移動； Gibbs采樣
中圖分類號：TP24文獻標志碼：A
文章編號：1001-3695(2008)11-3292-04
Mobile robot simultaneous localization and mapping
based on particle filtering with fixed-lag Gibbs sampling
ZHANG Heng1，2， LIU Yan-li1， FAN Xiao-ping2， QU Zhi-hua2
(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引言
在眾多移動機器人的工作任務中，機器人對工作空間的信息并不是預先就知道的，或只有部分不確定的信息。其中一些任務可能需要機器人對環境進行有效的探測并構建出環境地圖，或者邊構建地圖邊執行任務，這就帶來了機器人環境探測和地圖構建的問題。地圖構建就是建立機器人周圍環境的空間模型，并指導機器人導航或定位。為了得到地圖，機器人必須擁有外部傳感器來感知外界環境，但這些傳感器都存在測量誤差，且一般視野有限，使得機器人在構建地圖的同時必須移動。對機器人的運動控制為地圖構建提供了豐富的信息，因為它們包含了機器人在每次進行觀測時所處的位置信息。機器人運動測量同樣有誤差，僅僅依靠運動控制信息(包括里程計)不足以決定機器人的位姿，需要融合外部傳感器的信息進行定位。于是，地圖構建過程依賴于精確的位姿信息，而機器人定位過程反過來又依賴于精確的地圖信息，兩者既矛盾又相關，必須同時加以考慮。對該問題的系統研究最早出現在Smith等人[1]撰寫的文章中，他們將該問題稱為移動機器人同步定位與地圖構建(simultaneous localization and mapping，SLAM)。
自從Montemerlo等人[2]提出了基于Rao-Blackwellized粒子濾波器的移動機器人同步定位與地圖構建算法(RBPF-SLAM)以來，該算法得到了廣泛的關注。最近的研究[3]表明RBPF-SLAM算法容易產生不一致的地圖。其原因在于當粒子權值分布的方差較大時出現粒子退化現象，在這種情況下重采樣步驟對一些有較高權值的粒子多次復制而拋棄權值較小的粒子。隨著重采樣操作的反復運用，對機器人運動軌跡的過去部分的采樣趨于退化，導致其不足以表達估計的不確定性。最明顯的例子是機器人做環形運動后，往往不能得到一致地圖。
本文提出一種改進的采用柵格地圖的RBPF-SLAM算法。該算法在現有的改進型柵格RBPF-SLAM算法[4]的基礎上，加入一個用Gibbs采樣實現的向后MCMC(Markov chain Monte Carlo)移動步驟，利用當前新獲取的信息對機器人路徑樣本的最后一段進行調整，從而降低了樣本退化的可能性。
1RBPF-SLAM算法框架
采用Rao-Blackwellized粒子濾波器解SLAM問題的關鍵思想是：給定觀測信息z1:t和控制輸入信息u1:t的條件下，估計機器人運動軌跡ξ1:t和環境地圖m的聯合后驗分布p(ξ1:t，m｜u1:t，z1:t)，該思想最早由Murphy[5]提出。根據貝葉斯公式，有
p(ξ1:t，m｜u1:t，z1:t)=p(m｜ξ1:t，z1:t)p(ξ1:t｜z1:t，u1:t)(1)
這樣，SLAM問題就可以分為兩步解決：首先僅估計機器人的運動軌跡，然后基于估計的軌跡計算環境模型。式(1)中的p(ξ1:t｜z1:t，u1:t)部分類似于機器人定位問題，當觀測和控制輸入信息到達時，采用粒子濾波器遞增地進行估計;p(m｜ξ1:t，z1:t)部分類似于已知定位條件下的地圖構建問題，可以非常方便地解決。于是可以采用Rao-Blackwellized粒子濾波器來實現SLAM算法。在此濾波器中，每個粒子代表一條機器人的可能路徑，每個粒子與一個獨立的完整地圖相關聯，且基于粒子中的機器人軌跡的估計對相應的地圖進行更新。
最常用的粒子濾波算法是采樣重要性重采樣(sampling importance resampling，SIR)濾波器，Rao-Blackwellized SIR濾波器可以遞推地方式處理觀測信息和控制輸入信息來構建環境地圖。這個過程可以概括為以下四步：
a)采樣。基于前一時刻的樣本集{ξ(i)t-1}，從提議分布(proposal distribution)π抽取樣本{ξ(i)t}用于描述當前時刻機器人的位姿。通常，用機器人的概率運動模型作為提議分布。
b)重要性權值計算。給每個新樣本指定一權值，其計算方法是根據如下的重要性采樣原理計算：
w(i)t=p(ξ(i)1:t｜z1:t，u1:t)/π(ξ(i)1:t｜z1:t，u1:t)(2)
重要性權值用來補償目標分布(target distribution)與提議分布之間的差別。
c)重采樣。依據重要性權值{w(i)t}對樣本集{ξ(i)t}進行重采樣。由于只能用有限的樣本模擬連續分布，重采樣步驟是必要的，通過該步驟除去權值較小的樣本，使計算集中在權值較大的樣本上，從而在一定程度上避免樣本退化現象的發生。另外，重采樣使粒子濾波算法可以應用于提議分布與實際分布不同的情況，重采樣后，各樣本具有相同的重要性權值。
d)地圖估計。對于描述機器人軌跡的每個樣本ξ(i)1:t，其相應的地圖估計m(i)1:t可以利用p(m(i)1:t｜ξ(i)1:t，z1:t)計算。
在這樣實現框架中，當一次新的觀測信息獲得時，需要批量地計算機器人軌跡的重要性權值。由于機器人軌跡的長度隨時間不斷增長，這種計算方式使得該算法的效率越來越低。為了用遞推的方式計算重要性權值，選取提議分布滿足以下分解公式：
π(ξ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)，重要性權值計算方式如下：
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)為歸一化系數，對于每個粒子均相等。
至此，形成了一個完整的RBPF-SLAM算法框架，但實現時還要解決一些具體的問題，如何設計地圖的表達方式及存取算法，使之方便于RBPF計算過程；如何選擇合理的提議分布并能方便地抽取樣本；重采樣如何進行及什么時候進行重采樣。本文將從后面兩個方面深入研究RBPF-SLAM算法的改進方法。
2RBPF-SLAM算法改進
21融入當前觀測信息采樣
為減輕重要性權值的退化，一個關鍵的步驟是重要性分布函數的選擇。Doucet等人[6]證明，在粒子重要性權值方差最小意義下的最優提議分布函數為
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)
對應的重要性權值計算公式為
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)
在一些粒子濾波器的應用中，通常用運動模型p(ξt｜ξt-1，ut)作為提議分布函數。然而，若觀測模型相比運動模型更為精確(如本文中采用的激光掃描模型)，則似然函數p(zt｜m(i)1:t-1，ξt)對乘積p(zt｜m(i)1:t-1，ξt)p(ξt｜ξ(i)t-1，ut)的結果起主要作用，導致提取的樣本集中只有極少數樣本的重要性權值很大，而其他絕大多數樣本的重要性權值接近于0，這樣就容易出現粒子貧化問題。
文獻[4]設計了具體的融入當前觀測信息采樣RBPF-SLAM算法的計算方法，即將最優提議分布函數近似化為一個高斯分布，從而得到一種閉合的計算形式，可以方便地進行采樣。
p(ξt｜m(i)1:t-1，ξ(i)t-1，zt，ut)≈N(μ(i)t，(i)t)(7)
對于每一個粒子i，參數μ(i)t和(i)t可以通過測試在區間R(i)提取的K個樣本點{ξj}Kj=1的值來計算：
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)為歸一化系數，ξj∈{ξt｜p(ξt｜ξt-1，ut)＞λ}，對應的重要性權值計算公式為
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)
具體的融入當前觀測信息采樣過程為算法1。
算法1融入當前觀測信息的RBPF-SLAM采樣過程
輸入：t-1時刻的i號粒子〈ξ(i)1:t-1，w(i)t-1〉，控制輸入ut，截止到t時刻的環境觀測集合z1:t。
輸出：t時刻i號粒子的重要性采樣ξ′(i)t及對應的未歸一化權值w′(i)t。
//掃描匹配
22固定滯后Gibbs采樣
傳感器t時刻的觀測信息zt不僅給對ξt的估計提供信息，而且還通過地圖傳播給ξt-1，ξt-2，…，給它們的估計也提供信息。理想的粒子濾波器應該更新機器人路徑中的所有位姿，而這是不現實的。在一般的RBPF-SLAM算法框架中，i號粒子的先前路徑ξ(i)1:t-1在t時刻濾波后并不改變，向后傳播觀測信息是通過計算其重要性權值來實現的，使得重要性權值的方差不斷變大，需要頻繁地采用重采樣操作。
考慮到傳感器觀測范圍有限，可以考慮只對最后一段時間的位姿作出更新，也就是對ξt-L+1:t采樣，并相應地更新各粒子對應地圖。有兩個思路來解決該問題：a)對先前已經采樣的各粒子路徑的最后L個位姿進行修正，在本文中采用滯后Gibbs采樣實現；b)重新依據已有的所有信息提取粒子路徑的最后L個位姿樣本。
固定滯后Gibbs采樣的基本思想是：引入一個MCMC移動操作，在上一節介紹的融入當前觀測信息采樣的改進RBPF-SLAM算法的采樣步驟完成之后，對粒子路徑最近的L時間段進行移動。用如下方式抽取樣本ξ′(i)t-L+1:t：
這樣，采樣后新的i號樣本為{ξ(i)1:t-L，ξ′(i)t-L+1:t}。
MCMC移動之后，加權粒子集仍然近似地服從期望的驗后分布，因此，粒子的重要性權值不變。該操作降低了在最后L時段上的退化程度。同時，利用了當前的觀測信息對先前的位姿樣本抽取新的值。MCMC移動可以反復進行來得到更好的采樣，在本文中只采用一次，效果也非常明顯。
Gibbs采樣方法能有效地從聯合MCMC核q(ξt-L+1:t)進行采樣，其基本方法是：輪換地從基于給定其他元素值的條件概率分布，對ξt-L+1:t中的各元素采樣。具體地，按下面的方式對i號粒子采樣：
介紹的最優提議分布。其他式子有如下的一般形式：
ξ′(i)k~p(ξk｜ξ(i)1:k-1，k+1:t，u1:t，z1:t)(17)
接下來，考慮如何將式（17）變換成可以方便采樣的形式：
為歸一化系數。
具體的基于固定滯后Gibbs采樣的改進RBPF-SLAM算法為算法2。
算法2基于固定滯后Gibbs采樣的改進RBPF-SLAM算法
輸入：t-1時刻的帶權粒子集St-1，截止到t時刻的控制輸入集合u1:t，截止到t時刻的環境觀測集合z1:t，滯后長度L。
輸出：t時刻的帶權粒子集St。
St置空; wtotal=0;
3實驗結果
在開放系統[8]的基礎上筆者開發了一個RBPF-SLAM算法實驗平臺。其運行界面如圖1所示。為了驗證本文提出的算法，分別對開放數據庫Radish[9]中提供的兩組不同數據記錄利用算法1和2進行了處理，然后對各算法的結果進行了對比。在實驗過程中，算法1和2的所有參數設置相同。其中粒子數目均為20，算法2的固定滯后長度為5。實驗結果分別如圖2和3所示。
從實驗結果來看，在相同粒子數目的條件下，進一步改進后的算法2比原算法1有明顯的改進：a)環路閉合更加準確；b)在運行過程中有效粒子數目有所改善。但是，算法2也存在著一定的不足，雖然在空間復雜度方面沒有什么增加，但是時間復雜度增加了不少，這也是進一步研究的方向之一。
4結束語
本文對采用柵格地圖表示法的Rao-Blackwellized粒子濾波SLAM算法進行了改進，分析了采樣和重采樣操作對RBPF-SLAM算法估計一致性的影響，設計了一種基于滯后采樣策略——固定滯后Gibbs采樣的改進RBPF-SLAM算法。對比實驗表明，這種滯后采樣策略都能在一定程度上改善RBPF-SLAM算法的綜合性能：在采用相同粒子數目的情況下，重采樣次數顯著減少，地圖估計一致性也顯著提高。但是該算法存在著時間復雜度偏高的缺點，當L的取值較大時不一定能滿足實時的要求，需進一步對算法優化。
參考文獻：
［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.
