Abstract
FastSLAM is a popular framework which uses a Rao-Blackwellized particle filter to solve the simultaneous localization and mapping problem (SLAM). However, in this framework there are two important potential limitations, the particle depletion problem and the linear approximations of the nonlinear functions. To overcome these two drawbacks, this paper proposes a new FastSLAM algorithm based on revised genetic resampling and square root unscented particle filter (SR-UPF). Double roulette wheels as the selection operator, and fast Metropolis-Hastings (MH) as the mutation operator and traditional crossover are combined to form a new resampling method. Amending the particle degeneracy and keeping the particle diversity are both taken into considerations in this method. As SR-UPF propagates the sigma points through the true nonlinearity, it decreases the linearization errors. By directly transferring the square root of the state covariance matrix, SR-UPF has better numerical stability. Both simulation and experimental results demonstrate that the proposed algorithm can improve the diversity of particles, and perform well on estimation accuracy and consistency.
Article PDF
Similar content being viewed by others
Explore related subjects
Discover the latest articles, news and stories from top researchers in related subjects.Avoid common mistakes on your manuscript.
References
C. L. Wang, T. M. Wang, J. H. Liang, Y. C. Zhang, Y. Zhou. Bearing-only visual SLAM for small unmanned aerial vehicles in GPS-denied environments. International Journal of Automation and Computing, vol. 10, no. 5, pp. 387–396, 2013.
S. Thrun, W. Burgard, D. Fox. Probabilistic Robotics, Cambridge, USA: The MIT Press, 2005.
M. Kaess, A. Ranganathan, F. Dellaert, iSAM: Incremental smoothing and mapping, Robotics, vol. 24, no. 6, pp. 1365–1378, 2008.
S. Thrun, M. Montemerlo. The GraphSLAM Algorithm with Application to Large-Scale Mapping of Urban Structures, The International Journal of Robotics Research, vol. 25, no. 5–6, pp. 403–429, 2006.
H. Wang, W. Mou, G. Seet, M. H. Li, M. W. S. Lau, D. W. Wang. Real-time visual odometry estimation based on principal direction detection on ceiling vision. International Journal of Automation and Computing, vol. 10, no. 5, pp. 397–404, 2013.
M. Montemerlo. FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem with Unknown Data Association, Ph.D. dissertation, Carnegie Mellon University, USA, 2003.
C. Gamallo, M. Mucientes, C. V. Regueiro. A FastSLAMbased Algorithm for Omni directional Cameras, Journal of Physical Agents, vol. 7, no. 1, pp. 12–21, 2012.
S. M. Chen, J. F. Yuan, F. Zhang, H. J. Fang. Multirobot FastSLAM Algorithm Based on Landmark Consistency Correction, Mathematical Problems in Engineering, vol. 2014, pp. 1–7, 2014.
R. Martinez-Cantin, J. A. Castellanos. Unscented SLAM for large-scale outdoor environments. In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, IEEE, Edmonton, Canada, pp. 3427–3432, 2005.
D. B. Liu, G. R. Liu, M. H. Yu. An improved FastSLAM framework based on particle swarm optimization and unscented particle filter. Journal of Computational Information Systems, vol. 8, no. 7, pp. 2859–2866, 2012.
K. Chanki, R. Sakthivel, W. K. Chung. Unscented Fast- SLAM: A robust algorithm for the simultaneous localization and mapping problem. In Proceedings of International Conference on Robotics and Automation, IEEE, Roma, Italy, pp. 2439–2445, 2007.
J. H. Zhu, N. N. Zheng, Z. J. Yuan, Q. Zhang. A SLAM algorithm based on central difference particle filter. Acta Automatica Sinica, vol. 36, no. 2, pp. 249–257, 2010. (in Chinese)
Y. Song, Y. D. Song, Q. L. Li. Robust iterated sigma point FastSLAM algorithm for mobile robot simultaneous localization and mapping. Chinese Journal of Mechanical Engineering, vol. 24, no. 4, pp. 693–700, 2011.
T. Bailey, J. Nieto, E. Nebot. Consistency of the FastSLAM algorithm. In Proceedings of International Conference on Robotics and Automation, IEEE, Orlando, USA, pp. 424–429, 2006.
C. Stachniss, D. Hahnel, W. Burgard. Exploration with active loop-closing for FastSLAM. In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, IEEE, Sendai, Japan, pp. 1505–1510, 2004.
J. S. Liu. Metropolized independent sampling with comparisons to rejection sampling and importance sampling. Statistics and Computing, vol. 6, no. 2, pp. 113–119, 1996.
G. Grisetti, C. Stachniss, W. Burgard. Improved techniques for grid mapping with Rao-Blackwellized particle filters. IEEE Transactions on Robotics, vol. 23, no. 1, pp. 34–46, 2007.
N. Nosan, I. K. Kim, H. C. Lee, B. H. Lee. Analysis of resampling process for the particle depletion problem in fastslam. In Proceedings of the 16th International Symposium on Robot and Human Interactive Communication, IEEE, Jeju Island, Korea, pp. 200–205, 2007.
X. Z. Li, S. M. Jia, W. Cui. On sample diversity in particle filter based robot SLAM. In Proceedings of International Conference on Robotics and Biomimetics, IEEE, Phuket, Thailand, pp. 1072–1077, 2011.
Y. M. Xia, Y. M. Yang. An improved FastSLAM algorithm based on genetic algorithms. In Proceedings of International Symposium, Communications in Computer and Information Science, Springer, Guangzhou, China, pp. 296–302, 2011.
H. Wang, Y. Yan, D. W. Wang. A GA based SLAM with range sensors only. In Proceedings of the 11th International Conference on Control Automation Robotics and Vision, IEEE, Singapore, pp. 1796–1802, 2010.
P. Li, S. M. Song, X. L. Chen, G. R. Duan. Square root unscented Kalman filter incorporating Gaussian process regression. Systems Engineering and Electronics, vol. 32, no. 6, pp. 1281–1285, 2010. (in Chinese)
R. D. Merw, E. A. Wan. The square-root unscented kalman filter for state and parameter-estimation. Systems Engineering and Electronics, vol. 32, no. 6, pp. 1281–1285, 2010.
P. Giordani, R. Kohn. Adaptive independent Metropolis- Hastings by fast estimation of mixtures of normals. Journal of Computational and Graphical Statistics, vol. 19, no. 2, pp. 243–259, 2010.
J. E. Guivant, E. M. Nebot. Optimization of the simultaneous localization and map-building algorithm for real-time implementation. IEEE Transactions on Robotics and Automation, vol. 17, no. 3, pp. 242–257, 2001.
T. Bailey. SLAM simulations, [Online], Available: http://www-personal.acfr.usyd.edu.au/tbailey/software/, March 10, 2011.
Y. Bar-Shalom, X. R. Li, T. Kirubarajan. Estimation with Applications to Tracking and Navigation: Theory Algorithms and Software, Hoboken, USA: John Wiley and Sons, 2004.
E. Nebot, J Guivant, J. Nieto. ACFR, Experimental outdoor dataset, [Online], Available: http://www.acfr.usyd. edu.au/home-pages/academic/enebot/dataset.htm, July 28, 2012.
Acknowledgments
Tai-Zhi Lv would like to thank Jiangsu Overseas Research & Training Program for University Prominent Young & Middle-aged Teachers and Presidents for financial support.
Author information
Authors and Affiliations
Corresponding author
Additional information
This work was supported by National Natural Science Foundation of China (No. 61101197) and Research Fund for the Doctoral Program of Higher Education of China(No. 20093219120025).
Recommended by Associate Editor Min Tan
Tai-Zhi Lv received the B. Sc. degree in computer science from Nanjing University, China in 2002, and the M. Sc. degree in computer software and theory from Nanjing University of Science and technology, China in 2006. He is currently a Ph.D. degree candidate of Nanjing University of Science and Technology, China.
His research interests include SLAM (Simultaneous localization and mapping) and robot navigation.
Chun-Xia Zhao received the B. Sc. degree in industrial automatization from Harbin Institute of Technology, China in 1985, the M. Sc. degree in pattern recognition and artificial intelligence control from Harbin Institute of Technology, China in 1988, and the Ph.D. degree in electromechanics and automatization from Harbin Institute of Technology, China in 1998. She is currently a professor at School of Computer Science and Technology, Nanjing University of Science and Technology, China. She has published above 100 refereed journal and conference papers.
Her research interests include underground robotics, computer vision and navigation.
Hao-Feng Zhang received the B. Sc. in computer science and technology from Nanjing University of Science and Technology, China in 2003, and the Ph.D. degree in pattern recognition and intelligent systems from Nanjing University of Science and Technology, China in 2007. He is currently an associate professor at School of Computer Science and Technology, Nanjing University of Science and Technology, China.
His research interests include robotics, robot navigation and image process technology.
Rights and permissions
About this article
Cite this article
Lv, TZ., Zhao, CX. & Zhang, HF. An Improved FastSLAM Algorithm Based on Revised Genetic Resampling and SR-UPF. Int. J. Autom. Comput. 15, 325–334 (2018). https://doi.org/10.1007/s11633-016-1050-y
Received:
Accepted:
Published:
Issue Date:
DOI: https://doi.org/10.1007/s11633-016-1050-y