Seminar Dept. Automação e Sistemas - UFSC Scan-to-Map Matching Using the Hausdorff Distance for Robust Mobile Robot Localization Work presented at ICRA 2008, jointly with ANDRES GUESALAGA PUC Chile Miguel Torres-Torriti DEPT. DE INGENIERIA ELECTRICA PONTIFICIA UNIVERSIDAD CATÓLICA DE CHILE ESCUELA DE INGENIERIA 2011.03.24 Objective Solve the localization task despite clutter in a nonpolygonal dynamic environment using a range finder.? Localization involves: Solving a correspondence problem in which measurements in the robot s local coordinate system must be matched to map elements in global coordinates. Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 31
Motivation Self-localization is an essential task for autonomous navigation. Limitations of common localization approaches: Feature-based matching approaches assume structured environments and polygonal models, e.g. line-to-line via Split-and-Merge. Point-to-point matching approaches are more susceptible to misalignments due to occlusions. These methods often seek the alignment of consecutive scans to solve a robot tracking problem and thus are less suitable for global localization. Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 32 Geo-referrenced Satellite Image and Radar Scan Concepción Bay, 36 42 S - 73 05 W Satellite Image (reference) Landsat, resoultion 15 m UTM Zone 18H Radar Scan (measurement) Range resolution 7.5 m Bearing resolution 0.5º Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 33
Matching the Measurment to the Reference Before Matching After Matching Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 34 General Localization Scheme Odometry IMU/Compass Internal States Position Prediction (Motion Model) Pose Estimate Position Update (eg. EKF Estimation) Range Finder Raw or Interpreted Sensor Data (Perception) Predicted Position Scan Matching Position/Heading Observation (inferred from matching) External States Environment Description Maps DB Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 35
Shape Recognition Using the Hausdorff Distance Minimize the dissimilarity (largest deviations) between the reference set A (model) and the measurement set B (scan). Translate, rotate and scale B until the best match with respect to A is found. largest deviation = greatest distance between closest points Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 36 Hausdorff Distance (HD) The Hausdorff distance between to sets of points: is defined as where h A, B def ka bk a A b B is the directed Hausdorff distance. Thus the Hausdorff distance is the greatest distance between closest points from A to B and viceversa. Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 37
Hausdorff Distance (HD) Step 1: The Hausdorff Step 2: distance ensures that every point in ψ will be at most at a distance ψ(ψ,ψ ) from set ψ. A In order words, ψ(ψ,ψ ) yields a measure of the largest deviation of set ψ from ψ. All point in ψ are at most a distance ψ(ψ,ψ ) from ψ. Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 38 B Averaged Partial Hausdorff Distances Define the mapping that returns the distance from a point closest point in some set as: to the The partial HD of the best matching points in the measurements set to the model set can then be defined recursively for as: and with initial values: Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 39
Averaged Partial Hausdorff Distances Note that: Hence, measurements in within a distance from. The average of partial Hausdorff distances, also called modified Hausdorff distance (despite not being formally a distance), is simply defined as: Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 40 Robustness of the Approach Reference map Ladar distance measurements Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 41
Final Match Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 42 Distance Transform Map (L1-norm) Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 43
Distance Transform Map (L1-norm) Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 44 Robustness of the Approach 50 100 Ladar distance measurements 150 y [pixels] 200 250 300 Reference map 350 400 450 500 100 200 300 400 500 x [p ixels] Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 45
Final Match 50 100 150 200 y [pixels] 250 300 350 400 450 500 100 200 300 400 500 x [p ixels ] Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 46 Simulation Results: Matching Accuracy 1. Low final matching error when percentage of spurious/noisy measurements is below the specified threshold (30%). 2. Averaged partial HD increases proportionally to the number of noisy samples and the magnitude of the noise. The matching approach is robust to spurious measurements! Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 47
Averaged Partial Hausdorff Distances vs. Iteration Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 48 Partial Hausdorff Distances Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 49
Motion Model The state space model can be stated as: where s are the global position coordinate, are the right/left wheel velocities, is the heading angle wrt the -axis, is the range bias, are zero-mean, i.i.d., Gaussian disturbances. Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 50 Observation Model The observation model (output of the matching process) is given by: where s are assumed to be zero-mean, i.i.d., Gaussian noises. Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 51
ActivMedia Pioneer 3-AT Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 52 ActivMedia Pioneer 3-AT Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 53
Initial Measurement Hall Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 54 Final Matching Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 55
Estimated Trajectory Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 56 Estimated Position Error Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 57
Estimated Heading Error Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 58 Performance Results (using Sick PLS-101) Position Accuracy < Sensor Resolution (7 cm) Precision < Map Resolution (15 cm) Computation Time: In Matlab: 30 s on first iteration,.1 s on following iterations. Computational complexity for samples: where is the the largest axis of the reference map. Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 59
Conclusions The proposed approach for scan-to-map matching is very accurate and precise thanks to its robustness to occlusions or unspecified environment elements. Accuracy is mostly limited by the resolution of the rasterized maps. Precision is affected by the amount of occlusions and objects that do not appear in the reference map, as well as the number K of samples used in the modified HD. Accurate estimates of the robot s position, heading and velocity can be obtained in real-time. Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 60 Ongoing Research Improving the scan-matching technique to reduce the computation time by introducing multi-scale techniques. Developing methods to adjust the MHD threshold dynamically. Extending the approach to MCL in order to improve the robustness under multiple matching solutions. Extending the technique to solve the SLAM problem. Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 61
Range Finder Measurements Model Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 62 Range Finder Measurments Model The range finder measurement model is given by: where: Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 63
HD-computed and Filtered Trajectory The data corresponds to a sequence of 54 radar scans taken over a time interval of 135 seconds from a patrol boat in the Concepcion Bay, Chile (36 42 S - 73 05 W) sailing East (90 heading) with diminishing speed from 12 to 2 knots. -100-150 computed trajectory filtered trajectory metres -200-250 -300 100 200 300 400 500 600 metres Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 64 Estimated Heading 95 90 85 degrees 80 75 70 Estimated heading Gyroscope datum 65 0 10 20 30 40 50 60 scan # Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 65
Estimated Velocity 12 knots 11 10 9 8 7 6 5 4 3 2 Estimated speed Log datum 0 10 20 30 40 50 60 scan # Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 66 Estimated Range Bias 400 380 filtered range bias computed range bias metres 360 340 320 300 0 10 20 30 40 50 60 scan # Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 67
Estimated Bearing Bias -1,0 filtered angular bias -1,5 computed angular bias degrees -2,0-2,5-3,0 0 10 20 30 40 50 60 scan # Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 68