Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation
description
Transcript of Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation
![Page 1: Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation](https://reader035.fdocuments.us/reader035/viewer/2022062501/568167d3550346895ddd25da/html5/thumbnails/1.jpg)
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
![Page 2: Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation](https://reader035.fdocuments.us/reader035/viewer/2022062501/568167d3550346895ddd25da/html5/thumbnails/2.jpg)
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]
![Page 3: Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation](https://reader035.fdocuments.us/reader035/viewer/2022062501/568167d3550346895ddd25da/html5/thumbnails/3.jpg)
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
![Page 4: Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation](https://reader035.fdocuments.us/reader035/viewer/2022062501/568167d3550346895ddd25da/html5/thumbnails/4.jpg)
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
![Page 5: Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation](https://reader035.fdocuments.us/reader035/viewer/2022062501/568167d3550346895ddd25da/html5/thumbnails/5.jpg)
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
![Page 6: Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation](https://reader035.fdocuments.us/reader035/viewer/2022062501/568167d3550346895ddd25da/html5/thumbnails/6.jpg)
3) Correspondence Error = cijk
Estimate bounds of cijk from the geometry
of the boundary and robot poses
•Assume uniform distribution
Max error
where
![Page 7: Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation](https://reader035.fdocuments.us/reader035/viewer/2022062501/568167d3550346895ddd25da/html5/thumbnails/7.jpg)
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
![Page 8: Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation](https://reader035.fdocuments.us/reader035/viewer/2022062501/568167d3550346895ddd25da/html5/thumbnails/8.jpg)
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
![Page 9: Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation](https://reader035.fdocuments.us/reader035/viewer/2022062501/568167d3550346895ddd25da/html5/thumbnails/9.jpg)
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
![Page 10: Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation](https://reader035.fdocuments.us/reader035/viewer/2022062501/568167d3550346895ddd25da/html5/thumbnails/10.jpg)
Unweighted Weighted
![Page 11: Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation](https://reader035.fdocuments.us/reader035/viewer/2022062501/568167d3550346895ddd25da/html5/thumbnails/11.jpg)
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
![Page 12: Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation](https://reader035.fdocuments.us/reader035/viewer/2022062501/568167d3550346895ddd25da/html5/thumbnails/12.jpg)
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
![Page 13: Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation](https://reader035.fdocuments.us/reader035/viewer/2022062501/568167d3550346895ddd25da/html5/thumbnails/13.jpg)
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
![Page 14: Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation](https://reader035.fdocuments.us/reader035/viewer/2022062501/568167d3550346895ddd25da/html5/thumbnails/14.jpg)
Uncertainty From Sensor Noiseand Correspondence Error
1 mx500