Abstract
Data association is one of the key problems in the SLAM community. Several data association failures may cause the SLAM results to be divergent. Data association performance in SLAM is affected by both data association methods and sensor information. Two measures of handling sensor information are introduced herein to enhance data association performance in SLAM. For the first measure, truncating strategy of limited features, instead of all matched features, is used for observation update. These features are selected according to an information variable. This truncating strategy is used to lower the effect of false matched features. For the other measure, a special rejecting mechanism is designed to reject suspected observations. When the predicted robot pose is obviously different from the updated robot pose, all observed sensor information at this moment is discarded. The rejecting mechanism aims at eliminating accidental sensor information. Experimental results indicate that the introduced measures perform well in improving the stability of data association in SLAM. These measures are of extraordinary value for real SLAM applications.
1. Introduction
The simultaneous localization and map building (SLAM) problem asks if it is possible for an autonomous vehicle to start in an unknown location in an unknown environment and then to incrementally build a map of this environment while simultaneously using this map to compute absolute vehicle location [1]. The SLAM solution has been seen as the core of the mobile robotics community in the past two decades as it could provide methods to make a robot to be truly autonomous.
SLAM data association is often mentioned as “registration.” It means that when an observation from a sensor is used to estimate the movement of a robot or to update existing data in a map, it must be clear whether the observation corresponds to the existing data in the map or not. SLAM data association attracts the attention of many researchers all over the world. Some data association solutions have been proposed in the past decade. As for different sensors, the data association method differs to some extent. A laser range finder and a camera are the two common sensors in SLAM. In the laser range finder, SLAM data association can be divided into two categories: feature-based and scan-based approaches. As for the camera sensor, SLAM data association can be divided into three categories: feature-based methods, appearance-based methods, and hybrid methods.
When the feature-based approach is concerned, some well-known data association algorithms in SLAM include NN (Nearest Neighbor) [1, 2], JCBB (Joint Compatibility Branch and Bound) [3], MHT (Multihypothesis Tracker) [4], MDA (Multidimensional Assignment) [5], DJNN (Dynamic Joint Nearest Neighbor) [6], and HOHCT (highest order hypothesis compatibility test) [7]. In addition, Bacca et al. [8, 9] propose a system for long-term SLAM based on the feature stability histogram (FSH) model which is inspired by the human memory model. With a camera or other vision sensors, Scaramuzza [10] used single feature correspondence for motion estimation. He provided 1-point RANSAC and histogram voting to remove outliers. These data association methods aim to improve the performance of the data association process.
The scan-based approach is another important data association method in SLAM. It associates the adjacent scans with ICP [11] or ICP’s variants [12, 13]. The feature-based approach could be used as a remedy for the scan-based approach when one intends to obtain a map with abundant and accurate information.
Besides a laser range finder and optical camera, some other sensors could also be used in SLAM. For example, Magnabosco and Breckon [14] use a thermal sensor to solve the SLAM problem.
With the study results listed above, it seems that data association in SLAM is solved successfully. However, experiences of solving the SLAM problem have shown that a small quantity of data association failures may cause the SLAM algorithm to diverge. And no data association algorithm can ensure full accuracy and stability. The reason is that SLAM is for unknown environments, which is of severe uncertainty.
The above data association methods are useful in the data association process. But the data association performance in SLAM is affected by both data association methods and sensor information. Two measures of handling sensor information are put forward to enhance data association performance in SLAM. One is the truncating strategy of limited features, which is applied before the observation update step. The other is the rejecting mechanism, which is applied after the data association step. In short, two measures presented herein are not for the data association process but, respectively, for the prior step of observation update and the posteriori step of the data association process. The truncating strategy is suitable for feature-based data association approach, while the rejecting mechanism is compatible with general SLAM data association algorithms, such as feature-based approach and scan-based approach. With the two presented measures, observation uncertainty from the sensors and environment is reduced dramatically.
2. Process of SLAM
SLAM includes localization and mapping. The localization involves time update (prediction) and observation update. The mapping differs according to the type of map. Feature-based SLAM approach is emphasized herein. As for feature-based SLAM, the map is made up of many features. When the quantity of features is large, the computational burden is very difficult. In order to improve the computational efficiency, the “FastSLAM” [15] framework is adopted. The detailed process of SLAM is as follows.
2.1. Prediction
The robot pose is predicted incrementally at each time instance. Estimate of the robot pose includes its mean and covariance. The predicted mean of the robot pose at time is formulated as where and represent the estimated mean at time and controlling inputs at time .
The covariance of prediction at time is formulated as where is the Jacobian matrix of evaluated at the estimate . is the prediction noise.
2.2. Feature Extraction
Feature-based SLAM approach needs a feature extraction step before data association. It is very important to extract and choose reliable features contained in the sensor information. Then, data association methods like NN can be used to associate the extracted features with the existing features of the map. The laser range finder is a normal sensor used for collecting data in SLAM. It is very important to extract and choose correct features contained in the laser scan. The coordinate value is a basic characteristic for each feature. Except for this, the shape and size of a feature could be defined.
2.3. Data Association
DJNN [6] is adopted herein to match the extracted features with the existing features in the map. DJNN is a variant of NN (Nearest Neighbor). NN [1, 2] has two key ideas. One is the confirmation rule formulated as (3). It is used as the criteria of testing compatibility between the observed features and the existing features. The other is the criteria of selecting the nearest neighbor. It means that the observed feature with the shortest Mahalanobis distance is selected as the matching feature of an existing feature in the map.
The Mahalanobis distance between the th observed feature and the jth existing feature is described as follows: where and , respectively, represent the deviation and covariance between the th observed feature and the th existing feature.
Assuming that the deviation is a Gaussian distribution, meets the distribution. should meet the following formula: where . equals 2 normally. is the confidence value, which is usually set as 0.95.
If there are several existing features compatible with formula (4), the feature with the smallest regular distance is selected as the matching feature of the observed feature. The regular distance is formulated as the following equation:
“Nearest Neighbor” (NN) is an easy and applicable data association algorithm in SLAM. However, the accuracy of NN is susceptible to environments. In DJNN [6], two improvements are introduced to enhance its robustness. One is eliminating the interference among multiple matched features with the correlation of all observations. The other is dynamically filtrating spurious features with the help of continuous association results.
DJNN is practical for real applications. However, it cannot ensure full reliability. In this paper, we set some constraint conditions to improve the robustness of DJNN in practical applications. The truncating strategy is introduced into the data association step, aiming at lowering the effect of false matched features. When more than three features are matched in the data association process, only three features are used to update the robot pose and map. The three features are selected according to an information variable, which is formulated as the following equation: where is the information variable for the th feature. represents the measuring state of the th feature at time . are the data association results at time . And represent map features within the measuring range of a sensor. When the th feature is out of the measuring range of the sensor at time , is assigned the value “0.” If the th feature is in the measuring range of the sensor at time , is assigned the value “1” in the case that the th feature is observed by the sensor, and is assigned the value “−1” in the case that the th feature is not observed by the sensor.
Information variable integrates the data association results of all data frames. It plays the role of a dynamic record table. It records the data association results continuously. With the help of the information variable, the data association process becomes a dynamic and continuous process, rather than a static and intermittent process. With the information variable, pseudofeatures are filtrated dramatically. The computational efficiency and the association accuracy are also improved.
Up to three observed features are adopted to update the estimation of the robot pose. This truncating strategy filters unreliable features further. It helps improve the robustness and stability of the whole SLAM process. Experiments in Section 4 show that three features, instead of more than three features, are enough for the estimation accuracy of the robot pose.
2.4. Pose Update
Mean and covariance of robot pose at time are estimated with a particle filter. The robot pose estimate of every particle is updated using an extended Kalman filter (EKF) [16].
2.5. Map Update
Assuming that features in the map are mutually independent, the map could be formulated as where represents the th feature. represents the quantity of features in the map. And the map is factored into features as follows: where and are the mean and covariance of , respectively. They are updated recursively with an extended Kalman filter (EKF) [16].
2.6. Evaluation
Data association results should be evaluated with some constraints. If the data association results are not compatible with the constraints, the results are considered to be untrustworthy. The data association results should then be corrected or discarded.
We evaluate the data association results with a rejecting mechanism, which is described in the next chapter. The rejection coefficient is computed with (10). If is bigger than the threshold value, data association results are rejected. Then, both and are discarded. The SLAM process is temporarily interrupted at time . is estimated with and as follows: And the predicted robot pose is regarded as the estimated robot pose at time . This means that when any failure is found in the sensor data, obtained information should be discarded and the robot pose is estimated temporarily with the next frame of sensor observation.
After is computed with (9), the prediction step is processed again. Then, the prediction time is prolonged. If prediction step can be fulfilled before observation is processed, the pose update and map update at time will not be affected. This requires the computational efficiency of the SLAM solution to be adequate.
3. Two Enhancing Measures for Data Association in SLAM
For one thing, only a limited number of matched features are used for pose update. If many features are matched in the data association step, up to three features are accepted for pose update. The selected features should have high confidentiality. We select observed features according to the information variable, which is formulated as (6). The value of the information variable for every feature is an integer. The value of a feature’s information variable is bigger, and the feature’s confidentiality is better. This means that the selected three features should be the top three features in the value of the information variable.
On the other hand, the special rejecting mechanism is designed to get rid of suspected observations. Some obtained observation may be wrong without clear cause. When the observed information and data association results do not meet the rejecting mechanism, they are thrown away to avoid potential mistakes. However, when the rejecting mechanism is triggered continuously, we handled the measured data with different methods. When the rejecting mechanism is triggered for the first time, the observation is regarded as a random error. And the observed data is discarded. However, when the rejecting mechanism is triggered for the second consecutive time, the observed data should not be discarded directly. The observation is regarded as a systematic error. There are two reasons for the consecutive trigger. One reason is that the SLAM system is wrong. If this is the case, the SLAM system should be checked and adjusted. The other reason is that some measured data is flawed. In general, measured data of the inertial sensor is of low confidentiality, the reason being that the inertial sensor used for prediction is normally of low precision. Thus, the laser range finder or some other high precision sensor is used to correct the prediction error in the update step.
Parameters of the robot pose include the position coordinates and the bearing. The bearing error is much more serious than the coordinate error in a long period of SLAM. Thus, we define the robot bearing error as the rejecting coefficient, which is formulated as the following equation: where is the rejecting coefficient, is the updated bearing of the robot at time , and is the predicted bearing of the robot at time . should be less than the threshold value, which is set to be 3° herein. The threshold value of the rejecting coefficient is determined with the precision of sensors and the applied environment. Detailed reasons are explained at the end of Section 4.
The position error sourced from bearing error can be formulated as follows: where and represent the bearing error and the position error, respectively. is the linear displacement. In the SLAM process, the robot is athletic. When the robot moves, in (11) increases accordingly. Thus, increases gradually if is not corrected.
4. Experiments
4.1. Experimental Models
The intelligent vehicle, shown in Figure 1, of the ACFR (Australian Centre for Field Robotics) [17] is used as the experimental mobile robot.
Experimental datasets are collected with a laser range finder, a GPS, and an inertial sensor. The laser range finder is used to detect the distance and bearing of obstacles. The inertial sensor is used to measure the intelligent vehicle’s speed and steering direction. The GPS is used to provide position information of the robot. In the experiments, circular objects like tree trunks are treated as the standard features of a map.
The range precision and bearing precision of the laser range finder are 0.1 meter and 1*π/180 rad, respectively. The precision of an inertial sensor is easy to change in different environments. When the inertial sensor is static, its precision is high. However, when the inertial sensor is mounted on a dynamic robot or vehicle, its precision is low. This is especially serious for the fluctuant topography. In general, the bearing precision of an inertial sensor should be worse than 3*π/180 rad. Measuring precision of an inertial sensor is remarkably lower than that of a laser range finder.
4.1.1. Pose Prediction
The structure of the intelligent vehicle is shown in Figure 2. The vehicle’s pose is described as where , , and , respectively, represent the -coordinate, -coordinate, and direction of the intelligent vehicle at time .
The robot pose is predicted withwhere is the velocity of the intelligent vehicle’s back axle. is the steering angle of the intelligent vehicle. is the prediction noise.
4.1.2. Observation Model
The observation model is formulated as follows: where and are position coordinates of the th feature and the intelligent vehicle at time . is the distance between the vehicle and the th feature at time , while is the th feature’s bearing at time . and are measured with a laser range finder. represents the observation noise.
4.2. Experiments with “Victoria Park Dataset”
“Victoria Park Dataset” [17] is used to evaluate the performance of enhancing methods for data association in SLAM. Experimental results are shown as Figures 3 and 4.
(a) Robot path estimated with the inertial sensor
![(b) SLAM results of data association approach in [15]](https://static.hindawi.com/articles/js/volume-2014/326820/figures/326820.fig.003b.jpg)
(b) SLAM results of data association approach in [15]
(c) SLAM results with the enhancing measures in this paper
(a) SLAM results without the two presented measures
(b) SLAM results with the rejecting mechanism
(c) SLAM results with the rejecting mechanism and the truncating strategy
In Figure 3, the solid line shows the estimated path of the intelligent vehicle and the dotted line represents the intelligent vehicle’s route measured with GPS. And the dots in Figures 3(b) and 3(c) represent the estimated position of features in the map. In Figure 3(a), the solid line is the intelligent vehicle’s path estimated with the inertial sensor’s measurements. It can be seen in Figure 3(a) that the estimated path diverges significantly. Figure 3(b) shows the SLAM results of the data association approach in [15]. And Figure 3(c) indicates the SLAM results with the enhancing methods of data association. In Figure 3(c), the estimated path nearly coincides with the GPS records. It is clear that the estimation accuracy in Figure 3(c) is better than that in Figure 3(b). All in all, it can be concluded from Figure 3 that the methods introduced are effective to enhance the estimation accuracy and stability of data association in SLAM.
As for the SLAM results in Figure 3(c), the threshold value of the rejecting coefficient is set to be 3° (3* π/180 rad). The threshold value of the rejecting coefficient is determined according to the precision of sensors and the applied environment. Measured data of the “Victoria Park Dataset” are from two sensors: an inertial sensor and a laser range finder. Data measured with the inertial sensor is used in the prediction step, while data measured with the laser range finder is used for pose update and map update. Taking into account the fact that measuring precision of an inertial sensor is markedly lower than that of a laser range finder, when the rejecting mechanism is triggered successively, data measured with the laser range finder is used to correct the prediction error caused by data measured with the inertial sensor. However, the inertial sensor is necessary for the prediction step. It provides some important parameters, such as velocity and the steering angle of the intelligent robot.
In order to clearly show the performance of the enhancing methods introduced for SLAM data association, we have done some experiments as shown in Figure 4. They are the SLAM results of three cases. Neither enhancing measure is adopted in Figure 4(a). The rejecting mechanism is applied in Figure 4(b). Both the rejecting mechanism and the truncating strategy of limited features are adopted in Figure 4(c). It can be seen from Figure 4(a) that the SLAM process is not stable. In the left sector of the park, the estimated robot path conflicts seriously with the GPS records. When the rejecting mechanism is adopted, the pose error is alleviated greatly. When angle deviation exceeds the rejecting coefficient, the rejecting mechanism is triggered. Then, measured data triggering the rejecting mechanism are discarded. In experiments shown as Figure 4(b), the rejecting mechanism was triggered eight times. Positions of triggering the rejecting mechanism are marked with large dots in Figure 4(b). When the two enhancing measures are adopted, the estimated pose coincides well with the GPS records in Figure 4(c).
In short, it can be concluded that the two measures introduced are effective to improve the stability of the feature-based data association approach in SLAM.
5. Conclusions
Data association is of key importance in the SLAM process. However, no data association algorithm is fully trustworthy in all SLAM cases, the reason being that SLAM is for unknown environments, which are of severe uncertainty. Some improving methods for sensor information are introduced herein to enhance the data association performance in SLAM. On one hand, only a limited number of matched features are used for observation update. On the other hand, a special rejecting mechanism is designed to reject doubtful observations. Experimental results indicate that the introduced measures perform well in improving the stability of the data association process. Thus, these enhancing methods for data association are of excellent value for SLAM applications.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
This paper is financially supported by the National Natural Science Foundation of China (no. 51405450, no. 51305406) and the Educational Commission of Zhejiang Province of China (no. Y201431736). The authors would like to thank Australian Centre for Field Robotics (ACFR) of the University of Sydney and relevant members of ACFR for the Victoria Park Dataset.