Jose-Luis Blanco, Javier González, Juan-Antonio Fernández-Madrigal Dpt. of System Engineering and Automation University of Málaga (Spain) A Pure Probabilistic Approach to Range-Only SLAM Jose-Luis Blanco, Javier González, Juan-Antonio Fernández-Madrigal May 19-23 Pasadena, CA (USA)

Outline of the talk 1. RO-SLAM: the RBPF approach 2. The observation model 3. Map update 4. Experiments 5. Conclusions

Outline of the talk 1. RO-SLAM: the RBPF approach 2. The observation model 3. Map update 4. Experiments 5. Conclusions

1. RO-SLAM: the RBPF approach Range Only (RO) SLAM: Localization & Mapping with range-only devices. Our purpose: To enable a vehicle to localize itself using RO devices, without any previous information about the location of the beacons. Typical technologies: Radio, sonars.

1. RO-SLAM: the RBPF approach Advantages of RO-SLAM (depending on technologies): No need for line-of-sight between vehicle-beacons. Artificial beacons, can identify themselves: no data-association problem. Drawback of RO-SLAM (always): The high ambiguity of all measurements. Robot poses Two likely positions

1. RO-SLAM: the RBPF approach Why is it difficult to integrate RO-SLAM in a probabilistic framework? Multi-modality: With RO sensors, everything is multimodal by nature: - In global localization  vehicle location hypotheses [not in this paper] - In SLAM  beacon location hypotheses [addressed here].

1. RO-SLAM: the RBPF approach Why is it difficult to integrate RO-SLAM in a probabilistic framework? Multi-modality: With RO sensors, everything is multimodal by nature: - In global localization  vehicle location hypotheses [not in this paper] - In SLAM  beacon location hypotheses. Strongly non-linear problem, with non-Gaussian densities. - Classic approach to SLAM (EKF) is inappropriate to RO-SLAM: a covariance matrix is incapable of capturing the relations between all the variables. Alternative implementation in this work: Rao-Blackwellized Particle Filter (RBPF)

1. RO-SLAM: the RBPF approach The Rao-Blackwellized Particle Filter (RBPF) approach The full SLAM posterior can be separated into: - Robot path: estimated by a set of particles. - The map: only conditional distributions, for each path hypothesis. The covariances are represented implicitly by the particles, rather than explicitly  easier!

1. RO-SLAM: the RBPF approach Taking advantage of conditional independences Instead of keeping the joint map posterior, we can estimate each beacon independently: Robot path Beacon 1 Beacon 2 Beacon 3 Robot path Beacon 1 Robot path Beacon 2 Robot path Beacon 3

1. RO-SLAM: the RBPF approach The key insight of our approach: Each beacon, at each particle, can be represented by the probability distribution that best fits the current uncertainty.  The first time a beacon is observed, it is inserted as a second particle filter.  Eventually, it can be approximated by a Gaussian, then it becomes a Gaussian and it is updated through an EKF. Robot path Beacon 1 Robot path Beacon 2 Robot path Beacon 3

1. RO-SLAM: the RBPF approach The benefits of our approach: New beacons can be inserted into the map at any time: they are immediately used to improve robot localization. Computational complexity dynamically adapts to the uncertainty. Unified Bayesian framework: it’s not a two-stage algorithm: [Kantor, Singh ICRA02], [Kurth, et al. 2003]: EKF, assuming initial gross estimate of beacons. [Singh, et al. ICRA03]: Delayed initialization of beacons. [Newman & Leonard ICRA03]: Least square, batch optimization. [Olson et al. 2004], [Djugash et al. ICRA06]: Two steps, first probability grid for beacons, then converge to EKF.

Outline of the talk 1. RO-SLAM: the RBPF approach 2. The observation model 3. Map update 4. Experiments 5. Conclusions

2. The observation model Sensor model: (optional) bias + additive Gaussian noise p(z) Bias z (sensed range) Actual range

2. The observation model Sensor model: Implementation depends on representation of beacons “m”. In general, it is the integral over all the potential beacon positions: Robot path Robot path

2. The observation model Comparison of both methods: Small uncertainty Observation likelihood (Monte Carlo) Observation likelihood (Gaussian) Beacon position uncertainty -0.5 0.5 1 1.5 -1 Real robot location Beacon position samples and Gaussian fit. -1 1 Real robot location -1 1 Real robot location Small uncertainty Gaussian is acceptable -0.5 0.5 1 1.5 -1 Real robot location -0.5 0.5 1 1.5 -1 Real robot location Large uncertainty Must use samples

Outline of the talk 1. RO-SLAM: the RBPF approach 2. The observation model 3. Map update 4. Experiments 5. Conclusions

3. Map update With each iteration, new measurements are integrated into the map: We can find three situations to implement this: - The beacon is inserted into the map for the first time. - The beacon is represented by samples. - The beacon is represented by a Gaussian.

3. Map update Case 1: First insertion into the map Equally weighted particles are generated along a “thick ring” centered at the sensor: Radius: sensed range Sigma: sensor noise

3. Map update Case 2: Update of a beacon represented by samples Only the importance weights of samples are modified: One sample of the beacon density

3. Map update Case 2: Update of a beacon represented by samples Only the importance weights of samples are modified: Keys for efficiency: One sample of the beacon density  Remove samples with very small weights.  Eventually, a Gaussian becomes a good approximation. Simple & fast criterion: check for maximum size of the Gaussian fit of the samples.

The Gaussian represents 3. Map update Case 3: Update of a beacon represented by a Gaussian In this case, updating each beacon is done through a standard EKF. The Gaussian represents the beacon density

Outline of the talk 1. RO-SLAM: the RBPF approach 2. The observation model 3. Map update 4. Experiments 4.1. UWB radio beacons 4.2. Synthetic data 5. Conclusions

4.1. Experiments: UWB radio beacons Ultra Wide Band (UWB) technology: Measure time-of-flight of short radio pulses. Spread spectrum for robustness against multi-path. It does not require line-of-sight. We have used 3 beacons + 1 mobile transceiver on the robot. [Timedomain – PulsOn]

4.1. Experiments: UWB radio beacons The experimental setup: Beacon #3 Beacon #2 Beacon #1 Onboard UWB device

4.1. Experiments: UWB radio beacons Evolution of the estimated map: Beac. #2 Beac. #3 Beac. #1 1 m Time step 0 Beac. #2 Beac. #3 Beac. #1 Time step 15 Robot path Beac. #2 Beac. #3 Beac. #1 Time step 45

4.1. Experiments: UWB radio beacons

Outline of the talk 1. RO-SLAM: the RBPF approach 2. The observation model 3. Map update 4. Experiments 4.1. UWB radio beacons 4.2. Synthetic data 5. Conclusions

4.2. Experiments: synthetic data Simulation of a circular path while observing 15 beacons. Evolution of the estimated map Time step #0 2 m Time step #10 #8 #7 #6 #14 #3 #5 #4 #13 #1 #9 #10 Time step #30 Robot path #4 #8 #9 #1 #7 #6 #13 #14 #3 #5 #11 #12 #10 Time step #100 Robot path #4 #8 #9 #1 #7 #6 #13 #14 #3 #5 #11 #12 #10 #0

4.2. Experiments: synthetic data Simulation of a circular path while observing 15 beacons. Beacon errors (m) 6 Added #11 Added #12 Added #0 Added #2 5 4 3 2 1 20 40 60 80 100 120 Time steps 160 Computation time (sec) 100 10 Average: 0.69 sec 1 0.1 0.01 20 40 60 80 100 120 Time steps 160

4.2. Experiments: synthetic data

Outline of the talk 1. RO-SLAM: the RBPF approach 2. The observation model 3. Map update 4. Experiments 5. Conclusions

5. Conclusions We have presented a consistent probabilistic framework for Bayesian RO-SLAM. New beacons can be added to the map at any time. The density representations adapt dynamically. Tested with real UWB sensors. Future work: Explore more efficient representations of pdfs.

Final remark… http://mrpt.sourceforge.net/ Source code (MRPT C++ libs), datasets, slides and instructions to reproduce the experiments available online: http://mrpt.sourceforge.net/ papers ICRA 08

Jose-Luis Blanco, Javier González, Juan-Antonio Fernández-Madrigal Dpt. of System Engineering and Automation University of Málaga (Spain) A Pure Probabilistic Approach to Range-Only SLAM Jose-Luis Blanco, Javier González, Juan-Antonio Fernández-Madrigal Thanks for your attention!