T-shaped shaft bracket base point cloud registration method based on improved ICP (Inductively Coupled Plasma)

Abstract

The invention discloses a T-shaped shaft support base point cloud registration method based on an improved ICP, and belongs to the technical field of robot industrial application, and the method comprises the following steps: S1, carrying out the preprocessing of a segmented part source point cloud and a target point cloud through Voxel Grid filtering, and obtaining a downsampling point cloud; s2, calculating SHOT features of the downsampled part source point cloud and target point cloud; s3, extracting corresponding feature point pairs in the source point cloud and the target point cloud based on SHOT features, and registering the source point cloud and the target point cloud by using an SAC-IA algorithm to obtain a coarse registration result; and S4, based on a coarse registration result, using a GICP fine registration algorithm to further perform registration, solving an optimal transformation matrix, and using the optimal transformation matrix to update the position of the source point cloud. According to the method, the defects of high requirement on the initial position of the point cloud and low convergence speed of the existing ICP algorithm can be solved.

Description

Technical Field

[0001] The invention relates to the technical field of industrial application of robots, and in particular to a point cloud registration method of a T-axis bracket base based on improved ICP. Background Art

[0002] The grasping of the base of the T-axis bracket is one of the very important contents in the field of industrial application of robots. It can improve industrial production efficiency and reduce labor costs to a certain extent. The base of the T-axis bracket is usually placed in a material frame, and the parts are stacked in disorder. Such placement conditions make it impossible for the robot to always grasp the parts in a fixed posture. Therefore, how to obtain the posture of the parts is the key to the entire part grasping process. Through point cloud registration, the posture of the segmented parts can be obtained by aligning the segmented parts with the part template. How to align stably and quickly is of great significance to the entire grasping task.

[0003] The most widely used algorithm for point cloud registration is the Iterative Closest Point (ICP) algorithm, which is mainly used to accurately align two sets of point cloud data acquired from different perspectives or at different times, and find an optimal rotation and translation transformation matrix so that the source point cloud overlaps with the target point cloud as much as possible after the transformation.

[0004] However, the traditional ICP algorithm has the problems of long matching time in the point cloud registration of the T-axis bracket base, and is easily affected by the initial value, which leads to the problems of low positioning accuracy and poor robustness in the application of the algorithm in the point cloud registration of the T-axis bracket base.

[0005] The information disclosed in this background technology section is only intended to enhance the understanding of the overall background of the invention and should not be regarded as an acknowledgment or any form of suggestion that the information constitutes the prior art already known to a person skilled in the art. Summary of the invention

[0006] The purpose of the present invention is to provide a T-axis bracket base point cloud registration method based on improved ICP, which can solve the shortcomings of the existing ICP algorithm, such as high requirements on the initial position of the point cloud and slow convergence speed.

[0007] To achieve the above object, the present invention provides a T-axis bracket base point cloud registration method based on improved ICP, comprising the following steps:

[0008] S1: Preprocess the segmented part source point cloud and target point cloud through Voxel Grid filtering to obtain downsampled point cloud;

[0009] S2: Calculate the SHOT features of the downsampled part source point cloud and target point cloud;

[0010] S3: Extract the corresponding feature point pairs in the source point cloud and the target point cloud based on the SHOT feature, and use the SAC-IA algorithm to align the source point cloud and the target point cloud to obtain a rough registration result;

[0011] S4: Based on the results of the rough registration, the GICP fine registration algorithm is used for further registration to solve the optimal transformation matrix, and the position of the source point cloud is updated using the optimal transformation matrix.

[0012] In one embodiment of the present invention, in step S1, the operation of preprocessing by Voxel Grid filtering includes the following steps:

[0013] S101: specify the side length of each voxel;

[0014] S102: Map each point in the point cloud to a corresponding voxel according to the size of the voxel;

[0015] S103: For each voxel, use the centroid of all points in the voxel as a representative point;

[0016] S104: Output the downsampled point cloud.

[0017] In one embodiment of the present invention, only the representative point of each voxel is retained in the downsampled point cloud, and other points are discarded.

[0018] In one embodiment of the present invention, the specific operation of calculating the SHOT feature in step S2 includes the following steps:

[0019] S201: Construct a local reference coordinate system and convert the query point p i All the domain points within a certain radius of the center are set as the domain point set N(p i ), and construct a local reference system based on the covariance matrix C;

[0020] S202: For each point q in the neighborhood j , calculate the projection angle of its normal vector on the local reference system;

[0021] S203: At each query point p i In the sub-regions around , the distribution of the normal vector direction of each point is counted and quantified to construct a histogram;

[0022] S204: Connect the histograms of all sub-regions together to obtain a SHOT feature descriptor.

[0023] In one embodiment of the present invention, in step S204, for each point in each sub-region, the cosine value cosθ of the angle θ between the point and the normal line of the origin of the local reference coordinate system is calculated, and then the cosθ values ​​of the points in each sub-region are histogrammed.

[0024] In one embodiment of the present invention, in step S3, the following objective function is minimized and the rigid transformation T(R,t) is estimated using the RANSAC algorithm:

[0025]

[0026] Among them: C' is the corresponding point set obtained by feature matching; p i ∈S and q j ∈T is a pair of feature points in two point clouds; T * is the optimized rigid transformation; R is the rotation matrix between the source point cloud and the target point cloud, and t is the translation vector between the source point cloud and the target point cloud.

[0027] In one embodiment of the present invention, step S3 includes the following sub-steps:

[0028] S301: Based on the SHOT feature descriptor, find corresponding feature point pairs in the part source point cloud and the target point cloud;

[0029] S302: Iterative optimization is performed using the RANSAC method: a number of point pairs are randomly selected from the feature point pairs, the rigid transformation between them is calculated, and the transformation that makes the maximum number of point pairs consistent is selected;

[0030] S303: When a preset number of iterations is reached or the number of point pairs that meet the distance threshold reaches a preset threshold, the iteration is stopped, and a rigid transformation that makes the most point pairs consistent is selected as the initial transformation to obtain a rough registration result.

[0031] In one embodiment of the present invention, step S4 specifically includes the following steps:

[0032] S401: For each pair of nearest neighbor points, define the following error function to minimize the registration error of this pair of points:

[0033]

[0034] Where: p i is a point in the target point cloud; q j is a point in the source point cloud; T is the transformation matrix to be optimized; and They are point p i and q j The covariance matrix of

[0035] S402: solving an optimal transformation matrix T, where the optimal transformation matrix refers to a transformation matrix that can minimize the distance or error between the source point cloud and the target point cloud;

[0036] S403: Use the optimized transformation matrix T to update the position of the source point cloud.

[0037] In one embodiment of the present invention, in step S403, matrix multiplication is performed by the transformation matrix T and the pose matrix of the source point cloud to obtain an updated source point cloud pose, and the nearest neighbor search, error calculation and optimization steps are repeated until the registration error converges.

[0038] In one embodiment of the present invention, the conditions for convergence of the registration error are: the mean square error of the registration reaches a certain absolute threshold; the relative rate of change of the error between two iterations is less than a certain small threshold; the maximum number of iterations is set; the distance between the average point pairs of the source point cloud and the target point cloud is less than the threshold.

[0039] Compared with the prior art, a point cloud registration method for a T-axis bracket base based on improved ICP according to the present invention is based on the SHOT feature, first using SAC-IA coarse registration for positioning, and then using the ICP algorithm for fine matching. Compared with the traditional point cloud registration algorithm, the present invention greatly improves the registration speed and accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 It is a schematic flow chart of a point cloud registration method for a T-axis support base based on improved ICP according to an embodiment of the present invention;

[0041] Figure 2 It is a schematic diagram of the final registration effect according to an embodiment of the present invention. DETAILED DESCRIPTION

[0042] The specific implementation modes of the present invention are described in detail below in conjunction with the accompanying drawings, but it should be understood that the protection scope of the present invention is not limited by the specific implementation modes.

[0043] Unless explicitly stated otherwise, throughout the specification and claims, the term “comprise” or variations such as “include” or “comprising”, etc., will be understood to include the stated elements or components but not to exclude other elements or components.

[0044] like Figure 1 to Figure 2 As shown, according to a preferred embodiment of the present invention, a T-axis support base point cloud registration method based on improved ICP includes the following steps:

[0045] S1: Preprocess the segmented part source point cloud S and target point cloud T through Voxel Grid filtering to obtain downsampled point clouds to speed up subsequent operations. The target point cloud T is the template point cloud for registration after manually removing noise from the point cloud of the part captured by the camera; the source point cloud S is the part point cloud segmented by the segmentation algorithm and registered to the target point cloud T to obtain the point cloud with posture transformation relative to the target point cloud T.

[0046] Voxel Grid filtering is a downsampling method that divides the three-dimensional space into small voxels. For each voxel, a representative point is selected according to certain rules (such as taking the center of gravity of all points in the voxel, randomly selecting a point, etc.), thereby achieving downsampling of the point cloud; its main purpose is to reduce the number of points in the point cloud data while retaining the main features of the point cloud.

[0047] Specifically, in step S1, the operation of preprocessing by Voxel Grid filtering includes the following sub-steps:

[0048] S101: Specify the side length of each voxel LeafSize = [L x ,L y ,L z ], where L x , L y and L z They represent the voxel size in the x, y and z directions respectively, and are generally set to 0.01m; according to the boundary size of the point cloud, the entire point cloud space is divided into voxel grids.

[0049] S102: Map each point in the point cloud to a corresponding voxel according to the size of the voxel.

[0050] Specifically, for a point (x i ,y i ,z i ), the voxel coordinates of the point can be calculated as:

[0051]

[0052]

[0053] in, Indicates a floor operation.

[0054] S103: For each voxel, use the centroid of all points in the voxel as a representative point.

[0055] The calculation formula of the centroid coordinates is:

[0056]

[0057] Where n is the number of points contained in a voxel, (x i ,y i ,z i ) are the coordinates of each point within the voxel.

[0058] S104: Outputting a downsampled point cloud, wherein only a representative point of each voxel is retained in the downsampled point cloud, and other points are discarded.

[0059] S2: Calculate the SHOT features of the downsampled part source point cloud S and target point cloud T.

[0060] Among them, SHOT (Signature of Histogram of Orientation) is a local feature descriptor for three-dimensional point clouds. It establishes a local coordinate system at the feature points and combines the spatial position information of the neighborhood points and the geometric feature statistical information to describe the feature points. It can effectively describe the local geometric shape of the point cloud and has rotation invariance and certain scale invariance.

[0061] Using SHOT features can further improve the initial alignment of point clouds and improve the accuracy and efficiency of the overall registration.

[0062] Specifically, the specific operation of calculating the SHOT feature in step S2 includes the following steps:

[0063] S201: Construct a local reference coordinate system and convert the query point p i All the domain points within a certain radius r as the center are set as the domain point set N(p i ), and construct the local reference system (e1, e2, e3) according to the covariance matrix C.

[0064] The value of the radius r is an empirical value and needs to be adjusted according to the specific situation. The most suitable value can be found through experiments and optimization. In the present invention, the radius r is generally selected to be 0.05m.

[0065] The calculation process of the covariance matrix C is as follows:

[0066]

[0067] Among them, q j is a point in the domain point set, |N(p i )| is the number of points in the domain point set. The eigenvalues ​​and eigenvectors of the matrix C can be used to determine the local reference system.

[0068] In this step, the establishment of the local reference system (e1, e2, e3) depends on the geometric distribution of the surrounding neighborhood points, and the main change direction of the point cloud is obtained by eigenvalue decomposition of the covariance matrix.

[0069] By performing eigenvalue decomposition on the covariance matrix, three eigenvectors and corresponding eigenvalues ​​can be obtained. The eigenvectors represent the main change directions of the point cloud, while the eigenvalues ​​reflect the degree of change in these directions. The eigenvalues ​​are arranged in order from large to small, and the corresponding eigenvectors are recorded as e1, e2, and e3, respectively, to form a local reference system (e1, e2, e3).

[0070] S202: For each point q in the neighborhood j , calculate its normal vector Projection angle on the local reference system (e1, e2, e3):

[0071]

[0072]

[0073] S203: At each query point p i In the sub-regions around , the distribution of the normal vector direction of each point is counted and quantified to construct a histogram.

[0074] Among them, the sub-region is the query point p i The radius of the spherical area is an empirical value and needs to be adjusted according to the specific situation. The most suitable value can be found through experiments and optimization. In the present invention, the radius of the spherical area is generally selected as 0.05m.

[0075] Specifically, for each point in each sub-region, the cosine value cosθ of the angle θ between it and the normal of the origin of the local reference coordinate system (feature point) is calculated. This value can be calculated by the dot product of the normal vector of the point and the normal vector of the local coordinate system. Then, the cosθ values ​​of the points in each sub-region are histogram statistics.

[0076] S204: Connect the histograms of all sub-regions together to obtain the SHOT feature descriptor:

[0077] SHOT=[H1,H2,…,H n ]

[0078] Among them, H i is the direction histogram of the i-th sub-region.

[0079] The SHOT feature descriptor can effectively capture the local geometric shape information of the point cloud, providing a rich feature basis for the subsequent search for corresponding point pairs. The SHOT feature descriptor has rotation invariance and certain scale invariance, and can maintain relatively stable feature expression under different viewing angles and scales.

[0080] S3: Based on the SHOT feature, the corresponding feature point pairs in the source point cloud and the target point cloud are extracted, and the source point cloud and the target point cloud are registered using the SAC-IA algorithm to obtain a rough registration result.

[0081] The goal of the SAC-IA (Sampling Consistent Initial Alignment) algorithm is to find the rigid transformation T(R,t) between the source point cloud S and the target point cloud T of the part, where R is the rotation matrix between the source point cloud and the target point cloud, and t is the translation vector between the source point cloud and the target point cloud.

[0082] The rotation matrix R describes the rotation angle and direction between the source point cloud S and the target point cloud T. By determining the appropriate rotation matrix, the orientation of the source point cloud can be matched with that of the target point cloud, thereby achieving preliminary alignment.

[0083] The translation vector t represents the displacement of the source point cloud S on the three coordinate axes relative to the target point cloud T. By adjusting the translation vector, the source point cloud can be moved to a position closer to the target point cloud, further improving the accuracy of the registration.

[0084] Specifically, the following objective function is minimized and the RANSAC algorithm is used to estimate the rigid transformation T(R,t):

[0085]

[0086] Among them: C' is the corresponding point set obtained by feature matching; p i ∈S and q j ∈T is a pair of feature points in two point clouds; T * is the optimized rigid transformation.

[0087] Specifically, the RANSAC algorithm randomly selects feature point pairs (p i ,q j ) to estimate the transformation and select the optimal T according to the number of point pairs that meet the transformation consistency condition * The transformation consistency condition refers to the criteria used to determine which data points conform to a particular geometric transformation model, such as the Euclidean distance described below.

[0088] Specifically, step S3 includes the following sub-steps:

[0089] S301: Based on the feature descriptor, find the corresponding feature point pairs in the part source point cloud S and the target point cloud T. The feature point pairs are generally determined by calculating the Euclidean distance between the two feature descriptors.

[0090] The Euclidean distance less than a certain threshold is determined as a feature point pair. The threshold can be determined by studying the statistical properties of the distance between data points. For example, it can be assumed that the distance between data points obeys a known probability distribution (such as normal distribution), and then the threshold is determined based on this distribution. This method can be implemented through mathematical modeling and statistical analysis, which provides a more scientific and systematic method to determine the threshold and reduces the influence of subjective factors.

[0091] S302: Iterative optimization is performed using the RANSAC method: a number of point pairs are randomly selected from the feature point pairs, rigid transformations (including rotation and translation) between them are calculated, and the transformation that makes the maximum number of point pairs consistent is selected.

[0092] S303: When a preset number of iterations is reached or the number of point pairs that meet the distance threshold reaches a preset threshold, the iteration is stopped, and a rigid transformation that makes the most point pairs consistent is selected as the initial transformation to obtain a rough registration result.

[0093] The values ​​of the number of iterations and the preset threshold are empirical values ​​and need to be adjusted according to the specific situation. The most suitable values ​​can be found through experiments and optimization. In the present invention, the number of iterations is generally selected as 500 times.

[0094] S4: Based on the results of the rough registration, the GICP fine registration algorithm is used for further registration to solve the optimal transformation matrix, and the position of the source point cloud is updated using the optimal transformation matrix.

[0095] Similar to traditional ICP, GICP (Generalized-ICP) also finds the nearest neighbor point in the target point cloud for each point in the source point cloud.

[0096] In GICP, the local geometric information of each point is described by a covariance matrix Σ. i , whose covariance matrix Σ i It is calculated by the geometric distribution of points in the neighborhood of this point:

[0097]

[0098] Where N is the number of neighborhood points, u is the mean of the neighborhood points, and p k is a point in the neighborhood.

[0099] In this step S4, the following steps are specifically included:

[0100] S401: For each pair of nearest neighbor points, define an error function to minimize the registration error of this pair of points. In GICP, the error depends not only on the Euclidean distance between points, but also on the local covariance matrix of each point. Therefore, the error function is:

[0101]

[0102] Where: p i is a point in the target point cloud; q j is a point in the source point cloud; T is the transformation matrix to be optimized; and They are point p i and q j The covariance matrix of .

[0103] The goal of this error function is to minimize the weighted Mahalanobis distance between the source and target points.

[0104] S402: Solving the optimal transformation matrix T, where the optimal transformation matrix refers to a transformation matrix that can minimize the distance (or error) between the source point cloud and the target point cloud.

[0105] S403: Update the position of the source point cloud using the optimized transformation matrix T. Specifically, perform matrix multiplication of the transformation matrix T and the pose matrix of the source point cloud to obtain the updated pose of the source point cloud, and repeat the nearest neighbor search, error calculation and optimization steps of steps S401 and S402 until the registration error converges.

[0106] The pose matrix is ​​a 4x4 matrix used to represent the position and pose of the point cloud.

[0107] Among them, the convergence of the registration error needs to be set manually. The following conditions can be set and the registration will stop when one of them is met: error threshold, that is, the mean square error of the registration reaches a certain absolute threshold; relative error change rate, that is, the relative change rate of the error between two iterations is less than a small threshold; iteration number limit, that is, setting a maximum number of iterations; average point pair distance, that is, the distance between the average point pairs of the source point cloud and the target point cloud is less than the threshold.

[0108] The above-mentioned conditions are all empirical values ​​and need to be adjusted according to specific circumstances. The most suitable values ​​can be found through experiments and optimization. In the present invention, the error threshold is 0.001, the number of iterations is limited to 500 times, and the average point distance is 100 mm.

[0109] The method of the present invention can significantly improve the speed and accuracy of point cloud registration, and has better registration accuracy and robustness for point clouds with fewer features such as a T-shaped axis bracket base.

[0110] like Figure 2The above is a schematic diagram of the final alignment effect, in which the T-shaped shaft bracket base can be cleanly and accurately matched.

[0111] The foregoing description of specific exemplary embodiments of the present invention is for the purpose of illustration and demonstration. These descriptions are not intended to limit the present invention to the precise form disclosed, and it is clear that many changes and variations can be made based on the above teachings. The purpose of selecting and describing the exemplary embodiments is to explain the specific principles of the present invention and its practical application, so that those skilled in the art can realize and utilize various different exemplary embodiments of the present invention and various different selections and changes. The scope of the present invention is intended to be limited by the claims and their equivalents.

Claims

1. A point cloud registration method for a T-axis bracket base based on improved ICP, characterized in that: The following steps are involved: S1: Preprocess the segmented part source point cloud and target point cloud through Voxel Grid filtering to obtain downsampled point cloud; S2: Calculate the SHOT features of the downsampled part source point cloud and target point cloud; S3: Extract the corresponding feature point pairs in the source point cloud and the target point cloud based on the SHOT feature, and use the SAC-IA algorithm to align the source point cloud and the target point cloud to obtain a rough registration result; S4: Based on the results of the rough registration, the GICP fine registration algorithm is used for further registration to solve the optimal transformation matrix, and the position of the source point cloud is updated using the optimal transformation matrix.

2. The T-axis support base point cloud registration method based on improved ICP as claimed in claim 1, characterized in that: In step S1, the operation of preprocessing by Voxel Grid filtering includes the following steps: S101: specify the side length of each voxel; S102: Map each point in the point cloud to a corresponding voxel according to the size of the voxel; S103: For each voxel, use the centroid of all points in the voxel as a representative point; S104: Output the downsampled point cloud.

3. The point cloud registration method for the base of a T-axis support based on improved ICP as claimed in claim 2, characterized in that: In the downsampled point cloud, only the representative point of each voxel is retained, and other points are discarded.

4. The point cloud registration method for a T-axis support base based on improved ICP as claimed in claim 2, characterized in that: The specific operation of calculating the SHOT feature in step S2 includes the following steps: S201: Construct a local reference coordinate system and convert the query point p i All the domain points within a certain radius of the center are set as the domain point set N(p i ), and construct a local reference system based on the covariance matrix C; S202: For each point q in the neighborhood j , calculate the projection angle of its normal vector on the local reference system; S203: At each query point p i In the sub-regions around , the distribution of the normal vector direction of each point is counted and quantified to construct a histogram; S204: Connect the histograms of all sub-regions together to obtain a SHOT feature descriptor.

5. The point cloud registration method for the base of a T-axis support based on improved ICP as claimed in claim 4, characterized in that: In step S204, for each point in each sub-region, the cosine value cosθ of the angle θ between the point and the normal line of the origin of the local reference coordinate system is calculated, and then the cosθ values ​​of the points in each sub-region are histogram-generated.

6. The point cloud registration method for a T-axis support base based on improved ICP according to claim 4, characterized in that: In step S3, the following objective function is minimized and the rigid transformation T(R,t) is estimated using the RANSAC algorithm: Among them: C' is the corresponding point set obtained by feature matching; p i ∈S and q j ∈T is a pair of feature points in two point clouds; T * is the optimized rigid transformation; R is the rotation matrix between the source point cloud and the target point cloud, and t is the translation vector between the source point cloud and the target point cloud.

7. The T-axis support base point cloud registration method based on improved ICP as claimed in claim 6, characterized in that: This step S3 includes the following sub-steps: S301: Based on the SHOT feature descriptor, find corresponding feature point pairs in the part source point cloud and the target point cloud; S302: Iterative optimization is performed using the RANSAC method: a number of point pairs are randomly selected from the feature point pairs, the rigid transformation between them is calculated, and the transformation that makes the maximum number of point pairs consistent is selected; S303: When a preset number of iterations is reached or the number of point pairs that meet the distance threshold reaches a preset threshold, the iteration is stopped, and a rigid transformation that makes the most point pairs consistent is selected as the initial transformation to obtain a rough registration result.

8. The point cloud registration method for a T-axis support base based on improved ICP according to claim 7, characterized in that: Step S4 specifically includes the following steps: S401: For each pair of nearest neighbor points, define the following error function to minimize the registration error of this pair of points: Where: p i is a point in the target point cloud; q j is a point in the source point cloud; T is the transformation matrix to be optimized; and They are point p i and q j The covariance matrix of S402: solving an optimal transformation matrix T, where the optimal transformation matrix refers to a transformation matrix that can minimize the distance or error between the source point cloud and the target point cloud; S403: Use the optimized transformation matrix T to update the position of the source point cloud.

9. The T-axis support base point cloud registration method based on improved ICP as claimed in claim 8, characterized in that: In step S403, matrix multiplication is performed by the transformation matrix T and the pose matrix of the source point cloud to obtain the updated source point cloud pose, and the nearest neighbor search, error calculation and optimization steps are repeated until the registration error converges.

10. The T-axis support base point cloud registration method based on improved ICP as claimed in claim 9, characterized in that: The conditions for the convergence of the registration error are: the mean square error of the registration reaches a certain absolute threshold; the relative change rate of the error between two iterations is less than a small threshold; the maximum number of iterations is set; the distance between the average point pairs of the source point cloud and the target point cloud is less than the threshold.

Images