Map Alignment Based on PLICP Algorithm for Multi-robot SLAM

Weijun Xu, Rongxin Jiang, Yaowu Chen

Institute of Advanced Digital Technology and Instrumentation, Zhejiang University, Hangzhou, 310027, P.R China

xwjrox@zju.edu.cn, rongxinj@zju.edu.cn, cyw@mail.bme.zju.edu.cn

Abstract- Inter-robot observation (Rendezvous) strategy is usually adopted to align local maps in multi-robot SLAM system while the initial relative positions are unknown. However, the accuracy of the estimated alignment is often affected by various uncertainty sources. This paper presents a new approach based on the Point-to-Line Iterative Closest Point (PLICP) algorithm to improve the accuracy of the map alignment: First, local maps are generated by means of the FastSLAM algorithm, then the initial alignment parameters are calculated by inter-robot observation, and finally the PLICP algorithm is used to update these parameters. Experimental results illustrate the accuracy improvement of the proposed approach.

I. INTRODUCTION

The task of exploring and mapping an unknown environment is considered as one of the fundamental problems in mobile robotics. This task is well known as simultaneous localization and mapping (SLAM), where the robot is required to build a map and localize itself at the same time [1, 2]. Multi-robot SLAM (MR-SLAM) system is often applied to explore and map a complex environment more efficiently and accurately [3, 4]. The challenging thing for such a system is to generate a reliable global map from local maps constructed independently by each individual robot. A critical step for fusing the global map is to align the local maps onto a common reference frame by finding transformation between them.

Two strategies are usually adopted to determine the alignment between local maps. The first one is to compute the transformation by searching corresponding landmarks between overlapped maps [5, 6]. The other one is known as inter-robot observation or rendezvous strategy, in which the robots measure their relative ranges and bearings when encountering each other within robot’s detection range [7-9]. The map alignment can then be computed with these observations. Although the latter strategy is straightforward, the estimated alignment is often perturbed by noises in a real environment [7]. Recently, Iterative Closest Point (ICP) [10] algorithm has been widely applied in SLAM for matching two point sets accurately [5, 11, 12]. There are many variants of ICP developed so as to improve the efficiency and accuracy of the algorithm. For example, [10] suggested employing K-D tree to reduce the search time for finding the correspondences. Distance function was formalized by

fractional root mean squared distance to eliminate outliers in [13]. However, the convergence speed of ICP was restricted with point-to-point metric. In [14], a point-to-line metric was used to make the alignment more precise with less iteration from a good initial guess.

In this paper, a combination of the two alignment strategies is used to improve the accuracy. An initial transformation is first calculated by employing inter-robot observation. Then if the local maps were overlapped, PLICP updating process would be performed to exploit information of duplicated landmarks.

The rest of paper is organized as follows: In section II, general map alignment problem in MR-SLAM is presented. In section III, details of the proposed approach are presented. Section IV shows the simulation results. Section V presents the final conclusions.

II. MAP ALIGNMENT PROBLEM IN MR-SLAM

The goal of MR-SLAM is to build a global joint map of the unknown environment. This problem is more inclined to be solved in a distributed way for robustness by fusing local maps created by each robot in the team to form a global one. Map fusion processes can be divided into two main phases [15]: The first one is called map alignment that aims at computing the coordinate transformation between local maps. The second phase is known as map merging that merges matched landmarks to build a global map. This paper is focused on the first phase; i.e., map alignment. A multi-robot team consisting of two members is illustrated for simplicity, but the idea can easily be extended to larger multi-robot teams.

Assuming that two robots and explore an environment with the same control model and observation model. Control noise is Gaussian with zero mean and covariance matrix . Observation noise is also Gaussian with zero mean and covariance matrix . Two 2D reference frames and are built with regard to their initial locations. During the process of MR-SLAM, local maps and are created by robot and

respectively, and each of them is represented in its own reference frame. The map alignment is done by computing the transformation between the reference frames of the two

978-1-4673-0158-9/12/$31.00 ©2012 IEEE 926

maps. The transformation is represented by a rotation matrix and a translation vector , which can be described as; (1) where

(2)

The tuple is defined as alignment parameters or transformation parameters. is defined as the transformation from frame to , and is defined as the transformation from frame to . The way to compute is similar as computing . Therefore, only

is being considered in this paper. When the transformation is obtained, can be transformed to the reference frame as ; (3) where and are rotation matrix and translation vector of respectively.

As mentioned in section I, two strategies can be adopted to find the alignment. In this paper, inter-robot observation is employed to yield an initial alignment guess as the starting condition for the following process with PLICP algorithm.

III. PLICP BASED ON MAP ALIGNMENT

PLICP algorithm is used to find an accurate alignment of two 2D or 3D point sets with a few iterations. The algorithm selects the closest points as correspondences and calculates the transformation; i.e., rotation and translation , for which the error between the transformed points and the model points is minimal. The whole procedure is described as follows: FastSLAM [16] is first applied to each robot and local maps represented by 2D landmarks are created. After a certain time interval (depending on the threshold), the transformation between robot pairs is computed by the inter-robot observation. Then, both local maps are represented in the same reference frame and landmark correspondences can be found using K-D tree algorithm. When the correspondences number reaches a threshold, they will be used as the initial guess for PLICP to obtain a better alignment.

Fig. 1. Relative position (range and bearing) measurement is taken when two robots encounter each other.

A. Single Robot FastSLAM In this step, each robot performs single robot FastSLAM to

build its local map. For a single robot FastSLAM, particles are used to get weighted samples of estimated robot's position and observed landmarks with its mean and covariance. Each particle is denoted as;

(4) where is the particle weight, and are mean and covariance of the robot position,

are mean and covariance corresponding to the observed landmarks, and

and are particle and robot indices respectively.

As many as maps can be generated for each robot with particles, but only one map for each robot is necessary to

perform the alignment process. As in [15], the weighted average of the maps is calculated to represent the final local map where each landmark is described as (5) and (6);

(5)

(6)

In the similar way, final positions for each robot can be described as (7) and (8);

(7)

(8)

B. Inter-robot Observation

To apply inter-robot observation strategy, both robots are planned to meet and measure each other’s relative positions at rendezvous locations. As is shown in Fig. 1, relative ranges

and bearings are measured when two robots encounter each other. With these measurements, the final relative range and bearing between the robots and are computed as; (9) Moreover, current robot positions and

are obtained by (7). The transformation parameters between the two robot’s reference frames and are uniquely determined as (10), (11) and (12); (10) (11) (12)

In a conventional inter-robot observation application, information of local maps are not used to calculate the transformation between the robot frames. However, it has been proved in [7] that when there is an overlap between the two maps, those duplicated landmarks can provide additional

927

information. Therefore, an extra PLICP process is added to make use of this information.

C. PLICP Alignment Updating

Given a model set , a scene set , and an initial alignment between them, the ICP algorithm is applied to find transformation , for which the sum of the squares of the Euclidean distances between the correspondences of and

is minimized. For a point-to-point metric of a general ICP algorithm, the error function is defined as;

(13) where and are rotation matrix and translation vector of ,

and are numbers of points in and , is the i-th point of , and is the closest point of in .

Instead of the point-to-point metric, the PLICP algorithm uses a point-to-line metric to speed up convergence of the computation from a good first guess [14]. To achieve this, the second closest point of , denoted as , is also found. Then the error function is defined as the sum of the squares of the Euclidean distances from point to the line containing the segment - ;

(14) where is the normal vector of the segment - . After step A, all observed landmarks can be computed by (4), and they are integrated to create two local maps

= and = . In this step, PLICP algorithm is used to compute the alignment between the maps. There are two necessities for a good performance of the PLICP alignment algorithm; a good initial guess of the transformation and a robust eliminating approach of outliers. The initial guess is given by the result of inter-robot observation in step B. In addition, sub-maps and made up of correspondences between and are constructed to eliminate some possible outliers. PLICP alignment updating is more efficient and accurate with these new maps.

In order to find the correspondences between and , both maps have to be represented in the same reference frame. This is done by transform to by (3) with current transformation . A traverse search for the correspondences in of all landmarks in has the time complexity , which is very time consuming. Therefore, K-D tree search algorithm is employed to reduce the time complexity to : is first constructed in the form of K-D tree, then for each landmark in , its best match in is determined by minimizing the Euclid distance between each landmark pair. The matches with minimal Euclid distance greater than some threshold are regarded as outliers. As a result, sub-maps and are obtained after rejecting those outliers. When the number of

landmarks in reaches the minimum limitation, a new estimated transformation is calculated. The new transformation is applied to the original local maps in the next iteration. Finally, PLICP terminates when iterations reach a predefined maximum number, or the differences between two consecutive estimated transformations is close to zero. The detailed algorithm is described as pseudo code in Tab. I.

TABLE I

PLICP ALIGNMENT ALGORITHM Input: two maps = and =

an initial transformation

Output: the transformation T between and 1: ;2: iter = 0; 3: while (iter < a && not converged) do 4: iter = iter + 1; 5: Transform to with respect to transformation ; 6: Detect landmark correspondences using K-D tree with b; 7: Construct sub maps and with correspondences; 8: if (landmark number of > c) then 9: for (each landmark in ) 10: Find two closest landmarks , in ; 11: Compute the normal vector of segment - ; 12: end for 13: Compute new estimated transformation by (14); 14: end if 15: end while

aMaximal number of iterations; bThreshold of Euclid distance between points; cMinimal number of corresponding points

IV. RESULTS

Simulation experiments for multi-robot map alignment were carried out based on Tim Bailey's SLAM simulator [17] using FastSLAM2.0 algorithm. As shown in Fig. 2, a 2D environment of landmarks and two trajectories were first created. Two mobile robots were simulated to move along its corresponding trajectory and perform single robot FastSLAM. The basic configurations for each robot were as follows: The robots had 4 meters wheel base and moved at 3 m/s with a maximum steering angle of 30 degrees. The control frequency was 8 Hz and observation scans were obtained at 1 Hz. Total particle number was 100 for FastSLAM algorithm.

-50 -40 -30 -20 -10 0 10 20 30 40 50

-40

-30

-20

-10

0

10

20

30

40

Fig. 2. Robot trajectories and 2D map of environment created for simulation: The polygonal lines connecting circles are robot trajectories, stars are landmarks, and triangles represent robots’ initial traveling positions.

928

0

2

4

Stan

dard

Dev

iatio

ns o

f Alig

nmen

t Par

amet

ers

Inter-robot observation PLICP updating

0

2

4

σ(t y) (

m)

10 20 30 40 50 60 70 80 90 1000.00

0.05

0.10

s(t x) (

m)

σ(t Θ

) (ra

d)

Rendzevous Step

Fig. 3. Comparison of standard deviations of alignment parameters, blue and black curves are with respect to the cases with and without PLICP updating.

Landmarks could only be detected within a maximum range of 30 meters and 180 degrees frontal field-of-view. To focus on evaluating the proposed approach, inter-robot observation was assumed to be performed successively at a frequency of 1.6 Hz. The control noise and observation noise were (0.3 m/s, /180 rad), and (0.1m, /180rad) respectively. The above noise values were defined as the base ones. Besides, the basic configuration of PLICP updating algorithm was:

=10, =5, and =2m. TABLE II

MAES OF WITH FIXED CONTROL NOISE AND DIFFERENT OBSERVATION NOISE INTENSITIES

Alignment Parameters Observation Noise Multiple 2 4 8

a/m 2.051 3.930 7.868 b/m 1.496 3.482 7.706 c % 27.0 11.3 2.0

a/m 2.200 4.285 8.235 b/m 1.483 3.824 8.140 c % 32.5 10.7 1.1

a/rad 0.045 0.083 0.173 b/rad 0.028 0.070 0.169 c % 36.1 15.7 2.3

aCase of inter-robot observation; bCase of PLICP updating; cImprovement percentage of accuracy.

TABLE III MAES OF WITH FIXED OBSERVATION NOISE AND DIFFERENT

CONTROL NOISE INTENSITIES Alignment Parameters Control Noise Multiple

2 4 8 a/m 1.686 2.679 7.371

b/m 1.476 2.416 7.283 c % 12.4 9.7 1.2

a/m 1.914 2.748 7.300 b/m 1.665 2.466 7.233 c % 13.0 10.2 0.9

a/rad 0.036 0.049 0.157 b/rad 0.029 0.044 0.156 c % 18.4 11.0 0.7

aCase of inter-robot observation; bCase of PLICP updating; cImprovement percentage of accuracy.

The real alignment parameters were given as [74.555m, -76.090m, 2.618rad] in the simulation. A single trial completed when both robots reached its ending point. After performing 200 trials, the mean absolute errors (MAEs) obtained by inter-robot observation with and without PLICP updating were [0.698m, 0.733m, 0.015rad] and [1.086m, 1.143m, 0.023rad], respectively. Fig. 3 shows the comparison of standard deviations of alignment parameters calculated by the above two processes. It can be seen that during most rendezvous steps, qualities of the initial alignment parameters are much improved after PLICP updating process. However, the result of PLICP updating is the same as the initial guess during the forefront 27 rendezvous steps. This is due to the fact that PLICP updating was not performed because not enough correspondences between local maps were found. In those situations, final alignments were decided by the initial ones.

The impacts of different control noise and observation noise are also evaluated. The results are presented in Tab. II and Tab. III, in which one of them remained fixed while the other one is multiplied. It can be seen that PLICP updating could always obtain a better MAE under all these listing noise combinations. Furthermore, the improvement depends on the intensities of both control and observation noises: When the observation noise intensity gets higher, the improved accuracy of alignment degrades from 27.0% down to 1.1% (the least improvement percentage of three alignment parameters) as shown in Table II. When the control noise intensity gets higher, the improved accuracy of alignment degrades from 12.4% down to 0.7% as shown in Table III. This is because when either of noise intensities gets higher, the initial transformation guess calculated by inter-robot observation is cluttered, and meanwhile more spurious landmark matches are introduced.

V. CONCLUSIONS

This paper has presented a map alignment approach for multi-robot SLAM. The key idea of this approach is that an extra PLICP updating process is employed after inter-robot observation. When local maps are overlapped, the extra process can help to exploit the information of landmark correspondences. Thus, it is possible to get a higher quality of map alignment by exploiting the information of duplicated landmarks. There is a limitation to the approach; i.e. it does not work when there is little or no overlapping between local maps. In addition, the improvement of quality depends on both control and observation noise intensities. Experimental results have validated the characteristics of the proposed approach.

For future work to ensure an ideal alignment result even under the high noise intensity conditions, robust outlier rejection algorithms such as RANSAC could be adopted before updating process.

929

ACKNOWLEDGMENT

This paper is supported by the National Natural Science Foundation of China under Grant 40927001, the project of Key Scientific and Technological Innovation Team of Zhejiang Province under Grant 2011R09021-02 and the Fundamental Research Funds for the Central Universities.

REFERENCES [1] G. Dissanayake, P. Newman, and S. Clark, D. H. Whyte, and M. Csorba,

“A solution to the simultaneous localization and map building (SLAM) problem,” IEEE Transactions on Robotics and Automation, vol. 17, no. 3, pp. 229-241, 2001.

[2] H. D. Whyte and T. Bailey, “Simultaneous localization and mapping: part I,” IEEE Robotics and Automation Magazine, vol. 13, no. 2, pp. 99-110, 2006.

[3] A. Howard, “Multi-robot simultaneous localization and mapping using particle filters,” International Journal of Robotics Research, vol. 25, no. 12, pp. 1243-1256, 2006.

[4] D. Fox, J. Ko, K. Konolige, and B. Limketkai, “Distributed multi-robot exploration and mapping,” in Proceedings of the IEEE, vol. 94, no. 7, pp. 1325-1339, 2006.

[5] M, Ballesta, A. Gil, O. Reinoso, M. Julia and L. M. Jimenez, “Multi-robot map alignment in visual SLAM,” WSEAS Transactions on Systems, vol. 9, no. 2, pp. 213-222, 2010.

[6] H. C. Lee and B. H. Lee, “Improved Feature Map Merging Using Virtual Supporting Lines for Multi-Robot Systems,” Advanced Robotics, vol. 25, no. 13-14, pp. 1675-1696(22), 2011.

[7] X. Zhou and S. I. Roumeliotis, “Multi-robot slam with unknown initial correspondence: The robot rendezvous case,” in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2006.

[8] N. E. Ozkucur and H. L. Akin, “Cooperative Multi-robot Map Merging Using Fast-SLAM,” in Proceedings of the RoboCup International Symposium 2009 , vol.5949, pp. 449-460, 2009.

[9] L. Carlone, M. K. Ng, J. Du, B. Bona, and M. Indri, "Simultaneous Localization and Mapping Using Rao-Blackwellized Particle Filters in Multi Robot Systems," Journal of Intelligent and Robotic Systems, vol. 63, no. 2, pp. 283-307, 2010.

[10] P. J. Besl and N. McKay, “A method for registration of 3-D shapes,” IEEE PAMI, vol. 14, no. 2, pp. 239-256, Mar. 1992.

[11] M. Tomono, “Robust 3D SLAM with a stereo camera based on an Edge-Point ICP algorithm,” in Proceedings of the IEEE Conference on Robotics and Automation, pp. 4306-4311, 2009.

[12] R. Guo, F. Sun, and J, Yuan, “ICP based on Polar Point Matching with application to Graph-SLAM,” in Proceedings of the International Conference on Mechatronics and Automation, 2009.

[13] J. Phillips, R. Liu and C. Tomasi, “Outlier Robust ICP for Minimizing Fractional RMSD,” in Proceedings of the International Conference on 3-D Digital Imaging and Modeling, 2007.

[14] A. Censi, “An ICP variant using a Point-to-Line Metric,” in Proceedings of the International Conference on Robotics and Automation, 2008.

[15] V.A. Romero and O.L.V. Costa, “Map merging strategies for multi-robot FastSLAM: A Comparative Survey,” in Latin American Robotics Symposium and Intelligent Robotics Meeting, pp.61-66, 2010.

[16] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics, The MIT Press, Cambridge, Massachusetts, London, England, 2005.

[17] T. Bailey, “SLAM Package of Tim Bailey,” http://www.openslam.org/.

930

Powered by TCPDF (www.tcpdf.org)