基于平方根容积机器人蒙特卡罗定位算法研究

Study on the square-root cubature particle filter based mobile robot Monte Carlo localization algorithm

针对移动机器人Monte Carlo定位中粒子滤d波存在的粒子退化和粒子多样性匮乏问题,提出基于平方根容积粒子滤波的移动机器人Monte Carlo定位算法。新算法采用平方根容积卡尔曼滤波精确设计粒子的重要性函数,将当前观测信息融入重要性采样过程,提高对真实状态后验概率的逼近程度;新算法在Monte Carlo定位中直接传播及更新协方差阵的平方根因子,避免协方差阵分解与重构过程,保证协方差阵的对称性及正定性;基于排序的自适应局部重采样仅对部分粒子进行重采样,降低计算代价,增加粒子多样性;进而提高算法估计精度和一致性。实验结果表明:相同粒子条件下,新算法的计算代价比容积Monte Carlo定位算法约减少8%,不同粒子数目约多40%;新算法(10个粒子)的估计精度高于Monte Carlo定位算法(100个粒子),高于容积Monte Carlo定位算法(30个粒子)。

Aiming at the problems that particle degeneracy and particle diversity impoverishment exist in the particle filters in Monte Carlo localization, a square-root cubature particle filter based mobile robot Monte Carlo localization algorithm is proposed. In the new al- gorithm square-root cubature Kalman filter is used to accurately design the particle importance function, thus the latest observation infor- mation is merged into the importance sampling process, which improves the approaching level of the posterior probability of real state. In addition, in the new algorithm the square root factors of the covariance matrix are directly propagated and updated in Monte Carlo locali- zation process, which avoids the time-consuming Cholesky decomposition and reconstruction of the covariance matrix, and guarantees the symmetry and positive semi-definiteness of the covariance matrix. Moreover, the adaptive partial rank-based resampling （APRR） algo- rithm only conducts the resampling step on partial particles, which reduces the computational cost while at the same time increases the particle diversity; therefore the estimation accuracy and consistency of the algorithm are improved. The experiment results show that, compared with Cubature Monte Carlo localization algorithm, the computational cost of the proposed algorithm is reduced by 8% with the same number of particles, and the number of distinct particles is increased by 40%. The estimation accuracy of the proposed algorithm with 10 particles is higher than that of the Monte Carlo localization algorithm with 100 particles and that of the Cubature Monte Carlo lo- calization algorithm with 30 particles.