Abstract
In Laser-SLAM system of unmanned delivery vehicle, there are two kinds of association methods applied to solve the data association problem. Compared with the method of independent association for a single measurement and a single feature, the methods of batch association of measurements and features can provide more accurate association results in the state estimation stage of SLAM. In order to obtain a better association solution, a joint data association method based on heuristic search algorithm (HSA-JDA) is proposed to improve the robustness and accuracy of data association. In HSA-JDA, according to the joint maximum likelihood criterion, the data association problem is evolved into a combinatorial optimization problem of how to determine the optimal association set. A heuristic search algorithm that is an optimized artificial fish swarm algorithm by using adaptive step size and adding fish swarm jumping behavior is applied to search the optimal association solution. Experimental results show that HSA-JDA method ensures high association accuracy and then improves the robustness and accuracy of the whole state estimation results of SLAM. It can be used in the Laser-SLAM system based on Kalman filter to provide reliable association results for improving the accuracy of SLAM estimation results for unmanned delivery vehicle.
1. Introduction
As the terminal of the ecological chain of intelligent logistics system, unmanned delivery vehicle needs to interact with people, vehicles, and buildings, and it should have fast and effective task adaptation ability. Therefore, in order that unmanned delivery vehicle can be widely applied in industrial parks, campuses, communities, and urban environments, it should have a high-precision intelligent navigation system. In order to build an intelligent navigation system of unmanned delivery vehicle in outdoor unknown environment, the vehicle needs to use external sensors to determine its own position and build a high-precision map required by path planning. However, an accurate environmental map must be used to achieve precise positioning of the vehicle, and the precise positioning information of the vehicle must be known to establish a high-precision environmental map. In response to this “chicken and egg” problem, simultaneous localization and mapping (SLAM) technology is proposed [1, 2].
The implementation method and difficulty of SLAM are closely related to the type and installation mode of sensors, that is, the measurement of the sensor determines the SLAM solutions. Relatively few research works of SLAM solutions are based on some sensors such as ultrawideband (UWB) [3] or active radio range sensor [4], which all provide range-only information [5]. The most widely used measurements are from visual sensor and laser range sensor in SLAM solutions. The SLAM technology based on laser is abbreviated as Laser-SLAM, and the SLAM technology based on visual sensor is abbreviated as VSLAM [6]. Usually, the map accuracy constructed by Laser-SLAM is higher than that of VSLAM, and it can be directly used for positioning and navigation of vehicle in outdoor unknown environment. SLAM contains two important modules: data association and state estimation [7–9]. SLAM state estimation includes vehicle state estimation and the position estimation of environment landmarks. Despite Laser-SLAM or VSLAM, accurate data association is the premise of correct state estimation [10]. In this article, the Laser-SLAM association problem is mainly studied.
The methods of solving the Laser-SLAM association problem can be divided into two categories. One is the method of independent association for a single measurement and a single feature, such as individual compatibility nearest neighbor (ICNN) method and maximum likelihood (ML) association method. The other is the method of batch association of measurements and features, such as sequential compatibility nearest neighbor (SCNN) method [11], joint maximum likelihood (JML) association method, multihypothesis tracker (MHT) method [12], and joint compatibility branch and bound (JCBB) method [13, 14]. Compared with other data association methods, JCBB has better robustness of association process and can obtain more accurate association results. The search process of the optimal association solution based on the branch and bound method is a heuristic search process for the interpretation tree of single frame measurement. The computational complexity of this process is lower than that of many breadth-first searching algorithms such as multihypothesis data association algorithm and multidimensional assignment association algorithm.
From the perspective of algorithm complexity and optimality, many scholars are committed to studying SLAM data association algorithm based on heuristic search strategy. Feng et al. [15] proposed a heuristic graph search data association algorithm based on dynamic threshold, which corrected the error association results based on backtracking mechanism, and used the threshold filtering method based on dynamic threshold to reduce the space of association solution. An optimized joint SLAM data association algorithm is designed to shorten the solution time of the associated optimal solution and improve the matching degree of map features and measurements in [16]. Wang et al. used chaos ant colony algorithm and artificial fish swarm algorithm (AFSA) to find the optimal association solution of UAV SLAM in [17, 18].
In this work, according to the joint maximum likelihood (JML) criterion, a joint data association method based on heuristic search algorithm is proposed, which is abbreviated as HSA-JDA method, and apply it to the Laser-SLAM system based on adaptive fading extended Kalman filter (AFEKF). Among them, AFEKF replaces the traditional EKF to estimate and update the vehicle state and environment map. HSA-JDA method has two characteristics: (i)According to the joint maximum likelihood criterion, the SLAM data association problem is transformed into a combinatorial optimization problem of how to determine the optimal association set. The goal is to maximize the product of each association possibility in the set or minimize the sum of normalized distances(ii)A heuristic search algorithm that is an optimized AFSA is designed to heuristic search the association hypothesis set to obtain the association optimal solution. In optimized AFSA, the adaptive step size is applied, and the jump behavior of fish swarm is added to speed up the convergence of the search algorithm and optimize the accuracy of the search algorithm
In short, this work has three main contributions: (i)HSA-JDA method is proposed to obtain stronger robustness of SLAM association process and more accurate association results(ii)HSA-JDA method is integrated into the Laser-SLAM system and gives a higher accuracy of SLAM state estimation(iii)The proposed association method is compared and analyzed with DFJCBB association method [19], JCBB association method, and SCNN association method based on SLAM simulator [20]. Experimental results prove that the HSA-JDA method can have a better performance in data association and obtain more reliable results of localization and mapping
In the remainder of this paper, firstly, the Laser-SLAM system framework is introduced, and the data association problem in SLAM is described. Secondly, we introduce the HSA-JDA method in detail. Then, the effectiveness of HSA-JDA method is validated by the recognized SLAM algorithm simulator. At the end of this paper, we give the conclusion.
2. Laser-SLAM System Framework and Data Association
Compared with VSLAM, Laser-SLAM is more suitable for real-time simultaneous localization and mapping of unmanned delivery vehicle in outdoor large-scale scenes. In Laser-SLAM, the unmanned delivery vehicle can obtain environmental information with laser range sensor and then to build a map of the environment step by step. Secondly, the data association method is used to determine the association relationships between the sensor measurement and the environmental features that have been marked in the created map. Finally, the SLAM estimation algorithm is used to estimate the vehicle’s pose and the environmental landmarks’ position.
2.1. Laser-SLAM System Framework
In the Laser-SLAM system, the main input parameters are control information of the vehicle and measurement information of laser range sensor [21]. The control information includes vehicle speed and steering angle. Measurement information includes angle and distance between environmental landmarks and vehicle. The output information of the Laser-SLAM system includes vehicle trajectory and environment map. From the perspective of probability, SLAM is the problem about how to get the joint posterior probability of vehicle’s pose and environmental map at the next moment based on vehicle’s pose, measurements, and control input information at the previous moment. The joint posterior probability is expressed as where is the current pose of the vehicle and is the map. is the set of all measurements , and is the sensor measurement at time . can be represented by the set . is a control vector containing vehicle speed and steering angle. stands for associative set , and is data association of measurement at time . Based on the Bayes formula, the posterior probability can be calculated by [22]
The basic framework of Laser-SLAM system is shown in Figure 1.
2.2. Data Association
In Laser-SLAM, there are three possibilities for the features observed by laser range sensor at each time: (1) it is the same physical entity as a road landmark in the previously constructed environmental map; (2) a new environmental landmark; (3) a false signal caused by the noise of the sensor itself. It is assumed that features already exist in the constructed map. The geometry of features can be expressed as . At time , measurements were observed by laser range sensor. The geometry of measurements can be expressed as . The association hypothesis is defined as . where means that the -th measurement and the -th map feature are paired successfully. If , it indicates that the laser range sensor has observed new environmental landmark, or the measurement is a noise signal. The optimal association set can be expressed as follows [23]: where is generally calculated by association method. is the pose of vehicle. represents a map constructed with the help of external sensors installed on the vehicle.
At present, there are two models to describe SLAM data association: incidence matrix model and interpretation tree model. Because the incidence matrix model cannot directly display the association solution space, the interpretation tree model is applied to describe the SLAM association problem in this work. The interpretation tree model uses the interpretation tree to represent the association relationship between the sensor measurement and the environmental features in the constructed map. Its structure is shown in Figure 2.
In Figure 2, O1, O2, and O3 represent a frame of environmental information detected by the sensor at the current time, F1, F2, F3, and F4 represent four features in the constructed environmental map, and the whole interpretation tree intuitively and clearly describes the association relationship between three measurements and four features (due to limited space, the paths corresponding to all association assumptions are not drawn). The root node of the tree is an empty node, each layer of the tree corresponds to a measurement value, each node in this layer represents a corresponding association hypothesis, and a path from the root node to the leaf node represents a possible association hypothesis set corresponding to the measurement set at the current time. For example, the thick line shown in Figure 2 represents a possible association hypothesis: .
3. Proposed Association Method
The principles of JML association algorithm and JCBB association algorithm are similar. It is a method of batch correlation between measurements and map features. The advantage of this method is that it considers the overall maximum possibility of the association hypothesis set and can effectively reduce the data association ambiguity. However, the disadvantage is that in order to obtain the global optimal association solution, it must be based on a reliable search algorithm. Therefore, a joint data association algorithm based on heuristic search algorithm is proposed, abbreviated as HSA-JDA method.
3.1. Evolution of Data Association Problem
In HSA-JDA method, according to the joint maximum likelihood criterion, the SLAM data association problem is evolved into a combinatorial optimization problem of how to determine the optimal association set. The goal is to maximize the product of each association possibility in the set or minimize the sum of normalized distances.
ML algorithm is a basic method to solve data association based on maximum likelihood criterion. Usually, the “possibility” associated with a single measurement feature and a single map feature is calculated first, and then a set of association pairs with the greatest possibility are selected as the best association matching pair. The above “possibility” is expressed by the following formula. where represents the association between measurements and map features , represents the innovation vector, and represents the covariance matrix of the innovation vector. represents the dimension of . The following formula is obtained by taking logarithms on both sides of the above Formula (5). where represents the gauge distance. If is the smallest, it represents that and can form an optimal matching pair. Formulas (8) and (9) are defined as the ML criterion. The JML algorithm considers the compatibility of all data association matching based on the ML algorithm. The corresponding optimal association hypothesis set, when the product of each association possibility in the association set is the largest, can be obtained according to Formula (10), or the corresponding optimal association hypothesis set when the sum of gauge distances is the smallest can be obtained according to Formula (11).
The mathematical model of data association problem is established according to the JML criterion. The SLAM data association problem is evolved into a combinatorial optimization problem. The specific process is as follows: (i)On the basis of knowing vehicle’s pose and the effective measurement range of the laser range sensor, the local association region can be set withwhere is the vehicle’s pose. is the landmark’s coordinate in the established map. represents the effective scanning distance of laser range sensor. indicates the compensation distance. (ii)The individual compatibility (IC) criterion is used to test measurement feature pairs, and the association pairs that meet the criterion are combined into a new set as the candidate association set. The IC criterion is expressed as follows:where is the innovation determined by the predictors of and and represents the innovation covariance. represents the Mahalanobis distance between and . Generally, for innovations subject to Gaussian distribution, obeys a chi-squared distribution with degree of freedom and confidence level . In general, and the chi-squared values corresponding to the different confidence levels are shown in Table 1.
Assuming that the association set of measurement is empty according to IC criterion, then, whether this part of the measurement is a new measurement is determined based on the augmented threshold. If so, it is represented by the set ; otherwise, it is directly eliminated. The remaining measurements are represented by set , where . The map feature sets that may be associated with the remaining measurements are represented by , and their dimensions are , respectively. For example, the map feature set associated with the first measurement in set is , then , the map feature set corresponding to the second measurement in set is , then , other correspondences, and so on. (iii)The normalized distance between each measurement in and each map feature in set is calculated according to the ML criterion. Then, the objective function of SLAM association problem can be expressed by Formula (11).where represents the association hypothesis set corresponding to the minimum objective function at time .
3.2. Optimized Heuristic Search Algorithm
As a heuristic search algorithm, the artificial fish swarm algorithm (AFSA) has some characteristics that genetic algorithm and particle swarm optimization algorithm do not have, such as flexible use, easy implementation, insensitive to the selection of various parameters, and fast convergence speed.
The traditional AFSA simulates the four behaviors of fish swarm by constructing artificial fish, which are prey behavior, swarm behavior, follow behavior, and random behavior. Based on four behaviors, the optimization process of fish in virtual water area is completed. It is worth noting that in the optimization process, performing different fish school behaviors can solve different optimization problems. Four fish behaviors are described as (i)Prey Behavior. When fish find food, they move in the direction of more food.(ii)Swarm Behavior. When fish are swimming, they will flock together in groups involuntarily. The first purpose of this behavior is to make them avoid danger and improve their survivability. The second purpose is to avoid overcrowding with nearby partners.(iii)Follow Behavior. If one or more fish find food in a school of fish, their nearby partners will quickly follow them to find that place with food.(iv)Random Behavior. Fish swim randomly in their field of vision in order to find food points or partners in a wider range. The default state of this behavior is the prey behavior.
The traditional AFSA has great blindness in the later stage of operation and easy to encounter local optimization; moreover, the convergence speed is slow in the later stage of solving the optimal solution [24]. In order to better search the optimal association solution, the jump behavior of fish swarm is introduced into the traditional AFSA. The purpose is that when the optimal value recorded on the bulletin board changes little for many times, some artificial fish can be selected randomly to start the optimization again. In addition, adaptive step size is introduced to improve the convergence speed of AFSA.
The specific improvement steps for AFSA are as follows: (i)When the fish swarm algorithm iterates times, the difference on the bulletin board each time is less than the preset threshold , and this phenomenon shows that the value recorded on the bulletin board has not changed significantly, and the algorithm may fall into local optimization. At this time, some artificial fish are randomly selected according to the probability () for jumping behavior, that is, the selected artificial fish is initialized, and the optimization is restartedwhere represents the optimal value recorded on the bulletin board after the -th iteration. (ii)The selection of step size AFSA directly determines the convergence speed and solution accuracy of the algorithm. If a large moving step is selected in the iterative process of the algorithm, the convergence speed will be faster for individuals far from the optimal value. On the contrary, for individuals close to the optimal value, selecting a smaller moving step will avoid oscillation and improve the solution accuracy. Therefore, the step size is defined according to Formula (16) in optimized AFSA, and the step size can be adjusted adaptively on the basis of the state of artificial fish sought each time. Thus, the convergence speed of the algorithm is accelerated and the optimization accuracy is improvedwhere represents the basic step size. represents the adaptive step size. represents the objective function value corresponding to the searched of new artificial fish state. represents the objective function value corresponding to the current state of artificial fish.
The optimized AFSA by using adaptive step size and adding fish swarm jumping behavior can ensure the search quality and efficiency of association interpretation tree.
3.3. Obtaining Optimal Association Solution Set
When finding the optimal association solution of Laser-SLAM, the optimized heuristic search algorithm is used to search the association hypothesis set [25].
Step 1. The artificial fish swarm model is established, and the parameters of fish swarm algorithm are set. The position of each fish in the initial fish school is initialized, and the initial position of each artificial fish is randomly assigned association hypothesis pairs. Therefore, each artificial fish can be quantified as a matrix . The objective function of the data association problem is used as the fitness function of the individual artificial fish. Based on this function, the food concentration of the artificial fish is calculated, that is, the sum of the standard distances corresponding to the current state of the artificial fish is calculated.
Step 2. By evaluating the current status of each artificial fish, any of the three basic fish behaviors can be selected in addition to random behavior.
Step 3. The current status of the best artificial fish is found, and the sum of the standard distances corresponding to this status is compared with the values recorded in the bulletin board. If this value is less than the minimum value previously recorded, the recorded values are updated so that the sum of the standard distances recorded in the bulletin board is always smallest. If the number of iterations does not reach the maximum number, the number of iterations is increased by 1, and Step 2 is repeated.
Step 4. If consecutive iterations are performed, the values recorded on the bulletin board do not change significantly, that is, the differences on the bulletin board are less than the preset threshold. Then, some artificial fish were randomly selected according to the probability (), and they will be forcibly initialized. When the maximum number of iterations is reached, the execution process of this method jumps to the fifth step.
Step 5. The iteration is terminated, and the optimal association hypothesis set is output.
In summary, the flow diagram of HSA-JDA method is shown in Figure 3.
3.4. Laser-SLAM Optimization with HSA-JDA Method
In the Laser-SLAM system of this paper, the proposed association method is mainly used to obtain accurate association results. Secondly, the vehicle’s pose and environmental map are estimated by adaptive fading extended Kalman filter (AFEKF) [26, 27].
Based on the HSA-JDA method, the measurement at the current time can be more accurately divided into measurement corresponding to map features and new measurement , which are used to complete SLAM state update and map expansion, respectively. Firstly, according to , the vehicle’s pose and the map features’ position are updated. where is the state mean and is the square root of the covariance matrix. represents the Kalman gain matrix. where is the prediction error covariance with adaptive fading factor .
Secondly, according to the new measurement , the SLAM state will be augmented, and the new landmark will be added to the SLAM state vector . where is the augmented SLAM state. , r represents the distance between the -th feature observed by laser range sensor and the vehicle. represents the angle between the -th feature observed by laser range sensor and the driving direction of the vehicle.
The proposed association method is used to improve the overall search efficiency and search quality of the association process, so as to obtain better data association results and improve the data association performance of Laser-SLAM. As the premise and basis of SLAM state estimation, accurate association results can effectively ensure the real-time and accuracy of SLAM for unmanned delivery vehicles in an unknown outdoor environment.
4. Simulation Experiment and Analysis
The association performance of HSA-JDA, DFJCBB association method, JCBB association method, SCNN association method, and the SLAM estimation accuracy with the four association methods is compared and analyzed based on SLAM simulator.
4.1. Simulation Environment
Two simulation environments are used to provide a comprehensive evaluation. As shown in Figure 4, two simulation environments are designed based on SLAM simulator. In Figure 4, the vehicle is required to start from the zero point of the coordinate system and move uniformly around the region of the 120120 m2 and 240200 m2, respectively. The green line represents vehicle trajectory, and “” denotes the landmark location. In Figure 4(a), there are 184 landmarks, and there are 265 landmarks in Figure 4(b). The simulation parameters are set in Table 2.
(a) Environment I
(b) Environment II
4.2. Comparison and Analysis of Association Performance
The evaluation measure in machine learning is introduced to evaluate the association performance of HSA-JDA, DFJCBB, JCBB association method, and SCNN association method. Three indicators are selected to compare and analyze the performance of four association methods, which are true positive rate (TPR), false positive rate (FPR), and association accuracy (AA), respectively, [28, 29]. The specific formula of the three indicators is as follows. where reflects the ability of the association method to correctly detect the association pairs and to distinguish new environmental features. represents the association pairs detected by the data association method. True negative () represents the number of new measurements. indicates the number of association pairs that are wrongly checked by the data association method. False negative () indicates the number of association pairs ignored by the association method. represents the sum of all associated pairs. The relationship between , , , and FP is listed in the Table 3.
In order to compare the performance of four association methods, average TPR and FPR of each step with four data association methods is obtained, respectively, over 10 Monte Carlo runs under two simulation environments shown in Figure 4. The results are shown in Figures 5 and 6.
(a) HSA-JDA association method
(b) DFJCBB association method
(c) JCBB association method
(d) SCNN association method
(a) HSA-JDA association method
(b) DFJCBB association method
(c) JCBB association method
(d) SCNN association method
As shown in the eight performance graphs, due to the difference of the set environment, the association performance of the four algorithms in simulation environment I is worse than that in simulation environment II. In simulation environment I, the association performance of HSA-JDA method is significantly better than the other three algorithms. The TPR obtained based on the proposed algorithm is the highest, and the FPR is the lowest. In simulation environment II, the four algorithms show good association performance as a whole, and the whole process ensures high TPR and low FPR, but the HSA-JDA method achieves the highest compared with the other three methods based on the results in Table 4. It should be noted that the average of four algorithms is obtained, respectively, over 10 Monte Carlo runs.
In conclusion, HSA-JDA method can obtain high association accuracy in different environments, and the robustness and accuracy of data association are better than the other three methods.
4.3. Comparison of SLAM State Estimation Accuracy
As shown in Figure 7, the state estimation results of SLAM based on HSA-JDA method in two environments is displayed. Figure 7 shows that the estimated path by the SLAM based on HSA-JDA is closely to the true path. And the estimated position of landmarks is closely to actual landmark location.
Take the result of the second environment as an example, as shown in Figure 8, the accuracy of estimated path based on the four association methods is more intuitively contrasted. The overall deviation between the SLAM results based on the SCNN association method and the real path is the largest. From the magnified parts of 1 and 3 in Figure 8, the estimated path based on the SCNN association method almost deviate from the real path at several corners in the setting environment. The estimated path based on DJCBB and JCBB association methods is generally better than those based on SCNN association method. However, from the magnified parts of 1 and 3 in Figure 8, the estimated path based on DJCBB is more accurate than those based on JCBB. Compared with the simulation results of the four methods, the estimated path of vehicle by SLAM algorithm based on HSA-JDA matches with the true path.
The average estimation error of vehicle path at each time step is computed based on 10 Monte Carlo simulation runs for the sake of quantitative analysis for the SLAM accuracy based on HSA-JDA method. The absolute value of vehicle pose error at each time step in X direction and Y direction is shown in Figure 9. The average error in X direction and Y direction of estimated path is listed in Table 5.
The results of Figure 9 and Table 5 show that the SLAM algorithm based on SCNN has the largest estimation error and the lowest estimation accuracy in X and Y directions. The vehicle’s path errors by SLAM algorithm based on HSA-JDA are smaller than that of other three association methods. The reason is that the association results of HSA-JDA method are more accurate than the other three methods. Hence, the vehicle path and the location of true landmarks can be accurately estimated in the estimation stage of SLAM.
The simulation results show that HSA-JDA method can ensure high association accuracy and improve the estimation accuracy of SLAM.
5. Conclusion
In Laser-SLAM, the joint matching association method based on multiple measurements can provide better association results in the state estimation stage of SLAM. Aiming at the problem of how to quickly and accurately obtain the data association results, a joint data association method based on heuristic search algorithm is proposed based on the JML criterion. In proposed method, the SLAM data association problem is evolved into a combinatorial optimization problem of how to determine the optimal association set. A heuristic search algorithm that is an optimized AFSA with the jump behavior of fish swarm and adaptive step size is used to heuristic search the association hypothesis set. The association optimal solution by HSA-JDA method is used in the Laser-SLAM system and obtains a more accurate estimation result. Experiments based on simulator verify that the association robustness and accuracy of HSA-JDA method outperform DFJCBB, JCBB association method, and SCNN association method. The proposed association method can be widely used in Laser-SLAM based on Kalman filter, and it can provide accurate association results for real-time positioning and mapping of unmanned delivery vehicles in different outdoor unknown environments.
Data Availability
The data are available at this linkhttp://www-personal.acfr.usyd.edu.au/tbailey.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
We would like to thank T. Bailey and J. Nieto for providing the SLAM simulator. This work was supported by the Natural Science Foundation of Shandong Province under grant ZR2020QD047.