A method for reducing sway and vibration during the deployment and retrieval of an underwater robot

Abstract

This invention relates to a method for roll reduction and anti-sway control during the deployment and recovery of an underwater robot. The method includes: estimating the position coordinates of the gantry pivot in the ship's coordinate system; calculating the motion state of the gantry pivot in the geodetic coordinate system based on the position coordinates, the mother ship's heading angle, pitch angle, roll angle, and the mother ship's coordinate position in the geodetic coordinate system; obtaining the linear acceleration and linear velocity of the underwater robot in the robot's coordinate system, and projecting the linear acceleration and linear velocity onto the geodetic coordinate system to obtain the underwater robot's motion state in the geodetic coordinate system; calculating the lateral and longitudinal sway angles of the underwater robot relative to the gantry pivot based on the motion state; and generating control commands based on the lateral and longitudinal sway angles to drive the anti-sway device to perform motion compensation along the lateral and longitudinal directions, thereby achieving roll reduction and anti-sway control. This method achieves a fundamental shift from "passive servoing" to "active roll reduction," significantly improving the safety and reliability of deployment and recovery operations.

Description

Technical Field

[0001] This invention relates to the field of underwater robot technology, specifically to a method, equipment, and medium for reducing sway and preventing rocking during the deployment and retrieval of an underwater robot. Background Technology

[0002] ROVs play an irreplaceable role in marine engineering, resource exploration, and national defense, with deployment and recovery operations being crucial for underwater missions. However, this process typically takes place on a dynamic sea surface, where the mother ship experiences complex six-degree-of-freedom motions, particularly roll and pitch, under the influence of marine environmental loads such as wind, waves, and currents.

[0003] These movements are transmitted to the underwater robot through the gantry, cables, and other structures of the deployment and recovery system, which can easily induce large-scale swaying (i.e., "swaying") of the underwater robot in the horizontal plane. Such violent swaying can not only collide with the mother ship's side or the gantry, damaging the robot itself or its onboard precision equipment, but may also cause recovery docking failure and even threaten the safety of on-site operators, making it a major risk source in the entire operation process.

[0004] To suppress swaying, existing technologies mostly employ passive or simple active anti-sway solutions. A common approach is to use one or two-degree-of-freedom hydraulic or electric cylinders as actuators, installing the anti-sway device between the underwater robot and the deployment cable, attempting to counteract some of the swaying through the mechanism's servo motion. However, these methods have significant limitations: firstly, their control methods are mostly passive responses, meaning compensation is only initiated after swaying has occurred, resulting in inherent system time delays; secondly, in moderate to severe sea states, where the mother ship moves violently, this passive anti-sway system based on hysteresis feedback struggles to effectively track and counteract rapid swaying, significantly reducing its anti-roll effect or even causing complete failure, thus compromising operational safety.

[0005] Therefore, there is an urgent need in this field for a new control method and system that can anticipate the movement of the mother ship and actively compensate for its sway, in order to overcome the shortcomings of existing technologies such as slow response and insufficient performance in harsh sea conditions, thereby significantly improving the safety and reliability of underwater robot deployment and recovery operations. Summary of the Invention

[0006] This invention provides a method, device, and medium for reducing sway and undulation during the deployment and recovery of underwater robots. The purpose is to solve the problem of horizontal swaying of existing underwater robots caused by the movement of the mother ship during deployment and recovery, as well as the problem that existing passive anti-sway technology is ineffective in harsh sea conditions due to system time delay.

[0007] To achieve the above objectives, the first aspect of the present invention provides a method for reducing sway and preventing rocking during the deployment and retrieval process of an underwater robot, comprising the following steps:

[0008] Estimate the position coordinates of the gantry support point in the ship's coordinate system;

[0009] Based on the position coordinates of the gantry support point in the ship coordinate system, as well as the heading angle, pitch angle, roll angle of the mother ship and the coordinate position of the mother ship in the geodetic coordinate system, the motion state of the gantry support point in the geodetic coordinate system is calculated.

[0010] The linear acceleration and linear velocity of the underwater robot in the robot coordinate system are obtained, and the linear acceleration and linear velocity are projected onto the geodetic coordinate system to obtain the motion state of the underwater robot in the geodetic coordinate system.

[0011] Based on the motion state of the gantry support point in the geodetic coordinate system and the motion state of the underwater robot in the geodetic coordinate system, calculate the lateral and longitudinal swing angles of the underwater robot relative to the gantry support point;

[0012] Based on the lateral and longitudinal swing angles, control commands are generated to drive the sway stopper to perform motion compensation in the lateral and longitudinal directions, thereby achieving anti-sway control.

[0013] Furthermore, methods for estimating the position coordinates of the gantry support point in the ship's coordinate system include:

[0014] Obtain the predetermined installation position parameters of the gantry on the mother ship;

[0015] Real-time monitoring of the gantry's current status, including its angle or position information;

[0016] Based on the installation position parameters and real-time gantry status, the position coordinates of the gantry support point in the ship coordinate system are obtained by conversion through the geometric relationship of the gantry design.

[0017] Furthermore, methods for calculating the motion state of the gantry support points in the geodetic coordinate system include:

[0018] Based on the mother ship's heading angle, pitch angle, and roll angle, construct a coordinate transformation matrix from the ship's coordinate system to the geodetic coordinate system;

[0019] Using the coordinate transformation matrix, the position coordinates of the gantry support point in the ship coordinate system are transformed to obtain the position of the gantry support point relative to the center of gravity of the mother ship in the geodetic coordinate system;

[0020] By superimposing the position relative to the center of gravity of the mother ship with the coordinate position of the center of gravity of the mother ship in the geodetic coordinate system, the absolute position coordinates of the gantry support point in the geodetic coordinate system are finally obtained.

[0021] By differentiating the relationship between the absolute position coordinates of the gantry support and time, the velocity of the gantry support in the geodetic coordinate system is calculated.

[0022] Furthermore, coordinate transformation and motion state calculation are achieved through the following formula:

[0023] The position coordinates of the gantry support point in the geodetic coordinate system are calculated using the following formula:

[0024]

[0025] in, , and This indicates the position coordinates of the gantry support point in the geodetic coordinate system; , and This indicates the coordinates of the mother ship's center of gravity in the geodetic coordinate system. , and This indicates the position coordinates of the gantry support point in the ship's coordinate system; The rotation matrix from the ship coordinate system to the geodetic coordinate system, determined by the mother ship's attitude angles, is expressed as follows:

[0026]

[0027] in, Indicates the heading angle of the mother ship; Indicates the mother ship's pitch angle; Indicates the roll angle of the mother ship;

[0028] The velocity of the gantry support point in the geodetic coordinate system is calculated using the following formula:

[0029]

[0030]

[0031] in, and These respectively represent the gantry support points along the geodetic coordinate system. shaft and Velocity in the axial direction, The pitch angular velocity, This is the roll angular velocity.

[0032] Furthermore, methods for obtaining the motion state of an underwater robot in a geodetic coordinate system include:

[0033] The inertial navigation system installed on the underwater robot can be used to obtain its linear acceleration and linear velocity in the robot coordinate system in real time.

[0034] Determine the heading angle deviation between the underwater robot and the mother ship;

[0035] Based on the heading angle deviation, the linear acceleration and linear velocity are transformed by coordinate projection to calculate the acceleration and velocity of the underwater robot in the geodetic coordinate system.

[0036] Furthermore, the coordinate projection transformation is achieved through the following formula:

[0037] The acceleration of the underwater robot in the geodetic coordinate system is calculated using the following formula:

[0038]

[0039]

[0040] The velocity of the underwater robot in the geodetic coordinate system is calculated using the following formula:

[0041]

[0042]

[0043] in, , Indicates the underwater robot in its robot coordinate system , Axial linear acceleration; , Indicates the underwater robot in its robot coordinate system , Axial linear velocity; This indicates the heading angle deviation between the underwater robot and the mother ship; , This indicates the underwater robot obtained after projection in the geodetic coordinate system. , Axial acceleration; , This indicates the underwater robot obtained after projection in the geodetic coordinate system. , Axial velocity.

[0044] Furthermore, after obtaining the velocity and acceleration of the underwater robot in the geodetic coordinate system, a filtering algorithm is used to perform data fusion and smoothing on the position information of the underwater robot; the filtering algorithm is Kalman filtering or extended Kalman filtering.

[0045] Furthermore, methods for calculating the lateral and longitudinal sway angles of the underwater robot relative to the gantry pivot include:

[0046] A geometric motion model is established between the gantry fulcrum and the underwater robot. This geometric motion model correlates the relative positional relationship between the gantry fulcrum and the underwater robot with the lateral and longitudinal swing angles.

[0047] Based on the real-time position coordinates of the gantry support and the underwater robot in the geodetic coordinate system, the relative displacement between the two is calculated.

[0048] By combining the cable length connecting the gantry fulcrum to the underwater robot and the heading angle deviation between the mother ship and the robot, the values ​​of the lateral and longitudinal sway angles are calculated through the geometric motion model.

[0049] The lateral and longitudinal angular velocities are obtained by calculating the rate of change of the positional relationship over time.

[0050] Furthermore, assuming the swing angle is less than 10 degrees, a small-angle approximation method (less than 10 degrees) is used to calculate the swing angle and angular velocity using the following formulas:

[0051] The lateral swing angle and longitudinal swing angle Calculated using the following formula:

[0052]

[0053]

[0054] The lateral sway velocity and longitudinal angular velocity Calculated using the following formula:

[0055]

[0056]

[0057] in, , This indicates the position coordinates of the gantry support point in the geodetic coordinate system; , This represents the position coordinates of the underwater robot's center of gravity in the geodetic coordinate system after data fusion and smoothing. , This indicates the velocity of the gantry support point in the geodetic coordinate system. , This represents the underwater robot's velocity in the geodetic coordinate system after data fusion and smoothing. This indicates the heading angle deviation between the underwater robot and the mother ship; This indicates the length of the cable between the gantry fulcrum and the center of gravity of the underwater robot. Indicates the lateral swing angle; Indicates the longitudinal swing angle; Indicates the lateral angular velocity; This indicates the longitudinal angular velocity.

[0058] To achieve the above objectives, a second aspect of the present invention provides an electronic device, including a memory and a processor, wherein the memory is used to store a program that supports the processor in executing the anti-roll and anti-sway control method during the deployment and retrieval process of the underwater robot, and the processor is configured to execute the program stored in the memory.

[0059] To achieve the above objectives, a third aspect of the present invention provides a computer-readable storage medium storing a computer program, wherein the computer program, when executed by a processor, performs the steps of the anti-roll and anti-sway control method during the deployment and retrieval process of the underwater robot.

[0060] The beneficial effects of this invention are:

[0061] Compared with existing technologies, the present invention provides a method, device, and medium for reducing sway and undulation during the deployment and recovery process of an underwater robot. This effectively solves the aforementioned problems by constructing a closed-loop control system that integrates multi-source information and performs active prediction and compensation. Its core lies not in passively responding to the underwater robot's swaying, but in actively predicting and offsetting the root cause of the swaying (i.e., the mother ship's motion). Specifically, it acquires the mother ship's inertial navigation data and gantry geometric parameters in real time, accurately estimates the real-time motion state of the gantry pivot points in the geodetic coordinate system based on the established coordinate transformation model, and simultaneously acquires the underwater robot's own motion data through its inertial navigation system and projects it onto a unified geodetic coordinate system. Furthermore, it establishes... The geometric motion model calculates the precise lateral and longitudinal swing angles and angular velocities of the underwater robot relative to the gantry fulcrum. Finally, this swing state is used as feedback, and control commands are generated in real time through the control algorithm. This drives the two-degree-of-freedom anti-swing device to perform compensatory movements in the lateral and longitudinal directions that are opposite to the direction of the mother ship's movement and have the same amplitude. This allows the wave's influence on the mother ship's movement to be offset in advance in the time domain, significantly enhancing the system's response speed and control accuracy. It overcomes the shortcomings of existing passive anti-swing systems, which are ineffective in harsh sea conditions due to severe time delays. This achieves a fundamental shift from "passive homing" to "active roll reduction," greatly improving the safety and reliability of deployment and recovery operations. Attached Figure Description

[0062] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below.

[0063] Figure 1 This is a system composition diagram of a controlled object disclosed in an embodiment of the present invention.

[0064] Figure 2 This is a flowchart of a method for reducing sway and preventing rocking during the deployment and retrieval of an underwater robot, as disclosed in an embodiment of the present invention.

[0065] Figure 3 This is an architecture diagram of an active oscillation control system disclosed in an embodiment of the present invention.

[0066] Reference numerals: 1. Mother ship; 2. Gantry 2; 3. Stator; 4. Underwater robot. Detailed Implementation

[0067] 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.

[0068] According to embodiments of the present invention, it should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the following manufacturing method, in some cases the steps shown or described may be performed in a different order than that shown here.

[0069] The underwater robot deployment and retrieval anti-roll and anti-sway control method of the present invention comprises the following controlled object system: Figure 1 As shown, the system mainly includes a mother ship 1, a gantry 2 (also known as an A-frame), a sway stabilizer 3, and an underwater robot 4 (ROV). The mother ship 1 serves as the carrying platform, responsible for transporting the gantry, sway stabilizer 3, and underwater robot 4 to the target operating area. The gantry, mounted on the ship's side, functions to allow the underwater robot 4 to swing inwards and outwards from the ship's side for deployment and retrieval. The sway stabilizer 3 is the key mechanism for reducing roll and sway; driven by two hydraulic cylinders or a motor, it can actively compensate for two degrees of freedom in the horizontal plane—longitudinal (along the bow and stern of the mother ship) and lateral (along the port and starboard sides of the mother ship)—effectively suppressing the swaying of the underwater robot 4. The underwater robot 4 is used to perform various underwater operations such as inspecting oil and gas facilities and laying fiber optic cables.

[0070] To accurately describe the motion relationships between the various parts of the system, this invention defines, as follows: Figure 1 The three right-handed rectangular coordinate systems shown are:

[0071] Geodetic coordinate system Its origin Select a point on the sea level, The axis points to geographic due north. The axis points vertically downwards towards the Earth's center.

[0072] Ship coordinate system Fixed to the mother ship, its origin Located at the ship's center of gravity, The axis points towards the bow (directly in front). The axis points to the starboard side.

[0073] Robot coordinate system Fixed to the underwater robot, its origin Located at the robot's center of gravity, The axis points directly in front of the robot. The axis points to the right side of the robot.

[0074] The following will describe in detail the anti-sway and anti-roll control method for underwater robot deployment and retrieval provided by the present invention, based on the above definition. Figure 2 As shown, the method includes the following steps:

[0075] Step S100: Estimate the position coordinates of the gantry support point in the ship's coordinate system;

[0076] The goal of this step is to determine Figure 1 In the ship coordinate system, the gantry support point P The position coordinates in the middle are denoted as Because different models of underwater robot systems are equipped with different deployment and recovery systems and gantry structure designs, the gantry fulcrum P relative to the origin of the ship's coordinate system... The installation position of the (i.e., the ship's center of gravity) is not fixed.

[0077] Therefore, to achieve accurate calculations, it is necessary to obtain the specific installation position parameters of the gantry on the mother ship in advance. These installation position parameters are determined during the design phase and can be input into the control system as known quantities. During actual operation, the control system also needs to receive real-time status information from gantry sensors, such as the gantry's pitch angle and slewing angle. Combining these pre-set installation position parameters and real-time gantry status data, and through a series of geometric transformations related to the specific gantry mechanical structure design, the position coordinates of the gantry support point P in the ship's coordinate system can be dynamically and accurately solved. .

[0078] It should be noted that the geometric conversion process here is highly dependent on the specific gantry design and is a mature application of geometry. Therefore, the specific derivation process will not be elaborated here.

[0079] Step S200: Based on the position coordinates of the gantry support point in the ship coordinate system, and the heading angle, pitch angle, roll angle of the mother ship and the coordinate position of the mother ship in the geodetic coordinate system, calculate the motion state of the gantry support point in the geodetic coordinate system.

[0080] Obtain the position coordinates of the gantry support point P in the ship coordinate system. The core objective of this step is to calculate the motion state (including position and velocity) of the gantry pivot P in the geodetic coordinate system. This motion state is a key factor for active compensation because the mother ship's own motion (roll, pitch, heave, and translation) is transmitted through the gantry, thus causing the underwater robot to oscillate. To achieve this goal, precise coordinate transformation and kinematic derivation are required.

[0081] First, calculate the absolute position coordinates of the gantry support point P in the geodetic coordinate system. The calculation is accomplished by superimposing two parts: first, the position of the mother ship's center of gravity in the geodetic coordinate system. The first is the position vector of the gantry pivot P relative to the center of gravity of the mother ship, which can be directly provided by the mother ship's inertial navigation system; the second is the expression of the position vector in the geodetic coordinate system. The latter requires a rotation matrix from the ship's coordinate system to the geodetic coordinate system. Perform the transformation. This rotation matrix... Determined by the mother ship's three attitude angles, namely the heading angle. Pitch angle Roll angle Decision made. The specific calculation formula is as follows:

[0082]

[0083] in, , and This indicates the position coordinates of the gantry support point in the geodetic coordinate system; , and This indicates the coordinates of the mother ship's center of gravity in the geodetic coordinate system. , and This indicates the position coordinates of the gantry support point in the ship's coordinate system.

[0084] Let be the rotation matrix from the ship coordinate system to the geodetic coordinate system, determined by the mother ship's attitude angles. The expression for this rotation matrix is:

[0085]

[0086] in, Indicates the heading angle of the mother ship; Indicates the mother ship's pitch angle; Indicates the roll angle of the mother ship.

[0087] Secondly, for effective real-time feedforward and feedback control, knowing only the position is insufficient; the velocity of the gantry support point is also required. This invention calculates the horizontal velocity of the gantry support point in the geodetic coordinate system by differentiating the aforementioned positional relationship with time. and (axis) speed and Considering that roll reduction and sway control primarily compensates for the direct impact of the mother ship's pitch and roll motions on horizontal sway, the pitch angular velocity is taken into account in the speed derivation. and roll angular velocity Its function. The calculation formula is as follows:

[0088]

[0089]

[0090] in, and These respectively represent the gantry support points along the geodetic coordinate system. shaft and Velocity in the axial direction, The pitch angular velocity, This is the roll angular velocity.

[0091] Through the above calculations, the real-time position and precise horizontal movement speed of the gantry support point in the geodetic coordinate system are obtained.

[0092] Step S300: Obtain the linear acceleration and linear velocity of the underwater robot in the robot coordinate system, and project the linear acceleration and linear velocity onto the geodetic coordinate system to obtain the motion state of the underwater robot in the geodetic coordinate system;

[0093] This step aims to acquire the underwater robot's own motion information and unify it to a geodetic coordinate system for comprehensive calculation along with the motion state of the gantry pivot. To achieve this, an inertial navigation system (INS) mounted on the underwater robot is required. This INS can measure and output the robot's coordinates in its own coordinate system in real time and at high frequency. Triaxial acceleration and three-axis velocity .

[0094] However, since the underwater robot's attitude (especially its heading) may differ from that of the mother ship, there is an orientational deviation between its body coordinate system and the geodetic coordinate system. To analyze the relative motion between the robot and the gantry pivot point in a unified reference frame, the robot's motion must be projected onto the geodetic coordinate system. The key parameter for this transformation is the heading angle deviation between the underwater robot and the mother ship. Based on this heading angle deviation, the linear acceleration and linear velocity in the robot coordinate system can be projected to the geodetic coordinate system through coordinate rotation calculations. The projection formula is as follows:

[0095] For the projection of linear acceleration:

[0096]

[0097]

[0098] For the projection of linear velocity:

[0099]

[0100]

[0101] in, , Indicates the underwater robot in its robot coordinate system , Axial linear acceleration; , Indicates the underwater robot in its robot coordinate system , Axial linear velocity; This indicates the heading angle deviation between the underwater robot and the mother ship; , This indicates the underwater robot obtained after projection in the geodetic coordinate system. , Axial acceleration; , This indicates the underwater robot obtained after projection in the geodetic coordinate system. , Axial velocity.

[0102] Through the above projection calculations, the acceleration and velocity information of the underwater robot in a unified geodetic coordinate system were obtained.

[0103] However, in practical applications, relying solely on instantaneous data from INS to calculate absolute position may result in cumulative errors or numerical fluctuations. To obtain more accurate and stable robot position information... For use in subsequent steps, this invention further employs data fusion and smoothing algorithms. In specific implementations, the following methods can be used depending on different accuracy requirements and system configurations:

[0104] a) Reference position estimation based on ship inertial navigation

[0105] First, based on the information provided by the ship's inertial navigation system, the position of the location directly below the gantry in the geodetic coordinate system is estimated in real time. , , This estimate forms a dynamic reference point that moves with the mother ship. The underwater robot's relative position coordinates are... Calculated using the following formula:

[0106]

[0107]

[0108] in, , This represents the original position coordinates of the underwater robot in the geodetic coordinate system, which are directly provided or preliminarily calculated by the underwater robot's inertial navigation system. , This indicates the position information of the gantry directly below in the geodetic coordinate system, estimated in real time based on the ship's inertial navigation information. This indicates the heading angle deviation between the underwater robot and the mother ship; , This represents the relative position coordinates of the underwater robot on the horizontal plane, obtained after reference compensation and used for swing calculation.

[0109] b) Use filtering algorithms to improve estimation accuracy and stability

[0110] Considering the location information estimated by method a), , Since the accuracy may be low and the values ​​may be unstable, this invention further employs Kalman filtering or extended Kalman filtering methods. This algorithm integrates the linear acceleration measured in real-time by the underwater robot's inertial navigation system. and This information is used to optimally estimate the robot's position and velocity state, thereby significantly improving the accuracy and numerical stability of position estimation. Through this filtering process, the optimized position estimate is obtained. , By substituting these values ​​into the above formula, a more reliable relative position can be obtained:

[0111]

[0112]

[0113] c) Smoothing based on moving average

[0114] To address the characteristic that the mother ship typically stops or moves at low speed during the deployment and recovery of underwater robots, while the robot oscillates at a high frequency after emerging from the water, this invention also provides a smoothing method using moving average filtering. This method uses the robot's original position coordinates in the geodetic coordinate system... Perform a moving average calculation, taking the average value over the most recent period (e.g., 10 seconds). , This average value effectively represents the low-frequency motion of the mothership, thus enabling the separation of the robot's high-frequency oscillations relative to the mothership. The calculation formula is as follows:

[0115]

[0116]

[0117] By implementing one or more of the above data fusion and smoothing methods, the system can effectively suppress measurement noise and low-frequency drift, and finally output high-precision, high-stability effective relative position coordinates of the underwater robot relative to the dynamic reference. .

[0118] Step S400: Based on the motion state of the gantry support point in the geodetic coordinate system and the motion state of the underwater robot in the geodetic coordinate system, calculate the lateral and longitudinal swing angles of the underwater robot relative to the gantry support point;

[0119] This step transforms the motion state information of the gantry fulcrum and the underwater robot into a swing state quantity that can be directly used for control decisions, namely the lateral swing angle. Longitudinal swing angle and its corresponding angular velocity , .

[0120] First, a spatial geometric relationship model is established between the gantry fulcrum P and the underwater robot's center of gravity M. This spatial geometric relationship model describes the relationship considering the mother ship's heading angle. In this case, how is the absolute position of robot point M determined by the position of gantry fulcrum P and the length of the connecting cable? And the swing angle in both directions (Horizontal) (Vertical) Jointly determined. Their precise three-dimensional positional relationship is expressed by the following system of equations:

[0121]

[0122]

[0123]

[0124] in, This represents the position coordinates of the gantry support point P in the geodetic coordinate system. This represents the position coordinates of the underwater robot's center of gravity M in the geodetic coordinate system after data fusion and smoothing. This represents the cable length between the gantry fulcrum P and the underwater robot's center of gravity M. This length can be obtained through the cable length sensor of the cable winch or calculated by inversely using the three-dimensional coordinates of P and M. This indicates the heading angle of the mother ship. This indicates the lateral swing angle of the underwater robot relative to the gantry pivot point. This represents the longitudinal swing angle of the underwater robot relative to the gantry pivot point.

[0125] To obtain the pendulum angular velocity, the position equation above needs to be differentiated. By differentiating both sides of the equation with respect to time, the precise relationship between the pendulum angular velocity and the linear velocities of points P and M can be obtained:

[0126]

[0127]

[0128] In actual deployment and retrieval operations, compensatory control to suppress swaying typically occurs during periods of relatively small sway angles. When both the lateral and longitudinal sway angles are detected to be less than 10°, the complex nonlinear model described above can be rationally simplified using a small-angle approximation principle. This can greatly simplify the calculations and meet the requirements for real-time control. The approximations include: And all higher-order small quantities (such as the product of the two pendulum angles and its product with the angular velocity) are approximately zero, that is:

[0129]

[0130]

[0131]

[0132]

[0133]

[0134]

[0135]

[0136]

[0137] After applying this small-angle approximation, the position equation can be simplified to:

[0138]

[0139]

[0140] Accordingly, the velocity equation can be simplified to:

[0141]

[0142]

[0143] Finally, by solving the simplified position equation in reverse, the lateral and longitudinal swing angles used for control feedback can be directly obtained. , The calculation formula is as follows. Simultaneously, by solving the simplified velocity equation in reverse, the corresponding pendulum angular velocity can be obtained. , The final calculation formula is as follows:

[0144]

[0145]

[0146]

[0147]

[0148] in, , This indicates the position coordinates of the gantry support point in the geodetic coordinate system; , This represents the position coordinates of the underwater robot's center of gravity in the geodetic coordinate system after data fusion and smoothing. , This indicates the velocity of the gantry support point in the geodetic coordinate system. , This represents the underwater robot's velocity in the geodetic coordinate system after data fusion and smoothing. This indicates the heading angle deviation between the underwater robot and the mother ship; This indicates the length of the cable between the gantry fulcrum and the center of gravity of the underwater robot. Indicates the lateral swing angle; Indicates the longitudinal swing angle; Indicates the lateral angular velocity; This indicates the longitudinal angular velocity.

[0149] Through the above calculation process, the system finally obtains clear and quantified swing state information of the underwater robot in real time.

[0150] Step S500: Based on the lateral and longitudinal swing angles, generate control commands to drive the sway damper to perform motion compensation in the lateral and longitudinal directions, thereby achieving anti-sway control.

[0151] The calculated oscillation state quantity (lateral oscillation angle) Longitudinal swing angle and its angular velocity , The commands are converted into specific control instructions, which drive the oscillation damper actuator to actively counteract the underwater robot's swaying and achieve sway reduction.

[0152] After obtaining accurate feedback on the oscillation state, the control system needs to employ a suitable control algorithm to calculate the control input to be applied to the oscillator. This invention has broad applicability in this regard, allowing for the flexible selection and implementation of various control strategies based on different system requirements for control performance, computational complexity, and model dependence. These strategies include, but are not limited to, classical PID control, optimal control algorithms based on modern control theory such as linear quadratic regulators, or more advanced control methods that rely on a precise dynamic model of the oscillator system, such as model predictive control or dynamic inverse control.

[0153] To illustrate the control command generation process, this invention uses the widely used and simple PID control algorithm as an example. Within this algorithm framework, the system designs independent PD controllers for the lateral and longitudinal channels respectively. The input to the controller is the swing angle error and its rate of change (i.e., swing angular velocity) of the corresponding channel, and the output is the control signal used to drive the anti-sway mechanism in that direction. Its control law is given by the following formula:

[0154]

[0155]

[0156] in, This indicates the generated control commands for the lateral oscillation actuator. This indicates the generated control command for the longitudinal anti-sway actuator. , These represent the proportional coefficients of the lateral and longitudinal control channels, respectively. Their function is to respond to the magnitude of the swing angle deviation. The larger the coefficient, the faster the swing angle is suppressed, but too large a coefficient may lead to system instability. , These represent the differential coefficients of the lateral and longitudinal control channels, respectively. Their function is to sense and respond to the changing trend of the swing angle (angular velocity), provide system damping, effectively suppress swaying, and improve stability.

[0157] Ultimately, the generated control commands and The signals are transmitted in real time to the deployment and recovery control system. Based on the command signals, the system precisely controls the lateral and longitudinal hydraulic cylinders or servo motors driving the oscillation damper to produce corresponding compensating movements via power amplification components such as hydraulic servo valves or motor drivers. The direction of this compensating movement is opposite to the underwater robot's oscillation tendency, thereby effectively increasing system damping, absorbing oscillation energy, and ultimately achieving active suppression and precise stable control of the underwater robot's oscillation, ensuring the safety and efficiency of deployment and recovery operations.

[0158] The control system architecture of this invention is as follows: Figure 3 As shown. The control module of this invention can operate independently as a controller outside the original deployment and recovery system controller, achieving anti-sway control of the robot by communicating with the original deployment and recovery control system. Alternatively, it can operate as an algorithm software module within the original deployment and recovery control system to achieve anti-sway control.

[0159] This invention estimates the position of the gantry support point (e.g., by reading the current gantry system geometric parameters, underwater robot inertial navigation data, and ship inertial navigation data) Figure 1 Point P shown) and underwater robots (such as Figure 1 The motion state of point M (as shown) is used to estimate the lateral sway angle, lateral sway velocity, longitudinal sway angle, and longitudinal sway velocity of the underwater robot relative to the ship. Then, the existing deployment and recovery control system controls the stabilizer to perform longitudinal sway compensation and anti-sway control in the direction of sway, and lateral sway compensation and anti-sway control in the direction of sway.

[0160] According to another aspect of the embodiments of this application, an electronic device is also provided, including a processor and a memory, wherein the processor is configured to implement the steps of the method when executing a computer program stored in the memory.

[0161] In the above embodiments of the present invention, the descriptions of each embodiment have different focuses. For parts not described in detail in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0162] In the several embodiments provided in this application, it should be understood that the disclosed technical content can be implemented in other ways. The device embodiments described above are merely illustrative; for example, the division of units can be a logical functional division, and in actual implementation, there may be other division methods. For instance, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the displayed or discussed mutual coupling, direct coupling, or communication connection may be through some interfaces; the indirect coupling or communication connection between units or modules may be electrical or other forms.

[0163] Furthermore, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0164] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, read-only memory (ROM), random access memory (RAM), portable hard drives, magnetic disks, or optical disks.

[0165] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A method for reducing sway and preventing rocking during the deployment and retrieval process of an underwater robot, characterized in that, Includes the following steps: Estimate the position coordinates of the gantry support point in the ship's coordinate system; Based on the position coordinates of the gantry support point in the ship coordinate system, as well as the heading angle, pitch angle, roll angle of the mother ship and the coordinate position of the mother ship in the geodetic coordinate system, the motion state of the gantry support point in the geodetic coordinate system is calculated. The linear acceleration and linear velocity of the underwater robot in the robot coordinate system are obtained, and the linear acceleration and linear velocity are projected onto the geodetic coordinate system to obtain the motion state of the underwater robot in the geodetic coordinate system. Based on the motion state of the gantry support point in the geodetic coordinate system and the motion state of the underwater robot in the geodetic coordinate system, calculate the lateral and longitudinal swing angles of the underwater robot relative to the gantry support point; Based on the lateral and longitudinal swing angles, control commands are generated to drive the sway damper to perform motion compensation in the lateral and longitudinal directions, thereby achieving anti-sway control. Methods for calculating the motion state of the gantry support in the geodetic coordinate system include: Based on the mother ship's heading angle, pitch angle, and roll angle, construct a coordinate transformation matrix from the ship's coordinate system to the geodetic coordinate system; Using the coordinate transformation matrix, the position coordinates of the gantry support point in the ship coordinate system are transformed to obtain the position of the gantry support point relative to the center of gravity of the mother ship in the geodetic coordinate system; By superimposing the position relative to the center of gravity of the mother ship with the coordinate position of the center of gravity of the mother ship in the geodetic coordinate system, the absolute position coordinates of the gantry support point in the geodetic coordinate system are finally obtained. By differentiating the relationship between the absolute position coordinates of the gantry support and time, the velocity of the gantry support in the geodetic coordinate system is calculated. Coordinate transformation and motion state calculation are achieved through the following formulas: The position coordinates of the gantry support point in the geodetic coordinate system are calculated using the following formula: in, , and This indicates the position coordinates of the gantry support point in the geodetic coordinate system; , and This indicates the coordinates of the mother ship's center of gravity in the geodetic coordinate system. , and This indicates the position coordinates of the gantry support point in the ship's coordinate system; The rotation matrix from the ship coordinate system to the geodetic coordinate system, determined by the mother ship's attitude angles, is expressed as follows: in, Indicates the heading angle of the mother ship; Indicates the mother ship's pitch angle; Indicates the roll angle of the mother ship; The velocity of the gantry support point in the geodetic coordinate system is calculated using the following formula: in, and These respectively represent the gantry support points along the geodetic coordinate system. shaft and Velocity in the axial direction, The pitch angular velocity, This is the roll angular velocity.

2. The method for reducing sway and preventing rocking during the deployment and retrieval of an underwater robot as described in claim 1, characterized in that, Methods for obtaining the motion state of an underwater robot in a geodetic coordinate system include: The inertial navigation system installed on the underwater robot can be used to obtain its linear acceleration and linear velocity in the robot coordinate system in real time. Determine the heading angle deviation between the underwater robot and the mother ship; Based on the heading angle deviation, the linear acceleration and linear velocity are transformed by coordinate projection to calculate the acceleration and velocity of the underwater robot in the geodetic coordinate system.

3. The method for reducing sway and preventing rocking during the deployment and retrieval of an underwater robot as described in claim 2, characterized in that, The coordinate projection transformation is achieved through the following formula: The acceleration of the underwater robot in the geodetic coordinate system is calculated using the following formula: The velocity of the underwater robot in the geodetic coordinate system is calculated using the following formula: in, , Indicates the underwater robot in its robot coordinate system , Axial linear acceleration; , Indicates the underwater robot in its robot coordinate system , Axial linear velocity; This indicates the heading angle deviation between the underwater robot and the mother ship; , This indicates the underwater robot obtained after projection in the geodetic coordinate system. , Axial acceleration; , This indicates the underwater robot obtained after projection in the geodetic coordinate system. , Axial velocity.

4. The method for reducing sway and preventing rocking during the deployment and retrieval process of an underwater robot as described in claim 2 or 3, characterized in that, After obtaining the velocity and acceleration of the underwater robot in the geodetic coordinate system, a filtering algorithm is used to perform data fusion and smoothing on the position information of the underwater robot; the filtering algorithm is Kalman filtering or extended Kalman filtering.

5. The method for reducing sway and preventing rocking during the deployment and retrieval of an underwater robot as described in claim 4, characterized in that, Methods for calculating the lateral and longitudinal sway angles of an underwater robot relative to the gantry pivot include: A geometric motion model is established between the gantry fulcrum and the underwater robot. This geometric motion model correlates the relative positional relationship between the gantry fulcrum and the underwater robot with the lateral and longitudinal swing angles. Based on the real-time position coordinates of the gantry support and the underwater robot in the geodetic coordinate system, the relative displacement between the two is calculated. By combining the cable length connecting the gantry fulcrum to the underwater robot and the heading angle deviation between the mother ship and the robot, the values ​​of the lateral and longitudinal sway angles are calculated through the geometric motion model. The lateral and longitudinal angular velocities are obtained by calculating the rate of change of the positional relationship over time.

6. The method for reducing sway and preventing rocking during the deployment and retrieval of an underwater robot as described in claim 5, characterized in that, Assuming the swing angle is less than 10 degrees, the small-angle approximation method (less than 10 degrees) is used to calculate the swing angle and angular velocity using the following formulas: The lateral swing angle and longitudinal swing angle Calculated using the following formula: The lateral sway velocity and longitudinal angular velocity Calculated using the following formula: in, , This indicates the position coordinates of the gantry support point in the geodetic coordinate system; , This represents the position coordinates of the underwater robot's center of gravity in the geodetic coordinate system after data fusion and smoothing. , This indicates the velocity of the gantry support point in the geodetic coordinate system. , This represents the underwater robot's velocity in the geodetic coordinate system after data fusion and smoothing. This indicates the heading angle deviation between the underwater robot and the mother ship; This indicates the length of the cable between the gantry fulcrum and the center of gravity of the underwater robot. Indicates the lateral swing angle; Indicates the longitudinal swing angle; Indicates the lateral angular velocity; This indicates the longitudinal angular velocity.

7. An electronic device, comprising a memory and a processor, characterized in that, The memory is used to store programs that support the processor in executing the anti-roll and anti-sway control method during the deployment and retrieval process of any one of the underwater robots described in claims 1-6, and the processor is configured to execute the programs stored in the memory.

8. A computer-readable storage medium storing a computer program thereon, characterized in that, When the computer program is run by the processor, it executes the steps of the anti-sway and anti-roll control method in the deployment and recovery process of the underwater robot according to any one of claims 1-6.

Images