Abstract
Fast simultaneous localization and mapping (FastSLAM) is an efficient algorithm for autonomous navigation of mobile vehicle. However, FastSLAM must reconfigure the entire vehicle state equation when the feature points change, which causes an exponential growth in quantities of computation and difficulties in isolating potential faults. In order to overcome these limitations, an improved FastSLAM, based on the distributed structure, is developed in this paper. There are two state estimation parts designed in this improved FastSLAM. Firstly, a distributed unscented particle filter is used to avoid reconfiguring the entire system equation in the vehicle state estimation part. Secondly, in the landmarks estimation part, the observation model is designed as a linear one to update the landmarks states by using the linear observation errors. Then, the convergence of the proposed and improved FastSLAM algorithm is given in the sense of mean square. Finally, the simulation results show that the proposed distributed algorithm could reduce the computational complexity with high accuracy and high fault-tolerance performance.
1. Introduction
Simultaneous localization and mapping (SLAM) is the process of enabling a mobile robot to move through an unknown environment, building a map and estimating the position simultaneously, by estimating the features of the environment. In the last decade, SLAM has attracted lots of attention in the mobile robotics literature and is considered to be a key prerequisite for the truly autonomous robot navigation [1, 2].
In general, there are three main types of SLAM, the extended Kalman filter based SLAM (EKF-SLAM), the particle filter based SLAM (PF-SLAM), and FastSLAM, which have been widely used. In fact, EKF-SLAM is the first SLAM used in the real system and has been well developed over the past two decades. As the well-known algorithm to solve the Gaussian nonlinear problem, it linearizes the nonlinear model using Taylor series expansions. However, the series approximations lead to poor representations of the nonlinear functions due to the fact that EKF cannot solve the Gaussian problem [3]. Then, to own a feasible solution for non-Gaussian nonlinear SLAM, PF-SLAM is proposed in [4]. The particle filter relies on the importance of sampling to approximate the posterior distribution. However, since the feature points will change in the unknown environment, the entire system equation must be reconfigured. The computational complexity of PF-SLAM is a major problem for estimating the robot state and landmarks together. To overcome the former problems, a FastSLAM framework, using the Rao-Blackwellised particle filter (RBPF), is proposed in [5]. FastSLAM is an efficient algorithm for SLAM problems. It decomposes the entire SLAM system into a robot localization problem using PF and a collection of landmarks estimation problems using EKF. The FastSLAM approach separates the robot state from the landmarks state estimate; only the robot state equation needs to be reconfigured when the feature points change in the process. However, many limitations still need to be overcome in FastSLAM. For example, the feasible solution for non-Gaussian nonlinear SLAM and the convergence should be discussed.
In this paper, a new improved FastSLAM based on distributed structure is proposed to overcome the drawbacks of FastSLAM. In the improved FastSLAM algorithm, the SLAM problem is divided into robot localization problem and landmarks estimation problem [6]. In the localization part, the robot state is divided into several parts according to the effective landmarks based on the distributed structure, which can estimate the distributed states at the same time without reconfiguring and changing the dimension of the robot state equation, as well as improving the fault tolerance [7]. Then, each part of the state is estimated by a local unscented particle filter (UPF) which can permit a set of sigma points to be scaled as an arbitrary scaling parameter [8]. And the computational cost will not be increased in this algorithm. In the landmarks estimation part, since the observation error is used in the updating step, the landmarks estimation can be seen as a linear optimal estimation. The federated Kalman filter, an associated set of independent Kalman filters, estimating the position of landmarks effectively, is used to estimate the landmarks instead of EKF and without the linearization process [9].
The contents of this paper are as follows. In Section 2, the basic properties and theorems of SLAM model and FastSLAM framework are reviewed. Section 3 defines the improved distributed FastSLAM and shows the process of the robot state estimation and the landmarks states estimation. Meanwhile, the weight fusion method for the local filters is given. Section 4 discusses the mean square convergence of the proposed algorithm. Section 5 gives the simulation results and the conclusions are made in Section 6.
2. Background
2.1. SLAM Model
Indeed, the absolute position of the robot cannot be obtained directly; it needs to use the indirect observation from the sensor to estimate the position of the robot while maintaining error stable in a small range [10]. The SLAM algorithm uses dead reckoning and relative observation to estimate the position of the robot and build a navigation map. Generally, the model of SLAM is composed of the motion model and the observation model.
2.1.1. Motion Model
The robot motion model can be described as a probabilistic Markov chain where , , are the robot states defined at the external laser sensor at time , and is state noise.
The landmarks are assumed to be static, and the landmark motion model is given by where is the state of the th landmark.
2.1.2. Observation Model
Normally, the observation equation relating the robot states to the landmarks is where is the observation vector. is the measurement noise. is the distance from the beacon to the laser sensor and is the laser sensor bearing measured with respect to the robot coordinate frame. means the coordinate of the landmarks.
2.2. FastSLAM Framework
The FastSLAM algorithm, introduced by Montemerlo et al., is an efficient algorithm especially for the SLAM problems [11]. It utilizes a particle filter to approximate the ideal recursive Bayesian filter for estimating the robot pose. At each point of time, the FastSLAM algorithm maintains a set of particles to represent the posterior, denoted . Each particle represents a possible position of robot where refers to the th particle. And the set of particles in is considered for both the robot control and the measurement , which is denoted by the following sampling distribution:
After obtaining the new set , each particle is drawn with a probability proportional called the importance weight , and then the final robot position and orientation estimation results can be gotten. Finally, the landmarks estimation is represented by EKF, and this estimation is conditioned on the robot position and orientation.
In the FastSLAM algorithm, the SLAM posterior was partitioned into a localization problem with three positions and orientation parameters of robot and independent landmarks position estimation problem with landmarks with two state parameters conditioned on the robot position and orientation estimation. So that the update process in FastSLAM only involves a Gaussian of dimension two, whereas a Gaussian of size has to be updated in EKF-SLAM [3].
However, the FastSLAM algorithm also suffers from three limitations. Firstly, the FastSLAM algorithm belongs to the centralized approach. The robot position and orientation estimation are considered under measurement information. Since the number of effective landmarks is changing over time, the dimension of robot state equation must be reconfigured. It is difficult for the centralized approach to deal with the reconfiguration in runtime. Secondly, the fault tolerance of centralized approach is low. Thirdly, since EKF is used for landmarks estimation, it needs to approximates the nonlinear model using a linear function and take the derivative of the Jacobian matrices with complicated calculations.
3. Improved FastSLAM Based on Distributed Structure
In this section, the improved FastSLAM algorithm is introduced. The improved distributed FastSLAM is based on the distribution structure. And the federated Kalman filter algorithm is used to replace the EKF in FastSLAM. This improved FastSLAM algorithm consists of three parts: the robot state estimation, the landmarks state estimation, and the importance weights calculation. Figure 1 shows the framework of the proposed improved FastSLAM.
3.1. Robot State Estimation
Since it is complex for a centralized approach to handle the changing dimension of the state vectors, UPF approach is used to do the robot state estimation, which is based on the distributed structure. The state is divided into several parts based on the effective landmarks. Each part of the state is estimated by a local UPF filter which is separated from the others. Each estimated result of the states is transmitted to the master filter to make the final results. Even if there is an error in one part, it would not affect the estimated results of the other parts. The distributed algorithm can reduce the effect of error messages on the final estimated result, improve the fault tolerance, respond effectively to the changes in the number of effective landmarks, and have a better accuracy:
Based on the distributed structure, the detail motion model for robot states estimation is derived as (6). The local UPF filters are built based on the effective landmarks separately, and the process and measurement noises are added to each state vector. Then, each local filter deterministically extracts the sigma points from the Gaussian distribution. After that, the estimated mean and the covariance of each state vector are updated according to the UKF algorithm [12]. The updated mean and covariance are remarked as and , and then they are used to optimize the distribution of the particles in every local filter. From the Gaussian distribution generated by the estimated mean and covariance of the robot, the particles of each local filter are resampled by
Then, the next steps are calculating the weight of each local filter, resampling as normal particle filter algorithm and doing results fusion [13]. Finally, the estimation results of the robot state can be obtained accurately.
3.2. Landmarks State Estimation
In the normal FastSLAM, since the measurement is nonlinear as (3), EKF is used to do the landmarks state estimation and approximate the nonlinear process as the linear process. However, the linearization process needs to calculate the Jacobian matrices and has a high linear approximation error, which will increase the computational complexity while reducing the estimation accuracy [14]. In the proposed algorithm, a new observation model is proposed on the basis of FastSLAM algorithm. Because the estimation process of landmarks is separated from the robot state estimation process in FastSLAM, the estimated results of the robot state with high accuracy can be taken as given in the landmarks estimation process. Since the position of landmarks can be calculated by robot pose and the distance between the robot and landmark obtained by the laser sensor and there is an observation error in measurement results, the new observation model can be given by where and are the distance between the robot and landmarks obtained by the laser sensor. is the measurement noise.
Since the landmarks are assumed to be static and the motion model is given by (2), the state estimation model of each landmark is shown as
From (9), since the motion model and observation model of landmarks are linear in the part of landmarks estimation, then the landmarks estimation process can be seen as a linear optimal estimation problem, and Kalman filter is the best method for this problem. So the Kalman filter is used to estimate the landmarks position to avoid the complex calculation of Jacobian matrices in EKF. However, since the feature points change, it is difficult for the centralized Kalman filter algorithm to deal with it and the centralized approach does not get a good fault tolerance.
In this paper, the measurement information provided by the laser sensor may include outliers and the landmarks need to be estimated at the same time; the federated filter [15] is used here to estimate the states of landmarks for its stronger fault tolerance and its capability of dealing with several different estimations simultaneously. And Kalman filter is used as the local filter of the federated filter which can reduce the computation complexity while maintaining the estimation accuracy. According to every estimated result of local UPF filters and observation information, the corresponding landmarks are updated by local Kalman filters separately. The motion model for landmarks state estimation is as follows: where refers to the th landmark.
The update process of the landmark in each local filter is the same as the normal Kalman filter algorithm [16]. After the update process, all of the local Kalman filter estimation results will be combined to calculate the optimal estimated results of each landmark.
3.3. Importance of Weights Calculation
Generally, in distributed particle filters, the Neff method is used to calculate the importance of weights of every local filter [8]. The Neff method can be defined as follows: where is the number of effective particles in the th filter. is the weight of each particle in the th filter.
However, the Neff method distributes the weights through calculating the degree of the particles degeneracy of each filter; it is closely related to the number of particles. If in the case of a small number of particles, it cannot effectively response to the real weights distribution. Therefore, a new method is advanced which combines the Neff method with the innovation method in order to obtain a high precision of real weights distribution. The new method can be calculated by where is the weight generated by Neff algorithm, is the weight generated by innovation algorithm, is the mixed weight, and and are the proportion parameters, which stand for the proportion of these two methods in the mixed method, and the relationship between these two parameters can be defined as (12).
The weight of each local filter based on the innovation method is calculated by (13), and the innovation is the difference between the observation and the estimation:
4. Mean Square Convergence
For the proposed distributed FastSLAM, the convergence of the unscented particle filter with the distributed implementation is an important issue and requires careful investigation, as it is crucial for the successful applications. In this section, the proof of the mean square convergence of the distributed unscented particle filter to the optimal filter was described. First, the convergence results from particle filters were extended to prove the convergence of distributed particle filter in mean squares as the number particles go to infinity. Second, the mean square convergence of the distributed FastSLAM was proved by considering in turn each of the steps of prediction, measurement, and resampling.
The generic proof of distributed unscented particle filter convergence is as follows.
Theorem 1. If the importance of weight is an upper bounded for any () and if one uses one of the selection schemes described previously, then, for all , there exists independent of > such that for any where is the space of bounded, Borel measurable functions on . We denote , . Equation (14) is the mean square convergence of the filters. This convergence result shows that, under very loose assumptions, convergence of the unscented particle filter is ensured and the only crucial assumption is to ensure that is upper bounded. Hence, the weight of the distributed FastSLAM proposed must prove that it is upper bounded firstly.
Proof. The weight of the distributed FastSLAM is defined by
where is the weight to the th local filter, is the weights distribution for the new method in (12) to the th local filter, and is the number of local filters.
Because of the principle of information fusion, the weight distribution sets to individual local filters should be described by and then . Because each local filter is UPF which has the same structure and the literature has proved that the weight of UPF is an upper bounded [8], so we can get , where is constant. Because is also the number of features matching, which must be upper bounded, we suppose , where is constant. Then we have . Thus, the weight of the distributed FastSLAM is an upper bounded, so that the mean square error is convergent.
Then, the mean square convergence of the distributed FastSLAM will be proved by considering in turn each of the steps of prediction, measurement, and resampling. This proof is a modification of [17, 18]. The main difference from that proof is that the effect of UKF must be considered and an upper bound can be obtained.
The prediction update step will begin with the following lemma. The mean square error can be supposed by for any . So the mean square error in the prediction update step can be defined as .
Proof. We have
If is the -algebra generated by , then .
The first term on the right hand side of (16) is bounded above as follows:
Since Markov operators are contractions [19], so
The upper bound of the second term (16) is
so that , where .
Assume that, for any , the mean square error for the UKF step can be defined as .
Proof. From Minkowski’s inequality,
The first term (20) on the right hand side is bounded as follows:
Using the result of the prediction step, we can get
where .
The next distributed FastSLAM step is a measurement update, and the mean square error can be provided as .
Proof. We have
From the result of the UKF step, we can get
where , .
The last step is the resampling step, and the mean square error can be provided as .
Proof. We have
If is a -algebra generated by , then .
Thus, by the same procedure from (17), for some constant , finally we have
where .
Combing the above results, the following main conclusion can be gotten where the mean square error of the distributed FastSLAM is the upper bounded, and the conclusion is as follows:
5. Experiment Results
To verify the accuracy of the improved algorithm, a simulation experiment was finished in this section. We compared the estimated results of the normal FastSLAM algorithm, the distribute SLAM (DPF-SLAM) algorithm, and the distributed FastSLAM algorithm proposed in this paper.
These experiments were tested using the experiment data come from an experiment which was taken at Victoria Park in Sydney, Australia [20, 21]. The trees can be considered one of the most relevant features that a laser sensor can identify with this outdoor environment. The algorithm implemented in [20] was used in this paper, which tracks the center of the trunk by clustering a number of laser observations. The extraction algorithm of the center was shown in Figures 2 and 3.
The vehicle was started at a location with known uncertainty and driven in this area for more than 300 sec. The estimated results of the vehicle trajectory and navigation landmarks were given by three algorithms as shown in Figures 4 and 5. We used the GPS information on good quality of the standard data compared with the results estimated by the three different algorithms separately, so as to compare the accuracy of these algorithms directly. From Figures 4 and 5 we can see that all of the three algorithms can match the GPS standard data very well at the beginning of the estimated trajectory. However, as time went on the normal FastSLAM algorithm and the normal DPF-SLAM algorithm could not match the GPS standard data, especially after 148 sec, while the improved distributed FastSLAM algorithm can make better estimation results, although the accuracy of the normal DPF-SLAM algorithm is better than that of the FastSLAM algorithm. Since GPS was in the shade, it could be seen that GPS information was not available in some areas. The landmarks were used here to reduce the navigation errors.
The root mean square error (RMSE) of the vehicle position of three algorithms between 148 sec and 160 sec in the directions and , compared with GPS standard data, is shown in Figure 6, and Table 1 shows the statistical data of RMSE in Figure 6. The RMSE of the normal FastSLAM algorithm is higher than the normal DPF-SLAM algorithm, and the improved distributed FastSLAM algorithm has a little estimated error compared to the DPF-SLAM algorithm. Thus, we know that the performance of the improved distributed FastSLAM system is better than the others.
(a)
(b)
These results demonstrate that the improved distributed FastSLAM algorithm is workable and stable and has a higher accuracy at the same time.
6. Conclusion
This paper proposed a distributed FastSLAM algorithm as an effective solution to simultaneous localization and mapping. In the localization process, the distributed structure was used to reduce the effect of error messages on the final estimated results and improved the fault tolerance of the SLAM. The unscented Kalman filter was implemented in the prediction step of the robot state and provided a better proposal distribution. In the landmarks estimation process, the nonlinear landmark observation model was changed into linear models, and federated Kalman filter was implemented in this process to estimate the state of landmarks for its stronger fault tolerance and its capability of dealing with several different estimations at the same time. The convergence of the proposed distributed FastSLAM was proved in the sense of mean squares. The simulation results showed that the proposed distributed FastSLAM algorithm has better estimation accuracy than the previous algorithms, and more accurate estimation could be achieved consistently for longer time periods. The simulation results also proved that the proposed algorithm can reduce the computation complexity and maintain the estimation accuracy.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
The authors would like to thank the University of Sydney for the use of the Victoria Park dataset. This work was supported by the National Science Foundation of China under Grant 60975065 and supported by Program for the Top Young Innovative Talents of Beijing CITTCD201304046.