Automatic clamp TCP calibration method based on machine vision

Abstract

The invention discloses a clamp TCP automatic calibration method based on machine vision. The method comprises the following steps that A, an initial TCP is established, wherein a robot and a visual system complete calibration, and the initial TCP is established manually; b, fixing a feature block and a teaching scanning point position; c, a TCP compensation value is solved, wherein the TCP compensation value is solved through a Steepest Description method; d, automatically calibrating the TCP: enabling the robot to move back and forth at the three demonstrated point positions to automatically calibrate the TCP, and stopping and triggering the visual system to scan and position the feature block after the robot arrives at each point position; and E. space conversion of TCP calibration compensation: after iteration compensation deviation, the robot needs to be switched to the TCP after compensation of new deviation and re-moves to the three points which are demonstrated before to respectively trigger the camera to scan and identify. The automatic calibration device has the advantages that the technical requirements on production line workers are greatly reduced, and automatic calibration can be realized only by fixing the characteristic block at the tail end of the clamp by the workers and calling the function, so that the maintenance operation of the production line is simplified, and the downtime of the production line is reduced.

Description

TECHNICAL FIELD

[0001] The application relates to a clamp TCP automatic calibration method, in particular to a clamp TCP automatic calibration method based on machine vision. BACKGROUND

[0002] Tool Center Point (TCP) calibration is a key concept in the field of industrial robots, and plays a crucial role in robot programming, path planning and precise operation. The TCP of the robot end tool (such as a gripper, a spray gun, a welding gun, etc.) represents the "operation tip" of the tool in three-dimensional space, and the motion trajectory of the robot needs to be based on the accurate positioning of the TCP. If the definition of the TCP is inaccurate, it will directly lead to the deviation of the operation of the robot. At present, the conventional TCP calibration methods in the industry at home and abroad are four-point method (4-Point Method) and six-point method (6-Point Method). The four-point method touches the same fixed point in space 4 times through the tool tip, calculates the offset between the robot tool coordinate system and the base coordinate system, and is suitable for tools with simple shapes and without considering the direction of the tool. The six-point method additionally collects posture data of the tool in different directions on the basis of the four-point method, and calculates and calibrates the position and direction of the TCP through 6 groups of data, and is suitable for cases where the direction of the tool affects the operation. The four-point method and the six-point method are classic methods for TCP calibration of industrial robots, although they are widely used, but there are obvious limitations in actual operation:

[0003] Firstly, it depends on manual operation. Both methods depend on manual operation and contact, and need to manually control the robot to contact the fixed point in different postures, which requires high skills of the operator. If the contact points are not strictly aligned or the postures are not properly selected, the probe is worn out or the surface is contaminated, all of which will cause errors.

[0004] Secondly, the calibration efficiency is low. The six-point method improves the disadvantage of the four-point method that cannot accurately control the direction of the tool, but needs to additionally collect directional data and has complicated steps, which not only depends on the technology of the operator, but also significantly increases the calibration time. In practice, it usually takes 15-20 minutes for a skilled operator to calibrate using the four-point method, and more than 30 minutes for the six-point method. If the tool is replaced or worn out, it needs to be manually calibrated again, which is time-consuming and labor-intensive.

[0005] Thirdly, it cannot cope with complex tools. In the case of complex tools such as multi-tool cooperation, deformable tools (such as flexible grippers) or tools with sensors at the end (such as force control probes), the two traditional methods are difficult to define the TCP.

[0006] Fourth, it is unable to realize dynamic compensation, and the calibration result is one-time static data, which cannot compensate tool wear or external interference in real time. In practice, the TCP may gradually deviate (such as wear failure) during long-time continuous operation of the robot, and needs to be stopped for re-calibration. SUMMARY

[0007] The technical problem to be solved by the present application is to provide a fixture TCP automatic calibration method based on machine vision, which can realize TCP automatic calibration, improve measurement accuracy, cope with complex industrial scenes and improve production efficiency.

[0008] In order to solve the above technical problems, the fixture TCP automatic calibration method based on machine vision of the present application comprises the following steps:

[0009] A. Establishing an initial TCP: the robot and the vision system complete the calibration and the initial TCP is established manually, at this time the robot teaches three points with the same three-dimensional space coordinates but different poses;

[0010] B. Fixing the feature block and teaching the scanning points: fixing the feature block to the end of the robot fixture, and teaching three points with the same three-dimensional space coordinates but different poses under the condition of switching to the initial TCP established manually by the robot;

[0011] C. Calculating the TCP compensation value: calculating the TCP compensation value by the Steepest Descent method;

[0012] D. Automatic calibration of TCP: the robot moves back and forth between the three points taught to automatically calibrate the TCP, and stops after reaching each point and triggers the vision system to scan and locate the feature block, which can be positioned and matched by the model established in advance;

[0013] E. Space conversion of TCP calibration compensation: after iterative compensation of the deviation, the robot needs to switch to the TCP compensated for the new deviation and move to the three points taught before to trigger the camera scanning and identification respectively.

[0014] The three points in the A step are P R1 =(x,y,z,R x ,R y ,R z ), P R2 =(x,y,z,R' x ,R' y ,R' z ), and P R3 =(x,y,z,R″ x ,R″ y ,R″ z ), wherein x, y and z values represent three-dimensional space coordinates, (Rx y z x y z x y z wherein R represents rotation, R x is the rotation angle around the X-axis, R y is the rotation angle around the Y-axis, and R z is the rotation angle around the Z-axis.

[0015] The relationship formula between the spatial position of the feature block in the B step and the origin of the robot base coordinate system is as follows:

[0016]

[0017] wherein, represents the spatial position transformation relationship between the TCP and the origin of the robot base coordinate system; is obtained from the end pose parameters of the industrial robot; is an unknown quantity, corresponding to the spatial transformation relationship between the feature block and the robot six-axis flange, that is, the actual TCP to be solved after calibration, and the initial value is the manually established TCP.

[0018] The Steepest Descent method is used to iteratively solve the C step as follows: ① select the initial point Tcp 0 , that is, the manually established TCP, give the termination error ε>0, and let k:=0; ② the robot moves to the three teaching scanning points after switching to the latest TCP, wherein the value of TCP in the first iteration is the manually established initial , and the camera is triggered respectively; ③ the vision system completes recognition and positioning and obtains the spatial coordinates of the feature blocks corresponding to the three scanning points in the camera coordinate system P o1 , P o2 , and P o3 , respectively: P O1 =(x,y,z), P O2 =(x',y',z'), and P O3 =(x",y",z"); ④ calculate the spatial distance differences between P O1 , P O2 , and P O3 in the x direction, the y direction, and the z direction, respectively, and the formula is as follows:

[0019]

[0020] ​​​​​​​​V. If the spatial distance differences are all less than the preset spatial distance difference threshold or the maximum number of iterations is reached, the iteration loop is exited; VI. The spatial distance difference calculated in step IV is multiplied by a preset step length to calculate the compensation amount of the TCP and the compensation amount is compensated to the TCP, and the formula is as follows:

[0021]

[0022] wherein, a is a learning rate, P 01 , P 02 , P 03 is a spatial coordinate;

[0023] VII. The compensated TCP is first written into the robot control cabinet, and then the step II is performed.

[0024] 5. The method according to claim 1, wherein the specific process of the TCP automatic calibration in the D step is as follows: after completing a round of three times of scanning positioning, a TCP compensation value is obtained by using a Steepest Descent algorithm, and the compensation value is sent to a robot, the robot starts the next round of calibration after adjusting the TCP according to the compensation, moves to three points whose TCPs have been compensated to trigger scanning and recognition positioning respectively to calculate a new TCP compensation value, until a predetermined number of cycles is reached or a spatial distance difference is less than a predetermined threshold; the positioning of the feature block comprises the following steps: a. using point cloud registration; b. using a 6D pose estimation Point Pair Features algorithm for coarse registration; and c. using an Iterative Closest Point algorithm for fine registration.

[0025] The point cloud registration is to input two point cloud data sets P Source and P Target , and output a transformation matrix T, so that the coincidence degree of T(P Source ) and T(P Target ) is as high as possible, the transformation matrix T adopts a rigid transformation, that is, the transformation only contains rotation and translation; the specific steps are as follows: ① at each key point, a feature descriptor is calculated; ② based on the similarity between the feature and the spatial position, a transformation relationship between the two is calculated from the feature descriptor set and the spatial position of the feature descriptor set in the two point cloud data sets; ③ since the data set contains noise, that is, not all corresponding relationships are real and valid, so in the registration process, the corresponding relationship which will have a negative impact on the registration process needs to be discarded; ④ from the corresponding relationship with the highest coincidence degree, a rigid transformation matrix is obtained.

[0026] The specific steps of the PointPairFeatures algorithm include: ① Overall modeling: First, a global model description is performed on the source point cloud, that is, a set of feature vectors and a point pair set corresponding to each feature vector are constructed for the template point cloud; the point pair set for the relative position and posture of two directed points in the PointPairFeatures algorithm is described as follows: According to the PointPairFeatures algorithm, the point pair set is defined as:

[0027] F(m1,m2)=(||d||2,∠(n1,d),∠(n2,d),∠(n1,n2))

[0028] Among them, the normal vectors of points m1 and m2 correspond to n1 and n2, and d is the step size. When building the global model, it is necessary to calculate the feature vectors of all point pairs on the model surface, build a hash table for the feature vectors of all point pairs, and put the point pairs with the same feature vectors together. ② Perform local matching: arbitrarily select a reference point s in the scene point cloud r , its eigenvector is (s r ,s i ), assuming that the reference point is on the surface of the object, if the assumption is correct, there is a point m on the model r , its eigenvector is (m r ,m i ) corresponding to it; ③ align the two reference points that can correspond to each other, and after registration, align the positions and normal vectors of the two at the same time. Move the feature point pair of the model to the origin of a new space through the transformation matrix so that its normal vector axis coincides with the z-axis of the space, and replace the m in the feature vector of the corresponding point of the model with the m in the feature vector of the corresponding point of the model. i Rotate R around the x-axis of the given coordinate system x (αngle) and scene point cloud reference point s r s in the eigenvector i Registration, the transformation matrix of the registration process is defined as:

[0029]

[0030] in, is the transformation matrix T s→g The inverse matrix, s i and m i is the vector in the corresponding eigenvector, R x αngle in (αngle) is the angle, R x Represented as a rotation around the x-axis.

[0031] The local matching uses a voting algorithm: in local matching, an optimal transformation matrix needs to be found in each transformation, so that the most feature points in the scene point cloud can coincide with the model point cloud surface, the pose of the target can be obtained, a two-dimensional array is defined, the number of rows is the number of model point cloud feature points, and the number of columns is the number of rotation angles angle according to the sampling step, and the specific process is as follows: ① In the process of overall modeling, the feature points of the model point cloud are calculated, at this time, the feature point calculation of the scene point cloud also needs to be performed; ② Take the feature points of the scene point cloud as the key to search the hash table calculated in the overall modeling, and find the similar model feature point pairs of the scene point cloud feature points; ③ Calculate the rotation angle angle between the two point pairs, and vote +1 for the corresponding position of the discrete angle in the two-dimensional array; ④ After all the calculations are completed, the model point cloud feature point pair and the rotation angle corresponding to the maximum number of votes can be obtained; after the voting is completed, for each scene point cloud feature point pair, a group of poses that meet the conditions are obtained, and for all the poses, clustering is performed to divide them into multiple groups, and the weighted sum of the scores of all poses in each group is calculated as the score of the group, the score of each pose is the number of votes obtained in the voting link, and the mean value of all poses of the group with the highest score is selected as the final result.

[0032] The flow of the Iterative Closest Point algorithm is as follows: ① Point cloud preprocessing: that is, using the 6D pose estimation Point Pair Features algorithm for coarse registration; ② Matching: finding the nearest point by using the transformation obtained in the previous step; ③ Weighting: adjusting the weight of the corresponding points; ④ Eliminating unreasonable corresponding point pairs; ⑤ Calculating loss; ⑥ Minimizing loss to solve the current optimal transformation; ⑦ Returning to step 2 for iteration until the termination condition of iteration is met;

[0033] In step ②, finding the nearest point refers to using the initial rotation matrix R0, the translation matrix T0, or the R k-1 , T k-1 of the last iteration to find the nearest point in the target point cloud.

[0034] The specific steps of solving the optimal transformation in step ⑥ are as follows: under the condition of known point correspondence, let Let C P and C T denote the centroids of source point cloud and target point cloud respectively, let Let This is a 3x3 matrix, and SVD decomposition of H can get H = U∑V T First, calculate the optimal translation, let N = |P S |, loss is Take the derivative, then we have: Let the derivative be 0, then we have: No matter the value of R, according to the above formula, we can get the optimal t, where R is the rotation matrix, t is the translation matrix, so that the loss is minimized; Secondly, calculate the optimal rotation, through the derivation of the optimal translation, we know that no matter the value of the rotation, we can get the optimal translation by calculating the centroid of the point cloud, first convert the source point cloud and the target point cloud to the centroid coordinates, that is, let Without considering translation, write loss as:

[0035]

[0036] Since the coordinates of the point cloud are determined, minimizing the original loss is equivalent to solving:

[0037]

[0038] That is to solve:

[0039]

[0040] Since The problem is transformed into:

[0041] R * = argmax R trace(P t T RP s )

[0042] Where trace is the trace of the matrix, according to the property of trace trace(AB) = trace(BA), we have trace(P t T RP s ) = trace(RP s P t T )

[0043] Substitute and H = U∑V T into the above formula to get:

[0044] trace(P t T RPs ) = trace(RP s P t T ) = trace(RH) = trace(RU∑V T ) = trace(∑V T RU)

[0045] In view of V, U, R are all orthogonal matrices, so V T RU is also an orthogonal matrix;

[0046] Let matrix

[0047] Then: trace(∑V T RU) = trace(∑M) = σ1m 11 + σ2m 22 + σ3m 33 Where σ is a coefficient, and according to the non-negative property of singular value and the property that the element value of an orthogonal matrix is not greater than 1, it is known that only when M is a unit matrix, trace(∑M) is maximum, that is: V T RU = I, R = VU T , R * = VU T ; Since there is a possibility that |R| = -1, that is, R is a mapping rather than a rotation, the constraint of |R| = 1 is also added; according to the property of matrix determinant, and U, V are orthogonal matrices, it is deduced that |M| = |V T ||U||R| = |V T | = ±1, if |VU T | = 1, |M| = 1, R * = VU T , the optimal rotation can be solved; if |VU T | = -1, |M| = -1, at this time, trace(∑M) is maximum According to the above two cases of |M| = 1 and |M| = -1, it can be obtained:

[0048]

[0049] Advantages of the present application:

[0050] (1) When the robot performs TCP automatic calibration for the first time, the calibration points are taught to ensure that the 3D camera can clearly capture the feature block fixed at the end of the fixture without obstruction when used for automatic TCP calibration. Therefore, when TCP needs to be recalibrated later, the feature block can be fixed to approximately the same position at the end of the fixture to ensure that the 3D camera can capture the feature block clearly without obstruction. The robot program can then automatically run to complete the TCP automatic calibration. This greatly reduces the technical requirements for production line workers. Workers only need to fix the feature block at the end of the fixture and call this function to achieve automatic calibration, thereby simplifying production line maintenance operations and reducing production line downtime.

[0051] (2) In industrial applications, robots themselves have absolute accuracy errors. A skilled technician typically establishes a TCP rotation error of approximately 0.8mm to 1mm. Three-dimensional cameras also have accuracy errors. After hand-eye calibration between the three-dimensional camera and the robot, the calibration error is typically 0.5mm to 1mm. In traditional methods, these errors often overlap. By integrating three-dimensional vision with the robot in the design, the TCP with the minimum distance error is obtained by iteratively compensating for deviations to achieve automatic TCP calibration. This can eliminate errors and improve the stability of the overall system.

[0052] (3) By calculating and locating the feature block position in the 3D camera coordinate system and automatically switching to the latest compensated TCP movement at each iteration, the robot does not involve the forward and inverse solutions of the robot. Therefore, there is no need to consider the slight differences in the forward and inverse kinematic solutions of different robot manufacturers, thus achieving automatic TCP calibration for robots of different brands and models. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 Schematic diagram of overall modeling in the machine vision-based automatic calibration method for the fixture TCP of the present invention;

[0054] Figure 2 This is a hash representation of the Point Pair Features algorithm in the machine vision-based fixture TCP automatic calibration method of the present invention;

[0055] Figure 3 Schematic diagram of the local matching algorithm in the machine vision-based fixture TCP automatic calibration method of the present invention. DETAILED DESCRIPTION

[0056] The following is a detailed description of the automatic calibration method for the TCP of a fixture based on machine vision of the present invention in conjunction with the accompanying drawings and specific embodiments.

[0057] like Figure 1 As shown, the automatic calibration method of the fixture TCP based on machine vision of the present invention includes the following steps:

[0058] A. Establish initial TCP: the robot and vision system complete calibration and the initial TCP is established manually, at this time the robot teaches three points with the same three-dimensional coordinate but different poses, wherein the three points are P R1 =(x, y, z, R x , R y , R z ), P R2 =(x, y, z, R' x , R' y , R' z ), P R3 =(x, y, z, R" x , R" y , R" z ), wherein x, y and z represent three-dimensional coordinates, (R x , R y , R z ), (R' x , R' y , R' z ), (R" x , R" y , R" z ) are different, wherein R represents rotation, R x is the rotation angle around the X axis, R y is the rotation angle around the Y axis, and R z is the rotation angle around the Z axis;

[0059] B. Fix the feature block and teach the scanning points: fix the feature block to the end of the robot gripper, and teach three points with the same three-dimensional coordinate but different poses under the condition that the robot switches to manual establishment of the initial TCP, the spatial position of the feature block and the origin of the robot base coordinate system is related as follows:

[0060]

[0061] wherein, represents the spatial position transformation relationship between the TCP and the origin of the robot base coordinate system; is obtained from the end pose parameters of the industrial robot; is an unknown quantity, corresponding to the spatial transformation relationship between the feature block and the six-axis flange of the robot, that is, the actual TCP to be solved after calibration, the initial value is the TCP established manually;

[0062] C. Obtain the TCP compensation value: obtain the TCP compensation value by the Steepest Descent method, and the iterative solution steps using the Steepest Descent method are as follows ① select the initial point Tcp0 , i.e. the artificially established TCP, given the termination error ε > 0, let k := 0; 2 the robot moves to the three teaching scan points after switching to the latest TCP, and the value of the TCP in the first iteration is the artificially established initial and triggers the camera scanning respectively; 3 the vision system completes the recognition and positioning and obtains the spatial coordinates P o1 ,P o2 ,P o3 of the feature blocks of the corresponding three scan points in the camera coordinate system O1 = (x, y, z), P O2 = (x', y', z'), P O3 = (x", y", z"); 4 calculate the spatial distance differences between P O1 , P O2 and P O3 in the x direction, y direction and z direction respectively, and the formula is as follows:

[0063]

[0064] 5 if the above spatial distance differences are all less than the set spatial distance difference threshold or reach the set maximum number of iterations, exit the iteration loop; 6 multiply the spatial distance differences calculated in step 4 by the preset step length to calculate the compensation amount of the TCP and compensate it into the TCP, and the formula is as follows:

[0065]

[0066] wherein, a is the learning rate, P 01 , P 02 , P 03 are spatial coordinates;

[0067] 7 first write the above compensated TCP into the robot control cabinet, and then go to step 2;

[0068] D. Automatic calibration of TCP: the robot moves back and forth at the three points that have been taught to automatically calibrate the TCP, stops at each point and triggers the vision system to scan and locate the feature blocks, which can be matched and located through the model established in advance. The specific process of automatic calibration of TCP is as follows: after completing a round of three times of scanning and positioning, the TCP compensation value is obtained by the Steepest Descent algorithm, and the compensation value is sent to the robot. The robot adjusts the TCP according to the compensation and starts the next round of calibration, moves to the three points with compensated TCP, triggers the scanning and positioning to calculate the new TCP compensation value, until the predetermined number of cycles is reached or the spatial distance difference is less than the predetermined threshold;

[0069] Positioning the feature blocks comprises the following steps: a. using point cloud registration; the point cloud registration is to input two point cloud data sets P Source and P Target , and output a transformation matrix T, so that the coincidence degree of T(P Source ) and T(P Target ) is as high as possible, and the transformation matrix T adopts rigid transformation, that is, the transformation only contains rotation and translation; the specific steps are: ① at each key point, calculate the feature descriptor; ② based on the similarity between the feature and the spatial position, calculate the transformation relationship between the two from the feature descriptor set and the spatial position of the feature descriptor set in the two point cloud data sets; ③ if the data set contains noise, that is, not all corresponding relationships are real and effective, so in the registration process, the corresponding relationship that will have a negative impact on the registration process needs to be discarded; ④ from the corresponding relationship with the highest coincidence degree, obtain the rigid transformation matrix;

[0070] b. using the 6D pose estimation Point Pair Features algorithm for coarse registration; the specific steps of the Point Pair Features algorithm include: ① overall modeling: first, describe the global model of the source point cloud, that is, construct a feature vector set for the template point cloud and a point pair set corresponding to each feature vector; the point pair set for the relative position and pose of two directed points in the Point Pair Features algorithm is described as follows: according to the Point Pair Features algorithm, the point pair set is described as:

[0071] F(m1,m2)=(||d||2,∠(n1,d),∠(n2,d),∠(n1,n2))

[0072] wherein the points m1 and m2, the normal vectors corresponding to n1 and n2, and d is the step; as shown in Figure 2 , when constructing the global model, the feature vectors of all point pairs on the model surface need to be calculated, a hash table is constructed for all point pair feature vectors, and point pairs with the same feature vector are placed together; ② local matching: as shown in Figure 3 , a reference point s r is randomly selected in the scene point cloud, and the feature vector of the reference point s r is (s i , s r ); assuming that the reference point is on the surface of the object, if the assumption is correct, there is a point m r on the model, and the feature vector of the point m i) corresponding to it; local matching uses a voting algorithm, in local matching, an optimal transformation matrix needs to be found in each transformation, so that the feature points in the scene point cloud can coincide with the model point cloud surface the most, so as to obtain the pose of the target, define a two-dimensional array, the number of rows is the number of model point cloud feature points, and the number of columns is the number of rotation angles angle according to the sampling step, the specific process is as follows: ① in the process of overall modeling, the feature points of the model point cloud are calculated, at this time, the feature point calculation of the scene point cloud also needs to be done; ② take the feature points of the scene point cloud as the key, search the hash table calculated in the overall modeling, and find the similar model feature points of the scene point cloud; ③ calculate the rotation angle angle between the two point pairs, and vote +1 in the corresponding position of the two-dimensional array; ④ after all the calculations are completed, the model point cloud feature point pair and the rotation angle corresponding to the maximum number of votes can be obtained; after the voting is completed, for each scene point cloud feature point pair, we get a group of poses that meet the conditions, for all the poses, cluster them into multiple groups, calculate the weighted sum of the scores of all the poses in each group as the score of the group, and the score of each pose is the number of votes obtained in the voting link, select the mean value of all the poses in the group with the highest score as the final result;

[0073] ③ register the above two reference points that can correspond to each other, after registration, the positions and normal vectors of the two are aligned, move the feature point pair of the model to the origin of a new space through the transformation matrix, so that the normal vector axis coincides with the z axis of the space, and the m i axis of the feature vector of the above model corresponding point is rotated R x (αngle) around the x axis of the given coordinate system r axis of the feature vector of the above model corresponding point is rotated R i axis of the feature vector of the above model corresponding point is rotated R

[0074]

[0075] wherein, is the inverse matrix of the transformation matrix T s→g , s i and m i are vectors in the corresponding feature vectors, R x (αngle) angle in R x represents rotation around the x axis;

[0076] c. Iterative Closest Point algorithm for fine registration; the flow of the Iterative Closest Point algorithm is as follows: ① point cloud preprocessing: that is, coarse registration is performed by using the 6D pose estimation PointPairFeatures algorithm; ② matching: finding the closest point by applying the transformation solved in the previous step; ③ weighting: adjusting the weight of the corresponding points; ④ eliminating unreasonable corresponding point pairs; ⑤ calculating loss; ⑥ minimizing the loss to solve the current optimal transformation; the specific steps for solving the optimal transformation are as follows: under the condition that the corresponding relationship of the known points is known, let respectively represent the centroids of the source point cloud and the target point cloud, let let This is a 3x3 matrix, and SVD decomposition of H can obtain H = U∑V T ; first, the optimal translation is calculated, let N = |P S |, and the loss is Taking the derivative, we have: Let the derivative be 0, then we have: Regardless of the value of R, the optimal t can be obtained according to the above formula, where R is a rotation matrix and t is a translation matrix, so that the loss is minimized; secondly, the optimal rotation is calculated. Through the derivation of the optimal translation, it can be known that regardless of the value of the rotation, the optimal translation can be obtained by calculating the centroid of the point cloud. First, convert the source point cloud and the target point cloud to the centroid coordinates, that is, let Without considering translation, write the loss as:

[0077]

[0078] Since the coordinates of the point cloud are determined, minimizing the original loss is equivalent to solving:

[0079]

[0080] That is, solving:

[0081]

[0082] Since the problem is transformed into:

[0083] R * = argmax R trace(P t T RP s )

[0084] According to the property of trace, trace(AB) = trace(BA), we have trace(P t T RPs )=trace(RP s P t T )Will and H=U∑V T Substituting into the above formula we can get:

[0085] trace(P t T RP s )=trace(RP s P t T )=trace(RH)=trace(RU∑V T )=trace(∑V T RU)

[0086] Since V, U, and R are all orthogonal matrices, V T RU is also an orthogonal matrix;

[0087] Let the matrix

[0088] Then we have: trace(∑V T RU)=trace(∑M)=σ1m 11 +σ2m 22 +σ3m 33 Where σ is the coefficient. According to the non-negative property of singular values ​​and the property that the element value of the orthogonal matrix is ​​not greater than 1, it can be seen that trace(∑M) is the largest only when M is the unit matrix, that is: V T RU=I, R=VU T , we can get R * =VU T Since |R|=-1 is possible, that is, R is a mapping rather than a rotation, we need to add the constraint |R|=1. According to the properties of the matrix determinant and the fact that U and V are orthogonal matrices, we can deduce that |M|=|V T ||U||R|=|V T |=±1,if |VU T |=1, then |M|=1, R * =VU T The optimal rotation can be solved; if |VU T |=-1, then |M|=-1, which maximizes trace(∑M) According to the above two cases of |M|=1 and |M|=-1, we can get:

[0089]

[0090] ⑦ Return to step 2 and iterate until the conditions for terminating the iteration are met; in step ②, finding the nearest point refers to using the initial rotation matrix R0, translation matrix T0 or R obtained in the previous iteration. k-1 、T k-1 Perform spatial transformation on the initial point cloud to obtain a temporary transformed point cloud, which is then compared with the target point cloud to find the nearest neighbor of each point in the source point cloud in the target point cloud. If direct comparison is performed to find the nearest neighbor, the following three acceleration methods are used: a. Set a distance threshold. When the distance between a point in the source point cloud and a point in the target point cloud is less than the set threshold, the corresponding point is considered to have been found without traversing the entire point set. b. Use KD-tree to accelerate the search. c. Adjust the sampling point cloud of the calibration block, that is, after sampling, the interval between points in the template point cloud of the calibration block is greater than the distance between points of the current camera.

[0091] Positioning feature blocks: a. Use point cloud registration; b. Use the 6D pose estimation Point Pair Features algorithm for coarse registration; c. Use the Iterative Closest Point algorithm for fine registration;

[0092] Point cloud registration is to input two point cloud datasets P Source and P Target , output a transformation matrix T such that T(P Source ) and T(P Target ) as high as possible, the transformation matrix T generally adopts a rigid transformation, that is, the transformation only includes rotation and translation; the specific steps are: ① at each key point, calculate the feature descriptor; ② from the feature descriptor set and the spatial position of the feature descriptor set in the two point cloud datasets, calculate the transformation relationship between the two based on the similarity between the features and the spatial position; ③ assuming that the dataset contains noise, that is, not all correspondences are real and valid, so in the process of registration, it is necessary to discard the correspondences that will have a negative impact on the registration process; ④ from the correspondence with the highest degree of overlap, derive the rigid transformation matrix.

[0093] The specific steps of the Point Pair Features algorithm include: ① Overall modeling: First, a global model description is performed on the source point cloud, that is, a set of feature vectors and a point pair set corresponding to each feature vector are constructed for the template point cloud; the point pair set for the relative position and posture of two directed points in the Point Pair Features algorithm is described as follows: According to the definition of the Point Pair Features algorithm, the point pair set is described as follows:

[0094] F(m1,m2)=(||d||2,∠(n1,d),∠(n2,d),∠(n1,n2))

[0095] The normal vectors of points m1 and m2 are n1 and n2 respectively, and d is the step size. When building the global model, the feature vectors of all point pairs on the model surface need to be calculated, a hash table is constructed for the feature vectors of all point pairs, and point pairs with the same feature vectors are put together.

[0096] ② Perform local matching: randomly select a reference point s in the scene point cloud r , its eigenvector is (s r ,s i ), assuming that the reference point is on the surface of the object. If the assumption is correct, there is a point m on the model r , its eigenvector is (m r ,m i ) to correspond to it; align the two reference points that can correspond to each other, and after registration, align the positions and normal vectors of the two at the same time. Move the feature point pair of the model to the origin of a new space through the transformation matrix so that its normal vector axis coincides with the z-axis of the space, and use the reference point m on the model to r For example, the transformation process is through the transformation matrix T m→g M r Move to the given coordinate system origin and rotate it so that its normal vector Coincident with the x-axis of the given coordinate system, similarly, the feature points s of the scene point cloud are r Do the same operation on the transformation matrix of the transformation process is T s→g ; The m in the feature vector of the corresponding point of the above model i Rotate R around the x-axis of the given coordinate system x (α) and the scene point cloud reference point s r s in the eigenvector i Registration, the transformation matrix of the registration process is defined as:

[0097]

[0098] The local matching uses a voting algorithm: in local matching, an optimal transformation matrix needs to be found in each transformation so that the number of feature points in the scene point cloud that overlap with the surface of the model point cloud is the largest, and the target pose can be obtained. At this time, a two-dimensional array is defined, with the number of rows being the number of feature point pairs in the model point cloud and the number of columns being the number of rotation angles α according to the sampling step. The specific process is as follows: ① In the overall modeling process, the feature point pairs of the model point cloud are calculated, and at this time, the feature point pairs of the scene point cloud also need to be calculated;

[0099] ② Use the feature points of the scene point cloud as keys to search the hash table calculated during the overall modeling, and find the model feature point pairs that are similar to the scene point cloud feature point pairs; ③ Calculate the rotation angle α between the two point pairs, and vote +1 for the discrete angle corresponding positions in the two-dimensional array; ④ After all calculations are completed, the model point cloud feature point pair and rotation angle corresponding to the maximum number of votes can be obtained; After the voting is completed, for each scene point cloud feature point pair, we have a set of postures that meet the conditions. For all postures, we divide them into multiple groups through clustering, and calculate the weighted sum of the scores of all postures in each group as a score for the group. The score of each posture is the number of votes obtained by the posture in the voting link, and the average of all postures in the group with the highest score is selected as the final result.

[0100] The process of the Iterative Closest Point algorithm is as follows:

[0101] ① Point cloud preprocessing: that is, using the 6D pose estimation PointPairFeatures algorithm for coarse registration; ② Matching: Apply the transformation solved in the previous step to find the nearest point; ③ Weighting: Adjust the weights of corresponding points; ④ Eliminate unreasonable corresponding point pairs; ⑤ Calculate loss; ⑥ Minimize loss and solve the current optimal transformation; ⑦ Return to step 2 and iterate until the termination condition is met (the error is less than the threshold or the maximum number of iterations is reached). The minimum error here can be the absolute value of the root mean square difference of two adjacent times is less than a certain limit.

[0102] Among them, finding the nearest point in step ② refers to using the initial posture R0, transformation matrix T0 or R k-1 、T k-1 Perform spatial transformation on the initial point cloud to obtain a temporary transformed point cloud, and then compare the temporary transformed point cloud with the target point cloud to find the nearest neighbor of each point in the source point cloud in the target point cloud; if the comparison is done directly to find the nearest neighbor, a double loop is required, and the computational complexity is O(|P s ||P t |), which is time-consuming. Industrial environments have high requirements for cycle time. Therefore, the following three acceleration methods are used in this invention: a. Setting a distance threshold. When the distance between a point in the source point cloud and a point in the target point cloud is less than the set threshold, the corresponding point is considered to have been found, without traversing the entire point set; b. Using KD-tree to accelerate the search; the computational complexity of KD-tree construction is O(Nlog(N)), and the search complexity is usually O(log(N)); c. Adjusting the sampling point cloud of the calibration block according to the distance between the camera points used in the working scene, that is, the interval between the points in the template point cloud of the calibration block after sampling is greater than the distance between the points of the current camera;

[0103] The specific steps of the optimal transformation in the step ⑥ are as follows: for the ICP algorithm, the optimal transformation has a closed-form solution, which can be calculated by means of SVD; in the case of known correspondence of points, let respectively represent the centroids of the source point cloud and the target point cloud, let Let This is a 3 × 3 matrix, and SVD decomposition of H can obtain H = U∑V T ; first, calculate the optimal translation, let N = |P S |, and the loss is Take the derivative, and then have: Let the derivative be 0, and then have: Regardless of the value of R, the optimal t can be obtained according to the above formula, so that the loss is minimized; secondly, calculate the optimal rotation, and it can be known through the derivation of the optimal translation that the optimal translation can be obtained by calculating the centroid of the point cloud regardless of the value of the rotation. For the convenience of calculation, the influence of the translation can be ignored, and the source point cloud and the target point cloud are converted into the centroid coordinates, i.e. let Without considering the translation, the loss is written as:

[0104]

[0105] Since the coordinates of the point cloud are determined, minimizing the original loss is equivalent to solving:

[0106]

[0107] That is, solving:

[0108]

[0109] Since the problem is converted into:

[0110] R * = argmax R trace(P t T RP s )

[0111] According to the property of trace, trace(AB) = trace(BA), there is trace(P t T RP s ) = trace(RP s P t T Substitute and H = U∑V T into the above formula to obtain:

[0112] trace(P t T RP s )=trace(RP s P t T )=trace(RH)=trace(RU∑V T )=trace(∑V T RU)

[0113] Given that V, U, R are all orthogonal matrices, so V T RU is also an orthogonal matrix;

[0114] Let

[0115] Then: trace(∑V T RU)=trace(∑M)=σ1m 11 +σ2m 22 +σ3m 33 According to the non-negative property of singular values and the property that the element value of an orthogonal matrix is not greater than 1, it is known that only when M is a unit matrix, trace(∑M) is maximum, that is: V T RU=I, R=VU T , R * =VU T ; Since there is a possibility that |R|=-1, that is, R is a mapping rather than a rotation, the constraint |R|=1 is also added; According to the properties of matrix determinant, and U, V are orthogonal matrices, it is derived that |M|=|V T ||U||R|=|V T |=±1, if |VU T |=1, then |M|=1, R * =VU T can be solved to obtain the optimal rotation; if |VU T |=-1, then |M|=-1, which makes trace(∑M) maximum According to the above two cases of |M|=1 and |M|=-1, we can get:

[0116]

[0117] E.TCP calibration compensation space conversion: after iterative compensation deviation, the robot needs to switch to the TCP compensated for the new deviation and move to the three points previously taught and trigger the camera scanning and identification respectively.

Claims

1. A method for automatic calibration of TCP of a fixture based on machine vision, characterized in that: The following steps are involved: A. Establishing the Initial TCP: After the robot and vision system are calibrated and the initial TCP is manually established, the robot teaches three points with the same 3D coordinates but different poses. B. Fixing the feature block and teaching scanning points: Fix the feature block to the end of the robot fixture. The robot switches to manually establish the initial TCP and teaches three points with the same 3D coordinates but different postures. C. Calculate the TCP compensation value: Calculate the TCP compensation value using the Steepest Descent method; D. Automatic TCP calibration: The robot moves back and forth at three taught points to automatically calibrate the TCP, stopping at each point and triggering the vision system to scan and locate the feature block; E. Spatial transformation of TCP calibration compensation: After iteratively compensating for deviations, the robot needs to switch to the TCP that has been compensated for the new deviations and move back to the three previously taught points to trigger camera scanning and recognition respectively.

2. The method for automatic TCP calibration of a fixture based on machine vision according to claim 1, characterized in that: The three points P in step A are P R1 =(x,y,z,R x ,R y ,R z ), P R2 =(x,y,z,R' x ,R' y ,R' z ), P R3 =(x,y,z,R” x ,R” y ,R” z ), where x, y, and z values ​​represent three-dimensional space coordinates, (R x ,R y ,R z )、(R' x ,R' y ,R' z )、(R” x ,R” y ,R” z ) The three groups of angles are different, where R represents rotation, R x is the rotation angle around the X axis, R y is the rotation angle around the Y axis, R z is the rotation angle around the Z axis.

3. The method for automatic TCP calibration of a fixture based on machine vision according to claim 1, characterized in that: The relationship between the spatial position of the feature block in step B and the origin of the robot base coordinate system is as follows: in, Indicates the spatial position transformation relationship between TCP and the origin of the robot base coordinate system; Obtained from the end pose parameters of the industrial robot; is the unknown quantity, corresponding to the spatial transformation relationship between the feature block and the robot's six-axis flange, which is the actual calibrated TCP required. The initial value is the manually established TCP.

4. The method for automatic calibration of a fixture TCP based on machine vision according to claim 1, characterized in that: The iterative solution steps using the Steepest Descent method in step C are as follows: ① Select the initial point Tcp 0 , that is, the TCP established manually, given the termination error ε>0, let k:=0; ② After switching to the latest TCP, the robot moves to the three scanning points taught, where the TCP value at the first iteration is the initial value established manually And trigger the camera to scan respectively; ③ The visual system completes the recognition and positioning and obtains the spatial coordinates P of the feature blocks corresponding to the three scanning points in the camera coordinate system o1 ,P o2 ,P o3 They are: P O1 =(x,y,z),P O2 =(x',y',z'),P O3 =(x”,y”,z”);④Calculate P O1 、P O2 and P O3 The spatial distance difference between two pairs in the x-direction, y-direction, and z-direction is as follows: ⑤ If the above spatial distance differences are all less than the set spatial distance difference threshold or reach the set maximum number of iterations, the iteration loop is exited; ⑥ The spatial distance difference calculated in step ④ is multiplied by the preset step size to calculate the TCP compensation amount and compensate it into TCP. The formula is as follows: Among them, α is the learning rate, P 01 、P 02 、P 03 is the spatial coordinate; ⑦ First write the compensated TCP into the robot control cabinet, then go to step ②.

5. The method for automatic calibration of a fixture TCP based on machine vision according to claim 1, characterized in that: The specific process of TCP automatic calibration in step D is as follows: after completing a round of three-scan positioning, the TCP compensation value is obtained by the SteepestDescent algorithm, and the compensation value is sent to the robot. The robot adjusts the TCP according to the compensation and starts the next round of calibration. It moves to the three points of the TCP to which the compensation has been applied and triggers scanning and identification positioning to calculate the new TCP compensation value until the predetermined number of cycles is reached or the spatial distance difference is less than the predetermined threshold; the positioning of the feature block includes the following steps: a. using point cloud alignment; b. using the 6D posture estimation Point Pair Features algorithm for coarse alignment; c. using the Iterative Closest Point algorithm for fine alignment.

6. The method for automatic calibration of a fixture TCP based on machine vision according to claim 5, characterized in that: The point cloud registration is to input two point cloud datasets P Source and P Target , output a transformation matrix T such that T(P Source ) and T(P Target ) as high as possible, and the transformation matrix T adopts a rigid transformation, that is, the transformation only includes rotation and translation; the specific steps are: ① Calculate the feature descriptor at each key point; ② Calculate the transformation relationship between the feature descriptor set and the spatial position of the feature descriptor set in the two point cloud datasets based on the similarity between the features and the spatial positions; ③ If the dataset contains noise, that is, not all correspondences are real and valid, the correspondences that will have a negative impact on the registration process need to be discarded in the registration process; ④ Derive the rigid transformation matrix from the correspondence with the highest overlap.

7. The method for automatic calibration of a fixture TCP based on machine vision according to claim 5, characterized in that: The specific steps of the PointPairFeatures algorithm include: ① Overall modeling: First, a global model description is performed on the source point cloud, that is, a set of feature vectors and a point pair set corresponding to each feature vector are constructed for the template point cloud; the point pair set for the relative position and posture of two directed points in the PointPairFeatures algorithm is described as follows: According to the PointPairFeatures algorithm, the point pair set is defined as: F(m1,m2)=(||d||2,∠(n1,d),∠(n2,d),∠(n1,n2)) Among them, the normal vectors of points m1 and m2 correspond to n1 and n2, and d is the step size. When building the global model, it is necessary to calculate the feature vectors of all point pairs on the model surface, build a hash table for the feature vectors of all point pairs, and put the point pairs with the same feature vectors together. ② Perform local matching: arbitrarily select a reference point s in the scene point cloud r , its eigenvector is (s r ,s i ), assuming that the reference point is on the surface of the object, if the assumption is correct, there is a point m on the model r , its eigenvector is (m r ,m i ) corresponding to it; ③ align the two reference points that can correspond to each other, and after registration, align the positions and normal vectors of the two at the same time. Move the feature point pair of the model to the origin of a new space through the transformation matrix so that its normal vector axis coincides with the z-axis of the space, and replace the m in the feature vector of the corresponding point of the model with the m in the feature vector of the corresponding point of the model. i Rotate R around the x-axis of the given coordinate system x (αngle) and scene point cloud reference point s r s in the eigenvector i Registration, the transformation matrix of the registration process is defined as: in, is the transformation matrix T s→g The inverse matrix, s i and m i is the vector in the corresponding eigenvector, R x αngle in (αngle) is the angle, R x Represented as a rotation around the x-axis.

8. The method for automatic calibration of a fixture TCP based on machine vision according to claim 7, characterized in that: The local matching uses a voting algorithm: in local matching, an optimal transformation matrix needs to be found in each transformation so that the number of feature points in the scene point cloud that can overlap with the surface of the model point cloud is the largest, and the target posture can be obtained. A two-dimensional array is defined, with the number of rows being the number of feature point pairs of the model point cloud and the number of columns being the number of rotation angles according to the sampling step. The specific process is as follows: ① In the process of overall modeling, the feature point pairs of the model point cloud are calculated. At this time, the feature point pairs of the scene point cloud also need to be calculated; ② The feature points of the scene point cloud are used as keys to search the hash table calculated during the overall modeling and find the feature points corresponding to the scene point cloud feature points. Similar model feature point pairs; ③ Calculate the rotation angle αngle between the two point pairs, and vote +1 for the discrete angle corresponding position in the two-dimensional array; ④ After all calculations are completed, the model point cloud feature point pair and rotation angle corresponding to the maximum number of votes can be obtained; After the voting is completed, for each scene point cloud feature point pair, we have obtained a set of postures that meet the conditions. For all postures, we divide them into multiple groups through clustering, and calculate the weighted sum of the scores of all postures in each group as a score for the group. The score of each posture is the number of votes obtained by the posture in the voting link, and the average of all postures in the group with the highest score is selected as the final result.

9. The method for automatic calibration of a fixture TCP based on machine vision according to claim 5, characterized in that: The process of the Iterative Closest Point algorithm is as follows: ① point cloud preprocessing: using the 6D pose estimation PointPairFeatures algorithm for coarse registration; ② matching: applying the transformation solved in the previous step to find the closest point; ③ weighting: adjusting the weights of corresponding points; ④ eliminating unreasonable corresponding point pairs; ⑤ calculating the loss; ⑥ minimizing the loss and solving the current optimal transformation; ⑦ Return to step 2 and iterate until the conditions for terminating the iteration are met; Among them, finding the nearest point in step ② refers to using the initial rotation matrix R0, translation matrix T0 or R obtained in the previous iteration k-1 、T k-1 Perform a spatial transformation on the initial point cloud to obtain a temporary transformed point cloud. This temporary transformed point cloud is then compared with the target point cloud to find the nearest neighbor of each point in the source point cloud in the target point cloud. If the comparison is performed directly to find the nearest neighbor, the following three acceleration methods are used: a. Set a distance threshold. When the distance between a point in the source point cloud and a point in the target point cloud is less than the set threshold, the corresponding point is considered to have been found without traversing the entire point set. b. Use KD-tree to accelerate the search. c. Adjust the sampling point cloud of the calibration block, that is, the interval between points in the template point cloud of the calibration block after sampling is greater than the distance between points of the current camera.

10. The method for automatic calibration of a fixture TCP based on machine vision according to claim 9, characterized in that: The specific steps of solving the optimal transformation in step ⑥ are: when the corresponding relationship of the points is known, set Denote the centroid of the source point cloud and the target point cloud respectively, and let make This is a 3×3 matrix. Performing SVD decomposition on H yields H=U∑V T ,; first calculate the optimal translation, let N=|P S |, loss is Taking its derivative, we have: Let the derivative be 0, then: Regardless of the value of R, the optimal t can be obtained according to the above formula, where R is the rotation matrix and t is the translation matrix, so that the loss is minimized; secondly, calculate the optimal rotation. After the derivation of the optimal translation, it can be seen that no matter how the rotation is taken, the optimal translation can be obtained by calculating the center of mass of the point cloud. First, convert both the source point cloud and the target point cloud to the center of mass coordinates, that is, Without considering translation, the loss can be written as: Since the coordinates of the point cloud are fixed, minimizing the original loss is equivalent to finding: That is to ask: The problem is transformed into: R * =argmax R trace(P t T RP s ) Where trace is the trace of the matrix. According to the property of trace, trace(AB)=trace(BA), then trace(P t T RP s )=trace(RP s P t T ) Will and H=U∑V T Substituting into the above formula we can get: trace(P t T RP s )=trace(RP s P t T )=trace(RH)=trace(RU∑V T )=trace(∑V T RU) Since V, U, and R are all orthogonal matrices, V T RU is also an orthogonal matrix; Let the matrix Then we have: trace(∑V T RU)=trace(∑M)=σ1m 11 +σ2m 22 +σ3m 33 Where σ is the coefficient. According to the non-negative property of singular values ​​and the property that the element value of the orthogonal matrix is ​​not greater than 1, it can be seen that trace(∑M) is the largest only when M is the unit matrix, that is: V T RU=I, R=VU T , we can get R * =VU T Since |R|=-1 is possible, that is, R is a mapping rather than a rotation, we need to add the constraint |R|=1. According to the properties of the matrix determinant and the fact that U and V are orthogonal matrices, we can deduce that |M|=|V T ||U||R|=|V T |=±1If|VU T |=1, then |M|=1, R * =VU T The optimal rotation can be solved; if |VU T |=-1, then |M|=-1, which maximizes trace(∑M) According to the above two cases of |M|=1 and |M|=-1, we can get:

Images