A robust collaborative localization method for multiple robots

Abstract

The present invention discloses a method for robust collaborative positioning of multiple robots, which relates to the technical field of intelligent robots and can achieve correct collaborative positioning of multiple robots in the presence of a large number of erroneous data associations. The method comprises the following steps: for a system composed of multiple robots, wherein the multiple robots each obtain environmental information, each robot constructs a local map, each local map is composed of multiple sub-maps, and constraints between sub-maps are constructed; the constraints between sub-maps are composed of a number of matching point pairs. A vector filtering model is constructed to filter out erroneous constraints and point pairs. A hybrid probability model is constructed to calculate the correct probabilities of constraints and points in data associations from the perspectives of geometric distance, semantic association and vector consistency. A double-layer expectation maximization algorithm is used to determine the position and posture of the sub-maps and remove erroneous data associations to achieve collaborative positioning of multiple robots.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent robots, and in particular to a robust collaborative positioning method for multiple robots. Background Art

[0002] Compared to a single robot, multi-robot systems perform tasks more efficiently and robustly. In large-scale and GPS-denied environments, accurately estimating the relative positions of multiple robot systems is the foundation of multi-robot collaborative navigation. In this case, robots need to share their observations of the environment to establish data associations and estimate the relative positions of the robots. Given the massive amount of environmental data acquired by robots in large-scale environments and the limited bandwidth for data transmission between robots, the transmission of raw sensor data is inefficient. Therefore, sharing environmental sub-maps constructed by robots is generally used for efficient data transmission. In summary, existing collaborative localization methods generally achieve multi-robot collaborative localization by matching multiple sub-maps constructed by different robots.

[0003] In collaborative localization, the overlap in the environments observed by different robots is often low, and their perspectives vary significantly. This makes detecting the same scene traversed by multiple robots a challenging task. To match the observations of the environment across different robots, it is first necessary to characterize the environment using feature descriptors. However, even state-of-the-art feature descriptors are insufficient to uniquely characterize the environment, which inevitably leads to erroneous data associations. In collaborative localization, the large differences in perspectives between robots and the low overlap between submaps further exacerbate the occurrence of erroneous data associations. Existing collaborative localization methods often fail in the presence of numerous erroneous data associations.

[0004] Therefore, there is an urgent need to design a robust collaborative localization algorithm for large-scale, GPS-denied environments, which can achieve correct collaborative localization among multiple robots even in the presence of a large amount of erroneous data associations. Summary of the Invention

[0005] In view of this, the present invention provides a robust collaborative localization method for multiple robots, which can be applied to large-scale, GPS-denied environments and achieve correct collaborative localization among multiple robots in the presence of a large amount of erroneous data associations.

[0006] To achieve the above object, the technical solution of the present invention includes the following steps:

[0007] For a system composed of multiple robots, each robot obtains environmental information and constructs multiple sub-maps of the environment. Each robot constructs a local map, each local map consists of multiple sub-maps, and constraints are constructed between sub-maps; the constraints between sub-maps are composed of several matching point pairs.

[0008] A vector filtering model is constructed to filter out erroneous data associations, which include erroneous constraints and erroneous point pairs in correct constraints.

[0009] A hybrid probability model is constructed to calculate the correct probabilities of constraints and point pairs in data association from the perspectives of geometric distance, semantic association and vector consistency.

[0010] A two-layer expectation maximization algorithm determines the pose of the submap and removes erroneous constraints to achieve multi-robot collaborative localization.

[0011] Furthermore, in a system composed of multiple robots, for a local map constructed by one of the robots, there is a local map constructed by at least another robot, and there is an overlapping area between the two.

[0012] Furthermore, a vector filtering model is constructed to filter out erroneous constraints. The constructed vector filtering model includes a vector filtering model for constraints and a vector filtering model for point pairs.

[0013] The specific steps include:

[0014] Construct a vector filtering model for constraints to filter out erroneous constraints. The vector filtering model for constraints is specifically as follows:

[0015] For the kth submap created by robot h The lth submap established with robot j where p s ,p t For submap The point in q s ,q t For submap point in; define the flag variable v(s,t), and get the relationship of formula (1); if (p s ,q s ) and (p t ,q t ) are correct, then v(s,t)=1, and (p s ,q s ) and (p t ,q t ) is a consistent pair of vectors; otherwise v(s,t)=0.

[0016]

[0017] Where δ is a positive constant.

[0018] The correctness of a constraint or a matching point pair is estimated based on formula (1);

[0019] A constraint C is a submap and Loop Constraint Or an odometry constraint For every pair of points in C (p s ,q s ) and (p t ,q t ) calculates the value of v(s,t), then the sum of all v(s,t) in C is defined as

[0020]

[0021] where n C is the number of matching point pairs in C.

[0022] In the vector filtering model used for constraints, the odometry constraint is considered a correct loop constraint, a loop constraint The correct probability is based on To calculate, where Defined as all The average value of ; Formula (3) gives the correct probability of constraint C.

[0023]

[0024] in for The upper limit, ∈ for The upper limit, σ is set to

[0025] A vector filtering model for point pairs is constructed to filter out erroneous point pairs. The vector filtering model for point pairs is specifically as follows:

[0026] For a correct constraint C i Any matching point pair (p s ,q s ), define the number of point pairs that satisfy vector consistency as shown in formula (4)

[0027]

[0028] The larger the point pair (p s ,q s ) is correct, the greater the probability of the point pair being correct is, the correct probability of the point pair defined in formula (5) is

[0029]

[0030] where ∈1 is set to And σ1 is set to

[0031] This completes the modeling of constraints and point pairs in the vector filtering model.

[0032] Furthermore, a hybrid probability model is constructed to calculate the correct probabilities of constraints and points in data association from the perspectives of geometric distance, semantic association, and vector consistency, specifically:

[0033] Establish a problem model of co-location with incorrect data association: Let O, L, T 0 They are the odometry constraint set O, the loop constraint set L, and the submap initial pose set T. 0 ; Define two kinds of latent variables, latent variables about constraints and the latent variables about pairs of points Or 0 means is a true or false loop constraint, and or 0 means (p s ,q s ) is a true or false pair; let Z L and Z P They are and The set of Z={Z L ,Z P} collectively refers to all kinds of latent variables; then the co-location problem is modeled as the maximum likelihood estimation problem MLE of formula (6):

[0034] T=argmax p(O,L|T,Z) (6)

[0035] Where T represents the pose set of the submap.

[0036] The problem expressed in formula (6) is solved iteratively using the expectation maximization algorithm (EM algorithm), as shown in formula (7);

[0037]

[0038] In the E step of the EM algorithm, given O, L and T obtained by the i-th iteration (i)th (T (i)th is the set of submap poses at the i-th iteration), calculate p(Z|O,L,T (i)th ); In the M step of the EM algorithm, T is updated by maximizing the expectation of the likelihood function.

[0039] p(Z|O,L,T (i)th ) is split into three sub-functions of formula (8), where Z is replaced by and

[0040] p(Z|O,L,T (i)th )=p G (Z|O,L,T (i)th )·p S (Z|O,L,T (i)th )·p V (Z|O,L,T (i)th ) (8)

[0041] p G (Z|O,L,T (i)th ), p S (Z|O,L,T (i)th ), p V (Z|O,L,T (i)th ) are the probability values related to geometric distance, semantic information and vector consistency respectively. These three probability values are independent of each other. (i)th ) is expressed as the product of three probability values.

[0042] Next, we use loop constraints and point pairs Substitute Z respectively to get the probability expressions of the two sub-models:

[0043] (a) is the loop constraint Establish a mixed probability model, specifically:

[0044] For a loop constraint The three probability values in formula (8) are defined as follows.

[0045] Abbreviated as Its size and The sum of the spatial distances of the matching point pairs in ; is modeled as follows:

[0046]

[0047] in Positively correlated with the sum of squared distances between pairs of points, It is defined as follows (10): yes The number of midpoint pairs, Φ is a parameter;

[0048]

[0049] in The pose of the kth submap built for the hth robot, θ is a constant;

[0050] Abbreviated as Its size and The ratio of matching point pairs with the same semantic label in is related to the ratio defined as The larger the value of The greater the probability of being correct; It is defined as follows (11):

[0051]

[0052] Where τ is set to 0.1;

[0053] Abbreviated as Its definition is the same as formula (3) in the vector filtering model.

[0054] It is the product of equations (3), (9), and (11), which can be abbreviated as

[0055] (b) is a point pair Establish a mixed probability model, specifically:

[0056] For correct constraints A matching point pair (p s ,q s ), the three probability values in formula (8) will be defined as follows.

[0057] Abbreviated as Its size is similar to p s and q s The spatial distance is related; is modeled as follows:

[0058]

[0059] Abbreviated as When p s and q s The value is 1 when the semantic labels of p are the same. s and q s If the semantic labels are different, the value is 0.

[0060] Abbreviated as Its definition is the same as that of formula (5); for (p s ,q s ), if the number of vector pairs that are consistent with it is greater, then (p s ,q s )The greater the probability of being correct.

[0061] is Equation (5), Equation (12) and The product of

[0062] Furthermore, a two-layer expectation maximization algorithm determines the pose of the submap and removes incorrect constraints to achieve multi-robot collaborative localization. Specifically:

[0063] (a) The first-level expectation maximization algorithm is also known as the first-level EM algorithm

[0064] In the first layer of the EM algorithm, the poses of the submaps are not aligned, and the latent variables of the constraints are

[0065] In the E-step of the first-level EM algorithm: calculate the correct probability of the loop constraint. Considering that the initial spatial distance between the submaps output by different robots is large, when the E-step is executed for the first time, Cannot express The correctness of , based on this, the E-step formula of the first-level EM algorithm is rewritten as:

[0066]

[0067] In the M step of the first-level EM algorithm: is the weight of the objective function; the objective function is defined in Equation (14), and the submap pose is updated by minimizing the objective function; the odometry constraint is considered to be the correct constraint, and its weight is set to 1;

[0068]

[0069] (b) The second-level expectation maximization algorithm is the second-level EM algorithm

[0070] In the second layer EM algorithm, the input submap pose is obtained by the output of the first layer EM algorithm, and the point pair latent variable is The correct probability of the input loop constraint is greater than the threshold p0;

[0071] In the E step of the second-level EM algorithm: Based on formula (8), calculate each input point pair (p s ,q s )Probability of being correct:

[0072]

[0073] In the M step of the second-level EM algorithm: is the weight of the objective function of this layer. The pose of the submap is updated by minimizing the objective function. The objective function is defined as Equation (16). Considering that the wrong point pairs may exist in the loop constraint and the odometry constraint respectively, we define To express the constraints between the kth submap built by robot h and the lth submap built by robot j, define D as the set of all input constraints:

[0074]

[0075] The second-layer EM algorithm outputs the pose of the registered sub-map; the hierarchical EM algorithm stops iterating when equation (17) is satisfied:

[0076]

[0077] in is the threshold for judging whether it has converged.

[0078] After filtering out erroneous data associations and registering all sub-maps, the multi-robot collaborative localization task under extreme error conditions is completed, and the sub-map poses are finally output.

[0079] Beneficial effects:

[0080] 1: The present invention provides a method for robust collaborative positioning of multiple robots. First, a vector filtering model is used to filter out most erroneous data associations. Second, a hybrid probability model calculates the correct probabilities of constraints and points in data associations from the perspectives of geometric distance, semantic association, and vector consistency. Finally, a two-layer expectation maximization algorithm iteratively updates the pose of the calculated submap and removes erroneous data associations to achieve multi-robot collaborative positioning. The present invention designs a two-layer expectation maximization algorithm, in which the first and second layers use the correct probabilities of constraints and matching point pairs as latent variables, respectively, so as to simultaneously screen out erroneous loop constraints and erroneous matching point pairs in correct constraints, thereby improving the accuracy of multi-machine collaborative positioning under extreme error conditions.

[0081] 2: This paper first considers the geometric consistency of matching point pairs, considers the correctness of constraints and matching point pairs through a vector filtering model, and designs a hybrid probability model based on geometric space distance and semantic consistency. This model estimates the probability of correct data association from three perspectives: geometric distance, semantic consistency, and the vector filtering model. This hybrid probability model comprehensively estimates the correctness of constraints and matching point pairs from multiple perspectives, improving the robustness of multi-machine collaborative localization under extreme error conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0082] Figure 1 This is a framework diagram of a robust collaborative positioning method for multiple robots provided by the present invention. DETAILED DESCRIPTION

[0083] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0084] The purpose of this invention is to provide a robust multi-robot collaborative localization algorithm for large-scale, GPS-denied environments, which can achieve correct collaborative localization between multiple robots in the presence of a large number of erroneous data associations. Figure 1 .

[0085] The following combination Figure 1 The present invention is described in detail with reference to embodiments.

[0086] The robust collaborative localization algorithm of this invention consists of three modules: a vector filtering model, a hybrid probability model, and a two-layer expectation-maximization algorithm. First, the vector filtering model is used to filter out most erroneous data associations. Second, the hybrid probability model calculates the correct probabilities of constraints and points in the data associations based on geometric distance, semantic association, and vector consistency. Finally, the two-layer expectation-maximization algorithm iteratively updates the pose of the calculated submap and removes erroneous data associations, enabling multi-robot collaborative localization.

[0087] In order to make the objectives, technical solutions and advantages of the present invention more clear, the present invention is further described in detail below with reference to specific implementation cases.

[0088] Step 1: Multiple robots (usually 2-3; for larger numbers, for each robot's local map, there is at least one other robot's local map, and there is overlap between the two) each acquire environmental information and construct multiple submaps of the environment (each robot constructs a local map, and each local map is composed of multiple submaps). Feature descriptors are then used to extract and match features from each submap, and constraints are constructed between submaps. Due to low submap overlap and perceptual aliasing, these constraints may contain a large number of incorrect constraints. For a local map constructed by a single robot, constraints exist between adjacent submaps. Submaps are constructed in chronological order. For local maps of multiple robots, constraints between submaps are randomly generated.

[0089] The constraints between submaps are generally composed of several matching point pairs.

[0090] Step 2: Create the kth submap for robot h The lth submap established with robot j where p s ,p t For submap The point in q s ,q t For submap point in; define the flag variable v(s,t), we can get the relationship of formula (1); if (p s ,q s ) and (p t,q t ) are correct, then v(s,t)=1, and (p s ,q s ) and (p t ,q t ) is a consistent pair of vectors; otherwise v(s,t)=0.

[0091]

[0092] The correctness of a constraint or a matching point pair can be estimated based on formula (1). δ is a positive constant whose value is close to 0. The actual value can be 0.1 or 0.2.

[0093] A vector filtering model for constraints is constructed to filter out erroneous data associations, where the erroneous data associations include erroneous constraints and erroneous point pairs in correct constraints. The vector filtering model for constraints in the embodiment of the present invention is specifically:

[0094] A constraint C can be a submap and Loop constraints between Or an odometry constraint For every pair of points in C (p s ,q s ) and (p t ,q t ) calculates the value of v(s,t), then the sum of all v(s,t) in C can be defined as

[0095]

[0096] where n C is the number of matching point pairs in C.

[0097] In this model, the odometry constraint is always considered as a correct loop constraint. Then, a loop constraint The correct probability can be based on To calculate, where Defined as all The average value of . Equation (3) gives the correct probability of the estimated C.

[0098]

[0099] in And set σ to

[0100] A vector filtering model for point pairs is constructed to filter out erroneous point pairs. The vector filtering model for point pairs is specifically as follows:

[0101] For a correct constraint C i Any matching point pair (p s ,q s ), define the number of point pairs that satisfy vector consistency as shown in formula (4)

[0102]

[0103] The larger the point pair (p s .q s ) is correct. The probability is defined in formula (5)

[0104]

[0105] where ∈1 can be set to And σ1 can be set to

[0106] This completes the modeling of constraints and point pair correctness probabilities of the vector filtering model.

[0107] Step 3: Construct a probability model of the correctness of constraints and point pairs from three aspects: geometric distance, semantics, and vector consistency. This model is called a hybrid probability model and estimates the correct probability values of all constraints and point pairs.

[0108] (a) Model the problem of co-localization with erroneous data association.

[0109] Let O, L, T 0 They are the odometry constraint set O, the loop constraint set L, and the submap initial pose set T. 0 Due to the existence of incorrect data association, collaborative localization needs to find the correct data association and the correct pose of all sub-maps. Define two latent variables: the first latent variable and the second latent variable Or 0 means is a true or false loop constraint, and or 0 means (p s ,q s ) is a true or false pair. Let Z L and Z P They are and The set of Z={Z L ,Z P} collectively refers to all kinds of latent variables. Then the co-location problem can be modeled as the Maximum Likelihood Estimation (MLE) problem in Equation (6).

[0110] T=argmax p(O,L|T,Z) (6)

[0111] Where T is the pose set of the submap.

[0112] The problem expressed in formula (6) can be solved iteratively using the Expectation Maximization (EM) algorithm, as shown in formula (7).

[0113]

[0114] In step E, given O, L and T obtained by the i-th iteration (i)th (T (i)th is the set of submap poses at the i-th iteration), calculate p(Z|O,L,T (i)th ). In the M step, T is updated by maximizing the expectation of the likelihood function.

[0115] p(Z|O,L,T (i)th ) can be split into three sub-functions of formula (8), where Z can be replaced by and

[0116] p(Z|O,L,T (i)th )=p G (Z|O,L,T (i)th )·p S (Z|O,L,T (i)th )·p V (Z|O,L,T (i)th ) (8)

[0117] p G (Z|O,L,T (i)th ), p S (Z|O,L,T (i)th ), p V (Z|O,L,T (i)th ) are the probability values related to geometric distance, semantic information and vector consistency respectively. These three probability values are independent of each other, so p(Z|O,L,T (i)th ) is expressed as their product.

[0118] Next, we use loop constraints and point pairs By replacing Z respectively, we can get the probability expressions of the two sub-models.

[0119] (b) Building a mixed probability model

[0120] For a loop constraint The three probability values in formula (8) are defined as follows.

[0121] Abbreviated as Its size and The sum of the spatial distances of the matching point pairs in . is modeled as follows:

[0122]

[0123] in The sum of squared distances between pairs of points is defined as follows (10): yes The number of midpoint pairs, Φ is a parameter.

[0124]

[0125] in The pose of the kth submap built for the hth robot, θ is a constant.

[0126] Abbreviated as Its size and The ratio of matching point pairs with the same semantic label in is related to the ratio defined as The larger the value of The greater the probability of being correct; It is defined as follows (11):

[0127]

[0128] where τ is set to 0.1.

[0129] Abbreviated as Its definition is the same as formula (3) in the vector filtering model;

[0130] It is the product of equations (3), (9), and (11), which can be abbreviated as

[0131] (c) Building a mixed probability model

[0132] For correct constraints A matching point pair (p s ,q s ), the three probability values in formula (8) will be defined as follows.

[0133] Abbreviated as Its size is similar to p s and q s The spatial distance is related; is modeled as follows:

[0134]

[0135] Abbreviated as When p s and q s The value is 1 when the semantic labels of p are the same. s and q s If the semantic labels are different, the value is 0.

[0136] Abbreviated as Its definition is the same as that of formula (5); for (p s ,q s ), if the number of vector pairs that are consistent with it is greater, then (p s ,q s )The greater the probability of being correct.

[0137] is Equation (5), Equation (12) and The product of

[0138] In summary, the hybrid probability model establishes the probability of correct data association from three aspects, which can filter out incorrect data associations more accurately than the semantic or geometric distance information alone.

[0139] Step 4: Two-level expectation maximization algorithm

[0140] The hierarchical expectation-maximization algorithm includes two EM processes, which mainly iteratively update the probability of correct data association and the position of the sub-map, align the sub-maps output by different robots, and realize the collaborative positioning of multiple robots.

[0141] (a) The first-level expectation maximization algorithm is the first-level EM algorithm:

[0142] In the first layer of the EM algorithm, the poses of the submaps are not aligned, and the latent variables of the constraints are

[0143] E-step: In the E-step of the first-level EM algorithm, the correct probability of the loop constraint is mainly calculated. Considering that the initial spatial distance of the submaps output by different robots is large, when the E-step is executed for the first time, Cannot express Therefore, the E-step formula of the first-level EM algorithm can be rewritten as:

[0144]

[0145] M-step: In the M-step of the first-level EM algorithm, is the weight of the objective function. The objective function is defined in Equation (14), and the submap pose is updated by minimizing the objective function. In this paper, the odometry constraint is considered to be the correct constraint and its weight is set to 1.

[0146]

[0147] (b) The second-level expectation maximization algorithm is the second-level EM algorithm:

[0148] In the second layer EM algorithm, the input submap pose is obtained by the output of the first layer EM algorithm, and the point pair latent variable is The correct probability of the input loop constraint is greater than the threshold p0.

[0149] Step E: Calculate each input point pair (p s ,q s ) is correct, as shown in the following formula.

[0150]

[0151] M-step: In the M-step of the second-level EM algorithm, is the weight of the objective function of this layer. The pose of the submap is updated by minimizing the objective function, which is defined as shown in Equation (16). Considering that the wrong point pairs may exist in the loop constraint and the odometry constraint respectively, we define To represent the constraints between the kth submap established by robot h and the lth submap established by robot j, define D as the set of all input constraints.

[0152]

[0153] The second-level EM algorithm outputs the pose of the registered sub-map. The hierarchical EM algorithm stops iterating when Equation (17) is satisfied.

[0154]

[0155] in is the threshold for judging whether it has converged.

[0156] After filtering out erroneous data associations and registering all sub-maps, the multi-robot collaborative localization task under extreme error conditions is completed, and the sub-map poses are finally output.

[0157] In summary, the above are only preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A robust collaborative localization method for multiple robots, characterized in that: The steps include: For a system composed of multiple robots, each robot obtains environmental information independently, and each robot builds a local map. Each local map is composed of multiple sub-maps, and constraints are constructed between sub-maps. The constraints between sub-maps are composed of several matching point pairs. Constructing a vector filtering model for filtering out erroneous data associations, wherein the erroneous data associations include erroneous constraints and erroneous point pairs in correct constraints; A hybrid probability model is constructed to calculate the correct probabilities of constraints and point pairs in data association from the perspectives of geometric distance, semantic association, and vector consistency. A two-layer expectation maximization algorithm determines the pose of the submap and removes incorrect constraints to achieve multi-robot collaborative localization; In the system composed of multiple robots, for a local map constructed by one of the robots, there exists a local map constructed by at least one other robot, and there is an overlapping area between the local map constructed by the robot; The constructing of the vector filtering model is used to filter out erroneous data associations, and the constructed vector filtering model includes a vector filtering model for constraints and a vector filtering model for point pairs; The specific steps include: A vector filtering model for constraints is constructed to filter out erroneous constraints. The vector filtering model for constraints is specifically: For the kth submap created by robot h The lth submap established with robot j where p s ,p t For submap The point in q s ,q t For submap point in; define the flag variable v(s,t), and get the relationship of formula (1); if (p s ,q s ) and (p t ,q t ) are correct, then v(s,t)=1, and (p s ,q s ) and (p t ,q t ) is a consistent pair of vectors; otherwise v(s,t)=0; Where δ is a positive constant; The correctness of a constraint or a matching point pair is estimated based on formula (1); A constraint C is a submap and Loop Constraint Or an odometry constraint For every pair of points in C (p s ,q s ) and (p t ,q t ) calculates the value of v(s,t), then the sum of all v(s,t) in C is defined as where n C is the number of matching point pairs in C; In the vector filtering model used for constraints, the odometry constraint is considered a correct loop constraint, a loop constraint The correct probability is based on To calculate, where Defined as all The average value of; Formula (3) gives the correct probability of the estimated C; in for The upper limit of ∈ is The upper limit, σ is set to A vector filtering model for point pairs is constructed to filter out erroneous point pairs. Specifically, the vector filtering model for point pairs is: For a correct constraint C i Any matching point pair (p s ,q s ), define the number of point pairs that satisfy vector consistency as shown in formula (4) The larger the point pair (p s .q s ) is correct, the greater the probability of correctness, the constraint and point pair defined in formula (5) where ∈1 is set to And σ1 is set to This completes the modeling of constraints and point pairs in the vector filtering model.

2. The method for robust collaborative localization of multiple robots according to claim 1, wherein: The hybrid probability model is constructed to calculate the correct probabilities of constraints and point pairs in data association from the perspectives of geometric distance, semantic association, and vector consistency, specifically: Establish a problem model of co-location with incorrect data association: Let O, L, T 0 They are the odometry constraint set O, the loop constraint set L, and the submap initial pose set T. 0 ; Define two kinds of latent variables, latent variables about constraints and the latent variables about pairs of points Or 0 means is a true or false loop constraint, and or 0 means (p s ,q s ) is a true or false pair; Order Z L and Z P They are and The set of Z={Z L ,Z P } Collectively refers to all kinds of latent variables; Then the co-location problem is modeled as the maximum likelihood estimation problem MLE of formula (6): T=argmaxp(O,L|T,Z) (6) Where T is the pose set of the submap; The problem expressed in formula (6) is solved iteratively using the expectation maximization algorithm EM algorithm, as shown in formula (7); In the E step of the EM algorithm, given O, L and T obtained by the i-th iteration (i)th , T (i)th is the set of submap poses at the i-th iteration, and calculates p(Z|O,L,T (i)th ); In the M step of the EM algorithm, T is updated by maximizing the expectation of the likelihood function; p(Z|O,L,T (i)th ) is split into three sub-functions of formula (8), where Z is replaced by and p(Z|O,L,T (i)th )=p G (Z|O,L,T (i)th )·p S (Z|O,L,T (i)th )·p V (Z|O,L,T (i)th ) (8) where p G (Z|O,L,T (i)th ), p S (Z|O,L,T (i)th ), p V (Z|O,L,T (i)th ) are the probability values related to geometric distance, semantic information and vector consistency respectively. These three probability values are independent of each other. (i)th ) is expressed as the product of three probability values; Next, we use loop constraints and point to Z (ps,qs) Substitute Z respectively to get the probability expressions of the two sub-models: (a) is the loop constraint Establish a mixed probability model, specifically: For a loop constraint The three probability values in formula (8) are defined as follows: Abbreviated as Its size and The sum of the spatial distances of the matching point pairs in ; is modeled as follows: in Positively correlated with the sum of squared distances between pairs of points, It is defined as follows (10): yes The number of midpoint pairs, Φ is a parameter; Abbreviated as Its size and The ratio of matching point pairs with the same semantic label in is related to the ratio defined as The larger the value of The greater the probability of being correct; It is defined as follows (11): The pose of the kth submap created for the hth robot, θ is a constant: Where τ is set to 0.1; Abbreviated as Its definition is the same as formula (3) in the vector filtering model; It is the product of equations (3), (9), and (11), which can be abbreviated as (b) is a point pair Establish a mixed probability model, specifically: For correct constraints A matching point pair (p s ,q s ), the three probability values in formula (8) are defined as follows: Abbreviated as Its size is similar to p s and q s The spatial distance is related; is modeled as follows: Abbreviated as When p s and q s The value is 1 when the semantic labels of p are the same. s and q s If the semantic labels are different, the value is 0; Abbreviated as Its definition is the same as that of formula (5); for (p s ,q s ), if the number of its consistent vector pairs is greater, then (p s ,q s )The greater the probability of being correct; is Equation (5), Equation (12) and The product of 3. The method for robust collaborative positioning of multiple robots according to claim 1, wherein: The two-layer expectation maximization algorithm determines the position of the submap and removes incorrect constraints to achieve multi-robot collaborative localization. Specifically: (a) The first-level expectation maximization algorithm is also known as the first-level EM algorithm In the first layer of the EM algorithm, the poses of the submaps are not aligned, and the latent variables of the constraints are In the E step of the first-level EM algorithm: calculate the correct probability of the loop constraint; when the E step is executed for the first time, Cannot express The correctness of , based on this, the E-step formula of the first-level EM algorithm is rewritten as: In the M step of the first-level EM algorithm: is the weight of the objective function; The objective function is defined in Equation (14), and the submap pose is updated by minimizing the objective function; the odometry constraint is considered to be the correct constraint, and its weight is set to 1; (b) The second-level expectation maximization algorithm is the second-level EM algorithm In the second layer EM algorithm, the input submap pose is obtained by the output of the first layer EM algorithm, and the point pair latent variable is The correct probability of the input loop constraint is greater than the threshold p0; In the E step of the second-level EM algorithm: Based on formula (8), calculate each input point pair (p s ,q s )Probability of being correct: In the M-step of the second-level EM algorithm: In the M-step of the second-level EM algorithm, is the weight of the objective function of this layer. The pose of the submap is updated by minimizing the objective function. The objective function is defined as Equation (16). Considering that the wrong point pairs may exist in the loop constraint and the odometry constraint respectively, we define To express the constraints between the kth submap built by robot h and the lth submap built by robot j, define D as the set of all input constraints: The second-layer EM algorithm outputs the pose of the registered sub-map; the hierarchical EM algorithm stops iterating when equation (17) is satisfied: in Is the threshold for judging whether it converges; After filtering out erroneous data associations and registering all sub-maps, the multi-robot collaborative localization task under extreme error conditions is completed, and the sub-map poses are finally output.

Images