1 / 32

Laser Scan Matching in Polar Coordinates with Application to SLAM

Laser Scan Matching in Polar Coordinates with Application to SLAM. Albert Diosi Lindsay Kleeman ARC Centre for Perceptive and Intelligent Machines in Complex Environments Dept. of Electrical and Computer Systems Engineering Monash University, Australia www.pimce.edu.au. Overview.

inari
Download Presentation

Laser Scan Matching in Polar Coordinates with Application to SLAM

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript


  1. Laser Scan Matching in Polar Coordinates with Application to SLAM Albert Diosi Lindsay Kleeman ARC Centre for Perceptive and Intelligent Machines in Complex Environments Dept. of Electrical and Computer Systems Engineering Monash University, Australia www.pimce.edu.au

  2. Overview • New 2D laser scan match method presented. • Polar Scan Matching (PSM) – point to point scan match approach in polar coords. • Avoids searching for point correspondences by simply matching bearing. • Faster than Iterated Closest Point (ICP) method. • SLAM implementation demonstrated with PSM.

  3. Previous Work • Scan matching can be categorized by the association method: • Feature to feature, • eg line segments [Gutman PhD 2000] or range extrema [Lingemann et al IROS 2004] • Point to feature. • Eg [Cox IEEE R&A 1991] • Point to point. • Least dependent on environment and chosen here.

  4. Point to Point Literature • Iterative Closest Point (ICP) • [Besl and McKay IEEE PAMI 1992] • Iterative Matching Range Point (IMRP) • [Lu and Milios JIRS 1997] • Iterative Dual Correpondence (IDC) • Uses IMRP for rotation and ICP for translation in each iteration [Lu and Milios JIRS 1997]. • One iteration required if associations known. • Requires 15-20 iterations in practice since correct associations are initially unknown.

  5. Associations are the Key! • IDC needs to associate each point in the new scan with one in the reference scan. • Each point can require all points in other scan to be checked => O(n2) computation. • In practice some search restrictions can reduce this computation. • Polar Scan Matching (PSM) in this paper just needs to check the same angle of a transformed scan.

  6. Polar Scan Matching • PSM has these steps: • Scan Preprocessing • Scan Projection • Translation Estimation • Orientation Estimation • Error Covariance Estimation

  7. PSM Preprocessing • Points not suitable for matching are removed by median filtering: • Chair and people’s legs • Mixed pixels caused by range discontinuities • Maximum range measurements (ie no object) removed by thresholding

  8. Segmentation of objects • Interpolation between distinct objects avoided • Possibility of tracking and deleting moving objects between successive scans. • New segment when consecutive difference > 20 cm • Same segment when 3 measurements on nearly same polar line.

  9. Raw Scan Example

  10. Preprocessed Scan

  11. Scan Projection • Transform current scan into reference scan frame. Current scan transformed To reference frame Reference scan origin

  12. Points are occluded by nearerscan points. Ranges at reference scan positions are calculated by linear interpolation in polar coords.Shown as Occlusion handled by over-writing further ranges. Ranges are tagged invisible if in opposite order Scan Projection (cont’d)

  13. Translation Estimation • Aim is to find new that minimises • Association of ranges trivial – just use the bearing. • Linearized regression used over a few iterations – see paper for details. Sigmoid weight that favourssmall errors [Dudek2000] current projected reference

  14. Orientation Estimation • Change of orientation is simply a shift in polar coords. • The shift with min absolute average range error chosen. • Parabolic interpolation applied to find sub angle sample estimate using 3 samples centred on min.

  15. Error behaviour – simulated data Scan initial mismatch

  16. Error Estimation • If correct associations are known, error covariance can be estimated from the linear regression analysis. • In practice this covariance is optimistic since inevitable errors of association and moving objects cause larger errors. • Heuristic diagonal covariance used based on average range residual except in “corridors”. • Corridors are identified by the variance of orientations of lines segments < threshold. • Non-diagonal covariance used for corridors.

  17. Corridor Drift • Corridor environments tend to cause biased matches:

  18. Ground Truth Experiments • Various positions and orientations of a laser template were marked on a sheet. • Measurements from known relative positions of the sheet were taken of different environments using Sick LMS 200 • Displacements of 80 cm and up to 27°

  19. Ground truth experiments

  20. Ground truth matches: PSM

  21. Ground truth matches: ICP

  22. cm deg Iteration number msec PSM Error Behaviour X error o Y error Angle error + Note corridor scene fails to converge

  23. cm deg Iteration number msec ICP Error behaviour X error o Y error Angle error + Note corridor scene fails to converge

  24. Regions of Convergence o = no convergence o = incorrect convergence point + = true shift point + + PSM ICP

  25. Areas of Convergence • Averaged over 10 scenes with initial 12º offset • Convergence defined as within 10 cm and 2º of true position • PSM converged from 2.99 m2 • ICP converged from 2.80 m2

  26. SLAM Implementation

  27. SLAM • EKF based SLAM implemented similar to [Bosse,Newman,Leonard &Teller IJRR2004] • Laser scans are used as landmarks. • New scans stored every 1 m of robot and SLAM updated every 1 m or 15º • Scan matching acts as a landmark measurement. • Odometry used over short distances.

  28. 10 meters Polar Scan Matching SLAM

  29. 10 meters Iterative Closest Point (ICP) SLAM

  30. Polar Scan Matching SLAM Video (2 min) • Average scan match time during SLAM on a Celeron 900 MHz laptop was: • ICP: 65 matches successful at 12.6 msec each. • PSM: 100 matches successful at 3.1 msec each.

  31. Conclusions and Future Work • Presented a new scan match method that works in native Polar coordinates called Polar Scan Matching – code will be made available on www – see paper. • Removal of occluding points simply done based on further range. • PSM ~4 times faster than ICP. • PSM SLAM successfully demonstrated.

  32. Future Work • Comparison of PSM with IDC and other scan matching approaches. • Tracking and removal of moving segments to improve robustness. • Funding of the ARC Centre for Perceptive and Intelligent Machines in Complex Environments is acknowledged.

More Related