Adaptive Iterated Cubature Particle Filter for Mobile Robot Monte Carlo Localization^*

摘要

In order to solve the problem of computational burden and poor performance in real time in cubature Monte Carlo localization (CMCL), a novel algorithm is presented in this paper. Firstly, a Cubature Particle Filter (CPF) for generating the importance proposal distribution by Gauss-Newton iterative Cubature Kalman Filter (ICKF) is designed. This algorithm is not limited by the high-order truncation error of ordinary cubature particle filters. Subsequently, enhance CPF by automatically adjusting the particle set size using the Kullback-Leibler Distance (KLD) standard, thereby increasing the speed of the newly proposed Adaptive Iterative CPF (AICPF). Simulation result is compared with standard cubature particle filter which demonstrates that the proposed AICPF is superior to the previous method in estimating the mean square and computational cost of the error. In addition, this study also applies AICPF to robot positioning on robotic operating systems (ROS). An analysis is conducted to confirm feasibility and efficiency of the adaptive iterated cubature MCL (AICMCL), which improves the accuracy of robot localization, and recovers more quickly from interference. It adjusts the number of particles needed for localization in real time, reduces computational burden, and improves the real-time processing capability.