CN110427520B - SLAM data association method based on adaptive local and grouping association strategy - Google Patents

SLAM data association method based on adaptive local and grouping association strategy Download PDF

Info

Publication number
CN110427520B
CN110427520B CN201910599928.9A CN201910599928A CN110427520B CN 110427520 B CN110427520 B CN 110427520B CN 201910599928 A CN201910599928 A CN 201910599928A CN 110427520 B CN110427520 B CN 110427520B
Authority
CN
China
Prior art keywords
group
association
local
observation
representing
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910599928.9A
Other languages
Chinese (zh)
Other versions
CN110427520A (en
Inventor
罗元
杨成杰
张毅
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chongqing University of Post and Telecommunications
Original Assignee
Chongqing University of Post and Telecommunications
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Chongqing University of Post and Telecommunications filed Critical Chongqing University of Post and Telecommunications
Priority to CN201910599928.9A priority Critical patent/CN110427520B/en
Publication of CN110427520A publication Critical patent/CN110427520A/en
Application granted granted Critical
Publication of CN110427520B publication Critical patent/CN110427520B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/90—Details of database functions independent of the retrieved data types
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • G06F18/232—Non-hierarchical techniques
    • G06F18/2321—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions

Landscapes

  • Engineering & Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • General Engineering & Computer Science (AREA)
  • Databases & Information Systems (AREA)
  • Automation & Control Theory (AREA)
  • Probability & Statistics with Applications (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention requests to protect a SLAM data association method based on self-adaptive local and grouping association strategies. The method comprises the following specific steps: firstly, a local data association area is determined in a self-adaptive mode according to the current position of the mobile robot, the measurement range of a sensor and the distribution condition of an observation value at the current time; then, adaptively determining the grouping number of the observation values according to the local association area and the number and distribution of the current observation values, and grouping the current observation values by using a Gaussian mixture clustering algorithm on the basis of the grouping number; then, using a joint compatible branch-and-bound algorithm to complete data association between the observed values in each group and the feature points in the local area and screening out the optimal association solution of each group; and finally, integrating the groups of optimal solutions to obtain a final data association result. Experimental results prove that the method can reduce the correlation calculation amount while ensuring high correlation accuracy.

Description

SLAM data association method based on self-adaptive local and grouping association strategy
Technical Field
The invention belongs to the field of autonomous navigation of mobile robots, and particularly relates to a SLAM data association method based on adaptive local and group association strategies.
Background
Meanwhile, positioning and mapping (SLAM) are the key points for realizing autonomous navigation of the mobile robot. The scheme is firstly proposed by Smith et al in 1986, and specifically, the mobile robot observes environmental characteristics in an unknown environment by using a sensor (such as a depth camera, a laser range finder, a sonar and the like) carried by the mobile robot, incrementally constructs a surrounding environment map according to an observed value, and estimates the pose of the mobile robot by using the constructed map.
Data association is a difficult point in the SLAM problem, and means that the mobile robot matches the observed value of the sensor at the current moment with the features in the known local map, so as to determine whether the observed value corresponds to the known features or the newly added features. The wrong data association can cause the deviation of mapping and positioning, and in severe cases, the whole SLAM algorithm can be diverged. At present, most SLAM data correlation algorithms are based on a threshold selection idea, wherein an independent compatible nearest neighbor algorithm and a joint compatible branch and bound algorithm are representative. The independent compatible nearest neighbor algorithm has the greatest advantage of small calculation amount, but ignores the correlation between the errors of the sensor prediction observed values, so that the correlation accuracy is low. The joint compatible branch-and-bound algorithm takes the correlation into account, so that higher correlation accuracy can be obtained. However, the calculation amount of the algorithm is exponential to the number of observed values, which results in poor real-time performance of the algorithm, and the problem is more serious in a multi-feature environment. The mainstream SLAM data association algorithm cannot realize high association accuracy and low association time at the same time, so that the practical application effect of the mobile robot is poor.
Disclosure of Invention
The present invention is directed to solving the above problems of the prior art. The SLAM data association method of the mobile robot based on the combined compatible branch-and-bound algorithm is capable of reducing association time while ensuring high association accuracy. The technical scheme of the invention is as follows:
a SLAM data association method based on adaptive local and packet association strategies, comprising the steps of:
s1, adaptively determining a local association area according to the current position of the robot, the measurement range of the sensor and the current observation value distribution;
s2, adaptively determining the grouping number of the observation values according to the local association area defined in S1 and the number and distribution of the current observation values;
s3, on the basis of the number of groups in S2, calculating the probability that each observation value belongs to each group by using a Gaussian mixture clustering algorithm, and classifying each observation value as the group with the maximum probability;
s4, completing data association between each group of observed values and local area features by using a combined compatible branch-and-bound algorithm, and selecting the association result with the minimum combined Mahalanobis distance from each group as the optimal solution of the group;
and S5, integrating the optimal solutions of each group to obtain a final SLAM data association result.
Further, step S1 is to adaptively determine the local association area according to the current position of the robot, the measurement range of the sensor, and the current observation value distribution, and the specific steps are as follows:
s11: at the time t, the robot calculates to obtain the current position of the robot according to the self position of the robot at the time t-1 and internal control information from the time t-1 to the time t;
s12: calculating the compensation distance of the local correlation area according to the following formula:
d＝d min +αpr
wherein d is min 0.1 m, representing the minimum compensation distance; alpha represents a proportional adjustment coefficient and the value range [0.050.1 ]]The denser the environmental characteristics are, the larger the value of alpha is; r represents the effective measuring distance of the sensor; p represents the proportion of the number of the observed values of which the distance between the current moment and the sensor is more than 90% r to the total number of the observed values, and the value of each moment p is related to the distribution of the observed values.
S13: finally determining a local association area according to the current position of the mobile robot, the measurement range of the sensor and the compensation distance calculated in the step S12, wherein the formula is as follows:
(x R -x f ) 2 +(y R -y f ) 2 ≤(r+d) 2
wherein (x) R ,y R ) Coordinates (x) representing the mobile robot at the current time f ,y f ) The coordinate of a certain characteristic point in a known map is represented, r represents the effective measuring distance of the sensor, and d represents the compensation distance.
Further, step S2 is to adaptively determine the grouping number of the observation values according to the local association area defined in step S1 and the number and distribution of the current observation values, and the specific steps are as follows:
s21: the local data association region is divided into 8 parts, and is marked as { Q 1 ,Q 2 ,...,Q 7 ,Q 8 }；
S22: counting the number of observed values in each part at the current moment, and recording the number as { m 1 ,m 2 ,...,m 7 ,m 8 }；
S23: the number m of observed values of each part i (i is more than or equal to 1 and less than or equal to 8) are sorted from large to small;
s24: obtaining the difference value between the number of two adjacent observed values after sequencing;
s25: assuming number m of adjacent observations after sorting a And m b The difference value between the two is the largest, wherein a is more than or equal to 1 and less than or equal to 8, b is more than or equal to 1 and less than or equal to 8, and a is not equal to b;
if m i ≥m a Then the corresponding Q is set i Marking as dense parts; if m i ≤m b Then the corresponding Q is set i Marking as a sparse portion;
s26: and combining continuous dense parts in the local association area into larger dense parts, wherein the number of the dense parts is the grouping number K of the observation value at the current moment.
Further, in step S3, based on the number of groups in S2, the probability that each observation value belongs to each group is calculated by using a gaussian mixture clustering algorithm, and each observation value is classified as the group with the highest probability, which includes the specific steps of:
s31: calculating the posterior probability that a certain observation value belongs to a certain group, wherein the specific formula is as follows:
Figure BDA0002118939620000031
wherein K represents the number of packets; k (1. ltoreq. K. ltoreq.K) represents a group number; mu.s k And mu j Respectively representing the mean values of observed values of the kth group and the jth group; sigma k Sum Σ j Covariance representing the k-th and j-th group of observations, respectively; pi k And pi j Representing the weights of the kth group and the jth group respectively; o. o i Representing an ith observation of the current observations; z is a radical of ik Represents the observed value o i Belong to group k; n represents a gaussian distribution.
S32: updating pi according to posterior probability in S31 k ，μ k Sum Σ k The concrete formula is as follows:
Figure BDA0002118939620000032
Figure BDA0002118939620000033
Figure BDA0002118939620000041
in this step, p (z) ik ) Representing the observed value o i Posterior probability belonging to the kth group; m represents the total number of observations at the current time;
Figure BDA0002118939620000042
representing the number of observations in the kth group;
s33: and steps S31 and S32 are iterated until the algorithm converges, the probability that each observation value belongs to each group is obtained, and each observation value is classified into the group with the maximum probability.
Further, step S4 is to complete data association between each group of observation values and local area features by using a joint compatible branch-and-bound algorithm, and select an association result with the minimum joint Mahalanobis distance from each group as the optimal solution for the group, where the specific steps are as follows:
s41: acquiring the states of n feature points contained in the local association region at the time t, and recording the states as:
F t ＝[f 1 ,f 2 ,...,f n ] T
s42: acquiring m sensor observation values interfered by noise in a certain group at the time t, and recording the m sensor observation values as:
Z t ＝[o 1 ,o 2 ,...,o m ] T
s43: generating an interpretation tree according to the feature points in the local data association area and the sensor observation values in the group, wherein the interpretation tree is composed of all possible association assumptions, and the assumption that there is a certain association assumption is that:
H t ＝{c 1 ,c 2 ,...,c i ,...,c m-1 ,c m }
in the formula c i Representing the observed value o i And characteristic point
Figure BDA0002118939620000043
Match if c i When 0, it means that there is no feature point and the observed value o i Matching;
s44: computing association hypothesis H t The specific formula of the prediction observation value of the feature point is as follows:
Figure BDA0002118939620000044
in the above formula, h (-) represents the robot observation equation;
Figure BDA0002118939620000045
representing a predicted pose of the robot ontology;
Figure BDA0002118939620000046
representing a predicted state involving the feature points;
s45: calculating the joint innovation and the covariance thereof, wherein the formulas are respectively as follows:
Figure BDA0002118939620000051
Figure BDA0002118939620000052
wherein the content of the first and second substances,
Figure BDA0002118939620000053
is a combined innovation;
Figure BDA0002118939620000054
covariance as a joint innovation;
Figure BDA0002118939620000055
Figure BDA0002118939620000056
is a Jacobian matrix;
Figure BDA0002118939620000057
is a prediction error covariance matrix;
Figure BDA0002118939620000058
is an observation error covariance matrix.
S46: according to joint innovation
Figure BDA0002118939620000059
And its covariance
Figure BDA00021189396200000510
Determining a joint compatibility criterion as follows:
Figure BDA00021189396200000511
wherein the content of the first and second substances,
Figure BDA00021189396200000512
representing the joint Mahalanobis distance,
Figure BDA00021189396200000513
representing a degree of freedom of
Figure BDA00021189396200000514
Confidence level is the chi-square distribution of α if
Figure BDA00021189396200000515
If the condition is satisfied, then hypothesis H is correlated t All the matching pairs
Figure BDA00021189396200000516
Are jointly compatible;
s47: taking a joint compatibility judgment criterion as a limiting condition, and searching the interpretation tree incrementally from top to bottom by adopting a branch-and-bound search method until the bottom of the tree is reached;
s48: after the search is finished, selecting the association hypothesis with the minimum distance of the combined Mahalanobis as the optimal solution of the group;
s49: and respectively carrying out operations from S41 to S48 on the observed values in each of the rest groups to finally obtain the optimal association solution of each group.
Further, the step S5 integrates the sets of optimal solutions to obtain a final SLAM data association result.
The invention has the following advantages and beneficial effects:
the invention provides an optimized data association method aiming at the problem that the existing mainstream SLAM data association method of a mobile robot cannot realize high association accuracy and low association time at the same time. The method adopts a local data association strategy. And determining a local association area according to the current position of the robot, the measurement range of the sensor and the current observation value distribution, and performing data association operation in the area, thereby reducing the calculation overhead. In addition, the range of the local association region related in the invention can be adaptively scaled according to the observed value distribution at each moment, and the useful feature points are not omitted while the useless feature points are not included, so that the high association accuracy is ensured. The method also adopts a grouping association strategy, the data association time can be reduced by observing the grouping association, and whether the grouping is in accordance with the logical relation to the association correct rate or not is judged. In the invention, the grouping number is adaptively determined according to the local association area and the number and distribution of the current observed values, and then the grouping of the observed values is completed by using a Gaussian mixture clustering algorithm on the basis of the grouping number. This ensures that the packets at each time are in accordance with the distribution of the observed values at each time, and ensures the correct rate of association.
Drawings
FIG. 1 is a mobile robot SLAM data association method implementation framework (including a boundary process) based on an adaptive local association policy and an adaptive packet association policy in accordance with a preferred embodiment of the present invention;
FIG. 2 is a schematic diagram of a local data association area;
FIG. 3 is a schematic diagram of local data association region segmentation;
fig. 4 is a diagram for explaining a tree model.
Detailed Description
The technical solutions in the embodiments of the present invention will be described in detail and clearly with reference to the accompanying drawings. The described embodiments are only some of the embodiments of the present invention.
The technical scheme for solving the technical problems is as follows:
the method mainly comprises the following steps:
s1, adaptively determining a local association area according to the current position of the robot, the measurement range of the sensor and the current observation value distribution;
s2, adaptively determining the grouping number of the observation values according to the local association area defined in S1 and the number and distribution of the current observation values;
s3, on the basis of the number of groups in S2, calculating the probability that each observation value belongs to each group by using a Gaussian mixture clustering algorithm, and classifying each observation value as the group with the maximum probability;
s4, completing data association between each group of observed values and local area features by using a combined compatible branch-and-bound algorithm, and selecting the association result with the minimum combined Mahalanobis distance from each group as the optimal solution of the group;
and S5, integrating the optimal solutions of each group to obtain a final SLAM data association result.
The specific implementation process of step S1 is:
s11: at the time t, the robot calculates to obtain the current position of the robot according to the self position of the robot at the time t-1 and internal control information from the time t-1 to the time t;
s12: calculating the compensation distance of the local correlation area according to the following formula:
d＝d min +αpr
wherein d is min 0.1 meter, representing the minimum compensation distance; alpha represents a proportional adjustment coefficient and the value range [0.050.1 ]]The denser the environmental characteristics are, the larger the value of alpha is; r represents the effective measuring distance of the sensor; p represents the proportion of the number of the observed values of which the distance between the current moment and the sensor is more than 90% r to the total number of the observed values, and the value of each moment p is related to the distribution of the observed values.
S13: finally, the local association area is determined according to the current time position of the mobile robot, the measurement range of the sensor and the compensation distance calculated in S12, as shown in fig. 2, the formula is as follows:
(x R -x f ) 2 +(y R -y f ) 2 ≤(r+d) 2
wherein (x) R ,y R ) Coordinates (x) representing the mobile robot at the current time f ,y f ) The coordinate of a certain characteristic point in a known map is represented, r represents the effective measuring distance of the sensor, and d represents the compensation distance.
The specific implementation process of step S2 is:
s21: the local association region is divided into 8 parts according to the average shown in FIG. 3, and is marked as { Q 1 ,Q 2 ,...,Q 7 ,Q 8 }；
S22: counting the number of observed values in each part at the current moment, and recording the number as { m 1 ,m 2 ,...,m 7 ,m 8 }；
S23: the number m of observed values of each part i (i is more than or equal to 1 and less than or equal to 8) are sorted from large to small;
s24: obtaining the difference value between the number of two adjacent observed values after sequencing;
s25: assuming number m of adjacent observations after sorting a And m b The difference value between the two is the largest, wherein a is more than or equal to 1 and less than or equal to 8, b is more than or equal to 1 and less than or equal to 8, and a is not equal to b;
if m i ≥m a Then the corresponding Q is set i Marking as dense parts; if m i ≤m b Then the corresponding Q is set i Marking as a sparse portion;
s26: and combining continuous dense parts in the local association area into larger dense parts, wherein the number of the dense parts is the grouping number K of the observation value at the current moment.
The specific implementation process of step S3 is:
s31: calculating the posterior probability that a certain observation value belongs to a certain group, wherein the specific formula is as follows:
Figure BDA0002118939620000081
wherein K represents the number of packets; k (1. ltoreq. K. ltoreq.K) represents a group number; mu.s k And mu j Respectively representing the mean values of observed values of the kth group and the jth group; sigma k Sum Σ j Covariance representing the k-th and j-th group of observations, respectively; pi k And pi j Respectively representing the weights of the kth group and the jth group; o i Representing an ith observation of the current observations; z is a radical of ik Representing the observed value o i Belong to the kth group; n represents a gaussian distribution.
S32: updating pi according to posterior probability in S31 k ，μ k Sum Σ k The concrete formula is as follows:
Figure BDA0002118939620000082
Figure BDA0002118939620000083
Figure BDA0002118939620000084
in this step, p (z) ik ) Representing the observed value o i Posterior probability belonging to the kth group; m represents the total number of observations at the current time;
Figure BDA0002118939620000085
representing the number of observations already in the kth group;
s33: and steps S31 and S32 are iterated until the algorithm converges, the probability that each observation value belongs to each group is obtained, and each observation value is classified into the group with the maximum probability.
The specific implementation process of step S4 is:
s41: acquiring the states of n feature points contained in the local association region at the time t, and recording the states as:
F t ＝[f 1 ,f 2 ,...,f n ] T
s42: acquiring m sensor observation values interfered by noise in a certain group at the time t, and recording the m sensor observation values as:
Z t ＝[o 1 ,o 2 ,...,o m ] T
s43: an interpretation tree is generated according to the feature points in the local data association region and the sensor observation values in the group, as shown in fig. 4, the interpretation tree is composed of all possible association hypotheses, and it is assumed that there is a certain association hypothesis:
H t ＝{c 1 ,c 2 ,...,c i ,...,c m-1 ,c m }
in the formula c i Represents the observed value o i And feature point f ci Match if c i When 0, it means that there is no feature point or observation value o i Matching;
s44: computing association hypothesis H t The specific formula of the prediction observation value of the related characteristic point is as follows:
Figure BDA0002118939620000091
in the above formula, h (-) represents the robot observation equation;
Figure BDA0002118939620000092
representing a predicted pose of the robot ontology;
Figure BDA0002118939620000093
representing a predicted state involving the feature points;
s45: calculating the joint innovation and the covariance thereof, wherein the formulas are respectively as follows:
Figure BDA0002118939620000094
Figure BDA0002118939620000095
wherein the content of the first and second substances,
Figure BDA0002118939620000096
is a combined innovation;
Figure BDA0002118939620000097
covariance as a joint innovation;
Figure BDA0002118939620000098
Figure BDA0002118939620000099
is a Jacobian matrix;
Figure BDA00021189396200000910
is a prediction error covariance matrix;
Figure BDA00021189396200000911
is an observation error covariance matrix.
S46: according to joint innovation
Figure BDA00021189396200000912
And its covariance
Figure BDA00021189396200000913
Determining a joint compatibility criterion as follows:
Figure BDA00021189396200000914
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA00021189396200000915
representing the joint Mahalanobis distance,
Figure BDA00021189396200000916
representing a degree of freedom of
Figure BDA00021189396200000917
Confidence level is the chi-square distribution of α if
Figure BDA00021189396200000918
If the condition is satisfied, then hypothesis H is correlated t All the matching pairs in
Figure BDA00021189396200000919
Are jointly compatible;
s47: taking a joint compatibility judgment criterion as a limiting condition, and searching the interpretation tree incrementally from top to bottom by adopting a branch-and-bound search method until the bottom of the tree is reached;
s48: after the search is finished, selecting the association hypothesis with the minimum distance of the combined Mahalanobis as the optimal solution of the grouping;
s49: and respectively carrying out operations from S41 to S48 on the observed values in each group to finally obtain the optimal association solution of each group.
And after all the steps are completed, integrating all the groups of optimal solutions to obtain a final SLAM data association result at the current time.
The above examples are to be construed as merely illustrative and not limitative of the remainder of the disclosure. After reading the description of the invention, the skilled person can make various changes or modifications to the invention, and these equivalent changes and modifications also fall into the scope of the invention defined by the claims.

Claims (4)

1. A SLAM data association method based on adaptive local and packet association strategies is characterized by comprising the following steps:
s1, adaptively determining a local association area according to the current position of the robot, the measurement range of the sensor and the current observation value distribution;
s2, adaptively determining the grouping number of the observation values according to the local association area defined in S1 and the number and the distribution of the current observation values;
s3, on the basis of the number of groups in S2, calculating the probability that each observation value belongs to each group by using a Gaussian mixture clustering algorithm, and classifying each observation value as the group with the maximum probability;
s4, completing data association between each group of observed values and local area features by using a combined compatible branch-and-bound algorithm, and selecting the association result with the minimum combined Mahalanobis distance from each group as the optimal solution of the group;
s5, integrating the optimal solutions to obtain a final SLAM data association result;
step S1 is to adaptively determine a local association area according to the current position of the robot, the measurement range of the sensor, and the current observation value distribution, and the specific steps are as follows:
s11: at the time t, the robot calculates to obtain the current position of the robot according to the self position of the robot at the time t-1 and internal control information from the time t-1 to the time t;
s12: calculating the compensation distance of the local correlation area according to the following formula:
d＝d min +αpr
wherein d is min 0.1 meter, representing the minimum compensation distance; alpha represents a proportional adjustment coefficient and the value range [0.050.1 ]]The denser the environmental characteristics are, the larger the value of alpha is; r represents the effective measuring distance of the sensor; p represents the proportion of the number of observed values with the distance between the current moment and the sensor larger than 90% r to the total number of the observed values, and the value of each moment p is related to the distribution of the observed values;
s13: finally determining a local association area according to the current position of the mobile robot, the measurement range of the sensor and the compensation distance calculated in the step S12, wherein the formula is as follows:
(x R -x f ) 2 +(y R -y f ) 2 ≤(r+d) 2
wherein (x) R ,y R ) Coordinates (x) representing the mobile robot at the current time f ,y f ) The coordinate of a certain characteristic point in a known map is represented, r represents the effective measurement distance of the sensor, and d represents the compensation distance;
step S2 is to adaptively determine the number of groups of the observation values according to the local association area defined in step S1 and the number and distribution of the current observation values, and the specific steps are as follows:
s21: the local data association region is divided into 8 parts, and is marked as { Q 1 ,Q 2 ,...,Q 7 ,Q 8 }；
S22: counting the number of observed values in each part at the current moment, and recording the number as { m 1 ,m 2 ,...,m 7 ,m 8 }；
S23: the number m of observed values of each part i (i is more than or equal to 1 and less than or equal to 8) are sorted from large to small;
s24: obtaining the difference value between the number of two adjacent observed values after sequencing;
s25: assuming number m of adjacent observations after sorting a And m b The difference between is maximum, whereinA is more than or equal to 1 and less than or equal to 8, b is more than or equal to 1 and less than or equal to 8, and a is not equal to b;
if m i ≥m a Then the corresponding Q is set i Marking as dense parts; if m i ≤m b Then the corresponding Q is set i Marking as a sparse portion;
s26: and combining continuous dense parts in the local association area into larger dense parts, wherein the final number of the dense parts is the group number K of the observation value at the current moment.
2. The SLAM data association method based on the adaptive local and group association strategy of claim 1, wherein the step S3 is based on the number of groups in S2, and calculates the probability that each observation value belongs to each group by using a Gaussian mixture clustering algorithm, and classifies each observation value as the group with the highest probability, and the specific steps are as follows:
s31: calculating the posterior probability that a certain observation value belongs to a certain group, wherein the specific formula is as follows:
Figure FDA0003661924890000021
wherein K represents the number of packets; k (1. ltoreq. K. ltoreq.K) represents a group number; mu.s k And mu j Respectively representing the mean values of the k-th group observation value and the j-th group observation value; sigma k Sum Σ j Covariance representing the k-th and j-th group of observations, respectively; pi k And pi j Representing the weights of the kth group and the jth group respectively; o i Representing an ith observation of the current observations; z is a radical of ik Representing the observed value o i Belong to group k; n represents a Gaussian distribution;
s32: respectively updating pi according to the posterior probability in S31 k ，μ k Sum Σ k The concrete formula is as follows:
Figure FDA0003661924890000022
Figure FDA0003661924890000031
Figure FDA0003661924890000032
in this step, p (z) ik ) Representing the observed value o i Posterior probability belonging to the kth group; m represents the total number of observations at the current time;
Figure FDA0003661924890000033
representing the number of observations in the kth group;
s33: and steps S31 and S32 are iterated until the algorithm converges, the probability that each observation value belongs to each group is obtained, and each observation value is classified into the group with the maximum probability.
3. The SLAM data association method based on the adaptive local and grouped association strategies according to claim 2, wherein the step S4 is implemented by using a joint compatible branch-and-bound algorithm to complete data association between each group of observed values and local area features, and selecting the association result with the minimum joint Mahalanobis distance in each group as the optimal solution of the group, and the specific steps are as follows:
s41: acquiring the states of n feature points contained in the local association region at the time t, and recording the states as:
F t ＝[f 1 ,f 2 ,...,f n ] T
s42: acquiring m sensor observed values interfered by noise in a certain group at the time t, and recording the m sensor observed values as:
Z t ＝[o 1 ,o 2 ,...,o m ] T
s43: generating an interpretation tree according to the feature points in the local data association area and the sensor observation values in the group, wherein the interpretation tree is composed of all possible association assumptions, and the assumption that a certain association assumption exists is that:
H t ＝{c 1 ,c 2 ,...,c i ,...,c m-1 ,c m }
in the formula c i Representing the observed value o i And characteristic point
Figure FDA0003661924890000034
Match if c i When 0, it means that there is no feature point and the observed value o i Matching;
s44: computing association hypothesis H t The specific formula of the prediction observation value of the feature point is as follows:
Figure FDA0003661924890000035
in the above formula, h (-) represents the robot observation equation;
Figure FDA0003661924890000036
representing a predicted pose of the robot ontology;
Figure FDA0003661924890000041
representing a predicted state involving the feature points;
s45: calculating the combined innovation and the covariance thereof, wherein the formulas are respectively as follows:
Figure FDA0003661924890000042
Figure FDA0003661924890000043
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003661924890000044
is a combined innovation;
Figure FDA0003661924890000045
covariance as a joint innovation;
Figure FDA0003661924890000046
Figure FDA0003661924890000047
is a Jacobian matrix;
Figure FDA0003661924890000048
is a prediction error covariance matrix;
Figure FDA0003661924890000049
is an observation error covariance matrix;
s46: according to joint innovation
Figure FDA00036619248900000410
And its covariance
Figure FDA00036619248900000411
Determining a joint compatibility criterion as follows:
Figure FDA00036619248900000412
wherein the content of the first and second substances,
Figure FDA00036619248900000413
representing the joint Mahalanobis distance,
Figure FDA00036619248900000414
representing a degree of freedom of
Figure FDA00036619248900000415
The confidence level is the chi-square distribution of α, if
Figure FDA00036619248900000416
If the condition is satisfied, then the hypothesis H is correlated t All the matching pairs
Figure FDA00036619248900000417
Are jointly compatible;
s47: taking a joint compatibility judgment criterion as a limiting condition, and searching the interpretation tree incrementally from top to bottom by adopting a branch-and-bound search method until the bottom of the tree is reached;
s48: after the search is finished, selecting the association hypothesis with the minimum distance of the combined Mahalanobis as the optimal solution of the grouping;
s49: and respectively carrying out operations from S41 to S48 on the observed values in each of the rest groups to finally obtain the optimal association solution of each group.
4. The SLAM data association method based on the adaptive local and grouping association strategy of claim 3, wherein the step S5 integrates the optimal solutions of each group to obtain the final SLAM data association result.
CN201910599928.9A 2019-07-04 2019-07-04 SLAM data association method based on adaptive local and grouping association strategy Active CN110427520B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910599928.9A CN110427520B (en) 2019-07-04 2019-07-04 SLAM data association method based on adaptive local and grouping association strategy

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910599928.9A CN110427520B (en) 2019-07-04 2019-07-04 SLAM data association method based on adaptive local and grouping association strategy

Publications (2)

Publication Number Publication Date
CN110427520A CN110427520A (en) 2019-11-08
CN110427520B true CN110427520B (en) 2022-08-23

Family

ID=68408932

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910599928.9A Active CN110427520B (en) 2019-07-04 2019-07-04 SLAM data association method based on adaptive local and grouping association strategy

Country Status (1)

Country Link
CN (1) CN110427520B (en)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104966123A (en) * 2015-07-16 2015-10-07 北京工业大学 SLAM data association method based on fuzzy-self-adaptation
CN107590827A (en) * 2017-09-15 2018-01-16 重庆邮电大学 A kind of indoor mobile robot vision SLAM methods based on Kinect
CN108139225A (en) * 2015-07-29 2018-06-08 大众汽车有限公司 Determine the layout information of motor vehicle
CN108287550A (en) * 2018-02-01 2018-07-17 速感科技（北京）有限公司 The method of SLAM systems and construction data correlation based on data correlation and error detection
WO2018142395A1 (en) * 2017-01-31 2018-08-09 Arbe Robotics Ltd A radar-based system and method for real-time simultaneous localization and mapping
CN109241228A (en) * 2018-09-04 2019-01-18 山东理工大学 A kind of multiple mobile robot's cooperation synchronous superposition strategy
CN109459033A (en) * 2018-12-21 2019-03-12 哈尔滨工程大学 A kind of robot of the Multiple fading factor positions without mark Fast synchronization and builds drawing method
CN109572857A (en) * 2018-12-26 2019-04-05 石家庄铁道大学 A kind of Mecanum wheel intelligent storage AGV and its paths planning method

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10019801B2 (en) * 2016-03-11 2018-07-10 Kabushiki Kaisha Toshiba Image analysis system and method

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104966123A (en) * 2015-07-16 2015-10-07 北京工业大学 SLAM data association method based on fuzzy-self-adaptation
CN108139225A (en) * 2015-07-29 2018-06-08 大众汽车有限公司 Determine the layout information of motor vehicle
WO2018142395A1 (en) * 2017-01-31 2018-08-09 Arbe Robotics Ltd A radar-based system and method for real-time simultaneous localization and mapping
CN107590827A (en) * 2017-09-15 2018-01-16 重庆邮电大学 A kind of indoor mobile robot vision SLAM methods based on Kinect
CN108287550A (en) * 2018-02-01 2018-07-17 速感科技（北京）有限公司 The method of SLAM systems and construction data correlation based on data correlation and error detection
CN109241228A (en) * 2018-09-04 2019-01-18 山东理工大学 A kind of multiple mobile robot's cooperation synchronous superposition strategy
CN109459033A (en) * 2018-12-21 2019-03-12 哈尔滨工程大学 A kind of robot of the Multiple fading factor positions without mark Fast synchronization and builds drawing method
CN109572857A (en) * 2018-12-26 2019-04-05 石家庄铁道大学 A kind of Mecanum wheel intelligent storage AGV and its paths planning method

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
A Novel Association Approach for SLAM of Mobile Robot;yidong Du 等;《2018 37th Chinese Control Conference》;20181008;4744-4749 *
SLAM Data Association of Mobile Robot Based on Improved JCBB Algorithm;Chengjie Yang 等;《2020 IEEE 5th Imformation Technology and Mechatronics Engineering Conference》;20200716;363-368 *
一种基于聚类分组的快速联合兼容SLAM数据关联算法;刘丹 等;《机器人》;20180315;第40卷(第02期);158-168，177 *
基于嵌入式平台和子地图局部关联算法的AGV设计;胡佳辉 等;《电子测量技术》;20190608;第42卷(第11期);51-55 *
基于高斯分布重采样的Rao-Blackwellized粒子滤波SLAM算法;罗元 等;《控制与决策》;20161011;第31卷(第12期);2299-2304 *
室内多障碍物环境下移动机器人自主导航***的研究;杨成杰;《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》;20210215(第02期);I140-808 *

Also Published As

Publication number Publication date
CN110427520A (en) 2019-11-08

Similar Documents

Publication Publication Date Title
CN110514206B (en) Unmanned aerial vehicle flight path prediction method based on deep learning
CN111444767B (en) Pedestrian detection and tracking method based on laser radar
CN110726415B (en) Self-adaptive underwater multi-beam synchronous positioning and mapping method
CN113238232B (en) Target searching method of autonomous underwater vehicle system for ocean static target
CN110986956A (en) Autonomous learning global positioning method based on improved Monte Carlo algorithm
CN117314891B (en) Optical lens surface defect detection method and system based on image processing
CN113903395A (en) BP neural network copy number variation detection method and system for improving particle swarm optimization
CN114299442A (en) Pedestrian re-identification method and system, electronic equipment and storage medium
CN111930844A (en) Financial prediction system based on block chain and artificial intelligence
Leung et al. An improved weighting strategy for rao-blackwellized probability hypothesis density simultaneous localization and mapping
CN116912568A (en) Noise-containing label image recognition method based on self-adaptive class equalization
CN110427520B (en) SLAM data association method based on adaptive local and grouping association strategy
CN109492816B (en) Coal and gas outburst dynamic prediction method based on hybrid intelligence
CN117237902B (en) Robot character recognition system based on deep learning
CN111275748B (en) Point cloud registration method based on laser radar in dynamic environment
CN115932913B (en) Satellite positioning pseudo-range correction method and device
CN108280548A (en) Intelligent processing method based on network transmission
CN116563341A (en) Visual positioning and mapping method for processing dynamic object in complex environment
Grisetti et al. Speeding-up rao-blackwellized SLAM
CN116242381A (en) Path planning method, system, electronic equipment and storage medium
Xi et al. Simulation research for active simultaneous localization and mapping based on extended kalman filter
CN112419362A (en) Moving target tracking method based on prior information feature learning
Burguera Cluster-based scan matching for robust motion estimation and loop closing
CN1965332A (en) Method and system for multi-modal component-based tracking of an object using robust information fusion
Singh et al. Feature weighting for improved classification of anuran calls

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant