Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation
description
Transcript of Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation
Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement EstimationSam 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 1 Scan 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
CombinedUncertanties
Weighted Formulation
Error between kth scan point pair
Measured range data from poses i and j
sensor noise
Goal: Estimate displacement (pij ,ij )
bias true range
= rotation of ij
Correspondence ErrorNoise Error Bias Error
Lik
l
1) Sensor Noise
Covariance of Error EstimateCovariance of error between kth scan point pair =
2) Sensor Bias neglect for now see paper for details
Pose i
CorrespondenceSensor Noise Bias
3) Correspondence Error = cijk
Estimate bounds of cijk from the geometry
of the boundary and robot poses
•Assume uniform distribution
Max error
where
Finding incidence angles ik 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 PointsFit Lines
ik
Likelihood of obtaining errors {ijk} 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 DisplacementsUnweighted EstimatesWeighted 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 WorkDeveloped 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 WorkDeveloped 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 Noiseand Correspondence Error
1 mx500