CN106529678A - SLAM data association method based on maximum-minimum ant system optimization - Google Patents

Abstract

The invention discloses an SLAM (Simultaneous Localization and Mapping) data association method based on maximum-minimum ant system optimization. The SLAM data association method based on maximum-minimum ant system optimization includes the steps: utilizing examination threshold filtering to reduce the solution space range associated with data, and improving the search efficiency of an algorithm; converting a data association problem into an optimal path solution problem, and utilizing an explanation tree model to stand for an association path node; and combining a maximum-minimum ant system with a combination maximum likelihood criterion, and taking a combination maximum likelihood value assumed by data association as a target function to perform path search. The SLAM data association method based on maximum-minimum ant system optimization reduces the association solution space of data association, can improve the search efficiency of the algorithm, and can effectively make up the defect that the ant colony algorithm is low in the rate of convergence, and stagnates in search and is easy to fall into local optimum.

Description

A kind of SLAM data correlation methods optimized based on max-min ant system

Technical field

The present invention relates to robot autonomous navigation field, more particularly to a kind of optimized based on max-min ant system SLAM data correlation methods.

Background technology

The problem of simultaneous localization and mapping (Simultaneous Localization and Mapping, SLAM) Can be described as in unknown prior information environment, mobile robot utilizes installed sensor to build increment type map, together When carry out self poisoning with location estimation according to the map, be the key technical problem that robot realizes entirely autonomous navigation, and The emphasis and focus of current mobile robot area research.Data correlation is established as the important component part of SLAM problems Not in the same time, the sensor obtained by different locations is measured and the corresponding relation between characteristics map.However, in actual SLAM mistakes Cheng Zhong, the association accuracy of data association algorithm and computation complexity directly affect the accuracy and real-time of SLAM.

Data correlation is typically by the method for statistical estimate to determine.At present, the data correlation commonly used in SLAM problems Algorithm has single matching arest neighbors (Individual Compatibility Nearest Neighbor, ICNN) algorithm, joint phase Hold branch-and-bound (Joint Compatibility Branch and Bound, JCBB) algorithm and multiple hypotheis tracking (Multiple Hypothesis Tracking, MHT) algorithm etc..Wherein, ICNN algorithms calculating amount of storage is little, simple, But due to only considering that the association between single nearest environmental characteristic is matched, therefore it is only suitable for small-scale SLAM environment, and for feature The larger situation of distribution density or environment, its antijamming capability are poor；JCBB algorithms are although it is contemplated that each measurement feature The correlation of pairing, association accuracy are high, but due to needing constantly to utilize joint consistent test criterion to check all observations Compatibility and map feature between, and branch-bound algorithm is searching for solution space, causes computation complexity very high, it is actual Using being restricted；MHT algorithms are associated decision-making by considering the observation of adjacent a few frames, it is adaptable to initialization and see Survey the larger occasion of noise, but as it is assumed that number exponentially can increase over time, therefore be not suitable for application in real time.Additionally, 《Dementia&Geriatric Cognitive Disorders》In the document delivered for 2004《Towards Lazy Data Association in SLAM》In, it is proposed that a kind of inertia data correlation (Lazy Data Association, LDA) Algorithm, can recall amendment erroneous association in the past, but need to calculate the larger inverse of a matrix of dimension, it is difficult to apply in practice； 《Computer application》In the document delivered for 2009《A kind of SLAM data correlation methods based on ant group algorithm》In, it is proposed that one The data correlation method that joint maximum likelihood criterion is combined with ant colony optimization algorithm is planted, maximum likelihood calculation is considerably improved The association accuracy of method, but as ant group algorithm convergence rate is slow, search for the defect for stagnating and being easily absorbed in local optimum so that its Association accuracy and run time need further improvement.

The content of the invention

The technical problem to be solved is for defect involved in background technology, there is provided a kind of based on most The data correlation method of big minimum ant system optimization so that data correlation all increases in accuracy and real-time, has Effect ground solves the problems, such as the SLAM data correlations of mobile robot.

The present invention is employed the following technical solutions to solve above-mentioned technical problem：

A kind of SLAM data correlation methods optimized based on max-min ant system, are comprised the following steps：

Step 1), the road sign characteristic point of some static state is arranged in the working environment of mobile robot, and sets moving machine The motion initial position of device people and speed；

Step 2), environment road sign characteristic information is gathered using the sonar sensor that mobile robot is carried, make the ring deposited Border road sign characteristic set is x=(x₁,x₂,…,x_n), the environment road sign characteristic set for newly observing is z=(z₁,z₂,…,z_m), The data correlation set up between the environment road sign characteristic set z for the observing and environment road sign characteristic set x for having deposited assumes H_m：

H_m={ h₁,…,h_i,…,h_m}

Wherein, n is the number for having deposited road sign characteristic point, and m is the number of the road sign characteristic point for newly observing；h_iFor new observation Road sign feature z for arriving_iAssociation value, if road sign feature z for newly observing_iExist in the environment road sign characteristic set x for having deposited Road sign feature x consistent with which_j, value is j, now (z_i,x_j) for h_iCorresponding association matching is right；Otherwise value is 0；I is big In the natural number equal to 1 less than or equal to m, j is the natural number more than or equal to 1 less than or equal to n；

Step 3), using the inspection threshold condition in following formula to all of association matching to testing, and filter discontented The association matching of sufficient test condition is right：

In formula,For road sign feature z for newly observing_iWith environment road sign feature x deposited_jIn the mahalanobis distance of t, Chi square distribution value when γ is 1- α for confidence level, the wherein value of α are preset；

Then represent with explanation tree model that the association matching for meeting test condition is right, wherein each node is considered as ant migration Path node；

Step 4), the number for making ant is M, and iteration maximum times are N, and the taboo list of every ant is both configured to sky Ant set of minimal paths is set to empty set by collection, and the number of times that every ant selects is set to 0, and iterations Ne is arranged For 0, by pheromones initial value τ_ij(0) it is set to default pheromones original maximum τ_max；

Step 5), for M ant in each ant：

Step 5.1), obtaining the ant can be with the set A of all nodes of migration under present node；

Step 5.2), node of the ant in taboo list is weeded out in set A, set B is obtained；

Step 5.3), for each node in set B, the ant is calculated according to the following formula from present node migration To its probability

In formula, i, j, r are the natural number more than or equal to 1 less than or equal to m, and k is the nature more than or equal to 1 less than or equal to M Number, B_kFor the taboo list of the ant, β₁For information heuristic factor, β₂To expect heuristic factor, heuristic information η_ijAnd η_irValue point Wei not 1/N_ijAnd 1/N_ir, N_ijFor present node and the path of the node, N_irTo remove the node in present node and set B The path of other any nodes in addition；τ_ijFor the pheromones on present node to path between the node, τ_irIt is to work as prosthomere Pheromones in point and set B in addition to the node between other any nodes on path；

Step 5.4), in set B, select migration to its probability highest node as the next node of the ant, incite somebody to action This node carries out migration to which after being added to the taboo list of the ant；

Step 5.5), pheromones minimum of a value τ of the ant is calculated according to following formula_min：

In formula, p_bestFor step 5.4) in probability of the ant from present node migration to next node, num be the ant The number of times that ant selects；

Step 5.6), Jia 1 to frequency n um that the ant selects；

Step 6), 5) execution step, all reaches bottom leaf node up to all ants repeatedly；

Step 7), the path total length of every ant migration is calculated, and selects the most short ant of path total length, by the ant The path of ant and path total length add into ant set of minimal paths as an element, and the path that the ant is passed through Pheromones be updated according to the following formula：

τ_ij=ρ τ_ij+1/f(s^best)

In formula, ρ be pheromones volatilization factor, f (s^best) represent shortest path total length value, work as τ_ijValue be less than τ_min When be set to τ_min, work as τ_ijValue be more than τ_maxWhen be set to τ_max；

Step 8), Jia 1 to iterations Ne, the taboo list of every ant is both configured to into empty set, by every ant The number of times of selection is set to 0, and pheromones initial value is set to default pheromones original maximum τ_max；

Step 9), execution step is 5) to step 8 repeatedly), until being more than maximum iteration time N as Ne；

Step 10), the most short ant of outbound path total length is screened in ant set of minimal paths, using its path as most Good path, and exported the node in the path as optimal data relevance assumption.

Further optimize as a kind of SLAM data correlation methods optimized based on max-min ant system of the present invention Scheme, step 3) in determine road sign feature z that newly observes_iWith environment road sign feature x deposited_jBetween mahalanobis distance Detailed process include：

Step 3.1), set up observation model of the mobile robot in t：

z_t,i=h_t,j(x_t|t-1)+ω_t,i

In formula, h_t,j() for mobile robot t i-th observed quantity z_t,iWith system mode x_t|t-1Between it is non- Systems with Linear Observation function, ω_t,iBe average be zero, covariance be R_t,iRandom measurement noise；

Step 3.2), by the non-linear observation model in above formula in current estimatePlace is to its linearization process：

In formula, Jacobian matrix

Step 3.3), i-th observed quantity z of t is calculated respectively_t,iWith system mode x_t|t-1EstimateBetween The new breath vector ν of distance_t,ijAnd its covariance S_t,ijIt is as follows：

In formula, P_t|t-1For estimateVariance matrix；

Step 3.4), it is calculated as follows mahalanobis distanceFor：

Further optimize as a kind of SLAM data correlation methods optimized based on max-min ant system of the present invention Scheme, step 7) in by ant through path Pheromone update after the pheromones are smoothed using following formula：

τ_ij=τ_ij+δ·(τ_max-τ_ij)

In formula, 0≤δ≤1.

The present invention adopts above technical scheme compared with prior art, with following technique effect：

1. minimum possible relevance assumption is filtered using inspection thresholding, reduce the association solution space of data correlation, improved The search efficiency of algorithm；

2. matching will be associated to being expressed as path node using explanation tree-model, using joint maximum likelihood value as optimization mesh Mark, so as to joint maximum likelihood data correlation problem is changed into solution optimum path problems；

3. joint maximum likelihood criterion and max-min ant system are combined, improve maximum likelihood algorithm data pass Connection accuracy is low and positioning precision is poor problem, and effectively compensate for that ant group algorithm convergence rate is slow, search is stagnated and easily The defect of local optimum is absorbed in, this method improves data correlation accuracy and shortens run time to a certain extent.

Description of the drawings

Fig. 1 (a), Fig. 1 (b) respectively check thresholding, the schematic diagram of uncertain data association；

Fig. 2 is explanation tree model structure schematic diagram of the present invention；

Fig. 3 is data correlation flow chart of the present invention；

Fig. 4 (a), Fig. 4 (b), Fig. 4 (c), Fig. 4 (d) are respectively simulated environment map, maximum likelihood algorithm data correlation knot Really, based on the joint maximum likelihood data correlation result of ant group algorithm, data correlation result of the present invention；

Fig. 5 (a), Fig. 5 (b), Fig. 5 (c), Fig. 5 (d) are respectively data correlation accuracy and position with time step variation diagram, X-axis Error is with time step variation diagram, Y-axis position error with time step variation diagram, direction angular positioning error spacer step variation diagram at any time.

Specific embodiment

Below in conjunction with the accompanying drawings technical scheme is described in further detail：

The present invention is mainly made up of three parts：Part I, the solution for filtering to reduce data correlation using inspection thresholding are empty Between scope, improve algorithm search efficiency；Data correlation problem is converted into optimal path Solve problems, and profit by Part II Associated path node is represented with explanation tree model；Part III, by max-min ant system and joint maximum likelihood criterion phase With reference to, using data correlation assume joint maximum likelihood value carry out route searching as object function.

In present embodiment, for convenience of description, represent that the joint of max-min ant system optimization is maximum with MMAS-JML Likelihood data correlation method, represents maximum likelihood data correlation method with ML, represents the joint of ant group algorithm optimization with ACO-JML Maximum likelihood data correlation method.

A kind of SLAM data correlation methods optimized based on max-min ant system of the present invention, it is concrete real as shown in Figure 3 The mode of applying is comprised the following steps：

Step 1), the simulated environment map as shown in Fig. 4 (a) is built under MATLAB software platforms.Map be 10m × The square area of 10m, and outside is uniformly distributed 168 static road sign characteristic points in the path.Wherein, curve represents motion rail Mark, circle represent motion path node, and point represents environmental characteristic, and cross represents feature assessment, and triangular representation is equipped with sonar The mobile-robot system of sensor.Sonar sensor measurement distance is 3m, measures angular range for [- 90 °, 90 °], and angle is surveyed Amount error is 0.125 °, and range measurement error is 0.01m.Robot system measurement noise in x and y direction is 0.1m, The measurement noise of deflection is 0.3 °.Robot is transported along curve from origin position beginning counterclockwise with the speed of 0.25m/s It is dynamic, and closed-loop experiment is carried out using EKF SLAM methods.

Step 2), environment road sign characteristic information is gathered using the sonar sensor that mobile robot is carried, make the ring deposited Border road sign characteristic set is x=(x₁,x₂,…,x_n), the environment road sign characteristic set for newly observing is z=(z₁,z₂,…,z_m), The data correlation set up between the environment road sign characteristic set z for the observing and environment road sign characteristic set x for having deposited assumes H_m：

H_m={ h₁,…,h_i,…,h_m}

Wherein, n is the number for having deposited road sign characteristic point, and m is the number of the road sign characteristic point for newly observing；

h_iFor road sign feature z for newly observing_iAssociation value, if road sign feature z for newly observing_iIn the environment deposited There is road sign feature x consistent with which in road sign characteristic set x_j, value is j, now (z_i,x_j) for h_iCorresponding association matching It is right；Otherwise value is 0；Wherein, i is the natural number more than or equal to 1 less than or equal to m, and j is the nature more than or equal to 1 less than or equal to n Number；

Data association algorithm is needed from relevance assumption H_mIn select that a kind of optimum relevance assumption matching is right, this is not only sight Measured value is correctly associated with existing feature in map, in addition it is also necessary to is capable of identify that new feature and is excluded false observation.However, with The increase of observation number, the computation complexity of data association algorithm can be exponentially increased.In view of mobile robot in actual fortune During dynamic the range of movement of each time step be it is limited, therefore need not by each observed quantity with deposit feature and carry out data pass Connection.In order to exclude minimum possible relevance assumption, reduce association solution space, data association algorithm needs to examine each feature-set Test thresholding.

Step 3), the observation model for setting up mobile robot in t is：

z_t,i=h_t,j(x_t|t-1)+ω_t,i

In formula, h_t,j() for mobile robot t i-th observed quantity z_t,iWith system mode x_t|t-1Between it is non- Systems with Linear Observation function, ω_t,iBe average be zero, covariance be R_t,iRandom measurement noise.

By the non-linear observation model in above formula in current estimatePlace is to its linearization process：

In formula, Jacobian matrix

I-th observed quantity z of t is calculated respectively_t,iWith system mode x_t|t-1EstimateBetween distance newly cease to Amount ν_t,ijAnd its covariance S_t,ijIt is as follows：

In formula, P_t|t-1For estimateVariance matrix；

It is calculated as follows mahalanobis distanceFor：

Using the inspection threshold condition in following formula to all of association matching to testing, and filter and be unsatisfactory for test strip The association matching of part is right：

In formula,For road sign feature z for newly observing_iWith environment road sign feature x deposited_jIn the mahalanobis distance of t, Chi square distribution value when γ is 1- α for confidence level, the wherein value of α are preset；

As shown in Fig. 1 (a), inspection thresholding can regard as in observation space withCentered on hyperelliptic, wherein WithRoad sign feature z for newly observing is represented respectively_iWith environment road sign feature x deposited_jEstimate, only positioned at the ellipse In observation be possible to be accepted as effectively observation.When there are multiple road sign features for newly observing while being deposited positioned at one Environment road sign feature inspection thresholding in, shown in such as Fig. 1 (b), then can produce uncertain data association, thus be accomplished by according to Best suit actual conditions one is selected according to data correlation criterion from multiple relevance assumptions.

Maximum-likelihood criterion (Maximum Likelihood, ML), typically for considering each accessed by system The possibility that the road sign feature for newly observing is associated with single feature in the environment road sign characteristic set deposited.It is assumed that concept of reality Measurement is uniformly distributed, and observation noise Gaussian distributed, calculates what t was newly observed for i-th using maximum-likelihood criterion Road sign feature z_iWith environment road sign feature x deposited for j-th_jBetween association probability value f_ij：

H in formula_lRoad sign feature z that i-th of expression association probability maximum is newly observed_iWith the environment road sign deposited for j-th Feature x_jBetween incidence relation, n represents the dimension of new breath vector.

To association probability value f_ijTake the logarithm, and arrange：

In formula, N_ijRepresent the corresponding specification distance of association probability value.

The problem equivalent for asking for most relevance probability is converted into the problem for asking for minimum specification distance using following formula：

Although ML methods are simple, due to only considering the independent association between single observation and feature so that in environment When uncertainty increases, association accuracy can decline rapidly.For the stability of strengthening system, joint maximum likelihood criterion (JointMaximum Likelihood, JML) considers the whole matching maximum likelihood of data correlation, is asked using the criterion Take each association matching to association probability value total product, and as maximum relevance assumption：

The problem equivalent for asking for overall most relevance probability is converted into the problem for asking for minimum specification distance using following formula：

In order to intuitively describe the solution space of data correlation problem, construct such as Fig. 2 under the conditions of unique constraints is met Shown explanation tree-model, in the present embodiment, Fig. 2 only depicts the explanation tree node of part to facilitate description explanation tree mould Type, and explanation tree interstitial content is by a specific association number of pairs in actual algorithm.The root node of tree is empty node Φ, often One layer of correspondence, one road sign feature z for newly observing_iIf, z_iWith x_jAssociation is matched, then node represents a corresponding association matching To (z_i,x_j)；If z_iNot with any feature association, then it is empty node.Represent from root node to a paths of low layer leaf node Possible relevance assumption between the environment road sign characteristic set z for newly observing and the environment road sign characteristic set x for having deposited, in figure Path：Φ→(z₁,x₂)→Φ→(z₃,x₄).Represent that with the explanation tree model association matching for meeting test condition is right, wherein Each node is considered as the path node of ant migration, can thus utilize max-min ant system (Max-Min Ant System, MMAS) searching out optimal path, and as optimal data relevance assumption.

Step 4), the number for making ant is M, and iteration maximum times are N, and the taboo list of every ant is both configured to sky Ant set of minimal paths is set to empty set by collection, and the number of times that every ant selects is set to 0, and iterations Ne is arranged For 0, by pheromones initial value τ_ij(0) it is set to default pheromones original maximum τ_max；Other parameters arrange as shown in table 1.

Step 5), for M ant in each ant：

Step 5.1), obtaining the ant can be with the set A of all nodes of migration under present node；

Step 5.2), node of the ant in taboo list is weeded out in set A, set B is obtained；

Step 5.3), for each node in set B, the ant is calculated according to the following formula from present node migration To its probability

In formula, i, j, r are the natural number more than or equal to 1 less than or equal to m, and k is the nature more than or equal to 1 less than or equal to M Number, B_kFor the taboo list of the ant, β₁For information heuristic factor, β₂To expect heuristic factor, heuristic information η_ijAnd η_irValue point Wei not 1/N_ijAnd 1/N_ir, N_ijFor present node and the path of the node, N_irTo remove the node in present node and set B The path of other any nodes in addition；τ_ijFor the pheromones on present node to path between the node, τ_irIt is to work as prosthomere Pheromones in point and set B in addition to the node between other any nodes on path；

Step 5.4), in set B, select migration to its probability highest node as the next node of the ant, incite somebody to action This node carries out migration to which after being added to the taboo list of the ant；

Step 5.5), pheromones minimum of a value τ of the ant is calculated according to following formula_min：

In formula, p_bestFor step 5.4) in probability of the ant from present node migration to next node, num be the ant The number of times that ant selects；

Step 5.6), Jia 1 to frequency n um that the ant selects；

Step 6), 5) execution step, all reaches bottom leaf node up to all ants repeatedly；

Step 7), the path total length of every ant migration is calculated, and selects the most short ant of path total length, by the ant The path of ant and path total length add into ant set of minimal paths as an element, and the path that the ant is passed through Pheromones be updated according to the following formula：

τ_ij=ρ τ_ij+1/f(s^best)

In formula, ρ be pheromones volatilization factor, f (s^best) represent shortest path total length value, work as τ_ijValue be less than τ_min When be set to τ_min, work as τ_ijValue be more than τ_maxWhen be set to τ_max；

Step 8), Jia 1 to iterations Ne, the taboo list of every ant is both configured to into empty set, by every ant The number of times of selection is set to 0, and pheromones initial value is set to default pheromones original maximum τ_max；

Step 9), execution step is 5) to step 8 repeatedly), until being more than maximum iteration time N as Ne；

Step 10), the most short ant of outbound path total length is screened in ant set of minimal paths, using its path as most Good path, and exported the node in the path as optimal data relevance assumption.

In order to verify the validity of put forward data association algorithm herein, MMAS-JML is contrasted with ML, ACO-JML Test, and actual emulation result of each algorithm in environmental map is respectively obtained, shown in such as Fig. 4 (b), Fig. 4 (c), Fig. 4 (d).Phase The association accuracy answered and position error are with the result of variations of time step as shown in Fig. 5 and Biao 2.

Simulation parameter in table 1MMAS-JML algorithms

Symbol	Description	Parameter value
			α	Inspection thresholding	0.05
ρ	Pheromones volatilization factor	0.1
			β1	Information heuristic factor	1.0
β2	Expect heuristic factor	5.0
			M	The number of ant	50
N	Iteration maximum times	100
			δ	Pheromone flatness parameter	0
τmax	Initial maximum pheromones	1.0

2 different pieces of information association algorithm performance comparison of table

Association algorithm	Run time/s	Average association accuracy/%	Total run time step number
				ML	81.8304	0.8064	168
ACO-JML	117.5198	0.9089	168
				MMAS-JML	110.8654	0.9117	168

According to Fig. 4, Fig. 5 and the simulation result of table 2, analyzed as follows：

(1) from the point of view of whole road sign is estimated and associates accuracy result：In Fig. 4 (b), the red point of true road sign is estimated with road sign The distance of blue cross compares Fig. 4 (c) and Fig. 4 (d) farther out, and deviation is larger；As ML algorithms only consider single measurement and feature Associated possibility, so averagely association accuracy is far below ACO-JML and MMAS-JML；Compare ACO-JML, MMAS-JML The deficiency of locally optimal solution is easily absorbed in during overcoming ACO algorithm search, thus has been lifted in association accuracy.

(2) from the point of view of robot position error result：In operation starting stage, the positioning precision and ACO-JML of ML algorithms It is close to MMAS-JML, but the increase of step number over time, its accumulated error is significantly increased, and causes positioning precision to decline, and ACO-JML and MMAS-JML can preferably suppress the growth of robot accumulated error, and locating effect is preferable.

(3) from the point of view of run time：Whole matching maximum likelihood due to considering data correlation, and avoid and search Stagnate and the slow defect of convergence rate during rope, although MMAS-JML algorithms run time is long compared with ML algorithms, compare ACO- JML but decreases.

Consider two aspects of data correlation accuracy and real-time, MMAS-JML algorithms have bigger advantage.

It is understood that unless otherwise defined, all terms used herein (include skill to those skilled in the art of the present technique Art term and scientific terminology) with art of the present invention in those of ordinary skill general understanding identical meaning.Also It should be understood that those terms defined in such as general dictionary are should be understood that with the context with prior art The consistent meaning of meaning, and unless defined as here, will not be explained with idealization or excessively formal implication.

Above-described specific embodiment, has been carried out further to the purpose of the present invention, technical scheme and beneficial effect Describe in detail, the be should be understood that specific embodiment that the foregoing is only the present invention is not limited to this Bright, all any modification, equivalent substitution and improvements within the spirit and principles in the present invention, done etc. should be included in the present invention Protection domain within.

Claims (3)

1. it is a kind of based on max-min ant system optimize SLAM data correlation methods, it is characterised in that comprise the following steps：

Step 1), the road sign characteristic point of some static state is arranged in the working environment of mobile robot, and sets mobile robot Motion initial position and speed；

Step 2), environment road sign characteristic information is gathered using the sonar sensor that mobile robot is carried, make the environment road deposited Mark characteristic set is x=(x₁,x₂,…,x_n), the environment road sign characteristic set for newly observing is z=(z₁,z₂,…,z_m), set up Data correlation between the environment road sign characteristic set z for observing and the environment road sign characteristic set x for having deposited assumes H_m：

H_m={ h₁,…,h_i,…,h_m}

Wherein, n is the number for having deposited road sign characteristic point, and m is the number of the road sign characteristic point for newly observing；h_iNewly observe Road sign feature z_iAssociation value, if road sign feature z for newly observing_iExist and which in the environment road sign characteristic set x for having deposited Consistent road sign feature x_j, value is j, now (z_i,x_j) for h_iCorresponding association matching is right；Otherwise value is 0；I be more than etc. In 1 natural number for being less than or equal to m, j is the natural number more than or equal to 1 less than or equal to n；

Step 3), using the inspection threshold condition in following formula to all of association matching to testing, and filter and be unsatisfactory for inspection The association matching for testing condition is right：

$D_{t,ij}^2<\mathrm{\&gamma;}$

In formula,For road sign feature z for newly observing_iWith environment road sign feature x deposited_jIn the mahalanobis distance of t, γ is Chi square distribution value when confidence level is 1- α, the wherein value of α are preset；

Then represent with explanation tree model that the association matching for meeting test condition is right, wherein each node is considered as the road of ant migration Footpath node；

Step 4), the number for making ant is M, and iteration maximum times are N, and the taboo list of every ant is both configured to empty set, will Ant set of minimal paths is set to empty set, and the number of times that every ant selects is set to 0, and iterations Ne is set to 0, will Pheromones initial value τ_ij(0) it is set to default pheromones original maximum τ_max；

Step 5), for M ant in each ant：

Step 5.1), obtaining the ant can be with the set A of all nodes of migration under present node；

Step 5.2), node of the ant in taboo list is weeded out in set A, set B is obtained；

Step 5.3), for each node in set B, the ant is calculated according to the following formula from present node migration to which Probability

$p_{ij}^k=\left\{\begin{array}{c}\frac{{\mathrm{\&tau;}}_{ij}^{{\mathrm{\&beta;}}_1}\&CenterDot;{\mathrm{\&eta;}}_{ij}^{{\mathrm{\&beta;}}_2}}{\underset{r\&NotElement;B_k}{\&Sigma;}{\mathrm{\&tau;}}_{ir}^{{\mathrm{\&beta;}}_1}\&CenterDot;{\mathrm{\&eta;}}_{ir}^{{\mathrm{\&beta;}}_2}},j\&NotElement;B_k\\ 0,otherwise\end{array}\right.$

In formula, i, j, r be more than or equal to 1 less than or equal to m natural number, k be more than or equal to 1 less than or equal to M natural number, B_k For the taboo list of the ant, β₁For information heuristic factor, β₂To expect heuristic factor, heuristic information η_ijAnd η_irValue is respectively 1/ N_ijAnd 1/N_ir, N_ijFor present node and the path of the node, N_irFor in present node and set B in addition to the node its The path of his any node；τ_ijFor the pheromones on present node to path between the node, τ_irFor present node and collection Pheromones in conjunction B in addition to the node between other any nodes on path；

Step 5.4), in set B select migration to its probability highest node as the next node of the ant, this is saved Point carries out migration to which after being added to the taboo list of the ant；

Step 5.5), pheromones minimum of a value τ of the ant is calculated according to following formula_min：

${\mathrm{\&tau;}}_{min}=\frac{{\mathrm{\&tau;}}_{max}\&CenterDot;(1-\sqrt[num]{p_{best}})}{\sqrt[num]{p_{best}}\&CenterDot;(num/2-1)}$

In formula, p_bestFor step 5.4) in probability of the ant from present node migration to next node, num selected for the ant The number of times selected；

Step 5.6), Jia 1 to frequency n um that the ant selects；

Step 6), 5) execution step, all reaches bottom leaf node up to all ants repeatedly；

Step 7), the path total length of every ant migration is calculated, and selects the most short ant of path total length, by the ant Path and path total length add into ant set of minimal paths as an element, and the letter in the path that the ant is passed through Breath element is updated according to the following formula：

τ_ij=ρ τ_ij+1/f(s^best)

In formula, ρ be pheromones volatilization factor, f (s^best) represent shortest path total length value, work as τ_ijValue be less than τ_minShi Jiang Which is set to τ_min, work as τ_ijValue be more than τ_maxWhen be set to τ_max；

Step 8), Jia 1 to iterations Ne, the taboo list of every ant is both configured to into empty set, every ant is selected Number of times be set to 0, pheromones initial value is set to into default pheromones original maximum τ_max；

Step 9), execution step is 5) to step 8 repeatedly), until being more than maximum iteration time N as Ne；

Step 10), the most short ant of outbound path total length is screened in ant set of minimal paths, using its path as optimal road Footpath, and exported the node in the path as optimal data relevance assumption.

2. SLAM data correlation methods optimized based on max-min ant system according to claim 1, its feature exists In step 3) in determine road sign feature z that newly observes_iWith environment road sign feature x deposited_jBetween mahalanobis distance's Detailed process includes：

Step 3.1), set up observation model of the mobile robot in t：

z_t,i=h_t,j(x_t|t-1)+ω_t,i

In formula, h_t,j() for mobile robot t i-th observed quantity z_t,iWith system mode x_t|t-1Between nonlinear riew Survey function, ω_t,iBe average be zero, covariance be R_t,iRandom measurement noise；

Step 3.2), by the non-linear observation model in above formula in current estimatePlace is to its linearization process：

$h_{t,j}\left(x_{t|t-1}\right)\&cong;h_{t,j}\left({\hat{x}}_{t|t-1}\right)+H_{t,j}(x_t-{\hat{x}}_{t|t-1})$

In formula, Jacobian matrix

Step 3.3), i-th observed quantity z of t is calculated respectively_t,iWith system mode x_t|t-1EstimateBetween distance New breath vector ν_t,ijAnd its covariance S_t,ijIt is as follows：

$v_{t,ij}=z_{t,i}-h_{t,j}\left({\hat{x}}_{t|t-1}\right)$

$S_{t,ij}=H_{t,j}{P}_{t|t-1}{H}_{t,j}^T+R_{t,i}$

In formula, P_t|t-1For estimateVariance matrix；

Step 3.4), it is calculated as follows mahalanobis distanceFor：

$D_{t,ij}^2=v_{t,ij}^T{S}_{t,ij}^{-1}{v}_{t,ij}.$

3. SLAM data correlation methods optimized based on max-min ant system according to claim 1, its feature exists In step 7) in by ant through path Pheromone update after the pheromones are smoothed using following formula：

τ_ij=τ_ij+δ·(τ_max-τ_ij)

In formula, 0≤δ≤1.