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

• 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.

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

2/32

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.

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

3/32

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.

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

4/32

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(

n 2

) 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.

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

5/32

Polar Scan Matching

• PSM has these steps: – Scan Preprocessing – Scan Projection – Translation Estimation – Orientation Estimation – Error Covariance Estimation Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

6/32

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 Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

7/32

• 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.

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

8/32

Raw Scan Example

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

9/32

Preprocessed Scan

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

10/32

Scan Projection

• Transform

current scan

into reference scan frame.

Current scan transformed To reference frame Reference scan origin Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

11/32

Scan Projection (cont’d)

Ranges at reference scan positions are calculated by linear interpolation in polar coords.

Shown as Points are

occluded

by nearer scan points.

Occlusion handled by over-writing further ranges.

Ranges are tagged

invisible

if in opposite order Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

12/32

Translation Estimation

• Aim is to find new that minimises

Sigmoid weight that favours small errors [Dudek2000] reference current projected

• Association of ranges trivial – just use the bearing.

• Linearized regression used over a few iterations – see paper for details.

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

13/32

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.

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

14/32

Error behaviour – simulated data

Scan initial mismatch Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

15/32

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.

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

16/32

Corridor Drift

• Corridor environments tend to cause biased matches: Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

17/32

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° Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

18/32

Ground truth experiments

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

19/32

Ground truth matches: PSM

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

20/32

Ground truth matches: ICP

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

21/32

PSM Error Behaviour

cm deg

X error o Y error Angle error + Iteration number msec

Note corridor scene fails to converge

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

22/32

ICP Error behaviour

cm deg

X error o Y error Angle error + Iteration number msec

Note corridor scene fails to converge

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

23/32

Regions of Convergence

o = no convergence

o

= incorrect convergence point

+ = true shift point + +

PSM ICP Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

24/32

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 m 2 – ICP converged from 2.80 m 2 Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

25/32

SLAM Implementation

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

26/32

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.

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

27/32

Polar Scan Matching SLAM

10 meters Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

28/32

Iterative Closest Point (ICP) SLAM 10 meters Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

29/32

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.

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

30/32

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.

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

31/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.

Diosi and Kleeman,

Laser Scan Matching in Polar Coordinates with Application to SLAM

32/32