Underwater manipulator autonomous calibration method and system based on binocular camera

Abstract

The invention relates to the field of manipulator calibration, in particular to a method for realizing autonomous calibration of an underwater manipulator by using a binocular camera, which adopts the underwater binocular camera to be combined with an image recognition technology to establish a constraint relation by recognizing a feature object. And a kinematics and dynamics model is constructed, trajectory planning is carried out, the manipulator moves according to a specified trajectory, all motion angles are covered as much as possible, and a data set needed by error calculation is obtained. And then iterative identification and compensation of errors are carried out to obtain an accurate kinematics model, and calibration is completed by checking precision. According to the method, the positioning precision of the underwater manipulator is improved, and strict equipment mounting position information is not needed in the calibration process.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater robots, and particularly relates to a method and system for autonomous calibration of an underwater manipulator based on binocular vision, which is particularly suitable for autonomous calibration of an underwater manipulator, thereby improving the positioning accuracy of the manipulator. Background Art

[0002] With the continuous warming of global exploration and development activities of marine resources, the ocean has become a key area for humans to expand living space and obtain important resources, and the importance of underwater operations has become increasingly prominent. Among many underwater operation technical means, underwater robots, as core equipment, play a key role in many fields such as ocean exploration and resource development. As a core operation tool of underwater robots, underwater manipulators have played a crucial role in ocean exploration and resource development. The operation accuracy, as the core index of underwater manipulators, is directly related to the quality of the entire underwater operation. Therefore, improving the operation accuracy of underwater manipulators has become a crucial research content in this field, and the calibration of manipulators is an important means to ensure the operation accuracy of manipulators. At present, the calibration methods for onshore manipulators are very mature, but they are closely related to factors such as laboratory environment layout, sensor accuracy, and calibration personnel's technology. The calibration work of underwater manipulators usually draws on the calibration methods of onshore manipulators. However, during the long-term operation and service of underwater manipulators, due to underwater temperature changes, water flow impact, operation overload, collision and wear, etc., the parameters obtained by land-based calibration cannot always ensure that the accuracy is within the required range, resulting in a continuous decline in the operation accuracy of the manipulator. For underwater equipment, especially equipment that operates and serves underwater for a long time, it is difficult to regularly transfer the delivered manipulator to the laboratory for routine calibration operations. Summary of the Invention

[0003] The purpose of the present invention is to provide a method for autonomous calibration of a manipulator based on a binocular camera. In the process of this calibration method, strict equipment installation position information is not required, and no manual participation is required during the calibration process. By using binocular vision and image recognition to obtain a data set, and then through iterative calculation of the compensation value by matching with point cloud and error recognition function, the kinematic model of the manipulator is corrected, thereby realizing the autonomous calibration of the underwater manipulator, which has important practical significance for maintaining the long-term operation accuracy of the underwater manipulator.

[0004] The technical solution adopted by the present invention to achieve the above purpose is as follows:

[0005] An autonomous calibration method for an underwater manipulator based on a binocular camera, comprising the following steps:

[0006] 1) Calibrate the parameters of the underwater binocular camera using the Zhang-Zhengyou calibration method, and construct the MDH model of the underwater manipulator;

[0007] 2) Obtain the actual position and nominal position of the manipulator through an underwater binocular camera and the MDH model respectively, and construct an error data set;

[0008] 3) Use the error data set for error identification and perform iterative compensation based on the ICP matching result;

[0009] 4) Check the calibration accuracy according to the compensated kinematic parameters. When the requirements are met, exit the iteration and complete the autonomous calibration.

[0010] The step 2) includes the following steps:

[0011] 2.1) Use the underwater binocular camera to perform image recognition on the target object at the end of the manipulator, and obtain the three-dimensional coordinates of the target object in the camera coordinate system;

[0012] 2.2) Through coordinate transformation, convert the three-dimensional coordinates of the target object to the manipulator base coordinate system to generate an actual position data set;

[0013] 2.3) Based on the actual position data set, use the forward solution of the MDH model to obtain the nominal end position of the manipulator, subtract it from the measured actual end position of the manipulator, and save it as the error data set.

[0014] The step 2.2) is specifically:

[0015] Use the manipulator to grab a spherical object, perform image recognition using the left side image of the binocular camera, obtain the three-dimensional coordinates (x, y, z) of each pixel on the sphere surface, and then calculate the coordinates (X, Y, Z) of the sphere center, which is the position information of the manipulator end center, and use it as the actual position of the manipulator, where:

[0016]

[0017] where R is the radius of the spherical object.

[0018] The step 3) includes the following steps:

[0019] 3.1) Perform point cloud matching on the nominal position data set and the actual position data set, and calculate the coordinate transformation matrix;

[0020] 3.2) According to the error identification function, calculate the compensation value of the manipulator kinematic parameters;

[0021] 3.3) Update the kinematic parameters of the manipulator according to the compensation value, and re-perform point cloud matching. Iteratively perform steps 3.1) - 3.3) until the error converges.

[0022] An underwater manipulator autonomous calibration system based on a binocular camera, comprising:

[0023] The parameter calibration and model construction module is used to calibrate the parameters of the underwater binocular camera using the Zhang-Zhengyou calibration method and construct the MDH model of the underwater manipulator;

[0024] The error data set construction module is used to obtain the actual position and nominal position of the manipulator through the underwater binocular camera and the MDH model respectively, and construct an error data set;

[0025] The error identification module is used to identify errors using the error data set and perform iterative compensation based on the ICP matching result;

[0026] The self-calibration module is used to test the calibration accuracy according to the compensated kinematic parameters. When the requirements are met, the iteration is terminated and the self-calibration is completed.

[0027] The present invention has the following beneficial effects and advantages:

[0028] 1. The present invention improves the positioning accuracy of the underwater manipulator, and does not require strict equipment installation position information during the calibration process.

[0029] 2. Compared with other calibration methods, the present invention can be carried out without human participation throughout the process, providing a solution for manipulators serving underwater for a long time. Brief Description of the Drawings

[0030] Figure 1 is the self-calibration workflow diagram provided by the present invention;

[0031] Figure 2 is the auxiliary explanatory diagram of the calculation principle provided by the present invention;

[0032] Figure 3 is the calibration calculation principle diagram provided by the present invention. Detailed Embodiment

[0033] The following further describes the present invention in detail with reference to the drawings and embodiments.

[0034] Figure 1 The shown is the workflow diagram of the present invention. An autonomous calibration method for an underwater manipulator based on binocular vision and image recognition includes the following steps:

[0035] 1) Calibrate the parameters of the underwater binocular camera using the Zhang-Zhengyou calibration method to obtain the internal and external parameters of the camera. And construct the MDH model of the underwater manipulator to provide a basis for the subsequent calibration process.

[0036] 1.1) Collect the left and right images of the calibration board in different poses underwater, and calibrate the binocular camera using the Zhang-Zhengyou calibration method;

[0037] 1.2) Model the manipulator using the MDH (Modified Denavit-Hartenberg) method to obtain the kinematic parameters of the manipulator.

[0038] 2) Obtain the actual position of the manipulator end through binocular depth information and image recognition, establish a dataset of the actual positions of the manipulator end, subtract it from the dataset of the nominal positions of the manipulator obtained by forward kinematics to obtain error data, and then save it as a data pair with the joint angles at this moment. Multiple data pairs form a dataset for subsequent processing.

[0039] 2.1) Use a binocular camera to perform image recognition on the target object at the end of the manipulator to obtain the three-dimensional coordinates of the target object in the camera coordinate system;

[0040] 2.2) Through coordinate transformation, convert the three-dimensional coordinates of the target object to the base coordinate system of the manipulator to generate a dataset of actual positions.

[0041] 3) Perform error recognition on the dataset. After the initial calculation, perform ICP matching. After updating the transformation matrix, perform error recognition again. Iteratively compensate the kinematic parameters of the manipulator through the dataset until the error converges to a set value or reaches the time limit, and then stop the iteration and output the corrected value.

[0042] 3.1) Perform point cloud matching on the dataset of nominal positions and the dataset of actual positions to calculate the coordinate transformation matrix;

[0043] 3.2) Calculate the compensation value of the kinematic parameters of the manipulator according to the error recognition function;

[0044] 3.3) Iteratively update the kinematic parameters of the manipulator until the error converges.

[0045] 4) According to the compensated kinematic parameters, randomly select any position to test the calibration accuracy. If the requirements are met, exit the iteration and complete the self-calibration.

[0046] 4.1) Use the compensated kinematic parameters to recalculate the theoretical position of the manipulator end;

[0047] 4.2) Verify the error between the actual position and the theoretical position of the manipulator end through a binocular camera to determine the calibration accuracy.

[0048] First step, establish an imaging model for the underwater binocular camera and perform calibration. The imaging model is a mathematical model used to describe the geometric relationship between an object and an imaging device (such as a camera, telescope, etc.). Its basic principle is to establish a coordinate system and map the object points in three-dimensional space to the image points on the two-dimensional image plane.

[0049] The common imaging model of a camera is pinhole imaging. Its expression is as follows.

[0050]

[0051] Where the three - dimensional coordinates of a point in space are \(M = [X, Y, Z]\) T , and the pixel coordinates of a point are \(m = [u, v]\) T . The projection formula is abbreviated as where \(s\) is the scale factor, \(R_t\) is the external parameter matrix, and \(A\) is the internal parameter matrix of the camera. Specifically:

[0052]

[0053] where \((u_0, v_0)\) are the pixel principal point coordinates, \(\alpha\) and \(\beta\) are the focal length values in the \(u\) - axis and \(v\) - axis directions of the image respectively, and \(\gamma\) is the tilt factor of the pixel axis. When the camera is in an underwater environment, due to the scattering, absorption of light underwater and refraction between different media caused by adding a waterproof housing to the camera, the light will experience multiple refraction processes before reaching the camera and forming a perspective image. Using the calibration parameters in air directly in the underwater environment will lead to inaccurate underwater measurements. Therefore, photos should be taken in the underwater environment for parameter calibration. The calibration scheme uses the popular Zhang Zhengyou calibration method on land to calibrate the binocular camera and obtain the internal and external parameters of the binocular camera underwater.

[0054] It is also necessary to construct a kinematic model for the manipulator. The Denavit - Hartenberg (DH) model has become the most popular choice because of its few parameter requirements and simple modeling process. In the standard DH parameter method, the spatial relationship between adjacent coordinate systems \(i - 1\) and \(i\) is defined by four key parameters, determined in the order of \(\theta\) i , \(d\) i , \(a\) i , \(\alpha\) i . Representing joint variables, translation in the \(z\) - direction, link length, bending and torsional characteristics. However, the DH model may encounter problems of parameter mutation and singularity. Therefore, the

[0055] Modified - Denavit - Hartenberg (MDH) model is adopted. This model modifies the DH model by adding a rotation parameter around the \(Y\) - axis at the parallel joint, effectively avoiding the problem of parameter mutation while maintaining the simplicity and wide applicability of the DH model.

[0056] In the second step, to achieve autonomous calibration, it is necessary to be able to automatically obtain the motion error data set of the end - effector, and then compensate it through error parameter identification. Obtaining the motion error data set requires two sets of data: one is the three - dimensional position data of the end - effector obtained by the forward solution operation of the manipulator kinematic model, and the other is the actual position data obtained through camera recognition and calculation. After obtaining these two sets of data, perform a difference operation on them and substitute them into the error model for identification and compensation operations.

[0057] When obtaining the position of the end of the robotic gripper, referring to the principle of the laser target ball of the laser tracker, the robotic arm is used to grasp a spherical object, and the center coordinates of the ball are accurately calculated based on the recognized surface coordinates, so as to obtain the position information of the center of the end of the robotic arm and use it as the actual position data. Since the depth map of the binocular camera is calculated based on the left image as the reference, image recognition is performed using the left image of the binocular camera during the operation. Calculate the parallax of the point based on the recognized pixel coordinates, so as to successfully obtain the three-dimensional coordinates of the sphere surface, finally calculate the coordinates of the sphere center and record them, thus successfully generating the required data set. The binocular camera calculates the three-dimensional position of the object by viewing the parallax. The depth map can be calculated through the parallax map, and the depth map shows the distance relationship between each pixel point and the camera. The calculation formula is as follows:

[0058]

[0059] where f is the focal length (pixel focal length), b is the baseline length, d is the parallax, and c xl and c xr are the column coordinates of the principal points of the two cameras. The depth map can be calculated through the parallax map and then a dense point cloud can be constructed. The reprojectImageTo3D() function is provided in OpenCV to calculate the three-dimensional coordinates of pixel points. This function will return a 3-channel matrix, which stores the (x, y, z) coordinates of each pixel in the left camera coordinate system. At this point, the coordinate information of any pixel point can be obtained. Then use image recognition for the left eye, and obtain the sphere surface coordinates through the binocular camera. The three-dimensional coordinates of the sphere center cannot be directly obtained, but the sphere center is located on the extension line of the connection between the optical center and a point on the sphere surface. Therefore, it can be calculated from the three-dimensional coordinates of the sphere surface. The triangular relationship is obtained as shown in Formulas 1.4 and 1.5. The schematic diagram is as Figure 2 shown.

[0060]

[0061] At this point, the actual position of the end of the robotic arm can be obtained, and then the nominal position calculated by the kinematic model with errors is calculated through kinematics. Based on the MDH parameters, the homogeneous transformation matrix between adjacent coordinate systems can be constructed The specific form is as follows:

[0062]

[0063] By multiplying the transformation matrices of each joint in turn, the total transformation matrix from the base coordinate system to the end effector coordinate system can be obtained Its expression is 1.7.

[0064]

[0065] In matrix 1.8, the position vector p = [p x , p y , p z T . That is the nominal position, and the differences between the nominal position and the actual position are saved at equal intervals as a data set.

[0066] In the third step, the data set is processed. When there is an error change in the geometric parameters of any joint in the robotic arm, according to differential theory, this small change can be transformed into the base coordinate system. Let the actual kinematic parameters be H + ΔH, where ΔH is the parameter error vector, then the actual end position P′ can be expressed as follows:

[0067] P′ = F(Q, H + ΔH)(1.9)

[0068] By means of Taylor expansion and neglecting the higher-order terms, it can be deduced that:

[0069]

[0070] At this time is the Jacobian matrix J, and this element characterizes the partial derivative relationship between the i-th coordinate component of the end position and the j-th kinematic parameter. Find the compensation parameter ΔH that minimizes the error e. Therefore, the problem is transformed into the problem of finding the minimum value of e, which is applicable to the least squares method. According to the least squares method, the estimated value of the parameter error vector ΔH is:

[0071]

[0072] E is the error data set obtained in the second step. However, the data obtained by the external reference object needs to be transformed to the base coordinate system of the robotic arm through coordinate transformation. Generally speaking, the coordinate transformation matrix needs to be measured by specific methods, such as fixing other joints and only moving a single joint to solve the axis, or using laser interferometry ranging and calculating the transformation matrix by fixing the distance and combining angles. The coordinate transformation matrices obtained by these methods all have a certain degree of error. When solving the error, due to the error caused by the coordinate transformation, the solution accuracy decreases. Therefore, a point cloud registration algorithm is added during the parameter identification process. After parameter compensation, the updated parameters are used to perform the matching calculation again, so as to gradually approach the ideal transformation parameters to improve the calibration accuracy. The principle is as Figure 3 .

[0073] In the fourth step, use the calibrated model to control the manipulator to reach a certain point in space, and then detect the target position through the camera to obtain a data set to check the calibration result. If the accuracy meets the requirements, stop the iteration and the calibration is completed.​

Claims

1. An autonomous calibration method for an underwater manipulator based on a binocular camera, characterized in that: The following steps are involved: 1) Use Zhang Zhengyou calibration method to calibrate the parameters of underwater binocular camera and build the MDH model of underwater manipulator; 2) The actual position and nominal position of the manipulator are obtained through the underwater binocular camera and the MDH model respectively, and an error data set is constructed; 3) Using the error data set to identify errors and perform iterative compensation based on the ICP matching results; 4) Check the calibration accuracy based on the compensated kinematic parameters. When the requirements are met, exit the iteration and complete the autonomous calibration.

2. According to the method of autonomous calibration of underwater manipulator based on binocular camera in claim 1, it is characterized in that: The step 2) comprises the following steps: 2.1) Use an underwater binocular camera to perform image recognition on the target object at the end of the manipulator to obtain the three-dimensional coordinates of the target object in the camera coordinate system; 2.2) Through coordinate transformation, the three-dimensional coordinates of the target object are converted to the manipulator base coordinate system to generate an actual position data set; 2.3) Based on the actual position data set, the nominal end position of the robot is solved through the MDH model, and the difference is made between it and the measured actual end position of the robot and saved as the error data set.

3. The method for autonomous calibration of an underwater manipulator based on a binocular camera according to claim 2 is characterized in that: The step 2.2) is specifically as follows: Use the robot to grab a spherical object, use the left screen of the binocular camera for image recognition, obtain the three-dimensional coordinates (x, y, z) of each pixel on the surface of the sphere, and then calculate the coordinates (X, Y, Z) of the center of the sphere, which is the position information of the center of the robot end, and use it as the actual position of the robot, where: Where R is the radius of the spherical object.

4. The method for autonomous calibration of an underwater manipulator based on a binocular camera according to claim 1, characterized in that: The step 3) comprises the following steps: 3.1) Perform point cloud matching between the nominal position dataset and the actual position dataset and calculate the coordinate transformation matrix; 3.2) Calculate the compensation value of the manipulator kinematic parameters according to the error identification function; 3.3) Update the kinematic parameters of the manipulator according to the compensation value, re-match the point cloud, and iterate steps 3.1) to 3.3) until the error converges.

5. An autonomous calibration system for underwater manipulators based on binocular cameras, characterized in that: include: The parameter calibration and model building module is used to calibrate the parameters of the underwater binocular camera using the Zhang Zhengyou calibration method and build the MDH model of the underwater manipulator; The error data set construction module is used to obtain the actual position and nominal position of the manipulator through the underwater binocular camera and the MDH model respectively, and construct the error data set; An error identification module is used to identify errors using an error data set and perform iterative compensation based on ICP matching results; The autonomous calibration module is used to check the calibration accuracy according to the compensated kinematic parameters. When the requirements are met, the iteration is exited and the autonomous calibration is completed.

Images