FastSLAM for mobile robot based on box particle filter with intelligence optimization
-
摘要:
针对传统FastSLAM算法需要大量粒子构建地图导致计算复杂度高、难以提高估计精度等问题,研究构建了一种基于智能优化箱粒子滤波(IOBPF)的移动机器人FastSLAM算法。首先,将萤火虫算法(FA)的动态寻优机制引入箱粒子滤波(BPF),建立了箱粒子的荧光亮度更新公式、吸引度计算公式和位置更新公式,使箱粒子集智能化地向高似然区域移动,避免了箱粒子的退化现象。然后,以改进的智能优化箱粒子滤波进行机器人位姿估计,并采用扩展区间卡尔曼滤波(EIKF)完成地图的构建和更新。移动机器人的模型仿真和实体实验结果表明:所提智能化FastSLAM算法可有效提升箱粒子的性能,并降低地图构建所需粒子数,从而显著提高FastSLAM的定位精度和地图构建的鲁棒性。
-
关键词:
- 同步定位与地图构建 /
- 移动机器人 /
- 箱粒子滤波(BPF) /
- 萤火虫算法(FA) /
- 扩展区间卡尔曼滤波(EIKF)
Abstract:The traditional FastSLAM algorithm requires a large number of particles to build the map, thus resulting in high computational complexity and difficulty in improving the estimation accuracy. In view of these problems, an algorithm of FastSLAM for mobile robot is presented based on box particle filter with intelligence optimization (IOBPF). First, the dynamic optimization mechanism of firefly algorithm (FA) is applied to the box particle filter (BPF), and the formulas of fluorescence brightness updating, attraction calculation and position updating of box particle are constructed, which makes the box particles move toward the high-likelihood region intelligently and avoid the phenomenon of box particle degeneracy. Then, the improved BPF with intelligence optimization is utilized to estimate the pose of robot, and the extended interval Kalman filter (EIKF) is employed to complete the map building and updating. The results of model simulation and entity experiment of mobile robot show that the intelligent FastSLAM algorithm in this paper can effectively improve the performance of box particles and reduce the number of particles required for map construction, thus significantly improving the positioning accuracy and robustness of map construction.
-
表 1 二维箱粒子的基本数学定义
Table 1. Basic mathematical definition of two-dimensional box particle
参数 数学表示 宽度 中心 范数 体积 表 2 仿真参数设置
Table 2. Simulation parameter setting
参数 设定值 机器人最大转向角 ±30° 机器人最大转向角速度 ±2°/s 机器人轴间距 2 m 传感器的最大观测范围 20 m 传感器的观测频率 50 Hz 控制时间间隔 0.02 s 控制噪声 (0.35 m/s, 2°) 观测噪声 (0.3 m/s, 3°) 表 3 不同SLAM算法的性能比较
Table 3. Comparison of performance among different SLAM algorithms
算法 粒子数 CPU时间/s 运行时间/s GridSLAM 100 0.873 318.72 GMapping 30 0.401 261.37 BPF-SLAM 15 0.325 223.41 本文算法 10 0.337 247.69 -
[1] 王忠立, 赵杰, 蔡鹤皋. 大规模环境下基于图优化SLAM的图构建方法[J]. 哈尔滨工业大学学报, 2015, 47(1): 75-85. doi: 10.3969/j.issn.1009-1971.2015.01.011WANG Z L, ZHAO J, CAI H G. A survey of front-end method for graph-based SLAM under large-scale environment[J]. Journal of Harbin Institute of Technology, 2015, 47(1): 75-85(in Chinese). doi: 10.3969/j.issn.1009-1971.2015.01.011 [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. doi: 10.1177/027836498600500404 [3] HOLMES S A, KLEIN G, MURRAY D W. An O(N2) square root unscented Kalman filter for visual simultaneous localization and mapping[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2009, 31(7): 1251-1263. doi: 10.1109/TPAMI.2008.189 [4] DOUCET A, DE FREITAS N, MURPHY K, et al. Rao-Blackwellised particle filtering for dynamic Bayesian networks[M]//DOUCET A, DE FREITAS N, GORDON N. Sequential Monte Carlo method in practice. Berlin: Springer, 2001: 499-515. [5] MONTEMERLO M, THRUN S, KOLLER D, et al. FastSLAM: A factored solution to the simultaneous localization and mapping problem[C]//Proceedings of the National Conference on Artificial Intelligence, 2002: 593-598. [6] MONTEMERLO M, THRUN S, ROLLER D, et al. FastSLAM 2.0: An improved particle filtering algorithm for simultaneous localization and mapping that provably converges[C]//Proceeding of the International Joint Conference on Artificial Intelligence, 2003: 1151-1156. [7] ELIAZAR A, PARR R. DP-SLAM: Fast, robust simultaneous localization and mapping without predetermined landmarks[C]//Proceedings of the International Joint Conference on Artificial Intelligence, 2003: 1135-1142. [8] HAHNEL D, BURGARD W, FOX D, et al. An efficient fastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements[C]//International Conference on Intelligent Robots and Systems. Piscataway: IEEE Press, 2003, 1: 206-211. [9] GRISETTI G, STACHNISS C, BURGARD W. Improved techniques for grid mapping with Rao-Blackwellized particle filters[J]. IEEE Transactions on Robotics, 2007, 23(1): 34-46. doi: 10.1109/TRO.2006.889486 [10] 祝继华, 郑南宁, 袁泽剑, 等. 基于ICP算法和粒子滤波的未知环境地图创建[J]. 自动化学报, 2009, 35(8): 1107-1113. https://www.cnki.com.cn/Article/CJFDTOTAL-MOTO200908013.htmZHU J H, ZHENG N N, YUAN Z J, et al. A SLAM approach by combining ICP algorithm and particle filter[J]. Acta Automatica Sinica, 2009, 35(8): 1107-1113(in Chinese). https://www.cnki.com.cn/Article/CJFDTOTAL-MOTO200908013.htm [11] BAILEY T, NIETO J, NEBOT E. Consistency of the FastSLAM algorithm[C]//IEEE International Conference on Robotics and Automation (ICRA). Piscataway: IEEE Press, 2006: 424-429. [12] VINCKE B, LAMBERT A, ELOUARDI A. Guaranteed simultaneous localization and mapping algorithm using interval analysis[C] //International Conference on Control Automation Robotics and Vision. Piscataway: IEEE Press, 2018: 1409-1414. [13] ABDALLAH F, GNING A, BONNIFAIT P. Box particle filtering for nonlinear state estimation using interval analysis[J]. Automatica, 2008, 44(3): 807-815. doi: 10.1016/j.automatica.2007.07.024 [14] 李振兴, 刘进忙, 李松, 等. 基于箱式粒子滤波的群目标跟踪算法[J]. 自动化学报, 2015, 41(4): 785-798. https://www.cnki.com.cn/Article/CJFDTOTAL-MOTO201504010.htmLI Z X, LIU J M, LI S, et al. Group targets tracking algorithm based on box particle filter[J]. Acta Automatica Sinica, 2015, 41(4): 785-798(in Chinese). https://www.cnki.com.cn/Article/CJFDTOTAL-MOTO201504010.htm [15] GNING A, RISTIC B, MIHAYLOVA L. Bernoulli particle/box-particle filters for detection and tracking in the presence of triple measurement uncertainty[J]. IEEE Transactions on Signal Processing, 2012, 60(5): 2138-2151. doi: 10.1109/TSP.2012.2184538 [16] YANG X S. Firefly algorithm, stochastic test functions and design optimization[J]. International Journal of Bio-Inspired Computation, 2010, 2(2): 78-84. doi: 10.1504/IJBIC.2010.032124 [17] CHEN G, WANG J, SHIEH L S. Interval Kalman filtering[J]. IEEE Transactions on Aerospace and Electronic Systems, 1997, 33(1): 250-259. doi: 10.1109/7.570759 [18] JAULIN L, KIEFFER M, DIDRIT O, et al. Applied interval analysis[M]. Berlin: Springer, 2001: 1-116. [19] GNING A, MIHAYLOVA L, ABDALLAH F. Mixture of uniform probability density functions for non-linear state estimation using interval analysis[C]//The 13th Conference on Information Fusion. Piscataway: IEEE Press, 2010: 1-8. [20] GNING A, RISTIC B, MIHAYLOVA L, et al. An introduction to box particle filtering[J]. IEEE Signal Processing Magazine, 2013, 30(4): 166-171. doi: 10.1109/MSP.2013.2254601 [21] 田梦楚, 薄煜明, 陈志敏, 等. 萤火虫算法智能优化粒子滤波[J]. 自动化学报, 2016, 42(1): 89-97. https://www.cnki.com.cn/Article/CJFDTOTAL-MOTO201601008.htmTIAN M M, BO Y M, CHEN Z M, et al. Firefly algorithm intelligence optimized particle filter[J]. Acta Automatica Sinica, 2016, 42(1): 89-97(in Chinese). https://www.cnki.com.cn/Article/CJFDTOTAL-MOTO201601008.htm [22] LI N, MA H, YANG C. Interval Kalman filter based RFID indoor positioning[C]//Control and Decision Conference. Piscataway: IEEE Press, 2016: 6958-6963. [23] LE Y, HE X F, XIAO R Y. MEMS IMU and GPS/Beidou integration navigation system using interval Kalman filter[J]. Applied Mechanics and Materials, 2014, 568-570: 970-975. doi: 10.4028/www.scientific.net/AMM.568-570.970 [24] HE X F, VIK B. Use of extended interval Kalman filter on integrated GPS/INS system[C]//Proceedings of International Technical Meeting of the Satellite Division of the Institute of Navigation, 1999: 1907-1914. [25] 万振刚. MINS/GPS组合导航系统若干关键技术研究[D]. 南京: 东南大学, 2005: 55-56.WAN Z G. Research on some key technologies of MINS/GPS integrated navigation system[D]. Nanjing: Southeast University, 2005: 55-56(in Chinese). [26] BAILEY T. SLAM simulations[DS/OL]. (2008-06-10)[2020-08-20]. http://www.personal.acfr.usyd.edu.au/tbailey/software/index.html. [27] KIM C, SAKTHIVEL R, WAN K C. Unscented fastSLAM: A robust and efficient solution to the SLAM problem[J]. IEEE Transactions on Robotics, 2008, 24(4): 808-820. doi: 10.1109/TRO.2008.924946 [28] DUAN J M, LIU D, YU H X, et al. An improved FastSLAM algorithm for autonomous vehicle based on the strong tracking square root central difference Kalman filter[C]//2015 IEEE 18th International Conference on Intelligent Transportation Systems (ITSC 2015). Piscataway: IEEE Press, 2015: 693-698. [29] STACHNISS C. Robotic mapping and exploration[M]. Berlin: Springer, 2009: 10-16.