140 likes | 291 Views
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
E N D
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
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]
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
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
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
Correspondence Error =cijk • Estimate bounds of cijk from the geometry • of the boundary and robot poses Max error • Assume uniform distribution where
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
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
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
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
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
and Correspondence Error 1 m x500 Uncertainty From Sensor Noise