Abstract
Considering the influence of uncertain map information on multirobot SLAM problem, a multirobot FastSLAM algorithm based on landmark consistency correction is proposed. Firstly, electromagnetism-like mechanism is introduced to the resampling procedure in single-robot FastSLAM, where we assume that each sampling particle is looked at as a charged electron and attraction-repulsion mechanism in electromagnetism field is used to simulate interactive force between the particles to improve the distribution of particles. Secondly, when multiple robots observe the same landmarks, every robot is regarded as one node and Kalman-Consensus Filter is proposed to update landmark information, which further improves the accuracy of localization and mapping. Finally, the simulation results show that the algorithm is suitable and effective.
1. Introduction
Simultaneous localization and map building (SLAM) problem considers the situation in which an autonomous mobile robot moves through an unknown environment and incrementally builds a map of this space while simultaneously making use of this map to refresh its absolute location [1–3]; SLAM algorithm is deemed to be the key factor of the real sense of the mobile robot in autonomous navigation. Compared with single-robot SLAM, multirobot SLAM algorithm can not only get more accurate and faster environmental information but also have better self-positioning ability, which is mainly reflected in two aspects: the multirobot can cover the entire environment faster via communication and coordination in parallel; explore [4]; the localization and mapping errors can be reduced through integrating measurement and navigation information of multiple robots [5]. In recent years, research on multirobot SLAM has drawn more and more attention, and it gradually becomes a hot issue in robot field [6, 7].
At present, the existing approaches for multirobot SLAM mainly focus on expanding the single-robot SLAM algorithm to the multirobot case. Huang et al. [8] study the consistent fusion of multirobot relative measurements based on Extended Kalman Filtering (EKF); they employ the method in which multirobots jointly maintain the state of global map and its covariance matrix and use consistency protocol to fuse multirobot measurements information for the global update. However, system linearization error and high complexity are the biggest drawbacks of this kind of algorithm based on EKF; Yuan et al. [9] introduce a way about local submap, in which robots build their maps independent of each other and then get the global map by EKF traverse map fusion; Howard [10] proposes an effective online MCSLAM algorithm, which extends the Blackwellised particle filter, and multirobots share measurement information with each other for collaborative SLAM. However, the algorithm has no matching fusion for local map when two robots meet but rather employs the process of time reverse to share information about the history of measurements after robots encounters, which makes the global map update delay and reduces the efficiency of building map due to the same measurement and motion control information reused by multirobot SLAM process. Cai et al. [11] adopt auxiliary robot to provide the leader robot with measurement and put forward an online multirobot FastSLAM algorithm based on auxiliary measurements, which can improve the accuracy of localization and mapping about lead robot, but the SLAM problem of auxiliary robot cannot be solved. Gil et al. [12] and Lee et al. [13] study multirobot SLAM algorithm based on particle filter; they consider the uncertainty of localization, but no attention is paid to uncertainty of map information.
Considering uncertainty of the map information, this paper presents a new multirobot FastSLAM algorithm based on landmark consistency correction (MBLCC-FastSLAM). Firstly, electromagnetism-like mechanism is introduced to resampling procedure in single-robot FastSLAM; attraction-repulsion mechanism in electromagnetism field is used to simulate interactive force between particles, where attractions drive the particles approach to high likelihood region; moreover, repulsions drive the particles separate from each other and maintain a certain distance in moving process to assure diversity. Then, when multiple robots observe the same landmarks, every robot is regarded as one node and Kalman-Consensus Filter (KCF) is used to update landmark location, which further improves the accuracy of localization and mapping.
2. Problem Statement
Suppose the prediction and measurement model of robot system at time are represented by prior probability function and measurement probability function , where the state vector and map denote the pose of mobile robot and the position of environmental features of mobile robot at time , respectively, is control variable, and is observed quantity at time . The SLAM problem can be expressed as the joint posterior probability distribution function , where is robot trajectory, is measurement sequence, and is robot control sequence.
The FastSLAM algorithm is based on the idea of Rao-Blackwellised; the posterior joint probability distribution is decomposed into robot trajectory estimation and maps estimation [14, 15]. It states that if one knew the path of the robot, the landmark positions could be estimated independently; so, under Bayesian formula and a Markov assumption, the joint probability distribution of SLAM can make decomposition as follows: where is the number of landmarks. Particle filter is used to estimate the pose of robot in FastSLAM algorithm; each particle can save a map along with Gaussian landmarks; the remaining posterior of landmark locations is analytically calculated by EKF filter. Each particle in FastSLAM is of the form , where denotes the position of particle , is the weight of particle , and denote the mean and covariance of landmark , respectively. The posterior probability distribution is generally hard to obtain. So particles are sampled from proposal distribution function and it satisfies the following equation which can approximate the posterior probability distribution: Then the weight of sampling particles can be calculated by where is the weight normalized value.
The differences between FastSLAM2.0 and FastSLAM1.0 are as follows: FastSLAM1.0 uses only prior distribution as the proposal distribution function and thus ignores the current measurement, which is particularly troublesome, if the measurement is too accurate to the robot’s motion noise. FastSLAM2.0 is an improved version; it estimates robot pose by EKF algorithm at first and then uses the estimating mean and variance to construct a Gauss distribution function as proposal distribution, and, as a result, FastSLAM2.0 is greatly superior to FastSLAM1.0.
FastSLAM algorithm is based on sequential important sampling (SIS) method; as a result, the particle degradation and sample dilution are its major problems [16]. In order to solve the problems and improve the estimation performance of FastSLAM2.0, this paper presents a new algorithm called MBLCC-FastSLAM, as will be shown in the next paragraphs.
3. MBLCC-FastSLAM
3.1. Electromagnetism-Like Mechanism Is Introduced to Single-Robot Resampling
Electromagnetism-like mechanism algorithm is a new kind of natural heuristic global optimization algorithm [17]; its main idea is to assume that every solution is an electron, and then attraction-repulsion mechanism in electromagnetic field is simulated to drive search particles to move to the optimal solution by certain rules. The four basic procedures of electromagnetism-like mechanism algorithm are as follows (to calculate the maximum value of function as an example). (a) Initialization: points are sampled randomly from known feasible region; then the objective function value (OFV) of each particle is calculated by , and the point that has best OFV is stored in . (b) Local search: it is a neighborhood search procedure, which can be applied to one or many points for local refinement at each iteration. (c) Calculation of total force vector: in this procedure, a charged-like value is assigned to each point; the charges are calculated by where is the dimension of point . Then, the total force is calculated by the calculation formula of force and the superposition principle in electromagnetism field: as can be seen in (5), between two points, the point that has a larger OFV attracts the other one; contrary to it, the point with worse OFV repels the other. (d) Movement along total force vector: points are moved along the total force vector ; the movement is according to where is the random step length, which is uniformly distributed between 0 and 1; is the mobile coefficient, which denotes the allowed range of movement toward the lower bound or the upper bound for the corresponding dimension; through the above procedures, points complete electromagnetism-like mechanism iteration and move toward a better set of points.
Electromagnetism-like mechanism algorithm has advantages of less parameters and a good ability of global optimization; when the mobile coefficient is selected appropriately, the stochastic points can be driven close to quickly according to (6); it also has a relatively fast convergence speed [18]. In order to take both particle degradation and sample dilution into account, we introduce electromagnetism-like mechanism algorithm into FastSLAM2.0 resampling procedure for improving the particle distribution and overall quality of particles.
At a given time instant , FastSLAM2.0 algorithm based on electromagnetism-like mechanism (EM-FastSLAM) performs the following steps.
Step 1 (sampling). Considering both the control variable and the current measurement , EKF algorithm is used to estimate robot pose at first and obtain the improved proposal distribution, from which samples particles set .
Step 2 (weights calculation). The weight of particles is calculated by (3).
Step 3 (the electromagnetism-like mechanism optimization sampling process). Each particle is regarded as an electron. First, the charge of each particle is calculated by (4). Here, is the number of particles, is the weight calculation formula, and denotes the optimal location of the robot. Then, the total force vector is calculated; we use the following total force calculation model in this paper: where is the force particle on particle ; finally, particles are moved along the total force vector by (6) and thus get a new batch of high quality particles. Equation (3) is reused to calculate the weight of new particles and weight normalized by .
Step 4 (resampling). The effective particle number is . If is less than the given threshold, then the algorithm starts resampling and gets a new particles set or does not resample.
Step 5 (map updating). Each particle updates its observed landmarks by EKF with current state estimate and the previous map.
Step 6. Repeat the above steps until there are no new measurements.
As can be seen, after the electromagnetism-like mechanism optimization sampling process, between two particles, the particle that has a greater weight attracts the other one; contrary to it, the particle with worse weight repels the other. So the particles are driven to high likelihood region by electromagnetism-like attractions, which can reduce particle degradation. Simultaneously, electromagnetic-like repulsions drive the particles to separate from each other and maintain a certain distance in moving process to assure diversity.
3.2. Landmark Consistency Correction
When robot explores an unknown area and gets measurements of the landmark feature, all of the landmark position estimates are related. For the reason that they are based on the same pose state estimate of mobile robots, the accuracy of landmark estimation directly affects the positional accuracy and consistent estimation of the map. For each robot, the landmark estimated values are inaccurate. To make things worse, when calculating the particle weights, the uncertainty is brought into next moment. Aiming to decrease the uncertainty of map information, we adopt three robots to work at the same time and suppose each robot applies an EM-FastSLAM algorithm. Furthermore, each robot is regarded as a node and Kalman-Consensus Filter (KCF) is used to update landmark location, which corrects the estimation of landmark for further improving the accuracy of robot localization and map building. The illustration of MBLCC-FastSLAM is as shown in Figure 1.
The so-called core process of the KCF [19] is represented by where and are estimate and prior estimate of the node , respectively, and and are the Kalman and consensus gains of node , respectively. We refer to and as sensor data and measurement matrix. From (8), we can know that it includes a Kalman filtering part and a consensus part. The Kalman filtering updates target node prior estimate values with measurements. Meanwhile, because of the capacity of multiagent consistency, the node estimation error can be decreased to some extent and allow all nodes to maintain an approximate value. Based on this idea, each robot is regarded as a node and we use the KCF algorithm to estimate landmarks location for fusing measurements of other robots.
Now, a method for landmark consistency correction is presented.
Firstly, each robot calculates its prior estimate of the landmark: where is the prior estimate of robot with particle to landmark at time and is measurement equation. with (10) is the Jacobian matrix. And the Kalman gain is given by where is the covariance of robot with particle to landmark at time and is measurement noise.
It is assumed that the landmark is stationary; then its prior estimate value is itself at time . Landmark location update step by consensus-based filtering is thus given by where is the step length, is the number of robots for getting the same observed landmark at time , and denotes the maximum weight particle at time . Notice that, in (12), this method only requires prior estimates exchange between robots which observe the same landmarks. In practical applications, considering the limited network communication broadband, packet-dropping, delay, and other sorts of interference, the algorithm may be of weak anti-interference and poor robustness, if there was too much information exchange between robots [19].
4. Simulation Analysis
4.1. Simulation Model
In this paper, we adopt three robots to work simultaneously; robot motion model is as follows: where shows pose of robot in global coordinate, represents yaw angle, is motion speed, denotes deflection angle, is sampling time of odometer, , , and are noise terms, and denotes spacing of two axes.
The measurement model can be expressed as where and refer to the distance and direction angle between landmark and robot, which are measured by sensor, denotes observed position coordinates of the th landmark, and is measurement noise.
4.2. Result Analysis
In order to verify the effectiveness of the improved algorithm, the proposed MBLCC-FastSALM, multirobot SLAM [10] (with known initial pose), single-robot EM-FastSLAM, and traditional FastSLAM2.0 are simulated for SLAM experiment, respectively, and the estimation accuracy, error, and stability are analyzed to contrast.
The environment simulated is shown in Figure 2, where the asterisk represents a random set of landmarks and the three curves represent the three preset motion paths of robots; the robots run along this path from starting points to terminal points; the speed of robots is , , and ; the effective measurement distance of sensor is . Assume that the data association is known; using MBLCC-FastSLAM algorithm to obtain the results which are shown in Figure 3 in the simulation environment, where the triangle represents the position of estimated landmarks, the dotted line indicates the estimated trajectory. The process noise is , measurement noise is , mobile coefficient is , landmark update step length is , the number of particles is 10, and the resampling, when , is less than of the total particles.
As shown in Figure 3, the estimated path and landmarks of the robot utilizing MBLCC-FastSLAM algorithm are basically consistent with the true value, which performs with very good estimation ability.
Three robots are simulated in multirobot SLAM, EM-FastSLAM, and FastSLAM2.0 algorithm under the same conditions, respectively. Figure 4 represents the three robots’ estimation errors under four different algorithms in a simulation run, from which we can see that the proposed algorithm MBLCC-FastSLAM performs better than the other 3 algorithms in all three robots, particularly in robot 1 and robot 2 in which the estimated errors of MBLCC-FastSLAM are significantly lower both in pose error and landmark error. And EM-FastSLAM is lower than FastSLAM2. 0 most of the time.
(a) Robot 1
(b) Robot 2
(c) Robot 3
In order to eliminate randomness and more directly see the results of the four algorithms which are applied 50 times independently in Table 1, the simulation experiment of error statistics is given. By comparing EM-FastSLAM algorithm to FastSLAM2.0 algorithm, the estimated error of the former is lower. For that, the electromagnetism-like mechanism is introduced to resample process in FastSLAM2.0 algorithm, which improves the quality of the particles; similarly, the estimated errors of the proposed MBLCC-FastSLAM algorithm are the lowest; for that, the proposed algorithm gets more correct measurement results by revising landmark based on the consistency. As the correlation of landmark estimation and the pose estimation, the robot localization estimation and map building precision are improved further. Furthermore, we compute the averaging RMSE (ARMSE) in landmark with respect to different sampling particles . The simulation results are shown in Figure 5. From Figure 5, it can be seen that the accuracy of four algorithms is improved as the particles increase. On the other hand, when particles exceed about 150, it has little effect on the algorithms when particles continue to increase. At the given noise level, the ARMSE of MBLCC-FastSLAM is the lowest in four algorithms; it can maintain a good precision even when is small. In this case, the filtering accuracy of MBLCC-FastSLAM takes 10 particles approximately which is equal to FastSLAM2.0 taking 100 particles.
5. Conclusion
In this paper, we presented a multirobot FastSLAM algorithm based on landmark consistency correction. Firstly, electromagnetism-like mechanism is introduced to resampling procedure in single-robot FastSLAM, which improves the distribution of particles by the rule that greater weight particles attract the worse weight particles and the worse weight particles repel greater weight particles. And, then, the landmark location is updated with KCF, when it is observed by multiple robots, which further improves the robot localization and map building precision. Under the same conditions, simulation results show that the MBLCC-FastSLAM algorithm performs more accurately in the localization and map building precision compared to multirobot SLAM algorithm [11], EM-FastSLAM algorithm, and traditional FastSLAM2.0 algorithm, which is proved to be effective and feasible.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
This work is supported by National Natural Science Foundation of China (no. 61364017, no. 61034006, and no. 60804066) and the Jiangxi Education Department Scientific and Technological Project (no. GJJ12286).