An autonomous control method for underground pipeline robot based on visual servoing

Abstract

The present invention discloses a method for autonomous control of an underground pipeline robot based on visual servoing. First, a binocular camera is used to acquire three-dimensional information of a target object. Hand-eye calibration is then performed to unify the robot's coordinate system with the camera's coordinate system. PBVS is then used to control the robot's movement based on the target's three-dimensional information and the robot's three-dimensional information. Finally, an online supervised visual servoing system is used to correct the input in real time, enabling autonomous control of the robot and ensuring grasping accuracy. The proposed method for autonomous control of an underground pipeline robot based on visual servoing enables the robot to autonomously adjust itself during the grasping process, thereby accurately grasping the target object.

Description

Technical Field

[0001] The present invention relates to the field of underground pipeline robot control, and in particular to an autonomous control method of an underground pipeline robot based on visual servoing. Background Art

[0002] With the acceleration of urbanization, the scale of underground pipelines continues to expand, and the demand for their maintenance and inspection is increasing. Traditional underground pipeline inspection methods rely primarily on manual operation or simple automated equipment, which suffer from low efficiency, poor accuracy, and insufficient safety. In recent years, the application of robotics in underground pipeline inspection and maintenance has gradually become a research hotspot. In particular, autonomous robotic control methods based on visual servoing have attracted widespread attention due to their high precision, high flexibility, and intelligent features.

[0003] Visual servoing technology uses visual feedback to control robot motion, enabling real-time adjustments to the robot's trajectory to adapt to dynamic changes in complex environments. In underground pipelines, traditional control methods struggle to meet the demands of high-precision operations due to factors such as confined space, poor lighting conditions, and complex environments. However, autonomous robot control methods based on visual servoing, by combining visual perception and motion control, can effectively address these issues.

[0004] Currently, robot control methods based on visual servoing are mainly divided into position-based visual servoing (PBVS) and image-based visual servoing (IBVS). PBVS directly controls the position and posture of the robot's end effector by converting visual information into three-dimensional spatial coordinates; IBVS directly generates control signals based on changes in image features to drive robot motion. In underground pipeline environments, due to the complex environment and the presence of a large amount of uncertainty, PBVS is more suitable for high-precision manipulation tasks such as grasping and assembly due to its direct use of three-dimensional spatial information.

[0005] However, existing robot control methods based on visual servoing still face several challenges in underground pipeline applications. First, the poor lighting conditions in underground pipeline environments can lead to reduced data quality from visual sensors, affecting the stability of the visual servo system. Second, calibration errors between the robotic arm and the camera, as well as delays during robot motion, can affect control accuracy. Furthermore, dynamic obstacles and complex backgrounds in underground pipeline environments increase the computational complexity of the visual servo system, reducing its real-time performance. Summary of the Invention

[0006] In view of this, the actual problem to be solved by the present invention is: how to enable the robot to autonomously complete the task of grasping the target object, and how to enable the pipeline robot to adjust the hand-eye calibration parameters online.

[0007] To address this issue, the present invention provides a method for autonomously controlling a pipeline robot based on visual servoing, comprising the following steps:

[0008] 1) Use a binocular camera to obtain a binocular image of the object, obtain the depth information of the object through stereo matching, and convert the 3D coordinates in the camera coordinate system to the world coordinate system to obtain the 3D coordinates of the object;

[0009] 11) The binocular camera shoots the target and obtains the left eye image and the right eye image;

[0010] 12) Stereo matching: obtaining the disparity d of the object in the image through the left and right images obtained by the binocular camera;

[0011] 13) Using the disparity d and known information, calculate the depth Z of the object through triangulation;

[0012] 14) The following operations are performed only on the left camera. Based on the intrinsic parameters of the left camera and the calculated depth Z, the two-dimensional coordinates in the image are converted to three-dimensional coordinates in the left camera coordinate system;

[0013] 15) According to the external parameters of the left camera, the rotation matrix R ex and the translation vector T ex , transform the three-dimensional coordinates in the camera coordinate system into the world coordinate system.

[0014] 2) Perform hand-eye calibration to align the robot arm coordinate system with the camera coordinate system to obtain the hand-eye calibration matrix

[0015] 21) The robotic arm grabs the circular array calibration plate and fixes the binocular camera at a suitable position outside the robotic arm. The robotic arm then moves from the initial position to the first position and then to the second position. The robot arm forward kinematics is used to calculate the T1 EE and

[0016] 22) Based on the standard equation AX=XB for hand-eye calibration, list the calibration equations;

[0017] 23) Solve the calibration equations in step 22) to obtain the hand-eye calibration matrix

[0018] 3) Use the object 3D coordinates obtained in step 1) and the hand-eye calibration matrix obtained in step 2) Calculate the rotation matrix between the target coordinate system and the gripper coordinate system o R h and translation vectors o t h, then unify the target object 3D and the mechanical gripper 3D in a unified coordinate system, and then PBVS calculates the speed V required for the mechanical gripper to grasp the target based on the input of the target 3D and the mechanical gripper 3D h =[v h ,w h ];

[0019] 31) Calculate the error function e of position-based visual servo control;

[0020] 32) Decompose the characteristic state quantity s measured and calculated by the system into the position t in three-dimensional space s and posture θu;

[0021] 33) Let the unit vector of the posture θu be the rotation axis u, and the modulus of the posture θu be the rotation angle θ, calculate the values ​​of the rotation axis u and the rotation angle θ;

[0022] 34) When the mechanical gripper coordinate system completely coincides with the target object coordinate system, the optimal grasping state is achieved, and the ideal state quantity s calculated by the system is satisfied at this time. * =[0,0], obtain the visual servo error function for any time t;

[0023] 35) Calculate the rotation matrix between the object coordinate system and the mechanical gripper coordinate system o R h ;

[0024] 36) Use the linear velocity vector v h and the angular velocity vector w h Indicates the movement speed of the mechanical gripper;

[0025] 37) Calculate the derivative e′ of the error function e;

[0026] 38) In order to reduce the error e quickly, let the derivative of the error function e′ satisfy e′=-λe, and obtain the real-time motion speed of the mechanical gripper;

[0027] 39) Calculate the linear velocity v of the mechanical gripper h and angular velocity w h After obtaining the movement speed of the end effector (mechanical gripper) of the robotic arm, the controller can perform inverse kinematics calculation of the robotic arm to obtain the movement speed of each joint and drive the robotic gripper to move toward the desired posture.

[0028] 4) The online supervised visual servo system is based on the object's three-dimensional coordinates obtained in step 1) and the hand-eye calibration matrix obtained in step 2) and the current pose of the robotic arm Calculate the average error of the hand-eye relationship Determine whether to perform online hand-eye calibration or stop the work based on the relationship between the error and the threshold;

[0029] 41) Fix a marker M near the end tool of the robotic arm. The eye-in-hand system can measure the three-dimensional position of the marker M and the current robotic arm posture in real time during the visual servoing process. During the execution of the task by the PBVS system, the three-dimensional position of the marker can be obtained. Cam M and the corresponding current posture of the robot arm Thus, the change parameter e of the current hand-eye relationship is obtained. eh 42) Then obtain n groups of current hand-eye relationship change parameters e at regular intervals eh , then the change parameters e of the current hand-eye relationship of n groups eh Take the average to get the average error of the hand-eye relationship during this period

[0030] 43) Average error based on hand-eye relationship Determine whether to perform online calibration of the hand-eye relationship or stop the service. If the error Greater than the minimum threshold ρ min And less than the maximum threshold ρ max , then the hand-eye online self-calibration is performed to recalibrate the current hand-eye relationship; if the error exceeds the maximum threshold ρ max , a warning is issued and the robot arm stops working.

[0031] 5) Perform delay compensation on the robot and calculate the target pose after adjustment [ o R″ s , o t″ s ] T Then the robot is controlled to grasp the object, and during the grasping process, the posture is continuously adjusted according to the visual servo system to achieve autonomous control and ensure the accuracy of grasping;

[0032] 51) Delay compensation is performed on the mechanical gripper to predict and estimate the position of the grasped target. The average speed can be calculated based on the speed and acceleration of the target in the last k moments. and acceleration

[0033] 52) Based on the average speed of the target in 51) and acceleration Predict the pose of the target object[ o t′ s , o R′ s ] T ;

[0034] 53) During the visual tracking process, the robot arm or gripper may block the target object, which will cause the visual system to estimate the pose of the target object and lead to the final tracking failure. Therefore, a pose that is a certain distance away from the negative direction of the Z axis of the target coordinate system is selected as the pre-grasping pose point, and the adjusted tracking pose is calculated. o t″ s , o R″ s ] T ;

[0035] 54) As the robotic arm controls the gripper to gradually track and approach the target object, the servo vision system continuously controls it to adjust its posture, and then the robotic gripper accurately grasps the target object.

[0036] The beneficial effects brought about by the technical solution provided by the present invention are:

[0037] (1) A method of online supervised visual servoing was designed, which used self-checking parameters to supervise the robot parameters. If the robot's hand-eye relationship parameters changed, the parameters could be readjusted. If the parameters could not be corrected or the error exceeded the maximum threshold set by the self-checking parameters, the task would be stopped and a warning would be issued, allowing the pipeline robot to have higher robustness and accuracy when performing grasping tasks.

[0038] (2) Through the collaborative work of the binocular camera and the robotic arm, the system can obtain multi-dimensional information about the target object and, combined with the motion control of the robotic arm, realize the fusion processing of multi-sensor information. This multi-sensor fusion technology can effectively improve the system's perception ability and control accuracy and is suitable for complex underground pipeline environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 This is a flow chart of the autonomous control method of the underground pipeline robot based on visual servoing of the present invention.

[0040] Figure 2 Schematic diagram of binocular camera and robotic arm hand-eye calibration.

[0041] Figure 3 It is a schematic diagram of a robotic arm tracking and grasping a target object. DETAILED DESCRIPTION

[0042] To make the objectives, technical solutions and advantages of the present invention more clear, the embodiments of the present invention will be further described below with reference to the accompanying drawings.

[0043] like Figure 1 As shown, the present invention provides an autonomous control method for an underground pipeline robot based on visual servoing, and the specific process is as follows:

[0044] 1) Use a binocular camera to obtain a binocular image of the object, obtain the depth information of the object through stereo matching, and convert the 3D coordinates in the camera coordinate system to the world coordinate system to obtain the 3D coordinates of the object:

[0045] 11) The binocular camera shoots the target and obtains the left eye image and the right eye image;

[0046] 12) Stereo matching: Obtain the disparity d of the object in the image through the left and right images obtained by the binocular camera. The process is expressed as follows:

[0047] d=x L -x R (1)

[0048] Among them, x L and x R are the horizontal coordinates of the object in the left image and the right image respectively;

[0049] 13) Using the disparity d and known information, the depth Z of the object is calculated by triangulation. The process is expressed as follows:

[0050]

[0051] Where f is the focal length of the camera, B is the baseline distance between the binocular cameras;

[0052] 14) The following operations are performed only on the left camera. Based on the intrinsic parameters of the left camera and the calculated depth Z, the two-dimensional coordinates in the image are converted to three-dimensional coordinates in the left camera coordinate system. The process is shown as follows:

[0053]

[0054] in, is the inverse of the left camera's intrinsic parameter matrix, x L ,y L are the horizontal and vertical coordinates of the object in the left camera, X c ,Y c ,Z c is the coordinate of the object in the camera coordinate system;

[0055] 15) According to the external parameters of the left camera, the rotation matrix R ex and the translation vector T ex , transform the three-dimensional coordinates in the camera coordinate system into the world coordinate system. The process is expressed as follows:

[0056]

[0057] Among them, X w ,Y w ,Z ware the coordinates of the object in the world coordinate system.

[0058] 2) Perform hand-eye calibration to align the robot arm coordinate system with the camera coordinate system to obtain the hand-eye calibration matrix Figure 2 Schematic diagram of binocular camera and robotic arm hand-eye calibration:

[0059] 21) The robotic arm grabs the circular array calibration plate and fixes the binocular camera at a suitable position outside the robotic arm. The robotic arm then moves from the initial position to the first position and then to the second position. The robot arm forward kinematics is used to calculate the T1 EE and

[0060] in, T1 EE and is the homogeneous transformation matrix of the initial position, the first position and the second position of the manipulator end coordinate system;

[0061] 22) Based on the standard equation AX=XB for hand-eye calibration, the calibration equations are listed. The process is expressed as follows:

[0062]

[0063] in, Represents the transformation matrix between the end of the robotic arm and the calibration plate, Represents the transformation matrix between the left camera at the initial position and the calibration plate, Represents the transformation matrix between the left camera at the first position and the calibration plate, Represents the transformation matrix between the left camera at the second position and the calibration plate;

[0064] 23) Solve the calibration equations in step 22) to obtain the hand-eye calibration matrix

[0065]

[0066] Among them, R0 and t0 are the rotation matrix and translation matrix respectively.

[0067] 3) Use the object 3D coordinates obtained in step 1) and the hand-eye calibration matrix obtained in step 2) Calculate the rotation matrix between the target coordinate system and the gripper coordinate system o R h and translation vectors o t h, then the target object 3D and the mechanical gripper 3D are unified in a unified coordinate system, and then PBVS calculates the speed V required for the mechanical gripper to grasp the target based on the input of the target 3D and the mechanical gripper 3D. h =[ν h ,w h ], control the movement of the robotic arm, track and grasp the target object, Figure 3 This is a schematic diagram of a robotic arm tracking and grasping a target object:

[0068] 31) The error function of position-based visual servo control is expressed as follows:

[0069] e=s(m(t),a)-s * (7)

[0070] Among them, e represents the error function, s and s * are the characteristic state quantities and ideal state quantities measured and calculated by the system, respectively. m(t) represents the position of the end effector measured by the vision system at time t, and a is the model parameter of the system;

[0071] 32) Decompose the characteristic state quantity s calculated by the system into the position t in three-dimensional space s And posture θu, that is:

[0072] s=[t s ,θu] T (8)

[0073] Where T represents the transpose of the matrix;

[0074] 33) Let the unit vector of the posture θu be the rotation axis u, and the modulus of the posture θu be the rotation angle θ. By Rodrigues transformation, we can get:

[0075]

[0076] Where tr(R) represents the sum of the main diagonal elements of the rotation matrix R0;

[0077] 34) When the mechanical gripper coordinate system completely coincides with the target object coordinate system, the optimal grasping state is achieved, and the ideal state quantity s calculated by the system is satisfied at this time. * =[0,0], then for any time t, the visual servo error function is rewritten as:

[0078] e=s=[ 0 t h ,θu] (10)

[0079] in, It is the translation vector between the object coordinate system after adjustment and the mechanical gripper coordinate system at the end of the robot arm. bt0 is the translation vector between the object coordinate system and the robot base coordinate system, b t h is the translation vector between the coordinate system of the robot base and the coordinate system of the mechanical gripper at the end of the robot arm, is the transposed matrix of the rotation matrix between the manipulator base coordinate system and the object coordinate system;

[0080] 35) Rotation matrix between the object coordinate system and the mechanical gripper coordinate system o R h Calculated by the following formula:

[0081]

[0082] in, b R h is the rotation matrix between the robot base coordinate system and the mechanical gripper coordinate system, is the transposed matrix of the rotation matrix between the manipulator base coordinate system and the object coordinate system, is the inverse matrix of the rotation matrix of the hand-eye relationship;

[0083] 36) Mechanical gripper movement speed V h Expressed as:

[0084] V h =(v h ,w h ) (12)

[0085] Among them, v h and w h Represented as 3×1 linear velocity vector and angular velocity vector respectively;

[0086] 37) The derivative e′ of the error function e is calculated as follows:

[0087] e′=L e V h (13)

[0088] Among them, L e is the characteristic Jacobian matrix of the robot arm;

[0089] 38) In order to reduce the error e quickly, let its derivative satisfy e′=-λe. According to formula (13), the real-time motion speed of the mechanical gripper is obtained as:

[0090]

[0091] Where λ is the servo gain value greater than 0, It's L e The inverse matrix of

[0092] 39) The characteristic Jacobian matrix L of the robotic arme , L e The inverse matrix of And the characteristic Jacobian matrix L of the posture θu θu It is expressed as follows:

[0093]

[0094] Among them, [u] × is a skew-symmetric matrix about the unit rotation axis u, I3 is the third-order unit matrix, It's L θu The inverse matrix of

[0095] 310) Combining formulas (12), (14) and (15) we can get the linear velocity ν of the mechanical gripper: h and angular velocity w h :

[0096]

[0097] After obtaining the movement speed of the mechanical gripper, the controller can perform inverse kinematics calculations on the robotic arm to determine the movement speed of each joint and drive the mechanical gripper to move toward the desired position.

[0098] 4) The online supervised visual servo system is based on the object's three-dimensional coordinates obtained in step 1) and the hand-eye calibration matrix obtained in step 2) and the current pose of the robotic arm Calculate the average error of the hand-eye relationship The decision on whether to perform online hand-eye calibration or stop the work is based on the relationship between the error and the threshold:

[0099] 41) Fix a marker M near the end tool of the robotic arm. The eye-in-hand system can measure the three-dimensional position of the marker M and the current robotic arm posture in real time during the visual servoing process. During the execution of the task by the PBVS system, the three-dimensional position of the marker can be obtained. Cam M and the corresponding current posture of the robot arm Thus, the change parameter e of the current hand-eye relationship is obtained. eh :

[0100]

[0101] in, Cam M1 and Cam M2 is the three-dimensional position of the marker obtained by two samplings of the binocular camera, and the sampling interval is Δt. and is the corresponding robotic arm pose, is the hand-eye relationship matrix;

[0102] 42) Then, every once in a while, obtain n sets of current hand-eye relationship change parameters e eh , and then the change parameters e of the current hand-eye relationship of n groups eh Take the average to get the average error of the hand-eye relationship during this period:

[0103]

[0104] Among them, the value of n is kept in a small range, between 5 and 10;

[0105] 43) Average error based on hand-eye relationship Determine whether to perform online calibration of the hand-eye relationship or stop the service. If the error Greater than the minimum threshold ρ min And less than the maximum threshold ρ max , then the hand-eye online self-calibration is performed to recalibrate the current hand-eye relationship; if the error exceeds the maximum threshold ρ max , a warning is issued and the robot arm stops working.

[0106] 5) Perform delay compensation on the robot and calculate the target pose after adjustment [ o R″ s , o t″ s ] T , and then control the robot to grasp the object. During the grasping process, the posture is continuously adjusted according to the visual servo system to achieve autonomous control and ensure the accuracy of grasping:

[0107] 51) Perform delay compensation on the mechanical gripper, predict and estimate the position of the grasped target, and calculate the average speed of the target through the speed and acceleration of the target in the last k moments and acceleration

[0108]

[0109] Among them, w i is the weighted value at each moment, V oi ,i=1,2,...,k, is the target speed at k moments, a oi ,i=1,2,...,k, is the acceleration of the target at k moments;

[0110] 52) Based on the average speed of the target in 51) and acceleration Predict the pose of the target object:

[0111]

[0112] in, ot s and o R s are the 3D position and posture of the target object, o t′ s and o R′ s are the predicted three-dimensional position and posture of the target object, Δt o ≤0.1s is the delay compensation time, which can be approximately considered as Δt o The internal target object moves at a constant speed;

[0113] 53) During the visual tracking process, the robot arm or gripper may block the target object, causing the visual system to estimate the target object's pose inaccurately and leading to final tracking failure. Therefore, a pose that is a certain distance away from the negative Z-axis of the target coordinate system is selected as the pre-grab pose point. The adjusted tracking pose is:

[0114]

[0115] in, b t o " is the three-dimensional position of the target object after adjustment, b R o ″ is the posture of the target object after adjustment, Δz is the distance adjusted in the negative direction of the Z axis;

[0116] 54) As the robotic arm controls the gripper to gradually track and approach the target object, the servo vision system continuously controls it to adjust its posture, and then the robotic gripper accurately grasps the target object.

[0117] The above description is only a specific embodiment of the present invention, and the scope of protection of the present invention is not limited thereto. Any person familiar with the technology is within the technical scope disclosed by the present invention.

Claims

1. A method for autonomous control of an underground pipeline robot based on visual servoing, characterized in that: The following steps are involved: 1) Use a binocular camera to obtain a binocular image of the object, obtain the depth information of the object through stereo matching, and convert the 3D coordinates in the camera coordinate system to the world coordinate system to obtain the 3D coordinates of the object; 2) Perform hand-eye calibration to align the robot arm coordinate system with the camera coordinate system to obtain the hand-eye calibration matrix 3) Use the object 3D coordinates obtained in step 1) and the hand-eye calibration matrix obtained in step 2) Calculate the rotation matrix between the target coordinate system and the gripper coordinate system o R h and translation vectors o t h , then the target object 3D and the mechanical gripper 3D are unified in a unified coordinate system, and then PBVS calculates the speed V required for the mechanical gripper to grasp the target based on the input of the target 3D and the mechanical gripper 3D. h =[ν h ,w h ]; 4) The online supervised visual servo system is based on the object's three-dimensional coordinates obtained in step 1) and the hand-eye calibration matrix obtained in step 2) and the current pose of the robotic arm Calculate the average error of the hand-eye relationship Determine whether to perform online hand-eye calibration or stop the work based on the relationship between the error and the threshold; 5) Perform delay compensation on the robot and calculate the target pose after adjustment [ o R″ s , o t″ s ] T Then the robot is controlled to grasp the object, and during the grasping process, the posture is continuously adjusted according to the visual servo system to achieve autonomous control and ensure the accuracy of grasping.

2. The autonomous control method of an underground pipeline robot based on visual servoing according to claim 1 is characterized in that: The step 1) comprises the following steps: 11) The binocular camera shoots the target and obtains the left eye image and the right eye image; 12) Stereo matching: Obtain the disparity d of the object in the image through the left and right images obtained by the binocular camera. The process is expressed as follows: d=x L -x R (1) Among them, x L and x R are the horizontal coordinates of the object in the left image and the right image respectively; 13) Using the disparity d and known information, the depth Z of the object is calculated by triangulation. The process is expressed as follows: Where f is the focal length of the camera, B is the baseline distance between the binocular cameras; 14) The following operations are performed only on the left camera. Based on the intrinsic parameters of the left camera and the calculated depth Z, the two-dimensional coordinates in the image are converted to three-dimensional coordinates in the left camera coordinate system. The process is shown as follows: in, is the inverse of the left camera's intrinsic parameter matrix, x L ,y L are the horizontal and vertical coordinates of the object in the left camera, X c ,Y c ,Z c is the coordinate of the object in the camera coordinate system; 15) According to the external parameters of the left camera, the rotation matrix R ex and the translation vector T ex , transform the three-dimensional coordinates in the camera coordinate system into the world coordinate system. The process is expressed as follows: Among them, X w ,Y w ,Z w are the coordinates of the object in the world coordinate system.

3. The autonomous control method of an underground pipeline robot based on visual servoing according to claim 1 is characterized in that: The step 2) includes the following steps: 21) The robotic arm grabs the circular array calibration plate and fixes the binocular camera at a suitable position outside the robotic arm. The robotic arm then moves from the initial position to the first position and then to the second position. The robot arm forward kinematics is used to calculate the in, is the homogeneous transformation matrix of the initial position, the first position and the second position of the manipulator end coordinate system; 22) Based on the standard equation AX=XB for hand-eye calibration, the calibration equations are listed. The process is expressed as follows: in, Represents the transformation matrix between the end of the robotic arm and the calibration plate, Represents the transformation matrix between the left camera at the initial position and the calibration plate, Represents the transformation matrix between the left camera at the first position and the calibration plate, Represents the transformation matrix between the left camera at the second position and the calibration plate; 23) Solve the calibration equations in step 22) to obtain the hand-eye calibration matrix Among them, R0 and t0 are the rotation matrix and translation matrix respectively.

4. The autonomous control method of an underground pipeline robot based on visual servoing according to claim 1 is characterized in that: The step 3) comprises the following steps: 31) The error function of position-based visual servo control is expressed as follows: e=s(m(t),a)-s * (7) Among them, e represents the error function, s and s * are the characteristic state quantity and ideal state quantity measured and calculated by the system, m(t) represents the position of the end effector measured by the vision system at time t, and a is the model parameter in the system; 32) Decompose the characteristic state quantity s calculated by the system into the position t in three-dimensional space s And posture θu, that is: s=[t s ,θu] T (8) Where T represents the transpose of the matrix; 33) Let the unit vector of the posture θu be the rotation axis u, and the modulus of the posture θu be the rotation angle θ. By Rodrigues transformation, we can get: Where tr(R0) represents the sum of the main diagonal elements of the rotation matrix R0; 34) When the mechanical gripper coordinate system completely coincides with the target object coordinate system, the optimal grasping state is achieved, and the ideal state quantity s calculated by the system is satisfied at this time. * =[0,0], then for any time t, the visual servo error function is rewritten as: in, It is the translation vector between the object coordinate system after adjustment and the mechanical gripper coordinate system at the end of the robot arm. b t0 is the translation vector between the object coordinate system and the robot base coordinate system, b t h is the translation vector between the coordinate system of the robot base and the coordinate system of the mechanical gripper at the end of the robot arm, is the transposed matrix of the rotation matrix between the manipulator base coordinate system and the object coordinate system; 35) Rotation matrix between the object coordinate system and the mechanical gripper coordinate system o R h Calculated by the following formula: in, b R h is the rotation matrix between the robot base coordinate system and the mechanical gripper coordinate system, is the transposed matrix of the rotation matrix between the manipulator base coordinate system and the object coordinate system, is the inverse matrix of the rotation matrix of the hand-eye relationship; 36) Mechanical gripper movement speed V h Expressed as: Among them, v h and w h Represented as 3×1 linear velocity vector and angular velocity vector respectively; 37) The derivative of the error function is calculated as follows: Among them, L e is the characteristic Jacobian matrix of the robot arm; 38) In order to reduce the error e quickly, let its derivative satisfy e′=-λe. According to formula (13), the real-time motion speed of the mechanical gripper is obtained as: Where λ is the servo gain value greater than 0, It's L e The inverse matrix of 39) The characteristic Jacobian matrix L of the robotic arm e 、L e The inverse matrix of And the characteristic Jacobian matrix L of the posture θu θu It is expressed as follows: Among them, [u] × is a skew-symmetric matrix about the unit rotation axis u, I3 is the third-order unit matrix, It's L θu The inverse matrix of 310) Combining formulas (12), (14) and (15), we can get the linear velocity v of the mechanical gripper: h and angular velocity w h : After obtaining the movement speed of the mechanical gripper, the controller can perform inverse kinematics calculations on the robotic arm to determine the movement speed of each joint and drive the mechanical gripper to move toward the desired position.

5. The autonomous control method of an underground pipeline robot based on visual servoing according to claim 1 is characterized in that: The step 4) comprises the following steps: 41) Fix a marker M near the end tool of the robotic arm. The eye-in-hand system can measure the three-dimensional position of the marker M and the current robotic arm posture in real time during the visual servoing process. During the execution of the task by the PBVS system, the three-dimensional position of the marker can be obtained. Cam M and the corresponding current posture of the robot arm Thus, the change parameter e of the current hand-eye relationship is obtained. eh : in, Cam M1 and Cam M2 is the three-dimensional position of the marker obtained by two samplings of the binocular camera, and the sampling interval is Δt. is the corresponding robotic arm pose, is the hand-eye relationship matrix; 42) Then, every once in a while, obtain n sets of current hand-eye relationship change parameters e eh , and then the change parameters e of the current hand-eye relationship of n groups eh Take the average to get the average error of the hand-eye relationship during this period: Where n is between 5 and 10; 43) Average error based on hand-eye relationship Determine whether to perform online calibration of the hand-eye relationship or stop the service. If the error Greater than the minimum threshold ρ min And less than the maximum threshold ρ max , then the hand-eye online self-calibration is performed to recalibrate the current hand-eye relationship; if the error exceeds the maximum threshold ρ max , a warning is issued and the robot arm stops working.

6. The autonomous control method of an underground pipeline robot based on visual servoing according to claim 1 is characterized in that: The step 5) comprises the following steps: 51) Perform delay compensation on the mechanical gripper, predict and estimate the position of the grasped target, and calculate the average speed of the target through the speed and acceleration of the target in the last k moments and acceleration Among them, w i is the weighted value at each moment, V oi ,i=1,2,...,k, is the target speed at k moments, a oi ,i=1,2,...,k, is the acceleration of the target at k moments; 52) Based on the average speed of the target in 51) and acceleration Predict the pose of the target object: in, o t s and o R s are the 3D position and posture of the target object, o t′ s and o R′ s are the predicted three-dimensional position and posture of the target object, Δt o ≤0.1s is the delay compensation time, which can be approximately considered as Δt o The internal target object moves at a constant speed; 53) Select a pose that is some distance away from the negative Z-axis of the target coordinate system as the pre-grab pose point, then the tracking pose after adjustment is: in, b t″ o is the three-dimensional position of the target object after adjustment, b R″ o is the posture of the target object after adjustment, and Δz is the distance adjusted in the negative direction of the Z axis; 54) As the robotic arm controls the gripper to gradually track and approach the target object, the servo vision system continuously controls it to adjust its posture, and then the robotic gripper accurately grasps the target object.

Images