SLAM and Map Merging

Comunidad de Madrid and the University of Alcala, support through the projects RoboCity2030 (CAM-S-0505/DPI/000176) and SLAM-MULEX (CCG07-UAH/DPI-1736).

Abstract-This paper presents a multi-robot mapping and localization system.Learning maps and efficient exploration of an unknown environment is a fundamental problem in mobile robotics usually called SLAM (simultaneous localization and mapping problem).Our approach involves a team of mobile robots that uses Scan-Matching and Fast-SLAM techniques based on scan-laser and odometry information for mapping large environments.The single maps generated for each robot are more accurate than the maps generated only by odometry.When a robot detects another, it sends its processed map and the master robot generates a very accurate global map.This method cuts down the global map building time.Some experimental results and conclusions are presented.

I. INTRODUCTION
EARNING maps and efficient exploration of unknown environments is a fundamental problem in mobile robotics.This problem is usually called SLAM (simultaneous localization and mapping problem) [1,2,3,4,5,6], which includes estimating the robot's position on the map and building up a map using the sensory input and the estimated robot´s pose.
The problem of exploration of an unknown environment has been extensively studied, first using single robot systems with a variety of sensors and later using teams of robots.The first multi-robot exploration systems implementations were simple extensions of the single robot implementations.Multiple robot systems are more complex than other distributed systems because they have to deal with a real environment, which is more difficult to model since it is dynamic, unpredictable, noisy, etc.Therefore, the extension to multiple robots systems brings several new challenges and dif8iculties [7] [8]: robot coordination, integration of information collected by different robots into a consistent map and dealingwithlimitedcommunication.
Multirobot exploration systems are usually classified as centralized and decentralized.Centralized systems obtain solutions close to the optimal but they are computationally intensive and its good functioning depends on the central R. Barea is with the Electronics Department, University of Alcalá, Spain (e-mail: barea@depeca.uah.es).
M. Ocaña is with the Electronics Department, University of Alcalá, Spain (e-mail: mocana@depeca.uah.es).module.On the other hand, decentralized systems are flexible and robust, but frequently achieve significantly suboptimal solutions.Therefore, the difficulty of the coordination task strongly depends on the knowledge of the robots.If the robots know their relative locations and share a map of the area they explored so far, then effective coordination can be achieved by guiding the robots into different, nonoverlapping areas of the environment [9], [10], [11].However, if the robots do not know their relative locations, then it is far less obvious how to effectively coordinate them, since the robots do not share a common map or reference frame [7].
Map merging task consists on building a consistent model of an environment with data collected from different robots.If the initial locations of the robots are known, map merging is a rather straightforward extension of a single robot mapping [12], [13], [14].If robots do not know their relative locations, it is more dif8icult, since it is not clear how and wheretherobots'tracesshouldbeconnected.
One of the hardest problems in robotic mapping is the loop closure [15].When a robot navigates throughout a large cycle in the environment, it must face the hard data association problem of connecting correctly the data to its own map under large position errors.To scale to large-scale environments, oneoptionconsistsontransformsequencesoflaserrangescans into odometry measurements using range-scan registration techniques [16], which reduces the wellknownparticledeprivationproblem [17] [18].
Rao-Blackwellized particle filters have been introduced as effective means to solve the simultaneous localization and mapping (SLAM) problem.This approach uses a particle filter in which each particle carries an individual map of the environment [19], [20].The main problem of the Rao-Blackwellized approaches is their complexity, measured in terms of the number of particles required to build an accurate map.To solve this, Hahnel et al [15] combine Rao-Blackwellized particle filtering and scan matching with an improved motion model that reduces the number of required particles.Grissetti et al. [21] present an adaptive technique to reduce the number of particles in a Rao-Blackwellized particle filter for learning grid maps, they propose an approach to compute an accurate proposal distribution taking into account not only the movement of the robot but also the most recent observation.This drastic decrease the uncertainty about the robot's pose in the prediction step of the filter.
This paper presents a comparison of different grid-based SLAM algorithms over some application examples.A technique for merging maps from several robots is also described.The objective is to build up a highly accurate map of an unknown environment with unknown initial

SLAM and Map Merging
A. León, R. Barea, L.M. Bergasa, E. López, M. Ocaña and D. Schleicher L positionofrobotsusingRao-Blackwellized particle filtering and scan matching.Finally, some experimental results of our proposal and the main conclusions will be drawn and commented.

ALGORITMS
This section makes a comparison of different SLAM algorithms.To do that, firstly, we will study the basic of the SLAM techniques then, we will compare them over some application examples and finally we will present some conclusions about its performance.

A. Scan-Matching SLAM
The scan matching algorithm that we have implemented is an extension of the approach presented in [16], where the problem of concurrent mapping and localization can be treated as a maximum likelihood estimation problem, in which the aim is to find the most likely map given the data: Let m be a map.At time t, the map is written: where o T denotes a laser scan and x T its pose, and t is time index.
The goal of mapping is to find the most likely map given the data, that is: ( where data d t is a sequence of laser range measurements and odometry readings: ( where o denotes an observation (laser range scan), a denotes an odometry reading, and t are time indexes.
The map likelihood function P(m | d t ) can be transformed into the following product [18]: (4) where η is a normalizer and P(m) is prior over maps which, if assumed to be uniform, can safely be omitted.Thus, the map likelihood is a function of two terms, the motion model, P(x t+1 | a t , x t ), and the perceptual model, P(o t | m t , x t ).If stationarity is assumed (i.e., neither model depends on the time index t), the time index can be omitted and instead write P(x | a, x'), for the motion model and P(o | m, x ) for the perceptual model.

B. Grid-based Fast-SLAM
This algorithm adapts the Fast-SLAM algorithm to occupancy grid maps.This gives us a volumetric representation of the environment that does not require any predefined landmark and it can therefore model arbitrary types of environments.The pseudo-code for grid-based Fast-SLAM [22] in each iteration is the following:

C. Rao-Blackwellized mapping
The key idea of the Rao-Blackwellized particle filter [21] for SLAM is to estimate a posterior p(x 1:t | o 1:t , a 0:t ) about potential trajectories x 1:t of the robot given its observations o 1:t and its odometry measurements a 0:t and to use this afterwards to compute a posterior over maps and trajectories: (5) This can be done efficiently, since the posterior over maps can be computed analytically given the knowledge of x 1:t and o 1:t .
To estimate the posterior over the potential trajectories Rao-Blackwellized mapping uses a particle filter in which an individual map is associated to every sample.Each map is built given the observations o 1:t and the trajectory a 1:t represented by the corresponding particle.The trajectory of the robot evolves according to the robot motion and for this reason the proposal distribution is chosen to be equivalent to the probabilistic odometry motion model.One of the most common particle filtering algorithms is the Sampling Importance Resampling (SIR) filter.A Rao-Blackwellized SIR filter for mapping incrementally processes the observations and the odometry readings as they are available.This is done by updating a set of samples representing the posterior about the map and the trajectory of the vehicle.This is done by performing the following four steps: • Sampling: The next generation of particles is obtained from the current generation by sampling from a proposal distribution .
• Importance Weighting: An individual importance weight w i is assigned to each particle, according to (6) The weights w i account for the fact that the proposal distribution Β in general is not equal to the true distribution of successor states.
• Resampling: Particles with a low importance weight w are typically replaced by samples with a high weight.This step is necessary since only a finite number of particles are used to approximate a continuous distribution.Furthermore, resampling allows to apply a particle filter in situations in which the true distribution differs from the proposal.
• Map Estimating: for each pose sample , the corresponding map estimate is computed based on the trajectory and the history of observations according to .

D. Performance comparison
Firstly, we are going to compare the different SLAM algorithms using a manufactured rectangular map (figure 1).The objective of this initial test is to study the performance of these algorithms in environments with large corridors without points of interest (points where the symmetry of other types of information is conducive to accurate location).Figures 2 and 3 show the map obtained after one and three map laps, respectively, using the scan-matching SLAM algorithm.Figures 6 and 7 show the map obtained after one and three map laps, respectively, using the Rao-Blackwellized particle filter for SLAM.All cases under study show that errors decrease and results improve with the number of laps.This stands to reason, since the system progressively solves the mapped errors detected in the first lap.
Figure 8 shows the three maps obtained, superimposed over the original map.The red unbroken line shows the original map, the green dot-and-dash line (-•-) traces out the map obtained using scan-matching, the orange dashed line (--) shows the map generated using grid-based Fast-SLAM while the blue dotted line (•••) shows the map generated using Rao-Blackwellized mapping.Table I shows the dimensional errors detected using the three algorithms.After three laps these errors decrease very slowly and then improvements are very low.These results show that the detected errors are small and negligible in small-dimension environments.The best results would seem to be obtained by the grid-based Fast-SLAM algorithm, but the map geometry in fact shows that the generated map in this case is the worst of the three.
One of the problems that appear in environments of this type (especially for the case of scan-matching) is that if there is not outstanding information in the corridors (points of interest) the results record the overall corridor length smaller than the real one.This is because the maximum likelihood estimation method is used, in which the aim is to find the most likely map given the data.This occurs especially when we work in real environments and is due mainly to the environment-detection limitations of the sensor used.In our case this sensor is a laser SICK-LMS200 and in the corridors when the end is not detected, there are many points where the laser measurement is the same or very similar.The maximum likelihood estimation algorithm therefore generates significant estimation errors.
The next objective is to study a more complex environment.Figure 9 shows a manufactured map from the Electronics Department of University of Alcala (corridor 3 and 4). Figure 10 shows the map obtained using odometry and laser data without any correction.The starts in Lab_1, goes to Lab_2 and returns to Lab_1.Another experiment to determine the errors introduced by algorithms involves mapping a larger real environment as is the whole third floor of the Polytechnic building, as can be seen in Fig. 14.
The aim in this case is to compare results using scanmatching and grid-based Rao-Blackwellized SLAM.To do that a lap around the building will be carried out and the map generation errors will be studied.Figures 15 and 16 show the results obtained from one complete lap of the building.The start and the end point is a research laboratory in office 34.Robot trajectory has been superimposed on the real map to compare map-building errors.Both cases show that significant position estimation errors occur after one lap of the building.Figure 15 (scan-matching) shows a position error of about 2 meters, while Figure 16 (Rao-Blackwellized SLAM) gives a 6 meter error.It should be bore in mind here that more than 400 meters have been traveled without any loop closing or point of interest that might allow correction of the existing map.Simulations suggest that results improve after several laps around the building.The results obtained can therefore be considered satisfactory.

A. Robots
Four robotic platforms have been developed based on PeopleBot, pioneer DX and pioneer AT robots of ActivMedia Robotics [23] (see figure 17).Their architecture comprises four large modules: environment perception, navigation, human-machine interface and high-level services.The perception module is equipped with encoders, bumpers, sonar ring, laser sensor and a vision system based on a PTZ (pantilt-zoom) color camera connected to a frame grabber.The human-machine interface is composed of loudspeakers, microphone, a tactile screen, the same PTZ camera used in the perception module, and a wireless Ethernet link.The highlevel services block controls the rest of the modules and includes several tasks of tele-assistance, tele-monitoring, providing, reminding and social interaction [24].

B. Navigation module
The navigation module combines information from the perception module for carrying out different tasks.The core of this module is CARMEN (Carnegie Mellon Robot Navigation Toolkit) [25] which is an open-source collection of software for mobile robot control.CARMEN is modular software designed to provide basic navigation primitives including: base and sensor control, obstacle avoidance, localization, path planning, people-tracking, and mapping.
This source has been modified to implement multi-robot localization, because CARMEN permits only single-robot working; there is also a different initial distribution for studying the robot localization (based on Montecarlo localization).This source implements the motion, perception and detection models.A virtual simulator has also been developed for testing the detection model using visual information and the localization process.

C. Detection model
Each robot has the ability to sense each others.The detection model describes the probability that robot n is at location x, given that robot m is at location x´ and perceives robot n with measurement r m .
To determine the relative location of other robots, our approach combines visual information obtained from an onboard camera with proximity information coming from a laser range-finder.Camera images are used to detect other robots and, together with laser range-finder scans, determine the relative position of the detected robot and its distance.
The robots are marked by a unique and colored marker to facilitate their recognition (green cylinder).The marker can therefore be detected regardless of the robot's orientation.
To find robots in the images obtained from a camera, our system first filters the image by employing local color histograms (HIS space color).A basic segmentation algorithm, based on the histogram, is then employed to search for the marker's color.If it is found, this implies that a robot is present in the image.
Once a robot has been detected, its size and position in the image are processed and a laser scan is made for calculating the relative position of one robot respecting the other (see figures 18 to 20).This multi-sensor technique has been proven to be robust in practical tests.Currently, images are analyzed approximately at 20 fps.This framerate is enough for this application.
When a robot detects the other one, a detection model is generated (based on a Gaussian function) representing the probability that the detected robot is at this point.This detection model carries out the adjustment of the particles by means of Collaborative Monte Carlo localization.

IV. MULTI-ROBOT MONTE CARLO LOCALIZATION
Monte Carlo Localization has been widely studied in [6,14].MCL is a recursive Bayes filter that estimates the posterior distribution of robot poses conditioned on sensor data.
The key idea of Bayes filtering is to estimate a probability density over the state space x conditioned on the data.This posterior is typically called the belief and is denoted: Where x t is the vector state at time t, o denotes observations (perceptual data such as laser range or vision measurements) and a represents actions (odometry data which carry information about robot motion).
Bayes filters estimate the belief recursively.The initial belief characterizes the initial knowledge about the system state.In the absence of such knowledge, it is typically initialized by a uniform distribution over the state space.In mobile robot localization, a uniform initial distribution corresponds to the global localization problem, where the initial robot pose is unknown.
To derive a recursive update equation, (7) can be transformed by Bayes rule to: (8) (9) Bayes filters assume that the environment is Markov, that is, past and future data are (conditionally) independent if one knows the current state.The Markov assumption implies: (10) (11) Therefore, the belief can be denoted by: (12) Where p(o t |x t ) is called perceptual model and p(x t |x t-1 ,a t-1 ) represents the motion model.
The key idea of multi-robot localization is to integrate measurements taken at different platforms, so that each robot can benefit from data gathered by other robots than itself.Therefore, when a robot n is detected by robot m it is necessary to introduce the detection model according with data obtained r m in (12).In the absence of detections, the Markov localization works independently for each robot.A summary of the multi-robot Markov localization algorithm is: • Initialize the belief Bel n (x) according with initial data (typically uniform distribution).• If the robot n receives an observation on (new sensory input) o n , it is applied the perception model:: • If the robot n do some action a n (receives a new odometry reading), It is applied the motion model: • And finally, if the robot n is detected by the m-th robot it is applied the detection model: (15) The idea of MCL is to represent the belief by a set of m weighted samples distributed according to Bel(x): (16) Where x i is a sample of the random variable x (pose) and w i is called importance factor and represents the importance of each sample.The set of samples, thus, define a discrete probability function that approximates the continuous belief Bel(x).
The initial set of samples represents the initial knowledge Bel(x o ) about the state of the dynamical system.For instance, in global mobile robot localization, the initial belief is a set of poses drawn according to a uniform distribution over the robot's universe, annotated by the uniform importance factor 1/m.If the initial pose is known up to some small margin of error, Bel(x o ) may be initialized by samples drawn from a narrow Gaussian centered on the correct pose.
The recursive update is carried out in three steps: • Sample x i t-1 − Bel(x t-1 ).Each such particle x i t-1 is distributed according to the belief distribution Bel(x t-1 ).
• Sample x i t-1 − p(x t |x i t-1 ,a t-1 ).In this case, x i t is distributed according to the product distribution p(x t |x t-1 ,a t-

)•Bel(x t-).
• The importance factor is assigned to the i-th sample: The following example shows how collaborative multirobot Monte Carlo localization improves single localization [15].Robot 1 starts out with uniform belief and Robot 2 with Gaussian belief.If Robot 1 wanders across the top horizontal corridor (Fig. 22a), when Robot 2 detects Robot 1 (Fig. 22b), the detection model is sent to Robot 1 and this updates its belief distribution.Robot 1 is therefore well-located before reaching the corridor (Fig. 22c).V. MULTIROBOT MAP MERGING   This section describes how to build a map from data obtained by multiple robots.We are currently working with scan-match [16], grid-based Fast-SLAM [15] and grid-based SLAM with Rao-Blackwellized particle filter [21] for map merging; comparing the results.To do so, we have modified some codes and parameters in algorithms (GMapping and GridSLAM) obtained from OpenSlam.org[26] which provides SLAM researchers with a platform for publishing their algorithms.
In this initial work we are focused on developing multirobot map merging using a scan-match technique.Next, a merging map example is commented on, working with a scan-match technique (see figures 23 to 27).The goal is mapping corridors 3 and 4 of the Electronics Department of the Polytechnic School.Figure 23 shows the trajectories followed for each robot and figure 24 shows the global map using CARMEN (Montecarlo localization).Robot 1 explores across corridor 4 and Robot 2 explores corridor 3; each robot builds its partial map and calculates its pose at any time.When Robot 1 meets Robot 2, Robot 2 send its map to Robot 1. Robot 1 uses the partial Robot 2 map and the detection model (Robot 2´s pose detected) to generate the global map (Fig. 27).
The main problem we run into here is the need for very precise inter-robot detection for map merging purposes.Any small orientation error would mean that the map built up from the slave robot data would be displaced vis-à-vis the master robot.We are currently working to improve inter-robot detection, making it much more precise in the interests of correct global map reconstruction.
Our final goal is to be able to map exteriors and implement other SLAM techniques for reconstructing the most trustworthy environment possible.One of the problems we are finding is that 2D laser information outdoors may be of little value in wider settings and we are therefore trying to merge laser with vision.We are thus working towards the introduction of visual information in localization and mapping process to improve SLAM's performance outdoors.We present herein initial work in mobile robot mapping.We show and comment on results using several algorithms for SLAM (scan-matching, grid-FastSLAM and grid-based SLAM with Rao-Blackwellized particle filter) for a single robot and multi-robot map merging using a scan-match technique.The results show that it is possible to use a team of robots to explore and navigate in unknown environments.
Our future work will be focused on improve performance of our proposal for indoor environments and to generalize it for outdoor environments.According to this point, the results obtained show that it is necessary to use other sources of information (3D laser, vision, etc) to obtain similar results to those obtained indoors.

Figures 4
Figures 4 and 5 show the map obtained after one and three map laps, respectively, using the grid-based Fast-SLAM algorithm.

Fig. 10 .
Fig. 10.Map obtained using odometry and laser data without correction.

Figures 11 ,
Figures 11, 12 and 13  show the map obtained using scanmatching SLAM, grid-Fast-SLAM and grid-based Rao-Blackwellized SLAM, respectively.All algorithms solve existing errors using odometry and laser data, but a certain error exists in the orientation of the corridors.

Fig. 15 .
Fig. 15.Scan-matching SLAM map.Comparison of real and obtained map.