Multi-robot map fusion algorithm without prior information

Abstract

The embodiment of the invention relates to a multi-robot map fusion algorithm without prior information. The multi-robot map fusion algorithm comprises the steps that each robot constructs acquired surrounding environment information into a local map by using an SLAM (Simultaneous Localization and Mapping) algorithm; when there is a repeatedly accessed area between the local maps, the plurality of robots estimate closed-loop measurements between the plurality of robots through appearance-based scene recognition by a single robot according to closed-loop detection; false closed-loop measurements are rejected by calculating graph topology correctness of closed-loop measurements between multiple robots and consistency between closed loops and selecting an accurate topology-correct and consistent subset, the topology-correct and consistent subset is added to a factor graph to optimize a pose graph to construct a globally consistent map. According to the embodiment of the invention, the global map is constructed according to the space-time consistency of closed-loop measurement between the robots, compared with a traditional method, the real-time performance is improved by more than 70%, and in a multi-robot SLAM system, compared with a subset formed by sensing deviation, the real closed-loop measurement has higher topology correctness and consistency.

Description

Technical Field

[0001] The present disclosure relates to the field of communication security technology, and in particular to a multi-robot map fusion algorithm without prior information. Background Art

[0002] Existing SLAM methods have achieved good results in the field of single robots. However, with the complexity and diversification of application scenarios, single-robot SLAM systems have obvious limitations in scenarios such as field search and rescue and large-scale environmental mapping.

[0003] At present, multi-robot systems have become the mainstream solution for many modern robot applications, from warehouse logistics to automatic guided vehicles, and are further expanding to other application areas, including environmental monitoring, emergency rescue and planetary exploration. However, the price of these advantages is that a complex system is required to achieve collaboration and information exchange between robots. In a multi-robot SLAM system, each robot independently estimates its own local trajectory and its surrounding environment, and shares necessary information with other robots under certain conditions, improving the performance of a single robot in the system by means of information interaction and fusion, achieving the effect of 1+1≥2. In an environment without GPS signals, the key to realizing multi-robot SLAM is to integrate the available information between robots to establish a global reference coordinate system, so that the robots can estimate their relative positions to each other and build an accurate global map.

[0004] Therefore, it is necessary to propose a solution to improve one or more problems existing in the above-mentioned related technical solutions.

[0005] It should be noted that the information disclosed in the above background technology section is only used to enhance the understanding of the background of the present disclosure, and therefore may include information that does not constitute the prior art known to ordinary technicians in the field. Summary of the invention

[0006] The present disclosure provides a multi-robot map fusion algorithm without prior information, including the following steps:

[0007] Each robot uses the SLAM algorithm to construct a local map of the surrounding environment information it has collected;

[0008] When there are areas of repeated visits between the local maps, the multiple robots estimate closed-loop measurements between the multiple robots through appearance-based scene recognition by a single robot according to closed-loop detection;

[0009] By calculating the graph topological correctness and the consistency between closed loops of the closed loop measurements between the multiple robots, incorrect closed loop measurements are rejected and an accurate topologically correct and consistent subset is selected, and the topologically correct and consistent subset is added to the factor graph to optimize the pose graph and construct a globally consistent map.

[0010] In an exemplary embodiment of the present disclosure, the step of each robot using a SLAM algorithm to construct a local map of the surrounding environment information obtained by the robot includes:

[0011] Each robot uses the SLAM algorithm to construct a local map of the surrounding environment information collected at the same time;

[0012] Each robot uses the SLAM algorithm at different times to construct a local map of the surrounding environment information collected.

[0013] In an exemplary embodiment of the present disclosure, the method of rejecting erroneous closed-loop measurements and selecting an accurate topologically correct and consistent subset by calculating the graph topological correctness of closed-loop measurements between a plurality of robots and the consistency between closed-loops includes:

[0014] selecting a subset of measurements from a set of measurements between a plurality of said robots based on pairwise consistency of the measurements;

[0015] The measurement subset selects a pairwise consistent set by setting a threshold;

[0016] A topologically correct and consistent subset is selected according to the topological correctness of the pairwise consistent set in the time dimension.

[0017] In an exemplary embodiment of the present disclosure, the step of selecting a topologically correct and consistent subset according to the topological correctness of the pairwise consistent set in the time dimension further includes:

[0018] dividing a measurement set between the plurality of robots into a plurality of clusters consisting of closed-loop measurements of topological correctness;

[0019] The objective function is established to optimize closed-loop measurements with topological correctness and consistency.

[0020] In an exemplary embodiment of the present disclosure, the step of dividing the measurement sets between the plurality of robots into a plurality of clusters consisting of closed-loop measurements of topological correctness further includes:

[0021] Determine the position and posture nodes of the trajectory overlap area according to the motion trajectory of the robot;

[0022] The topological correctness of the corresponding measurement is calculated based on the timestamp information of the pose node to make cluster classification judgment.

[0023] In an exemplary embodiment of the present disclosure, the expression for determining the classification of the cluster is:

[0024]

[0025] Among them, t g is the threshold for defining cluster boundaries, the operator is the AND operation in the Boolean logic function. Node i and node j belong to the pose nodes on the motion trajectory of robot a, and node k and node l belong to the pose nodes on the motion trajectory of robot b. is the relative pose measurement of robot a and robot b between node j and node l, is the relative pose measurement of robot a and robot b between node i and node k, t i Represents the timestamp corresponding to node i, the relative position transformation of robot a on the trajectory Represents the transition from past node i to current node j (t j >t i ,t l >t k ), Represents a cluster, using To initialize

[0026] In an exemplary embodiment of the present disclosure, for the classification of the cluster, the front-end scene recognition system continuously generates corresponding measurements. If the expression of the classification judgment of the cluster is satisfied, the corresponding measurements are dynamically allocated to the existing cluster; otherwise, the new cluster is reinitialized using the corresponding measurements at the current moment.

[0027] In an exemplary embodiment of the present disclosure, the expression of the objective function is:

[0028]

[0029] Among them, function h(·) is used to calculate the topological correctness of the two constraints; function c(·) is the consistency calculation formula, is the relative pose measurement of robot a and robot b between node j and node l, is the relative pose measurement of robot a and robot b between node i and node k, Represents a cluster.

[0030] In an exemplary embodiment of the present disclosure, the optimization method of the objective function includes:

[0031] Calculating the topological relationship of each pair of measurements in the cluster and saving the results into a matrix;

[0032] Calculating the pairwise consistency of each pair of measurements in the matrix in a block search form to form a topologically correct and consistent matrix;

[0033] The topologically correct and consistent matrix is ​​represented by an undirected graph, and each topologically correct consistent subset corresponds to a clique in a graph model;

[0034] The topologically correct consistent subset is determined by the clique with the largest cardinality having the largest cumulative sum of the measurement pair scores.

[0035] In an exemplary embodiment of the present disclosure, the optimization of the global map adopts the least squares form, and the expression is:

[0036]

[0037] Among them: Operator represents the squared Mahalanobis distance with covariance matrix Σ; in the absence of relative pose prior information, p r Used to determine the posture of the robot's local coordinate system relative to the global reference system; Δ r ,Δ r′ Represents the offset of each trajectory relative to the global coordinates; is the position of robot r at time i; It is the relative position and posture measurement between the trajectories of robot a and robot b at time k.

[0038] The present invention proposes a multi-robot map fusion algorithm without prior information, which can include the following beneficial effects: based on the spatiotemporal consistency of closed-loop measurements between robots, the real-time performance of building a global map is improved by more than 70% compared with traditional methods. In a multi-robot SLAM system, the real closed-loop measurement has higher topological correctness and consistency than the subset composed of perception deviations. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] The accompanying drawings herein are incorporated into the specification and constitute a part of the specification, illustrate embodiments consistent with the present disclosure, and together with the specification are used to explain the principles of the present disclosure. Obviously, the accompanying drawings described below are only some embodiments of the present disclosure, and for ordinary technicians in this field, other accompanying drawings can be obtained based on these accompanying drawings without creative work.

[0040] Figure 1 A schematic diagram showing the steps of a multi-robot map fusion algorithm without prior information in an exemplary embodiment of the present disclosure;

[0041] Figure 2 A schematic diagram showing a flow chart of a multi-robot map fusion algorithm without prior information in an exemplary embodiment of the present disclosure;

[0042] Figure 3 A schematic diagram of closed-loop constraints between robots in an exemplary embodiment of the present disclosure is shown;

[0043] Figure 4 A schematic diagram showing measurement based on continuity of similar scenes in an exemplary embodiment of the present disclosure is shown;

[0044] Figure 5 A schematic diagram of topology-related measurement in an exemplary embodiment of the present disclosure is shown;

[0045] Figure 6 A schematic diagram showing measurement constraints between multiple robots in an exemplary embodiment of the present disclosure is shown;

[0046] Figure 7 A schematic diagram of a topology correctness matrix in an exemplary embodiment of the present disclosure is shown;

[0047] Figure 8 A schematic diagram of a topologically correct consistency matrix in an exemplary embodiment of the present disclosure is shown;

[0048] Fig. 9 A schematic diagram showing clusters corresponding to different subsets in an exemplary embodiment of the present disclosure;

[0049] Fig.10 A schematic diagram of a pose graph result in an exemplary embodiment of the present disclosure is shown;

[0050] Fig.11 A schematic diagram of a local posture graph of a robot before fusion in the first simulation experiment of the present disclosure is shown;

[0051] Fig.12 A schematic diagram showing the global map results of scenario 1 estimated by PCM, MEWC and the method of the present application in simulation experiment 1 of the present disclosure;

[0052] Fig.13 A schematic diagram showing the global map results of scenario 2 estimated by PCM, MEWC and the method of the present application in simulation experiment 1 of the present disclosure;

[0053] Fig.14 A schematic diagram showing a map corresponding to the data set CASIL in the second simulation experiment of the present disclosure;

[0054] Fig.15 A schematic diagram showing the fusion results of the Manhattan map of the data set in the second simulation experiment of the present disclosure is shown;

[0055] Fig.16 A schematic diagram of EVO error evaluation in the second simulation experiment of the present disclosure is shown. DETAILED DESCRIPTION

[0056] Example embodiments will now be described more fully with reference to the accompanying drawings. However, example embodiments can be implemented in a variety of forms and should not be construed as limited to the examples set forth herein; rather, these embodiments are provided so that the disclosure will be more comprehensive and complete and to fully convey the concepts of the example embodiments to those skilled in the art. The described features, structures, or characteristics may be combined in any suitable manner in one or more embodiments.

[0057] In addition, the accompanying drawings are only schematic illustrations of the present disclosure and are not necessarily drawn to scale. The same reference numerals in the figures represent the same or similar parts, and their repeated description will be omitted. Some of the block diagrams shown in the accompanying drawings are functional entities and do not necessarily correspond to physically or logically independent entities. These functional entities can be implemented in software form, or implemented in one or more hardware modules or integrated circuits, or implemented in different networks and / or processor devices and / or microcontroller devices.

[0058] This example implementation provides a multi-robot map fusion algorithm without prior information, such as Figure 1 As shown, the following steps may be included:

[0059] Step S101: Each robot uses the SLAM algorithm to construct a local map from the collected surrounding environment information.

[0060] Step S102: when there are areas that are repeatedly visited between the local maps, the multiple robots estimate the closed-loop measurements between the multiple robots through appearance-based scene recognition by a single robot according to closed-loop detection.

[0061] Step S103: By calculating the graph topological correctness of closed-loop measurements between multiple robots and the consistency between closed loops, erroneous closed-loop measurements are rejected and accurate topologically correct and consistent subsets are selected, and the topologically correct and consistent subsets are added to the factor graph to optimize the pose graph and construct a globally consistent map.

[0062] The disclosed embodiment proposes a multi-robot map fusion algorithm without prior information, which can include the following beneficial effects: rejecting erroneous closed-loop measurements and selecting accurate topologically correct and consistent subsets based on the topological correctness of the closed-loop measurements between robots and the consistency between the closed-loops, and constructing a global map with a real-time performance improved by more than 70% compared with traditional methods. In a multi-robot SLAM system, the real closed-loop measurements have higher topological correctness and consistency than the subset composed of perception deviations.

[0063] The following is a more detailed description of each step of the above method in this exemplary embodiment.

[0064] Step S101: Each robot uses the SLAM algorithm to construct a local map from the collected surrounding environment information.

[0065] Specifically: It is an existing technology to construct a local map using the SLAM algorithm using the surrounding environment information collected by a single robot, which will not be described in detail here.

[0066] It is important to understand that robots have visited the same place at the same time or at different times, indicating that the trajectories of the two robots have a certain overlap in this area. Based on the continuity of similar scenes, when the front end detects a measurement between robots, other measurements will also be generated in the adjacent areas before and after the measurement. For example, the appearance-based SLAM front end generates constraints. Correct constraints between robots are most likely to occur in areas with a high degree of overlap between robot trajectories. These constraints satisfy the consistency of time and space dimensions. The time dimension is reflected in the topological correctness of the constraints and is one of the important indicators for measuring consistency. The local maps of multiple robots or the local maps generated by a single robot at different times are merged into a global map.

[0067] Step S102: when there are areas that are repeatedly visited between the local maps, the multiple robots estimate the closed-loop measurements between the multiple robots through appearance-based scene recognition by a single robot according to closed-loop detection.

[0068] In multi-robot SLAM based on indirect observation, there is usually a lack of prior information on the relative pose transformation between robots. It is necessary to determine whether a measurement in the measurement set is accurate or abnormal during the pose graph optimization process. Figure 3 As shown in the figure, the schematic diagram of the measurement between robots in a multi-robot system is shown. According to the local pose graph estimated by the single-robot SLAM system, the prior information of the relative pose transformation in the local map can be easily obtained, such as is the estimated position of robot a from time i to time j. It is the closed-loop measurement between multiple robots.

[0069] Step S103: By calculating the graph topological correctness of closed-loop measurements between multiple robots and the consistency between closed loops, erroneous closed-loop measurements are rejected and accurate topologically correct and consistent subsets are selected, and the topologically correct and consistent subsets are added to the factor graph to optimize the pose graph and construct a globally consistent map.

[0070] The method of rejecting erroneous closed-loop measurements and selecting an accurate topologically correct and consistent subset by calculating the graph topological correctness and the consistency between closed-loops of the closed-loop measurements between the plurality of robots comprises the following steps:

[0071] Step S1021: selecting a measurement subset from the measurement sets of the multiple robots according to the pairwise consistency of the measurements;

[0072] Specifically: Use the consistency of closed-loop measurements between robots. According to the pairwise consistency of closed-loop measurements, find multiple measurement subsets that do not contain incorrect closed loops in the entire measurement set, thereby transforming the selection problem of closed-loop detection between robots into a combinatorial optimization problem, and finally selecting the qualified measurement subsets. There are many ways to define consistency based on the measurement model and system state. Assuming that all inter-robot measurements are relative posture measurements with full degrees of freedom, the mathematical expression of the selected consistency concept is:

[0073]

[0074] Where: ——Position estimation of robot a from time i to time j; operator ||·|| Σ ——Used to calculate Mahalanobis distance; symbol and ——represent the multiplication and inverse operation of posture respectively. For example, when the robot moves in a 2D plane, assuming that the posture x at a certain moment ij =[x ij y ij θ ij ] T , control input x jk =[x jk y jk θ jk ] T , from x ij and x jk Calculate x ik The formula is:

[0075]

[0076] This operation is used to determine the position of a mobile robot after a series of relative movements. Since the transformation involves a rotation matrix, this operation is not just a vector addition. Given the robot pose x ij =[x ij y ij θ ij ] T , and its inverse operation is in the form of:

[0077]

[0078] Step S1022: The measurement subset selects a pairwise consistent set by setting a threshold;

[0079] Specifically: the measurements in the selected subset are mutually consistent in the graph, and their errors are within a certain statistical threshold. By setting the threshold, the pairwise internally consistent set It can be expressed as:

[0080]

[0081] Where: ——Threshold for judging whether consistency is met, based on which the criterion is expressed Only those whose consistency is less than the threshold Measurement of δ c ——The degrees of freedom of the graph. Since the consistency between measurements is calculated by the Mahalanobis distance, the threshold It obeys the chi-square distribution and sets α=0.1.

[0082] Step S1023: selecting a correct and consistent subset according to the topological correctness of the pairwise consistent set in the time dimension.

[0083] wherein the measurement sets of the plurality of robots are divided into a plurality of clusters consisting of closed-loop measurements of topological correctness;

[0084] Specifically: In the front-end scene recognition system based on appearance perception, after a closed-loop measurement is generated, other measurements will be generated in the adjacent area due to the continuity of similar scenes, and the distribution of these measurements is consistent with the distribution of the area where the two robot trajectories overlap to a certain extent. Figure 4 As shown in the figure, a schematic diagram of closed-loop constraint measurement between two robots is shown. The dashed line in the figure represents the accurate measurement under ideal conditions, which is distributed in multiple areas of the entire trajectory. The black solid line in the local trajectory of a single robot connects two consecutive pose nodes, while the dotted line connects two non-continuous pose nodes. Based on the timestamp information of the pose nodes on each trajectory, these accurate measurements are topologically correct in the overlapping areas of each trajectory. From the perspective of the entire measurement set, such an assumption does not necessarily meet the definition of consistency. The dotted line represents an incorrect closed-loop measurement, which connects pose nodes in different areas and does not meet the definition of topological correctness with other measurements.

[0085] Measurement set Z abThe existence of means that the robot has repeatedly visited the same place at different times or by different robots. There is a partial overlap between two independent robot trajectories, but the overlap between the two trajectories cannot be determined based on the definition of consistency alone. Therefore, it is necessary to determine which areas of the two trajectories overlap. Each overlapping area contains topologically related measurements. These measurements are only associated with the pose nodes of the overlapping areas of the trajectories. The topological relationship between them can be determined based on the timestamp information of the pose nodes. More formally, the measurements contained in each overlapping area in the trajectory are represented as a cluster by clustering. Then the measurement set Z consisting of multiple clusters is ab It is expressed as:

[0086]

[0087] On the one hand, suppose that the measurement set Z ab If there are no erroneous measurements in the cluster, the measurements within each cluster are mutually consistent and topologically correct, and the clusters also satisfy the consistency condition, so that the true subset can be composed of all clusters. If the appearance-based front-end scene recognition system introduces erroneous measurements, the subset can only be composed of some measurements within the cluster. On the other hand, after the measurement set is represented as multiple clusters using a clustering method, it allows searching in the entire measurement set in a block-by-block manner, and then detecting each closed-loop area in segments.

[0088] The clustering method is used to process the inter-robot measurement set, because the idea of ​​segmented processing of closed-loop measurements conforms to the continuity characteristics of similar scenes. The constraints contained in each cluster calculate the corresponding topological correctness based on the timestamp information of the pose nodes in its associated trajectory, satisfying the topological relationship defined based on the time category.

[0089] The measurement set is divided into multiple clusters consisting of topologically related measurements. For the closed-loop constraints between robots, assume that nodes i and j belong to the pose nodes on the motion trajectory of robot a, and nodes k and l belong to the pose nodes on the motion trajectory of robot b. is the relative pose measurement of robot a and robot b at node j and node l, that is, the relative pose measurement between robot a at node j and robot b at node l. is the relative pose measurement of robot a and robot b at node i and node k, that is, the relative pose measurement between robot a at node i and robot b at node k, t i represents the timestamp corresponding to node i. Further assume that the relative position transformation of the robot on the trajectory Represents the transition from past node i to current node j (t j >t i ,t l >t k ). Using the measurements first generated by the front end To initialize Formula (6) is introduced to determine the measurement generated by the front end at the current moment is classified as an existing cluster Or use it to reinitialize a new cluster.

[0090]

[0091] Where: t g is the threshold for defining cluster boundaries, the value of which depends on the operating frequency of the front-end scene recognition system; operator It is the AND operation in Boolean logic function. Figure 3 , two robot measurements are classified into the same cluster if and only if the measurement The timestamps and measurements of the corresponding nodes j and l The deviation between the timestamps of the corresponding nodes i and k is within the threshold range, ensuring that the overlapping areas of the two local maps correspond to the same similar scene. According to the definition of formula (6), the corresponding cluster is maintained if and only if the constraints of the same trajectory area are associated. Since the front-end scene recognition system continuously generates corresponding measurements in continuous similar scenes, if the conditions of formula (6) are met, these measurements are dynamically assigned to the existing clusters; otherwise, the new cluster is reinitialized using the measurements at the current moment.

[0092] The effectiveness of the clustering method proposed in this application is demonstrated by running a set of public data sets New College. The experimental results are as follows: Figure 5 shown.

[0093] A complete data set is divided into two independent segments, each of which represents the motion trajectory of a robot and is marked by different colors, such as Figure 5 (a). Formula (6) is used to divide the measurement set into multiple clusters. Each cluster consists of constraints between topologically related robots. Different colors are used to represent different clusters in the figure, such as Figure 5 (b); At the same time, the motion trajectories of the two robots are drawn in 3D for the convenience of representation, so the z axis has no practical meaning. It can be seen that the clustering method proposed in this application successfully divides the original inter-robot measurements into multiple clusters based on formula (6), where the largest cluster contains 93 measurements and the smallest cluster contains only one measurement. g The size of is set to 50, where the timestamp interval deviation between measurements is less than the threshold and is considered to belong to the same cluster. A reasonable analysis of the results shows that the larger the size of the cluster, the greater the overlap range of the two trajectories in this area, that is, the greater the probability that the robot repeatedly visits the same scene.

[0094] Considering that similar scenes in the environment usually appear in the form of local continuous fragments rather than flashing by in the form of isolated points, only one or a few scattered and isolated closed-loop measurements are produced. Therefore, it can be inferred that those scattered clusters containing only a few constraints are very likely to be erroneous measurements.

[0095] Among them, the objective function is established to optimize the closed-loop measurement with topological correctness and consistency.

[0096] Specifically: the size of the cluster is positively correlated with the size of the overlap area of ​​the two trajectories, and the larger the cluster size, the more likely it is to dominate the actual distribution of the true measurement set. The true measurements have higher topological correctness and mutual consistency compared to the subset composed of perceptual biases.

[0097] Define a measurement pair scoring function to calculate inter-robot constraints and The probability of satisfying both topological correctness and consistency, that is, being consistent in the time dimension and consistent in the space dimension. The objective function is used to estimate the pairwise internal consistency set Z with the highest cumulative sum of measurement pair scores * The expression is:

[0098]

[0099] In the formula: function h(·) - calculates the topological correctness of the two constraints; function c(·) - consistency calculation formula, the form is as formula (1). The objective function should be maximized in the subset that contains as many measurements between robots as possible. Secondly, the larger the cumulative sum of the scores, the more measurements that simultaneously meet the topological correctness. The larger the size of the cluster, the greater the probability that it dominates the distribution of the true measurement set. The pairwise internally consistent set estimated by formula (7) satisfies: a topologically correct and consistent measurement subset with the largest cardinality. It is further verified that in the absence of prior knowledge of relative pose, it is reasonable to select a pairwise internally consistent set in the area with the largest overlap between the two trajectories. So far, formulas (4) and (6) have been used to transform the problem of loop closure detection selection into a combinatorial optimization problem. For Z * Formula (7) can be solved by introducing graph theory knowledge, and then Z * The inter-robot loop closure measurements contained in are added to the SLAM backend optimizer for global map estimation.

[0100] Specifically: Before building a global map, we first introduce some basic knowledge in graph theory for easy understanding. For an undirected graph G = {V, E}, where V is the set of vertices in the graph, It is the set of edges in the graph. An undirected graph can be represented in the form of an adjacency matrix. An element of 0 in the matrix means that the corresponding edge does not exist in the graph. A clique is a subset of vertices in an undirected graph such that every two different vertices in a clique must be connected by an edge. A graph may contain more than one clique. The maximum cluster is a cluster that cannot be expanded by merging more adjacent nodes. In other words, the maximum cluster cannot be contained in the vertex set of a larger cluster.

[0101] The closed-loop selection problem between robots is transformed into the maximum clique problem in graph theory and solved. Assume that there are two local pose graphs in a small scene containing 13 measurements of closed-loop detection between robots, constituting the entire measurement set Z ab ,like Figure 6 As shown, add a label to each measurement to distinguish it.

[0102] The clustering method defined in formula (6) introduces the factor of continuity of similar scenes. By introducing the time relationship, the measurement set composed of external points and internal points is divided into multiple clusters composed of topologically related constraints that analyze the closed-loop constraint measurements between robots by topological dimensions. Concrete the function in formula (7) To calculate the topological relationship between the robots

[0103]

[0104] All clusters calculated by the clustering method are traversed one by one. For each pair of measurements in the cluster, the topological relationship between them is calculated using formula (8) and the result is saved in the matrix H c Since it is represented in the form of an undirected graph, the matrix H c It is a symmetric matrix. The main diagonal of the matrix is ​​0, and the remaining elements h i,j =1,(i≠j), because the constraints within the cluster are topologically correct, the corresponding H c like Figure 7 As shown, we can see that the clustering method divides the measurement set into two clusters of different sizes according to the definition of topological correctness. On the one hand, the closed-loop measurements numbered 6, 7, 12, and 13 are not included in these two clusters because they do not meet the topological correctness. On the other hand, for discrete and isolated measurements, according to the above analysis of the continuity of similar scenes, those smaller clusters consisting of only one or two measurements can usually be considered as outliers. Therefore, in this example, the cluster consisting only of the measurement numbered 8 has a high probability of belonging to an outlier, so it can be directly discarded.

[0105] Different from the traditional method, directly in the entire measurement set Z abThe pairwise consistency between each pair of constraints is calculated at the level and the consistency matrix is ​​constructed. c Based on , the pairwise consistency relationship of each pair of robots in the cluster is calculated in the form of block search. c , the pairwise consistency values ​​calculated by formula (4) are saved in a matrix. According to the definition of an undirected graph, this matrix is ​​also a symmetric matrix, in which all elements on the main diagonal are 0, because calculating the consistency with itself has no practical meaning. Next, all items m in the matrix that are greater than a preset threshold are i,j Reset to 0, indicating that these measurements do not meet the definition of consistency with each other. Redefine the matrix to be a topologically correct and consistent matrix M c ,like Figure 8 shown.

[0106] Each cluster generated by the clustering method has a corresponding topologically correct and consistent matrix M c , which is in the form of an adjacency matrix, which means that there is a matrix that is similar to the matrix M c The corresponding undirected graph is Fig. 9 As shown in the figure, each topologically correct consistency subset corresponds to a group in the graph. At this time, the definition of nodes and edges in the graph has changed. The closed-loop measurement between robots becomes a node in an undirected graph. Whether there is an edge connection between two measurement nodes depends on the topologically correct consistency matrix M. c Considering the full connectivity of the cluster model, the nodes in the cluster represent that all measurements satisfy the pairwise internal consistency.

[0107] The edges in the clique mean that the corresponding closed-loop measurements satisfy both topological correctness and consistency. In other words, the topologically correct and consistent subset with the largest cardinality in the graph is the answer to formula (7), which has the largest cumulative sum of measurement pair scores. Therefore, in the entire measurement set Z ab In the selection of the accurate measurement subset Z * Corresponds to the topologically correct and consistent subset with the largest cardinality in the graph. Corresponding to the small case shown in this section, the selected subset Z * The corresponding measurement results in the pose graph are as follows Fig.10 shown.

[0108] So far, the problem of closed-loop selection between robust robots has been realized, and then the selected subset Z *The measurements between the robots contained in are added as constraints to the factor graph for optimization, thereby completing the fusion of multiple local maps into the global map in the multi-robot SLAM system. In the implementation process, the high-dimensional consistency matrix corresponding to the complete measurement set is decomposed into sub-matrix blocks corresponding to multiple trajectory overlap areas for solution, which greatly improves the real-time performance of the algorithm. In solving the problem of the maximum clique, the method proposed in this application can be calculated using any existing related algorithm.

[0109] The above description is mainly based on the scenario of two robots, but the proposed method is not limited to the scenario of two robots. In fact, it can be extended and applied to scenarios involving multiple robots. In the process of building a global map, anchor nodes are used to transform each pose node of each pose graph in the system into the global coordinate system, making it possible to compare with the measurement in the global coordinate system. Pose nodes from trajectory a and the pose node from another trajectory b The deviation c between them can be expressed as

[0110]

[0111] It is known that the essence of the optimization-based SLAM problem is a least squares problem, corresponding to the maximum a posteriori estimation of the robot trajectory X. Taking two robots a and b as an example, the final optimization problem can be expressed in the least squares form as follows:

[0112]

[0113] Among them: Operator represents the squared Mahalanobis distance with covariance matrix Σ; in the absence of relative pose prior information, p r Used to determine the posture of the robot's local coordinate system relative to the global reference system; Δ r ,Δ r′ Represents the offset of each trajectory relative to the global coordinates; is the position of robot r at time i; is the relative position and posture measurement between the trajectories of robot a and robot b at time k. The nonlinear least squares problem expressed in formula (10) is solved using incremental smoothing and map building algorithms.

[0114] The implementation process of the multi-robot SLAM algorithm can be described as follows: Based on the single-robot SLAM system, each local map is constructed, the relationship between each pair of local maps in the system is determined, and the closed-loop constraints between robots are constructed through the front-end scene recognition system based on appearance. According to the closed-loop constraints between robots, the topologically correct and consistent subsets of each local map matching pair are calculated. Once the topologically correct and consistent subset with the largest cardinality is found in all local map matching pairs, the corresponding two local maps are fused using the pose graph optimizer of the map fusion module. The fused map is again used as a new local map to calculate the topologically correct and consistent subsets between each other. This process is iterated until all local maps are fused into a complete global map. It should be noted that the first map fusion process needs to select the most qualified local map matching pair, because if the map constructed in the initial stage is wrong, the subsequent map construction process may be affected. Secondly, obtaining a larger local map through successful map fusion will increase the probability of subsequent matching between local maps, which will bring greater deterministic elements to the next map iteration, because the cardinality of the pairwise internal consistent set will increase, and ultimately expand the size of the cluster of topological correctness constraints.

[0115] In order to further verify the effect of multi-robot map fusion without prior information proposed in this disclosure, the following simulation experiments were conducted:

[0116] The simulation experiment was implemented using C++ programming. In the specific experimental process, multiple quantitative experiments were conducted based on publicly released data sets and collected data sets to verify the accuracy and real-time performance of the algorithm in terms of outlier rejection and building a global map. At the same time, in order to verify the practicality of the algorithm in a real environment, multiple groups of experiments were conducted in different real scenarios based on the built mobile robot platform to test the accuracy of the algorithm. The experimental results were compared and analyzed with the current representative methods such as PCM and MEWC.

[0117] In order to qualitatively analyze the experimental results, different from the statistics of closed-loop detection of a single robot system: recall rate and accuracy, the closed-loop detection evaluation between robots in a multi-robot SLAM system involves two new statistics, the True Positive Rate (TPR) and the False Positive Rate (FPR), whose corresponding mathematical forms are:

[0118]

[0119] The evaluation indicators of trajectory accuracy, Absolute Translation Error (ATE) and Absolute Rotation Error (ARE), are used to analyze the deviation between the fused pose graph and the true value.

[0120]

[0121] Where: and is the real displacement vector of robot a at time i and the displacement vector of the estimated fusion map; and is the real rotation matrix of robot a at time i and the estimated rotation matrix of the fusion map; log(·) ∨ To map the rotation matrix to the Lie algebra.

[0122] Experiment 1: Real data experiment

[0123] In the real scene experiment, the main environmental perception sensor used by the appearance-based front-end scene recognition system is 3D laser radar. The corresponding closed-loop keyframes are identified through the scene recognition algorithm, indicating that the two scenes belong to the same location, and the relative pose transformation between the robots is estimated using the ICP algorithm. Multiple local maps of the same scene are collected at different times, and each pair of local maps contains some repeated environmental areas to ensure that the front-end scene recognition system can estimate the potential closed-loop measurements between robots. Fig.11 As shown in the figure, the local trajectories of two robots in two sets of real environments are shown. Due to the lack of prior information, the starting position of the robot trajectory starts from (0, 0). The closed-loop measurement between the robots is represented by gray segments in the figure. How to select an accurate measurement subset to fuse multiple local maps into a consistent global map.

[0124] In practice, the challenge faced by the experiment is that the collected data set lacks real positioning information as a reference trajectory for comparison. In order to solve this problem, two solutions are adopted.

[0125] Scenario 1: In the same scenario, a complete dataset is collected along the motion trajectory of the robot corresponding to the two datasets as a reference, providing a global reference for the map after the fusion of the two local maps. Since the experiment in this section is aimed at testing the performance of the algorithm in outlier removal rather than navigation, the global map is only used as a reference benchmark for the fused global map and has no other practical significance.

[0126] Scenario 2: In order to verify the accuracy and continuity of the global map generated after fusion, the end position of the motion trajectory of robot 1 is set as the starting position of the motion trajectory of robot 2 during data collection. Compare whether the two positions in the generated global map overlap. If they do not overlap, calculate the corresponding deviation size of the two positions. The calculated deviation value can be used as a new evaluation indicator for quantitative analysis. The above two solutions can solve the challenges brought by the lack of real positioning information.

[0127] Considering that the size of the collected dataset is smaller than that of the publicly released dataset, the threshold t in the clustering method is set to g It is set to 15 to achieve clustering, which is more in line with the actual size of the dataset. Similar to the public dataset experiment, 30 randomly generated outliers with random means and covariances are added to the pose graph to increase the difficulty of the algorithm for outlier rejection. Figure 12-13 As shown, they are Fig.11 Results of fusing local maps into a global map using a selected subset of closed-loop measurements in two sets of real-world experiments.

[0128] The global reference maps corresponding to the complete dataset in two different experimental scenarios are shown in Figure 2. Fig.12 (a) and Fig.13 (a) As shown in the figure, for the two local maps, different colors are used to mark and distinguish the local pose graphs of the two robots. The red squares and blue triangles in the figure represent the local Figure 1 The end position and local Figure 2 The starting position of the local map. In the experimental results, the green line segments represent the correctly selected internal points, and the gray line segments represent the external points that the algorithm successfully rejected during the closed-loop selection. From a qualitative perspective, although there are still certain deviations in the position points represented by the red squares and blue triangles in the global map generated after fusion, it can be intuitively seen that the three methods have successfully realized the fusion of the local map to the global map, and the estimated global map is almost consistent with the reference trajectory. This shows that the method of the present application can be adapted to real-scene experiments. The performance of different algorithms is compared by quantitatively analyzing the experimental results. Since the self-collected data set in the experiment lacks real trajectory information, ATE cannot be used as an effective evaluation indicator. Therefore, the performance evaluation of different algorithms in this experiment is based on four indicators: time, FPR, TPR and deviation, as shown in Table 1.

[0129] From the experimental data in Table 1, when the deviation index is approximately the same, the calculation time used by the method of the present application is as short as 0.01 seconds, while the calculation time of the MEWC method is as long as 0.071 seconds. Secondly, the MEWC method not only considers the consistency factor between measurements, but also introduces the data similarity between two frames as one of the weights for constructing pairwise internal consistency, so the cardinality of its pairwise internal consistency set is not always the largest, consistent with the size of its TPR. Since the PCM algorithm assumes that all closed-loop measurements that meet the threshold have the same weight size by default, the best measurement subset must be selected at the level of the entire measurement set, resulting in the method being unable to accurately reject all false positive closed-loop measurements caused by perceptual bias. In the experiment of scenario 1, the PCM algorithm introduced a false positive closed-loop measurement, which to some extent aggravated the deviation of the position of the trajectory corresponding to the red square and blue triangle in the fused global map.

[0130] Table 1 Analysis of real scene experimental results

[0131]

[0132] By analyzing the experimental results of the real data set, we can see that on the basis of achieving the same accuracy, the proposed algorithm discards some potential closed-loop measurements in the selection of the measurement subset, but from the analysis of the constructed global map results, the proposed algorithm does not lose any meaningful information, and the algorithm has higher real-time performance and accuracy. The experimental results are consistent with the assumptions proposed by the algorithm, and the performance of the algorithm will be verified by publicly released data sets.

[0133] Experiment 2: Public Dataset Experiment

[0134] This experiment runs the publicly released CASIL dataset and Manhattan dataset to analyze the performance of the proposed algorithm. The CASIL dataset represents a scenario with a small number of closed-loop measurements between robots, while the Manhattan dataset represents an experimental scenario with dense closed-loop measurements between robots. Only the CPU is used in the real-time test of the algorithm, and other modules such as the GPU are not involved.

[0135] 1) Dataset CASIL

[0136] The main goal is to verify the effectiveness of the proposed algorithm by running a publicly released dataset, and further evaluate the performance of the algorithm in terms of outlier rejection by qualitative analysis. The dataset CASIL is data collected using a 2D lidar in the indoor environment of the MIT Computer Building, which contains 1045 pose nodes and 1172 measured edges (including 128 closed-loop detections of single robots). In order to simulate the local pose graphs of the two robots, the complete robot trajectory generated by the publicly released dataset CASIL is divided into two independent segments, representing the local pose graphs of the two robots. The convenient benefit of this processing method is that part of the closed-loop constraints that originally belonged to the robot are now converted into closed-loop constraints between robots. In this case, it is very easy to determine whether the constraints between the robots are accurate, because the relative pose between the two robots is known on the complete trajectory. The closed-loop constraints between the robots obtained by these conversions are called inliers.

[0137] In order to increase the difficulty of the map fusion process, a part of closed-loop measurements is randomly generated by means of random numbers to represent inaccurate measurements. These generated measurements are added to the pose graph to test the performance of the algorithm proposed in this application when the number of external points changes dynamically. In the experiment, the number of external points is set to change from 0 to 250, and the step size of each increase is 50. It should be noted that the added external points are composed of two parts. One part is the false positive measurement corresponding to the perceptual bias, that is, multiple groups of mutually consistent abnormal edges are used to simulate the wrong closed loop caused by perceptual bias; the other part is the closed-loop measurement with random mean and covariance generated by a random function, which is used to simulate the influence of random noise on the closed-loop measurement. During the experiment, the threshold t in the clustering method g The size of is set to 50 to divide the clusters, and 50 of the inliers are randomly selected as the measurement set. Then, the best measurement subset is selected according to the algorithm proposed in this application, and the optimizer iSAM and the concept of anchor points are used to fuse multiple pose graphs.

[0138] First, the performance of the proposed method in outlier rejection is tested. Fig.14 As shown in FIG. 1 , the estimated results of the pose graph corresponding to the dataset CASIL are shown when the number of external points is 50, 100, 200 and 250 respectively.

[0139] Fig.14 The green line in the middle represents the correctly selected inliers, and the gray line represents the successfully rejected outliers. No false positive constraint measurements were introduced into the optimizer in the experiment. The blue track in the figure is the final estimated fused global pose graph. In order to verify the impact of the closed loop detection selection method on the map fusion results, Fig.14(b) shows the result of directly performing map fusion without executing the closed loop detection selection method when the number of external points is 250. It can be seen that the fused pose map is severely distorted due to the inclusion of erroneous measurements.

[0140] Table 2 quantitative analysis Fig.14 The global maps estimated under different conditions are shown in the table. The table contains two basic indicators to describe the relevant information of the cluster. The measurement set is divided into multiple clusters according to formula (6). The maximum cluster size is the number of measurements contained in the cluster with the largest cardinality. The number of clusters refers to the number of clusters obtained in each group of experiments under different numbers of external points.

[0141] Table 2 Analysis of CASIL outlier rejection results of dataset

[0142]

[0143] The clustering method based on formula (6) is used to segment the closed-loop measurement between robots to generate multiple clusters. It can be seen that as the number of external points increases, the number of clusters first gradually increases to 21, and then decreases to 19 when the number of external points is 250. The reason for this phenomenon is that as the number of external points increases, the originally scattered and isolated clusters are associated with the added external points, and finally multiple discrete measurements are merged into a new cluster. Therefore, the number of clusters increases first and then decreases.

[0144] Other evaluation indicators include the time to solve the maximum clique problem, TPR, FPR, ATE and ARE, which are used to test the performance of the algorithm and compare it with the PCM algorithm.

[0145] In terms of accuracy, it can be seen that the ATE and ARE of PCM and the method of this application are very small, mainly because the errors of the initial position and rotation matrix of the dataset CASIL are very small, and the odometer measurement and closed-loop detection are also reliable. In this case, only the successful implementation of external point rejection can meet the operating conditions of PCM and the method of this application, so as to use the optimizer to accurately realize the fusion of the pose map.

[0146] In terms of computing time, the method proposed in this application uses the divided clusters as basic units to estimate the topologically correct and consistent measurement subsets, that is, the entire measurement set is divided into multiple sub-matrices for solution during the implementation process, which greatly reduces the optimization time compared to the existing method of solving the high-dimensional consistency matrix composed of the complete measurement set. In the test, the maximum computing time of the method proposed in this application is 0.24 seconds, which is 82% less than the 1.35 seconds of the PCM method under the same circumstances.

[0147] In terms of TPR and FPR, the PCM method has the highest TPR of 0.7, which is consistent with the implementation principle of the algorithm in this application. Because when selecting the measurement subset, the idea of ​​the method in this application is to determine the measurement subset at the maximum overlap area of ​​the two trajectories, rather than selecting the subset at the level of the complete measurement set like the PCM method. In the next experiment, it will be proved that such an algorithm design is reasonable and very meaningful.

[0148] Dataset Manhattan

[0149] The experiment is used to test the performance of the proposed algorithm in outlier rejection and map construction in the case of dense closed-loop measurements between robots. It also proves that in the absence of prior information on relative measurements between robots, the closed-loop constraints between robots measured by the system in practice, not all closed-loop measurements are meaningful for the fusion of the pose map, and only a subset of measurements that meet the conditions need to be selected.

[0150] The experimental comparison was performed by running the public dataset Manhattan, which simulates a virtual world map containing 3500 pose nodes and 2099 closed-loop measurements. Similarly, the complete map corresponding to the dataset Manhattan is divided into two independent local maps. In the process of dividing the pose graph, the number of closed-loop measurements between robots is not a fixed value, but is related to the lengths of the two trajectories after division, which results in the corresponding closed loop within the robot being converted into a closed loop between robots. In the experiment, two different versions of the dataset were generated according to the difference in the number of external points, with the number of external points being 200 and 500 respectively. Ten random experiments were performed on each set of datasets to compare the estimated global maps. When the number of external points is 500, the global maps estimated by PCM, MEWC and the method of the present application are as follows. Fig.15 Similar to Fig.14 (b) Fig.15 (d) shows the result of directly performing map fusion without executing the closed loop detection selection method when the number of outliers is 500. Due to the influence of the outliers, the fused map is wrong.

[0151] In order to further analyze the degree of damage to the fusion map caused by the external points accepted during the back-end optimization process, multiple groups of experiments were conducted for analysis, as shown in Table 3.

[0152] Table 3. Analysis of Manhattan experimental results

[0153]

[0154] On the one hand, it can be intuitively seen that in the case of dense closed-loop measurements, an obvious change in the evaluation index is the calculation time for solving the maximum clique problem. In multiple random experiments with 500 external points, the average calculation time of the PCM and MEWC methods exceeded 53 seconds. This is because both the PCM and MEWC methods construct a consistency matrix based on the entire measurement set level, and then find the largest consistency subset in it. The higher the dimension of the constructed consistency matrix, the longer it takes to solve it. On the contrary, the high-dimensional consistency matrix corresponding to the entire measurement set of the method of the present application is decomposed into multiple sub-matrix blocks, and the topologically correct and consistent set is determined in a local range. When the number of external points is 500, the calculation time is only 3.51 seconds, and the calculation time is reduced by 93%. On the other hand, since the PCM and MEWC methods select a trusted measurement subset at the level of the entire measurement set, the corresponding TPR exceeds 0.9, which is higher than the TPR estimated by the algorithm of this application.

[0155] The experimental results are consistent with the method of this application. The scale of the trajectory overlap area will dominate the distribution of the trusted subset. The larger the size of the corresponding cluster, the greater the probability of generating a topologically correct and consistent set with the largest cardinality. The above two sets of experimental results are analyzed in combination with Table 2 and Table 3 to determine the impact of the change in the number of outliers on time, TPR and FPR, as well as the relationship between ATE and TPR and FPR.

[0156] 1) By correlating ATE with TPR, we can see that the worst TPR result of the method proposed in this application is 0.54 in Table 3, but the average ATE between the global map and the real map estimated using this measurement subset is only 0.36, which is approximately the same as the ATE mean corresponding to the existing PCM and MEWC methods. The SLAM accuracy evaluation tool EVO is used to convert the quantitative experimental result data into intuitive graphs for comparison. Fig.16 It shows that there is no obvious deviation between the global map estimated by the algorithm of this application and the real map of the Manhattan dataset, and the two maps completely overlap.

[0157] 2) In traditional methods, the larger the size of the measurement set, the higher the dimension of the corresponding consistency matrix, and the longer it takes to solve the high-dimensional consistency matrix. The method of this application uses the clusters corresponding to the overlapping areas of the trajectories as the basic unit to estimate the consistency subset, decomposes the high-dimensional matrix into multiple sub-matrix blocks, and reduces the computational time required to solve the maximum clique problem by 93%.

[0158] It can be seen that the experimental results of the public dataset are consistent with the experimental results of the collected dataset: a lower TPR means that the algorithm proposed in this application loses some potential correct closed-loop measurement constraints between robots during the implementation process, but a lower ATE indicates that no important and meaningful measurement information is lost, and the proposed method maintains a high real-time performance.

[0159] It should be noted that although several units of the system for action execution are mentioned in the above detailed description, this division is not mandatory. In fact, according to the embodiment of the present disclosure, the features and functions of two or more units described above can be concretized in one unit. Conversely, the features and functions of a unit described above can be further divided into multiple units for concretization. Some or all of the units can be selected according to actual needs to achieve the purpose of the disclosed solution. Those of ordinary skill in the art can understand and implement it without paying creative work.

[0160] Those skilled in the art will readily appreciate other embodiments of the present disclosure after considering the specification and practicing the invention disclosed herein. This application is intended to cover any modification, use or adaptation of the present disclosure, which follows the general principles of the present disclosure and includes common knowledge or customary techniques in the art that are not disclosed in the present disclosure. The specification and examples are intended to be exemplary only, and the true scope and spirit of the present disclosure are indicated by the appended claims.

Claims

1. A multi-robot map fusion algorithm without prior information, characterized in that: The following steps are involved: Each robot uses the SLAM algorithm to construct a local map of the surrounding environment information it has collected; When there are areas of repeated visits between the local maps, the multiple robots estimate closed-loop measurements between the multiple robots through appearance-based scene recognition by a single robot according to closed-loop detection; By calculating the graph topological correctness and the consistency between closed loops of the closed loop measurements between the multiple robots, incorrect closed loop measurements are rejected and an accurate topologically correct and consistent subset is selected, and the topologically correct and consistent subset is added to the factor graph to optimize the pose graph and construct a globally consistent map.

2. The multi-robot map fusion algorithm without prior information according to claim 1 is characterized in that: The step of each robot using the SLAM algorithm to construct the collected surrounding environment information into a local map includes: Each robot uses the SLAM algorithm to construct a local map of the surrounding environment information collected at the same time; Each robot uses the SLAM algorithm at different times to construct a local map of the surrounding environment information collected.

3. The multi-robot map fusion algorithm without prior information according to claim 1 is characterized in that: The method of rejecting erroneous closed-loop measurements and selecting an accurate topologically correct and consistent subset by calculating the graph topological correctness and consistency between closed-loop measurements of a plurality of the robots comprises: Selecting a subset of measurements from a set of measurements between the plurality of robots based on pairwise consistency of closed-loop measurements; The measurement subset selects a pairwise consistent set by setting a threshold; A topologically correct and consistent subset is selected according to the topological correctness of the pairwise consistent set in the time dimension.

4. The multi-robot map fusion algorithm without prior information according to claim 3 is characterized in that: The step of selecting a topologically correct and consistent subset according to the topological correctness of the paired consistent set in the time dimension also includes: dividing a measurement set between the plurality of robots into a plurality of clusters consisting of closed-loop measurements of topological correctness; The objective function is established to optimize closed-loop measurements with topological correctness and consistency.

5. The multi-robot map fusion algorithm without prior information according to claim 4 is characterized in that: The step of dividing the measurement sets between the plurality of robots into a plurality of clusters consisting of closed-loop measurements of topological correctness further includes: Determine the position and posture nodes of the trajectory overlap area according to the motion trajectory of the robot; The topological correctness of the corresponding measurement is calculated based on the timestamp information of the pose node to classify the cluster.

6. The multi-robot map fusion algorithm without prior information according to claim 5 is characterized in that: The expression for the classification judgment of the cluster is: Among them, t g is the threshold for defining cluster boundaries, the operator is the AND operation in the Boolean logic function. Node i and node j belong to the pose nodes on the motion trajectory of robot a, and node k and node l belong to the pose nodes on the motion trajectory of robot b. is the relative pose measurement of robot a and robot b between node j and node l, is the relative pose measurement of robot a and robot b between node i and node k, t i Represents the timestamp corresponding to node i, the relative position transformation of robot a on the trajectory Represents the transition from past node i to current node j (t j >t i ,t l >t k ), Represents a cluster, using To initialize 7. The multi-robot map fusion algorithm without prior information according to claim 6 is characterized in that: The front-end scene recognition system continuously generates corresponding measurements for the classification of the cluster. If the expression for judging the classification of the cluster is satisfied, the corresponding measurements are dynamically allocated to the existing cluster; otherwise, the new cluster is reinitialized using the corresponding measurements at the current moment.

8. The multi-robot map fusion algorithm without prior information according to claim 4 is characterized in that: The expression of the objective function is: Among them, function h(·) is used to calculate the topological correctness of the two constraints; function c(·) is the consistency calculation formula, is the relative pose measurement of robot a and robot b between node j and node l, is the relative pose measurement of robot a and robot b between node i and node k, Represents a cluster.

9. The multi-robot map fusion algorithm without prior information according to claim 4 is characterized in that: The optimization method of the objective function includes: Calculating the topological relationship of each pair of measurements in the cluster and saving the results into a matrix; Calculating the pairwise consistency of each pair of measurements in the matrix in a block search form to form a topologically correct and consistent matrix; The topologically correct and consistent matrix is ​​represented by an undirected graph, and each topologically correct consistent subset corresponds to a clique in a graph model; The topologically correct consistent subset is determined by the clique with the largest cardinality having the largest cumulative sum of the measurement pair scores.

10. The multi-robot map fusion algorithm without prior information according to claim 7, characterized in that: The optimization of the global map adopts the least square form, and the expression is: Among them: Operator represents the squared Mahalanobis distance with covariance matrix Σ; in the absence of relative pose prior information, p r Used to determine the posture of the robot's local coordinate system relative to the global reference system; Δ r ,Δ r′ Represents the offset of each trajectory relative to the global coordinates; is the position of robot r at time i; It is the relative position and posture measurement between the trajectories of robot a and robot b at time k.

Images