基于粒子濾波的移動機器人定位關鍵技術研究綜述

摘要：針對粒子濾波固有的問題，結合在移動機器人蒙特卡羅定位中的最新應用成果，分別從建議分布的選擇、重采樣策略的改進、有效推理的執行、自適應機制的引入、與其他方法的集成等幾個方面對其當前研究的關鍵技術進行了歸納總結，并對該研究領域需要解決的研究難點進行了分析，展望了進一步研究的方向。

關鍵詞：移動機器人； 蒙特卡羅定位； 粒子濾波

中圖分類號：TP24文獻標志碼：A

文章編號：1001-3695(2007)11-0009-06

定位問題（“where am I” problem）是實現移動機器人自主能力的基本問題[1]。依據完成任務要求的不同，可以將移動機器人定位問題分為位姿跟蹤、全局定位、綁架問題[2]；具體實現方法包括擴展卡爾曼濾波[3]、馬爾可夫定位[4]、多假設跟蹤[5]和蒙特卡羅定位[6]等方法。相對于其他定位方法，基于粒子濾波的蒙特卡羅定位因其能夠使用樣本（也稱粒子）表達任意分布，包括多模的概率密度函數（probability density function，PDF），以及對于非線性非高斯過程進行評估等優點而得以廣泛應用。

粒子濾波（PF）也稱為序列蒙特卡羅，源于Gordon等人[7]提出的一種基于序列重要性采樣（sequential importance samping，SIS）的bootstrap非線性濾波方法。Dallert等人將粒子濾波與移動機器人運動和感知的概率模型相結合，提出了移動機器人蒙特卡羅定位（MCL）的思想。基于SIS的PF算法潛在的問題是樣本退化問題，即在濾波過程中經過幾次迭代，除了一個樣本外其余樣本的重要性權重均很小[8]。為了解決退化問題，采樣重要性重采樣（sampling－importance resamping，SIR）被引入PF中，但是SIR又導致樣本多樣性喪失，也稱為樣本耗盡。由于存在這些潛在問題，使得PF在移動機器人MCL應用中還存在局限和不足。為了解決這些問題，目前基于PF的MCL方法的關鍵技術集中在建議分布的選擇、重采樣策略的改進、有效推理的執行、自適應機制的引入和與其他方法的集成。

對于基于PF的移動機器人MCL方法的研究盡管尚不足十年，但是已經成為移動機器人領域的研究熱點問題。為此，國內外學者展開了諸多研究，提出了許多研究思路和實用算法。在此基礎之上，本文對基于PF的移動機器人MCL方法所開展的研究進行了綜述。本文介紹了移動機器人MCL方法，包括移動機器人運動和感知模型、PF的基本算法及其當前依然存在的問題；結合在移動機器人MCL中的最新應用成果，對當前PF研究的關鍵技術進行了歸納總結。

1基本MCL算法

1．1機器人定位問題

移動機器人定位可看做是Bayesian評估問題，即通過給定輸入數據、觀測數據、運動與感知模型，使用預測/更新步驟估計當前時刻機器人隱式位姿狀態信度的最優化問題[6]。典型的評估狀態一般為s=(x，y，θ)。其中：(x，y)表示Cartesian坐標系中機器人的位置；θ表示機器人的航向角。輸入數據u通常來自內部傳感器里程計；觀測數據z來自外部傳感器如激光雷達、攝像頭等；運動模型p(st｜st-1，ut-1)表示t時刻系統起始狀態為st-1，輸入ut-1到達狀態st的概率；感知模型P(zt｜st)則表示t時刻在狀態st所能觀測到zt的似然概率。假定環境服從Markov的前提，利用上述信息可以求得機器人當前t時刻狀態后驗概率p(st｜z1:t，u0:t-1)。這分為以下兩步遞歸執行：

2．5集成其他方法

針對PF固有的缺陷及不足，可以通過提出PF算法的新變種來改進，也可以考慮集成其他方法如進化機制、其他數字近似方法來改進。

1）結合進化機制針對引入重采樣技術所造成的樣本多樣性的喪失，目前粒子濾波研究的一個方向是與進化機制相結合來改善樣本集的多樣性。Higuchi[49]通過分析遞歸MCF的重采樣和GA的子代繁殖在結構上的相似性，使用遺傳算子變異和交叉操作代替MCF中的預測步，以克服MCF的采樣誤差。Bienvenue等人[50，51]考慮GA研究中防止早熟收斂提高群體多樣性的Niching優化技術，將其引入到MCF中以解決樣本貧化問題。基于GA的Niching技術已經被相關的研究人員考慮用于移動機器人的MCL中。Kwok等人[52，53]對于需要的樣本數、樣本貧化原因及時間進行分析，基于GA算法設計了進化粒子濾波（evolutionary particle filter，EPF），并將該研究應用于非結構化環境中基于視覺傳感器的移動機器人的SLAM問題中。Hong等人[54]通過結合協同進化模型以增強樣本的多樣化，來解決高對稱環境中移動機器人的MCL早熟收斂問題。莫以為等人[55]則將進化規劃（evolutionary programming，EP）引入到粒子濾波中構造了進化粒子濾波算法，可使樣本集保持一定的多樣性。該方法也可以考慮用于移動機器人定位。

2)混合其他近似方法不同于前面采用高斯模型來近似建議分布，可以采用其他數字近似方法來近似后驗密度。典型的一個例子是高斯和粒子濾波[56，57]（Gaussian sum PF）。其基于這樣的思想：使用足夠多的高斯混合密度可以近似一定精度的任意非高斯密度的觀測，它是使用加權的高斯密度和來近似后驗密度，即有p=∑Npi=1wiN(i，i）。另外還可以采用確定性的采樣近似方法unscented變換（unscented transformation，UT）[46，58]。UT利用sigma點和偏態參數來覆蓋和傳播數據信息。已經開發的粒子濾波有UKF、UPF。盡管利用UT的計算復雜度稍高，但其對非線性評估具有較好的效果。

3難點問題及解決途徑

1）收斂檢測與失敗恢復問題MCL算法對于狀態評估要求具有一定的收斂速度。目前的研究成果對于算法收斂提供了一定的理論依據[59]：建議分布幾乎肯定收斂于后驗概率；狀態估計均方誤差的收斂性。但是要求的條件依然很強，只有樣本數目Np隨時間增加而增加或濾波器有能力忘記指數級增長的誤差，算法的一致收斂才能確保。目前，關于收斂性研究大多集中于非線性濾波的誤差極限研究上。此外，MCL可能遭受錯誤定位而發展成綁架問題。盡管現有的MCL算法已經直接應用于位姿跟蹤、全局定位，但是對于運動模型和觀測模型都是似是而非的評估的綁架問題還不能較好解決。目前一些研究采用傳感器重置技術，可以不受Markov假設的限制使得樣本分布產生改變。關于這方面的研究還有待進一步深入下去。

2）算法魯棒性問題由于在每個遞歸過程中樣本的不精確性，累積誤差可能呈指數級增加。對于SIS或SIR濾波器而言，偏差和方差都將隨著時間增加而增加。這使得算法的魯棒性話題極為重要。對于MCL算法魯棒性有兩個基本問題：a)出現異常時，重要性權重分布會極為不均勻，通常需要大量的樣本數Np來確保經驗密度近似，因此測量密度p(zt｜st)假定對st非常敏感；b)來自樣本的經驗密度對于長尾分布，無論是建議分布還是后驗分布都很難近似。許多研究結果顯示，即使混合分布也不能較好地處理目標分布的長尾行為。異常可能造成濾波器發散或產生較差的濾波性能。魯棒性改進包括：對有效性樣本大小的計算；在IS、SIS和SIR框架下的重要性權重的評估；使用核平滑技術對建議分布進行平滑；魯棒的建議或似然模型的建立。

3）在MCL算法中到底需要多少獨立的樣本如何選擇算法的有效樣本數目依舊缺乏嚴格的理論證明。如果通過增加樣本數目來改善算法的精度，不可避免地會引進方差（由于偏差/方差的兩難問題，依據不確定性原理不可能使得偏差和方差同時保持很小），更不用說計算復雜度和采樣的效率。許多技術已經用于改進樣本退化問題，但還沒有完美的解決方案。同時，選擇/添加輔助樣本的自適應過程仍然是開放話題。這在遭遇尺度問題時非常重要，即空間緯度Nx成百上千地遞增如何維持合適的計算時間。此外，有效樣本數目依靠所選擇的建議分布。好的建議分布則誤差隨著樣本數目Np的增加呈線性變化減小到零；差的建議分布則無論樣本數目Np多大，誤差都會隨著空間維度Nx呈指數級增加。

4）Bayesian規則不是統計推理的惟一歸納規則它也存在著其他一些規則，如最大最小化原則、結構風險最小化原則、最小描述長度等。Bayesian方法只有在定量先驗是正確時，才有可能推出最優的解，在缺乏先驗知識時，Bayesian方法可能得到一個錯誤的解。在Bayesian濾波框架中，需要的定量先驗信息是建議分布的選擇、初始化狀態密度（PCS）和噪聲統計學。然而，這些定量的先驗信息在實踐中可能沒有辦法確定。這可以與相關性評估和統計學習一些嚴格的理論結果關聯，如度量熵、信息復雜度等都可以為MCL算法提供強有力的數學基礎。

參考文獻：

[1]COX I J. Blanche:an experiment in guidance and navigation of an autonomous robot vehicle[J]. IEEE Transactions on Robotics and Automation， 1991，7(2):193-204.

[2]FOX D， BURGARD W， THRUN S. Markov localization for mobile robot in dynamic environments[J]. Journal of Artificial Intelligence Research， 1999，11(1):391-427.

[3]LEONARD J J， DURRANT－WHYTE H F. Mobile robot localization by tracking geometric beacons[J]. IEEE Transactions on Robotics and Automation，1991，7(3):376-382.

[4]FOX D. Markov localization: a probabilistic framework for mobile robot localization and navigation[D]. Bonn，Germany: University of Bonn， 1998.

[5]JENSFELT P， CHRISTENSEN H I. Active global localization for a mobile robot using multiple hypothesis tracking [J]. IEEE Transactions on Robotics and Automation， 2001，17(2):748-760.

[6]DELLAERT F， BURGARD W， FOX D， et al. Monte carlo localization for mobile robots[C]//Proc of the IEEE International Conference on Robotics and Automation.Detroit:[s.n.]， 1999:1322－1328.

[7]GORDON N J， SALMOND D J， SMITH A F M. Novel approach to nonlinear/non－Gaussian Bayesian state estimation[J]. IEEE Proceedings on Radar and Signal Processing， 1993，140(2): 107－113.

[8]ARULAMPALAM M S， MASKELL S， GORDON N， et al. A tutorial on particle filters for online nonlinear/non－Gaussian Bayesian tracking[J]. IEEE Transactions on Signal Processing， 2002，50(20):174－188.

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

[10]LIU J S， CHEN R. Sequential Monte Carlo methods for dynamical systems[J]. Journal of American Statistical Association， 1998，93(5):1032－1044.

[11]DOUCET A， De FREITAS N， MURPHY K， et al. Rao－Blackwellised particle filtering for dynamic Bayesian networks[C]//Proc of the Conference on Uncertainty in Artificial Intelligence.San Francisco:[s.n.]，2000:176－183.

[12]MONTEMERLO M， THRUN S， KOLLER D， et al. FastSLAM: a factored solution to simultaneous localization and mapping problem[C]//Proc of the National Conference on Artificial Intelligence.Edmonton:[s.n.]，2002:593－598.

[13]DEUTSCHER J， BLAKE A， REID I. Articulated body motion capture by annealed particle filtering[C]//Proc of the IEEE Conference on Computer Vision and Pattern Recognition.Columbia:[s.n.]， 2000:2126-2133.

[14]PUPILLI M， CALWAY A. Particle filtering for robust single camera localization[C]//Proc of the 1st International Workshop on Mobile Vision.Graz:[s.n.]，2006:1－14.

[15]THRUN S， FOX D， BURGARD W. Monte Carlo localization with mixture proposal distribution[C]//Proc of the National Conference on Artificial Intelligence.Austin:[s.n.]，2000:859-865.

[16]THRUN S， FOX D， BURGARD W， et al. Robust Monte Carlo localization for mobile robots[J]. Artificial Intelligence， 2001， 128(1－2): 99－141.

[17]CHEN R， LIU J S. Mixture Kalman filters[J]. Journal of the Royal Statistical Society， 2000，62(3):493-508.

[18]PITT M， SHEPHARD N. Filtering via simulation: auxiliary particle filters[J]. Journal of the American Statistical Association， 1999，94(446):590-599.

[19]VLASSIS N， TERWIJN B， KROSE B. Auxiliary particle filter robot localization from high－dimensional sensor observations[C]//Proc of the IEEE International Conference on Robotics and Automation.Washin￣gton，DC:[s.n.]，2002:7－12.

[20]ANDRIEU C， DAVY M， DOUCET A. Improved auxiliary particle filtering: applications to time－varying spectral analysis[C]//Proc of the IEEE Signal Processing Workshop on Statistical Signal Processing.Singapore:[s.n.]，2001:309-312.

[21]RUBIN D B. Comment on the calculation of posterior distributions by data augmentation[J]. Journal of the American Statistical Associa￣tion， 1987，82(398): 543-546.

[22]LIU J S， CHEN R. Blind deconvolution via sequential imputation[J]. Journal of the American Statistical Association， 1995，90(2):567-576.

[23]SMITH A F M， GELFAND A E. Bayesian statistics without tears: a sampling－resampling respective[J]. American Statistician， 1992，46(2):84-88.

[24]KWOK C， FOX D， MEILA M. Adaptive real－time particle filters for robot localization[C]//Proc of the IEEE International Conference on Robotics and Automation.Taipei:[s.n]，2003:2836-2841.

[25]BEEVERS K R. Sampling strategies for particle filtering SLAM[R]. Troy: Department of Computer Science， Rensselaer Polytechnic Institute， 2006.

[26]KITAGAWA G. Monte Carlo filter and smoother for non－Gaussian non－linear state space models[J]. Journal of Computational and Graphical Statistics， 1996，5(1):1-25.

[27]WOLF J. Omnidirectional vision system for mobile robot localization in the RoboCup environment[D].Graz，Austria: Graz University of Technology，2003.

[28]MUSSO C， OUDJANE N， LeGLAND F. Improving regularised particle filters[M]//De DOUCET A. Sequential Monte Carlo methods in practice.New York:Springer－Verlag， 2001.

[29]PRESS W H， FARRAR G R. Recursive stratified sampling for multidimensional Monte Carlo integration[J]. Computers in Physics， 1990， 4(2): 190－195.

[30]LIU J S， CHEN R， WONG W H. Rejection control and sequential importance sampling[J]. Journal of the American Statistical Association， 1998， 93(443): 1022－1031.

[31]LENSER S. On－line robot adaptation to environment change[D].Pittsburgh: Carnegie Mellon University， 2005.

[32]LIU J S， CHEN R， LOGVINENKO T. A theoretical framework for sequential importance sampling with resampling[C]//Sequential Monte Carlo Methods in Practice.New York:Springer－Verlag， 2001:225-246.

[33]KAESS M， DELLAERT F. A Markov chain Monte Carlo approach to closing the loop in SLAM[C]//Proc of the IEEE International Confe￣rence on Robotics and Automation.Barcelona:[s.n.]，2005: 634-638.

[34]CASELLA G. Statistical inference and Monte Carlo algorithms[J]. Test， 1997， 5(2): 249-344.

[35]GELMAN A， RUBIN D B. Inference from iterative algorithms using multiple sequences(with discussions)[J]. Statistical Science， 1992， 7(4): 457-511.

[36]CASELLA G， ROBERT C P. Rao－Blackwellization of sampling schemes[J]. Biometrika， 1996， 83(1): 81-94.

[37]DEMPSTER A P， LAIRD N M， RUBIN D B. Maximum likelihood from incomplete data via the EM algorithm[J]. Journal of the Royal Statistical Society:Series B， 1977， 39(1): 1-38.

[38]DYK D A van， MENG X L. The art of data augmentation (with discussion)[J]. Journal of Computational and Graphical Statistics， 2001，10(1): 1－111.

[39]ANDRIEU C， De FREITAS N， DOUCET A， et al. An introduction to MCMC for machine learning[J]. Machine Learning， 2003， 50(1): 5-43.

[40]KOLLER D， FRATKINA R. Using learning for approximation in stochastic processes[C]//Proc of the International Conference on Machine Learning.Madison: [s.n.]，1998:287-295.

[41]FOX D， BURGARD D， DELLAERT F， et al. Monte Carlo localization: efficient position estimation for mobile robots[C]//Proc of the National Conference on Artificial Intelligence.Menlo Park:[s.n.]，1999: 343-349.

[42]FOX D. KLD－sampling: adaptive particle filters and mobile robot localization[J]. Advances in Neural Information Processing Systems， 2001， 14(1): 26-32.

[43]FOX D. Adapting the sample size in particle filters through KLD－sampling[J]. The International Journal of Robotic Research， 2003， 22(12): 985－1004.

[44]LIU J S. Metropolized independent sampling with comparisons to rejection sampling and importance sampling[J]. Statistics and Computing， 1996， 6(1): 113－119.

[45]GRISETTI G， STACHNISS C， BURGARD W. Improving grid－based SLAM with Rao－Blackwellized particle filters by adaptive proposals and selective resampling[C]//Proc of the IEEE International Confe￣rence on Robotics and Automation.Barcelona:[s.n.]，2005: 2443-2448.

[46]LI Mao－hi， HONG Bing－rong， CAI Ze－su， et al. Novel Rao－Blackwellized particle filter for mobile robot SLAM using monocular vision[J]. International Journal of Intelligent Technology， 2006， 1(1): 63-69.

[47]BEESON P， MURARKA A， KUIPERS B. Adapting proposal distributions for accurate， efficient mobile robot localization[C]//Proc of the IEEE International Conference on Robotics and Automation.Orlando:[s.n.]，2006:49-55.

[48]MILSTEIN A， WANG T. Localization with dynamic motion models－determining motion model parameters dynamically in Monte Carlo localization[C]//Proc of the 3rd International Conference on Informa￣tics in Control，Automation and Robotics.Setubal:[s.n.]，2006:1-8.

[49]HIGUCHI T. Monte Carlo filter using the genetic algorithm operators[J]. Journal of Statistical Computation and Simulation， 1997， 59(1): 1-23.

[50]BIENVENUE A， JOANNIDES M， BERARD J， et al. Niching in Monte Carlo filtering algorithms[C]//Proc of the 5th International Conference on Artificial Evolution.Le Creusot:[s.n.]，2001: 19-30.

[51]KOOTSTRA G， De BOER B. Improving Monte Carlo localization with niching methods from genetic algorithms[J]. Autonomous Robots， 2006.

[52]KWOK N M， FANG Gu， ZHOU Wei－zhen. Evolutionary particle filter: resampling from the genetic algorithm perspective[C]//Proc of the IEEE/RSJ International Conference on Intelligent Robots and Systems.Edmonton:[s.n.]，2005: 1053－1058.

[53]KWOK N M， RAD A B. A modified particle filter for simultaneous localization and mapping[J]. Journal of Intelligent and Robotic Systems， 2006， 46(4): 365-382.

[54]LUO Rong－hua， HONG Bing－rong. Coevolution based adaptive Monte Carlo localization[J]. International Journal of Advanced Robotic Systems， 2004， 1(3): 183－190.

[55]莫以為， 蕭得云. 進化粒子濾波算法及其應用[J]. 控制理論與應用， 2005， 22(2): 269-272.

[56]KOTECHA J， DJURIC P M. Gaussian sum particle filtering[J]. IEEE Transactions on Signal Processing， 2003， 51(10): 2602-2612.

[57]厲茂海， 洪炳熔， 蔡則蘇. 一種新的移動機器人全局定位算法[J]. 電子學報， 2006， 34(3): 553-558.

[58]MERWE R van der， DOUCET A， De FREITAS N， et al. The unscented particle filter， CUED/F－INFENG/TR 380[R]. Cambridge: Cambridge University， 2000.

[59]CRISAN D， DOUCET A. A survey of convergence results on particle filtering methods for practitioners[J]. IEEE Transactions on Signal Processing， 2002，50(3): 736-746.

“本文中所涉及到的圖表、注解、公式等內容請以PDF格式閱讀原文”