Original Article

Natural corners-based SLAM with partial compatibility algorithm

Proc IMechE Part I: J Systems and Control Engineering 2014, Vol. 228(8) 591–611 Ó IMechE 2014 Reprints and permissions: sagepub.co.uk/journalsPermissions.nav DOI: 10.1177/0959651814533533 pii.sagepub.com

Rui-Jun Yan1, Jing Wu1, Quan Yuan2, Chao Yuan1, Lu-Ping Luo2, Kyoo-Sik Shin3, Ji-Yeong Lee3 and Chang-Soo Han3

Abstract This article presents natural corner-based simultaneous localization and mapping (SLAM) using a new data association algorithm that achieves partial compatibility in a real unknown environment. In the proposed corners’ extraction algorithm, both the end points of an extracted line segment far away from the other segments and the intersection point of the two closer line segments are considered as corners. In data association, a partial compatibility algorithm obtaining a robust matching result with low computational complexity is proposed. This method divides all the extracted corners at every step into several groups. In each group, the local best matching vector between the extracted corners and the stored ones is found by joint compatibility, while the nearest feature for every new extracted corner is checked by individual compatibility. All these groups with the local best matching vector and the nearest feature candidate of each new extracted corner are combined, and its joint compatibility is checked with the linear matching time. The experimental results in an indoor environment with natural corners show the robust matching result and low computational complexity of the partial compatibility algorithm in comparison with individual compatibility nearest neighbor and joint compatibility branch and bound.

Keywords Features’ extraction, extended Kalman filter, simultaneous localization and mapping, unknown environment, data association, partial compatibility

Date received: 4 October 2013; accepted: 7 April 2014

Introduction Simultaneous localization and mapping (SLAM) is the process of estimating the pose of a mobile robot and building a map of an unknown environment based on the extracted features of the environment. Due to the low accuracy of using only odometry, the features are used as landmarks to correct the position of a mobile robot by integrating the incremental information over time to measure the distance or the angle transformed. To relate new observations with features stored in the map correctly, data association is posed as a search problem in the space of the observation–feature correspondences. The geometric shape of the features in the environment has an important effect on robot localization, navigation, perception and mapping. Most experiments, with laser sensor or vision sensor, of robot localization and mapping environments estimate the position of a mobile robot using man-made landmarks, like a cup in Choi and Lee,1 a green circle in Mata et al.2 or other artificial landmarks as in Li and Jiang,3

Lee and Song,4 Meyer-Delius et al.5 and Ronzoni et al.6 Unfortunately, artificial landmarks can be applied only to a very narrow range with respect to the natural features in a real unknown environment. This is because the man-made ones cannot be put into the desired area that people may not enter. Landmark features should be carefully considered because the uncertainties of the landmark position can affect the case of updating the position of a mobile robot as well as other stored 1

Department of Mechatronics Engineering, Hanyang University, Ansan, South Korea 2 Department of Mechanical Engineering, Hanyang University, Ansan, South Korea 3 Department of Robot Engineering, Hanyang University, Ansan, South Korea Corresponding author: Chang-Soo Han, Department of Robot Engineering, Hanyang University, Ansan 426-791, South Korea. Email: [email protected]

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

592

Proc IMechE Part I: J Systems and Control Engineering 228(8)

landmarks. For this reason, the corners in an unknown environments have been chosen as landmarks by using vision sensors as in Rosten and Drummond,7 Shen and Wang8 and Hwang and Song,9 while others have used sonar or laser sensors10,11 but without the exact calculation of the distance uncertainty and angle uncertainty of the corner landmarks. The basis of the corner extraction is line segments’ extraction from the raw sensor data. Line features have been used successfully in many experiments,12,13 and there have also been many methods of line extraction, including the split-and-merge,10,14 incremental probabilistic technique15 and Hough transform.16 Some experiments have been done by the evaluating different line extraction methods as in Nguyen et al.17 which showed that the split-and-merge and incremental method demonstrate the best performance. The principle of realization of line segments’ extraction in this article is based on a substantially modified split-andmerge where the uncertainty of the extracted corner is propagated from the uncertainty of the lines calculated as in Arras and Siegwart.18 This article attempts to solve both the features’ extraction problem and the data association problem. The corners are extracted by considering both the intersection points of the possible line segments and the end points of some segments that are far away from the other line segments. Many researchers have extracted the intersection point of the two adjacent line segments as corners. However, this method cannot obtain sufficient corners because of the high uncertainty of the line segments extracted from the rugged position. The angle between the two line segments and the distance from the intersection point to the end points of the line segment are compared with some limit value for extracting the appropriate corner. The real corners are easily ignored if the conditions of the two continuous segments are not satisfied. Even though the corner can be found by the previous method in some cases, the much closer corner to the real corner can be extracted by considering all the possible line segments that can extract a corner. After extracting the corners from a new scan, these corners should be matched with the stored map features in the previous step. However, most research develops a data association method with the assumption that the two measurements cannot come from the same feature.19,20 For stochastic mapping, the two most common methods are the individual compatibility nearest neighbor (ICNN) algorithm with low computation complexity in Castellanos et al.21 and Feder et al.22 and the joint compatibility branch and bound (JCBB) algorithm increasing the robustness of the data association but with much more computation time in Neira and Tardos.19 The randomized joint compatibility algorithm detecting the overlap between the two local maps successfully was developed based on JCBB in Paz et al.23 and was applied to a large-scale SLAM in Paz et al.24 When clutter is high and the features are sparse,

a multi-dimensional assignment (MDA)-based data association algorithm for multi-target tracking is extended using the sensor location uncertainty with the joint likelihood of the measurements over multiple frames as the objective function.25 Different from the above algorithms based on the squared Mahalanobis distance (SMD), the alternative matching likelihood was maximized in Blanco et al.26 The data association algorithm in this article also relies on SMD but considers the partial compatibility of all the detected features. To reduce the computation time and achieve better matching results, the detected features at every step in the proposed method are separated into different small groups with the local joint compatibility matching vector and the nearest unit matching pair for everyone in that group. Then the two types of matching pairs in all the small groups are combined by using the branch and bound algorithm. In addition, the computation time of the standard extended Kalman filter (EKF) SLAM in Thrun et al.27 is high. This result is due to the great costs of updating the covariance matrix for prediction and correction. Although the prediction part of the algorithm is optimized to reduce the computation complexity in Guivant and Nebot,28 the computation complexity in the correction part is also very high. In this article, the computation time is decreased in both the prediction and correction, compared with the standard SLAM algorithm based on the principle of the block matrix. There are five sections in this article. The line extraction algorithm with its covariance matrix and corner extraction algorithm with the propagation of uncertainties is presented in section ‘‘Line segments’ extraction and corners’ extraction.’’ The partial compatibility algorithm (PCA) is explained in section ‘‘Data association algorithms’’ and the simplification of the EKFSLAM algorithm is introduced in section ‘‘Improved EKF-SLAM.’’ The experimental results of the extracted line segments, extracted natural corners and SLAM with proposed data association algorithm are shown in section ‘‘Experiment results.’’ Finally, this article is concluded in the final section.

Line segments’ extraction and corners’ extraction Line segments’ extraction In this section, the line segments’ segmentation algorithm and the line segments’ merging algorithm are explained in detail for separating the raw sensor data into appropriate groups and extracting them as the corresponding line segments. Moreover, the covariance matrix of every line segment is derived step by step based on the uncertainty of the sensor data in that group. Line segments’ extraction algorithm. The first step in the process of line segments’ extraction is splitting the

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

Yan et al.

593

Algorithm 1: Line segments extraction algorithm Input: Raw sensor data vector Vpoint Output: Extracted line segments vector Vline 1: for raw sensor data point pi from point set Vpoint do 2: find the distance dp of point pi and pi + 1; 3: if dp . dmax then 4: Vline.add (Lnew(., pi)); 5: end if 6: end for 7: for line segment Lk in Vline do 8: for pj in the point set of line segment Lk do 9: FlagOne = false; 10: calculate angle da of line (pj, pj + 1) and (pj + 2, pj + 3); 11: if da . amax then 12: Vline.insert(Lnew(., pj + 1)); 13: FlagOne = true; 14: end if 15: end for 16: if (FlagOne) then 17: Vline.delete(Lk) 18: end if 19: end for 20: for Lm in Vline do 21: for pn in the point set of line segment Lm do 22: FlagTwo = false; 23: find the distance l from point pn to segment Lm; 24: if l . lmax then 25: Vline.insert(Lnew(., pn)); 26: FlagTwo = true; 27: end if, 28: end for 29: if (FlagTwo) then 30: Vline.delete(Lm); 31: end if 32: end for 33: for Ln in Vline do 34: calculating the distance dmerge and amerge of Ln and Ln + 1; 35: if (dmerge \ dmergMax and amerge \ amergeMax) then 36: Lnew LineMerging (Ln, Ln + 1); 37: Vline.swap([Ln, Ln + 1], Lnew); 38: end if 39: end for sensor points into separate data sets to reduce the segmentation time in the following steps. The distance dp between the adjacent points pi and pi + 1 is calculated and the data set is divided into two sets when this distance is bigger than the limit value. For each separated data set, a line segment is extracted based on the data points in this set. The extracted new line segment is expressed by Lnew(., pi), in which pi is the last sensor point in the new data set. For a better segmentation result, the limit value dmax dynamically changes with the measured distance of the raw sensor data. The pseudo code of the detailed process is summarized in Algorithm 1. The second step of Algorithm 1 is to check whether the sensor points in the previous segmented line set

belong to the same line segment or not by checking the angle between the two short line segments l1(pi, pi + 1) and l2(pi + 2, pi + 3) as shown in Figure 1. If the angle is bigger than the limit value, this data set is separated into two sets, and the previous line segment is replaced by two new segments. In the final step of the line segmentation, the distance l from a sensor point of the data set to the extracted line segment is computed. If l is bigger than the maximum distance lmax, this single line segment is divided into two segments at this sensor point, as expressed in Figure 2. The last step of Algorithm 1 merges the line segments that could be represented as a single line segment. Two parameters are used for this, the angle of the adjacent line segments Ln and Ln + 1 and the distance from

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

594

Proc IMechE Part I: J Systems and Control Engineering 228(8) by this new line segment and an example is shown in Figure 4, in which the data points belong to the same line but are extracted as two line segments.

pi +1

pi p i

2

pi

Raw sensor data

3

Extracted line segment

Figure 1. Angle between the four adjacent data points (pi, pi + 1, pi + 2 and pi + 3).

Raw sensor data Desired line segments

Uncertainty of the extracted line segments. Since the sensor data have uncertainties, the relative parameters of the line segments also have uncertainties. This section shows the derivation of the covariance matrix of the line segment from that of the raw sensor data. The ith raw sensor data pi is represented with two parameters, measuring distance ri and direction angle ui. The uncertainty of a raw sensor point is assumed to be Gaussian distributed, which is expressed as follows     Pi ;N ri , s2ri , Qi ;N ui , s2ui ð1Þ

Extracted line segment

Figure 2. The distance l from a raw data point to the extracted line segment is calculated for all the data points in separated group, and the single line segment extracted from this group is divided into two at a point whose l is bigger than the limit value lmax.

Raw sensor data

i

i

The two parameters of line segment can be obtained by calculating the derivatives of equation (2) with respect to the variables a and r, expressed as follows

Extracted line segment

merge

d merge Figure 3. Intersection angle and the distance of the two adjacent line segments.

the second end point of Ln to the first end point of Ln + 1 as shown in Figure 3. If these two values are smaller than the corresponding limit value, the sensor points in these two sets are merged to a single new line segment. The previous two line segments are replaced

Raw sensor data

where the uncertainty of the sensing distance Pi and the uncertainty of the rotation angle Qi are assumed to be independent as in Arras and Siegwart.18 Each extracted line segment is represented with two parameters, intercept r and angle a, as in Figure 5. To extract the line segment, the square of all the errors together with the weighted value wi = (1/su)2 + (1/sr)2 for the measurement points is summed as X X S= wi d2i = wi ðri cosðui  aÞ  rÞ2 ð2Þ

1 a = atan  3 2P 2 2 PP wi ri sin 2ui  P1 w wi wj ri rj sin ui + uj i 4P  5 PP wi wj ri rj cos ui + uj wi r2i cos 2ui  P1 w i   1 A ð3Þ = atan 2 B P wi ri cosðui  aÞ P r= ð4Þ wi

After the derivation of the parameters of the line segment, the first-order Taylor expansion is used to calculate the covariance matrix Cl of the extracted line segment as follows Cl = Mr Cr MTr + Mu Cu MTu

Extracted line segment

ð5Þ

Desired line segment

Figure 4. Two line segments (solid line) belonging to the same line segment but extracted as two in the segmentation step can be merged as one (dotted line) in merging step.

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

Yan et al.

595

y

Raw sensor data Extracted line segment

(a)

( i, i) di r

(b) Extracted line segment Real line in the environment

O

x

Figure 5. Each extracted line segment is represented with two parameters, intercept r and angle a.

The first part of equation (5) is about the uncertainty of the measuring distance of the laser sensor, which is calculated by transforming the matrix Cr = diag ((sr)2) into the line space using the 2 3 N Jacobian matrix Mr. Here, N is the number of raw sensor data in the data set of the extracted line segment. The second entry in equation (5) is the uncertainty of the angle, which is expressed by Cu=diag ((su)2) with a similar 2 3 N Jacobian matrix Mu. The detailed expression of equation (5) is Cl = 2 N N P ∂r P ∂r ∂r ∂r 6 j = 1 ∂rj Crj ∂rj + j = 1 ∂uj Cuj ∂uj 6 6 N N 4 P ∂a P ∂r ∂a ∂r ∂r Crj ∂r + ∂uj Cuj ∂uj j=1

j

j

j=1

3 ∂a Cuj ∂u j 7 j j=1 j=1 7 7 N N 5 P P ∂a ∂a ∂a ∂a C + C r u j ∂uj j ∂r ∂r ∂uj N P

∂r ∂rj

j=1

j

∂a Crj ∂r +

j

N P

∂r ∂uj

j=1

ð6Þ

The entries in equation (6) are expressed by the following equations ∂r 1 = cosðui  aÞ, ∂ri N

ð7Þ

# N   1X ri sin 2ui  r sin ui + uj B N j=1 j # ) N   1X  ri cos 2ui  r cos ui + uj A N j=1 j (" # N   ∂a 1 1X 2 = ri cos 2ui  r r cos ui + uj B ∂ui A2 +B2 N j=1 i j " # ) N   1X 2 + ri sin 2ui  r r sin ui +uj A ð8Þ N j=1 i j ∂a 1 = 2 ∂ri A + B2 "

("

∂r 1 =  ri sinðui  aÞ ∂ui N

The two symbols A and B in equation (8) are shown in equation (3).

Extracted corner Real corner

Figure 6. Comparison of the extracted natural corners using two methods: (a) only the intersection point of two adjacent line segments is considered as corner in the two examples and (b) much closer corners to the real corner in the examples are extracted because the intersection points of all the possibly intersecting line segments are considered as corners (two).

Natural corners’ extraction Natural corner extraction algorithm. Two types of natural corners are extracted based on the extracted line segments above. For extracting all possible corners, the distance and the angle between one line segment and the other feasible line segment candidates are computed and compared with two conditional values. For the first type of corner, the intersection point of the two eligible line segments is found and stored in the corner vector. Nevertheless, the corners cannot be effectively extracted by only checking the two adjacent line segments because of the existing error of the raw data points. An example of the extracted corner by only considering the two adjacent line segments is shown in Figure 6(a). In the left figure, no corner is extracted due to some inaccurate line segments located in the neighboring position of the real corner. In order to extract the corner as in the bottom instance of Figure 6, the intersection points of all the possibly intersecting line segments are considered as corners. By using this method, the corner in the left side of Figure 6(a) is extracted successfully, which is shown in the left side of Figure 6(b). Even though the goal corner is found by only considering two adjacent line segments in the right side of Figure 6(a), a much closer corner to the real corner can be found in the right side of Figure 6(b). The reason for this is that the two line segments of this new closer corner represent the real environment better than the two previous segments. Not only the intersection points but also the end points of some line segments are stored as new corners. When the two adjacent line segments are far away from each other, the second end point of the first line and the first end point of the second one are chosen as corners since these two lines may be from two different objects. A detailed realization of the corner extraction is presented in Algorithm 2.

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

596

Proc IMechE Part I: J Systems and Control Engineering 228(8)

Algorithm 2: Corner extraction algorithm Input: The line segments vector Vline Output: The corners vector Vcor 1: for line segment Li (i:1 - Vline.size)do 2: j = i + 1; 3: Calculate the distance dc, angle uc between Li and Lj; 4: if (dc . dmin or uc . umax) then 5: Vcor.add(Li.SecondEndPoint, Lj. FirstEndPoint); 6: continue; 7: end if 8: while (line segment Lj (j: i + 1 - Vline.size)) do 9: if (dc \ dmin and uc \ umax) then 10: calculate the intersection point pc of Li and Lj; 11: calculate the distance dp-1(pc to Li) and dp-2 (pc to Lj); 12: if (dp-1 \ dpMax and dp-2 \ dpMax) then 13: Vcor.add (pc); 14: end if 15: update dc and uc; 16: end if 17: end while 18: end for

The uncertainty of the extracted corners. The natural corners in indoor environments can be extracted as landmarks for mobile robot–based SLAM after the extraction of the line segments from the raw sensor data. Based on the two parameters (ri, ai) of the ith line segment, it can be simply expressed as the traditional equation y 2 ai 3 x + bi = 0 with ai = tan (ai + 90), bi = ri 3 cos(ai) 3 ai2ri 3 sin(ai). There are three parameters in the expression of every corner vector, including the position xcor, ycor and the angle ucor. The intersection point of the two line segments with four parameters (ri, ai, rj and aj) are shown in Figure 7 and the detailed calculation is shown in equation (9). Except for the intersection points, the second type of corner is the end point of the line segment which is far away from the adjacent line segments. This point can be considered as the intersection point of this line segment and its normal line, which is perpendicular to this segment and passes through the end point in the corresponding data set. The final expression of this type of corner is similar to the first type of corner because all these corners are extracted as the intersection points of the two line segments

Extracted line segment Raw sensor data Extracted corner

y

rj

ri i j

x Figure 7. Intersection point of the two line segments extracted from the raw sensor data.

(x

y

f )T = ð xcor



ycor

fcor ÞT + Nð0, Ccor Þ ð10Þ

       ri cos ai tan ai + p2  ri sin ai  rj cos aj tan aj + p2  rj sin aj     xcor = tan ai + p2  tan aj + p2    ð9Þ p p ycor = tan ai + xcor  ri cos ai tan ai + + ri sin ai 2 2 fcor = jai  aj j The covariance matrix of the corners is propagated from the covariance matrix of the two lines. The propaEvery extracted corner is expressed by the position gation is a multi-input and multi-output system based in the two-dimensional (2D) Cartesian coordinate syson the first-order Taylor expansion.29 The covariance tem with the additional Gaussian noise matrix Ccor is a 3 3 3 matrix expressed as

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

Yan et al.

597

Ccor = Fcor Cli lj FTcor

ð11Þ

which is the similarity transformation of the 4 3 4 matrix Clilj of Li and Lj   02 3 2 Cli Cli lj = ð12Þ 02 3 2 Clj where two line segments are assumed to be independent because the sensor data in each data set are independent. The Jacobean matrix Fcor is a 3 3 4 matrix 3 2 ∂xcor ∂xcor ∂xcor ∂xcor 6 ∂r ∂rj ∂aj 7 ∂ai i 7 6 6 ∂ycor ∂y ∂y ∂y cor cor 7 cor 7 ð13Þ Fcor = 6 6 ∂ri ∂rj ∂aj 7 ∂ai 7 6 4 ∂fcor ∂fcor ∂fcor ∂fcor 5 ∂ri ∂ai ∂rj ∂aj

Data association algorithms Existing data association algorithms The two most widely employed data association algorithms in SLAM are ICNN with individual compatibility21 and JCBB with joint compatibility.19 The ICNN algorithm matches the map feature for every new measurement based on the maximum likelihood without considering the other new measurement. The new measurements are considered independently ICNN:

in this method so that it may accept an incorrect matching easily leading to divergence in the estimation part. Although the matching accuracy is lower, the advantage of ICNN is the lower computation complexity O(mn) where m is the number of new sensor measurements and n is the number of stored map features in the matching step. The result will be at most m unit matching pairs, which is shown in the top-left of Figure 8. Here, the unit matching pair includes only one measurement and one stored feature, of which the individual compatibility should satisfy the limiting condition. Unlike ICNN, the dependence of the new detected measurements from the same sensor scan is considered in the JCBB algorithm. For checking the joint compatibility and finding the best matching, all the possible unit matches are combined by using the branch and bound method and its joint maximum likelihood (ML) are calculated. Even though the robustness of the data association is increased, huge computation time is spent in the matching step, especially when the number of measurements and map features are very large. The solution space of JCBB is (c + 1)m with c (c \ n) feature candidates and the computational complexity is O((1.53)m) obtained from the experiment results in Neira and Tardos.19 The interpretation tree of the JCBB is expressed in the lower left picture in Figure 8, in which the dimension of the testing matching vector increases with the increasing number of measurements.

PCA:

M1

( M 1 , f1nearest )

M2

( M 2 , f 2nearest )

Mm

( M m , f mnearest )

:( M 2 , f 2nearest )

1 1 best

JCBB:

Mm Mm

:( M 3 , f3nearest ) :( M 4 , f 4nearest )

:

empty

:( M 5 , f5nearest )

2 2 :H best

M 1 ,* M 1 ,*

M2

empty

:( M 1 , f1nearest )

:H

M1

:

:( M 6 , f6nearest ) ) :(M m 3 , f mnearest 3

M 2 ,* 1

) :(M m 2 , f mnearest 2 M 1 ,* M 2 ,*

m p

) :(M m 1 , f mnearest 1 m

M m ,*

:H bestp

:(M m , f mnearest )

Figure 8. Interpretation tree of three data association algorithms: ICNN (left-top), JCBB (left-bottom) and PCA (right). In ICNN method, each new extracted corner M is matched with one nearest stored map corner f nearest to construct a unit matching pair (M, f nearest). In JCBB method, many possible stored map corners are matched with one new corner to construct the unit matching pairs. These unit matching pairs of each new corner are combined with those of all the other new corners to find the best matching vector. In PCA method, p unit matching pairs and one partial best matching vector H are constructed for the p new corners in each separated group. The entire members for m/p groups are combined to find the best matching vector for all these m new corners. ICNN: individual compatibility nearest neighbor; JCBB: joint compatibility branch and bound.

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

598

Proc IMechE Part I: J Systems and Control Engineering 228(8)

These two methods cannot be used in an environment with dense features because of the low matching accuracy of ICNN and the high computation complexity of JCBB. Except for these disadvantages, the linearized computation time of ICNN and the robust matching of JCBB can be merged into one good data association algorithm. However, not all the best matching vectors using JCBB matched very well, as is shown in the following experimental result. In spite of the joint compatibility, some incorrect unit matching can be included into the matching result.

Stored corner (q) q10 p6 q12 p10

p8 q11 p9

Real environment p5 p4

q9

q7 q6

p7 Scanning area of laser sensor

Group 2

p3 q5

p2 q4

p1 q3

Group 1

Mobile robot

q13

q14 p11 q15

Proposed algorithm Based on these two data association algorithms, a PCA with a more robust matching result and lower computation time is proposed by combining the advantages of ICNN and JCBB. All the extracted corners from the raw sensor data in every step are divided into several groups with a small number of corners. In stochastic mapping, the shape formed by these small numbers of corners is also stable. The small number lowers the probability of drawing into the wrong unit matching pair. The computation time will be low in each group for the same reason. The JCBB algorithm is used to find the local best matching in each group, while the ICNN algorithm is used to check the nearest stored corner for every new corner in a group. Assume that there are p new extracted corners in every group. There will be at most 1 + p members: p nearest unit matching pairs from ICNN and one best matching vector from JCBB. After obtaining all the possible unit matching pairs and local best matching vectors, the group will be integrated into a big group by the algorithm shown in the right picture of Figure 8. If the total number of the new extracted corners is m, the number of groups is m/p. Because the number of members in every group is smaller than the constant value p + 1, this algorithm can do well in cases where large quantities of stored features are matched with the new detected features. The number of nodes of the interpretation tree will be much bigger if all the new features are matched with the stored ones by using JCBB. However, for PCA, a little of these corners is

New extracted corner (p)

q8

q1

p12 q16

q2

p14 p13

q17

p15

Group 3

Figure 9. A data association example with proposed PCA method. There are 17 stored map corners and 15 new extracted corners. All these new corners are divided into three groups with five corners in each group.

included, and the computation time of checking the joint compatibility is lower than JCBB. In addition, the process of combining the groups is not affected by the large number of possible feature candidates. To easily understand the interpretation tree, a data association example with the proposed PCA method is shown in Figure 9. There are 17 stored map corners (n = 17) and 15 new extracted corners (m = 15) in the figure. However, due to the error of the odometry data, the positions of the extracted corners transformed in the absolute coordinate system are not exactly the same as the position of the corresponding stored corners. To match the new corners with the stored corners using PCA, the new corners are sequentially divided into three groups with five corners in each group. By using ICNN and JCBB, five unit matching pairs and one partial best matching vector are found. Finally, all the groups with six members are combined to find the best matching vector by using branch and bound method. From Figure 9, it can be seen that the stable geometry

Algorithm 3: Partial compatibility algorithm Input: New measurement vector mtotal Output: Stored feature vector ftotal in the map 1: GroupOfMeasurement GroupDivision(mtotal, p); []; 2: Vgroup 3: for GroupOfMeasurement[i] (i: 1 !m/p) then JCBB(GroupOfMeasurement [i], ftotal); 4: HGroupBest ICNN(GroupOfMeasurement [i], ftotal); 5: UGroupICNN 6: Vgroup [i]. H = HGroupBest; Vgroup [i]. U = UGroupICNN; 7: i = i + 1; 8: end for Total_Best_Matching_PCA(Vgroup); 9: HTotalBest

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

Yan et al.

599

Algorithm 4: Total_Best_Matching_PCA Input: The vector Vgroup stored best matching vector and nearest unit matching pair in every group Output: The best matching vector HTotalBest 1: Best_Matching_Checking(Ugroup); 2: HTotalBest = []; Hiter = []; 3: PCBB(Hiter, 1,1,1); 4: return HTotalBest; 5: PCBB(Hiter, i, t, s); //Consider the s-th element in group t. 6: if i . m/p then //Is it the last group for the whole measurement? 7: if dim(Hiter) . dim (HTotalBest) then Hiter; 8: HTotalBest 9: else if dim(Hiter) = dim (HTotalBest) 10: && joint_compatibility(Hiter) \ joint_compatibility(HTotalBest) Hiter; 11: HTotalBest 12: end if 13: else 14: if feature_collision_checking(H, Ugroup[i]. mbest) 15: && joint_compatibility(H, Ugroup[i]. mbest) then 16: Hiter [Hiter, Vgroup[t]. H]; 17: PCBB(Hiter, i + Vgroup[t].size(), t + 1, s); 18: end if 19: NUBB(Hiter, i + 1, t, s + 1); 20: //Checking the next nearest unit matching in group t. 21: end if

Algorithm 5: NUBB Input: Four parameters (Hiter, i, j, k) Output: Instantaneous matching vector Hiter 1: if Feature_Collision_Checking(Hiter, Vgroup [i].U[k]) 2: && joint_compatibility(Hiter, Vgroup [i].U[k]) then 3: Hiter [Hiter, Vgroup [i].U[k]]; 4: if (s == p) then 5: PCBB(Hiter, i + Vgroup[j].size(), j + 1, k); 6: else 7: NUBB(Hiter, i, j, k + 1); 8: end if 9: end if 10: if dim(Hiter) + m– (p*(j-1) + k) . dim (HTotalBest) then 11: if (s == p) then 12: PCBB(Hiter, i + Vgroup[j].size(), j + 1, k); 13: else 14: NUBB(Hiter, i, j, k + 1); 15: end if 16: end if of the corners in each group is formed. Due to the small number of corners in each group, the partial best matching vector can be constructed using JCBB with low computation complexity. If the JCBB method is used for all the new corners, huge computation complexity would occur in the calculation of the best matching vector. If the ICNN method is used, a wrong unit matching pair (p6, q9) is obviously constructed in group 2 by considering the nearest neighbor. Actually, the new corners p6 and p7 should match with the stored corners q8 and q9, respectively.

The detailed process of PCA is presented in Algorithm 3, in which the inputs are the stored map features ftotal and the new measurements vector mtotal which is the extracted corners’ vector Vcor in this article. The output of this algorithm is the best matching vector HTotalBest with partial compatibility. First of all, all the new detected features are separated into different groups and then the local best matching vector using JCBB and the nearest unit matching for every new corner using ICNN are stored in each group. The ML of every unit matching in JCBB is calculated with

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

600

Proc IMechE Part I: J Systems and Control Engineering 228(8)

the nearest neighbor using ICNN easily found by comparing the ML of the possible candidate. To check the compatibility of the combination of some matching pairs from different groups, two recursive functions, Partial Compatibility Branch and Bound (PCBB) and Nearest Unit-matching Branch and Bound (NUBB), based on the branch and bound method are stated in Algorithms 4 and 5. In every group, the local best matching vector is considered first by PCBB, and then the nearest unit matching is checked one by one by NUBB. We assume that two new features cannot match with the same stored features successfully so that the matched feature is checked every time whether it has been matched with the other ones in the current matching vector or not. The reason for the collision checking is that there is just one feature close to the real feature in the environment. In this article, this means that there is only one good corner which can be used for correcting the state of the mobile robot, while the other stored corners are in a rugged place or a real corner. In addition, the function dim(H) in these algorithms is the number of unit matching pairs of the current matching vector H.

Computation complexity of PCA One of the advantages of PCA is its low computation complexity which is proven based on that of ICNN and JCBB. The same assumption is established as before: the number of new features is m, the number of map features is n and the number of features in each group is p with (p  m and p  n). For every group, the computational complexity by using JCBB and ICNN is O((1.53)p + pn). To simplify the process of the proof, we assume that all the combinations of the unit matching pair for every new feature should be checked; however, only part of the combination is tested in PCA. The maximum computational complexity in the checked part is about the computation of the covariance matrix of the matching vector, whose dimension is 3p and the complexity is O((3p)3) according to the analysis of the matrix computation. Due to the p unit matching pairs of each group, there are at most 2p testing vectors in the group, and the result is O(2p*(3p)3). The total computational complexity of all the calculating in m/p groups can be expressed as  m m (1:53)p + mn +  2p  p3 O ð14Þ p p After finishing all the calculations in all the groups, the groups with at most 1 + 2p elements are then checked by using JCBB. It can be regarded as m/p new measurement with 1 + 2p stored feature candidates including one empty matching vector, whose solution space is (1 + 2p)m/p. Because p is a small constant, the computation complexity can be considered as O((1.53)m/p). The sum of equation (14) and this part will be larger than the real complexity of PCA.

Furthermore, that sum is smaller than O((1.53)m), which can be proven as  m m m p 3 p p (1:53) + mn +  2  p + (1:53) O p p ð15Þ  m ’ O (1:53) p The computation complexity is decreased with the increase in the value p. Even when p = 3, the equation changes as O(1.15m) and the comparison result with ICNN, JCBB is     m 1 m O(mn) \ O (1:53) p \ O 1:533 = Oð1:15m Þ

ð16Þ

and  m O(PCA) \ O (1:53) p ) O(PCA) \ Oð1:53m Þ

ð17Þ

For having the appropriate value p, the logarithmic computation complexities of JCBB and PCA in equation (15) are compared with different numbers of measurements in Figure 10. The value p changes from 1 to 50 and the number of stored map features n are assumed as 1000 in the four cases. The lowest computation complexity of PCA occurs when p = 2–5 for m = 50, p = 4–6 for m = 100, p = 10–12 for m = 500 and p = 16–18 for m = 1000. The value of p increases with the increase in the number of new measurements. There are two special cases that can be seen from the picture, p = 1 and p = m. If just one new feature is included in every group, the best matching of the total new features is the combination of the unit matching pairs by using ICNN. Moreover, if there is just one group, the matching result and the matching time are the same as the use of JCBB.

Improved EKF-SLAM Prediction The two important parameters of the standard EKFSLAM algorithm, introduced in Thrun et al.,27 in the nth step are the state vector mn21 from the n2 1th step and its covariance matrix Sn21. There are two parts in the state vector mn21, the state of the mobile robot, coordinate xn21, yn21 and direction angle fn21, and the state of the stored landmarks mn1 =

ðxn1 , yn1 , Fn1 Þ, !   N N mN x , my , mf



 m1x , m1y , m1f , . . . , ð18Þ

where N is the number of stored features in the previous step n 2 1. Three parameters are used to express the natural corners, which are positions (x, y) in the 2D coordinate system and the angle u between the two line segments. The position of both the robot and the landmarks is expressed in the absolute coordinate system,

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

601

25

Logarithmic Computation Complexity

Logarithmic Computation Complexity

Yan et al.

20 15 PCA JCBB

10 5

25 20 15 10 5

0

0 0

5

10 15 20 25 30 35 40 p−−The number of measuremens in each group

45

50

100 80 60 PCA JCBB

40

0

Logarithmic Computation Complexity

Logarithmic Computation Complexity

PCA JCBB

20

5

10 15 20 25 30 35 40 p−−The number of measuremens in each group

45

50

200

150

PCA JCBB

100

50

0

0 0

5

10 15 20 25 30 35 40 p−−The number of measuremens in each group

45

0

50

5

10 15 20 25 30 35 40 p−−The number of measuremens in each group

45

50

Figure 10. Comparison of the logarithmic computation complexity of JCBB and PCA (left-top: m = 50; right-top: m = 100; leftbottom: m = 500; right-bottom: m = 1000 with n = 1000, p = 1:50). PCA: partial compatibility algorithm; JCBB: joint compatibility branch and bound.

of which the origin is the initial position of the mobile robot in the whole experiment. If the odometry data (dntrans , dnrot ) are known, the state vector can be predicted as 0



1

dntrans  cos Fn1 + dnrot robot robot @ m  n = mn1 + dntrans  sin Fn1 + dnrot dnrot

A + N(0, Rn )

X 

0

P 3 3N 1 Gn 3n1 P3N 3 3N A

P 33 T Gn + Rn Gn 3n1  =@ P3 3 3N T n Gn n1

ð22Þ

n1

Here, the 3 3 3 sub-matrix from the n2 1th step is the covariance matrix of the robot and the 3 3 3N submatrix is the covariance of the robot and the extracted corners.

ð19Þ

Assume that the first two entries on the right side of the equation are function gn(un, Xn21) (Xn21 = (xn21, yn21, Fn21)), which can be approximated by EKF linearization using the Taylor expansion as gn ðun , Xn1 Þ ’ gn ðun , mn1 Þ + Gn ðXn1 , mn1 Þ

ð20Þ

The Jacobian matrix of the mobile robot is the partial derivative of gn(un, mn21) with respect to mn21 which can be expressed as 2

1 Gn ðXn1 , mn1 Þ = 4 0 0

0 1 0

 3 dntrans  sin Fn1 + dnrot dntrans  cos Fn1 + dnrot 5 1 ð21Þ

Correction After the successful matching of the ith new extracted feature (corner) Zin with the stored map feature Z^j(i) n , the state vector and the covariance matrix should be updated by these matching pairs using the conventional method in Thrun et al.27   ð23Þ m n = m  n + Kin zin  z^j(i) n X X     = I  Kit Hj(i) ð24Þ n n n where the Kalman gain and the matrix H are shown as X  j(i) T 1 Kin = Hn cj(i) ð25Þ n Hkn =



1

Hkn

03 3 3

...

03 3 3

k

Hkn

03 3 3

...

03 3 3



ð26Þ

In the prediction part, if the Jacobian matrix Gn and the covariance matrix Rn of the odometry data are known, then the covariance matrix of the entire state vector can be predicted through changing part of the covariance matrix Sn21 as follows

with 0

k

Hkn

1 0 =@0 1 0 0

1 0 0 A, 1

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

0

1

Hkn

1 =@ 0 0

1 0 0 1 0 A 0 0

ð27Þ

602

Proc IMechE Part I: J Systems and Control Engineering 228(8)

Table 1. Comparison of the computation time with two methods. Method

N of multiplications or divisions

N of additions or subtractions

IEKF-SLAM EKF-SLAM

(3n + 3) 3 (9n + 18) (3n + 3) 3 (3n + 3) 3 (3n + 3)

(3n + 3) 3 (9n + 15) (3n + 3) 3 (3n + 3) 3 (3n + 2)

EKF: extended Kalman filter; IEKF: improved extended Kalman filter; SLAM: simultaneous localization and mapping.

The dimension of the state vector and its covariance matrix augments dramatically with the robot moving because many more new landmarks are detected. The computational complexity increases with the dimension that can be simplified using the principle of the block matrix. The new method can be realized as X  X = I  Kit Hj(i) n n n  X X

 j(i) T 1 j(i) = I H c H n j(i) n n n ð28Þ X X

X  T j(i) 1 j(i) =  H c H n j(i) n n n X n T  X X

 j(i) T 1 =  Hn cj(i) Hj(i) n n n n Also, the covariance matrix X  n

= ð A0

...

Ak

...

P

n

AN Þ

can be written as with Ai : (3N + 3) 3 3

ð29Þ

Then, the entry in equation (28) can be computed by X  j(i) T Hn = A0 1 Hkn + Ak ð30Þ n And another 3 3 3 matrix can be calculated by the above sub-matrix   ck = Hkn A00 1 Hkn + A0k + Ak0 1 Hkn + Akk + Ccor ð31Þ with  Aj = A0j

...

Akj

...

AN j



and Am j : 333

ð32Þ

The computation time decreases dramatically with these simplifications, in which the number of multiplications or divisions and the number of additions or subtractions are shown in Table 1. From this table, we can see that the computation complexity of the improved extended Kalman filter (IEKF)-SLAM is smaller than that of the standard EKF-SLAM algorithm.

Experiment results The experiments were done in the first underground floor of Engineering building #5 at Hanyang University by using a Pioneer 3-DX mobile robot and Hokuyo laser sensor with a measurement range of 270° as shown in Figure 11. Due to the calculation of the covariance matrix of the extracted features, the uncertainty distribution of the distance and the angle of the Hokuyo laser sensor used in the following experiments are given as

Figure 11. Experimental tools: Hokuyo laser sensor, Pioneer 3-DX mobile robot and PC. PC: personal computer.

sr =

su =

(630) mm      30 ! 1000 mm (60:03  r) mm  1000 ! 8000 mm

p 1024

ð33Þ ð34Þ

in which there are two sections in different calculating principles of the distance error with a sensing distance from 30 to 8000 mm on the basis of the instruction book and the variance of the angle error in Muelaner et al.30 The sketch map of the robot trajectory and the pictures of the real experimental environment with a magnitude of 50 m 3 15 m are shown in Figure 12. The map and robot trajectory constructed by the raw sensor data based on the robot odometry data are shown in Figure 13, in which the small black points are the sensor data from the environment and the position of the mobile robot is instantaneous where the sensor scan is obtained. The sensor data of the same object obtained at different positions with the robot moving cannot overlap very well owing to the error of the robot encoder. For eliminating these uncertainties, the natural corners are extracted from the raw laser sensor data and the IEKF-SLAM algorithm is used for correcting the state vector based on these corners.

Extracted line segments and extracted corners The basis of SLAM in an unknown environment is that the natural features can be extracted very well when a

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

Yan et al.

603

Initial-Final Position Motion Trtajectory Trash Can

Figure 12. Sketch map of the mobile robot trajectory and pictures of the real unknown environment in the experiment.

Position of mobile robot with odometry data

Raw sensor data

Figure 13. The raw laser sensor data in SLAM experiment and the robot trajectory using odometry data.

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

604

Proc IMechE Part I: J Systems and Control Engineering 228(8)

End point of a line segment far from other segments Intersection point of two closer line segments Extracted line segment

Position of mobile robot

Intersection point of two adjacent line segments Extracted line segment

Position of mobile robot

Figure 14. Corners (pentagon and dots) extracted from the line segments (straight line) by using two different methods: left— corner extraction algorithm in Algorithm 2; right—intersection points of two adjacent line segments li and li + 1.

End point of a line segment far from other segments Intersection point of two closer line segments Extracted line segment Uncertainty ellipse of extracted corner ( ), plotted using covariance matrix Uncertainty ellipse of a laser point Uncertainty ellipse of the extracted corner by combining the ellipse of a laser point and a line segment Uncertainty ellipse of an extracted line segment

Position of mobile robot

Figure 15. Proof of the covariance matrix of the second type of corner, the end point of the line segment, by comparing the uncertainty ellipse plotted using its covariance matrix and the uncertainty ellipse plotted by combining the uncertainty ellipse of the line segment and the uncertainty ellipse of the last raw sensor data in the data set of this line segment.

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

Yan et al.

605

real feature is located in the range of the sensor. This problem is solved perfectly based on Algorithms 1 and 2. If we only consider the intersection point of the line segments li and li + 1, nothing can be extracted in most of the steps because of the restrictive condition. However, if both the end points of the line segment and the intersection point of the possible segments are considered, a sufficient number of corners can be extracted from the broad environment with only a few objects. A comparison of the corners’ extraction result at the initial step of the SLAM experiment by using these two methods is shown in Figure 14, in which the extracted line segments are also displayed by the blue lines. We can see that there is only one corner in the right figure with the previous method, but there are 14 corners with our algorithm: five intersection points drawn by a bold dot and eight end points of the line segments drawn by a pentagram. These corners are extracted from the edge of the box, column and doors in the environment. The uncertainty ellipse of the pentagonal corner in Figure 15 is proven by comparing the ellipse drawn by the covariance matrix of this corner and the ellipse derived from the ellipse of the point and the line segment. There are three ellipses expressed with a solid line in Figure 15, the ellipse of an extracted corner, the ellipse of the line segment and the ellipse of the end sensing data in this set. All these three solid ellipses are plotted using their corresponding covariance matrix. According to the properties of an uncertainty ellipse, the points located in the ellipse of the line segment are the intersection points of the line segment with its normal lines. Therefore, the lines connecting the center point of laser sensor and the points in the ellipse are the normal lines of the line segment. Four normal lines are chosen in the figure, and the possible line segment candidates are the perpendicular line of these normal lines. To find the end points of the possible line segments, four points are selected from the small ellipse of the end sensing point. Then, the perpendicular lines of the possible line segments passing through these selected points are constructed. The intersection points of these lines and the possible line segments are the corner candidates. By using this method, many more corner candidates can be found. The ellipse covering these corners is the uncertainty ellipse of the extracted corner, plotted with a dashed line in Figure 15. It can be seen that this ellipse is almost the same as the ellipse of the corner plotted using the covariance matrix, which shows that the derivation of the covariance matrix of the extracted corner is correct.

Comparison of the SLAM results Based on the extracted natural corners, the SLAM experiment is done with three different data association algorithms: ICNN, JCBB and PCA. In the experiment, a mobile robot moves from the initial position, which is assumed as the origin point of the absolute coordinate

Robot trajectory with motion command Robot trajectory with odometry data Corrected position of mobile robot using ICNN (a), JCBB(b) and PCA (c)

(a)

(b)

(c) Figure 16. Comparison of the SLAM results by using different data association algorithms: (a) ICNN, (b) JCBB and (c) PCA. Also the robot trajectory with odometry data (red line), the motion command (black line) and correction result using algorithms are shown. ICNN: individual compatibility nearest neighbor; JCBB: joint compatibility branch and bound; PCA: partial compatibility algorithm.

system, and moves back to this position at the final step. Therefore, the loop closure is checked at the last step, with which the computation time should be greater due to the large number of matched corners. The sensor points from the environment are cumulated step by step with the robot movement. The SLAM result with ICNN is shown at the top of Figure 16, in which the sensor points are not very well overlapped with the data points in the previous step. Some objects are not constructed the same as the real shape of the object, proving the poor accuracy of the correction of the mobile robot and landmarks. On the

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

606

Proc IMechE Part I: J Systems and Control Engineering 228(8)

25 Prediction Time Matching Time Update Time

Time(sec)

20

15

10

5

0 0

20

40

60 Number of steps

80

100

120

40

60 Number of steps

80

100

120

40

60 Number of steps

80

100

120

3500 Prediction Time Matching Time Update Time

3000

Time(sec)

2500 2000 1500 1000 500 0 0

20

100 Prediction Time Matching Time Update Time

Time(sec)

80

60

40

20

0 0

20

Figure 17. Computation time in the prediction part, matching part and update part with three data association methods: ICNN (top), JCBB (middle) and PCA (bottom). SLAM: simultaneous localization and mapping.

contrary, the SLAM result by using JCBB is better than using ICNN, which can be seen from the middle of Figure 16. Most of the sensor points overlap with the previous data, but some do not cover them very well. Although many more new measurements are included in the best matching vector of JCBB, the correction with the matched stored corners in this matching vector

is not perfect due to some possible incorrect unit matching pairs. The SLAM result by using PCA in the bottom of Figure 16 shows that the correction of the mobile robot is successful at every step. The sensor points based on the corrected position with PCA always cumulate in the same position, which is better than the results with ICNN and JCBB. In this experiment, the

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

Yan et al.

607

14 Detected Corner Matched Corner

Number of Corners(n)

12 10 8 6 4 2 0 0

20

40

60 Number of steps

80

100

120

40

60 Number of steps

80

100

120

40

60 Number of steps

80

100

120

14 Detected Corner Matched Corner

Number of Corners(n)

12 10 8 6 4 2 0 0

20

14 Detected Corner Matched Corner

Number of Corners(n)

12 10 8 6 4 2 0 0

20

Figure 18. Number of new extracted corners and matched corners at every step with three data association methods: ICNN (top), JCBB (middle) and PCA (bottom). SLAM: simultaneous localization and mapping.

new extracted corners at each step are divided into different small groups with three corners in every group. The prediction time, matching time and update time at every step are shown in Figure 17. The prediction time by using three algorithms at every step is almost constant because there is only a slight change in the

number of extracted corners. The matching time with ICNN is linear with an increasing number of stored corners which can be seen from the top figure. The matching time with JCBB increases exponentially because of the large number of possible candidates for every new corner. The large computation time can be

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

608

Proc IMechE Part I: J Systems and Control Engineering 228(8)

5000 ICNN JCBB PCA

4500 4000

Time(sec)

3500 3000 2500 2000 1500 1000 500 0

0

20

40

60 Number of steps

80

100

120

20

40

60 Number of steps

80

100

120

300 ICNN JCBB PCA

Number of Corners

250

200

150

100

50

0

0

Figure 19. Comparison of the total matching time (top) and number of total corners (bottom) in the previous steps with three data association algorithms, ICNN, JCBB and PCA. ICNN: individual compatibility nearest neighbor; JCBB: joint compatibility branch and bound; PCA: partial compatibility algorithm.

found in a special case in the 47th step, at which the 12 new corners are extracted and the number of possible corners for every new one is expressed as the following vector ½0

0 0

9

8

3

6 7

1

0

1 0

ð35Þ

Before checking the joint compatibility, the unit matching pairs of the stored corner and the extracted corner are checked for individual compatibility with the ML. Seven new corners are matched with at least one stored corner and four of them are a little bigger. The matching time at this step takes 3091 s as shown in the middle of Figure 17, which gives the conclusion that JCBB are not appropriate for environments with dense features. The matching time with PCA in the same special case takes just 72 s, which is much faster than JCBB. In the final step, the matching time is the largest of all these steps because the loop closure is detected. The number of new extracted corners and the matched corner at every step is shown in Figure 18. To obtain a clear comparison result, the total matching and the total corners after every step, with ICNN, JCBB and PCA, are displayed in Figure 19. We can see

that the matching time of ICNN and PCA increases much slower than that of JCBB. Of course, the matching time of PCA is bigger than ICNN and smaller than JCBB. In contrast to the matching time, the number of total corners of the PCA is smaller than that of JCBB and bigger than that of ICNN. This result can be understood easily because the more stored corners the new corners match with at every step, the greater the computation time this takes. If many more corners are matched successfully, fewer new extracted corners are regarded as new features stored in the state vector. Except for a comparison of the SLAM results, the line segments extracted from a real environment based on the corrected position of the mobile robot with PCA are shown in Figure 20(a). Most of the line segments from the same place of the environment are located in the same position. Although this result is better than the other two methods, not all the robot positions at each step can be perfectly correct because of this real experiment without any idealization. In addition, the distribution of the extracted corners, used as landmarks, in the entire SLAM process is shown in Figure 20(b), in which all the corners come from the wall or edge of the environment and more than one

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

Yan et al.

609

Corrected position of mobile robot using PCA

Extracted line segment

Extracted corner

(a)

(b)

Figure 20. (a) Experimental result of the SLAM drawn by the extracted line segments with corrected robot position and (b) distribution of the total extracted corners (pink point) during the entire process of the experiment. PCA: partial compatibility algorithm.

corner is extracted from the real corner of the environment. The experimental results of the line segments and the extracted corners demonstrate the reliability of all the algorithms presented in this article.

Conclusion In this article, natural corners are extracted from raw laser sensor data and considered as the landmarks used for robot localization and mapping with the proposed new data association algorithm. The corners can be successfully extracted from a roomy environment with fewer features with our corner extraction algorithm. The main idea of this algorithm is that not only the intersection points of the line segments but also the end points of the lines are considered as landmarks and the intersection points should be used to extract from all the possible line segments that can be intersecting. The distinction between these two types of corners is not made in the SLAM experiments. To robustly match the stored map features with the new extracted features, a new data association algorithm called PCA is proposed based on two well-known algorithms, ICNN and JCBB. In order to have a robust matching result at a lower matching time, all the

features are divided into different groups with a small constant number of features because the shape of these features is much more stable than the shape of all the features. Then, a SLAM experiment with these three data association algorithms was done based on the redundant natural corners. From a comparison result, we can see that the accuracy of the robot position correction by using PCA at every step is more robust than using ICNN, and the computation time is smaller than using JCBB. Also, the matching time of the PCA is much smaller than that of JCBB when the number of possible stored corners for the newly extracted corners is very high. All these experimental results show that PCA is efficient and that can be used in unknown environments with dense features. Declaration of conflicting interests The authors declare that there is no conflict of interest. Funding This research was supported by the research fund of Hanyang University (HY-2013-P), and the Ministry of Knowledge Economy, under the Industrial Foundation Technology Development Program supervised by the

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

610

Proc IMechE Part I: J Systems and Control Engineering 228(8)

Korea Evaluation Institute of Industrial Technology (No. 10038660, Development of the control technology with sensor fusion based recognition for dual-arm work and the technology of manufacturing process with multi-robot cooperation). Also, this work was funded by Building-facxade Maintenance Robot Research Center, supported by Korea Institute of Construction and Transportation Technology Evaluation and Planning under the Ministry of Land, Transport, and Maritime Affairs (MLTM). References 1. Choi KS and Lee SJ. Enhanced SLAM for a mobile robot using extended Kalman filter and neural networks. Int J Precis Eng Man 2010; 11(2): 255–264. 2. Mata M, Armingol JM, Escalera A, et al. A visual landmark recognition system for topological navigation of mobile robots. In: Proceedings of the 2001 ICRA/IEEE international conference on robotics and automation, Seoul, Korea, 21–26 May 2001, vol. 2, pp.1124–1129. New York: IEEE. 3. Li GH and Jiang ZJ. An artificial landmark design based on mobile robot localization and navigation. In: International conference on intelligent computation technology and automation, Shenzhen, China, 28–29 March 2011, pp.588–591. New York: IEEE. 4. Lee SY and Song JB. Mobile robot localization using infrared light reflecting landmarks. In: International conference on control, automation and systems, Seoul, Korea, 17–20 October 2007, pp.674–677. New York: IEEE. 5. Meyer-Delius D, Beinhofer M, Kleiner A, et al. Using artificial landmarks to reduce the ambiguity in the environment of a mobile robot. In: Proceedings of the IEEE international conference on robotics and automation, Shanghai, China, 9–13 May 2011, pp.5173–5178. New York: IEEE. 6. Ronzoni D, Olmi R, Secchi C, et al. AGV global localization using indistinguishable artificial landmarks. In: Proceedings of the IEEE international conference on robotics and automation, Shanghai, China, 9–13 May 2011, pp.287–292. New York: IEEE. 7. Rosten E and Drummond T. Machine learning for highspeed corner detection. In: Proceedings of the 9th European conference on computer vision (ECCV ‘06), Graz, 7–13 May 2006. Berlin/Heidelberg: Springer-Verlag. 8. Shen F and Wang H. Corner detection based on modified Hough transform. Pattern Recogn Lett 2002; 23: 1039–1049. 9. Hwang SY and Song JB. Monocular vision-based SLAM in indoor environment using corner, lamp, and door features from upward-looking camera. IEEE T Ind Electron 2011; 58(10): 4804–4812. 10. Namoshe M, Matsebe O and Tlale N. Corner feature extraction: techniques for landmark based navigation systems. In: Thomas C (ed.) Sensor fusion and its applications. Rijeka: InTech, 2010, pp.347–374. 11. Nunez P, Vazquez-Martin R, del Toro JC, et al. Feature extraction from laser scan data based on curvature estimation for mobile robotics. In: IEEE international conference on robotics and automation, Orlando, FL, 15–19 May 2006, pp.1167–1172. New York: IEEE.

12. An SY, Kang JG, Lee LK, et al. SLAM with salient line feature extraction in indoor environments. In: International conference on control, automation, robotics & vision, Singapore, 7–10 December 2010, pp.410–416. New York: IEEE. 13. Bahari N, Becker M and Firouzi H. Feature based localization in an indoor environment for a mobile robot based on odometry, laser, and panoramic vision data. ABCM Symp Series Mechatron 2008; 3: 266–275. 14. Borges GA and Aldon MJ. A split-and-merge segmentation algorithm for line extraction in 2D range images. In: Proceedings of the 15th international conference on pattern recognition, Barcelona, 3–7 September 2000, pp.441–444. New York: IEEE. 15. Brunskill E and Roy N. SLAM using incremental probabilistic PCA and dimensionality reduction. In: Proceedings of the IEEE international conference on robotics and automation, Barcelona, 18–22 April 2005, pp.342–347. New York: IEEE. 16. Pfister ST, Roumeliotis SI and Burdick JW. Weighted line fitting algorithms for mobile robot map building and efficient data representation. In: Proceedings of the IEEE international conference on robotics and automation, Taipei, Taiwan, 14–19 September 2003, pp.1304–1311. New York: IEEE. 17. Nguyen V, Ga¨chter S, Martinelli A, et al. A comparison of line extraction algorithms using 2D range data for indoor mobile robotics. Auton Robot 2007; 23(2): 97–111. 18. Arras KO and Siegwart RY. Feature extraction and scene interpretation for map-based navigation and map building. In: Proceedings of SPIE, mobile robotics XII, vol. 3210, Pittsburgh, PA, 14 October 1997, pp.42–53. Bellingham, WA: SPIE. 19. Neira J and Tardos JD. Data association in stochastic mapping using the joint compatibility test. IEEE T Robotic Autom 2001; 17(6): 890–897. 20. Kaess M, Ranganathan A and Dellaert F. iSAM: incremental smoothing and mapping. IEEE T Robot 2008; 24(6): 1365–1378. 21. Castellanos J, Montiel J, Neira J, et al. The SPmap: a probabilistic framework for simultaneous localization and mapping. IEEE T Robotic Autom 1999; 15(5): 948– 953. 22. Feder HJS, Leonard JJ and Smith CM. Adaptive mobile robot navigation and mapping. Int J Robot Res 1999; 18(7): 650–668. 23. Paz LM, Tardos JD, Neira J, et al. Divide and conquer: EKF SLAM in O(n). IEEE T Robot 2008; 24(5): 1107–1120. 24. Paz LM, Pinie´s P, Tardos JD, et al. Large scale 6 DOF SLAM with stereo-in-hand. IEEE T Robot 2008; 24(5): 946–957. 25. Wijesoma WS, Perera LDL and Adams MD. Toward multidimensional assignment data association in robot localization and mapping. IEEE T Robot 2006; 22(2): 350–364. 26. Blanco JL, Gonzalez-Jime´nez J and Fernandez-Madrigal JA. An alternative to the Mahalanobis distance for determining optimal correspondences in data association. IEEE T Robot 2012; 28(4): 980–986. 27. Thrun S, Burgard W and Fox D. Probabilistic robotics. Cambridge, MA: MIT Press, 2005, pp.39–84. 28. Guivant J and Nebot E. Optimization of the simultaneous localization and map building algorithm for real

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014

Yan et al.

611

time implementation. IEEE T Robotic Autom 2001; 17: 242–257. 29. Hartley R. Multiple view geometry in computer vision. 2nd ed.Cambridge: Cambridge University Press, 2003, pp.157–163. 30. Muelaner JE, Wang Z, Jamshidi J, et al. Study of the uncertainty of angle measurement for a rotary-laser automatic theodolite (R-LAT). Proc IMechE, Part B: J Engineering Manufacture 2009; 223(3): 217–229.

p P Q r S w

Appendix 1 Notation C K m n N O

covariance matrix of line segments or corners Kalman gain number of new measurements number of stored map features normal distribution time complexity of algorithm

a u m r S f F

number of features in each separated group uncertainty of the sensing distance of raw sensor data uncertainty of the rotation angle of raw sensor data intercept of extracted line segment sum of the square error between raw sensor data and extracted line segment weighted value slope angle of extracted line segment rotation angle of laser sensor state vector, including pose of mobile robot and features of extracted corners measuring distance of laser sensor covariance matrix of state vector intersection angle of extracted corner direction angle of mobile robot in absolute coordinate system

Downloaded from pii.sagepub.com at SungKyunKwan University on September 5, 2014