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

Authors: Dan Liu; Yongkang Feng; Zhifeng Han; Bo Zou
Journal: Journal of Sensors
 (2022)

---

## 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.


---

## Body

## 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 algorithmIn 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 mappingIn 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)pxt,mzt,ut,Ht,where xt is the current pose of the vehicle and m is the map. zt is the set of all measurements z1,⋯,zt, and zt is the sensor measurement at time t. ut can be represented by the set u1,⋯,ut. ut is a control vector containing vehicle speed and steering angle. Ht stands for associative set h1,⋯,ht, and ht is data association of measurement at time t. Based on the Bayes formula, the posterior probability can be calculated by [22]
(2)pxt,mzt,ut,Ht=η⋅pztxt,m,cht⏟MeasurementModel⋅∫pxtxt−1,ut⏟MotionModel⋅pxt−1,mzt−1,ut−1,Ht−1⏟PosteriorProbabilityatt−1dxt−1.The basic framework of Laser-SLAM system is shown in Figure1.Figure 1
The Laser-SLAM system framework.


### 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 thatn features already exist in the constructed map. The geometry of n features can be expressed as F1,F2,⋯,Fn. At time t, m measurements were observed by laser range sensor. The geometry of m measurements can be expressed as O1,O2,⋯,Om. The association hypothesis is defined as Ht.
(3)Ht=h1,h2,⋯,hm,where hi=j means that the i-th measurement and the j-th map feature are paired successfully. If hi=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)d^t≜argmaxPdtxt,M,zt,ut,dt⊆Ht,where dt is generally calculated by association method. xt 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 Figure2.Figure 2
The interpretation tree model for data association.In Figure2, 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: ∅⟶O1,F4⟶O2,F1⟶O3,F2.


## 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)pxt,mzt,ut,Ht,where xt is the current pose of the vehicle and m is the map. zt is the set of all measurements z1,⋯,zt, and zt is the sensor measurement at time t. ut can be represented by the set u1,⋯,ut. ut is a control vector containing vehicle speed and steering angle. Ht stands for associative set h1,⋯,ht, and ht is data association of measurement at time t. Based on the Bayes formula, the posterior probability can be calculated by [22]
(2)pxt,mzt,ut,Ht=η⋅pztxt,m,cht⏟MeasurementModel⋅∫pxtxt−1,ut⏟MotionModel⋅pxt−1,mzt−1,ut−1,Ht−1⏟PosteriorProbabilityatt−1dxt−1.The basic framework of Laser-SLAM system is shown in Figure1.Figure 1
The Laser-SLAM system framework.


## 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 thatn features already exist in the constructed map. The geometry of n features can be expressed as F1,F2,⋯,Fn. At time t, m measurements were observed by laser range sensor. The geometry of m measurements can be expressed as O1,O2,⋯,Om. The association hypothesis is defined as Ht.
(3)Ht=h1,h2,⋯,hm,where hi=j means that the i-th measurement and the j-th map feature are paired successfully. If hi=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)d^t≜argmaxPdtxt,M,zt,ut,dt⊆Ht,where dt is generally calculated by association method. xt 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 Figure2.Figure 2
The interpretation tree model for data association.In Figure2, 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: ∅⟶O1,F4⟶O2,F1⟶O3,F2.


## 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)fij=12πn/2⋅St,ijexp−12vijT⋅St,ij−1⋅vij,(6)el=argmaxfij,where el represents the association between measurements Oi and map features Fj, vij represents the innovation vector, and St,ij represents the covariance matrix of the innovation vector. n represents the dimension of vij. The following formula is obtained by taking logarithms on both sides of the above Formula (5).
(7)lnfij=ln12πn/2⋅St,ij−12vijT⋅St,ij−1⋅vij=−ln2πn/2⋅St,ij−12vijT⋅St,ij−1⋅vij=−n2⋅ln2π−12lnSt,ij+vijT⋅St,ij−1⋅vij,(8)Nij=lnSt,ij+vijT⋅St,ij−1⋅vij,(9)el=argminNij,where Nij represents the gauge distance. If Nij is the smallest, it represents that Oi and Fj 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)Ht=argmax∏∀Oi∈Ot,Fj∈Ftfij,(11)Ht=argmin∑∀Oi∈Ot,Fj∈FtNij.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)absxi−xv<cd+ed,absyi−yv<cd+ed,xi−xv⋅cosθ+yi−yv⋅sinθ>0,xi−xv2+yi−yv2<cd+ed2,where xv,yv,θ is the vehicle’s pose. xi,yi 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 testm 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)dij=vijT⋅St,ij−1⋅vij<χdimv,1−α2,where vij is the innovation determined by the predictors of Oi and Fj and St,ij represents the innovation covariance. dij represents the Mahalanobis distance between Oi and Fj. Generally, for innovations subject to Gaussian distribution, dij obeys a chi-squared distribution with degree of freedom dimv and confidence level 1−α. In general, dimv=2 and the chi-squared values χdimv,1−α2 corresponding to the different confidence levels are shown in Table 1.Table 1
Chi-squared values with two-degree-of-freedom.
1−αχdimv,1−α290%4.6195%5.9997.5%7.3899%9.2199.5%10.6099.9%13.82Assuming that the association set ofm1 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 Onew; otherwise, it is directly eliminated. The remaining m2 measurements are represented by set Omatch, where m2=m−m1. The map feature sets that may be associated with the remaining measurements are represented by G=G1,G2,⋯,Gm2, and their dimensions are p1,p2,⋯,pm2, respectively. For example, the map feature set associated with the first measurement O1 in set Omatch is G1=F1,F3,F5,F8,F13, then p1=5, the map feature set corresponding to the second measurement O2 in set Omatch is G2=F2,F4,F7, then p2=3, other correspondences, and so on.
(iii)
The normalized distanceNij between each measurement in Omatch and each map feature Fj 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)fHtm2=∑∀Oi∈Omatch,Fj∈GNij,Htm2=argminfHtm2=argmin∑∀Oi∈Omatch,Fj∈GNij,where Htm2=h1,h2,⋯,hm2 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 iteratesm 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)bestEm−bestEm‐1<ε,where bestEm 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)S1=1−YjYi⋅S,where S represents the basic step size. S1 represents the adaptive step size. Yj represents the objective function value corresponding to the searched of new artificial fish state. Yj 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 assignedm2 association hypothesis pairs. Therefore, each artificial fish can be quantified as a matrix 1×m2. 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.
Ifm 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 Figure3.Figure 3
Proposed method flow diagram.


### 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 measurementzt at the current time can be more accurately divided into measurement zft corresponding to map features and new measurement znt, which are used to complete SLAM state update and map expansion, respectively. Firstly, according to zft, the vehicle’s pose and the map features’ position are updated.
(17)xt=x¯tt−1+Kt⋅zft−z^t,Pt=P¯tt−1−Kt⋅St⋅KtT,where xt is the state mean and Pt is the square root of the covariance matrix. Kt represents the Kalman gain matrix.
(18)P¯tt−1=λt⋅Gx⋅Pt−1⋅GxT+Gu⋅Qt⋅GuT,where P¯tt−1 is the prediction error covariance with adaptive fading factor λt.Secondly, according to the new measurementznt, the SLAM state will be augmented, and the new landmark will be added to the SLAM state vector xt.
(19)xaug=xtxv+r⋅cosθv+φyv+r⋅sinθv+φ,where xaug is the augmented SLAM state. znt=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.


## 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)fij=12πn/2⋅St,ijexp−12vijT⋅St,ij−1⋅vij,(6)el=argmaxfij,where el represents the association between measurements Oi and map features Fj, vij represents the innovation vector, and St,ij represents the covariance matrix of the innovation vector. n represents the dimension of vij. The following formula is obtained by taking logarithms on both sides of the above Formula (5).
(7)lnfij=ln12πn/2⋅St,ij−12vijT⋅St,ij−1⋅vij=−ln2πn/2⋅St,ij−12vijT⋅St,ij−1⋅vij=−n2⋅ln2π−12lnSt,ij+vijT⋅St,ij−1⋅vij,(8)Nij=lnSt,ij+vijT⋅St,ij−1⋅vij,(9)el=argminNij,where Nij represents the gauge distance. If Nij is the smallest, it represents that Oi and Fj 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)Ht=argmax∏∀Oi∈Ot,Fj∈Ftfij,(11)Ht=argmin∑∀Oi∈Ot,Fj∈FtNij.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)absxi−xv<cd+ed,absyi−yv<cd+ed,xi−xv⋅cosθ+yi−yv⋅sinθ>0,xi−xv2+yi−yv2<cd+ed2,where xv,yv,θ is the vehicle’s pose. xi,yi 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 testm 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)dij=vijT⋅St,ij−1⋅vij<χdimv,1−α2,where vij is the innovation determined by the predictors of Oi and Fj and St,ij represents the innovation covariance. dij represents the Mahalanobis distance between Oi and Fj. Generally, for innovations subject to Gaussian distribution, dij obeys a chi-squared distribution with degree of freedom dimv and confidence level 1−α. In general, dimv=2 and the chi-squared values χdimv,1−α2 corresponding to the different confidence levels are shown in Table 1.Table 1
Chi-squared values with two-degree-of-freedom.
1−αχdimv,1−α290%4.6195%5.9997.5%7.3899%9.2199.5%10.6099.9%13.82Assuming that the association set ofm1 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 Onew; otherwise, it is directly eliminated. The remaining m2 measurements are represented by set Omatch, where m2=m−m1. The map feature sets that may be associated with the remaining measurements are represented by G=G1,G2,⋯,Gm2, and their dimensions are p1,p2,⋯,pm2, respectively. For example, the map feature set associated with the first measurement O1 in set Omatch is G1=F1,F3,F5,F8,F13, then p1=5, the map feature set corresponding to the second measurement O2 in set Omatch is G2=F2,F4,F7, then p2=3, other correspondences, and so on.
(iii)
The normalized distanceNij between each measurement in Omatch and each map feature Fj 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)fHtm2=∑∀Oi∈Omatch,Fj∈GNij,Htm2=argminfHtm2=argmin∑∀Oi∈Omatch,Fj∈GNij,where Htm2=h1,h2,⋯,hm2 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 iteratesm 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)bestEm−bestEm‐1<ε,where bestEm 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)S1=1−YjYi⋅S,where S represents the basic step size. S1 represents the adaptive step size. Yj represents the objective function value corresponding to the searched of new artificial fish state. Yj 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 assignedm2 association hypothesis pairs. Therefore, each artificial fish can be quantified as a matrix 1×m2. 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.
Ifm 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 Figure3.Figure 3
Proposed method flow diagram.


## 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 measurementzt at the current time can be more accurately divided into measurement zft corresponding to map features and new measurement znt, which are used to complete SLAM state update and map expansion, respectively. Firstly, according to zft, the vehicle’s pose and the map features’ position are updated.
(17)xt=x¯tt−1+Kt⋅zft−z^t,Pt=P¯tt−1−Kt⋅St⋅KtT,where xt is the state mean and Pt is the square root of the covariance matrix. Kt represents the Kalman gain matrix.
(18)P¯tt−1=λt⋅Gx⋅Pt−1⋅GxT+Gu⋅Qt⋅GuT,where P¯tt−1 is the prediction error covariance with adaptive fading factor λt.Secondly, according to the new measurementznt, the SLAM state will be augmented, and the new landmark will be added to the SLAM state vector xt.
(19)xaug=xtxv+r⋅cosθv+φyv+r⋅sinθv+φ,where xaug is the augmented SLAM state. znt=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 Figure4, 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 m2 and 240∗200 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.Figure 4
Simulation environments.
(a)
Environment I(b)
Environment IITable 2
Parameter setting.
ParameterValueVehicle speed3 m/sMaximum steering angle30°Maximum range30 mControl noise(0.3 m/s, 3°)Measurement noise(0.1 m, 1°)Control frequency40 HzConfidence level0.95Measurement frequency5 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)TPR=TPTP+TN+FP+FN,FPR=FPTP+TN+FP+FN,AA=TP+TNTP+TN+FP+FN,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.Table 3
Contingency table.
AlgorithmPredictedTotalActualTPFNActual positive (TP+FN)FPTNActual negative (FP+TN)TotalPredicted positive (TP+FP)Predicted negative (FN+TN)TP+FP+FN+TNIn 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 Figure4. The results are shown in Figures 5 and 6.Figure 5
Association performance of three methods in environment I.
(a)
HSA-JDA association method(b)
DFJCBB association method(c)
JCBB association method(d)
SCNN association methodFigure 6
Association performance of four methods in environment II.
(a)
HSA-JDA association method(b)
DFJCBB association method(c)
JCBB association method(d)
SCNN association methodAs 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 highestAA 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.Table 4
Association accuracy of four algorithms.
AlgorithmEnvironment IAAEnvironment IIAAHSA-JDA0.94290.9871DFJCBB0.94070.9813JCBB0.92580.9792SCNN0.88700.9757In 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 Figure7, 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.Figure 7
The state estimation results of SLAM with HSA-JDA method.Take the result of the second environment as an example, as shown in Figure8, 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.Figure 8
The estimated path based on four association methods.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 Figure9. The average error in X direction and Y direction of estimated path is listed in Table 5.Figure 9
The estimation error of vehicle path at each time step.Table 5
Average error of estimated path.
AlgorithmX (m)Y (m)HSA-JDA-SLAM0.25970.5021DFJCBB-SLAM0.43580.5148JCBB-SLAM0.45590.7653SCNN-SLAM1.38261.7334The results of Figure9 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.


## 4.1. Simulation Environment

Two simulation environments are used to provide a comprehensive evaluation. As shown in Figure4, 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 m2 and 240∗200 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.Figure 4
Simulation environments.
(a)
Environment I(b)
Environment IITable 2
Parameter setting.
ParameterValueVehicle speed3 m/sMaximum steering angle30°Maximum range30 mControl noise(0.3 m/s, 3°)Measurement noise(0.1 m, 1°)Control frequency40 HzConfidence level0.95Measurement frequency5 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)TPR=TPTP+TN+FP+FN,FPR=FPTP+TN+FP+FN,AA=TP+TNTP+TN+FP+FN,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.Table 3
Contingency table.
AlgorithmPredictedTotalActualTPFNActual positive (TP+FN)FPTNActual negative (FP+TN)TotalPredicted positive (TP+FP)Predicted negative (FN+TN)TP+FP+FN+TNIn 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 Figure4. The results are shown in Figures 5 and 6.Figure 5
Association performance of three methods in environment I.
(a)
HSA-JDA association method(b)
DFJCBB association method(c)
JCBB association method(d)
SCNN association methodFigure 6
Association performance of four methods in environment II.
(a)
HSA-JDA association method(b)
DFJCBB association method(c)
JCBB association method(d)
SCNN association methodAs 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 highestAA 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.Table 4
Association accuracy of four algorithms.
AlgorithmEnvironment IAAEnvironment IIAAHSA-JDA0.94290.9871DFJCBB0.94070.9813JCBB0.92580.9792SCNN0.88700.9757In 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 Figure7, 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.Figure 7
The state estimation results of SLAM with HSA-JDA method.Take the result of the second environment as an example, as shown in Figure8, 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.Figure 8
The estimated path based on four association methods.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 Figure9. The average error in X direction and Y direction of estimated path is listed in Table 5.Figure 9
The estimation error of vehicle path at each time step.Table 5
Average error of estimated path.
AlgorithmX (m)Y (m)HSA-JDA-SLAM0.25970.5021DFJCBB-SLAM0.43580.5148JCBB-SLAM0.45590.7653SCNN-SLAM1.38261.7334The results of Figure9 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.



---

*Source: 1491079-2022-11-23.xml*