Abstract
In this paper we consider a Simultaneous Localization and Mapping (SLAM) problem for a moving agent using Visual Odometry (VO) while measuring the range from a set of Ultra Wide Band (UWB) antennas, deployed in unknown position in the environment. The solution approach is based on a switching observer which, under standard working conditions, for each observed UWB, uses a two dimensional Extended Kalman Filter (EKF) providing an estimate of the range and bearing of the observed UWB with respect to the agent. This information is then used in a Robust EKF algorithm which solves the SLAM problem with performances that, even before closing the loop, are comparable to the ones that a VO algorithm (namely ORB-SLAM2) would obtain only after closing the loop. Moreover, a resilient module is added to the algorithm to evaluate the reliability of the position estimate of each observed UWB. When the Visual Odometry is not available, the switching observer uses an auxiliary EKF to provide an estimate of the agent position. This makes the proposed approach robust with respect to several kinds of unmodeled disturbances, like multipath effects, and automatically adapts to sensor failures with resilience (e.g. when Visual Odometry or UWB measurements are not available).
Article PDF
Similar content being viewed by others
Avoid common mistakes on your manuscript.
References
Menegatti, E., Zanella, A., Zilli, S., Zorzi, F., Pagello, E.: Range-only SLAM with a mobile robot and a wireless sensor networks. In IEEE Int. Conf. Robot. Autom. 8–14 (2009)
Leonard, J.J., Olson, E., Teller, S.: Robust range-only beacon localization. IEEE J Oceanic Eng. (2006)
Djugash, J., Singh, S.: A robust method of localization and mapping using only range. Experimental Robot. (2009)
Wang, J.J., Ahmad, A., Huang, S., Dissanayake, G.: A new state vector and a map joining algorithm for range-only SLAM. In 12th international conference on control, automation, robotics & vision, Guangzhou, China (2012)
Martinez-de Dios, J.R., Torres-González, A., Ollero, A.: Range-only SLAM for robot-sensor network cooperation. Autonomous Robot. 42(3), 649–663 (2018)
Kim, J., Kim, D.: Cooperative range-only SLAM based on sum of gaussian filter in dynamic environments. In 2019 IEEE/RSJ international conference on intelligent robots and systems (IROS), pages 2139–2144, Macau, China, (2019). https://doi.org/10.1109/IROS40897.2019.8967646
Segura, M.J., Auat Cheein, F.A., Toibero, J.M., Mut, V., Carelli, R.: Ultra wide-band localization and SLAM: A comparative study for mobile robot navigation. Sensors. 11(2), 2035–2055 (2011). ISSN 1424-8220. https://doi.org/10.3390/s110202035. https://www.mdpi.com/1424-8220/11/2/2035
Shi, Q., Cui, X., Li, W., Xia, Y., Lu, M.: Visual-UWB navigation system for unknown environments. ION GNSS+, The International Technical Meeting of the Satellite Division of The Institute of Navigation (2018). ISSN 2331-5954. https://doi.org/10.33012/2018.15962
Martinelli, F., Mattogno, S., Romanelli, F.: A resilient solution to range-only slam based on a decoupled landmark range and bearing reconstruction. Robot. Autonomous Syst. 160, 104324 (2023). ISSN 0921-8890. https://doi.org/10.1016/j.robot.2022.104324. https://www.sciencedirect.com/science/article/pii/S0921889022002135
Di Giampaolo, E., Martinelli, F.: Range and bearing estimation of an UHF-RFID tag using the phase of the backscattered signal. IEEE J. Radio Frequency Identif. 4(4), 332–342, 12 (2020). https://doi.org/10.1109/JRFID.2020.3016168
Fabrizio Romanelli, Francesco Martinelli, and Emidio Di Giampaolo. Robust simultaneous localization and mapping using range and bearing estimation of radio ultra high frequency identification tags. IEEE Trans. Control Syst. Technol. 1–14 (2022). https://doi.org/10.1109/TCST.2022.3204386
Mur-Artal, Raúl., Tardós, Juan D.: Orb-slam2: An open-source slam system for monocular, stereo, and rgb-d cameras. IEEE Transactions on Robotics 33(5), 1255–1262 (2017). https://doi.org/10.1109/TRO.2017.2705103
Li, D., Wang, Y., Wang, J., Wang, C., Duan, Y.: Recent advances in sensor fault diagnosis: A review. Sens. Actuators A: Phys. 309 (2020). ISSN 0924-4247. https://doi.org/10.1016/j.sna.2020.111990
Thyagharajan, A., Omer, O.J., Mandal, D., Subramoney, S.: Towards noise resilient SLAM. In 2020 IEEE international conference on robotics and automation (ICRA), 72–79 (2020). https://doi.org/10.1109/ICRA40945.2020.9196745
Lajoie, Pierre-Yves., Ramtoula, Benjamin, Chang, Yun, Carlone, Luca, Beltrame, Giovanni: DOOR-SLAM: Distributed, online, and outlier resilient slam for robotic teams. IEEE Robotics and Automation Letters 5(2), 1656–1663 (2020). https://doi.org/10.1109/LRA.2020.2967681
Hsu, L.Y., Chen, T.L.: Vehicle full-state estimation and prediction system using state observers. Vehic. Technol. IEEE Trans. 58, 2651–2662, 08 (2009). https://doi.org/10.1109/TVT.2008.2008811
Hao Zhao, Hao Luo, and Yunkai Wu. A data-driven scheme for fault detection of discrete-time switched systems. Sensors 21(12) (2021). ISSN 1424-8220. https://doi.org/10.3390/s21124138. https://www.mdpi.com/1424-8220/21/12/4138
Geneva, P., Eckenhoff, K., Huang, G.: Asynchronous multi-sensor fusion for 3D mapping and localization. In 2018 IEEE international conference on robotics and automation (ICRA), 5994–5999 (2018). https://doi.org/10.1109/ICRA.2018.8460204
Wang, C., Han, H., Wang, J., Yu, H., Yang, D.: A robust extended Kalman filter applied to Ultrawideband positioning. Mathematical Problems in Engineering, 1–12 (2020). https://doi.org/10.1155/2020/1809262
Izenman A.J.: Modern Multivariate Statistical Techniques: Regression, Classification, and Manifold Learning, 1st edn. Springer Publishing Company, Incorporated (2008). ISBN 0387781889
Chang, G.: Robust Kalman filtering based on Mahalanobis distance as outlier judging criterion. J Geod 88, 391–401 (2014). https://doi.org/10.1007/s00190-013-0690-8
Martinelli, F., Romanelli, F.: A resilient ro-slam algorithm with bearing reconstruction of detected landmarks. In 2022 3rd International conference on artificial intelligence, robotics and control (AIRC), 61–66 (2022). https://doi.org/10.1109/AIRC56195.2022.9836986
Romanelli F.: ORB-SLAM2 - New parameters management, ARM compilation, CUDA GPU compatibility. Version 1.0.0, 8 (2021). https://github.com/fabrizioromanelli/ORBSLAM2
Hao-jun L., Shi-hua, T., Jie H.: Discussion for the selection of constant in selecting weight iteration method in robust estimation. Sci. Surv. Map. (2006)
Burri, Michael, Nikolic, Janosch, Gohl, Pascal, Schneider, Thomas, Rehder, Joern, Omari, Sammy, Achtelik, Markus, Siegwart, Roland: The euroc micro aerial vehicle datasets. The International Journal of Robotics Research 35, 01 (2016). https://doi.org/10.1177/0278364915620033
Cao, Y., Beltrame, G.: Vir-slam: visual, inertial, and ranging slam for single and multi-robot systems. Autonomous Robots 45, 905–917 (2021). https://doi.org/10.1007/s10514-021-09992-7
Romanelli, Fabrizio, Martinelli, Francesco: Synthetic sensor data generation exploiting deep learning techniques and multimodal information. IEEE Sensors Letters 7(7), 1–4 (2023). https://doi.org/10.1109/LSENS.2023.3290209
Alzubaidi, L., Zhang, J., Humaidi, A.J., et al.: Review of deep learning: concepts, cnn architectures, challenges, applications, future directions. Journal of Big Data, 53(8) (2021). https://doi.org/10.1186/s40537-021-00444-8
Prokhorov, D., Zhukov, D., Barinova, O., Anton, K., Vorontsova, A.: Measuring robustness of visual slam. In 2019 16th International Conference on Machine Vision Applications (MVA), 1–6 (2019). https://doi.org/10.23919/MVA.2019.8758020
Acknowledgements
Data related to has been partially presented in from the same authors. In particular, a similar approach for the detection of global outliers and the scaling for Kalman gain has been exploited in this paper in order to address the problem of countering the effect of outliers in the REKF-SLAM; however, this has been presented to make the discussion more comprehensive and not as an innovative aspect.
Funding
Open access funding provided by Università degli Studi di Roma Tor Vergata within the CRUI-CARE Agreement.
Author information
Authors and Affiliations
Corresponding author
Ethics declarations
Conflicts of interest
This work has been supported in part by the Italian Ministry for Research in the framework of the Program for Research Projects of National Interest (PRIN), under Grants 2017YKXYXJ and 2020RTWES4. The authors have no relevant financial or non-financial interests to disclose. All authors contributed to the study conception and design. Material preparation, data collection and analysis were performed by Fabrizio Romanelli, Francesco Martinelli and Simone Mattogno. The first draft of the manuscript was written by Francesco Martinelli and Fabrizio Romanelli and all authors commented on previous versions of the manuscript. All authors read and approved the final manuscript.
Additional information
Publisher's Note
Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.
Rights and permissions
Open Access This article is licensed under a Creative Commons Attribution 4.0 International License, which permits use, sharing, adaptation, distribution and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons licence, and indicate if changes were made. The images or other third party material in this article are included in the article’s Creative Commons licence, unless indicated otherwise in a credit line to the material. If material is not included in the article’s Creative Commons licence and your intended use is not permitted by statutory regulation or exceeds the permitted use, you will need to obtain permission directly from the copyright holder. To view a copy of this licence, visit http://creativecommons.org/licenses/by/4.0/.
About this article
Cite this article
Romanelli, F., Martinelli, F. & Mattogno, S. Resilient Simultaneous Localization and Mapping Fusing Ultra Wide Band Range Measurements and Visual Odometry. J Intell Robot Syst 109, 64 (2023). https://doi.org/10.1007/s10846-023-01995-z
Received:
Accepted:
Published:
DOI: https://doi.org/10.1007/s10846-023-01995-z