A point cloud registration method combining key point information and its application

Abstract

The present invention discloses a point cloud registration method that combines key point information. Key points are pre-marked on a workpiece; a template workpiece point cloud is obtained, and the three-dimensional coordinates of the key points are calculated and stored in a template key point set; a measured workpiece point cloud is obtained, and the three-dimensional coordinates of the key points are calculated and stored in a measured key point set; a nearest neighbor search method is used to obtain a measured matching point set; an objective function is constructed using the template workpiece point cloud, the measured matching point set, the template workpiece point cloud, and the template key point set; and an optimization method is used to iteratively solve the objective function to derive a rotation matrix R and a translation matrix t, thereby completing the registration between the template workpiece point cloud and the measured workpiece point cloud. This method adds key point information constraints, resulting in more accurate registration results and effectively determining the rotation and translation values ​​of key points on the workpiece. This provides an effective basis for assembly trajectory correction and ensures smooth precision assembly.

Description

Technical Field

[0001] The present invention relates to the field of point cloud registration, and in particular to a point cloud registration method combining key point information and its application. Background Art

[0002] Point Cloud Registration refers to inputting two point clouds Ps (s represents source) and Pt (t represents target), and outputting a rotation and translation relationship so that the degree of overlap between Ps and Pt is as high as possible. At present, the traditional point cloud registration method is the ICP method (Iterative Closest Point). The core idea of ​​the ICP method is to solve the transformation based on the current closest point of Ps in Pt, and iterate repeatedly to obtain the optimal solution. At present, the ICP method only considers the alignment of the overall morphology of the workpiece, and cannot take into account whether specific key areas are aligned, which easily leads to inaccurate matching problems in local areas. In the field of automated assembly, it is necessary to ensure the accurate installation of the workpiece to be assembled (such as pin hole matching), that is, the key points of assembly (such as threaded holes, pin holes, screws, etc.) need to be paid attention to, and the accuracy of the registration results will directly affect the assembly results.

[0003] When a robot assembles workpieces in batches, it is typically first taught an assembly trajectory. This trajectory ensures accurate assembly of the first workpiece (the reference workpiece). However, in subsequent assembly applications, due to manufacturing deviations among multiple workpieces of the same model and variations in the placement of different workpieces, the assembly trajectory may not fully match all workpieces to be assembled. This means that the robot's trajectory is inaccurate when assembling the remaining workpieces. This requires obtaining the rotational-translational relationship between the workpiece to be assembled and the reference workpiece, using it to correct the assembly trajectory and ensure a smooth assembly process. However, the accuracy of the rotational-translational relationship obtained using existing ICP methods does not meet the assembly process requirements. Summary of the Invention

[0004] In order to solve the above technical problems, the present invention provides a point cloud registration method combined with key point information. On the basis of the existing ICP method, key point information constraints are added to obtain more accurate registration results, and the rotation and translation amounts of the key points on the workpiece are effectively obtained, which provides an effective basis for assembly trajectory correction and ensures the smooth progress of precise assembly.

[0005] To this end, the technical solutions of the present invention are as follows:

[0006] A point cloud registration method that combines key point information. Key points are pre-marked on a workpiece. The key points include holes, corners, cylindrical protrusions, edge midpoints, and plane centroids on the workpiece surface. When a key point is an edge midpoint, the direction of the edge coincides with any coordinate axis of the spatial coordinate system in which the point cloud resides, and the edge midpoint is the geometric center of the edge. When a key point is a plane centroid, the normal vector of the plane in which it resides coincides with any coordinate axis of the spatial coordinate system in which the point cloud resides.

[0007] Obtain the reference workpiece point cloud, select the point cloud of the area of ​​interest and store it as the template workpiece point cloud; in this point cloud, calculate the 3D coordinates of the key points and store them in the template key point set;

[0008] Use the following steps to perform point cloud registration:

[0009] 1) Obtain the actual scene point cloud, perform preprocessing, and record the preprocessed point cloud as the measured workpiece point cloud; in this point cloud, calculate the three-dimensional coordinates of the key points and store them in the measured key point set;

[0010] The measured workpiece point cloud includes a workpiece point cloud of the same model as the reference workpiece;

[0011] For the 3D points in the template workpiece point cloud, the nearest neighbor search method is used to find the points closest to it from the measured workpiece point cloud, and the found points are stored in the measured matching point set;

[0012] 2) Construct the objective function E(R,t):

[0013]

[0014] Among them, p i represents a point in the template workpiece point cloud, q i represents a point in the measured matching point set, k i Indicates q i The normal vector of the point, n represents the number of points in the template workpiece point cloud; h j Represents a point in the template key point set, g j represents a point in the measured key point concentration, m represents the number of key points; λ is the preset weight value; s i Indicates the monitoring direction of the key point, and its value is one of (1,0,0), (0,1,0), (0,0,1), (1,0,1), (0,1,1), (1,1,0), (1,1,1);

[0015] The optimization method is used to iteratively solve the objective function E(R,t) to obtain the rotation matrix R and translation matrix t, and complete the registration between the template workpiece point cloud and the measured workpiece point cloud.

[0016] Preferably, while acquiring the reference workpiece point cloud and the actual scene point cloud, a two-dimensional image is also acquired at the same location; there is a one-to-one correspondence between the pixels in the two-dimensional image and the three-dimensional points in the point cloud;

[0017] The two-dimensional image corresponding to the reference workpiece point cloud is recorded as the reference two-dimensional image; the two-dimensional image corresponding to the point cloud in the actual scene is recorded as the measured two-dimensional image;

[0018] In the reference two-dimensional image, the image of the area where the key points are manually selected is stored as image I; the three-dimensional points corresponding to each point in image I in the reference workpiece point cloud are stored in the candidate point set;

[0019] Use the following method to solve the three-dimensional coordinates of the key points:

[0020] When the key point is a circular hole, a waist hole or a polygonal hole, the alternative point set is used to fit the corresponding circle, ellipse or polygon, and the point coordinates at the geometric center of the fitted figure are marked as the three-dimensional coordinates of the key point;

[0021] When the key point is a cylinder or prism, the cylinder or prism is fitted using the candidate point set to obtain the central axis of the cylinder or prism, and then the plane is fitted using the candidate point set. The intersection of the central axis and the fitted plane is recorded as the three-dimensional coordinate of the key point;

[0022] When the key point is a corner point, two straight lines are fitted using the candidate point set, and the intersection of the two straight lines is recorded as the three-dimensional coordinates of the key point;

[0023] When the key point is the midpoint of an edge, a straight line is fitted using the candidate point set, the centroid of the candidate point set is projected onto the fitted straight line, and the projected point coordinates are marked as the three-dimensional coordinates of the key point;

[0024] When the key point is the center of mass of a plane, the plane is fitted using the candidate point set, the center of mass of the candidate point set is projected onto the fitted plane, and the coordinates of the projected point are marked as the three-dimensional coordinates of the key point;

[0025] In step 1), in the measured two-dimensional image, image I is used for template matching, and the matching area is recorded as a new image I. The three-dimensional points corresponding to each point in the new image I in the measured workpiece point cloud are stored in a new candidate point set. In the same way, the new candidate point set is used to solve the three-dimensional coordinates of the key points.

[0026] Preferably, while acquiring the reference workpiece point cloud and the actual scene point cloud, a two-dimensional image is also acquired at the same location; there is a one-to-one correspondence between the pixels in the two-dimensional image and the three-dimensional points in the point cloud;

[0027] The two-dimensional image corresponding to the reference workpiece point cloud is recorded as the reference two-dimensional image; the two-dimensional image corresponding to the point cloud in the actual scene is recorded as the measured two-dimensional image;

[0028] In the reference two-dimensional image, the image of the area where the key points are manually selected is stored as image I; the three-dimensional points corresponding to each point in image I in the reference workpiece point cloud are stored in the candidate point set;

[0029] Use the following method to solve the three-dimensional coordinates of the key points:

[0030] When the key point is a circular hole, waist hole, polygonal hole, cylinder, prism, edge midpoint, or plane centroid, edge extraction is performed on the key point in image I to obtain the edge contour line of the key point, and then the pixel coordinates of the geometric center of the edge contour line are obtained; according to the camera imaging model, the coordinate S of the pixel coordinate in the camera coordinate system is obtained; the origin of the camera coordinate system and the coordinate S are used to establish the spatial line L1; the spatial plane is fitted using the candidate point set; the intersection between the spatial line L1 and the spatial plane is obtained, and the coordinates of the intersection are marked as the three-dimensional coordinates of the key point;

[0031] When the key point is a corner point, edge extraction is performed on the two edges forming the corner point in image I. The extracted edge points are then used to fit two straight lines. The coordinates of the intersection of the two straight lines in the camera coordinate system are recorded as coordinate A. The spatial line L2 is established using the origin of the camera coordinate system and coordinate A. The spatial plane is fitted using the candidate point set. The intersection point between the spatial line L2 and the spatial plane is obtained, and the coordinates of the intersection point are marked as the three-dimensional coordinates of the key point.

[0032] In step 1), in the measured two-dimensional image, template matching is performed using image I, and the matching area is recorded as a new image I. The three-dimensional points corresponding to each point in the new image I in the measured workpiece point cloud are stored in a new candidate point set. In the same way, the new image I and the new candidate point set are used to solve the three-dimensional coordinates of the key points.

[0033] In order to accurately obtain the measured workpiece point cloud and eliminate the background point cloud, it is preferred that a two-dimensional image is collected at the same position while obtaining the reference workpiece point cloud and the actual scene point cloud; there is a one-to-one correspondence between the pixels in the two-dimensional image and the three-dimensional points in the point cloud;

[0034] The two-dimensional image corresponding to the reference workpiece point cloud is recorded as the reference two-dimensional image; the two-dimensional image corresponding to the point cloud in the actual scene is recorded as the measured two-dimensional image;

[0035] In step 1), the actual scene point cloud is preprocessed as follows to filter out the measured workpiece point cloud from the actual scene point cloud:

[0036] Find the two-dimensional pixel points corresponding to the template workpiece point cloud in the reference two-dimensional image, and record the image area formed by all the found pixel points as area one; manually set a selection box in the reference two-dimensional image, the number of pixels in the selection box is greater than area one and completely covers area one; save the image area within the selection box as a feature map, and select at least four points on the feature map as feature points;

[0037] A small local area in the reference 2D image is selected with each feature point as the center. In step 1), each small local area is used to perform template matching in the measured 2D image. The matched area is recorded as area 2, and the center point of area 2 is recorded as the adaptation point.

[0038] Solve the homography matrix using feature points and adaptation points;

[0039] Then, the homography matrix is ​​used to convert the single two-dimensional pixel points on the feature map into the measured two-dimensional image to obtain its pixel coordinates in the measured two-dimensional image, and the three-dimensional points corresponding to the pixel coordinates in the point cloud in the actual scene are stored in the measured workpiece point cloud; each two-dimensional pixel point on the feature map is traversed, and each corresponding three-dimensional point is stored in the measured workpiece point cloud in the same way.

[0040] Preferably, the characteristic points are evenly distributed in the characteristic graph. To facilitate searching, preferably, the characteristic points are the four corner points of the characteristic graph.

[0041] Preferably, when the objective function E(R,t) is iteratively solved using an optimization method, the initial values ​​of the rotation matrix R and the translation matrix t are obtained by SVD decomposition of the homography matrix.

[0042] Preferably,

[0043] Furthermore, in step 1), the nearest neighbor search method is a kd-tree search method or an octree search method.

[0044] Furthermore, the optimization method in step 2) is a linear least squares method, a Gauss-Newton method, or a Levenberg-Marquardt method;

[0045] When the objective function E(R,t) is iteratively solved using an optimization method, the initial values ​​of the rotation matrix R and the translation matrix t are obtained through coarse registration, or the initial values ​​of the rotation matrix R and the translation matrix t are set to the identity matrix.

[0046] As an application of this method, the key point is an assembly point; after step 2), step 3) is further performed to assemble the workpiece:

[0047] 3) Feedback the obtained rotation matrix R and translation matrix t to the robot controller to correct the robot assembly trajectory;

[0048] The robot assembly trajectory is the motion trajectory of the robot that can accurately assemble the reference workpiece.

[0049] Compared with the prior art, the method provided by this application has the following advantages:

[0050] (1) The traditional ICP method only constrains the alignment of the workpiece surface's overall topography point cloud. The registration result can only guarantee the positioning accuracy of the workpiece's overall contour, and is prone to local feature registration errors. The method of the present invention simultaneously considers the overall topography information of the workpiece surface and the edge information of local key point features, and can produce more accurate registration results.

[0051] (2) In the process of solving key points, a combination of two-dimensional images and three-dimensional point clouds is used to effectively suppress the interference of noise points, making the results more accurate. By selecting the key point area, the amount of calculation is reduced.

[0052] (3) The actual scene point cloud is pre-processed by combining template matching with homography matrix solution to quickly obtain the effective registration area, reducing the number of point clouds involved in the solution in the objective function and improving the calculation speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 This is a schematic diagram of the template workpiece point cloud and the measured workpiece point cloud before point cloud registration;

[0054] Figure 2 is a schematic diagram of a reference two-dimensional image in a specific embodiment;

[0055] Figure 3 is a schematic diagram of selecting an image I from a reference two-dimensional image in a specific embodiment;

[0056] Figure 4 is a schematic diagram of a measured two-dimensional image in a specific implementation manner;

[0057] Figure 5 is a schematic diagram of matching a new image I in a measured two-dimensional image in a specific embodiment;

[0058] Figure 6 Schematic diagram of the template workpiece point cloud and the measured workpiece point cloud after point cloud registration in a specific implementation manner;

[0059] Figure 7 It is a schematic diagram of obtaining feature maps and feature points in a reference two-dimensional image in a specific implementation manner. DETAILED DESCRIPTION

[0060] The technical solution of the present invention is described in detail below with reference to the accompanying drawings and specific embodiments.

[0061] A point cloud registration method that combines key point information. Key points are pre-marked on the workpiece. Key points include holes, corners, cylindrical protrusions, edge midpoints, and plane centroids on the workpiece surface. When a key point is an edge midpoint, the direction of the edge coincides with any coordinate axis of the spatial coordinate system of the point cloud, and the edge midpoint is the geometric center of the edge. When a key point is a plane centroid, the normal vector of the plane in which it lies coincides with any coordinate axis of the spatial coordinate system of the point cloud.

[0062] Obtain the reference workpiece point cloud, select the point cloud of the area of ​​interest and store it as the template workpiece point cloud; in this point cloud, calculate the 3D coordinates of the key points and store them in the template key point set;

[0063] Use the following steps to perform point cloud registration:

[0064] 1) Obtain the actual scene point cloud, perform preprocessing, and record the preprocessed point cloud as the measured workpiece point cloud; in this point cloud, calculate the three-dimensional coordinates of the key points and store them in the measured key point set;

[0065] The measured workpiece point cloud includes the workpiece point cloud of the same model as the reference workpiece;

[0066] For the 3D points in the template workpiece point cloud, the nearest neighbor search method (kd-tree search method or octree search method) is used to find the point closest to it from the measured workpiece point cloud, and the found point is stored in the measured matching point set;

[0067] 2) Construct the objective function E(R,t):

[0068]

[0069] Among them, p i represents a point in the template workpiece point cloud, q i represents a point in the measured matching point set, k i Indicates q i The normal vector of the point, n represents the number of points in the template workpiece point cloud; h j Represents a point in the template key point set, g j represents a point in the measured key point concentration, m represents the number of key points; λ is the preset weight value; s i Indicates the key point (h j or g j ) is the monitoring direction, which can be one of (1,0,0), (0,1,0), (0,0,1), (1,0,1), (0,1,1), (1,1,0), (1,1,1);

[0070] Among them, (1,0,0) indicates that the key point monitors the offset in the X-axis direction, (0,1,0) indicates that the key point monitors the offset in the Y-axis direction, (0,0,1) indicates that the key point monitors the offset in the Z-axis direction, (1,0,1) indicates that the key point monitors the offset in both the X-axis and Z-axis directions, (0,1,1) indicates that the key point monitors the offset in both the Y-axis and Z-axis directions, (1,1,0) indicates that the key point monitors the offset in both the X-axis and Y-axis directions, and (1,1,1) indicates that the key point monitors the offset in all the X, Y, and Z-axis directions. These settings are set according to actual conditions (such as assembly requirements).

[0071] The optimization method is used to iteratively solve the objective function E(R,t) to obtain the rotation matrix R and translation matrix t, and complete the registration between the template workpiece point cloud and the measured workpiece point cloud.

[0072] As a preferred embodiment, The optimization method is linear least squares, Gauss-Newton or Levenberg-Marquardt method;

[0073] When the objective function E(R,t) is iteratively solved using an optimization method, the initial values ​​of the rotation matrix R and the translation matrix t are obtained through coarse registration, or the initial values ​​of the rotation matrix R and the translation matrix t are set to the identity matrix.

[0074] Specifically, there are two ways to solve the three-dimensional coordinates of key points:

[0075] Method 1:

[0076] While acquiring the reference workpiece point cloud and the actual scene point cloud, a 2D image is also collected at the same location; there is a one-to-one correspondence between the pixels in the 2D image and the 3D points in the point cloud;

[0077] The two-dimensional image corresponding to the reference workpiece point cloud is recorded as the reference two-dimensional image; the two-dimensional image corresponding to the point cloud in the actual scene is recorded as the measured two-dimensional image;

[0078] In the reference two-dimensional image, the image of the area where the key points are manually selected is stored as image I; the three-dimensional points corresponding to each point in image I in the reference workpiece point cloud are stored in the candidate point set;

[0079] Use the following method to solve the three-dimensional coordinates of the key points:

[0080] When the key point is a circular hole, a waist hole or a polygonal hole, the alternative point set is used to fit the corresponding circle, ellipse or polygon, and the point coordinates at the geometric center of the fitted figure are marked as the three-dimensional coordinates of the key point;

[0081] When the key point is a cylinder or prism, the cylinder or prism is fitted using the candidate point set to obtain the central axis of the cylinder or prism, and then the plane is fitted using the candidate point set. The intersection of the central axis and the fitted plane is recorded as the three-dimensional coordinate of the key point;

[0082] When the key point is a corner point, two straight lines are fitted using the candidate point set, and the intersection of the two straight lines is recorded as the three-dimensional coordinates of the key point;

[0083] When the key point is the midpoint of an edge, a straight line is fitted using the candidate point set, the centroid of the candidate point set is projected onto the fitted straight line, and the projected point coordinates are marked as the three-dimensional coordinates of the key point;

[0084] When the key point is the center of mass of a plane, the plane is fitted using the candidate point set, the center of mass of the candidate point set is projected onto the fitted plane, and the coordinates of the projected point are marked as the three-dimensional coordinates of the key point;

[0085] In step 1), in the measured two-dimensional image, image I is used for template matching, and the matching area is recorded as a new image I. The three-dimensional points corresponding to each point in the new image I in the measured workpiece point cloud are stored in a new candidate point set. In the same way, the new candidate point set is used to solve the three-dimensional coordinates of the key points.

[0086] Method 2:

[0087] While acquiring the reference workpiece point cloud and the actual scene point cloud, a 2D image is also collected at the same location; there is a one-to-one correspondence between the pixels in the 2D image and the 3D points in the point cloud;

[0088] The two-dimensional image corresponding to the reference workpiece point cloud is recorded as the reference two-dimensional image; the two-dimensional image corresponding to the point cloud in the actual scene is recorded as the measured two-dimensional image;

[0089] In the baseline 2D image, such as Figure 3 As shown, the image of the area where the key points are manually selected is stored as image I; the three-dimensional points corresponding to each point in image I in the reference workpiece point cloud are stored in the candidate point set;

[0090] Use the following method to solve the three-dimensional coordinates of the key points:

[0091] When the key point is a circular hole, waist hole, polygonal hole, cylinder, prism, edge midpoint or plane centroid, edge extraction is performed on the key point in image I to obtain the edge contour line of the key point, and then the pixel coordinates of the geometric center of the edge contour line are obtained; according to the camera imaging model, the coordinate S of the pixel coordinate in the camera coordinate system is obtained; the origin of the camera coordinate system (0, 0, 0) and the coordinate S are used to establish a spatial line L1; the spatial plane is fitted using the candidate point set; the intersection between the spatial line L1 and the spatial plane is obtained, and the coordinates of the intersection are marked as the three-dimensional coordinates of the key point;

[0092] When the key point is a corner point, edge extraction is performed on the two edges forming the corner point in image I. The extracted edge points are then used to fit two straight lines. The coordinates of the intersection of the two straight lines in the camera coordinate system are recorded as coordinate A. The spatial line L2 is established using the origin of the camera coordinate system and coordinate A. The spatial plane is fitted using the candidate point set. The intersection point between the spatial line L2 and the spatial plane is obtained, and the coordinates of the intersection point are marked as the three-dimensional coordinates of the key point.

[0093] In step 1), if Figure 5 As shown in FIG, in the measured two-dimensional image, image I is used for template matching, the matching area is recorded as a new image I, and the three-dimensional points corresponding to each point in the new image I in the measured workpiece point cloud are stored in a new candidate point set. In the same way, the new image I and the new candidate point set are used to solve the three-dimensional coordinates of the key points.

[0094] Below Figure 2 Take the artifact in as an example to illustrate:

[0095] A point cloud registration method combining key point information, pre-marking key points on the workpiece, in this embodiment, such as Figure 2 As shown in the figure, the key point is the lower left hole of the workpiece; in actual application, multiple key points can also be selected, such as marking all three holes in the workpiece as key points.

[0096] Obtain the reference workpiece point cloud, select the point cloud of the area of ​​interest (remove the background point cloud and retain only the overall workpiece point cloud) and store it as the template workpiece point cloud; in this point cloud, calculate the 3D coordinates of the key points (method 2) and store them in the template key point set;

[0097] Use the following steps to perform point cloud registration:

[0098] 1) Obtain the actual scene point cloud, perform preprocessing, and record the preprocessed point cloud as the measured workpiece point cloud; in this point cloud, calculate the three-dimensional coordinates of the key points (method 2) and store them in the measured key point set;

[0099] The measured workpiece point cloud includes the workpiece point cloud of the same model as the reference workpiece; the measured workpiece point cloud includes not only the workpiece point cloud, but also the partial point cloud of the area around the workpiece;

[0100] In order to accurately obtain the measured workpiece point cloud and eliminate the background point cloud, in this embodiment, the actual scene point cloud is preprocessed as follows to filter out the measured workpiece point cloud from the actual scene point cloud:

[0101] While acquiring the reference workpiece point cloud and the actual scene point cloud, a 2D image is also collected at the same location; there is a one-to-one correspondence between the pixels in the 2D image and the 3D points in the point cloud;

[0102] The two-dimensional image corresponding to the reference workpiece point cloud is recorded as the reference two-dimensional image (such as Figure 2 ); record the two-dimensional image corresponding to the point cloud in the actual scene as the measured two-dimensional image (such as Figure 4 );

[0103] like Figure 7 As shown, the corresponding two-dimensional pixel points of the template workpiece point cloud in the reference two-dimensional image are found, and the image area formed by all the found pixel points is recorded as area 1; a selection box is manually set in the reference two-dimensional image, and the number of pixel points in the selection box is greater than area 1 and completely covers area 1; the image area in the selection box is saved as a feature map, and at least four points on the feature map are selected and recorded as feature points;

[0104] That is, the reference two-dimensional image includes a feature map, and the feature map includes region one.

[0105] In order to obtain a more accurate homography matrix, the feature points are evenly distributed in the feature map. In this embodiment, in order to facilitate the search, Figure 7 As shown, the feature points are the four corner points of the feature map.

[0106] A small local area in the reference 2D image is selected with each feature point as the center. In step 1), each small local area is used to perform template matching in the measured 2D image. The matched area is recorded as area 2, and the center point of area 2 is recorded as the adaptation point.

[0107] Solve the homography matrix using multiple feature points and multiple adaptation points;

[0108] Then, the homography matrix is ​​used to convert the single two-dimensional pixel points on the feature map into the measured two-dimensional image to obtain its pixel coordinates in the measured two-dimensional image, and the three-dimensional points corresponding to the pixel coordinates in the point cloud in the actual scene are stored in the measured workpiece point cloud; each two-dimensional pixel point on the feature map is traversed, and the corresponding three-dimensional points are stored in the measured workpiece point cloud in the same way.

[0109] Next, proceed as follows:

[0110] For the 3D points in the template workpiece point cloud, the nearest neighbor search method (kd-tree search method) is used to find the points closest to it from the measured workpiece point cloud, and the found points are stored in the measured matching point set;

[0111] 2) Construct the objective function E(R,t):

[0112]

[0113] Among them, p i represents a point in the template workpiece point cloud, q i represents a point in the measured matching point set, k i Indicates q i The normal vector of the point, n represents the number of points in the template workpiece point cloud; h j Represents a point in the template key point set, g j represents a point in the measured key point concentration, m represents the number of key points (in this embodiment, m=1); λ is a preset weight value, s i Indicates the monitoring direction of the key point. In this embodiment, the value is (1,1,1);

[0114] The optimization method is used to iteratively solve the objective function E(R,t) to obtain the rotation matrix R and translation matrix t, and complete the registration between the template workpiece point cloud and the measured workpiece point cloud.

[0115] The initial values ​​of the rotation matrix R and the translation matrix t are obtained by SVD decomposition of the homography matrix.

[0116] As an application, the key point is the assembly point; after step 2), step 3) is also performed to assemble the workpiece:

[0117] 3) Feedback the obtained rotation matrix R and translation matrix t to the robot controller to correct the robot assembly trajectory;

[0118] The robot assembly trajectory is the motion trajectory along which the robot can accurately assemble the reference workpiece.

[0119] like Figure 1 This is a schematic diagram of the template workpiece point cloud before point cloud registration and the measured workpiece point cloud. It can be seen that there is a large offset between the positions of the two. Under such conditions, the robot cannot accurately assemble the measured workpiece. The point cloud registration effect after processing by this method is as follows: Figure 6 As shown in the figure, the registered point cloud not only realizes the overall alignment of the workpiece, but also takes into account the alignment of the key points (assembly holes), making the positioning result more accurate. When applied to the assembly field, it can enable accurate assembly of key points (such as pin-hole matching).

[0120] The foregoing descriptions of specific exemplary embodiments of the present invention have been presented for purposes of illustration and description. The foregoing descriptions are not intended to be exhaustive or to limit the invention to the precise forms disclosed, and it is apparent that many variations and modifications are possible in light of the foregoing teachings. The exemplary embodiments have been chosen and described in order to explain the specific principles of the invention and their practical application, thereby enabling others skilled in the art to make and utilize the various exemplary embodiments of the invention and various alternatives and modifications thereof. The scope of the invention is intended to be defined by the appended claims and their equivalents.

Claims

1. A point cloud registration method that combines key point information. Key points are pre-marked on a workpiece. These key points include holes, corners, cylindrical protrusions, edge midpoints, and plane centroids. When a key point is an edge midpoint, the direction of the edge coincides with any coordinate axis of the spatial coordinate system in which the point cloud resides, and the edge midpoint is the geometric center of the edge. When a key point is a plane centroid, the normal vector of the plane in which it resides coincides with any coordinate axis of the spatial coordinate system in which the point cloud resides. Obtain the reference workpiece point cloud, select the point cloud of the area of ​​interest and store it as the template workpiece point cloud; in the template workpiece point cloud, calculate the 3D coordinates of the key points and store them in the template key point set; It is characterized in that point cloud registration is performed using the following steps: 1) Obtain the actual scene point cloud, perform preprocessing, and record the preprocessed point cloud as the measured workpiece point cloud; in the measured workpiece point cloud, calculate the three-dimensional coordinates of the key points and store them in the measured key point set; The measured workpiece point cloud includes a workpiece point cloud of the same model as the reference workpiece; For the 3D points in the template workpiece point cloud, the nearest neighbor search method is used to find the points closest to it from the measured workpiece point cloud, and the found points are stored in the measured matching point set; 2) Construct the objective function E(R,t): Among them, p i represents a point in the template workpiece point cloud, q i represents a point in the measured matching point set, k i Indicates q i The normal vector of the point, n represents the number of points in the template workpiece point cloud; h j Represents a point in the template key point set, g j represents a point in the measured key point concentration, m represents the number of key points; λ is the preset weight value; s i Indicates the monitoring direction of the key point, and its value is one of (1,0,0), (0,1,0), (0,0,1), (1,0,1), (0,1,1), (1,1,0), (1,1,1); The objective function E(R,t) is iteratively solved using the optimization method to obtain the rotation matrix R and translation matrix t, and the registration between the template workpiece point cloud and the measured workpiece point cloud is completed.

2. The point cloud registration method incorporating key point information according to claim 1, characterized in that: While acquiring the reference workpiece point cloud and the actual scene point cloud, a 2D image is also collected at the same location; there is a one-to-one correspondence between the pixels in the 2D image and the 3D points in the point cloud; The two-dimensional image corresponding to the reference workpiece point cloud is recorded as the reference two-dimensional image; The two-dimensional image corresponding to the point cloud in the actual scene is recorded as the measured two-dimensional image; In the reference two-dimensional image, the image of the area where the key points are manually selected is stored as image I; the three-dimensional points corresponding to each point in image I in the reference workpiece point cloud are stored in the candidate point set; Use the following method to solve the three-dimensional coordinates of the key points: When the key point is a circular hole, a waist hole or a polygonal hole, the alternative point set is used to fit the corresponding circle, ellipse or polygon, and the point coordinates at the geometric center of the fitted figure are marked as the three-dimensional coordinates of the key point; When the key point is a cylinder or prism, the cylinder or prism is fitted using the candidate point set to obtain the central axis of the cylinder or prism, and then the plane is fitted using the candidate point set. The intersection of the central axis and the fitted plane is recorded as the three-dimensional coordinate of the key point; When the key point is a corner point, two straight lines are fitted using the candidate point set, and the intersection of the two straight lines is recorded as the three-dimensional coordinates of the key point; When the key point is the midpoint of an edge, a straight line is fitted using the candidate point set, the centroid of the candidate point set is projected onto the fitted straight line, and the projected point coordinates are marked as the three-dimensional coordinates of the key point; When the key point is the center of mass of a plane, the plane is fitted using the candidate point set, the center of mass of the candidate point set is projected onto the fitted plane, and the coordinates of the projected point are marked as the three-dimensional coordinates of the key point; In step 1), in the measured two-dimensional image, image I is used for template matching, and the matching area is recorded as a new image I. The three-dimensional points corresponding to each point in the new image I in the measured workpiece point cloud are stored in a new candidate point set. In the same way, the new candidate point set is used to solve the three-dimensional coordinates of the key points.

3. The point cloud registration method according to claim 1, wherein: While acquiring the reference workpiece point cloud and the actual scene point cloud, a 2D image is also collected at the same location; there is a one-to-one correspondence between the pixels in the 2D image and the 3D points in the point cloud; The two-dimensional image corresponding to the reference workpiece point cloud is recorded as the reference two-dimensional image; The two-dimensional image corresponding to the point cloud in the actual scene is recorded as the measured two-dimensional image; In the reference two-dimensional image, the image of the area where the key points are manually selected is stored as image I; the three-dimensional points corresponding to each point in image I in the reference workpiece point cloud are stored in the candidate point set; Use the following method to solve the three-dimensional coordinates of the key points: When the key point is a circular hole, waist hole, polygonal hole, cylinder, prism, edge midpoint, or plane centroid, edge extraction is performed on the key point in image I to obtain the edge contour line of the key point, and then the pixel coordinates of the geometric center of the edge contour line are obtained; according to the camera imaging model, the coordinate S of the pixel coordinate in the camera coordinate system is obtained; the origin of the camera coordinate system and the coordinate S are used to establish the spatial line L1; the spatial plane is fitted using the candidate point set; the intersection between the spatial line L1 and the spatial plane is obtained, and the coordinates of the intersection are marked as the three-dimensional coordinates of the key point; When the key point is a corner point, edge extraction is performed on the two edges forming the corner point in image I. The extracted edge points are then used to fit two straight lines. The coordinates of the intersection of the two straight lines in the camera coordinate system are recorded as coordinate A. The spatial line L2 is established using the origin of the camera coordinate system and coordinate A. The spatial plane is fitted using the candidate point set. The intersection point between the spatial line L2 and the spatial plane is obtained, and the coordinates of the intersection point are marked as the three-dimensional coordinates of the key point. In step 1), in the measured two-dimensional image, template matching is performed using image I, and the matching area is recorded as a new image I. The three-dimensional points corresponding to each point in the new image I in the measured workpiece point cloud are stored in a new candidate point set. In the same way, the new image I and the new candidate point set are used to solve the three-dimensional coordinates of the key points.

4. The point cloud registration method according to claim 1, wherein: While acquiring the reference workpiece point cloud and the actual scene point cloud, a 2D image is also collected at the same location; there is a one-to-one correspondence between the pixels in the 2D image and the 3D points in the point cloud; The two-dimensional image corresponding to the reference workpiece point cloud is recorded as the reference two-dimensional image; The two-dimensional image corresponding to the point cloud in the actual scene is recorded as the measured two-dimensional image; In step 1), the actual scene point cloud is preprocessed as follows to filter out the measured workpiece point cloud from the actual scene point cloud: Find the two-dimensional pixel points corresponding to the template workpiece point cloud in the reference two-dimensional image, and record the image area formed by all the found pixel points as area one; manually set a selection box in the reference two-dimensional image, the number of pixels in the selection box is greater than area one and completely covers area one; save the image area within the selection box as a feature map, and select at least four points on the feature map as feature points; A small local area in the reference 2D image is selected with each feature point as the center. In step 1), each small local area is used to perform template matching in the measured 2D image. The matched area is recorded as area 2, and the center point of area 2 is recorded as the adaptation point. Solve the homography matrix using feature points and adaptation points; Then, the homography matrix is ​​used to convert the single two-dimensional pixel points on the feature map into the measured two-dimensional image to obtain its pixel coordinates in the measured two-dimensional image, and the three-dimensional points corresponding to the pixel coordinates in the point cloud in the actual scene are stored in the measured workpiece point cloud; each two-dimensional pixel point on the feature map is traversed, and each corresponding three-dimensional point is stored in the measured workpiece point cloud in the same way.

5. The point cloud registration method according to claim 4, wherein: The feature points are the four corner points of the feature map.

6. The point cloud registration method according to claim 4, wherein: When the objective function E(R,t) is iteratively solved using the optimization method, the initial values ​​of the rotation matrix R and the translation matrix t are obtained by SVD decomposition of the homography matrix.

7. The point cloud registration method according to claim 1, wherein:

8. The point cloud registration method according to claim 1, wherein: Step 1) The nearest neighbor search method is a kd-tree search method or an octree search method.

9. The point cloud registration method according to claim 1, wherein: Step 2) the optimization method is linear least squares method, Gauss-Newton method or Levenberg-Marquardt method; When the objective function E(R,t) is iteratively solved using an optimization method, the initial values ​​of the rotation matrix R and the translation matrix t are obtained through coarse registration, or the initial values ​​of the rotation matrix R and the translation matrix t are set to the identity matrix.

10. The point cloud registration method according to claim 1, wherein: The key point is the assembly point; after step 2), step 3) is also performed to assemble the workpiece: 3) Feedback the obtained rotation matrix R and translation matrix t to the robot controller to correct the robot assembly trajectory; The robot assembly trajectory is the motion trajectory of the robot that can accurately assemble the reference workpiece.

Images