A control method and system for a multi-joint variable structure robot for submarine facility operation and maintenance

Abstract

The present invention relates to a control method and system for a multi-joint variable-structure robot for submarine facility operation and maintenance. The method includes: S1. Establish a multi-joint variable-structure robot model, and obtain the thrust-degree-of-freedom component matrix of each thruster for different degrees of freedom of motion at different joint angles; S2. Collect the sensor data of the multi-joint variable-structure robot for submarine facility operation and maintenance, obtain the current robot control signal error, and solve it in real time into the desired motion control signal; S3. Based on the desired motion control signal and the current motion mode of the robot, use the thrust-degree-of-freedom component matrix to calculate the thruster thrust; S4. According to the dynamic model and the hydrodynamic model, combined with the thruster thrust-degree-of-freedom component matrix, calculate the thrust of each thruster required to compensate for the influence of hydrodynamic force, obtain the final thruster thrust, and the thruster outputs the corresponding force to respond to the motion signal. Compared with the prior art, the present invention has the advantages of high stability and strong anti-interference ability.

Description

Technical Field

[0001] The present invention relates to the technical field of marine robots, and particularly to a control method and system for a multi-joint variable structure robot for underwater facility operation and maintenance. Background Art

[0002] The internal spaces of subsea energy and information infrastructure equipment such as subsea oil and gas production systems and subsea observation networks are narrow and complex. Conventional robots can only perform simple perimeter inspections and it is difficult to enter the narrow internal spaces for inspection and maintenance.

[0003] Underwater multi-joint robots are flexible in movement and variable in structure, with high spatial passability, and are suitable for underwater facility operation and maintenance inspections. Changes in the joint angles of such robots will cause changes in their own structures, and the positions of their carried thrusters will also change accordingly, affecting the movement modes under different degrees of freedom. Therefore, the key to the motion control of underwater variable structure robots lies in determining appropriate control parameters according to the changing structures.

[0004] Currently, conventional research focuses more on the motion control of underwater robots with fixed structures, lacking relevant research on the motion control of underwater variable structure robots. A special mathematical model needs to be established for the variable structure system and special control methods need to be applied.

[0005] Therefore, precise and stable motion control of underwater variable structure robots is the key to realizing intelligent inspection of subsea facilities. Summary of the Invention

[0006] The purpose of the present invention is to provide a control method and system for a multi-joint variable structure robot for underwater facility operation and maintenance with high stability and strong anti-interference ability to overcome the defects of the above-mentioned existing technologies.

[0007] The purpose of the present invention can be achieved through the following technical solutions:

[0008] According to the first aspect of the present invention, a control method for an intelligent operation and maintenance multi-joint variable structure robot for subsea facilities is provided. The method includes the following steps:

[0009] Step S1: Establish a multi-joint variable structure robot model, and obtain the thrust-degree of freedom component matrix of each thruster for different degrees of freedom movements at different joint angles;

[0010] Step S2: Collect sensor data of the multi-joint variable structure robot for underwater facility operation and maintenance, obtain the current robot control signal error, and real-time solve it into an expected motion control signal;

[0011] Step S3: Based on the expected motion control signal and the current motion mode of the robot, use the thrust-degree of freedom component matrix to solve the thruster thrust;

[0012] Step S4: According to the kinetic model and the hydrodynamic model, and combining with the thruster thrust - degree - of - freedom component matrix, calculate the thrust F of each thruster required to compensate for the hydrodynamic influence w , and obtain the final thruster thrust F = F o +F w . The thruster outputs the corresponding force to achieve the response to the motion signal

[0013] Preferably, the step S1 includes the following sub - steps:

[0014] Step S11: Simplify the multi - joint variable - structure robot model into a multi - link articulated model, and establish local coordinate systems at the middle of each link and at the joints

[0015] Step S12: According to the DH parameters between the local coordinate systems, including link length, link twist angle, link offset, and joint rotation angle, obtain the rotation matrix R and homogeneous transformation matrix A between different coordinate systems, and establish the conversion relationship between each coordinate system

[0016] Step S13: According to the kinematic and kinetic models, obtain the thrust - degree - of - freedom component matrix A DOF corresponding to different joint angles and discretize it

[0017] Preferably, the establishment of local coordinate systems at the middle of each link and at the joints is specifically:

[0018] Based on a robot joint with two degrees of freedom, establish two local coordinate systems with a distance of l j at the joint and joint j i at the b i end for rotational simulation of the yaw joint in the x i y i plane, and for rotational simulation of the pitch joint in the plane perpendicular to it at the b i-1 end. At the centroid of each link, establish a local coordinate system O i to describe the coordinates and vectors on the link

[0019] Preferably, the thrust - degree - of - freedom component matrix A DOF is a 6×6 matrix. A thrust - degree - of - freedom component matrix corresponds to the influence factors of different thrusters on different degrees of freedom at a certain joint angle. Each row in the matrix corresponds to the influence factors of a thruster on six degrees of freedom, and each column corresponds to the influence factors of six thrusters on a certain degree of freedom

[0020] Preferably, for the thrust - degree - of - freedom component matrix A DOFDiscretization, specifically: Determine the corresponding thrust-degree of freedom component matrix according to the joint angles in different ranges. For every set degree change in the joint angle, there corresponds a different thrust-degree of freedom component matrix.

[0021] Preferably, the step S2 includes the following sub-steps:

[0022] Step S21: Obtain the desired control objectives according to the planned path or the input control instructions, including position, angle, speed, angular velocity, acceleration, and angular acceleration.

[0023] Step S22: Obtain the real-time motion data of the robot sensor and calculate the control error of each parameter.

[0024] Step S23: Solve and normalize the control error to obtain the motion control signal.

[0025] Preferably, for different control signals, the solution of the control error in step S23 is specifically as follows:

[0026] For the control of position and angle, speed and angular velocity, acceleration, and angular acceleration, the overall control system is a double-loop design. The outer loop controls the position or angle in each axis direction, and the inner loop controls the speed or angular velocity in each axis direction.

[0027] When the given control signals are position and angle signals, through the outer loop of the controller, the position and angle errors in each axis direction are processed through an integral separation link, a proportional link, and a differential link to calculate the control signals of the speed and angular velocity in each axis direction.

[0028] For the speed and angular velocity error signals in each axis direction obtained from the outer loop solution or system input, the control signals of the acceleration and angular acceleration in each axis direction are calculated through the proportional link of the inner loop.

[0029] For the control signals of the acceleration and angular acceleration in each axis direction obtained from the inner loop, normalize them to obtain the final motion signals in each axis direction.

[0030] Preferably, the expression for the thrust solution in step S3 is:

[0031] F o =DOF / INF

[0032] In the formula, DOF is the desired motion signal of different degrees of freedom, and INF is the six-degree-of-freedom thrust-degree of freedom component matrix.

[0033] Preferably, the step S4 includes the following sub-steps:

[0034] Step S41: Use Fluent hydrodynamic software to perform hydrodynamic simulation on the simplified robot model and obtain the hydrodynamic forces and moment magnitudes F in different directions of the system at different speeds. D ;

[0035] Step S42: Obtain the component forces and component moments of the unit thrust of each thruster in different directions. t ;

[0036] Step S43: According to the solution formula F w =-F D / F t , obtain the compensation thrust F of each thruster. w ;

[0037] Step S44: Obtain the final thruster thrust F = F o +F w .

[0038] According to the second aspect of the present invention, there is provided an intelligent operation and maintenance multi-joint variable structure robot control system for subsea facilities, adopting the method described in any one of the above, and the system includes:

[0039] A path planning module for generating control target signals of position and angle;

[0040] A motion signal calculation module for calculating the desired motion signals in real time according to the sensor data;

[0041] A motion control basic module for calculating the thruster thrust by using the thrust-degree of freedom component matrix according to the desired motion signals and the current motion mode of the robot;

[0042] A hydrodynamic compensation module for estimating the hydrodynamic force according to the hydrodynamic model and calculating the thruster thrust required for compensation;

[0043] A thrust synthesis module for combining the results of the motion control basic module and the hydrodynamic compensation module to obtain the final thrust and output it.

[0044] Compared with the prior art, the present invention has the following advantages:

[0045] 1) The present invention determines the relationship between the thruster thrust of each thruster and the motion of different degrees of freedom at different joint angles of the variable structure system through the discretized thrust-degree of freedom component matrix, improving the control stability and anti-interference ability;

[0046] 2) The present invention obtains the hydrodynamic force received by the robot at the current angle based on the hydrodynamic model and compensates the thruster thrust, improving the accuracy of underwater control and can be applied to underwater multi-joint variable structure systems;

[0047] 3) The multi-joint variable structure robot model is simplified to a multi-link articulated model, and local coordinate systems are established at the middle of each link and at the joints, reducing the difficulty of constructing the thrust-degree of freedom component matrix. Description of the Drawings

[0048] Figure 1 Schematic diagram of the method flow of the present invention;

[0049] Figure 2 Schematic diagram of the calculation flow of the discretized thrust-degree of freedom component matrix of the present invention;

[0050] Figure 3 Schematic diagram of the calculation flow of the control signals at all levels of the present invention;

[0051] Figure 4 Schematic diagram of the calculation flow of the thruster thrust of the present invention;

[0052] Figure 5 System structure diagram in the embodiment of the present invention. Detailed Embodiment

[0053] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0054] Embodiment

[0055] The embodiment of the present invention provides a control method for a multi-joint variable structure robot for underwater facility operation and maintenance. The relationship between the thrust of each thruster and the movement of different degrees of freedom at different joint angles of the variable structure system is determined through a discretized thrust-degree of freedom component matrix, which is used as the core of the control system. First, based on the simplified model of the variable structure, the coordinate transformation relationship and the kinematic model are established. Based on the Fluent hydrodynamic simulation software, the hydrodynamic model of the robot is obtained to establish the dynamic model, and the thrust-degree of freedom component matrix at different angles is obtained, which is discretized and stored in the control system. Secondly, based on the control error of the movement parameters in each axis direction of the variable structure system, the movement control signal is obtained through the integral separation link, the proportional link, and the differential link. Secondly, based on the movement signal and the thrust-degree of freedom component matrix, the required thruster thrust is calculated. Finally, based on the hydrodynamic model, the hydrodynamic force received by the robot at the current angle is obtained and compensated by the thruster. Next, the method embodiment of the present invention will be introduced in detail. As Figure 1 shown, the method includes the following steps:

[0056] Step S1. Establish a simplified model of the multi-joint variable structure robot and each local coordinate system, obtain the DH parameters, and based on the DH method, obtain the transformation relationship between coordinate systems. Based on the coordinate transformation relationship and the dynamic model, obtain the thrust-degree of freedom component matrix A of each thruster for different degrees of freedom motions at different joint angles DOF , and perform discretization processing and storage for different angular ranges.

[0057] As Figure 2 shown, step S1 includes the following sub-steps:

[0058] Step S11. Simplify the structure of the multi-joint variable structure robot into a multi-link articulated model, and establish each local coordinate system at the middle of each link and at the joints. Since the robot joints are double-degree-of-freedom joints, two local coordinate systems with a distance of l j are established at the joint and joint j i at the b i end for rotation in the x i y i plane (yaw joint), and rotation in the plane perpendicular to it at the b i-1 end (pitch joint). At the centroid of each link, establish a local coordinate system O i .

[0059] Step S12. According to the DH parameters between each local coordinate system, including link length, link twist angle, link offset, and joint rotation angle, obtain the rotation matrix R and homogeneous transformation matrix A between different coordinate systems, and establish the transformation relationship between each coordinate system. The specific DH parameters and transformation matrix are shown in Table 1:

[0060] Table 1

[0061]

[0062] Among them, l0 is the length of the head link, l1 is the length of the middle link, and l2 is the length of the tail link; a is the link length, α is the link twist angle, d is the link offset, and θ is the link rotation angle.

[0063]

[0064]

[0065]

[0066]

[0067]

[0068]

[0069]

[0070]

[0071]

[0072]

[0073]

[0074]

[0075]

[0076]

[0077]

[0078]

[0079] Among them, represents the rotation matrix from coordinate system O j to O i ; represents the homogeneous transformation matrix from coordinate system O j to O i .

[0080] Step S13: According to the kinematic and dynamic model, obtain the thrust-degree of freedom component matrix A corresponding to different joint angles DOF and discretize it. The specific process of selecting the thrust-degree of freedom component matrix is as follows:

[0081] 1) The position coordinates of the robot thruster in the local coordinate system of the fuselage are P, and the unit thrust is F, which is transformed and represented in the middle coordinate system through the rotation matrix and the homogeneous transformation matrix;

[0082]

[0083]

[0084] 2) Obtain the thrust of the thruster represented in the middle coordinate system The component forces in the directions of each axis, and the expression is:

[0085]

[0086] where x, y, z are the unit vectors of the three axes in the O1 coordinate system.

[0087] 3) Construct the moment arm is the directional force of connecting rod b1 The projection vector of the vector on the three axes of O1, the torque of pitch, yaw and roll motion is expressed as:

[0088]

[0089] 4) The resultant force and torque of all thrusters for six-degree-of-freedom motion are expressed as:

[0090]

[0091] 5) The thrust-degree-of-freedom component matrix INF is represented as a 6×6 matrix, where the jth row represents the influence of the thruster numbered i on the six degrees of freedom motion, and the i-th row of the matrix is represented as:

[0092]

[0093] 6) Since embedded platforms are usually limited by computing power, the matrix is discretized. Every 10° change in joint angle corresponds to a thrust-degree of freedom component matrix, which is stored in the embedded control platform.

[0094] Step S2: According to the sensor data of the gyroscope and accelerometer carried by the robot, the current robot position, angle, speed, angular velocity, acceleration, angular acceleration error and the expected control signal are obtained, and the expected normalized motion signal is calculated in real time. The normalized motion signal is the acceleration of the robot in each axis direction and the normalized error of angular acceleration. Figure 3 As shown, the specific process is:

[0095] 1) Obtain the robot's desired control signal Q e The control signal can be a group of signal quantities of position angle, speed angular velocity, acceleration angular acceleration or their combination. The control signal can come from system input, operator input and given by the upper control system. For example, the position and angle control quantities are usually given by the path planning module, while the speed and angular velocity control quantities are calculated by the outer loop position and angle module, or can be given by direct input by the operator.

[0096] 2) Get the actual motion parameters Q of the robot r The motion parameters are the actual motion of the robot corresponding to the control signal, which are obtained by processing the sensor data such as inertial navigation, gyroscope, accelerometer, etc.

[0097] 3) Calculate the robot control error and calculate the control signal and sub-control signal in each axis direction.

[0098] For the outer ring position angle control quantity, first calculate the control error of the robot's axis position and angle, Qerr = Q e -Q r , and finally through the integral separation link K i , the proportional link K p and the differential link K d to obtain the control signals of the speed and angular velocity in each axis direction, and the expression is:

[0099]

[0100] β is the open-loop coefficient of the integral separation link, and the integral link will only be enabled when the error is less than a certain range. ε is the integral separation threshold coefficient, r is the expected value of the controlled quantity in the current flight segment, and v(0) is the system output when initially switching to this flight segment.

[0101] For the inner-loop speed and angular velocity control quantities, first calculate the control errors of the position and angle in each axis direction of the robot, Q err = Q e -Q r , and finally through the proportional link K p to obtain the control signals of the acceleration and angular acceleration in each axis direction, and the expression is: u(t) = K p Qerr.

[0102] For the acceleration and angular acceleration signals, normalize them to obtain the motion control signals of the robot in each axis direction.

[0103] Step S3: Select an appropriate thrust-degree-of-freedom component matrix according to the joint angles. According to the expected motion signal input or calculated by the system and the current motion mode of the robot, calculate the thruster thrust F o . As shown in Figure 4, the specific steps are:

[0104] 1) According to the current joint angles, correspondingly select the discretized thrust-degree-of-freedom component matrix;

[0105] 2) Calculate the thruster thrust Fo according to the control signal and the thrust-degree-of-freedom component matrix. Specifically:

[0106] DOF = [D A] = [F L T R P Y]

[0107] Fo = DOF / INF

[0108] where F, L, T, R, P, and Y are the motion control signals of the robot in the forward / backward, lateral, vertical, roll, yaw, and pitch directions respectively, and INF is the selected thrust-degree-of-freedom component matrix.

[0109] Step S4: Based on the kinetic model and the hydrodynamic model obtained from hydrodynamic simulation, calculate the thrust F of each thruster required to compensate for the influence of hydrodynamic force by combining the thruster thrust - degree - of - freedom component matrix. w . The final thruster thrust F = F o +F w . The thruster outputs the corresponding force to achieve the response to the motion signal. The specific steps are as follows:

[0110] 1) Obtain the hydrodynamic model of the robot at different joint angles and speeds through simulation using Fluent fluid dynamics software.

[0111] 2) Calculate the estimated hydrodynamic force and moment F D acting on the robot currently according to the hydrodynamic model and sensor data. The expression is:

[0112] F D = [F Dx F Dy F Dz τ Dx τ Dy τ Dz

[0113] where the hydrodynamic force acting on the robot is [F Dx , F Dy , F Dz , which are the hydrodynamic forces in the three - axis directions in the middle fuselage coordinate system O1. The specific expressions are:

[0114]

[0115]

[0116] f Dik is the component of the hydrodynamic force acting on the robot body i in the k - axis direction of its appendage coordinate system. f ik is the calculation formula for the hydrodynamic force in the k - axis axial direction of the robot body i in the theoretical part, v ik is the velocity of the robot body i along the k - axis axial direction of the appendage coordinate system. F Di is the vector representation of the hydrodynamic force acting on the robot body i in its appendage coordinate system. is the hydrodynamic force vector of the robot body i in the O1 coordinate system; represents the component of the hydrodynamic force acting on the robot in the k - axis direction in the O1 coordinate system; x, y, z are the unit vectors in each axial direction respectively.

[0117] The hydrodynamic moment acting on the robot is [τ Dx , τ Dy , τ Dz ​, which are the hydrodynamic moment components in the three-axis directions of the middle fuselage coordinate system. The specific calculation steps are as follows:

[0118] First, construct the moment arms of the hydrodynamic force acting on each axis in the O1 coordinate system:

[0119]

[0120] Second, calculate the moments of the hydrodynamic force about each axis in the O1 coordinate system:

[0121]

[0122] Finally, obtain the resultant moment τ of the hydrodynamic force received in each axial direction in the O1 coordinate system D :

[0123]

[0124] 3) According to the components of the unit thrust of each thruster in the directions of each axis in the O1 coordinate system, calculate the thrust of each thruster required to compensate for the hydrodynamic force. The specific steps are as follows.

[0125] Given the matrix F composed of the components of the unit thrust and moment of 6 thrusters in the axial directions in the O1 coordinate system t is:

[0126]

[0127] The calculation expressions of each element have been given in step S13.

[0128] Then the magnitude of the compensation force F required for each thruster w can be calculated as:

[0129] F w =-F D / F t

[0130] 4) Combine the thruster thrust and the hydrodynamic force compensation force, and calculate the final thruster thrust F = F o +F w . The thruster output thrust responds to the control signal.

[0131] 5) Judge whether the robot achieves the control target according to the sensor data. If it is achieved, switch to the next control target and jump to step S2.

[0132] The following gives the system embodiment of the present invention. As Figure 5 shown, a multi-joint variable structure robot control system for intelligent operation and maintenance of subsea facilities, the system includes:

[0133] A path planning module for generating control target signals of position and angle;

[0134] A position and angle outer loop control module for calculating velocity and angular velocity control signals in each axis direction;

[0135] A velocity and angular velocity inner loop control module for calculating acceleration and angular acceleration control signals in each axis direction;

[0136] A six-degree-of-freedom motion control basic module for solving thruster thrust for acceleration and angular acceleration control signals;

[0137] A hydrodynamic compensation module for estimating hydrodynamic forces based on a hydrodynamic model and solving thruster thrust required for compensation;

[0138] A thrust synthesis module for combining the results of the six-degree-of-freedom motion control basic module and the hydrodynamic compensation module to obtain and output the final thrust.

[0139] As described above, it is only the specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention can easily think of various equivalent modifications or substitutions, and these modifications or substitutions should all be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.

Claims

1. An intelligent operation and maintenance multi-joint variable structure robot control method for subsea facilities, characterized in that, The method includes the following steps: Step S1: Establish a multi-joint variable structure robot model, and obtain the thrust-degree of freedom component matrix of each thruster for different degrees of freedom motions at different joint angles; The thrust-degree of freedom component matrix A DOF is a 6×6 matrix. A thrust-degree of freedom component matrix corresponds to the influence factors of different thrusters on different degrees of freedom at a certain joint angle. Each row in the matrix corresponds to the influence factors of a thruster on six degrees of freedom, and each column corresponds to the influence factors of six thrusters on a certain degree of freedom; For the thrust-degree of freedom component matrix A DOF Discretize it specifically as follows: Determine the corresponding thrust-degree of freedom component matrix according to different ranges of joint angles. For every set-degree change in the joint angle, there is a different thrust-degree of freedom component matrix; Step S2: Collect the sensor data of the multi-joint variable structure robot for seabed facility operation and maintenance, obtain the current robot control signal error, and resolve it into the desired motion control signal in real time, including the following sub-steps: Step S21: According to the planned path or the input control instruction, obtain the desired control objectives, including position, angle, speed, angular speed, acceleration, and angular acceleration; Step S22: Obtain the real-time motion data of the robot sensors, and calculate the control error of each parameter; Step S23: Resolve and normalize the control error to obtain the motion control signal; For different control signals, in step S23, the resolution of the control error is specifically as follows: For the control of position and angle, speed and angular speed, acceleration, and angular acceleration, the overall control system is designed with a double-loop. The outer loop controls the position or angle in each axis direction, and the inner loop controls the speed or angular speed in each axis direction; When the given control signals are position and angle signals, through the outer loop of the controller, the position and angle errors in each axis direction are processed through an integral separation link, a proportional link, and a differential link, and the control signals of speed and angular speed in each axis direction are calculated; For the speed and angular speed error signals in each axis direction obtained from the outer loop resolution or system input, the control signals of acceleration and angular acceleration in each axis direction are calculated through the proportional link of the inner loop; For the control signals of acceleration and angular acceleration in each axis direction obtained from the inner loop, they are normalized to obtain the final motion signals in each axis direction; Step S3: Based on the desired motion control signal and the current motion mode of the robot, use the thrust-degree of freedom component matrix to calculate the thruster thrust. The expression for the thrust calculation is: In the formula, DOF is the desired motion signal of different degrees of freedom, and INF is the six-degree-of-freedom thrust-degree of freedom component matrix; Step S4: According to the kinetic model and the hydrodynamic model, and in combination with the thruster thrust - degree - of - freedom component matrix, calculate the thrust of each thruster required to compensate for the hydrodynamic influence , and obtain the final thruster thrust , and the thruster outputs the corresponding force to respond to the motion signal.

2. The intelligent operation and maintenance multi-joint variable structure robot control method for subsea facilities according to claim 1, characterized in that Step S1 includes the following sub-steps: Step S11: Simplify the multi-joint variable structure robot model into a multi-link articulated model, and establish local coordinate systems at the middle and joints of each link; Step S12: According to the DH parameters between the local coordinate systems, including link length, link twist angle, link offset, and joint rotation angle, obtain the rotation matrix R and homogeneous transformation matrix A between different coordinate systems, and establish the conversion relationship between each coordinate system; Step S13: Obtain the thrust-degree of freedom component matrix A corresponding to different joint angles according to the kinematic and dynamic models DOF And discretize it.

3. The intelligent operation and maintenance multi-joint variable structure robot control method for subsea facilities according to claim 2, characterized in that, The establishment of local coordinate systems at the middle and joints of each link is specifically as follows: Based on the dual-degree-of-freedom robot joint, two distances are established at the joint. The local coordinate system and ,joint exist End The rotation of the plane simulates the yaw joint, The end rotates in a plane perpendicular to it to simulate the pitch joint. At the center of mass of each link, a local coordinate system is established to describe the coordinates and vectors on the link. .

4. The intelligent operation and maintenance multi-joint variable structure robot control method for subsea facilities according to claim 1, characterized in that Step S4 includes the following sub-steps: Step S41: Use Fluent hydrodynamic software to perform hydrodynamic simulation on the simplified robot model and obtain the hydrodynamic forces and moment magnitudes in different directions of the system at different speeds. ; Step S42: Obtain the component forces and component torques of the unit thrust of each thruster in each direction ; Step S43. According to the calculation formula , obtain the compensation thrust of each thruster ; Step S44, obtain the final thruster thrust .

5. An intelligent operation and maintenance multi-joint variable structure robot control system for subsea facilities, characterized in that, Using the method described in any one of claims 1 to 4, the system includes: A path planning module for generating control target signals of position and angle; A motion signal resolution module for resolving the desired motion signals in real time according to the sensor data; A motion control basic module for using the desired motion signals and the current motion mode of the robot to calculate the thruster thrust using the thrust-degree of freedom component matrix; A hydrodynamic compensation module for estimating the hydrodynamic force according to the hydrodynamic model and calculating the thruster thrust required for compensation; A thrust synthesis module, which is used to combine the results of the motion control basic module and the hydrodynamic compensation module to obtain the final thrust and output it.

Images