A Light-and-Fast SLAM Algorithm for Robots in Indoor Environments Using Line Segment Map by Bor-Woei Kuo, et al (2011)

7/27/2019 A Light-and-Fast SLAM Algorithm for Robots in Indoor Environments Using Line Segment Map by Bor-Woei Kuo, et

1/13

Hindawi Publishing CorporationJournal of RoboticsVolume 2011, Article ID 257852, 12 pagesdoi:10.1155/2011/257852

Research Article A Light-and-Fast SLAM Algorithm for Robots in IndoorEnvironments Using Line Segment Map

Bor-Woei Kuo, 1 Hsun-Hao Chang, 1 Yung-Chang Chen, 2 and Shi-Yu Huang 3

1 Department of Electrical Engineering, National Tsing-Hua University 101, Section 2, Kuang-Fu Road, Hsinchu 30013, Taiwan 2Department of Electrical Engineering, National Tsing-Hua University 720R, EECS Bldg, 101, Section 2, Kuang-Fu Road,

Hsinchu 30013, Taiwan3 Department of Electrical Engineering, National Tsing-Hua University 818, EECS Bldg, 101, Section 2, Kuang-Fu Road,

Hsinchu 30013, TaiwanCorrespondence should be addressed to Bor-Woei Kuo, [email protected]

Received 19 January 2011; Accepted 15 March 2011

Academic Editor: Heinz W orn

Copyright 2011 Bor-Woei Kuo et al. This is an open access article distributed under the Creative Commons Attribution License,which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

Simultaneous Localization and Mapping (SLAM) is an important technique for robotic system navigation. Due to the highcomplexity of the algorithm, SLAM usually needs long computational time or large amount of memory to achieve accurate results.In this paper, we present a lightweight Rao-Blackwellized particle lter- (RBPF-) based SLAM algorithm for indoor environments,which uses line segments extracted from the laser range nder as the fundamental map structure so as to reduce the memory usage. Since most major structures of indoor environments are usually orthogonal to each other, we can also e fficiently increasethe accuracy and reduce the complexity of our algorithm by exploiting this orthogonal property of line segments, that is, we treatline segments that are parallel or perpendicular to each other in a special way when calculating the importance weight of eachparticle. Experimental results shows that our work is capable ofdrawing maps in complex indoor environments, needing only very low amount of memory and much less computational time as compared to other grid map-based RBPF SLAM algorithms.

1. Introduction

In recent years, Simultaneous Localization and Mapping(SLAM) has become one of the basic requirements forrobotic navigation. This technique allows robots to have theability to simultaneously build up a map and localize itself in an unknown environment. The main issue involved is thatwhile localizing a robot we need an accurate map, and forupdating the map the robot needs to estimate its locationaccurately. The relation between robot localization and mapupdating is usually described as a highly complex chicken-and-egg problem.

Clearly, lightweight SLAM algorithms are needed in in-telligent robotic systems, because mobile robots are some-times limited by its size and power budget, so it is usually equipped with a microprocessor which has lower capability than ordinary PCs. For instance, robotic vacuum cleaner hasbeen widely used in many indoor housekeeping applications,but most of these robotic cleaners just randomly wander

around the environment and cleans up the oor along itspath, which is a very inefficient way to clean up a room.After integrating SLAM into the robotic system [ 1, 2], wecan develop an intelligent robotic vacuum cleaner, which iscapable of planning its path in a more e fficient way by usingthe map and location information. However, as mentionedin [3], price, size, and accuracy of the sensor is not the majorproblem, due to the fast progress in sensing technology. Thebiggest challenge will be how to develop a robust lightweightalgorithm and how to integrate it into these embedded ro-botic systems.

Rao-Blackwellized particle lter (RBPF), which was rstintroduced by Murphy and his colleagues [ 4, 5], has becomeone of the most successful ways of solving theSLAM problem[510]. Montemerlo et al. extended this ideal and proposedthe FASTSLAM [7] algorithm which uses a Rao-Blackwel-lized particle lter to estimate the robots poses and tracksthe location of landmarks by using an extended Kalmanlter (EKF). Another effi cient approach to solve the SLAM


2/13

2 Journal of Robotics

problem using RBPF is the DP-SLAM [8, 11], which usesgrid maps rather than landmark maps so that it does notneed any assumption on landmarks. The algorithm assignseach so-called particle (i.e., a snapshot of the environmentthe robot recognizes in terms of a local map and the locationand pose of the robot in the map) an individual map and

maintains these maps by sharing the same parts of the mapbetween particles using a technique called distributed parti-cle mapping, which effi ciently reduce thememory neededforgrid map-based RPBF SLAM. Recently [9] proposed a hybridapproach which uses the grid map as the main map structureand enhance the grid map by a set of line segment maps; asa result, this hybrid approach is able to increase the accuracy of the algorithm.

However, the techniques mentioned above needs largeamount of memory and high computational resource toachieve good results. This is due to the fact that most of theRBPF-based SLAM needs to maintain a large number of particles where each particle has its own map. The mostcommonly used map structure in RBPFSLAM is still the gridmap, because it is capable of describing objects of randomshapes and can easily calculate the importance weight of eachparticle by using scan matching methods [ 12, 13], but thereis a drawback. Since each grid map is built in a xed-sized2D array where each cell represents a specic coordinatelocation, it requires extremely large amount of memory forstoring these grid maps.

In this work, we aim to develop a lightweight laser rangender-based SLAM algorithm for indoor environmentsusing Rao-Blackwellized particle lter (RBPF), while most of the major structures (walls, doors, etc.) and furniture of indoor environments can be easily represented as line seg-ments, we decided to use line segments as our map structureinstead of grid map. Also when storing line segments intothe map, we only need to store the location and parametersof each line segment. As a result, the memory requirement ismuch more e fficient as compared to grid map.

On the other hand, we also use the orthogonal property of indoor environments to increase accuracy of our algo-rithm. This property is based on the fact that most structuresof indoor environments are parallel or perpendicular to eachother [ 3]. By just considering these orthogonal lines, we canlter out most of the erroneous line segments caused by sensor noise or line extraction error. Although this concepthas already been used in many other works [ 3, 14, 15],there is still a main diff erentiating factor in our workwedynamically modify the reference direction, which is used toidentify the orthogonal lines, rather than just aligning thelines with the x -axis and y -axis. This is extremely importantsince it can thereby allow the robot to have the ability tostart its initial pose at any angle and need not to align itsinitial direction with the major structures of the environ-ment.

The rest of this paper is organized as follows. Section 2quickly describes how the Rao-Blackwellized particle ltersolves the SLAM problem. In Section 3, we will discussthe details of our lightweight RBPF SLAM algorithm.Section 4 presents the experimental results, and Section 5concludes.

2. RBPF for SLAM

As mentioned in [ 16], a full SLAM problem is to estimate the joint posterior by which a robot recognizes the environmentafter moving around for a while. This joint posterior is de-noted as p(x 1:t , m | z 1:t , u1:t 1), where x 1:t denotes the entirerobot trajectory estimated so far from time instant 1 to timeinstant t , m is the associated mapin terms of some data struc-ture (e.g., grid map or line segment based map in our sys-tem). The joint posterior is constantly derived and updatedbased on two given pieces of informationthe observationsz 1:t from the environment-measuring sensor (which is a laserrange nder in our system), andtheodometry measurementsu1:t 1, which is a rough estimation of the trail a robothave traveled up to some point in time. Since odometry isbased on how many rounds each of the robots two wheelshas turned, it often has signicant errors referred to asodometry noise. A robust SLAM algorithm should be ableto derive accurate joint posterior p(x 1:t , m | z 1:t , u1:t 1) undersignicant observation inaccuracy and/or odometry noise.

The key idea of the Rao-Blackwellized particle lter basedalgorithm is to solve the SLAM problem by splitting the jointposterior p(x 1:t , m | z 1:t , u1:t 1) into two separate parts [ 4,6]. The rst part is about estimating the robot trajectory , thatis, x 1:t , using a particle lter (to be explained in detail later).The second part is about the update of the map based onthe derived trajectory. With this concept, we can rewrite theformula in two cascaded parts as

p(x 1:t , m | z 1:t , u1:t 1) = p(m | x 1:t , z 1:t ) p(x 1:t z 1:t , u1:t 1).(1)

While using a particle lter to estimate the robots poten-

tial trajectories, a motion model is needed regarding thegeneration of thenewparticles {x (i)t } from the correspondingprevious particle {x (i)t 1} by taking into account the odometry data. Due to the enormous error of the odometer, we needto determine how accurate each new particle is, that is, weassign each of them an importance weight according to how well the current measurement of the environment matcheswith the map. After assigning each particle an importanceweight, the particles that have lower weight will be discarded,until only a small number of particles are left. Finally, themapis updated foreach remaining particlesusing thecurrentmeasurements from the sensor. By repeatedly executing thesteps mentioned above, we can maintain a set of particles at

any given time, indicating the mostly likely robots poses anda map built by its previous trajectory. By doing so, the robotis capable of moving around in an unknown environmentwithout getting lost.

3. Proposed Lightweight RBPF SLAM

3.1. SystemOverview. Figure 1 shows theoverall owchart of our lightweight RBPF SLAM algorithm. When a robot startsexecuting SLAM in an unknown environment, we will rstset the robots initial pose at the origin of its coordinatesystem as (x , y , ) = (0,0, 0), and build an initial map usingthe information from the laser range nder according to


3/13

Journal of Robotics 3

Particle generation

Rotate and shift

Data association

Initialize

Assign importanceweight

Line merging

Sampling

Importanceweighting

Map update

Line extraction

Raw scan dataDetect robot

moving

Resampling

Figure 1: Overall owchart of a particle lter-based algorithm.

the initial pose. After initializing the system, it will repeatedly estimates the correct robot pose using a particle lter andupdate the map of each particle by a line merging techniqueeach time when the robot moves farther than a distance.

Usually an RBPF-based SLAM algorithm can be imple-mented by using the Sampling Importance Resampling (SIR)lter, which is one of the most commonly used particle lter-ing algorithms [ 6], and a map update technique. The entireprocedure can be summarized in the following steps,

(1) Sampling : New particles are generated from the pre-

vious particle using a motion model.(2) Importance Weighting : Each new particle is assigned

an importance weight to determine the accuracy of the particle according to how well the current obser-vation matches the map it has already built.

(3) Resampling : Particles with low weights are likely to bereplaced by the ones with high weights.

(4) Map Update : The most current map observed by thelaser range nder is updated to each remaining par-ticle after the resampling step according to its indi-vidual pose, so that each particle has a most updatedmap of the environment.

Because of using line segments as our map structure, weneed some extra procedure to maintain these lines. First of all, we need to extract the line segments from the raw scandata provided by a laser range nder; this will allow usto have the information about the environment the robotcurrently sees. But extracting the line segments for every new particle generated in the particle generation step willcost an enormous e ff ort, so our algorithm only extracts aset of reference line segments, then by rotating and shiftingthis set of line segments to the corresponding new particlespose. In the importance weighting step, we need to ndout the relation between the new extracted line segmentsand the map we have already built before calculating the

importance weight of each particle; this procedure is calleddata association. Also in the data association step, we mainly consider the orthogonal lines [ 3] extracted from the laserrange nder, to lter out erroneous lines caused by sensornoise or line extraction error. Finally, in the map-update step,line segments which are too close to each other are mergedtogether to maintain consistency of our map and lower thenumber of line segments. After updating the map for eachparticle, we will wait until the robot has moved farther thana distance before starting the new iteration over again.

In the following subsections, we will discuss the details of each step mentioned in Figure 1 to implement a lightweightRBPF SLAM for indoor environments.

3.2. Particle Generation. This is the rst step of the particleltering process, where a motion model is used to generatenew potential robot poses according to its previous posebased on the odometry data. These potential robot posesare usually called particles. The motion model we used herecan be found in [ 17], where the authors proposed a motionmodel that is capable of capturing the relationship betweenodometry data and the change of robot conguration, thenby using this information to modify its parameters andincrease the accuracy of the model. The motion model canbe written as

x t = x t 1 + D cos t 1 +T 2

+ C cos t 1 +T +

2,

y t = y t 1 + D sin t 1 +T 2

+ C sin t 1 +T +

2,

t = t 1 + T mod 2 ,(2)

where D, T , and C are three noise models which representthe robots travel distance, amount of turns performed , and theshift in the orthogonal direction to the major axis, respec-tively.


4/13


Line segmentationand corner detection

Scanpoints

Least squareline tting

Any morescan points?

Ref.scan lines

No

Yes

Rotate andshift

Motionmodel

Scan linesfor particle i

(x i , y i, i)

Sequentialsegmentation

algorithm

Figure 2: Line extraction procedure.

L

y

x

Figure 3: The relationship of line L in two diff erent spaces, wherethe black dots are the points that contains line L.

After the particle generation step, we will have a set of new particles {x i} i= 1,...,m generated by the motion model,each representing a potential pose of the robot ( x , y , ) andan updated map considering its previous trajectory. Due tothe enormous error of the odometry data, we cannot rely only on themotionmodel to estimate the true robot pose. So,we need the help of a laser range nder, which is signicantly more precise, to provide the information of the current envi-ronment and matches it with the map we have already builtto lter out particles that are too much inconsistent with thetrue pose, which will be introduced in the following sections.

3.3.Line Segment Extraction. The accuracy of a feature-basedSLAM algorithm can be increased by using a robust featureextraction method ; this is because feature-based SLAMheavily depends on the features extracted from the sensorto estimate the robot pose. Figure 2 shows the procedure of our line extraction algorithm, where an enhanced sequential

segmentation algorithm is used to extract a set of referenceline segments from raw scan data provided by a laser rangender, and, then, we rotate and shift the reference scan linesto the corresponding particle pose ( x i, y i, i) generated by themotion model, so that each particle has its own set of scanlines.

In this section, we will describe how to enhance the se-quential segmentation algorithm proposed by [ 18], and cre-ating scan lines for each new particle by rotating and shiftingthe reference scan lines.

3.3.1. Least Square Fitting. Least square tting is a techniquethat nds the best tting line L : x cos + y sin = to

a given set of sample points {(x i, y i)} i= 1,...,n , which minimizesthe following error:

Et =n

i= 1x i cos + y i sin , (3)

and can be computed by using a least square method [ 19]as follows:

=12

atan2 2 Sxy NX N Y N

S yy Sxx Y 2N X 2N ,

= X N cos + Y N sin ,

(4)

where

Sxx =N

i= 1x 2i , S yy =

N

i= 1 y 2i , Sxy =

N

i= 1x i y i,

X N =1N

N

i= 1x i, Y N =

1N

N

i= 1 y i.

(5)

The relation of line L : x cos + y sin = betweenHough space and x - y plane are shown in Figure 3.

3.3.2. Enhanced Sequential Segmentation Algorithm. The se-quential segmentation line extraction algorithm was rstintroduced in [ 18], which is known as a very efficientmethodfor line extraction [ 18, 20]. The main di ff erence between ourwork and [ 18] is that we enhanced the algorithm by adding acorner detectiontechniqueandan adaptive breakpoint detector for line segmentation. This will lead to better quality incomplex indoor environments.

Sequential segmentation algorithm works by rst givinga set of raw scan data, denoted as {d i} i= 1,...,N . If the distancebetween three consecutive points {d k, . . . , d k+2 }1 k N 2 arewithin a threshold d , which is d k+1 d k d and

d k+2 d k+1 d , then a line L is extracted from the points{d k, . . . , d k+2 } using least square tting [ 19]. After nding


5/13


d k

d k+nStart point

End point

Figure 4: Projecting the start/end points d k and d k+n onto the ex-tracted line.

d n+2

d n

L2

L1 12

Figure 5: An example of corner detection.

the rst three consecutive points that can be extracted intoa line segment L, we will determine whether the next consec-utive point d k+3 can be merged into line L by the followingsteps:

(a) d k+3 d k+2 d ,(b) the perpendicular distance from scan point d k+3 to

line L is within a threshold.If the two conditions all achieves, we will merge the scan

point d k+3 into line L by using a sequential least squaremethod proposed in [ 18], and the next point d k+4 is tested.Otherwise, a new scan line Si is determined from L, and thesame process is repeated forextracting new scan lines startingfrom d k+3 and ends until all the scan points have been tested.When a new scan line Si is extracted from a set of scan points{d k, d k+1 , . . . , d k+n} , the two terminal points of the new scanline are determined by projecting the rst point d k (startpoint) and the last point d k+n (end point) onto the scan lineas shown in Figure 4.

The enhanced sequential segmentation algorithm takesplace in two parts in the original algorithm. First, we deter-mine the threshold value d , which decides if two consecutivepoints are close enough to be in the same line segment, by using an adaptive breakpoint detector introduced in [ 21]rather than a constant value. This is due to the fact that thedistance between two consecutive points will increase whilethe distance between the laser range nder and the scan pointincreases. So, d can be dened as

d = r k+1 sin

sin + 3 r , (6)

where is the angular resolution of a laser range nder, r k+1is the distance between laser range nder and scan point d k+1 ,

is a constant parameter, and r is the residual variance. As aresult, by dynamically changing the threshold d can let ourline extraction algorithm achieve a better quality in detectingbreak points when two consecutive scan points are very closeto or very far away from the laser range nder.

Second, a corner detection technique is performed while

adding new consecutive scan point d n into the line L, sonow we not only detect if the distance d n d n 1 and theperpendicular distance from d n to L are under a threshold,but also detect if d n is on the corner of two line segments.The corner detection procedure can be summarized by thefollowing steps, which is also shown in Figure 5.

(a) Detect if the scan point d n and its next two consecu-tive points {d n+1 , d n+2 } can be merged into a line L2,which is positive when the distance from d n to d n+1and d n+1 to d n+2 are both under a threshold d .

(b) Detect if the perpendicular distance from d n+2 to L1is larger than a threshold.

(c) If one of the two conditions mentioned above doesnot achieve, the scan point will not be at the cornerof two line segments. Otherwise, we will calculatethe angle 12 between the lines we are currently extracting, that is, L1, and L2, which are extractedfrom the three new scan points {d n , d n+1 , d n+2 }. If 12is smaller than a threshold, a corner is detected.

After the enhanced sequential segmentation has com-pleted, we will have a set of scan lines {si} i= 1,...,N extractedfrom the raw scan data, and each scan line is dened as.

Si = spi, epi , N , Sxx , S yy , Sxy , X N , Y N , (7)

where (sp i, epi) are the start point and end point of the scanline and N , Sxx , S yy , Sxy , X N , Y N are parameters generatedin the least square tting step, which are used to speed up thealgorithm while updating the scan lines and merging two linesegments.

3.3.3. Rotate and Shift. Since each new particle generated by the motion model has its own pose ( x i, y i, i), each particleneeds a set of line segments representing the informationabout the current environment according to its individualpose. However, due to the fact that all particles extractline segments by projecting from the same set of raw scan data, performing line extraction for each particle isa very inefficient way since it has a lot of repeated andredundant computation. In our work, we speed up theprocess tremendously by exploiting a propertythere existsshifting and rotating relations between scan lines derived forthe particles generated by the motion model, that is, once wehave derived the scan lines of one particle, we can use themas the reference scan lines, and map these reference scan linesinto those for some other particle via shifting and rotationtransforms. In detail, we pick up a reference particle rstand generate a set of reference scan lines from the raw scandata for this particle. Then for every other particle, we rotateand shift the reference scan lines according to the particlesrelative location to the reference particle.


6/13


{Si} i= 1,...,N

{LM j} j= 1,...,l l M

Scan lines

For eachscan line

Data association

Map Lines

Yes

Particle(x , y , )

Local mapOrientation

Referencedirection

Sampling

Finish

No

{ M j} j= 1,..., M

Any nonevaluatedscan line

Figure 6: Overall process of data association.

The wayof rotating and shifting the reference scan lines isinspired by the sequence least square tting algorithm [ 18],where we do not need to recalculate all the parameters usingleast square tting for each particle, but only updates thesix parameters of a line segment in a sequential manner asfollows.

(a) Rotation

Sxx rot = Sxx ref cos2 i 2S

xy ref sin i cos i + S

yy ref sin

2 i,

S yy rot = Sxx ref sin2 i + 2S

xy ref sin i cos i + S

yy ref cos

2 i,

Sxy rot = Sxx ref S yy ref sin i cos i + S

xy ref cos

2 i sin2 i ,

X nrot = cos i X nref sin i Y nref ,

Y nrot = sin i X nref + cos i Y nref .

(8)

(b) Shift

Sxx shift = Sxx rot + 2N x i X

nrot + N x 2i ,

S yy shift = S yy rot + 2N y i Y

nrot + N y 2i ,

Sxy shift = Sxy rot + N y i X nrot + N x i Y nrot + N x i y i,

X nshift = X nrot + x i,

Y nshift = Y nrot + y i,

(9)

where the start/end points of each line can also be computedby rotating and shifting from the reference scan lines by

x = x ref cos i y ref sin i + x i,

y = y ref cos i + x ref sin i + y i.(10)

3.4. Data Association. Data association is to nd the relation-ship between the new extracted scan lines and the map lineswhich we have already built in the map for each particle, andby using this relationship to calculate an importance weightof each particle to indicate the correctness of its pose. As wecan see, the accuracy of data association is a crucial issue toachieve a robust SLAM algorithm.

Asshown in Figure 6, before entering thedata associationstep, a local map is created to reduce search space whilematching scan lines with the map; this is due to the factthat the motion model will generate a large amount of new particles (i.e., 500 particles per iteration), where eachparticle has its own set of scan lines and each scan line needsto be matched with the entire map. Also, the orthogonalassumption for indoor environments [ 3] is performed in theorientation step, where we only consider scan lines whichare parallel or perpendicular to each other. This is becausemost major structures of indoor environments usually havethe orthogonal relationship, and by using this assumption wecan reduce the number of scan lines and lter out erroneousscan lines caused by sensor noise or line extraction error toincrease theaccuracy of thealgorithm.Thedetails of buildinga local map, the orientation step, and data association aredescribed below:

3.4.1. Local Map. The size of a local map is determined by a few meters larger than the largest area that can cover allthe scan lines of the new particles generated by the sameprevious particle, which we called parent particle. All thenew particles generated from the same parent particle sharesa local map, because the maps they contain are built fromthe same previous robot trajectory. This can let us reduce thenumber of times to build local maps, and e ffi ciently reducethe search space for nding the best match of each scan linein the data association step.

Also a reference direction ref is calculated in order to letthe orientation step determine which scan line extractedfrom the laser range nder are orthogonal lines. This is doneby rst determining the most observed line in the local map

as a reference line, which is the line that has been observedmost of the time. If there is more than one most observedline, the longest one is picked. Then, by using all the linesthat are orthogonal to the reference line in the local mapto compute a weighted reference direction similar to themethod proposed in [ 3] by the following formula:

ref =N ||i= 1 wi i +

N j= 1 w j j / 2

N ||i= 1 wi +

N j= 1 w j

, (11)

where N || and N are the number of lines that are paralleland perpendicular to the reference line, respectively, wi is thelength of line i, and i is the angular parameter of line i. Our


7/13


Start point

End point

Robotscurrent pose

(a)

B

Starting point A

(b)

Figure 7: (a) Vector lines determined by its start/end points. (b) Example of vector lines representing opposite sides of a wall.

work dynamically modies the reference direction each timethe local map is built rather than just aligning the orthogonallines with the x axis, so that the robot is capable of starting itsinitial pose without aligning with the major structures of theenvironments and can still correctly detect the orthogonallines.

3.4.2. Orientation. The orientation step uses the referencedirection ref to identify whether the scan line is an orthogo-nal line by the following equation:

| i ref | , parallel to ref ,

i ref 2 , perpendicular to ref , (12)

where is a threshold about 5 and i is the angle of ascan line i in polar coordinates. If a line segment does notmatch one of the above two equations, it is considered as anonorthogonal line and will not be taken into account forcalculating the importance weight of the particle.

3.4.3. Vector-Based Line Representation. Before discussingthe details of the data association step, we will explain thevector-based line representation technique [18, 20, 21] used inour line matching method, in which each line segment in themap is assigned a direction starting from its rst point to itslast as shown in Figure 7(a) , so that each line segment can bepresented as a vector. The main advantage of using a vector-ased line representation is we can avoid mismatch in enclosedareas (e.g., opposite sides of a thin wall) by determining thedirection of the vectors. Figure 7(b) shows an example of using vector lines to represent an indoor environment, wherethe robot starts at one side of the wall (at starting point A)and moves along to the other point B. We can see that thevectors representing two sides of the wall (enclosed in a box in Figure 7) have diff erent directions, and thus they can beeasily distinguished from each other to avoid mismatch inthe data association step.

3.4.4. Data Association. In the data association step, only or-thogonal scan lines {Si} i= 1,...,N of each particle are taken intoaccount to nd a best match in the local map {LM j} j= 1,...,l .If a scan line does not matches any lines in the map, it willbe dened as a new observed line and be added into themap during the map-update step. Otherwise, the scan lineis able to nd the best match, which means it had already been observed by the robot previously and can be taken intoaccount to calculate the particles importance weighting.

Our line matching algorithm for nding the best matchin the local map {LM j} j= 1,...,l for each scan line Si is done by the following procedures.

(a) Detect if the angle i j between a scan line Si and alocal map line LM j is under a threshold. Here, wedetermine the line segments as vectors, so we cancompute the angle quickly by

i j = cos 1

Si LM j

Si LM j

. (13)

(b) Detect if there is an overlap between Si and LM j by calculating Lov as follows:

l t = (x e x s)2

+ y e y s2,

Lov = l m + l s l t =

positive, if there exists overlap,

negative, otherwise,(14)

where l m is the length of a local map line LM j, l s isthe length of a scan line Si, and l t is the length of apotential line Lt by merging Si and LM j, as shown inFigure 8.

(c) If one of the conditions mentioned above does notachieve, this means that Si and LM j are too far away


8/13


Scan lineSi

(x s, y s) Lt

(x e, y e)LM j

Local map lineFigure 8: Overlap detection.

Scan lineSi

Lm

l overlap

LM jLocal map line

Figure 9: Illustration of the overlap distance l overlap between thematching pair Si and LM j.

and could not be the best match. Otherwise theweighted Euclidean distance DH j [14] are computedto nd out how well Si and LM j matches by using theparameters and of the two lines in Hough spaceas follows:

DH j = w s m 2 + w ( s m)2,w + w = 1.

(15)

After all the lines in the local map {LM j} j= 1,...,l arematched with the scan line Si, we can determine the bestmatch by nding a matched pair that has the smallestweighted Euclidean distance DH j by

DH = MIN(DH 1, DH 2, . . . , DH l ). (16)

If no matching pairs are found for a scan line Si, we willdetermine the scan line as a new discovered line, andtherefore insert it into the map in the map-update stage.

3.5. Importance Weighting. Each particle generated from themotion model is assigned an importance weight according

to how well its current measurement of the environmentmatches the map we have already built, so that we candetermine the accuracy of each particle by its importanceweighting.

In our case, the importance weight W itot for particle i iscomputed using the matching pairs we found in the dataassociation step, by rst assigning each matching pair aweight as follows:

w = l overlape DH , (17)

where DH is the weighted Euclidean distance of the matchedpair and l overlap is the overlap distance between the matchedscan line and the map line, which is shown in Figure 9.

End point

Start point

Lm

Si

LM j

Figure 10: Merging two lines Si and LM j by considering all the scanpoints.

After all the matched pairs are assigned a weight, we cancompute the importance weight of the ith particle by

W itot =N m

N tot scan

N m

k= 1wk, (18)

where N m is the number of matched pairs, N tot scan is the totalnumber of orthogonal scan lines extracted in this iteration,and wk is the weight of the kth matching pair. The mainpurpose of multiplying the ratio N m /N tot scan is to avoid someparticles having extremely high importance weight causedby matching pairs which have long overlap distance betweenthem.

After assigning each particle an importance weight, theresampling step will take place by deleting the ones thathave lower weights. Also particles that are assigned a higherweight (meaning that it has a higher probability to be thecorrect robot pose) will have the opportunity to generatemore new o ff spring particles from the motion model in thenext iteration.

3.6.MapUpdate. The map of each particle is maintained andupdated in this step, where new observed lines are added tothe map and lines that are close to each other, will be mergedinto a single line segment.

Figure 10 shows an example of merging a scan line Siand a map line LM j into a single line Lm, where Lm isgenerated using all the scan points constituting Si and LM j.By considering all the points, line segments can be mergedmore accurately, but with higher requirement of memory since it requires the storing of all the points constituting theindividual lines. To overcome this issue, the recursive least-squares (RLSs) method [ 20] for line merging is used in ourwork, which does not need to store all the points, but only requires six additional parameters ( N , Sxx , S yy , Sxy , X N , Y N )of each line being merged. The RLS method operates by rst updating the six parameters for the merged line andthen determines the start/end points of the line by projectingthe terminal points onto the new line. Here, we briey review the RLS algorithm. More information can be found in[20].

In order to reduce the computational complexity of maintaining the map for each particle, the map-update pro-cedure is split into two modes: (1) local map update and (2)global map update, where the local map update executes atevery iteration and the global map update only executes oncein a period of time.


9/13


Table 1: Summarizes the total memory, average time, and maximum time needed for the two algorithms.

Total memory (MB) Avg. time (ms) Max. time (ms)General approach 193.2 (100%) 453.4 (100%) 1062 (100%)Proposed algorithm 10.7 (5.5%) 29.5 (6.5%) 78 (7.4%)

Si

LM j

l 1l 2

Figure 11: Illustrates the perpendicular distance l 1 and l 2 from theterminal points in the overlap section to the opposite line.

3.6.1. Local Map Update. In this step, new scan lines areadded into the map and the matched pairs determined in

the data association step will be merged together. Also linesegments in the local map are merged if they are close toeach other and have an overlap between them. The way of determining if two line segments in the local map can bemerged together is similar to nding the matching pairsin the data association step, but instead of calculating theweighted Euclidean distance, we only calculates the averagedistance between two overlapped lines to determine if thelines can be matched as shown in Figure 11.

Where l 1 and l 2 are the perpendicular distance from theterminal points covering the overlap section to the oppositelines, the average distance can be computed as

l dist =l 1 + l 2

2 . (19)

If the average distance l dist is smaller than a threshold, the twolines are close to each other and will be merged together.

3.6.2. Global Map Update. In order to avoid having severalbroken line segments that are meant to be the same line inthe map, we need to check all the map lines and determinewhether the lines can be merged together by using the sameway as in local map update, but this time the whole map willbe tested. Due to the fact that checking all the line segmentswill cost an extremely large eff ort and the local map updatecan merge most of the broken lines together, we only execute

the global map update once in a predetermined period of time. As a result, we are able to maintain the map moreefficiently and reduce the number of line segments in ourmap.

4. Experimental Results

Our algorithm has been implemented for a P3-DX robotequipped with SICK LMS-100 laser rangender, wherewe also test its performance using di ff erent datasets fromRadish [22]. All the experiments are performed on a laptopcomputer with 2G CPU and 3G RAM, which our algorithmis capable of running in real-time with low memory usage.

The rst dataset is recorded by our P3-DX robot at theCenter of Innovation Incubator, room 212, in our NTHUcampus, where the size of the room is about 7m 4 m.Our main purpose is to test whether the algorithm cansuccessfully detect the orthogonal lines in the environmenteven when the robot does not starts its initial pose alignedwith the walls. Figure 12 shows an experiment of startingthe robot at di ff erent directions, all of which are not alignedwith the major structures of the environment. We use ourproposed algorithm to determine the orthogonal lines (bluelines) and nonorthogonal lines (red lines) in the initial map.Also, error line segments caused by sensor noise or lineextraction error can be ltered out by simply considering theorthogonal lines in themap. Figure 13(a) shows the oorplanof the room, and Figure 13(b) is the map of the entire roombuilt by ouralgorithm,which only considersorthogonal linesin the environment. As we can see, by dynamically modifyingthe reference direction to identify the orthogonal lines, thealgorithm is capable of determining the correct orthogonaldirection even when the robots initial pose is not alignedwith the major structures of the environment.

The second experiment uses the dataset called intel oregon by M. Batalin, which was recorded at Intel Lab inHillsboro, Ore, USA, using a P2-DX robot with LSM-200laser rangender. As mentioned in [ 20], the covering area of this dataset is about 750 m 2 and the total moving distance of the robot is 110 m, which contains several loops and complex indoor structures. In this experiment, we also implementeda general approach RPBF SLAM using grid map as itsmap structure to compare the total memory usage andcomputation time against our proposed lightweight SLAMalgorithm, which uses line segments as our map structure.Figure 14 depicts the mapping result of intel oregon usinggeneral approach ( Figure 14(a) ) and our proposed algorithm(Figure 14(b) ), and Figure 15 shows the computation timeof each iteration needed for executing the two algorithms,respectively. As it can be seen, our lightweight SLAMnotonly reduces the memory usage, but also needs less computationtime as compared to the general RBPF SLAM. This is mainly due to the fact that line segment map only needs to storethe geometric position and some parameters of each line, butgrid map needs to store the whole environments informationin a large 2D array. Also when a new particle is generated,a map built by the robots previous trajectory is copied tothe new particle so that each particle maintains its own map.Since copying memories is often time consuming, we canthereby increase the speed of the algorithm by lowering theamount of data needed to be copied. Table 1 summarizesthe total required memory, average computation time, andmaximum computation time needed in an iteration of thegeneral approach RBPF and in our lightweight algorithm.As compared to the general approach RBPF SLAM, the total


10/13


(a) (b)

(c) (d)

Figure 12: Initial map built by robots starting at di ff erent directions and indicates orthogonal (blue lines) and nonorthogonal (red lines)lines.

(a) (b)

Figure 13: (a) Floorplan of the room. (b) Resulting map drawn by our algorithm.


11/13


(a) (b)

Figure 14: Mapping results of intel oregon using (a) general RBPF SLAM (grid map) and (b) proposed lightweight SLAM (line segmentmap).

0 250 500 750 1000 1250 1500Iteration

0200400600800

10001200

T i m e

( m s )

General RBPF SLAMProposed algorithm

Figure 15: Computation time of each iteration needed for a gridmap-based RBPF SLAM (blue line) and our proposed lightweightapproach (red line).

memory usage and average computation time can be reducedto only 5.5% and 6.5%, respectively, by our algorithm. Sinceour algorithm is also much faster, it can support the SLAMoperation for a faster-moving robot. In some sense, ouralgorithm is 453 .4 / 29.5 = 15.36 times faster.

5. Conclusion

In this paper, we have successfully developed a robust light-and-fast RBPF-based SLAM algorithm for indoor environ-ments. We use an enhanced sequential segmentation algo-rithm to increase the reliability of line segments extractionfrom the raw scan data. We also integrated the vectorbased line representation with the orthogonal assumptionof indoor environments to avoid mismatches in the dataassociation step. Experimental results show that our work needs much low amount of memory (e.g., only 5.5%) andmuch less computation time (e.g., only 6.5%) as compared toprevious grid map-based RBPF SLAM. In other words, oursis capable of performing accurate SLAM for 15.36x faster-moving robots in complex indoor environments.

Acknowledgment

This work was supported in part by Project SOC Joint Re-search Lab sponsored by NOVATEK.

References

[1] W. Jeong and K. Lee, CV-SLAM: a new ceiling vision-basedSLAM technique, in Proceedings of the IEEE/RJS International Conference on Intelligent Robots and Systems, Albert, Canada,August 2005.

[2] H. Myung, H. M. Jeon, and W. Y. Jeong, Virtual door algo-rithm for coverage path planning of mobile robot, in Pro-ceedings of the IEEE International Symposium on Industrial Electronics (ISIE 09), Seoul, Korea, July 2009.

[3] V. Nguyen, A. Harati, A. Martinelli, and R. Siegwart, Orthog-onal SLAM: a step toward lightweight indoor autonomousnavigation, in Proceedings of the IEEE/RSJ International Con- ference on Intelligent Robots and Systems (IROS 06), Beijing,China, October 2006.

[4] K. Murphy, Bayesian map learning in dynamic environ-ments, in Proceedings of the Conference on Neural InformationProcessing Systems (NIPS 99), pp. 10151021, Denver, Colo,USA, November-December 1999.

[5] A. Doucet, J. F. G. de Freitas, K. Murphy, and S. Russel,Rao-Blackwellized particle ltering for dynamic Bayesiannetworks, in Proceedings of the Conference on Uncertainty in Articial Intelligence (UAI 00), pp. 176183, Stanford, Calif,USA, June-July 2000.

[6] G. Grisetti, C. Stachniss, and W. Burgard, Improved tech-niques for grid mapping with rao-blackwellized particle l-ters, IEEE Transactions on Robotics, vol. 23, no. 1, pp. 3446,2007.

[7] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, Fast-SLAM: a factored solution to the simultaneous localizationand mapping problem, in Proceedings of the AAAI National Conference on Articial Intelligence, Edmonton, Canada, July-August 2002.

[8] A. Eliazr and R. Parr, DP-SLAM: fast, robust simultaneouslocalization and mapping without predetermined landmarks,


12/13


in Proceedings of the the International Conference on Articial Intelligence (IJCAI 03), Acapulco, Mexico, August 2003.

[9] W. J. Kuo, S. H. Tseng, J. Y. Yu, and L. C. Fu, A hybridapproach to RBPF based SLAM with grid mapping enhancedby line matching, in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 09), St.Louis, Mo, USA, October 2009.

[10] C. Schroter, H.-J. Bohme, and H.-M. Gross, Memory-ef-cientgridmaps in Rao-Blackwellized particle lters for SLAMusing sonar range sensors, in Proceedings of the European Con- ference on Mobile Robots (ECMR 07), pp. 138143, Freiburg,Germany, September 2007.

[11] A. Eliazr and R. Parr, DP-SLAM 2.0, in Proceedings of theIEEE International Conference on Robotics and Automation(ICRA 04), New Orleans, La, USA, April-May 2004.

[12] S. Thrun, W. Burgard, and D. Fox, A real-time algorithmfor mobile robot mapping with applications to multi-robotand 3D mapping, in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA 00), SanFrancisco, Calif, USA, April 2000.

[13] D. Hahnel, D. Schulz, and W. Burgard, Map building with

mobile robots in populated environments, in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 02), EPFL, Switzerland, September-October2002.

[14] Y. H. Choi, T. K. Lee, and S. Y. Oh, A line feature based SLAMwith low grade range sensors using geometric constraints andactive exploration for mobile robot, Autonomous Robots, vol.24, no. 1, pp. 1327, 2008.

[15] P. Newman, J. Leonard, J. D. Tardos, and J. Neira, Exploreand return: experimental validation of real-time concurrentmapping and localization, in Proceedings of the IEEE Inter-national Conference on Robotics and Automation (ICRA 02),Washington, DC, USA, May 2002.

[16] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics, TheMIT Press, 2005.

[17] A. I. Eliazar and R. Parr, Learning probabilistic motion mod-els for mobile robots, in Proceedings of the International Con- ference on Machine Learning , Alberta, Canada, July 2004.

[18] H. J. Sohn and B. K. Kim, An effi cient localization algorithmbased on vector matching for mobile robots using laser rangenders, Journal of Intelligent and Robotic Systems, vol. 51, no.4, pp. 461488, 2008.

[19] F. Lu and E. Milios, Robot pose estimation in unknown envi-ronments by matching 2D range scans, Journal of Intelligent and Robotic Systems, vol. 18, no. 3, pp. 249275, 1997.

[20] H. J. Sohn and B. K. Kim, VecSLAM: an effi cient vector-based SLAM algorithm for indoor environments, Journal of Intelligent and Robotic Systems, vol. 56, no. 3, pp. 301318,2009.

[21] G. A. Borges and M. Aldon, Line extraction in 2D rangeimages for mobile robotics, Journal of Intelligent and Robotic Systems, vol. 40, no. 3, pp. 267297, 2004.

[22] http://cres.usc.edu/radishrepository/view-all.php .


13/13

Submit your manuscripts athttp://www.hindawi.com

A Light-and-Fast SLAM Algorithm for Robots in Indoor Environments Using Line Segment Map by Bor-Woei Kuo, et al (2011)

Documents