1

Feature-Based Laser Scan Matching For Accurate

and High Speed Mobile Robot Localization
A. A. Aghamohammadi, H. D. Taghirad, A. H. Tamjidi, and E. Mihankhah
Advanced Robotics and Automated Systems (ARAS),
Department of Electrical Engineering,
K. N. Toosi University of Technology

AbstractThis paper introduces an accurate and high speed

pose tracking method for mobile robots based on matching of
extracted features from consecutive scans. The feature extraction
algorithm proposed in this paper uses a global information of the
whole scan data and local information around feature points.
Uncertainty of each feature is represented using covariance
matrices determined due to observation and quantization error.
Taking into account each feature's uncertainty in pose shift
calculation leads to an accurate estimation of robot pose.
Experiments with low range URG_X002 laser range scanner
illustrate the effectiveness of the proposed method for mobile
robot localization.
Index Termsautonomous mobile robot, localization, laser
scan matching, feature uncertainty

I. INTRODUCTION

and accurate localization algorithm is one of the most

important requirements in mobile robotics. Consequently,
the subjects of localization and mapping have justifiably
received considerable attention in the last 15 years, [1-4].
Standard methods often use odometric sensors for relative
localization which is also known as dead reckoning. It is well
known that the errors due to dead reckoning are accumulating
over time, and one possible method to assist with localization
is to use two-dimensional range finders such as laser range
finders (LRF) [5], or ring of ultrasonic range sensors [6].
However, using laser range finders is more accurate compared
to ultrasonic range sensors.
Usually, two consecutive laser scans are matched to obtain
an estimate of the relative displacement of the robot. Scan
matching algorithms can be categorized based on their
association method, i.e. point to point or feature to feature. In
point-wise scan matching algorithms, which are implemented
in many cases to solve the matching problem, usually two
scans are compared directly. Different routines are developed
to use point to point matching approaches such as the
iterative closet point (ICP), iterative matching range point
(IMPR) [7], and the popular iterative dual correspondence
(IDC) [7]. Pfister et al. proposed recently an interesting new
approach to point-wise scan matching [8]. This algorithm
weights the contribution of each scan point to the overall
matching error according to its uncertainty. Although their
implementation results are impressive, its main drawback is its
high computation cost. Moreover, in matching algorithms it is
AST

assumed that corresponding points of two scans captured from

two different poses of robot are successfully matched. This
assumption is generally not true and especially in a point-wise
scan matching algorithm, this is an optimistic assumption.
SLIP method [9] uses a probabilistic distance metric to robust
rejection
of
outliers
and
establishes
point-wise
correspondences. Metric-based ICP (MbICP) [10] presents a
metric-based matching algorithm by point-wise matching
dense range scans. It uses geometric distance that takes into
account the translation and rotation of a robot simultaneously.
Histogram based approaches, e.g. [11], use a special
representation of the scanned data for matching of two
consecutive scans. These algorithms are not robust with
respect to the sensor noise and they require high computation
cost. Some other approaches are also proposed for matching
which utilize either active or passive artificial landmarks [12].
Above mentioned algorithms are subjected to high
computation cost and imprecision, particularly in case of selflocalization for high speed robot in large environments. Due
to the iterative calculations required to obtain optimal scan
matching in these algorithms, computational complexity are
high, and in order of O(nk), in which k >1 and n is the number
of scan points. In point-wise approaches, n is approximately
two orders of magnitude more than that of feature-based
methods.
Recently Lingemann et al. [13] proposed a promising
algorithm (HAYAI) to solve self-localization problem of high
speed robots. Although this method is a fast and feature based
method for scan matching, it suffers from the lack of
satisfying robustness property of feature extraction and it is
well-suited for high range sensors. In this paper the idea of
this approach is borrowed for the scan matching and similar
arguments are given regarding the computation cost and
precision of feature based scan matching. However, by
introducing appropriate algorithm for extracting features, and
modeling existence features' uncertainty, the robustness of the
method is improved significantly, even for low range sensors.
The main idea behind the proposed algorithm in this paper is
to address the issue of feature matching extracted from two
consecutive scans, such that they can be integrated based on
their covariance, in order to form accurate displacement
estimation. Analyzing different sources of errors which affect
displacement estimation process, appropriate uncertainty

2
models is developed for each feature extracted from the scan.
Incorporating these models in displacement estimation, results
in more accurate estimations. This paper is organized as
follows: section 2 describes the feature extraction and
matching algorithms in detail. Section 3 aims to calculate
optimal pose shift of robot between two consecutive scans
using matched feature pairs and their covariance. Section 4
demonstrates the experimental results and the concluding
results are given in last section.
II. FEATURE EXTRACTION AND MATCHING
Some techniques of data analysis are applied to the scan S,
provided by the scanner, in order to extract features from the
scan. Desired features are divided into two types: features
corresponding to the jump-edges and those corresponding to
the corners detected in the scan. Formal notation for the scan S
is given by:
x
S = ( p i )i =1,...,n , pi = i
yi
In order to detect jump-edges in scan S , scan points are
divided into some clusters consisting of consecutive scan
points in their natural order. As a result, every cluster features
a dedicated start point (pi) and an end point (pj), in which, i
and j are indices of points in whole scan S, and also i < j .
Therefore, kth cluster is defined by:
ck = { pm pm S , i m j }
Clustering procedure is as following: p1 c 1 and for all points
in scan like pi (i = 1,..., n) the distance di = pi +1 pi
calculated, then
p i +1 c k
p i +1 c k +1

if
if

is

( p i c k , d i d th )
( p i c k , d i > d th )

In which, dth is the maximum distance between two

consecutive points within a cluster. The start and end point of
each cluster are good candidates for being a feature. On the
other hand all features have to be invariant with respect to
robots displacement. Thus, only invariant points are reliable
for being selected as a feature and thus must be chosen. There
are two cases in which one can see variation in end-cluster
features:

Fig. 1. Lines in black represent the environment, blue and red lines are the
acquired scans from pose i and j, respectively. Upper feature is varying
with respect to robots displacement, but the other feature is an invariant
one.

angle difference between two clusters is more than cth , no

occlusion has happened. cth is often equal to the sensors
angular resolution.
Case 2) When the start or the end of a cluster established due
to sensors low range and not to a real feature in the
environment. As illustrated in Fig. 2 low range sensors are
subjected to this problem more than high range sensors. So if
pi is the start or end of a cluster one can say:

if ri > crs

pi is not a feature

otherwise

pi is a feature

In which rs is the maximum range of laser scanner sensor.

c = 0.9 can be a reasonable value.
Second class of features corresponds to the corners within
the clusters. In [13] filters with small lengths are introduced to
operate on the sequence of distance values (ri) of points in the
scan. Because of their small length, this method suffers from
the lack of global sight on the scan data and some global
information is missed. For example it may choose some
features on flat walls due to scan noise. Therefore, in this
paper it is proposed that for detecting corner features, at first a
line fitting algorithm is applied as a global filter. In the
literature several approaches are developed to solve the
problem of extracting line segments from a sequence of
points, with different speed and accuracy. Reference [14] has
compared the state-of-art methods for line extracting, and

Case 1) Clusters correlated to objects partially occluded by the

other object. The start or the end point of a cluster, which is
established by occlusion, is a growing (variant) point with
respect to robot's displacement, and is not a good feature as
illustrated in Fig 1. Therefore, the feature selection can be
found by the following routine, given pi the start or end point
of cluster ck,

if i i1 < cth and ri > ri1 and pi is start of ck pi isnotafeature

if i+1 i < cth and ri > ri+1 and pi is end of ck pi isnotafeature

otherwise pi isafeature
Where i = tan 1 ( yi / xi ) and ri = xi2 + yi2

represent pi in

polar coordinates. According to the above conditions, if the

Fig. 2. End cluster feature, established due to sensor low range, is variant
with respect to robots displacement.

reported that for real-time applications, split-and-merge

3
algorithm is the best choice by its fair correctness and superior
speed. Thus, split-and-merge algorithm is used in here for line
fitting within each cluster. Due to the primary clustering,
merging stage is often not required, and if required it is very
fast.
The parameter tuple specifying a line reads as:
l q = q , nq , lenq , where, q is the angle between line and x

axis; nq is the number of associated scan points in line lq , and

lenq is the length of lq . If intersection of two successive lines,

lines established this corner. Moreover, each start(end)-cluster

feature is specified by point pcc of scan and pre ( next ) which
is the angle of line connecting to that.
After extracting features from two consecutive scans,
matching algorithm, based on a dissimilarity function, has to
be calculated. Owing to the fact that features topology can
not change fundamentally from one scan to the other, below
dissimilarity function between p i and p i , two features from
two consecutive scans, is introduced as:
d ( p i , p j ) = p i p j + B
2

If next next
j or prei pre j (which one exists) exceeds
i
a maximum threshold, the pi and p j are not matched, and B
becomes infinity, otherwise B = 0 .
After dissimilarity matrix is constructed, at each step the
minimum value element of this matrix is chosen, its
corresponding row and column are omitted and correlated
features are matched, until all rows and columns of
dissimilarity matrix are omitted or the minimum value of
elements exceeds a maximum matching distance.
III. POSE SHIFT CALCULATION
Fig. 3. Blue dots represent a part of scan around pcc. pi and pj are scan
points, which have distance r from pcc and pcc1 is the point produces the
maximum perpendicular distance from the line connecting pi to pj.

l q and l q +1 , satisfies two following conditions, pcc , which is

the end point of l q , is introduced as a candidate corner feature.

Condition1) | q +1 q | exceeds the minimum threshold th .
Condition2) For both l q and l q +1 , either lenq > lenth or n > nth .
lenth is the minimum acceptable length and nth is the

This section aims to find proper uncertainty model for each

of extracted features. Uncertainty of each feature has two
major causes, rooted in physical properties of laser range
finder: measurement process noise and quantized nature of
rays' angles. First phenomenon that affects range sensing is
measurement process noise in each scan point, p i S , which
is calculated as follows:
e ob = p i p i

find the nearest point to exact corner feature.

First, two points p i l q and p j l q +1 are selected which
have distance r to pcc (see figure 3). r is proportional to
lenq and lenq +1 . p i and p j are connected by a straight line
and for all scan points, pm , lying between pi and p j ,
i < m < j , distance d ,m to this line is calculated. The scan

point, produces the maximum distance, is named p cc . If

2
pcc2 = pcc , procedure is terminated and pcc is selected as a

corner feature. If pcc pcc , then pcc = pcc , r is decreased

2
2
and the above procedure is recursively called until pcc = pcc .
2
Finally, each corner feature is specified by [ p cc , pre , next ] ,
in which pcc is the feature point in scan and cc is its index in
the whole scan. pre and next are angles of two consecutive

cos( i + e )
p i n = ri + e ri

sin(i + e )
cos(i )
(2)
p i = ri

sin(i )
is a noisy measurement of point pi. e is a zero mean

in which,

minimum acceptable number of points of lines intersected at

this corner. pcc may be not the exact corner point. Thus, using
local information around pcc , the following algorithm tries to

(1)

pin

Gaussian noise with small standard deviation, represents the

i'th direction's angle uncertainty. e r is a noise added to true
i

range of p i . It comprises of two terms, er = ari + b , in which

i

ri is the measured range of pi , a is a zero mean Gaussian

random variable and b is the representation of a bias noise
exists in range measurement process, which modeled by an
additive Gaussian noise with mean value equals to b0 . These

values can be obtained by statistical analysis of measurement

data. Approximating cos(e ) = 1 , sin(e ) = e , e er 0 yields:
cos( i ) e sin(i )
p i n = (ri + ari + b )

(3)
sin( i ) + e cos( i )
sin(i )
cos(i )
eob = ri e

+ (ari + b )
cos(

)
i

sin( i )
Second source of error in considering a scan point as a

4
feature, is a quantized nature of laser scanner rays' angles.
This issue causes that the point pi, considered as a feature
point, not necessarily be the same physical feature in the
environment. Particularly, when incidence angle between laser
scanner's i'th ray and surface of object related to pi is near to
zero or 180 degrees, this error is much considerable.
Quantization error for start(end)-cluster features is collinear
with the boundary tangent of the object [8], on which pi is
selected (see fig. 4).

Figure 4 illustrates that q d is the maximum value of and

can be calculated by the formula:
sin( )
(9)
qd =
ri
sin( i +1 )
In which is the separation angle of the laser scanner's rays
and i +1 is the incidence angle between fitted line to scan
points at pi and (i+1)'th ray of LRF. Due to the unknown
shape of objects in the environment, it is reasonable to assume
that is a random variable with uniform distribution, so:
G
G qd G
(10)
E (eq ) = E ( t i ) = E ( ) t i =
ti
2
After considering fk as the k'th feature, its error covariance
i

should be calculated as:

f k = fk + eobi + eq i

(11)

Where

eobi = eobi E (eobi )

(12)

eqi = e qi E (e qi )
Assuming that ~
eobi and ~
eqi are independent:

 i ) (13)
Cov (f k ) = E (f k fk )T (f k fk ) = Cov (eobi ) + Cov (eq

Again assuming that e , a and b are independent equation8

p i is an end-cluster point, and f k is a real feature in the environment.
is the incidence angle between ray i + 1 and direction of wall, on which

Fig. 4.

i +1

p i lies.

G
eq = t

(4)

G
In which t is a unit vector collinear with fitted line to scan
points around pi and is a random variable represents

quantization error's quantity. For an end-cluster feature,

T
t = [ cos( )
sin( ) ] and for a start cluster feature,
t = [ cos( )

T
sin( ) ] , where:

= atan 2 ( p p , p p
y
e

y
s

x
e

x
s

y
s

x
e

sin 2 ( )
-sin( ) cos( )

i
i
i
Cov (e
)= r 2

ob
i
2
i
-sin( ) cos( )

cos ( )
i
i
i

cos 2 ( )

sin( ) cos( )

i
i
i
+ a2 r 2 + b2

i
2
sin( ) cos( )
cos ( )
i
i
i

(14)

in which,

is the standard deviation of the theta

measurement noise, a that for the range measurement noise,

).

(5)

p s = [ p p ] and pe = [ p p ] are leading and ending

points of the fitted line around pi . Thus if f k is the true
x
s

results into,

y
e

position of the k'th feature in the environment in robot's

coordinate and pi is the selected point of the scan as the end
of a cluster, we have:
f k = pi + eobi + eqi

(6)

Accounting for the fact that the expected value of the

modeled errors are nonzero, considered position for the k'th
feature, p k , should differ from pi
(7)
fk = E (f k ) = p i + E (eob ) + E (e q )
Where E () is the expected value operator and:
sin( i )
cos( i )
E (e ob ) = ri E (e )
+ ( E (a ) ri + E (b ) )
=

cos(
)
i

sin( i )
cos( i )
cos( i )
E (b )
= b0

sin(
)
i

sin( i )

(8)

and b is the bias standard deviation. The covariance of the

quantization error can be calculated as follows:
G q d G G q d G T
i t i i t i i t i i t i =
2
2
(15)

2
2

qd G G
q d cos 2 ( i )
cos( i ) sin( i )
E i i t i t i T = i

2
3 cos( i ) sin( i )
sin 2 ( i )

Cov e
q
i

= E

Using feature measurements and their corresponding

covariance, the algorithm aims to calculate robot's
displacement between scans. Displacement considered here, is
composed of a translation, T, followed by a rotation, R. To
find optimal T and R, the following error function should be
minimized [16].
m

E = f knew ( R f k pre + T )
k =1

C f1 f knew ( R f k pre + T )
k

(16)

5
fkpre and fknew are the k'th pair of matched features and m is the

number of such pairs. C represents the amount of error that

f
k

uncertainty of innovation vector, fknew (Rfkpre +T ) , imposes

to the pose shift calculation. Assuming that errors in two
different scans are independent, we have,
(17)
C f = Cov eob + eq + RCov eob + eq R T
k

By substitution of Eq. 17 into Eq. 16 we reach to an error

function that should be minimized. Sequential quadratic
programming (SQP) method is used to solve the above
nonlinear optimization. Since in feature-based approach the
number of points taken into account in optimization procedure
is often reduced to 0.01 of number of points used in point
based approach, SQP method produces an accurate solution
for R and T in a small portion of the computing time.
IV. EXPERIMENTS
Proposed method was implemented on the Melon robot,
which is equipped with a Hokuyo URG_X002 Laser range
scanner. The maximum measurable distance of this scanner is
4095mm. It's angular resolution per step is (180/512)=0.3515
degrees. Noise/maximum-range ratio in these laser scanners,
are considerably more than high range sensors. In the
experiments the following values are used by statistical
analysis of measurement data: a = 5 103 , b = 1mm and
= 10 3 degree . All reported graphs are scaled in centimeters.

Fig. 4. Melon, the mobile robot equipped with low range laser scanner

Fig. 5 shows the extracted features of 74th scan along with

corresponding ellipsoids. The ellipsoids are illustrated by a
scaling factor of 10, indicating the 99 percent confidence
region of each features covariance. There are two ellipsoids
calculated in each scan for every feature, which are illustrated
in two colors. The observation uncertainty ellipsoid (OUE)
represents the measurement noise covariance of each feature,
while the whole uncertainty ellipsoid (WUE) represents the
whole covariance of the feature, consisting of the observation
and quantization errors. The relative size of these ellipsoids
reveals the contribution of the different sources of errors in
the pose shift estimation. As it is seen in this figure the
extracted features correspond to the real features in the
environment. The growing start(end)-cluster points are
effectively excluded from the selected features, and no
features on flat walls are selected. Matching of the extracted

features from scan numbers 74 and 75 is shown in Fig. 6. As it

is shown in this figure, the extracted features, which are
shown by stars, are well matched and related to each other
with connecting lines.
400
OUE

350
300

WUE
250
200
150
100
50
0
-200

-150

-100

-50

50

100

150

Fig. 5. Extracted features from scan number 74. Ellipsoids in magenta

represent the measurement covariance (OUE) of each feature and cyan
ellipsoids (WUE) related to whole covariance of each feature.

Figure 7 shows the map constructed by superimposing 193

scans gagged during drive in an L shape corridor of our lab. It
should be mentioned that the only sensor used for localization
is laser range scanner and none of the map improving
algorithms such as global relaxation [16] is considered in this
experiment. For the sake of comparison, we also implemented
the HAYAI method and the same map produced with this
method. Due to feature extraction and matching methods in
HAYAI, this method uses some filters. In the case of high
length filters, information relative to the details in scan is
omitted, so only if there are features correspond to big jumpedges or features correspond to sharp corners with high length
lines in the scan, it has a chance to choose appropriate
features. However, even in the presence of such features, due
to high length filtering, position of scan points varies and error
between selected point as feature and exact position of feature
and features uncertainty will increase. After all, in low range
LRFs probability of such features existence is considerably
low. Therefore, small length filters should be used, (which is
the case in [13]) but this case suffers from the lack of robust
feature extraction and inaccurate localization in noisy scans.
Besides, weakness of this method to block variant features,
and discrimination between effects of features with high and
low uncertainty in localization are other reason for the
inaccuracy.
As it is shown in figure 7, although there are small details
in the environment, map is relatively well extracted by the
proposed method. This is mainly due to the structure of the
proposed method, in which basically no data reduction
filtering is used, and therefore, the small size walls are well
extracted in the map. In addition, omitting variant features and
optimally incorporating features regarding their uncertainty
lead to accurate superimposing raw scans data. The cost of the
significant improvement in the accuracy is mainly the relative
computation time. On a 1.1 GHZ processor, Processing 193
scans by our method needed 5.67 seconds which is
approximately five times of that for HAYAI procedure.
However, the amount of time required is still much lower

6
compared to that of histogram based or point-based method
(like IDC) and the algorithm is fast enough for real-time
localization in high speed robots. A comparison between
HAYAI and other algorithms speed is reported in [13].

400

V. CONCLUSIONS

250

350

300

connecting lines

This paper describes a feature based matching algorithm in

which features are incorporated in robot pose calculation,
based on their uncertainty. Total uncertainty of each
individual feature is represented by calculating the
observation and quantization covariance. Optimal pose
displacement estimation is calculated by minimizing the
objective function derived from maximum likelihood concept.
The feature extraction algorithm proposed in this paper uses a
global filtering of the data, using a line fitting algorithm, in
which a split-and-merge method is used. In order to accurately
estimate the features, local information is used after global
filtering stage. Matching of the features for consecutive scans
is accomplished by analyzing the parameters specify each
feature. The method is implemented on a mobile robot
equipped with small range URG_X002 laser range scanner,
and the experimental results verifies the effectiveness of the
proposed method in feature extraction, mapping, and robot
localization.

200

150

100

50

0
-200

-150

-100

-50

50

100

150

Fig. 6. Reference scan is shown by '' and the new scan is shown by '+'.
Matched features are specified by red stars and connected to each other.

REFERENCES
[1]

R. C. Smith and P. Cheeseman, .On the representation and estimation of

spatial uncertainty, Int. J. of Robotics Research, vol. 5, no. 4, pp. 56.68,
1986.
[2] S. Thrun, D. Fox, and W. Burgard, A Probabilistic Approach to
Concurrent Mapping and Localization for Mobile Robots, Machine
Learning, vol. 31, pp. 2953, 1998.
[3] S.I. Roumeliotis and G.A. Bekey, Bayesian estimation and Kalman
filtering: A unified framework for Mobile Robot Localization, in Proc.
IEEE Int. Conf. on Robotics and Automation, San Fransisco, CA, April
24-28 2000, pp. 29852992.
[4] J. Leonard and H. Durrant-Whyte, Mobile robot localization by tracking
geometry beacons, IEEE Trans. on Robotics and Automation, vol. 7, no.
3, pp. 376382, June 1991.
[5] M. Amann, T. Bosch, M. Lescure, R. Myllyla, and M. Rioux, .Laser
Ranging: A critical review of usual techniques for distance
measurement, Opt. Eng., vol. 40, no. 1, pp. 10.19, Jan. 2001.
[6] J. Crowley, .World modeling and position estimation for a mobile robot
using ultrasonic ranging,. in Proc. IEEE Int. Conf. on Robotics and
Automation, 1989, pp. 674.680.
[7] F. Lu and E. Milios, Robot Pose Estimation in Unknown Environments
by Matching 2D Range Scans, J. of Intelligent and Robotic Systems, vol.
20, pp. 249275, 1997.
[8] S. Pfister, K. Kriechbaum, S. Roumeliotis, J. Burdick, Weighted range
sensor matching algorithms for mobile robot displacement estimation,
in: Proceedings of the IEEE International Conference on Robotics and
Automation (ICRA02), 2002, pp. 6671674.
[9] AJensen, B.; Siegwart, R. "Scan alignment with probabilistic distance
metric", in Proceedings of the IEEE/RSJ International Conference on
Intelligent Robots and Systems, 2004.
[10] J. Minguez, F. Lamiraux, and L. Montesano, "Metric-based scan
matching algorithms for mobile robot displacement estimation," in
Proceedings of the IEEE International Conference on Robotics and
Automation (ICRA), Barcelona, Spain, 2005.
[11] J. H. Moon, B. J. You, H. Kim, S. R. Oh, Fast Markov localization using
angle-histogram, in: Proceedings of the 11th IEEE International
Conference on Advanced Robotics (ICAR03), Coimbra, Portugal, 2003,
pp. 411416.
[12] J.V. Bitterling, S. Kupper, B. Mertsching, Incremental feature-based
pose tracking for a mobile robot, in: Proceedings of the 11th IEEE

Fig. 7. Constructed map using proposed method

Fig. 8. Constructed map using HAYAI method

[13]

[14]

[15]

[16]

International Conference on Advanced Robotics (ICAR03), Coimbra,

Portugal, 2003, pp. 10381043.
K. Lingemann, H. Surmann, A. Nuchter, and J. Hertzberg, High-speed
laser localization for mobile robots, Journal of Robotics and
Autonomous Systems, vol. 51, no. 4, pp. 275296, 2005.
V. Nguyen, A. Martinelli, N. Tomatis, and R. Siegwart, A Comparison
of Line Extraction Algorithms using 2D Laser Rangefinder for Indoor
Mobile Robotics, in Proc. 2005 IEEE Int. Conf. on Intelligent Robots
and Systems, 2005.
Coleman, T.F. and Y. Li, An Interior, Trust Region Approach for
Nonlinear Minimization Subject to Bounds, SIAM Journal on
Optimization, Vol. 6, pp. 418-445, 1996.
F. Lu, E. Milios, Globally consistent range scan alignment for
environment mapping, Autonomous. Robots 4 (1997) 333349.