Abstract
This paper proposes an innovative simultaneous localization and mapping (SLAM) algorithm which combines a strong tracking filter (STF), an unscented Kalman filter (UKF), and a particle filter (PF) to deal with the low accuracy of unscented FastSLAM (UFastSLAM). UFastSLAM lacks the capacity for online self-adaptive adjustment, and it is easily influenced by uncertain noise. The new algorithm updates each Sigma point in UFastSLAM by an adaptive algorithm and obtains optimized filter gain by the STF adjustment factor. It restrains the influence of uncertain noise and initial selection. Therefore, the state estimation would converge to the true value rapidly and the accuracy of system state estimation would be improved eventually. The results of simulations and practical tests show that strong tracking unscented FastSLAM (STUFastSLAM) has a significant improvement in accuracy and robustness.
1. Introduction
SLAM can solve a problem that a mobile robot can build a global map and estimate its own pose [1, 2] in an unknown environment. The basic principle is to use sensor information and odometer data with multifeature matching to build an environment map while determining the location of the robot in the map. However, because of the complicated external environment, unpredictable system noise, and weak sensor perception problems, the accuracy of SLAM is usually lower than expected [3].
Nowadays, many solutions are used to solve the problem of SLAM’s low accuracy. The classic SLAM algorithm is based on an extended Kalman filter (EKF) [4]. In essence, EKF is based on a first-order Taylor formula, and it can solve the problem that the Kalman filter (KF) is not suitable for nonlinear systems [5]. However, there are also some fatal flaws [6], such as its high computational complexity. Moreover, under the condition of high nonlinearity of the system, since EKF is adopted in the SLAM model, there are some problems of lower accuracy, even divergence and nonconvergence. So, based on the EKF-SLAM, some new algorithms have been proposed.
Aimed at the disadvantages of computational complexity and the erroneous data associations in EKF-SLAM, the Rao-Blackwellized particle filter was used in the SLAM algorithm by Murphy [7] and Doucet et al. [8]. After that, Montemerlo et al. improved the SLAM algorithm and proposed FastSLAM [9].
Based on the FastSLAM algorithm, Kim et al. proposed the UFastSLAM algorithm [10]. The core of the algorithm uses an unscented transform [11, 12] to estimate the transition probability density of SLAM [13]. It cancels the restriction condition of the system model. In other words, the system is not required to be linear. Meanwhile, there is no need to calculate the Jacobian matrix, so the state function and the measurement function are not required to be continuously differentiable. An unscented transform can even be applied to discontinuous systems.
UFastSLAM uses an unscented transform to estimate the robot’s pose and uses UKF instead of EKF to estimate the position of environmental features. In practical application, the presence of system noise among relevant information may influence the accuracy of UKF. Meanwhile, the main parameters and the filter gain cannot be adjusted online. It also lacks adaptive ability. In order to improve adaptability and robustness over a rough terrain, we add a strong tracking filter (STF) [14] on the basis of UFastSLAM. STF can adjust filter gain online to achieve the goal of an algorithm self-adapting. STF not only has strong model mismatch robustness but also has the advantages of clear conception and simple calculation. However, because it adds a strong tracking factor, which increases the amount of calculation, the computation time increases slightly with the same number of particles.
This paper presents a strong tracking adaptive UFastSLAM (STUFastSLAM) algorithm which is based on the suboptimal fading factor. The new algorithm updates every sigma point by using an adaptive algorithm. It gets the optimized filter gain by utilizing an ST online regulatory factor which can restrain the influence brought by uncertain noise and initial selection on state estimation. STUFastSLAM combines the adaptability of STF with the nonlinear high approximation of UKF. In addition, it also introduces a particle filter which is not influenced by non-Gaussian and nonlinear models. This algorithm has better adaptive ability, accuracy, and robustness [15]. The simulation and experiment demonstrate that the accuracy of the new algorithm is much better than other algorithms in complex environment.
2. Related Work
2.1. FastSLAM Algorithm Framework
The FastSLAM algorithm combines a particle filter with EKF and develops a new solution of localization and mapping simultaneously. FastSLAM estimates the robot’s pose and environmental features by using PF and EKF. In this way, the computational complexity of FastSLAM is turned into . Here, M is the number of particles and N is the features of environment.
FastSLAM divides the whole progress of localization and mapping into separate parts. Because the conditions of each environmental landmark are independent, it can also divide the whole mapping progress into mapping on each landmark. If the moving trajectories of the robot are known and the position of environmental landmarks is relatively independent, the joint posterior distribution can be represented as follows: where is a sequence of observed data and is a sequence of control information. Both of them are used to estimate the moving trajectories of the robot , not just its position. is the correlation variable.
In (1), PF is used to estimate the path and EKF is used to estimate landmarks. The FastSLAM method combines PF and EKF [9].
There are two versions of FastSLAM. Based on FastSLAM 1.0, the latest FastSLAM 2.0 introduces observation information FastSLAM 2.0 improves the accuracy of the algorithm in the process of localization and mapping [16]. However, FastSLAM 2.0 also has some inherent defects. Firstly, FastSLAM 2.0 uses first-order linearization during the process of new position sampling. At this point, it will bring relatively large errors if there is an equation with a high degree of nonlinearity. Secondly, FastSLAM 2.0 uses standard EKF during the process of the landmarks’ observation updating. There are some disadvantages when the FastSLAM 2.0 algorithm deals with equations with higher degrees of nonlinearity. Therefore, in order to solve these problems, some researchers have improved this algorithm under the frame of FastSLAM 2.0.
Under the frame of FastSLAM, Grisetti et al. proposed an improved FastSLAM for grid mapping with a Rao-Blackwellized particle filter [17]. This algorithm discretizes the map and uses lasers and odometers to design an important sampling function. However, the accuracy of the grid will affect FastSLAM greatly. In 2009, Arasaratna and Haykin proposed the cubature Kalman filter (CKF) [18]. CKF has a greater performance in regards to nonlinear approximation, numerical accuracy, and filter stability. However, there are some inherent drawbacks of this algorithm. For instance, it has to calculate the tedious covariance matrix square root and reconstruct the covariance matrix during the iteration of SLAM. Kang et al. proposed the neuroevolutionary optimization SLAM (Neo-SLAM) algorithm [19]. Neo-SLAM is a kind of SLAM algorithm which uses neural networks to learn the nonlinear dynamics model and noise statistics data autonomously. Neo-SLAM is insensitive to the changes of system and sensor noise compared with the other algorithms, so it can be used to reduce the robot motion errors [20]. In order to reduce the linearization error of the Kalman filter family, Yadkuri and Khosrowjerdi proposed a slam algorithm based on improved weighted mean extended Kalman filter (IWMEKF) [21].
3. Theoretical Background
3.1. Strong Tracking Filter
According to the literature [22], when the theoretical model matches the actual system exactly, the output residual sequence of the KF is nonautocorrelation or a mutually orthogonal Gaussian white noise sequence. However, EKF is a suboptimal filter. It is impossible that the residual sequence is completely uncorrelated. Therefore, as long as the EKF residual sequences are weakly autocorrelated, it can be considered that EKF has good filtering effect.
In regards to the STF online real-time adjustment of variable gain matrix , the theoretical basis (orthogonality principle) is as follows:
Condition 1
Condition 2
Condition 1 is the performance index of EKF.
Condition 2 requires that the output residual sequences in different time remain orthogonal.
When there is uncertainty in the system model, if the EKF state estimation value deviates from the true state of the system, it will inevitably show up on the mean and magnitude of the output residual sequence. In this case, if the gain matrix is adjusted online, condition 2 is forcibly established so that the residual sequences can still be orthogonal to each other and always have Gaussian white noise-like properties. Therefore, when the system model is uncertain, the STF can still keep the capability of effective tracking of status.
STF adjusts the gain matrix in real time online and forces the output residual sequence to keep orthogonality by introducing a fading factor in the state prediction covariance [22]. Therefore, STF can keep the traceability for the system state even if the system model is uncertain. The fading factor is calculated as follows [23]: where tr(·)is the operator of trace of the matrix. where is the m × n dimension measurement matrix, is the covariance matrix of control noise, is the covariance matrix of observation noise, and is the n × n dimension state-transition matrix. is the state prediction covariance.
is the covariance matrix of the actual output residual sequence, which is estimated by where is the forgetting factor; the value is set to 0.95 usually.
The derivation process is shown in the appendix.
4. Description of STUFastSLAM
STUFastSLAM is a kind of process which is based on FastSLAM 2.0 as a frame and uses UKF instead of first-order linearization and EKF. Besides, we introduce STF to solve the problem of the poor robustness of UKF when the model is uncertain. The process of STUFastSLAM algorithm is as follows [24]:
4.1. Robot State Estimation
At first, the robot’s pose, the control input, and the observation are estimated as a whole. The state matrix after augmentation is where is an augmented matrix which contains the robot’s pose, the control input, and the observation, and its dimension is 7 × 1. is an augmented covariance matrix which contains robot covariance, control noise, and observation noise, and its dimension is 7 × 7. and are the observation noise covariance and control noise covariance, respectively [25].
Step 1. 2N + 1 sigma points are extracted around the mean point deterministically. N is the dimension of the mean vector. Here . So, the set of extracted sigma points are [26] where, at this moment, each sigma point contains robot state, control input, and observation information, so that we can calculate the robot state information by the sigma point which is transformed by the nonlinear kinematic model. We can get the robot state information as follows.
Step 2. Get the prediction equation
The robot state mean and the prediction of the covariance can be calculated as follows:
In order to make the filter more robust when the system model is uncertain, the fading factor is introduced into the state prediction covariance matrix. is calculated as follows:
Here is unknown; we will calculate the value of in the latter part.
is used for calculating the weight of the mean. is used for calculating the weight of the covariance. The formulas are as follows. where . In order to obtain more accuracy on the robot’s state, observation information must be considered. We update the prediction by using the observed features. The formulas are as follows.
Utilizing and , the sigma point is calculated according to the sampling method selected by step 1. Based on the nonlinear function and (19), (20), and (21), we can get the sigma point . From , we can get the output prediction , the autocovariance , and the cross covariance .
is the sigma point which is calculated via the nonlinear observation equation. is the prediction of observation, and is the updating vector (autocovariance). Thus, the cross covariance matrix can be obtained.
, , and are known. The fading factor in (15) can be calculated by (4), (5), (6), and (7).
The fading factor λt has already been obtained. So we use and to calculate the sigma point in accordance with the sample strategy in step 1. The sigma point transforms into through the nonlinear measurement function. We can calculate the output prediction , the autocovariance , and the cross-covariance from .
Finally, the Kalman gain can be calculated as follows:
At time t, the robot state mean and the covariance estimation can be obtained as follows:
We update the set of sigma points in (10) based on (28) and (29). The new sigma points set are as follows:
4.2. Environmental Feature Position Estimation
We then have to estimate and update the observed environmental features. Similar with the robot’s pose estimation, we need to structure the sigma points set before we estimate the environmental features. As the SLAM is in 2D scene, the dimension of the environmental feature is 2. The sigma points are as follows: where is the mean of the nth feature at , and is its covariance. The other parameters can be calculated like in the previous section. Then, we can get
Here, is the sigma point set which is calculated by nonlinear observation formulas. and are the predicted mean and covariance of observation, respectively. Then, we can calculate the Kalman gain and update the mean and covariance of the environmental feature.
4.3. Resampling Strategy
In the last step, we have to make the judgment resample for the particles. The resampling method is exactly the same as that of FastSLAM 2.0.
Here,
5. STUFastSLAM Overview
According to Section 4, the STUFastSLAM algorithm is composed of two parts. One is the estimation of robot pose, and the other is the estimation of the location of environmental features. We drew flow charts for these two parts separately. The flow charts of the algorithm are as follows:
Figure 1 is the flow chart of robot state estimation. It is divided into two parts: prediction and update.
The flow chart of environmental feature position estimation is shown as Figure 2.
The explanation of the STUFastSLAM algorithm is in the pseudocode. The pseudocode of the STUFastSLAM algorithm is as follows.
| ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
The covariance , the autocovariance , and the cross covariance indicate that the fading factor has not been introduced.
The covariance , the autocovariance, and the cross-covariance indicate that the fading factor has been introduced.
6. Simulation Result
In order to verify the reliability and validity of STUFastSLAM, we designed a simulation experiment and compared the results among STUFastSLAM, FastSLAM 2.0, and UFastSLAM. The simulation experiment was carried out on the Matlab2015 platform. The SLAM simulator was published by the University of Sydney robot center [27]. We use the car-like motion model, and the observation model is the Bearing-Rang radius scan model. The wheel base is 4 m, and the robot velocity is . The effective range of sensor is 40 m, and its front view is 180°. The noise is (0,1) distributional Gaussian white noise. The process noise is (, ), and the observation noise is (, ).
Figures 3 and 4 are simulation environment graphs. The green “※”’s are the environmental features, and the “O”’s are the waypoints of the robot’s moving path. The curve is the robot’s reference path. The robot moves from the point of origin and follows the reference path for 2 laps. In this experiment, we set 35 features and 17 waypoints. The robot starts from (0,0) point and moves along the path (Figure 3). All the tests were repeated 30 times, and we obtained the average value as the statistical results.
In Figure 4, the black curve is the estimated path trajectory of the robot. The red points show the estimated landmarks. Because of odometer and observation errors, there is some deviation between the estimated and true paths and between estimated and true landmarks. In the simulation experiment, we compare the true path with the estimated path of different algorithms and compare the location deviation of the true landmarks with the estimated ones to compare the accuracy of the algorithms. The smaller the deviation, the more accurately the robot draws the map. By comparing the time of the car run the whole distance, we can compare the calculation speed of the algorithms. The shorter the time, the lower the computational complexity of the algorithm, and the simpler the calculation.
Figures 5–7 show the comparison of the simulation path of FastSLAM 2.0, UFastSLAM, and STUFastSLAM. In the beginning, the estimation accuracy of each of these 3 algorithms is high. The estimated path is basically consistent with the true path, and the deviation between them is very small. With the increase of robot walking distance, there is some deviation between the estimated path and the true path. We find that the deviation of FastSLAM 2.0 is the greatest. The accuracy of UFastSLAM is higher than that of FastSLAM 2.0 in the latter period. Due to the addition of adaptive factors, STUFastSLAM not only has greatly improved the estimation accuracy but also can keep high position estimation in the process of robot exploration. The coincidence degree of the estimated path and the true path is the highest.
Figure 8 is the comparison of the position estimation error among standard FastSLAM 2.0, UFastSLAM, and STUFastSLAM. The system noise and the observation noise are in the condition of Gauss white noise. The x-axis presents the number of steps the experiment took, and each experiment takes 17,383 steps. The first lap is 1~8691 steps, and the second lap is 8692~17,383 steps. Firstly, the estimation accuracy is significantly improved in the second lap in Figure 8. This is because when the robot runs the second lap, it matches and corrects the path of the first lap. Secondly, the accuracy of FastSLAM 2.0 is lower than that of the other two algorithms, and the maximum location error is higher than in the other two. In STUFastSLAM, the introduction of an adaptive factor causes the error in the simulation state to be well controlled. In most cases, the error of STUFastSLAM is less than those of FastSLAM 2.0 and UFastSLAM. Especially in the simulation of the second lap, the error during the overall process is in the minimum state, which is consistent with the previous theoretical analysis.
In order to verify the accuracy and stability of the three algorithms in different levels of environmental noise, we set five groups of environmental noise: (, ), (, ), (, ), and (σr = 0.4 m, σb = 8°). The particle number is set to 10, and the odometer noise is set to (, ). We use the root mean square error (RMSE) to estimate the deviation between the observation value and the true value. RMSE is defined as where is the true state of the robot and is the estimated state at time k.
From Figure 9, we can find that the RMSE of these three algorithms rise to different extents as the noise grows, which represents their accuracy decreasing. But in all the cases, the RMSE of STUFastSLAM is minimal. When the noise is , , the RMSE of UFastSLAM is very close to that of FastSLAM, and the RMSE of STUFastSLAM is significantly lower. With the increase in the noise level, the advantage of STUFastSLAM becomes more and more clear and the gap between the RMSE of STUFastSLAM and those of the other two algorithms grows. When the noise is , , the gap between the RMSEs of UFastSLAM and STUFastSLAM becomes small, but the gap with FastSLAM 2.0 is even more evident.
Figure 10 shows the computation time of these three algorithms under the same RMSE. From Figure 10, the computation time of STUFastSLAM is least, and the computation time of UFastSLAM is slightly more than that of STUFastSLAM. The computation time of FastSLAM 2.0 is far greater than those of the other two. It indicates that STUFastSLAM still has the time advantage compared with the other two algorithms under the same accuracy.
Figure 11 shows the comparison of RMSE in the case of different quantities of particles when STUFastSLAM has the same noise level. When the number of particles is less than or equal to 20, the RMSE value of this algorithm shows a linear downtrend basically. When the number of particles is greater than 20, the decrease in the RMSE value becomes gentler. It indicates that the accuracy of this algorithm is gradually improved with the increase in particle number. However, when the number of particles is greater than 20, the increase in accuracy is not obvious. Meanwhile, a great deal of computation time is also needed. Therefore, the STUFastSLAM algorithm essentially achieves its optimal level when the number of particles is 20.
Figure 12 shows the position estimation error of STUFastSLAM in the case of different numbers of particles. Figure 12 corresponds to Figure 11; when the number of particles is between 5 and 20, the estimated accuracy improves obviously as the number of particles increases. When the number of particles is 40, the accuracy is only slightly higher than that of 20 particles.
7. Experiment Base on P3DX Robot
In order to verify the actual performance of the algorithm, we simulated robot real-time localization and mapping under rough road conditions in the office.
This experimental platform adopts the Pioneer-3DX (P3DX) autonomous mobile robot which is produced by American Active Media Company, as shown in Figure 13. It is a small intelligent robot driven by two wheels with differential drive. It has many advantages, such as flexibility, stability, generality, and easy operation. P3DX is the most mature experimental platform for the wheeled mobile robots in the world.
7.1. Experiment 1
Figure 14 is a real photo of our office. The experiment will build a 2D environment map of the office in this condition. In order to verify the SLAM performance under rough road conditions, we threw some gravel and sand on the ground to simulate the condition of rough roads and introduce skidding.
In order to test the performance of the algorithm quantitatively, we used the laser rangefinder to measure the office and drew the following environment map with SolidWorks as shown in Figure 15.
We chose 5 points A, B, C, D, and E in Figure 16 as the distance control points of indoor environment which can be used to compare the accuracy of algorithms. The dimensions are as shown in Table 1:
We compared the performance with UFastSLAM and STUFastSLAM in the same test experiment conditions. We controlled the robot to walk in the office to build an office map.
Figure 17 is the environment model which was built by UFastSLAM. Figure 17 is the somewhat distorted map due to the robot’s wheels skidding. The map is too vague to show the real environment of the office.
Figure 18 is the environment model which was built by STUFastSLAM. As the reader can see in Figure 18, the map is much clearer than in Figure 17. In this map, the corners can be seen clearly and the wall is vertical. The entire map is seen in much sharper detail.
In order to quantify the gap between the two algorithms, we built the office map 10 times to obtain the average dimensions of the control points as follows:
Table 2 shows the average dimensions of the control points obtained by STUFastSLAM.
Table 3 shows the average dimensions of the control points obtained by UFastSLAM.
The resulting analysis table is shown below:
From Figure 19, we can see that in the same conditions, the average error of STUFastSLAM is much lower than the one of UFastSLAM when mapping an indoor environment.
Figure 20 shows the RMSE of different algorithms in a real environment. We can see that the RMSE of STUFastSLAM is much lower than that of UFastSLAM.
7.2. Experiment 2
In this experiment, we let the robot go from point A, through gate B, walk in a 13.36 m long, 4.77 m wide hut, and then walk out of gate C and enter a 2.26 m wide corridor and return to the starting point (as shown in Figure 21). The road was covered with gravel to simulate the situation of the robot walking on harsh road conditions. The accuracy of the drawn map is judged by comparing the map size measured by the slam algorithm with the actual layout of the building.
We used the STUFastSLAM algorithm and the UFastSLAM algorithm to create maps and compare the two maps.
In Figure 22, the red map is drawn by the STUFastSLAM algorithm, and the gray map is drawn by the UFastSLAM algorithm. As can be seen from the figure, the red map is obviously more regular than the gray map and can reflect various indoor details not mapped by UFastSLAM. In order to quantitatively compare the accuracy of the two algorithms, the distances between the points in the map are measured and compared with the actual measurements of the building. We got Figure 23.
From Figure 23, the error of STUFastSLAM is far lower than that of UFastSLAM in terms of maximum error, minimum error, average error, and standard deviation.
Figure 24 shows the RMSE of different algorithms in a real environment. We can see that the RMSE of STUFastSLAM is much lower than that of UFastSLAM. The RMSE value is slightly different from that of Figure 20 due to the differences in the road conditions and the indoor space. It still, however, certifies that the new algorithm can obtain better performance in practical applications.
To verify the effect of the number of particles on map accuracy, we set the number of particles in the STUFastSLAM algorithm to be 10, 20, and 40 particles, respectively.
As can be seen from Figure 25, as the number of particles increases, the quality of the map drawn by the robot progressively improves. We measured the size of the three maps and compared them with the actual maps, as shown in Figure 26.
Figure 26 and Figure 11 provide confirmation for one another. As the number of particles increases, the RMSE value of the map decreases, and thus the accuracy of the map is increased.
8. Conclusions
This article puts forth a solution for the problems of rescue robots’ poor mapping accuracy and mapping maps easily diverging in complex terrain environments in the form of the STUFastSLAM algorithm. This article is based on the framework of UFastSLAM and innovatively introduces the strong tracking adaptive factor. For each sigma point in the UFastSLAM algorithm, adaptive algorithms are used to update them in real time. Optimized filter gain is obtained through the STF online adjustment factor, and the influence of uncertain noise and initial value selection on the state estimation is suppressed. By this, system state estimation accuracy is increased. The robustness of the system is enhanced, so the new algorithm is more suitable for environments with harsh terrain. In addition, we carried out simulation experiments to verify the effectiveness of the algorithm. The experimental results show that the accuracy and stability of STUFastSLAM are better than those of the other two algorithms under different noise conditions. Even under the same accuracy, it has less computation time than the other two algorithms have. Finally, in the office, we simulated robot real-time localization and mapping under rough road conditions. The results show that the map drawn by the rescue robot much better reflects the surrounding environment, and the map has high accuracy and good reliability.
Through a series of simulations and experiments, the performance of STUFastSLAM is verified. The algorithm is superior to other similar algorithms in accuracy and reliability. It can successfully accomplish the task of mapping in complex terrain environments.
Appendix
Detailed Derivation and Calculation of Suboptimal Fading Factor
Given the nonlinear model where is the state parameter of robot, is the observation of environmental feature, is the nonlinear kinematic model, and is the environment observation model. is the control action for the robot in the discrete time [, ], is the control noise and its covariance matrix is Q, and is the observation noise of sensor and its covariance matrix is R. and represent unrelated white Gaussian noise.
The recurrence formula of the strong tracking filter with fading factor is shown: where is the suboptimal fading factor. is the estimate, is the prediction error covariance matrix, is the prediction of measurement value, K is Kalman gain, and k is the kth time. is the n × n dimension state-transition matrix, and H is the m × n dimension measurement matrix. The theoretical output residual sequence is assumed as ; the suboptimal fading factor is then calculated as follows: where tr(·) is the operator of trace of the matrix. represents the state prediction covariance without introducing fading factors, which is calculated by
is the covariance matrix of the actual output residual sequence, which is estimated by
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work is financially supported by the State Key Research Development Program of China (Grant no. 2016YFC0801404), the National Science and Technology Major Project of China (Grant no. 2016ZX05043005), and the National Natural Science Foundation of China (51674050), which are gratefully acknowledged.