A sway compensation control method for underwater robotic arm used in suspended operation scenarios

Abstract

The present invention discloses a method for controlling sway compensation of an underwater manipulator for a suspended operation scenario. The method comprises: installing a sensor positioning system at the rigid connection between the ROV platform and the underwater manipulator, and setting a positioning mark at the target point of the suspended operation; controlling the end of the manipulator to move toward the target point, establishing the initial homogeneous transformation matrix of the manipulator according to the initial frame sensor positioning data, and then obtaining the position point of the end of the manipulator as the expected value; establishing the current homogeneous transformation matrix of the sway base according to the real-time sensor positioning data, and establishing the homogeneous transformation matrix of the sway base relative to the world coordinate system in combination with the expected value of the end position point, using the sway compensation algorithm to obtain the quasi-compensation angle value in real time, and inputting it into the joint controller after pre-processing to perform real-time sway compensation control on each joint. The method of the present invention optimizes and compensates each joint of the manipulator through the sway compensation algorithm, which can ensure the stability and controllability of the operation of the end of the manipulator, thereby improving the safety of the operation.

Description

Technical Field

[0001] The present invention relates to a sway compensation control method for an underwater manipulator arm, and relates to the technical field of underwater operations, and in particular to a sway compensation control method for an underwater manipulator arm used in a suspended operation scenario. Background Art

[0002] Traditional underwater facility operation and maintenance is often performed by divers. However, due to the complexity of the underwater environment, the equipment divers can carry, and the time window for diving operations, some O&M tasks cannot be carried out effectively. To address the challenges of traditional underwater operations, remotely operated vehicles (ROVs), a representative type of underwater operation robot, have emerged as a highly effective alternative. ROVs offer advantages such as unlimited depth, no need for direct exposure to hazardous environments, and the ability to carry more specialized equipment, greatly expanding the scope and efficiency of underwater operations.

[0003] Precise motion control of underwater manipulators is crucial for executing complex underwater operations. However, remotely operated vehicles (ROVs) face a number of challenges when performing suspended operations. The ROV serves as the manipulator's floating base, and the manipulator is rigidly connected to the ROV platform. The ROV's main control suffers from control lag and compensation accuracy limitations, which do not always provide the required fine manipulation. Summary of the Invention

[0004] In order to solve the problems existing in the background technology, the present invention provides an underwater robotic arm sway compensation control method for a suspended operation scenario.

[0005] The technical solution adopted in the present invention is:

[0006] The present invention provides a method for controlling sway compensation of an underwater manipulator arm for a suspended operation scenario, comprising:

[0007] Step S1: Install a sensor positioning system at the rigid connection between the ROV platform and the underwater robotic arm, and set a marker for positioning the sensor positioning system near the target point of the suspended operation.

[0008] Step S2: Control the end of the underwater manipulator to move toward the target point of the suspended operation, establish the initial homogeneous transformation matrix of the base and each joint of the underwater manipulator based on the first frame of sensor positioning data obtained by the sensor positioning system and the marker, and then obtain the position point of the end of the underwater manipulator as the expected value.

[0009] Step S3: Establish the current homogeneous transformation matrix of the shaking base of the underwater robotic arm based on the real-time sensor positioning data obtained by the sensor positioning system and the marker, and establish the homogeneous transformation matrix of the shaking base relative to the world coordinate system based on the current homogeneous transformation matrix of the shaking base of the underwater robotic arm and the expected value of the end position point.

[0010] Step S4: Based on the shaking information in the homogeneous transformation matrix of the shaking base relative to the world coordinate system, a shaking compensation algorithm is used to obtain the pseudo-compensation angle value of each joint of the underwater manipulator in real time. The pseudo-compensation angle value of each joint is pre-processed and input into the joint controller of the underwater manipulator to control each joint, thereby finally achieving real-time and precise shaking compensation control of the end position of the underwater manipulator.

[0011] In step S1, the sensor positioning system includes a calibrated monocular vision sensor and an inertial measurement unit (IMU) suitable for underwater environments. The monocular vision sensor acquires a two-dimensional image of the target point for the suspended operation in real time. Based on the actual size and image size of the markers in the two-dimensional image and the target point, the base's posture and position information relative to the target point is obtained. The inertial sensor IMU then acquires independent posture information of the base to supplement the posture data in subsequent algorithms. The monocular vision sensor's posture and position information relative to the target point and the IMU's independent posture information together constitute the sensor positioning data. The markers are specifically Aruco QR codes, which the visual system uses to assist in positioning.

[0012] In step S2, the first frame of the base attitude information is first obtained synchronously using the monocular vision sensor and the inertial sensor IMU to ensure the timeliness and calculation accuracy of the data when the base attitude is solved, and then the initial homogeneous transformation matrix of the base and each joint of the underwater manipulator in the world coordinate system is established. Initial homogeneous transformation matrix It reflects the position and posture of the base in the world coordinate system before the shake compensation is turned on; the current joint angles of each joint of the underwater manipulator are obtained through the joint controller of the underwater manipulator, and finally the initial homogeneous transformation matrix of the base and each joint of the underwater manipulator in the world coordinate system is obtained. The current joint angle of each joint is calculated to obtain the position of the end of the underwater manipulator in the world coordinate system The number of joints of the underwater manipulator is i-1. The position of the end of the underwater manipulator in the world coordinate system is As expected value.

[0013] In the step S3, according to the camera intrinsic parameter matrix K and distortion parameter D of the monocular vision sensor and the actual size and imaging size of the two-dimensional image at the target point of the suspended operation and the mark therein, the mapping relationship between the two-dimensional image obtained by the monocular vision sensor and the actual three-dimensional point in space is obtained in real time, and then the external parameter information of the shaking base is obtained, including the rotation vector rvec and the translation vector tvec, rvec = (r x ,r y ,r z ), tvec=(tx,ty,tz), rx, r y and rz are the X-axis, Y-axis and Z-axis rotations of the shaking base in the world coordinate system, respectively. x , t y and t z The X-axis, Y-axis, and Z-axis translations of the shaking base in the world coordinate system are respectively used. The rotation vector rvec in the external parameter information of the shaking base is transformed using Rodrigues transformation to obtain the rotation matrix R rodrigues , according to the rotation matrix R rodrigues Obtain the posture information of the shaking base; use the particle filter algorithm to filter and fuse the posture information of the shaking base obtained in real time by the monocular vision sensor and the inertial sensor IMU to obtain the first posture information of the shaking base after fusion, correct the cumulative error of the inertial sensor IMU and the measurement error of the monocular vision sensor to correct the error and improve the accuracy; then use the particle filter algorithm to filter and fuse according to the fused first posture information of the shaking base and the translation vector tvec to obtain the second posture information of the shaking base after fusion, and establish the current homogeneous transformation matrix of the shaking base according to the fused second posture information of the shaking base Finally, the base's posture is converted from the original axis-angle information and translation information into the form of rotation matrix, Euler angle and homogeneous transformation matrix. Finally, according to the current homogeneous transformation matrix of the shaking base Combined with the expected value of the end position point, that is, the homogeneous transformation matrix of the base at the initial moment Create a homogeneous transformation matrix of the shaking base relative to the world coordinate system Perform pose calculation for relative positioning; homogeneous transformation matrix of the base at the initial moment Contains the position and attitude information of the base relative to the world coordinate system at the initial moment.

[0014] In step S4, the homogeneous transformation matrix of the shaking base relative to the world coordinate system is Obtain the sway information in the homogeneous transformation matrix of the swaying base relative to the world coordinate system, and then use the sway compensation algorithm to obtain the pseudo-compensation angle value of each joint of the underwater manipulator in real time, as follows:

[0015] First, based on the configuration of the underwater manipulator, a DH (Denavit-Hartenberg) parameter table is established, and the homogeneous transformation matrix of the base and each joint of the underwater manipulator relative to the world coordinate system is established. Then, the homogeneous transformation matrix of the base and each adjacent joint of the underwater manipulator relative to the world coordinate system is multiplied in sequence to establish the homogeneous transformation matrix of the end of the underwater manipulator on the base relative to the world coordinate system. Including the rotation and translation information of the base relative to the world coordinate system and the transformation relationship between the adjacent joints of the underwater manipulator; then the homogeneous transformation matrix of the shaking base relative to the world coordinate system is Inverse and left-multiply the homogeneous transformation matrix of the end of the underwater manipulator on the base relative to the world coordinate system Obtain the homogeneous transformation matrix of the underwater manipulator relative to the world coordinate system Homogeneous transformation matrix of underwater manipulator relative to the world coordinate system The first three elements of the fourth column are the positions to be reached by the underwater manipulator in the coordinate system of the shaking base; when the homogeneous transformation matrix of the shaking base relative to the world coordinate system is When the posture position changes, the end of the underwater manipulator is always stable at the expected value of the position point of the end of the underwater manipulator in the world coordinate system. According to the homogeneous transformation matrix of the underwater manipulator relative to the world coordinate system And the DH parameter table obtains the optimal joint angle value of each joint as the quasi-compensation angle value q comp , the angle value to be compensated q comp After Kalman filtering, the data is input into the joint controller of the underwater manipulator. Kalman filtering ensures the continuity and smoothness of the angle change, and then controls the movement of each joint of the underwater manipulator to the quasi-compensated angle value q after filtering. comp .

[0016] The electronic device of the present invention comprises: a memory and a processor coupled to each other, wherein the memory stores program data, and the processor calls the program data to execute the method described above.

[0017] The computer-readable storage medium of the present invention stores program data thereon, and when the program data is executed by a processor, the method described above is implemented.

[0018] The beneficial effects of the present invention are:

[0019] The method of the present invention uses a monocular vision sensor and an inertial sensor (IMU) installed at the connection between the ROV body and the base, as well as an Aruco QR code set in the target operation area, to measure and solve the base shaking information of the ROV-robotic arm suspension platform during operation. Finally, the desired input value of each joint of the robotic arm is optimized and compensated through the shaking compensation control algorithm, ensuring the stability and controllability of the operating gripper at the end of the robotic arm, thereby improving the safety of the operation. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 Schematic diagram of the flow of a method for compensating for arm shaking according to an embodiment of the present invention;

[0021] Figure 2 Schematic diagram of base trajectory positioning data for a shake compensation algorithm experiment in an embodiment of the present invention;

[0022] Figure 3 Schematic diagram of base trajectory error data in a shake compensation algorithm experiment according to an embodiment of the present invention;

[0023] Figure 4 Schematic diagram of the end-point error data of the robot arm's gripper after algorithm compensation and the end-point data without compensation in an embodiment of the present invention. DETAILED DESCRIPTION

[0024] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0025] like Figure 1 As shown, the present invention takes an underwater three-degree-of-freedom electric manipulator as an example to implement a sway compensation control method for an underwater manipulator in a suspension operation scenario, which is specifically as follows:

[0026] Step S1: Install a sensor positioning system at the rigid connection between the ROV platform and the underwater manipulator, and place a marker for the sensor positioning system near the target point of the suspended operation. The sensor positioning system includes a calibrated monocular vision sensor and an inertial sensor (IMU) suitable for underwater environments. The monocular vision sensor acquires a two-dimensional image of the target point of the suspended operation in real time. The monocular vision sensor acquires the actual size and image size of the marker in the two-dimensional image of the target point of the suspended operation and obtains the attitude and position information of the base relative to the target point of the suspended operation. The inertial sensor (IMU) then acquires independent attitude information of the base to supplement the attitude data in the subsequent algorithm. The attitude and position information of the base relative to the target point of the suspended operation obtained by the monocular vision sensor and the independent attitude information of the base obtained by the inertial sensor (IMU) together constitute the sensor positioning data. The marker specifically uses an Aruco QR code to assist the visual system in positioning.

[0027] The monocular vision sensor has been pre-calibrated, and reliable camera intrinsic parameter matrix K and distortion parameter D are obtained. To address the scale uncertainty of the monocular vision sensor, an Aruco QR code of known size is posted in the target area of ​​the suspended operation. By comparing the actual size of the Aruco QR code with the image size, the scale and distance information of the monocular vision sensor relative to the suspended operation area are indirectly obtained.

[0028] Step S2: Control the end of the underwater manipulator to move toward the target point of the suspended operation. According to the first frame of sensor positioning data obtained by the sensor positioning system and the marker, the initial homogeneous transformation matrix of the base and each joint of the underwater manipulator is established, and then the position point of the end of the underwater manipulator is obtained as the expected value, as follows:

[0029] First, the first frame of the base's posture information is obtained synchronously using the monocular vision sensor and the inertial sensor IMU to ensure the timeliness and calculation accuracy of the data when solving the base's posture, and then establish the initial homogeneous transformation matrix of the base and each joint of the underwater manipulator in the world coordinate system. Initial homogeneous transformation matrix It reflects the position and posture of the base in the world coordinate system before the shake compensation is turned on; the current joint angles of each joint of the underwater manipulator are obtained through the joint controller of the underwater manipulator, and finally the initial homogeneous transformation matrix of the base and each joint of the underwater manipulator in the world coordinate system is obtained. The current joint angle of each joint is calculated to obtain the position of the end of the underwater manipulator in the world coordinate system The end of the underwater manipulator is located in the world coordinate system As expected value.

[0030] Step S3: Based on the real-time sensor positioning data obtained by the sensor positioning system and the marker, a current homogeneous transformation matrix of the swaying base of the underwater manipulator is established. Based on the current homogeneous transformation matrix of the swaying base of the underwater manipulator and the expected value of the end position point, a homogeneous transformation matrix of the swaying base relative to the world coordinate system is established, as follows:

[0031] First, according to the camera intrinsic parameter matrix K and distortion parameter D of the monocular vision sensor and the actual size and imaging size of the two-dimensional image at the target point of the suspended operation and the mark therein, the mapping relationship between the two-dimensional image obtained by the monocular vision sensor and the actual three-dimensional point in space is obtained in real time, and then the external parameter information of the shaking base is obtained, including the rotation vector rvec and the translation vector tvec, rvec = (r x ,r y ,r z ), tvec=(t x ,t y ,t z ), rx 、r y and r z are the X-axis, Y-axis, and Z-axis rotations of the shaking base in the world coordinate system, t x , t y and t z The X-axis, Y-axis, and Z-axis translations of the shaking base in the world coordinate system are respectively used. The rotation vector rvec in the external parameter information of the shaking base is transformed using Rodrigues transformation to obtain the rotation matrix R rodrigues , according to the rotation matrix R rodrigues Obtain the posture information of the shaking base; use the particle filter algorithm to filter and fuse the posture information of the shaking base obtained in real time by the monocular vision sensor and the inertial sensor IMU to obtain the first posture information of the shaking base after fusion, correct the cumulative error of the inertial sensor IMU and the measurement error of the monocular vision sensor to correct the error and improve the accuracy; then use the particle filter algorithm to filter and fuse according to the fused first posture information of the shaking base and the translation vector tvec to obtain the second posture information of the shaking base after fusion, and establish the current homogeneous transformation matrix of the shaking base according to the fused second posture information of the shaking base Finally, the base's posture is converted from the original axis-angle information and translation information into the form of rotation matrix, Euler angle and homogeneous transformation matrix. Finally, according to the current homogeneous transformation matrix of the shaking base Combined with the expected value of the end position point, that is, the homogeneous transformation matrix of the base at the initial moment Create a homogeneous transformation matrix of the shaking base relative to the world coordinate system Perform pose calculation for relative positioning; homogeneous transformation matrix of the base at the initial moment Contains the position and attitude information of the base relative to the world coordinate system at the initial moment.

[0032] Step S4: Based on the shake information in the homogeneous transformation matrix of the shaken base relative to the world coordinate system, a shake compensation algorithm is used to obtain the pseudo-compensation angle value of each joint of the underwater manipulator in real time. The pseudo-compensation angle value of each joint is pre-processed and input into the joint controller of the underwater manipulator to control each joint, ultimately achieving real-time and accurate shake compensation control of the end position of the underwater manipulator. The details are as follows:

[0033] First, a DH parameter table is established based on the configuration of the underwater manipulator, and the homogeneous transformation matrix of the base and each joint of the underwater manipulator relative to the world coordinate system is established. Then, the homogeneous transformation matrix of the base and each adjacent joint of the underwater manipulator relative to the world coordinate system is multiplied in sequence to establish the homogeneous transformation matrix of the end of the underwater manipulator on the base relative to the world coordinate system. This includes the rotation and translation information of the base relative to the world coordinate system and the transformation relationship between adjacent joints of the underwater manipulator, as follows:

[0034] The underwater three-degree-of-freedom electric manipulator carried by the ROV platform is modeled using the DH representation method, and the corresponding posture modeling is performed for the shaking base. Its homogeneous transformation matrix It is expressed as follows:

[0035]

[0036] in, Indicates the rotation information of the base shaking, n x 、n y and n z Respectively represent the projection of the x-axis on the original x, y, and z-axis after rotation, o x 、o y and o z Respectively represent the projection of the y-axis on the original x, y, and z axes after rotation, a x 、a y and a z Respectively represent the projection of the z-axis on the original x, y, and z-axes after rotation; Indicates the translation information of the base shaking, p x 、p y and p z Respectively represent the X-axis, Y-axis, and Z-axis translation of the base shake.

[0037] The shoulder joint, upper arm joint and lower arm joint of the underwater manipulator are respectively and Represented, the transformation matrix of each joint is given by:

[0038]

[0039]

[0040] Among them, T joint Represents the transformation matrix of each joint, R joint Represents the rotation matrix generated by the rotation of each joint, t joint Represents the translation vector under each joint rotation; q1, q2 and q3 represent the joint angle values ​​of the three joints respectively, d1, d2 and d3 represent the first, second and third DH parameters respectively, d i Represents Z i X axis direction i-1 Axis and X i The distance between the axes; a i Represents X i Z axis direction i-1 Axis and Z iThe distance between the axes.

[0041] The transformation matrix of the end-of-arm gripper on the shaking base relative to the world coordinate system can be obtained by multiplying the transformation matrices of adjacent joints, as follows:

[0042]

[0043] in, Represents the transformation matrix of the end gripper of the robotic arm on the shaking base relative to the world coordinate system, the transformation matrix The first three elements of the fourth column Used to indicate the position of the end of the robotic arm under the shaking base relative to the world coordinate system.

[0044] For a multi-link robotic arm, the Jacobian matrix method is used to solve the inverse kinematics of the robotic arm based on the various DH parameters. The optimal solution is selected from multiple solutions based on the current joint angle values ​​as the output of the inverse kinematics solution. The optimal solution is the joint angle value with the smallest change from the previous moment. Through the above forward and inverse kinematic modeling and solution of the robotic arm, a mathematical model and relationship between the posture of the robotic arm's end gripper, the posture of the robotic arm base, and the robotic arm's joint angles are established.

[0045] The base shake information is saved in the homogeneous transformation matrix of the shaken base relative to the world coordinate system In the code, the homogeneous transformation matrix of the shaking base relative to the world coordinate system is Inverse And multiply the homogeneous transformation matrix of the end of the underwater manipulator on the base relative to the world coordinate system by Obtain the homogeneous transformation matrix of the underwater manipulator relative to the world coordinate system Homogeneous transformation matrix of underwater manipulator relative to the world coordinate system The first three elements of the fourth column are the positions to be reached by the underwater manipulator in the coordinate system of the shaking base; the homogeneous transformation matrix of the underwater manipulator relative to the world coordinate system is obtained The details are as follows:

[0046]

[0047] Take the first three elements of the fourth column It is used to indicate the position that the end gripper of the robot arm needs to reach in the coordinate system of the shaking base. At this time, in the world coordinate system, there is a transformation matrix when the base shakes. When the posture position changes, the end of the manipulator gripper will stabilize at the expected value of the position point of the end of the underwater manipulator in the world coordinate system The above-mentioned geometric method is used to solve the inverse kinematics solver of the manipulator.

[0048] When the shaking base is relative to the world coordinate system, the homogeneous transformation matrix When the posture position changes, the end of the underwater manipulator is always stable at the expected value of the position point of the end of the underwater manipulator in the world coordinate system. According to the homogeneous transformation matrix of the underwater manipulator relative to the world coordinate system And the DH parameter table obtains the optimal joint angle value of each joint as the quasi-compensation angle value q comp , the joint compensation angle value q comp Perform Kalman filtering to ensure the continuity and smoothness of angle changes. The filtered joint compensation angle value q comp The input is given to the joint controller of the manipulator, which then controls the movement of each joint of the underwater manipulator to the pseudo-compensation angle value q after filtering. comp , to achieve real-time and accurate compensation of the end gripper position.

[0049] Finally, in order to verify the control effect of the shake compensation control method proposed in this invention, a physical experiment was conducted on the robotic arm and the shake platform:

[0050] First, the robotic arm base equipped with a monocular vision sensor and an inertial sensor IMU is installed on the shaking platform. During the free movement of the platform, the data solved by the base shaking compensation is measured and recorded. The attitude shaking amplitude of the base in the bow degree of freedom is kept greater than 0.5 radians, and the displacement shaking amplitude in the front and back, up and down, and left and right degrees of freedom is kept greater than 300 mm. The actual shaking data of the base is collected using a motion capture device and compared with the data collected and fused by the sensors. Figure 2 what Figure 3 As shown, the base sway posture acquisition positioning data and error data of the sway compensation algorithm experiment can be obtained. The average error between the acquired sway trajectory and the true trajectory is calculated to be 19.1 mm, and the maximum error is 35.0 mm, which proves that the relative positioning accuracy of the algorithm of the present invention for the base sway meets the experimental requirements.

[0051] At the same time, the robot arm and base are installed on the shaking platform, and the position data of the end of the robot arm gripper is collected to ensure that the amplitude of the posture shaking in each direction is greater than 0.5 radians and the amplitude of the displacement shaking in each direction is greater than 300 mm. In the case of horizontal shaking, front and back shaking, up and down shaking, bow shaking and mixed shaking, the above algorithm is applied to compensate the position of the end of the robot arm gripper. The motion capture device is used to collect the compensated working gripper end data and display the final trajectory effect and error, such as Figure 4 As shown, the average operating error achieved at the end is 19.1 mm, and the maximum operating error is 52.5 mm, which proves that the method of the present invention can ensure the stability and controllability of the operating gripper at the end of the robotic arm, thereby improving the safety of the operation.

[0052] The above is only a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any modifications, equivalent substitutions and improvements made by any technician familiar with this technical field within the technical scope disclosed by the present invention and within the spirit and principles of the present invention should be covered by the scope of protection of the present invention.

Claims

1. A method for compensating and controlling underwater manipulator sway for suspended operation scenarios, characterized in that: include: Step S1: installing a sensor positioning system at the rigid connection between the ROV platform and the underwater manipulator, and setting a marker for positioning the sensor positioning system at the target point of the suspended operation; Step S2: Control the end of the underwater manipulator to move toward the target point of the suspended operation, establish the initial homogeneous transformation matrix of the base and each joint of the underwater manipulator based on the first frame of sensor positioning data obtained by the sensor positioning system and the marker, and then obtain the position point of the end of the underwater manipulator as the expected value; Step S3: establishing a current homogeneous transformation matrix of the swaying base of the underwater manipulator based on the real-time sensor positioning data acquired by the sensor positioning system and the markers, and establishing a homogeneous transformation matrix of the swaying base relative to the world coordinate system based on the current homogeneous transformation matrix of the swaying base of the underwater manipulator and the expected value of the end position point; Step S4: Based on the shaking information in the homogeneous transformation matrix of the shaking base relative to the world coordinate system, a shaking compensation algorithm is used to obtain the pseudo-compensation angle value of each joint of the underwater manipulator in real time. The pseudo-compensation angle value of each joint is pre-processed and input into the joint controller of the underwater manipulator to control each joint, thereby finally realizing real-time shaking compensation control of the end position of the underwater manipulator.

2. The underwater manipulator sway compensation control method for a suspended operation scenario according to claim 1 is characterized in that: In step S1, the sensing and positioning system includes a calibrated monocular vision sensor and an inertial sensor IMU. The monocular vision sensor obtains a two-dimensional image of the target point of the suspended operation in real time, obtains the posture and position information of the base relative to the target point of the suspended operation based on the actual size and imaging size of the two-dimensional image of the target point of the suspended operation and the mark therein, and obtains independent posture information of the base based on the inertial sensor IMU. The posture and position information of the base relative to the target point of the suspended operation obtained by the monocular vision sensor and the independent posture information of the base obtained by the inertial sensor IMU together constitute the sensing positioning data.

3. The underwater manipulator sway compensation control method for suspension operation scenarios according to claim 2 is characterized by: In step S2, the first frame of the base posture information is obtained synchronously using the monocular vision sensor and the inertial sensor IMU, and then the initial homogeneous transformation matrix of the base and each joint of the underwater manipulator in the world coordinate system is established. The current joint angles of each joint of the underwater manipulator are obtained through the joint controller of the underwater manipulator, and finally the initial homogeneous transformation matrix of the base of the underwater manipulator and each joint in the world coordinate system is obtained. The current joint angle of each joint is calculated to obtain the position of the end of the underwater manipulator in the world coordinate system The number of joints of the underwater manipulator is i-1. The position of the end of the underwater manipulator in the world coordinate system is As expected value.

4. The underwater manipulator sway compensation control method for suspension operation scenarios according to claim 2 is characterized in that: In the step S3, according to the camera intrinsic parameter matrix K and distortion parameter D of the monocular vision sensor and the actual size and imaging size of the two-dimensional image at the target point of the suspended operation and the mark therein, the mapping relationship between the two-dimensional image obtained by the monocular vision sensor and the actual three-dimensional point in space is obtained in real time, and then the external parameter information of the shaking base is obtained, including the rotation vector rvec and the translation vector tvec, rvec = (rx, ry, rz), tvec = (tx, ty, tz), rx, r y and rz are the X-axis, Y-axis and Z-axis rotations of the shaking base in the world coordinate system, respectively. x , t y and t z The X-axis, Y-axis, and Z-axis translations of the shaking base in the world coordinate system are respectively used. The rotation vector rvec in the external parameter information of the shaking base is transformed using Rodrigues transformation to obtain the rotation matrix R rodrigues , according to the rotation matrix R rodrigues Obtain the posture information of the shaking base; use the particle filter algorithm to filter and fuse the posture information of the shaking base obtained in real time by the monocular vision sensor and the inertial sensor IMU to obtain the first posture information of the shaking base after fusion, and then use the particle filter algorithm to filter and fuse the first posture information of the shaking base after fusion and the translation vector tvec to obtain the second posture information of the shaking base after fusion, and establish the current homogeneous transformation matrix of the shaking base according to the second posture information of the shaking base after fusion Finally, according to the current homogeneous transformation matrix of the shaking base Combined with the expected value of the end position point, the homogeneous transformation matrix of the shaking base relative to the world coordinate system is established 5. The underwater manipulator sway compensation control method for suspension operation scenarios according to claim 2 is characterized in that: In step S4, the homogeneous transformation matrix of the shaking base relative to the world coordinate system is Obtain the sway information in the homogeneous transformation matrix of the swaying base relative to the world coordinate system, and then use the sway compensation algorithm to obtain the pseudo-compensation angle value of each joint of the underwater manipulator in real time, as follows: First, a DH parameter table is established based on the configuration of the underwater manipulator, and the homogeneous transformation matrix of the base and each joint of the underwater manipulator relative to the world coordinate system is established. Then, the homogeneous transformation matrix of the base and each adjacent joint of the underwater manipulator relative to the world coordinate system is multiplied in sequence to establish the homogeneous transformation matrix of the end of the underwater manipulator on the base relative to the world coordinate system. Then the homogeneous transformation matrix of the shaking base relative to the world coordinate system Inverse and left-multiply the homogeneous transformation matrix of the end of the underwater manipulator on the base relative to the world coordinate system Obtain the homogeneous transformation matrix of the underwater manipulator relative to the world coordinate system When the shaking base is relative to the world coordinate system, the homogeneous transformation matrix When the posture position changes, the end of the underwater manipulator is always stable at the expected value of the position point of the end of the underwater manipulator in the world coordinate system. According to the homogeneous transformation matrix of the underwater manipulator relative to the world coordinate system And the DH parameter table obtains the optimal joint angle value of each joint as the quasi-compensation angle value q comp , the angle value to be compensated q comp After Kalman filtering, the input is sent to the joint controller of the underwater manipulator, and then the joints of the underwater manipulator are controlled to move to the quasi-compensated angle value q after filtering. comp .

6. An electronic device, characterized in that: include: A memory and a processor coupled to each other, wherein the memory stores program data, and the processor calls the program data to execute the method according to any one of claims 1 to 5.

7. A computer-readable storage medium having program data stored thereon, characterized in that: When the program data is executed by a processor, the method according to any one of claims 1 to 5 is implemented.

Images