Seminar Dept. Automação e Sistemas - UFSC Scan-to-Map Matching Using the Hausdorff Distance for Robust Mobile Robot Localization

Transcription

1 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 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

2 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, S 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

3 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

4 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

5 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

6 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

7 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

8 Distance Transform Map (L1-norm) Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 44 Robustness of the Approach Ladar distance measurements 150 y [pixels] Reference map x [p ixels] Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 45

9 Final Match y [pixels] 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

10 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

11 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

12 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

13 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

14 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

15 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

16 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

17 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

18 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 W) sailing East (90 heading) with diminishing speed from 12 to 2 knots computed trajectory filtered trajectory metres metres Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 64 Estimated Heading degrees Estimated heading Gyroscope datum scan # Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 65

19 Estimated Velocity 12 knots Estimated speed Log datum scan # Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 66 Estimated Range Bias filtered range bias computed range bias metres scan # Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 67

20 Estimated Bearing Bias -1,0 filtered angular bias -1,5 computed angular bias degrees -2,0-2,5-3, scan # Seminar on Visual Servoing UFSC 2011 M. Torres-Torriti 68