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

基于自适应多提议分布粒子滤波蒙特卡洛定位算法 　　摘要：针对基于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算法