Weighted ternary feature loopback detection method for laser SLAM (Simultaneous Localization and Mapping)

Abstract

The invention discloses a weighted ternary feature loopback detection method for laser SLAM (Simultaneous Localization and Mapping), and relates to the technical field of positioning and navigation. The invention provides a global descriptor of weighted ternary features, and the global descriptor has rotation and translation invariance; firstly, plane points and edge points which are easy to distinguish are screened out through geometric feature constraints; then, a local self-adaptive coordinate system is constructed on the basis of the feature points and the main direction of the feature points and used for extracting stable ternary features with physical significance, and parameterized expression of the point pair relation is achieved; and finally, fusing normal difference and curvature difference to construct a weighting mechanism, reinforcing geometric description of a local field, introducing density weight to obtain frame-level descriptors with global expression capability, and using Euclidean distance between the descriptors as an inter-frame similarity measurement standard to judge whether loopback exists or not. An experiment result on a public data set KITTI shows that compared with LeGO-LOAM, the root mean square error is reduced by 32% at most.

Description

Technical Field

[0001] This invention relates to the field of positioning and navigation technology, specifically to a weighted ternary feature loop closure detection method for laser SLAM. Background Technology

[0002] SLAM (Simultaneous Localization and Mapping) technology has wide applications in the field of unmanned aerial vehicles (UAVs). It enables UAVs to autonomously locate and navigate in unknown or dynamic environments. A typical application of SLAM is that it allows UAVs to build real-time maps of their surroundings and determine their own position within those maps, without relying on external positioning systems (such as GPS), using their sensors (e.g., LiDAR, visual sensors, IMU). Autonomous navigation and obstacle avoidance are also key features: Using maps generated by SLAM, UAVs can autonomously plan their flight paths, avoid obstacles, and reduce the risk of collisions. This is particularly important in complex or unknown environments, such as indoor flights, underground or urban environments. Furthermore, SLAM can generate high-precision maps of the environment, aiding in 3D modeling for UAVs. This is extremely useful for tasks such as building inspection, disaster assessment, and agricultural monitoring.

[0003] Loop closure detection is a crucial component of SLAM systems, used to identify historical locations, correct accumulated pose estimation errors, and ultimately obtain a globally optimized map.

[0004] Laser SLAM is a reliable solution due to its strong robustness to environmental changes. LOAM, a classic SLAM algorithm, achieves low drift and low computational complexity without requiring high-precision ranging or inertial measurement, but it lacks loop closure detection and cannot correct long-term accumulated errors. In 2018, Shan et al. proposed the lightweight and ground-optimized laser SLAM algorithm LeGO-LOAM, which also introduced loop closure detection. In 2020, Shan proposed a tightly coupled LIO-SAM algorithm between a lidar and an inertial measurement unit. Both methods retrieve historical keyframes that satisfy distance and time constraints using Euclidean distance to form loop closure candidate frames. This method only considers the proximity of keyframes in position, ignoring spatial structure and viewpoint similarity, resulting in poor stability and failure in large-scale scenes. In 2018, Kim et al. proposed the Scan Context global descriptor, which projects the point cloud as a polar coordinate matrix to record the spatial structure, but only uses the maximum height value as a feature, resulting in insufficient descriptive power. In 2020, Wang et al. proposed the LiDAR Iris method, which uses LoG-Gabor filtering and thresholding to obtain a binary signature image from the generated LiDAR-Iris image. However, this method loses longitudinal stereo information when compressing 2D images, making it difficult to handle 3D structural changes. In the same year, they proposed the Intensity Scan Context descriptor, which integrates geometric and intensity features. It performs geometric relationship retrieval through binary operations and then re-identifies the intensity structure. However, intensity features are greatly affected by the environment, and the two-stage processing suffers from information fragmentation. In 2023, Yuan et al., based on the stability of triangular structures during rigid body transformations, encoded the side lengths between any three points to achieve loop closure matching. They then used the side lengths of the matching descriptor to achieve 3D position recognition. However, the encoding relies on keypoint selection, and keypoint occlusion occurs when the viewpoint changes, leading to incorrect matching.

[0005] Local descriptors for LiDAR, such as FPFH and SHOT, are used for matching parts in 3D models, not for position recognition. Therefore, to improve the discriminative power of the global descriptor, planar and edge feature points are selected based on geometric constraints. On this basis, a local coordinate system suitable for point cloud structure modeling is designed. This coordinate system uses the query point's normal vector as the principal axis, and defines the other two axes based on the cross product of the point pair's direction and the normal vector. Under this coordinate system, the point-pair geometric features formed by the query point and its neighboring points are parameterized, i.e., ternary features. This feature can uniformly represent the local structure from different viewpoints and is sensitive to changes in surface details. To further improve the discriminative power and stability of the ternary features, a weighting mechanism based on geometric consistency is designed. This mechanism comprehensively considers the differences in the normal vector direction and curvature of neighboring points, assigning different weights to each ternary feature, and generating the global descriptor for the current frame through weighted averaging. This descriptor possesses rotation and translation invariance. In the loop closure detection stage, the Euclidean distance between descriptors is calculated to determine whether a loop has formed.

[0006] In summary, laser-based simultaneous localization and mapping (SLAM) in large-scale scenes suffers from loop closure detection failure, leading to an inability to correct global cumulative errors. Compared to various feature detectors and descriptors for visual scenes, research on laser SLAM utilizing structural information to describe scenes is relatively limited.

[0007] Therefore, a new solution is needed to address the above problems. Summary of the Invention

[0008] The purpose of this invention is to provide a weighted ternary feature loop closure detection method for laser SLAM, so as to solve the technical problems mentioned in the background art.

[0009] To achieve the above objectives, the present invention provides the following technical solution: a weighted ternary feature loop closure detection method for laser SLAM, comprising at least the following steps:

[0010] S1: First, extract planar feature points and edge feature points from the keyframe point cloud, and generate a global descriptor of weighted ternary features based on the neighborhood geometry information;

[0011] S2: Subsequently, the global descriptors of the keyframe point cloud are stored in the database as feature records of historical trajectories. In the loop closure detection stage, candidate loop closure frames are matched by calculating the Euclidean distance between the current frame and the descriptors in the database, and further verification is performed using point cloud registration to ensure the accuracy of loop closure detection.

[0012] S3: Finally, the candidate loopback frames are matched. If a match is successful, global optimization is performed to reduce accumulated errors and improve the accuracy and stability of the SLAM system.

[0013] Furthermore, the global descriptor for generating weighted ternary features based on domain geometric information adopts a descriptor construction method constrained by geometric features.

[0014] Furthermore, the descriptor construction method for the geometric feature constraints includes at least the following steps:

[0015] First, geometric feature constraints are introduced to improve the discriminative power of the global descriptor. These constraints are applied only to planar features P with significant structural characteristics. pla and edge feature point P edg Extract information from above;

[0016] Next, normal vector estimation is performed;

[0017] Finally, a weighted ternary feature descriptor is generated.

[0018] Furthermore, in the geometric feature constraints, planar feature points are located at points in flat regions of three-dimensional space with small curvature, while edge feature points are located at edges or turning points with large curvature in three-dimensional space.

[0019] These two types of feature points are distinguished by calculating curvature. The curvature calculation formula is as follows:

[0020]

[0021] Where S is a continuous set of points on the same laser scanning line; k represents one period of the scanning point; L is the radar coordinate system; and c represents the curvature of the current point i. and Let i and j represent the coordinates of point i and point j in the radar coordinate system, respectively.

[0022] The same laser scanning line is divided into six sub-regions. The 20 points with the largest curvature within each sub-region are identified as edge feature points, and the remaining points are marked as planar feature points. The set of all features in the k-th frame is...

[0023] Furthermore, the normal vector estimation includes at least the following steps:

[0024] Since ternary features reflect the interaction between the query point and its neighboring points in terms of geometric relationship and normal direction, and the quality of normal estimation affects the accuracy and stability of the descriptor, we first determine a query point in the point cloud. The normal vector is a vector perpendicular to the tangent plane of the surface where the point is located. The normal vector of the query point can be estimated by estimating the normal of the tangent plane, thus transforming it into a plane fitting problem using the least squares method.

[0025] Construct the covariance matrix for the query point and its k-neighbors, point p i The covariance matrix m is defined as follows:

[0026]

[0027] m·v j =λ j ·v j (3)

[0028] Where T represents the transpose operation of a matrix or vector; v represents the mean of the points; j λ represents the eigenvectors of the covariance matrix m; j These are the corresponding eigenvalues;

[0029] Solving for the eigenvalues ​​of the covariance matrix m yields the corresponding normal vector for that point. The eigenvalues ​​reflect the distribution of the local point cloud, while the minimum eigenvalue λ... min eigenvector v min That is, point p i The normal vector.

[0030] Furthermore, the weighted ternary feature descriptor includes at least the following steps:

[0031] A local fixed coordinate system uvw is introduced to uniformly represent the geometric feature relationships of points in the point cloud. The local fixed coordinate system uvw consists of three orthogonal basis vectors, which are defined as follows:

[0032] u = η i (4)

[0033]

[0034] w = u × v (6)

[0035] In the constructed local coordinate system, u = η i η i Point P i The normal vector v is perpendicular to vectors u and p. i -p j The plane in which w lies is obtained by the cross product of u and v;

[0036] Secondly, a local fixed coordinate system is constructed for each query point, and the ternary features between point pairs within the neighborhood are calculated. Based on this, three angular features α, φ, and θ are defined. These angular features are used to reflect the changes in the normal direction within the local area, capturing the angle between point normals, the difference in vectors between points, and their spatial distribution in three-dimensional space. The three angular features are defined as follows:

[0037] α=v·η j (7)

[0038]

[0039] θ = arctan(w·η) j ,u·η j (9)

[0040] α represents the angle between the query point's normal vector and the neighboring point's normal vector; φ represents the angle between the query point's normal vector and the line connecting the two points; θ represents the angle between the neighboring point's normal vector and the line connecting the two points; d = p j -p i Point P i and P j The Euclidean distance between them can measure small changes in the neighborhood of a point cloud and describe the geometric relationships of local regions of the point cloud;

[0041] The query point will fix a range of domains, point P. q Centered on the query point, five neighboring points are found within its k-neighborhood. The line connecting these points represents the simplified ternary feature between the query point and its neighboring points, denoted as STF. In this process, the angular features are calculated by quantifying the local geometric information of the point cloud surface, providing a basis for feature description.

[0042] For query point P q Calculate its STF(P) q ) and its k-neighborhood i-th point P ki STF(P) ki ), and sum them by weight;

[0043] By extracting easily distinguishable planar feature points and edge feature points, the geometric consistency within the domain can be determined. It is expected that neighboring points with similar geometric shapes will contribute more to the query point. To measure the contribution of neighboring points to the query point, a geometric consistency metric is proposed.

[0044] The difference Δθ between the normal directions of the i-th neighboring points in the k-neighborhood of the query point is calculated. i and curvature difference Δc i To describe the similarity of geometric shapes, suppose point P q and P ki The normals are N q and N ki The curvatures are c q and c ki The differences in normal direction and curvature are expressed as follows:

[0045]

[0046] Δc i =|c q -c ki | (11)

[0047] Where, N q ·Nki The dot product of the normals, ||N q ||||N ki || represents the magnitude of the normal; Δc i This indicates the difference in their curvature;

[0048] Therefore, the weighting factor is designed to adjust the contribution of the descriptor based on the difference in normal and curvature, using a decay function that varies with Δθ. i and Δc i As ω increases, the weighting factor gradually decreases. k (Δθ i , Δc i The following is represented:

[0049]

[0050] Where λ is a parameter that adjusts the influence of the difference between the normal and curvature on the weighting factor. This weighting factor adjusts its contribution to the query point descriptor based on the geometric consistency of the neighborhood points. The weighted ternary feature WTF is represented as follows:

[0051]

[0052] Density is used to measure the distribution of point clouds per unit volume. Let the query point P be... q The neighborhood point set is The radius of the neighborhood is k; the radius of the neighboring neighborhood is fixed, and the corresponding volume V is constant, therefore point P... q density ρ(P) q Simplified to the number of neighborhood points, a neighborhood weighting mechanism is also introduced for point P. q density D q The calculation is as follows:

[0053]

[0054] Among them, P ki Let ω represent the i-th point in the k-th neighborhood. i For P q With P ki Distance weights;

[0055] The global descriptor (WTF) of the point cloud for this frame is obtained by weighting all points with density information and using weighted ternary features:

[0056] WTF = ∑ i ω i ·TFP(p i (15)

[0057]

[0058] Where, ω iThe density weighting factor; N(i) represents point P. i The set of neighborhood points; this weighting method adjusts the local point cloud features to ensure that points with higher density have a larger weight in the calculation of the global feature vector, while points in sparse regions have a lower weight, thus reducing the impact of noise;

[0059] WTF is obtained by decomposing the angular features α, φ, and θ. Each angular feature can be divided into B intervals, ultimately forming a 3B-dimensional feature vector, as shown in Equation 17:

[0060]

[0061] The experiment set parameter B to 11.

[0062] Compared with the prior art, the beneficial effects of the present invention are:

[0063] This invention proposes a global descriptor based on weighted ternary features, which possesses rotation and translation invariance. First, easily distinguishable planar points and edge points are selected through geometric feature constraints. Then, a local adaptive coordinate system is constructed based on the feature points themselves and their principal directions to extract stable and physically meaningful ternary features, realizing the parameterized expression of point-pair relationships. Finally, a weighting mechanism is constructed by fusing normal differences and curvature differences to enhance the geometric description of the local domain, and density weights are introduced to obtain frame-level descriptors with global expressive capabilities. The Euclidean distance between descriptors is used as a metric for inter-frame similarity to determine whether loop closure occurs. Experimental results on the public dataset KITTI show that the root mean square error is reduced by up to 32% compared to LeGO-LOAM. Attached Figure Description

[0064] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0065] Figure 1 This is a framework diagram of the loop closure detection system of the present invention;

[0066] Figure 2 This is a coordinate system diagram of the present invention;

[0067] Figure 3 This is a schematic diagram of the neighborhood range of the present invention;

[0068] Figure 4 The sequence trajectory diagrams of each algorithm on KITTI 00, 02, and 05 are shown. Detailed Implementation

[0069] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments.

[0070] Please see Figure 1 A weighted ternary feature loop closure detection method for laser SLAM includes at least the following steps:

[0071] S1: First, extract planar feature points and edge feature points from the keyframe point cloud, and generate a global descriptor of weighted ternary features based on the neighborhood geometry information;

[0072] S2: Subsequently, the global descriptors of the keyframe point cloud are stored in the database as feature records of historical trajectories. In the loop closure detection stage, candidate loop closure frames are matched by calculating the Euclidean distance between the current frame and the descriptors in the database, and further verification is performed using point cloud registration to ensure the accuracy of loop closure detection.

[0073] S3: Finally, the candidate loopback frames are matched. If a match is successful, global optimization is performed to reduce accumulated errors and improve the accuracy and stability of the SLAM system.

[0074] Furthermore, the global descriptor for generating weighted ternary features based on domain geometric information adopts a descriptor construction method constrained by geometric features.

[0075] Furthermore, the descriptor construction method for the geometric feature constraints includes at least the following steps:

[0076] First, geometric feature constraints are introduced to improve the discriminative power of the global descriptor. These constraints are applied only to planar features P with significant structural characteristics. pla and edge feature point P edg Extract information from above;

[0077] Next, normal vector estimation is performed;

[0078] Finally, a weighted ternary feature descriptor is generated.

[0079] Furthermore, in the geometric feature constraints, planar feature points are located at points in flat regions of three-dimensional space with small curvature, while edge feature points are located at edges or turning points with large curvature in three-dimensional space.

[0080] These two types of feature points are distinguished by calculating curvature. The curvature calculation formula is as follows:

[0081]

[0082] Where S is a continuous set of points on the same laser scanning line; k represents one period of the scanning point; L is the radar coordinate system; and c represents the curvature of the current point i. and Let i and j represent the coordinates of point i and point j in the radar coordinate system, respectively.

[0083] The same laser scanning line is divided into six sub-regions. The 20 points with the largest curvature within each sub-region are identified as edge feature points, and the remaining points are marked as planar feature points. The set of all features in the k-th frame is...

[0084] Furthermore, the normal vector estimation includes at least the following steps:

[0085] Since ternary features reflect the interaction between the query point and its neighboring points in terms of geometric relationship and normal direction, and the quality of normal estimation affects the accuracy and stability of the descriptor, we first determine a query point in the point cloud. The normal vector is a vector perpendicular to the tangent plane of the surface where the point is located. The normal vector of the query point can be estimated by estimating the normal of the tangent plane, thus transforming it into a plane fitting problem using the least squares method.

[0086] Construct the covariance matrix for the query point and its k-neighbors, point p i The covariance matrix m is defined as follows:

[0087]

[0088] m·v j =λ j ·v j (3)

[0089] Where T represents the transpose operation of a matrix or vector; ν represents the mean of the points; j λ represents the eigenvectors of the covariance matrix m; j These are the corresponding eigenvalues;

[0090] Solving for the eigenvalues ​​of the covariance matrix m yields the corresponding normal vector for that point. The eigenvalues ​​reflect the distribution of the local point cloud, while the minimum eigenvalue λ... min eigenvector v min That is, point p i The normal vector.

[0091] Furthermore, the weighted ternary feature descriptor includes at least the following steps:

[0092] A local fixed coordinate system uvw is introduced to uniformly represent the geometric feature relationships of points in the point cloud. The local fixed coordinate system uvw consists of three orthogonal basis vectors, which are defined as follows:

[0093] u = η i (4)

[0094]

[0095] w = u × v (6)

[0096] In the constructed local coordinate system, u = η i η i Point P i The normal vector v is perpendicular to vectors u and p. i -p j The plane in which it lies, w, is obtained by the cross product of u and v. The uvw coordinate system is as follows: Figure 2 As shown;

[0097] Secondly, a local fixed coordinate system is constructed for each query point, and the ternary features between point pairs within the neighborhood are calculated. Based on this, three angular features α, φ, and θ are defined. These angular features are used to reflect the changes in the normal direction within the local area, capturing the angle between point normals, the difference in vectors between points, and their spatial distribution in three-dimensional space. The three angular features are defined as follows:

[0098] α=v·η j (7)

[0099]

[0100] θ = arctan(w·η) j ,u·η j (9)

[0101] α represents the angle between the query point's normal vector and the neighboring point's normal vector; φ represents the angle between the query point's normal vector and the line connecting the two points; θ represents the angle between the neighboring point's normal vector and the line connecting the two points; d = p j -p i Point P i and P j The Euclidean distance between them can measure small changes in the neighborhood of a point cloud and describe the geometric relationships of local regions of the point cloud;

[0102] The query point will fix a neighborhood range, and its neighborhood range is as follows: Figure 3 As shown. Point P q Centered on the query point, five neighboring points are found within its k-neighborhood. The lines connecting these points represent the simplified ternary feature between the query point and its neighboring points, denoted as STF (Simplified Ternary Feature). In this process, angular features are calculated by quantifying the local geometric information of the point cloud surface, providing a basis for feature description.

[0103] For query point P q Calculate its STF(P) q ) and its k-neighborhood i-th point P ki STF(P)ki ), and sum them by weight;

[0104] Traditional methods only consider the geometric distance between points, ignoring the consistency of geometric shapes among neighboring points. In the experimental design, by extracting easily distinguishable planar feature points and edge feature points, the geometric shape consistency within the neighborhood can be determined. It is expected that neighboring points with similar geometric shapes will contribute more to the query point. To measure the contribution of neighboring points to the query point, a geometric consistency metric is proposed.

[0105] The difference Δθ between the normal directions of the i-th neighboring points in the k-neighborhood of the query point is calculated. i and curvature difference Δc i To describe the similarity of geometric shapes, suppose point P q and P ki The normals are N q and N ki The curvatures are c q and c ki The differences in normal direction and curvature are expressed as follows:

[0106]

[0107] Δc i =|c q -c ki | (11)

[0108] Where, N q ·N ki The dot product of the normals, ||N q ||||N ki || represents the magnitude of the normal; Δc i This indicates the difference in their curvature;

[0109] Therefore, the weighting factor is designed to adjust the contribution of the descriptor based on the difference in normal and curvature, using a decay function that varies with Δθ. i and Δc i As ω increases, the weighting factor gradually decreases. k (Δθ i , Δc i The following is represented:

[0110]

[0111] Where λ is a parameter that adjusts the influence of the difference between the normal and curvature on the weighting factor. This weighting factor adjusts its contribution to the query point descriptor based on the geometric consistency of the neighborhood points. The weighted ternary feature (WTF) is represented as follows:

[0112]

[0113] Density is used to measure the distribution of point clouds per unit volume. Let the query point P be... q The neighborhood point set is The radius of the neighborhood is k; the radius of the neighboring neighborhood is fixed, and the corresponding volume V is constant, therefore point P... q density ρ(p) q Simplified to the number of neighborhood points, a neighborhood weighting mechanism is also introduced for point P. q density D q The calculation is as follows:

[0114]

[0115] Among them, P ki Let ω represent the i-th point in the k-th neighborhood. i For P q With P ki Distance weights;

[0116] The global descriptor (WTF) of the point cloud for this frame is obtained by weighting all points with density information and using weighted ternary features:

[0117] WTF = ∑ i ω i ·TFP(p i (15)

[0118]

[0119] Where, ω i The density weighting factor; N(i) represents point P. i The set of neighborhood points; this weighting method adjusts the local point cloud features to ensure that points with higher density have a larger weight in the calculation of the global feature vector, while points in sparse regions have a lower weight, thus reducing the impact of noise;

[0120] WTF is obtained by decomposing the angular features α, φ, and θ. Each angular feature can be divided into B intervals, ultimately forming a 3B-dimensional feature vector, as shown in Equation 17:

[0121]

[0122] The experiment set parameter B to 11.

[0123] Based on the above, the experimental results and analysis are presented as follows:

[0124] The experiment was validated on the KITTI dataset with loop-closure sequences 00, 02, and 05. The experimental platform was equipped with an Intel Core i7-13700H processor, 16GB of RAM, and Ubuntu 18.04LTS operating system. The robot operating system used was melodic.

[0125] The true values ​​of the KITTI trajectory are provided by GNS, using the root mean square error (RMSE) and standard deviation (SD) of the absolute trajectory error (ATE) as the main statistical indicators of accuracy. The calculation formula is as follows:

[0126]

[0127]

[0128] Among them, e RMSE e is the root mean square error; SD is the standard deviation; m is the number of poses; F i Let be the pose error between the estimated value and the true value for the i-th frame; The mean of the absolute trajectory error; trans(·) indicates taking the translation part of the Euclidean transformation within the parentheses.

[0129] Conduct a loop closure detection positioning accuracy experiment

[0130] To verify the performance of the algorithm in outdoor environments, experiments were conducted using sequences 00, 02, and 05 from the KITTI dataset. These three sequences represent travel distances exceeding 2000 meters, and each sequence includes a loop closure detection scenario, making them suitable for verifying the effectiveness and robustness of the algorithm in long-distance travel, complex environmental changes, and loop closure detection. Information for each sequence is given in Table 1.

[0131] Table 1. KITTI Sequence Information Data Table

[0132]

[0133] Some sequences in the KITTI dataset (such as the 00 sequence) have missing or abrupt IMU data, causing LIO-SAM to malfunction and some sequences to fail to locate without IMU assistance. Therefore, our algorithm (denoted as Ours) selects LeGO-LOAM and its Scan Context-integrated SC-LeGO-LOAM as comparison objects. To comprehensively evaluate the performance of each algorithm, experiments were conducted on three typical sequences, and the bag playback rate was set to 0.5x to avoid system stuttering. The trajectory results are as follows: Figure 4 As shown.

[0134] Table 2 compares the absolute pose errors of each algorithm in sequences 00, 02, and 05. LeGO-LOAM uses a loop closure candidate frame selection method based on Euclidean distance and time thresholds, combined with ICP to complete pose correction. This method relies on the accuracy of the initial geometric values ​​of historical keyframes. However, in complex scenes like sequence 02, which contain many turns, irregular structures, and occlusions, inaccurate initial values ​​will lead to ICP matching failures or getting stuck in local optima, ultimately causing errors to accumulate. In sequence 02, LeGO-LOAM's RMSE is as high as 11.51m, while the method of this invention reduces it to 9.13m. Scan Context extracts global geometric information of the scene by constructing a circular image descriptor, exhibiting strong rotation invariance. However, this method focuses on the top-view projection structure of the point cloud, ignoring local 3D geometry and point density features, resulting in decreased matching accuracy in scenes with structural occlusion, sparse point clouds, or height variations, leading to mismatches or missed detections in loop closures. In sequence 02, its error standard deviation is 9.53m, reflecting insufficient stability in complex urban environments. In contrast, the global descriptor proposed in this invention extracts direction, angle, and distance information between point pairs by constructing a stable local coordinate system, and introduces a weighting mechanism based on normal vector and curvature differences to improve geometric consistency modeling capabilities. This allows the descriptor to maintain strong discriminative ability and global expressive power even with point cloud occlusion, pose changes, and increased environmental complexity. In the simple sequence 00, the method accurately identifies loop closure frames and corrects accumulated errors, resulting in a 32.5% reduction in RMSE and a 46.3% reduction in standard deviation compared to LeGO-LOAM. In the complex sequence 02 with frequent turns, the RMSE decreases by 20.7%, demonstrating stronger robustness. In the moderately structured sequence 05 with repetitive buildings, the RMSE still decreases by approximately 8.5%, with both mean and median errors decreasing simultaneously, verifying the universality and stability of the method in different scenarios.

[0135] Table 2. Absolute pose errors of each algorithm on sequences 00, 02, and 05 (unit: meters)

[0136]

[0137] In summary:

[0138] This invention proposes a weighted ternary feature-based global point cloud descriptor for loop closure detection in laser-based scenarios. To enhance the descriptor's discriminative power and representativeness, points are divided into planar points and edge points based on their geometric attributes. On this basis, a local coordinate system is constructed using ternary features to obtain parameterized point-pair geometric relationships. Then, a weighting strategy based on geometric consistency is introduced to weight and fuse point-pair features within the local neighborhood, enhancing the ability to describe local geometric structures. Finally, local geometric information is fused with point cloud density features to construct a descriptor with global expressive power. This descriptor is constructed based on a histogram, exhibiting rotation and translation invariance, adapting to changes in viewing angle, and effectively improving the accuracy and stability of loop closure detection in laser SLAM systems.

[0139] Experimental results show that, in tests on the publicly available KITTI dataset, the algorithm of this invention reduces the root mean square error by up to 32% compared to LeGO-LOAM. This invention effectively improves the accuracy and stability of loop closure detection in large-scene laser SLAM, providing a new approach for the development of loop closure detection.

[0140] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other specific forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered in all respects as exemplary and non-limiting, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of equivalents of the claims are intended to be included within the present invention. No reference numerals in the claims should be construed as limiting the scope of the claims.

Claims

1. A weighted ternary feature loop closure detection method for laser SLAM, characterized in that: At least the following steps are included: S1: First, extract planar feature points and edge feature points from the keyframe point cloud, and generate a global descriptor of weighted ternary features based on the neighborhood geometry information; S2: Subsequently, the global descriptors of the keyframe point cloud are stored in the database as feature records of historical trajectories. In the loop closure detection stage, candidate loop closure frames are matched by calculating the Euclidean distance between the current frame and the descriptors in the database, and further verification is performed using point cloud registration to ensure the accuracy of loop closure detection. S3: Finally, the candidate loopback frames are matched. If a match is successful, global optimization is performed to reduce accumulated errors and improve the accuracy and stability of the SLAM system.

2. The weighted ternary feature loop closure detection method for laser SLAM according to claim 1, characterized in that: The global descriptor that generates weighted ternary features based on domain geometric information adopts a descriptor construction method constrained by geometric features.

3. The weighted ternary feature loop closure detection method for laser SLAM according to claim 2, characterized in that: The descriptor construction method for the geometric feature constraints includes at least the following steps: First, geometric feature constraints are introduced to improve the discriminative power of the global descriptor. These constraints are applied only to planar features P with significant structural characteristics. pla and edge feature point P edg Extract information from above; Next, normal vector estimation is performed; Finally, a weighted ternary feature descriptor is generated.

4. The weighted ternary feature loop closure detection method for laser SLAM according to claim 3, characterized in that: In the geometric feature constraints, planar feature points are located in flat areas of three-dimensional space with small curvature, while edge feature points are located at edges or turning points with large curvature in three-dimensional space. These two types of feature points are distinguished by calculating curvature. The curvature calculation formula is as follows: Where S is a continuous set of points on the same laser scanning line; k represents one period of the scanning point; L is the radar coordinate system; and c represents the curvature of the current point i. and Let i and j represent the coordinates of point i and point j in the radar coordinate system, respectively. The same laser scanning line is divided into six sub-regions. The 20 points with the largest curvature within each sub-region are identified as edge feature points, and the remaining points are marked as planar feature points. The set of all features in the k-th frame is...

5. The weighted ternary feature loop closure detection method for laser SLAM according to claim 4, characterized in that: The normal vector estimation includes at least the following steps: Since ternary features reflect the interaction between the query point and its neighboring points in terms of geometric relationship and normal direction, and the quality of normal estimation affects the accuracy and stability of the descriptor, we first determine a query point in the point cloud. The normal vector is a vector perpendicular to the tangent plane of the surface where the point is located. The normal vector of the query point is estimated by estimating the normal of the tangent plane, thus transforming it into a plane fitting problem using the least squares method. Construct the covariance matrix for the query point and its k-neighbors, point p i The covariance matrix m is defined as follows: m·v j =λ j ·v j (3) Where t represents the transpose operation of a matrix or vector; ν represents the mean of the points; j λ represents the eigenvectors of the covariance matrix m; j These are the corresponding eigenvalues; Solving for the eigenvalues ​​of the covariance matrix m yields the normal vector associated with that point. The eigenvalues ​​reflect the distribution of the local point cloud, while the minimum eigenvalue λ... min eigenvector v min That is, point p i The normal vector.

6. The weighted ternary feature loop closure detection method for laser SLAM according to claim 5, characterized in that: The weighted ternary feature descriptor includes at least the following steps: A local fixed coordinate system uvw is introduced to uniformly represent the geometric feature relationships of points in the point cloud. The local fixed coordinate system uvw consists of three orthogonal basis vectors, which are defined as follows: u=η i (4) w = u × v (6) In the constructed local coordinate system, u = η i η i Point P i The normal vector v is perpendicular to vectors u and p. i -p j The plane in which w lies is obtained by the cross product of u and v; Secondly, a local fixed coordinate system is constructed for each query point, and the ternary features between point pairs within the neighborhood are calculated. Based on this, three angular features α, φ, and θ are defined. These angular features are used to reflect the changes in the normal direction within the local area, capturing the angle between point normals, the difference in vectors between points, and their spatial distribution in three-dimensional space. The three angular features are defined as follows: a=v·h j (7) θ=arctan(w·η j ,u·h j ) (9) α represents the angle between the query point's normal vector and the neighboring point's normal vector; φ represents the angle between the query point's normal vector and the line connecting the two points; θ represents the angle between the neighboring point's normal vector and the line connecting the two points; d = p j -p i Point P i and P j The Euclidean distance between them can measure small changes in the neighborhood of a point cloud and describe the geometric relationships of local regions of the point cloud; The query point will fix a range of domains, point P. q Centered on the query point, five neighboring points are found within its k-neighborhood. The line connecting these points represents the simplified ternary feature between the query point and its neighboring points, denoted as STF. In this process, the angular features are calculated by quantifying the local geometric information of the point cloud surface, providing a basis for feature description. For query point P q Calculate its STF(P) q ) and its k-neighborhood i-th point P ki STF(P) ki ), and sum them up by weight; By extracting easily distinguishable planar feature points and edge feature points, the geometric consistency within the domain can be determined. It is expected that neighboring points with similar geometric shapes will contribute more to the query point. To measure the contribution of neighboring points to the query point, a geometric consistency metric is proposed. The difference Δθ between the normal directions of the i-th neighboring points in the k-neighborhood of the query point is calculated. i and curvature difference Δc i To describe the similarity of geometric shapes, suppose point P q and P ki The normals are N q and N ki The curvatures are c q and c ki The differences in normal direction and curvature are expressed as follows: Δc i =|c q -c ki | (11) Where, N q ·N ki The dot product of the normals, ||N q ||||N ki || represents the magnitude of the normal; Δc i This indicates the difference in their curvature; Therefore, the weighting factor is designed to adjust the contribution of the descriptor based on the difference in normal and curvature, using a decay function that varies with Δθ. i and Δc i As ω increases, the weighting factor gradually decreases. k (Δθ i , Δc i The following is represented: Where λ is a parameter that adjusts the influence of the difference between the normal and curvature on the weighting factor. This weighting factor adjusts its contribution to the query point descriptor based on the geometric consistency of the neighborhood points. The weighted ternary feature WTF is represented as follows: Density is used to measure the distribution of point clouds per unit volume. Let the query point P be... q The neighborhood point set is The radius of the neighborhood is k; the radius of the neighboring neighborhood is fixed, and the corresponding volume V is constant, therefore point P... q density ρ(p) q Simplified to the number of neighborhood points, a neighborhood weighting mechanism is also introduced for point P. q density D q The calculation is as follows: Among them, P ki Let ω represent the i-th point in the k-th neighborhood. i For P q With P ki Distance weights; The global descriptor (WTF) of the point cloud for this frame is obtained by weighting all points with density information and using weighted ternary features: WTF=∑ i ω i ·TFP(p i ) (15) Where, ω i The density weighting factor; N(i) represents point P. i The set of neighborhood points; this weighting method adjusts the local point cloud features to ensure that points with higher density have a larger weight in the calculation of the global feature vector, while points in sparse regions have a lower weight, thus reducing the impact of noise; WTF is obtained by decomposing the angular features α, φ, and θ. Each angular feature can be divided into B intervals, ultimately forming a 3B-dimensional feature vector, as shown in Equation 17: The experiment set parameter B to 11.

Images