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

Mobile Robot Localization Proprioceptive Sensors: ( Encoders, IMU ) - Odometry, Dead reckoning Exteroceptive Sensors: ( Laser, Camera ) - Global, Local Correlation Scan-Matching Scan 1Scan 2 Iterate Displacement Estimate Initial Guess Point Correspondence 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]

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

Weighted Formulation Error between k th scan point pair Measured range data from poses i and j sensor noise Goal: Estimate displacement (p ij,  ij ) bias true range = rotation of  ij Correspondence Error Noise Error Bias Error

LikLik  ll 1)Sensor Noise Covariance of Error Estimate Covariance of error between k th scan point pair = 2)Sensor Bias neglect for now see paper for details Pose i Correspondence Sensor Noise Bias

3)Correspondence Error = c ij k Estimate bounds of c ij k from the geometry of the boundary and robot poses Assume uniform distribution Max error where

Finding incidence angles  i k and  j k 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 Scan Points Fit Lines ikik

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

Experimental Results Increased robustness to inaccurate initial displacement guesses Fewer iterations for convergence 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

Unweighted Weighted

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

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

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

Uncertainty From Sensor Noise and Correspondence Error 1 m x500