A Robot Spherical Simplex-Radial Cubature FastSLAM Algorithm
ZHU Qiguang1,2, YUAN Mei1, WANG Ziwei1, CHEN Ying3, CHEN Weidong1,2
1. School of Information Science and Engineering, Yanshan University, Qinhuangdao 066004, China;
2. The Key Laboratory for Special Fiber and Fiber Sensor of Hebei Province, Qinhuangdao 066004, China;
3. School of Electrical Engineering, Yanshan University, Qinhuangdao 066004, China
Standard FastSLAM algorithm suffers from the calculation of the Jacobian matrices and linearization error accumulation. To overcome these problems, a spherical simplex-radial cubature FastSLAM(SSRCFastSLAM) algorithm is proposed. The 3rd-degree spherical simplex-radial rule is utilized to calculate the nonlinear Gaussian weighted integral in order to improve SLAM accuracy. The proposed algorithm uses spherical simplex-radial cubature particle filter to estimate the path, and uses spherical simplex-radial cubature Kalman filter to maintain the landmarks. The performance of the proposed algorithm is compared with that of FastSLAM2.0, UFastSLAM and CFastSLAM through simulations and Victoria Park dataset. The results show that the proposed algorithm yields better localization and mapping ability than the other three algorithms with different particle numbers and noise levels, and the advantage is more significant when the number of particles is small or the environment disturbance is large, therefore the superiority of the proposed algorithm is verified.
[1] Durrant-Whyte H, Bailey T. Simultaneous localization and mapping:Part I[J]. IEEE Robotics and Automation Magazine, 2006, 13(2):99-108. [2] Smith R C, Cheeseman P. On the representation and estimation of spatial uncertainty[J]. International Journal of Robotics Research, 1986, 5(4):56-68. [3] Martinez-Cantin R, Castellanos J A. Unscented SLAM for large-scale outdoor environments[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Edmonton, AB, Canada:IEEE, 2005:328-333.[4] 张文玲,朱明清,陈宗海.基于强跟踪 UKF 的自适应 SLAM 算法[J].机器人,2010,32(2):190-195. Zhang W L, Zhu M Q, Chen Z H. An adaptive SLAM algorithm based on strong tracking UKF[J]. Robot, 2010, 32(2):190-195.[5] Murphy K, Russell S. Rao-Blackwellised particle filtering for dynamic Bayesian networks[M]//Doucet A, Freitas D N, Murphy K. Sequential Monte Carlo methods in practice. New York, USA:Springer-Verlag, 2001:499-515.[6] Montemerlo M, Thrun S, Koller D, et al. FastSLAM:A factored solution to the simultaneous localization and mapping problem[C]//18th National Conference on Artificial Intelligence. Cambridge, USA:MIT Press, 2002:593-598.[7] Montemerlo M, Thrun S, Koller D, et al. FastSLAM 2.0:An improved particle filtering algorithm for simultaneous localization and mapping that provably converges[C]//18th International Joint Conference on Artificial Intelligence. San Francisco, USA:Morgan Kaufmann Publishers Inc., 2003:1151-1156.[8] Kim C, Sakthivel R, Chung W K. Unscented FastSLAM:A robust and efficient solution to the SLAM problem[J]. IEEE Transactions on Robotics, 2008, 24(4):808-820. [9] Havangi R, Taghirad H D, Nekoui M A, et al. A square root unscented FastSLAM with improved proposal distribution and resampling[J]. IEEE Transactions on Industrial Electronics, 2014, 61(5):2334-2345. [10] Song Y, Li Q L, Kang Y F, et al. CFastSLAM:A new Jacobian free solution to SLAM problem[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2012:3063-3068.[11] 宋宇,李庆玲,康轶非,等.平方根容积 Rao-Blackwell- ized 粒子滤波 SLAM 算法[J].自动化学报,2014,40(2):357-367.Song Y, Li Q L, Kang Y F, et al. SLAM with square-root cubature Rao-Blackwillised particle filter[J]. Acta Automatica Sinica, 2014, 40(2):357-367.[12] 陈世明,袁军锋,陈小玲,等.基于类电磁机制优化的 FastSLAM2.0 算法[J].控制理论与应用,2015,32(1):127-132.Chen S M, Yuan J F, Chen X L, et al. A FastSLAM2.0 algorithm based on electromagnetism-like mechanism[J]. Control Theory & Application, 2015, 32(1):127-132.[13] Zikos N, Petridis V. 6-DoF low dimensionality SLAM(L-SLAM)[J]. Journal of Intelligent & Robotic Systems, 2015, 79(1):55-72. [14] Arasaratnam I, Haykin S. Cubature kalman filters[J]. IEEE Transactions on Automatic Control, 2009, 54(6):1254-1269. [15] Wang S Y, Feng J C, Tse C K. Spherical simplex-radial cubature Kalman filter[J]. IEEE Signal Processing Letters, 2014, 21(1):43-46. [16] Australian Centre for Field Robotics. Source code[DB/OL].(2008-06-10)[2015-06-28]. http://www-personal.acfr.usyd.edu.au/tbailey/.[17] Bailey T, Nieto J, Nebot E. Consistency of the FastSLAM algorithm[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2006:424-429.[18] Nebot E. Victoria park dataset[DB/OL]. 2014-07-03/2015-06-28. http://www-personal.acfr.usyd.edu.au/nebot/victoria_park.htm.
