ICE Matching, a Robust Mobile Robot Localization with Application to ...

2010 22nd International Conference on Tools with Artificial Intelligence

ICE Matching, a Robust Mobile Robot Localization with Application to SLAM Sanaz Taleghani, Maziar A.Sharbafi, Abolfazl T.Haghighat, Edris Esmaeili Department of Electrical, Computer and IT Engineering Mechatronics Research Laboratory, Qazvin azad universiry Qazvin, Iran [email protected], [email protected] of popular methods which can be considerably seen in the literature [5], [10]. In these feature-based approaches the computational complexity raises with increment of the size of the environment and number of landmarks. The extended Kalman filter has become incompetent in terms of the growth of complexity due to the update step requiring computation time proportional to the square of the number of landmarks [6]. Some techniques have been proposed to alleviate this problem, such as the extended information filter [7], un-scented Kalman filter, and FastSLAM [8] but the growth of complexity is inherent in maintaining the map. Another approach is DP-SLAM provided a very accurate solution to the dense data case by efficiently maintaining a joint distribution over robot maps and poses using the particle filter [9],[10]. DP-SLAM is able to resolve map ambiguities as a natural part of the particle filtering process and obviates the explicit loop closing phase needed for other approaches. In [11] DP-SLAM uses novel data structure that enables the algorithm to maintain and update several candidate robot’s poses and maps in real time as the robot moves through the environment. However, the efficiency in memory utilization comes with the complexities in data retrieving. Furthermore this approach might require a very large number of particles; this limitation strongly may cause the worst-complexity of run time. The heuristic approach is another choice to solve SLAM e.g. “integrating fuzzy logic and genetic algorithm” which has significantly the high time consumption property of the genetic algorithm search [12]. Because of its high accuracy and reliability, scan matching has been applied to many SLAM algorithms [13]. This paper presents “ICE matching” as a novel method from scan matching family, to perceive the robot state. This technique solves the main drawback of SM which is high computational cost or being time consuming. It could be combined with any mapping technique to produce a complete SLAM algorithm. To investigate the performance of ICE matching we employed it with Grid based mapping to have a complete SLAM. Experimental tests have been performed using different kinds of Robots on the simulation environment USARSim. USARSim is a physical realistic environment based on Unreal Tournament 2004 which is used in Robocop competitions. The results show that our algorithm is able to successfully map the whole difficult parts of the environment, and simultaneously localize the robot with the lowest position error and computational cost.

Abstract—Scan matching as a basic part of SLAM has a key role in localization and even mapping of mobile robots. Our innovative method named ICE matching presents a fast and accurate method to solve the challenges of this problem. Novelty in defining new features, matching mechanism and new state estimation approach congregated in this method creates a robust practical technique. Comparison with some high quality scan matching methods from different viewpoints illustrates the performance of ICE matching. It was applied besides Grid based mapping to generate fine tuned maps to compare with slam methods. Mobile Robot; Localization; SLAM; Scan Matching; Grid Mapping

I.

INTRODUCTION

In robotic science the mobile robot must know where it is and how it can move to the target position. During the motion, the robot requires to extract a map from its perception and simultaneously determine its own position in the environment. The concept of simultaneous localization and mapping (SLAM) was firstly introduced by Leonard and Durrant-White [1]. SLAM is a fundamental problem in mobile robot system which has been studied extensively in the robotics literature [1]-[12]. Determining the position and producing the map simultaneously in an unknown environment is mostly performed based on range sensors. Among existing sensors, laser range finder has been the most widely used one because of its high accuracy and reliability. The first step of most of such method, namely Scan Matching (SM), is based on comparing the laser beams, depicting some existing points in the environment, in two sequential snapshots. After finding the displacement via SM upgrading the map will enrich the first snapshot to be employed in the next SM. In this way two steps reinforce each other to realize more precise information about environment and robot situation. Recently several attempts have been made to overcome the SLAM challenges [2], [10]-[12]. The efficient SLAM algorithm must address all the following issues: sensor uncertainty, correspondence problem, loop closing, time complexity. The Probabilistic approaches have concluded several successes at addressing the basic problems of localization using filters [3]. Expectation Maximization (EM) has been utilized effectively to conquer the mapping challenges [4]. In this field, variations of Kalman or particle filter are the base 1082-3409/10 $26.00 © 2010 IEEE DOI 10.1109/ICTAI.2010.33

186

II.

SCAN MATCHING OVERVIEW

III.

As mentioned before the most popular methods to find the robot position and direction during its motion will be categorized as scan-matching approach. Matching the range scans, obtained by a laser range finder, taken at different times in the environment and updating the position estimation established upon the match result are the bases of scan matching methods. Selecting two or more scans to compare may affect the computational cost and the accuracy of the algorithms. The tradeoff between these two performance indices is the main bottleneck in this method. Each scan includes many points, their compressing information in fewer features without losing valuable data and the method of calculating displacement from them will result in innovative techniques. Indeed the advantages of our algorithm are founded on these two concepts too: finding features (including introducing novel feature) and matching method. In localization process, the main purpose of matching two scans is to find the displacement between two positions at which the scans were taken. Most of prominent algorithms are founded on their abilities to minimize distances between the corresponding primitive pairs such that two scans describe the same geometry. According to the various primitives that the different algorithms assigned, these techniques were classified to three types: point to point, point to feature and feature to feature. In the point to point approach, the raw range data, collected from two different positions, are individually weighted and aligned to estimate the relative robot position. In the second approach, line segment features extracted from the raw point data are used as the bases for efficient and robust global localization and even map construction. In feature to feature approach, a new data representation is used as features which are compared with each other to calculate the relative displacement. Our approach is from the recent group with attempt to reduce computational complexity. Normal Distribution Transform (NDT), as a member of scan matching family, would inherently avoid the correspondence error of point-based algorithms [14]. Iterative Dual Correspondence (IDC) focuses on minimizing the distance from point to point. But there is no suitable way to estimate a covariance matrix of the position approximation, which prohibits an effective fusion with other position estimates of other sensors [15]. Iterative Closest Point (ICP) algorithm work well in static environments [16], but its performance degrades in noisy environments where the correspondence process becomes more difficult. Feature of Metric-based Iterative Closest Point (MbICP) includes rotation in the distance metric of the general ICP algorithm [17]. Finally, the Weighted Scan Matcher (WSM) explicitly models all possible error sources in the general point-based case, which elevates robustness significantly [18]. Its improved version using quad-tree data structure to increase the accuracy and robustness was introduced as QWSM [19].

ICE MATCHING

In this section we describe our robust feature based scan matching algorithm in three parts. A. Inspiration Many algorithms of artificial intelligence are inspired from the nature. This point encouraged us to analyze the human perception from the environment after seeing it in one shot. After observation of the situation, everybody can draw an approximate map of its surroundings. Comparing this drawing with the real map shows that the general overview containing the corners, entrances and walls are correct but they may make mistakes in their size or the walls’ lengths proportion. Thus there exist some features that contain a considerable amount of information and we tried to use them in our feature extraction. From three aforementioned categories of matching in SM family, the point to point matching have O(k2) complexity, due to the matching point have to be searched among two scans with k points. When SLAM applies such approaches, after some iteration the number of points in the estimated map will increase and result in curse of dimensionality. Therefore to solve this problem the third category with features, saving useful information, is the best choice. Defining some novel types of features lead to decrease computational complexity and the number of required features during the process of matching. Instead of line segment, common in several ICP approaches, we defined different point-type features such as Intersection, Corner and End Of Wall (EOW). The name of ICE matching comes from first letters of these features’ name which should be matched to each other to develop the method. B. Novel Feature Comprehensive explanation about features which is one of the main properties of our method is presented in this section. The significant goal of defining these concepts is data abstraction. One of the main points containing informative data is corner of the rooms which is the contact point of two or more walls. Detecting the position of corner, the angle between its edges and its orientation enable us to identify a considerable part of the map that may be whole of the information exists in a scan and matching two such features could determine the new position and direction of robots with high precision. Some instances of abstraction with this concept, forming the first type of features as Corner, are displayed in Fig. 2. The next feature is End of Wall which its definition can be guessed from its name. If you know these points the only remained possible state for relation between points and lines will be covered (See Fig. 1). Position of the point and the angle of the line connected to it are all parameters required to define such features. Finally to increase the features especially when they are rare, Intersections are introduced. Intersection is the contact point of the continuation of the lines (walls) indicated in Fig.1 and Fig.2. Its characteristics are as the same as corner’s ones.

187

Sohn presented a method to extract vecctor features from the scan data in [20]. They assumed that tthe left side of a vector is open space and the opposite sidde is unreachable space. The main advantage of the vectorr map is that it simplifies discrimination between the oppoosite sides of the wall. Without considering direction, it is ambiguous that which side of the wall matches the detected scan line segment. ICE matching exploit mentionedd advantages by new definition of vector’s direction which demonstrates the robustness of this approach. Vector directions are determined with a unique manner for Corner and EOW which is “start from m feature point” (Intersections do not have direction). The vectors specified for EOW and Corner are displayed in Fiig. 1 and Fig. 2 respectively. This kind of vector assignmennt makes it robust regarded to robot position. For examplle in Fig.1, the direction of B, which is an EOW, does nott differ when the robot is at point P2 or P3. This property iis very important when the difference between lines in two scans may result the rotation of the robot.

The work done by Nguyen preseented an evaluation of six line extraction algorithms applied to t laser range scans. It is shown that for real-time applicatiions, Split-and-Merge is clearly the best choice for its superior speed [21]. An efficient sequential segmentation algorithm is suggested, employing the maximum likelihood fitting process simultaneously. Hence robust line seegment features would be obtained with less computation [22]. nd-merge algorithm and Taking advantages of split-an maximum likelihood function lead us to prepare an efficient e line segments, line segmentation method. After extracting The Intersection, Corner and End Of O Wall were determined simply as our novel features.

Figure 2. Some intersection and Corn ner points are shown. These vectors are determined by Corner C features.

2) Match process: The proccedure of matching is performed by hierarchal selection with w the aim of optimizing the computation. It is easy to prove p that this reliable hierarchal process is better than sim mple matching process of point based approach for accurate an nd stable localization. At the first level the features aree compared based on their types. Typically, each point can bee matched with the point with the same type. There is only on ne exception that in some especial cases a Corner may be maatched with an EOW. For example, in Fig. 1 when the robot is at point P1, point A is specified as an EOW feature becaause the second wall is invisible from this point. Robot folllows the movement path that is illustrated with dash gray lin ne in the figure and when the robot reaches to P2, the unvisiteed wall of A will become observable. Therefore this feature turns t into a Corner type (see Fig. 2). This exception can be applied to match such explained scenarios. In the hierarchal structure, the next filter is the feasible area defined by admissible motion distance. In other words, nly features placed in the to select two features to match, on feasible area should be checked. There are several other

Figure 1. Some intersection and EOW points are sshown. These vectors are determined by EOW features.

C. Algorithm ICE matching is an iterative scan maatching algorithm consisting of three steps. The first step is finnding all types of features in current scan. The map couldd be reproduced completely by these features which are the core of our operation in next steps. The second step is comparing this scan with the resulted map from previouus iterations and finding corresponding features in two mapss. Finally the best matching with the highest matching fitnesss index should be detected to compute the new state. All of stteps are based on geometrical concepts but the third step couuld be considered as an optimization problem too. 1) Finding Features: There are two maiin tasks in feature extraction: finding the best method to extrract line segment with the least cost function and detecting ddifferent types of features.

188

filters to remove matching candidates sequentially. For example the angle between two edges of a Corner or an Intersection is an important parameter in matching. The difference between these angles in two scans should not be more than a low threshold. Hence for each point regarded to the hierarchical mechanism, only a few admissible candidates will remain at the end. This approach reduces the computational cost considerably which results in a fast method that can be applied online during robot motion. 3) Optimization Function: The third step minimizes translation and rotation error by a new optimization function. As described before each feature besides its type and intrinsic characteristics has a position and an angle necessary to draw the maps based on them. Thus after finding a feasible feature to match with a determined point, vector and the angle difference between them show the translation (dx,dy) and rotation(dθ) required to convert these points to each other. Then whole the features of new map shift and rotate with these parameters around the robot position to achieve the new converted map. This map is compared with the previous one to evaluate the performance of this state estimation. Many parameters are significant in defining the fitness function (correlation), expressed by (1). Correlation demonstrates the performance of related displacement estimation. ∑ 1 ∑

0.4

0.2

0.2

0.6

3

Various types of optimization algorithms can be applied to find the best matching with correlation as a fitness function. From this point of view numerous combinations of translation and rotation (dx,dy,dθ) can be evaluated by a search method. Our research on implementing Genetic Algorithms is under construction but it seems that because of its computational cost, it will not be so helpful to use online. Investigating some other faster optimization methods may be practical more. In our approach, the best estimation is discovered in hierarchical manner. We start it from the conversion with the most count and then comparing the correlation is performed, considering the number of matches. Each feature has a time-validity property which means that after a while some features placed in visible area but observed rarely, will be omitted from the map. This improvement procedure is designed to modify the errors occurred during feature extraction process. IV.

EXPERIMENTAL RESULT

In this section our ICE matching merged with Grid based mapping to generate a SLAM (we named it ICEG) to compare with other methods. To evaluate the performance of ICEG, two major groups of SLAM are implemented. The first group includes methods like DP-SLAM which are not based on scan matching and other set contains SM based approaches. IDC, MbICP and QWSM are three popular methods of the second group which are compared with ICEG in this section. The simulation environment selected as our test bed is USARSim. It is a physical realistic environment based on Unreal Tournament 2004 which is used in RoboCup competitions. In results of this section P2AT was selected as our robot and SICK LMS200 was the range scanner mounted on it. This sensor gathers 181 range measurements at 5Hz with the maximum range of 20m and the resolution of one degree. At first, ICEG is compared to DP-SLAM with two similar paths. To achieve the same path for different methods a robot moved by an operator in an environment and then the log files containing the laser scanner and other sensors data have given to the algorithms to find the robot situation and generate the map. In this manner every input data in compared methods are exactly the same. Fig. 3 displays the map obtained by ICEG and DP-SLAM moving through rooms and totally 2126 scans gained during the motion. Satisfying results in both methods shows their suitable performance. The critical drawback of DP-SLAM was its process time, shown in Table I, which is more than ten times longer than ICEG and made it useless for online applications. The time required to move and generate this data was 425 second which is about 7 minutes. But elapsed time to generate map with ICEG is about 3 minute, verifying the ability to utilize online. This period is about 35 minutes for the other method avoiding it from online usage. In other words, if you want to employ the SLAM algorithm to explore an environment autonomously (without the human

In this equation dis and Angle are the distance and angle between two features in the converted and old map respectively. Conversion may produce too similar features in new and old map. It means that this transformation matches some other features in two maps. The number of these coincidences is another important parameter which its influence is considered in (1) by count. The last parameter in (1) is acc computed by (2). 2 In which nf and of are the parameters defined as the features selected for matching, in the new and the reference map respectively. fImp is a function to compute the importance of the features regarded to the distance between robot and the mentioned feature (d(f)). Equation (3) illustrates the function allotting importance respect to distance. It means that the importance of points far from robot is less than near ones because of the difference between their precisions. With an example this concept will be clarify more; compare two objects with distances 2 and 20 meters from robot respectively. Assuming angle between laser beams is equal to one degree, it will be obvious that the minimum distances between points near first feature is about 3 centimeter where this value for second one is about 35 cm. It means that the precision of feature for far points are very lower than near ones. Analyzing the shape of fImp displays that it satisfies our desires with assigning 0.1 to the furthest points (the laser reliable range is 20 meter) against 0.65 for near ones.

189

operator) the algorithm loop time should be less than laser data receiving frequency to prevent losing data.

DP-SLAM increases because it should be checked with larger previous data set. TABLE I.

THE COMPARISON OF COMPUTATION TIME

Algorithm

Log

Total Time

Average Time

ICE

2126

202 sec

0.095sec

DP-SLAM

2126

2122 sec

0.998 sec

ICE

987

88

DP-SLAM

987

sec

711 sec

0.089 sec 0.72 sec

In these two simulations the Grid based mapping was based on binary distribution. In the following in mapping probabilistic attitude is considered which assign a probability to each point that varies according to its observation. This recent approach consequence is more appropriate shown in Fig 5. The following simulations are based on this mapping technique. In Fig 5 the passed path is similar to log file 2126, but the map is more accurate. (a) DP_SLAM

(b) ICEG

Figure 3. The maps obtained by log file 2126 and passed path by robot.

Figure 5. The maps obtained from log file 2126 by ICEG, the mapping is probabilistic grid based mapping

(a) DP_SLAM

Second experiment set is designed to compare the ICE with three scan matching algorithms. The simulation mechanism is similar to previous experiment based on moving with an operator and analyzing methods on Log files. But for this test a map with more complexity is considered to evaluate the method in more challenging environment. In Fig. 6 the results of QWSM, IDC and MbICP are displayed beside ICEG. In this figure the advantages of ICEG regarded to two last methods (IDC and MbICP) is obvious. It is notable that these algorithms are utilized widely in literature [17]-[19] and their unpleasant results depict the complexity of the problem. QWSM as a Robust WSM has an acceptable performance and comparing it with ICEG shows that except a negligible error in determining the angle between two walls occurred in both methods the map quality in ICEG is more admirable. In QWSM in addition to this tiny error a shift in vertical direction enters some uncertainty in resulted map and position estimation. From process time viewpoints after ICEG which spends the minimum time QWSM, IDC are close and MbICP is the worst case, which makes it unusable for online applications.

(b) ICEG

Figure 4. The maps obtained by log file 987 and passed path by robot.

In the second test the robot moved through the corridor that distorted the map in DP-SLAM. This method rotated a part of the map after coming out of the corridor which seems that it is a bottleneck for this algorithm. It contains shorter path with only 987 scans and shortage of features in the corridor makes the problem too difficult. In Fig. 4 the preference of ICEG than DP_SLAM is illustrated. Table I compared the process time of these methods in two runs as another aspect of this preference. This table shows that with longer path the average time of

190

implementation. It’s satisfying perfformance compared with some popular methods is shown in this paper. Three different novelties of this t method are feature extraction, matching and optimization. This method has the nt optimization algorithms ability to be combined with differen to generate better results that can be considered as future i employed beside ICE researches. Grid based mapping is matching to generate SLAM method d (ICEG). Combination of other mapping techniques is the subject of our research which is under construction.

(a) ICEG

(b) QW WSM

Figure 7. Comparing ICE Localization with real trajectory of moving robot.

REFERENCE ES (c) IDC

[1]

(d)MbIICP

Figure 6. The maps obtained four Scan Matchinng based SLAM

[2]

Experimental results also show thatt the estimated positions are near up to an actual position evven in high noisy environment. As illustrated in Fig.7 the resulting poses are compared with ground truth. The errors in X and Y axes had the mean 0.4 and 0.6 meter respectively. Thhe error variance was 0.35 for X direction and 0.5 for Y. Thesse parameters are admirable comparing with the distance passed during motion as shown in Fig. 7. V.

[3]

[4]

[5]

[6]

CONCLUSION AND FUTURE WORKS

Two required attributes to have a desirrable locomotion are localization and mapping which are thee goals of SLAM methods. Accuracy and speed are two aspeccts to evaluate the techniques investigated in this paper for ouur new suggested method, namely ICE matching. Extractinng features and matching based on geometrical concepts prepared a fast and flexible structure for ICE matching. Onlinne application, in different simulated robots from Zerg to P P2AT in Virtual Robot Environment, proved its advantages in

[7]

[8]

191

J. Leonard, H. Durrant-Whyte, “Dy ynamic map building for an autonomous mobile robot,” Int. J. Rob botic Research., vol. 11, no. 4, pp. 286–298, Aug. 1992. ous localization and mapping,” U. Frese, “A discussion of simultaneo Autonomous Robots, vol. 20, no. 1, pp. 25–42, 2006. D. Fox, W. Burgard, and S. Thrun, “M Markov localization for mobile robots in dynamic environments,” J. Artificial A Intelligence Research, vol. 11, 1999. nar, and S. Thrun, “Sonar-based W. Burgard, D. Fox, H. Jans, C. Maten mapping with mobile robots using EM M,” in Proc. International Conf. Machine Learning, 1999. G. Dissanayake, S. Clark, P. Newmaan, H. Durrant-Whyte, and M. Csorba, “A solution to the simulttaneous localization and map building (slam) problem,” IEEE Tran ns. Robotics and Automation, vol. 17, pp. 229–241, June 2001. B. Lisien, D. Morales, D. Silver, G. Kantor, I. Rekleities, and H. L and Mapping,” in Choset, “Hierarchical Simultaneous Localization Proc. IEEE/RSJ Conf. Intelligent Rob bots and Systems, Las Vegas, 2003. H Durrant-Whyte, and Ng. A.Y, S. Thrun, D. Koller, Z. Ghahramani, H. “Simultaneous mapping and localizzation with sparse extended information filters,” in Proc. 5th Work kshop, Algorithmic Foundations of Robotics, France, 2002. M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, “FastSLAM: A factored solution to the simultaneeous localization and mapping problem,” in Proc. AAAI National Conf. Artificial Intelligence, Canada, 2002.

[9]

[10] [11]

[12]

[13]

[14]

[15]

[16]

A. Eliazar, R. Parr, “DP-SLAM: fast, robust simultaneous localization and mapping without predetermined landmarks,” in Proc. Int. Conf. Artificial Intelligence, 2003. A. Eliazar, R. Parr, “DP-SLAM 2.0,” in Proc. IEEE Int. Conf. Robotics and Automation, 2004. A. I. Eliazar, R. Parr, “Hierarchical Linear/Constant Time SLAM using Particle Filters for Dense Maps” in. Advances in Neural Information Processing Systems, 2005. M. Begum, G. Mann, R. Gosine, “Integrated fuzzy logic and genetic algorithmic approach for simultaneous localization and mapping of mobile robots” Appl. Soft. Compu. J., vol. 8, pp. 150–165, 2008. C. Ze-Su, H. Bing-Rong, and L. Hong, “An Improved Polar Scan Matching Using Genetic Algorithm,” Info. Tech. J., vol. 6, no. 1, pp. 89-95, 2007. P. Biber, W. Stra_er, “The normal distribution transform: a new approach to laser scan matching,” in Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, 2003, pp. 2743 – 2748. O. Bengtsson, A.-J. Baerveldt, “Robot localization based on scanmatching - estimating the covariance matrix for the idc algorithm,” Robot. Auto. Syst. J., vol. 44, pp. 29-40, 2003. D. Rodriguez-Losada, J. Minguez, “Improved data association for icp-based scan matching in noisy and dynamic environments,” in Proc. IEEE Int. Conf. Robotics & Automation, 2007.

[17] J. Minguez, F. Lamiraux, L. Montesano, “Metric-based scan matching algorithms for mobile robot displacement estimation,” in Proc. Int. Conf. Robotics and Automation, Barcelona, 2005, pp. 3557–3563. [18] S.T. Pfister, K.L. Kriechbaum, S.L Roumeliotis, J.W Burdick, “Weighted range sensor matching algorithms for mobile robot displacement estimation,” in Proc. IEEE Int. Conf. Robotics and Automation, 2002, pp.1667– 1674. [19] A. Visser, B. A. Slamet, M. Pfingsthorn, “Robust Weighted Scan Matching with Quadtrees”, in "Proc. Fifth International Workshop on Synthetic Simulation and Robotics to Mitigate Earthquake Disaster (SRMED 2009), Graz, Austria, July 2009. [20] H.J. Sohn, B.K Kim, “An Efficient Localization Algorithm Based on Vector Matching for Mobile Robots Using Laser Range Finders,” Intell. Robot. Syst. J., vol. 51, no. 4, pp. 461-488, 2008. [21] V. Nguyen, S. Gachter, A. Martinelli, N. Tomatois, and R. Siegwart, “A comparison of line extraction algorithms using 2d range data for indoor mobile robotics,” Auto. Robot. J., vol. 23, no. 2, 2007. [22] F. Lu, E. Milios, “Robot pose estimation in unknown environments by matching 2D range scans,” Intell. Robot. Syst. J., vol. 18, no. 3, pp. 249–275, 1997.

192