An underwater robot travel control method, device and medium

Abstract

This invention relates to a method, device, and medium for controlling the movement of an underwater robot. The method includes: detecting the track slippage state of the robot and acquiring operational parameters; controlling the robot's vertical thruster system based on the track slippage state to adjust the track's ground pressure and stabilize the robot's attitude; calculating and outputting thrust commands for each vertical thruster based on the robot's real-time attitude and motion state during the control of the vertical thruster system; calculating the dynamic interference generated by the operational tool on the robot based on the operational parameters; calculating the compensation thrust and compensation torque required by the horizontal thruster system based on the dynamic interference, and controlling the rotation angle of the horizontal thrusters to align the thrust direction; distributing the compensation thrust and compensation torque to each horizontal thruster and outputting the corresponding thrust command for the horizontal thruster. This method achieves high stability, high maneuverability, and high operational efficiency for robot movement in uncertain and complex seabed environments.

Description

Technical Field

[0001] This invention belongs to the field of underwater operation engineering equipment underwater walking control, specifically involving an underwater robot walking control method, equipment and medium. Background Technology

[0002] Underwater trenching machines, seabed mining robots, and other underwater operational equipment are core engineering equipment for marine resource development, subsea pipeline laying, and maintenance. These machines are typically characterized by high power, large size, and heavy weight, and their operating environments face complex seabed topography and varied seabed soil and sediment (such as soft silt and hard bedrock). To adapt to this complex environment, existing underwater robots are usually equipped with multimodal locomotion mechanisms, such as integrating tracks, skids, and propulsion systems, to meet the locomotion needs under different geological conditions.

[0003] However, existing technologies have significant shortcomings in practical applications:

[0004] Firstly, regarding walking control, underwater robots cannot accurately predict seabed topography and soil composition when navigating the ocean floor. Even at the current moment, it is extremely difficult to detect the specific working environment within a localized area surrounding the robot in real time. This introduces significant uncertainty into walking control. Existing control methods largely rely on operator experience or employ simple threshold response strategies, resulting in low levels of intelligence. When the tracks spin or slip on soft seabed, timely and effective adjustments are difficult to make, often leading to the robot sinking, getting stuck, or experiencing a sharp decline in walking efficiency, and even the risk of the entire robot overturning due to localized collapse.

[0005] Secondly, in terms of posture stability, existing technologies are insufficient for robots to adapt to steep slopes. When walking on lateral or longitudinal slopes, robots are prone to tilting to the side, leaning forward, or tilting backward. There is a lack of effective active stability control mechanisms to dynamically adjust various mechanisms to counteract overturning moments, resulting in high safety risks.

[0006] Furthermore, regarding the coordination of work and movement, when a robot is equipped with high-powered tools such as water jets, the enormous reaction forces they generate (e.g., jet reaction forces) will significantly disrupt the robot's dynamic interference forces and torques. This interference severely damages the robot's trajectory and posture stability. Existing solutions often employ closed-loop feedback for passive compensation, resulting in delayed response, complex control algorithms, and difficulty in effectively suppressing interference in its early stages, leading to poor coordinated control performance for "walking and working simultaneously."

[0007] Therefore, there is an urgent need in this field for an underwater robot movement control method that can intelligently sense the walking status, adaptively adjust the walking mode, and actively compensate for operational interference, so as to improve its walking stability, passability and operational efficiency in complex seabed environments. Summary of the Invention

[0008] This invention provides a method, device, and medium for controlling the movement of an underwater robot. Its purpose is to solve the problems of unstable movement, poor passability, and low work efficiency caused by track slippage, body overturning, and interference from the reaction force of working tools when underwater robots walk in complex seabed terrain and sediment environments.

[0009] To achieve the above objectives, the first aspect of the present invention provides a method for controlling the movement of an underwater robot, comprising the following steps:

[0010] The system detects the track slippage state of the underwater robot and obtains the operating parameters of the operating tools carried by the underwater robot.

[0011] Based on the track slippage state, the vertical thruster system of the underwater robot is controlled to adjust the track pressure on the ground and stabilize the underwater robot's attitude.

[0012] During the control of the vertical thruster system, the thrust commands of each vertical thruster are calculated and output based on the real-time attitude and motion state of the underwater robot.

[0013] Based on the operating parameters, calculate the dynamic interference generated by the operating tool on the underwater robot;

[0014] Based on the dynamic disturbance, the required compensating thrust and compensating torque of the horizontal thruster system are calculated, and the rotation angle of the horizontal thruster is controlled to align with the thrust direction.

[0015] The compensated thrust and compensated torque are distributed to each horizontal thruster, and the corresponding thrust command for the horizontal thruster is output.

[0016] Furthermore, the method for detecting the track slippage state of the underwater robot includes:

[0017] Acquire the inertial navigation data and speed data of the left and right track encoders of the underwater robot;

[0018] Based on the forward velocity and yaw rate in the inertial navigation data, and the installation position of the left and right tracks relative to the robot center, the actual travel speed of the left and right tracks is calculated.

[0019] Based on the speed data of the left and right track encoders and the calculated actual travel speed of the left and right tracks, the slip ratio of the left and right tracks is calculated respectively.

[0020] Based on the slip ratios of the left and right tracks, the average slip ratio of the tracks is calculated, and this average slip ratio is used as a quantitative indicator of the track slip state.

[0021] Furthermore, the method for controlling the vertical thruster system of the underwater robot according to the track slippage state to adjust the track-to-ground pressure and stabilize the underwater robot's attitude includes:

[0022] Calculate the average slip ratio of the tracks of the underwater robot;

[0023] The average slip ratio is compared with a first threshold.

[0024] When the average slip ratio is greater than or equal to the first threshold, the track lifting cylinder of the underwater robot is controlled to lift the track to switch to the slipper walking mode, and the anti-slip control component of the vertical thruster is set to zero.

[0025] When the average slip ratio is less than the first threshold, the track walking mode is maintained, and the anti-slip control component of the vertical thruster is calculated by comparing the slip ratio of the left and right tracks with the second threshold.

[0026] Furthermore, the method for calculating the anti-slip control component of the vertical thruster includes:

[0027] For the left vertical thruster, if the left track slip ratio is less than the second threshold, the left anti-slip control component is zero; otherwise, the left anti-slip control component is calculated based on the difference between the left track slip ratio and the second threshold.

[0028] For the right vertical thruster, if the right track slip ratio is less than the second threshold, the right anti-slip control component is zero; otherwise, the right anti-slip control component is calculated based on the difference between the right track slip ratio and the second threshold.

[0029] Furthermore, the method for calculating and outputting thrust commands for each vertical thruster based on the real-time attitude and motion state of the underwater robot includes:

[0030] The anti-collapse control components are calculated based on the vertical velocity and vertical acceleration of the underwater robot.

[0031] The anti-tipping control components are calculated based on the roll angle and roll angular velocity of the underwater robot;

[0032] The anti-longitudinal overturning control component is calculated based on the pitch angle and pitch angular velocity of the underwater robot;

[0033] Combining the anti-slip control component, the anti-collapse control component, the anti-lateral overturning control component, and the anti-longitudinal overturning control component, the thrust command for each vertical thruster is calculated using the thrust distribution matrix;

[0034] The thrust distribution matrix is ​​a 4×3 matrix, used to map the anti-collapse control component, the anti-lateral overturning control component, and the anti-longitudinal overturning control component to the thrust commands of the four vertical thrusters, and superimpose them with the anti-slip control component for output.

[0035] Furthermore, the operating tool is a water jet, and the method for calculating the dynamic interference generated by the operating tool on the underwater robot based on the operating parameters includes:

[0036] Based on the nozzle area, nozzle inlet pressure, and flow coefficient of the water jet, the reaction force of the water jet is calculated using empirical formulas.

[0037] Based on the water jet's lowering angle, the connection point between the water jet and the underwater robot, and the inherent angle of the jet reaction force, the component of the jet reaction force in the underwater robot's coordinate system and the torque generated on the underwater robot's center of gravity are calculated.

[0038] Furthermore, based on the aforementioned dynamic disturbance, the method for controlling the rotation angle of the horizontal thruster to align with the thrust direction includes:

[0039] Based on the lowering angle of the water jet and the inherent angle of the jet reaction force, calculate the target rotation angle required for the horizontal propeller to compensate for the jet reaction force;

[0040] When the calculated target rotation angle is less than zero, it is forcibly set to zero to keep the horizontal thruster support perpendicular to the underwater robot frame.

[0041] Furthermore, based on the aforementioned dynamic disturbances, the method for calculating the compensating thrust and compensating torque required by the horizontal thruster system includes:

[0042] The magnitude of the compensating thrust is equal to the magnitude of the jet reaction force;

[0043] The magnitude of the compensation torque is equal to the magnitude of the torque generated by the component of the jet reaction force in the underwater robot coordinate system about its center of gravity.

[0044] Furthermore, the step of distributing the compensating thrust and compensating torque to each horizontal thruster is achieved through a fixed thrust distribution matrix, which maps the compensating thrust and compensating torque to the thrust commands of the four horizontal thrusters.

[0045] To achieve the above objectives, a second aspect of the present invention provides an electronic device including a memory and a processor, the memory being used to store a program that supports the processor in executing the underwater robot travel control method, and the processor being configured to execute the program stored in the memory.

[0046] 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 underwater robot travel control method.

[0047] The beneficial effects of this invention are:

[0048] Compared with existing technologies, the present invention provides an underwater robot movement control method, device, and medium that detects track slippage in real time, automatically switches between track and slipper walking modes based on a slip ratio threshold, and dynamically adjusts the vertical thruster output according to the slip ratio in track mode to change the track's ground pressure, effectively suppressing slippage. Simultaneously, the vertical thruster control incorporates real-time robot attitude and motion parameters, calculates a comprehensive thrust command including anti-collapse and anti-overturning components, and outputs it to each thruster through a thrust distribution matrix, actively maintaining the robot's stability on complex terrain. Furthermore, by acquiring operational parameters and calculating the dynamic interference generated by the operational tools, the horizontal thruster rotation angle is controlled to align its thrust direction with the interference force, and compensating thrust and torque are precisely distributed to each horizontal thruster, achieving active feedforward compensation for operational reaction forces. This collaboratively solves the problems of unstable walking, poor maneuverability, and low operational efficiency caused by complex seabed topography and operational interference. Attached Figure Description

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

[0050] Figure 1 This is a schematic diagram of the layout of the underwater robot tracks and thrusters disclosed in an embodiment of the present invention.

[0051] Figure 2 This is a schematic diagram of the layout and switching of the underwater robot tracks and skids disclosed in an embodiment of the present invention, wherein... Figure 2 (a) A schematic diagram of controlling the telescopic cylinder to "press down" the track. Figure 2 (b) Schematic diagram of driving the hydraulic cylinder to “lift” the track.

[0052] Figure 3 This is a diagram of the collaborative control framework for the underwater robot's composite vertical thruster, as disclosed in an embodiment of the present invention.

[0053] Figure 4 This is a flowchart of the collaborative control of the underwater robot's composite vertical thruster, as disclosed in an embodiment of the present invention.

[0054] Figure 5 This is a force analysis diagram of the water jet and horizontal propeller disclosed in an embodiment of the present invention.

[0055] Figure 6 This is a diagram of the collaborative control framework for the underwater robot's composite vertical thruster, as disclosed in an embodiment of the present invention.

[0056] Figure 7 This is a flowchart of the collaborative control of the underwater robot's composite vertical thruster, as disclosed in an embodiment of the present invention.

[0057] Reference numerals: 1. Left track; 2. Right track; 3. Vertical thruster; 4. Horizontal thruster; 5. Thruster steering cylinder; 6. Lateral thruster. Detailed Implementation

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

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

[0060] Figure 1 The layout of a walking and propulsion system for an underwater robot is shown. The structure includes a left track 1 and a right track 2 for driving on the seabed, a vertical thruster 3 for providing vertical motion control and attitude adjustment for the underwater robot, a horizontal thruster 4 for propulsion and heading control in the horizontal plane, a thruster steering cylinder 5 that can adjust the thrust direction of the horizontal thruster 4, and a lateral thruster 6 for assisting lateral movement.

[0061] According to this Figure 1 Regarding underwater robots, this invention provides a method for controlling the movement of an underwater robot, the method comprising the following steps:

[0062] Step S100: Detect the track slippage state of the underwater robot and obtain the operating parameters of the operating tools carried by the underwater robot;

[0063] Specifically, this step acquires the robot's overall motion data, including the robot's velocity along the forward direction, through an inertial navigation system installed on the robot itself. and yaw rate about the vertical axis Meanwhile, the speed of the left track encoder is read and calculated in real time by encoders installed on the left and right track drive mechanisms. and right track encoder speed .

[0064] The actual travel speed of the robot's left and right track center points is calculated based on inertial navigation data. This is because the actual speed of the two tracks differs depending on rotation when the robot turns. The specific calculation formula is as follows:

[0065] (1)

[0066] in, This indicates the actual speed of travel at the center point of the left track. This indicates the actual speed of travel at the center point of the right track. Let the speed be the robot's velocity in the direction of travel. For the robot's yaw speed, and These represent the lateral installation distances of the left and right tracks from the center of the robot, respectively.

[0067] Track slippage or idling is diagnosed by comparing the theoretical speed of the track encoder with the calculated actual travel speed. The slip ratios of the left and right tracks are also considered. and Calculate using the following formulas respectively:

[0068] (2)

[0069] in, and This represents the left and right track speeds calculated by the encoder. The actual travel speed at the center point of the left track, as calculated above. This represents the actual travel speed of the right track center point obtained from the aforementioned calculation.

[0070] when and If the value is small, it is assumed that the track is not slipping or spinning freely; if and When the value is large, it is determined that the track is slipping or spinning freely, and the larger the corresponding data, the more serious the slipping and spinning is.

[0071] Finally, a comprehensive evaluation of the slippage of the tracks on both sides was conducted, and the average slippage rate of the tracks was calculated. This is used as a quantitative indicator to determine the slippage state of the robot's tracks. The calculation formula is:

[0072] (3)

[0073] Meanwhile, in this step, it is also necessary to obtain the operating parameters of the working tool carried by the underwater robot. When the working tool is a water jet, these parameters include, but are not limited to: the outlet area of ​​the water jet nozzle, the working pressure at the nozzle inlet, the flow coefficient determined by the nozzle structure and layout (with a value between 0.9 and 0.95), the lowering angle of the water jet relative to the robot frame, and the inherent angle between the high-pressure jet jet force and the water jet arm.

[0074] Step S200: Based on the track slippage state, control the vertical thruster system of the underwater robot to adjust the track pressure on the ground and stabilize the underwater robot's attitude.

[0075] The purpose of this step is to determine the walking mode and generate preliminary anti-slip control commands for the vertical thruster based on the track slip state calculated in step S100, so as to achieve self-adaptation to the seabed sediment.

[0076] In practice, the average slip ratio of the tracks calculated in step S100 is first read. Two slip ratio thresholds are preset: the first threshold... The second threshold is used to determine whether a walking mode switch is needed. Used to determine whether fine anti-slip pressure adjustment is needed in tracked mode.

[0077] average slip With the first threshold The comparison is performed, and different control strategies are implemented based on the comparison results:

[0078] when This indicates that the seabed geology is too soft, causing the tracks to slip and spin excessively, making it unsuitable to continue using tracked locomotion. For example... Figure 2 As shown, at this time, the output control signal is sent to the telescopic cylinder connecting the track and the robot body frame, driving the cylinder to "lift" the track (see...). Figure 2 (b) switches the underwater robot from "tracked walking mode" to "slippered walking mode". The slippered shoes reduce the ground contact area, thereby reducing resistance in the direction of travel and preventing the robot from sinking. Conversely, when the geological conditions are deemed suitable for tracked walking, the telescopic cylinders are controlled to "press down" the tracks (see...). Figure 2 (a) to switch back to tracked walking mode. Meanwhile, in slipper walking mode, it is determined that no further anti-slip control is needed for the vertical thrusters; therefore, all anti-slip control components of the vertical thrusters are... Set to zero. ,Right now:

[0079] (4)

[0080] Among them, subscript These represent four vertical thrusters: forward to the right, backward to the right, forward to the left, and backward to the left.

[0081] when The current seabed geology indicates that the tracked path is generally suitable, but localized or slight slippage may still occur. The "tracked walking mode" will be maintained (with the tracks in a "pressed-down" state), and the anti-slip / anti-idle slip control of the vertical thrusters will be activated. This control is based on the slip ratios of the left and right tracks calculated in step S100. and With the second threshold The comparison results are used to dynamically calculate the anti-slip control components of each thruster. The calculation logic is expressed using conditional operators as follows:

[0082] For the two vertical thrusters on the right (front right and rear right):

[0083] (5-1)

[0084] For the two vertical thrusters on the left (front left and rear left):

[0085] (5-2)

[0086] in, This is the anti-slip control component moving forward to the right. The anti-slip control component is located to the right. This is the anti-slip control component moving forward to the left. The anti-slip control component is located to the left. It is a control gain coefficient. The formula means that when the slip ratio of one side of the track is lower than... When the slippage on that side is considered minor, the corresponding vertical thruster does not output anti-slip control force (component is 0); when the slip ratio exceeds When that happens, it depends on the excess value and the gain coefficient. The product of the two components generates a positive anti-slip control component, which will be used in subsequent calculations to ultimately increase the ground pressure of the track on that side by increasing the thrust of the corresponding side thruster, thereby suppressing slippage.

[0087] Step S300: During the process of controlling the vertical thruster system, the thrust command of each vertical thruster is calculated and output based on the real-time attitude and motion state of the underwater robot.

[0088] Based on the generated anti-slip control components, the real-time motion state information of the robot body is further introduced to calculate a comprehensive thrust command that can simultaneously prevent the robot from collapsing and overturning, thereby achieving all-round attitude stability of the underwater robot on complex seabed terrain.

[0089] In practice, it is necessary to continuously acquire real-time motion state parameters from the robot's inertial navigation system, including the robot's vertical velocity. Vertical acceleration Roll angle Roll angular velocity Pitch angle Pitch angular velocity Based on these parameters, the robot's anti-collapse control components and anti-overturning control components are calculated respectively:

[0090] Anti-collapse control: To prevent the robot from sinking into the soft seabed, damping of vertical motion is introduced to suppress rapid vertical displacement. Anti-collapse control force. The calculation formula is as follows:

[0091] (6)

[0092] in, It is the gain coefficient used for vertical motion control. It is the robot's vertical velocity. This represents the control gain coefficient related to vertical acceleration in anti-collapse control. This is the robot's vertical acceleration. The anti-collapse control force generated by this formula... Its direction is opposite to the vertical motion direction, thus providing a damping effect.

[0093] Anti-tipping control: To prevent the robot from rolling sideways (rolling) and tilting forward or backward (pitching) on ​​steep slopes, negative feedback of angle and angular velocity is introduced to increase the damping of the robot's roll and pitch movements. Anti-tipping control torque. (Roll direction) and The formula for calculating (pitch direction) is as follows:

[0094] (7)

[0095] (8)

[0096] in, To control the gain for roll angle, To control the gain of roll angular velocity, Gain is used to control the pitch angle. To control the gain of pitch angular velocity, It is the robot's roll angle. It is the roll angular velocity. It's the robot's pitch angle. It is the pitch angular velocity.

[0097] Finally, the anti-slip control components of the four vertical thrusters calculated in step S200 are... , , , In conjunction with the aforementioned overall anti-collapse control force Anti-roll control torque Anti-pitch control torque Perform synthesis.

[0098] By using a fixed 4×3 thrust distribution matrix, , , The thrust is mapped onto the four thrusters and superimposed with their respective anti-slip control components to ultimately generate the total thrust command for each vertical thruster. , , , The synthesis formula is as follows:

[0099] (9)

[0100] The design of this thrust distribution matrix ensures that the control force and torque can be correctly distributed to the thrusters located at the four corners of the robot, thereby achieving vertical stability, anti-collapse and anti-tipping control of the robot in a coordinated manner.

[0101] Figure 3 The system demonstrates a collaborative control framework for a composite vertical thruster for an underwater robot. The system uses a collaborative control algorithm for the vertical thruster as its core processing unit. It receives attitude and position data from the robot's inertial navigation system, as well as velocity information from the encoders on both sides of the tracks. The algorithm calculates control commands based on this real-time data and outputs them to the track lifting cylinder controller and the thruster controller, respectively. Finally, the track lifting cylinder executes the "pressing down" or "lifting up" action of the tracks to switch walking modes, while the vertical thruster outputs precise thrust, jointly enabling the robot to walk stably and control its attitude in complex seabed environments.

[0102] Figure 4The collaborative control process of the vertical thruster of the underwater robot is demonstrated: First, the actual speed of the left and right tracks is calculated according to equation (1), then the slip ratio of the left and right tracks is calculated according to equation (2), and then the average slip ratio of the tracks is obtained by equation (3). Then, the condition of whether the average slip ratio exceeds the threshold is used as the judgment condition. If the condition is met (yes), the anti-slip collaborative control component of the vertical thruster is calculated according to equation (4). Otherwise (no), the corresponding component is calculated according to equations (5-1 and 5-2). On this basis, the anti-rapid deviation control component is calculated according to equation (6), and the anti-lateral overturning control component is calculated according to equations (7) and (8). Finally, all control components are fused according to equation (9) and the collaborative control command of the vertical thruster is output.

[0103] Step S400: Calculate the dynamic interference of the operating tool on the underwater robot based on the operating parameters;

[0104] This step involves precise modeling and quantification of the dynamic effects generated during operation by the underwater robot's onboard tools (using a water jet as an example in this invention), providing accurate input for the subsequent active compensation control of the horizontal thruster. This calculation mainly comprises three parts: calculation of the water jet's reaction force, calculation of the position of its equivalent point of application in the robot's coordinate system, and calculation of the specific disturbance force and torque generated by this force on the robot.

[0105] Before proceeding with the calculations, the composition of the horizontal thruster will be explained. The horizontal thruster consists of four thrusters used to generate thrust. and rotational torque This is to actively compensate for the interference of water jet operations on the robot's movement control.

[0106] Force analysis of water jet and horizontal propulsion unit as follows Figure 5 As shown. Where A represents the pivot point of the water jet on the robot; B represents the equivalent point of application of the high-pressure jet force of the water jet; and This indicates the mounting position of the water jet on the robot frame, expressed by its relative position to the robot's center of gravity; This indicates the lowering angle of the water jet. During operation, this lowering angle is adjusted by adjusting the length of the hydraulic cylinder connecting the water jet and the robot frame. This represents the angle between the high-pressure jet force of the water jet and the water jet itself. It is determined by the nozzle layout and installation angle on the water jet and remains unchanged during operation; it is a fixed value. This represents the distance between points A and B, and is a fixed value. and This indicates that point B lies on the coordinate axis. and Projection on; Represents the force of a horizontal thruster The angle with the horizontal plane; This represents the distance between the center of rotation of the thruster and the center of rotation of the horizontal thruster, and assumes that the distances between the four corresponding horizontal thrusters and their centers of rotation are the same.

[0107] Based on the water jet operation parameters obtained in step S100, the jet reaction force generated by the high-pressure water jet is calculated using empirical formulas in fluid dynamics. The calculation formula is as follows:

[0108] (10)

[0109] in, The flow coefficient of the water jet nozzle is a dimensionless parameter that varies between 0.9 and 0.95 depending on the nozzle structure. This indicates the outlet area of ​​the water jet nozzle; This represents the pressure at the inlet of the water jet nozzle. This formula quantifies the reaction force generated by the change in momentum during high-pressure water jet injection, which acts on the water jet body and is transmitted to the robot frame.

[0110] Secondly, it is necessary to determine the reaction force of this jet. The position of the equivalent point of action B in the underwater robot's body coordinate system. The calculation of this position depends on the water jet's geometric parameters and the real-time lowering angle:

[0111] (11)

[0112] in, and These are the equivalent action point B in the robot coordinate system. shaft and Coordinates on the axis; and These are the coordinates of the fixed installation position of the water jet rotation pivot point A in the robot coordinate system; It is a fixed distance between the pivot point A and the equivalent point of action B; It is the current lowering angle of the water jet, which is a real-time changing operating parameter.

[0113] Finally, the jet reaction force The dynamic disturbance is decomposed into the robot's coordinate system, and the torque it generates about the robot's center of gravity is calculated to fully describe the dynamic disturbance. The force decomposition formula (i.e., jet reaction force) is used. The projection in the robot coordinate system is:

[0114] (12)

[0115] in, and These are the jet reaction forces In robot coordinate system shaft and Components on the axis; This is the inherent angle between the jet reaction force and the waterjet arm, determined by the waterjet head structure and is a fixed value. These two components directly constitute the interference forces on the robot's movement (X direction) and vertical (Z direction). The interference torque generated by these interference forces... It can be calculated using the following formula:

[0116]

[0117] The disturbance torque This demonstrates the impact of dynamic disturbances on the robot's attitude (especially pitch). At this point, the dynamic disturbances generated by the working tool on the robot have been fully quantified as disturbance forces. , and disturbance torque .

[0118] Step S500: Based on the dynamic disturbance, calculate the compensation thrust and compensation torque required by the horizontal thruster system, and control the rotation angle of the horizontal thruster to align with the thrust direction;

[0119] Based on the calculated dynamic disturbance, the total combined force required by the horizontal thruster system to counteract the disturbance is precisely calculated, and the thrust direction of the thrusters is adjusted to precisely align with the direction of the disturbance force. This step involves two actions: thrust direction alignment control and compensation calculation.

[0120] The thrust direction of the horizontal thrusters needs to be aligned. To most effectively counteract the interference generated by the water jet reaction force in the horizontal plane, the thrust direction of the horizontal thruster assembly needs to be controlled to be collinear with but opposite to the direction of the interfering force. This is achieved by adjusting the overall rotation angle of the horizontal thrusters. This is achieved by... The formula for calculating the target rotation angle is as follows:

[0121] (13)

[0122] in, It is the angle between the thrust direction of the horizontal thruster and a certain reference direction (such as the robot's longitudinal axis); It is the inherent angle between the jet reaction force and the water jet arm; It is the real-time lowering angle of the water jet.

[0123] This formula ensures that the thruster's thrust direction can track the changing direction of the disturbance force due to variations in the water jet's lowering angle in real time. Simultaneously, the system incorporates a protective logic: if the calculated value is... If the angle is less than 0, then force the command. This operation is intended to keep the horizontal thruster support in a vertical or preset safe initial position relative to the robot frame, avoiding meaningless or potentially unstable negative angles.

[0124] This invention calculates the total compensated thrust and compensated torque required by the horizontal thruster system. It employs a direct and efficient open-loop feedforward compensation strategy. The total compensated thrust required by the system is... The magnitude is set to be relative to the jet reaction force. They are equal in size but opposite in direction, that is:

[0125] (14-1)

[0126] At the same time, in order to balance the disturbance torque generated by the jet reaction force on the robot's center of gravity The horizontal thruster system needs to generate a compensating torque that is equal in magnitude and opposite in direction. The calculation formula is as follows:

[0127] (14-2)

[0128] in, and These are the components of the jet reaction force along the X and Z axes of the robot coordinate system; and These are the coordinates of the equivalent point of action of the jet, B, on the Z and X axes of the robot coordinate system. (Calculated...) and This refers to the total compensation action that the horizontal thruster system needs to output in coordination.

[0129] Step S600: Distribute the compensation thrust and compensation torque to each horizontal thruster, and output the corresponding thrust command for the horizontal thruster.

[0130] The calculated total system compensation thrust and compensation torque By using a preset, fixed thrust distribution matrix, the thrust is decomposed into specific thrust commands for four independent horizontal thrusters, thereby materializing the early compensation strategy and effectively offsetting the interference of water jet operation.

[0131] In specific implementation, two inputs are received from step S500: the total compensation thrust required from the horizontal thruster system. Total compensation torque To allocate these thrusters to the four horizontal thrusters positioned at different locations on the robot (e.g., top left, top right, bottom left, bottom right), this invention employs a linear thrust allocation matrix. This allocation process is accomplished through the following matrix operations:

[0132] (15)

[0133] in: , , , These are the final thrust commands output to the four horizontal thrusters: the left side of the top row, the right side of the top row, the left side of the bottom row, and the right side of the bottom row. It is the total compensation thrust required by the system calculated in step S500, which is used to counteract the translational interference caused by the reaction force of the water jet on the underwater robot. It is the total compensation torque required by the system calculated in step S500, which is used to balance the rotational interference caused by the water jet operation on the underwater robot.

[0134] The thrust allocation matrix is ​​a 4x2 constant matrix, structured based on the symmetrical arrangement of four horizontal thrusters on the robot and their lever arm relationships. In the matrix, each row corresponds to a specific thruster, and all elements in the first column (each with a value of 1) represent the contribution of each thruster to generating the aforementioned total compensated thrust. The contribution coefficients are the same; the elements in the second column (with values ​​of +1 or -1) represent the contribution of each thruster to the total compensation torque generated. The direction of contribution, and its sign, are determined by the direction of the lever arm of the thruster relative to the center of the robot. This symmetrical sign configuration allows thrusters in symmetrical positions to efficiently synthesize a pure compensating torque by outputting thrust in opposite directions.

[0135] Through this matrix operation, the overall thrust and torque requirements of the system are efficiently and decoupledly mapped to the independent commands of each thruster. Ultimately, these four thrust commands... , , , The output is sent to the corresponding horizontal thruster controller, which drives the thruster to generate precise thrust, thereby actively counteracting the dynamic interference caused by water jet operations and ensuring that the underwater robot maintains a stable trajectory and attitude during complex operations.

[0136] Figure 6 The collaborative control framework of the underwater robot's horizontal thruster was demonstrated: the system takes the horizontal thruster collaborative control algorithm as its core, and receives the water jet rotation angle α and nozzle pressure P as input parameters; after the algorithm calculates the control commands based on these parameters, it outputs them to the horizontal thruster rotary cylinder controller and the thruster controller respectively; finally, the thruster thrust direction is adjusted by the horizontal thruster rotary cylinder, and the horizontal thruster outputs precise thrust to achieve active compensation control of the reaction force of the water jet operation.

[0137] Figure 7The collaborative control process of the underwater robot's horizontal thrusters is demonstrated: First, the reaction force of the water jet is estimated according to equation (10), and the position of the equivalent action point of the water jet is calculated through equation (11); then, the angle command of the horizontal thrusters is output according to equation (13) to adjust the thrust direction; then, the required horizontal thruster compensation thrust and torque are calculated based on equations (12) and (14-1, 14-2); finally, the collaborative control command of each horizontal thruster is output through the thrust distribution strategy of equation (15) to complete the active compensation for operational interference.

[0138] Compared with existing technologies, this invention effectively solves the above problems through an integrated adaptive cooperative control mechanism. Specifically, firstly, by integrating inertial navigation data and track encoder speed, the track slip ratio is calculated in real time and the seabed conditions are accurately quantified accordingly. When the average slip ratio exceeds the first threshold, the lifting cylinder is automatically controlled to lift the track and switch to slipper mode to cope with the soft seabed. When the slip ratio in track mode is lower than the threshold but there is still a risk of slipping, the anti-slip control component of the vertical thruster is dynamically calculated and output based on the difference between the slip ratio of the left and right tracks and the second threshold to adjust the track pressure on the ground.

[0139] Secondly, the robot's real-time motion state is deeply integrated into the vertical thruster control. By introducing vertical velocity and acceleration to calculate the anti-collapse control component, and combining roll and pitch angles and their angular velocities to calculate the anti-overturning control component, the anti-slip, anti-collapse, and anti-overturning control components are finally synthesized into independent thrust commands for the four vertical thrusters through a specific thrust distribution matrix. This actively and comprehensively stabilizes the robot's attitude on complex slopes and substrates.

[0140] Finally, to address interference from operational tools (such as water jets), this invention innovatively employs an open-loop feedforward compensation strategy. Based on nozzle parameters and lowering angle, the magnitude, direction, and point of application of the jet reaction force are estimated in real time. This allows for the calculation of the compensation thrust and torque required from the horizontal thruster system. By controlling the rotation angle of the horizontal thrusters, the thrust direction is aligned with the direction of the interference force. Then, another thrust distribution matrix precisely distributes the total compensation requirement to each horizontal thruster. This proactively cancels out the interference force as it is generated, significantly reducing the coupling between operation and movement. Ultimately, this achieves high stability, high maneuverability, and high operational efficiency for the underwater robot in uncertain and complex seabed environments.

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

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

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

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

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

[0146] 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 controlling the movement of an underwater robot, characterized in that, Includes the following steps: The system detects the track slippage state of the underwater robot and obtains the operating parameters of the operating tools carried by the underwater robot. Based on the track slippage state, the vertical thruster system of the underwater robot is controlled to adjust the track pressure on the ground and stabilize the underwater robot's attitude. During the control of the vertical thruster system, the thrust commands of each vertical thruster are calculated and output based on the real-time attitude and motion state of the underwater robot. Based on the operating parameters, calculate the dynamic interference generated by the operating tool on the underwater robot; Based on the dynamic disturbance, the required compensating thrust and compensating torque of the horizontal thruster system are calculated, and the rotation angle of the horizontal thruster is controlled to align with the thrust direction. The compensated thrust and compensated torque are distributed to each horizontal thruster, and the corresponding thrust command for the horizontal thruster is output.

2. The underwater robot movement control method as described in claim 1, characterized in that, The method for detecting the track slippage state of the underwater robot includes: Acquire the inertial navigation data and speed data of the left and right track encoders of the underwater robot; Based on the forward velocity and yaw rate in the inertial navigation data, and the installation position of the left and right tracks relative to the robot center, the actual travel speed of the left and right tracks is calculated. Based on the speed data of the left and right track encoders and the calculated actual travel speed of the left and right tracks, the slip ratio of the left and right tracks is calculated respectively. Based on the slip ratios of the left and right tracks, the average slip ratio of the tracks is calculated, and this average slip ratio is used as a quantitative indicator of the track slip state.

3. The underwater robot movement control method as described in claim 1, characterized in that, The method for controlling the vertical thruster system of the underwater robot according to the track slippage state to adjust the track-to-ground pressure and stabilize the underwater robot's attitude includes: Calculate the average slip ratio of the tracks of the underwater robot; The average slip ratio is compared with a first threshold. When the average slip ratio is greater than or equal to the first threshold, the track lifting cylinder of the underwater robot is controlled to lift the track to switch to the slipper walking mode, and the anti-slip control component of the vertical thruster is set to zero. When the average slip ratio is less than the first threshold, the track walking mode is maintained, and the anti-slip control component of the vertical thruster is calculated by comparing the slip ratio of the left and right tracks with the second threshold.

4. The underwater robot movement control method as described in claim 3, characterized in that, The method for calculating the anti-slip control component of the vertical thruster includes: For the left vertical thruster, if the left track slip ratio is less than the second threshold, the left anti-slip control component is zero; otherwise, the left anti-slip control component is calculated based on the difference between the left track slip ratio and the second threshold. For the right vertical thruster, if the right track slip ratio is less than the second threshold, the right anti-slip control component is zero; otherwise, the right anti-slip control component is calculated based on the difference between the right track slip ratio and the second threshold.

5. The underwater robot movement control method as described in claim 4, characterized in that, The method for calculating and outputting thrust commands for each vertical thruster based on the real-time attitude and motion state of the underwater robot includes: The anti-collapse control components are calculated based on the vertical velocity and vertical acceleration of the underwater robot. The anti-tipping control components are calculated based on the roll angle and roll angular velocity of the underwater robot; The anti-longitudinal overturning control component is calculated based on the pitch angle and pitch angular velocity of the underwater robot; Combining the anti-slip control component, the anti-collapse control component, the anti-lateral overturning control component, and the anti-longitudinal overturning control component, the thrust command for each vertical thruster is calculated using the thrust distribution matrix; The thrust distribution matrix is ​​a 4×3 matrix, used to map the anti-collapse control component, the anti-lateral overturning control component, and the anti-longitudinal overturning control component to the thrust commands of the four vertical thrusters, and superimpose them with the anti-slip control component for output.

6. The underwater robot movement control method as described in claim 1, characterized in that, The operating tool is a water jet. The method for calculating the dynamic interference generated by the operating tool on the underwater robot based on the operating parameters includes: Based on the nozzle area, nozzle inlet pressure, and flow coefficient of the water jet, the reaction force of the water jet is calculated using empirical formulas. Based on the water jet's lowering angle, the connection point between the water jet and the underwater robot, and the inherent angle of the jet reaction force, the component of the jet reaction force in the underwater robot's coordinate system and the torque generated on the underwater robot's center of gravity are calculated.

7. The underwater robot movement control method as described in claim 6, characterized in that, Based on the aforementioned dynamic disturbance, a method for controlling the rotation angle of the horizontal thruster to align with the thrust direction includes: Based on the lowering angle of the water jet and the inherent angle of the jet reaction force, calculate the target rotation angle required for the horizontal propeller to compensate for the jet reaction force; When the calculated target rotation angle is less than zero, it is forcibly set to zero to keep the horizontal thruster support perpendicular to the underwater robot frame.

8. The underwater robot movement control method as described in claim 7, characterized in that, Based on the aforementioned dynamic disturbances, methods for calculating the compensating thrust and compensating torque required by the horizontal thruster system include: The magnitude of the compensating thrust is equal to the magnitude of the jet reaction force; The magnitude of the compensation torque is equal to the magnitude of the torque generated by the component of the jet reaction force in the underwater robot coordinate system about its center of gravity.

9. 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 any of the underwater robot travel control methods of claims 1-8, and the processor is configured to execute the programs stored in the memory.

10. A computer-readable storage medium storing a computer program thereon, characterized in that, When the computer program is run by the processor, it performs the steps of the underwater robot travel control method according to any one of claims 1-8.

Images