A long-term laser SLAM method and device based on graph pruning and a storage medium

Abstract

The application relates to a long-term laser SLAM method and device based on graph pruning and a storage medium, wherein the method comprises the following steps: acquiring real-time laser data, performing a scan matching process in the front end of a laser SLAM algorithm, performing loop detection based on a subgraph constructed by the scan matching process and laser data, constructing a pose graph, performing redundancy judgment and marking on the subgraph, pruning the marked redundant subgraph, performing a sparse process, taking the pruned pose graph as an input of a global optimization in the rear end and performing optimization solving, and obtaining an output optimized trajectory and a map of the laser SLAM algorithm. Compared with the prior art, the application adds a graph pruning process and a pose graph sparse process, can effectively control the number of vertices and edges in the pose graph, avoids unlimited increase of the number, thereby guaranteeing effectiveness of a long-term running SLAM algorithm, and through pruning of a redundant subgraph, can make a mapping result always reflect the latest environment information, and can effectively update and maintain the map.

Description

Technical Field

[0001] This invention relates to the field of autonomous mapping and localization of mobile robots, and in particular to a long-lasting laser SLAM method, apparatus and storage medium based on graph pruning. Background Technology

[0002] SLAM (Simultaneous Localization and Mapping) technology for mobile robots is a crucial foundation for their autonomous movement in unknown environments. When a mobile robot constructs a map by observing indoor environmental information from a continuous, long-term perspective, there will be objects in the environment whose positions change outside the robot's "viewpoint." To cope with these changes in the positions of semi-static objects, the mobile robot can execute SLAM algorithms while operating in the environment for an extended period to perceive these changes and thus maintain and update the environmental map.

[0003] Mobile robots often employ the mature graph optimization-based laser SLAM method. However, during long-term operation in a given environment, vertices and edges are continuously added to the graph, and the number of vertices and edges increases infinitely with the length of the trajectory. Intuitively, this puts pressure on memory requirements and increases the computational cost of solving graph optimization problems, thus affecting the speed of optimization and even compromising the real-time performance of SLAM. Furthermore, when considering loop closure detection, the robot repeatedly runs through the environment and observes the same environment multiple times, resulting in a large number of similar frames and subgraphs. This also increases the search volume for loop closure detection, raising the cost of establishing loop closure constraints. In summary, if the number of vertices and edges in the corresponding graph grows infinitely over a long period, this is clearly unreasonable and significantly increases the overall complexity of SLAM. Summary of the Invention

[0004] The purpose of this invention is to provide a long-lasting laser SLAM method, device, and storage medium based on graph pruning, which controls the number of vertices and edges, i.e. the size of the graph, thereby controlling the overall complexity of SLAM.

[0005] The objective of this invention can be achieved through the following technical solutions:

[0006] A long-lasting laser SLAM method based on graph pruning includes the following steps:

[0007] S1. Acquire real-time laser data and execute the scanning and matching process at the front end of the laser SLAM algorithm;

[0008] S2: Loop closure detection is performed based on the sub-graph constructed by the scanning matching process and the laser data to construct a pose graph;

[0009] S3: Perform redundancy checks on the subgraph and mark it;

[0010] S4: Prune the marked redundant subgraphs and perform a sparsification process;

[0011] S5: The pruned pose map is used as the input for the backend global optimization and optimized solution is obtained to obtain the output optimized trajectory and map of the laser SLAM algorithm.

[0012] The scanning matching process specifically involves inserting a pre-configured number of laser frame data into the subgraph to complete the construction of the subgraph, and establishing an adjacency constraint relationship between the laser frames and the inserted subgraph.

[0013] The loop closure detection specifically involves matching laser frame data with historically constructed subgraphs, and establishing a loop closure constraint relationship between laser frames and subgraphs that meet the loop closure conditions.

[0014] The pose graph construction in S2 is specifically as follows: using the laser frame and the subgraph as vertices, and the adjacency constraint relationship and lap constraint relationship between the laser frame and the subgraph as edges, the pose graph is constructed.

[0015] S3 includes the following steps:

[0016] S31: Traverse each grid cell in the subgraph, calculate the latest preset number of subgraph coverages for each grid cell, and record the corresponding ID of the subgraph;

[0017] S32: If the number of covered grid cells in a subgraph corresponding to a certain ID is less than the set minimum threshold, then mark the subgraph as a redundant subgraph.

[0018] After a preset number of subgraphs have been constructed, the redundancy check and marking process of S3 is executed.

[0019] S4 includes the following steps:

[0020] S41: Based on the redundant subgraph to be pruned within the current pose graph and all laser frame nodes and subgraph nodes related to the laser frame nodes contained within that redundant subgraph, construct a correlation set. Wherein, the correlation refers to the existence of an adjacent constraint relationship or a loop constraint relationship between the two;

[0021] S42: Select the construction center v of the newly added constraint relationship from the relevant set. i ;

[0022] S43: Calculate the construction center v respectively i If the length of the edge formed by the vertex and other vertices in the relevant set is less than the preset maximum length limit, then the pose relationship between the two vertices will be used as a new constraint and added as a new edge in the pose graph.

[0023] S44: Delete the redundant subgraph, the laser frame nodes contained in the redundant subgraph, and all related constraints of both to obtain a new pose graph.

[0024] The construction center v of the newly added constraint relationship i The selection criteria are as follows:

[0025] Based on the pose graph, different weight coefficients λ are assigned to vertices in the relevant set and the redundant subgraph or the laser frame nodes contained in the redundant subgraph, depending on the relationship between them. These relationships are either adjacency constraints or loop closure constraints. The weighted sum of all edges formed by each vertex in the relevant set and all other vertices in the relevant set is calculated using the following formula. The vertex v corresponding to the minimum sum is selected. i As the central point for constructing new constraint relationships;

[0026]

[0027] A graph-pruning-based long-lasting laser SLAM device includes a memory, a processor, and a program stored in the memory, wherein the processor executes the program to implement the method described above.

[0028] A storage medium having a program stored thereon, which, when executed, implements the method described above.

[0029] Compared with the prior art, the present invention has the following beneficial effects:

[0030] (1) The present invention adds a graph pruning process and a pose graph sparsification process to the execution of the graph-based laser SLAM algorithm, which can effectively control the number of vertices and edges in the pose graph and avoid their infinite growth, thereby ensuring the effectiveness of the SLAM algorithm during long-term operation.

[0031] (2) By pruning the redundant subgraphs in history, the present invention ensures that the map building results of the algorithm always reflect the latest environmental information, and the map can be effectively updated and maintained. Attached Figure Description

[0032] Figure 1 This is a flowchart of the method of the present invention;

[0033] Figure 2 This is a flowchart of the redundant subgraph judgment process of the present invention;

[0034] Figure 3 This is a flowchart of the pose graph sparsification process of the present invention. Detailed Implementation

[0035] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. These embodiments are based on the technical solution of the present invention and provide detailed implementation methods and specific operating procedures. However, the scope of protection of the present invention is not limited to the following embodiments.

[0036] This embodiment provides a long-lasting laser SLAM method based on graph trimming, such as... Figure 1 As shown, it includes the following steps:

[0037] S1. Acquire real-time laser data and execute the front-end scanning and matching process in the laser SLAM algorithm.

[0038] For the scanning and matching process, the initial pose estimate of the current laser frame is first obtained based on information such as odometry. A multi-resolution search approach is used to reduce computational load, and the optimal pose estimate is obtained through a Ceres-based optimization process. The subgraph is constructed by inserting a certain number of laser frame data into the subgraph, and an adjacency constraint relationship is established between the laser frames and the inserted subgraphs.

[0039] When constructing a subgraph, the current laser frame point cloud is inserted into the two adjacent subgraphs. Once a set number of laser frames have been inserted into a subgraph, the subgraph is considered to be constructed.

[0040] S2: Loop closure detection is performed based on the sub-graph constructed by the scanning matching process and the laser data to construct a pose graph.

[0041] The laser frame data is matched with the previously constructed subgraphs. If the poses of the two matched data are sufficiently similar, it is considered that a loop has been found. The laser frame and the subgraph that constitute the loop closure condition form a loop closure constraint relationship. The laser frame and the subgraph are used as vertices, and the constraint relationship between the laser frame and the subgraph is used as edges to form a pose graph.

[0042] S3: Perform redundancy checks on the subgraph and mark it.

[0043] After a preset number of subgraphs have been constructed, the redundancy check and marking process of S3 is executed, such as... Figure 2 As shown.

[0044] S31: Traverse each grid cell in the subgraph, calculate the latest preset number of subgraph coverages for each grid cell, and record the corresponding ID of the subgraph;

[0045] S32: If the number of covered grid cells in a subgraph corresponding to a certain ID is less than the set minimum threshold, then mark the subgraph as a redundant subgraph.

[0046] S4: Prune the marked redundant subgraphs and perform a sparsification process.

[0047] Specifically, such as Figure 3 As shown, it includes the following steps:

[0048] S41: Let m be the redundant subgraph to be pruned. trim Find subgraph m trim Laser frame nodes within Find the subgraph m trim Nodes constituting lap constraint and Subgraphs constituting closure constraints Subgraph m trim Adjacent subgraphs node neighboring nodes Will Constitute a related set

[0049] S42: In the relevant set, based on the vertex belonging to... or Assign different weight coefficients λ, and calculate the weighted sum of all edges centered at each vertex in the correlation set and formed by all other vertices in the correlation set according to the following formula. Select the vertex v corresponding to the minimum sum. i As the central point for constructing new constraint relationships;

[0050]

[0051] S43: Calculate the construction center v respectively i Related sets If the length of the edge formed by other vertices is less than the preset maximum length limit, then the pose relationship between the two vertices will be used as a new constraint and added as a new edge to the pose graph.

[0052] S44: Delete redundant subgraphs m trim ,node By combining the constraints of both, a new pose graph is obtained.

[0053] S5: The pruned pose map is used as the input for the backend global optimization and optimized solution is obtained to obtain the output optimized trajectory and map of the laser SLAM algorithm.

[0054] For the backend optimization process, all constraint information, including adjacency constraints and closure constraints between laser frame nodes and subgraphs, is used to distribute the overall accumulated error of the system among all nodes, thereby minimizing the overall system error. Specifically:

[0055] use Representing the pose of the laser frame node in the world coordinate system, using ξ represents the pose of the subgraph in the map coordinate system. ij Σ represents the relative pose relationship between the two. ij Let represent the relevant covariance matrix. Then, transforming the above process into a graph optimization problem, the optimization objective can be expressed as:

[0056]

[0057] in,

[0058]

[0059]

[0060] For the residual term Weights can be further added and different values ​​can be assigned to different constraints to represent the degree of trust in the different constraints. The ρ function uses the following Huberloss robust kernel function:

[0061]

[0062] Where δ is the parameter of the robust kernel function.

[0063] After completing a certain number of subgraphs, optimization is performed. Based on the SPA algorithm, the above optimization problem is iteratively calculated to obtain the optimal results of the subgraph and laser frame pose, thereby outputting the optimized trajectory and map.

[0064] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0065] The preferred embodiments of the present invention have been described in detail above. It should be understood that those skilled in the art can make numerous modifications and variations based on the concept of the present invention without creative effort. Therefore, all technical solutions that can be obtained by those skilled in the art based on the concept of the present invention through logical analysis, reasoning, or limited experimentation on the basis of existing technology should be within the scope of protection defined by the claims.

Claims

1. A long-term laser SLAM method based on graph pruning, characterized in that, The method comprises the following steps: S1, acquiring real-time laser data, and performing a scan matching process in a front end of a laser SLAM algorithm; S2, performing loop detection based on a subgraph constructed by the scan matching process and laser data, and constructing a pose graph; S3, performing redundancy judgment and marking of the subgraph: S31, traversing each grid in the subgraph, counting coverage of a preset number of subgraphs of each grid, and recording an id corresponding to the subgraph; S32, if the subgraph corresponding to the id counted has a number of covered grids less than a set minimum threshold, marking the subgraph as a redundant subgraph; S4, pruning the marked redundant subgraph, and performing a sparsification process; S5, taking the pruned pose graph as an input of a global optimization in a back end, and performing optimization and solving to obtain an output optimized trajectory and map of the laser SLAM algorithm.

2. The long-term laser SLAM method based on graph pruning according to claim 1, wherein, The scan matching process specifically comprises: inserting a preconfigured number of laser frame data into a subgraph to complete construction of the subgraph, and forming an adjacent constraint relationship between the laser frame and the inserted subgraph.

3. The long-term laser SLAM method based on graph pruning according to claim 2, wherein, The loop detection specifically comprises: matching laser frame data with a historical subgraph that has been constructed, and forming a loop constraint relationship between a laser frame meeting a loop condition and the subgraph.

4. The long-term laser SLAM method based on graph pruning according to claim 3, wherein, The pose graph constructed in S2 specifically comprises: taking the laser frame and the subgraph as vertices, and taking an adjacent constraint relationship and a loop constraint relationship between the laser frame and the subgraph as edges, to construct the pose graph.

5. The long-term laser SLAM method based on graph pruning according to claim 1, wherein, The redundancy judgment and marking process of S3 is performed each time a preset number of subgraphs are constructed.

6. The long-term laser SLAM method based on graph pruning according to claim 3, wherein, S4 comprises the following steps: S41: based on all laser frame nodes and subgraph nodes related to the redundant subgraph to be pruned and the laser frame nodes contained in the redundant subgraph in the current pose graph, a correlation set is constructed wherein the correlation is an adjacent constraint relationship or a loop constraint relationship between the two. S42: Select a construction center of the new constraint relationship from the correlation set ; S43: Calculate the construction center respectively If the length is less than the preset maximum length limit, the pose relationship between the two is formed as a new constraint, and a new edge of the pose graph is added. S44, deleting a redundant subgraph, a laser frame node contained in the redundant subgraph, and all related constraint relationships therebetween, to obtain a new pose graph.

7. The long-term laser SLAM method based on graph pruning according to claim 6, wherein, The construction center of the new constraint relationship The selection principle is: According to the pose graph, different weight coefficients are given to the relationship between the vertices in the relevant set and the redundant subgraph or the laser frame nodes contained in the redundant subgraph , the relationship is adjacent constraint or loop constraint, the weighted sum of all edges formed by each vertex in the relevant set and all other vertices in the relevant set is calculated according to the following formula, and the vertex corresponding to the minimum sum is selected as the construction center of the new constraint relationship 。 8. A long-lasting laser SLAM device based on graph pruning, comprising a memory, a processor, and a program stored in the memory, wherein, The processor implements the method of any one of claims 1-7 when executing the program.

9. A storage medium having stored thereon a program, characterized by The program is executed to implement the method of any one of claims 1-7.

Images