基于自适应多提议分布粒子滤波的蒙特卡洛定位算法

罗元　庞冬雪　张毅　苏琴

摘要：针对基于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.）