“Efficient Probabilistic Range-Only SLAM” IEEE/RSJ 2008 International Conference on Intelligent Robots and Systems (IROS) Jose-Luis Blanco, Juan-Antonio Fern´andez-Madrigal, and Javier Gonz´alez Department of System Engineering and Automation University of Málaga (Spain) Outline • Background • Range-Only SLAM • Map Update • Observation Model • Experiment simulations 2 Background • SLAM : - Sensors : Odometry, IMU(internal) ; LiDAR, Stereo Camera(external) - Bearing-Only SLAM, Range-Only(RO) SLAM • RO-SLAM : - Localization & Mapping with range-only devices - To enable a vehicle to localize itself using RO devices, without any previous information about the 3D location of the beacons - Application : submarine autonomous vehicles [1] P. Newman and J. Leonard, “Pure range-only sub-sea SLAM,” Robotics and Automation, 2003. Proceedings. ICRA’03. IEEE International Conference on, vol. 2, 2003. [2] J. Fern´andez-Madrigal, E. Cruz-Martin, J. Gonzalez, C. Galindo, and J. Blanco, “Application of UWB and GPS Technologies for Vehicle Localization in Combined Indoor-Outdoor Environments,” International Symposium on Signal Processing and Its Applications (ISSPA), 2007. 3 RO-SLAM (RBPF approach) • Pros & Cons No data association problem High ambiguity of measurement • Difficult to integrate RO-SLAM with probabilistic framework - Multi-modality : with RO sensors, everything is multimodal Eg, beacon location hypotheses - Strongly non-linear problem & non-Gaussian density classic EKF SLAM is inappropriate • Approach implemented in this research Rao-Blackwellized Particle Filter (RBPF) [3] Yilmaz, Sezcan, et al. "Mobile robot localization via outlier rejection in sonar range sensor data." 2011 7th International Conference on Electrical and Electronics Engineering (ELECO). IEEE, 2011. 4 RO-SLAM (RBPF approach) • Rao-Blackwellized Particle Filter (RBPF) 𝑝 𝑥 𝑡 , 𝑚 𝑧 𝑡 , 𝑢𝑡 = 𝑝 𝑥 𝑡 𝑧 𝑡 , 𝑢𝑡 Robot path 𝑝 𝑚 𝑥 𝑡 , 𝑧 𝑡 , 𝑢𝑡 Map - Robot path : estimate by a set of particles - Map : Only conditional distributions • Estimate each beacon independently, 𝑝 𝑚𝑙 𝑥 𝑡 , 𝑧𝑙𝑡 𝑝 𝑚 𝑥 𝑡 , 𝑧 𝑡 , 𝑢𝑡 = 𝑙 5 RO-SLAM (RBPF approach) • Each beacon, at each particle, can be represented by a different kind of pdf to fit the actual uncertainty - First beacon is observed, a sum of Gaussians(SOG) is created - With new observations, unlikely Gaussian modes are discarded Finally, each beacon is represented by a single EKF • Benefits - New beacons can be added at any time, to improve robot localization - Computational complexity dynamically adapts to the uncertainty - More robust and efficient 6 Map Update Each iteration updates the map with new measurement 𝑝 𝑚 𝑥 𝑡 , 𝑧 𝑡 , 𝑢𝑡 ∝ 𝑝 𝑚 𝑥 𝑡−1 , 𝑧 𝑡−1 𝑝 𝑧 𝑡 𝑚, 𝑥 𝑡 , 𝑧 𝑡−1 Posterior Prior Sensor Model Case 1. First insertion to the map : in 2D 7 Map Update Each iteration updates the map with new measurement 𝑝 𝑚 𝑥 𝑡 , 𝑧 𝑡 , 𝑢𝑡 ∝ 𝑝 𝑚 𝑥 𝑡−1 , 𝑧 𝑡−1 𝑝 𝑧 𝑡 𝑚, 𝑥 𝑡 , 𝑧 𝑡−1 Posterior Prior Sensor Model Case 1. First insertion to the map : in 3D Covariance matrix Σijt v1 v2 s2 0 0 v1T T 2 v3 0 t 0 v2 2 T 0 0 t v3 s2 : Uncertainty of sensor ranges “thickness” t2 : Variance in both tangent directions 8 Map Update Each iteration updates the map with new measurement 𝑝 𝑚 𝑥 𝑡 , 𝑧 𝑡 , 𝑢𝑡 ∝ 𝑝 𝑚 𝑥 𝑡−1 , 𝑧 𝑡−1 𝑝 𝑧 𝑡 𝑚, 𝑥 𝑡 , 𝑧 𝑡−1 Posterior Prior Sensor Model Case 2. Update a beacon by SOG - Only the importance weight of individual Gaussians are modified - When weights become insignificant, some SOG modes are discarded Observed range Importance weight Standard EKF 9 Observation Model Sensor model Integral over all the potential beacon position pdfs Eventually, most of Gaussians will have negligible weights 10 Observation Model 2D planar surface Initial state, t1 Robot moves, t2 Symmetry quickly disappears Gaussian modes with lowest weight in the SOG are discarded with time Robot moves further, t3 Robot deviates from straight path, t4 11 Experiment simulations Monte-Carlo (MC) vs Sum of Gaussians (SOG) Monte-Carlo SOG (this work) 12 Experiment simulations Monte-Carlo (MC)：15 beacons, Time : 24.211 sec 13 Experiment simulations Sum of Gaussians (SOG)：15 beacons, Time : 11.148 sec 14 Thank you