alight-and-fastslamalgorithmforrobotsinindoor

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 Huang3

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

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

Correspondence should be addressed to Bor-Woei Kuo, [email protected]

Received 19 January 2011; Accepted 15 March 2011

Academic Editor: Heinz Worn

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 filter- (RBPF-) based SLAM algorithm for indoor environments,which uses line segments extracted from the laser range finder as the fundamental map structure so as to reduce the memoryusage. Since most major structures of indoor environments are usually orthogonal to each other, we can also efficiently 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 of drawing maps in complex indoor environments, needing only verylow 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 itselfin 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 usuallyequipped with a microprocessor which has lower capabilitythan 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 floor 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 efficient 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 filter (RBPF), which was firstintroduced by Murphy and his colleagues [4, 5], has becomeone of the most successful ways of solving the SLAM problem[5–10]. Montemerlo et al. extended this ideal and proposedthe FASTSLAM [7] algorithm which uses a Rao-Blackwel-lized particle filter to estimate the robot’s poses and tracksthe location of landmarks by using an extended Kalmanfilter (EKF). Another efficient approach to solve the SLAM

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 andmaintains these maps by sharing the same parts of the mapbetween particles using a technique called distributed parti-cle mapping, which efficiently reduce the memory needed forgrid 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 accuracyof 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 ofparticles where each particle has its own map. The mostcommonly used map structure in RBPF SLAM 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 fixed-sized2D array where each cell represents a specific coordinatelocation, it requires extremely large amount of memory forstoring these grid maps.

In this work, we aim to develop a lightweight laser rangefinder-based SLAM algorithm for indoor environmentsusing Rao-Blackwellized particle filter (RBPF), while most ofthe major structures (walls, doors, etc.) and furniture ofindoor 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 efficient as compared to grid map.

On the other hand, we also use the orthogonal propertyof 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 canfilter out most of the erroneous line segments caused bysensor noise or line extraction error. Although this concepthas already been used in many other works [3, 14, 15],there is still a main differentiating factor in our work—wedynamically 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 filtersolves 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 thejoint posterior by which a robot recognizes the environmentafter moving around for a while. This joint posterior is de-noted as p(x1:t ,m | z1:t,u1:t−1), where x1:t denotes the entirerobot trajectory estimated so far from time instant 1 to timeinstant t,m is the associated map in 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 information—the observationsz1:t from the environment-measuring sensor (which is a laserrange finder in our system), and the odometry 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 robot’s two wheelshas turned, it often has significant errors referred to asodometry noise. A robust SLAM algorithm should be ableto derive accurate joint posterior p(x1:t ,m | z1:t ,u1:t−1) undersignificant observation inaccuracy and/or odometry noise.

The key idea of the Rao-Blackwellized particle filter basedalgorithm is to solve the SLAM problem by splitting the jointposterior p(x1:t ,m | z1:t ,u1:t−1) into two separate parts [4,6]. The first part is about estimating the robot trajectory, thatis, x1:t , using a particle filter (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(x1:t ,m | z1:t,u1:t−1)= p(m | x1:t , z1:t)·p(x1:tz1:t,u1:t−1).(1)

While using a particle filter to estimate the robot’s poten-tial trajectories, a motion model is needed regarding the

generation of the new particles {x(i)t } from the corresponding

previous 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 howwell 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, themap is updated for each remaining particles using the currentmeasurements from the sensor. By repeatedly executing thesteps mentioned above, we can maintain a set of particles atany given time, indicating the mostly likely robot’s 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. System Overview. Figure 1 shows the overall flowchart ofour lightweight RBPF SLAM algorithm. When a robot startsexecuting SLAM in an unknown environment, we will firstset the robot’s 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 finder according to

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 flowchart of a particle filter-based algorithm.

the initial pose. After initializing the system, it will repeatedlyestimates the correct robot pose using a particle filter 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)filter, which is one of the most commonly used particle filter-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 assignedan importance weight to determine the accuracy ofthe 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 finder 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 ofall, we need to extract the line segments from the raw scandata provided by a laser range finder; this will allow usto have the information about the environment the robotcurrently sees. But extracting the line segments for everynew particle generated in the particle generation step willcost an enormous effort, 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 findout 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 mainlyconsider the orthogonal lines [3] extracted from the laserrange finder, to filter 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 ofeach step mentioned in Figure 1 to implement a lightweightRBPF SLAM for indoor environments.

3.2. Particle Generation. This is the first step of the particlefiltering 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 configuration, thenby using this information to modify its parameters andincrease the accuracy of the model. The motion model canbe written as

xt = xt−1 + D cos(θt−1 +

T2

)+ C cos

(θt−1 +

T + π2

),

yt = yt−1 + D sin(θt−1 +

T2

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


Line segmentationand corner detection

Scanpoints

Least squareline fitting

Any morescan points?

Ref.scan lines

No

Yes

Rotate andshift

Motionmodel

Scan linesfor particle i

(xi, yi, θi)

Sequentialsegmentation

algorithm

Figure 2: Line extraction procedure.

L

y

ρ

x

θ

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

After the particle generation step, we will have a set ofnew particles {xi}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 relyonly on the motion model to estimate the true robot pose. So,we need the help of a laser range finder, which is significantlymore precise, to provide the information of the current envi-ronment and matches it with the map we have already builtto filter 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 ofour line extraction algorithm, where an enhanced sequentialsegmentation algorithm is used to extract a set of referenceline segments from raw scan data provided by a laser rangefinder, and, then, we rotate and shift the reference scan linesto the corresponding particle pose (xi, yi, θ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 fitting is a techniquethat finds the best fitting line L : x · cosθ + y · sin θ = ρ toa given set of sample points {(xi, yi)}i=1,...,n, which minimizesthe following error:

Efit =n∑i=1

(xi cos θ + yi sin θ

)− ρ, (3)

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

θ = 12

atan 2−2(Sxy −NXNYN

)(Syy − Sxx

)− (Y 2

N − X2N

) ,

ρ = XN cosθ + YN sin θ,

(4)

where

Sxx =N∑i=1

x2i , Syy =

N∑i=1

y2i , Sxy =

N∑i=1

xi yi,

XN = 1N

N∑i=1

xi, YN = 1N

N∑i=1

yi.

(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 firstintroduced in [18], which is known as a very efficient methodfor line extraction [18, 20]. The main difference between ourwork and [18] is that we enhanced the algorithm by adding acorner detection technique and an adaptive breakpoint detectorfor line segmentation. This will lead to better quality incomplex indoor environments.

Sequential segmentation algorithm works by first givinga set of raw scan data, denoted as {di}i=1,...,N . If the distancebetween three consecutive points {dk, . . . ,dk+2}1�k�N−2 arewithin a threshold δd , which is ‖dk+1 − dk‖ � δd and‖dk+2− dk+1‖ � δd, then a line L is extracted from the points{dk, . . . ,dk+2} using least square fitting [19]. After finding


dk

dk+nStart point

End point

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

dn+2

dn

L2

L1θ12

Figure 5: An example of corner detection.

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

(a) ‖dk+3 − dk+2‖ � δd,

(b) the perpendicular distance from scan point dk+3 toline L is within a threshold.

If the two conditions all achieves, we will merge the scanpoint dk+3 into line L by using a sequential least squaremethod proposed in [18], and the next point dk+4 is tested.Otherwise, a new scan line Si is determined from L, and thesame process is repeated for extracting new scan lines startingfrom dk+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{dk,dk+1, . . . ,dk+n}, the two terminal points of the new scanline are determined by projecting the first point dk (startpoint) and the last point dk+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, byusing 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 finder and the scan pointincreases. So, δd can be defined as

δd = rk+1 · sinΔϕ

sin(λ− Δϕ

) + 3σr , (6)

where Δϕ is the angular resolution of a laser range finder, rk+1

is the distance between laser range finder and scan point dk+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 finder.

Second, a corner detection technique is performed whileadding new consecutive scan point dn into the line L, sonow we not only detect if the distance ‖dn − dn−1‖ and theperpendicular distance from dn to L are under a threshold,but also detect if dn 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 dn and its next two consecu-tive points {dn+1,dn+2} can be merged into a line L2,which is positive when the distance from dn to dn+1

and dn+1 to dn+2 are both under a threshold δd.

(b) Detect if the perpendicular distance from dn+2 to L1

is 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 currentlyextracting, that is, L1, and L2, which are extractedfrom the three new scan points {dn,dn+1,dn+2}. If θ12

is 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 defined as.

Si ={(

spi, epi

),N , Sxx, Syy , Sxy ,XN ,YN

}, (7)

where (spi, epi) are the start point and end point of the scanline and N , Sxx, Syy , Sxy , XN , YN are parameters generatedin the least square fitting 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 bythe motion model has its own pose (xi, yi, θ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 rawscan 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 property—there 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 firstand 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 particle’srelative location to the reference particle.


{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

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

Any nonevaluatedscan line

Figure 6: Overall process of data association.

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

(a) Rotation

Sxxrot = Sxxrefcos2θi − 2Sxyref sin θi cosθi + S

yyrefsin2θi,

Syyrot = Sxxrefsin2θi + 2S

xyref sin θi cosθi + S

yyrefcos2θi,

Sxyrot =

(Sxxref − S

yyref

)sin θi cosθi + S

xyref

(cos2θi − sin2θi

),

Xnrot = cosθi · Xn

ref − sin θi · Ynref,

Ynrot = sin θi · Xn

ref + cosθi · Ynref.

(8)

(b) Shift

Sxxshift = Sxxrot + 2N · xi·Xnrot + N · x2

i ,

Syyshift = S

yyrot + 2N·yi · Yn

rot + N · y2i ,

Sxyshift = S

xyrot + N · yi · Xn

rot + N · xi · Ynrot + N · xi · yi,

Xnshift = Xn

rot + xi,

Ynshift = Yn

rot + yi,(9)

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

x = (xref · cosθi − yref · sin θi)

+ xi,

y = (yref · cosθi + xref · sin θi)

+ yi.(10)

3.4. Data Association. Data association is to find 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.

As shown in Figure 6, before entering the data 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 ofnew 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 filter out erroneousscan lines caused by sensor noise or line extraction error toincrease the accuracy of the algorithm. The details 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 bya 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 efficiently reducethe search space for finding 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 finder are orthogonal lines. This is doneby first determining the most observed line in the local mapas 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 wj

(θj − π/2

)∑N||

i=1 wi +∑N⊥

j=1 wj

, (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


Start point

End point

Robot’scurrent 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 modifies 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 first 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 ofusing 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 boxin Figure 7) have different 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 find 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 defined as a new observed line and be added into themap during the map-update step. Otherwise, the scan lineis able to find the best match, which means it had alreadybeen observed by the robot previously and can be taken intoaccount to calculate the particles importance weighting.

Our line matching algorithm for finding 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 bycalculating Lov as follows:

lt =√

(xe − xs)2 +(ye − ys

)2,

Lov = lm + ls − lt =⎧⎨⎩

positive, if there exists overlap,

negative, otherwise,(14)

where lm is the length of a local map line LM j , ls isthe length of a scan line Si, and lt 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


Scan lineSi

(xs, ys) Lt

(xe , ye)LM j

Local map line

Figure 8: Overlap detection.

Scan lineSi

Lm

loverlap

LM j

Local map line

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

and could not be the best match. Otherwise theweighted Euclidean distance DH j [14] are computedto find 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 finding a matched pair that has the smallestweighted Euclidean distance DH j by

DH = MIN(DH1,DH 2, . . . ,DHl). (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 accordingto 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 Witot for particle i is

computed using the matching pairs we found in the dataassociation step, by first assigning each matching pair aweight as follows:

w = loverlape−DH , (17)

where DH is the weighted Euclidean distance of the matchedpair and loverlap 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

Witot =

Nm

Ntot scan

Nm∑k=1

wk, (18)

where Nm is the number of matched pairs, Ntot 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 Nm/Ntot 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 offspring particles from the motion model in thenext iteration.

3.6. Map Update. 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 memorysince 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 onlyrequires six additional parameters (N , Sxx, Syy , Sxy ,XN ,YN)of each line being merged. The RLS method operates byfirst 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 brieflyreview the RLS algorithm. More information can be found in[20].

In order to reduce the computational complexity ofmaintaining 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.


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

l1l2

Figure 11: Illustrates the perpendicular distance l1 and l2 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 inthe 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 ofdetermining if two line segments in the local map can bemerged together is similar to finding 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 l1 and l2 are the perpendicular distance from theterminal points covering the overlap section to the oppositelines, the average distance can be computed as

ldist = l1 + l22

. (19)

If the average distance ldist 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 effort and the local map updatecan merge most of the broken lines together, we only executethe global map update once in a predetermined period oftime. 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 rangefinder, wherewe also test its performance using different 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 first 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 7 m × 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 different 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 filtered out by simply considering theorthogonal lines in the map. Figure 13(a) shows the floorplanof the room, and Figure 13(b) is the map of the entire roombuilt by our algorithm, which only considers orthogonal 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 robot’s initial pose is not alignedwith the major structures of the environment.

The second experiment uses the dataset called inteloregon by M. Batalin, which was recorded at Intel Lab inHillsboro, Ore, USA, using a P2-DX robot with LSM-200laser rangefinder. As mentioned in [20], the covering area ofthis dataset is about 750 m2 and the total moving distance ofthe robot is 110 m, which contains several loops and complexindoor 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 SLAM not onlyreduces the memory usage, but also needs less computationtime as compared to the general RBPF SLAM. This is mainlydue 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 robot’s 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


(a) (b)

(c) (d)

Figure 12: Initial map built by robots starting at different 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.


(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 1500

Iteration

0200400600800

10001200

Tim

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 workneeds 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 InternationalConference 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 IndustrialElectronics (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. 1015–1021, Denver, Colo,USA, November-December 1999.

[5] A. Doucet, J. F. G. de Freitas, K. Murphy, and S. Russel,“Rao-Blackwellized particle filtering for dynamic Bayesiannetworks,” in Proceedings of the Conference on Uncertainty inArtificial Intelligence (UAI ’00), pp. 176–183, Stanford, Calif,USA, June-July 2000.

[6] G. Grisetti, C. Stachniss, and W. Burgard, “Improved tech-niques for grid mapping with rao-blackwellized particle fil-ters,” IEEE Transactions on Robotics, vol. 23, no. 1, pp. 34–46,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 NationalConference on Artificial Intelligence, Edmonton, Canada, July-August 2002.

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


in Proceedings of the the International Conference on ArtificialIntelligence (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 InternationalConference 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-ficient gridmaps in Rao-Blackwellized particle filters for SLAMusing sonar range sensors,” in Proceedings of the European Con-ference on Mobile Robots (ECMR ’07), pp. 138–143, 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 InternationalConference on Robotics and Automation (ICRA ’00), SanFrancisco, Calif, USA, April 2000.

[13] D. Hahnel, D. Schulz, and W. Burgard, “Map building withmobile robots in populated environments,” in Proceedings ofthe IEEE/RSJ International Conference on Intelligent Robots andSystems (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. 13–27, 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 efficient localization algorithmbased on vector matching for mobile robots using laser rangefinders,” Journal of Intelligent and Robotic Systems, vol. 51, no.4, pp. 461–488, 2008.

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

[20] H. J. Sohn and B. K. Kim, “VecSLAM: an efficient vector-based SLAM algorithm for indoor environments,” Journal ofIntelligent and Robotic Systems, vol. 56, no. 3, pp. 301–318,2009.

[21] G. A. Borges and M. Aldon, “Line extraction in 2D rangeimages for mobile robotics,” Journal of Intelligent and RoboticSystems, vol. 40, no. 3, pp. 267–297, 2004.

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

International Journal of

AerospaceEngineeringHindawi Publishing Corporationhttp://www.hindawi.com Volume 2010

RoboticsJournal of

Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014


Active and Passive Electronic Components

Control Scienceand Engineering

Journal of



RotatingMachinery


Hindawi Publishing Corporation http://www.hindawi.com

Journal ofEngineeringVolume 2014

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

VLSI Design



Shock and Vibration


Civil EngineeringAdvances in

Acoustics and VibrationAdvances in



Electrical and Computer Engineering

Journal of

Advances inOptoElectronics

Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014

The Scientific World JournalHindawi Publishing Corporation http://www.hindawi.com Volume 2014

SensorsJournal of


Modelling & Simulation in EngineeringHindawi Publishing Corporation http://www.hindawi.com Volume 2014


Chemical EngineeringInternational Journal of Antennas and

Propagation




Navigation and Observation



DistributedSensor Networks


alight-and-fastslamalgorithmforrobotsinindoor

Documents