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