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

羅元　龐冬雪　張毅　蘇琴

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

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

中圖分類號：TP242.6

文獻標志碼：A

0引言

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

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

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

2改進的CMCL算法

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

2.1多提議分布粒子濾波

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

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

2.2基于KLD的重采樣

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

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

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

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

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

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

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

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

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

4結(jié)語

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

參考文獻：

[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].計算機學(xué)報，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].自動化學(xué)報，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]周艷聰，董永峰，王安娜，等.新的室內(nèi)移動機器人自定位方法[J].計算機應(yīng)用，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]張建偉，張立新，胡穎，等.開源機器人操作系統(tǒng)——ROS[M].北京：科學(xué)出版社，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.）