Loopback detection method based on stable static point cloud cluster word bag

Abstract

The invention provides a loopback detection method based on a stable static point cloud cluster word bag. The loopback detection method is suitable for dynamic, degenerated and large-scale disordered scenes. The method comprises the following steps of: 1, analyzing a point cloud degradation degree and screening a stable static point cloud cluster; 2, selecting a key frame by adopting a fuzzy comprehensive evaluation algorithm; 3, constructing a local descriptor bag-of-word model based on the point cloud cluster; and 4, realizing efficient loopback detection by using the key frame and the bag-of-word model. The method is high in detection accuracy, low in false detection rate and short in single-frame processing time, and the robustness and efficiency of point cloud loopback detection are remarkably improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of Light Detection and Ranging Simultaneous Localization and Mapping (LiDAR SLAM), and in particular relates to a loop detection method based on stable static point cloud cluster bag-of-words. Background Art

[0002] Driven by the demand for integrated high-precision positioning and high-quality data collection for both indoor and outdoor use, the types of sensors and data processing algorithms used in simultaneous localization and mapping (SLAM) are constantly evolving. This technology has become a core technology in the field of autonomous navigation and positioning. LiDAR-based SLAM (Light Detection and Ranging SLAM) is widely used in a variety of fields, including autonomous driving, logistics, and environmental monitoring, thanks to its intuitive mapping capabilities and its independence from lighting conditions. However, despite its impressive performance in these applications, LiDAR SLAM still faces a key challenge common to relative positioning technologies: the accumulation of errors in positioning results over time, which can lead to positioning errors and significantly degrade mapping quality. To mitigate this issue, loop closure detection (LCD) has been added as a key component to the LiDAR SLAM framework. LCD detects loop frames and uses them for global optimization to effectively correct these errors.

[0003] Currently, LCD methods used in LiDAR SLAM can be divided into five categories: Euclidean distance screening, global descriptor matching, local descriptor matching, laser intensity-assisted, and multi-sensor fusion. Shan et al. proposed a lightweight and ground-optimized Lidar Odometry and Mapping on Variable Terrain (LeGO-LOAM) algorithm and, based on this, a smoothing and mapping-based LiDAR inertial odometry algorithm. All of these algorithms employ Euclidean distance screening for LCD, using inter-frame distance as a loop closure reference. This approach is highly efficient but prone to missed detections and false detections. In response to the global descriptor matching method, Kim et al. proposed the Scan Context method, which constructs a global descriptor based on the maximum height of the two-dimensional mapped point cloud to encode the environmental structure to assist LCD. It has strong rotation sensitivity and low resource consumption, but is sensitive to noise and translation changes and is not suitable for dynamic, degraded and cluttered scenes. Kim et al. proposed the Scan Context++ method based on Scan Context. Through semi-metric positioning and lightweight design, it takes into account rotation and translation sensitivity and high efficiency, but it still has shortcomings in dealing with dynamic, degraded scenes and changes in point cloud density. Wang et al. proposed the LiDAR Iris method, which converts point clouds into Iris images and uses LoG-Gabor filtering and Fourier transform to process image features, effectively enhancing rotation and translation sensitivity, but is also affected by dynamic, degraded scenes and changes in point cloud density. In response to local descriptor matching methods, Cui et al. proposed the BoW3D method, which constructs a bag-of-words model based on the LinK3D features of point clouds for LCD. This method enhances robustness to rotational changes, translational changes, and reverse loop closure. However, dynamic objects and sparse features significantly interfere with it, and single-point local descriptors are not suitable for large-scale scenes. Ji et al. proposed the LLOAM algorithm, which uses random forests to match 21 point cloud cluster features and utilizes graph optimization information to improve loop closure accuracy. It can handle large-scale point clouds, but is only suitable for frame-to-subimage LCD. In response to laser intensity-assisted methods, Wang et al. proposed the ISC (Intensity Scan Context) method based on Scan Context. It constructs an ISC global descriptor based on geometric and intensity information and accelerates matching through binary encoding. It is suitable for degraded scenes and sparse point clouds, but is not translation-sensitive and is significantly affected by dynamic objects. Pan et al. further improved the matching speed of the ISC descriptor using front-end registration results, but did not address issues related to translation sensitivity and dynamic objects.Regarding multi-sensor fusion methods, Nguyen et al. proposed the VIRAL SLAM algorithm, which constitutes a three-stage LCD based on visual feature matching and point cloud inter-frame registration, enhancing the robustness of the system in degraded scenes, but is significantly affected by dynamic targets; Lin et al. proposed the R2LIVE algorithm, which is based on LiDAR, inertial measurement unit (IMU), and tight visual coupling and uses error state iterative Kalman filtering and factor graph optimization to achieve high-precision LCD, ensuring global consistency and being suitable for dynamic and degraded scenes, but the equipment cost and computational overhead are relatively high, and the real-time performance of processing large-scale data sets is poor.

[0004] In order to solve the problems that the current LCD method based only on point cloud processing generally has poor adaptability to dynamic, degraded, large-scale cluttered scenes, weak translation sensitivity and low processing efficiency, this patent proposes an LCD method based on stable static point cloud cluster bag of words, which mainly includes four parts: degraded frame evaluation, point cloud cluster classification, key frame screening and loop detection. In the degraded frame evaluation part, this patent extracts four features based on the structural characteristics of the preprocessed point cloud, and uses the point cloud degradation detection algorithm proposed in previous studies to evaluate its degradation; for the point cloud cluster classification part, this patent proposes a point cloud cluster classification algorithm; when the point clouds of adjacent frames are all degraded, the point cloud clusters are re-segmented based on the corrected laser intensity and the iterative closest point (ICP) algorithm is used to coarsely align the center point sets of the adjacent frame point cloud clusters; otherwise, the ICP algorithm is directly used to coarsely align the center point sets of the adjacent frame point cloud clusters; based on the alignment results, the missing point cloud clusters are marked through bidirectional nearest neighbor search, and the geometric differences between the nearest point cloud clusters of adjacent frames are calculated to mark the unstable point cloud clusters; for all stable point cloud clusters of adjacent frames, the four-parameter coordinate transformation model is used to accurately align the center point sets of the point cloud clusters, and the dynamic point cloud clusters are marked according to the clustering results of the alignment error. In the key frame screening part, in order to reduce the redundancy of loop information, this patent proposes a key frame screening algorithm based on fuzzy comprehensive evaluation; based on the structural features of the current frame and the benchmark key frame, the fuzzy comprehensive evaluation algorithm is used to calculate the similarity between frames to construct a key frame sequence. In the loop detection part, this patent proposes an LCD algorithm based on the bag of words of local descriptors of point cloud clusters. For each stable static point cloud cluster in the non-degenerate key frame, the distance between the point cloud clusters in the neighborhood is calculated to construct a descriptor; for each stable static point cloud cluster in the degraded key frame, the descriptor is constructed based on the distance and the laser intensity; based on this, a key frame descriptor list and a loop database are established; according to the storage and retrieval method of the bag of words model, similarity retrieval is performed on each element of each descriptor in the current frame descriptor list in the loop database, and the final LCD result is determined through multi-layer verification. Summary of the Invention

[0005] In view of the fact that the current LCD methods based only on point cloud processing generally have poor adaptability to dynamic, degraded, and large-scale cluttered scenes, and have weak translation sensitivity and low processing efficiency, the present invention proposes an LCD method based on stable static point cloud cluster bag of words, which effectively improves the robustness and universal applicability of the LCD method based only on point cloud processing.

[0006] An LCD method based on bag-of-words of stable static point cloud clusters includes the following steps:

[0007] Step 1: Unsupervised point cloud optimization and the velocity updating-iterative closest point (V-ICP) method are used to calibrate the intrinsic parameters of the point cloud and perform dedistortion processing. Voxel filtering and point cloud clustering are combined to obtain independent point cloud clusters. The degree of degradation of the point cloud frame is accurately assessed based on a fuzzy comprehensive evaluation algorithm based on multi-feature fusion.

[0008] Step 2: A point cloud cluster classification algorithm is proposed. Based on the correction of the laser intensity of the degraded frame and the intensity connectivity segmentation, it combines the coarse registration of adjacent frames, the similarity analysis of structural descriptors, and the density clustering method to comprehensively identify and classify the missing, unstable, dynamic, and stable static point cloud clusters in each frame of the point cloud.

[0009] Step 3. A key frame screening algorithm based on fuzzy comprehensive evaluation is proposed. Based on multi-feature fuzzy comprehensive evaluation, the number of stable static point cloud clusters is introduced to replace dynamic interference. Distance and structural features are integrated, and the hierarchical analysis method is used to determine the weights. The similarity between frames is calculated to achieve efficient screening and updating of key frames.

[0010] Step 4: Construct a local descriptor for each stable static point cloud cluster, establish a bag-of-words database for loop closure retrieval, and effectively verify the retrieval results.

[0011] In step 1, the point cloud internal parameter calibration and dedistortion processing are performed through unsupervised point cloud optimization and V-ICP method, and independent point cloud clusters are obtained by combining voxel filtering and point cloud clustering. The degree of degradation of the point cloud frame is accurately assessed based on the fuzzy comprehensive evaluation algorithm based on multi-feature fusion;

[0012] The specific steps are as follows:

[0013] Step 1-1: Use unsupervised point cloud optimization methods to calibrate the point cloud internal parameters to compensate for equipment system errors; and use the V-ICP method to remove point cloud motion distortion. Since the main research object is independent point cloud clusters, a three-dimensional voxel filtering method is used to remove discrete points and noise points, and the point cloud is downsampled based on the center of each voxel. For the point cloud after internal parameter calibration and preprocessing, ground segmentation and point cloud clustering methods are used for further processing to obtain independent point cloud cluster clustering results;

[0014] Step 1-2: For each independent point cloud cluster, the degradation of each frame point cloud is evaluated using the LiDAR degradation environment detection method based on multi-feature fusion. Among them, four features describing the geometric structure of a single frame point cloud are extracted, including: the number of line feature points N l , number of independent planes N p , the number of independent point cloud clusters N c , the degree of asymmetry P. The larger the above feature value is, the lower the possibility that the current frame is a degraded frame. Therefore, this method adopts the fuzzy comprehensive evaluation algorithm to comprehensively consider the four features, so as to accurately and efficiently evaluate the point cloud degradation.

[0015] The proposed point cloud cluster classification algorithm described in step 2, based on the correction of the laser intensity of the degraded frame and the intensity connectivity segmentation, combines the coarse registration of adjacent frames, the similarity analysis of the structural descriptors and the density clustering method to comprehensively identify and classify the missing, unstable, dynamic and stable static point cloud clusters in each frame of the point cloud;

[0016] The specific steps are as follows:

[0017] Step 2-1: Use the laser intensity correction algorithm to correct the laser intensity of the degraded frame, and re-segment the point cloud clusters based on intensity connectivity. At the same time, update the center point set of the point cloud cluster. Due to the high frequency of LiDAR acquisition, it can be assumed that there is no sudden change in the corresponding environmental structure of the point clouds of adjacent frames. Therefore, the above process is only performed when the point clouds of adjacent frames are all degraded frames. Based on this, the point cloud clusters are classified;

[0018] Step 2-2: The point cloud cluster center point set based on the adjacent frame point cloud is recorded as E A and E B , the ICP algorithm is used for coarse registration, and the rough relative pose is used to unify the coordinate reference. B For each point in E, set the appropriate neighborhood radius. A Perform the nearest neighbor search in , mark the point cloud cluster without the search object as the missing point cloud cluster, the same as above, for E A There are no marked or searched points in E B Reverse search and mark missing point cloud clusters;

[0019] Step 2-3: Due to the straight-line propagation property of lasers, point cloud clusters scanned by the same target in adjacent frames may exhibit significant structural differences due to occlusion or changes in scanning angles. This paper considers such point cloud clusters as unstable point cloud clusters. To mark such point cloud clusters, we first construct structural descriptors μ based on the three-axis span of the point cloud cluster, as follows:

[0020]

[0021] in, Represent the spans of the X, Y, and Z axes respectively. Based on the combination of nearest paired point cloud clusters constructed by bidirectional nearest neighbor search, the cosine similarity S between the corresponding μ is calculated as follows:

[0022]

[0023] in, Indicates E B The descriptor of the k-th point cloud cluster in μ A Indicates E A The descriptor of the paired point cloud cluster in , j represents the element number in the descriptor. If S is less than 0.5, the current nearest neighbor paired point cloud cluster combination is marked as an unstable point cloud cluster;

[0024] Step 2-4: By marking the missing point cloud clusters and unstable point cloud clusters, the current center point set of each point cloud cluster is only composed of stable static point cloud clusters and dynamic point cloud clusters. To mark the dynamic point cloud clusters, the E A and E B Project it onto the XY plane, randomly select two pairs of nearest neighboring paired point cloud clusters as local points, use the four-parameter coordinate transformation model to calculate the transformation matrix T, and transform E B The remaining points in the equation are converted to E A In the coordinate system, the two-dimensional Euclidean distance between the current nearest paired point cloud clusters is calculated, and all distances are clustered using a density-based spatial clustering algorithm with noise. If the clustering result is 1 category, they are all regarded as static point cloud clusters and the number is counted; if the clustering result is 2 categories, the category with the smaller average distance is regarded as a static point cloud cluster and the number is counted; if the clustering result is more than 2 categories, the categories are merged until only 2 categories remain, and the number of point cloud clusters in the category with the smaller average distance is also counted. The above process of selecting in-region points and counting the number of static point cloud clusters is continuously iterated until all in-region point combinations are processed and then stopped. The T corresponding to the largest number of static point cloud clusters is taken as E A and E B The transformation matrix of the precise registration is calculated and the corresponding dynamic point cloud clusters are marked. So far, each frame of point cloud contains 4 types of point cloud clusters, namely missing point cloud clusters, unstable point cloud clusters, dynamic point cloud clusters and stable static point cloud clusters.

[0025] The key frame screening algorithm based on fuzzy comprehensive evaluation mentioned in step 3 is based on multi-feature fuzzy comprehensive evaluation. By introducing the number of stable static point cloud clusters to replace dynamic interference, integrating distance and structural features, using hierarchical analysis method to determine weights, and calculating the similarity between frames, efficient screening and updating of key frames are achieved;

[0026] The specific steps are as follows:

[0027] Step 3-1: Using the N extracted in the degraded frame evaluation process l 、N p、N c , P, a total of 4 features, the fuzzy comprehensive evaluation algorithm is used to calculate the similarity between the current frame and the benchmark key frame, as follows:

[0028]

[0029] Among them, U represents the factor set, whose elements correspond to the four characteristics in turn, V represents the evaluation set, whose elements correspond to the three evaluation results of dissimilar, undetermined, and similar, R represents the fuzzy evaluation matrix, and μ(u,v) represents the membership function;

[0030] Step 3-2: Based on the characteristics, the semi-trapezoidal membership calculation method is used to construct R, with N l For example, as follows:

[0031]

[0032] in, Indicates the N corresponding to the A frame l , Indicates the N corresponding to the B frame l , min means taking the minimum value operation, max means taking the maximum value operation, and the membership calculation models corresponding to other evaluation factors all adopt the form of formula (4);

[0033] Step 3-3: To obtain comprehensive evaluation results, the analytic hierarchy process is used to construct the evaluation weight matrix D. First, the judgment matrix C is determined as follows:

[0034]

[0035] The weight d of each evaluation factor is calculated using the square root method based on the elements in C, as follows:

[0036]

[0037] in, represents the weight before normalization;

[0038] Based on the calculation results of formula (6), D is constructed as follows:

[0039] D=[d1 d2 d3 d4]=[0.250.250.250.25] (7)

[0040] Apply D to R to calculate the fuzzy evaluation result set B as follows:

[0041]

[0042] Step 3-4: Compare the elements in B according to the maximum membership principle. If b1 is the largest, the similarity between the two frames of point clouds is low, and the current frame is added to the key frame sequence and used as the new reference key frame. If b3 is the largest, the two frames of point clouds are completely similar, and the current frame is skipped directly. If b2 is the largest, then when b2 is less than 0.5, the current frame is added to the key frame sequence and the reference key frame is updated.

[0043] Construct local descriptors for each stable static point cloud cluster as described in step 4, thereby establishing a bag-of-words database for loop closure retrieval and effectively verifying the retrieval results;

[0044] The specific steps are as follows:

[0045] Step 4-1: Based on the results of degraded frame evaluation, point cloud cluster classification, and keyframe screening, for non-degraded keyframe sequences and degraded keyframe sequences, respectively, a non-degraded keyframe descriptor list and a degraded keyframe descriptor list are constructed according to the data storage mode of the bag-of-words model;

[0046] Taking the non-degenerate key frame A as an example, for the center point set of the stable static point cloud cluster projected onto the two-dimensional plane, a 10m×10m rectangular neighborhood is constructed with each point as the center, and the distances from other points in the neighborhood to the center point are calculated and arranged from small to large to construct a descriptor. Taking the i-th point as an example, the descriptor as follows:

[0047]

[0048] in, Indicates the frame number to which the current point cloud cluster belongs. Indicates the number of the current point cloud cluster in the current frame, D represents the distance, and n represents the number of points contained in the current neighborhood;

[0049] Construct a descriptor list based on κ of all points in frame A as follows:

[0050]

[0051] Among them, m represents the number of stable static point cloud clusters contained in frame A;

[0052] Taking the degenerate keyframe B as an example, we also construct a neighborhood for each point, calculate the distance D and build a descriptor based on the laser intensity I of the corresponding point cloud cluster; taking the i-th point as an example, the descriptor as follows:

[0053]

[0054] Construct a descriptor list based on κ of all points in the B frame as follows:

[0055]

[0056] Where m represents the number of stable static point cloud clusters contained in the B frame;

[0057] Taking the degenerate keyframe B as an example, we also construct a neighborhood for each point, calculate the distance D and build a descriptor based on the laser intensity I of the corresponding point cloud cluster; taking the i-th point as an example, the descriptor as follows:

[0058]

[0059] Construct a descriptor list based on κ of all points in the B frame as follows:

[0060]

[0061] Where m represents the number of stable static point cloud clusters contained in the B frame;

[0062] Step 4-2: For each non-degenerate keyframe, use equations (9) and (10) to construct a descriptor list; for each degenerate keyframe, use equations (11) and (12) to construct a descriptor list; based on this, update the non-degenerate loop database F and the degenerate loop database F′ in real time as follows:

[0063]

[0064] Step 4-3: Based on the retrieval mode of the bag-of-words model, perform loop retrieval in the corresponding type database for the descriptor list of the latest key frame; Frame as an example, The descriptor of the i-th point cloud cluster in is as follows:

[0065]

[0066] against The first distance element in Expand the search in F, record and The corresponding IDs of elements with a difference of less than 0.1m f and ID c Same as above, record The search results of all distance elements in ; if the search result is empty, it is considered that there is no loop object in the current point cloud cluster; otherwise, the ID is counted f Key frames with a frequency greater than 2n / 3 are sorted from highest to lowest in order to construct a candidate key frame sequence W;

[0067] If W is empty, it is considered that the current point cloud cluster has no loop object. Otherwise, the first candidate key frame is selected from W and the corresponding ID is counted. cThe stable static point cloud clusters with a frequency greater than 2n / 3 are sorted from large to small according to the frequency to construct the candidate point cloud cluster sequence P. If P is empty, the candidate key frames in W are replaced in sequence to construct P until P is not empty or W is empty. Finally, the first element that meets the conditions in W and P is recorded as the optimal loop object of the i-th point cloud cluster. In addition, if the corresponding P of all candidate key frames in W are empty, it is considered that the current point cloud cluster has no loop object. As above, record The optimal loop object of the remaining point cloud clusters and the ID f The key frame with a frequency greater than 2m / 3 is regarded as the final loop frame. If all key frames do not meet the loop condition, they are regarded as There is no loopback frame.

[0068] Step 4-4: For the latest degraded key frame The process of determining the final loop frame is the same as the above determination The final loopback process of the frame differs only in the description sub-element retrieval part. The descriptor of the i-th point cloud cluster in is as follows:

[0069]

[0070] against The first set of distance and intensity elements in and Expand the search in F′ and record The difference is less than 0.1m and The corresponding ID of the exact same element f and ID c , same as above, record All corresponding retrieval results are then subjected to subsequent loopback verification.

[0071] Beneficial effects of the present invention:

[0072] 1. To mitigate the impact of dynamic targets on LCDs, the present invention proposes a point cloud cluster classification algorithm to obtain stable static point cloud clusters and construct point cloud cluster local descriptors. To enhance the differences between degraded or similar environments and thereby improve the robustness of LCDs, the present invention uses a point cloud degradation detection algorithm to evaluate degraded frames and uses laser intensity to assist in constructing point cloud cluster local descriptors for degraded frames.

[0073] 2. In order to reduce loop information redundancy and prevent the loss of effective information, the present invention proposes a key frame screening algorithm based on fuzzy comprehensive evaluation, which takes the stable static point cloud cluster in the key frame as the descriptor construction object, effectively taking into account the accuracy and efficiency of LCD.

[0074] 3. To be applicable to large-scale, cluttered scenes, this invention uses a bag-of-words model and multi-layer validation to ensure LCD performance, while leveraging hash tables and multi-threaded processing to ensure LCD speed. Experimental results demonstrate that the present invention offers significant advantages over comparable methods in terms of both LCD accuracy and efficiency. The present invention's LCD results effectively assist in optimizing point cloud inter-frame registration errors, thereby improving LiDAR SLAM positioning accuracy and reducing the RMSE by 90.71%.

[0075] 4. The present invention takes stable static point cloud clusters as the research object, adopts point cloud degradation detection and point cloud cluster classification algorithms, accurately extracts stable static point cloud clusters that are not affected by dynamic interference in the environment, further constructs local descriptors, and realizes efficient loop retrieval based on the bag-of-words model. Loop candidate relationships are established by matching local descriptors of stable static point cloud clusters between frames, and the accuracy of loop detection is improved by combining a multi-layer verification mechanism. Compared with the existing loop detection methods for dynamic, degraded and cluttered scenes, the alignment objects selected by the method of the present invention are highly stable, the description information structure is clear, and the alignment process does not rely on global features or external sensor information, which significantly enhances the robustness and adaptability of loop detection in complex environments, thereby obtaining more stable, efficient and accurate loop detection results. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] Figure 1 This is a flow chart of a loop closure detection method based on stable static point cloud cluster bag of words in the present invention;

[0077] Figure 2 This is a specific flow chart of step 1 of one embodiment of the present invention;

[0078] Figure 3 This is a specific flow chart of step 2 of one embodiment of the present invention;

[0079] Figure 4 This is a specific flow chart of step 3 of one embodiment of the present invention;

[0080] Figure 5 This is a specific flow chart of step 4 of one embodiment of the present invention;

[0081] Figure 6 A flowchart illustrating an embodiment of the present invention;

[0082] Figure 7 It is the experimental platform and experimental scenario of the present invention;

[0083] Figure 8 The plane structure of the experimental scene and the carrier motion trajectory diagram of the present invention;

[0084] Figure 9 The LCD effect diagrams of the method of the present invention and the other two methods in three scenarios;

[0085] Figure 10 This is a positioning trajectory diagram of the method of the present invention and the LeGO-LOAM algorithm with the LCD module turned off in scenario 3;

[0086] Figure 11 CDF diagram of positioning error between the method of the present invention and the LeGO-LOAM algorithm with the LCD module turned off in scenario 3; DETAILED DESCRIPTION

[0087] An embodiment of the present invention will be further described below with reference to the accompanying drawings.

[0088] In the embodiment of the present invention, the loop detection method based on the bag-of-words of stable static point cloud clusters is as follows: Figure 1 As shown, the following steps are included:

[0089] Step 1: Use unsupervised point cloud optimization and V-ICP method to calibrate the point cloud internal parameters and perform dedistortion processing. Combine voxel filtering and point cloud clustering to obtain independent point cloud clusters. Then, use the fuzzy comprehensive evaluation algorithm based on multi-feature fusion to accurately assess the degree of degradation of the point cloud frame.

[0090] Step 1-1: Use unsupervised point cloud optimization methods to calibrate the point cloud internal parameters to compensate for equipment system errors; and use the V-ICP method to remove point cloud motion distortion. Since the main research object is independent point cloud clusters, a three-dimensional voxel filtering method is used to remove discrete points and noise points, and the point cloud is downsampled based on the center of each voxel. For the point cloud after internal parameter calibration and preprocessing, ground segmentation and point cloud clustering methods are used for further processing to obtain independent point cloud cluster clustering results;

[0091] Step 1-2: For each independent point cloud cluster, the degradation of each frame point cloud is evaluated using the LiDAR degradation environment detection method based on multi-feature fusion. Among them, four features describing the geometric structure of a single frame point cloud are extracted, including: the number of line feature points N l , number of independent planes N p , the number of independent point cloud clusters N c , the degree of asymmetry P. The larger the above feature value is, the lower the possibility that the current frame is a degraded frame. Therefore, this method adopts the fuzzy comprehensive evaluation algorithm to comprehensively consider the four features, so as to accurately and efficiently evaluate the point cloud degradation.

[0092] Step 2: A point cloud cluster classification algorithm is proposed. Based on the correction of the laser intensity of the degraded frame and the intensity connectivity segmentation, it combines the coarse registration of adjacent frames, the similarity analysis of structural descriptors, and the density clustering method to comprehensively identify and classify the missing, unstable, dynamic, and stable static point cloud clusters in each frame of the point cloud.

[0093] Step 2-1: Use the laser intensity correction algorithm to correct the laser intensity of the degraded frame, and re-segment the point cloud clusters based on intensity connectivity. At the same time, update the center point set of the point cloud cluster. Due to the high frequency of LiDAR acquisition, it can be assumed that there is no sudden change in the corresponding environmental structure of the point clouds of adjacent frames. Therefore, the above process is only performed when the point clouds of adjacent frames are all degraded frames. Based on this, the point cloud clusters are classified;

[0094] Step 2-2: The point cloud cluster center point set based on the adjacent frame point cloud is recorded as E A and E B , the ICP algorithm is used for coarse registration, and the rough relative pose is used to unify the coordinate reference. B For each point in E, set the appropriate neighborhood radius. A Perform the nearest neighbor search in , mark the point cloud cluster without the search object as the missing point cloud cluster, the same as above, for E A There are no marked or searched points in E B Reverse search and mark missing point cloud clusters;

[0095] Step 2-3: Due to the straight-line propagation property of lasers, point cloud clusters scanned by the same target in adjacent frames may exhibit significant structural differences due to occlusion or changes in scanning angles. This paper considers such point cloud clusters as unstable point cloud clusters. To mark such point cloud clusters, we first construct structural descriptors μ based on the three-axis span of the point cloud cluster, as follows:

[0096]

[0097] in, Represent the spans of the X, Y, and Z axes respectively. Based on the combination of nearest paired point cloud clusters constructed by bidirectional nearest neighbor search, the cosine similarity S between the corresponding μ is calculated as follows:

[0098]

[0099] in, Indicates E B The descriptor of the k-th point cloud cluster in μ A Indicates E A The descriptor of the paired point cloud cluster in , j represents the element number in the descriptor. If S is less than 0.5, the current nearest neighbor paired point cloud cluster combination is marked as an unstable point cloud cluster;

[0100] Step 2-4: By marking the missing point cloud clusters and unstable point cloud clusters, the current center point set of each point cloud cluster is only composed of stable static point cloud clusters and dynamic point cloud clusters. To mark the dynamic point cloud clusters, the E A and E BProject it onto the XY plane, randomly select two pairs of nearest neighboring paired point cloud clusters as local points, use the four-parameter coordinate transformation model to calculate the transformation matrix T, and transform E B The remaining points in the equation are converted to E A In the coordinate system, the two-dimensional Euclidean distance between the current nearest paired point cloud clusters is calculated, and all distances are clustered using a density-based spatial clustering algorithm with noise. If the clustering result is 1 category, they are all regarded as static point cloud clusters and the number is counted; if the clustering result is 2 categories, the category with the smaller average distance is regarded as a static point cloud cluster and the number is counted; if the clustering result is more than 2 categories, the categories are merged until only 2 categories remain, and the number of point cloud clusters in the category with the smaller average distance is also counted. The above process of selecting in-region points and counting the number of static point cloud clusters is continuously iterated until all in-region point combinations are processed and then stopped. The T corresponding to the largest number of static point cloud clusters is taken as E A and E B The transformation matrix of the precise registration is calculated and the corresponding dynamic point cloud clusters are marked. So far, each frame of point cloud contains 4 types of point cloud clusters, namely missing point cloud clusters, unstable point cloud clusters, dynamic point cloud clusters and stable static point cloud clusters.

[0101] Step 3. A key frame screening algorithm based on fuzzy comprehensive evaluation is proposed. Based on multi-feature fuzzy comprehensive evaluation, the number of stable static point cloud clusters is introduced to replace dynamic interference. Distance and structural features are integrated, and the hierarchical analysis method is used to determine the weights. The similarity between frames is calculated to achieve efficient screening and updating of key frames.

[0102] Step 3-1: Using the N extracted in the degraded frame evaluation process l 、N p 、N c , P, a total of 4 features, the fuzzy comprehensive evaluation algorithm is used to calculate the similarity between the current frame and the benchmark key frame, as follows:

[0103]

[0104] Among them, U represents the factor set, whose elements correspond to the four characteristics in turn, V represents the evaluation set, whose elements correspond to the three evaluation results of dissimilar, undetermined, and similar, R represents the fuzzy evaluation matrix, and μ(u,v) represents the membership function;

[0105] Step 3-2: Based on the characteristics, the semi-trapezoidal membership calculation method is used to construct R, with N l For example, as follows:

[0106]

[0107] in, Indicates the N corresponding to the A frame l , Indicates the N corresponding to the B frame l , min means taking the minimum value operation, max means taking the maximum value operation, and the membership calculation models corresponding to other evaluation factors all adopt the form of formula (4);

[0108] Step 3-3: To obtain comprehensive evaluation results, the analytic hierarchy process is used to construct the evaluation weight matrix D. First, the judgment matrix C is determined as follows:

[0109]

[0110] The weight d of each evaluation factor is calculated using the square root method based on the elements in C, as follows:

[0111]

[0112] in, represents the weight before normalization;

[0113] Based on the calculation results of formula (6), D is constructed as follows:

[0114] D=[d1 d2 d3 d4]=[0.250.250.250.25] (7)

[0115] Apply D to R to calculate the fuzzy evaluation result set B as follows:

[0116]

[0117] Step 3-4: Compare the elements in B according to the maximum membership principle. If b1 is the largest, the similarity between the two frames of point clouds is low, and the current frame is added to the key frame sequence and used as the new reference key frame. If b3 is the largest, the two frames of point clouds are completely similar, and the current frame is skipped directly. If b2 is the largest, then when b2 is less than 0.5, the current frame is added to the key frame sequence and the reference key frame is updated.

[0118] Step 4: Construct local descriptors for each stable static point cloud cluster, establish a bag-of-words database for loop closure retrieval, and effectively verify the retrieval results;

[0119] Step 4-1: Based on the results of degraded frame evaluation, point cloud cluster classification, and keyframe screening, for non-degraded keyframe sequences and degraded keyframe sequences, respectively, a non-degraded keyframe descriptor list and a degraded keyframe descriptor list are constructed according to the data storage mode of the bag-of-words model;

[0120] Taking the non-degenerate key frame A as an example, for the center point set of the stable static point cloud cluster projected onto the two-dimensional plane, a 10m×10m rectangular neighborhood is constructed with each point as the center, and the distances from other points in the neighborhood to the center point are calculated and arranged from small to large to construct a descriptor. Taking the i-th point as an example, the descriptor as follows:

[0121]

[0122] in, Indicates the frame number to which the current point cloud cluster belongs. Indicates the number of the current point cloud cluster in the current frame, D represents the distance, and n represents the number of points contained in the current neighborhood;

[0123] Construct a descriptor list based on κ of all points in frame A as follows:

[0124]

[0125] Among them, m represents the number of stable static point cloud clusters contained in frame A;

[0126] Taking the degenerate keyframe B as an example, we also construct a neighborhood for each point, calculate the distance D and build a descriptor based on the laser intensity I of the corresponding point cloud cluster. Taking the i-th point as an example, the descriptor as follows:

[0127]

[0128] Construct a descriptor list based on κ of all points in the B frame as follows:

[0129]

[0130] Where m represents the number of stable static point cloud clusters contained in the B frame;

[0131] Taking the degenerate keyframe B as an example, we also construct a neighborhood for each point, calculate the distance D and build a descriptor based on the laser intensity I of the corresponding point cloud cluster. Taking the i-th point as an example, the descriptor as follows:

[0132]

[0133] Construct a descriptor list based on κ of all points in the B frame as follows:

[0134]

[0135] Where m represents the number of stable static point cloud clusters contained in the B frame;

[0136] Step 4-2: For each non-degenerate keyframe, a descriptor list is constructed using equations (9) and (10). For each degenerate keyframe, a descriptor list is constructed using equations (11) and (12). Based on this, the non-degenerate loop database F and the degenerate loop database F′ are updated in real time as follows:

[0137]

[0138] Step 4-3: According to the retrieval mode of the bag-of-words model, a loop retrieval is performed in the corresponding type database for the descriptor list of the latest key frame, and the current latest non-degenerate key frame is used. Frame as an example, The descriptor of the i-th point cloud cluster in is as follows:

[0139]

[0140] against The first distance element in Expand the search in F, record and The corresponding IDs of elements with a difference of less than 0.1m f and ID c , same as above, record The search results of all distance elements in . If the search result is empty, it is considered that there is no loop object in the current point cloud cluster; otherwise, the ID is counted f The key frames whose occurrence frequency is greater than 2n / 3 are arranged in descending order of frequency to construct the candidate key frame sequence W.

[0141] If W is empty, it is considered that the current point cloud cluster has no loop object. Otherwise, select the first candidate key frame from W and count the corresponding ID c Find stable static point cloud clusters with a frequency greater than 2n / 3, and construct a candidate point cloud cluster sequence P according to the frequency from large to small. If P is empty, replace the candidate keyframes in W to construct P in sequence until P is not empty or W is empty. Finally, record the first element in W and P that meets the conditions as the optimal loop object of the i-th point cloud cluster. In addition, if the corresponding P of all candidate keyframes in W is empty, it is considered that the current point cloud cluster has no loop object. Same as above, record The optimal loop object of the remaining point cloud clusters and the ID f The key frame with a frequency greater than 2m / 3 is regarded as the final loop frame. If all key frames do not meet the loop condition, it is regarded as There is no loopback frame.

[0142] Step 4-4: For the latest degraded key frame The process of determining the final loop frame is the same as the above determination The process of the final loop frame differs only in the description sub-element retrieval part. The descriptor of the i-th point cloud cluster in is as follows:

[0143]

[0144] against The first set of distance and intensity elements in and Expand the search in F′ and record The difference is less than 0.1m and The corresponding ID of the exact same element f and ID c Same as above, record All corresponding retrieval results are then subjected to subsequent loopback verification.

[0145] like Figure 7 As shown in Figure 2, in order to verify the actual performance of the LCD method based on the stable static point cloud cluster bag of words of the present invention, an experimental platform is built based on LiDAR, total reflection prism and combined navigation module, as shown in Figure 2. Figure 7 As shown in (a), the LiDAR uses the RS-LiDAR-32 from RoboSense, with a point cloud acquisition frequency of 10Hz; the total station uses the Leica TS50, which automatically tracks the total reflection prism and collects data at a frequency of 10Hz to provide indoor reference trajectories; the integrated navigation module uses the X1 integrated navigation system from Beiyun Technology, with a global navigation satellite system observation frequency of 10Hz and an IMU acquisition frequency of 200Hz to provide outdoor reference trajectories. Experimental data were collected in three real indoor and outdoor scenes. Scene 1 is an underground parking lot. Figure 7 As shown in (b), load-bearing structures are regularly distributed and objects such as cars, fire hydrants, and trash cans are randomly distributed. There are also a few dynamic pedestrians and cars. Scene 2 is a dormitory corridor, such as Figure 7 As shown in (c), there are garbage cans, fire hydrants, debris piles and other objects distributed, there are a few dynamic pedestrians, and the environmental similarity between some corridor sections is high; Scene 3 is an outdoor street, such as Figure 7 As shown in (d), there are green belts, trees, billboards and other features, and there are many dynamic pedestrians and vehicles.

[0146] like Figure 8 As shown in the figure, the planar structure of the three scenes and the sketch of the carrier motion trajectory are respectively about 350m, 650m and 2800m in length and there are multiple intersections or overlaps in the round-trip trajectories. The data acquisition time is respectively about 180s, 350s and 1200s. In order to test the overall performance of the LCD method based on the stable static point cloud cluster bag of words of the present invention, the BoW3D method and the ISC method are used as comparative methods. Since the present invention adopts a round-trip mode to collect data of each scene, based on the trajectory coordinates corresponding to each key frame of the outbound trip, the point cloud with a distance of less than 5m and the closest point cloud is selected for the trajectory coordinates corresponding to each key frame of the return trip as the loop frame, so as to construct a reference benchmark for verifying the experimental results. Based on the reference benchmark and the processing results, the loop frame correct detection rate η is calculated using formula (16) respectively r and the non-loop frame error detection rate η e , and statistically process the time t required for a single key frame.

[0147]

[0148] like Figure 9 Figure 2 shows the LCD visualization results of the three scene data processed by the three methods mentioned above. Because the BoW3D method is less susceptible to external interference such as dynamic objects and sparse features and has strong sensitivity to rotation, translation, and inversion, it can effectively detect loops with large translation differences in scene 1 and maintain stable detection results in scene 3, which has many dynamic objects and scattered features. Because scene 2 contains many similar corridor sections, the single-point descriptor constructed by this method is prone to incorrect matching relationships, resulting in a high number of erroneous LCD results. Furthermore, single-point detection results in a low processing efficiency. The ISC method uses laser intensity to assist LCD, which can reduce the interference of similar scenes on detection results. Therefore, compared with the BoW3D method, it has fewer erroneous LCD results in scene 2. Because the global descriptor constructed by this method is not translation-sensitive and is significantly affected by dynamic objects, it has a high number of loop frame omissions in all three scenes. This method performs encoding matching on the global descriptor and additionally utilizes intensity information, resulting in lower efficiency than the BoW3D method. The method of the present invention extracts stable static targets and constructs local descriptors for LCD, eliminating the interference of dynamic targets from the root. Compared with single-point local descriptors, its construction efficiency and matching efficiency are significantly improved. In addition, the method of the present invention uses a modified laser intensity to assist LCD for scenes with sparse features or similar features, effectively balancing loop closure accuracy and processing efficiency, and thus obtains stable LCD results in all three scenes. Compared with the BoW3D method and the ISC method, η r They increased by 9.76% and 212.79% respectively, and η e They were reduced by 85.45% and 66.65%, and t was reduced by 13.87% and 33.92%, respectively, which effectively verified the LCD performance of the proposed method.

[0149] like Figure 10 As shown in FIG, two algorithms are used to process the scene 3 data respectively, and the positioning trajectory diagrams are obtained. The two algorithms are respectively the present invention replacing the corresponding LCD module in LeGO-LOAM, which is recorded as U-LeGO-LOAM (Update-LeGO-LOAM), and the LeGO-LOAM with the LCD module turned off is used as a comparison algorithm.

[0150] like Figure 11The figure shows the Cumulative Distribution Function (CDF) of the positioning error. In addition, the root mean square error (RMSE), maximum error (ME), and the time required to process a single frame of point cloud are calculated. Scene 3 has rich but scattered features and many dynamic targets, resulting in large and gradually accumulated inter-frame registration errors. Without adding loop closure constraints, relying solely on backend optimization cannot effectively correct the above errors. Intuitive comparison Figure 10 and Figure 11 , it is concluded that compared with the reference trajectory, the positioning trajectory of LeGO-LOAM with the LCD module turned off gradually diverges, but the positioning trajectory of U-LeGO-LOAM is better overall. The positioning error of the former is mainly distributed between 2m and 20m, while over 60% of the positioning errors of U-LeGO-LOAM are less than 1m. Quantitative comparison of the indicators in Table 3 shows that compared with LeGO-LOAM with the LCD module turned off, the positioning RMSE of U-LeGO-LOAM is reduced by 90.71%, the positioning ME is reduced by 94.75%, and the efficiency is reduced by 27.96%. The ratio of the reduction in positioning RMSE to the reduction in efficiency is 3.24, indicating that while meeting the requirements of real-time processing, some computational efficiency is sacrificed but a significant improvement in positioning accuracy is achieved, successfully verifying the research value and effectiveness of the method of the present invention.

[0151] The above is merely a basic embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any substitutions that can be understood by persons skilled in the art within the technical scope disclosed by the present invention are intended to be encompassed by the scope of protection of the present invention. Therefore, the scope of protection of the present invention shall be subject to the scope of protection of the claims.

Claims

1. A loop closure detection method based on stable static point cloud cluster bag of words, characterized by: The following steps are involved: Step 1: Unsupervised point cloud optimization and the V-ICP (Velocity Updating-Iterative Closest Point) method are used to calibrate and dedistort the point cloud internal parameters. Independent point cloud clusters are obtained by combining voxel filtering and point cloud clustering. A fuzzy comprehensive evaluation algorithm based on multi-feature fusion is then used to accurately assess the degree of degradation of the point cloud frame. Step 2: A point cloud cluster classification algorithm is proposed. Based on the correction of the laser intensity of the degraded frame and the intensity connectivity segmentation, it combines the coarse registration of adjacent frames, the similarity analysis of structural descriptors, and the density clustering method to comprehensively identify and classify the missing, unstable, dynamic, and stable static point cloud clusters in each frame of the point cloud. Step 3. A key frame screening algorithm based on fuzzy comprehensive evaluation is proposed. Based on multi-feature fuzzy comprehensive evaluation, the number of stable static point cloud clusters is introduced to replace dynamic interference. Distance and structural features are integrated, and the hierarchical analysis method is used to determine the weights. The similarity between frames is calculated to achieve efficient screening and updating of key frames. Step 4: Construct a local descriptor for each stable static point cloud cluster, establish a bag-of-words database for loop closure retrieval, and effectively verify the retrieval results.

2. The loop closure detection method based on stable static point cloud cluster bag of words according to claim 1 is characterized in that: In step 1, the point cloud internal parameter calibration and dedistortion processing are performed through unsupervised point cloud optimization and V-ICP method, and independent point cloud clusters are obtained by combining voxel filtering and point cloud clustering. The degree of degradation of the point cloud frame is accurately assessed based on the fuzzy comprehensive evaluation algorithm based on multi-feature fusion; The specific steps are as follows: Step 1-1: Use unsupervised point cloud optimization methods to calibrate the point cloud internal parameters to compensate for equipment system errors; and use the V-ICP method to remove point cloud motion distortion. Since the main research object is independent point cloud clusters, a three-dimensional voxel filtering method is used to remove discrete points and noise points, and the point cloud is downsampled based on the center of each voxel. For the point cloud after internal parameter calibration and preprocessing, ground segmentation and point cloud clustering methods are used for further processing to obtain independent point cloud cluster clustering results; Step 1-2: For each independent point cloud cluster, the degradation of each frame point cloud is evaluated using the LiDAR degradation environment detection method based on multi-feature fusion. Among them, four features describing the geometric structure of a single frame point cloud are extracted, including: the number of line feature points N l , number of independent planes N p , the number of independent point cloud clusters N c , the degree of asymmetry P. The larger the above feature value is, the lower the possibility that the current frame is a degraded frame. Therefore, this method adopts the fuzzy comprehensive evaluation algorithm to comprehensively consider the four features, so as to accurately and efficiently evaluate the point cloud degradation.

3. The loop closure detection method based on stable static point cloud cluster bag of words according to claim 1, characterized in that: The proposed point cloud cluster classification algorithm described in step 2, based on the correction of the laser intensity of the degraded frame and the intensity connectivity segmentation, combines the coarse registration of adjacent frames, the similarity analysis of the structural descriptors and the density clustering method to comprehensively identify and classify the missing, unstable, dynamic and stable static point cloud clusters in each frame of the point cloud; The specific steps are as follows: Step 2-1: Use the laser intensity correction algorithm to correct the laser intensity of the degraded frame, and re-segment the point cloud clusters based on intensity connectivity. At the same time, update the center point set of the point cloud cluster. Due to the high frequency of LiDAR acquisition, it can be assumed that there is no sudden change in the corresponding environmental structure of the point clouds of adjacent frames. Therefore, the above process is only performed when the point clouds of adjacent frames are all degraded frames. Based on this, the point cloud clusters are classified; Step 2-2: The center point set of the point cloud cluster based on the adjacent frame point cloud is recorded as E A and E B , the ICP algorithm is used for coarse registration, and the rough relative pose is used to unify the coordinate reference. B For each point in E, set the appropriate neighborhood radius. A Perform the nearest neighbor search in , mark the point cloud cluster without the search object as the missing point cloud cluster, the same as above, for E A There are no marked or searched points in E B Reverse search and mark missing point cloud clusters; Step 2-3: Due to the straight-line propagation property of lasers, point cloud clusters scanned by the same target in adjacent frames may exhibit significant structural differences due to occlusion or changes in scanning angles. This paper considers such point cloud clusters as unstable point cloud clusters. To mark such point cloud clusters, we first construct structural descriptors μ based on the three-axis span of the point cloud cluster, as follows: in, Represent the spans of the X, Y, and Z axes respectively. Based on the combination of nearest paired point cloud clusters constructed by bidirectional nearest neighbor search, the cosine similarity S between the corresponding μ is calculated as follows: in, Indicates E B The descriptor of the k-th point cloud cluster in μ A Indicates E A The descriptor of the paired point cloud cluster in , j represents the element number in the descriptor. If S is less than 0.5, the current nearest neighbor paired point cloud cluster combination is marked as an unstable point cloud cluster; Step 2-4: By marking the missing point cloud clusters and unstable point cloud clusters, the current center point set of each point cloud cluster is only composed of stable static point cloud clusters and dynamic point cloud clusters. To mark the dynamic point cloud clusters, the E A and E B Project it onto the XY plane, randomly select two pairs of nearest neighboring paired point cloud clusters as local points, use the four-parameter coordinate transformation model to calculate the transformation matrix T, and transform E B The remaining points in the equation are converted to E A In the coordinate system, the two-dimensional Euclidean distance between the current nearest paired point cloud clusters is calculated, and all distances are clustered using a density-based spatial clustering algorithm with noise. If the clustering result is 1 category, they are all regarded as static point cloud clusters and the number is counted; if the clustering result is 2 categories, the category with the smaller average distance is regarded as a static point cloud cluster and the number is counted; if the clustering result is more than 2 categories, the categories are merged until only 2 categories remain, and the number of point cloud clusters in the category with the smaller average distance is also counted. The above process of selecting in-region points and counting the number of static point cloud clusters is continuously iterated until all in-region point combinations are processed and then stopped. The T corresponding to the largest number of static point cloud clusters is taken as E A and E B The transformation matrix of the precise registration is calculated and the corresponding dynamic point cloud clusters are marked. So far, each frame of point cloud contains 4 types of point cloud clusters, namely missing point cloud clusters, unstable point cloud clusters, dynamic point cloud clusters and stable static point cloud clusters.

4. The loop closure detection method based on stable static point cloud cluster bag of words according to claim 1, characterized in that: The key frame screening algorithm based on fuzzy comprehensive evaluation mentioned in step 3 is based on multi-feature fuzzy comprehensive evaluation. By introducing the number of stable static point cloud clusters to replace dynamic interference, integrating distance and structural features, using hierarchical analysis method to determine weights, and calculating the similarity between frames, efficient screening and updating of key frames are achieved; The specific steps are as follows: Step 3-1: Using the N extracted in the degraded frame evaluation process l 、N p 、N c , P, a total of 4 features, the fuzzy comprehensive evaluation algorithm is used to calculate the similarity between the current frame and the benchmark key frame, as follows: Among them, U represents the factor set, whose elements correspond to the four characteristics in turn, V represents the evaluation set, whose elements correspond to the three evaluation results of dissimilar, undetermined, and similar, R represents the fuzzy evaluation matrix, and μ(u,v) represents the membership function; Step 3-2: Based on the characteristics, the semi-trapezoidal membership calculation method is used to construct R, with N l For example, as follows: in, Indicates the N corresponding to the A frame l , Indicates the N corresponding to the B frame l , min means taking the minimum value operation, max means taking the maximum value operation, and the membership calculation models corresponding to other evaluation factors all adopt the form of formula (4); Step 3-3: To obtain comprehensive evaluation results, the analytic hierarchy process is used to construct the evaluation weight matrix D. First, the judgment matrix C is determined as follows: The weight d of each evaluation factor is calculated using the square root method based on the elements in C, as follows: in, represents the weight before normalization; Based on the calculation results of formula (6), D is constructed as follows: D=[d1 d2 d3 d4]=[0.25 0.25 0.25 0.25] (7) Apply D to R to calculate the fuzzy evaluation result set B as follows: Step 3-4: Compare the elements in B according to the maximum membership principle. If b1 is the largest, the similarity between the two frames of point clouds is low, and the current frame is added to the key frame sequence and used as the new reference key frame. If b3 is the largest, the two frames of point clouds are completely similar, and the current frame is skipped directly. If b2 is the largest, then when b2 is less than 0.5, the current frame is added to the key frame sequence and the reference key frame is updated.

5. The loop closure detection method based on stable static point cloud cluster bag of words according to claim 1, characterized in that: Construct local descriptors for each stable static point cloud cluster as described in step 4, thereby establishing a bag-of-words database for loop closure retrieval and effectively verifying the retrieval results; The specific steps are as follows: Step 4-1: Based on the results of degraded frame evaluation, point cloud cluster classification, and keyframe screening, for non-degraded keyframe sequences and degraded keyframe sequences, respectively, a non-degraded keyframe descriptor list and a degraded keyframe descriptor list are constructed according to the data storage mode of the bag-of-words model; Taking the non-degenerate key frame A as an example, for the center point set of the stable static point cloud cluster projected onto the two-dimensional plane, a 10m×10m rectangular neighborhood is constructed with each point as the center, and the distances from other points in the neighborhood to the center point are calculated and arranged from small to large to construct a descriptor. Taking the i-th point as an example, the descriptor as follows: in, Indicates the frame number to which the current point cloud cluster belongs. Indicates the number of the current point cloud cluster in the current frame, D represents the distance, and n represents the number of points contained in the current neighborhood; Construct a descriptor list based on κ of all points in frame A as follows: Among them, m represents the number of stable static point cloud clusters contained in frame A; Taking the degenerate keyframe B as an example, we also construct a neighborhood for each point, calculate the distance D and build a descriptor based on the laser intensity I of the corresponding point cloud cluster. Taking the i-th point as an example, the descriptor as follows: Construct a descriptor list based on κ of all points in the B frame as follows: Where m represents the number of stable static point cloud clusters contained in the B frame; Taking the degenerate keyframe B as an example, we also construct a neighborhood for each point, calculate the distance D and build a descriptor based on the laser intensity I of the corresponding point cloud cluster. Taking the i-th point as an example, the descriptor as follows: Construct a descriptor list based on κ of all points in the B frame as follows: Where m represents the number of stable static point cloud clusters contained in the B frame; Step 4-2: For each non-degenerate keyframe, a descriptor list is constructed using equations (9) and (10). For each degenerate keyframe, a descriptor list is constructed using equations (11) and (12). Based on this, the non-degenerate loop database F and the degenerate loop database F′ are updated in real time as follows: Step 4-3: Based on the retrieval mode of the bag-of-words model, perform loop retrieval in the corresponding type database for the descriptor list of the latest key frame, and use the current latest non-degenerate key frame as the Frame as an example, The descriptor of the i-th point cloud cluster in is as follows: against The first distance element in Expand the search in F, record and The corresponding IDs of elements with a difference of less than 0.1m f and ID c , same as above, record The search results of all distance elements in the , if the search result is empty, it is considered that there is no loop object in the current point cloud cluster; otherwise, the ID is counted f Key frames with a frequency greater than 2n / 3 are sorted from highest to lowest in order to construct a candidate key frame sequence W; If W is empty, it is considered that the current point cloud cluster has no loop object. Otherwise, the first candidate key frame is selected from W and the corresponding ID is counted. c The stable static point cloud clusters with a frequency greater than 2n / 3 are sorted from large to small according to the frequency to construct the candidate point cloud cluster sequence P. If P is empty, the candidate key frames in W are replaced in sequence to construct P until P is not empty or W is empty. Finally, the first element that meets the conditions in W and P is recorded as the optimal loop object of the i-th point cloud cluster. In addition, if the corresponding P of all candidate key frames in W are empty, it is considered that the current point cloud cluster has no loop object. As above, record The optimal loop object of the remaining point cloud clusters and the ID f The key frame with a frequency greater than 2m / 3 is regarded as the final loop frame. If all key frames do not meet the loop condition, they are regarded as The frame does not have a loopback frame; Step 4-4: For the latest degraded key frame The process of determining the final loop frame is the same as the above determination The final loopback process of the frame differs only in the description sub-element retrieval part. The descriptor of the i-th point cloud cluster in is as follows: against The first set of distance and intensity elements in and Expand the search in F′ and record The difference is less than 0.1m and The corresponding ID of the exact same element f and ID c , same as above, record All corresponding retrieval results are then subjected to subsequent loopback verification.

Images