An underwater robot target grabbing control method for ocean current disturbance

Abstract

The application discloses a kind of ocean current disturbance-oriented underwater robot target grabbing control method, belong to underwater robot control technical field.Its steps include: establishing underwater robot-single degree of freedom mechanical arm system six degrees of freedom dynamics model and reconstructing mechanical arm reaction load;Through underwater perception, obtain target position and combine mechanical arm and gripper state, based on hysteresis comparator is divided into search, approach, pre-grabbing, grabbing and recycling five operation stages;Nonlinear disturbance observer is constructed, and mechanical arm reaction load is regarded as feedforward to realize internal and external disturbance decoupling;Finite time sliding mode controller is constructed, and robust switching gain is adjusted online by stage variable;The successful determination mechanism of grabbing is constructed, and the joint verification of spatial distance, gripper closure and positioning accuracy is constructed.The application realizes the accurate separation of mechanical arm motion disturbance and ocean current disturbance, improves the success rate of underwater robot target grabbing and hover stability.

Description

Technical Field

[0001] This invention belongs to the field of underwater robot control technology, specifically relating to an underwater robot target grasping control method accommodating ocean current disturbances. Background Technology

[0002] ROVs are widely used in marine resource development, underwater fishing, underwater maintenance, and marine scientific research. As the complexity of underwater operations continues to increase, the use of robotic arms for target grasping has become an important application of ROVs.

[0003] Due to hardware size and cost limitations, many small and medium-sized underwater robots typically use single-degree-of-freedom (DOF) robotic arms. Existing single-DOF robotic arm grasping systems usually rely on the stable hovering of the ROV itself to achieve target alignment. However, under the influence of ocean current disturbances, wave disturbances, and thruster uncertainties, the ROV is prone to positional shifts and attitude changes, leading to an increased grasping failure rate.

[0004] Existing dynamic positioning control methods, such as PID, adaptive control, and traditional sliding mode control, suffer from problems such as strong model dependence, limited disturbance rejection capability, slow convergence, or large steady-state error. Although existing disturbance observers can compensate for ocean current disturbances, they usually do not consider the additional forces and torques generated by the movement of the robotic arm. Furthermore, the lack of real-time information interaction and collaborative control mechanisms between the dynamic positioning system and the robotic arm control system leads to significant drift and insufficient positioning accuracy during the grasping phase.

[0005] Therefore, there is an urgent need for an adaptive finite-time dynamic positioning control method for underwater target grasping. By establishing a collaborative mechanism between dynamic positioning and robotic arm control, and combining a nonlinear disturbance observer with finite-time sliding mode control, the robot can achieve high-precision hovering and high success rate of robotic arm grasping in complex ocean current environments, thereby improving the system's operational efficiency and reliability. Summary of the Invention

[0006] To address the aforementioned problems in existing technologies, this invention proposes a target-grabbing control method for underwater robots accommodating ocean current disturbances. The method is rationally designed, overcomes the shortcomings of existing technologies, and achieves excellent results.

[0007] To achieve the above objectives, the present invention adopts the following technical solution: A target grasping control method for an underwater robot accommodating ocean current disturbances includes the following steps: Step 1: Establish a spatial six-degree-of-freedom dynamic model of the underwater robot and single-degree-of-freedom robotic arm system; Step 2: Obtain the spatial geometric state of the target object through the underwater sensing system, and combine it with the gripper closure state. Based on the hysteresis comparator mechanism, the entire operation process is divided into multiple heterogeneous operation mode stages. Step 3: Construct a decoupled nonlinear disturbance observer, and use the reaction dynamic load vector of the robotic arm as a feedforward to input the disturbance observer to estimate the time-varying environmental disturbance; Step 4: Based on the operational mode stage information output in Step 2 and the environmental disturbance estimate output in Step 3, construct a finite-time convergent multi-degree-of-freedom spatial tracking error sliding mode, and adjust the robust switching gain of the sliding mode controller online according to the current operational mode stage. Step 5: Construct a grasping success determination mechanism based on the joint verification of spatial distance constraints, gripper closure state and carrier positioning accuracy. When the grasping is determined to be successful, switch to the recovery and reset mode.

[0008] Furthermore, in step 1, the reaction load vector of the single-degree-of-freedom robotic arm on the carrier base... Reconstructed as: ; in, For the total state vector of the generalized system, Let be the generalized pose vector, where For three-dimensional Euclidean space coordinates These represent the roll angle, pitch angle, and yaw angle of the carrier, respectively. This refers to the rotation angle of the robotic arm joint; For the angular velocity of the robotic arm joints, For the joint angular acceleration of the robotic arm, Let be the generalized inertial coupling matrix of the single-degree-of-freedom robotic arm to the carrier base; This is the coupling matrix of the Coriolis force and centripetal force of the single-degree-of-freedom robotic arm on the carrier base. The carrier base servo speed, This is the feedforward term for fluid impact resistance. This is a displacement hydrostatic compensation item.

[0009] Furthermore, in step 2, the specific method for dividing the entire job process into multiple heterogeneous job modal stages based on the hysteresis comparator is as follows: Constructing the physical closed feedback variable of the gripper When the grippers are in the open or unlocked state, When the grippers are fully closed ; Configure a small decoupling hysteresis tolerance width at the boundary of each stage. The hysteresis tolerance width The value range is 0.02m to 0.05m; Define job modal phase variables These five operational modes correspond to the autonomous search mode, cooperative approach mode, end-effector pre-grab hovering mode, dynamic grasp contact mode, and load recovery and reset mode, respectively. Each mode corresponds to a different control strategy. The five operational modes are divided as follows: when and At this time, it is the autonomous search mode phase S1; when , and At that time, it is the cooperative approach mode phase S2; when , and At this time, it is the end-effector pre-grabbing hovering mode stage S3; when And the gripper state changed from Evolved into At this time, it is the dynamic capture contact mode stage S4; when Furthermore, when the L2 norm of the multi-degree-of-freedom transient positioning and tracking error of the carrier converges to within the steady-state holding threshold, it is the load recovery and reset mode stage S5.

[0010] Furthermore, in step 3, the nonlinear disturbance observer constructs auxiliary state variables. , These are the internal auxiliary state observation components of the nonlinear disturbance observer for the six spatial motion degrees of freedom of the underwater robot: longitudinal, lateral, vertical, roll, pitch and yaw. Environmental disturbance estimates The expression is: ; in, For the generalized system inertia matrix, The gain matrix is ​​a constant for a symmetric positive definite observer. Constructing a dual-source dynamic self-hedging nominal function term : ; in, The time-varying Coriolis force and centripetal force vectors, For linear fluid damping matrix, This is the hydrostatic restoring moment vector. This refers to the generalized pose vector of the carrier in the geographic inertial coordinate system. Will The auxiliary state variable is added to the observation equation. The differential evolution equation is: ; in, This is the physical output signal of the thruster saturation limiting module; Through the Perform numerical integration to solve auxiliary state variables in real time Then, the estimated value of environmental disturbance is solved. .

[0011] Furthermore, in step 4, the desired dynamic positioning hovering pose space point is set. , For the desired three-dimensional spatial coordinates, For the desired three-dimensional attitude angle, define the L2 norm of the multi-degree-of-freedom transient positioning and tracking error of the carrier. : ; in, This is the real-time pose vector of the carrier in the geographic coordinate system; Constructing a nonlinear finite-time integral sliding mode surface function vector : ; in, and It is a positive definite diagonal matrix. and It is a positive odd number and satisfies ; Using the inverse kinematic transformation matrix The sliding surface function in the geographic coordinate system is transformed into the generalized velocity tracking error sliding surface in the volume-fixed coordinate system. : ; in, Reference velocity vector for the virtual volume coordinate system: .

[0012] Furthermore, in step 4, the total output control law of the sliding mode controller... for: ; in, For equivalent feedforward control terms, For adaptive robust switching control terms; The expression is: ; in, For linear fluid damping matrix, This is the hydrostatic restoring moment vector; The expression is: ; in, To preset the boundary layer smoothing adjustment coefficient, This is the adaptive robust time-varying switching gain matrix.

[0013] Furthermore, in step 4, the adaptive robust time-varying switching gain matrix gain components of each axis Due to real-time sliding surface error Online adjustment of the first-order adaptive differential evolution equation driven by: ; in, It is the stiffness toughening factor. The state forgetting factor; the stiffness toughening factor With state forgetting factor The variables of the operation mode stage Online tuning; The stiffness toughening factor under each operating modal stage With state forgetting factor The tuning rules are as follows: Pre-configure lower limit constant for gain With upper limit of gain constant The stiffness toughening factor under each operating mode stage With state forgetting factor The tuning rules are as follows: when At that time, set and ,when Reduce to season To switch gain Monotonically decreasing and remaining at the lower gain limit constant ; when At that time, set and and satisfy the inequality ,when Increase to season To switch gain It exhibits a monotonically increasing trend and is bounded by the upper limit constant of the gain. ; when At that time, set and ,when Reduce to season To switch gain Monotonically decreasing and reconverging to the lower gain limit constant. ; when At that time, set and and satisfy the inequality ,when Increase to season To switch gain Monotonically increasing and bounded by the upper limit of the gain constant ; when At that time, set and ,when Reduce to season To switch gain It decreases monotonically again and converges to the lower bound constant of the gain. .

[0014] Furthermore, in step 5, a multivariate joint verification vector is constructed. : ; ; ; in, The spatial geometric distance constraint function, This is the spatial geometric tolerance threshold; For the gripper actuator's closing locking function, The steady-state constraint function for dynamic positioning of the carrier base. The L2 norm of the multi-degree-of-freedom transient positioning and tracking error of the carrier. To maintain the threshold in steady state; Define a time-sliding verification window Successful capture criteria The complex logic is refactored as follows: ; If and only if Upon successful capture, switch to the load recovery and reset mode; In the load recovery and reset mode, the joint angle of the robotic arm With a limited angular velocity Return angle to reference The trajectory change was recovered; simultaneously, the stiffness toughening factor from step 4 was applied. Reduce the state forgetting factor Increase the stiffness of the unloading controller.

[0015] The beneficial technical effects of this invention are as follows: 1. This invention proposes a full-process job partitioning mechanism based on the hysteresis comparator mechanism: It constructs a full-process job feature matrix vector by integrating the relative spatial Euclidean distance and the binary discrete feedback variables of the end gripper. It innovatively introduces the Schmitt trigger (hysteresis comparator) mechanism, configuring a small-amplitude decoupling hysteresis tolerance width at each distance partitioning boundary. The entire operation process is divided into five heterogeneous state stages: search, approach, pre-grab, grab, and retrieval.

[0016] 2. In the nonlinear disturbance observer (NDO), the Coriolis torque of the underwater robot body (Munk torque term) and the aforementioned reaction load term of the robotic arm are added. The combined self-hedging function; simultaneously, the control force input of the observer is cascaded to the physical output of the thruster saturation limiting module. .

[0017] 3. The robust switching gain of the controller does not use discrete hard parameter switching, but is configured to be determined by the real-time sliding surface error. The driving first-order adaptive differential evolution equation. Stage variables passed in through the outer state machine. Online tuning of stiffness toughening factor With state forgetting factor The system achieves online self-adjustment of control bandwidth and stiffness at different operating stages. When hovering at the origin, the gain actively and spontaneously decays, and combined with the hyperbolic tangent continuous boundary layer vibration damping mechanism, the origin chattering problem is eliminated. Attached Figure Description

[0018] Figure 1 This is a flowchart of the method of the present invention.

[0019] Figure 2 This is a graph showing the estimated output curve of the nonlinear disturbance observer in this invention.

[0020] Figure 3 This is a three-dimensional trajectory diagram of the underwater robot's dynamic positioning in this invention.

[0021] Figure 4 This is a position convergence curve diagram in this invention.

[0022] Figure 5 This is a graph showing the adaptive evolution of the time-varying switching control gain in this invention. Detailed Implementation

[0023] The specific embodiments of the present invention will be further described below with reference to specific examples: A target grasping control method for underwater robots accommodating ocean current disturbances, such as Figure 1 As shown, it includes the following steps: Step 1: Establish a spatial six-degree-of-freedom dynamic model of the underwater robot and single-degree-of-freedom robotic arm system (UVMS); This step aims to establish a complete spatial kinematic transformation matrix and derive the six-degree-of-freedom spatial dynamic state equations between the underwater robot and the robotic arm from both rigid body dynamics and fluid dynamics dimensions. This provides a precise mechanical and physical benchmark for subsequent decoupled disturbance observation and adaptive control. It includes the following sub-steps: Step 1.1: To fully describe the motion of UVMS in three-dimensional underwater conditions, two orthogonal coordinate systems are defined: Define a geographic coordinate system (North-East-Down coordinate system, Inertial coordinate system) (and the fixed coordinate system of the body) The origin of the fixed coordinate system is chosen at the geometric center (or center of gravity) of the carrier. Define the carrier in the inertial coordinate system The generalized pose vector is ,in For three-dimensional Euclidean space coordinates These represent the roll, pitch, and yaw angles of the carrier, respectively. The carrier is defined in the coordinate system. The generalized velocity vector below is ,in For linear velocity components, This represents the angular velocity component.

[0024] The kinematic transformation relationship of the carrier is as follows: ; In the formula, This is the kinematic transformation matrix from the volume-fixed coordinate system to the geographic inertial coordinate system. ; This is the Euler angle rotation matrix from the volume coordinate system to the inertial coordinate system; Let be the angular velocity transformation matrix. Its component expansion satisfies: ; ; Step 1.2: Construct the strongly coupled fluid dynamics equations in the full state space of UVMS: This invention treats the nonlinear joint motion of a single-degree-of-freedom robotic arm as an intrinsic time-varying dynamic load flow within the carrier dynamics system. Combining Fossen hydrodynamics theory and Lagrange tree-like multibody dynamics formulas, a strongly coupled state equation for the UVMS space is constructed: ; In the formula, For the total state vector of the generalized system, This refers to the real-time rotation angle of a single-degree-of-freedom robotic arm joint. The thruster control input force and torque vector acting on the carrier base, This represents the external marine environmental disturbance vector.

[0025] Generalized system inertia matrix This matrix is ​​symmetric positive definite and is derived from the robot's own rigid body mass inertial matrix. Additional mass inertial array generated with the surrounding fluid Linear superposition constitutes: Among them, the additional mass array The component matrix of the additional fluid inertial effect generated by the displacement of surrounding fluid when the carrier accelerates in water is as follows: ; Time-varying Coriolis and Centripetal Term This refers to the nonlinear rotational torque coupling (Munk torque effect) caused by interleaved motion in multi-degree-of-freedom space. It is determined by both the rigid body term and the added mass term. .in, Here is the matrix of Coriolis force and centripetal force for a rigid body. To represent the Coriolis force and centripetal force matrix of the fluid's added mass, The partial derivative form is used to ensure the tightness of the hydrodynamic cross-coupling: ; In the formula, A skew-symmetric matrix operator representing a three-dimensional vector, used to implement the spatial cross product operation; , ; Nonlinear fluid damping matrix Because water flow has viscosity, the damping force exhibits a high degree of nonlinearity. This scheme decomposes it into a decoupled superposition of a linear fluid damping term and a quadratic damping term of the velocity square term. ; ; ; in, , , , , , These are the secondary fluid damping coefficients of the underwater robot in six degrees of freedom in space (longitudinal translation, lateral translation, vertical translation, and roll, pitch, and yaw rotation); hydrostatic restoring moment vector Considering the stabilizing spring effect caused by the tilting of the underwater vehicle, the total gravity of the system is taken into account. With total buoyancy and the coordinates of the center of gravity coordinates of the buoyancy center The three-dimensional misalignment constructs a complete attitude angle recovery feedback flow: .

[0026] Step 1.3: Reconstruct the reaction load vector of the robotic arm on the base : When a single-degree-of-freedom robotic arm starts at high speed in water, performs a harvesting operation by swinging its arm, or encounters a grasping contact collision, it applies reverse dynamic load vectors to the six degrees of freedom of the carrier base. To prevent the high-dynamic transient instability caused by the use of static Jacobian matrices in traditional techniques, this invention will... The reconstructed nonlinear state mapping equation comprises four composite terms: robotic arm inertial coupling, Coriolis force coupling, fluid impact resistance, and gravity center drift. ; in, For the total state vector of the generalized system, , where is the generalized pose vector, For three-dimensional Euclidean space coordinates These represent the roll, pitch, and yaw angles of the carrier, respectively. For the angular velocity of the robotic arm joints, For the joint angular acceleration of the robotic arm, Let be the generalized inertial coupling matrix of the single-degree-of-freedom robotic arm with respect to the carrier base. This is the coupling matrix of the Coriolis force and centripetal force of the single-degree-of-freedom robotic arm on the carrier base. The carrier base servo speed; (Inertial cross-coupling term) is the joint angular acceleration of a single degree of freedom. The reverse pulse torque is instantaneously sensed in the six degrees of freedom of the carrier base through the generalized inertial matrix of the rigid fluid of the robotic arm. (Coriolis and centripetal coupling term) represents the rotational speed of the robotic arm joints. With the carrier base follow speed After cross-product, a time-varying nonlinear coupled torque flow is generated due to the centrifugal force effect.

[0027] This is the feedforward term of the fluid impact resistance, i.e., the water resistance experienced by the single-degree-of-freedom robotic arm during the impact effect, which is transmitted in reverse to the damping load of the carrier base. Its magnitude satisfies... .

[0028] This refers to the hydrostatic compensation term for displacement, i.e., the time required for the robotic arm to change joint angles. At this time, the overall center of gravity coordinate of the system undergoes physical drift, thereby breaking the original hydrostatic balance of the carrier base and generating a change in the compensatory unbalance restoring torque.

[0029] Step 2 involves acquiring the spatial geometry of the target object through an underwater sensing system and calculating the real-time relative motion between the carrier and the target object using a three-dimensional coordinate system nonlinear mapping operator. Combining the gripper closure state with a hysteresis comparator, the entire operation process is divided into multiple heterogeneous operational modal stages, including the following sub-steps: Step 2.1: Nonlinear mapping solution of the three-dimensional coordinate system for the spatial pose of the underwater target; The underwater robot is equipped with a binocular underwater high-definition camera or a high-resolution forward-looking sonar to acquire images within its field of view in real time, lock onto the target object to be grasped, and output the target object's position in the sensor's optical coordinate system (denoted as coordinate system). ) three-dimensional position vector .

[0030] In order for this location information to be directly used for the spatial dynamic positioning of the carrier, the target pose must be mapped to the geographic inertial coordinate system through two levels of nonlinear rigid body transformation: Sensor coordinate system to the fixed base coordinate system Mapping: The physical installation offset of the known sensing system on the carrier (i.e., the offset vector of the sensor center relative to the geometric center of the carrier). ) and installing rotation matrix The target object in the carrier coordinate system The spatial position below The formula is: Fixed coordinate system To geographic inertial coordinate system Mapping: Using the Euler angle matrix transformation operator of the current geographic pose of the carrier calculated in step 1 and the current absolute spatial coordinates of the carrier. The target object in the geographic inertial coordinate system Absolute three-dimensional spatial coordinates Strictly meet: ; Obtain the absolute coordinates of the target in the inertial coordinate system. Subsequently, the carrier control system calculates its own geometric center in real time. Spatial Euclidean distance between the target object and the target object : ; Step 2.2: Construct the full-space feature vector of heterogeneous states; In order for the outer state machine to make collaborative decisions on both the external spatial geometric relationship and the physical operating state of the end effector, the joint angles of the robotic arm are collected in real time. angular velocity and the physical closed feedback variable of the gripper . Defined as a binary discrete operator: when the grippers are in an open or unlocked state. When the built-in torque sensor or photoelectric encoder of the gripper detects that it is fully closed, By combining spatial distance and end-point status, a spatial feature matrix vector of the UVMS full-process operation status is constructed. : .

[0031] Step 2.3: Task-driven outer state machine partitioning based on hysteresis comparator mechanism; In actual underwater environments, the calculated spatial relative distance is affected by time-varying ocean wave interference and wave noise. High-frequency micro-pulsations will occur. To prevent the "edge oscillation" phenomenon of frequent hard switching of control strategies near the boundary between each stage, this invention introduces the Schmitt trigger (hysteresis comparator) mechanism and configures a micro-amplitude decoupling hysteresis tolerance width at the boundary of each stage. Hysteresis tolerance width The value range is 0.02m to 0.05m; Define job modal phase variables These correspond to the autonomous search mode, cooperative approach mode, end-effector pre-grab hovering mode, dynamic grasp contact mode, and load recovery and reset mode, respectively. The five operational modes are divided as follows: (1) The autonomous search mode phase is denoted as S1, and the state variables are... When relative distance And the gripper state At that time, the single-degree-of-freedom robotic arm controller drives the joint to rotate to the compact retracted reference position where hydrodynamic resistance is minimized. The carrier dynamic positioning controller then adaptively switches to a low-power energy-saving cruise mode.

[0032] (2) The cooperative approach mode phase is denoted as S2, and the state variables are... :exist In this state, when the relative distance continuously decreases and satisfies And satisfy gripper state At this point, the state machine seamlessly switches to this stage. The robotic arm remains retracted, and the carrier controller automatically widens the control bandwidth through an adaptive evolution law, driving the thrusters to output high thrust with a high-response maneuvering mode, rapidly reducing the three-dimensional spatial position deviation within a finite time.

[0033] (3) End-effector pre-grabbing hovering mode phase S3, state variables :exist Under the condition that the relative distance satisfies And satisfy gripper state At this point, the state machine seamlessly switches to this stage. The carrier controller automatically and smoothly attenuates the gain, and in conjunction with the hyperbolic tangent boundary layer, switches to a zero-jitter, high-precision hovering filter mode to lock the position and provide a stable working base; at the same time, the single-degree-of-freedom robotic arm motor starts, unfolds the joints, and adaptively aligns the gripper's central axis with the target axis.

[0034] (4) The dynamic capture contact mode phase is denoted as S4, and the state variables are... :exist In this state, when the relative distance shrinks to the physical limit boundary And the gripper state changes from During the dynamic closure evolution, the robotic arm drive module issues a closing command, and the single-degree-of-freedom arm rapidly swings to strike the water. At this time, the carrier controller spontaneously activates the ultra-high impact stiffness mode, the robust switching gain surges, and the response, in conjunction with the feedforward flow counteracting the disturbance observer, controls the transient spatial deviation caused by the robotic arm striking the water and the collision within the centimeter level.

[0035] (5) The load recovery and reset modal phase is denoted as S5, and the state variables are... When the joint decision matrix confirms that the grippers are fully closed and fixed ( When the L2 norm of the multi-degree-of-freedom transient positioning and tracking error of the carrier converges back to within the steady-state holding threshold, the single-degree-of-freedom robotic arm controls the motor to reverse direction, smoothly retracting the grabbed target with load into the hull's hull compartment. The carrier control system then unloads the high-stiffness robust term and switches to a low-power, smooth control mode.

[0036] Step 3: Construct a decoupled nonlinear disturbance observer (NDO) and input the reaction dynamic load vector of the robotic arm as a feedforward quantity into the disturbance observer to estimate the time-varying environmental disturbance; This step aims to avoid introducing high-noise carrier acceleration signals. Under the premise of realizing external non-passive time-varying environmental interference Precise decoupling from the complex nonlinear physical coupling forces within the system. This includes the following sub-steps: Step 3.1: Multi-channel acquisition and interaction of the robotic arm's full-state operation feature flow; The main control unit synchronously collects the joint angles of the single-degree-of-freedom robotic arm in real time. Joint angular velocity And internally calculate the angular acceleration of the robotic arm joints. .

[0037] Gripper operating status: Real-time reading of the gripper motor current feedback or fingertip thin-film pressure sensor output to obtain the discrete closure characteristic variables of the gripper. Using the reconstructed time-varying nonlinear state mapping equation of the robotic arm in step 1, the reaction dynamic load vector of the single-degree-of-freedom robotic arm maneuver applied to the underwater robot at the current moment is calculated. This vector is fed into the input of the lumped disturbance observer module via a feedforward channel.

[0038] Step 3.2: Mathematical reconstruction of acceleration-free differential state observation equations and auxiliary variables; Under complex marine conditions, the linear and angular velocity signals ν of the water carrier are mixed with a large amount of fluid noise and high-frequency sensor noise. This invention introduces a multidimensional logarithmic nonlinear perturbation observer to construct auxiliary state variables. , These are the internal auxiliary state observation components of the nonlinear perturbation observer for the six spatial motion degrees of freedom of the underwater robot: longitudinal, lateral, vertical, roll, pitch, and yaw, respectively, eliminating interference with... Item dependencies.

[0039] Non-passive environmental disturbance estimates The expression is: ; in, The inertia matrix of the generalized system constructed in step 1, For a symmetric positive definite constant observer, the gain matrix is ​​adjusted, and the values ​​of each diagonal component determine the convergence bandwidth and response rate of the corresponding degree of freedom disturbance observation. Taking the time derivative of the above perturbation estimation expression, its differential evolution equation is: ; Based on the nominally slowly varying constraint assumption of time-varying marine environmental disturbance The UVMS spatially strongly coupled global state equation in step 1 is transformed as follows: ; Substituting into the time differential evolution equation of the perturbation estimation, and using Replace real disturbance Ultimately, an auxiliary variable completely devoid of acceleration differential terms was derived and constructed. Time differential evolution state solution equation: ; This formula only requires... The auxiliary state can be solved in real time by performing numerical integration. This allows for the calculation of accurate estimates of marine environmental disturbances. .

[0040] Step 3.3: Explicit injection of multi-source dynamic states and decoupling mechanism of internal and external disturbances; As a key safeguard for eliminating disturbance estimation in this invention, a dual-source dynamic self-hedging nominal function term is constructed within the disturbance observer. : ; in, These are the time-varying Coriolis force and centripetal force vectors (Munk torque vectors). For linear fluid damping matrix, This is the hydrostatic restoring moment vector. This refers to the generalized pose vector of the carrier in the geographic inertial coordinate system. Will The auxiliary state variable is added to the observation equation. The differential evolution equation is reconstructed as follows: ; in, This is the physical output signal of the thruster saturation limiting module; Through the Perform numerical integration to solve auxiliary state variables in real time Then, the estimated value of environmental disturbance is solved. .

[0041] Traditional observers are missing and When the robotic arm swings to grasp and trigger the carrier's movement, it incorrectly identifies the system's own fluid cross-coupling torque and the arm's impact force as "external waves," causing transient singular distortions in the disturbance estimation curve. This solution addresses this by actively injecting and subtracting these forces in the feedforward. and This allows the observer to predict internal dynamic changes. During residual calculation, it automatically filters out the carrier's self-excited coupling force and the impact of the boom's movement, thus achieving a balance between strong internal coupling forces and external pure wave interference. Decoupling.

[0042] Step 3.4: Backward feedback closed-loop correction of thruster physical saturation limiting; During the transient phase of UVMS operation involving large-scale approach or sudden wave impacts, the theoretical control torque calculated by the sliding mode controller can easily exceed the physical limits of the thruster hardware, leading to thrust oversaturation. To prevent thrust saturation from causing secondary damage to observation accuracy, the following backfeedback cascade rule is strictly configured: the control force input signal in the above observation equation... It must be connected to the physical output of the carrier thruster saturation limiting module (SaturationBlock) via a dedicated signal path through cross-layer cascading, and is strictly prohibited from being connected to the controller's original computational output. Its mathematical constraints are as follows: ; In the formula, The original calculated control force output by the controller. This corresponds to the upper limit of the maximum rated forward and reverse water-pumping thrust along the thruster's axial direction. This cascaded feedback rule avoids miscalculating the force that is physically limited as external environmental interference and eliminates the risk of lumped disturbance divergence caused by saturation runaway.

[0043] Step 4: Based on the operational mode stage information output in Step 2 and the environmental disturbance estimate output in Step 3, construct a finite-time convergent multi-degree-of-freedom spatial tracking error sliding mode, and adjust the robust switching gain of the sliding mode controller online according to the current operational mode stage; including the following sub-steps: Step 4.1: Construct the multi-degree-of-freedom spatial tracking error and finite-time sliding mode surface; Set the desired dynamic positioning hover pose space point , For the desired three-dimensional spatial coordinates, Define the real-time pose tracking error vector of the vehicle in the geographic coordinate system as the desired three-dimensional attitude angle. : ; in, This is the real-time pose vector of the carrier in the geographic coordinate system; To impart fast finite-time convergence performance to the system near the equilibrium origin, a nonlinear finite-time integral sliding mode surface function vector is constructed. : ; in, and Let be the static recovery stiffness matrix of the control system in each degree of freedom in space, and be a positive definite diagonal matrix. and It is a positive odd number and satisfies the core bounded interval constraint condition. This ensures that the control system maintains a high convergence rate of the square root order even during the small error approach phase.

[0044] Using the inverse kinematic transformation matrix The sliding surface function in the geographic coordinate system is transformed into the generalized velocity tracking error sliding surface in the volume-fixed coordinate system. : ; in, Reference velocity vector for the virtual volume coordinate system: .

[0045] Step 4.2: Design the control law for the continuous hyperbolic tangent boundary layer; To eliminate the switching chattering at the equilibrium origin in conventional sliding mode control, a hyperbolic tangent function of Lipschitz Continuous is introduced. A flexible and smooth control boundary layer is established. The total output control law of the sliding mode controller... for: ; in, This is the equivalent feedforward control term (equivalent feedforward force based on the known nominal model of the system), responsible for offsetting the nominal torque based on the known dynamic model of the system. For adaptive robust switching control terms; This represents the environmental disturbance feedforward value output by the disturbance observer; The expression is: ; in, For linear fluid damping matrix, This is the hydrostatic restoring moment vector; The expression is: ; in, To preset the boundary layer smoothing adjustment coefficient, This is the adaptive robust time-varying switching gain matrix.

[0046] Step 4.3: Task state-driven gain adaptive tuning mechanism; As the core task of this scheme, the adaptive protection point and the adaptive robust time-varying switching gain matrix are... gain components of each axis Due to real-time sliding surface error Online adjustment of the first-order adaptive differential evolution equation driven by: ; in, It is the stiffness toughening factor. State forgetting factor; stiffness toughening factor With state forgetting factor Variables from the work mode stage Online tuning enables intelligent adjustment of control bandwidth and stiffness; Pre-configure lower limit constant for gain With upper limit of gain constant The stiffness toughening factor under each operating mode stage With state forgetting factor The tuning rules are as follows: Core Mode 1: Autonomous Search Phase ( ): System Settings and Forgotten terms in differential equations Dominance forces time-varying control gain It smoothly converges to the preset low-level maintenance without large-range errors, significantly reducing the ineffective power consumption of the thruster. Reduce to season To switch gain Monotonically decreasing and remaining at the lower gain limit constant .

[0047] Core Mode 2: Cooperative Approach Phase ( ): Because the system has just entered the approximation process, the relative position error is large, causing the absolute value of the sliding surface in the body coordinate system to be affected. It is at a high level. At this time, the toughening component... Greater than the forgotten item, drive the switching gain. To rise rapidly within a limited time, therefore, the following setting is made: and and satisfy the inequality ,when Increase to season To switch gain It exhibits a monotonically increasing trend and is bounded by the upper limit constant of the gain. The transient recovery stiffness and control bandwidth of the control system are actively maximized, driving the thruster to output the maximum rated thrust and guiding the carrier to reduce relative spatial deviation with an extremely high transient response rate, thus solving the problem of response hysteresis in traditional fixed parameter control.

[0048] Core Mode 3: Endpoint Pre-grabbing Stage ( ): set up and ,when Reduce to season To switch gain Monotonically decreasing and reconverging to the lower gain limit constant. As the carrier guides the robotic arm to smoothly enter the harvesting space of the target organism, the tracking error converges, and the sliding surface state value undergoes a reverse evolution. At this point, the toughening term is reset to zero, and the forgetting term... Guided control gain Attenuation. Combined with the hyperbolic tangent boundary layer, the thruster's output torque exhibits complete Lipschitz continuity near the origin equilibrium state, and the system transitions to a chatter-free, high-precision filtered hovering mode.

[0049] Core Mode 4: Dynamically capturing contact transients ( ): set up and and satisfy the inequality ,when Increase to season To switch gain Monotonically increasing and bounded by the upper limit of the gain constant When a single-degree-of-freedom robotic arm performs a grasping action, the resulting reaction pulse load... This is offset by the feedforward of the NDO module in step 3. However, the residual unknown high-frequency transient fluid impact at the moment of contact and collision will still cause the carrier to deviate at the microsecond level, affecting the sliding surface. A mutation occurred. Toughening term. Response, drive rise.

[0050] Core Mode 5: Load Recovery and Reset Mode Phase ( ): set up and ,when Reduce to season To switch gain It decreases monotonically again and converges to the lower bound constant of the gain. .

[0051] Step 5: Construct a successful grasping determination mechanism based on the joint verification of spatial distance constraints, gripper closure state, and carrier positioning accuracy. When the grasping is determined to be successful, switch to the recovery and reset mode.

[0052] This step aims to construct a multi-source heterogeneous state joint verification mechanism to guide the robotic arm and carrier to smoothly reset in a coordinated manner; it includes the following sub-steps: Step 5.1: Construct the spatiotemporal multivariate joint convergence determination matrix; To eliminate anomalies in data from a single sensor, three independent scalar convergence evaluation functions are defined, and a multivariate joint verification vector is constructed. : ; ; ; in, The spatial geometric distance constraint function, The Euclidean distance in three-dimensional space calculated in step 2. The spatial geometric tolerance threshold is preferably set to [value]. ; For the gripper actuator's closing locking function, The discrete gripper feedback variable acquired in step 3. The discrete operator is triggered to 1 if and only if the reading of the photoelectric encoder embedded in the gripper reaches the preset closing limit angle and the feedback current of the joint drive motor continuously exceeds the set threshold. The steady-state constraint function for dynamic positioning of the carrier base. Let L be the norm of the real-time pose tracking error vector in the geographic coordinate system. To maintain a steady-state threshold, the preferred value is... ; Step 5.2: Introduce transient noise filtering judgment rules for time sliding windows; To overcome transient measurement noise caused by sudden underwater surges, this invention introduces a time-sliding window decision operator. A continuous time-sliding verification window is defined. (Preferred) Successful capture criteria The complex logic is refactored as follows: ; Only when the three heterogeneous indicators of spatial distance constraint, gripper closure state, and base dynamic positioning accuracy are within the time sliding window... The judgment criterion is that the condition is met simultaneously in every microsecond (i.e., the product of products is strictly kept to be 1). Only then will it jump from 0 to 1, switching to the load recovery and reset mode; Step 5.3: Load recovery and reset modal stage ( Coordinated control and recovery planning; The main control chip captured The task-driven outer state machine responds instantly, switching the current operation state variables into the payload reset phase. The following collaborative control strategy will be implemented: Under load recovery and reset mode, the robot arm's rate-limited low-resistance retraction plan: the robot arm drive module activates the rate limiter, and the robot arm joint angle... With a limited angular velocity Slowly return to the reference angle The trajectory retraction will revert to its original state, and the reaction load term of the reconstructed robotic arm base from step 1 will be returned. The impact of high-order angular acceleration inertial coupling is reduced.

[0053] Smooth unloading of the base control loop stiffness: As the robotic arm returns to its original position, the system's sliding surface error... The system reconverges and returns to zero. The co-controller automatically switches the gain adaptive update law parameters from step 4, adjusting the stiffness toughening factor. Decrease the state forgetting factor. Increase it. This makes the controller's robust switching gain more robust. The system transitions from the dynamic grasping contact mode (S4) to the low-power cruise control mode, completing the state loop.

[0054] like Figure 2 The figure shows the real-time tracking evolution curves of a nonlinear disturbance observer (NDO) under the influence of mixed wave disturbances with multidimensional random amplitudes and sudden internal mechanical load impacts. Mixed wave disturbances with random force amplitudes of 10-15 N are applied in the x, y, and z directions, while moment disturbances with random torque amplitudes are applied around the z-axis. Internal disturbances within the robotic arm are introduced at the 10-second mark. The red line represents the actual disturbance, and the blue line represents the estimated disturbance. Within a 25-second period, the estimated values ​​(blue solid line) and the actual values ​​(red dashed line) almost perfectly coincide across the four channels. Despite the internal strong coupling disturbance superimposed at the 10-second mark, no non-physical abrupt changes, distortions, or impulse overshoot peaks were observed in the total disturbance estimation curves (blue solid line) across the longitudinal force, lateral force, vertical force, and yaw moment channels.

[0055] like Figure 3 As shown, the initial starting position of the underwater ROV is [0,0,0]. The bold blue solid line in the figure represents the actual trajectory of the underwater robot driven by the closed-loop controller. After starting from the starting point, it smoothly and continuously approaches the working area diagonally upwards. In the middle section of the trajectory, the trajectory exhibits a slight nonlinear arc transition consistent with the effects of real fluid damping and time-varying environmental disturbances, without any non-physical control overshoot, severe oscillations, or overshoot phenomena. Finally, the actual trajectory accurately converges and stabilizes at the red star mark in the upper right corner of the figure, i.e., the target hovering point [2,-4,3]. The entire approach and positioning response process demonstrates that even under multi-dimensional hydrodynamic coupling, the control system still possesses excellent transient trajectory tracking performance and high-precision steady-state position maintenance capability.

[0056] like Figure 4 As shown, the underwater robot's closed-loop control dynamic response process is illustrated in the longitudinal (X-axis position), lateral (Y-axis position), and vertical (Z-axis depth) dimensions. The combined trends of the three channels throughout the entire timeframe demonstrate that the control strategy employed in this invention enables the underwater robot to exhibit extremely short settling times and zero overshoot transient tracking quality in all three degrees of freedom. Even in complex, strongly coupled hydrodynamic environments, the curves of each channel remain absolutely flat and stable after reaching the target value, without significant disturbance from ocean waves.

[0057] Depend on Figure 5 As shown in the gain adaptive evolution curve, the switching gain of the control system dynamically and adaptively adjusts throughout the five task-driven stages (S1 to S5) in the entire task time domain. The gain of traditional control schemes remains fixed at a constant level of approximately 49 throughout. This static configuration cannot perceive the switching of external task stages, lacks sufficient anti-disturbance capability in the grasping stage requiring high stiffness, and easily leads to continuous system control chattering and energy waste in the stage requiring smooth hovering. The time-varying switching control gain of this invention exhibits highly dynamic, stage-wise smooth adaptive evolution characteristics. Specifically, in stage S1, the gain smoothly decays from an initial 40 to a low level of around 10. This ensures that the system has good zero-chattering and energy-saving operation characteristics in the far-end search stage. In stage S2, as the system enters the mid-to-short-range approximation phase, the gain rapidly rebounds from 10 and smoothly converges to a high level of approximately 89, thus endowing the system with extremely high transient spatial positioning stiffness, ensuring rapid and accurate target approximation. After the S3 phase approximation is completed, the gain smoothly drops to a low level of around 10 at the pre-grab point to maintain low energy consumption and an extremely stable spatial hovering state. In the S4 phase, at the moment the end effector gripper contacts the target, facing the severe nonlinear internal impact of internal collisions and robotic arm movements, the gain adaptively rebounds and surges to a high level of approximately 76, significantly improving the system's shock resistance and self-toughening capability, and suppressing spatial transient positioning overshoot. After successfully locking onto the target and transitioning to the recovery mode in the S5 phase, the gain smoothly decays back to a low level, allowing the system to return to a stable and energy-efficient state.

[0058] Of course, the above description is not intended to limit the present invention, and the present invention is not limited to the examples given above. Any changes, modifications, additions or substitutions made by those skilled in the art within the scope of the present invention should also fall within the protection scope of the present invention.

Claims

1. A target grasping control method for an underwater robot accommodating ocean current disturbances, characterized in that, Includes the following steps: Step 1: Establish a spatial six-degree-of-freedom dynamic model of the underwater robot and single-degree-of-freedom robotic arm system; Step 2: Obtain the spatial geometric state of the target object through the underwater sensing system, and combine it with the gripper closure state. Based on the hysteresis comparator mechanism, the entire operation process is divided into multiple heterogeneous operation mode stages. Step 3: Construct a decoupled nonlinear disturbance observer, and use the reaction dynamic load vector of the robotic arm as a feedforward to input the disturbance observer to estimate the time-varying environmental disturbance; Step 4: Based on the operational mode stage information output in Step 2 and the environmental disturbance estimate output in Step 3, construct a finite-time convergent multi-degree-of-freedom spatial tracking error sliding mode, and adjust the robust switching gain of the sliding mode controller online according to the current operational mode stage. Step 5: Construct a grasping success determination mechanism based on the joint verification of spatial distance constraints, gripper closure state and carrier positioning accuracy. When the grasping is determined to be successful, switch to the recovery and reset mode. In step 2, the specific method for dividing the entire job process into multiple heterogeneous job modal stages based on the hysteresis comparator is as follows: Constructing the physical closed feedback variable of the gripper When the grippers are in the open or unlocked state, When the grippers are fully closed ; Configure a small decoupling hysteresis tolerance width at the boundary of each stage. Hysteresis tolerance width The value range is 0.02m to 0.05m; Define job modal phase variables These five operational modes correspond to the autonomous search mode, cooperative approach mode, end-effector pre-grab hovering mode, dynamic grasp contact mode, and load recovery and reset mode, respectively. Each mode corresponds to a different control strategy. The five operational modes are divided as follows: When relative distance and At this time, it is the autonomous search mode phase S1; When relative distance , and At that time, it is the cooperative approach mode phase S2; When relative distance , and At this time, it is the end-effector pre-grabbing hovering mode stage S3; When relative distance And the gripper state changed from Evolved into At this time, it is the dynamic capture contact mode stage S4; when Furthermore, when the L2 norm of the multi-degree-of-freedom transient positioning and tracking error of the carrier converges to within the steady-state holding threshold, it is the load recovery and reset mode stage S5; In step 4, the desired dynamic positioning hovering pose space point is set. , For the desired three-dimensional spatial coordinates, For the desired three-dimensional attitude angle, define the L2 norm of the multi-degree-of-freedom transient positioning and tracking error of the carrier. : ； in, This is the real-time pose vector of the carrier in the geographic coordinate system; Constructing a nonlinear finite-time integral sliding mode surface function vector : ； in, and It is a positive definite diagonal matrix. and It is a positive odd number and satisfies ; Using the inverse kinematic transformation matrix The sliding surface function in the geographic coordinate system is transformed into the generalized velocity tracking error sliding surface in the volume-fixed coordinate system. : ； in, Reference velocity vector for the virtual volume coordinate system: ； The total output control law of the sliding mode controller for: ； in, For equivalent feedforward control terms, For adaptive robust switching control terms; The expression is: ； in, For linear fluid damping matrix, This is the hydrostatic restoring moment vector; The expression is: ； in, To preset the boundary layer smoothing adjustment coefficient, For adaptive robust time-varying switching gain matrix; The adaptive robust time-varying switching gain matrix gain components of each axis Due to real-time sliding surface error Online adjustment of the first-order adaptive differential evolution equation driven by: ； in, It is the stiffness toughening factor. The state forgetting factor; the stiffness toughening factor With state forgetting factor The variables of the operation mode stage Online tuning; The stiffness toughening factor under each operating modal stage With state forgetting factor The tuning rules are as follows: Pre-configure lower limit constant for gain With upper limit of gain constant Under each operational modal stage, the stiffness toughening factor With state forgetting factor The tuning rules are as follows: when At that time, set and ,when Reduce to season To switch gain Monotonically decreasing and remaining at the lower gain limit constant ; when At that time, set and and satisfy the inequality ,when Increase to season To switch gain It exhibits a monotonically increasing trend and is bounded by the upper limit constant of the gain. ; when At that time, set and ,when Reduce to season To switch gain Monotonically decreasing and reconverging to the lower gain limit constant. ; when At that time, set and and satisfy the inequality ,when Increase to season To switch gain Monotonically increasing and bounded by an upper limit constant for the gain ; when At that time, set and ,when Reduce to season To switch gain It decreases monotonically again and converges to the lower bound constant of the gain. .

2. The underwater robot target grasping control method for ocean current disturbance as described in claim 1, characterized in that, In step 1, the reaction load vector of the single-degree-of-freedom robotic arm on the carrier base Reconstructed as: ； in, For the total state vector of the generalized system, Let be the generalized pose vector, where For three-dimensional Euclidean space coordinates These represent the roll angle, pitch angle, and yaw angle of the carrier, respectively. This refers to the rotation angle of the robotic arm joint; For the angular velocity of the robotic arm joints, For the joint angular acceleration of the robotic arm, Let be the generalized inertial coupling matrix of the single-degree-of-freedom robotic arm to the carrier base; This is the coupling matrix of the Coriolis force and centripetal force of the single-degree-of-freedom robotic arm on the carrier base. The carrier base servo speed, This is the feedforward term for fluid impact resistance. This is a displacement hydrostatic compensation item.

3. The underwater robot target grasping control method for ocean current disturbance as described in claim 1, characterized in that, In step 3, the nonlinear disturbance observer constructs auxiliary state variables. , These are the internal auxiliary state observation components of the nonlinear disturbance observer for the six spatial motion degrees of freedom of the underwater robot: longitudinal, lateral, vertical, roll, pitch and yaw. Environmental disturbance estimates The expression is: ； in, For the generalized system inertia matrix, The gain matrix is ​​a constant observer that is symmetric and positive definite. Constructing a dual-source dynamic self-hedging nominal function term : ； in, The time-varying Coriolis force and centripetal force vectors, For linear fluid damping matrix, This is the hydrostatic restoring moment vector. This refers to the generalized pose vector of the carrier in the geographic inertial coordinate system. Will The auxiliary state variable is added to the observation equation. The differential evolution equation is: ； in, This is the physical output signal of the thruster saturation limiting module; Through the Perform numerical integration to solve auxiliary state variables in real time Then, the estimated value of environmental disturbance is solved. .

4. The underwater robot target grasping control method for ocean current disturbance as described in claim 1, characterized in that, In step 5, a multivariate joint verification vector is constructed. : ； ； ； in, The spatial geometric distance constraint function, This is the spatial geometric tolerance threshold; For the gripper actuator's closing locking function, The steady-state constraint function for dynamic positioning of the carrier base. The L2 norm of the multi-degree-of-freedom transient positioning and tracking error of the carrier. To maintain the threshold in steady state; Define a time-sliding verification window Successful capture criteria The complex logic is refactored as follows: ； If and only if Upon successful capture, switch to the load recovery and reset mode; In the load recovery and reset mode, the joint angle of the robotic arm With a limited angular velocity Return angle to reference The trajectory change was recovered; simultaneously, the stiffness toughening factor from step 4 was applied. Reduce the state forgetting factor Increase the stiffness of the unloading controller.

Images