王宏健, 傅桂霞, 边信黔, 李娟. 基于SRCKF的移动机器人同步定位与地图构建[J]. 机器人, 2013, 35(2): 200-207.DOI: 10.3724/SP.J.1218.2013.00200.
WANG Hongjian, FU Guixia, BIAN Xinqian, LI Juan. SRCKF Based Simultaneous Localization and Mapping of Mobile Robots. ROBOT, 2013, 35(2): 200-207. DOI: 10.3724/SP.J.1218.2013.00200.
In order to solve the large computing cost and numerical instabilities of simultaneous localization and mapping (SLAM), a square root cubature Kalman filter (SRCKF) based SLAM algorithm (SRCKF-SLAM) for mobile robots is designed according to cubature Kalman filter (CKF) principle. The SRCKF-SLAM algorithm accomplishes prediction and observation through motion model and observation model, and it is updated iteratively by propagating square root factors of the mean and covariance of the state variable, which guarantees the symmetry and positive semi-definiteness of the covariance matrix and therefore improves numerical accuracy and stability. The simulation experiments show that, compared with the CKF-SLAM algorithm, the root-mean square error of the SRCKF-SLAM algorithm decreases 14%, and the percentage of points in consistency area increases 36%, therefore the SRCKF-SLAM algorithm effectively satisfies the requirement of SLAM navigation of mobile robots.
[1] Durrant-Whyte H F, Bailey T. Simultaneous localization and mapping (SLAM): Part I, The essential algorithms[J]. IEEE Robotics and Automation Magazine, 2006, 13(2): 99-108. [2] Bailey T, Durrant-Whyte H F. Simultaneous localization and mapping (SLAM): Part II, State of the art[J]. IEEE Robotics and Automation Magazine, 2006, 13(3): 108-117. [3] Smith R C, Cheeseman P. On the representation and estimation of spatial uncertainty[J]. International Journal of Robotics Research, 1986, 5(4): 56-68. [4] Thrun S, Liu Y F, Koller D, et al. Simultaneous localization and mapping with sparse extended information filters[J]. International Journal of Robotics Research, 2004, 23(7/8): 693-716.[5] Julier S J, Uhlmann J K. Unscented filtering and nonlinear estimation[J]. Proceedings of the IEEE, 2004, 92(3): 401-422. [6] Bharani Chandra K P, Gu D W, Postlethwaite I. Cubature Kalman filter based localization and mapping[C]//18th IFAC World Congress. Laxenburg, Australia: IFAC, 2011: 2121-2125.[7] Doucet A, de Freitas N, Murphy K, et al. Rao-Blackwellised particle filtering for dynamic Bayesian networks[C]//16th Conference on Uncertainty in Artificial Intelligence. San Francisco, CA, USA: Morgan Kaufmann Pubishers, 2000: 176-183.[8] Doucet A, Godsill S, Andrieu C. On sequential Monte Carlo sampling methods for Bayesian filtering[J]. Statistics and Computing, 2000, 10(3): 197-208. [9] Song Y, Li Q L, Kang Y F, et al. Square-root cubature FastSLAM algorithm for mobile robot simultaneous localization and mapping[C]//IEEE International Conference on Mechatronics and Automation. Piscataway, NJ, USA: IEEE, 2012: 1162-1167.[10] Arasaratnam I, Haykin S. Cubature Kalman filters[J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269. [11] Dhital A. Bayesian filtering for dynamic systems with applications to tracking[D]. Barcelona, Spain: Universitat Politecnica de Catalunya, 2010.[12] Bae J, Kim Y. Nonlinear estimation for spacecraft attitude using decentralized unscented information filter[C]//International Conference on Control, Automation and Systems. Piscataway, NJ, USA: IEEE, 2010: 1562-1566.[13] Jia B, Xin M, Cheng Y. Sparse Gauss-Hermite quadrature filter with application to spacecraft attitude estimation[J]. Journal of Guidance, Control, and Dynamics, 2011, 34(2): 367-379. [14] Liu J, Cai B G, Tang T, et al. A CKF based GNSS/INS train integrated positioning method[C]//IEEE International Conference on Mechatronics and Automation. Piscataway, NJ, USA: IEEE, 2010: 1686-1689.[15] Pesonen H, Piche R. Cubature-based Kalman filters for positioning[C]//7th Workshop on Positioning, Navigation and Communication. Piscataway, NJ, USA: IEEE, 2010: 45-49.[16] 孙枫,唐李军.基于CKF的SINS大方位失准角初始对准[J].仪器仪表学报,2012,33(2):327-333. Sun F, Tang L J. Initial alignment of large azimuth misalignment angle in SINS based on CKF[J]. Chinese Journal of Scientific Instrument, 2012, 33(2): 327-333.[17] 郝燕玲,杨峻巍,陈亮,等.平方根容积卡尔曼滤波器[J].弹箭与制导学报,2012,32(2):169-172. Hao Y L, Yang J W, Chen L, et al. Square root cubature Kalman filter[J]. Journal of Projectiles, Rockets, Missiles and Guidance, 2012, 32(2): 169-172.[18] Thrun S, Fox D, Burgard W. Monte Carlo localization with mixture proposal distribution[C]//17th National Conference on Artificial Intelligence. Cambridge, MA, USA: MIT Press, 2000: 859-865.[19] Havlicek M, Jan J, Brazdil M, et al. Nonlinear estimation of BOLD signal based on cubature particle filter[C]//20th International EURASIP Conference. Brno, Czech: Brno University Technology Vut Press, 2010: 328-332.[20] 弋英民,刘丁.有色过程噪声下的轮式机器人同步定位与地图构建[J].电子学报,2010,38(6):1339-1343. Yi Y M, Liu D. Colored-state-noise simultaneous localization and map building for wheel robots[J]. Acta Electronica Sinica, 2010, 38(6): 1339-1343.[21] Australian Centre for Field Robotics. Source code[DB/OL]. (2008-06-10) [2012-05-21]. http://www-personal.acfr.usyd. edu.au/tbailey/.[22] Ribas D, Ridao P, Tardos J D, et al. Underwater SLAM in man-made structured environments[J]. Journal of Field Robotics, 2008, 25(11/12): 898-921.