Evaluation of an ICP Based Algorithm for Simultaneous Localization

2010 Latin American Robotics Symposium and Intelligent Robotics Meeting

Evaluation of an ICP based Algorithm for Simultaneous Localization and Mapping Using a 3D Simulated P3DX Robot Wilian França Costa‡ , Jackson Paul Matsuura∗ , Fabiana Soares Santana† , Antonio Mauro Saraiva‡ ‡ Laboratório

de Automação Agrícola, Escola Politécnica da Universidade de São Paulo Av. Prof. Luciano Gualberto, travessa 3, número 158, sala C2-56, Edifício de Engenharia Elétrica - Cidade Universitária, São Paulo - SP - CEP: 05508-900 Email: [email protected], [email protected] ∗ Divisão de Sistemas e Controle, Instituto Tecnológico de Aeronáutica Praça Mal. Eduardo Gomes, 50, São José dos Campos - SP - Brazil- CEP 12228-900 Email: [email protected] † Centro

de Matemática, Computação e Cognição, Universidade Federal do ABC Rua Santa Adélia, 166, Bloco B - Sala 811, Santo André - SP - Brasil - CEP 09210-170 Email: [email protected]

Abstract— Autonomous mobile robots can be applied to perform activities that should not, or cannot, be performed by humans due to inhospitable conditions or high level of danger. An autonomous mobile robot must be able to navigate safely in unfamiliar environments by reconstructing information from its sensors so as to plan and execute routes. Simultaneous Localization And Mapping, SLAM, technique allows the gradual creation of a map using data obtained from sensors while estimating the robot localization, and the Iterative Closest Point, ICP, algorithm is one of the approaches adopted for SLAM. This work proposes and evaluates an ICP-based algorithm for simultaneous localization and mapping of a robot. The algorithm was implemented in a simulated environment using R Microsoft Robotics Developer Studio, MRDS. Experimental results show that, in the evaluated trajectory, the method presented in this work has a better performance than the one obtained by the original ICP algorithm.

Limited resources of embedded hardware usually restrict the use of computationally expensive methods for SLAM, so SLAM algorithms usually apply stochastic [11] or iterative [12] methods, or a combination of both [13]. Due the facility of implementation and popularity, the Iterative Closest Point, ICP [14], algorithm is sometimes used for helping one approach of the SLAM problem [15]. This work presents an algorithm based on the ICP method for on line positioning of robots and incremental mapping of the environment, based on data obtained from a laser sensor installed in the robot. The method was implemented with the framework R Microsoft Robotics Developer Studio, MRDS, in the 2008 Academic Edition R1 [16]. The main advantage of using MRDS is that the same algorithms and control code can be applied to real robots without changes in the source code. On MRDS simulated environments and robots were applied to evaluate this approach.

II. MATERIAL AND METHODS

I. INTRODUCTION

A. Scan Matching

growing use of mobile robots to perform dangerous activities or activities that cannot be directly executed by humans, such T as extra-planetary exploration [1], complex actions of search and HE

Scan matching [17, p. 39], or scan correlation [6, p. 70], can be described as a search method to obtain the shortest distance between the data observed in two different subsets of points (2D or 3D). A subset is composed of observed data and the other of reference data. Both subsets are aligned in order to obtain the maximum correspondence, represented by a function describing the relative position between the two subsets and defining the translation and rotation required for a maximum overlap between the elements. Points can be obtained from scans performed by a laser sensor in 2D plane or 3D space. In this work, robot movements are estimated through the fusion of consecutive laser sensor scans, projected onto the 2D plane. Each scan shows precisely the outlying of the surrounded environment. Dead-reckoning estimation is usually applied as the starting point for this search though the error growth of this approach, applied to global localization and mapping of robot’s position is unlimited [12]. Scan matching algorithms can be classified in: 1) Feature to feature: correspondence is based only in features present in structured environments, such as wall corners, segments and extreme [18]; 2) Point to feature: correspondence is based in both points and features, where the points are matched to features extracted from

rescue [2], [3] and the prospection of the bottom sea, oil reserves, shafts, caves and other harsh environments [4], [5], shows the relevance of methods for its navegation. A mobile robot must have the ability to navigate safely in unfamiliar environments, avoiding obstacles, so as to be autonomous. Autonomous navigation of mobile robots and reconstruction of information from its sensors requires methods for enabling the construction of a reliable representation of the environment and for route planning and execution. Thus, a consistent representation of the environment is necessary in order to enable both reactive and active behaviours. Without this representation, a robot only can act when its sensors are stimulated and their actions are restricted to the range of available sensors [6, p. 2]. Simultaneous Localization and Mapping, SLAM, is a technique where robots set up a knowledge base through the gradual construction of the map, using the data from its sensors [7], [8] while estimating its localization on this map [9]. SLAM is also known as Concurrent Mapping and Localization, CML [10, p. 9], and is fundamental for robots navigation when the coverage of a particular region is required, avoiding obstacles [6]. 978-0-7695-4231-7/10 Unrecognized Copyright $26.00 Information © 2010 IEEE DOI 10.1109/LARS.2010.23

103

a previously known map or a map constructed during previous readings [19]; and 3) Point to point: correspondence is based only in the data points obtained by the scanner [20]. In this work, point to point is applied due the not so hard difficulty to implement a ICP like approach.

n Tk =

(7)

The degree of overlap transformation is defined by equation (8) and an index to accumulate errors in the established correspondences is defined by equation (9).

A laser scanner is a device where a laser beam is rotated at a predefined angle to measure the distances between the sensor and the obstacles in the environment, by the reflection of each pulse emitted by the laser. This process is defined as sensor scan and enables the robot to observe a 2D clip of the surrounding area. These sensor can be used in a configuration where the beam just rotates horizontally. In this case, readings are obtained only from the walls that surround the robot, disregarding ground and ceiling. Other configurations [15] combine rotations from horizontal and vertical axes, resulting in a 3D representation from the environment that also allows identification of obstacles on the ground. In this work, sensor is fixed on the robot and scans are parallel to the plane of the ground. The set of scans in a discrete time k defines a set of points Pk = {p0k , p1k , ..., pik , ..., pNk }, 0 ≤ i ≤ N , where each point pik represents the intersection of the laser beam with the closest objects. Each point is indicated by its polar coordinate, pik = (dik , αik ) is the i-th point of a scan performed by the i-th beam, dik and αik indicate the distance between the origin and localization of the device. The angles of each beam are consecutive and spaced by an angle resolution ρ. Defining Θ as the sensor field of vision, N is (Θ/ρ), or (Θ/ρ) − 1 for Θ = 2π. Suppose XYk is the coordinate system associated to the laser sensor at the discrete time k, and the first beam is aligned with the X axis, the angle of each a row of beams is given by equation (1) while the Cartesian coordinates of the i-th point are represented by equation (2). αi k = ρi. (1)

Λ Tk = N j=1

μ Tk =

n Tk N +1

ε (j)Tk ei Tk Λ Tk

(8)

(9)

C. Iterative Closest Point Algorithm The ICP algorithm is usually applied in computer graphics and computational vision problems to find correspondences between different 3D surfaces [14] but it also has been applied as an auxiliary method to solve the SLAM problem [15], [21] or to estimate a robot navigation in 2D [12]. The algorithm is composed of four basic steps: 1) The initial transformation T 0 k must consider some initial approximation which, in this case, can be given by the estimated odometry. Applying equation 3) to this value, it is possible to calculate Cartesian coordinates of the Pˆk as a projection of the Pk+1 in the XY k plane. 2) Quadratic distances for all possible combinations between Pˆk and Pk are calculated according to equation (10). 3) Correspondences are established by the function I(j), based on minimum quadratic distances, as defined in equation (11). The correspondence error related to equation (5) is given by (12) while outliers detection is performed by equation (6). 4) Estimate of relative displacement between the planes XY k e XY k+1 can be updated by minimizing equation (9), resulting in equations defined in equation (13), where the S terms are defined by equations in (14) [20].

(2)

Two consecutive readings, obtained in the discrete time k and k + 1, represent two different positions of the robot, Pk and Pk+1 , respectively in XYk and XYk+1 . In order to establish a correspondence between the scans, Pk+1 can be projected in XYk using the transformation Tk = (Δx, Δy, Δθ), containing the relative decoupling and the rotational increment between XYk and XYk+1 . Resulting points from this transformation will be inserted in the subset and defined by equation (3), with the respective polar coordinates calculated by equation (4).        cos(Δθ) − sin(Δθ) xi k+1 Δx x ˆi k (3) = + sin(Δθ) cos(Δθ) Δy yˆi k yi k+1  dˆi k = (xi k+1 + Δx)2 + (yi k+1 + Δy)2 ,   (4) yi k+1 + Δy . α ˆ i k = Δθ + arctan xi k+1 + Δx

ˆj k )2 + (yi k − yˆj k )2 eij = (xi k − x

I(j) = m,

se

emj = minN i=0 [eij ] .

2

2

e(j)Tk = xI(j) k − x ˆj k + yI(j) k − yˆj k

∂μTk = 0, ⇒ ∂ΔθTk

Assuming that Tk exists, an index of correspondence m = I(j) can be defined as the m-index of the pmk point [20], with a function indicative of the error established by equation (5). The correspondence hardly will be of 100% because the same point hardly will be read twice, some points will be discarded. This can be performed by the outliers detection function defined in equation (6). Thus, the number of valid correspondences can be established by equation (7). e(j)Tk = eTk [pm k , pˆi k ] .  0 if |e(j)Tk | ≥ , ε(j)Tk = 1 otherwise.

ε(j)Tk

j=1

B. Points generation by the laser sensor scanner

xi k = di k cos(αi k ), yi k = di k sin(αi k ).

N 

Δθnew = arctan



(10)

(11)

(12)

  Syk xk+1 −Sxk yk+1 Sxk Syk+1 −Syk Sxk+1 +nT  k ; Sxk xk+1 −Syk yk+1 −Sxk xk+1 −Syk yk+1 nT k

∂μTk = 0, ⇒ ∂ΔxTk Sxk − cos (Δθnew ) Sxk+1 + sin (Δθnew ) Syk+1 Δxnew = ; n Tk ∂μTk = 0, ⇒ ∂ΔyTk Sy − sin (Δθnew ) Sxk+1 − cos (Δθnew ) Syk+1 Δy new = k ; n Tk

(5) (6)

104

(13)

Sxk =

N 

ε(j)Tk xI(j) k ,

Algorithm ICP Input Pk : Laser reading at time k. Pk+1 : Laser reading at time k + 1. T ini k : Initial relative displacement between Pk and Pk+1 . Output Tk : Minimum distance transformation between Pk e Pk+1 .

j=1

S yk =

N 

ε(j)Tk yI(j) k ,

j=1

Sxk+1 =

N 

ε(j)Tk xj k+1 ,

j=1

Syk+1 =

N 

ε(j)Tk yj k+1 ,

j=1

Sxk xk+1 =

N 

ε(j)Tk xI(j) k xj k+1 ,

i ←0; {Start with maximum initial correspondence distances distmax and η max } 3 distlim ←distmax ; 4 η lim ←η max ; 5 {Start initial estimation T ini k } 6 T i k ←T ini k ; 7 do 8 Project Pk+1 to Pˆk using T i k 9 for each point in Pˆk do 10 Find quadratic distances between Pˆk and Pk given for eij ; 11 Identify the correspondence given for I(j); 12 Save found correspondences in R; 13 end for 14 i ←i + 1; ˆk and 15 T i k ←Rigid transformation (using eq.13) between P Pk using relations in R; lim 16 distlim ← dist 2 ; lim 17 η lim ← η2 ; lim 18 while num. of iter. i ≤ max. of iter. AND dist ≥ min dist 19 Tk ←T i k ; 20 return Tk end

(14)

1 2

j=1

Sxk yk+1 =

N 

ε(j)Tk xI(j) k yj k+1 ,

j=1

Syk xk+1 =

N 

ε(j)Tk yI(j) k xj k+1 ,

j=1

Syk yk+1 =

N 

ε(j)Tk yI(j) k yj k+1 .

j=1

This technique ensures the convergence to local minimum [14]. However, when applied with odometry estimation, the method can converge to an incorrect displacement because the odometry error is not limited when the local minimum is not the global minimum. Thus, a solution using this algorithm must have means to avoid or recover from local minimum convergence. The ICP algorithm is presented in Alg. 1. Complexity is O(n2 ) and is mainly defined by the method used to find the Cartesian correspondence [20]. This search can be optimized by kd-trees or other method, thus reducing the complexity to O(log n) for building the search tree [22] and O(log n) for selection [15]. R D. Microsoft Robotics Developer Studio

MRDS [16] is a tool for development of applications for a variety of different robotic hardware platforms. Based on the Representational State Transfer Model, it has a lightweight oriented service runtime using SOAP, and a group of tools for robot creation and simulation. MRDS also includes a 3D visual simulation. Communication in MRDS is based in XML over HTTP, and each robot and their sensors are referenced using Uniform Resources Identifiers, URI, allowing a wide variety of implementations and facilitating the extensibility. Runtime is composed basically by two mechanisms, named as Concurrency and Coordination Runtime, CCR, and Decentralized Software Services, DSS. Both are managed libraries running on .NET Commom Language Runtime (CLI). CCR provides classes and methods for the management of concurrency, tasks coordination and manipulation of flaws, and a programming model for multi-threading and synchronization of tasks inside a single process. DSS allows application interoperation using services and provides a communication infrastructure so that the communication between services may occur in a transparent way. A typical DSS application consists of multiple independent services running in parallel. The robot used in the simulations is a MobileRobots Pioneer 3DX (P3DX). This model is a commercial robot used for research, equipped in (your real version) with a PC, distance sensor (laser scanner and sonar) and wireless (Ethernet R 802.11) communication. SICK LMS200 was the laser scanner used. This sensor has an angular resolution of 0.5o and opening of o 180 , thus 361 different laser beams that compose one scan were available at the interval of 250 ms with maximum reach of 8 meters. Fig. 1 presents the P3DX simulated robot. Above the base, in blue, R it is the 3D representation of SICK LMS200.

Algorithm 1.

ICP Algorithm.

III. THE ALGORITHM This section introduces an algorithm for calculating an optimum Tk optm so as to obtain a better alignment between the planes XY k and XY k+1 . The algorithm uses a global and a local map. The heuristics to solve the minimum local problem consists of generating solutions near to the first calculated Tk in the cases where it does not reach a minimum quality index. Tk is replaced by the solution with the best quality index. Global map is defined by the subset of all scans before k − 1, projected onto XY k . Local map is the subset that contains only the k-th scan. M global k , defined by equation (15), is the subset of all corrected projections, where Pˆioptm is the projection of the laser scan applied to optimum Ti optm applying equations (3) or (4) to i, (0 ≤ i ≤ N ). Equation (16) defines τk as the subset of all optimum Ti , 1 ≤ i = k. Robot position at the instant k, with odometry coordinates (xodo k , y odo k ) and direction (θodo k ) is defined by equation (17).

105

optm M global k = {Pˆ0optm , Pˆ1optm , ... , Pˆk−1 }.

(15)

τk = {T0 optm , T1 optm , ... , Tk−1 optm }

(16)

Tk cand.i = ICP (Pkglobal , Pklocal , Tk aprox. i ), i, (1 ≤ i ≤ 4). Tk optm = Tk cand.i

(22)

⇒ μTk cand. i = min{τk cand. }. ⎡

⎤ ⎡ xpc cos(Δθ) k pc ⎣ yk ⎦ = ⎣ sin(Δθ) pc 0 θˆk

⎤ ⎡ odo ⎤ ⎡ ótm. ⎤ ΔxTk xk − sin(Δθ) ⎦ cos(Δθ) ⎦ ⎣ykodo ⎦ + ⎣Δy ótm. Tk 1 θkodo Δθótm. Tk

pc pc pc poscorr k = (x k , y k , θ k ) .

Fig. 1.

The tests were performed using the simulated environment presented in Fig. 2(a). Route is presented in red, in Fig.2(b).

Proposed solution is composed of the following steps: 1) A local map is projected using estimated odometry so as to obtain the initial approach for the ICP algorithm. First scan is assumed as optimal, so the optimum is defined as T0 ótimo = (0, 0, 0), and Pˆ0ótimo = Pklocal is inserted in the global map. 2) Tk−1 optm , obtained by in the subset defined by equation (16), is applied as the initial estimation of displacement between maps, and is provided as input for ICP, which returns a candidate solution, Tk cand.0 . that will be inserted in the subset of candidate solutions τk cand. . The subset τk cand. is defined by equation (19). 3) Quality of Tk cand. 0 is calculated using equation (9). 4) If μTk cand. 0 < υ then μTk cand. 0 is considered an adequate approximation and will be inserted in the subset defined by equation (16). In this work, υ = 4 × 10−4 and this value was experimentally obtained by seeking misplaced solutions. 5) If μTk cand. 0 is not considered an adequate approximation, other four candidate solutionsmax are generated by ICP using (20) and (21) where ψ = dist2 . 6) Transformation in the set τk cand. with the best quality index will be defined as the optimal, Tk optm , according to equation (22). 7) Tk optm allows to correct the odometry position by applying equation (23). Correct position is presented in equation (24). τk

= {Tk

cand. 0

, Tk

cand. 1

, ... , Tk

cand. n

(24)

IV. RESULTS AND DISCUSSION (17)

We define distmax is initial maximum distance, and distmin is initial minimum distance, to seek a correspondence. η max is the maximum angular distance for seeking a correspondence. The  from equation (6) is a critical parameter for ICP running time and for this work is given by equation (18). After performing successive tests, the following values were chosen: η max = 25o , distmax = 0, 5m, ω = 2 and pv = (0, 0).

2 (18) i = η lim pv − pˆik  + distlim , i = 1...N.

cand.

(23)

Pˆkoptm is composed of Pk local points after applying projection, according to equation (3), using Tk optm . Pˆkoptm is inserted in M global k .

R Simulated P3DX robot with SICK LMS200.

posk = xodo k , y odo k , θodo k .

(21)

}

0 0 0 Tk aprox. 1 = Δxcand. − ψ, Δy cand. , Δθcand. Tk Tk Tk

0 0 0 , Tk aprox. 2 = Δxcand. + ψ, Δy cand. , Δθcand. Tk Tk Tk

aprox. 3 cand. 0 cand. 0 cand. 0 , Tk = ΔxTk , Δy Tk − ψ, ΔθTk

0 0 0 Tk aprox. 4 = Δxcand. , Δy cand. + ψ, Δθcand. . Tk Tk Tk

(a) 3D environment map simulation *URXQGíWUXWK0DSDQG5RERW3DWK 6 Trajectory 4

m

2

0

−2 Start

−4 Finish

−6 −8

−6

−4

−2

0

m

2

4

6

8

(b) Ground-truth map with robot test path Fig. 2.

3D Simulation (a) and robot trajectory (b).

The simulated version of the differential drive has no odometer reading, so it was necessary to implement an artificial odometer using the actual position of the robot in the simulated environment, equation (25), and adding significant noise in order to simulate landslides and integration errors, equations (26).  mgt k = Δxgt k 2 + Δy gt k 2 ,   (25) Δy gt k . θgt k = arctan Δxgt k

(19)

(20)

106

xodo k =

k 

5RERW3RVLWLRQí,&3

mi cos(θgt i ) + bx + rx Dxi ,



i=1

y odo k =

k 

(26)

k

=

θ

gt

i





+ bθ + r θ D θ i .







P

θ

odo





i=1 k 





mi sin(θgt i ) + by + ry Dy i ,

&RUUHFWHG (VWLPDWHGRGR 5HDO

i=1

í

The accuracy of the algorithms was evaluated according to the following performance indicators: 1) Absolute Position Error, APE: defined by equation (27), APE evaluates errors associated to the trajectory reconstruction, for each moment a new reading of the robot’s position is obtained; 2) Absolute Angular Error, AAS: defined by equation (28), evaluates errors associated to the correction of the robot´s direction, for each moment a new position is obtained; and 3) Kolmogorov-Smirnov, KS, Two-Sample Test: in order to asses the relevance of results, the KS Two-Sample Test was applied to verify whether two samples belong to the same continuous distribution (hypothesis zero), or belong to different distributions (hypothesis alternative). The test returns the parameters h and p, where h = 1 when the null hypothesis is refuted, and h = 0 otherwise. p represents the asymptotic probability to h, where the null hypothesis is rejected if p is greater than the defined significance level (5% in this work). Differences between the samples increase as this value decreases.  edk = (x* k − xgt k )2 + (y * k − y gt k )2 . (27)

 

 



í



í  í

 í

í

í

í

P









(a) ICP Trajectory 5RERW3RVLWLRQí6ROXWLRQ 6

Corrected Estimated(odo) Real

100 4

2 70

80

90 0

P

60

−2

110

30 20

40

50 120

−4 10 −6

eθk = |θ* − θgt |

[0o , 360o [.

130

(28)

−10

Fig. 3 shows the trajectories performed by the ICP algorithm and by the solution proposed in this work. They do not differ significantly to the vicinity of coordinates (3, 1) and from this point on the ICP algorithm propagates the error, worsening the following estimates, as shown in Fig. 3(a). The proposed solution, on the other hand, can recover itself, reaching the final position with a good approximation of the actual position of the robot, as presented in Fig. 3(b). Table I contains the performance indicators obtained as the result of the tests. Note the relative difference of the presented solution, compared to the ICP algorithm, of 94, 82% to edfinal (m) and 67, 39% to eθfinal (o ). The obtained average and standard deviation were also smaller to the proposed solution. Table II presents the KS Two-Sample tests for the simulated environment. Note that the null hypothesis was refuted for both (ICP and Sol.). And finally the figure 4 show the final results maps.

−8

−6

−4

−2

P

0

2

4

6

(b) Proposed Solution Trajectory Fig. 3.

Trajectory for (a) ICP test and (b) proposed solution test.

R EFERENCES [1] M. Maimone, A. Johnson, Y. Cheng, R. Willson, and L. Matthies, “Autonomous navigation results from the mars exploration rover (MER) mission,” in Experimental Robotics IX, 2006, pp. 3–13. [2] E. L. Colombini, A. A. Silva, C. F. Alves, R. I. G. Silva, and J. P. Matsuura, “Itandroids-vr approach for the virtual robots rescue task.” in Jornada de Robótica Inteligente - XXVI CSBC. Campo Grande/MS, Brazil: SBC, 2006, pp. 1–10. [3] R. Ouellette and K. Hirasawa, “A comparison of slam implementations for indoor mobile robots,” in Intelligent Robots and Systems, 2007. IROS 2007. IEEE/RSJ International Conference on, oct. 2007, pp. 1479 –1484. [4] O. N. J. Benedito and C. A. J. Dalla., “A Petrobras e a exploração de petróleo offshore no brasil: um approach evolucionário.” Rev. Bras. Econ. [online]., vol. 61, no. 1, pp. 95–109, Mar. 2007. [5] D. Silver, D. Bradley, and S. Thayer, “Scan matching for flooded subterranean voids,” in IEEE conference on Robotics Automation and Mechatronics (RAM), vol. 1, December 2004, pp. 422 – 427. [6] T. Bailey, “Mobile robot localization and mapping in extensive outdoor environments,” PhD Thesis, University of Sydney, Aug. 2002. [7] J. Gutmann and K. Konolige, “Incremental mapping of large cyclic environments,” in Computational Intelligence in Robotics and Automation, 1999. CIRA ’99. Proceedings. 1999 IEEE International Symposium on, 1999, pp. 318–325. [8] A. Howard, “Multi-robot mapping using manifold representations,” in Robotics and Automation, 2004. Proceedings. ICRA ’04. 2004 IEEE International Conference on, vol. 4, 2004, pp. 4198–4203 Vol.4. [9] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics. The MIT Press, Sept. 2005. [10] J. E. Guivant, “Efficient simultaneous localization and mapping in large environments,” PhD Thesis, The University of Sydney, May 2002.

V. CONCLUSION This work introduces a new ICP-based algorithm for simultaneous localization and mapping of robots using data obtained from sensors so as to estimate the robot localization. The algorithm was implemented and evaluated in a simulated environment, using R Microsoft Robotics Developer Studio, MRDS. Indicators of errors in the robot trajectory shows that the method presented in this work is able to obtain a better performance than the original ICP algorithm, at least for the environment where the tests were performed. Future works include the evaluation of this solution in other simulated and in real environments. The purpose is to infer the applications of this new solution in order to verify in what conditions this approach can be as accurate as, or in some cases, even better than ICP algorithm, since it can recover itself from the problems related to the autonomous robot trajectory caused by the existence of local minima.

107

TABLE I P ERFORMANCE INDICATORS

ICP Sol. Odom.

edfinal (m)

eθfinal (o )

e¯d (m)

e¯θ (o )

σed (m)

σ eθ ( o )

3.99400 0.20690 7.29400

8.20500 2.67600 43.72000

0.46850 0.07765 1.98900

1.60400 0.40710 12.49000

0.75950 0.17250 2.01400

1.91300 0.69980 13.34000

TABLE II KS Odom.

TESTS RESULTS

Sol.   ed =⇒ h = 1, p = 3, 9974 × 10−41  , eθ =⇒ h = 1, p = 4, 5851 × 10−36 2GRPHWU\0DS

ICP   ed =⇒ h = 1, p = 4, 2859 × 10−30  , eθ =⇒ h = 1, p = 2, 3100 × 10−14

,&30DS

8 6

6ROXWLRQ0DS













4

−2



P

0

P

P

2

í

−4

í

í

−6 í

−8 −10

−5

P

0

5

10

 í

í í

(a) Odometry Map

í

í

í



P









(b) ICP Map Fig. 4.

Simulations Final Maps

[11] D. Hahnel, W. Burgard, D. Fox, and S. Thrun, “An efficient fastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements,” in Intelligent Robots and Systems, 2003. (IROS 2003). Proceedings. 2003 IEEE/RSJ International Conference on, vol. 1, 2003, pp. 206–211 vol.1. [12] F. Lu and E. Milios, “Robot pose estimation in unknown environments by matching 2D range scans,” JOURNAL OF INTELLIGENT AND ROBOTIC SYSTEMS, vol. 18, pp. 249—275, 1994. [13] P. Biber and W. Strasser, “nScan-Matching: simultaneous matching of multiple scans and application to SLAM,” in Proc. IEEE International Conference on Robotics and Automation (ICRA), 2006, pp. 2270— 2276. [14] P. J. Besl and N. D. McKay, “A method for registration of 3-D shapes,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 14, no. 2, pp. 239–256, 1992. [15] H. Surmann, A. Nüchter, K. Lingemann, and J. Hertzberg, “6D SLAM - preliminary report on closing the loop in six dimensions,” in Proc. of the 5th IFAC Symposium on Intelligent Autonomous Vehicles (IAV´04), Lisbon, 2004. [16] MICROSOFT. (2009) Microsoft Robotics Developer Studio. [Online]. Available: M http://www.microsoft.com/robotics/ [17] B. Slamet, A. Visser, M. Pfingsthorn, and N. Vlassis, “ManifoldSLAM: a Multi-Agent simultaneous localization and mapping system for the RoboCup rescue virtual robots competition,” Master’s thesis, Dec. 2006. [18] K. Lingemann, H. Surmann, A. Nüchter, and J. Hertzberg, “Indoor and outdoor localization for fast mobile robots,” in Proc. of IEEE/RSJ Internacional Conference on Intelligent Robots and Systems (IROS´04), vol. 2004, 2004, pp. 2185—2190. [19] I. Cox, “Blanche-an experiment in guidance and navigation of an autonomous robot vehicle,” Robotics and Automation, IEEE Transactions on, vol. 7, no. 2, pp. 193–204, 1991. [20] J. L. Martínez, J. González, J. Morales, A. Mandow, and A. J. García-Cerezo, “Mobile robot motion estimation by 2D scan matching with genetic and iterative closest point algorithms,” Journal of Field Robotics, vol. 23, no. 1, pp. 21–34, 2006. [21] K. Pathak, A. Birk, N. Va˘skevi˘cius, and J. Poppinga, “Fast registration based on noisy planes with unknown correspondences for 3-d mapping,” Robotics, IEEE Transactions on, vol. 26, no. 3, pp. 424 –441, june 2010. [22] Z. Zhang, “Iterative point matching for registration of free-form curves and surfaces,” Int. J. Comput. Vision, vol. 13, no. 2, pp. 119–152, 1994.

108

í

í

í

í



P





(c) Solution Map