Pose map simplified anti-interference method for multi-robot cooperative mapping
Through the methods of pose map chunking, simplification and anti-interference processing, the problem of large calculation load and low accuracy of traditional multi-robot collaborative graph construction is solved, and high-quality and efficient pose map optimization is achieved.
Patent Information
- Application Number
- CN202411705514.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-11-26
- Publication Date
- 2025-03-14
AI Technical Summary
The traditional multi-robot collaborative mapping optimization method lacks simplification and anti-interference processing, resulting in large calculation load and long time-consuming, and may cause map alignment and fusion errors due to erroneous motion constraints.
Through pose map chunking, simplification and anti-interference processing, the sub-position pose map of multiple robots is gradually optimized to build a globally consistent pose map. Specific steps include block processing of the original position pose map, simplified processing of the sub-position pose map, anti-interference processing and joint optimization of the multi-robot sub-position pose map.
The pose map optimization quality and accuracy of multi-machine cooperative mapping construction is improved, the calculation load is reduced, and the accuracy of map alignment and fusion is enhanced. It is suitable for scenes where multiple types of robots jointly build maps.
Smart Images
Figure CN119624797A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the field of robotics technology, and in particular relates to a pose graph simplification and anti-interference method for multi-robot collaborative mapping. Background Art
[0002] The pose graph optimization method for multi-machine collaborative mapping is traditionally that each robot constructs its own independent sub-pose graph. There is a certain range of common overlapping areas between the sub-pose graphs of different robots so that they can establish associations with each other. When constructing the pose graph map of the entire scene, the sub-pose graphs of multiple robots are aligned and integrated with each other by constructing closed-loop constraints and optimizing the pose graph, so as to obtain a globally consistent pose graph.
[0003] However, the inventors of the present application have discovered that the above-mentioned traditional technology has at least the following technical problems: directly integrating sub-pose graphs from different robots through pose graph optimization without simplifying and anti-interference processing, which may result in: on the one hand, because there are too many motion constraints in the original pose graph of a large-scale scene, the computational load is large, resulting in a long calculation time or difficulty in obtaining an effective map after optimization in a timely manner; on the other hand, there may be erroneous motion constraints in the pose graph, and not using an anti-interference mechanism will lead to map alignment and fusion errors.
[0004] The inventors of the present application have proposed a new method for simplifying and anti-interference processing of pose graphs in response to the pain points of problems existing in the traditional process of pose graph optimization methods. Summary of the invention
[0005] The present application provides a pose graph simplification and anti-interference method for multi-robot collaborative mapping, operates on the pose graph, gradually divides the pose graph into blocks, simplifies and anti-interferences the pose graph, and then based on this, performs joint alignment optimization on the sub-pose graphs of multiple robots, thereby constructing a global pose graph that is consistently aligned with each other, greatly improving the quality and effect of multi-robot collaborative mapping.
[0006] The present invention provides a pose graph simplification and anti-interference method for multi-robot collaborative mapping, which is characterized by comprising the following steps:
[0007] Step S1: Block processing of the original pose graph
[0008] Decompose the original pose graph into multiple independent sub-pose graphs, and then align, integrate and optimize the multiple sub-pose graphs;
[0009] Step S2: Simplification of sub-pose graph
[0010] Through marginalization and sparse processing, the nodes with little significance in the pose graph are removed, and the structure of the pose graph is sparsely processed to obtain a simplified graph equivalent to the original pose graph;
[0011] Step S3: Anti-interference processing of sub-pose graph
[0012] Several motion constraints in which the outer points and inner points in the motion constraints in the pose graph are consistent with each other are screened out as valid inner point motion constraints.
[0013] Step S4: Joint optimization of multi-robot sub-pose graphs
[0014] The sub-pose graphs of different robots construct closed-loop constraints between the sub-pose graphs through the common parts. The closed-loop constraints associate the different sub-pose graphs with each other. Then, the simplification and anti-interference processing of steps S2 to S3 are used at the pose graph level to obtain an equivalent simplified pose graph. Finally, a globally consistent and aligned global map is obtained through further optimization.
[0015] Preferably, the block processing step of the original pose graph in step S1 includes:
[0016] Step S1.1: Decomposition of the original pose graph
[0017] S1.1.1. Divide the robot motion trajectory into multiple trunks, each trunk is a local sub-pose graph, and each local sub-pose graph consists of an origin node and a relative node;
[0018] S1.1.2, solve the pose values of each node by direct method;
[0019] S1.1.3, the origin node is a node that associates the sub-pose graph with the ground truth, and the relative node is associated with the origin node through a motion constraint;
[0020] S1.1.4. A single local sub-pose graph is calculated and processed by direct method, and the structure of the scene pose graph is restored by approximate processing. Multiple different local sub-pose graphs are linearized in parallel.
[0021] Step S1.2: Association between sub-pose graphs
[0022] The two adjacent sub-pose graph Trunks are associated with each other through the origin nodes of the common pose graph through motion constraints, the non-common nodes of the sub-pose graphs containing less information are marginalized and sparsely processed, the contained information is integrated into the retained pose graph, the common nodes and origin nodes of the two sub-pose graphs are screened out, and the skeleton-associated pose graph between the two sub-pose graphs is constructed; the retained pose graph is the processed pose graph left after finding the common nodes of the two adjacent pose graphs and simplifying, aligning and integrating them;
[0023] Step S1.3 Global sub-pose graph construction and optimization
[0024] Multiple sub-pose graphs are associated with each other through the method in step S1.2, and then a skeleton graph of the global pose graph is constructed, and then a global optimization process is performed to obtain a globally consistent overall pose graph.
[0025] Preferably, the simplified processing step of the single pose graph in step S2 includes:
[0026] Step 2.1 Node edge processing
[0027] For the node to be removed, a series of nodes in the pose graph within a threshold range of 1m around the node and the inter-node motion constraints between the corresponding nodes are intercepted and separated from the original pose graph, and then the node information to be removed is converted into a dense factor through Schur complement marginalization processing;
[0028] Step 2.2 Pose graph edge sparsification
[0029] For the dense factor after marginalization, a KLD minimization optimization problem is constructed, and the factordescent factor gradient method is used. Through multiple linear approximation iterations to solve the nonlinear problem, a series of new pose graph factors are calculated, and the gradient of each factor of the pose graph is calculated one by one. Through continuous iteration, a new pose graph factor that approximates the characteristics of the original pose graph is obtained.
[0030] The purpose of the sparsification operation is to further abstract and integrate the information of the remaining pose graph nodes after removing unnecessary nodes in the marginalization operation: the sparsification process extracts more lightweight and abstract pose graph equivalent information, abstracting fewer necessary information nodes. The sparsification process transforms the solution of the pose graph nonlinear optimization problem into a linear approximation of multiple iterations, that is, the process of solving the gradient of each factor (motion constraint) of the pose graph in each iteration.
[0031] Preferably, the node marginalization processing in step 2.1 includes the sequential marginalization removal of a single node and the overall single removal of multiple nodes;
[0032] The individual nodes are marginalized one by one and removed
[0033] The original motion constraints between the marginalized nodes and the retained nodes of a single pose graph are converted into triangular motion constraints, and the original pose graph is sparsely simplified;
[0034] The plurality of nodes are marginalized as a whole and removed in a single
[0035] Markov Blanket is used to remove a series of nodes as a whole, and the distance between the removed nodes and the nodes to be marginalized is less than the threshold.
[0036] Preferably, in step 2.2, in the pose graph edge sparseness,
[0037] In the linear approximation process of pose graph optimization, the original nonlinear quadratic function is optimized, where the deviation formula is:
[0038]
[0039] The deviation e(x) is defined as the deviation between the recorded value calculated by h(x)) and the actual measured value of z, plus a white bar v with a normal distribution with a signed mean;
[0040] Perform a first-order Jacobian approximation to obtain the approximated quadratic objective function:
[0041]
[0042] Where: h k (x) is the function value corresponding to the iteration point, J k Δx is the value of the corresponding first-order Jacobian matrix partial derivative, h k (x)-x k +J k Δx represents the deviation between the linear approximation and the measured value. is the weight coefficient;
[0043] This is further processed using KLD optimization to keep the deviation as small as possible:
[0044]
[0045] The KID function is used to describe the degree of deviation between the pose graph after equivalent processing and the original pose graph: ∑ represents potential information, represents the linear pose factor, Represents the posterior probability value contained in the pose graph node, μ represents the optimized value; d is the bias, and fine-tuning can be performed by adjusting the value.
[0046] The original dense pose graph and the simplified sparse pose graph are associated to obtain the optimized robot trajectory to retain the relevant information in the original pose graph.
[0047] The original dense pose graph and the simplified sparse pose graph are associated to obtain the optimized robot trajectory to retain the relevant information in the original pose graph.
[0048] Preferably, the anti-interference processing method of the pose graph in step S3 includes:
[0049] Step 3.1, use the linear approximation method to first calculate the orientation angle part of the pose graph, and then calculate the position part of the pose graph;
[0050] Step 3.2: Use the basic extrapolated values toward the angle part and the displacement part to solve the corresponding motion constraint value, and compare it with the observation value in the original pose graph. If the deviation is within the preset range, it meets the condition that the motion constraint remains consistent and is determined to be a valid motion constraint. Otherwise, it is an erroneous motion constraint and is determined to be an invalid motion constraint.
[0051] The technical solution provided by this application has at least the following technical effects or advantages:
[0052] 1. In the multi-machine collaborative mapping scenario, the method of the present invention improves the quality and accuracy of pose graph optimization through pose graph level processing, and obtains a globally consistent and overall improved global map.
[0053] 2. The present invention directly simplifies and resists interference at the pose graph level. The pose graph level belongs to the abstract level and is not limited by physical sensors, so as to obtain a pose graph with dimensionality reduction processing, provide a modulated pose graph for subsequent pose graph optimization, improve the pose graph optimization efficiency, and expand the scope of application of the pose graph optimization framework in multi-robot collaborative mapping. It is suitable for scenarios where multiple types of robots are used for joint mapping, and has a wide range of applications.
[0054] 3. The pose graph simplification processing method adopted by the present invention aims to obtain a skeleton pose graph representing the original pose graph, and will integrate the non-skeleton nodes or motion constraint information into the skeleton pose graph after processing, thereby reducing the size of the pose graph, removing pose graph nodes (Pose Graph Node Removal) and sparse pose graph edges (Pose Graph Edge Sparsification). Simplify the pose graph, extract the pose graph skeleton, and construct a transformed pose graph that is as equivalent as possible to the information contained in the original pose graph, so as to facilitate expansion in large-scale scenes (scalability in large-scale pose graph), thereby improving the applicability to the scene.
[0055] 4. The pose graph anti-interference processing method adopted by the present invention aims to screen the effective constraints of the pose graph and ignore the invalid constraints of the pose graph, thereby reducing the negative impact of the wrong constraints on the optimization results. The effective constraints correspond to as many motion constraints as possible in the pose graph that can be consistent with each other, which is also the largest subset of the motion constraints in the original pose graph that are as consistent as possible, so as to ignore or reduce the impact of the wrong motion constraints on the pose graph optimization, and then obtain a globally consistent and more accurate pose graph optimization result. BRIEF DESCRIPTION OF THE DRAWINGS
[0056] Figure 1 FIG. 1 is a diagram illustrating a single node of the pose graph edge thinning in an embodiment of the present invention.
[0057] Figure 2 It is a diagram illustrating the sequential marginalization processing of multiple pairs of nodes in an embodiment of the present invention.
[0058] Figure 3 It is a diagram illustrating the overall marginalization processing in an embodiment of the present invention. DETAILED DESCRIPTION
[0059] The technical solution of the present invention mainly operates on the pose graph, gradually divides the pose graph into blocks, simplifies and anti-interferences the pose graph, and then based on this, jointly aligns and optimizes the sub-pose graphs of multiple robots, thereby constructing a global pose graph that is consistently aligned with each other.
[0060] In order to better understand the above technical solution, the above technical solution will be described in detail below in conjunction with the accompanying drawings and specific implementation methods.
[0061] This embodiment provides a pose graph simplification and anti-interference method for multi-robot collaborative mapping. Figure 1-Figure 3 As shown: The specific process is as follows:
[0062] Step S1: Block processing of a single sub-pose graph
[0063] The purpose of this step of block processing of the pose graph is to decompose the original pose graph into multiple local sub-graphs, and use optimization in parallel to align the internal parts of each sub-graph, thereby laying the foundation for simplification of the pose graph: each local sub-graph is optimized using a linear approximation method, which can make the poses of each robot in the sub-graph within the local range more consistent with each other. The optimized local sub-graph lays the foundation for subsequent pose graph simplification, namely marginalization and sparse processing.
[0064] The processing of each sub-block, i.e., local sub-graph, during the segmentation process can be regarded as the alignment of pose graph nodes in a local range, thus laying the foundation for marginalization and sparsification of pose graph simplification.
[0065] The block processing mainly consists of step S1.1 block sub-pose graph, step S1.2 sub-pose graph association and step S1.3 global sub-pose graph optimization: the block processing mainly decomposes the original pose graph into multiple independent sub-pose graphs; the association step reflects the alignment and integration between multiple sub-pose graphs. The effect of the block processing can be regarded as decomposing into multiple local maps and then building associations between these local maps.
[0066] Step S1.1: Divide the pose graph into blocks
[0067] The robot motion trajectory is divided into multiple trunks, each of which is a local sub-pose graph; each local sub-pose graph consists of an origin node and a relative node, and the pose values of each node can be solved by a direct method. The origin node is the node that establishes an association between the sub-pose graph and the ground truth. The relative node is associated with the origin node through motion constraints. The direct method of calculating and processing a single local sub-pose graph restores the structure of the scene pose graph through approximate processing; multiple different local sub-pose graphs can be linearized in parallel.
[0068] Step S1.2 Sub-pose graph association
[0069] For two sub-pose graphs that need to build association constraints with each other, that is, between two adjacent sub-pose graph trunks, they are associated through common pose graph nodes (commonnode): through common nodes, the origin nodes of the two sub-pose graphs are associated with each other through motion constraints. Other non-common nodes contain less information and can be removed for marginalization to perform sparse processing, so that the information contained is integrated into the retained pose graph, and the common nodes and origin nodes of the two pose graphs are screened out, so that the skeleton association pose graph between the two pose graphs can be constructed.
[0070] Step S1.3 Global sub-pose graph optimization
[0071] Multiple sub-pose graphs are linked to each other through the above method to construct a skeleton graph of the global pose graph, so that a globally consistent overall pose graph can be obtained through global optimization.
[0072] S2. Simplified processing of a single pose graph
[0073] The purpose of pose graph simplification is to obtain a simplified graph equivalent to the original pose graph. After simplification, the number of pose nodes is reduced: nodes that are not very meaningful in the pose graph are removed, and the structure of the pose graph is sparse. The nodes in the pose graph can be divided into:
[0074] a. Common nodes: nodes corresponding to the common parts of the scene between multiple pose graphs. These nodes are associated with each other through laser matching of the common parts of the scene.
[0075] b. Non-public nodes: nodes in the pose graph that correspond to different parts of the scene. Since they are different parts of the scene, they cannot be associated with each other through laser matching. Non-public nodes are divided into nodes with greater significance and nodes with less significance.
[0076] Part of the local network of the pose graph can be simplified by selecting representative nodes from the local network, removing all nearby nodes through marginalization, and integrating the information of the removed nodes into the representative nodes. The representative nodes of the pose graph can be regarded as nodes with greater significance, and other nodes removed by marginalization are nodes with less significance.
[0077] The simplification process is mainly completed through two steps: marginalization and thinning. The simplification process can be performed multiple times on a single node in the pose graph, or it can be performed once on multiple nodes in the pose graph.
[0078] Step S2.1 Node marginalization processing
[0079] For the node to be removed, a series of nodes in the markovblanket (i.e., the pose graph within the threshold range of 1m) around the node and the intra-nodes motion constraints between these nodes are intercepted and separated from the original pose graph. Then, through the Schur complementary marginalization, the node information to be removed can be converted into a dense factor. Since it is a dense factor, further sparse processing is required to simplify the pose graph structure;
[0080] Step S2.2: Pose graph edge sparseness
[0081] For the dense factor after edge sparsification, a KLDminimization optimization problem is constructed. The factor descent factor gradient method is used. Through multiple linear approximation iterations to solve the nonlinear problem, a series of new pose graph factors are calculated, and the topological structure information contained in the mean and covariance matrix of the optimized nodes is guaranteed to be the closest to the topological structure of the pose graph before optimization, so as to reflect the topological structure of the original pose graph as much as possible. Specifically, a nonlinear iterative method is used in the optimization process to calculate the gradient of each factor of the pose graph one by one, and obtain new pose graph factors that approximate the characteristics of the original pose graph through continuous iteration. Note that the sparsification process needs to be converted into a sparse structure that can be continuously linearized, re-linearizable sparsification. In order to approximate the nonlinear characteristics of the original structure through multiple iterations.
[0082] Pose graph sparsification and multiple linearization are achieved by iteratively processing factor gradients on some factors of the pose graph, while preserving the structure of other factors. The advantage of this treatment is that the factor gradient has the characteristic of non-cyclic, which can converge faster. Sparsification will construct a series of new factors. For example Figure 1 The illustration of the sparse edge of the pose graph shown shows that the pose graph has fewer nodes after sparseness, but the information contained is approximately equivalent to the original pose graph.
[0083] Preferably, the marginalization process in this embodiment can be performed in two ways: marginalization of a single node one by one and removal of multiple nodes at a time. This embodiment is described by way of diagrams:
[0084] Remove individual nodes from the margins one by one
[0085] like Figure 1 As shown in the figure, a diagram of the sparse edge of the pose graph is described, in which the gray nodes are edge-processed nodes, and the original motion constraints (black box nodes in the figure) between them and the retained nodes (white nodes) are converted into triangular motion constraints. In this way, the sparse simplification of the original pose graph is achieved, that is, the original pose graph (a) is simplified into graph (b) and finally becomes pose graph (c), which is more sparse and lightweight.
[0086] Single removal of multiple nodes for overall marginalization
[0087] In the marginalization process of this embodiment, multiple nodes are marginalized as a whole, and MarkovBlanket is used to remove a series of nodes as a whole, and the distance between these removed nodes and the points to be marginalized is less than the threshold. Compared with the successive marginalization and removal of multiple nodes, a more simplified pose graph can be obtained. Because the pose graph obtained by the overall marginalization and removal of the associated multiple nodes is a single simplification, while the number of marginalization processes is equal to the number of nodes when the nodes are marginalized one by one multiple times, a tree structure that is superimposed on each other will be continuously obtained, making the pose graph structure more complex.
[0088] like Figure 2 , Figure 3 As shown, edge processing is performed on node A and node B as a whole. Figure 2 It is to marginalize the nodes one by one. Figure 3 This is the effect of marginalization as a whole. By comparison, the pose graph obtained by overall marginalization is more simplified than the pose graph obtained by marginalizing nodes one by one multiple times.
[0089] In the linear approximation process of pose graph optimization in this embodiment, the original nonlinear quadratic function is optimized, where the deviation is defined as follows
[0090]
[0091] Perform a first-order Jacobian approximation to obtain the approximated quadratic objective function:
[0092]
[0093] Here we describe the solution process of quadratic optimization, i.e., least squares optimization problem: the target deviation function of nonlinear optimization is linearly approximated by first-order Taylor expansion: h k (x) is the functional value corresponding to the iteration point, J k Δx is the value of the corresponding first-order Jacobian matrix partial derivative, h k (x)-z k +J k Δx describes the deviation between the linear approximation and the measured value. Indicates that the least squares objective function is adopted The scale is scaled as the weight coefficient, and the summation symbol represents the summation of each node in the posture graph to the target function.
[0094] In order to ensure that the information in the original pose graph is retained to the greatest extent during the pose graph simplification and sparsification process, KLD optimization is needed for further processing to ensure that the deviation is as small as possible:
[0095]
[0096] Where: Σ represents potential information, represents the linear pose factor, Represents the posterior probability value contained in the pose graph node, μ represents the optimized value, d is the bias, and fine-tuning can be performed by adjusting the value. The specific empirical value can be obtained based on the actual situation.
[0097] The original dense pose graph and the simplified sparse pose graph are associated to obtain an optimized robot trajectory to retain as much relevant information as possible in the original pose graph.
[0098] KLD is a mathematical parameter of statistical significance, which is used to describe the degree of deviation between the original probability distribution and the probability distribution after approximate processing. The pose graph simplification process will remove some unnecessary nodes and retain key nodes. In principle, the ideal situation is that the processed pose graph can retain as much information of the original pose graph as possible, which is manifested in that the value of the KLD parameter will be more convergent. Specifically, the method proposed in this embodiment includes marginalizing the pose graph simplification of unnecessary nodes and further anti-interference processing. The marginalization processing will obtain a linear pose graph constraint factor containing potential information.
[0099] During the process of factor simplification of the pose graph, all factors will be simplified one by one. The order of factor simplification needs to follow the direction of minimizing the deviation between the optimized pose graph described by KLD and the original pose graph, that is, starting from the factor that can minimize KLD. This can reduce the structure of the pose graph as a whole.
[0100] Step S3: Anti-interference processing of pose graph
[0101] The main purpose of anti-interference processing is to filter out the motion constraints that are as consistent as possible in the original pose graph as valid constraints, and the remaining inconsistent motion constraints are considered invalid constraints. When optimizing the pose graph, only valid constraints are processed, which can reduce the impact of invalid motion constraints on pose graph optimization.
[0102] After the pose graph is divided into blocks and simplified, outliers, that is, wrong motion constraints, are common. Therefore, anti-interference processing is needed to reduce the negative impact of wrong motion constraints outliers on pose graph optimization. Generally speaking, it is difficult to accurately identify the outliers and inliers in the motion constraints in the pose graph, so the anti-interference processing method proposed here is mainly to screen out as many motion constraints as possible that are consistent with each other, and then use them as inliers (valid) motion constraints for pose graph optimization, which embodies the idea of maximizing consistency of random sampling. The outliers and inliers related to the pose graph are relative to each other, and the measurement is whether the motion constraints are correct or wrong: inliers refer to the correct motion constraints in the pose graph, that is, the motion constraints that are consistent with each other; outliers refer to the wrong motion constraints in the pose graph, that is, there are conflicting motion constraints. The conflict mainly comes from the fact that these motion constraints are wrong, and there is a conflict between the wrong motion constraints and the correct motion constraints.
[0103] The specific operation method of anti-interference processing of the pose graph to filter out consistent motion constraints is described as follows:
[0104] The linear approximation method first calculates the orientation angle part of the pose graph, and then calculates the position part of the pose graph, which is the basis for evaluating the consistency of motion constraints.
[0105] The basic estimated values (the orientation angle and the displacement) are used to solve the corresponding motion constraint values and compared with the observed values in the original pose graph. If the deviation is within a certain range, it is considered to meet the conditions for keeping the motion constraint consistent, that is, a valid motion constraint. Otherwise, it is considered to be an erroneous motion constraint, that is, an invalid motion constraint.
[0106] The above method of selecting motion constraints in the pose graph that are as consistent as possible with each other uses the premise of straight line approximation.
[0107] Through linear approximation to non-linear posegraph optimization, the robot pose on the original pose graph is initially solved, and then the motion constraints calculated by the optimized pose are compared with the motion constraints of the original pose graph. According to the deviation, the motion constraints of the original pose graph are classified into valid motion constraints (consistent with each other) and invalid motion constraints (outlier constraints). Through the above method, the valid motion constraints of the pose graph are filtered.
[0108] Step S4: Joint optimization of multi-robot sub-pose graphs
[0109] The simplification and anti-interference processing of the above-mentioned single robot sub-pose graph is applied as a basic operation to the multi-robot scene, and the basic logic is similar. The sub-pose graphs constructed by multiple robots at the same time are similar to the sub-pose graphs constructed by the same robot at different times. The processing logic is similar, so it will not be repeated here. It should be noted that the sub-pose graphs from different robots need to be associated through the common part (the overlapping part of the observed scene), which is also the closed-loop constraint between the sub-pose graphs. The closed-loop constraint is used to associate different sub-pose graphs with each other, and then the above-mentioned simplification and anti-interference processing is used at the pose graph level to obtain an equivalent simplified pose graph, so that a globally consistent and aligned global map can be obtained through further optimization. The common part here refers to the common part that overlaps between the two pose graphs, that is, the part that corresponds to the same actual scene and is used by the two pose graphs. The significance of the common part of the pose graph is to associate the two pose graphs with each other. In the process of pose graph optimization, the two parts of the pose graph are aligned and integrated with each other through the common part.
[0110] The method proposed in this embodiment is applicable to a wide range of scenarios where multiple robots collaborate to build maps. It operates directly on the pose graph level and has a wide range of applications: it is not limited to specific sensors or specific robots. Because the method in this embodiment processes directly on the pose graph level, the pose graph motion constraints are the relative displacements (relative translations and relative rotations) obtained after processing the sensor data, which can come from laser matching of a lidar, or from a visual camera through multi-view geometry or visual odometers, or from pre-integration calculations of inertial navigation, or from wheel speedometer track calculations, or from different robot entities, which makes the method more applicable.
[0111] The above description is only a preferred embodiment of the present invention. It should be pointed out that a person skilled in the art can make several improvements without departing from the principle of the present invention. These improvements should also be regarded as within the scope of protection of the present invention.
Claims
1. A pose graph simplification and anti-interference method for multi-robot collaborative mapping, characterized in that the steps include: Step S1: Block processing of the original pose graph Decompose the original pose graph into multiple independent sub-pose graphs, and then align, integrate and optimize the multiple sub-pose graphs; Step S2: Simplification of sub-pose graph Through marginalization and sparse processing, the nodes with little significance in the sub-pose graph are removed, and the structure of the sub-pose graph is sparsely processed to obtain a simplified graph equivalent to the original pose graph; Step S3: Anti-interference processing of pose graph Filter out several motion constraints in which the outer points and the inner points in the motion constraints of the sub-pose graph are consistent with each other as valid inner point motion constraints; S4. Joint Optimization of Multi-Robot Sub-pose Graphs The sub-pose graphs of different robots construct closed-loop constraints between the sub-pose graphs through the common parts. The closed-loop constraints associate the different sub-pose graphs with each other. Then, the simplification and anti-interference processing of steps S2 to S3 are used at the sub-pose graph level to obtain an equivalent simplified pose graph. Finally, a globally consistent and aligned global map is obtained through further optimization.
2. The pose graph simplification and anti-interference method for multi-robot collaborative mapping according to claim 1 is characterized in that: The block processing step of the original pose graph in step S1 includes: Step S1.1: Decomposition of the original pose graph S1.1.
1. Divide the robot motion trajectory into multiple trunks, each trunk is a local sub-pose graph, and each local sub-pose graph consists of an origin node and a relative node; S1.1.2, solve the pose values of each node by direct method; S1.1.3, the origin node is a node that associates the sub-pose graph with the ground truth, and the relative node is associated with the origin node through a motion constraint; S1.1.
4. A single local sub-pose graph is calculated and processed by direct method, and the structure of the scene pose graph is restored by approximate processing. Multiple different local sub-pose graphs are linearized in parallel. Step S1.2: Association between sub-pose graphs The two adjacent sub-pose graph Trunks are associated with each other through the origin node of the common pose graph through motion constraints, the non-common nodes of the sub-pose graph containing less information are marginalized and sparsely processed, the contained information is integrated into the retained pose graph, the common nodes and the origin nodes of the two sub-pose graphs are screened out, and the skeleton-associated pose graph between the two sub-pose graphs is constructed; the retained pose graph is the processed pose graph left after finding the common nodes of the two adjacent pose graphs and simplifying, aligning and integrating them; Step S1.3 Global sub-pose graph construction and optimization Multiple sub-pose graphs are associated with each other through the method in step S1.2, and then a skeleton graph of the global pose graph is constructed, and then a global optimization process is performed to obtain a globally consistent overall pose graph.
3. The pose graph simplification and anti-interference method for multi-robot collaborative mapping according to claim 2 is characterized in that: The simplified processing steps of the sub-pose graph in step S2 include: Step 2.1 Node edge processing For the node to be removed, a series of nodes in the pose graph within a threshold range of 1m around the node and the inter-node motion constraints between the corresponding nodes are intercepted and separated from the original pose graph, and then the node information to be removed is converted into a dense factor through Schur complement marginalization processing; Step 2.2 Pose graph edge sparsification For the dense factor after marginalization, a KLD minimization optimization problem is constructed, and the factor descent factor gradient method is used. Through multiple linear approximation iterations to solve the nonlinear problem, a series of new pose graph factors are calculated, and the gradient of each factor of the pose graph is calculated one by one. Through continuous iteration, a new pose graph factor that approximates the characteristics of the original pose graph is obtained.
4. The pose graph simplification and anti-interference method for multi-robot collaborative mapping according to claim 3 is characterized in that: The node marginalization processing in step 2.1 includes the sequential marginalization removal of a single node and the overall single removal of multiple nodes; The individual nodes are marginalized one by one and removed The original motion constraints between the marginalized nodes and the retained nodes of a single pose graph are converted into triangular motion constraints, and the original pose graph is sparsely simplified; The plurality of nodes are marginalized as a whole and removed in a single time Markov Blanket is used to remove a series of nodes as a whole, and the distance between the removed nodes and the nodes to be marginalized is less than the threshold.
5. The pose graph simplification and anti-interference method for multi-robot collaborative mapping according to claim 3 is characterized in that: In step 2.2 of pose graph edge sparsification, In the linear approximation process of pose graph optimization, the original nonlinear quadratic function is optimized, where the deviation formula is: The deviation e(x) is defined as h(x), the deviation between the calculated value and the measured value of z, plus a normally distributed white noise v with a mean of zero; Using the Jacobian matrix to perform a first-order Taylor expansion approximation, we get the quadratic objective optimization function: Where: h k (x) is the function value corresponding to the iteration point, J k Δx is the value of the corresponding first-order Jacobian matrix partial derivative, h k (x)-z k +J k Δx represents the deviation between the linear approximation and the measured value. is the weight coefficient; KLD optimization is used for further processing to ensure that the deviation is as small as possible, and the following function is constructed: ∑ represents potential information, represents the linear pose factor, represents the posterior probability value contained in the pose graph node, μ represents the optimized value; d is the bias; The original dense pose graph and the simplified sparse pose graph are associated to obtain the optimized robot trajectory to retain the relevant information in the original pose graph.
6. The pose graph simplification and anti-interference method for multi-robot collaborative mapping according to claim 1, characterized in that: The anti-interference processing method of the pose graph in step S3 includes: Step 3.1, use the linear approximation method to first calculate the orientation angle part of the pose graph, and then calculate the position part of the pose graph; Step 3.2: Use the basic extrapolated values toward the angle part and the displacement part to solve the corresponding motion constraint value, and compare it with the observation value in the original pose graph. If the deviation is within the preset range, it meets the condition that the motion constraint remains consistent and is determined to be a valid motion constraint. Otherwise, it is an erroneous motion constraint and is determined to be an invalid motion constraint.