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