Volume 2022, Issue 1 1491079

Research Article

Open Access

A Joint Data Association Method for Laser-SLAM of Unmanned Delivery Vehicle Based on Heuristic Search Algorithm

Dan Liu,

Dan Liu

orcid.org/0000-0001-7126-9937

Shandong University of Science and Technology, Qingdao, Shandong, China sdust.edu.cn

Search for more papers by this author

Yongkang Feng,

Yongkang Feng

orcid.org/0000-0002-2412-9429

Shandong University of Science and Technology, Qingdao, Shandong, China sdust.edu.cn

Search for more papers by this author

Zhifeng Han,

Zhifeng Han

orcid.org/0000-0002-7493-4530

Shandong University of Science and Technology, Qingdao, Shandong, China sdust.edu.cn

Search for more papers by this author

Bo Zou,

Corresponding Author

Bo Zou

[email protected]

orcid.org/0000-0002-1734-9618

Shandong Hi-Speed Yellow River Industrial Investment & Development Co. Ltd, Jinan, Shandong, China

Search for more papers by this author

Dan Liu,

Dan Liu

orcid.org/0000-0001-7126-9937

Shandong University of Science and Technology, Qingdao, Shandong, China sdust.edu.cn

Search for more papers by this author

Yongkang Feng,

Yongkang Feng

orcid.org/0000-0002-2412-9429

Shandong University of Science and Technology, Qingdao, Shandong, China sdust.edu.cn

Search for more papers by this author

Zhifeng Han,

Zhifeng Han

orcid.org/0000-0002-7493-4530

Shandong University of Science and Technology, Qingdao, Shandong, China sdust.edu.cn

Search for more papers by this author

Bo Zou,

Corresponding Author

Bo Zou

[email protected]

orcid.org/0000-0002-1734-9618

Shandong Hi-Speed Yellow River Industrial Investment & Development Co. Ltd, Jinan, Shandong, China

Search for more papers by this author

First published: 23 November 2022

https://doi.org/10.1155/2022/1491079

Academic Editor: Carlos Marques

Share a link

Email
Wechat
Bluesky

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

(1)

where x_t is the current pose of the vehicle and m is the map. z^t is the set of all measurements {z₁, ⋯, z_t}, and z_t is the sensor measurement at time t. u^t can be represented by the set {u₁, ⋯, u_t}. u_t is a control vector containing vehicle speed and steering angle. H_t stands for associative set {h₁, ⋯, h_t}, and h_t is data association of measurement at time t. Based on the Bayes formula, the posterior probability can be calculated by [22]

(2)

The basic framework of Laser-SLAM system is shown in Figure 1.

Details are in the caption following the image — Open in figure viewer PowerPoint

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 n features already exist in the constructed map. The geometry of n features can be expressed as {F₁, F₂, ⋯, F_n}. At time t, m measurements were observed by laser range sensor. The geometry of m measurements can be expressed as {O₁, O₂, ⋯, O_m}. The association hypothesis is defined as H_t.

(3)

where h_i = j means that the i-th measurement and the j-th map feature are paired successfully. If h_i = 0, 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]:

(4)

where d_t is generally calculated by association method. x_t is the pose of vehicle. M 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, O₁, O₂, and O₃ represent a frame of environmental information detected by the sensor at the current time, F₁, F₂, F₃, and F₄ 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: ∅⟶(O₁, F₄)⟶(O₂, F₁)⟶(O₃, F₂).

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.

(5)

(6)

where e_l represents the association between measurements O_i and map features F_j, v_ij represents the innovation vector, and S_t,ij represents the covariance matrix of the innovation vector. n represents the dimension of v_ij. The following formula is obtained by taking logarithms on both sides of the above Formula (5).

(7)

(8)

(9)

where N_ij represents the gauge distance. If N_ij is the smallest, it represents that O_i and F_j 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).

(10)

(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 with

(12)

where (x_v, y_v, θ) is the vehicle’s pose. (x_i, y_i) is the landmark’s coordinate in the established map. ed represents the effective scanning distance of laser range sensor. cd indicates the compensation distance.

(ii)
The individual compatibility (IC) criterion is used to test m 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:

(13)

where v_ij is the innovation determined by the predictors of O_i and F_j and S_t,ij represents the innovation covariance. d_ij represents the Mahalanobis distance between O_i and F_j. Generally, for innovations subject to Gaussian distribution, d_ij obeys a chi-squared distribution with degree of freedom dimv and confidence level 1 − α. In general, dimv = 2 and the chi-squared values

corresponding to the different confidence levels are shown in Table 1.

1. Chi-squared values with two-degree-of-freedom.

1−α
90%	4.61
95%	5.99
97.5%	7.38
99%	9.21
99.5%	10.60
99.9%	13.82

Assuming that the association set of m₁ 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 O_new; otherwise, it is directly eliminated. The remaining m₂ measurements are represented by set O_match, where m₂ = m − m₁. 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 O₁ in set O_match is G₁ = {F₁, F₃, F₅, F₈, F₁₃}, then p₁ = 5, the map feature set corresponding to the second measurement O₂ in set O_match is G₂ = {F₂, F₄, F₇}, then p₂ = 3, other correspondences, and so on.

(iii)
The normalized distance N_ij between each measurement in O_match and each map feature F_j in set G is calculated according to the ML criterion. Then, the objective function of SLAM association problem can be expressed by Formula (11).

(14)

where

represents the association hypothesis set corresponding to the minimum objective function at time t.

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 m 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 p (0 < p < 1) for jumping behavior, that is, the selected artificial fish is initialized, and the optimization is restarted

(15)

where bestE(m) represents the optimal value recorded on the bulletin board after the m -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 improved

(16)

where S represents the basic step size. S₁ represents the adaptive step size. Y_j represents the objective function value corresponding to the searched of new artificial fish state. Y_j 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 m₂ association hypothesis pairs. Therefore, each artificial fish can be quantified as a matrix 1 × m₂. 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 m 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 p (0 < p < 1), 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 z_t at the current time can be more accurately divided into measurement zf_t corresponding to map features and new measurement zn_t, which are used to complete SLAM state update and map expansion, respectively. Firstly, according to zf_t, the vehicle’s pose and the map features’ position are updated.

(17)

where x_t is the state mean and P_t is the square root of the covariance matrix. K_t represents the Kalman gain matrix.

(18)

where

is the prediction error covariance with adaptive fading factor λ_t.

Secondly, according to the new measurement zn_t, the SLAM state will be augmented, and the new landmark will be added to the SLAM state vector x_t.

(19)

where x_aug is the augmented SLAM state. zn_t = [r φ]^T, r represents the distance between the j -th feature observed by laser range sensor and the vehicle. φ represents the angle between the j -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 120 ^∗120 m² and 240 ^∗200 m², 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.

2. Parameter setting.

Parameter	Value
Vehicle speed	3 m/s
Maximum steering angle	30°
Maximum range	30 m
Control noise	(0.3 m/s, 3°)
Measurement noise	(0.1 m, 1°)
Control frequency	40 Hz
Confidence level	0.95
Measurement frequency	5 Hz

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.

(20)

where AA reflects the ability of the association method to correctly detect the association pairs and to distinguish new environmental features. TP represents the association pairs detected by the data association method. True negative (TN) represents the number of new measurements. FP indicates the number of association pairs that are wrongly checked by the data association method. False negative (FN) indicates the number of association pairs ignored by the association method. TP + TN + FP + FN represents the sum of all associated pairs. The relationship between TP, TN, FN, and FP is listed in the Table 3.

3. Contingency table.

Algorithm	Predicted		Total
Actual	TP	FN	Actual positive (TP + FN)
Actual	FP	TN	Actual negative (FP + TN)
Total	Predicted positive (TP + FP)	Predicted negative (FN + TN)	TP + FP + FN + TN

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.

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 AA compared with the other three methods based on the results in Table 4. It should be noted that the average AA of four algorithms is obtained, respectively, over 10 Monte Carlo runs.

4. Association accuracy of four algorithms.

Algorithm	Environment I AA	Environment II AA
HSA-JDA	0.9429	0.9871
DFJCBB	0.9407	0.9813
JCBB	0.9258	0.9792
SCNN	0.8870	0.9757

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.

5. Average error of estimated path.

Algorithm	X (m)	Y (m)
HSA-JDA-SLAM	0.2597	0.5021
DFJCBB-SLAM	0.4358	0.5148
JCBB-SLAM	0.4559	0.7653
SCNN-SLAM	1.3826	1.7334

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.

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.

Open Research

Data Availability

The data are available at this linkhttp://www-personal.acfr.usyd.edu.au/tbailey.

References

1 Dissanayake M. W. M. G., Newman P., Clark S., Durrant-Whyte H. F., and Csorba M., A solution to the simultaneous localization and map building (SLAM) problem, IEEE Transactions on Robotics and Automation. (2001) 17, no. 3, 229–241, https://doi.org/10.1109/70.938381, 2-s2.0-0035357061.
10.1109/70.938381
Web of Science® Google Scholar
2 Nieto J., Guivant J., Nebot E., and Thrun S., Real time data association for Fast SLAM, 2003 IEEE International Conference on Robotics and Automation, 2003, Taipei, Taiwan, 412–418.
Google Scholar
3 Zhu X., Yi J., Cheng J., and He L., Adapted error map based mobile robot UWB indoor positioning, IEEE Transactions on Instrumentation and Measurement. (2020) 69, no. 9, 6336–6350, https://doi.org/10.1109/TIM.2020.2967114.
10.1109/TIM.2020.2967114
Web of Science® Google Scholar
4 Jung J., Lee S., and Myung H., Indoor mobile robot localization and mapping based on ambient magnetic fields and aiding radio sources, IEEE Transactions on Instrumentation and Measurement. (2015) 64, no. 7, 1922–1934, https://doi.org/10.1109/TIM.2014.2366273, 2-s2.0-85027957500.
10.1109/TIM.2014.2366273
Web of Science® Google Scholar
5 Wang J., Meng Z., and Wang L., Efficient probabilistic approach to range-only SLAM with a novel likelihood model, IEEE Transactions on Instrumentation and Measurement. (2021) 70, 1–12.
10.1109/TIM.2021.3123218
PubMed Web of Science® Google Scholar
6 Wang H. P., Jiang L. Z., and Xiang P., Improving the durability of the optical fiber sensor based on strain transfer analysis, Optical Fiber Technology. (2018) 42, 97–104, https://doi.org/10.1016/j.yofte.2018.02.004, 2-s2.0-85043513751.
10.1016/j.yofte.2018.02.004
CAS ADS Web of Science® Google Scholar
7 Hähnel D., Thrun S., Wegbreit B., and Burgard W., Towards lazy data association in SLAM, Robotics Research. The Eleventh International Symposium, 2005, Berlin, Heidelberg, 421–431.
Google Scholar
8 Ji X. C., Zhang H., Hai D., and Zheng Z. Q., An incremental SLAM algorithm with backtracking revisable data association for mobile robots, 2008 IEEE International Conference on Mechatronics and Automation, 2008, Takamatsu, Japan, 831–838.
Google Scholar
9 Chen B. F., Cai Z. X., and Zou Z. R., A hybrid data association approach for mobile robot SLAM, Control Automation and Systems, 2010, Gyeonggi-do, Korea (South), 1900–1903.
Google Scholar
10 Ran T., Yuan L., Zhang J., He L., Huang R., and Mei J., Not only look but infer: multiple hypothesis clustering of data association inference for semantic SLAM, IEEE Transactions on Instrumentation and Measurement. (2021) 70, 1–9, https://doi.org/10.1109/TIM.2021.3074954.
10.1109/TIM.2021.3074954
PubMed Web of Science® Google Scholar
11 Zeng W. J., Zhang T. D., and Jiang D. P., Analysis of data association methods of SLAM, Systems Engineering & Electronics. (2010) 32, no. 4, 860–864.
Google Scholar
12 Guo J. and Zhang R., Novel implementation of track-oriented multiple hypothesis tracking algorithm, Chinese Journal of Electronics. (2012) 18, no. 2, 138–150.
Google Scholar
13 Neira J. and Tardos J., Data association in stochastic mapping using the joint compatibility test, IEEE Transactions on Robotics and Automation. (2001) 17, no. 6, 890–897, https://doi.org/10.1109/70.976019, 2-s2.0-0035705785.
10.1109/70.976019
Web of Science® Google Scholar
14 Du Y., A novel data association approach for SLAM of mobile robot, 37th Chinese Control Conference, 2018, Wuhan, China, 4744–4749.
Google Scholar
15 Feng L., Chai H. X., Sun T., and Yin Z. Y., SLAM algorithm based on heuristic graph search with dynamic threshol, Computer Engineering. (2011) 37, no. 17, 185–187.
Google Scholar
16 Jun X. U., Zhang G., Jing B., and Zhang L., A joint maximum likelihood method optimized data association approach to SLAM problem, Robot. (2012) 34, no. 4, 492–497.
Google Scholar
17 Wang X. B., Zhao G. R., and Li H. J., Chaos ant colony algorithm based on Tabu search for data association of SLAM, Beijing Ligong Daxue Xuebao/Transaction of Beijing Institute of Technology. (2012) 32, no. 7, 725–728.
Google Scholar
18 Wang X. B. and Liu T., Artificial fish swarm algorithm of SLAM data association for UAV, Instrumentation Technology. (2012) 32, no. 7, 725–728.
Google Scholar
19 Liu D., Duan J., and Wang C., A fast joint compatibility data association algorithm for SLAM based on clustering, ROBOT. (2018) 40, no. 2, 158–168.
Google Scholar
20 http://www-personal.acfr.usyd.edu.au/tbailey.
Google Scholar
21 Liu D., Duan J. M., and Meng X. Y., A SLAM system of intelligent vehicle based on laser radar, Laser Journal. (2018) 39, no. 7, 76–82.
Google Scholar
22 Liu D., A data association algorithm for SLAM based on central difference joint compatibility criterion and clustering, Robotica. (2021) 39, no. 9, 1674–1691, https://doi.org/10.1017/S0263574720001435.
10.1017/S0263574720001435
Web of Science® Google Scholar
23 Thrun S., Burgard W., and Fox D., Probabilistic robotics, MIT press. (2005) 45, 437–483.
Google Scholar
24 Ji P. F., Qi J. D., and Zhu W. F., Application of improved artificial fish swarm algorithm in Hadoop scheduling algorithm, Application Research of Computers. (2014) 31, no. 12, 3572–3574,.
Google Scholar
25 Liu D. and Han Z., A SLAM data association method for unmanned distribution vehicle based on improved artificial fish swarm algorithm, 2021 International Conference on Information Science, Parallel and Distributed Systems (ISPDS), 2021, Hangzhou, China, 19–24.
Google Scholar
26 Marques C. A. F., Bilro L., Kahn L., Oliveira R. A., Webb D. J., and Nogueira R. N., Acousto-optic effect in microstructured polymer fiber Bragg gratings: simulation and experimental overview, Journal of Lightwave Technology. (2013) 31, no. 10, 1551–1558, https://doi.org/10.1109/JLT.2013.2254110, 2-s2.0-84875836780.
10.1109/JLT.2013.2254110
Web of Science® Google Scholar
27 Yang L., Xiao Q., Niu Y., and Hu S., Design of localization system based on reducing Kalman filter, Journal of Nanjing University of Aeronautics & Astronautics. (2012) 44, no. 1, 134–138.
Google Scholar
28 Ruan X. G., Zhang J. J., and Zhu X. Q., Simultaneous localization and mapping data association based on maximum expectation clustering for Gaussian mixture model, Control Theory & Applications. (2020) 37, no. 2, 265–274.
Google Scholar
29 Marques C. A. F., Bilro L., Alberto J., Webb D. J., and Nogueira R. N., Narrow bandwidth Bragg gratings imprinted in polymer optical fibers for different spectral windows, Optics Communications. (2013) 307, 57–61, https://doi.org/10.1016/j.optcom.2013.05.059, 2-s2.0-84879908829.
10.1016/j.optcom.2013.05.059
CAS PubMed Web of Science® Google Scholar

All articles

A Joint Data Association Method for Laser-SLAM of Unmanned Delivery Vehicle Based on Heuristic Search Algorithm

Abstract

1. Introduction