Geometric feature reliability-based Lidar point cloud registration optimization method

Abstract

The invention discloses a Lidar point cloud registration optimization method based on geometric feature reliability, and the method comprises the steps: carrying out the preprocessing and initial alignment of a laser radar scanning point cloud, extracting the features of a maximum principal curvature and a minimum principal curvature based on the local curvature of the point cloud, dividing a point region into two types of geometric features of angular points and plane points according to the threshold value of the maximum principal curvature, and carrying out the registration of the angular points and the plane points. On the basis, fitting quality factors including fitting errors, local curvatures and spectral entropies of the linear features and the plane features are calculated respectively, then the three factors are fused into a unified reliability weight based on a Bayesian probability model, and finally the feature reliability weight is introduced into an optimization objective function of iterative nearest point registration to execute weighted ICP registration. And outputting positioning and attitude determination results. According to the method, the reliability of geometric features is quantitatively evaluated, and a weighted optimization framework is constructed, so that the point cloud registration precision and robustness are remarkably improved, and the problem that a traditional ICP algorithm is sensitive to unreliable features in feature degradation or high-dynamic scenes is effectively solved.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of Lidar positioning point cloud registration, and particularly relates to a Lidar point cloud registration optimization method based on geometric feature reliability. BACKGROUND

[0002] In Lidar positioning, point cloud registration is a key link, which is to find a transformation including translation and rotation to align the current frame point cloud data with the map point cloud data in space, so as to solve the corresponding pose of the current frame point cloud. Iterative Closest Point (ICP) is a commonly used method for point cloud registration, which finds the nearest point correspondence between two point clouds through iterative calculation, so as to solve the rigid body transformation. As a classical method for point cloud registration, the ICP algorithm and its derivative algorithms have the characteristics of high efficiency and accuracy in structured environments, and lay an important foundation for Lidar positioning methods.

[0003] Although the current ICP and its improved algorithms have improved the registration efficiency by introducing point-to-line and point-to-plane geometric constraints, they generally ignore the influence of feature quality difference on registration accuracy, resulting in significant performance decline in sparse or degenerate feature scenes. Therefore, a Lidar point cloud registration optimization method based on geometric feature reliability is proposed to solve the above problems. SUMMARY

[0004] To solve the above problems, the application discloses a Lidar point cloud registration optimization method based on geometric feature reliability, which quantifies the feature fitting quality by using three factors of feature fitting error, local curvature and spectral entropy, forms a unified reliability weight through a Bayesian model, and constructs a weighted point cloud registration optimization framework based on the weight. By quantitatively evaluating and using the reliability of geometric features, the accuracy and robustness of point cloud registration are significantly improved.

[0005] To achieve the above purpose, the technical scheme of the application is as follows:

[0006] A Lidar point cloud registration optimization method based on geometric feature reliability, comprising the following steps:

[0007] Step 1, preprocessing and initial alignment of Lidar scanning point cloud;

[0008] Step 2, extracting geometric features based on point cloud local curvature and performing geometric feature matching;

[0009] Step 3, calculating the fitting quality factors of linear features and plane features respectively, including fitting error, local curvature and spectral entropy;

[0010] Step 4, evaluating the reliability of geometric features based on a Bayesian probability model, and quantifying the fitting quality thereof.

[0011] Step 5, based on the feature reliability weight, performing weighted Iterative Closest Point (ICP) registration optimization;

[0012] Step 6, outputting the positioning and pose determination result.

[0013] The specific steps are:

[0014] Step 1. Preprocessing and initial alignment of laser radar scan point cloud

[0015] Firstly, the synchronization of IMU and LiDAR data is realized through timestamp correction, the pose of each point in the point cloud is interpolated based on IMU pre-integration, and then the continuous trajectory model is constructed by using the high-frequency motion measurement value of IMU, and the motion distortion is eliminated according to the accurate timestamp of laser points; At the same time, the point cloud is filtered and denoised by outlier filtering and spatial downsampling compression, and finally the initial spatial alignment of the scan point cloud and the map point cloud is realized based on the prior pose of IMU.

[0016] Step 2. Extracting geometric features based on point cloud local curvature and performing geometric feature matching

[0017] The principal component analysis method based on scan line neighborhood is adopted to calculate the curvature feature. For a given point p i on the scan line, its k neighboring points before and after are selected to form a local neighborhood N i ={p i-k ,...,p i-1 ,p i ,p i+1 ,...,p i+k}. Based on the local neighborhood N i , the distribution characteristics of the point are analyzed by constructing the covariance matrix Σ i :

[0018]

[0019] Where p j is the point cloud in the neighborhood of p i , μ i represents the centroid of the neighborhood N i , (·) T represents the matrix transpose operation, and further eigenvalue decomposition is performed on Σ i to obtain three eigenvalues λ1>λ2>λ3 and their corresponding eigenvectors Two key curvature features are defined: the maximum principal curvature κ max and the minimum principal curvature κ min , which represent the maximum and minimum bending degree of the local surface of the point cloud, respectively:

[0020]

[0021] Based on the curvature, the points in the point cloud are divided into two categories by setting a maximum principal curvature threshold: corner points and planar points; where the corner points are suitable for subsequent line feature fitting, and the planar points are suitable for plane feature fitting.

[0022] Step 3. Calculate the fitting quality factors of line features and plane features respectively, including fitting error, local curvature and spectral entropy

[0023] ① Factor 1: Fitting error. The fitting error of fitting line features and plane features is as follows:

[0024]

[0025] Where, parameter d is a constant value in the fitting line equation parameter, N l and N p are the number of point clouds fitting the current line and plane respectively, and are the point cloud coordinates fitting the current line and plane respectively, the line direction vector is and the plane normal vector is

[0026] ② Factor 2: Local curvature. The local curvature of fitting line features and plane features is as follows:

[0027]

[0028] Where, and are the eigenvalues of the covariance matrix of the fitted feature point cloud arranged from small to large.

[0029] ③ Factor 3: Spectral entropy. The spectral entropy of fitting line features and plane features is as follows:

[0030]

[0031] Where, and are the normalized eigenvalues of the line and plane in each direction respectively.

[0032] Step 4. Based on the Bayesian probability model, evaluate the reliability of geometric features and quantify their fitting quality

[0033] Based on the three fitting quality factors obtained in step 3, a unified reliability evaluation model needs to be constructed; according to the Bayesian theory, the posterior probability of the current model as a "reliable event" is:

[0034]

[0035] Assuming that the fitting error, local curvature, and spectral entropy are independent of each other under the condition of a given reliability, and assuming that the probability of each feature under the condition of reliability obeys an exponential distribution, ignoring the prior probability and the evidence term, the comprehensive reliability R of the straight line or plane feature line and R plane are respectively:

[0036]

[0037] Step 5. Based on the feature reliability weight, a weighted iterative closest point (ICP) registration optimization is performed

[0038] Based on the feature reliability model of step 4, the values of the reliability R of the straight line feature and the plane feature line and R plane are input into the weighted optimization module and are combined into R' k , (k = 1...N l +N p ); for a given source point cloud P = {P k}, (k = 1...N), and each feature Q = {Line i , Plane j} in the corresponding extracted local map, (i = 1...N l , j = 1...N p ), the distance from P to Q needs to be calculated respectively and is unified as d k , and the optimization objective function of the weighted ICP can be obtained:

[0039]

[0040] Step 6. Output the positioning and pose results

[0041] From the ICP optimization objective function of step 5, the least square method is used for solving, and a convergence condition in line with the expectation is designed, and then the pose estimation of the current frame can be obtained

[0042] The present application has the following beneficial effects:

[0043] The Lidar point cloud registration optimization method based on geometric feature reliability provided by the application, by constructing a multi-dimensional fitting quality factor including fitting error, local curvature and spectral entropy, and quantitatively evaluating the reliability of geometric features based on a Bayesian probability model, overcomes the problem that the traditional ICP algorithm is sensitive to unreliable features under the interference of dynamic objects or local geometric degradation in complex scenes, and significantly improves the robustness of feature matching; meanwhile, by introducing the feature reliability weight into the ICP optimization objective function to realize weighted registration, the interference of noise and outliers on pose estimation is effectively suppressed, and the robustness of the registration process is enhanced; finally, under the premise of ensuring the real-time performance of the algorithm, the positioning accuracy and stability of the laser radar in the feature degradation or high dynamic scene are significantly improved. BRIEF DESCRIPTION OF DRAWINGS

[0044] Figure 1 The implementation flowchart of the application is shown in the figure;

[0045] Figure 2 The visualization of the reliability of the planar feature and the linear feature of the point cloud fitting in the same frame scene is shown in the figure;

[0046] Figure 3 The three-dimensional positioning error comparison of the conventional PL-PP-ICP point cloud registration method, the algorithm of the application and the ablation algorithm is shown in the figure. DETAILED DESCRIPTION

[0047] The application will be further illustrated in combination with the drawings and the specific embodiments, and it should be understood that the following specific embodiments are only used to illustrate the application and not to limit the scope of the application.

[0048] As shown in the figure, the embodiment discloses a Lidar point cloud registration optimization method based on geometric feature reliability, and the specific steps are as follows:

[0049] Step 1. Preprocessing and initial alignment of the laser radar scanning point cloud

[0050] Firstly, the IMU and LiDAR data are synchronized through timestamp correction, the pose of each point in the point cloud is interpolated based on IMU pre-integration, and then a continuous trajectory model is constructed using the high-frequency motion measurement value of the IMU, and the motion distortion is eliminated according to the accurate timestamp of the laser point. At the same time, the point cloud is filtered and denoised and spatially down-sampled and compressed, and finally the scanning point cloud and the map point cloud are initially spatially aligned based on the IMU prior pose.

[0051] Step 2. Extracting geometric features based on the local curvature of the point cloud, and performing geometric feature matching

[0052] The principal component analysis method based on the neighborhood of the scan line is used to calculate the curvature feature, and for a point p i on a given scan line, k adjacent points before and after it are selected to form a local neighborhood Ni ={p i-k ,...,p i-1 ,p i ,p i+1 ,...,p i+k Based on local neighborhood N i By constructing the covariance matrix Σ i Let's analyze the distribution characteristics of the points:

[0053]

[0054] Where, p j For p i Point cloud within the neighborhood, μ i Represents the neighborhood N i The center of mass, (·) T This represents the matrix transpose operation, further applied to Σ i Eigenvalue decomposition yields three eigenvalues ​​λ1 > λ2 > λ3 and their corresponding eigenvectors. Define two key curvature features: the maximum principal curvature κ max and minimum principal curvature κ min These two parameters characterize the maximum and minimum curvature of the local surface of the point cloud, respectively:

[0055]

[0056] Based on the aforementioned curvature, points in the scanned point cloud are divided into two categories by setting a maximum principal curvature threshold: corner points and planar points. Corner points are suitable for subsequent linear feature fitting, while planar points are suitable for planar feature fitting.

[0057] Step 3. Calculate the fitting quality factors for linear and planar features respectively, including fitting error, local curvature, and spectral entropy.

[0058] ① Factor 1: Fitting error. The fitting error between the fitted linear features and the planar features is as follows:

[0059]

[0060] Where, parameter d is a constant value in the parameters of the fitted straight line equation, and N l With N p These represent the number of point clouds used to fit the current line and the plane, respectively. and These represent the point cloud coordinates of the fitted current line and the plane, respectively, with the line direction vector being... With the plane normal vector as

[0061] ② Factor 2: Local curvature. The local curvature of the fitted linear and planar features is as follows:

[0062]

[0063] in, and The eigenvalues ​​of the covariance matrix of the fitted feature point cloud are arranged in ascending order.

[0064] ③ Factor 3: Spectral Entropy. The spectral entropy of the fitted linear features and planar features is as follows:

[0065]

[0066] in, and These are the normalized eigenvalues ​​for each direction of the line and the plane, respectively.

[0067] Step 4. Evaluate the reliability of geometric features based on the Bayesian probability model and quantify their fitting quality.

[0068] Based on the three fitting quality factors obtained in step 3, a unified reliability assessment model needs to be constructed. According to Bayesian theory, the current model represents the posterior probability of a "reliable event":

[0069]

[0070] Assuming that fitting error, local curvature, and spectral entropy are independent under a given reliability condition, and further assuming that the probability of each feature under the reliability condition follows an exponential distribution, and ignoring prior probabilities and evidence terms, the comprehensive reliability R of linear or planar features is... line With R plane They are respectively:

[0071]

[0072] Step 5. Perform weighted iterative nearest neighbor (ICP) registration optimization based on feature reliability weights.

[0073] Based on the feature reliability model in step 4, the reliability values ​​R for linear features and planar features are... line With R plane Input weighted optimization module, merge into R' k ,(k=1...N l +N p For a given source point cloud P = {P} k}, (k=1...N), and the corresponding extracted features Q={Line i Plane j},(i=1...N l j=1...N p The distances from P to Q need to be calculated separately and then unified as d.k The optimization objective function for weighted ICP can be obtained as follows:

[0074]

[0075] Step 6. Output localization and orientation results

[0076] The pose estimate of the current frame can be obtained by solving the ICP optimization objective function from step 5 using the least squares method and designing convergence conditions that meet the expectations.

[0077] It should be noted that the above content merely illustrates the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. For those skilled in the art, various improvements and modifications can be made without departing from the principle of the present invention, and all such improvements and modifications fall within the scope of protection of the claims of the present invention.

Claims

1. A LiDAR point cloud registration optimization method based on geometric feature reliability, characterized in that, include: Step 1: Preprocess and initially align the LiDAR scan point cloud; Step 2: Extract geometric features based on the local curvature of the point cloud and perform geometric feature matching; Step 3: Calculate the fitting quality factors for linear features and planar features respectively, including fitting error, local curvature, and spectral entropy; Step 4: Evaluate the reliability of geometric features based on the Bayesian probability model and quantify their fitting quality; Step 5: Based on the feature reliability weights, perform weighted iterative nearest point registration optimization; Step 6: Output the localization and orientation results.

2. The LiDAR point cloud registration optimization method based on geometric feature reliability as described in claim 1, characterized in that, Step 1 describes the preprocessing and initial alignment of the lidar scan point cloud; First, IMU and LiDAR data synchronization is achieved through timestamp correction. Pose interpolation is performed on each point in the point cloud based on IMU pre-integration. Then, a continuous trajectory model is constructed using IMU high-frequency motion measurement values. Motion distortion is eliminated based on the precise timestamps of the laser points. At the same time, outlier filtering and noise reduction and spatial downsampling compression are performed on the point cloud. Finally, the scanned point cloud and the map point cloud are initially spatially aligned based on the IMU prior pose.

3. The LiDAR point cloud registration optimization method based on geometric feature reliability as described in claim 1, characterized in that, Step 2 involves extracting geometric features based on the local curvature of the point cloud and performing geometric feature matching. Curvature characteristics are calculated using principal component analysis based on the neighborhood of the scan line. For a given point p on the scan line... i Select the k nearest neighbors before and after it to form a local neighborhood N. i ={p i-k ,...,p i-1 ,p i ,p i+1 ,...,p i+k Based on local neighborhood N i By constructing the covariance matrix Σ i Let's analyze the distribution characteristics of the points: Where, p j For p i Point cloud within the neighborhood, μ i Represents the neighborhood N i The center of mass, (·) T This represents the matrix transpose operation, further applied to Σ i Eigenvalue decomposition yields three eigenvalues ​​λ1 > λ2 > λ3 and their corresponding eigenvectors. Define two key curvature features: the maximum principal curvature κ max and minimum principal curvature κ min These two parameters characterize the maximum and minimum curvature of the local surface of the point cloud, respectively: Based on the curvature mentioned above, the points in the scanned point cloud are divided into two categories by setting the maximum principal curvature threshold: corner points and planar points. Corner points are suitable for subsequent linear feature fitting, while planar points are suitable for planar feature fitting.

4. The LiDAR point cloud registration optimization method based on geometric feature reliability as described in claim 1, characterized in that, Step 3 involves calculating the fitting quality factors for linear and planar features, including fitting error, local curvature, and spectral entropy. ① Factor 1: Fitting error; The fitting error between the fitted linear features and the planar features is as follows: Where, parameter d is a constant value in the parameters of the fitted straight line equation, and N l With N p These represent the number of point clouds used to fit the current line and the plane, respectively. and These represent the point cloud coordinates of the fitted current line and the plane, respectively, with the line direction vector being... With the plane normal vector as ② Factor 2: Local curvature; The local curvature of the fitted linear and planar features is as follows: in, and The eigenvalues ​​of the covariance matrix of the fitted feature point cloud are arranged in ascending order. ③ Factor 3: Spectral entropy; The spectral entropy of the fitted linear features and planar features is as follows: in, and These are the normalized eigenvalues ​​for each direction of the line and the plane, respectively.

5. The LiDAR point cloud registration optimization method based on geometric feature reliability as described in claim 1, characterized in that, Step 4 describes the reliability of evaluating geometric features based on a Bayesian probability model and quantifying its fitting quality. Based on the three fitting quality factors obtained in step 3, a unified reliability assessment model needs to be constructed. According to Bayesian theory, the current model represents the posterior probability of a "reliable event": Assuming that fitting error, local curvature, and spectral entropy are independent under a given reliability condition, and further assuming that the probability of each feature under the reliability condition follows an exponential distribution, and ignoring prior probabilities and evidence terms, the comprehensive reliability R of linear or planar features is... line With R plane They are respectively:

6. The LiDAR point cloud registration optimization method based on geometric feature reliability as described in claim 1, characterized in that, Step 5 describes performing weighted iterative nearest-point registration optimization based on feature reliability weights; Based on the feature reliability model in step 4, the reliability values ​​R for linear features and planar features are... line With R plane Input weighted optimization module, merge into R' k ,(k=1...N l +N p For a given source point cloud P = {P} k }, (k=1...N), and the corresponding extracted features Q={Line i Plane j },(i=1...N l j=1...N p The distances from P to Q need to be calculated separately and then unified as d. k The optimization objective function for weighted ICP is obtained as follows:

7. The LiDAR point cloud registration optimization method based on geometric feature reliability as described in claim 1, characterized in that, The output positioning and orientation results are described in step 6; The ICP optimization objective function from step 5 is solved using the least squares method, and a convergence condition that meets the expectations is designed to obtain the pose estimate for the current frame.

Images