Point cloud key frame management method for long-term operation laser SLAM system

Abstract

The invention provides a point cloud key frame management method for a long-term operation laser SLAM system, and the method specifically comprises the following steps: aligning current laser observation to a world coordinate system, carrying out the voxel map division, carrying out the voxel feature judgment through a principal component analysis algorithm, and screening out new plane features; calculating a plane feature observation increment between the currently observed laser point cloud and the previous key frame, and screening the key frames based on an observation increment threshold; constructing a scanning context descriptor for the newest key frame, calculating the distance from the newest key frame to all key frames, obtaining a similar key frame match closest to the descriptor, forming a local map by using the similar key frame match and the adjacent key frames, recalculating the observation increment of the key frames in the local map, and removing the key frame with the minimum observation increment; and carrying out local reconstruction on the key frames, taking the relative pose constraint with the highest confidence coefficient as association between the key frames, and obtaining a final key frame sequence with the pose constraint.

Description

Technical Field

[0001] The present invention belongs to the field of laser SLAM technology, and in particular relates to a point cloud key frame management method for a long-term running laser SLAM system. Background Art

[0002] A keyframe refers to a series of representative data points selected from a sequence of laser point cloud data. In laser SLAM systems, the introduction of a keyframe mechanism can significantly reduce the state dimensions involved in optimization, thereby reducing the computational burden of the system. However, existing keyframe management methods have many shortcomings. First, in terms of keyframe screening, the keyframe screening strategy based on state increments is currently the most widely used keyframe selection strategy. This strategy has problems such as being too sensitive to angle changes, parameters needing to be manually adjusted according to lidar specifications, and poor adaptability to scenarios such as geometric structure degradation, making it difficult to screen keyframes at appropriate intervals. In addition, existing laser SLAM systems generally lack a method for maintaining historical keyframes when managing keyframes, and are unable to clear redundant observations generated during long-term operation, which will lead to the continuous growth of the system state dimension, thereby seriously reducing the real-time performance of the system. Summary of the Invention

[0003] Conventional laser SLAM systems store laser point cloud measurements as keyframes. In actual operation, the number of keyframes increases over time, degrading the overall real-time performance of the system. To address the issue of keyframe inflation during long-term operation, this paper proposes a point cloud keyframe management method for long-term laser SLAM systems.

[0004] The present invention is achieved through the following technical solutions:

[0005] A point cloud keyframe management method for a long-term laser SLAM system includes the following steps:

[0006] S1: Align the current laser observation to the world coordinate system and perform voxel map division based on the spatial hash function;

[0007] S2: Use the principal component analysis algorithm to determine voxel features and filter out new plane features;

[0008] S3: Based on the newly added plane features obtained in S2, the plane feature observation increment between the currently observed laser point cloud and the previous key frame is calculated, and the key frame is filtered based on the observation increment threshold;

[0009] S4: Construct the scan context descriptor for the latest key frame obtained in S3, and calculate the scan context descriptor distance between all historical key frames and the latest key frame, perform repeated observation search, and obtain the similar key frame match with the closest descriptor distance;

[0010] S5: Construct a local map consisting of similar keyframe matches obtained in S4 and several keyframes before and after them, recalculate the observation increment of the keyframe in the local map, and remove the redundant keyframe with the smallest observation increment;

[0011] S6: Perform local reconstruction of key frames based on the minimum spanning tree algorithm, use the relative pose constraint with the highest confidence as the association between key frames, and obtain the final key frame sequence with pose constraints.

[0012] Furthermore, in step S2, whether it is a planar feature is determined based on the ratio of the eigenvalues ​​of the covariance matrix of the point cloud distribution within the voxel:

[0013]

[0014] Among them, plane(v i ) is the voxel v i The feature discriminant function, 1 represents a plane feature, 0 represents a non-plane feature, λ1, λ2, λ3 are the matrices A(v i ), and λ1>λ2>λ3, θ is the plane feature threshold.

[0015] Furthermore, the specific calculation formula for the laser point cloud observation increment is:

[0016]

[0017] Among them, f(P) is the observation increment of point cloud P, ΔN is the number of newly extracted plane features of point cloud P, and N v is the total number of voxels observed in the point cloud P, and λ3 is the number of voxels v i The minimum eigenvalue of the covariance matrix of the inner point cloud distribution, is the feature value threshold.

[0018] Furthermore, the point cloud observation increment is weighted based on the uncertainty of the current system state and the moving speed of the lidar sensor. The specific formula is:

[0019]

[0020] Among them, s(P,Σ,v) is the observation increment weighted by the pose uncertainty and the system movement speed, Σ is the current system pose uncertainty, v is the current system movement speed, and t(Σ) is the trace of the matrix Σ.

[0021] Furthermore, in step S4, similar key frames are matched and several key frames before and after them form a local map, and the number of the selected key frames before and after them is 5 to 20 frames respectively.

[0022] Furthermore, the local key frame reconstruction step includes:

[0023] Calculate the relative spatial relationship between any two key frames among all key frames and construct relative pose constraints;

[0024] Calculate the relative pose uncertainty and use the relative pose uncertainty as the relative pose constraint weight;

[0025] Based on the minimum spanning tree algorithm, the minimum weight constraint is retained, and the relative pose constraint with the highest confidence is used as the association between key frames to obtain a reconstructed key frame sequence with pose constraints.

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

[0027] The present invention provides a point cloud key frame management method for a long-term running laser SLAM system. In key frame screening, key frame screening is performed based on feature observation increments between laser point clouds to avoid excessive overlap between key frames, so as to ensure good sparsity of the point cloud map. In key frame maintenance, repeated observations are detected by scanning context descriptors, and the key frame sequence after removing redundant observations is reconstructed based on the minimum spanning tree algorithm, so as to achieve effective restriction of the system state dimension in long-term operation, thereby improving the overall real-time performance of the algorithm. The present invention can effectively suppress the growth of the number of point cloud key frames of the laser SLAM system in long-term operation without losing the positioning and mapping accuracy of the laser SLAM system, reduce the data storage scale and computing consumption, achieve sparsity maintenance of the global point cloud map, reduce the system state dimension, and achieve improvement of the overall computing efficiency of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 is a flow chart of an embodiment of the present invention;

[0029] Figure 2 Schematic diagram of laser voxel segmentation according to an embodiment of the present invention;

[0030] Figure 3 This is a flowchart of the key frame local reconstruction according to an embodiment of the present invention;

[0031] Figure 4 A comparison diagram of the number of key frames before and after key frame management in multiple experimental examples according to an embodiment of the present invention;

[0032] Figure 5 A comparison diagram of a local map before and after key frame management according to an embodiment of the present invention;

[0033] Figure 6 A comparison diagram of key frame trajectories before and after key frame management according to an embodiment of the present invention;

[0034] Figure 7 3 is a box plot of the absolute trajectory error before and after key frame management according to an embodiment of the present invention. DETAILED DESCRIPTION

[0035] Exemplary embodiments of the present invention will be described in more detail below with reference to the accompanying drawings. Although exemplary embodiments of the present invention are shown in the accompanying drawings, it should be understood that the present invention can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided to enable a more thorough understanding of the present invention and to fully convey the scope of the present invention to those skilled in the art. It should be noted that, unless there is a conflict, the embodiments of the present invention and the features within the embodiments may be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in conjunction with the embodiments.

[0036] The present invention discloses a specific implementation method of a point cloud key frame management method for a long-term laser SLAM system. The process is as follows: Figure 1 As shown, the details are as follows:

[0037] S1: Align the current laser measurement to the world coordinate system through spatial transformation. The specific calculation formula for point cloud spatial transformation is as follows:

[0038]

[0039] in, Align the coordinates of point j in the laser point cloud to the world coordinate system, Represents the rotation matrix of the current laser measurement in the world coordinate system, p j is the coordinate of point j in the laser point cloud under the current laser measurement system, Represents the translation vector of the current laser measurement system in the world coordinate system.

[0040] The 3D voxel index to which the laser point belongs is calculated based on the spatial hash function. The specific calculation formula is:

[0041]

[0042] Among them, Hash(p) is the hash value of point p, x p 、y p 、z p is the three-dimensional coordinate value of point p in the world coordinate system, is the floor symbol, mod is the remainder operator, B is a random prime constant, and M is the maximum capacity of the hash table.

[0043] S2: Calculate the point cloud distribution covariance matrix of the voxel to which each point in the current laser frame belongs. The specific calculation formula is:

[0044]

[0045] Among them, A(v i ) is the voxel v i The point cloud distribution covariance matrix, is the voxel v in the world coordinate system i The coordinates of the j-th point in is the voxel v in the world coordinate system i The centroid of the interior points.

[0046] According to the matrix A(v i ) is used to extract the plane features, and the following is obtained: Figure 2 The plane feature point cloud shown, for voxel v i , its plane feature discriminant function is:

[0047]

[0048] Among them, plane(v i ) is the voxel v i The feature discriminant function, 1 represents a plane feature, 0 represents a non-plane feature, λ1, λ2, λ3 are the matrices A(v i ), and λ1>λ2>λ3, θ is the plane feature threshold.

[0049] S3: Based on the newly added plane features obtained in S2, the plane feature observation increment between the currently observed laser point cloud and the previous keyframe is calculated. The specific formula is:

[0050]

[0051] Among them, f(P) is the observation increment of point cloud P, ΔN is the number of newly extracted plane features of point cloud P, and N v is the total number of voxels observed in the point cloud P, and λ3 is the number of voxels v i The minimum eigenvalue of the covariance matrix of the inner point cloud distribution, is the feature value threshold.

[0052] The observation increment of the current laser point cloud is weighted based on the uncertainty of the current system state and the moving speed of the lidar sensor. The specific calculation formula is:

[0053]

[0054] Among them, s(P,Σ,v) is the observation increment weighted by the pose uncertainty and the system movement speed, Σ is the current system pose uncertainty, v is the current system movement speed, and t(Σ) is the trace of the matrix Σ.

[0055] When the observation increment of the laser observation in the current frame is greater than the threshold, it will be selected as a new key frame and stored.

[0056] S4: Construct the scanning context descriptor for the latest key frame obtained in S3. The specific calculation formula is:

[0057]

[0058] in, represents the descriptor at the key frame index (i, j) of the kth frame, z p It is the z-axis coordinate value of point p in the world coordinate system, indicating the height of point p.

[0059] The key frame sequence is searched to obtain key frames similar to the latest key frame, and the scan context descriptor distance between all historical key frames and the latest key frame is calculated to obtain the similar key frame matching with the closest descriptor distance.

[0060] S5: Construct a local voxel map consisting of the similar keyframe matches obtained in S4 and the five keyframes before and after it, recalculate the weighted observation increment s(P,Σ,v) of each keyframe in the local map, and remove the redundant keyframe with the smallest weighted observation increment.

[0061] S6: For the retained local key frames, calculate the relative transformation relationship and covariance matrix between any two key frames. The specific calculation formula is:

[0062]

[0063]

[0064] in, is the relative pose transformation matrix between the i-th and j-th key frames, T i w is the pose matrix of the i-th key frame in the world coordinate system, is the pose matrix of the jth key frame in the world coordinate system, Σ ij represents the uncertainty of the relative pose between the i-th frame and the k-th key frame, Σ i is the uncertainty of the pose of the i-th key frame in the world coordinate system, Σ j is the uncertainty of the pose of the j-th key frame in the world coordinate system, I is the unit matrix, log e (·) is the exponential mapping of the matrix, (·)^ is the antisymmetric matrix operator of the vector, ρ, is the pose matrix Translational and rotational components under Lie algebras.

[0065] Σ ijAs the weight of edge (i, j) to construct a complete subgraph, according to Figure 3 The process shown in the figure reconstructs local keyframes based on the minimum spanning tree, retains the minimum weight constraint, takes the relative pose constraint with the highest confidence as the association between keyframes, and obtains the final keyframe sequence with pose constraints.

[0066] Taking LIO-SAM as the basic algorithm, a control experiment of the key frame management method is carried out to verify the effectiveness of the present invention. Figure 4 : is a comparison chart of the number of key frames before and after key frame management according to the present invention in different experimental examples. Figure 4 It can be seen that in each experimental example, the reduction rate of the number of key frames is 56%, 36%, 64% and 49% respectively, which proves that the present invention can effectively reduce the number of point cloud key frames in long-term operation. Figure 5-7 The specific experimental results in one embodiment. Figure 5 This is a comparison diagram of the local point cloud map before and after key frame management according to the present invention, where Figure 5 (a) is a local point cloud map before keyframe management using the present invention, Figure 5 (b) is a local point cloud map after key frame management using the present invention. Figure 6 This is a comparison chart of key frame curves before and after key frame management. Figure 6 (a) is the global trajectory, Figure 6 (b) is a zoomed-in image of a local trajectory. The gray dotted line represents the true value of the trajectory, the blue solid line represents the trajectory before keyframe management, and the red solid line represents the trajectory after keyframe management. Figure 7 It is a box plot of the absolute trajectory error before and after key frame management. The blue represents the absolute trajectory error before key frame management, and the green represents the absolute trajectory error after key frame management. Figure 5 It can be seen that in long-term operation, due to the measurement noise of the laser radar, multiple scans of the same position will cause the map to be blurred. However, the key frame management according to the present invention can clear redundant observations in time, effectively improve the sparsity of the point cloud map, and significantly improve the quality of the point cloud map. Figure 6 and Figure 7 It can be seen that the key frame management according to the present invention can bring about a certain improvement in positioning accuracy, and the absolute trajectory error is reduced from 0.167 meters to 0.098 meters.

[0067] In summary, the present invention can effectively control the number of key frames selected in the laser SLAM system, reduce the scale of stored data during long-term operation, achieve sparsity maintenance of the global point cloud map, and bring about a certain improvement in accuracy while improving the overall operation efficiency of the system.

[0068] The present invention has been described in detail above through the embodiments, but the contents described are only exemplary embodiments of the present invention and cannot be considered to limit the scope of implementation of the present invention. The scope of protection of the present invention is defined by the claims. Any use of the technical solution described in the present invention, or any person skilled in the art who, inspired by the technical solution of the present invention, designs a similar technical solution within the essence and scope of protection of the present invention to achieve the above-mentioned technical effects, or any equivalent changes and improvements made to the scope of application, shall still fall within the scope of protection covered by the patent of the present invention. It should be noted that for the sake of clarity, the description of some components and processes that have no direct and obvious connection with the scope of protection of the present invention but are known to those skilled in the art are omitted in the description of the present invention.

Claims

1. A point cloud keyframe management method for a long-term laser SLAM system, characterized in that: The following steps are involved: S1: Align the current laser observation to the world coordinate system and perform voxel map division based on the spatial hash function; S2: Use the principal component analysis algorithm to determine voxel features and filter out new plane features; S3: Based on the newly added plane features obtained in S2, the plane feature observation increment between the currently observed laser point cloud and the previous key frame is calculated, and the key frame is filtered based on the observation increment threshold; S4: Construct the scan context descriptor for the latest key frame obtained in S3, and calculate the scan context descriptor distance between all historical key frames and the latest key frame, perform repeated observation search, and obtain the similar key frame match with the closest descriptor distance; S5: Construct a local map consisting of similar keyframe matches obtained in S4 and several keyframes before and after them, recalculate the observation increment of the keyframe in the local map, and remove the redundant keyframe with the smallest observation increment; S6: Perform local reconstruction of key frames based on the minimum spanning tree algorithm, use the relative pose constraint with the highest confidence as the association between key frames, and obtain the final key frame sequence with pose constraints.

2. The point cloud key frame management method for a long-term laser SLAM system according to claim 1, characterized in that: In step S2, whether it is a plane feature is determined based on the ratio of the eigenvalues ​​of the covariance matrix of the point cloud distribution within the voxel: Among them, plane(v i ) is the voxel v i The feature discriminant function, 1 represents a plane feature, 0 represents a non-plane feature, λ1, λ2, λ3 are the matrices A(v i ), and λ1>λ2>λ3, θ is the plane feature threshold.

3. The point cloud key frame management method for a long-term laser SLAM system according to claim 2, characterized in that: The specific calculation formula for the laser point cloud observation increment is: Among them, f(P) is the observation increment of point cloud P, ΔN is the number of newly extracted plane features of point cloud P, and N v is the total number of voxels observed in the point cloud P, and λ3 is the number of voxels v i The minimum eigenvalue of the covariance matrix of the inner point cloud distribution, is the feature value threshold.

4. The point cloud key frame management method for a long-term laser SLAM system according to claim 3, characterized in that: The point cloud observation increment is weightedly calculated based on the current system state uncertainty and moving speed of the lidar sensor. The specific formula is: Among them, s(P,Σ,v) is the observation increment weighted by the pose uncertainty and the system movement speed, Σ is the current system pose uncertainty, v is the current system movement speed, and t(Σ) is the trace of the matrix Σ.

5. The point cloud key frame management method for a long-term laser SLAM system according to claim 1, characterized in that: In step S4, similar key frames are matched and several key frames before and after them form a local map, and the number of the key frames before and after them is selected from 5 to 20 frames respectively.

6. The point cloud key frame management method for a long-term laser SLAM system according to claim 1, characterized in that: The local key frame reconstruction step includes: Calculate the relative spatial relationship between any two key frames among all key frames and construct relative pose constraints; Calculate the relative pose uncertainty and use the relative pose uncertainty as the relative pose constraint weight; Based on the minimum spanning tree algorithm, the minimum weight constraint is retained, and the relative pose constraint with the highest confidence is used as the association between key frames to obtain a reconstructed key frame sequence with pose constraints.

Images