Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation

1. Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation Sam Pfister, Kristo Kriechbaum, Stergios Roumeliotis, Joel Burdick Mechanical Engineering, California Institute of Technology Overview: • Motivation • Problem Formulation • Experimental Results • Conclusion, Future Work

2. Initial Guess Scan 1 Scan 2 Point Correspondence Scan-Matching Iterate Displacement Estimate • Mobile Robot Localization • Proprioceptive Sensors: (Encoders, IMU) - Odometry, Dead reckoning • Exteroceptive Sensors: (Laser, Camera) - Global, Local Correlation • Scan-Matching • Correlate range measurements to estimate displacement • Can improve (or even replace) odometry – Roumeliotis TAI-14 • Previous Work - Vision community and Lu & Milios [97]

3. 1 m Correspondence Errors x500 Weighted Approach • Explicit models of uncertainty & noise sources for each scan point: • Sensor noise & errors • Range noise • Angular uncertainty • Bias • Point correspondence uncertainty Combined Uncertanties • Improvement vs. unweighted method: • More accurate displacement estimate • More realistic covariance estimate • Increased robustness to initial conditions • Improved convergence

4. Weighted Formulation Goal: Estimate displacement (pij ,fij ) Measured range data from poses i and j sensor noise true range bias Error between kth scan point pair = rotation of fij Correspondence Error Bias Error Noise Error

5. Correspondence Bias Sensor Noise sq sl Lik Covariance of Error Estimate Covariance of error between kth scan point pair = • Sensor Noise Pose i • Sensor Bias • neglect for now see paper for details

6. Correspondence Error =cijk • Estimate bounds of cijk from the geometry • of the boundary and robot poses Max error • Assume uniform distribution where

7. Scan Points Fit Lines Finding incidence angles aik and ajk Hough Transform -Fits lines to range data -Local incidence angle estimated from line tangent and scan angle -Common technique in vision community (Duda & Hart [72]) -Can be extended to fit simple curves aik

8. Maximum Likelihood Estimation Likelihood of obtaining errors {eijk} given displacement Non-linear Optimization Problem • Position displacement estimate obtained in closed form • Orientation estimate found using 1-D numerical • optimization, or series expansion approximation methods

9. Experimental Results Weighted vs. Unweighted matching of two poses • 512 trials with different initial displacements within : • +/- 15 degrees of actual angular displacement • +/- 150 mm of actual spatial displacement Initial Displacements Unweighted Estimates Weighted Estimates • Increased robustness to inaccurate initial displacement guesses • Fewer iterations for convergence

10. Unweighted Weighted

11. Eight-step, 22 meter path • Displacement estimate errors at end of path • Odometry = 950mm • Unweighted = 490mm • Weighted = 120mm • More accurate covariance estimate • Improved knowledge of • measurement uncertainty • - Better fusion with other sensors

12. Conclusions and Future Work • Developed general approach to incorporate uncertainty into scan-match displacement estimates. • range sensor error models • novel correspondence error modeling • Method can likely be extended to other range sensors (stereo cameras, radar, ultrasound, etc.) • requires some specific sensor error models • Showed that accurate error modelling can significantly improve displacement and covariance estimates as well as robustness • Future Work: • Weighted correspondence for 3D feature matching

13. Conclusions and Future Work • Developed general approach to incorporate uncertainty into scan-match displacement estimates. • range sensor error models • novel correspondence error modeling • Method can likely be extended to other range sensors (stereo cameras, radar, ultrasound, etc.) • requires some specific sensor error models • Showed that accurate error modelling can significantly improve displacement and covariance estimates as well as robustness • Future Work: • Weighted correspondence for 3D feature matching

14. and Correspondence Error 1 m x500 Uncertainty From Sensor Noise