A Fast Map Merging Algorithm in the Field of Multirobot SLAM

Abstract

In recent years, the research on single-robot simultaneous localization and mapping (SLAM) has made a great success. However, multirobot SLAM faces many challenging problems, including unknown robot poses, unshared map, and unstable communication. In this paper, a map merging algorithm based on virtual robot motion is proposed for multi-robot SLAM. The thinning algorithm is used to construct the skeleton of the grid map’s empty area, and a mobile robot is simulated in one map. The simulated data is used as information sources in the other map to do partial map Monte Carlo localization; if localization succeeds, the relative pose hypotheses between the two maps can be computed easily. We verify these hypotheses using the rendezvous technique and use them as initial values to optimize the estimation by a heuristic random search algorithm.

1. Introduction

Recent advances in mobile robotics have allowed autonomous systems to be involved in many successful applications including planetary exploration, search and rescue, surveillance, and other service scenarios [1].

For the purpose of successfully accomplishing a generic task, a main prerequisite for a mobile robot, deployed in an unknown area, is the ability to autonomously navigate, exploiting the information acquired through the joint estimation of its positions and a model of the surrounding environment. The problem of estimating both the robot pose and the environment representation is usually defined as simultaneous localization and mapping (SLAM) [2, 3]. While the maturity of SLAM in single robot scenarios is recognized in many recent works, a challenging issue is to extend these approaches to multirobot scenarios [4–7].

Recently, in order to cope with dynamic environment and multiple tasks, a lot of researches have been presented in multiagent and multiple distributed autonomous robotic systems. Each robot has its own task, such as building a map of local position. Moreover, they have to combine their data into shared maps. Using shared maps, robots coordinate their exploration strategies to maximize the efficiency of exploration. Multiple autonomous mobile robots can complete the task through cooperation and give a more accurate map by data fusion. The robots coordinate their exploration strategies to maximize the efficiency of their exploration using these shared maps.

Fenwick et al. extended single robot SLAM algorithm based on EKF to the multirobot [8–10]. Howard extended single-robot SLAM algorithm based on particle filter to multi-robot applications [11]. Thrun put forward multi-robot hybrid map building method that combines fast maximum likelihood map growing with a Monte Carlo localizer [12]. Thrun [10] points out that the key to multi-robot map building is to determine the initial position and orientation of each single robot. However, most current algorithms bypass the problem, either some algorithms assume that the road signs in the environment are identified, or some algorithms assume that each robot starting from a similar position, so that their initial map has large coverage area. Ko et al. [13] specialize in the methods by which the robots determine relative pose in environmental exploration. Each robot builds its own maps, and meanwhile, estimates the pose of other robots in its own map. If the estimation probability reaches the higher accuracy, two robots rendezvous [14] to confirm the pose between them, and this paper also adopts a similar approach. Another strategy is to assume that the robots can be cross-observed to determine the relative pose between them [13, 15]. The deficiency of the method is that each robot in the process of movement must be “accidentally” encounter.

In this paper we present a novel fast and accurate algorithm for merging multiple maps represented as occupancy grids. The main idea is as follows: make a virtual mobile robot in one partial map and control its move and measure in the map. At the same time, these simulated data are used as information sources in the other map to do partial map Monte Carlo localization [16]; if localization succeeds, the relative pose hypotheses between the two maps can be computed easily. In order to speed up the process, we can constrain the virtual robot move along the local topological edge of the partial map, so in the process of Monte Carlo localization within the partial map, the initial random samples only are generated near the edges of the corresponding topology of the other partial map. Prior knowledge of the relative pose between maps or robots is not demanded in the method, which enhances autonomous exploration and multi-robot SLAM.

2. Concept of Map Merging

The research on map merging is limited, and the previous work focused on feature-based approaches [17–19], which rely on special landmarks that can be recognized through properly processing the data gathered by the robots. For example, the topological maps are graphs in which vertices represent recognizable places such as doorways, corridors, and different types of corner and edges represent the passage connecting two places. Merging map A and map B turns into searching subgraphs with the same structure in map A and map B. Our goal is to merge maps which are not based on features, but rather on occupancy grids. For sake of clearness, we formally define the several concepts [20].

Definition 1. Let 𝑁 and 𝑀 be two positive real numbers. A 𝑁×𝑀 map is a function. Consider 𝑚∶[1,𝑁]×[1,𝑀]⟶𝐑, (1) where 𝐼_𝑁×𝑀 denoting the set of 𝑁×𝑀 maps.

Discrimination is needed when a map is processed in the practical application, and the map is represented as a matrix with 𝑁 rows and 𝑀 columns. The function 𝑚 is a confidence model in the map. For example, one could assume that if the value of 𝑚(𝑥,𝑦) is positive, the point (𝑥,𝑦) in the map is free, while a negative value represents the point (𝑥,𝑦) as occupied. The absolute value represents the degree of belief. Especially, we assume that if the value of 𝑚(𝑥,𝑦) is zero, the point (𝑥,𝑦) is unknown. The definition is consistent with the grid map. In the following, the planar transformation is defined, which will be used to find the best matching between two partial maps.

Definition 2. 𝑡_𝑥, 𝑡_𝑦, and 𝑡_𝜃 are three real numbers; the pose transformation function consists of 𝑡_𝑥, 𝑡_𝑦, and 𝑡_𝜃 is defined as follows: 𝑇_{(𝑡𝑥,𝑡𝑦,𝑡𝜃)}𝑥,𝑦∶𝐑²⟶𝐑², (2) where 𝑡_𝑥, 𝑡_𝑦, and 𝑡_𝜃 are three real numbers. It is defined as follows: 𝑇_{(𝑡𝑥,𝑡𝑦,𝜃)}𝑥,𝑦=100010cos𝜃−sin𝜃𝑡_𝑥sin𝜃cos𝜃𝑡_𝑦𝑥𝑦0011. (3)

Definition 3. 𝑚₁ and 𝑚₂ are the two maps of 𝐼_𝑁×𝑀; the coverage between 𝑚₁ and 𝑚₁ is defined as follows: 𝜔𝑚₁,𝑚₂=^𝑁𝑀_𝑖=1𝑗=1𝑒𝑚₁𝑖,𝑗,𝑚₂𝑖,𝑗, (4) where 𝑒(𝑎,𝑏)=1,if𝑎=𝑏,0,otherwise. (5)

The coverage function is used to measure the degree of the match between two maps. Ideally, the map, which the two robots build, is entirely consistent with the physical environment and covers the entire environment space. In this case, the coverage function is 𝜔(𝑚₁,𝑚₂)=𝑁×𝑀; it is maximum.

According to the above definitions, we can formally make definitions on map merging.

Definition 4. Given two maps 𝑚₁ and 𝑚₂, pose transformation function 𝑇_{(𝑡𝑥,𝑡𝑦,𝑡𝜃)}(𝑥,𝑦) is searched to maximize the function 𝜔(𝑚₁,𝑇_{(𝑡𝑥,𝑡𝑦,𝑡𝜃)}𝑚₂).

From the previous definition, the key to map merging is to search the optimal pose transformation 𝑇_{(𝑡𝑥,𝑡𝑦,𝑡𝜃)}. The preliminary estimations of the transformation are got by the Monte Carlo localization in the partial map [16]. In order to improve computational efficiency, we adopt the sampling method with confined area. The heuristic random search algorithm is used to further optimize the pose transformation.

3. Fast Monte Carlo Global Localization

3.1. Topological Information of a Map

The topological information of a map is the abstraction of environmental information. Vertices represent discrete places in the environment, and edges represent paths connecting these places. The information can be obtained by generalized Voronoi graph (GVG) algorithm [21] or thinning algorithm [22, 23]. Useless boundaries and virtual intersections for navigation are generated in GVG algorithm, the thinning method does not generate this information because it is based on probabilistic framework. It is robust for the sensor noise and various environments.

Thinning algorithm is the method that extracts the image skeleton in the digital image processing algorithm. Figure 1 illustrates the concept of thinning. The object on the left figure can be described well by the structure composed of connected lines (i.e., “T” shape design with thin lines on the right figure). Note that connectivity of the structure is still preserved even with thin lines. For mobile robots, the connecting lines can be used as collisionless paths of the robot navigation.

Figure 1 

Concept of thinning.

In the paper, eight neighbour grid operators are used to solve the skeleton of free areas in grid map. Centre grid 𝑝₁ and 8 neighbor grids (𝑝₂∼𝑝₉) are described in Figure 2. “0” indicates that the grid is empty and “1” indicates the grid is occupied. For the occupied grid 𝑝₁, if its eight neighbour grids satisfy the thinning condition, grid 𝑝₁ is change into empty because it is not a part of the skeleton.

Figure 2 

Centre grid 𝑝1 and its 8 neighbor grids.

Thinning Condition 1. Consider(1)2≤𝑁(𝑝₁)≤6, (2)𝑆(𝑝₁)=1, (3)𝑝₂⋅𝑝₄⋅𝑝₆=0, (4)𝑝₄⋅𝑝₆⋅𝑝₈=0.

Thinning Condition 2 (1)  and (2) are the same as Thinning Condition 1,(3) 𝑝₂⋅𝑝₄⋅𝑝₈=0,(4) 𝑝₂⋅𝑝₆⋅𝑝₈=0.Here, 𝑝_𝑖=0,grid𝑝_𝑖isempty,1,grid𝑝_𝑖isoccupied. (6)𝑁(𝑝₁) denotes the number of occupied grids of 8 neighbours, and it is defined as follows: 𝑁𝑝₁=⁹_𝑖=2𝑝_𝑖. (7)𝑆(𝑝_𝑖) denotes times of the change from 0 to 1 in the sequence 𝑝₂,𝑝₃,…,𝑝₈,𝑝₉.

3.2. The Sample Space Limitation

The map skeleton is constructed for designating confined sample area and navigation path. If robot moves along bisector of the obstacles (approximately along the skeleton), it has more opportunities to collect more environment information.

The performance of MCL is heavily dependent on density of particles if the other conditions are the same. The sample space is usually very large in the stages of MCL global localization and processing the kidnapped robot problem (the worst case is all free areas in the environmental map). A lot of particles are required in order to localize successfully, and the request of real time is not met. However, if we could acquire the relevant knowledge about pose of robot in advance, for example, we are sure that robot is located in the area that is much smaller than free area in the map, we can only sample in this area, which improves the speed of the algorithm.

4. Relative Pose Estimation of Maps

4.1. Hypothesis Generation

The simplest situation is considered: two complete maps 𝑚₁ and 𝑚₂ (their coordinates are different in global coordinates) in the same closed environment Ω save two parts of information: grid occupancy information and skeleton of corresponding free area. If a robot constantly moves along the skeleton of free area in environment Ω and observes the environment at the same time, MCL global localization is used to localize the robot in map 𝑚₁ and map 𝑚₂. Suppose the location estimations of robot in two maps are 𝜉_𝑡⁽¹⁾ and 𝜉_𝑡⁽²⁾, respectively, at time 𝑡, as shown in Figure 3, the following formula can be concluded: 𝑇₁=𝑇₂₁⋅𝑇₂, (8) where 𝑇₁ and 𝑇₂ are corresponding homogeneous transformation matrixes to 𝜉_𝑡⁽¹⁾ and 𝜉_𝑡⁽²⁾, respectively. 𝑇₂₁ is homogeneous transformation matrix that the coordinates of map 𝑚₂ is relative to one of map 𝑚₁.

Figure 3 

Map merging coordinate diagram.

According to (4), the relative pose between 𝑚₁ and 𝑚₂ is as follows: 𝑇₂₁=𝑇₁⋅𝑇₂⁻¹. (9)

The specific approach is put forward in the paper. A robot is simulated to move along the skeleton of the partial map 𝑚₁ and constantly observe the map, which is obtained by a robot using the single-robot-SLAM algorithm. At the same time, these simulated data are used as information sources to do Monte Carlo localization for partial map in the map 𝑚₂. The localization process is carried out until the particles converge to one or more clusters. Because the simulation data is obtained near the map skeleton, the initial particles only need to be generated near the skeleton of 𝑚₂ in the process of MCL in the map 𝑚₂. In this way the initial pose estimation between 𝑚₁ and 𝑚₂ is got quickly. Figure 4 shows the model of the method.

Figure 4 

Calculation of relative pose of maps based on virtual robot motion.

4.2. Hypothesis Verification

From the previous method, we get the preliminary estimation to the relative pose between two partial maps 𝑚₁ and 𝑚₂ which were constructed by Robot1 and Robot2, respectively. The estimation may have one or more peak values, which are supposed to be as relative pose hypotheses between 𝑚₁ and 𝑚₂. Suppose that homogeneous transformation matrix 𝑇 is one of these hypotheses, as show in Figure 5, the homogeneous transformation matrix 𝑇₃ of the two robots’ relative pose can be calculated with 𝑇, 𝑇₁, and 𝑇₂ as follows: 𝑇₃=𝑇₁⁻¹⋅𝑇⋅𝑇₂. (10)

Figure 5 

Relative pose of robots.

We can use 𝑇₃ to path planning two robots’ paths in the map 𝑚₁ and make them meet as far as possible. The plan is operated on Robot1, and the results are sent to Robot2 in order to collaborate with each other. If the two robots can meet according to the path planning, the relative pose hypothesis 𝑇 is true, and we go to the stage of estimation optimization; otherwise we test the next hypothesis. If all hypotheses are false, the robot still constructs map independently (single-robot SLAM), waiting for the next operation of map merging.

5. Estimation Optimization

After the previous steps, the preliminary estimation to the relative pose of map is got. Based on the research of literature [24], the relative pose is further optimized. The concrete optimization process is as follows.

5.1. Dissimilarity Measurement Function of Map

From the definition of map merging, the map merging problem can be seen as an optimization problem, whose optimization function is 𝜔(). It is the problem that 𝜔() is directly used as optimization function. Because the values of 𝜔(𝑚₁,𝑇_{(𝑡𝑥,𝑡𝑦,𝑡𝜃)}(𝑚₂)) are arbitrarily leap with continuous changes of variables (𝑡_𝑥,𝑡_𝑦,𝑡_𝜃), the function 𝜔() delivers no effective gradients to do optimization like hill-climbing algorithm. The dissimilarity function is as follows: 𝜓𝑚₁,𝑚₂=_𝑐∈𝐶𝑑𝑚₁,𝑚₂,𝑐+𝑑𝑚₁,𝑚₂,𝑐 (11) with 𝑑𝑚₁,𝑚₂=∑,𝑐^{𝑚1[𝑝1]=𝑐}min𝑚𝑑𝑝₁,𝑝₂∣𝑚₂𝑝₂=𝑐#_𝑐𝑚₁, (12) where 𝐶 denotes grid range of map 𝑚₁ and map 𝑚₂, 𝑚₁[𝑝₁] denotes the value of grid 𝑝₁ in map 𝑚₁, 𝑚𝑑(𝑝₁,𝑝₂)=|𝑥₁−𝑥₂|+|𝑦₁−𝑦₂| denotes the Manhattan-distance between points 𝑝₁ and 𝑝₂, and #_𝑐(𝑚₁)=#{𝑝₁|𝑚₁[𝑝₁]=𝑐} denotes the number of grids with value 𝑐 in map 𝑚₁.

To simplify the calculation, the grids’ value of the map is marked as “free” or “occupied” or “unknown” according to the predefined threshold. Only occupied and free grids are considered for computing dissimilarity function, so 𝐶={occ,free}. In order to compute the dissimilarity function in linear time, a so called distance-map is introduced. Distance-map 𝑑𝑚𝑎𝑝_𝑐[𝑥₁][𝑦₁] denotes the Manhattan distance between the grid 𝑝₁=(𝑥₁,𝑦₁) in map 𝑚₁ and a grid which is the nearest point to 𝑝₁ with value 𝑐 in map 𝑚₂: 𝑑𝑚𝑎𝑝_𝑐𝑥₁𝑦₁=min𝑚𝑑𝑝₁,𝑝₂𝑚₂𝑝₂=𝑐. (13)

The concrete calculation process for 𝑑𝑚𝑎𝑝_c is shown in Algorithm 1.

Input:  the grid map 𝑚 and grid value 𝑐 Output:  distance-map 𝑑𝑚𝑎𝑝𝑐 1  for 𝑦=0  to  𝑛+1 do 2    for 𝑥=0  to  𝑛+1 do 3      if 𝑚(𝑥,𝑦)==𝑐  then 4     𝑑𝑚𝑎𝑝𝑐[𝑥][𝑦]=0; 5      else 6     𝑑𝑚𝑎𝑝𝑐[𝑥][𝑦]=∞; 7      end 8    end 9   end 10   for  𝑦=1  to  𝑛 do 11    for  𝑥=1  to  𝑛 do 12      ℎ=min(𝑑𝑚𝑎𝑝𝑐[𝑛−𝑥+1][𝑛−𝑦]+1,𝑑𝑚𝑎𝑝𝑐[𝑛−𝑥][𝑛−𝑦+1]+1); 13       𝑑𝑚𝑎𝑝𝑐[𝑛−𝑥][𝑛−𝑦]=min(𝑑𝑚𝑎𝑝𝑐[𝑛−𝑥][𝑛−𝑦],ℎ); 14    end 15   end 16   for 𝑦=1  to  𝑛 do 17    for 𝑥=1  to  𝑛 do 18       ℎ=min(𝑑𝑚𝑎𝑝𝑐[𝑥−1][𝑦]+1,𝑑𝑚𝑎𝑝𝑐[𝑥][𝑦−1]+1); 19       𝑑𝑚𝑎𝑝𝑐[𝑥][𝑦]=min(𝑑𝑚𝑎𝑝𝑐[𝑥][𝑦],ℎ); 20    end 21   end 22  return 𝑑𝑚𝑎𝑝𝑐;

Algorithm 1 

Calculation process of distance-map 𝑑𝑚𝑎𝑝_𝑐 in grid map.

Using the distance-map 𝑑𝑚𝑎𝑝_𝑐, we can calculate 𝑑(𝑚₁,𝑚₂,𝑐) with Algorithm 2.

Input:  the grid map 𝑚1, 𝑚2 and grid value 𝑐 Output:  𝑑(𝑚1,𝑚2,𝑐) 1 calculate the 𝑑𝑚𝑎𝑝𝑐 of 𝑚2 according to Algorithm 1; 2 𝑑(𝑚1,𝑚2,𝑐)=0; 3  for  𝑦=0  to  𝑛+1  do 4    for  𝑥=0  to  𝑛+1  do 5      if  𝑚1(𝑥,𝑦)==𝑐  then 6     𝑑(𝑚1,𝑚2,𝑐)=𝑑(𝑚1,𝑚2,𝑐)+𝑑𝑚𝑎𝑝𝑐[𝑥][𝑦]; 7      end 8    end 9  end 10 return  𝑑(𝑚1,𝑚2,𝑐);

Algorithm 2 

Calculation process of 𝑑(𝑚₁,𝑚₂,𝑐).

5.2. Random Walk Optimization

The basic idea of random walk optimization is to search in the given solution space using the way of random walk. At each step a random solution is generated and the corresponding heuristic rule of next step is computed. The concrete process is shown in Algorithm 3. And a more detailed description is in the literature [24].

Input: the grid map 𝑚1, 𝑚2, initial relative pose estimation 𝑡start Output: the relative pose 𝑡 between map 𝑚1 and 𝑚2 1  𝑘=0;  𝑡𝑘=𝑡start; 2  Σ0=Σinit;  𝜇0=𝜇init; 3  𝑐0=𝜓(𝑚1,𝑇𝑡start(𝑚2)); 4 while 𝑘<𝑛𝑢𝑚𝑠𝑡𝑒𝑝𝑠  do 5   𝜈𝑘∼𝑁(𝑢𝑘,Σ𝑘); 6   𝑠=𝑡𝑘+𝜈𝑘; 7   𝑐𝑠=𝜓(𝑚1,𝑇𝑠(𝑚2)); 8   if  𝑐𝑠<𝑐𝑘  or  RS(𝑡𝑘,𝑠)=𝑠  then 9    𝑘=𝑘+1; 𝑡𝑘=𝑠; 𝑐𝑘=𝑐𝑠; 10   Σ𝑘+1=(𝑡𝑘,𝑡𝑘−1,…,𝑡𝑘−𝑀); 11   𝜇𝑘+1=(𝑡𝑘,𝑡𝑘−1,…,𝑡𝑘−𝑀); 12   end 13 end 14 return  𝑠;

Algorithm 3 

Random walk optimization.

6. Experimental Results

We perform experiment based on the map merging algorithm proposed in this paper. The process of experiment is as follows. Firstly, grid map of the identical simulation environment (Figure 6(a)) is constructed by using SLAM algorithm twice, as shown in Figures 6(b) and 6(c). Then, the relative pose between two partial maps is calculated using the method proposed in the paper. Finally, the results of map merging are illustrated in Figures 6(d) and 6(e). Figure 6(d) is the result based on calculation of relative pose of maps using virtual robot motion, and its optimized result using random walk optimization is showed in Figure 6(e).

(a) Simulation environment

(b) Partial map m 1

(c) Partial map m 2

(d) Map merging based on preliminary estimation

(e) Map merging based on optimized estimation

Figure 6 

Experimental results.

7. Conclusions

In the paper, map merging method based on virtual robot motion is proposed in the field of multirobot SLAM. For multi-robot SLAM, there are four kinds of interaction effect between two robots. The first kind is no interaction between two robots. The second kind is hypothesis generation because communication is permitted between robots, but relative pose of other robot is unknown. The third kind is hypothesis verification because communication is permitted between robots, and relative localization hypothesis is generated in the process of hypothesis generation. The forth kind is coordinated exploration because robots have relative pose and can share map and explore environment.

In this paper, a mobile robot is simulated in one map; it moves along the map’s skeleton and measures the virtual environment. At the same time, these simulated data are used as information sources in the other map to do partial map Monte Carlo localization; if localization succeeds, the relative pose hypotheses between the two maps can be computed easily. Then, they actively verify one hypothesis using a rendezvous technique. If successful, using the hypothesis as initial value, the estimation is optimized by a heuristic random search algorithm. The algorithm is not only for grid maps but also other types of map. The experimental results have verified the algorithm.

In the future, the corresponding problems, such as network transmission and collaboration of robots, are required to be considered. Cloud robotics is considered to be the next great-leap-forward development of robotics. The method will be improved to apply to cloud robotics.

Acknowledgments

This paper was supported by the National Nature Science Foundation of China (no. 61165007), the Nature Science Foundation of Jiangxi Province (no. 20132BAB211036), and the Research Foundation of Education Bureau of Jiangxi Province (no. GJJ12290).