An iterative classification matching data association method adapting to complex environments

Abstract

The application discloses an iterative classification matching data association method suitable for complex environment, and comprises the following steps: performing primary association; the characteristic group of primary association success constitutes a data set Z t + and F t + ; the characteristic group of primary association failure constitutes a data set Z t ‑ and F t ‑ ; solving a least square matching vector Θ according to the data set Z t + and F t + ; updating the data set F t ‑ by the least square matching vector Θ to obtain an updated data set, combining the data set Z t ‑ and F into new input, and performing iteration until a mean square error detection is satisfied. The application optimizes the data association method, improves the consistency of algorithm estimation, and reduces the algorithm calculation complexity, and makes up for the deficiency of ICNN and JCBB algorithms in large-scale underwater environment.

Description

Technical Field

[0001] The present invention belongs to the field of data association, real-time positioning and map construction, and relates to an iterative classification matching data association algorithm, and in particular to an iterative classification matching data association method that is adaptable to complex environments. Background Art

[0002] SLAM primarily consists of two components: state estimation and data association. State estimation is the process of estimating the robot's position and the location of environmental features. Data association involves identifying relationships between environmental features observed at different times and points, ultimately determining whether these features correspond to the same entity in the physical environment. Data association is a key technology in SLAM. Incorrect associations often lead to divergent map feature estimates and inaccurate feature localization. Therefore, data association is fundamental to the reliable execution of any SLAM algorithm.

[0003] The earliest data association algorithm proposed for SLAM target tracking is the nearest neighbor method. This method is simple to use but only works well when the targets are few and sparse. The Individual Compatibility Nearest Neighbor (ICNN) method is a widely used data association method, but its performance degrades sharply when clutter density is high.

[0004] To improve the robustness of the ICNN algorithm, Neira, Tardos, and others proposed the Joint Compatibility Branch and Bound (JCBB) algorithm. Compared to the ICNN algorithm, which only performs independent compatibility tests, the JCBB algorithm uses a joint compatibility criterion to comprehensively consider all possible associations in the measurement-map feature set and select the best data association. Therefore, JCBB is a more constrained data association algorithm that can reduce the probability of data association errors when there are large deviations in the robot pose estimation. However, the computational complexity of the JCBB method increases exponentially with the number of environmental feature points, making it unsuitable for large-scale environments. Although many improved algorithms have been proposed to reduce the algorithm's computational cost, JCBB is still not conducive to the practical application of SLAM algorithms when the number of environmental feature points is large. Summary of the Invention

[0005] Purpose of the invention: In order to overcome the deficiencies in the prior art, an iterative classification matching data association method that is adaptable to complex environments is provided to address the defects of the ICNN and JCBB algorithms that are not suitable for large-scale environments. An improved data association algorithm is designed to optimize the data association method, improve the consistency of the algorithm estimation, and reduce the algorithm computational complexity, thereby making up for the deficiencies of the ICNN and JCBB algorithms in large-scale underwater environments.

[0006] Technical Solution: To achieve the above objectives, the present invention provides an iterative classification matching data association method that is adaptable to complex environments, comprising the following steps:

[0007] S1: Use the standard joint compatible branch and bound algorithm to perform the initial association and transform the sensor measurement feature dataset Z t and map feature dataset F t Divided into two groups, the features that are successfully associated for the first time constitute the dataset Z t + and F t + , the features that failed to be associated for the first time constitute the dataset Z t - and F t - ;

[0008] S2: According to data set Z t + and F t + , solve the least squares matching vector Θ;

[0009] S3: Least squares matching vector Θ to dataset F t - Update and get the updated data set

[0010] S4: Dataset Z t - and the updated dataset Combine into a new input and iterate steps S1 to S3 until the mean square error test is satisfied, and the iteration ends.

[0011] Furthermore, the step S1 is specifically as follows:

[0012] Let vector F t Represents the state of n features contained in the known local map at time t:

[0013] F t =[F1,f2,…,f n ] T (1)

[0014] In a similar way, let vector Z t Represents the m observations obtained by the sensor at time t:

[0015] Z t =[O1,O2,…,O m ] T (2)

[0016] Based on the known local map features and the sensor observations at time t, an association hypothesis can be given:

[0017] H t ={c1,c2,…,c i ,…,c m-1 ,c m} (3)

[0018] where c i Indicates observation o i With feature f ci Matches if c i = 0, it means that there is no local map that matches the observation o i Matching features;

[0019] Association hypothesis H t The predicted observations for the features involved are as follows:

[0020]

[0021] Where h(·) is the robot observation equation, R ^ The predicted position and posture of the robot, vector It is H t The predicted status of the features in ;

[0022] Then the simultaneous equations are obtained from the above formula:

[0023]

[0024] In the formula represents actual observation;

[0025] The covariance matrix of the simultaneous equations is:

[0026]

[0027] In the formula is the Jacobian matrix; is the forecast error covariance matrix; is the observation error covariance matrix;

[0028] From (5) and (6), the joint compatibility condition can be obtained as follows:

[0029]

[0030] in is the joint Mahalanobis distance; represents a chi-square distribution with degrees of freedom The confidence level is α (usually 0.95); if If formula (7) is satisfied, then H t All matching pairs in is joint compatible;

[0031] Finally, the sensor measures the feature dataset Z t and map feature dataset F t Classified into two groups: Features that are jointly compatible are stored in the new feature set Z t + and F t + Those that do not meet the requirements are stored in the feature set Z t - and F t - middle.

[0032] Furthermore, in step S1, an m-layer interpretation tree model is used to represent all possible association hypotheses at time t.

[0033] Furthermore, in step S2, the least squares matching vector Θ is solved using a quaternion method, and the least squares matching vector Θ is composed of a rotation matrix and a translation matrix;

[0034] If you use q R =[q0 q1 q2 q3] T Represents a unit rotation quaternion vector, where q0≥0, Then the quaternion vector q is rotated by the above unit R The resulting 3×3 rotation matrix can be written as:

[0035]

[0036] If you use q T =[q4 q5 q6] T represents a translation matrix, then the entire least squares matching vector can be expressed as Θ=[q R ,q T ] T .

[0037] Furthermore, the specific solution process of the least squares matching vector Θ in step S2 is:

[0038] Known a set of sensor measurement features that have been successfully associated, whose corresponding set of map features is where i = 1,..., K, then its mean square matching error can be expressed as:

[0039]

[0040] if and denote the set of points O + and F + centered at

[0041]

[0042] then the set of features Z t + and F t + the cross-covariance matrix P can be expressed as:

[0043]

[0044] Given the anti-symmetric matrix Λ = P - P T the cyclic periodic components of Λ are used to construct the column vector Δ = [Λ 23 Λ 31 Λ 12 ] T then the symmetric matrix Γ can be expressed as:

[0045]

[0046] where tr(P) denotes the trace of the cross-covariance matrix P, i.e., the sum of all elements on the main diagonal of the matrix P, and I3 denotes the 3x3 identity matrix;

[0047] The unit eigenvector corresponding to the largest eigenvalue of the symmetric matrix Γ is finally selected as the optimal rotation vector q R = [q0 q1 q2 q3] T while the optimal translation vector can be expressed as:

[0048]

[0049] If we denote the least-squares quaternion operator by θ, then the entire least-squares matching process can be summarized as:

[0050] (Θ,E) = θ (Z t + ,F t + ) (15).

[0051] Furthermore, the step S3 is specifically as follows:

[0052] The least squares matching vector Θ = [q R ,q T ] T It is used to select the map feature set F that does not meet the nearest neighbor selection rule, that is, the map feature set F that is not successfully associated. t - Make updates;

[0053]

[0054] in, represents the set of unsuccessfully associated map features after being updated by the least squares matching vector;

[0055] If ζ is used to represent the least squares matching update operator, the entire least squares matching update process can be summarized as:

[0056]

[0057] in, Represents the mean square matching error in the matching update phase.

[0058] Furthermore, the mean square error detection in step S4 is:

[0059] The entire iterative process will terminate when the difference between the mean square error before and after the two updates is less than the pre-set threshold τ, τ>0 (the threshold is used to set the required matching accuracy);

[0060]

[0061] If the difference in mean square error does not satisfy the above formula, then set Z t =Z t - , F t =F t - To continue the above iterative data association process.

[0062] The present invention provides an iterative classification matching data association method that is adaptable to complex environments. It involves a novel iterative data association algorithm that can address the shortcomings of the ICNN and JCBB algorithms. The idea of ​​the method of the present invention is to use the corresponding information extracted from the measurement-map feature set that was successfully associated for the first time to update the feature set that was not successfully associated for the first time, and then perform data association on it again. First, the initial association process based on the nearest neighbor selection rule divides the sensor measurement feature data set O and the map feature data set F into two groups. One of the groups is the data set O consisting of the successfully associated features. + and F +, which is then used to generate the least squares matching vector (using the quaternion method). The features that failed to be associated initially are placed in the feature set O - and F - The map already has a feature set F that is not associated with any sensor measurement feature. - The obtained least squares matching vector is updated to weaken the influence of incorrect robot pose estimation on data association results. Finally, the updated map feature dataset -Unassociated sensor measurement feature dataset O - The two are combined into a new input and the iteration process continues until the difference between the two mean square errors is less than a preset threshold (the threshold is used to set the accuracy required for matching).

[0063] Beneficial effects: Compared with the existing technology, the present invention addresses the defects of ICNN and JCBB algorithms that are not suitable for large-scale environments, and provides an iterative classification matching data association method that is adaptable to complex environments. It designs an improved data association algorithm, optimizes the data association method, improves the consistency of algorithm estimation, and reduces the algorithm calculation complexity. Due to the significant reduction in the algorithm calculation complexity, the algorithm of the present invention can adapt to data association in complex environments, making up for the shortcomings of ICNN and JCBB algorithms in large-scale underwater environments, and can simultaneously have good matching accuracy and matching time. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 FIG1 is a diagram showing the execution process of the method of the present invention;

[0065] Figure 2 To explain the tree model diagram;

[0066] Figure 3 Comparison chart of the cumulative matching time of ICNN, JCBB and the method of the present invention;

[0067] Figure 4 Comparison chart of the correlation cumulative errors of ICNN, JCBB and the method of the present invention. DETAILED DESCRIPTION

[0068] The present invention is further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that these embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention. After reading the present invention, modifications of various equivalent forms of the present invention made by those skilled in the art all fall within the scope defined by the claims attached to this application.

[0069] The present invention provides an iterative classification matching data association method that adapts to complex environments, such as Figure 1 As shown, the following steps are included:

[0070] S1: Use the standard joint compatible branch and bound algorithm to perform the initial association and transform the sensor measurement feature dataset Z t and map feature dataset F t Divided into two groups, the features that are successfully associated for the first time constitute the dataset Z t + and F t + , the features that failed to be associated for the first time constitute the dataset Z t - and F t - ;

[0071] S2: According to data set Z t + and F t + , solve the least squares matching vector Θ;

[0072] S3: Least squares matching vector Θ to dataset F t - Update and get the updated data set

[0073] S4: Dataset Z t - and the updated dataset Combine into a new input and iterate steps S1 to S3 until the mean square error test is satisfied, and the iteration ends.

[0074] In this embodiment, step S1 is specifically as follows:

[0075] Let vector F t Represents the state of n features contained in the known local map at time t:

[0076] F t =[f1,f2,…,f n ] T (1)

[0077] In a similar way, let vector Z t Represents the m observations obtained by the sensor at time t:

[0078] Z t =[O1,O2,…,O m ] T (2)

[0079] Based on the known local map features and the sensor observations at time t, an association hypothesis can be given:

[0080] H t ={c1,c2,…,c i ,…,cm-1 ,c m} (3)

[0081] where c i Indicates observation o i With feature f ci Matches if c i = 0, it means that there is no local map that matches the observation o i Matching features; in this embodiment, Figure 2 As shown, an m-layer explanation tree model is used to represent all possible association hypotheses at time t.

[0082] Association hypothesis H t The predicted observations for the features involved are as follows:

[0083]

[0084] Where h(·) is the robot observation equation, R^ is the robot's predicted position and posture, and the vector It is H t The predicted status of the features in ;

[0085] Then the simultaneous equations are obtained from the above formula:

[0086]

[0087] In the formula represents actual observation;

[0088] The covariance matrix of the simultaneous equations is:

[0089]

[0090] In the formula is the Jacobian matrix; is the forecast error covariance matrix; is the observation error covariance matrix;

[0091] From (5) and (6), the joint compatibility condition can be obtained as follows:

[0092]

[0093] in is the joint Mahalanobis distance; represents a chi-square distribution with degrees of freedom The confidence level is α (0.95 in this embodiment); if If formula (7) is satisfied, then H t All matching pairs in is joint compatible;

[0094] Finally, the sensor measures the feature dataset Z t and map feature dataset F t Classified into two groups: Features that are jointly compatible are stored in the new feature set Z t + and F t + Those that do not meet the requirements are stored in the feature set Z t - and F t - middle.

[0095] In step S2 of this embodiment:

[0096] The core of the nearest neighbor method is to calculate the predicted measurement value of the feature based on the current estimate of the robot's pose. Therefore, if the error of the robot's pose estimate is large, then the feature set Z t and F t The Mahalanobis distance between two feature points corresponding to the same object will become larger. The two feature points that should have been associated may no longer satisfy formula (5) because of the increased Mahalanobis distance between the two points, which will eventually lead to an error in the data association result. Since the successfully associated feature set Z t + and F t + Now we can consider whether we can find a certain correspondence between the two points and express it with a mathematical vector Θ. t + and the existing feature set F from the map t + If it is applied to the remaining map feature sets F that are not associated t - In the example, the relationship vector Θ is used to map the feature set F - The above assumption actually has a certain theoretical basis, because the feature set Z t + and F t + Originally, it represents the representation of the same object in different coordinate systems (i.e., local coordinate system and global coordinate system). Therefore, there must be some mathematical transformation relationship between the two. t + and F t + The correspondence between them is the least squares matching problem in mathematics.

[0097] To step S2, the embodiment uses quaternion method to solve the least square matching vector Θ, which is composed of a rotation matrix and a translation matrix;

[0098] If q R = [q0 q1 q2 q3] T represents a unit rotation quaternion vector, where q0≥0, Then the 3x3 rotation matrix generated by the unit rotation quaternion vector q R can be written as:

[0099]

[0100] If q T = [q4 q5 q6] T represents a translation matrix, then the entire least square matching vector can be expressed as Θ = [q R , q T ] T ;

[0101] The specific solving process of the least square matching vector Θ is as follows:

[0102] Given that Z represents a set of successfully associated sensor measurement features, and its corresponding map feature set is F , where i = 1, …, K, its mean square matching error can be expressed as:

[0103]

[0104] If and represent the center of the point set O + composed of the successfully associated sensor measurement features and its corresponding map features F + , then:

[0105]

[0106] Then the intercovariance matrix P of the feature sets Z t and F + t + can be expressed as:

[0107]

[0108] Given that the cyclic period component of the skew-symmetric matrix Λ = P - P T is used to construct the column vector Δ = [Λ 23 Λ 31 Λ 12 ]T , then the symmetric matrix Γ can be expressed as:

[0109]

[0110] Where tr(P) represents the trace of the cross-covariance matrix P, that is, the sum of all elements on the main diagonal of the matrix P, and I3 represents the 3×3 identity matrix;

[0111] The unit eigenvector corresponding to the largest eigenvalue of the symmetric matrix Γ is finally selected as the optimal rotation vector q R =[q0 q1 q2 q3] T , and the optimal translation vector can be expressed as:

[0112]

[0113] If θ is used to represent the least squares quaternion operator, the entire least squares matching process can be summarized as:

[0114] (Θ,E)=θ(Z t + ,F t + ) (15).

[0115] In this embodiment, step S3 is specifically as follows:

[0116] The least squares matching vector Θ = [q R ,q T ] T It is used to select the map feature set F that does not meet the nearest neighbor selection rule, that is, the map feature set F that is not successfully associated. t - Make updates;

[0117]

[0118] in, represents the set of unsuccessfully associated map features after being updated by the least squares matching vector;

[0119] If ζ is used to represent the least squares matching update operator, the entire least squares matching update process can be summarized as:

[0120]

[0121] in, Represents the mean square matching error in the matching update phase.

[0122] In this embodiment, the mean square error detection in step S4 is:

[0123] The entire iterative process will terminate when the difference between the mean square error before and after the two updates is less than the pre-set threshold τ, τ>0 (the threshold is used to set the required matching accuracy);

[0124]

[0125] If the difference in mean square error does not satisfy the above formula, then set Z t =Z t - , F t =F t - To continue the above iterative data association process.

[0126] Based on the above scheme, in order to verify the actual effect of the method of the present invention, this embodiment conducts a simulation comparison experiment, which is as follows:

[0127] Neira simulations of EKF SLAM compared the matching accuracy and computational complexity of JCBB, ICNN, and the proposed method. The simulations were performed using MATLAB. The scenario involved a mobile robot equipped with wheel odometers and laser ranging sensors navigating a square corridor. Point features were located around the corridor, and the sensors observed their positions relative to the robot. Using an extended Kalman filter, the features were mapped and their positions, along with the robot's, were estimated.

[0128] Get as Figure 3 The cumulative matching time and Figure 4 The associated cumulative error data shown is from Figure 3 and Figure 4 Comparison results show that the matching accuracy of the proposed method is similar to that of JCBB, and both methods are far superior to ICNN. The matching time is much shorter than that of JCBB. This shows that the proposed method can achieve both better matching accuracy and matching time than JCBB and ICNN, verifying that the proposed method can be used in unknown environments with dense features.

Claims

1. An iterative classification matching data association method adapted to complex environments, characterized by: The steps include: S1: Use the standard joint compatible branch and bound algorithm to perform the initial association and transform the sensor measurement feature dataset Z t and map feature dataset F t Divided into two groups, the features that are successfully associated for the first time constitute the dataset Z t + and F t + , the features that failed to be associated for the first time constitute the dataset Z t - and F t - ; S2: According to data set Z t + and F t + , solve the least squares matching vector Θ; S3: Least squares matching vector Θ to dataset F t - Update and get the updated data set S4: Dataset Z t - and the updated dataset Combine into a new input and iterate steps S1 to S3 until the mean square error test is satisfied, and the iteration ends.

2. The iterative classification matching data association method adapted to complex environments according to claim 1, characterized in that: The step S1 is specifically as follows: Let vector F t Represents the state of n features contained in the known local map at time t: F t =[f1,f2,…,f n ] T (1) Let vector Z t Represents the m observations obtained by the sensor at time t: Z t =[O1,O2,…,O m ] T (2) Based on the known local map features and the sensor observations at time t, an association hypothesis can be given: H t ={c1,c2,…,c i ,…,c m-1 ,c m } (3) where c i Indicates observation o i With feature f ci Matches if c i = 0, it means that there is no local map that matches the observation o i Matching features; Association hypothesis H t The predicted observations for the features involved are as follows: Where h(·) is the robot observation equation, R^ is the robot's predicted position and posture, and the vector It is H t The predicted status of the features in ; Then the simultaneous equations are obtained from the above formula: In the formula represents actual observation; The covariance matrix of the simultaneous equations is: In the formula is the Jacobian matrix; is the forecast error covariance matrix; is the observation error covariance matrix; From (5) and (6), the joint compatibility condition can be obtained as follows: in is the joint Mahalanobis distance; represents a chi-square distribution with degrees of freedom The confidence level is α; if If formula (7) is satisfied, then H t All matching pairs in is joint compatible; Finally, the sensor measures the feature dataset Z t and map feature dataset F t Classified into two groups: Features that are jointly compatible are stored in the new feature set Z t + and F t + Those that do not meet the requirements are stored in the feature set Z t - and F t - middle.

3. The iterative classification matching data association method adapted to complex environments according to claim 1, characterized in that: In step S1, an m-layer interpretation tree model is used to represent all possible association hypotheses at time t.

4. The iterative classification matching data association method adapted to complex environments according to claim 1, characterized in that: In step S2, the least squares matching vector Θ is solved using a quaternion method, where the least squares matching vector Θ is composed of a rotation matrix and a translation matrix; If you use q R =[q0 q1 q2 q3] T Represents a unit rotation quaternion vector, where q0≥0, Then the quaternion vector q is rotated by the above unit R The resulting 3×3 rotation matrix can be written as: If you use q T =[q4 q5 q6] T represents a translation matrix, then the entire least squares matching vector can be expressed as Θ=[q R ,q T ] T .

5. The iterative classification matching data association method adapted to complex environments according to claim 4, characterized in that: The specific solution process of the least square matching vector Θ in step S2 is: Known Represents the successfully associated sensor measurement feature set, and its corresponding map feature set is Where i = 1,…,K, then the mean square matching error can be expressed as: like and The point set O that represents the successfully associated sensor measurement features and their corresponding map features + and F + The center of , then: Then the feature set Z t + and F t + The cross-covariance matrix P can be expressed as: It is known that the antisymmetric matrix Λ=PP T The cyclic periodic components are used to form the column vector Δ=[Λ 23 Λ 31 Λ 12 ] T , then the symmetric matrix Γ can be expressed as: Where tr(P) represents the trace of the cross-covariance matrix P, that is, the sum of all elements on the main diagonal of the matrix P, and I3 represents the 3×3 identity matrix; The unit eigenvector corresponding to the largest eigenvalue of the symmetric matrix Γ is finally selected as the optimal rotation vector q R =[q0q1q2q3] T , and the optimal translation vector can be expressed as: If θ is used to represent the least squares quaternion operator, the entire least squares matching process can be summarized as: (Θ,E)=θ(Z t + ,F t + ) (15).

6. The iterative classification matching data association method adapted to complex environments according to claim 5, characterized in that: The step S3 is specifically as follows: The least squares matching vector Θ = [q R ,q T ] T It is used to select the map feature set F that does not meet the nearest neighbor selection rule, that is, the map feature set F that is not successfully associated. t - Make updates; in, represents the set of unsuccessfully associated map features after being updated by the least squares matching vector; If ζ is used to represent the least squares matching update operator, the entire least squares matching update process can be summarized as: in, Represents the mean square matching error in the matching update phase.

7. The iterative classification matching data association method adapted to complex environments according to claim 1, characterized in that: The mean square error detection in step S4 is: The entire iterative process will terminate when the difference in the mean square error before and after two updates is less than the pre-set threshold τ, τ>0; If the difference in mean square error does not satisfy the above formula, then set Z t =Z t - , F t =F t - To continue the above iterative data association process.

Images