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