Abstract
For fast simultaneous localization and mapping (FastSLAM) problem, to solve the problems of particle degradation, the error introduced by linearization and inconsistency of traditional algorithm, an improved algorithm is described in the paper. In order to improve the accuracy and reliability of algorithm which is applied in the system with lower measurement frequency, a new decomposition strategy is adopted for a posteriori estimation. In proposed decomposition strategy, the problem of solving a 3-dimensional state vector and N 2-dimensional state vectors in traditional FastSLAM algorithm is transformed to the problem of solving N 5-dimensional state vectors. Furthermore, a nonlinear adaptive square root unscented Kalman filter (NASRUKF) is used to replace the particle filter and Kalman filter employed by traditional algorithm to reduce the model linearization error and avoid solving Jacobian matrices. Finally, the proposed algorithm is experimentally verified by vehicle in indoor environment. The results prove that the positioning accuracy of proposed FastSLAM algorithm is less than 1 cm and the azimuth angle error is 0.5 degrees.
1. Introduction
In simultaneous localization and mapping (SLAM), vehicle uses the carried sensors to sense surroundings and uses the sensed information to create environment map on one hand. On the other hand, vehicle uses created map to locate and guide.
There are several methods to deal with SLAM problem, in which EKF-SLAM and FastSLAM are the two most popular methods. EKF-SLAM has some obvious limitations: inconsistency due to errors accumulation introduced by linearization, complex computation to deal with high-dimensional joint covariance, lack of robustness to incorrect data association, and so on [1–3]. FastSLAM employs the Rao-Blackwellized particle filter (RBPF) to estimate position and extended Kalman filter (EKF) to estimate map features. Compared with EKF-SLAM, FastSLAM has a lower complexity and is more robust regarding the data association problem. Nevertheless, the FastSLAM based on RBPF also suffers from some drawbacks such as particle degeneration, Jacobian Matrix solving and linear processing of nonlinear function [4–6]. To deal with these problems, the SLAM based on square root unscented Kalman filter (SRUKF) is proposed. Instead of approximating the state and measurement transition functions by Taylor series expansion, the unscented transformation employed by SLAM is used to update nonlinear state and measurement functions and the accuracy of the state estimation has been significantly improved [7–9].
The filtering approaches used in all aforementioned SLAM methods can achieve good performance under certain assumptions. However, the assumptions are typically not entirely satisfied in practical applications. Thus, the performance of SLAM algorithm may be downgraded from the theoretical performance, which can potentially lead to divergence. To prevent divergence and to improve the practicability of SLAM algorithm, the so-called adaptive filtering approach has been used in SLAM algorithm to dynamically adjust the parameters of the supposedly optimum filter based on estimating the unknown parameters for online estimation of motion and by estimating the signal and noise statistics from the available data. The adaptive filter can be realized to adjust important coefficients through fuzzy logic algorithm or neural network algorithm [10, 11]. Another popular method to realize the adaptive filtering is to use estimators [12–14].
To overcome the shortcomings of traditional adaptive filter, we developed a new nonlinear adaptive square root unscented Kalman filter (NASRUKF) which can be used in nonlinear or linear system for multisensory data fusion with uncertain process noise [15, 16]. In [17], the prototype of FastSLAM algorithm based on NASRUKF is described, and the validity is demonstrated by simple preliminary experiment.
In this paper, the improved FastSLAM algorithm based on NASRUKF is described in detail. To verify the accuracy of FastSLAM under the condition of low measurement frequency, the algorithms based on SRUKF and ASRUKF are compared and analyzed. And a complete experiment is designed for further validation.
2. FastSLAM Framework
The idea of FastSLAM algorithm comes from an analysis result of Dynamic Bayesian Network (DBN): if the vehicle pose is fully determined, then the map features are mutually independent. As shown in Figure 1, if the vehicle pose estimation is known, the estimations of map features and are independent from each other. The input information of FastSLAM includes the control information and the measurement information .
From a probabilistic point of view, SLAM problem is to solve the posterior probability distribution of system state vectors composed of vehicle pose vector and map feature vector , which can be expressed as follows [18]:
From the aforementioned DBA analysis result and Bayesian formula, formula (1) can be expressed as follows:
Formula (2) shows the idea of FastSLAM algorithm that the joint SLAM state estimation can be factored into two independent estimations: vehicle pose component and map feature component. Thus, the problem of solving () dimension state vectors is converted into two parts: a 3-dimension state vector solving of vehicle pose and 2-dimension state vectors solving of map features.
For traditional FastSLAM algorithm, estimation of vehicle pose is achieved by particle filtering. Each map feature, in each particle, can be estimated using Extended Kalman Filters conditioned on the robot pose of the particle. And the factor weight of particles is calculated to determine the probability that a certain particle enters the final set.
The implementation process of FastSLAM algorithm is as follows. The posterior probability distribution of vehicle pose is estimated using the control information and the motion model. Map features of particles are associated with measurement information via maximum likelihood estimation. According to correlated measurement information of each particle, update the estimations of each map feature and vehicle pose.
3. Optimization of FastSLAM
As the performance of FastSLAM depends on measurement information, the accuracy and reliability of traditional algorithm are downgraded when measurement data are limited by detecting frequency of sensors. In addition, the linearization method increases the estimation error for the reason that the actual system has strong nonlinearity and uncertain noise.
In [17], the basic idea of an modified FastSLAM was introduced simply. In this section, the modified FastSLAM introduced in [17] will be described in detail which is especially appropriate for low speed vehicle system (such as the wheelchair used for the aged). Considering low scanning speed sensors, a new decomposition strategy is designed for the posterior probability distribution. Moreover, an improved a posteriori estimation method is proposed which simultaneously estimates the joint state vectors comprised of vehicle pose and map features.
3.1. Posterior Probability Distribution Decomposition Strategy
The proposed posterior probability distribution decomposition strategy of state vectors is shown as
Assuming that there are particle state estimations and relevant covariance matrices of estimation error , the particle state estimations are irrelevant; that is, . Then optimal estimation of the joint state vectors can be expressed as
The physical meaning of formula (4) is that if the estimation accuracy of is low, its global contribution will be low.
It can be drawn from (3) and (4) that
As shown in (3) and (5), the proposed FastSLAM algorithm converts the high-dimensional joint state estimation into several independent low dimensional joint state estimations. That is, the state vectors solving of () dimensions is converted into 5-dimension state vectors solving.
Compared with the traditional FastSLAM algorithms, the correlation between vehicle pose and map features in low dimension state vectors is considered which is helpful to improve the accuracy and reliability of FastSLAM, especially when one frame data of the sensor can be used to observe at most one map feature which cannot create a new full map feature. This often appears in a vehicle system with lower measuring frequency sensors, such as sonar or infrared sensor, which can only obtain one map feature in a sampling cycle.
Equations (3) and (5) can be simplified aswhere represents the created map feature; represents the th state vector with created feature. The meaning of formula (6) is that if the th measurement data is from created feature, then joint estimation of vehicle pose and the created feature is carried out. If not, the vehicle pose is estimated only and the th measurement data is saved to create a new map feature.
3.2. NASRUKF
For a nonlinear and discrete system, stochastic sequence is described bywhere and are nonlinear functions of the system; is the state vector of system; is the control vector applied at time ; is process noise; is observation vector; is the measurement noise.
The sigma points are calculated as
The time update equations are the following:
Augment sigma points are
Estimate the square root of the measurement noise matrix:
The measurement update equations are as follows:where ; and is the forgetting factor, typically ; the weights ( and ) of the mean and covariance are given bywhere is a scaling parameter. The constant determines the spread of the sigma points around the mean, which is typically set to a small, positive value. The constant is a secondary scaling parameter. is used to incorporate the prior knowledge of the distribution (for Gaussian distributions, is optimal).
4. Implementation of New FastSLAM Algorithm
The proposed FastSLAM algorithm implementation process is shown as in Figure 2.
Initialization. During initialization, the initial pose of vehicle is obtained by the static state estimation method based on the measurement data of sensors. In addition, the initial feature of the map is created using the feature extraction method.
After initialization, the vehicle location and mapping will be calculated and updated based on proposed FastSLAM algorithm in the following steps.
Vehicle Pose Estimation. The vehicle pose is estimated using ASRUKF.
Data Association. According to (14), (15), and (16), data association of the measurement data at time is dealt with to determine whether it is a created map feature. If the measurement data is a created map feature, algorithm turns to vehicle pose and map feature updating; otherwise, the data shall be stored as new feature data, and algorithm turns to new map feature creating.
Vehicle Pose and Map Feature Updating. According to [17], this step consists of two cases. The first one is that the measurement data matches the created linear feature. The second case is that the measurement data matches the created arc feature.
(1) The Measurement Data Matches the Created Linear Feature. If the line segment is not parallel to -axis, that is, , it can be defined that is the state variable of the nonlinear discrete system described as
If the line segment is parallel to -axis, that is, , can be defined as the state variable of the nonlinear discrete system, which is described by where and have the statistical property of zero mean, mutual independence, and Gaussian distribution. The covariance matrix of and is and , respectively.
(2) The Measurement Data Matches the Created Arc Feature. It can be defined that is the state variable of the nonlinear discrete system described as where
New Map Feature Creating. If the number of new feature data exceeds threshold value , a new map feature should be created.
5. Experimental Results
In order to evaluate the performance of proposed FastSLAM algorithm, experimental results are shown in this section. The test environment is built as Figure 3. The main parameters are depicted as follows: , the length threshold of linear segment is , the distance threshold between point and line is , and the threshold of average vector distance is .
As shown in Figure 3, CPU Motion Controller implements the proposed FastSLAM algorithm and uploads the results to the computer via the receiving node and the transmitting node for real-time display.
The whole experiment process is divided into the static state estimation and the dynamic update.
5.1. The Static State Estimation
The surroundings are scanned by the sensors of vehicle under stationary state. The scanned data set is clustered by adaptive breakpoint detector.
Then the scanned data set is segmented:(1)First, the point C is detected as the maximum distance from line AB which exceeds the threshold. Thus, the continuous data set AB is divided into two subsets of AC and CB. Then, the point D is detected as the maximum distance from line AC which exceeds the threshold and the data set AC is divided into two subsets of AD and DC. Similarly, the data set CB is divided into two subsets of CE and EB and the data set EB is divided into two subsets of EF and FB, as shown in Figure 4(a). At last, the distance between point C and line DE is detected to be less than the threshold. Thus, the subsets DC and CE are combined into one data set DE.(2)Second, the lengths of AD, DE, EF, and FB are calculated, which exceed the line threshold. Thus, the subsets AD, DE, EF, and FB are processed as line segment. Then the subsets FB are analyzed by most similar algorithm which belongs to the arc segment, as shown in Figure 4(b).
(a)
(b)
5.2. Dynamic Update
(1) Evaluation of Vehicle Pose Estimation Accuracy. The real-time pose is estimated using ASRKF and SRKF, respectively, when the vehicle travels along the desired trajectory. The statistic characteristic of process noise is unknown. The data measurement period is 25 ms.
The result of experiment is shown as in Figure 5. It is observed from Figure 5 that the errors of vehicle state ( and ) estimated by NASRUKF are significantly smaller than the errors estimated by SRUKF. After 4 seconds, as the identification of process noise statistical feature, the conclusions are more obvious. () From Figure 5(a), the estimated error of - plane calculated by ASRUKF is within 1 cm, while the estimated error calculated by SRUKF is within 5 cm. (2) From Figure 5(b), the estimated error of azimuth calculated by ASRUKF is within 1.15° (0.02 rad), while the estimated error calculated by SRUKF is almost 5.16° (0.09 rad).
(a) Results of estimation
(b) Results of estimation
(2) Evaluation of Map Creating Accuracy. When vehicle slowly moves along the line, the vehicle track and environmental map feature are dynamically updated. The test results are shown in Figure 6 and Table 1.
(a) Results of map creating
(b) Results of estimation
(c) Results of estimation
Note. Full lines in the figure (real map, real track, and real theta) denote the true value of actual state. Short dash lines (estimate map) denote the map feature estimated with the improved FastSLAM algorithm. Long dash lines (estimate track and estimate theta) denote the state estimation or estimation error using the improved FastSLAM algorithm.
As shown in Figure 6 and Table 1, the localization accuracy based on the improved FastSLAM algorithm is within 1 cm and the azimuthal error is approximately 0.5°. The position error of the corner point in the map feature is approximately 3 cm. The errors in both the center position and radius of the arc are within 2 cm. The slope error of line segment is within 0.07. The intercept error is within 2 cm. It is demonstrated by the test results that the proposed FastSLAM algorithm is effective.
6. Conclusions
An improved FastSLAM algorithm based on NASRUKF is proposed in paper. To improve the accuracy and reliability of the FastSLAM limited by measuring frequency, a new posterior probability distribution decomposition strategy is proposed. And the vehicle pose and map features are estimated using NASRUKF algorithm, which overcomes the limitations of the traditional FastSLAM algorithm (e.g., solutions to Jacobian matrix are needed; failure to meet consistency conditions and occurrence of “degeneration of particle set”). The adaptive ability of filters is improved while SLAM accuracy is maintained, which provides an effective way to solve SLAM problem. The effectiveness of the proposed FastSLAM algorithm is verified by indoor experiment results.
Conflicts of Interest
The authors declare that there is no conflict of interest regarding the publication of this paper.
Acknowledgments
The authors would like to express their great appreciation to the National Natural Science Foundation of China (no. 51307137), the doctoral scientific research foundation of XUST (no. 2016018), the Fundamental Research Funds of Xi’an University of Science & Technology (no. 201317), and the open fund project of the key laboratory for embedded system and service computing (Tongji University) of the education ministry of China (no. ESSCKF 2016-05).