A point cloud registration method based on curvature density feature extraction

Abstract

The application discloses a point cloud registration method based on curvature density feature extraction, mainly including three processes of feature point extraction based on curvature density, calculation of an initial rigid body pose transformation matrix and accurate registration of the pose transformation matrix based on a Tukey robust function. The method calculates the feature vectors of a source point cloud and a target point cloud through a covariance matrix, calculates the initial rigid body pose transformation matrix, and can realize fast coarse matching of the source point cloud and the target point cloud. Through the Tukey robust function and the iteration optimization of the pose transformation matrix, accurate point cloud registration with high precision and fast robustness can be realized, the shortcoming that accurate registration is prone to local optimization is overcome, the efficiency and precision of point cloud registration are improved, and the efficiency and precision of mobile robot positioning are improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of point cloud registration, in particular to a point cloud registration method based on curvature density feature extraction. BACKGROUND

[0002] At present, the in-situ machining mode (the component is not moved and the robot is moved for machining) for the machining detection of large and complex components (such as aircraft skin, wind power blade and high-speed rail shell) can well overcome the problems of high machining cost and small machining range existing in the traditional machining mode. In the in-situ machining process, the positioning accuracy of the mobile robot is a crucial link.

[0003] The large and complex components generally have the characteristics of large curvature change and surface texture loss. The point cloud-based registration technology can solve the positioning problem of the robot, but the traditional feature extraction method has the problem of feature insensitivity for large and complex components. In addition, the traditional coarse registration algorithm (such as SAC-IA, NDT and K-4PCS) has the problems of fast calculation speed and poor registration accuracy, and the fine registration algorithm (such as ICP) has the problems of long iteration time and easy to fall into local optimum without initial value. SUMMARY

[0004] The purpose of the present application is to solve the problem of positioning accuracy error of the mobile robot in the in-situ machining detection process of the large component in the prior art, and provide a point cloud registration method based on curvature density feature extraction, which can effectively improve the efficiency, accuracy and robustness of point cloud registration and mobile robot positioning.

[0005] The technical scheme adopted to achieve the purpose of the present application is:

[0006] A point cloud registration method based on curvature density feature extraction comprises the following steps:

[0007] Step 1: obtaining a point cloud image of the component as a source point cloud set; converting a three-dimensional model of the component into point cloud data as a target point cloud set H ;

[0008] Step 2: performing noise reduction and filtering processing on the source point cloud set obtained in step 1, and then performing segmentation to obtain a source point cloud set G ;

[0009] Step 3: performing feature point cloud extraction on the source point cloud set in step 2 and the target point cloud set in step 1 based on a curvature density parameter algorithm, and then performing voxel downsampling to obtain a source point cloud set after feature point cloud extraction and a target point cloud set G ; H P Q ;

[0010] ​​Step 4: Calculate the source point cloud from Step 3. P center covariance matrix and target points cluster Q center covariance matrix and calculate and eigenvectors and The initial rigid body pose transformation matrix is ​​obtained. ;

[0011] Step 5, transform the initial rigid body pose matrix obtained in Step 4. Application to Source Cloud P Each source cloud in Searching for target points Q Zhongyu Yuandianyun Corresponding target point cloud By minimizing and corresponding target point cloud The distance between them makes the source point cloud With the corresponding target point cloud Alignment, completing the initial rigid pose transformation matrix. One iteration; for the initial rigid pose transformation matrix conduct k In the next iteration, the Tukey function is used to enhance the robustness of the registration process, resulting in the final optimized rigid pose transformation matrix. This final optimized rigid pose transformation matrix is ​​then applied to the source point cloud. P Each source cloud in , making the source point cloud To the greatest extent possible with the target point cloud coincide.

[0012] In the above technical solution, the noise reduction and filtering of the source point cloud data in step 2 is specifically as follows: Gaussian statistical filtering is used to determine whether the sampling point is a noise point by judging whether the point cloud density of the point cloud around the sampling point meets the set requirements.

[0013] In the above technical solution, the source point aggregation mentioned in step 3 G and the target point cloud in step 1 H The specific process of feature point cloud extraction is as follows: Calculate the source point cloud set. G any point in and target points cluster H any point in radius is k Neighboring points in the neighborhood and covariance matrix and And calculate the covariance matrix. and eigenvalues and According to eigenvalues and Calculate the variation factor of the surface in the neighborhood of the source point cloud. and the variation factor of the target point cloud neighborhood surface Construct source point cloud curvature density parameters and target point cloud curvature density parameters Calculate the average curvature density parameter of the source point cloud. and the average curvature density parameter of the target point cloud The source point cloud curvature density parameter Greater than the average The point cloud is selected as the feature point cloud of the source point cloud, and the curvature density parameter of the target point cloud is... Greater than the average The point cloud is selected as the feature point cloud of the target point cloud.

[0014] In the above technical solution, the Neighborhood and Covariance matrix of the neighborhood and The calculation formulas are as follows:

[0015] ;

[0016] In the formula, Source point gathering G Any source point cloud in The covariance matrix of the neighborhood Gathering at the target point H Any target point cloud The covariance matrix of the neighborhood k The neighborhood radius, Source Point Cloud The centroid coordinates of the neighborhood For target point cloud The centroid coordinates of the neighborhood.

[0017] In the above technical solution, the change factor of the source point cloud neighborhood surface and the variation factor of the target point cloud neighborhood surface The calculation formula is:

[0018] ;

[0019] In the formula, Source Point Cloud Eigenvalues of the covariance matrix of the neighborhood Eigenvalues of the covariance matrix of the neighborhood Eigenvalues of the covariance matrix of the neighborhood Eigenvalues of the covariance matrix of the neighborhood Eigenvalues of the covariance matrix of the neighborhood

[0020] In the above technical solution, the calculation formula of the source point cloud curvature density parameter and the target point cloud curvature density parameter is:

[0021] ;

[0022] In the formula, m is the number of neighborhood points of the source point cloud is the change factor of the neighborhood surface of the source point cloud is the number of neighborhood points of the target point cloud is the change factor of the neighborhood surface of the target point cloud n The calculation formula of the average curvature density parameter of the source point cloud and the average curvature density parameter of the target point cloud

[0023] is:

[0024] ;

[0025] In the formula, is the average curvature density parameter of the source point cloud, is the average curvature density parameter of the target point cloud, wherein, m is the number of neighborhood points of the source point cloud is the change factor of the neighborhood surface of the source point cloud is the number of neighborhood points of the target point cloud is the change factor of the neighborhood surface of the target point cloud n The calculation formula of the initial rigid body pose transformation matrix in step 4 is:

[0026]

[0027] ;

[0028] In the formula, R is a rotation matrix, t is a translation vector.

[0029] In the above technical solution, the source point cloud set P ​​​​​​​​center of the source point cloud set The calculation formula of the center of the target point cloud set

[0030] ;

[0031] In the formula, is the i th source point cloud in the source point cloud set P ; i is the number of source point clouds in the source point cloud set N ; P The calculation formula of the covariance matrix of the center of the source point cloud set

[0032] Q The calculation formula of the covariance matrix of the center of the target point cloud set

[0033] ;

[0034] In the formula, is the i th target point cloud in the target point cloud set Q ; j is the number of target point clouds in the target point cloud set M ; Q The calculation formula of the covariance matrix of the center of the source point cloud set

[0035] P The calculation formula of the covariance matrix of the center of the target point cloud set Q ; In the formula,

[0036] is the iteration number, is the distance between the source point cloud after the i th iteration and the corresponding target point cloud, is the Tukey function,

[0037] is the indicator function of the special orthogonal group ;

[0038] ;

[0039] In the formula, k is the iteration number, is the distance between the source point cloud after the i th iteration and the corresponding target point cloud, k is the Tukey function, is the indicator function of the special orthogonal group ; L The expression of the Tukey function is as follows:

[0040]

[0041] ; ​​​​​​​

[0042] wherein, c is a cutoff point, e is an error value, when the error e is greater than the cutoff point c , the loss reaches a maximum value and remains unchanged.

[0043] Compared with the prior art, the present application has the following advantages:

[0044] The point cloud registration method of the present application mainly includes three processes: feature point extraction based on curvature density, calculation of initial rigid body pose transformation matrix, and fine registration of pose transformation matrix based on Tukey robust function. The method calculates the feature vectors of the source point cloud and the target point cloud through the covariance matrix, calculates the initial rigid body pose transformation matrix, and can realize fast and coarse matching of the source point cloud and the target point cloud; through the Tukey robust function combined with the nearest point iteration optimization of the pose transformation matrix, high-precision and fast-robust fine point cloud registration can be realized, which overcomes the shortcoming of easy falling into local optimum in accurate registration, improves the efficiency and accuracy of point cloud registration, and further improves the efficiency and accuracy of mobile robot positioning. BRIEF DESCRIPTION OF DRAWINGS

[0045] Figure 1 The flowchart of the point cloud registration method based on curvature density feature extraction of the present application is shown.

[0046] Figure 2 The overall structure of the mobile robot is shown.

[0047] Figure 3 The three-dimensional model of the component is shown.

[0048] Figure 4 The profile point cloud diagram of the component is shown.

[0049] Figure 5 The feature point diagram based on curvature density parameter extraction of the present application is shown.

[0050] Figure 6 The process diagram of voxel downsampling of the present application is shown.

[0051] Figure 7 The feature point diagram after voxel downsampling of the present application is shown.

[0052] Figure 8 The coarse registration flowchart in the present application is shown.

[0053] Figure 9 The fine registration flowchart in the present application is shown.

[0054] Figure 10 The Tukey function image in the present application is shown.

[0055] Figure 11 The initial position of the point cloud of the application is shown.

[0056] Figure 12 The schematic diagram of the point cloud after fine registration of the application is shown. DETAILED DESCRIPTION

[0057] The application will be further described in conjunction with specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the application and not to limit the application.

[0058] Referring to Figure 1 , a point cloud registration method based on curvature density feature extraction is divided into three processes of feature point extraction, coarse registration and fine registration, which specifically includes the following steps:

[0059] Step 1, referring to Figure 2 , the mobile robot is divided into two parts of a mobile chassis and a six-axis robot arm as a whole, and an RGB-D camera is used to obtain the point cloud image of the component in real time as the source point cloud; referring to Figure 3 , the three-dimensional model of the component is converted into point cloud data as the target point cloud set H . The initial position of the source point cloud and the target point cloud is shown in Figure 11 .

[0060] Step 2, the source point cloud data obtained in step 1 is preprocessed, including noise reduction and filtering of the source point cloud, and Gaussian statistical filtering is used to determine whether the point cloud density around the sampling point meets the set requirements (here, the point cloud around the sampling point refers to the point cloud formed by the sampling point and the points within a certain range around it) to determine whether the sampling point is a noise point. Preferably, if the point cloud density around the sampling point is significantly smaller than the overall point cloud density (here, significantly smaller means that the point cloud density around the sampling point is smaller than the overall point cloud density, and the absolute value of the difference between the two is greater than the set deviation threshold), the sampling point is considered to be a noise point. The source point cloud after noise reduction and filtering is segmented by using CloudCompare software, and the component surface point cloud obtained after removing the background is taken as the source point cloud set G , as shown in Figure 4 .

[0061] Step 3, the source point cloud set G in step 2 and the target point cloud set H in step 1 are read by using VScode software, and the feature points of the source point cloud set G and the target point cloud set H are extracted based on the curvature density parameter algorithm, and the source point cloud set and the target point cloud set after extracting the feature points are down-sampled by voxels to reduce the number of point clouds. The specific process is as follows:

[0062] For the source point cloud collection G and target points cluster H Any source point cloud in and any target point cloud ,by k The neighboring points in the neighborhood with radius are respectively denoted as and ,but Covariance matrix of the neighborhood and Covariance matrix of the neighborhood The calculation formula is:

[0063] ;

[0064] In the formula, Source point gathering G Any source point cloud in The covariance matrix of the neighborhood Gathering at the target point H Any target point cloud The covariance matrix of the neighborhood k The neighborhood radius, Source Point Cloud The centroid coordinates of the neighborhood For target point cloud The centroid coordinates of the neighborhood.

[0065] According to the source cloud Covariance matrix of the neighborhood eigenvalues and target point cloud Covariance matrix of the neighborhood Computational source point cloud Change factor of neighborhood surface and target point cloud Change factor of neighborhood surface The calculation formula is:

[0066] ;

[0067] In the formula, Source Point Cloud Covariance matrix of the neighborhood eigenvalues, For target point cloud Covariance matrix of the neighborhood eigenvalues.

[0068] Curvature density parameters for constructing the source point cloud curvature density parameters of the target point cloud To quantify the relationship between the surface undulations of a point cloud and its density, the following definition is used:

[0069] ;

[0070] In the formula, m Source Point Cloud The number of neighboring points, Source Point Cloud The variation factor of the neighborhood surface; n For target point cloud The number of neighboring points, For target point cloud The variation factor of the neighborhood surface.

[0071] Calculate the average curvature density parameter of the source point cloud respectively. and the average curvature density parameter of the target point cloud The formula is as follows:

[0072] ;

[0073] Gathering source points G Medium curvature density parameter Greater than the average The source point cloud is selected as the feature point cloud of the source point cloud set. The target point cloud set... H Medium curvature density parameter Greater than the average The target point cloud is selected as the feature point cloud of the target point cloud set. A schematic diagram of the extracted feature point cloud is shown below. Figure 5 As shown.

[0074] Due to the large number of extracted feature point clouds, voxel downsampling is required for both the source and target point cloud sets. First, the space containing the source and target point clouds is divided into a finite number of voxel grids. Second, the source and target point clouds closest to the centroid of each voxel grid are used to replace all other source and target point clouds within a voxel. This voxel downsampling yields the source point cloud set. and target points cluster Voxel downsampling can effectively reduce point cloud data while preserving the shape features of the point cloud, thus improving the efficiency of subsequent point cloud operations. The voxel downsampling process is as follows: Figure 6 As shown, the results of component voxel downsampling are as follows: Figure 7 As shown.

[0075] Step 4, refer to Figure 8 For the source point cloud obtained in step 4 and target points cluster Calculate the initial rigid body pose transformation matrix between the two. The initial rigid body pose transformation matrix is ​​used. It enables fast coarse matching between source and target point clouds. It calculates the initial rigid body pose transformation matrix. The specific process is as follows.

[0076] Computational source cloud center and target points cluster center The calculation formula is as follows:

[0077] ;

[0078] In the formula, Source point gathering P The first in i Individual point clouds, N Source point gathering P The number of source point clouds in the data; Gathering at the target point Q The first in j A target point cloud, M Gathering at the target point Q The number of target point clouds in the target cloud.

[0079] Calculate the source point cloud separately P center covariance matrix and target points cluster Q center covariance matrix :

[0080] ;

[0081] right and Singular Value Decomposition (SVD) is performed to obtain and eigenvectors and Source point gathering P and target points cluster Q The main direction.

[0082] Calculate the initial rigid pose transformation matrix ,in, R It is a rotation matrix. t It is a translation vector:

[0083] ;

[0084] In the formula, R Let be a rotation matrix. t It is a translation vector. Source point gathering Pcenter covariance matrix eigenvectors, Gathering at the target point Q center covariance matrix eigenvectors.

[0085] Step 5: Iterate and optimize the initial rigid body pose transformation matrix obtained in Step 4 to obtain the final accurate rigid body pose transformation matrix, thereby achieving precise registration between the source point cloud and the target point cloud. (See appendix) Figure 9 The specific process is as follows.

[0086] Find each source cloud in the source cloud set The corresponding target point cloud in the target point cloud set The initial rigid pose transformation matrix obtained in step 4 is used as the initial rigid pose transformation matrix. Effect on source point cloud cluster Each source cloud in Obtain the transformed source point cloud Target points are concentrated Q Neutralization and transformation of the source point cloud Minimum distance target point cloud That is, the source point cloud The corresponding target point cloud, source point cloud Corresponding target point cloud The minimum distance between them is The calculation formula is: ;

[0087] In the formula, The initial rigid pose transformation matrix The transformed source point cloud, To be compatible with Source Cloud The corresponding target point cloud.

[0088] Source cloud With the corresponding target point cloud Alignment: By minimizing and corresponding points The distance between them (Euclidean distance) yields the updated rigid pose transformation matrix. ; make the source point cloud With the corresponding target point cloud , The calculation formula is as follows:

[0089] ;

[0090] In the formula, The initial rigid pose transformation matrix Transformed source point cloud To the corresponding target point cloud distance, It is a special orthogonal group Indicator functions, R Let be a rotation matrix. t It is a translation vector.

[0091] The The calculation formula is:

[0092] ;

[0093] In the formula, when the rotation matrix R Left multiplication by the transpose of a rotation matrix The value of the above expression is 0 when the matrix is ​​an identity matrix and its determinant is 1; otherwise, it is 0. .

[0094] The process of finding corresponding points and aligning is iterated. To accelerate the computation and improve efficiency, Anderson acceleration is used to speed up the convergence of the function iteration. The initial rigid body pose transformation matrix is ​​also optimized. Update and obtain iterations k The subsequent rigid pose transformation matrix :

[0095] ;

[0096] In the formula, k For the number of iterations, For iteration k The subsequent source cloud Gather at the target point Q The corresponding target point cloud The distance.

[0097] To enhance the robustness of the fine registration process, the rigid pose transformation matrix is ​​based on the Tukey function. Optimization is performed to obtain the final optimized rigid pose transformation matrix. The calculation formula is:

[0098] ;

[0099] In the formula, L This refers to the Tukey function.

[0100] The Tukey function L expression for:

[0101] ;

[0102] In the formula, cis a cut-off point, determining when the loss function reaches the maximum value, e is an error value. When the error value e is greater than the cut-off point c , the loss reaches the maximum value and remains unchanged, which can effectively avoid the influence of outliers on point cloud registration. Figure 10 is when c the commonly used values 3-6 are taken, the images of Tukey function are shown in the following table, the horizontal coordinate represents the error value e , and the vertical coordinate represents the loss value of Tukey function.

[0103] The final rigid pose transformation matrix is applied to each of the source point clouds P in the source point cloud , so that each source point cloud is maximally coincident with the corresponding target point cloud , that is, the fine registration of the source point cloud and the target point cloud is completed. The fine registration result of the source point cloud and the target point cloud is shown in Figure 12 .

[0104] The above only describes the preferred embodiments of the present application, and it should be noted that for those skilled in the art, without departing from the principles of the present application, a number of improvements and refinements can be made, and these improvements and refinements should also be considered as the protection scope of the present application.

Claims

1. A point cloud registration method based on curvature density feature extraction, characterized in that, The method comprises the following steps: Step 1, acquire the point cloud image of the component as a source point cloud set; convert the three-dimensional model of the component into point cloud data as a target point cloud set H ; Step 2, after noise reduction and filtering processing on the source point cloud set obtained in step 1, segmentation is performed to obtain a source point cloud set G ; Step 3, feature point cloud extraction based on curvature density parameter algorithm on the source point cloud set in step 2 G and the target point cloud set in step 1 H Step 4, voxel down-sampling on the source point cloud set after feature point cloud extraction P and the target point cloud set Q ; Step 4: Calculate the source point cloud from Step 3. P center covariance matrix and target points cluster Q center covariance matrix and calculate and eigenvectors and The initial rigid body pose transformation matrix is ​​obtained. ; Step 5, transform the initial rigid body pose matrix obtained in Step 4. Application to Source Cloud P Each source cloud in Searching for target points Q Zhongyu Yuandianyun Corresponding target point cloud By minimizing and corresponding target point cloud The distance between them makes the source point cloud With the corresponding target point cloud Alignment, completing the initial rigid pose transformation matrix. One iteration; for the initial rigid pose transformation matrix conduct k In the next iteration, the Tukey function is used to enhance the robustness of the registration process, resulting in the final optimized rigid pose transformation matrix. This final optimized rigid pose transformation matrix is ​​then applied to the source point cloud. P Each source cloud in , making the source point cloud To the greatest extent possible with the target point cloud coincide; The final rigid pose transformation matrix in step 5 The formula for calculating is: ； wherein k is the iteration number, is the iteration k source point cloud and the corresponding target point cloud distance, L is the Tukey function, is the indicator function of the special orthogonal group is the indicator function of the special orthogonal group The expression of the Tukey function is: ； wherein c is the cutoff point, e is the error value, when the error e is greater than the cutoff point c the loss reaches a maximum value and remains constant.

2. The curvature density feature based point cloud registration method of claim 1, wherein, The noise reduction and filtering processing of the source point cloud set in step 2 is specifically: Gaussian statistical filtering is adopted; whether the point cloud density of the point cloud around the sampling point meets the set requirement is judged to determine whether the sampling point is a noise point.

3. The curvature density feature extraction based point cloud registration method of claim 1, wherein, The source point cloud set mentioned in step 3 G and the target point cloud in step 1 H The specific process of feature point cloud extraction is as follows: Calculate the source point cloud set. G any point in and target points cluster H any point in radius is k Neighboring points in the neighborhood and covariance matrix and And calculate the covariance matrix. and eigenvalues and According to eigenvalues and Calculate the variation factor of the surface in the neighborhood of the source point cloud. and the variation factor of the target point cloud neighborhood surface Construct source point cloud curvature density parameters and target point cloud curvature density parameters Calculate the average curvature density parameter of the source point cloud. and the average curvature density parameter of the target point cloud The source point cloud curvature density parameter Greater than the average The point cloud is selected as the feature point cloud of the source point cloud, and the curvature density parameter of the target point cloud is... Greater than the average The point cloud is selected as the feature point cloud of the target point cloud.

4. The curvature density feature based point cloud registration method of claim 3, wherein, The Neighborhood and Covariance matrix of neighborhood And The calculation formula is respectively: ； wherein is a source point cloud set G is any source point cloud in is a covariance matrix of the neighborhood, is a target point cloud set H is any target point cloud in is a covariance matrix of the neighborhood, k is a neighborhood radius, is a source point cloud is a centroid coordinate of the neighborhood, is a target point cloud is a centroid coordinate of the neighborhood.

5. The curvature density feature based point cloud registration method of claim 3, wherein, a change factor of the source point cloud neighborhood surface a change factor of the target point cloud neighborhood surface The calculation formula is: ； wherein is the source point cloud is the covariance matrix of the neighborhood of is the eigenvalue of is the target point cloud is the covariance matrix of the neighborhood of is the eigenvalue of 6. The curvature density feature based point cloud registration method of claim 3, wherein, The source point cloud curvature density parameter And the target point cloud curvature density parameter The calculation formula is: ； wherein m is the source point cloud is the number of neighboring points, is the source point cloud is the change factor of the neighboring surface; n is the target point cloud is the number of neighboring points, is the target point cloud is the change factor of the neighboring surface; The average curvature density parameter of the source point cloud The average curvature density parameter of the target point cloud The calculation formula is: ； wherein, is the average curvature density parameter of the source point cloud, is the average curvature density parameter of the target point cloud, wherein, m is the source point cloud the number of neighborhood points, is the source point cloud the change factor of the neighborhood surface; n is the target point cloud the number of neighborhood points, is the target point cloud the change factor of the neighborhood surface.

7. The curvature density feature extraction based point cloud registration method of claim 1, wherein, The initial rigid body pose transformation matrix described in step 4 The formula for calculating is: ； wherein R is a rotation matrix, t is a translation vector.

8. The curvature density feature based point cloud registration method of claim 6, wherein, The source point cloud set P The center of the circle The calculation formula is: ； In the formula, is a source point cloud set P is a source point cloud i in the source point cloud set N is a source point cloud P in the source point cloud set The target point cloud set Q The center The calculation formula is: ； In the formula, target point cloud set Q target point cloud set j target point cloud set M target point cloud set Q target point cloud set The source point cloud set P The center of the source point cloud set The covariance matrix of the source point cloud set The target point cloud set Q The center of the target point cloud set The covariance matrix of the target point cloud set The calculation formula is: 。

Images