# A Light-and-Fast SLAM Algorithm for Robots in Indoor Environments Using Line Segment Map

Authors: Bor-Woei Kuo; Hsun-Hao Chang; Yung-Chang Chen; Shi-Yu Huang
Journal: Journal of Robotics
 (2011)

---

## Abstract

Simultaneous Localization and Mapping (SLAM) is an important technique for robotic system navigation. Due to the high complexity 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 memory usage. Since most major structures of indoor environments are usually orthogonal to each other, we can also efficiently increase the accuracy and reduce the complexity of our algorithm by exploiting this orthogonal property of line segments, that is, we treat line segments that are parallel or perpendicular to each other in a special way when calculating the importance weight of each particle. Experimental results shows that our work is capable of drawing 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.


---

## Body

## 1. Introduction

In recent years, Simultaneous Localization and Mapping (SLAM) has become one of the basic requirements for robotic navigation. This technique allows robots to have the ability to simultaneously build up a map and localize itself in an unknown environment. The main issue involved is that while localizing a robot we need an accurate map, and for updating the map the robot needs to estimate its location accurately. The relation between robot localization and map updating is usually described as a highly complex chicken-and-egg problem.Clearly, lightweight SLAM algorithms are needed in intelligent robotic systems, because mobile robots are sometimes 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 has been 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 its path, which is a very inefficient way to clean up a room. After integrating SLAM into the robotic system [1, 2], we can develop an intelligent robotic vacuum cleaner, which is capable of planning its path in a more efficient way by using the map and location information. However, as mentioned in [3], price, size, and accuracy of the sensor is not the major problem, due to the fast progress in sensing technology. The biggest challenge will be how to develop a robust lightweight algorithm and how to integrate it into these embedded robotic systems.Rao-Blackwellized particle filter (RBPF), which was first introduced by Murphy and his colleagues [4, 5], has become one of the most successful ways of solving the SLAM problem [5–10]. Montemerlo et al. extended this ideal and proposed the FASTSLAM [7] algorithm which uses a Rao-Blackwellized particle filter to estimate the robot’s poses and tracks the location of landmarks by using an extended Kalman filter (EKF). Another efficient approach to solve the SLAM problem using RBPF is the DP-SLAM [8, 11], which uses grid maps rather than landmark maps so that it does not need any assumption on landmarks. The algorithm assigns each so-called particle (i.e., a snapshot of the environment the robot recognizes in terms of a local map and the location and pose of the robot in the map) an individual map and maintains these maps by sharing the same parts of the map between particles using a technique called distributed particle mapping, which efficiently reduce the memory needed for grid map-based RPBF SLAM. Recently [9] proposed a hybrid approach which uses the grid map as the main map structure and enhance the grid map by a set of line segment maps; as a result, this hybrid approach is able to increase the accuracy of the algorithm.However, the techniques mentioned above needs large amount of memory and high computational resource to achieve good results. This is due to the fact that most of the RBPF-based SLAM needs to maintain a large number of particles where each particle has its own map. The most commonly used map structure in RBPF SLAM is still the grid map, because it is capable of describing objects of random shapes and can easily calculate the importance weight of each particle by using scan matching methods [12, 13], but there is a drawback. Since each grid map is built in a fixed-sized 2D array where each cell represents a specific coordinate location, it requires extremely large amount of memory for storing these grid maps.In this work, we aim to develop a lightweight laser range finder-based SLAM algorithm for indoor environments using Rao-Blackwellized particle filter (RBPF), while most of the major structures (walls, doors, etc.) and furniture of indoor environments can be easily represented as line segments, we decided to use line segments as our map structure instead of grid map. Also when storing line segments into the map, we only need to store the location and parameters of each line segment. As a result, the memory requirement is much more efficient as compared to grid map.On the other hand, we also use the orthogonal property of indoor environments to increase accuracy of our algorithm. This property is based on the fact that most structures of indoor environments are parallel or perpendicular to each other [3]. By just considering these orthogonal lines, we can filter out most of the erroneous line segments caused by sensor noise or line extraction error. Although this concept has already been used in many other works [3, 14, 15], there is still a main differentiating factor in our work—we dynamically modify the reference direction, which is used to identify the orthogonal lines, rather than just aligning the lines with the x-axis and y-axis. This is extremely important since it can thereby allow the robot to have the ability to start its initial pose at any angle and need not to align its initial direction with the major structures of the environment.The rest of this paper is organized as follows. Section2 quickly describes how the Rao-Blackwellized particle filter solves the SLAM problem. In Section 3, we will discuss the details of our lightweight RBPF SLAM algorithm. Section 4 presents the experimental results, and Section 5 concludes.


## 2. RBPF for SLAM

As mentioned in [16], a full SLAM problem is to estimate the joint posteriorby which a robot recognizes the environment after moving around for a while. This joint posterior is denoted as p(x1:t,m∣z1:t,u1:t-1), where x1:t denotes the entire robot trajectory estimated so far from time instant 1 to time instant t, m is the associated map in terms of some data structure (e.g., grid map or line segment based map in our system). The joint posterior is constantly derived and updated based on two given pieces of information—the observations z1:t from the environment-measuring sensor (which is a laser range finder in our system), and the odometry measurements u1:t-1, which is a rough estimation of the trail a robot have traveled up to some point in time. Since odometry is based on how many rounds each of the robot’s two wheels has turned, it often has significant errors referred to as odometry noise. A robust SLAM algorithm should be able to derive accurate joint posterior p(x1:t,m∣z1:t,u1:t-1) under significant observation inaccuracy and/or odometry noise.The key idea of the Rao-Blackwellized particle filter based algorithm is to solve the SLAM problem by splitting the joint posteriorp(x1:t,m∣z1:t,u1:t-1) into two separate parts [4, 6]. The first part is about estimating the robot trajectory, that is, x1:t, using a particle filter (to be explained in detail later). The second part is about the update of the map based on the derived trajectory. With this concept, we can rewrite the formula in two cascaded parts as(1)p(x1:t,m∣z1:t,u1:t-1)=p(m∣x1:t,z1:t)⋅p(x1:tz1:t,u1:t-1).While using a particle filter to estimate the robot’s potential trajectories, amotion model is needed regarding the generation of the new particles {xt(i)} from the corresponding previous particle {xt-1(i)} by taking into account the odometry data. Due to the enormous error of the odometer, we need to determine how accurate each new particle is, that is, we assign each of them an importance weight according to how well the current measurement of the environment matches with the map. After assigning each particle an importance weight, the particles that have lower weight will be discarded, until only a small number of particles are left. Finally, the map is updated for each remaining particles using the current measurements from the sensor. By repeatedly executing the steps mentioned above, we can maintain a set of particles at any given time, indicating the mostly likely robot’s poses and a map built by its previous trajectory. By doing so, the robot is capable of moving around in an unknown environment without getting lost.


## 3. Proposed Lightweight RBPF SLAM

### 3.1. System Overview

Figure1 shows the overall flowchart of our lightweight RBPF SLAM algorithm. When a robot starts executing SLAM in an unknown environment, we will first set the robot’s initial pose at the origin of its coordinate system as (x,y,θ)=(0,0,0), and build an initial map using the information from the laser range finder according to the initial pose. After initializing the system, it will repeatedly estimates the correct robot pose using a particle filter and update the map of each particle by a line merging technique each time when the robot moves farther than a distance.Figure 1
Overall flowchart of a particle filter-based algorithm.Usually an RBPF-based SLAM algorithm can be implemented by using theSampling Importance Resampling (SIR) filter, which is one of the most commonly used particle filtering algorithms [6], and a map update technique. The entire procedure can be summarized in the following steps,(1)
Sampling: New particles are generated from the previous 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 observation matches the map it has already built.(3)
Resampling: Particles with low weights are likely to be replaced by the ones with high weights.(4)
Map Update: The most current map observed by the laser range finder is updated to each remaining particle after the resampling step according to its individual pose, so that each particle has a most updated map of the environment.Because of using line segments as our map structure, we need some extra procedure to maintain these lines. First of all, we need to extract the line segments from the raw scan data provided by a laser range finder; this will allow us to have the information about the environment the robot currently sees. But extracting the line segments for every new particle generated in the particle generation step will cost an enormous effort, so our algorithm only extracts a set of reference line segments, then by rotating and shifting this set of line segments to the corresponding new particles pose. In the importance weighting step, we need to find out the relation between the new extracted line segments and 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 laser range finder, to filter out erroneous lines caused by sensor noise or line extraction error. Finally, in the map-update step, line segments which are too close to each other are merged together to maintain consistency of our map and lower the number of line segments. After updating the map for each particle, we will wait until the robot has moved farther than a distance before starting the new iteration over again.In the following subsections, we will discuss the details of each step mentioned in Figure1 to implement a lightweight RBPF SLAM for indoor environments.


### 3.2. Particle Generation

This is the first step of the particle filtering process, where a motion model is used to generate new potential robot poses according to its previous pose based on the odometry data. These potential robot poses are usually called particles. The motion model we used here can be found in [17], where the authors proposed a motion model that is capable of capturing the relationship between odometry data and the change of robot configuration, then by using this information to modify its parameters and increase the accuracy of the model. The motion model can be written as(2)xt=xt-1+Dcos⁡⁡(θt-1+T2)+Ccos⁡⁡(θt-1+T+π2),yt=yt-1+Dsin⁡(θt-1+T2)+Csin⁡(θt-1+T+π2),θt=θt-1+Tmod⁡2π,
where D, T, and C are three noise models which represent the robots travel distance, amount of turns performed, and the shift in the orthogonal direction to the major axis, respectively.After the particle generation step, we will have a set of new particles{xi}i=1,…,m generated by the motion model, each representing a potential pose of the robot (x,y,θ) and an updated map considering its previous trajectory. Due to the enormous error of the odometry data, we cannot rely only on the motion model to estimate the true robot pose. So, we need the help of a laser range finder, which is significantly more precise, to provide the information of the current environment and matches it with the map we have already built to filter out particles that are too much inconsistent with the true pose, which will be introduced in the following sections.


### 3.3. Line Segment Extraction

The accuracy of a feature-based SLAM algorithm can be increased by using a robustfeature extraction method; this is because feature-based SLAM heavily depends on the features extracted from the sensor to 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 reference line segments from raw scan data provided by a laser range finder, and, then, we rotate and shift the reference scan lines to the corresponding particle pose (xi,yi,θi) generated by the motion model, so that each particle has its own set of scan lines.Figure 2
Line extraction procedure.In this section, we will describe how to enhance the sequential segmentation algorithm proposed by [18], and creating scan lines for each new particle by rotating and shifting the reference scan lines.


#### 3.3.1. Least Square Fitting

Least square fitting is a technique that finds the best fitting lineL:x·cosθ+y·sinθ=ρ to a given set of sample points {(xi,yi)}i=1,…,n, which minimizes the following error:(3)Efit=∑i=1n(xicos⁡⁡θ+yisin⁡θ)-ρ,θ and ρ can be computed by using a least square method [19] as follows:(4)θ=12atan2-2(Sxy-NXNYN)(Syy-Sxx)-(YN2-XN2),ρ=XNcos⁡⁡θ+YNsin⁡θ,
where(5)Sxx=∑i=1Nxi2,Syy=∑i=1Nyi2,Sxy=∑i=1Nxiyi,XN=1N∑i=1Nxi,YN=1N∑i=1Nyi.The relation of lineL:x·cosθ+y·sinθ=ρ between Hough space and x-y plane are shown in Figure 3.Figure 3
The relationship of lineL in two different spaces, where the black dots are the points that contains line L.


#### 3.3.2. Enhanced Sequential Segmentation Algorithm

The sequential segmentation line extraction algorithm was first introduced in [18], which is known as a very efficient method for line extraction [18, 20]. The main difference between our work and [18] is that we enhanced the algorithm by adding a corner detection technique and an adaptive breakpoint detector for line segmentation. This will lead to better quality in complex indoor environments.Sequential segmentation algorithm works by first giving a set of raw scan data, denoted as{di}i=1,…,N. If the distance between three consecutive points {dk,…,dk+2}1≦k≦N-2 are within 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 the first three consecutive points that can be extracted into a line segment L, we will determine whether the next consecutive point dk+3 can be merged into line L by the following steps:(a)
∥dk+3-dk+2∥≦δd,(b)
the perpendicular distance from scan pointdk+3 to line L is within a threshold.If the two conditions all achieves, we will merge the scan pointdk+3 into line L by using a sequential least square method proposed in [18], and the next point dk+4 is tested. Otherwise, a new scan line Si is determined from L, and the same process is repeated for extracting new scan lines starting from 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 scan line are determined by projecting the first point dk (start point) and the last point dk+n (end point) onto the scan line as shown in Figure 4.Figure 4
Projecting the start/end pointsdk and dk+n onto the extracted line.The enhanced sequential segmentation algorithm takes place in two parts in the original algorithm. First, we determine the threshold valueδd, which decides if two consecutive points 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 the distance between two consecutive points will increase while the distance between the laser range finder and the scan point increases. So, δd can be defined as(6)δd=rk+1⋅sin⁡Δφsin⁡(λ-Δφ)+3σr,
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 a result, by dynamically changing the threshold δd can let our line extraction algorithm achieve a better quality in detecting break points when two consecutive scan points are very close to or very far away from the laser range finder.Second, a corner detection technique is performed while adding new consecutive scan pointdn into the line L, so now we not only detect if the distance ∥dn-dn-1∥ and the perpendicular 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 the following steps, which is also shown in Figure 5.(a)
Detect if the scan pointdn and its next two consecutive 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 fromdn+2 to L1 is larger than a threshold.(c)
If one of the two conditions mentioned above does not achieve, the scan point will not be at the corner of two line segments. Otherwise, we will calculate the angleθ12 between the lines we are currently extracting, that is, L1, and L2, which are extracted from the three new scan points {dn,dn+1,dn+2}. If θ12 is smaller than a threshold, a corner is detected.Figure 5
An example of corner detection.After the enhanced sequential segmentation has completed, we will have a set of scan lines{si}i=1,…,N extracted from the raw scan data, and each scan line is defined as.(7)Si={(spi,epi),N,Sxx,Syy,Sxy,XN,YN},
where (spi,epi) are the start point and end point of the scan line and N, Sxx, Syy, Sxy, XN, YN are parameters generated in the least square fitting step, which are used to speed up the algorithm while updating the scan lines and merging two line segments.


#### 3.3.3. Rotate and Shift

Since each new particle generated by the motion model has its own pose(xi,yi,θi), each particle needs a set of line segments representing the information about the current environment according to its individual pose. However, due to the fact that all particles extract line segments by projecting from the same set of raw scan data, performing line extraction for each particle is a very inefficient way since it has a lot of repeated and redundant computation. In our work, we speed up the process tremendously by exploiting a property—there exists shifting and rotating relations between scan lines derived for the particles generated by the motion model, that is, once we have derived the scan lines of one particle, we can use them as the reference scan lines, and map these reference scan lines into those for some other particle via shifting and rotation transforms. In detail, we pick up a reference particle first and generate a set of reference scan lines from the raw scan data for this particle. Then for every other particle, we rotate and shift the reference scan lines according to the particle’s relative location to the reference particle.The way of rotating and shifting the reference scan lines is inspired by the sequence least square fitting algorithm [18], where we do not need to recalculate all the parameters using least square fitting for each particle, but only updates the six parameters of a line segment in a sequential manner as follows.(a) Rotation
(8)Srot⁡xx=Srefxxcos⁡2⁡θi-2Srefxysin⁡θicos⁡⁡θi+Srefyysin2⁡θi,Srot⁡yy=Srefxxsin2⁡θi+2Srefxysin⁡θicos⁡⁡θi+Srefyycos⁡2⁡θi,Srot⁡xy=(Srefxx-Srefyy)sin⁡θicos⁡⁡θi+Srefxy(cos⁡2⁡θi-sin2⁡θi),Xrot⁡n=cos⁡⁡θi⋅Xrefn-sin⁡θi⋅Yrefn,Yrot⁡n=sin⁡θi⋅Xrefn+cos⁡⁡θi⋅Yrefn.(b) Shift
(9)Sshiftxx=Srot⁡xx+2N⋅xi⋅Xrot⁡n+N⋅xi2,Sshiftyy=Srot⁡yy+2N⋅yi⋅Yrot⁡n+N⋅yi2,Sshiftxy=Srot⁡xy+N⋅yi⋅Xrot⁡n+N⋅xi⋅Yrot⁡n+N⋅xi⋅yi,Xshiftn=Xrot⁡n+xi,Yshiftn=Yrot⁡n+yi,
where the start/end points of each line can also be computed by rotating and shifting from the reference scan lines by
(10)x=(xref⋅cos⁡⁡θi-yref⋅sin⁡θi)+xi,y=(yref⋅cos⁡⁡θi+xref⋅sin⁡θi)+yi.


### 3.4. Data Association

Data association is to find the relationship between the new extracted scan lines and the map lines which we have already built in the map for each particle, and by using this relationship to calculate an importance weight of each particle to indicate the correctness of its pose. As we can see, the accuracy of data association is a crucial issue to achieve a robust SLAM algorithm.As shown in Figure6, before entering the data association step, a local map is created to reduce search space while matching scan lines with the map; this is due to the fact that the motion model will generate a large amount of new particles (i.e., 500 particles per iteration), where each particle has its own set of scan lines and each scan line needs to be matched with the entire map. Also, the orthogonal assumption for indoor environments [3] is performed in the orientation step, where we only consider scan lines which are parallel or perpendicular to each other. This is because most major structures of indoor environments usually have the orthogonal relationship, and by using this assumption we can reduce the number of scan lines and filter out erroneous scan lines caused by sensor noise or line extraction error to increase the accuracy of the algorithm. The details of building a local map, the orientation step, and data association are described below:Figure 6
Overall process of data association.


#### 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 all the scan lines of the new particles generated by the same previous particle, which we called parent particle. All the new particles generated from the same parent particle shares a local map, because the maps they contain are built from the same previous robot trajectory. This can let us reduce the number of times to build local maps, and efficiently reduce the search space for finding the best match of each scan line in the data association step.Also a reference directionθref is calculated in order to let the orientation step determine which scan line extracted from the laser range finder are orthogonal lines. This is done by first determining the most observed line in the local map as a reference line, which is the line that has been observed most of the time. If there is more than one most observed line, the longest one is picked. Then, by using all the lines that are orthogonal to the reference line in the local map to compute a weighted reference direction similar to the method proposed in [3] by the following formula:(11)θref=∑i=1N||wiθi+∑j=1N⊥wj(θj-π/2)∑i=1N||wi+∑j=1N⊥wj,
where N|| and N⊥ are the number of lines that are parallel and perpendicular to the reference line, respectively, wi is the length of line i, and θi is the angular parameter of line i. Our work dynamically modifies the reference direction each time the local map is built rather than just aligning the orthogonal lines with the x axis, so that the robot is capable of starting its initial pose without aligning with the major structures of the environments and can still correctly detect the orthogonal lines.


#### 3.4.2. Orientation

The orientation step uses the reference directionθrefto identify whether the scan line is an orthogonal line by the following equation:(12)|θi-θref|≤εθ,paralleltoθref,|θi-θref-π2|≤εθ,perpendiculartoθref,
where εθ is a threshold about 5° and θi is the angle of a scan line i in polar coordinates. If a line segment does not match one of the above two equations, it is considered as a nonorthogonal line and will not be taken into account for calculating the importance weight of the particle.


#### 3.4.3. Vector-Based Line Representation

Before discussing the details of the data association step, we will explain thevector-based line representation technique [18, 20, 21] used in our line matching method, in which each line segment in the map is assigned a direction starting from its first point to its last as shown in Figure 7(a), so that each line segment can be presented as a vector. The main advantage of using a vector-ased line representation is we can avoid mismatch in enclosed areas (e.g., opposite sides of a thin wall) by determining the direction of the vectors. Figure 7(b) shows an example of using vector lines to represent an indoor environment, where the robot starts at one side of the wall (at starting point A) and moves along to the other point B. We can see that the vectors representing two sides of the wall (enclosed in a box in Figure 7) have different directions, and thus they can be easily distinguished from each other to avoid mismatch in the data association step.(a) Vector lines determined by its start/end points. (b) Example of vector lines representing opposite sides of a wall.
(a)(b)


#### 3.4.4. Data Association

In the data association step, only orthogonal scan lines{Si}i=1,…,N of each particle are taken into account to find a best match in the local map {LMj}j=1,…,l. If a scan line does not matches any lines in the map, it will be defined as a new observed line and be added into the map during the map-update step. Otherwise, the scan line is able to find the best match, which means it had already been observed by the robot previously and can be taken into account to calculate the particles importance weighting.Our line matching algorithm for finding the best match in the local map{LMj}j=1,…,l for each scan line Si is done by the following procedures.(a)
Detect if the angleθij between a scan line Si and a local map line LMj is under a threshold. Here, we determine the line segments as vectors, so we can compute the angle quickly by
(13)θij=cos⁡-1⁡(Si⇀⋅LMj⇀|Si⇀||LMj⇀|).(b)
Detect if there is an overlap betweenSi and LMj by calculating Lov as follows:
(14)lt=(xe-xs)2+(ye-ys)2,Lov=lm+ls-lt={positive,ifthereexistsoverlap,negative,otherwise,
where lm is the length of a local map line LMj, ls is the length of a scan line Si, and lt is the length of a potential line Lt by merging Si and LMj, as shown in Figure 8.(c)
If one of the conditions mentioned above does not achieve, this means thatSi and LMj are too far away and could not be the best match. Otherwise the weighted Euclidean distance DHj [14] are computed to find out how well Si and LMj matches by using the parameters θ and ρ of the two lines in Hough space as follows:(15)DHj=wρ(ρs-ρm)2+wθ(θs-θm)2,wρ+wθ=1.Figure 8
Overlap detection.After all the lines in the local map{LMj}j=1,…,l are matched with the scan line Si, we can determine the best match by finding a matched pair that has the smallest weighted Euclidean distance DHj by(16)DH=MIN(DH1,DH2,…,DHl).If no matching pairs are found for a scan lineSi, we will determine the scan line as a new discovered line, and therefore insert it into the map in the map-update stage.


### 3.5. Importance Weighting

Each particle generated from the motion model is assigned an importance weight according to how well its current measurement of the environment matches the map we have already built, so that we can determine the accuracy of each particle by its importance weighting.In our case, the importance weightWtoti for particle i is computed using the matching pairs we found in the data association step, by first assigning each matching pair a weight as follows:(17)w=loverlape-DH,
where DH is the weighted Euclidean distance of the matched pair and loverlap is the overlap distance between the matched scan line and the map line, which is shown in Figure 9.Figure 9
Illustration of the overlap distanceloverlap between the matching pair Si and LMj.After all the matched pairs are assigned a weight, we can compute the importance weight of theith particle by(18)Wtoti=NmNtot_scan∑k=1Nmwk,
where Nm is the number of matched pairs, Ntot_scan is the total number of orthogonal scan lines extracted in this iteration, and wk is the weight of the kth matching pair. The main purpose of multiplying the ratio Nm/Ntot_scan is to avoid some particles having extremely high importance weight caused by matching pairs which have long overlap distance between them.After assigning each particle an importance weight, the resampling step will take place by deleting the ones that have lower weights. Also particles that are assigned a higher weight (meaning that it has a higher probability to be the correct robot pose) will have the opportunity to generate more new offspring particles from the motion model in the next iteration.


### 3.6. Map Update

The map of each particle is maintained and updated in this step, where new observed lines are added to the map and lines that are close to each other, will be merged into a single line segment.Figure10 shows an example of merging a scan line Si and a map line LMj into a single line Lm, where Lm is generated using all the scan points constituting Si and LMj. By considering all the points, line segments can be merged more accurately, but with higher requirement of memory since it requires the storing of all the points constituting the individual lines. To overcome this issue, the recursive least-squares (RLSs) method [20] for line merging is used in our work, which does not need to store all the points, but only requires six additional parameters (N,Sxx,Syy,Sxy,XN,YN) of each line being merged. The RLS method operates by first updating the six parameters for the merged line and then determines the start/end points of the line by projecting the terminal points onto the new line. Here, we briefly review the RLS algorithm. More information can be found in [20].Figure 10
Merging two linesSi and LMj by considering all the scan points.In order to reduce the computational complexity of maintaining the map for each particle, the map-update procedure is split into two modes: (1) local map update and (2) global map update, where the local map update executes at every iteration and the global map update only executes once in a period of time.


#### 3.6.1. Local Map Update

In this step, new scan lines are added into the map and the matched pairs determined in the data association step will be merged together. Also line segments in the local map are merged if they are close to each other and have an overlap between them. The way of determining if two line segments in the local map can be merged together is similar to finding the matching pairs in the data association step, but instead of calculating the weighted Euclidean distance, we only calculates the average distance between two overlapped lines to determine if the lines can be matched as shown in Figure11.Figure 11
Illustrates the perpendicular distancel1 and l2 from the terminal points in the overlap section to the opposite line.Wherel1 and l2 are the perpendicular distance from the terminal points covering the overlap section to the opposite lines, the average distance can be computed as(19)ldist⁡=l1+l22.
If the average distance ldist is smaller than a threshold, the two lines are close to each other and will be merged together.


#### 3.6.2. Global Map Update

In order to avoid having several broken line segments that are meant to be the same line in the map, we need to check all the map lines and determine whether the lines can be merged together by using the same way as in local map update, but this time the whole map will be tested. Due to the fact that checking all the line segments will cost an extremely large effort and the local map update can 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 more efficiently and reduce the number of line segments in our map.


## 3.1. System Overview

Figure1 shows the overall flowchart of our lightweight RBPF SLAM algorithm. When a robot starts executing SLAM in an unknown environment, we will first set the robot’s initial pose at the origin of its coordinate system as (x,y,θ)=(0,0,0), and build an initial map using the information from the laser range finder according to the initial pose. After initializing the system, it will repeatedly estimates the correct robot pose using a particle filter and update the map of each particle by a line merging technique each time when the robot moves farther than a distance.Figure 1
Overall flowchart of a particle filter-based algorithm.Usually an RBPF-based SLAM algorithm can be implemented by using theSampling Importance Resampling (SIR) filter, which is one of the most commonly used particle filtering algorithms [6], and a map update technique. The entire procedure can be summarized in the following steps,(1)
Sampling: New particles are generated from the previous 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 observation matches the map it has already built.(3)
Resampling: Particles with low weights are likely to be replaced by the ones with high weights.(4)
Map Update: The most current map observed by the laser range finder is updated to each remaining particle after the resampling step according to its individual pose, so that each particle has a most updated map of the environment.Because of using line segments as our map structure, we need some extra procedure to maintain these lines. First of all, we need to extract the line segments from the raw scan data provided by a laser range finder; this will allow us to have the information about the environment the robot currently sees. But extracting the line segments for every new particle generated in the particle generation step will cost an enormous effort, so our algorithm only extracts a set of reference line segments, then by rotating and shifting this set of line segments to the corresponding new particles pose. In the importance weighting step, we need to find out the relation between the new extracted line segments and 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 laser range finder, to filter out erroneous lines caused by sensor noise or line extraction error. Finally, in the map-update step, line segments which are too close to each other are merged together to maintain consistency of our map and lower the number of line segments. After updating the map for each particle, we will wait until the robot has moved farther than a distance before starting the new iteration over again.In the following subsections, we will discuss the details of each step mentioned in Figure1 to implement a lightweight RBPF SLAM for indoor environments.


## 3.2. Particle Generation

This is the first step of the particle filtering process, where a motion model is used to generate new potential robot poses according to its previous pose based on the odometry data. These potential robot poses are usually called particles. The motion model we used here can be found in [17], where the authors proposed a motion model that is capable of capturing the relationship between odometry data and the change of robot configuration, then by using this information to modify its parameters and increase the accuracy of the model. The motion model can be written as(2)xt=xt-1+Dcos⁡⁡(θt-1+T2)+Ccos⁡⁡(θt-1+T+π2),yt=yt-1+Dsin⁡(θt-1+T2)+Csin⁡(θt-1+T+π2),θt=θt-1+Tmod⁡2π,
where D, T, and C are three noise models which represent the robots travel distance, amount of turns performed, and the shift in the orthogonal direction to the major axis, respectively.After the particle generation step, we will have a set of new particles{xi}i=1,…,m generated by the motion model, each representing a potential pose of the robot (x,y,θ) and an updated map considering its previous trajectory. Due to the enormous error of the odometry data, we cannot rely only on the motion model to estimate the true robot pose. So, we need the help of a laser range finder, which is significantly more precise, to provide the information of the current environment and matches it with the map we have already built to filter out particles that are too much inconsistent with the true pose, which will be introduced in the following sections.


## 3.3. Line Segment Extraction

The accuracy of a feature-based SLAM algorithm can be increased by using a robustfeature extraction method; this is because feature-based SLAM heavily depends on the features extracted from the sensor to 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 reference line segments from raw scan data provided by a laser range finder, and, then, we rotate and shift the reference scan lines to the corresponding particle pose (xi,yi,θi) generated by the motion model, so that each particle has its own set of scan lines.Figure 2
Line extraction procedure.In this section, we will describe how to enhance the sequential segmentation algorithm proposed by [18], and creating scan lines for each new particle by rotating and shifting the reference scan lines.


### 3.3.1. Least Square Fitting

Least square fitting is a technique that finds the best fitting lineL:x·cosθ+y·sinθ=ρ to a given set of sample points {(xi,yi)}i=1,…,n, which minimizes the following error:(3)Efit=∑i=1n(xicos⁡⁡θ+yisin⁡θ)-ρ,θ and ρ can be computed by using a least square method [19] as follows:(4)θ=12atan2-2(Sxy-NXNYN)(Syy-Sxx)-(YN2-XN2),ρ=XNcos⁡⁡θ+YNsin⁡θ,
where(5)Sxx=∑i=1Nxi2,Syy=∑i=1Nyi2,Sxy=∑i=1Nxiyi,XN=1N∑i=1Nxi,YN=1N∑i=1Nyi.The relation of lineL:x·cosθ+y·sinθ=ρ between Hough space and x-y plane are shown in Figure 3.Figure 3
The relationship of lineL in two different spaces, where the black dots are the points that contains line L.


### 3.3.2. Enhanced Sequential Segmentation Algorithm

The sequential segmentation line extraction algorithm was first introduced in [18], which is known as a very efficient method for line extraction [18, 20]. The main difference between our work and [18] is that we enhanced the algorithm by adding a corner detection technique and an adaptive breakpoint detector for line segmentation. This will lead to better quality in complex indoor environments.Sequential segmentation algorithm works by first giving a set of raw scan data, denoted as{di}i=1,…,N. If the distance between three consecutive points {dk,…,dk+2}1≦k≦N-2 are within 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 the first three consecutive points that can be extracted into a line segment L, we will determine whether the next consecutive point dk+3 can be merged into line L by the following steps:(a)
∥dk+3-dk+2∥≦δd,(b)
the perpendicular distance from scan pointdk+3 to line L is within a threshold.If the two conditions all achieves, we will merge the scan pointdk+3 into line L by using a sequential least square method proposed in [18], and the next point dk+4 is tested. Otherwise, a new scan line Si is determined from L, and the same process is repeated for extracting new scan lines starting from 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 scan line are determined by projecting the first point dk (start point) and the last point dk+n (end point) onto the scan line as shown in Figure 4.Figure 4
Projecting the start/end pointsdk and dk+n onto the extracted line.The enhanced sequential segmentation algorithm takes place in two parts in the original algorithm. First, we determine the threshold valueδd, which decides if two consecutive points 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 the distance between two consecutive points will increase while the distance between the laser range finder and the scan point increases. So, δd can be defined as(6)δd=rk+1⋅sin⁡Δφsin⁡(λ-Δφ)+3σr,
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 a result, by dynamically changing the threshold δd can let our line extraction algorithm achieve a better quality in detecting break points when two consecutive scan points are very close to or very far away from the laser range finder.Second, a corner detection technique is performed while adding new consecutive scan pointdn into the line L, so now we not only detect if the distance ∥dn-dn-1∥ and the perpendicular 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 the following steps, which is also shown in Figure 5.(a)
Detect if the scan pointdn and its next two consecutive 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 fromdn+2 to L1 is larger than a threshold.(c)
If one of the two conditions mentioned above does not achieve, the scan point will not be at the corner of two line segments. Otherwise, we will calculate the angleθ12 between the lines we are currently extracting, that is, L1, and L2, which are extracted from the three new scan points {dn,dn+1,dn+2}. If θ12 is smaller than a threshold, a corner is detected.Figure 5
An example of corner detection.After the enhanced sequential segmentation has completed, we will have a set of scan lines{si}i=1,…,N extracted from the raw scan data, and each scan line is defined as.(7)Si={(spi,epi),N,Sxx,Syy,Sxy,XN,YN},
where (spi,epi) are the start point and end point of the scan line and N, Sxx, Syy, Sxy, XN, YN are parameters generated in the least square fitting step, which are used to speed up the algorithm while updating the scan lines and merging two line segments.


### 3.3.3. Rotate and Shift

Since each new particle generated by the motion model has its own pose(xi,yi,θi), each particle needs a set of line segments representing the information about the current environment according to its individual pose. However, due to the fact that all particles extract line segments by projecting from the same set of raw scan data, performing line extraction for each particle is a very inefficient way since it has a lot of repeated and redundant computation. In our work, we speed up the process tremendously by exploiting a property—there exists shifting and rotating relations between scan lines derived for the particles generated by the motion model, that is, once we have derived the scan lines of one particle, we can use them as the reference scan lines, and map these reference scan lines into those for some other particle via shifting and rotation transforms. In detail, we pick up a reference particle first and generate a set of reference scan lines from the raw scan data for this particle. Then for every other particle, we rotate and shift the reference scan lines according to the particle’s relative location to the reference particle.The way of rotating and shifting the reference scan lines is inspired by the sequence least square fitting algorithm [18], where we do not need to recalculate all the parameters using least square fitting for each particle, but only updates the six parameters of a line segment in a sequential manner as follows.(a) Rotation
(8)Srot⁡xx=Srefxxcos⁡2⁡θi-2Srefxysin⁡θicos⁡⁡θi+Srefyysin2⁡θi,Srot⁡yy=Srefxxsin2⁡θi+2Srefxysin⁡θicos⁡⁡θi+Srefyycos⁡2⁡θi,Srot⁡xy=(Srefxx-Srefyy)sin⁡θicos⁡⁡θi+Srefxy(cos⁡2⁡θi-sin2⁡θi),Xrot⁡n=cos⁡⁡θi⋅Xrefn-sin⁡θi⋅Yrefn,Yrot⁡n=sin⁡θi⋅Xrefn+cos⁡⁡θi⋅Yrefn.(b) Shift
(9)Sshiftxx=Srot⁡xx+2N⋅xi⋅Xrot⁡n+N⋅xi2,Sshiftyy=Srot⁡yy+2N⋅yi⋅Yrot⁡n+N⋅yi2,Sshiftxy=Srot⁡xy+N⋅yi⋅Xrot⁡n+N⋅xi⋅Yrot⁡n+N⋅xi⋅yi,Xshiftn=Xrot⁡n+xi,Yshiftn=Yrot⁡n+yi,
where the start/end points of each line can also be computed by rotating and shifting from the reference scan lines by
(10)x=(xref⋅cos⁡⁡θi-yref⋅sin⁡θi)+xi,y=(yref⋅cos⁡⁡θi+xref⋅sin⁡θi)+yi.


## 3.3.1. Least Square Fitting

Least square fitting is a technique that finds the best fitting lineL:x·cosθ+y·sinθ=ρ to a given set of sample points {(xi,yi)}i=1,…,n, which minimizes the following error:(3)Efit=∑i=1n(xicos⁡⁡θ+yisin⁡θ)-ρ,θ and ρ can be computed by using a least square method [19] as follows:(4)θ=12atan2-2(Sxy-NXNYN)(Syy-Sxx)-(YN2-XN2),ρ=XNcos⁡⁡θ+YNsin⁡θ,
where(5)Sxx=∑i=1Nxi2,Syy=∑i=1Nyi2,Sxy=∑i=1Nxiyi,XN=1N∑i=1Nxi,YN=1N∑i=1Nyi.The relation of lineL:x·cosθ+y·sinθ=ρ between Hough space and x-y plane are shown in Figure 3.Figure 3
The relationship of lineL in two different spaces, where the black dots are the points that contains line L.


## 3.3.2. Enhanced Sequential Segmentation Algorithm

The sequential segmentation line extraction algorithm was first introduced in [18], which is known as a very efficient method for line extraction [18, 20]. The main difference between our work and [18] is that we enhanced the algorithm by adding a corner detection technique and an adaptive breakpoint detector for line segmentation. This will lead to better quality in complex indoor environments.Sequential segmentation algorithm works by first giving a set of raw scan data, denoted as{di}i=1,…,N. If the distance between three consecutive points {dk,…,dk+2}1≦k≦N-2 are within 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 the first three consecutive points that can be extracted into a line segment L, we will determine whether the next consecutive point dk+3 can be merged into line L by the following steps:(a)
∥dk+3-dk+2∥≦δd,(b)
the perpendicular distance from scan pointdk+3 to line L is within a threshold.If the two conditions all achieves, we will merge the scan pointdk+3 into line L by using a sequential least square method proposed in [18], and the next point dk+4 is tested. Otherwise, a new scan line Si is determined from L, and the same process is repeated for extracting new scan lines starting from 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 scan line are determined by projecting the first point dk (start point) and the last point dk+n (end point) onto the scan line as shown in Figure 4.Figure 4
Projecting the start/end pointsdk and dk+n onto the extracted line.The enhanced sequential segmentation algorithm takes place in two parts in the original algorithm. First, we determine the threshold valueδd, which decides if two consecutive points 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 the distance between two consecutive points will increase while the distance between the laser range finder and the scan point increases. So, δd can be defined as(6)δd=rk+1⋅sin⁡Δφsin⁡(λ-Δφ)+3σr,
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 a result, by dynamically changing the threshold δd can let our line extraction algorithm achieve a better quality in detecting break points when two consecutive scan points are very close to or very far away from the laser range finder.Second, a corner detection technique is performed while adding new consecutive scan pointdn into the line L, so now we not only detect if the distance ∥dn-dn-1∥ and the perpendicular 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 the following steps, which is also shown in Figure 5.(a)
Detect if the scan pointdn and its next two consecutive 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 fromdn+2 to L1 is larger than a threshold.(c)
If one of the two conditions mentioned above does not achieve, the scan point will not be at the corner of two line segments. Otherwise, we will calculate the angleθ12 between the lines we are currently extracting, that is, L1, and L2, which are extracted from the three new scan points {dn,dn+1,dn+2}. If θ12 is smaller than a threshold, a corner is detected.Figure 5
An example of corner detection.After the enhanced sequential segmentation has completed, we will have a set of scan lines{si}i=1,…,N extracted from the raw scan data, and each scan line is defined as.(7)Si={(spi,epi),N,Sxx,Syy,Sxy,XN,YN},
where (spi,epi) are the start point and end point of the scan line and N, Sxx, Syy, Sxy, XN, YN are parameters generated in the least square fitting step, which are used to speed up the algorithm while updating the scan lines and merging two line segments.


## 3.3.3. Rotate and Shift

Since each new particle generated by the motion model has its own pose(xi,yi,θi), each particle needs a set of line segments representing the information about the current environment according to its individual pose. However, due to the fact that all particles extract line segments by projecting from the same set of raw scan data, performing line extraction for each particle is a very inefficient way since it has a lot of repeated and redundant computation. In our work, we speed up the process tremendously by exploiting a property—there exists shifting and rotating relations between scan lines derived for the particles generated by the motion model, that is, once we have derived the scan lines of one particle, we can use them as the reference scan lines, and map these reference scan lines into those for some other particle via shifting and rotation transforms. In detail, we pick up a reference particle first and generate a set of reference scan lines from the raw scan data for this particle. Then for every other particle, we rotate and shift the reference scan lines according to the particle’s relative location to the reference particle.The way of rotating and shifting the reference scan lines is inspired by the sequence least square fitting algorithm [18], where we do not need to recalculate all the parameters using least square fitting for each particle, but only updates the six parameters of a line segment in a sequential manner as follows.(a) Rotation
(8)Srot⁡xx=Srefxxcos⁡2⁡θi-2Srefxysin⁡θicos⁡⁡θi+Srefyysin2⁡θi,Srot⁡yy=Srefxxsin2⁡θi+2Srefxysin⁡θicos⁡⁡θi+Srefyycos⁡2⁡θi,Srot⁡xy=(Srefxx-Srefyy)sin⁡θicos⁡⁡θi+Srefxy(cos⁡2⁡θi-sin2⁡θi),Xrot⁡n=cos⁡⁡θi⋅Xrefn-sin⁡θi⋅Yrefn,Yrot⁡n=sin⁡θi⋅Xrefn+cos⁡⁡θi⋅Yrefn.(b) Shift
(9)Sshiftxx=Srot⁡xx+2N⋅xi⋅Xrot⁡n+N⋅xi2,Sshiftyy=Srot⁡yy+2N⋅yi⋅Yrot⁡n+N⋅yi2,Sshiftxy=Srot⁡xy+N⋅yi⋅Xrot⁡n+N⋅xi⋅Yrot⁡n+N⋅xi⋅yi,Xshiftn=Xrot⁡n+xi,Yshiftn=Yrot⁡n+yi,
where the start/end points of each line can also be computed by rotating and shifting from the reference scan lines by
(10)x=(xref⋅cos⁡⁡θi-yref⋅sin⁡θi)+xi,y=(yref⋅cos⁡⁡θi+xref⋅sin⁡θi)+yi.


## 3.4. Data Association

Data association is to find the relationship between the new extracted scan lines and the map lines which we have already built in the map for each particle, and by using this relationship to calculate an importance weight of each particle to indicate the correctness of its pose. As we can see, the accuracy of data association is a crucial issue to achieve a robust SLAM algorithm.As shown in Figure6, before entering the data association step, a local map is created to reduce search space while matching scan lines with the map; this is due to the fact that the motion model will generate a large amount of new particles (i.e., 500 particles per iteration), where each particle has its own set of scan lines and each scan line needs to be matched with the entire map. Also, the orthogonal assumption for indoor environments [3] is performed in the orientation step, where we only consider scan lines which are parallel or perpendicular to each other. This is because most major structures of indoor environments usually have the orthogonal relationship, and by using this assumption we can reduce the number of scan lines and filter out erroneous scan lines caused by sensor noise or line extraction error to increase the accuracy of the algorithm. The details of building a local map, the orientation step, and data association are described below:Figure 6
Overall process of data association.


### 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 all the scan lines of the new particles generated by the same previous particle, which we called parent particle. All the new particles generated from the same parent particle shares a local map, because the maps they contain are built from the same previous robot trajectory. This can let us reduce the number of times to build local maps, and efficiently reduce the search space for finding the best match of each scan line in the data association step.Also a reference directionθref is calculated in order to let the orientation step determine which scan line extracted from the laser range finder are orthogonal lines. This is done by first determining the most observed line in the local map as a reference line, which is the line that has been observed most of the time. If there is more than one most observed line, the longest one is picked. Then, by using all the lines that are orthogonal to the reference line in the local map to compute a weighted reference direction similar to the method proposed in [3] by the following formula:(11)θref=∑i=1N||wiθi+∑j=1N⊥wj(θj-π/2)∑i=1N||wi+∑j=1N⊥wj,
where N|| and N⊥ are the number of lines that are parallel and perpendicular to the reference line, respectively, wi is the length of line i, and θi is the angular parameter of line i. Our work dynamically modifies the reference direction each time the local map is built rather than just aligning the orthogonal lines with the x axis, so that the robot is capable of starting its initial pose without aligning with the major structures of the environments and can still correctly detect the orthogonal lines.


### 3.4.2. Orientation

The orientation step uses the reference directionθrefto identify whether the scan line is an orthogonal line by the following equation:(12)|θi-θref|≤εθ,paralleltoθref,|θi-θref-π2|≤εθ,perpendiculartoθref,
where εθ is a threshold about 5° and θi is the angle of a scan line i in polar coordinates. If a line segment does not match one of the above two equations, it is considered as a nonorthogonal line and will not be taken into account for calculating the importance weight of the particle.


### 3.4.3. Vector-Based Line Representation

Before discussing the details of the data association step, we will explain thevector-based line representation technique [18, 20, 21] used in our line matching method, in which each line segment in the map is assigned a direction starting from its first point to its last as shown in Figure 7(a), so that each line segment can be presented as a vector. The main advantage of using a vector-ased line representation is we can avoid mismatch in enclosed areas (e.g., opposite sides of a thin wall) by determining the direction of the vectors. Figure 7(b) shows an example of using vector lines to represent an indoor environment, where the robot starts at one side of the wall (at starting point A) and moves along to the other point B. We can see that the vectors representing two sides of the wall (enclosed in a box in Figure 7) have different directions, and thus they can be easily distinguished from each other to avoid mismatch in the data association step.(a) Vector lines determined by its start/end points. (b) Example of vector lines representing opposite sides of a wall.
(a)(b)


### 3.4.4. Data Association

In the data association step, only orthogonal scan lines{Si}i=1,…,N of each particle are taken into account to find a best match in the local map {LMj}j=1,…,l. If a scan line does not matches any lines in the map, it will be defined as a new observed line and be added into the map during the map-update step. Otherwise, the scan line is able to find the best match, which means it had already been observed by the robot previously and can be taken into account to calculate the particles importance weighting.Our line matching algorithm for finding the best match in the local map{LMj}j=1,…,l for each scan line Si is done by the following procedures.(a)
Detect if the angleθij between a scan line Si and a local map line LMj is under a threshold. Here, we determine the line segments as vectors, so we can compute the angle quickly by
(13)θij=cos⁡-1⁡(Si⇀⋅LMj⇀|Si⇀||LMj⇀|).(b)
Detect if there is an overlap betweenSi and LMj by calculating Lov as follows:
(14)lt=(xe-xs)2+(ye-ys)2,Lov=lm+ls-lt={positive,ifthereexistsoverlap,negative,otherwise,
where lm is the length of a local map line LMj, ls is the length of a scan line Si, and lt is the length of a potential line Lt by merging Si and LMj, as shown in Figure 8.(c)
If one of the conditions mentioned above does not achieve, this means thatSi and LMj are too far away and could not be the best match. Otherwise the weighted Euclidean distance DHj [14] are computed to find out how well Si and LMj matches by using the parameters θ and ρ of the two lines in Hough space as follows:(15)DHj=wρ(ρs-ρm)2+wθ(θs-θm)2,wρ+wθ=1.Figure 8
Overlap detection.After all the lines in the local map{LMj}j=1,…,l are matched with the scan line Si, we can determine the best match by finding a matched pair that has the smallest weighted Euclidean distance DHj by(16)DH=MIN(DH1,DH2,…,DHl).If no matching pairs are found for a scan lineSi, we will determine the scan line as a new discovered line, and therefore insert it into the map in the map-update stage.


## 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 all the scan lines of the new particles generated by the same previous particle, which we called parent particle. All the new particles generated from the same parent particle shares a local map, because the maps they contain are built from the same previous robot trajectory. This can let us reduce the number of times to build local maps, and efficiently reduce the search space for finding the best match of each scan line in the data association step.Also a reference directionθref is calculated in order to let the orientation step determine which scan line extracted from the laser range finder are orthogonal lines. This is done by first determining the most observed line in the local map as a reference line, which is the line that has been observed most of the time. If there is more than one most observed line, the longest one is picked. Then, by using all the lines that are orthogonal to the reference line in the local map to compute a weighted reference direction similar to the method proposed in [3] by the following formula:(11)θref=∑i=1N||wiθi+∑j=1N⊥wj(θj-π/2)∑i=1N||wi+∑j=1N⊥wj,
where N|| and N⊥ are the number of lines that are parallel and perpendicular to the reference line, respectively, wi is the length of line i, and θi is the angular parameter of line i. Our work dynamically modifies the reference direction each time the local map is built rather than just aligning the orthogonal lines with the x axis, so that the robot is capable of starting its initial pose without aligning with the major structures of the environments and can still correctly detect the orthogonal lines.


## 3.4.2. Orientation

The orientation step uses the reference directionθrefto identify whether the scan line is an orthogonal line by the following equation:(12)|θi-θref|≤εθ,paralleltoθref,|θi-θref-π2|≤εθ,perpendiculartoθref,
where εθ is a threshold about 5° and θi is the angle of a scan line i in polar coordinates. If a line segment does not match one of the above two equations, it is considered as a nonorthogonal line and will not be taken into account for calculating the importance weight of the particle.


## 3.4.3. Vector-Based Line Representation

Before discussing the details of the data association step, we will explain thevector-based line representation technique [18, 20, 21] used in our line matching method, in which each line segment in the map is assigned a direction starting from its first point to its last as shown in Figure 7(a), so that each line segment can be presented as a vector. The main advantage of using a vector-ased line representation is we can avoid mismatch in enclosed areas (e.g., opposite sides of a thin wall) by determining the direction of the vectors. Figure 7(b) shows an example of using vector lines to represent an indoor environment, where the robot starts at one side of the wall (at starting point A) and moves along to the other point B. We can see that the vectors representing two sides of the wall (enclosed in a box in Figure 7) have different directions, and thus they can be easily distinguished from each other to avoid mismatch in the data association step.(a) Vector lines determined by its start/end points. (b) Example of vector lines representing opposite sides of a wall.
(a)(b)


## 3.4.4. Data Association

In the data association step, only orthogonal scan lines{Si}i=1,…,N of each particle are taken into account to find a best match in the local map {LMj}j=1,…,l. If a scan line does not matches any lines in the map, it will be defined as a new observed line and be added into the map during the map-update step. Otherwise, the scan line is able to find the best match, which means it had already been observed by the robot previously and can be taken into account to calculate the particles importance weighting.Our line matching algorithm for finding the best match in the local map{LMj}j=1,…,l for each scan line Si is done by the following procedures.(a)
Detect if the angleθij between a scan line Si and a local map line LMj is under a threshold. Here, we determine the line segments as vectors, so we can compute the angle quickly by
(13)θij=cos⁡-1⁡(Si⇀⋅LMj⇀|Si⇀||LMj⇀|).(b)
Detect if there is an overlap betweenSi and LMj by calculating Lov as follows:
(14)lt=(xe-xs)2+(ye-ys)2,Lov=lm+ls-lt={positive,ifthereexistsoverlap,negative,otherwise,
where lm is the length of a local map line LMj, ls is the length of a scan line Si, and lt is the length of a potential line Lt by merging Si and LMj, as shown in Figure 8.(c)
If one of the conditions mentioned above does not achieve, this means thatSi and LMj are too far away and could not be the best match. Otherwise the weighted Euclidean distance DHj [14] are computed to find out how well Si and LMj matches by using the parameters θ and ρ of the two lines in Hough space as follows:(15)DHj=wρ(ρs-ρm)2+wθ(θs-θm)2,wρ+wθ=1.Figure 8
Overlap detection.After all the lines in the local map{LMj}j=1,…,l are matched with the scan line Si, we can determine the best match by finding a matched pair that has the smallest weighted Euclidean distance DHj by(16)DH=MIN(DH1,DH2,…,DHl).If no matching pairs are found for a scan lineSi, we will determine the scan line as a new discovered line, and therefore insert it into the map in the map-update stage.


## 3.5. Importance Weighting

Each particle generated from the motion model is assigned an importance weight according to how well its current measurement of the environment matches the map we have already built, so that we can determine the accuracy of each particle by its importance weighting.In our case, the importance weightWtoti for particle i is computed using the matching pairs we found in the data association step, by first assigning each matching pair a weight as follows:(17)w=loverlape-DH,
where DH is the weighted Euclidean distance of the matched pair and loverlap is the overlap distance between the matched scan line and the map line, which is shown in Figure 9.Figure 9
Illustration of the overlap distanceloverlap between the matching pair Si and LMj.After all the matched pairs are assigned a weight, we can compute the importance weight of theith particle by(18)Wtoti=NmNtot_scan∑k=1Nmwk,
where Nm is the number of matched pairs, Ntot_scan is the total number of orthogonal scan lines extracted in this iteration, and wk is the weight of the kth matching pair. The main purpose of multiplying the ratio Nm/Ntot_scan is to avoid some particles having extremely high importance weight caused by matching pairs which have long overlap distance between them.After assigning each particle an importance weight, the resampling step will take place by deleting the ones that have lower weights. Also particles that are assigned a higher weight (meaning that it has a higher probability to be the correct robot pose) will have the opportunity to generate more new offspring particles from the motion model in the next iteration.


## 3.6. Map Update

The map of each particle is maintained and updated in this step, where new observed lines are added to the map and lines that are close to each other, will be merged into a single line segment.Figure10 shows an example of merging a scan line Si and a map line LMj into a single line Lm, where Lm is generated using all the scan points constituting Si and LMj. By considering all the points, line segments can be merged more accurately, but with higher requirement of memory since it requires the storing of all the points constituting the individual lines. To overcome this issue, the recursive least-squares (RLSs) method [20] for line merging is used in our work, which does not need to store all the points, but only requires six additional parameters (N,Sxx,Syy,Sxy,XN,YN) of each line being merged. The RLS method operates by first updating the six parameters for the merged line and then determines the start/end points of the line by projecting the terminal points onto the new line. Here, we briefly review the RLS algorithm. More information can be found in [20].Figure 10
Merging two linesSi and LMj by considering all the scan points.In order to reduce the computational complexity of maintaining the map for each particle, the map-update procedure is split into two modes: (1) local map update and (2) global map update, where the local map update executes at every iteration and the global map update only executes once in a period of time.


### 3.6.1. Local Map Update

In this step, new scan lines are added into the map and the matched pairs determined in the data association step will be merged together. Also line segments in the local map are merged if they are close to each other and have an overlap between them. The way of determining if two line segments in the local map can be merged together is similar to finding the matching pairs in the data association step, but instead of calculating the weighted Euclidean distance, we only calculates the average distance between two overlapped lines to determine if the lines can be matched as shown in Figure11.Figure 11
Illustrates the perpendicular distancel1 and l2 from the terminal points in the overlap section to the opposite line.Wherel1 and l2 are the perpendicular distance from the terminal points covering the overlap section to the opposite lines, the average distance can be computed as(19)ldist⁡=l1+l22.
If the average distance ldist is smaller than a threshold, the two lines are close to each other and will be merged together.


### 3.6.2. Global Map Update

In order to avoid having several broken line segments that are meant to be the same line in the map, we need to check all the map lines and determine whether the lines can be merged together by using the same way as in local map update, but this time the whole map will be tested. Due to the fact that checking all the line segments will cost an extremely large effort and the local map update can 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 more efficiently and reduce the number of line segments in our map.


## 3.6.1. Local Map Update

In this step, new scan lines are added into the map and the matched pairs determined in the data association step will be merged together. Also line segments in the local map are merged if they are close to each other and have an overlap between them. The way of determining if two line segments in the local map can be merged together is similar to finding the matching pairs in the data association step, but instead of calculating the weighted Euclidean distance, we only calculates the average distance between two overlapped lines to determine if the lines can be matched as shown in Figure11.Figure 11
Illustrates the perpendicular distancel1 and l2 from the terminal points in the overlap section to the opposite line.Wherel1 and l2 are the perpendicular distance from the terminal points covering the overlap section to the opposite lines, the average distance can be computed as(19)ldist⁡=l1+l22.
If the average distance ldist is smaller than a threshold, the two lines are close to each other and will be merged together.


## 3.6.2. Global Map Update

In order to avoid having several broken line segments that are meant to be the same line in the map, we need to check all the map lines and determine whether the lines can be merged together by using the same way as in local map update, but this time the whole map will be tested. Due to the fact that checking all the line segments will cost an extremely large effort and the local map update can 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 more efficiently and reduce the number of line segments in our map.


## 4. Experimental Results

Our algorithm has been implemented for a P3-DX robot equipped with SICK LMS-100 laser rangefinder, where we also test its performance using different datasets from Radish [22]. All the experiments are performed on a laptop computer with 2G CPU and 3G RAM, which our algorithm is capable of running in real-time with low memory usage.The first dataset is recorded by our P3-DX robot at the Center of Innovation Incubator, room 212, in our NTHU campus, where the size of the room is about7m×4m. Our main purpose is to test whether the algorithm can successfully detect the orthogonal lines in the environment even when the robot does not starts its initial pose aligned with the walls. Figure 12 shows an experiment of starting the robot at different directions, all of which are not aligned with the major structures of the environment. We use our proposed algorithm to determine the orthogonal lines (blue lines) and nonorthogonal lines (red lines) in the initial map. Also, error line segments caused by sensor noise or line extraction error can be filtered out by simply considering the orthogonal lines in the map. Figure 13(a) shows the floorplan of the room, and Figure 13(b) is the map of the entire room built by our algorithm, which only considers orthogonal lines in the environment. As we can see, by dynamically modifying the reference direction to identify the orthogonal lines, the algorithm is capable of determining the correct orthogonal direction even when the robot’s initial pose is not aligned with the major structures of the environment.Initial map built by robots starting at different directions and indicates orthogonal (blue lines) and nonorthogonal (red lines) lines.
(a)(b)(c)(d)(a) Floorplan of the room. (b) Resulting map drawn by our algorithm.
(a)(b)The second experiment uses the dataset calledintel_oregon by M. Batalin, which was recorded at Intel Lab in Hillsboro, Ore, USA, using a P2-DX robot with LSM-200 laser rangefinder. As mentioned in [20], the covering area of this dataset is about 750 m2 and the total moving distance of the robot is 110 m, which contains several loops and complex indoor structures. In this experiment, we also implemented a general approach RPBF SLAM using grid map as its map structure to compare the total memory usage and computation time against our proposed lightweight SLAM algorithm, which uses line segments as our map structure. Figure 14 depicts the mapping result of intel_oregon using general approach (Figure 14(a)) and our proposed algorithm (Figure 14(b)), and Figure 15 shows the computation time of each iteration needed for executing the two algorithms, respectively. As it can be seen, our lightweight SLAM not only reduces the memory usage, but also needs less computation time as compared to the general RBPF SLAM. This is mainly due to the fact that line segment map only needs to store the geometric position and some parameters of each line, but grid map needs to store the whole environments information in a large 2D array. Also when a new particle is generated, a map built by the robot’s previous trajectory is copied to the new particle so that each particle maintains its own map. Since copying memories is often time consuming, we can thereby increase the speed of the algorithm by lowering the amount of data needed to be copied. Table 1 summarizes the total required memory, average computation time, and maximum computation time needed in an iteration of the general approach RBPF and in our lightweight algorithm. As compared to the general approach RBPF SLAM, the total memory usage and average computation time can be reduced to only 5.5% and 6.5%, respectively, by our algorithm. Since our algorithm is also much faster, it can support the SLAM operation for a faster-moving robot. In some sense, our algorithm is 453.4/29.5=15.36 times faster.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 approach193.2 (100%)453.4 (100%)1062 (100%)Proposed algorithm10.7 (5.5%)29.5 (6.5%)78 (7.4%)Mapping results ofintel_oregon using (a) general RBPF SLAM (grid map) and (b) proposed lightweight SLAM (line segment map).
(a)(b)Figure 15
Computation time of each iteration needed for a grid map-based RBPF SLAM (blue line) and our proposed lightweight approach (red line).


## 5. Conclusion

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



---

*Source: 257852-2011-06-08.xml*