“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