Jose-Luis Blanco , Javier González, Juan-Antonio Fernández-Madrigal

1. 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 May 19-23 Pasadena, CA (USA)

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

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

4. 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.

5. Two likely positions Robot poses 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.

6. 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].

7. Alternative implementation in this work: Rao-Blackwellized Particle Filter (RBPF) 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.

8. 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!

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

10. Beacon 1 Beacon 2 Robot path Robot path 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.

11. 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.

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

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

14. Robot path Robot path 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:

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

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

17. 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.

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

19. 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

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

21. 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

22. 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

23. 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]

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

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

26. 4.1. Experiments: UWB radio beacons

27. 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

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

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

30. 4.2. Experiments: synthetic data

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

32. Future work: • Explore more efficient representations of pdfs. 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.

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

34. 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 Thanks for your attention!