基于自適應多提議分布粒子濾波的蒙特卡洛定位算法

羅元　龐冬雪　張毅　蘇琴

摘要：針對基于Cubature粒子濾波的蒙特卡羅定位（CMCL）算法存在的計算量大、實時處理能力較差的問題，提出一種基于自適應多提議分布粒子濾波的蒙特卡羅定位（AMPD-MCL）算法。該算法利用Cubature卡爾曼濾波和擴展卡爾曼濾波改進提議分布，融入當前觀測信息，減弱粒子退化現象；重采樣部分采用Kullback-Leibler距離（KLD）采樣，根據粒子在狀態空間的分布狀況，在線調整下一次濾波迭代所需粒子數，從而減小計算量。仿真實驗驗證了自適應多提議分布粒子濾波（AMPD-PF）的有效性；同時在機器人操作系統（ROS）上進行實驗，結果表明改進算法的平均定位精度達到19.891cm，定位所需粒子數穩定在60，定位時間為45.543s，較CMCL算法在定位精度上提高了71.03%，時間縮短了63.10%。實驗結果表明，AMPD-MCL算法減小了定位誤差，能實時在線調整粒子數，有效減少了算法計算量，提高了實時處理能力。

關鍵詞：蒙特卡洛定位；多提議分布；Cubature卡爾曼濾波；擴展卡爾曼濾波；Kullback-Leibler距離采樣；機器人操作系統

中圖分類號：TP242.6

文獻標志碼：A

0引言

移動機器人定位[1]利用先驗環境地圖信息、前一時刻位姿估計以及傳感器的觀測信息，經過一系列的處理和變換，產生對當前位姿的估計，從而確定其在工作環境中所處位置。基于粒子濾波[2]的蒙特卡羅定位（Monte Carlo Localization， MCL）算法是以先驗分布代替后驗分布進行采樣，并結合觀測似然函數來評估每個粒子的重要性權重，忽略了當前移動機器人環境的觀測信息對其狀態估計的修正作用，使預測粒子集分布在觀測似然函數的尾部，因此導致粒子集退化問題。為解決這一問題，學者們作了大量研究工作，如：Khan等[3]將馬爾可夫蒙特卡羅采樣引入粒子濾波，解決了粒子濾波在高維空間中采樣效率低的問題，并在一定程度上避免了粒子集的退化；Pfaff等[4]通過平滑觀測似然函數使粒子濾波中的有效粒子增加；van de Merwe等[5]利用Unscented卡爾曼濾波（Unscented Kalman Filter， UKF）設計粒子濾波器的提議分布，提出了Unscented粒子濾波（Unscented Particle Filter， UPF） 算法，使粒子更加集中于高觀測似然區域，能有效解決粒子退化問題；Wang 等[6]提出了一種多提議分布粒子濾波算法，使用混合的重要性采樣密度UKF和 擴展卡爾曼濾波（Extended Kalman Filter， EKF）作為提議分布，一定程度上減小了Unscented粒子濾波帶來的計算量，為多提議分布粒子濾波算法研究提供理論基礎，并對粒子濾波算法[7]深入研究分析，得出相關結論與展望；宋宇等[8]對Unscented粒子濾波進行迭代改進，并應用于移動機器人蒙特卡羅定位，提高了定位精度；Alhashimi等[9]對MCL算法中的觀測模型進行改進，通過設置閾值來選擇所需粒子，有效減小了計算量；周艷聰等[10]提出一種基于粒子剔除策略和依據粒子方位賦予粒子權值策略的室內機器人定位方法，提高了定位精度和執行效率；Li 等[11]將Cubature卡爾曼濾波（Cubature Kalman Filter， CKF）引入粒子濾波器，提出了基于Cubature粒子濾波的蒙特卡洛定位（Cubature Monte Carlo Localization， CMCL）算法，并證明了其性能優于一般MCL和Unscented MCL算法，但該算法每次迭代都要利用CKF計算重要性密度函數，計算量大，實時處理能力不強。

因此，本文提出一種基于自適應多提議分布粒子濾波 （Particle Filter with Adaptive Multi-Proposal Distribution， AMPD-PF）的蒙特卡羅定位算法，結合Cubature卡爾曼濾波和擴展卡爾曼濾波對提議分布進行改進；同時，在重采樣過程中，利用Kullback-Leibler距離（Kullback-Leibler Distance， KLD）采樣原理，根據預測粒子在狀態空間的分布狀況來在線調整重采樣所需粒子數；最后，通過仿真和實驗驗證在保證最優狀態估計和穩定性不受影響時，改進算法避免了粒子退化現象，減少了CMCL算法中的計算量，提高了實時處理能力和定位精度。

從MCL算法中提議分布的設計可看出，其缺少當前系統的觀測信息zt，易導致采樣粒子集分布于觀測似然的尾部，或觀測似然函數分布過于尖銳，使粒子集權重過低甚至為零，最終導致移動機器人定位失敗。

2改進的CMCL算法

將當前環境觀測信息zt融入到提議分布中，是解決粒子集退化和提高濾波性能的有效途徑，利用Cubature卡爾曼濾波[13]對提議分布進行設計，雖然減弱了粒子退化現象，卻因每次迭代過程中均利用CKF進行狀態更新，帶來了繁重的計算量，同時減弱了定位過程中實時處理能力。因此，本文提出一種基于自適應多提議分布粒子濾波（AMPD-PF）的MCL（AMPD-MCL）算法，以提高定位精度，減少算法計算量，增加實時處理能力。

2.1多提議分布粒子濾波

多提議分布粒子濾波[6，14]利用Cubature卡爾曼濾波和擴展卡爾曼濾波設計提議分布，將當前環境觀測信息zt融入到提議分布中，以相對百分比c分別從中采集粒子，從而使采樣粒子集中地分布于高觀測似然區域，其提議分布表達為高斯分布：

如果機器人同時觀測到多個環境特征信息，則對粒子集中的每一個粒子均執行相應的Cubature卡爾曼濾波狀態更新和擴展卡爾曼濾波狀態更新，之后再進行重要性采樣，根據式（3）計算粒子的重要性權重。

2.2基于KLD的重采樣

多提議分布較好利用了新的觀測信息，在概率匹配上具有很高的精度，粒子的選取也更加準確，一定程度上減少了Cubature卡爾曼濾波更新的繁重計算量，但仍不能滿足移動機器人定位實時處理的需求。因此，利用KLD采樣[15]對其重采樣部分進行改進，根據預測粒子在狀態空間的分布狀況在線調整重采樣所需粒子數。

KLD采樣核心思想是：在粒子濾波的每次迭代中，以概率1-δ使真實后驗概率和基于樣本的估計概率密度之間的誤差小于ε，由此來確定重采樣樣本數目，該誤差是通過計算Kullback-Lerbler（K-L）距離來確定。K-L距離用來表示兩個概率分布p和q之間的逼近誤差，即：

圖1為Cubature 粒子濾波（CPF）和自適應多提議分布粒子濾波（AMPD-PF）的狀態估計誤差，可看出改進的AMPD-PF與CPF有相近的收斂速度，在相同狀態及誤差條件下，AMPD-PF估計誤差均方值較CPF估計誤差均方值小，精度更高；圖2為兩種濾波迭代粒子數的變化情況，可看出CPF利用固定粒子數進行重要性采樣，而AMPD-PF在重采樣中根據自由空間狀態在線調整下一時刻采樣粒子數，采樣粒子數明顯少于CPF；表1為兩種濾波器的性能比較，可知AMPD-PF算法迭代所用的時間小于CPF算法，有效粒子百分比也相對較高，減弱了粒子退化。由此說明改進算法具有估計精度高、減弱粒子退化的特點，有效解決了CPF計算量大、實時處理能力差的問題。

3.2ROS上基于AMPD-MCL算法的移動機器人定位

分別采用CMCL算法和AMPD-MCL算法進行實驗驗證，平臺為裝載URG-hokuyo激光傳感器的Pioneer3-DX機器人及配有 Intel 雙核、CPU 2.19GHz、內存 1.96GB 的筆記本電腦，筆記本電腦上安裝有Linux（Ubuntu10.4）操作系統和 hydro 版本的ROS（Robot Operating System）[16]。實驗環境為重慶郵電大學信息無障礙工程研發中心一樓，利用ROS中的gmapping節點構建環境地圖，且地圖的分辨率設置為0.05m/pix。移動機器人的系統噪聲誤差和觀測噪聲誤差為40cm和3m的高斯分布，其速度設置為0.2m/s，初始位置未知，兩種算法的初始粒子均設置為1000，其余相關參數與仿真實驗中參數設置相同。移動機器人將里程計和激光數據作為觀測信息，完成在已知先驗地圖上的移動機器人定位。

Pionner3-DX機器人從A點到B點的運動路徑如圖3所示；運動過程中粒子變化情況如圖4、5，分別表示兩種定位算法在移動機器人運動路徑中的位置①、②、③的粒子變化情況，機器人周圍的小箭頭表示粒子。圖4可看出移動機器人利用CMCL算法進行定位時，粒子在空間中的分布狀態不變，且隨著移動機器人的運動，機器人的位置出現了偏差，導致定位失敗。而利用AMPD-MCL算法進行移動機器人定位時，在初始位置，機器人對其位姿不確定性高，粒子數較多且分散在機器人周圍，如圖5（a）所示；隨著機器人的運動，結合觀測值和控制值對其位姿的矯正，定位所需粒子數不斷減少，且激光傳感器采集的觀測信息與當前機器人的位置信息相對吻合，定位精度相對較高。

圖6為兩種算法的粒子數隨機器人運動距離的變化情況，進一步說明了CMCL算法采用固定粒子數，而AMPD-MCL算法在機器人運動過程中可根據自由空間狀態來實時調整定位所需粒子數，結合激光傳感器的觀測信息，移動機器人定位所需要的粒子數穩定在60。

圖7為兩種算法的定位精度隨移動距離的變化情況，CMCL算法由于粒子退化，機器人定位精度隨運動距離的增加而降低，導致機器人定位失敗；AMPD-MCL算法在移動機器人運動過程中，根據激光傳感器采集的觀測信息進行定位修正，平均定位精度達到19.891cm，減弱了粒子退化現象，能實現移動機器人較精確的定位。

表2為兩種定位算法的性能對比，由此說明改進的AMPD-MCL算法有較高的定位精度，可根據移動機器人在自由空間的狀態實時調整采樣粒子數目，降低計算復雜度，提高計算效率，實現移動機器人室內定位。

4結語

本文提出了一種基于自適應多提議分布粒子濾波的MCL定位算法。利用Cubature卡爾曼濾波和擴展卡爾曼濾波對提議分布進行設計，在重采樣階段運用KLD采樣對粒子數進行實時調整，在減弱了粒子退化現象和提高了定位精度的同時，也減小了計算量，增強了實時處理能力。最后通過仿真和實驗驗證了改進算法的可行性和高效性。但是，本文只考慮了移動機器人在二維環境中的定位，忽略了高度帶來的影響，在一定程度上造成了定位偏差，因此三維環境下移動機器人的定位問題也是亟待解決的問題。

參考文獻：

[1]THRUN S， FOX D， BURGARD W， et al. Robust Monte Carlo localization for mobile robots [J]. Artificial Intelligence， 2001， 128（1/2）： 99-141.

[2]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（2）： 174-188.

[3]KHAN Z， BALCH T， DELLAERT F. MCMC-based particle filtering for tracking a variable number of interacting targets [J]. IEEE Transactions on Pattern Analysis and Machine Intelligence， 2005， 27（11）： 1805-1918.

[4]PFAFF P， PLAGEMANN C， BURGARD W. Improved likelihood models for probabilistic localization based on range scans [C]// Proceedings of 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway， NJ： IEEE， 2007： 2192-2197.

[5]VAN DE MERWE R， DOUCET A， DE FREITAS N， et al. The unscented particle filter [C]// Advances in Neural Information Processing Systems 13. Cambridge， MA： MIT Press， 2001： 584-590.

[6]WANG F， ZHAO Q， ZHANG Y， et al. Particle filtering with multi proposal distributions [J]. International Journal of Communications Network & System Sciences， 2008， 1（1）： 22-28.

[7]王法勝，魯明羽，趙清杰，等.粒子濾波算法[J].計算機學報，2014，37（8）：1679-1694. （WANG F S， LU M Y， ZHAO Q J， et al. Particle filtering algorithm[J]. Chinese Journal of Computers， 2014， 37（8）： 1679-1694.）

[8]宋宇，孫富春，李慶玲.移動機器人的改進無跡粒子濾波蒙特卡羅定位算法[J].自動化學報，2010，36（6）：851-857. （SONG Y， SUN F C， LI Q L. Mobile robot Monte Carlo localization based on improved unscented particle filter [J]. Acta Automatica Sinica， 2010， 36（6）： 851-857.）

[9]ALHASHIMI A W， HOSTETTLER R， GUSTAFSSON T. An improvement in the observation model for Monte Carlo localization [C]// ICINCO 2014：Proceedings of the 2014 11th International Conference on Informatics in Control， Automation and Robotics. Piscataway， NJ： IEEE， 2014： 498-505.

[10]周艷聰，董永峰，王安娜，等.新的室內移動機器人自定位方法[J].計算機應用，2015，35（2）：585-589. （ZHOU Y C， DONG Y F， WANG A N， et al. New self-localization method for indoor mobile robot [J]. Journal of Computer Applications， 2015， 35（2）： 585-589.）

[11]LI Q， SONG Y. Cubature MCL： Mobile robot Monte Carlo localization based on Cubature particle filter [C]// Proceedings of the 2012 31st Chinese Control Conference. Piscataway， NJ： IEEE， 2012： 5141-5145.

[12]THRUN S， BURGARD W， FOX D. Probabilistic robotics [J]. Communications of the ACM — Robots： intelligence， versatility， adaptivity， 2005， 45（3）： 52-57.

[13]ARASARATNAM I， HAYKIN S. Cubature Kalman filters [J]. IEEE Transactions on Automatic Control， 2009， 54（6）： 1254-1269.

[14]WANG F， LIN Y. Improving particle filter with a new sampling strategy [C]// ICCSE 09：Proceedings of the 2009 4th International Conference on Computer Sciences and Convergence Information Technology. Piscataway， NJ： IEEE， 2009： 408-412.

[15]FOX D. Adapting the sample size in particle filters through KLD-sampling [J]. International Journal of Robotics Research， 2003， 22（12）： 985-1103.

[16]張建偉，張立新，胡穎，等.開源機器人操作系統——ROS[M].北京：科學出版社，2012：1-6. （ZHANG J W， ZHANG L X， HU Y， et al. The Open Source Robot Operating System — ROS [M]. Beijing： Science Press， 2012： 1-6.）