Workpiece internal key point positioning method based on ICP algorithm

Abstract

The application discloses a workpiece internal key point positioning method based on an ICP algorithm, calibrates a camera and a mechanical arm, and obtains a coordinate transformation matrix between a world coordinate system and a camera coordinate system; fixes the position of a workpiece on a conveying belt, scans the workpiece by using a 3D laser camera to obtain source point cloud of the workpiece, simultaneously debugs the mechanical arm, moves the mechanical arm to the position of a key point, and obtains the position of the key point in the world coordinate system; changes the position of the workpiece, scans the workpiece again to obtain target point cloud; pre-processes the two times of point cloud data; registers by using the ICP algorithm, obtains a registration matrix between the source point cloud and the target point cloud; and with the aid of the coordinate transformation matrix and the registration matrix, calculates the three-dimensional coordinates of the internal key point of the workpiece in the world coordinate system after the position is changed. The application realizes efficient and accurate detection of the internal key point of the workpiece, replaces manual detection, reduces production cost, and improves production efficiency.

Description

Technical Field

[0001] This invention relates to the technical field of three-dimensional posture detection, and in particular to a method for locating key points inside a workpiece based on the ICP algorithm. Background Technology

[0002] In many factories in China, workpieces on production lines require inspection of key internal points to verify their conformity. However, this work currently relies heavily on manual labor, leading to two main problems: first, it consumes significant human resources, increasing production costs; second, due to the subjectivity and instability of manual operation, inspection results cannot be reliably guaranteed, potentially resulting in defective products leaving the factory and entering the market, posing safety hazards to consumers and causing economic losses to the factory. Therefore, effectively and efficiently utilizing automated equipment to inspect key internal points of workpieces has become an urgent problem to solve. Summary of the Invention

[0003] The purpose of this invention is to overcome the shortcomings and deficiencies of the existing technology and propose a method for locating key points inside a workpiece based on the ICP algorithm. By using a 3D laser camera to quickly scan the inside of the workpiece, the position of key points is accurately located. With the help of a robotic arm, the status of key points is effectively detected. This not only greatly reduces production costs, but also ensures that every workpiece is effectively inspected, preventing missed or incorrect inspections. It can significantly improve production efficiency and product quality, and provide more basis and support for the quality management of the factory.

[0004] To achieve the above objectives, the technical solution provided by this invention is: a method for locating key points inside a workpiece based on the ICP algorithm, comprising the following steps:

[0005] 1) First, perform hand-eye calibration on the robotic arm and 3D laser camera to obtain the coordinate transformation matrix between the camera coordinate system and the world coordinate system;

[0006] 2) Fix the position of the workpiece on the conveyor belt, scan it with a 3D laser camera to obtain the source point cloud, that is, the first point cloud image of the workpiece. At the same time, adjust the robotic arm to move the robotic arm to the position of the key point to obtain the three-dimensional position information of the key point in the world coordinate system.

[0007] 3) Change the position of the workpiece and scan the workpiece again to obtain the target point cloud, which is the second point cloud image of the workpiece;

[0008] 4) Preprocess the two point cloud images, including random sampling, voxelization, and outlier removal, to make the point cloud data smaller and flatter, which helps with matching;

[0009] 5) Use the ICP algorithm to register the two point cloud images to obtain the registration matrix between the source point cloud and the target point cloud. Decompose the registration matrix into a rotation matrix and a translation matrix.

[0010] 6) Using the coordinate transformation matrix obtained in step 1) and the registration matrix obtained in step 5), calculate the three-dimensional coordinates of the key points inside the workpiece in the world coordinate system after the position is changed.

[0011] Further, in step 1), the robotic arm and 3D laser camera are calibrated. A calibration board needs to be selected, and the calibration board needs to have some marker points that can be used for positioning. First, the 3D laser camera is used to observe the calibration board and detect the image coordinates of the marker points. Then, the robotic arm is used to move to the position of the marker points to obtain the coordinates of the marker points in the robotic arm coordinate system. With the help of the three-dimensional coordinates of multiple marker points in two coordinate systems, the coordinate transformation matrix T_G between the world coordinate system and the camera coordinate system is calculated.

[0012] Further, in step 2), the position of the workpiece on the conveyor belt is fixed, and a 3D laser camera is used to scan the 3D image of the workpiece to obtain the source point cloud, which is used for subsequent registration. Then, a robotic arm is used to move to the position of the key points inside the workpiece to obtain the 3D coordinates W of the key points in the world coordinate system.

[0013] W = {x, y, z}

[0014] In the formula, x, y, and z are the x, y, and z coordinates of the key point in the world coordinate system, respectively.

[0015] Furthermore, in step 3), changing the workpiece position is to simulate the actual production process where the workpiece is randomly placed on the conveyor belt by personnel, and a 3D laser camera is used to scan the workpiece to obtain a point cloud image of the workpiece in a random state, i.e., the target point cloud, which is used for registration with the source point cloud.

[0016] Furthermore, in step 4), the source point cloud and target point cloud are preprocessed. The specific preprocessing content is as follows:

[0017] Read the source point cloud data and construct the point set S:

[0018] S = {a1, a2, ..., a} N}={a i |i=1,2,…,N}

[0019] In the formula, a i This represents the i-th data point in the point set, which is the three-dimensional coordinate of the i-th point scanned on the workpiece surface in the camera coordinate system.

[0020] Read the target point cloud data and construct the point set P:

[0021] P = {b1, b2, ..., b} M}={b j |j=1,2,…,M}

[0022] In the formula, b j This represents the j-th data point in the point set, which is the three-dimensional coordinate of the j-th point scanned on the surface of the workpiece to be matched, in the camera coordinate system.

[0023] Random sampling is performed on point set S and point set P respectively, with the sampling ratio set to 500, that is, the number of point clouds after sampling is 1 / 500 of the original number of point clouds;

[0024] The randomly sampled point cloud is voxelized, and outliers are removed to obtain regularized point cloud data. A new source point cloud dataset K is then constructed.

[0025] K = {d1, d2, ..., d} Q}={d i′ |i′=1,2,…,Q}

[0026] In the formula, d i′ This represents the i′-th data point in the source point cloud dataset, where Q is much smaller than N;

[0027] Construct a new target point cloud dataset G:

[0028] G = {e1,e2,…,e} L}={e j′ |j′=1,2,…,L}

[0029] In the formula, e j′ Let L represent the j′-th data point in the target point cloud dataset, where L is much smaller than M.

[0030] Furthermore, in step 5), the target point cloud is registered using the ICP algorithm. The specific steps are as follows:

[0031] Define an initial transformation matrix T, which defaults to a 4x4 unit diagonal matrix;

[0032] For each point in the source point cloud, find the corresponding point in the target point cloud, use the nearest neighbor search method to find the nearest neighbor of the point in the source point cloud, and match them to form a corresponding point pair.

[0033] Calculate the Euclidean distance between each pair of points, and sum these distances as the error E;

[0034] The optimal rigid body transformation is calculated by minimizing the error, and the optimal transformation matrix T_opt is obtained by solving the problem using an optimization algorithm.

[0035] The process involves iterative calculations to update the optimal transformation matrix T_opt until the error E is less than the set threshold Y. Then, the optimal transformation matrix T_opt that meets the conditions is returned, which is the registration matrix.

[0036] To facilitate the subsequent calculation of the keypoint positions in the world coordinate system, the registration matrix needs to be decomposed into a rotation matrix R and a translation matrix t, as shown below:

[0037] R = T_opt[:3,:3]

[0038] t = T_opt[:3,3].

[0039] Furthermore, in step 6), the specific process of calculating the positions of key points in the target point cloud in the world coordinate system is as follows:

[0040] First, calculate the 3D coordinates W_camera of the key points in the source point cloud in the camera coordinate system. The calculation formula is as follows:

[0041] W_camera=T_G -1 *W

[0042] Next, calculate the corresponding position W_tg of the keypoint in the target point cloud:

[0043] W_tg=R*W_camera+t

[0044] Finally, calculate the position W_global of the keypoints in the target point cloud in the world coordinate system:

[0045] W_global = T_G * W_tg.

[0046] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0047] 1. This invention is the first to combine a 3D laser camera with a robotic arm to achieve efficient and precise positioning and detection of key points inside a workpiece. Previously, the detection of key points inside workpieces typically relied on manual operation, which not only consumed significant human resources but also, due to human subjectivity and instability, could not guarantee reliable results. This could lead to the release of substandard products, posing safety hazards to factories and consumers and causing economic losses.

[0048] 2. This invention employs the ICP algorithm for locating key points inside the workpiece. The ICP algorithm boasts high accuracy and stability, effectively matching the source point cloud with the target point cloud, thus improving the accuracy and stability of the detection. Compared to traditional methods, this invention can more accurately match corresponding points, avoiding inaccurate detection caused by human error.

[0049] 3. The method of this invention is not only applicable to specific types of workpieces, but also has good versatility and scalability. It can be applied to workpieces of different materials and shapes, meeting the testing needs of different production lines, and has broad application prospects. Attached Figure Description

[0050] Figure 1 This is a schematic diagram of the installation of a 3D laser camera and a robotic arm on a production line. The 3D laser camera is placed directly above the production line and can scan the surface structure of the entire workpiece. The robotic arm is close to the production line, and the workpiece moves on the assembly line. When it moves in front of the robotic arm, it stops for inspection.

[0051] Figure 2 This is a flowchart of the method of the present invention. Detailed Implementation

[0052] The present invention will be further described below with reference to specific embodiments, but the implementation of the present invention is not limited thereto.

[0053] like Figure 1 and Figure 2 As shown, this embodiment discloses a method for locating key points inside a workpiece based on the ICP algorithm. The specific implementation includes the following steps:

[0054] 1) First, perform hand-eye calibration on the robotic arm and 3D laser camera to obtain the coordinate transformation matrix between the camera coordinate system and the world coordinate system;

[0055] To calibrate the robotic arm and 3D laser camera, a calibration board needs to be selected. The calibration board needs to have some marker points that can be used for positioning. First, the 3D laser camera is used to observe the calibration board and detect the image coordinates of the marker points. Then, the robotic arm is used to move to the position of the marker point to obtain the coordinates of the marker point in the robotic arm coordinate system. Using the three-dimensional coordinates of multiple marker points in two coordinate systems, the coordinate transformation matrix T_G between the world coordinate system and the camera coordinate system is calculated.

[0056] 2) Fix the position of the workpiece on the conveyor belt, scan the 3D image of the workpiece with a 3D laser camera to obtain the source point cloud, which is the first point cloud image of the workpiece, used for subsequent registration. Then, use a robotic arm to move to the position of the key points inside the workpiece to obtain the 3D coordinates W of the key points in the world coordinate system:

[0057] W = {x, y, z}

[0058] In the formula, x, y, and z are the x, y, and z coordinates of the key point in the world coordinate system, respectively.

[0059] 3) Change the workpiece position and rescan the workpiece to obtain the target point cloud, which is the second point cloud image of the workpiece. Changing the workpiece position is to simulate the actual production process where the workpiece is randomly placed on the conveyor belt by personnel. The workpiece is scanned with a 3D laser camera to obtain the point cloud image of the workpiece in a random state, which is the target point cloud and is used for registration with the source point cloud.

[0060] 4) Preprocess the two point cloud images, including random sampling, voxelization, and outlier removal, to make the point cloud data smaller and flatter, which helps with matching. The specific steps are as follows:

[0061] Read the source point cloud data and construct the point set S:

[0062] S = {a1, a2, ..., a} N}={a i |i=1,2,…,N}

[0063] In the formula, a i This represents the i-th data point in the point set, which is the three-dimensional coordinate of the i-th point scanned on the workpiece surface in the camera coordinate system.

[0064] Read the target point cloud data and construct the point set P:

[0065] P = {b1, b2, ..., b} M}={b j |j=1,2,…,M}

[0066] In the formula, b j This represents the j-th data point in the point set, which is the three-dimensional coordinate of the j-th point scanned on the surface of the workpiece to be matched, in the camera coordinate system.

[0067] Random sampling is performed on point set S and point set P respectively, with the sampling ratio set to 500, that is, the number of point clouds after sampling is 1 / 500 of the original number of point clouds;

[0068] The randomly sampled point cloud is voxelized, and outliers are removed to obtain regularized point cloud data. A new source point cloud dataset K is then constructed.

[0069] K = {d1, d2, ..., d} Q}={d i′ |i′=1,2,…,Q}

[0070] In the formula, d i′ This represents the i′-th data point in the source point cloud dataset, where Q is much smaller than N;

[0071] Construct a new target point cloud dataset G:

[0072] G = {e1,e2,…,e} L}={e j′ |j′=1,2,…,L}

[0073] In the formula, e j′ Let L represent the j′-th data point in the target point cloud dataset, where L is much smaller than M.

[0074] 5) Use the ICP algorithm to register the two point cloud images to obtain the registration matrix between the source and target point clouds. Decompose the registration matrix into rotation and translation matrices. The specific steps are as follows:

[0075] Define an initial transformation matrix T, which defaults to a 4x4 unit diagonal matrix;

[0076] For each point in the source point cloud, find the corresponding point in the target point cloud, use the nearest neighbor search method to find the nearest neighbor of the point in the source point cloud, and match them to form a corresponding point pair.

[0077] Calculate the Euclidean distance between each pair of points, and sum these distances as the error E;

[0078] The optimal rigid body transformation is calculated by minimizing the error, and the optimal transformation matrix T_opt is obtained by solving the problem using an optimization algorithm.

[0079] The process involves iterative calculations to update the optimal transformation matrix T_opt until the error E is less than the set threshold Y. Then, the optimal transformation matrix T_opt that meets the conditions is returned, which is the registration matrix.

[0080] To facilitate the subsequent calculation of the keypoint positions in the world coordinate system, the registration matrix needs to be decomposed into a rotation matrix R and a translation matrix t, as shown below:

[0081] R = T_opt[:3,:3]

[0082] t = T_opt[:3,3].

[0083] 6) Using the coordinate transformation matrix from step 1) and the registration matrix obtained in step 5), calculate the three-dimensional coordinates of the key points inside the workpiece in the world coordinate system after the position is changed. The specific process is as follows:

[0084] First, calculate the 3D coordinates W_camera of the key points in the source point cloud in the camera coordinate system. The calculation formula is as follows:

[0085] W_camera=T_G -1 *W

[0086] Next, calculate the corresponding position W_tg of the keypoint in the target point cloud:

[0087] W_tg=R*W_camera+t

[0088] Finally, calculate the position W_global of the keypoints in the target point cloud in the world coordinate system:

[0089] W_global = T_G * W_tg.

[0090] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A method for locating key points inside a workpiece based on an ICP algorithm, characterized in that, The method comprises the following steps: 1) First, the hand-eye calibration of the mechanical arm and the 3D laser camera is performed to obtain a coordinate transformation matrix between the camera coordinate system and the world coordinate system; 2) The position of the workpiece on the conveying belt is fixed, the 3D laser camera is used for scanning to obtain a source point cloud, i.e., a first point cloud picture of the workpiece, and the mechanical arm is debugged to move to the position of the key point to obtain the three-dimensional position information of the key point in the world coordinate system; 3) The position of the workpiece is changed, and the workpiece is scanned again to obtain a target point cloud, i.e., a second point cloud picture of the workpiece; 4) The two point cloud pictures are preprocessed, including random sampling, voxelization and outlier removal, so that the point cloud data is less and flat, which is helpful for matching; 5) The ICP algorithm is used to register the two point cloud pictures to obtain a registration matrix between the source point cloud and the target point cloud, and the registration matrix is decomposed into a rotation matrix and a translation matrix; wherein the target point cloud is registered using the ICP algorithm, and the specific steps are as follows: An initial transformation matrix T is set, and T is a default 4*4 unit diagonal matrix; For each point in the source point cloud, find the corresponding point in the target point cloud, find the nearest neighbor of the point in the source point cloud using the nearest neighbor search method, and form a corresponding point pair; Calculate the Euclidean distance between each point pair, and the sum of these distances is taken as the error E; The optimal rigid body transformation is calculated by minimizing the error, and the optimal transformation matrix T_opt is obtained by optimization algorithm; Iterative calculation is continuously performed, and the optimal transformation matrix T_opt is updated until the error E is less than a set threshold Y, and the optimal transformation matrix T_opt that meets the condition is returned, which is the registration matrix; The registration matrix is decomposed into a rotation matrix R and a translation matrix t, and the decomposition method is as follows: R=T_opt[:3,:3] t=T_opt[:3,3] 6) With the aid of the coordinate transformation matrix of step 1) and the registration matrix obtained in step 5), the three-dimensional coordinates of the key points in the workpiece after the position change in the world coordinate system are calculated; wherein the specific process of calculating the position of the key point in the target point cloud in the world coordinate system is as follows: First, calculate the three-dimensional coordinates W_camera of the key point in the source point cloud in the camera coordinate system, and the calculation formula is as follows: W_camera = T_G -1 *W In the formula, T_G is the coordinate transformation matrix between the world coordinate system and the camera coordinate system, and W is the three-dimensional coordinates of the key point in the world coordinate system; Then, calculate the corresponding position W_tg of the key point in the target point cloud: W_tg=R*W_camera+t Finally, the position W_global of the key point in the target point cloud in the world coordinate system is obtained: W_global=T_G*W_tg.

2. The ICP algorithm based method for locating key points inside a workpiece according to claim 1, wherein, In step 1), the mechanical arm and 3D laser camera are calibrated, a calibration board needs to be selected, and the calibration board needs to have some marker points that can be used for positioning. First, use the 3D laser camera to observe the calibration board and detect the image coordinates of the marker points. Then, use the mechanical arm to move to the marker point position to obtain the coordinates of the marker point in the mechanical arm coordinate system. With the three-dimensional coordinates of multiple marker points in the two coordinate systems, the coordinate transformation matrix T_G between the world coordinate system and the camera coordinate system is calculated.

3. The ICP algorithm based method for locating key points inside a workpiece according to claim 2, wherein, In step 2), the position of the workpiece on the conveyor belt is fixed, and the 3D laser camera is used to scan the three-dimensional image of the workpiece to obtain the source point cloud, which is used for registration later. Then, use the mechanical arm to move to the position of the key points inside the workpiece to obtain the three-dimensional coordinates of the key points in the world coordinate system W: W = {x, y, z} Where x, y, z are the x, y, z axis coordinates of the key points in the world coordinate system.

4. The ICP algorithm-based internal key point positioning method of a workpiece according to claim 3, characterized in that, In step 3), the position of the workpiece is changed to simulate the actual production process, where the workpiece is randomly placed on the conveyor belt by personnel. Use the 3D laser camera to scan the workpiece to obtain the point cloud image in the random state, i.e. the target point cloud, which is used for registration with the source point cloud.

5. The ICP algorithm based method for locating key points inside a workpiece according to claim 4, wherein, In step 4), the source point cloud and the target point cloud are preprocessed, and the preprocessing content is as follows: Read the data of the source point cloud and construct the point set S: S = {a1, a2,..., a N} = {a i |i = 1, 2,..., N} In the formula, a i represents the i-th data in the point set, that is, the three-dimensional coordinates of the i-th point scanned on the workpiece surface under the camera coordinates; Read the data of the target point cloud and construct the point set P: P = {b1, b2,..., b M} = {b j |j = 1, 2,..., M} wherein b j represents the jth data in the point set, i.e. the three-dimensional coordinates of the jth point on the surface of the workpiece to be matched scanned under the camera coordinates; Randomly sample the point sets S and P, and set the sampling ratio to 500, i.e. the number of sampled point clouds is 1 / 500 of the original point cloud number. Voxelize the randomly sampled point cloud and remove abnormal points to obtain regular point cloud data, and construct a new source point cloud data set K: K = {d1, d2,..., d Q} = {d i′ |i' = 1, 2,..., Q} In the formula, d i′ represents the i'th data in the source point cloud data set, where Q is much smaller than N; Construct a new target point cloud data set G: G = {e1, e2,..., e L} = {e j′ |j' = 1, 2,..., L} In the formula, e j′ represents the j'th data in the target point cloud data set, where L is much smaller than M.

Images