A small-view-angle laser radar rapid loop detection method under an unstructured environment

Abstract

A small-view-angle laser radar fast loop detection method in an unstructured environment, the method comprehensively utilizes point cloud intensity and geometric plane features to quickly screen historical key frames to obtain accurate loop frames corresponding to the current frame, the invention effectively solves the false detection and missed detection problems of loop detection caused by insufficient and incomplete point cloud scene features of solid-state laser radar small view angle in unstructured scenes. The method first accumulates the point cloud output by the front-end odometer of the solid-state radar and judges whether it is a key frame, then uses the intensity feature to coarsely screen out a small number of candidate frames, uses the fast screening method based on the plane geometric feature to judge whether the current frame has a loop frame, if there is, uses the process matrix in the screening to quickly calculate the pose matrix of the current frame and the loop frame, adds the calculation result to the pose graph and performs global graph optimization. The invention can effectively improve the accuracy and efficiency of the loop detection method of the small-view-angle laser radar in the unstructured scene.

Description

Technical Field

[0001] This invention relates to the field of lidar detection, specifically a method for rapid loop closure detection of small-view lidar in unstructured environments. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a crucial technology for small unmanned vehicles to achieve navigation and localization in complex environments and build maps of their surroundings. SLAM technology is mainly divided into visual SLAM and laser SLAM. Compared to visual sensors, laser radar has stronger environmental adaptability, and laser SLAM technology has been widely used in various fields such as air-to-ground transportation, intelligent transportation, disaster relief, and military operations. Moving laser SLAM systems suffer from the problem of accumulated point cloud pose estimation errors over time, and loop closure detection technology is one of the key technologies to suppress these accumulated errors. In recent years, low-power, low-cost solid-state laser radar has been widely used in mapping and localization of small unmanned vehicles. However, its relatively small field of view brings a series of new challenges to solid-state laser radar-based laser SLAM technology, especially loop closure detection technology.

[0003] Most existing fast loop closure detection methods are based on generating global descriptors for a single feature of a large field-of-view mechanical radar point cloud. A typical example is the ScanContext (SC) loop closure detection scheme, which uses geometric features as a global descriptor. This scheme divides the point cloud into multiple annular regions and constructs a circumferential and radial descriptor matching search of historical frame point clouds, achieving fast keyframe recognition. However, the ScanContext scheme, which only extracts the maximum elevation feature of the point cloud, imposes certain requirements on the radar's detection range and loses many scene features, easily leading to false loop closure detection. Later, some teams built upon this by generating an IntensityScanContext (ISC) based on the maximum intensity of the point cloud, designing an intensity scan context scheme to reduce the false recognition rate of loop closure detection methods. However, this scheme requires a low downsampling rate for keyframes to ensure recognition accuracy, and both methods require a relatively long detection range to improve detection accuracy. Furthermore, neither method effectively addresses the high computational load of the pose matrix between subsequent loop closure frames.

[0004] The current patent search results are as follows:

[0005] Technical comparison with patent CN115047487A "Loop Detection Method and Device Based on Point Cloud Intensity and Height Information":

[0006] Patent CN115047487A aims to provide accurate loop closure detection results for robotic laser SLAM systems based on traditional 360° horizontal view mechanical lidar in general environments. Our method aims to improve the loop closure detection accuracy and efficiency of small-view solid-state lidar in unstructured environments, so as to promote the wider application of miniaturized unmanned vehicles in unstructured environments.

[0007] In patent CN115047487A, the point cloud preprocessing process simply transforms the point cloud from the carrier coordinate system to the world coordinate system, converting it into keyframes. Our preprocessing method, however, accumulates the calculated pose from the carrier coordinate system to the world coordinate system, uses the pose of intermediate frames to determine the pose of the current frame, and adds a pose change threshold to determine whether it is a keyframe. Therefore, the keyframe identification strategies for loop closure detection differ between the two methods, and this method improves the detection performance of small-view radar point clouds.

[0008] The intensity information encoding process in patent CN115047487A involves constructing a mapping relationship from the three-dimensional spherical coordinate system of all point clouds to a two-dimensional planar matrix, and recording the intensity of all point clouds in the matrix values, resulting in a very high matrix dimension. In contrast, our proposed intensity descriptor encoding process adds a morphological filtering process for invalid ground point clouds and performs normalization correction on the intensity information. It then directly constructs a two-dimensional matrix divided into planar blocks using the two-dimensional polar coordinates of the point clouds, with the matrix values ​​recording the maximum intensity of the point clouds within a region, resulting in a lower matrix dimension. Therefore, the intensity encoding processes and methods of the two methods are different, and our method has a lower matrix dimension and can accelerate the generation and retrieval speed of intensity information.

[0009] The geometric information encoding process in patent CN115047487A simply constructs a relatively simple two-dimensional matrix, and the matrix values ​​only record the maximum height information of the point cloud. This encoding method only retains the geometric information of the maximum height point. Our geometric information encoding method, however, involves planarizing a point cloud frame into voxels, dividing the voxels into two categories to extract planar keypoints, and then using these planar keypoints as the geometric information encoding for that frame of point cloud. Therefore, the encoding methods and results of the two methods are fundamentally different, and our method can accurately improve the recognition accuracy in unstructured environments.

[0010] Comparison with patent CN114187418A "Loop closure detection method, point cloud map construction method, electronic device and storage medium":

[0011] The loop closure detection method proposed in patent CN114187418A aims to solve the problem in existing loop closure detection methods based on traditional 360° horizontal view mechanical lidar in general environments, where the coordinates of two frames are basically overlapping and the position difference cannot be identified. Our method aims to solve the problem of insufficient and incomplete point cloud scene features of small-view solid-state lidar in unstructured environments that cause loop closure detection.

[0012] Patent CN114187418A does not perform additional preprocessing on the front-end point cloud, while our preprocessing method uses the calculated pose to accumulate the point cloud frame in the volume coordinate system to the world coordinate system, and uses the pose of the intermediate frame as the carrier pose of the frame, and adds a pose change threshold to determine whether it is a key frame. This processing method will improve the detection performance of small-view radar point clouds.

[0013] The loop closure detection process designed in patent CN114187418A only utilizes some scene features such as the average height of the point cloud. Our method, on the other hand, comprehensively utilizes the intensity information of the point cloud to coarsely screen candidate frames and the planar geometric information of the point cloud to finely screen them. This approach provides a more comprehensive and sufficient acquisition of scene information and is more suitable for point cloud scene features that are insufficient or incomplete for small-view radar, effectively improving the accuracy of loop closure detection.

[0014] In patent CN114187418A, during the geometric information encoding process of the surrounding scene, a two-dimensional matrix is ​​constructed, and the matrix values ​​record the average height information of the point cloud within this region. Offset is used to identify loopback frames. Our geometric information encoding method, however, involves planarizing a point cloud frame into voxels, dividing the voxels into two categories to extract planar keypoints, and then using these keypoints as the geometric information encoding for that frame. SVD decomposition is used to obtain the pose matrix, and the overlap of the planar voxels is used to finally identify loopback frames. Therefore, the encoding methods and results of the two methods are fundamentally different. The geometric information encoding method proposed in this paper is more suitable for unstructured environments.

[0015] In summary, existing typical single-feature loop closure detection methods are difficult to directly apply to scene features where small-view solid-state lidar point clouds are insufficient or incomplete in unstructured environments. This problem leads to missed and false detections of loop closures, resulting in severe trajectory drift. Furthermore, the high computational cost of the inter-frame pose matrix for loop closures further hinders the deployment of SLAM systems on small unmanned platforms. Therefore, to address the problems and challenges faced by solid-state lidar carriers in unstructured environments, a fast loop closure detection method for small-view lidar in unstructured environments needs to be designed. Summary of the Invention

[0016] The technical problem to be solved by this invention is to overcome the shortcomings of the prior art, break through the limitations of typical fast loop closure detection methods, and propose a fast loop closure detection method for small-view lidar in unstructured environments.

[0017] The method first accumulates the point cloud output from the solid-state radar front-end odometer and determines whether it is a keyframe. Then, it enters a coarse screening stage based on intensity features. For the candidate frames selected by the coarse screening, a fast screening method based on planar geometric features is used to determine whether the current keyframe has a corresponding loop-loop frame. If a loop-loop frame exists, the process matrix in the fast screening is used to calculate the pose transformation matrix between the current frame and the loop-loop frame. The calculation result is added as an additional constraint to the pose map and global optimization is performed to update the pose trajectory of all frames and the global map. The invention can effectively improve the accuracy and efficiency of loop-loop detection of small-view lidar in unstructured scenes.

[0018] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0019] A fast loop closure detection method using a small-view lidar in unstructured environments, comprising the following steps, characterized in that:

[0020] Step 1: After the point cloud data collected by the solid-state lidar is processed by the front-end odometer, the estimated pose and the carrier coordinate system point cloud are output. After downsampling the carrier point cloud, the estimated pose is used to accumulate the solid-state lidar point cloud frame to the world coordinate system. The pose of the intermediate frame is used as the carrier pose of the accumulated frame. The pose change threshold is set to determine whether the current accumulated frame is a key frame. If it is a key frame, it enters the coarse screening stage based on intensity features.

[0021] Step 2: The ground point cloud is filtered using a progressive morphological filter on the current keyframe. To enhance the descriptive performance of the point cloud intensity features, the intensity values ​​of the filtered point cloud are normalized. A global descriptor for the intensity scanning context is constructed on the processed point cloud. The circumferential vectors in the global descriptor are extracted separately and stored in a kd-tree structure. The similarity between the circumferential vectors of the current frame and the historical frames in the storage structure is calculated. A small number of historical frames that pass the similarity threshold are selected as candidate frames. After that, the fast screening stage based on planar geometric features will begin.

[0022] Step 3: Perform planar voxelization on the point clouds of the current frame and candidate frames, divide the planar voxels into two categories to extract the planar key points, construct the covariance matrix using the planar key points of each candidate frame and the current frame and perform SVD decomposition to obtain multiple pairs of pose transformation matrices, compare the number of key point overlaps after transformation to obtain the optimal pose transformation matrix of each candidate frame and the current frame, use this matrix to calculate the number of overlaps of all planar voxels of each candidate frame and the current frame for verification again, and take the candidate frame with the largest number of planar overlaps as the loop closure frame;

[0023] Step 4: Perform ICP matching between loop closure frames, add the matching results to the pose map and perform global optimization, update the pose of all frames and the global map, and complete the fast loop closure detection of small-view LiDAR in unstructured environment.

[0024] As a further improvement to the present invention, the specific steps of step 1 include:

[0025] 1-1 Accumulate solid-state radar point clouds into frames and convert them to the world coordinate system; single-frame carrier coordinate system point cloud. To the world coordinate system After conversion, the data is accumulated into keyframe P. k The formula is:

[0026]

[0027]

[0028] in, This represents the point cloud data in the carrier coordinate system of the i-th frame. R represents the point cloud data in the world coordinate system of the i-th frame. i T is the rotation matrix of the i-th frame. i It is the translation vector of the i-th frame. The pose matrix of a single frame is obtained by front-end pose matching of SLAM. The key frame P used for loop closure detection is obtained by accumulating the point clouds in the world coordinate system of k frames. k The point cloud pose matrix of the n / 2th frame is used as P. k The corresponding pose.

[0029] As a further improvement to the present invention, step 2 includes the following specific steps:

[0030] 2-1 Perform progressive morphological ground filtering on the current keyframe point cloud. The specific filtering process is as follows:

[0031] The point cloud is divided into two-dimensional grids, and the minimum elevation point cloud is selected in each grid to construct the minimum surface grid. Then, a filter with morphological opening operation is applied to each grid to smooth the grid surface and output the point cloud after elevation difference threshold filtering. Then, the filter size and elevation difference threshold are increased, and the above operation is repeated for each grid. The iteration is until the predefined maximum value is reached to obtain the keyframe point cloud after ground filtering.

[0032] 2-2 The intensity normalization correction is performed on the point cloud after ground filtering. The specific process is as follows: For the point cloud intensity value collected between 0 and 255, normalization processing is performed to correct it to the range of [0,1], where intensity data greater than 100 are uniformly set to 1.

[0033] 2-3 generates an intensity scan context descriptor matrix for the current frame point cloud after processing in 2-1 and 2-2;

[0034] Specifically, for a keyframe point cloud P obtained after the above steps... k ={P1,P2,P3...P i Let P be the spatial Cartesian coordinates and intensity information of one of the points. i ={x i ,y i ,z i ,η i Transform the point cloud from Cartesian coordinates to polar coordinates:

[0035]

[0036]

[0037] Then P can be used in polar coordinates. i ={ρ i ,θ i ,z i ,η i} represents the point, and the point cloud in polar coordinate space is divided into two-dimensional circumferential and radial regions, dividing the space into N. s ×N r Block space:

[0038]

[0039] Among them, L max n represents the maximum detection range. s n represents the distance of the circumferential gap. r Let S represent the interval angle of each radial sector. Then, for each subspace S... ij The corresponding point cloud P in n Defined as:

[0040]

[0041] Therefore, keyframe P k The point cloud in the image is divided into corresponding N coordinates according to polar coordinates. s ×N r In a block subspace, the maximum intensity of the point cloud in each block subspace is taken as the value of that subspace, denoted as φ(S). ij ), thus obtaining N s ×N r 3D descriptor matrix I:

[0042] φ(S ij )=maxη n ,P n ∈S ij

[0043]

[0044] Where, η n It is S ij Intensity information of neutron space point clouds, I is N s ×N r A 3D real matrix;

[0045] 2-4 Calculate the similarity between the current frame and historical frames to obtain candidate keyframes. The specific process for obtaining candidate frames is as follows: For a frame intensity descriptor matrix I, use the row vectors (i.e., the circumferential vectors) in the matrix to perform dimensionality reduction. Therefore, for a certain frame descriptor matrix I, use the following N... r The dimension K vector represents:

[0046]

[0047] Where, r i Let r represent the cyclic vector in the i-th row. To improve search speed, the cyclic vector r will be defined. i Occupancy ratio ψ:

[0048]

[0049] Where, ||r i ||0 is r i The L0 norm, i.e., r i The number of non-zero values ​​in ψ(r) i The size of ) is independent of the radar viewpoint, and therefore has rotation invariance. The N of each frame descriptor I r The dimension vector K will be used to construct a kd-tree, and the nearest neighbor search will be used to select the corresponding keyframes that are similar to the K vector of the current keyframe as candidate keyframes for loop closure.

[0050] As a further improvement to the present invention, step 3 specifically includes the following steps:

[0051] 3-1 The process of planar voxelization and planar keypoint extraction for the current frame is as follows: For the loop closure candidate keyframes obtained in step 2-4, find the original point cloud of the corresponding frame and divide it into voxel meshes. Then, extract the point cloud p from the voxel mesh... i Calculate its covariance matrix:

[0052]

[0053]

[0054] To calculate the eigenvalues ​​of the matrix Σ, planar voxels must satisfy the conditions λ3<σ1, λ2>σ2, where λ2 and λ3 are the second and third largest eigenvalues ​​of the matrix, respectively, and σ1 and σ2 are preset parameter thresholds. Voxel meshes that satisfy these conditions are extracted as planar voxel meshes. Planar voxel meshes with the same planar normal direction and a distance below the threshold are joined with adjacent meshes to form the same plane. Planar voxel meshes that are not in the same direction during the joining process are classified as boundary planes. Therefore, a frame of planar voxelized point cloud will be divided into two types of planes. Voxels belonging to the boundary plane are projected onto the other plane. The distance of the projection point is taken as the distance of the point formed by the maximum distance among the boundary voxels. These projection points are divided into equal regions, and the point with the maximum distance in each region is retained as the planar key point.

[0055] The calculation process for obtaining the overlap between the planar keypoints of each candidate frame and the current frame in 3-2 is as follows: For all planar keypoints within a frame obtained in 3-1, a kd-tree structure is constructed and the nearest points are found to form a triangle structure. Since the position of the corresponding vertex of the triangle structure is also determined after the side length is determined, the corresponding points in the current frame and the candidate frames are found using this correspondence. For each pair of triangle structures in the two frames, the following SVD decomposition is performed:

[0056]

[0057] [U,S,V]=SVD(H)

[0058] in, and These are the coordinates of the three vertices of the corresponding triangle in the current frame and the candidate frame, respectively. and These are the center coordinates of the corresponding triangles in the current frame and the candidate frame, respectively. The coordinate difference is calculated to obtain the covariance matrix H. After SVD decomposition, the orthogonal matrix UV and the singular value matrix S are obtained. The decomposition results are processed as follows:

[0059]

[0060] T=(R,t),T∈SE(3)

[0061] That is, to obtain a pose transformation matrix T between two frames, select the T that maximizes the number of overlapping planar keypoints between the two frames after the transformation. max The matrix serves as the optimal transformation matrix between two frames;

[0062] 3-3 The process of performing planar voxel ICP matching between the current frame and the loopback frame and verifying the planar overlap between the two frames is as follows:

[0063] Let the planar voxel mesh be the set of its center points g and normal vector u. Then, the point cloud P after a frame of planar voxelization is... π Represented as:

[0064] P π =[(g1,u1),...(g i ,u i )]

[0065] Where i represents the number of planar voxels in the current frame, using the current frame P obtained in 3-2 π K With candidate frame P π C The optimal transformation matrix T max For a candidate frame Pπ C (g) j C ,u j C Perform pose transformation and find the position Pπ in the current frame. K (g) i K ,u i K The nearest planar voxel (g,u) that satisfies the following conditions:

[0066] ||Ru i K -u j C ||2<σ n

[0067]

[0068] Where, σ n and σ d It is the overlap threshold of the preset value, denoted as Let q be the number of planar voxels that meet the conditions in the i-th frame. The largest candidate frame is the final selected loopback frame.

[0069] As a further improvement to the present invention, step 4 includes the following specific steps:

[0070] The optimal pose matrix obtained from SVD decomposition in step 3 is used as an initial value and substituted into the inter-frame ICP matching process to accelerate the solution of the pose transformation matrix between the original point cloud of the current frame and the original point cloud of the loop-back frame. The transformation matrix is ​​added to the pose map, and the ISAM solver in the GTSAM optimization library is used for global optimization of the pose map to update the pose of each frame and update the global map.

[0071] Compared with the prior art, the present invention, employing the above technical solution, has the following technical effects:

[0072] To address the problems of small-view LiDAR SLAM technology in unstructured environments using small unmanned vehicles, this invention proposes a fast loop closure detection method for small-view LiDAR in unstructured environments. This method primarily utilizes point cloud intensity to coarsely screen historical keyframes to obtain candidate frames, and then employs a fast screening method based on the overlap of planar keypoints to quickly obtain the corresponding loop closure frames from the candidate frames. This invention effectively solves the problem of low accuracy and high computational cost of typical loop closure detection methods caused by insufficient and incomplete scene features of small-view solid-state LiDAR in unstructured scenarios. The two-step detection method using intensity features and planar geometric features effectively alleviates the problem of false loop closure detection in unstructured environments. One-dimensional descriptors for global historical frame retrieval and the use of process variables to provide initial values ​​for ICP matching reduce the computational cost of loop closure detection. A dataset was collected using a small unmanned vehicle equipped with a Livox Horizon small-view solid-state LiDAR on a suburban highway with sparse structured features. After applying the loop closure detection method of this invention to this dataset, the trajectory curve successfully closed at the origin, further demonstrating the effectiveness of the proposed fast loop closure detection method for small-view LiDAR in unstructured environments. This invention provides a valuable reference for the design of a faster and more reliable real-time navigation and positioning system for small carrier robots in unstructured environments. Attached Figure Description

[0073] Figure 1 This is a flowchart of a fast loop closure detection method using small-view lidar in unstructured environments.

[0074] Figure 2 This is a flowchart of loopback frame recognition based on intensity and geometric features. Detailed Implementation

[0075] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments:

[0076] As attached Figure 1 The diagram shows a flowchart of a fast loop closure detection method for small-view lidar in unstructured environments designed in this invention. This invention first accumulates the point cloud output from the solid-state lidar front-end odometer and determines whether it is a keyframe. Then, it enters a coarse screening stage based on intensity features. For the candidate frames selected by the coarse screening, a fast screening method based on planar geometric features is used to determine whether the current keyframe has a corresponding loop closure frame. If a loop closure frame exists, the process matrix in the fast screening is used to calculate the pose transformation matrix between the current frame and the loop closure frame. The calculation result is added as an additional constraint to the pose graph and global optimization is performed, updating the pose trajectory of all frames and the global map.

[0077] As attached Figure 2The diagram shows the flowchart of the fast loop closure detection method designed in this invention, which is based on intensity and geometric features for loop closure frame recognition. First, the point cloud of the current keyframe is filtered, then the point cloud intensity is normalized. An intensity scanning context descriptor is constructed from the processed point cloud, and a kd-tree structure is built to store the descriptor. A small number of candidate frames are obtained by quickly calculating the similarity using the cyclic descriptor in the intensity scanning context. The current frame is then subjected to planar voxelization to generate planar keypoints in the frame. The planar voxelized point cloud corresponding to the candidate frame is extracted, and the overlap of planar keypoints between frames is calculated using SVD decomposition. The candidate frame with the highest overlap and its corresponding transformation matrix are retained. This matrix is ​​then used to calculate the planar voxel overlap between two frames to further verify whether the threshold condition for the existence of a loop closure frame is met.

[0078] Specifically, the implementation process will include the following steps:

[0079] Step 1: The point cloud data acquired by the solid-state lidar is processed by the front-end odometry, outputting the estimated pose and the carrier coordinate system point cloud. After downsampling the carrier point cloud, the estimated pose is used to accumulate the solid-state lidar point cloud frames in the world coordinate system, and the pose of the intermediate frames is used to obtain the carrier pose of the accumulated frame. A pose change threshold is set to determine whether the current accumulated frame is a key frame. If it is a key frame, the process proceeds to the coarse screening step based on intensity features.

[0080] a. Accumulate the solid-state radar point cloud into frames and convert them to the world coordinate system; single-frame carrier coordinate system point cloud. To the world coordinate system After conversion, the data is accumulated into keyframe P. k The formula is:

[0081]

[0082]

[0083] Among them, P c i This represents the point cloud data in the carrier coordinate system of the i-th frame. R represents the point cloud data in the world coordinate system of the i-th frame. i T is the rotation matrix of the i-th frame. i It is the translation vector of the i-th frame. The pose matrix of a single frame is obtained by front-end pose matching of SLAM. The key frame P used for loop closure detection is obtained by accumulating the point clouds in the world coordinate system of k frames. k The point cloud pose matrix of the n / 2th frame is used as P. k The corresponding pose.

[0084] Step 2 involves filtering the ground point cloud using a progressive morphological filter on the incoming current keyframe. To enhance the descriptive performance of point cloud intensity features, the filtered point cloud intensity values ​​are normalized. A global descriptor for the intensity scanning context is constructed from the processed point cloud. To accelerate the search, the cyclic vectors in the global descriptor are extracted separately and stored using a kd-tree structure. Similarity is calculated between the cyclic vectors of the current frame and historical frames in the storage structure, and a small number of historical frames that pass the similarity threshold are selected as candidate frames. Following this, a rapid selection stage based on planar geometric features will commence.

[0085] a. To improve the performance of the intensity descriptor, progressive morphological ground filtering is required on the current keyframe point cloud. This filtering process involves: dividing the point cloud into a two-dimensional grid, and selecting the point cloud with the minimum elevation in each grid to construct a minimum surface grid. Then, a filter with morphological opening operations is applied to each grid to smooth the grid surface, and the point cloud is output after elevation difference threshold filtering. The filter size and elevation difference threshold are then increased, and the above operation for each grid is repeated iteratively until a predefined maximum value is reached, obtaining the ground-filtered keyframe point cloud.

[0086] b. Perform intensity normalization correction on the filtered point cloud. This process is as follows: For point cloud intensity values ​​collected between 0 and 255, normalize the data to the range of [0,1], and set the intensity data greater than 100 to 1.

[0087] c. Generate an intensity scan context descriptor matrix for the current frame point cloud after processing steps 2-1 and 2-2. Specifically: for a keyframe point cloud P obtained after the above steps... k ={P1,P2,P3...P i Let P be the spatial Cartesian coordinates and intensity information of one of the points. i ={x i ,y i ,z i ,η i Transform the point cloud from Cartesian coordinates to polar coordinates:

[0088]

[0089]

[0090] Then P can be used in polar coordinates. i ={ρ i ,θ i ,z i ,η i} represents the point, and the point cloud in polar coordinate space is divided into two-dimensional circumferential and radial regions, dividing the space into N. s ×Nr Block space:

[0091]

[0092] Among them, L max n represents the maximum detection range. s n represents the distance of the circumferential gap. r This represents the interval angle of each radial sector. Therefore, for each subspace S... ij The corresponding point cloud P in n Defined as:

[0093]

[0094] Therefore, keyframe P k The point cloud in the image is divided into corresponding N coordinates according to polar coordinates. s ×N r Block subspace. The maximum intensity of the point cloud in each block subspace is taken as the value of that subspace, denoted as φ(S). ij From this, we can obtain N. s ×N r 3D descriptor matrix I:

[0095] φ(S ij )=maxη n ,P n ∈S ij

[0096]

[0097] Where, η n It is S ij Intensity information of neutron space point clouds, I is N s ×N r A real matrix of dimension 1.

[0098] d. Calculate the similarity between the current frame and historical frames to obtain candidate keyframes. The process of obtaining candidate frames is as follows: For a frame intensity descriptor matrix I, to improve its global search speed, the row vectors, i.e., the circumdirectional vectors, in the matrix will be used for dimensionality reduction. Therefore, for a certain frame descriptor matrix I, the following N can be used. r The dimension K vector represents:

[0099]

[0100] Where, r i Let r represent the cyclic vector in the i-th row. To improve search speed, the cyclic vector r will be defined. i Occupancy ratio ψ:

[0101]

[0102] Where, ||r i ||0 is r i The L0 norm, i.e., r i The number of non-zero values ​​in ψ(r) i The size of ) is independent of the radar viewpoint, and therefore has rotation invariance. The N of each frame descriptor I r The dimension vector K will be used to construct a kd-tree. Nearest neighbor search will be used to select keyframes whose K vectors are similar to the current keyframe as candidate keyframes for loop closure.

[0103] Step 3: Perform planar voxelization on the current frame point cloud, dividing the planar voxels into two categories to extract planar keypoints. Extract the planar voxelized point clouds corresponding to candidate frames, construct a covariance matrix using the planar keypoints of each candidate frame and the current frame, and perform SVD decomposition to obtain multiple pairs of pose transformation matrices. Compare the number of overlapping keypoints after transformation to obtain the optimal pose transformation matrix for each candidate frame and the current frame. To further improve recognition accuracy, use this matrix to calculate the number of overlapping planar voxels between each candidate frame and the current frame for further verification. The candidate frame with the largest number of overlapping planar voxels is designated as the loop-closure frame.

[0104] a. The process of planar voxelization and planar keypoint extraction for the current frame is as follows: For loop closure candidate keyframes, find the original point cloud of the corresponding frame, divide it into voxel meshes, and extract the point cloud p in the voxel mesh. i Calculate its covariance matrix:

[0105]

[0106]

[0107] The eigenvalues ​​of the matrix Σ are calculated. Planar voxels must satisfy λ3 < σ1, λ2 > σ2, where λ2 and λ3 are the second and third largest eigenvalues ​​of the matrix, respectively. σ1 and σ2 are preset parameter thresholds. Voxel meshes satisfying these conditions are extracted as planar voxel meshes. Planar voxel meshes with the same planar normal direction and a distance below the threshold are joined with adjacent meshes to form the same plane. Planar voxel meshes not in the same direction during the joining process are classified as boundary planes. Therefore, a frame of planar voxelized point cloud will be divided into two types of planes. Voxels belonging to the boundary plane are projected onto the other plane, and the distance to the point formed by the projection is taken from the maximum distance among the boundary voxels. These projected points are evenly divided into regions, and the point with the maximum distance in each region is retained as the planar keypoint.

[0108] b. For all planar keypoints obtained in 3-1 within a frame, construct a kd-tree structure and find the nearest points to form a triangle structure. Since the position of the corresponding vertex of the triangle structure is also determined after the side length is determined, use this correspondence to find the corresponding points in the current frame and the candidate frame. Perform the following SVD decomposition on each pair of triangle structures in the two frames:

[0109]

[0110] [U,S,V]=SVD(H)

[0111] in, and These are the coordinates of the three vertices of the corresponding triangle in the current frame and the candidate frame, respectively, q a K and q b C These are the center coordinates of the corresponding triangles in the current frame and the candidate frame, respectively. The coordinate difference is calculated to obtain the covariance matrix H. After SVD decomposition, the orthogonal matrix UV and the singular value matrix S are obtained. The decomposition results are processed as follows:

[0112]

[0113] T=(R,t),T∈SE(3)

[0114] This involves obtaining a pose transformation matrix T between two frames. The matrix T that maximizes the overlap of planar keypoints between the two frames after the transformation is then selected. max The matrix serves as the optimal transformation matrix between two frames.

[0115] c. The process of matching the planar voxel ICP of the current frame and the loopback frame and verifying the planar overlap between the two frames is as follows: Let the planar voxel mesh be the set of its center point g and normal vector u, then the point cloud P after one frame of planar voxelization is... π It can be represented as:

[0116] Pπ=[(g1,u1),...(g i ,u i )]

[0117] Where i represents the number of planar voxels in the current frame. Using the current frame P obtained in 3-2... π K With candidate frame P π C The optimal transformation matrix T max For a candidate frame Pπ C (g) j C ,u j C Perform pose transformation and find the position Pπ in the current frame.K (g) i K ,u i K The nearest planar voxel (g,u) that satisfies the following conditions:

[0118] ||Ru i K -u j C ||2<σ n

[0119]

[0120] Where, σ n and σ d It is the overlap threshold of the preset value, denoted as Let q be the number of planar voxels that meet the conditions in the i-th frame. The largest candidate frame is the loopback frame finally selected by this invention.

[0121] Step 4 is described in detail as follows: The optimal pose matrix obtained from SVD decomposition in Step 3 is used as an initial value and substituted into the inter-frame ICP matching process to accelerate the solution of the pose transformation matrix between the original point cloud of the current frame and the original point cloud of the loop-back frame. The transformation matrix is ​​added to the pose map, and the ISAM solver in the GTSAM optimization library is used for global optimization of the pose map, updating the pose of each frame and updating the global map.

[0122] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any other way. Any modifications or equivalent changes made based on the technical essence of the present invention shall still fall within the scope of protection claimed by the present invention.

Claims

1. A method for fast loop closure detection using a small-view lidar in unstructured environments, comprising the following steps, characterized in that: Step 1: After the point cloud data collected by the solid-state lidar is processed by the front-end odometer, the estimated pose and the carrier coordinate system point cloud are output. After downsampling the carrier point cloud, the estimated pose is used to accumulate the solid-state lidar point cloud frame to the world coordinate system. The pose of the intermediate frame is used as the carrier pose of the accumulated frame. The pose change threshold is set to determine whether the current accumulated frame is a key frame. If it is a key frame, it enters the coarse screening stage based on intensity features. Step 2: The ground point cloud is filtered using a progressive morphological filter on the current keyframe. To enhance the descriptive performance of the point cloud intensity features, the intensity values ​​of the filtered point cloud are normalized. A global descriptor for the intensity scanning context is constructed on the processed point cloud. The circumferential vectors in the global descriptor are extracted separately and stored in a kd-tree structure. The similarity between the circumferential vectors of the current frame and the historical frames in the storage structure is calculated. A small number of historical frames that pass the similarity threshold are selected as candidate frames. After that, the fast screening stage based on planar geometric features will begin. Step 3: Perform planar voxelization on the point clouds of the current frame and candidate frames, divide the planar voxels into two categories to extract the planar key points, construct the covariance matrix using the planar key points of each candidate frame and the current frame and perform SVD decomposition to obtain multiple pairs of pose transformation matrices, compare the number of key point overlaps after transformation to obtain the optimal pose transformation matrix of each candidate frame and the current frame, use this matrix to calculate the number of overlaps of all planar voxels of each candidate frame and the current frame for verification again, and take the candidate frame with the largest number of planar overlaps as the loop closure frame; Step 4: Perform ICP matching between loop closure frames, add the matching results to the pose map and perform global optimization, update the pose of all frames and the global map, and complete the fast loop closure detection of small-view LiDAR in unstructured environment.

2. The fast loop closure detection method for small-view lidar in unstructured environments according to claim 1, characterized in that: The specific steps of step 1 include: 1-1 Accumulate solid-state radar point clouds into frames and convert them to the world coordinate system; single-frame carrier coordinate system point cloud. To the world coordinate system After conversion, the data is accumulated into keyframe P. k The formula is: in, This represents the point cloud data in the carrier coordinate system of the i-th frame. R represents the point cloud data in the world coordinate system of the i-th frame. i T is the rotation matrix of the i-th frame. i It is the translation vector of the i-th frame. The pose matrix of a single frame is obtained by front-end pose matching of SLAM. The key frame P used for loop closure detection is obtained by accumulating the point clouds in the world coordinate system of k frames. k The point cloud pose matrix of the n / 2th frame is used as P. k The corresponding pose.

3. The method for fast loop closure detection using a small-view lidar in an unstructured environment according to claim 1, characterized in that, The specific steps of step 2 include: 2-1 Perform progressive morphological ground filtering on the current keyframe point cloud. The specific filtering process is as follows: The point cloud is divided into two-dimensional grids, and the minimum elevation point cloud is selected in each grid to construct the minimum surface grid. Then, a filter with morphological opening operation is applied to each grid to smooth the grid surface and output the point cloud after elevation difference threshold filtering. Then, the filter size and elevation difference threshold are increased, and the above operation is repeated for each grid. The iteration is until the predefined maximum value is reached to obtain the keyframe point cloud after ground filtering. 2-2 The intensity normalization correction is performed on the point cloud after ground filtering. The specific process is as follows: For the point cloud intensity value collected between 0 and 255, normalization processing is performed to correct it to the range of [0,1], where intensity data greater than 100 are uniformly set to 1. 2-3 generates an intensity scan context descriptor matrix for the current frame point cloud after processing in 2-1 and 2-2; Specifically, for a keyframe point cloud P obtained after the above steps... k ={P1,P2,P3...P i Let P be the spatial Cartesian coordinates and intensity information of one of the points. i ={x i ,y i ,z i ,η i Transform the point cloud from Cartesian coordinates to polar coordinates: Then P can be used in polar coordinates. i ={ρ i ,θ i ,z i ,η i } represents the point, and the point cloud in polar coordinate space is divided into two-dimensional circumferential and radial regions, dividing the space into N. s ×N r Block space: Among them, L max n represents the maximum detection range. s n represents the distance of the circumferential gap. r Let S represent the interval angle of each radial sector. Then, for each subspace S... ij The corresponding point cloud P in n Defined as: Therefore, keyframe P k The point cloud in the image is divided into corresponding N coordinates according to polar coordinates. s ×N r In a block subspace, the maximum intensity of the point cloud in each block subspace is taken as the value of that subspace, denoted as φ(S). ij ), thus obtaining N s ×N r 3D descriptor matrix I: φ(S ij )=maxη n ,P n ∈S ij Where, η n It is S ij Intensity information of neutron space point clouds, I is N s ×N r A 3D real matrix; 2-4 Calculate the similarity between the current frame and historical frames to obtain candidate keyframes. The specific process for obtaining candidate frames is as follows: For a frame intensity descriptor matrix I, use the row vectors (i.e., the circumferential vectors) in the matrix to perform dimensionality reduction. Therefore, for a certain frame descriptor matrix I, use the following N... r The dimension K vector represents: Where, r i Let r represent the cyclic vector in the i-th row. To improve search speed, the cyclic vector r will be defined. i Occupancy ratio ψ: Where, ||r i ||0 is r i The L0 norm, i.e., r i The number of non-zero values ​​in ψ(r) i The size of ) is independent of the radar viewpoint, and therefore has rotation invariance. The N of each frame descriptor I r The dimension vector K will be used to construct a kd-tree, and the nearest neighbor search will be used to select the corresponding keyframes that are similar to the K vector of the current keyframe as candidate keyframes for loop closure.

4. The method for fast loop closure detection using a small-view lidar in an unstructured environment according to claim 1, characterized in that, The specific steps of step 3 include: 3-1 The process of planar voxelization and planar keypoint extraction for the current frame is as follows: For the loop closure candidate keyframes obtained in step 2-4, find the original point cloud of the corresponding frame and divide it into voxel meshes. Then, extract the point cloud p from the voxel mesh... i Calculate its covariance matrix: To calculate the eigenvalues ​​of the matrix Σ, planar voxels must satisfy the conditions λ3<σ1, λ2>σ2, where λ2 and λ3 are the second and third largest eigenvalues ​​of the matrix, respectively, and σ1 and σ2 are preset parameter thresholds. Voxel meshes that satisfy these conditions are extracted as planar voxel meshes. Planar voxel meshes with the same planar normal direction and a distance below the threshold are joined with adjacent meshes to form the same plane. Planar voxel meshes that are not in the same direction during the joining process are classified as boundary planes. Therefore, a frame of planar voxelized point cloud will be divided into two types of planes. Voxels belonging to the boundary plane are projected onto the other plane. The distance of the projection point is taken as the distance of the point formed by the maximum distance among the boundary voxels. These projection points are divided into equal regions, and the point with the maximum distance in each region is retained as the planar key point. The calculation process for obtaining the overlap between the planar keypoints of each candidate frame and the current frame in 3-2 is as follows: For all planar keypoints within a frame obtained in 3-1, a kd-tree structure is constructed and the nearest points are found to form a triangle structure. Since the position of the corresponding vertex of the triangle structure is also determined after the side length is determined, the corresponding points in the current frame and the candidate frames are found using this correspondence. For each pair of triangle structures in the two frames, the following SVD decomposition is performed: [U,S,V]=SVD(H) in, and These are the coordinates of the three vertices of the corresponding triangle in the current frame and the candidate frame, respectively. and These are the center coordinates of the corresponding triangles in the current frame and the candidate frame, respectively. The coordinate difference is calculated to obtain the covariance matrix H. After SVD decomposition, the orthogonal matrix UV and the singular value matrix S are obtained. The decomposition results are processed as follows: T=(R,t),T∈SE(3) That is, to obtain a pose transformation matrix T between two frames, select the T that maximizes the number of overlapping planar keypoints between the two frames after the transformation. max The matrix serves as the optimal transformation matrix between two frames; 3-3 The process of performing planar voxel ICP matching between the current frame and the loopback frame and verifying the planar overlap between the two frames is as follows: Let the planar voxel mesh be the set of its center points g and normal vector u. Then, the point cloud P after a frame of planar voxelization is... π Represented as: Pπ=[(g1,u1),...(g i ,u i )] Where i represents the number of planar voxels in the current frame, using the current frame P obtained in 3-2 π K With candidate frame P π C The optimal transformation matrix T max For a candidate frame P π C (g) j C ,u j C Perform pose transformation and find the position relative to the current frame P. π K (g) i K ,u i K The nearest planar voxel (g,u) that satisfies the following conditions: ||Ru i K -u j C ||2<σ n Where, σ n and σ d It is the overlap threshold of the preset value, denoted as Let q be the number of planar voxels that meet the conditions in the i-th frame, and take the value N. i C The largest candidate frame is the final selected loopback frame.

5. The method for fast loop closure detection using a small-view lidar in an unstructured environment according to claim 4, characterized in that, The specific steps of step 4 include: The optimal pose matrix obtained by SVD decomposition in step 3 is used as the initial value and substituted into the inter-loop frame ICP matching process to accelerate the solution of the pose transformation matrix between the original point cloud of the current frame and the original point cloud of the loop frame. The transformation matrix is ​​added to the pose map, and the ISAM solver in the GTSAM optimization library is used to perform global optimization on the pose map, update the pose of each frame and update the global map.

Images