A hand-eye calibration method for a motor-driven rotary scanning device

Abstract

The present invention discloses a hand-eye calibration method for a rotation scanning device driven by a motor. First, color images, depth images, and motor position encoding data are acquired; the obtained color and depth images are used for ICP pose calculation to convert the motor position encoding data into the rotation angle of the motor; finally, the pose relationship equation is solved to obtain the hand-eye pose transformation. The present invention improves the traditional method of hand-eye calibration using a calibration board, and can flexibly perform robust positioning of the camera in many scenarios without relying on the calibration board when performing camera positioning. While simplifying the calibration calculation process, it avoids the error accumulation of the pose relationships between multiple groups of camera-calibration board and hand coordinate system-base coordinate system in the traditional calibration method.

Description

Technical Field

[0001] This document relates to the field of computer vision and proposes a hand-eye calibration method for a rotary scanning device driven by a motor. Background Art

[0002] With the continuous penetration of robot technology into industrial and life scenarios, for the usage requirements of different scenarios, the production and usage technologies of various single robots such as robotic arms, AGV vehicles, and ARMs have become increasingly mature. As an actuator, these single robots can well replace humans to complete some work with high risks, strong repetitiveness, and heavy work intensity, such as cargo box transportation, logistics sorting, and mechanical assembly. However, as a terminal actuator, robots often do not have sensing capabilities. 3D cameras can obtain three-dimensional information of objects and have the ability to sense the spatial position and posture of objects. In most working scenarios, robots often need to cooperate with 3D vision to provide guidance and jointly complete some intelligent work. 3D vision and robots are individual and independent functional modules, and the hand-eye calibration method is to establish a connection between the two; convert the object position information recognized by 3D vision into coordinate values that the robot can understand, and guide the robot to the target position to complete the corresponding work.

[0003] Hand-eye calibration is mainly to establish the conversion relationship between the camera coordinate system and the robot coordinate system. The current main research work on hand-eye calibration methods is on the hand-eye calibration of robotic arms and 3D cameras, which is used to guide robotic arms to perform tasks such as part loading and unloading and logistics palletizing and depalletizing. The existing Eye-in-Hand hand-eye calibration technology mainly uses a calibration board tool for hand-eye calibration. Briefly described, the pose of the calibration board relative to the camera coordinate system is obtained through the image information of the calibration board captured by the camera at different positions of the robotic hand. At the same time, the position relationship between the base coordinate system of the mechanical device and the robotic hand is known. Through the conversion relationships of the base coordinate system, camera coordinate system, hand coordinate system, and calibration board coordinate system, an equation similar to the form AX = XB is obtained, and the equation can be solved by the classic Tsai-Lenz two-step method for robot hand-eye calibration. The existing technology is mainly applied to six-degree-of-freedom robotic arms, so it limits the robot to perform spatial rotation and translation movements in a specific manner. The hand-eye calibration methods proposed by existing researchers either require teaching the robot, which is cumbersome to operate and has low accuracy due to the introduction of the robot's motion error, or require the robot to perform large-amplitude movements, and the calibration of the calibration board and the base coordinate system will also introduce errors. Therefore, there is a great need to propose a method with smaller calibration errors.

[0004] At the same time, the ICP (Iterative Closest Point) algorithm is widely used in the field of 3D reconstruction. This algorithm mainly calculates the pose between two point clouds by iteratively minimizing the error function between the two sets of point clouds. However, when applied to a relatively fast-moving scenario, that is, when the pose transformation between two frames is too large, this algorithm often fails to converge. Therefore, if a better initial value can be proposed for the ICP algorithm before iteration, the convergence effect of the algorithm will be greatly enhanced. When the rotation angle transformation of the motor is obtained, the movement of the camera can be calculated through the pose transformation relationship between the motor and the camera, and once used as the initial value of the ICP algorithm, the convergence efficiency of the algorithm will be greatly improved. Summary of the Invention

[0005] In view of the deficiencies in the prior art, the present invention proposes a hand-eye calibration method for a rotation scanning device driven by a motor (the device is taken as an example). By processing the color and depth images obtained by an RGB-D camera, using ICP to calculate the pose transformation between two frames of the camera, and at the same time processing the motor position encoding information to obtain the rotation angle of the motor corresponding to two frames of the camera, so as to generate the pose transformation of the motor coordinate system between two frames of the camera. Furthermore, the corresponding relationship between the camera pose transformation and the motor pose transformation is deduced, and the pose transformation between the camera coordinate system and the motor coordinate system is calculated by solving the hand-eye calibration equation. Figure 3 A hand-eye calibration method for a rotation scanning device driven by a motor, the steps are as follows:

[0006] S1. Synchronously acquire and save the color, depth images and motor position encoding data;

[0007] S2. Use the obtained color and depth images for ICP pose calculation;

[0008] S3. Convert the motor position encoding data into the rotation angle of the motor;

[0009] S4. Solve the pose relationship equation to obtain the hand-eye pose transformation.

[0010] The beneficial effects of the present invention are as follows:

[0011] The present invention improves the traditional method of using a calibration board for hand-eye calibration. When performing camera positioning, it can flexibly perform robust positioning of the camera in many scenarios without relying on a calibration board. While simplifying the calibration calculation process, it avoids the error accumulation of the pose relationships between multiple sets of camera-calibration board and hand coordinate system-base coordinate system in the traditional calibration method.

[0012] BRIEF DESCRIPTION OF THE DRAWINGS BRIEF DESCRIPTION OF THE DRAWINGS

[0013] Figure 1 It is a block diagram of an instantiated system for the method of the embodiment of the present invention;

[0014] Figure 2 Schematic diagram of a system for hand-eye calibration;

[0015] Figure 3 Example of a camera rotation data acquisition structure driven by a motor.

[0016] Specific implementation steps

[0017] The technical solution of the present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0018] As Figure 1 shown, a hand-eye calibration method for a rotation scanning device driven by a motor is as follows:

[0019] S1. Synchronously acquire and save image and position encoding data;

[0020] S11. Acquisition of image data.

[0021] Use an RGBD camera to capture the color image and depth image of the scene, with a collection frequency of 15HZ. Record the timestamp of the current image when the first frame of the image is obtained, and use this as the starting point for data collection. Perform distortion removal processing on each pair of color and depth images using the internal parameter information obtained by calibration, and align the depth map to the color map using the known pose transformation relationship between the color and depth cameras to facilitate the subsequent ICP solution process. Name the obtained color and depth maps according to their corresponding timestamps, and save the image data and its timestamp information.

[0022] S12. Acquisition of position encoding data.

[0023] The rotation scanning device completes the control and data communication tasks for the motor driving the rotation through an embedded chip (STM32). The motor transmits the position encoding data measured by the position sensor to the embedded chip through the CAN bus. The data collection frequency is 200HZ. Obtain the corresponding relationship between the motor and the camera in time through the timestamp of the position encoding data of the motor, and at the same time save the position encoding data of the motor and the corresponding timestamp.

[0024] Figure 3 Example of a camera rotation data acquisition structure driven by a motor.

[0025] S2. Perform ICP pose solution using the obtained color and depth maps;

[0026] S21. For each pair of adjacent two frames of images, generate a 3-layer image pyramid, filter the depth map, and perform point cloud registration in a Coarse-to-Fine manner.

[0027] S22. Calculate the three-dimensional coordinates of each point in the image collected by the depth camera (for point cloud registration and fusion) given the internal parameters of the depth image. Also calculate the three-dimensional point cloud coordinates for the filtered depth image (for calculating normal vectors).

[0028] S23. Determine the initial predicted value of the current frame camera pose and iteratively solve for the camera pose. The predicted value for the start of the iteration is set to the optimal pose of the previous frame, or the identity matrix if the current frame is the first frame; the predicted value during the iteration is set to the optimal pose obtained from the previous iteration. Use this predicted value to transform the coordinates of the three-dimensional points in the corresponding frame.

[0029] S24. Calculate the matching points for two point clouds according to the projection algorithm.

[0030] S25. Minimize the distance from the point to the plane for the matching points to calculate the pose. The error function is defined as follows:

[0031]

[0032] S26. Stop the iteration when the error of the objective function is less than the set threshold or when the set number of iterations is reached; otherwise, return to step S23.

[0033] S27. Iterate until the set number of iterations is reached or the above objective function is less than the set threshold through non-linear optimization. Record the pose transformation between two adjacent frames.

[0034] S28. Obtain the pose transformation of each frame relative to the first frame and save it as a txt file together with the timestamp of this frame.

[0035] S3. Angle conversion. Convert the obtained motor position encoding data into angle data. The encoding from 0 - 8191 corresponds to 0 - 360 degrees. Save the angle data and the corresponding timestamp as a txt file.

[0036] S4. Solve the pose relationship equation to obtain the hand-eye pose transformation.

[0037] S41. Read the timestamped motor rotation angle data recorded in step S3, and read the pose transformation of each frame of the camera relative to the starting frame and the corresponding timestamp obtained in step S28. Since the sampling frequency of the motor is higher than that of the camera, the rotation of the camera between two frames can be determined through the correspondence between the motor timestamp and the camera timestamp, that is, by counting the angle data between two frames of the camera to obtain the rotation angle of the motor between the two frames.

[0038] S42. Determine the pose transformation equation to be solved based on the Eye In Hand calibration model;

[0039] The Eye In Hand calibration model needs to determine four coordinate systems: the camera coordinate system, the hand coordinate system, the calibration board coordinate system, and the base coordinate system. Using Bese T end2 to represent the pose transformation of the hand coordinate system of the (i + 1)-th frame of the camera with respect to the base coordinate system, and using end2 T cam2 to represent the pose transformation of the camera coordinate system of the (i + 1)-th frame of the camera with respect to the hand coordinate system, and using cam2 T Object to represent the pose transformation of the calibration board coordinate system with respect to the (i + 1)-th frame coordinate system of the camera, the pose transformation relationship between the (i + 1)-th frame and the previous i-th frame of the camera can be obtained as follows:

[0040] Base T end2 × end2 T cam2 × cam2 T Object

[0041] = Base T end1 × edn1 T cam1 × cam1 T Object

[0042] Simplifying the above formula gives:

[0043] end1 T end2 × end2 T cam2 = end1 T cam1 × cam1 T cam2

[0044] In the formula end1 T end2 is the conversion relationship from the (i + 1)-th frame to the i-th frame of the hand coordinate system. Assuming that the y-axis of the hand coordinate system is the vertical direction and the rotation is the positive direction, a pose transformation matrix for rotating by θ around the y-axis is generated according to the motor rotation angle θ between two frames:

[0045]

[0046] In the formula cam1 T cam2 is the conversion relationship from the (i + 1)-th frame to the i-th frame of the camera, and a pose transformation matrix is calculated through the rotation angle of the motor between two frames of the camera obtained in step S41.

[0047] Figure 2 is a schematic diagram of the system for hand-eye calibration;

[0048] The pose transformation between the camera and the motor is obtained by solving the pose transformation equation using the Tsai-Lenz method in S43.

[0049] For simplicity of explanation and without loss of generality, g is used to represent the hand, c is used to represent the camera, and i, j represent the i-th and j-th frames of the camera. The pose transformation equation is solved according to the following steps.

[0050] ① Convert the rotation matrix to a rotation vector using the Rodriguez formula.

[0051]

[0052] R gij and R cij are the rotation transformation parts of the pose transformation matrices of the i-th and j-th frames in the hand coordinate system and the camera coordinate system respectively, and r gij , r cij are the corresponding rotation vectors.

[0053] ② Normalize the rotation vector.

[0054]

[0055] θ gij and θ cij represent the rotation angles of the hand coordinate system and the camera coordinate system corresponding to the i-th and j-th coordinate systems, and N represents the normalized rotation vector.

[0056] ③ Represent the attitude transformation using the modified Rodriguez parameters.

[0057] P gij = 2sin(θ gij / 2)N rgij

[0058] P cij = 2sin(θ cij / 2)N rcij

[0059] P gij , P cij represent the rotation axes of the rotation transformation between the hand coordinate system and the camera coordinate system for the i-th and j-th frames respectively.

[0060] ④ Calculate P' cg ;

[0061] Use P cg to represent the rotation axis of the rotation matrix P cg between the camera coordinate system and the hand coordinate system.

[0062] Assume a P' cg , and there is the following relationship:

[0063]

[0064] ⑤ Solve the following equation:

[0065] Skew(P gij +P cij )P′ cg =P cij -P gij

[0066] where Skew represents transformation into an anti-symmetric matrix. Since P gij and P cij are known, this equation is equivalent to a problem of solving the form AX = XB. If there are multiple sets of data, an optimal solution can be obtained.

[0067] ⑥ Calculate P cg ;

[0068]

[0069] ⑦ Use Rodrigues' formula to obtain the rotation matrix R cg between the camera coordinate system and the hand coordinate system;

[0070]

[0071] ⑧ Calculate the translation vector T cg ;

[0072] (R gij -I) = R cg T cij -T gij

[0073] The above R cg and T cg are the motor-camera hand-eye poses obtained by calibration.

[0074] The above content is a further detailed description of the present invention in combination with specific / preferred embodiments. It cannot be determined that the specific implementation of the present invention is limited only to these descriptions. For those of ordinary skill in the technical field to which the present invention pertains, without departing from the concept of the present invention, they can make several substitutions or modifications to these described embodiments, and these substitution or modification methods should all be regarded as belonging to the protection scope of the present invention.

[0075] The parts not detailed in the present invention are well-known technologies in the art.

Claims

1. A hand-eye calibration method for a motor-driven rotary scanning device, characterized in that, The steps are as follows: S1. Synchronously acquire and save color images, depth images, and motor position encoding data; S2. Use the obtained color and depth images for ICP pose calculation; S3. Convert the motor position encoding data into the rotation angle of the motor; S4. Solve the pose relationship equation to obtain the hand-eye pose transformation; The specific method of S2 is as follows; S21. For each pair of adjacent two frames of images, generate a 3-layer image pyramid, filter the depth image, and perform point cloud registration in a Coarse-to-Fine manner; S22. Given the internal parameters of the depth image, calculate the three-dimensional coordinates of each point in the image collected by the depth camera, and also calculate the three-dimensional coordinates of the point cloud for the filtered depth image; S23. Determine the initial predicted value of the current frame camera pose, and iteratively solve the camera pose; the predicted value for starting the iteration is set to the optimal pose of the previous frame, and if the current frame is the first frame, it is set to the identity matrix; the predicted value during the iteration process is set to the optimal pose obtained in the previous iteration; use this predicted value to transform the three-dimensional coordinates of the points in the corresponding frame; S24. Calculate the matching points for the two point clouds according to the projection algorithm; S25. Minimize the distance from the point to the plane for the matching points to calculate the pose, and the error function is defined as follows: S26. When the error of the objective function is less than the set threshold, or when the set number of iterations is reached, stop the iteration, otherwise return to step S23; S27. Iterate until the set number of iterations is reached, or until the above objective function is less than the set threshold through nonlinear optimization; record the pose transformation between adjacent two frames; S28. Obtain the pose transformation of each frame relative to the first frame, and save it together with the timestamp of this frame as a txt file.

2. The hand-eye calibration method of a rotation scanning device driven by a motor according to claim 1, characterized in that The specific method of S1 is as follows; S11. Acquisition of image data; Use an RGBD camera to capture color images and depth images of the scene, with a capture frequency of 15HZ; record the timestamp of the current image when the first frame of image is obtained, and use this as the starting point for data acquisition; perform distortion removal processing on each pair of color and depth images using the internal parameter information obtained by calibration, and align the depth image to the color image using the known pose transformation relationship between the color and depth cameras; name the obtained color and depth images according to their corresponding timestamps, and save the image data and its timestamp information; S12. Acquisition of position encoding data; The rotation scanning device completes the tasks of controlling and data communication for the motor that drives the rotation through an embedded chip. The motor transmits the position encoding data measured by the position sensor to the embedded chip through the CAN bus. The data acquisition frequency is 200HZ. Obtain the corresponding relationship between the motor and the camera in terms of time through the timestamp of the motor position encoding data, and at the same time save the motor position encoding data and the corresponding timestamp.

3. A hand-eye calibration method for a motor-driven rotary scanning device according to claim 2, characterized in that, The specific method of S3 is as follows; Convert the obtained motor position encoding data into angle data. The encoding from 0 to 8191 corresponds to 0 to 360 degrees, and save the angle data and the corresponding timestamp as a txt file.

4. The hand-eye calibration method of a rotation scanning device driven by a motor according to claim 3, characterized in that The specific method of S4 is as follows; S41. Read the timestamped motor rotation angle data recorded in step S3, and read the pose transformation of each frame of the camera relative to the starting frame and the corresponding timestamp obtained in step S28; since the sampling frequency of the motor is higher than that of the camera, the rotation between two frames of the camera can be determined through the correspondence between the motor timestamp and the camera timestamp, that is, the rotation angle of the motor between two frames is obtained by counting the angle data between two frames of the camera. S42. Determine the pose transformation equation to be solved based on the EyeIn Hand calibration model. The EyeIn Hand calibration model needs to determine four coordinate systems: the camera coordinate system, the hand coordinate system, the calibration board coordinate system, and the base coordinate system. Using Base T end2 to represent the pose transformation of the hand coordinate system of the (i + 1)-th frame of the camera with respect to the base coordinate system, and using end2 T cam2 to represent the pose transformation of the camera coordinate system of the (i + 1)-th frame of the camera with respect to the hand coordinate system, and using cam2 T Object to represent the pose transformation of the calibration board coordinate system of the (i + 1)-th frame of the camera with respect to the camera coordinate system, the pose transformation relationship between the (i + 1)-th frame and the previous i-th frame of the camera is obtained as follows: Base T end2 × end2 T cam2 × cam2 T Object = Base T end1 × end1 T cam1 × Cam1 T Object Simplify the above formula to obtain: end1 T end2 × end2 T cam2 = end1 T cam1 × cam1 T cam2 where end1 T end2 is the transformation relationship from the (i + 1)-th frame to the i-th frame of the hand coordinate system. Assuming that the y-axis of the hand coordinate system is the vertical direction and the rotation is in the positive direction, a pose transformation matrix for rotating by θ around the y-axis is generated according to the motor rotation angle θ between the two frames: where cam1 T cam2 is the transformation relationship from the (i + 1)-th frame to the i-th frame in the camera coordinate system, and the pose transformation matrix is calculated by the rotation angle of the motor between two frames of the camera obtained through step S41; S43. Use the Tsai-Lenz method to solve the pose transformation equation to obtain the camera-motor pose transformation. For simplicity of explanation and without loss of generality, use g to represent the hand, c to represent the camera, and i, j to represent the i-th and j-th frames of the camera; solve the pose transformation equation according to the following steps. ① Use the Rodriguez formula to convert the rotation matrix into a rotation vector. P gij and R cij are respectively the rotation transformation parts of the pose transformation matrices of the i-th and j-th frames in the hand coordinate system and the camera coordinate system, and r gij , r cij are the corresponding rotation vectors; ② Normalize the rotation vector. θ gij 、 θ cij represent the rotation angles of the hand coordinate system and the camera coordinate system corresponding to the i-th and j-th frames, and N represents the normalized rotation vector; ③ Use the modified Rodriguez parameters to represent the attitude transformation. P gij = 2sin(θ gij / 2)N rgij P cij = 2sin(θ cij / 2)N rcij P gij and P cij respectively represent the rotation axes of the rotational transformations between the hand coordinate systems of the i-th and j-th frames and the camera coordinate system; ④ Calculate P' cg ; Use P cg to represent the rotation matrix R between the camera coordinate system and the hand coordinate system cg as the rotation axis, Assume a P' cg , there is the following relationship: ⑤ Solve the following equation: Skew(P gij +P cij )P' cg =P cij -P gij where Skew represents the transformation into an anti-symmetric matrix, and P gij , P cij are known, and the optimal solution can be obtained if there are multiple sets of data; ⑥ Calculate P cg ; ⑦ Use the Rodriguez formula to obtain the rotation matrix R between the camera coordinate system and the hand coordinate system cg ; ⑧ Calculate the translation vector T cg ; (R gij -I)T cg =R cg T cij -T gij The above-mentioned R cg and T cg are the motor-camera hand-eye poses calibrated.

Images