Laser Scan Matching in Polar Coordinates with Application to SLAM

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 RA 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 gt 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 peoples 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 gt 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
Scan Projection (contd)
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 lt 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
PSM Error Behaviour
X error o Y error Angle error
Note corridor scene fails to converge
23
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
Polar Scan Matching SLAM
29
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.