A multi-level graph partitioning based distributed pose graph optimization method and device

Abstract

The application provides a distributed pose graph optimization method and device based on multi-level graph segmentation, which is used for communication between multiple robots, and the method comprises the following steps: performing a multi-level graph segmentation operation on an initial pose graph and constructing a distributed optimization problem; updating selected variable blocks by using a Riemannian gradient coordinate descent algorithm until an optimal solution meeting a condition is searched; and projecting the optimal solution to a feasible solution of the pose graph optimization by using singular value decomposition. Through the design of the distributed pose graph optimization method based on multi-level graph segmentation, the application constructs more balanced optimization sub-problems which are easier to process, reduces the communication cost in the multi-robot network, and improves the overall optimization performance of the multi-robot system.

Description

Technical Field

[0001] This invention relates to the field of multi-robot communication technology, and more specifically, to a distributed pose graph optimization method and apparatus based on multi-level graph segmentation. Background Technology

[0002] In recent years, multi-robot systems, with robots as the main body, have attracted much attention due to their efficient collaborative capabilities, robust generalization capabilities, and flexible reconfiguration capabilities. They are being used more and more widely in many research fields such as autonomous localization and exploration in large-scale unknown scenarios, multi-target search, and autonomous driving.

[0003] Collaborative Simultaneous Localization and Mapping (CSLAM) enables multi-robot systems to possess accurate and reliable environmental perception capabilities. This core technology achieves the estimation of the agents' own poses and the joint construction of map models by sharing environmental perception information in a multi-agent network. Depending on whether a central node exists in the multi-robot system, CSLAM systems can be divided into centralized CSLAM or distributed CSLAM. Distributed CSLAM relies only on the agents' local perception information, local computation, and local communication; therefore, this system is more suitable than centralized systems.

[0004] Currently, existing distributed frameworks construct subgraphs and form pose graph optimization problems based on the local pose information of agents. Since the agents are located in different geographical locations, this leads to an imbalance in the connections between subgraphs, causing an increase in the amount of data transmitted between agents. This imposes additional communication burdens on the subsequent optimization process, and even when agents fail, it cannot guarantee pairwise consistency of the estimated poses. Summary of the Invention

[0005] To address the shortcomings of existing technologies, the present invention aims to provide a distributed pose graph optimization method and apparatus based on multi-level graph segmentation. By designing a distributed pose graph optimization method based on multi-level graph segmentation, a more easily handled equilibrium optimization sub-problem is constructed, reducing the communication cost in multi-robot networks and improving the overall optimization performance of multi-robot systems.

[0006] To solve the above problems, the technical solution of the present invention is as follows:

[0007] A distributed pose graph optimization method based on multi-level graph segmentation for communication between multiple robots includes the following steps:

[0008] Perform multi-level graph partitioning operations on the initial attitude graph and construct a distributed optimization problem;

[0009] The Riemann gradient coordinate descent algorithm is used to update the selected variable block until the optimal solution that meets the conditions is found.

[0010] Singular value decomposition is used to project the optimal solution onto the attitude diagram to optimize the feasible solution.

[0011] Preferably, the step of performing multi-level graph segmentation on the initial attitude graph and constructing a distributed optimization problem specifically includes: performing multi-level graph segmentation on the initial attitude graph to obtain variable blocks with tighter coupling relationships, and constructing a semidefinite programming problem with relaxed rank constraints.

[0012] Preferably, the step of performing multi-level graph segmentation on the initial attitude graph and constructing a distributed optimization problem specifically includes: letting The pose graph constructed using the robot's index number is a directed graph. in Each vertex in the equation is related to the pose x of the i-th robot in the τ-th frame. i τ =(R i τ ,T i τ Corresponding to )∈SE(d), Each edge in x i τ and Relative pose measurements between keyframes Correspondingly, SE(d) is a special Euclidean group in d dimensions, corresponding to the pose graph. Perform multi-level graph segmentation.

[0013] Preferably, the pose diagram Multi-level graph segmentation includes a coarsening stage, an initial segmentation stage, and a refinement stage. In the coarsening stage, an evaluation function based on edge weights is used to estimate the value of the edge pair matching set, and nodes that need to be merged are determined based on the evaluation value. When the graph is compressed to the point where it can be directly segmented, a recursive binary search algorithm is used for initial segmentation. After the initial segmentation is completed, the coarse-grained result of the initial segmentation is mapped onto the original input graph. In the refinement stage, a local search algorithm is used to fine-grainedly adjust the boundary nodes in different blocks until a segmented graph with the fewest connected edges is generated.

[0014] Preferably, the step of performing multi-level graph segmentation on the initial pose graph and constructing a distributed optimization problem specifically includes: obtaining a distributed pose graph optimization problem based on graph segmentation through maximum likelihood estimation.

[0015]

[0016]

[0017]

[0018]

[0019] Among them, f i It is a graph optimization subproblem constructed by each agent; ε i Let be the set of all edges for each pose of the i-th agent, and satisfy . l i w represents the pose size formed after segmentation by the i-th agent. R and w T The weight matrix measures the importance of rotation and translation errors in the optimization problem; each variable block x i It includes both local poses and poses from other robots, and can be composed of N variable blocks to form a variable group X.

[0020] :=(x1,…,x N )∈(SE(d)) n .

[0021] Preferably, the step of performing multi-level graph segmentation on the initial pose graph and constructing a distributed optimization problem specifically includes: assuming there are n poses after segmentation, using appropriate positive semidefinite relaxation and Burer-Monteiro decomposition to transform the distributed pose graph optimization problem into a low-rank constrained convex optimization problem:

[0022]

[0023]

[0024] Where the connecting Laplace matrix G = X T X, the low-rank factor Y belongs to

[0025] Preferably, the step of updating the selected variable block using the Riemann gradient coordinate descent algorithm until the optimal solution that satisfies the conditions specifically includes: for optimization problems with special constraint structures, the Riemann block coordinate descent method is used to iteratively search for feasible solutions under a distributed setting. In each iteration, the variable block with the largest gradient norm in the variable group is selected for updating according to a greedy strategy. After an update is completed, it is necessary to determine whether the result satisfies the rank constraint condition. If it does, the loop is exited directly; otherwise, rank improvement is performed and the loop is re-entered for updating until the optimal solution that satisfies the stopping condition is generated.

[0026] Preferably, the Riemannian block coordinate descent method searches for a feasible solution to the semi-definite programming problem in a low-rank space, and uses the Riemannian ladder method to perform rank search. Starting from a given initial rank r0 > d, it is necessary to increase the rank of the pose variable x to obtain a variable Y, which satisfies rank(Y) = r0. After one optimization is completed, it is necessary to determine whether the optimization result Y satisfies rank(Y) < r. If not, the rank is increased again: Iterate again until the optimal solution Y is generated * .

[0027] Preferably, the step of projecting the optimal solution onto the feasible solution of the pose graph optimization by using singular value decomposition specifically includes: using singular value decomposition to project the optimal solution Y * back onto the feasible solution X of the pose graph optimization * ∈(SE(d)) n .

[0028] Furthermore, the present invention also provides a distributed pose graph optimization device based on multi-level graph segmentation. The device includes a processor and a memory for storing executable instructions of the processor. The processor is configured to execute the distributed pose graph optimization method based on multi-level graph segmentation as described above by executing the executable instructions.

[0029] Compared with the prior art, the present invention designs a distributed pose graph optimization method based on multi-level graph segmentation, constructs a more easily processed balanced optimization sub-problem, reduces the communication cost in the multi-robot network, and improves the overall optimization performance of the multi-robot system. The distributed pose graph optimization method and device based on multi-level graph segmentation of the present invention have achieved significant improvements in both the convergence speed and the optimization quality. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] By reading the following detailed description of the non-limiting embodiments with reference to the accompanying drawings, other features, objects, and advantages of the present invention will become more apparent:

[0031] Figure 1 is a flowchart of the distributed pose graph optimization method based on multi-level graph segmentation provided by an embodiment of the present invention;

[0032] Figure 2 is the robot-level pose graph generated after splitting and compressing the pose graph in two ways provided by an embodiment of the present invention;

[0033] Figure 3a 、 Figure 3b shows the test result graphs of the optimization frameworks combining four multi-level graph segmentation algorithms with the Riemannian gradient coordinate descent method on the Garage dataset;

[0034] Figure 4a 、 Figure 4b The figure shows the test results of four multi-level graph segmentation algorithms combined with the Riemann gradient coordinate descent method on the Tours dataset. Detailed Implementation

[0035] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the protection scope of the present invention.

[0036] Specifically, Figure 1 The flowchart of the distributed pose graph optimization method based on multi-level graph segmentation provided in the embodiments of the present invention is as follows: Figure 1 As shown, the method includes the following steps:

[0037] S1: Perform multi-level graph partitioning operations on the initial attitude graph and construct a distributed optimization problem;

[0038] Specifically, a multi-level graph segmentation operation is performed on the initial pose graph to obtain variable blocks with tighter coupling relationships, and a rank-constrained relaxed semidefinite programming problem is constructed. First, the original cooperative SLAM back-end trajectory optimization problem is modeled as a pose graph optimization problem with large-scale characteristics, where each node in the graph can be regarded as a variable block composed of rotation and translation variables. Subsequently, the coarsening stage of the multi-level graph segmentation recursively compresses multiple small variable blocks in the pose graph into a large variable block. The result after the initial segmentation is further iteratively refined to obtain coarse-grained partitions to obtain variable blocks with tighter coupling relationships. Thus, a rank-constrained relaxed semidefinite programming problem evolving in Riemannian manifold space can be constructed for the multi-robot system.

[0039] Consider a multi-robot system consisting of N robots, and let... The robot's index number is used to construct a directed graph of its pose graph. in Each vertex in the equation is related to the pose x of the i-th robot in the τ-th frame. i τ =(R i τ ,T i τ Corresponding to )∈SE(d), Each edge in x i τ and Relative pose measurements between keyframes Correspondingly, SE(d) is a special Euclidean group in d dimensions.

[0040] Next, the pose diagram Multi-level graph segmentation is performed, consisting of three stages: coarsening, initial segmentation, and refinement. First, an edge-weight-based evaluation function is used to estimate the value of each edge pair in the matching set, and nodes to be merged are determined based on this evaluation value. Second, in the initial segmentation stage, when the graph is compressed to a point where direct segmentation is possible, a recursive binary search algorithm is used for preliminary segmentation. After initial segmentation, the coarse-grained results are mapped back to the original input graph. Finally, in the refinement stage, a local search algorithm is used to fine-grainedly adjust the boundary nodes in different blocks until a segmented graph with the fewest connected edges is generated. Figure 2 This is to generate a robot-level pose graph by segmenting and compressing the pose graph in two ways.

[0041] Assuming the measurement model follows a standard noise distribution, the distributed pose graph optimization problem based on graph segmentation is obtained through maximum likelihood estimation:

[0042]

[0043]

[0044]

[0045]

[0046] Among them, f i It is a graph optimization subproblem constructed by each agent; ε i Let be the set of all edges for each pose of the i-th agent, and satisfy . l i w represents the pose size formed after segmentation by the i-th agent. R and w T The weight matrix measures the importance of rotation and translation errors in the optimization problem; each variable block x i It includes both local poses and poses from other robots. N variable blocks can form a variable set X:=(x1,…,x…). N )∈(SE(d)) n .

[0047] Assuming there are n poses after segmentation, problems (1), (2), (3), and (4) can be transformed into low-rank constrained convex optimization problems using appropriate positive semidefinite relaxation and Burer-Monteiro decomposition:

[0048]

[0049]

[0050] Among them, the connected Laplacian matrix G = X T X, and the low-rank factorization factor Y belongs to

[0051] S2: Use the Riemannian gradient coordinate descent algorithm to update the selected variable block until an optimal solution that meets the conditions is found;

[0052] Furthermore, for optimization problems with special constraint structures, the Riemannian block coordinate descent method is used to iteratively search for feasible solutions in a distributed setting. In each iteration, the variable block with the largest gradient norm in the variable group is selected according to the greedy strategy for updating. After one update is completed, it is necessary to determine whether the result meets the rank constraint condition. If it meets, the loop is directly exited; otherwise, rank promotion is performed and the loop is entered again for updating until an optimal solution that meets the stop condition is generated.

[0053] Specifically, the Riemannian block coordinate descent method searches for feasible solutions to the semidefinite programming problem in a low-rank space. Since it is not certain what the specific rank r of the feasible solution is, but the rank of its space satisfies d < r ≤ n(d + 1), where d is the dimension of the manifold space on which the pose depends, usually a two-dimensional space or a three-dimensional space. The Riemannian ladder method is used to perform rank search. Starting from the given initial rank r0 > d, it is necessary to perform rank promotion on the pose variable x to obtain the variable Y, which satisfies rank(Y) = r0. Next, the Riemannian trust region method is selected to iterate on the subproblems of the quadratic forms in (5) and (6). In each iteration, the variable block with the largest gradient norm in the variable group X is selected according to the greedy rule for updating. After one optimization is completed, it is necessary to determine whether the optimization result Y satisfies rank(Y) < r. If it does not satisfy, rank promotion is performed again: Iterate again until the optimal solution Y is generated * .

[0054] S3: Project the optimal solution onto the pose graph optimization feasible solution using singular value decomposition.

[0055] Finally, use singular value decomposition to project the optimal solution Y * back onto the pose graph optimization feasible solution X * ∈(SE(d)) n .

[0056] Figure 3a 、 Figure 3b The following shows the test result graphs of the optimization frameworks combining four multilevel graph segmentation algorithms with the Riemannian gradient coordinate descent method on the Garage dataset, and a comparison is made with the Riemannian gradient coordinate descent method with sequential segmentation (RBCD method). Among them, the four multilevel graph segmentation methods are named Strong method, Fast method, Eco method, and Highest method. Figure 3aThis is a graph showing how the error between the calculated value and the true value changes with the number of iterations. Figure 3b This is a graph showing the change of the Riemann gradient norm with the number of iterations. Figure 4a , Figure 4b The image shows the test results of four multi-level graph segmentation algorithms combined with the Riemann gradient coordinate descent method on the Tours dataset. Figure 4a This is a graph showing how the error between the calculated value and the true value changes with the number of iterations. Figure 4b This is a graph showing the change of the Riemann gradient norm with the number of iterations. The results show that the distributed pose graph optimization method based on multi-level graph segmentation can reach smaller function values ​​in a shorter time, obtain more accurate solutions, and achieve smoother gradient norm change curves.

[0057] In summary, this invention designs a distributed pose graph optimization method based on multi-level graph segmentation, constructs a more manageable equilibrium optimization sub-problem, reduces communication costs in multi-robot networks, and improves the overall optimization performance of multi-robot systems. The distributed pose graph optimization method and apparatus based on multi-level graph segmentation of this invention have achieved significant improvements in both convergence speed and optimization quality.

[0058] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.

Claims

1. A distributed pose graph optimization method based on multi-level graph segmentation for communication between multiple robots, characterized in that, The method includes the following steps: Perform multi-level graph partitioning operations on the initial pose graph and construct a distributed optimization problem, specifically including: Let The pose graph constructed using the robot's index number is a directed graph. ,in Each vertex in the equation is related to the i-th robot's... frame pose Correspondingly, Each edge in and Relative pose measurements between keyframes Correspondingly, for The special Euclidean group of dimensional, corresponding to the pose diagram Perform multi-level graph segmentation; The distributed pose graph optimization problem based on graph segmentation is derived through maximum likelihood estimation: in, It is a graph optimization subproblem constructed by each agent; Let be the set of all edges for each pose of the i-th agent, and satisfy . ; Let be the pose size formed after segmentation of the i-th agent; and The weight matrix measures the importance of rotation and translation errors in the optimization problem; each variable block... It includes both local poses and poses from other robots, and can be composed of N variable blocks to form a variable group. ; The Riemann gradient coordinate descent algorithm is used to update the selected variable block until the optimal solution that meets the conditions is found. Specifically, for optimization problems with special constraint structures, the Riemann block coordinate descent method is used to iteratively search for feasible solutions under a distributed setting. In each iteration, the variable block with the largest gradient norm in the variable group is selected for updating according to a greedy strategy. After an update is completed, it is necessary to determine whether the result meets the rank constraint condition. If it does, the loop is exited directly. Otherwise, the rank is increased and the loop is re-entered for updating until the optimal solution that meets the stopping condition is generated. Singular value decomposition is used to project the optimal solution onto the attitude diagram to optimize the feasible solution.

2. The distributed pose graph optimization method based on multi-level graph segmentation according to claim 1, characterized in that, The steps of performing multi-level graph partitioning on the initial attitude graph and constructing a distributed optimization problem specifically include: performing multi-level graph partitioning on the initial attitude graph to obtain variable blocks with tighter coupling relationships, and constructing a semidefinite programming problem with relaxed rank constraints.

3. The distributed pose graph optimization method based on multi-level graph segmentation according to claim 1, characterized in that, The alignment pose diagram Multi-level graph segmentation includes a coarsening stage, an initial segmentation stage, and a refinement stage. In the coarsening stage, an evaluation function based on edge weights is used to estimate the value of the edge pair matching set, and nodes that need to be merged are determined based on the evaluation value. When the graph is compressed to the point where it can be directly segmented, a recursive binary search algorithm is used for initial segmentation. After the initial segmentation is completed, the coarse-grained result of the initial segmentation is mapped onto the original input graph. In the refinement stage, a local search algorithm is used to fine-grainedly adjust the boundary nodes in different blocks until a segmented graph with the fewest connected edges is generated.

4. The distributed pose graph optimization method based on multi-level graph segmentation according to claim 1, characterized in that, The steps of performing multi-level graph segmentation on the initial pose graph and constructing a distributed optimization problem specifically include: assuming there are n poses after segmentation, using appropriate positive semidefinite relaxation and Burer-Monteiro decomposition to transform the distributed pose graph optimization problem into a low-rank constrained convex optimization problem: Wherein, the connecting Laplacian matrix Low-rank factorization .

5. The distributed pose graph optimization method based on multi-level graph segmentation according to claim 1, characterized in that, The Riemann block descent method searches for feasible solutions to a semidefinite programming problem in a low-rank space, and uses the Riemann step ladder method to perform a rank search, starting from a given initial rank. Initially, it is necessary to adjust the attitude variables. Rank promotion yields variables Its satisfaction After an optimization is completed, the optimization result needs to be evaluated. Does it meet the requirements? If the condition is not met, then perform another rank promotion: Repeat the iteration until the optimal solution is obtained. .

6. The distributed pose graph optimization method based on multi-level graph segmentation according to claim 5, characterized in that, The step of projecting the optimal solution onto the attitude diagram using singular value decomposition to optimize feasible solutions specifically includes: projecting the optimal solution onto the attitude diagram using singular value decomposition. Projecting back to attitude map to optimize feasible solutions .

7. A distributed pose graph optimization device based on multi-level graph segmentation, characterized in that, The apparatus includes a processor and a memory for storing executable instructions of the processor, the processor being configured to perform the distributed pose graph optimization method based on multi-level graph segmentation as described in any one of claims 1 to 6 by executing the executable instructions.

Images