A hand-eye calibration method and device for picking robots based on TOF cameras

Abstract

The present invention discloses a hand-eye calibration method, device and equipment for a harvesting robot based on a TOF camera, including the present invention collecting multiple sets of calibration plate depth maps and corresponding point cloud data, using a TOF depth camera to image the calibration plate and establish a point cloud model, and measuring the coordinate value of the marker point in the robot base coordinate system by center point contact, thereby solving the initial rotation matrix and translation vector of the hand-eye relationship, estimating the residual of each point position, reallocating the weight coefficient of the marker point, and iteratively optimizing the hand-eye parameters. The method is simple to operate, and the calibration plate is easy to make and low in cost. An experimental platform was built using a JAKA six-axis robot and a depth camera. The experimental results show that the method has good stability, is suitable for on-site calibration, has a calibration accuracy of 4mm, and meets the operational accuracy requirements of fruit picking.

Description

Technical Field

[0001] The present invention relates to the field of 3D machine vision technology, and in particular to a hand-eye calibration method, device and equipment for a picking robot based on a TOF camera. Background Art

[0002] At present, fruit and vegetable picking is mainly done by manual labor. However, with the urbanization process, the labor shortage has been aggravated, resulting in a significant increase in the cost of picking fruits and vegetables. Therefore, the development of picking robots that can improve production efficiency and reduce picking costs is an inevitable trend in fruit and vegetable production.

[0003] The working environment of harvesting robots is complex, characterized by numerous uncertainties and significant harvesting challenges. Therefore, an efficient, fast, and stable fruit and vegetable harvesting robot system is urgently needed. Research has shown that the integration of vision and robot positioning, known as hand-eye calibration, is a key technology and prerequisite for enabling active vision and automated harvesting in harvesting robots. The primary goal of hand-eye calibration is to obtain the transformation relationship from the camera to the robot's end-point, facilitating control of the robot's arm to complete the task. Therefore, research on hand-eye calibration is of great significance to harvesting robots.

[0004] In recent years, hand-eye calibration methods based on traditional 2D cameras have been continuously studied, but most of them extract information such as contours and corners from color images for hand-eye calibration. However, the complex outdoor environment (lighting, etc.) makes it difficult for calibration methods using color images to ensure robustness and difficult to reflect the actual size and position relationship of the picked objects. With the development of depth cameras, depth cameras are increasingly being used in the field of visual picking, improving the recognition ability and efficiency of fruit and vegetable picking. In addition, depth cameras are also used in hand-eye calibration. TOF depth camera is a camera with active visual measurement function. Its working principle is to continuously send light pulses to the target, and then use the sensor to receive the light returned from the object. The distance to the target object is obtained by detecting the round-trip flight time of the light pulse. Compared with other 3D cameras, it has the advantages of low price, small size, low power consumption, strong resistance to light interference and fast calculation of depth information, which is very suitable for the application scenario of wild fruit picking.

[0005] From the above, it can be seen that how to use TOF depth cameras to improve the size and position of picking objects is a problem that needs to be solved. Summary of the Invention

[0006] The purpose of the present invention is to provide a hand-eye calibration method for a picking robot based on a TOF camera, which solves the disadvantage of low accuracy of the 2D recognition method in the prior art.

[0007] To solve the above technical problems, the present invention provides a hand-eye calibration method for a picking robot based on a TOF camera, comprising:

[0008] S1: Use the TOF camera to shoot the calibration plate to obtain multiple sets of calibration plate depth images, multiple sets of point cloud information and multiple sets of pose parameters;

[0009] S2: Processing the multiple sets of calibration plate depth images using a circle center positioning sorting algorithm to obtain sub-pixel coordinates of the center marker points of the multiple sets of calibration plate depth images;

[0010] S3: Calculate the mapping relationship between the two-dimensional point cloud coordinate system and the three-dimensional point cloud coordinate system based on the multiple sets of point cloud information, and map the sub-pixel coordinates of the center mark point in each set of calibration plate depth images into point cloud coordinates based on the mapping relationship;

[0011] S4: Convert each set of pose parameters into a rotation and translation matrix from the robot base coordinate system to the end coordinate system to obtain multiple sets of rotation and translation matrices;

[0012] S5: converting the coordinates of the initial circle center marker point in the robot base coordinate system into the coordinates of the circle center marker point in the robot terminal coordinate system according to multiple sets of rotation and translation matrices;

[0013] S6: Calculate the initial hand-eye calibration matrix based on the dual quaternion method, the point cloud coordinates of each group of circle center markers, and the coordinates of the circle center markers in the robot end coordinate system;

[0014] S7: Converting the point cloud coordinates of each group of circle center marker points into the target circle center marker point coordinates in the robot end coordinate system according to the initial hand-eye calibration matrix, and calculating the position residual of each group of circle center marker points in combination with the circle center marker point coordinates in the robot end coordinate system;

[0015] S8: Redistribute the weight coefficients according to the point residuals of each group of center mark points, update the weight coefficients of the initial hand-eye calibration matrix, and return to step S7 for iterative optimization until the preset number of iterations is reached or the error value of the residuals of the two iterations is less than or equal to the error threshold, then stop the iteration and obtain the final hand-eye calibration matrix.

[0016] Preferably, the step S1 includes:

[0017] The robot end is controlled to contact the center mark points of the calibration plate in sequence according to a preset order to obtain the initial center mark point coordinates in the robot base coordinate system.

[0018] Preferably, the step S2 includes:

[0019] Gaussian filtering is used to enhance edge contour information in the multiple sets of depth images to obtain multiple sets of depth images processed by Gaussian filtering.

[0020] Preferably, step S2 includes:

[0021] Extracting edge contours of the multiple sets of depth images processed by Gaussian filtering using a Canny operator to obtain multiple sets of edge contour information;

[0022] Screening the plurality of sets of edge contour information according to a preset area and a preset roundness to obtain all sub-pixel circular target contours;

[0023] Performing ellipse least squares fitting on the outlines of all circular targets to calculate the coordinates of the center positions of the circular targets;

[0024] The sub-pixel coordinates of the circle center marker point of the depth image taken by the robot each time are obtained by using a circle center positioning sorting algorithm based on vector angle and center of mass coordinates.

[0025] Preferably, calculating a mapping relationship from a two-dimensional point cloud coordinate system to a three-dimensional point cloud coordinate system based on the multiple sets of point cloud information, and mapping the sub-pixel coordinates of the center marker point in each set of calibration plate depth images to point cloud coordinates according to the mapping relationship includes:

[0026] According to the principle that each three-dimensional point cloud coordinate corresponds to a pixel in the depth image, the sub-pixel coordinates (u, v) of a circle center marker in the depth image are extracted to obtain the point cloud set N of the circle center marker in the three-dimensional coordinates, and the mapping relationship N = u*Col+Row of the point cloud coordinate system to the three-dimensional system is obtained.

[0027] For each extracted circle center marker point with sub-pixel coordinates P = (u, v), the coordinates of the four surrounding integer pixels are P0 = (u0, v0), P1 = (u0+1, v0), P2 = (u0, v0+1), and P3 = (u0+1, v0+1). With P as the center, the square surrounded by the four pixels is divided into four small rectangular areas. The areas of these four rectangular areas are calculated to obtain the interpolation weight function based on the inverse of the area.

[0028] The four integer pixel coordinates are preliminarily located in their three-dimensional point cloud coordinates through the mapping relationship from the two-dimensional point cloud coordinate system to the three-dimensional system, and the four three-dimensional point cloud coordinates are interpolated according to the interpolation weight function to obtain the point cloud coordinates of each center mark point.

[0029] Preferably, the calculating of the initial hand-eye calibration matrix according to the dual quaternion method, the point cloud coordinates of each group of circle center marker points and the coordinates of the circle center marker points in the robot terminal coordinate system comprises:

[0030] Through the dual quaternion The real part of and the imaginary part And the rotation and translation matrix is obtained:

[0031]

[0032] Among them, R is the rotation matrix and T is the translation matrix;

[0033] Based on the point cloud coordinate set of the circle center marker point and the circle center marker point coordinate set in the robot end coordinate system, the initial hand-eye calibration matrix is obtained:

[0034]

[0035] Among them, w i is the weight coefficient, the initial value is set to 1, n is the number of circle center markers; P CamAll The point cloud coordinate set representing the center point of the circle; P EndAll Represents the coordinate set of the circle center marker point in the robot end coordinate system.

[0036] Preferably, the redistributing weight coefficients according to the position residuals of each group of circle center marker points to update the weight coefficients of the initial hand-eye calibration matrix includes:

[0037] Inputting the position residuals of each group of circle center marker points into a selection weight function, and assigning a new weight coefficient according to the result of the selection weight function;

[0038] Inputting the new weight coefficient into the initial hand-eye calibration matrix to replace the previous weight coefficient to obtain a new hand-eye calibration matrix;

[0039] Among them, the selection weight function is:

[0040]

[0041] Where e i is the position residual of each center mark point, is the absolute average of the position residuals of all circle center marker points, and l is a constant.

[0042] The present invention also provides a device for hand-eye calibration of a picking robot based on a TOF camera, comprising:

[0043] The acquisition module is used to use the TOF camera to shoot the calibration plate to obtain multiple sets of calibration plate depth images, multiple sets of point cloud information and multiple sets of pose parameters;

[0044] A processing module, configured to process the plurality of calibration plate depth images using a circle center positioning sorting algorithm to obtain sub-pixel coordinates of circle center marker points of the plurality of calibration plate depth images;

[0045] a calculation module, configured to calculate a mapping relationship between a two-dimensional point cloud coordinate system and a three-dimensional point cloud coordinate system based on the plurality of sets of point cloud information, and to map the sub-pixel coordinates of the center marker point in each set of calibration plate depth images to point cloud coordinates based on the mapping relationship;

[0046] The calculation matrix module is used to convert each set of posture parameters into the rotation and translation matrix from the robot base coordinate system to the end coordinate system, and obtain multiple sets of rotation and translation matrices;

[0047] A conversion module is used to convert the coordinates of the initial circle center mark point in the robot base coordinate system into the coordinates of the circle center mark point in the robot end coordinate system according to multiple sets of rotation and translation matrices;

[0048] Construct a hand-eye calibration matrix module to calculate the initial hand-eye calibration matrix based on the dual quaternion method, the point cloud coordinates of each group of circle center markers, and the coordinates of the circle center markers in the robot end coordinate system;

[0049] a residual calculation module, configured to convert the point cloud coordinates of each group of circle center markers into the coordinates of the target circle center markers in the robot end coordinate system according to the initial hand-eye calibration matrix, and calculate the position residual of each group of circle center markers in combination with the coordinates of the circle center markers in the robot end coordinate system;

[0050] The iterative optimization module is used to redistribute the weight coefficients according to the point residuals of each group of center mark points, update the weight coefficients of the initial hand-eye calibration matrix, and return to the calculation residual module for iterative optimization until the preset number of iterations is reached or the error value of the residuals of the two previous and subsequent iterations is less than or equal to the error threshold. The iteration is stopped to obtain the final hand-eye calibration matrix.

[0051] The present invention also provides a device for hand-eye calibration of a picking robot based on a TOF camera, comprising:

[0052] The memory is used to store computer programs; the processor is used to implement the steps of the above-mentioned hand-eye calibration method of a picking robot based on TOF camera when executing the computer program.

[0053] The present invention provides a hand-eye calibration method for a harvesting robot based on a time-of-flight (TOF) camera. The method performs multiple rotational and translation transformations, uses a TOF depth camera to image a calibration plate and establish a point cloud model. Multiple sets of calibration plate depth maps and corresponding point cloud data are collected, and the coordinate values of the center marker in the robot's base coordinate system are measured. This method can then calculate the rotation matrix and translation vector of the hand-eye relationship. A center positioning and sorting algorithm based on vector angles and the centroid coordinates of a circular calibration plate is proposed. This algorithm can address the issue of out-of-order center marker extraction, specify a sorting direction, estimate point position residuals, reallocate the weight coefficients of the center marker, and iteratively optimize the hand-eye parameters, thereby improving the accuracy and stability of the harvesting robot's hand-eye calibration. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] In order to more clearly illustrate the embodiments of the present invention or the technical solutions of the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0055] Figure 1 This is a flowchart of the first specific embodiment of the TOF camera-based picking robot hand-eye calibration method provided by the present invention;

[0056] Figure 2 This is a flow chart of a second specific embodiment of the TOF camera-based picking robot hand-eye calibration method provided by the present invention;

[0057] Figure 3 A circular calibration plate used for image acquisition in the present invention;

[0058] Figure 4 Schematic diagram of the circle center positioning and sorting algorithm based on vector angle and centroid coordinates in the present invention;

[0059] Figure 5 This is the result of the circle center positioning sorting in the present invention;

[0060] Figure 6 It is the residual map of the position of the marker points in each direction of the present invention;

[0061] Figure 7 This is a comparison chart of the robustness of the hand-eye calibration method proposed in the present invention and other methods;

[0062] Figure 8 This is a comparison chart of the gross error positioning capabilities of the calibration method proposed in this invention and the traditional dual quaternion method;

[0063] Figure 9 This is a structural block diagram of a device for hand-eye calibration of a picking robot based on a TOF camera provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0064] The core of the present invention is to provide a hand-eye calibration method for a picking robot based on a TOF camera. Through multiple iterative calculation optimizations, the method and device can have greater stability in an outdoor picking environment, making it very suitable for outdoor field calibration, and the calibration accuracy can reach 4mm.

[0065] In order to enable those skilled in the art to better understand the present invention, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments. Obviously, the embodiments described are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

[0066] Please refer to Figure 1 , Figure 1 This is a flowchart of the first specific embodiment of the TOF camera-based picking robot hand-eye calibration method provided by the present invention; the specific operating steps are as follows:

[0067] Step S101: Use a TOF camera to shoot the calibration plate to obtain multiple sets of calibration plate depth images, multiple sets of point cloud information, and multiple sets of pose parameters;

[0068] Step S102: processing the multiple sets of calibration plate depth images using a circle center positioning sorting algorithm to obtain sub-pixel coordinates of the center marker points of the multiple sets of calibration plate depth images;

[0069] Step S103: Calculating a mapping relationship between a two-dimensional point cloud coordinate system and a three-dimensional point cloud coordinate system based on the plurality of sets of point cloud information, and mapping the sub-pixel coordinates of the center marker point in each set of calibration plate depth images to point cloud coordinates based on the mapping relationship;

[0070] Step S104: converting each set of posture parameters into a rotation and translation matrix from the robot base coordinate system to the end coordinate system to obtain multiple sets of rotation and translation matrices;

[0071] Step S105: converting the coordinates of the initial circle center marker point in the robot base coordinate system into the coordinates of the circle center marker point in the robot terminal coordinate system according to multiple sets of rotation and translation matrices;

[0072] Step S106: Calculate the initial hand-eye calibration matrix based on the dual quaternion method, the point cloud coordinates of each group of circle center markers, and the coordinates of the circle center markers in the robot terminal coordinate system;

[0073] Step S107: converting the point cloud coordinates of each group of circle center markers into the target circle center marker coordinates in the robot end coordinate system according to the initial hand-eye calibration matrix, and calculating the position residual of each group of circle center markers in combination with the circle center marker coordinates in the robot end coordinate system;

[0074] Step S108: Redistribute the weight coefficients according to the point residuals of each group of center mark points, update the weight coefficients of the initial hand-eye calibration matrix, and return to step S107 for iterative optimization until the preset number of iterations is reached or the error value of the residuals of the two iterations is less than or equal to the error threshold, then stop the iteration and obtain the final hand-eye calibration matrix.

[0075] In this embodiment, the present invention first photographs the calibration plate to obtain multiple sets of calibration plate depth images, multiple sets of point cloud information, and multiple sets of posture parameters, then processes the photographed image, and then converts the image coordinates into three-dimensional point cloud coordinates, and then converts the posture coordinates into a rotation and translation matrix. Based on the rotation and translation matrix, the initial center coordinates of the robot are converted into coordinates under the end of the robot. The hand-eye calibration matrix is constructed according to the dual quaternion method to construct the initial hand-eye calibration matrix, calculate the residual of the coordinates of the marker, and redistribute the coefficients of the hand-eye calibration matrix according to the residual. Repeat the iteration until the preset number of iterations is reached or the residual is less than a threshold, stop the iteration, and obtain the final hand-eye calibration matrix. The present invention uses the residual to update the coefficients of the hand-eye calibration matrix, thereby improving the accuracy and stability of the hand-eye calibration of the picking robot and meeting the accuracy requirements of the fruit picking operation.

[0076] Please refer to Figure 2 , Figure 2 This is a flow chart of the second specific embodiment of the TOF camera-based picking robot hand-eye calibration method provided by the present invention; the specific operation steps are as follows:

[0077] Step S201: Controlling the robot end to contact the circle center mark points of the calibration plate in sequence according to a preset order to obtain the coordinates of the initial circle center mark points in the robot base coordinate system;

[0078] Control the robot's end effector (needle tip) to contact the center points of the four corners of the calibration plate in a fixed order of upper left, upper right, lower left, and lower right. Obtain a set of homogeneous coordinates of the center points in the robot's base coordinate system and record the index number of the points. Please refer to Figure 3 , Figure 3 Circular calibration plate used for image acquisition;

[0079] Teach the robot's posture to ensure that the TOF camera can capture a complete and clear depth image of the calibration plate in each set of postures.

[0080] Step S202: photographing the calibration plate according to the robot's taught posture to obtain multiple sets of calibration plate depth images, point cloud information, and the robot's posture coordinates when photographing;

[0081] Step S203: pre-processing the depth image of the calibration plate, and obtaining the sub-pixel coordinates of the center marker points on the four corners of each set of calibration plates through a circle center positioning and sorting algorithm;

[0082] First, the depth image of the calibration plate is converted into a grayscale image; then the grayscale image is binarized using the maximum inter-class difference method; finally, the dilation and erosion method is used to remove irrelevant information in the grayscale image and only retain the most essential ellipse information.

[0083] S3.1: Define the Gaussian function and use the Gaussian filter to enhance the edge contour information of the area containing the closed circle in the depth image of the calibration plate, where the Gaussian function is defined as Where σ is the standard deviation and u is the mean;

[0084] S3.2: Use the Canny operator to extract the edge contours of the image after Gaussian filtering. Set the area and circularity to filter the edge contours to obtain the contours of all circular targets.

[0085] S3.3: The plane image of the circular target is a plane ellipse. Using the edge contour information obtained in S3.2, perform the least squares fitting of the ellipse. The center position of the circular target can be directly calculated from the obtained ellipse plane equation coefficients. The general equation of the plane ellipse is Ax 2 +Bxy+Cy 2 +Dx+Ey+F=0(2), the pixel coordinates of the ellipse center are and Where x0 and y0 are the pixel coordinates of the ellipse center on the x and y axes respectively, and A, B, C, D, E, and F are the coefficients of the ellipse plane equation;

[0086] S3.4: Use the circle center positioning sorting algorithm based on vector angle and centroid coordinates to make the circle center coordinate reading order consistent. Please refer to Figure 4 and Figure 5 , Figure 4 Schematic diagram of the circle center positioning sorting algorithm based on vector angle and centroid coordinates in the present invention, Figure 5 It is the result after the center positioning and sorting in the present invention.

[0087] Based on the circular contour obtained by Canny edge detection and contour screening, the contour with the most sub-pixel points on the contour is searched, and then the least squares ellipse fitting is performed, that is, the initial point P start ;

[0088] Calculate the Euclidean distances from the centers of other circles to the initial point, sort them in ascending order according to the Euclidean distance, and select the two closest points, i.e., P x0 and P y0 ;

[0089] Find the centroid coordinates P of all circle centers centroid , get three P startis the direction vector of the starting point, according to To determine the positional relationship between vectors: If exist counterclockwise direction; if exist Clockwise direction; if and collinear; from this we can conclude that The positional relationship of the three direction vectors. exist counterclockwise direction, That represents the X direction; exist In the clockwise direction, That represents the Y direction;

[0090] Calculate the angle α between the vector from the center of other circles to the starting point and the X direction, set the angle threshold, and the points that meet the conditions are the reference points in the Y direction. Then sort them in ascending order according to the Euclidean distance from the starting point to get P. y0 and P y1 ;

[0091] Using the reference point in the Y direction as the reference point, calculate the angle β between the vectors from the center of other circles to the reference point and the Y direction. Set the angle threshold, filter out the centers of circles that are collinear with the X direction, and sort them in ascending order based on the Euclidean distance from the reference point.

[0092] Step S204: construct a 2D to 3D relationship between point cloud coordinates based on multiple sets of point cloud information, and map the sub-pixel coordinates of the center marker points on the four corners of each calibration plate to point cloud coordinates based on the relationship.

[0093] S4.1: Based on the principle that each 3D point cloud coordinate corresponds to a pixel in the image, by extracting the pixel coordinates (u, v) of a feature point in the image, we can preliminarily determine the sequence number N of the 3D coordinate corresponding to this pixel in the point cloud set. Assuming the image size is Row×Col, the sequence number N is calculated as N=u*Col+Row.

[0094] S4.2: For each extracted landmark point with sub-pixel coordinates P = (u, v), the coordinates of the four surrounding integer pixels are P0 = (u0, v0), P1 = (u0+1, v0), P2 = (u0, v0+1), and P3 = (u0+1, v0+1). With P as the center, divide the square enclosed by the four pixels into four small rectangular regions. Calculate the areas of these four rectangular regions to obtain an interpolation weight function based on the inverse of the area.

[0095] S4.3: The four integer pixel coordinates in S4.2 are used to preliminarily locate their three-dimensional point cloud coordinates using the method in S4.1, and the four three-dimensional point cloud coordinates are interpolated according to the interpolation weight function to obtain the point cloud coordinates of each landmark point.

[0096] Step S205: converting each set of pose parameters into a rotation and translation matrix from the robot base coordinate system to the end coordinate system;

[0097] Assume the camera pose is: P = [xyz Rx Ry Rz], then the rotation matrix is: R = R x *R y *R z

[0098] in,

[0099]

[0100] The translation matrix is: T = [xyz] T .

[0101] Step S206: converting the coordinates of the initial circle center marker point in the robot base coordinate system into the coordinates of the circle center marker point in the robot terminal coordinate system according to each set of rotation and translation matrices;

[0102] Step S207: using the dual quaternion method, combining multiple sets of circle center point cloud coordinates and the circle center marker coordinates in the robot end coordinate system, to solve the initial hand-eye calibration matrix;

[0103] Step S208: Calculate the difference in the coordinates of the center mark points in the front and rear robot terminal coordinate systems, and then update the coefficients of the hand-eye calibration matrix based on the difference, and perform iterative optimization until the difference in the coordinates of the center mark points in the front and rear robot terminal coordinate systems is less than the error threshold or reaches the preset number of iterations, stop the iteration, and obtain the final hand-eye calibration matrix.

[0104] Expression via dual quaternion: And its properties: satisfy ε 2 = 0 and ε≠0, where and Dual quaternions The real and imaginary parts of , both of which are quaternions, use the real part of the dual quaternion Represents the rotation between coordinate systems, combined with the imaginary part represents the translation between coordinate systems, as shown in formula (7):

[0105]

[0106] Where R is the rotation matrix; T is the translation matrix;

[0107]

[0108] Assume that the coordinate set of the circle center point cloud is P CamAll , the center coordinate set in the robot terminal coordinate system is P EndAll ,

[0109] The mathematical model for solving the initial hand-eye calibration matrix based on the dual quaternion is:

[0110]

[0111] Where w i is the weight coefficient, the initial value is set to 1; n is the number of landmark points.

[0112] Calculate the residuals of the points before and after each group of circle center mark points and compare them with the preset error value. If it is less than or equal to the value, the weight will not be changed. If it is greater than the value, the weight will be modified. Figure 6 It is the residual map of the marker points in various directions of the present invention.

[0113] Choose a weighting function:

[0114]

[0115] where e i is the position residual of each landmark point, is the absolute average value of the position residuals of all landmark points, l is a constant of 2.5, and the weight coefficient obtained by redistribution after each iteration is put into formula (8) for iterative solution. If the number of iterations is reached or the error threshold is reached, the iteration is terminated and the final hand-eye calibration matrix is output.

[0116] In order to verify the performance of the present invention, the hand-eye calibration of the picking robot is carried out by using the method of the present invention (weighted DQ) and the traditional dual quaternion method (DQ), the weighted dual quaternion method with Danish weight function and the LM method as a comparison. The absolute average value of the point residual is used. As an evaluation criterion, the position residual refers to the deviation between the coordinates of the landmark points in the robot end coordinate system converted to the camera coordinate system using the obtained hand-eye calibration matrix and the previously obtained three-dimensional point cloud coordinates. Generally, the smaller the deviation, the higher the hand-eye calibration accuracy. The results are shown in Table 1 below. Compared with other methods, the method of the present invention has higher accuracy.

[0117] Table 1 Algorithms without gross errors

[0118]

[0119] Taking into account the robustness requirements of on-site hand-eye calibration for outdoor picking, the present invention adds (1,0,1), (-1,0,1), (0,-1,0), (1,-1,0) to the point set in the robot base coordinate system in the order of contact positioning mark points. The gross errors in units of mm are simulated due to human factors caused by contact positioning misoperation. The absolute errors of the point positions of each algorithm under different numbers of gross error points are shown in the attached figure. Figure 6 As shown, it can be seen that the LM algorithm, the Danish-DQ algorithm and the weighted DQ algorithm proposed in the present invention have better robustness than the DQ method, but the performance of the method proposed in the present invention is better.

[0120] In order to test the ability of the weighted DQ method proposed in this invention to locate gross errors, the mark point (10, 10, 10) with the sequence number 3 in the point set of the robot base coordinate system is introduced, and the large gross error in units of mm is analyzed by taking the mark point data of the first four groups of calibration plates. Figure 7 It can be seen that the absolute errors of the other three marking points are closer than those of the DQ method under normal data and interference data, and can more effectively locate the position of the gross error, indicating that the weighted DQ algorithm proposed in this invention can more effectively suppress the influence of a single interference data and has a stronger ability to identify gross errors.

[0121] The present invention is also compared with other methods in the prior art, please refer to Figure 7 and Figure 8 , Figure 7 This is a comparison chart of the robustness of the hand-eye calibration method proposed in the present invention and other methods; Figure 8 This is a comparison chart of the gross error positioning capabilities of the calibration method proposed in this invention and the traditional dual quaternion method.

[0122] The present invention proposes a simple hand-eye calibration method for a harvesting robot based on optimization and combined with a time-of-flight camera. This method only requires the robot to perform multiple rotational and translational transformations, use a time-of-flight depth camera to image the calibration plate and establish a point cloud model, collect multiple sets of calibration plate depth maps and corresponding point cloud data, and use the tool center point (TCP) of the actuator to contact-measure the coordinates of the marker points in the robot's base coordinate system. This can then solve the rotation matrix and translation vector of the hand-eye relationship. A circle center positioning and sorting algorithm based on vector angles and the centroid coordinates of a circular calibration plate is proposed. This algorithm can solve the problem of disordered circle center extraction, specify the sorting direction, estimate the point position residuals, redistribute the weight coefficients of the marker points, and iteratively optimize the hand-eye parameters, thereby improving the accuracy and stability of the harvesting robot's hand-eye calibration, with a calibration accuracy of up to 4mm.

[0123] Please refer to Figure 9 , Figure 9 This is a block diagram of a device structure for hand-eye calibration of a picking robot based on a TOF camera provided by an embodiment of the present invention; the specific device may include:

[0124] The acquisition module 100 is used to shoot the calibration plate using a TOF camera to obtain multiple sets of calibration plate depth images, multiple sets of point cloud information, and multiple sets of pose parameters;

[0125] The processing module 200 is configured to process the plurality of calibration plate depth images using a circle center positioning sorting algorithm to obtain sub-pixel coordinates of circle center marker points of the plurality of calibration plate depth images.

[0126] A calculation module 300 is configured to calculate a mapping relationship between a two-dimensional point cloud coordinate system and a three-dimensional point cloud coordinate system based on the plurality of sets of point cloud information, and to map the sub-pixel coordinates of the center marker point in each set of calibration plate depth images to point cloud coordinates based on the mapping relationship;

[0127] The calculation matrix module 400 is used to convert each set of posture parameters into a rotation and translation matrix from the robot base coordinate system to the end coordinate system to obtain multiple sets of rotation and translation matrices;

[0128] A conversion module 500 is used to convert the coordinates of the initial circle center marker point in the robot base coordinate system into the coordinates of the circle center marker point in the robot terminal coordinate system according to multiple sets of rotation and translation matrices;

[0129] Constructing a hand-eye calibration matrix module 600, for calculating an initial hand-eye calibration matrix based on the dual quaternion method, the point cloud coordinates of each group of circle center markers, and the coordinates of the circle center markers in the robot end coordinate system;

[0130] A residual calculation module 700 is used to convert the point cloud coordinates of each group of circle center marker points into the target circle center marker point coordinates in the robot end coordinate system according to the initial hand-eye calibration matrix, and calculate the point position residual of each group of circle center marker points based on the center marker point coordinates in the robot end coordinate system;

[0131] The iterative optimization module 800 is used to reallocate the weight coefficients according to the point residuals of each group of center mark points, update the weight coefficients of the initial hand-eye calibration matrix, and return to the calculation residual module for iterative optimization until the preset number of iterations is reached or the error value of the residuals of the two previous and subsequent iterations is less than or equal to the error threshold. The iteration is stopped to obtain the final hand-eye calibration matrix.

[0132] In this embodiment, a device for hand-eye calibration of a picking robot based on a TOF camera is used to implement the aforementioned method for hand-eye calibration of a picking robot based on a TOF camera. Therefore, the specific implementation method of a device for hand-eye calibration of a picking robot based on a TOF camera can be seen in the embodiment part of the method for hand-eye calibration of a picking robot based on a TOF camera in the previous text, for example, the acquisition module 100, the processing module 200, the calculation module 300, the calculation matrix module 400, the conversion module 500, the construction of the hand-eye calibration matrix module 600, the calculation residual module 700 and the iterative optimization module 800 are respectively used to implement steps S101, S102, S103, S104, S105, S106, S107, and S108 in the aforementioned method for hand-eye calibration of a picking robot based on a TOF camera. Therefore, its specific implementation method can refer to the description of the corresponding embodiments of each part, and will not be repeated here.

[0133] A specific embodiment of the present invention also provides a device for hand-eye calibration of a picking robot based on a TOF camera, comprising: a memory for storing a computer program; and a processor for implementing the steps of the above-mentioned method for hand-eye calibration of a picking robot based on a TOF camera when executing the computer program.

[0134] A specific embodiment of the present invention also provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the above-mentioned hand-eye calibration method of a picking robot based on a TOF camera are implemented.

[0135] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from the other embodiments. Reference can be made to the descriptions of the identical or similar parts between the various embodiments. For the devices disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple, and the relevant parts can be referred to the descriptions of the methods.

[0136] Professionals may further appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed in the present invention can be implemented in electronic hardware, computer software, or a combination of the two. In order to clearly illustrate the interchangeability of hardware and software, the above description has generally described the components and steps of each example according to their functions. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professionals and technicians may use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0137] The steps of the methods or algorithms described in conjunction with the embodiments disclosed herein can be implemented directly using hardware, software modules executed by a processor, or a combination of the two. The software modules can be placed in random access memory (RAM), internal memory, read-only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disks, removable disks, CD-ROMs, or any other form of storage medium known in the art.

[0138] The above is a detailed introduction to the hand-eye calibration method, device and equipment for a picking robot based on a TOF camera provided by the present invention. Specific examples are used in the present invention to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core idea. It should be pointed out that for ordinary technicians in this technical field, without departing from the principles of the present invention, the present invention can also be improved and modified in several ways, and these improvements and modifications also fall within the scope of protection of the claims of the present invention.

Claims

1. A hand-eye calibration method for a picking robot based on a TOF camera, characterized in that: include: S1: Use the TOF camera to shoot the calibration plate to obtain multiple sets of calibration plate depth images, multiple sets of point cloud information and multiple sets of pose parameters; S2: Processing the multiple sets of calibration plate depth images using a circle center positioning sorting algorithm to obtain sub-pixel coordinates of the center marker points of the multiple sets of calibration plate depth images; S3: Calculate the mapping relationship between the two-dimensional point cloud coordinate system and the three-dimensional point cloud coordinate system based on the multiple sets of point cloud information, and map the sub-pixel coordinates of the center mark point in each set of calibration plate depth images into point cloud coordinates based on the mapping relationship; S4: Convert each set of pose parameters into a rotation and translation matrix from the robot base coordinate system to the end coordinate system to obtain multiple sets of rotation and translation matrices; S5: converting the coordinates of the initial circle center marker point in the robot base coordinate system into the coordinates of the circle center marker point in the robot end coordinate system according to multiple sets of rotation and translation matrices; S6: Calculate the initial hand-eye calibration matrix based on the dual quaternion method, the point cloud coordinates of each group of circle center markers, and the coordinates of the circle center markers in the robot end coordinate system; S7: Converting the point cloud coordinates of each group of circle center marker points into the target circle center marker point coordinates in the robot end coordinate system according to the initial hand-eye calibration matrix, and calculating the position residual of each group of circle center marker points in combination with the circle center marker point coordinates in the robot end coordinate system; S8: Redistribute the weight coefficients according to the point residuals of each group of center mark points, update the weight coefficients of the initial hand-eye calibration matrix, and return to step S7 for iterative optimization until the preset number of iterations is reached or the error value of the residuals of the two iterations is less than or equal to the error threshold, then stop the iteration and obtain the final hand-eye calibration matrix.

2. The hand-eye calibration method for a picking robot based on a TOF camera according to claim 1, characterized in that: The step S1 includes: The robot end is controlled to contact the center mark points of the calibration plate in sequence according to a preset order to obtain the initial center mark point coordinates in the robot base coordinate system.

3. The hand-eye calibration method for a picking robot based on a TOF camera as claimed in claim 1, characterized in that: The step S2 includes: Gaussian filtering is used to enhance edge contour information in the multiple sets of depth images to obtain multiple sets of depth images processed by Gaussian filtering.

4. The hand-eye calibration method for a picking robot based on a TOF camera as claimed in claim 3, characterized in that: The step S2 comprises: Extracting edge contours of the multiple sets of depth images processed by Gaussian filtering using a Canny operator to obtain multiple sets of edge contour information; Screening the plurality of sets of edge contour information according to a preset area and a preset roundness to obtain all sub-pixel circular target contours; Performing ellipse least squares fitting on the contours of all circular targets to calculate the coordinates of the center positions of the circular targets; The sub-pixel coordinates of the circle center marker point of the depth image taken by the robot each time are obtained by using a circle center positioning sorting algorithm based on vector angle and center of mass coordinates.

5. The hand-eye calibration method for a picking robot based on a TOF camera as claimed in claim 1, characterized in that: The step of calculating a mapping relationship between a two-dimensional point cloud coordinate system and a three-dimensional point cloud coordinate system based on the plurality of sets of point cloud information, and mapping the sub-pixel coordinates of the center marker point in each set of calibration plate depth images to point cloud coordinates according to the mapping relationship includes: Based on the principle that each 3D point cloud coordinate corresponds to a pixel in the depth image, the sub-pixel coordinates (u, v) of a circle center marker point in the depth image are extracted to obtain the point cloud set N of the circle center marker point in the 3D coordinates, and the mapping relationship N = u*Col+Row from the 2D point cloud coordinate system to the 3D point cloud coordinate system is obtained; For each extracted circle center marker point with sub-pixel coordinates P = (u, v), the coordinates of the four surrounding integer pixels are P0 = (u0, v0), P1 = (u0+1, v0), P2 = (u0, v0+1), and P3 = (u0+1, v0+1). With P as the center, the square surrounded by the four pixels is divided into four small rectangular areas. The areas of these four rectangular areas are calculated to obtain the interpolation weight function based on the inverse of the area. The four integer pixel coordinates are preliminarily located in their three-dimensional point cloud coordinates through the mapping relationship from the two-dimensional point cloud coordinate system to the three-dimensional system, and the four three-dimensional point cloud coordinates are interpolated according to the interpolation weight function to obtain the point cloud coordinates of each center mark point.

6. The hand-eye calibration method for a picking robot based on a TOF camera as claimed in claim 1, characterized in that: Calculating the initial hand-eye calibration matrix according to the dual quaternion method, the point cloud coordinates of each group of circle center markers, and the coordinates of the circle center markers in the robot end coordinate system includes: Through the dual quaternion The real part of and the imaginary part And the rotation and translation matrix is obtained: Among them, R is the rotation matrix and T is the translation matrix; Based on the point cloud coordinate set of the circle center marker point and the circle center marker point coordinate set in the robot end coordinate system, the initial hand-eye calibration matrix is obtained: Among them, w i is the weight coefficient, the initial value is set to 1, n is the number of circle center markers; P CamAll The point cloud coordinate set representing the center point of the circle; P EndAll Represents the coordinate set of the circle center marker point in the robot end coordinate system.

7. The hand-eye calibration method for a picking robot based on a TOF camera as claimed in claim 6, characterized in that: The step of reallocating weight coefficients according to the position residuals of each group of circle center marker points to update the weight coefficients of the initial hand-eye calibration matrix includes: Inputting the position residuals of each group of circle center marker points into a selection weight function, and assigning a new weight coefficient according to the result of the selection weight function; Inputting the new weight coefficient into the initial hand-eye calibration matrix to replace the previous weight coefficient to obtain a new hand-eye calibration matrix; Among them, the weight function is selected as: Where e i is the position residual of each center mark point, is the absolute average of the position residuals of all circle center marker points, and l is a constant.

8. A device for hand-eye calibration of a picking robot based on a TOF camera, characterized in that: include: The acquisition module is used to use the TOF camera to shoot the calibration plate to obtain multiple sets of calibration plate depth images, multiple sets of point cloud information and multiple sets of pose parameters; A processing module, configured to process the plurality of calibration plate depth images using a circle center positioning sorting algorithm to obtain sub-pixel coordinates of circle center marker points of the plurality of calibration plate depth images; a calculation module, configured to calculate a mapping relationship between a two-dimensional point cloud coordinate system and a three-dimensional point cloud coordinate system based on the plurality of sets of point cloud information, and to map the sub-pixel coordinates of the center marker point in each set of calibration plate depth images to point cloud coordinates based on the mapping relationship; The calculation matrix module is used to convert each set of posture parameters into the rotation and translation matrix from the robot base coordinate system to the end coordinate system, and obtain multiple sets of rotation and translation matrices; A conversion module is used to convert the coordinates of the initial circle center mark point in the robot base coordinate system into the coordinates of the circle center mark point in the robot end coordinate system according to multiple sets of rotation and translation matrices; Construct a hand-eye calibration matrix module to calculate the initial hand-eye calibration matrix based on the dual quaternion method, the point cloud coordinates of each group of circle center markers, and the coordinates of the circle center markers in the robot end coordinate system; a residual calculation module, configured to convert the point cloud coordinates of each group of circle center markers into the coordinates of the target circle center markers in the robot end coordinate system according to the initial hand-eye calibration matrix, and calculate the position residual of each group of circle center markers in combination with the coordinates of the circle center markers in the robot end coordinate system; The iterative optimization module is used to redistribute the weight coefficients according to the point residuals of each group of center mark points, update the weight coefficients of the initial hand-eye calibration matrix, and return to the calculation residual module for iterative optimization until the preset number of iterations is reached or the error value of the residuals of the two previous and subsequent iterations is less than or equal to the error threshold. The iteration is stopped to obtain the final hand-eye calibration matrix.

9. A device for hand-eye calibration of a picking robot based on a TOF camera, characterized in that: include: memory for storing computer programs; A processor is used to implement the steps of a hand-eye calibration method for a picking robot based on a TOF camera as described in any one of claims 1 to 7 when executing the computer program.

Images