Integrated coordination control method for collaborative operation of double underwater robots

Abstract

The invention relates to the technical field of underwater robot collaborative operation, in particular to a double-underwater robot collaborative operation integrated coordination control method, which comprises the following steps: S1, establishing an overall kinetic equation for describing an underwater robot actuator system based on a Lagrange method; s2, a kinematics mapping relation of the system is constructed in a memory of a processor, and the coordinate transformation operand between the underwater robot body and the mechanical arm is eliminated; s3, providing a joint space velocity observer independent of an acceleration sensor; and S4, designing an underwater robot-actuator integrated coordination controller on the basis of the observer, and designing an adaptive law to cope with the model uncertainty of the underwater complex environment. According to the invention, a hydrodynamic simplified model is adopted, the accuracy and complexity of the model are balanced, a novel double-underwater-robot integrated coordination control method is provided, and under the condition that the calculation complexity is reduced, the stabilization of a robot body and the cooperation of the tail end of an actuator are realized at the same time.

Description

Technical Field

[0001] This invention relates to the field of underwater robot collaborative operation technology, specifically to an integrated coordination and control method for collaborative operation of two underwater robots. Background Technology

[0002] The underwater robot body is equipped with an actuator (usually a manipulator), which is called an Underwater Vehicle-Manipulator System (UVMS). It has a certain degree of autonomy and can be separated from the mother ship and operators to a certain extent, and can perform tasks such as sampling and transportation. It is an important tool for exploring the ocean [2]. In particular, the underwater robot-actuator system is constrained by structure and energy, and has the characteristics of small carrier size, high center of gravity and strong maneuverability. It can be applied to a variety of different types of operation scenarios, but it also has limitations such as small actuator load capacity and difficulty in controlling the system hovering operation. Entering a new stage of ocean exploration and development, a single underwater robot is no longer able to meet the complex and diverse operation requirements of today. The cooperation of two underwater robots (such as Figure 1 Compared to a single complex underwater robot-actuator system, it has more powerful operational capabilities, more flexible system structure and organization, and lower costs, giving it advantages in tasks such as collaborative handling of large objects and underwater collaborative assembly and welding.

[0003] A Chinese patent (CN121115524A) describes a method and system for elastic consistency control of multiple underwater robot systems. The method involves discretizing and linearizing the multiple autonomous underwater vehicle (AUV) system, introducing performance indicators to characterize the impact of communication topology on the system's dynamic behavior, designing an elastic consistency controller based on a centralized optimal control method for locally decoupled systems and robust stability theory, and obtaining the controller gain by solving the local algebraic Riccati equation to achieve consistent state control of the multi-AUV system. This method treats the underwater robots as point masses to handle the coordinated position control problem of multiple underwater robots. It is suitable for simple tasks such as reconnaissance and detection performed by multiple autonomous underwater robots autonomously networking. However, it is not applicable to the collaborative operation control of UVMS (Autonomous Underwater Vehicle Systems) with more complex dynamic models and more complex tasks, such as the coordinated motion of actuators and robotic arms.

[0004] Most existing underwater robot coordination control technologies address the autonomous formation and consistency control of multiple autonomous underwater robots, focusing only on motion control at their center of mass. This fails to solve the problem of collaborative operation between underwater robots carrying actuators. In a floating state, the coupling between the actuator and the underwater robot body generates reaction forces and torques on the robot body, affecting the robot's attitude and the control accuracy of the actuator's end effector. Therefore, an integrated controller that simultaneously coordinates the underwater robot's attitude and the actuator's end effector pose is needed. A few techniques have proposed coordinated control methods for underwater robot-actuator systems with complex dynamic models, but they do not consider the stability of the underwater robot body in the coupled state, the actuator's motion state under coordinated coupling, or the impact of complex hydrodynamic disturbances, model uncertainties, and computational complexity on control effectiveness. Summary of the Invention

[0005] The purpose of this invention is to address the shortcomings of existing underwater robot coordination control technologies. Most technologies focus on the autonomous formation and consistency control of multiple autonomous underwater robots, only considering motion control at their center of mass, and fail to address the collaborative operation of underwater robots carrying actuators. While a few techniques have proposed coordination control methods for underwater robot-actuator systems with complex dynamic models, they do not consider the stability of the robot body and actuators under coupled conditions, the motion state of the actuators under coordinated coupling, or the impact of complex hydrodynamic disturbances, model uncertainties, and computational complexity on control effectiveness.

[0006] To achieve the above objectives, the present invention provides the following technical solution: This invention provides an integrated coordinated control method for dual underwater robot cooperative operations, comprising the following steps: S1, establishing an overall dynamic equation describing the underwater robot actuator system based on the Lagrange method, wherein the overall dynamic equation includes coupled dynamic terms of the underwater robot body and the manipulator arm, as well as hydrodynamic disturbance terms; S2, constructing the kinematic mapping relationship of the system in the processor's memory; to eliminate the computational complexity of coordinate transformation between the underwater robot body and the manipulator arm, the underwater robot body is mapped as a virtual base link at the root of the manipulator arm, wherein the link length parameter of the virtual base link is set to 0, and it is given three rotational freedoms corresponding to the body posture. S3. By unifying the parameterization of the underwater robot body and the robotic arm into a single-chain multi-degree-of-freedom system, the position signals collected by the sensors are processed through the joint space velocity observer to estimate the generalized velocity of the system online without using an accelerometer; S4. Based on the unified kinematic architecture and the observed generalized velocity, the uncertain parameters in the overall dynamic equation are compensated in real time using an adaptive law to calculate the control torque command that can simultaneously stabilize the body posture and control the end effector of the robotic arm; S5. The control torque command is converted into a drive signal to drive the thrusters of the two underwater robots and the joint motors of the robotic arm, realizing the cooperative operation of the two underwater robots.

[0007] Further, in step S1, the hydrodynamic disturbance term is simplified and modeled using the Morrison formula; the overall dynamic equation is expressed as a set of nonlinear differential equations including the inertia matrix, Coriolis force and centripetal force matrix, fluid drag matrix and gravity buoyancy vector, and the specific form of the overall dynamic equation is described as follows: ,in For the system's generalized coordinate vector, The inertia matrix includes the added mass. The matrix of Coriolis force and centripetal force. The fluid resistance matrix includes both linear and nonlinear hydrodynamic damping. The restoring force vector is the sum of gravity and buoyancy. These are the Euler angles of the underwater robot's position and attitude in an inertial frame. This is a vector composed of the joint angular velocities of the underwater robot actuators.

[0008] Further, in step S2, the three rotational degrees of freedom correspond to the roll, pitch, and yaw motions of the underwater robot body in the inertial coordinate system; through the mapping of the virtual base linkage, the generalized coordinate vector of the system is defined as... ,in, This represents the virtual joint angle vector that represents the body's posture. This represents the physical joint angle vector of the robotic arm.

[0009] Further, in step S3, the joint space velocity observer includes an auxiliary variable for compensating for model uncertainties, the derivative of which is associated with the system dynamics residual; the gain matrix of the observer is configured such that the velocity estimation error converges exponentially.

[0010] Further, in step S4, the control torque command consists of a feedback control term and a feedforward compensation term; the feedback control term is constructed based on the position tracking error of the cooperative operation and the stabilization error of the body attitude; the feedforward compensation term uses the parameter estimates provided by the adaptive law to dynamically compensate for the uncertain inertial parameters and hydrodynamic drag coefficient of the system.

[0011] Furthermore, the adaptive law is designed based on Lyapunov stability theory, and its specific update law formula is as follows: in, Let be the vector of dynamic parameters to be estimated. It is a positive definite gain matrix. The regression matrix of the system dynamics, The defined composite tracking error variable.

[0012] Furthermore, the collaborative operation involves two underwater robots jointly clamping the same rigid load for transport; in step S4, the internal force constraints generated by the two underwater robots through the rigid load are taken into account when calculating the control torque command, and the control input of each underwater robot is determined through a coordinated allocation strategy.

[0013] Furthermore, the position signal processed in step S3 comes from the inertial measurement unit, Doppler velocimeter, and robotic arm joint encoder mounted on the underwater robot; the method enables real-time interaction of status data between the two underwater robots via the User Datagram Protocol.

[0014] Beneficial effects Compared with known public technologies, the technical solution provided by this invention has the following beneficial effects: This invention derives the system dynamics equations based on the Lagrange method. These equations effectively describe the dynamic relationship between the underwater robot and its actuator as a whole. A simplified hydrodynamic model is employed, balancing accuracy and complexity. A dynamic observer in joint space is designed to project the motion trajectory in the Cartesian space of the end effector onto the joint space, avoiding the computational complexity of solving complex inverse kinematics. For the underwater robot-actuator system with the body in a floating state and dynamic coupling between the body and actuator, an integrated dual underwater robot coordinated controller is designed. This controller can simultaneously stabilize the underwater robot body and coordinate the controller at the end effector, improving the control accuracy of the underwater robot. The controller directly adopts the inverse Jacobi matrix approach, designing virtual desired joint velocities. The control inputs are obtained through simple calculations, again avoiding the computational complexity of solving complex inverse kinematics. To address the model uncertainties in the unknown underwater environment, adaptive laws for dynamics and kinematic parameters are designed separately. These laws can simultaneously handle uncertainties in both dynamics and kinematic parameters, giving the system good robustness. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of the dual underwater robots and a schematic diagram of kinematic constraints of the present invention; Figure 2 This is a schematic diagram of the underwater robot sensor hardware configuration of the present invention; Figure 3 This is a schematic diagram of the collaborative handling operation of two underwater robots according to the present invention; Figure 4 This is a flowchart illustrating the control process of the dual underwater robots of this invention. Figure 5 This is a schematic diagram of the simulated trajectory of the actuator end position error using the control method of this invention; Figure 6 This is a schematic diagram of the underwater robot's attitude simulation trajectory according to the control method of the present invention; Figure 7 This is a schematic diagram of the simulated trajectory for estimating the dynamic parameters of this invention; Figure 8 This is a schematic diagram of the simulated trajectory for kinematic parameter estimation according to the present invention; Figure 9 This is a schematic diagram of the end-effector position tracking error trajectory of a conventional fixed-base robotic arm control actuator according to the present invention; Figure 10 This invention relates to a fixed-base robotic arm system for controlling the pitch and trajectory of an underwater robot body. Figure 11 This is a schematic diagram of the adaptive coordination control process structure for dual underwater robot cooperative operation according to the present invention; Figure 12This is the experimental platform structure for the dual underwater robot collaborative operation control system of the present invention; Figure 13 This is a schematic diagram of the method flow of the present invention. Detailed Implementation

[0016] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0017] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but includes other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0018] The present invention will now be described in further detail with reference to the accompanying drawings: Example 1: The controller can simultaneously ensure the stability of the underwater robot body and the motion state of each actuator end effector, providing control technology support for dual underwater robots to autonomously complete collaborative transport and other tasks without human intervention. It also addresses unknown water currents and model uncertainties, thereby improving the collaborative operation accuracy of the dual underwater robots. This method eliminates the need for human intervention, enabling autonomous collaborative operation of the underwater robots.

[0019] Step 1: Establish a dynamic model describing the overall behavior of the underwater robot-actuator system. Establish a DH coordinate system and derive the second type of Lagrange equations for the entire system using the Lagrange energy method. To simplify the impact of complex hydrodynamics on the controller design and computational complexity, the Morison formula [5] is used to describe the most important water resistance and additional mass force terms, and the remaining terms are simplified to dynamic uncertainties.

[0020] Step 2: Kinematic analysis of the dual underwater robots and design of a joint space velocity observer. Since the task control target is in the Cartesian space of the end effector, while the control input is in the joint space, it is necessary to derive the body posture of each underwater robot-actuator system and the kinematic relationship between the end effector and the joint. Based on the kinematics, a dynamic velocity observer in the joint space is designed to address the difficulty of directly measuring the joint state trajectory.

[0021] Step 3: Design of an integrated controller for coordinating the underwater robot's body stabilization and the actuator's end effector. First, virtual joint velocities and dynamic sliding surfaces are designed based on the joint space velocity observer state. Then, a coordination controller is designed using the inverse Jacobi matrix method to coordinate the underwater robot's body attitude stabilization and the actuator's end effector motion. Finally, considering the impact of uncertainties in the complex underwater environment model on control performance, adaptive laws for dynamics and kinematic parameters are designed to estimate the system's unknown parameters.

[0022] Step 4: Construct a collaborative experimental platform for two underwater robots, the Wanderer and the Aohai ROV. Both underwater robots are equipped with sensors such as Doppler rangefinders (DVL) and inertial measurement units (IMUs). The motion state of each robot is obtained through data fusion and transmitted to the other via cable and UDP communication. The control program is embedded in the RK3588 core card. When the control program is activated, the underwater robots will autonomously coordinate the movement of their bodies and actuators based on sensor information.

[0023] Step 1 presents a dynamic model suitable for the coordinated control of two underwater robots. A coordinate system is established using the DH method, with system 0 fixed to the underwater robot body. A coordinate system is established on the rod. At the end of the axis, the z-axis is the joint axis, and there are n joints in total. Let For system origin, For system The velocity of the origin in an inertial frame. Let be the angular velocity of the underwater robot body in an inertial frame. For the first The joint angular velocity of each joint. From basic physics, we know that the center of mass of a rod not at the origin... kinetic energy for in It is the first The mass of the rod It is the first The inertial tensor of a rod, This represents the antisymmetric matrix operator. The total kinetic energy of the system is... in It is the dynamic correlation matrix. Using the Lagrange equations... The dynamic equations of the system can then be obtained. in It is the inertia matrix. This involves centrifugal force and the Coriolis matrix. Representing the velocity quantities in the fixed coordinate system of the underwater robot using coordinate transformation simplifies the system model and facilitates control without requiring additional coordinate transformations. The hydrodynamics of the underwater robot actuators primarily reference the Morison formula; the water resistance and additional mass force per unit length can be expressed as... in For water resistance, To add mass force, For fluid density, The drag coefficient, Let be the equivalent diameter of the object. The velocity of an object relative to a fluid. Let the length be the infinitesimal element. This is the additional mass force coefficient.

[0024] After coordinate transformation and considering hydrodynamic factors, the dynamic equation of the underwater robot-actuator system is: in in The position and attitude Euler angles of the underwater robot in the inertial frame (the two are combined into a vector). (representation), and actuator joint angles The vector formed It is the linear velocity of the underwater robot body in its fixed coordinate system. The vector consisting of angular velocity and actuator joint angular velocity. These are the inertial matrix (including additional inertia) of the underwater robot body, the centrifugal force and Coriolis matrix, the fluid lift and damping matrix, and the gravity and buoyancy terms. These are the inertia matrix (including additional inertia), centrifugal force and Coriolis matrix, fluid lift and damping matrix, and gravity and buoyancy terms of the underwater robot actuator. These are the additional inertia matrix, centrifugal force and Coriolis matrix, and friction matrix provided by the actuator to the underwater robot body. These are the centrifugal force and Coriolis matrix resulting from the coupling between the underwater robot body and its actuators, and the secondary fluid resistance matrix. It refers to control force and torque.

[0025] Analyze the kinematic relationships described in step two. Typically, an actuator robotic arm has only one end effector, making it easy to obtain the mapping relationship between the end effector velocity and the joint velocities. An innovative approach of this invention is to consider the underwater robot body as an actuator with three rotational degrees of freedom and zero link length. Its end effector attitude space and the actual Cartesian space of the actuator end effector together constitute the controlled variable in the task space. Its kinematic relationship with the joint space is as follows: Subscript here Indicates the first Several underwater robots, among which These are Euler angles, used to describe the posture of the underwater robot. It is the position of the actuator end. Euler angles are the attitude description of the actuator end effector. and These are the linear velocity and angular velocity of the underwater robot body. It is the actuator joint angle. The Jacobi matrix of the rate of change of angular velocity to Euler angle in a self-fixed coordinate system for a free rigid body. and These are the Jacobi matrices representing the joint space velocity to the end effector's Cartesian linear velocity and angular velocity, respectively, from the actuator's fixed base to the joint space velocity. The above equation can be compactly represented as... During collaborative operations, the end effectors of the dual underwater robots are rigidly connected to the object being transported, and the corresponding kinematic constraints must be met.

[0026] like Figure 1 The dual underwater robot cooperative transport system shown has the following desired trajectory for the object gripped by the end effectors of the two underwater robots: ,in It is the expected location trajectory. This is the Euler angle representation of the desired attitude trajectory. The underwater robot's actuator end effector grips the target object, the... The pose of the underwater robot's end effector is denoted as... , The target object and the underwater robot's end effector are rigidly connected. The vector from the origin of the target object's fixed coordinate system to the end effector of each underwater robot actuator is... And the difference between the Euler angles of the actuator end effector's fixed coordinate system attitude and the Euler angles of the target object's fixed coordinate system attitude. Let be a constant. Using the target object's fixed coordinate system and the collaborative robot's end effector's fixed coordinate system as references, the desired underwater robot end effector pose trajectory can be calculated using geometric relationships, respectively. and Combining the two reference desired trajectories above, their weighted average is used as the desired trajectory for the actuator's end-effector control. , and It is an interval scalars in, satisfying This design integrates and balances the end-effector trajectory required to complete the target trajectory tracking task with cooperation with collaborative robots. , No. The underwater robot is controlled exactly according to the expected trajectory of the target object; when At that time, the first The underwater robot completely coordinates with the behavior of its collaborative robot partner. The control objective is to design control inputs that cause the actuator's end effector to achieve the desired pose trajectory. Tracking the desired trajectory And the underwater robot body maintains a stable posture. ;Right now , .

[0027] Based on this, a velocity observer was designed. in It is a constant. It can be regarded as joint space velocity The estimate, This can be considered as using space velocity estimation. Calculated end velocity error, Represents the Jacobi matrix kinematic parameters By its estimate The replaced Jacobi matrix can be regarded as The estimate, plus the superscript +, indicates its Moore-Penrose inverse matrix. It is its derivative. This observer can obtain the desired trajectory of an underwater robot in joint space without solving complex inverse kinematics, significantly reducing the computational complexity of the control process and improving the real-time performance and reliability of cooperative operations between two underwater robots.

[0028] Step three requires designing a virtual joint velocity. in These are constants to be determined. Due to the linearization of the Euler-Lagrange equations, the following equation can be written as a linear combination of the dynamic parameters. in It is a dynamic linear regression matrix. These are the actual dynamic parameters. For each underwater robot, its coordination controller is designed as follows: in For feedback control gain matrix, It is an estimate of the dynamic parameters, and its update law is: in It is a symmetric positive definite constant matrix. Kinematic parameter estimation. Update according to the following update law in It is a symmetric positive definite constant matrix. It is a kinematic linear regression matrix.

[0029] The theoretical proof can be achieved using the Lyapunov method combined with input-output stability theory. Constructing the Lyapunov function... Differentiation yields The derivative is half-negative definite, so we get , ,in This represents a square-integrable signal. This represents a bounded signal. Furthermore, we construct the Lyapunov function. Differentiation yields The sufficient condition for the derivative to be half-negative is used to set appropriate controller parameters and obtain... And their derivatives are square-integrable signals. This is derived from the input-output stability theory [6] and Barbalat's lemma. The actuator end effector and the underwater robot body converge to the desired trajectory.

[0030] The proposed control method can simultaneously handle underwater robot body stabilization and actuator end-effector coordination control with relatively low computational complexity. Adaptive dynamic and kinematic laws are designed separately for complex underwater environments and system model uncertainties. Furthermore, as can be seen from the kinematic observer, it uses the observer velocity instead of the actual end-effector velocity, avoiding the need for additional sensors to measure the actuator end-effector velocity and the underwater robot's angular velocity, thus reducing hardware dependence. In addition, this control method satisfies a good dynamic and kinematic separation property by using virtual joint velocities. Considered as a control input, and combined with the kinematic adaptive rate, it can be regarded as a coordinated controller under speed servo, which is very useful because speed control mode is very common in today's industrial robots.

[0031] Step four requires the construction of two underwater robot platforms with communication capabilities, along with supporting computing units. For self-localization, the Ellipse-2A inertial sensors provide six degrees of freedom kinematic data (linear acceleration, angular velocity, and attitude angles). The TCM XB electronic compass corrects the Ellipse-2A's heading angle, ensuring yaw accuracy. For target localization, an ultra-short baseline sonar system acquires the target's position coordinates, and the ZED2 binocular camera estimates the target's six degrees of freedom pose (position and orientation). Furthermore, the platform employs a 9602 calculator for sensor data fusion, motion control, and thruster actuation, and is equipped with a dedicated Jetson Orin NX graphics card for real-time vision processing, enabling the determination of the end effector position of the other underwater robot.

[0032] For two underwater robots to achieve coordinated control, data communication is also necessary. Using UDP communication, the first step is software initialization: reading parameter configuration information from the hard drive, completing parameter initialization, allocating UDP communication resources, and binding them to the main control computer's IP address and port. The second step is network data input. When data is available on the UDP-bound port, the network communication module reads the data via the UDP protocol, calls the corresponding data parsing and calibration functions based on the data source, and sends the collected data to the common data area. The third step is information processing, calling functional modules such as log statistics, data storage, operation control, and fault diagnosis. Each functional module comprehensively processes the input data (including communication statistics) in the common data area and places the processed information into the output area, program variable area, and parameter area of ​​the common data area. The fourth step is network data output. The network communication module extracts data from the output area of ​​the common data area, packages it according to the communication format requirements of different devices, and then sends the data packets to the other device via UDP communication.

[0033] The control program is written into the RK3588 core card mounted on the underwater robot. The control program is implemented using the ROS2 framework. The controller is a subclass of ROS2 node, which can subscribe to messages from various sensors, calculate the required control inputs using the proposed control method, and issue thrust percentage commands to control the thrusters and joint control motors.

[0034] IV. In another embodiment: Are there any other alternative solutions to the technical solution of the present invention described in Part Two that can also achieve the purpose of the present invention?

[0035] Some fixed-base dual-manipulator coordinated control methods can be used in underwater robot-actuator systems with a high ratio of robot body to actuator mass, where the motion of the actuator has a smaller impact on the underwater body.

[0036] Some more precise modeling methods or robust control methods, such as sliding mode control and finite time control, can cope with the uncertainties of underwater environment models. However, more precise modeling and robust control methods require more computational resources, while sliding mode control and finite time control may require greater control gain.

[0037] Example 2: This embodiment provides an integrated coordinated control system for collaborative operation of two underwater robots. This system can be installed within each underwater robot and includes: a sensing module: Functional description: Used to collect real-time status information of the underwater robot-actuator system. Hardware implementation: Specifically, the sensing module includes an inertial measurement unit (IMU) mounted on the underwater robot body to measure the body's attitude (roll, pitch, yaw angle); a Doppler velocimeter (DVL) to measure the body's linear velocity; and rotary encoders at the joints of the robotic arm to collect angle and angular velocity information of each joint. A calculation module (or controller): Functional description: Used to execute the control method described in the above embodiment and calculate control torque commands. Hardware implementation: This module can be implemented using an embedded computing platform. The calculation module internally stores program code for executing the above dynamic modeling, observer algorithm, and adaptive control law. During runtime, the calculation module receives data from the sensing module, performs information exchange between the two robots via the UDP protocol, and calculates the control torque at each moment. A drive module: Functional description: Used to respond to the commands output by the calculation module and generate physical thrust. Hardware Implementation: This module includes the thruster driver for the underwater robot and the motor servo driver for the robotic arm. The control torque command output from the calculation module is converted into a PWM signal or voltage signal and sent to the drive module, which in turn drives the thruster to rotate to adjust the body's posture and drives the joint motor to rotate to move the end effector of the robotic arm.

[0038] This embodiment also provides a computer-readable storage medium on which a computer program is stored. When the computer program is run on a processor (such as the RK3588 processor described above), it causes the processor to execute the adaptive coordination control method for cooperative operation of dual underwater robots described in any of the above embodiments, including but not limited to: Step S1: Establish the overall dynamic equations of the underwater robot-actuator system; Step S2: Map the body as a virtual base link with a length of 0 to construct a unified kinematic architecture; Step S3: Estimate the generalized velocity using the joint space velocity observer; Step S4: Calculate the control torque command including adaptive compensation; Step S5: Output drive signal to the underlying actuator.

[0039] The computer-readable storage medium can be a tangible device capable of holding and storing instructions used by an instruction execution device. Storage media can be, for example, but not limited to, electrical storage devices, magnetic storage devices, optical storage devices, electromagnetic storage devices, semiconductor storage devices, or any suitable combination thereof. More specific examples (a non-exhaustive list) include: portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), static random access memory (SRAM), portable compact disc read-only memory (CD-ROM), digital discs (DVDs), memory sticks, floppy disks, etc.

[0040] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions will not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for integrated coordinated control of two underwater robots operating collaboratively, characterized in that, Includes the following steps: S1. Establish the overall dynamic equations describing the underwater robot actuator system based on the Lagrange method. The overall dynamic equations include the coupling dynamics terms of the underwater robot body and the robotic arm, as well as the hydrodynamic disturbance terms. S2. Construct the kinematic mapping relationship of the system in the processor's memory; In order to eliminate the coordinate transformation computation between the underwater robot body and the robotic arm, the underwater robot body is mapped as a virtual base link at the root of the robotic arm. The link length parameter of the virtual base link is set to 0 and is given three rotational degrees of freedom corresponding to the body posture, thereby unifying the underwater robot body and the robotic arm into a single-chain multi-degree-of-freedom system. S3. Process the position signals collected by the sensors through the joint space velocity observer to estimate the generalized velocity of the system online without using an accelerometer; S4. Based on the unified kinematics architecture and the observed generalized velocity, the uncertain parameters in the overall dynamic equation are compensated in real time using an adaptive law, and a control torque command that can simultaneously stabilize the body posture and control the end of the robotic arm is calculated. S5. Convert the control torque command into a drive signal to drive the thrusters of the two underwater robots and the joint motors of the robotic arms, so as to realize the collaborative operation of the two underwater robots.

2. The integrated coordinated control method for collaborative operation of two underwater robots according to claim 1, characterized in that, In step S1, the hydrodynamic disturbance term is simplified and modeled using the Morrison formula; the overall dynamic equation is expressed as a system of nonlinear differential equations including the inertia matrix, Coriolis force and centripetal force matrices, fluid resistance matrix and gravity-buoyancy vector. The specific form of the overall dynamic equation is described as follows: ,in For the system's generalized coordinate vector, The inertia matrix includes the added mass. The matrix of Coriolis force and centripetal force. The fluid resistance matrix includes both linear and nonlinear hydrodynamic damping. The restoring force vector is the sum of gravity and buoyancy. These are the Euler angles of the underwater robot's position and attitude in an inertial frame. This is a vector composed of the joint angular velocities of the underwater robot actuators.

3. The integrated coordinated control method for collaborative operation of two underwater robots according to claim 2, characterized in that, In step S2, the three rotational degrees of freedom correspond to the roll, pitch, and yaw motions of the underwater robot body in the inertial coordinate system; through the mapping of the virtual base linkage, the generalized coordinate vector of the system is defined as... ,in, This represents the virtual joint angle vector that represents the body's posture. This represents the physical joint angle vector of the robotic arm.

4. The integrated coordinated control method for collaborative operation of two underwater robots according to claim 3, characterized in that, In step S3, the joint space velocity observer includes an auxiliary variable for compensating for model uncertainties, the derivative of which is associated with the system dynamics residuals; the gain matrix of the observer is configured such that the velocity estimation error converges exponentially.

5. The integrated coordinated control method for collaborative operation of two underwater robots according to claim 4, characterized in that, In step S4, the control torque command consists of a feedback control term and a feedforward compensation term; the feedback control term is constructed based on the position tracking error of the cooperative operation and the stabilization error of the body attitude; the feedforward compensation term uses the parameter estimates provided by the adaptive law to dynamically compensate for the uncertain inertial parameters and hydrodynamic drag coefficient of the system.

6. The integrated coordinated control method for collaborative operation of two underwater robots according to claim 5, characterized in that, The adaptive law is designed based on Lyapunov stability theory, and its specific update law formula is as follows: in, Let be the vector of dynamic parameters to be estimated. It is a positive definite gain matrix. The regression matrix of the system dynamics, The defined composite tracking error variable.

7. The integrated coordinated control method for collaborative operation of two underwater robots according to claim 6, characterized in that, The collaborative operation involves two underwater robots jointly clamping the same rigid load for transport. In step S4, the internal force constraints generated by the two underwater robots through the rigid load are taken into account when calculating the control torque command, and the control input of each underwater robot is determined through a coordinated allocation strategy.

8. The integrated coordinated control method for collaborative operation of two underwater robots according to claim 7, characterized in that, The position signal processed in step S3 comes from the inertial measurement unit, Doppler velocimeter, and robotic arm joint encoder mounted on the underwater robot; the method enables real-time interaction of status data between the two underwater robots via the User Datagram Protocol.

9. A coordinated control system for collaborative operation of two underwater robots, characterized in that, include: The perception module is configured to collect the body posture information and robotic arm joint angle information of the two underwater robot-actuator systems. The calculation module is configured to execute the method as described in any one of claims 1 to 8, calculating the control torque command required for each underwater robot-actuator system based on the collected information; the drive module is configured to receive the control torque command and control the movement of the underwater robot's thrusters and the joint motors of the robotic arm.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the method as described in any one of claims 1 to 8.

Images