This article was downloaded by: [University of Kent]On: 10 November 2014, At: 14:01Publisher: Taylor & FrancisInforma Ltd Registered in England and Wales Registered Number: 1072954 Registered office: Mortimer House,37-41 Mortimer Street, London W1T 3JH, UK
Advanced RoboticsPublication details, including instructions for authors and subscription information:http://www.tandfonline.com/loi/tadr20
A scan restoration method for robust polar scanmatching in dynamic environmentsSeung-Hee Lee a , Heon-Cheol Lee & Beom-Hee Leea Department of Electrical and Computer Engineering , Seoul National University , 1Gwanak-ro, Gwanak-gu , Seoul , 151-742 , KoreaPublished online: 01 May 2013.
To cite this article: Seung-Hee Lee , Heon-Cheol Lee & Beom-Hee Lee (2013) A scan restoration method for robust polar scanmatching in dynamic environments, Advanced Robotics, 27:11, 877-891, DOI: 10.1080/01691864.2013.791741
To link to this article: http://dx.doi.org/10.1080/01691864.2013.791741
PLEASE SCROLL DOWN FOR ARTICLE
Taylor & Francis makes every effort to ensure the accuracy of all the information (the “Content”) containedin the publications on our platform. However, Taylor & Francis, our agents, and our licensors make norepresentations or warranties whatsoever as to the accuracy, completeness, or suitability for any purpose of theContent. Any opinions and views expressed in this publication are the opinions and views of the authors, andare not the views of or endorsed by Taylor & Francis. The accuracy of the Content should not be relied upon andshould be independently verified with primary sources of information. Taylor and Francis shall not be liable forany losses, actions, claims, proceedings, demands, costs, expenses, damages, and other liabilities whatsoeveror howsoever caused arising directly or indirectly in connection with, in relation to or arising out of the use ofthe Content.
This article may be used for research, teaching, and private study purposes. Any substantial or systematicreproduction, redistribution, reselling, loan, sub-licensing, systematic supply, or distribution in anyform to anyone is expressly forbidden. Terms & Conditions of access and use can be found at http://www.tandfonline.com/page/terms-and-conditions
FULL PAPER
A scan restoration method for robust polar scan matching in dynamic environments
Seung-Hee Lee, Heon-Cheol Lee* and Beom-Hee Lee
Department of Electrical and Computer Engineering, Seoul National University, 1 Gwanak-ro, Gwanak-gu, Seoul 151-742, Korea
(Received 13 June 2012; accepted 18 August 2012)
This paper addresses the problem of scan matching which is highly indispensable for mobile robot systems based onrange sensors. Recently, polar scan matching (PSM) has been used in solving the problem because it is accurate and fastenough to be performed in real time. However, the performance of PSM degenerates when the portion of scan data fromdynamic objects is excessively large. This paper proposes a scan restoration method to overcome this problem andimprove the performance of PSM in dynamic environments. The proposed method restores the scan data from dynamicobjects to appropriate scan data from static objects. First, whole scan data is segmented and classified as static anddynamic objects. Next, curvature functions are extracted from the classified segments and smoothed by interpolating thesegments indicating dynamic obstacles. PSM with the proposed method was tested and evaluated in various real dynamicenvironments, which reveals that the proposed method can improve the performance of PSM in dynamic environments.
Keywords: polar scan matching; curvature function; interpolation; scan restoration method
1. Introduction
Laser scan matching is a key technique for the localiza-tion of mobile robots by matching two laser scan dataobtained from a mobile robot with a laser range finder(LRF). The basic concept of laser scan matchinginvolves estimating the differences in mobile robot posesbetween two times when two laser scan data wereobtained. There are three categories of laser scan match-ing techniques according to the concept of data associa-tion. First, point-to-point matching is a methodology thatfinds corresponding points between two laser scan dataand matches scan data with these points. One such tech-nique is called iterative closest point (ICP),[1] whichiteratively finds corresponding points of laser scan databy searching the minimum Euclidean distance betweenthe scan points of another laser scan data. Similarly, Luet al. [2] suggested another way called iterative matchingrange point (IMRP) that finds the corresponding pointsby introducing a matching range based on the LaserRange Finder (LRF) coordinate system at a certain time.They also proposed iterative dual correspondence (IDC)which simultaneously uses both ICP and IMRP. Otherscan matching techniques are based on point-to-featurematching. Here the features indicate lines, curves, orrange extrema that are extracted from scan data. Cox [3]proposed one of the point-to-feature scan matching tech-niques that extracts features by calculating the Gaussianmean and variance of each scan point that belongs to the
grid map segments. Cox then matched the scan pointswith the extracted features. Finally, a feature-to-featurematching technique matches the features extracted fromtwo scan data.[4] Weiss et al. [5] proposed the histo-grams of scan data as the features. They suggested away to match two scan data by calculating the phase thatmaximizes a cross-correlation between two histograms.
Generally, the conventional point-to-feature or fea-ture-to-feature laser scan matching techniques are notappropriate to be used in real-time localization of amobile robot because they have to compare all of thepre-learned features with the scan data in every matchingprocess, causing a heavy computational load. Point-to-point scan matching techniques should be used in thiscase. Diosi et al. [6,7] proposed another point-to-pointscan matching technique, polar scan matching (PSM),which matches the range values of LRF bearingsbetween two scan data in the polar coordinate systeminstead of finding corresponding scan points in the Carte-sian coordinate system. PSM projects one scan data toanother scan data and resamples the projected scan datato rearrange the scan points in the same LRF bearings.The computational load of PSM is much less than thatof ICP or IDC. Also, as previously shown,[6,7] PSMcan reject occlusions by segmenting the scan data.
Although PSM can perform scan matching despiteocclusion, it fails to perform matching when there aremore than two dynamic obstacles, a problem that causes
*Corresponding author. Email: [email protected]
Advanced Robotics, 2013Vol. 27, No. 11, 877–891, http://dx.doi.org/10.1080/01691864.2013.791741
� 2013 Taylor & Francis and The Robotics Society of Japan
Dow
nloa
ded
by [
Uni
vers
ity o
f K
ent]
at 1
4:01
10
Nov
embe
r 20
14
the limitation of robots not being able to appropriatelyperform tasks in dynamic environments. In the future,such robots will coexist with people and other robots, sothe robotics technologies that are working robustly indynamic environments should be developed. This paperproposes an approach that can overcome this limitation,called the scan restoration method, which makes scanpoints that indicate dynamic obstacles or occlusions tobe restored to nearby static obstacles like walls or corri-dors. This method segments the scan data not just bycalculating the Euclidean distance between two scanpoints suggested in PSM but by considering the nearbyenvironmental structure and noise of the LRF using anadaptive breakpoint detector (ABD).[8] After segmenta-tion, restoration targets are chosen by some standards forselection, and curvature functions of scan data are thenextracted based on the Cartesian coordinate system.[9]Curvature functions can be considered as scan data fea-tures, and Nunez et al. [10] suggested a feature-to-featurescan matching algorithm using curvature functions asfeatures. In the proposed method, the curvature values ofthe restoration target are interpolated using nearby curva-ture values of two non-restoration targets. All segmentsof the scan data indicating dynamic obstacles arerestored to static environments using the above pro-cesses, and PSM can perform scan matching robustly inthe dynamic environment using these restored scan data.The main idea of the scan restoration method was origi-nated by Gil et al. [11], who suggested a way to restorethe original shape of a noisy object using curvatureflows. Similarly, scan restoration method considers thelaser scan data as objects that must be restored, andregards dynamic obstacles or occlusions as the noisesources that scatter the original data. Experiments invarious real dynamic environments have shown that theproposed method restored the scan points, indicating sta-tic environments that were hidden by dynamic obstacles,and PSM using these restored scan data estimated thepose difference correctly. Also, the proposed method wasverified as being much faster than the PSM scan match-ing processes.
This paper is organized as follows. The erroneousscan matching result of PSM in the dynamic environmentis described with an example in Section 2. In Section 3,the way to implement the proposed method is summa-rized. The experimental results are shown in Section 4.And finally, the conclusions are presented in Section 5.
2. Problem description
This section describes the problem of PSM, which failsto perform scan matching in dynamic environments. Anexample of a situation that a mobile robot moves in adynamic environment and the corresponding two laserscan data are shown in Figure 1(a) and (b), respectively.
When dynamic obstacles exist in the same environment,corridors are hidden by them and do not stand in thesame position in the two scan data. Due to theocclusions, PSM fails to match the current scan C intothe reference scan R as shown in Figure 1(c). Thisfailure is caused in the process of the pose estimation ofPSM, and the analysis of this problem is describedbelow. Notations used in PSM are summarized inTable 1.
First, C is projected into the frame of R. The polarcoordinates of the i-th scan point in the projected currentscan C0 are computed as follows:
r0C;i ¼ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi(rC;i cos (hC þ /C;i)þ xC)
2 þ (rC;i sin (hC þ /C;i)þ yC)2
q:
(1)
/0C;i ¼ a tan2(rC;i sin (hC þ /C;i)þ yC; rC;i
cos (hC þ /C;i)þ xC) ð2Þ
Then, the projected current scan C0 is resampled bythe linear interpolation in the polar coordinate system toalign /R;i and /0C;i. For the resampled current scan
C00 ¼ f(r00C;i;/00C;i)ji ¼ 1; � � � ;Ng where N is the number
of current scan points, the translation estimation can beperformed by
r00C;1 � rR;1
..
.
r00C;N � rR;N
264
375 ¼ H
DxCDyC
� �þ v (3)
where v is a noise vector, (DxC;DyC) is the estimatedpose difference between the times corresponding R andC. The Jacobian matrix H is computed as follows:
H ¼
@r00c;1@xc
@r00c;1@yc
..
. ...
@r00c;N@xc
@r00c;N@yc
2664
3775 �
cos/r;1 sin/r;1
..
. ...
cos/r;N sin/r;N
264
375 (4)
If moving obstacles stand at different locations intwo times, the error of range values indicating the samespots in real environments becomes serious regardless ofthe robot movement. For this reason, projection via (1),(2), or estimation via (4) cannot derive an accurateresult. Similarly, the orientation estimation is done byshifting C00 to the left or right side in the polar coordi-nate system to find the shift angle value that creates aminimum distribution error between R and C00. In thesame manner explained above, if R and C00 arecontaminated by the moving obstacles, a correct shiftvalue cannot be derived because the local minimum isoccurred despite of many iteration steps.[12]
878 S.-H. Lee et al.
Dow
nloa
ded
by [
Uni
vers
ity o
f K
ent]
at 1
4:01
10
Nov
embe
r 20
14
There are several approaches to solve these problems,most of which suggest completely new scan matchingalgorithms or get help from other sensors. Zhao et al.[13] proposed the Global Positioning System (GPS) asan assistant sensor to improve localization accuracy.They also suggested the classification method of the scandata segment, in which the segments are sorted intomoving, static, new, or seed states, and updated themusing a Bayesian filter each step. Rizzini et al. [14]adopted the Adaboost algorithm to locate human data inthe scan data using 14 predefined features. All segmentsare iteratively compared to predefined features andhuman-like segments are rejected from scan matchingprocess. Lingemann et al. [15] and Kim et al. [16]suggested new scan matching algorithms, HAYAIand CVFSAC, respectively, which can perform scanmatching in the dynamic environment.
In this paper, we suggest a scan restoration methodwhich can be easily applied to conventional scan match-ing algorithms by handling only raw laser scan data. Theproposed method restores the scan points indicatingdynamic obstacles as static environments according tothe following criteria. We defined a restoration targetindicating a dynamic object as the segment which has arelatively small portion of scan data compared to nearbysegments indicating static environments. Therefore, somestatic obstacles occupying small portions against theentire scan data can be also regarded as the restorationtargets. This is reasonable because both the segmentswhich indicate static and dynamic obstacle occupying
small portions may degenerate the performance of scanmatching algorithm. Although the proposed method isdescribed with PSM in this paper, we expect that theproposed method can be applied to not only PSM butalso other scan matching algorithms to improve therobustness of scan matching in dynamic environments.
3. Scan restoration method
A scan restoration method is a pre-processing scan datastep that should be performed before all scan matchingprocesses. The purpose of this method is to obtain therestored scan data which is refined data from the originalscan data in order to improve the performance of scanmatching. Firstly, the proposed method performs asegmentation process on both scans R and C to find therestoration targets indicating the dynamic obstacles.Then, the curvature functions for both scans areextracted. The curvature values corresponding to the scandata points of the restoration targets are interpolatedalong the curvature values of the non-restoration targets.This section describes the above processes of theproposed scan restoration method in detail. The notationsused for the description are summarized in Table 2.
3.1. Scan segmentation
In PSM, the scan data points are segmented based on theEuclidean distance between two scan points. Twoconsecutive scan points, ri�1 and ri, are separated intodifferent segments, if they meet the following condition:
(a) Dynamic environment (b) Laser scan data (c) Scan matching result
Figure 1. Scan matching thorough PSM in a dynamic environment. Grid size is 0.5m.
Table 1. Notations used in PSM.
Notation Meaning
N Total number of scan points in one scan data(rR;i;/R;i);
(rC;i;/C;i)
Polar coordinates of the i-th scan pointrespectively in R and C
(xR;i; yR;i);(xC;i; yC;i)
Cartesian coordinates of the i-th scan pointrespectively in R and C
(xR; yR; hR) Robot position and orientation at the timecorresponding R
(xC ; yC ; hC) Robot position and orientation at the timecorresponding C
Table 2. Notations and meanings used in the scan restorationmethod.
Notation Meaning
SR;i; SC;i The i-th segment in R and CKR; KC The total number of segments in R and ClR;i; lC;i The total number of scan points in the i-th
segment in R and CgR; gC Curvature functions of R and C#R; #C Gradients of curvature functions of R and C
Advanced Robotics 879
Dow
nloa
ded
by [
Uni
vers
ity o
f K
ent]
at 1
4:01
10
Nov
embe
r 20
14
kri � ri�1k � D (5)
where D is a predefined constant set to 0.1m. Otherwise,the two scan points belong to the same segment. Differ-ently from the segmentation process in PSM, the ABD[8] searches the breakpoints used as the boundaries ofeach segment. The basic concept of the ABD is shownin Figure 2. A virtual planar obstacle passing through(xi�1; yi�1) is drawn based on a predefined angle value kset to 10°. The hypothetical scan point (xhi ; y
hi )
corresponding to (xi; yi) is then set by finding the inter-secting point between the virtual planar obstacle and thethreshold circle, which has a radius of Dmax. The formu-las used to determine Dmax are as follows:
rhi ¼ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi(xhi )
2 þ (yhi )2
q(6)
ri�1 sin (k) ¼ rhi sin (k� D/) (7)
By the law of sine at the triangle in Figure 2 with(7),
krhi � ri�1k ¼ ri�1sin (D/)
sin (k� D/)(8)
Then, we can get an adaptive threshold value forsegmentation from (8) as follows:
Dmax ¼ ri�1sin (D/)
sin (k� D/)þ r (9)
where r represents the noise component related to theLRF noise. If (xi; yi) does not exist in the threshold circlethat has a radius Dmax from (9), we then set (xi�1; yi�1)and (xi; yi) as breakpoints. Segmentation is done usingthe next conditions:
(1) Scan points between two breakpoints belong to thesame segment.
(2) If three breakpoints appear consecutively, a break-point at the middle is considered as a distinguishedsegment that has only one scan point.
The comparison result of segmenting the samescan data between the conventional method and theABD is shown in Figure 3. As shown in figure, theconventional method is vulnerable to LRF noise. Forexample, the same corridor is divided into two seg-ments (S6; S7). Also, a scan point that is far removedfrom the other points is set to a distinguished seg-ment (S5). However, the ABD corrects these problems,and makes the segments reflect the real environmentwell.
As described above, the ABD can offset LRF noiseto some degree and make an identical object belong tothe same segment. Therefore, by using the ABD, we cancorrectly reflect nearby real environments and decreasethe dependence of environmental alteration. Because
Figure 2. Concept of the ABD. D/ represents the scanresolution of the LRF which was 0.01m. (xhi ; y
hi ) is the
hypothetical point of (xi; yi).
(a) Conventional method (b) Adaptive breakpoint detector (ABD)
Figure 3. Comparison between the conventional segmentation method and the ABD. Si and B represent the i-th segment and abreakpoint, respectively. The size of grids is 0.5m.
880 S.-H. Lee et al.
Dow
nloa
ded
by [
Uni
vers
ity o
f K
ent]
at 1
4:01
10
Nov
embe
r 20
14
of these advantages, the ABD is utilized in thesegmentation step of the scan restoration method.
3.2. Curvature function extraction
Curvature functions of the scan data are extracted as pre-viously suggested.[9] First, the maximum length vi ateach scan point (xi; yi) is calculated. The maximumlength indicates the number of scan points between(xi; yi) and another scan point (xiþvi�1; yiþvi�1) in whichthere is no occurrence of discontinuity between scanpoints. The maximum length is calculated following thenext condition:
vi ¼ argmaxq
Xqþi�1j¼ikrjþ1 � rjk\kriþq � rik þ U
!(10)
Here, U is a predefined threshold value that relievesthe LRF noise component. This value must be set withconsidering nearby environments and LRF performance.The maximum length of the (N� 1)th scan point is fixedat 1, and the maximum length of the Nth scan pointcannot be calculated. The concept of calculating themaximum length is shown in Figure 4. After calculatingall maximum lengths of the N� 1 scan points, we canobtain the curvature function gi as follows:
gi ¼ arctan
Piþvi�1j¼i (yjþ1 � yj)Piþvi�1j¼i (xjþ1 � xj)
!(11)
where gN ¼ gN�1. Figure 5 shows the results of extract-ing curvature function of scan data R and C, which areshown in Figure 1(b). As shown in figure, the gradientsof the curvature values corresponding to the boundary ofthe segments (or breakpoints detected using ABD) arelarger than non-boundary gradients. Therefore, thegradients are helpful for the segmentation of scan data.
The gradients of the curvature function are obtained asfollows:
#i ¼ giþ1 � gi (12)
The absolute value of the gradients of the curvaturefunctions extracted from the scan data are shown inFigure 6. The gradients of curvature function are used ascriteria to obtain the restoration targets. The detailexplanation is given in the following subsection.
3.3. Searching targets to be restored
The most important part of the scan restoration methodis to determine the restoration targets. The i-th segmentis determined as a restoration target if the number ofscan points, li, in the segment meets the followingcondition:
c1N � li � c2N (i ¼ 1; � � � ;K) (13)
where c1 and c2 are adjustable constants. This conditionmeans that if the number of scan points included in thesegment is lower than some portion of the whole scandata (c2N ), the segment is determined to be a restorationtarget. Also, we can avoid to select the segments withonly one or two scan point caused by the LRF noise byintroducing the minimum criteria (c1N ). The values, c1and c2, should be chosen by considering surroundingwhole environment and the size of the obstacles whichare the targets to be restored. The more detailed processto choose the constants is described in Appendix A.
However, this condition alone is not enough becauseboth conventional segmentation technique and the ABDcannot segment a wall including a door, as shown inFigure 7. This indicates that opening or closing doorscannot be selected as restoration targets. To overcomethis problem, the gradients of curvature function #i areutilized. As shown in Figure 8, if there are two scanpoints satisfying the following condition:
#i � p (14)
Then, the scan points between them are set asrestoration targets. p is a predefined constant which isempirically obtained by 30° for p. The searchingalgorithm to determine the restoration targets for thereference scan R is described in Table 3. This is equallyapplied to the current scan C.
3.4. Scan restoration through interpolation techniques
Finally, the restoration targets are restored to staticenvironments hidden by dynamic obstacles. The key ideaof the restoration is to use interpolation techniques in
Figure 4. Concept of the maximum length vi and curvaturevalue gi of scan point (xi; yi).
Advanced Robotics 881
Dow
nloa
ded
by [
Uni
vers
ity o
f K
ent]
at 1
4:01
10
Nov
embe
r 20
14
curvature functions or in the Cartesian coordinatesystem.[17] In this paper, four interpolation techniquesare introduced and explained in detail to show how theyare utilized in the scan restoration method. We define aninitial index of a scan point in the specific restorationtarget segment Sd (which has ld scan points), is, and aterminal index, ie respectively.
3.4.1. Linear interpolation in the curvature function
The basic concept of the linear interpolation technique isshown in Figure 9(a). A virtual straight line is drawnbetween (is � 1; gis�1) and (ie þ 1; gieþ1). Then, therestored curvature values gresj (j 2 ½is; ie�) are placed on
the virtual straight line. The value gres between two indi-ces, is and ie, is obtained as follows:
gresj ¼ gis�1 þ (gieþ1 � gis�1)f j� (is � 1)g
(ie þ 1)� (is � 1)(15)
After all of the curvature values of the targetsegments are interpolated by (14), the scan pointscorresponding to them are restored in the Cartesian coor-dinate system. However, there is a trivial problem in thatthe residual degree of freedom occurs when the scanpoints are being restored using only gres. As shown inFigure 9(b), there are several candidates for the restoredscan points because we have curvature only between(xresi�1; y
resi�1) and (xresi ; yresi ). To solve this problem, we used
the Euclidean distance between nearby two non-targetscan points as described in Figure 9(c). First, the Euclid-ean distance d between (xis�1; yis�1) and (xieþ1; yieþ1) iscalculated as follows:
(a) Curvature function of R (b) Curvature function of C
Figure 5. Curvature function gR and gC extracted from scan data in Figure 1.
(a) Gradients of curvature function of R (b) Gradients of curvature function of C
Figure 6. The gradients of the curvature functions, j#Rj and j#Cj.
Figure 7. Description of a problematic situation. Neither bothsegmentation methods can distinguish a movable door from awall. B indicates the breakpoints.
882 S.-H. Lee et al.
Dow
nloa
ded
by [
Uni
vers
ity o
f K
ent]
at 1
4:01
10
Nov
embe
r 20
14
d ¼ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi(xieþ1 � xis�1)
2 þ (yieþ1 � yis�1)2
q(16)
Then, d is divided by ld which is the number of therestored scan points. The divided value is considered asthe Euclidean distance between the consecutive points ofthe restored scan data in Cartesian coordinate system.Therefore, the x -coordinates and y -coordinates of therestored scan points are obtained as follows:
xresi ¼ xresi�1 þd
ld
cos (gresi�1) (17)
yresi ¼ yresi�1 þd
ld
sin (gresi�1) (18)
3.4.2. Nearest-neighbor interpolation in the curvaturefunction
The nearest-neighbor interpolation technique fixes gres
to a constant value that is the same as the nearbycurvature values of the non-restoration target gis�1
(a) Curvature function (b) Gradients of curvature function
Figure 8. The curvature function and the gradients of curvature function extracted from a situation described in Figure 7. Scan pointsbetween /j and /jþ5 correspond to the door.
Table 3. Searching algorithm to determine the restorationtargets.
Input: All Kr segments Sr;i and gradients of curvaturefunctions #r
Output: Restoration target segments
1: X FALSE2: for i ¼ 1 to Kr do3: if c1N 6 lR;i 6 c2N , then Si restoration target
segment4: else5: for j ¼Pi¼1
k¼1 lR;k þ 1 toPi
k¼1 lR;k6: if j#R;jjP p, then7: if X=TRUE, then, a ¼Pi�1
k¼1 lR;k þ j, X TRUE
8: else9: b ¼Pi�1
k¼1 lR;k þ j, X FALSE10: (xR;u; yR;u); u 2 fa; � � � ; bg restoration
target scan points11: end if12: end if13: end for14: end if15: end for
(a) Basic concept (b) Residual degree of freedom (c) Proposed solution
Figure 9. Linear interpolation technique. (a) The curvature values belonging to the restoration target segment Sd are replaced withvalues that are located on the straight line. (b) Residual degree of freedom may occur. (c) They can be restored from the inter-polated curvature function gres.
Advanced Robotics 883
Dow
nloa
ded
by [
Uni
vers
ity o
f K
ent]
at 1
4:01
10
Nov
embe
r 20
14
or gieþ1. The concept is shown in Figure 10(a). Asshown in the figure, the curvature values belonging tothe left half side of Sd are replaced with gis�1 andthose belonging to the right half side are replaced withgieþ1. Equations (15)–(17) are adapted here as well, toprevent the occurrence of residual degree of freedomwhen restoring the target scan points in the Cartesiancoordinate system.
3.4.3. Polynomial interpolation in the curvature function
The polynomial interpolation technique draws a virtualpolynomial curve using several curvature values near gisand gie . If we want to obtain n th degree polynomialcurve, we need nþ 1 curvature values and have tocalculate nþ 1 polynomial constants using the followingformula:
gresi ¼ anin þ an�1i
n�1 þ � � � þ a2i2 þ a1iþ a0 (19)
In this paper, interpolation using a third-degreepolynomial provides accuracy and speed. The concept ofpolynomial interpolation technique is shown in Figure 10(b). The restored curvature values are obtained by inser-tion of indices i into (18). To obtain the third-degreepolynomial, four curvature values – gis�2, gis�1, gieþ1,and gieþ2 – are employed as follows:
(is � 2)3 (is � 2)2 is � 2 1(is � 1)3 (is � 1)2 is � 1 1(ie þ 1)3 (ie þ 1)2 ie þ 1 1(ie þ 2)3 (ie þ 2)2 ie þ 2 1
2664
3775
a3a2a1a0
2664
3775
¼gis�2gis�1gieþ1gieþ2
2664
3775 (20)
The 4� 4 degree matrix located on the left side of(19) is called the Vandermonde matrix. This matrix isalways nonsingular so we can obtain polynomial con-stants ½ a3 a2 a1 a0 �T in (19) by calculating theinverse of the Vandermonde matrix.[18,19] Here too,equations (15)–(17) are used when the scan points arerestored in the Cartesian coordinate system.
3.4.4. Interpolation in the Cartesian coordinate system
Unlike three interpolation techniques using the curvaturefunctions described above, another technique directlyinterpolates the target scan points in the Cartesian coor-dinate system not using the curvature functions. Thisidea is described in Figure 10(c). The linear interpolationtechnique is directly adopted in the Cartesian coordinatesystem as follows:
xresj ¼ xis�1 þ (xieþ1 � xis�1)f j� (is � 1)g
(ie þ 1)� (is � 1)(21)
(a) Nearest-neighbor interpolation (b) Polynomial interpolation (c) Interpolation in Cartesian coordinates
Figure 10. The concepts of other interpolation techniques. (a) The curvature values belonging to the target segment Sd are replacedwith constant values located near is or ie. (b) The curvature values belonging to the target segment Sd are replaced with valuesplaced on the polynomial curve. Here n ¼ 3. (c) The target scan points are restored directly in the Cartesian coordinate systemnot using the curvature function.
Figure 11. Hardware system for experiments.
884 S.-H. Lee et al.
Dow
nloa
ded
by [
Uni
vers
ity o
f K
ent]
at 1
4:01
10
Nov
embe
r 20
14
yresj ¼ yis�1 þ (yieþ1 � yis�1)fj� (is � 1)g
(ie þ 1)� (is � 1)(22)
The scan restoration method described in this sectionis equally applied to R and C to obtain the restored scandata Rres and Cres. After the two restored scan data are
(a) The raw reference scan data R (b) The raw current scan data C
Figure 12. The raw scan data obtained in the dynamic environment.
(a) Linear interpolation technique (b) Nearest-neighbor interpolation technique
(c) Polynomial interpolation technique (d) Interpolation in the Cartesian coordinate
Figure 13. The restored scan data Rres and Cres obtained by the four interpolation techniques.
(a) PSM only (b) PSM using the proposed method
Figure 14. Comparison of the scan matching result between PSM only and PSM using the proposed method. Here, the linearinterpolation technique was used.
Advanced Robotics 885
Dow
nloa
ded
by [
Uni
vers
ity o
f K
ent]
at 1
4:01
10
Nov
embe
r 20
14
obtained, PSM is performed using these data. In the nextsection, we suggest the comparison scan matching resultsin a dynamic environment whether the scan restorationmethod is used or not.
4. Experimental results
In this section, the comparison of scan matching resultsbetween PSM only and PSM with scan restorationmethod is described. All experiments were conductedbased on the system as shown in Figure 11. The mobilerobot and the laser scanner were Pioneer-3DX andUTM-30LX, respectively. The used laptop computer has2.66GHz dual-core CPU and 2GB RAM. The tech-niques including the proposed scan restoration methodwere implemented in C++ language.
4.1. Scan matching results with scan restorationmethod
First, for the two raw scan data R and C as shown inFigure 12, the results of applying the proposed methodwith the four interpolation techniques are shown in Fig-ure 13, where S2 and S4 in R and C were the restorationtargets. Environments like walls or corners hidden bydynamic obstacles were restored to their original shapes.The restoration results were slightly different accordingto the interpolation techniques. Figure 14 describes thecomparison between PSM only and PSM using the scanrestoration method. As shown in the figure, PSM usingthe proposed method correctly estimated pose differencebetween two times, but PSM only could not. The com-parison of pose estimation errors and computation timesin PSM without and with the proposed method according
(a) Two obstacles are moving (b) A human is moving and a door is closing
(c) Two men are wandering (d) A door is opening
Figure 15. Four dynamic environments encountered by a moving mobile robot.
Table 4. Comparison result of pose estimation error between PSM only and PSM with scan restoration (SR) method. Also, thecomputation time of SR method and PSM are measured.
Interpolation technique
Pose estimation error
Computation time (ms)PSM without SR method PSM with SR method
Scan restoration PSMx (mm) y (mm) h (°) x (mm) y (mm) h (°)
Linear 4130 5560 17.12 38 163 0.09 0.03 3.48Nearest 60 135 0.08 0.03 3.43Polynomial 306 21 1.99 0.15 2.33Cartesian 401 196 1.99 0.04 2.32
886 S.-H. Lee et al.
Dow
nloa
ded
by [
Uni
vers
ity o
f K
ent]
at 1
4:01
10
Nov
embe
r 20
14
to the interpolation techniques is summarized in Table 4.The table shows that pose estimation errors were signifi-cantly reduced by the proposed method. Also, the tableshows that the proposed method required only 6.5% ofthe computation time required to PSM, which means thatthe proposed method is fast enough to be performed inreal time.
We conducted more experiments in various dynamicindoor environments as shown in Figure 15. Figure 16shows the raw scan data with erroneous scan matching
results, the restored scan data obtained by the proposedmethod with different interpolation techniques, and theimproved scan matching results with the restored scandata. Obviously, the scan restoration method improvedthe performance of scan matching.
The total comparison results of PSM only and PSMwith the proposed method are summarized in Table 5,which shows that the pose estimation errors were signifi-cantly reduced by the proposed method. Similarly to theresults in Table 4, the proposed method required only
(a) Scan data corresponding to Scene 1. Nearest-neighbor interpolation was used.
(b) Scan data corresponding to Scene 2. Interpolation in Cartesian coordinate system was used.
(c) Scan data corresponding to Scene 3. Interpolation in Cartesian coordinate system was used.
(d) Scan data corresponding to Scene 4. Linear interpolation was used.
Figure 16. Results in various dynamic environments. (Left) Raw scan data and the scan matching result by PSM only. (Middle) Theresult of scan restoration. (Right) The improved scan matching result by PSM with the scan restoration method. Grid size is 1m.
Advanced Robotics 887
Dow
nloa
ded
by [
Uni
vers
ity o
f K
ent]
at 1
4:01
10
Nov
embe
r 20
14
1.7% of the computation time required to PSM, whichmeans that the proposed method is fast enough to beperformed in real time. From the experimental results,we figured out that the shape of the obstaclecorresponding to the restoration target, linear (like door)or circular (like human leg), did not affect the
performance of the proposed method because of theinterpolation technique. The selection of interpolationtechniques did not affect significantly the performance ofthe proposed method. Besides, although the restored scandata did not perfectly exhibit the real static environment,PSM with the proposed method was performed well.
Table 5. Comparison between PSM only and PSM using scan restoration (SR) method in four situations. Also, the computation timeof SR method and PSM are measured.
Dynamic environments in Figure 15
Pose estimation error
Computation time (ms)PSM without SR method PSM with SR method
scan restoration PSMx (mm) y (mm) h (°) x (mm) y (mm) h (°)
(a) Scene 1 96 290 4.05 18 92 0.04 0.05 7.76(b) Scene 2 909 588 1.13 25 18 0.30 0.10 5.88(c) Scene 3 1005 161 0.98 75 28 0.03 0.09 6.52(d) Scene 4 1046 180 3.06 19 75 1.02 0.03 5.35
(a) Only PSM in Environment 1 (b) PSM with the proposed method in Environment 1
(c) Only PSM in Environment 2 (d) PSM with the proposed method in Environment 2
(e) Only PSM in Environment 3 (f) PSM with the proposed method in Environment 3
Figure 17. Comparison of the experimental results of SLAM using PSM only and PSM with the proposed method in three differentenvironments.
888 S.-H. Lee et al.
Dow
nloa
ded
by [
Uni
vers
ity o
f K
ent]
at 1
4:01
10
Nov
embe
r 20
14
4.2. Simultaneous localization and mapping with theproposed method
Through the above experimental results, the performanceof the proposed scan restoration method was verified byimproving the accuracy of scan matching. Moreover, theproposed scan restoration method was performed muchfaster than the original PSM. Therefore, our scan restora-tion method can be utilized in simultaneous localizationand mapping (SLAM) without considering the problemof computational load.
In Figure 17(a) and (b), Figure 17(c) and (d), andFigure 17(e) and (f), the experimental results of SLAMusing PSM only and PSM with the proposed methodwas performed in various dynamic environments whosesize were 35m� 18m, 40m� 20m, and 38m� 15m,respectively. The used SLAM algorithm technique wasbased on our previous works,[20–23] which were theimproved version of SLAM using particles filters. Theestimated trajectories of the robot using only PSM werecorrupted by the moving objects like moving people,opening or closing doors as shown in the left sides ofFigure 17. When the proposed method was applied toPSM, the accuracy of SLAM was significantly improvedas shown in the right sides of Figure 17. The linear inter-polation technique was used for the three cases.
5. Conclusions
Laser scan matching is important for a LRF-equippedmobile robot to localize its pose in unknown environ-ments. However, most conventional scan matching algo-rithms do not work well in dynamic environments. Inthis paper, a scan restoration method was proposed toovercome this problem in PSM. The proposed methodrestores the scan points indicating the original staticenvironments hidden by moving obstacles through sev-eral interpolation techniques within the curvature func-tion of scan data or directly in the Cartesian coordinatesystem. To implement the proposed method, we sug-gested a searching algorithm for the restoration targetsand the extraction method of the curvature function. Wealso provided a solution to overcome the occurrence ofresidual degree of freedom problem.
The performance of the proposed method was veri-fied by real experiments. The restoration results wereslightly different according to the interpolation tech-niques but showed equally good performance. The com-putational load of the proposed method was low ascompared with PSM and the entire SLAM process. Thepose estimation errors of PSM with the proposed methodwere much smaller than those of the original PSM indynamic environments. In future work, the research onthe more robust interpolation techniques will be con-ducted because the performance of the proposed methodmay degenerate in excessively cluttered environments.
AcknowledgmentsThis work was supported in part by a Korea Science andEngineering Foundation (KOSEF) NRL Program grant fundedby a Korean government (MEST) (No.R0A-2008-000-20,004-0), in part by the BK21 Project, in part by ASRI, and in partby the Industrial Foundation Technology Development Programof MKE/KEIT [Development of CIRT (Collective IntelligenceRobot Technologies)].
Notes on contributors
Seung-Hee Lee received the BS and MSdegrees in Electrical Engineering andComputer Science from Seoul NationalUniversity, South Korea, in 2010 and 2012,respectively. Now, he works for R&Ddepartment in Microinspection� andresearches the effective way for detectingelectrical defect in Liquid Crystal Display.
Heon-Cheol Lee received the BS degree inElectronic and Electrical engineering fromKyungpook National University, SouthKorea, in 2006, and the MS degree inElectrical Engineering and ComputerScience from Seoul National University,South Korea, in 2008. He is now a PhDstudent in Electrical Engineering andComputer Science, Seoul NationalUniversity. He also belongs to the
Automation and Systems Research Institute in Seoul NationalUniversity. His major research fields are SLAM, motionplanning, and multi-robot systems.
Beom-Hee Lee received the BS and MSdegrees in Electronics Engineering fromSeoul National University in 1978 and 1980,respectively, and the PhD degree inComputer Information, and ControlEngineering from the University ofMichigan, Ann Arbor, MI, USA, in 1985.Since then, he was associated with theSchool of Electrical Engineering at PurdueUniversity as an Assistant Professor, until
1987. He is now with the School of Electrical Engineering andComputer Science as a Professor, and with the Office ofInformation Systems as Dean at Seoul National University. Hismajor research interests include motion planning and control ofrobot manipulators, multi-robot operation, sensor fusionapplications, and factory automation. He was the President ofthe Korea Robotics Society in 2009. He is a Fellow of theIEEE Robotics and Automation Society.
References
[1] Besl PJ, McKay ND. A method for registration of 3D shapes.IEEE Trans. Pattern Anal. Mach. Intell. 1992;14:239–256.
[2] Lu F, Milios E. Robot pose estimation in unknown envi-ronments by matching 2D range scans. J. Intell. Rob.Syst. 1997;20:249–275.
Advanced Robotics 889
Dow
nloa
ded
by [
Uni
vers
ity o
f K
ent]
at 1
4:01
10
Nov
embe
r 20
14
[3] Cox IJ. Blanche: an experiment in guidance and naviga-tion of an autonomous robot vehicle. IEEE Trans. Rob.Autom. 1991;7:193–203.
[4] Lingemann K, Sumann H, Nüchter A, Hertzberg J. Indoorand outdoor localization for fast mobile robots. In:Proceedings of IEEE/RSJ International Conference on Intelli-gent Robots and Systems; 2004; Sendai, Japan. p. 2185–2190.
[5] Weiss G, Puttkamer E. A map based on laser scans withoutgeometric interpretation. In: Proceedings of Intelligent Auton-omous Systems 4(IAS-4); 1995; Germany; p. 403–407.
[6] Diosi A, Kleeman L. Laser scan matching in polar coordi-nates with application to SLAM. In: Proceedings of IEEE/RSJ International Conference on Intelligent Robots andSystems; 2005; Edmonton, Alberta, Canada.
[7] Diosi A, Kleeman L. Fast laser scan matching using polarcoordinates. Int. J. Robot. Res. 2007;26:1125–1153.
[8] Borges GA, Aldon MJ. Line extraction in 2D range imagesfor mobile robotics. J. Intell. Rob. Syst. 2004;40:267–297.
[9] Nunez P, Vazquez-Martin R, del Toro JC, Bandera A,Sandoval F. Natural landmark extraction for mobile robotnavigation based on an adaptive curvature estimation.Rob. Auton. Syst. 2008;56:247–264.
[10] Nunez P, Vazquez-Martin R, Bandera A, Sandoval F. Fastlaser scan matching approach based on adaptive curvatureestimation for mobile robots. Robotica. 2009;27:469–479.
[11] Gil D, Radeva P. Shape restoration via a regularized cur-vature flow. J. Math. Imaging Vision. 2004;21:205–223.
[12] Ze-Su C, Bing-Rong H, Hing L. An improved polar scanmatching using genetic algorithm. Inf. Technol. J.2007;6:89–95.
[13] Zhao H, Chiba M, Shibasaki R, Shao X, Cui J, Zha H.SLAM in a dynamic large outdoor environment using alaser scanner. In: Proceedings of IEEE International Con-ference on Robotics and Automation; 2005; Barcelona,Spain.
[14] Rizzini DL, Caselli S. Experimental evaluation of apeople detection algorithm in dynamic environments. In:Proceedings of IEEE International Conference onRobotics and Automation; 2009.
[15] Lingemann K, Nüchter A, Hertzberg K, Sumann H. High-speed laser localization for mobile robots. Rob. Auton.Syst. 2005;51:275–296.
[16] Kim HY, Lee SO, You BJ. Robust laser scan matching indynamic environments. In: Proceedings of IEEE Interna-tional Conference on Robotics and, Biomimetics; 2009.
[17] Crochiere RE, Rabiner LR. Multirate digital signal pro-cessing. Englewood Cliffs (NJ): Prentice-Hall; 1983.
[18] Bjorck A, Pereyra V. Solutions of vandermonde systemsof equations. Math. Comput. 1970;24:893–903.
[19] Higham NJ. Fast solution of vandermonde-like systemsinvolving orthogonal polynomials. IMA J. Numer. Anal.1988;8:473–486.
[20] Kim I, Kwak N, Lee H, Lee B. Improved particle filterusing geometric relation between particles in FastSLAM.Robotica. 2009;27:853–859.
[21] Kwak N, Yokoi K, Lee B-H. Analysis of rank-basedresampling based on particle diversity in the rao–blackw-ellized particle filter for simultaneous localization andmapping. Adv. Rob. 2010;24:585–604.
[22] Lee H-C, Lee B-H. Improved feature map merging usingvirtual supporting lines for multi-robot systems. Adv.Rob. 2011;25:1675–1696.
[23] Lee H-C, Lee S-H, Choi MH, Lee B-H. Probabilistic mapmerging for multi-robot RBPF-SLAM with unknown ini-tial poses. Robotica. 2012;30:205–220.
Appendix
A. Determination of constant values for searchingrestoration target
In this section, we explain the way how to determine thevalues of the constants, c1 and c2, which are the basis forsearching restoration targets as described in (13) and Table 3.These values are chosen by considering both the size ofnearby environment and the size of the obstacles.
As shown in Figure A1, the size of the mobile robot isM , and the maximum detectable range is D. If the distancebetween the moving circular obstacle whose diameter is Rand the robot is d, the angular detection range of the obsta-cle h and the range of d can be respectively computed by
h ¼ 2 sin�1R
2d(A1)
M
2þ R
2þ e 6 d 6 D� R
2(A2)
where e means the safety area from the robot. From (A1)and (A2), we can obtain the following:
hmin 6 h 6 hmax(hmin ¼ 2 sin�1R
2D� R; hmax
¼ 2 sin�1R
M þ R) (A3)
The constants, c1 and c2 in (13), mean the proportion ofthe entire number of the scan points, so we can get thesevalues by dividing hmin and hmax by ND/, the whole angu-lar scan range of LRF:
Figure A1. Scenario of detecting a moving obstacle.
890 S.-H. Lee et al.
Dow
nloa
ded
by [
Uni
vers
ity o
f K
ent]
at 1
4:01
10
Nov
embe
r 20
14
c1 ¼ hmin
ND/; c2 ¼ hmax
ND/(A4)
Table A1 shows the result of calculating constant valuesof the experimental environment shown in Figure 15 whenD ¼ 3:5m;R ¼ 0:15m; e ¼ 0:5m; and M ¼ 0:38m. Wedefined the human leg as the moving object and limited thathuman cannot approach to the robot closer than 0.5m. Thesize of the mobile robot, shown in Figure 15, is 0.38m andscan range of LRF is 181° (N ¼ 181;D/ ¼ 1). As a result,
if a specific segment has scan points about 3–6 percent ofwhile scan points in scan data, this segment should bechosen as a restoration target.
Table A1. The example of calculating c1 and c2.
Constant hmin hmax c1 c2
Calculation result 6.360° 11.253° 0.035 0.062
Advanced Robotics 891
Dow
nloa
ded
by [
Uni
vers
ity o
f K
ent]
at 1
4:01
10
Nov
embe
r 20
14
Recommended
