An adaptive anti-disturbance method and system for a heterogeneous kinetic underwater robot
By adaptively adjusting the bandwidth of the extended state observer and using smooth sliding mode control, the high-frequency noise amplification and singularity problems of the underwater remotely operated vehicle were solved, enabling high-precision hovering and trajectory tracking of the underwater robot, protecting the thruster hardware, and improving the system's stability and anti-disturbance capability.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- HEBEI UNIV OF TECH
- Filing Date
- 2026-05-12
- Publication Date
- 2026-07-10
AI Technical Summary
Traditional PID control is difficult to meet the high-precision hovering and trajectory tracking requirements of ROVs. Fixed high-bandwidth extended state observers amplify high-frequency noise, causing thruster chattering. A single control algorithm cannot simultaneously meet the needs of large inertia translation and rapid rotation channels. Traditional sliding mode control has singularity issues.
A second-order linear tracking differentiator is used to generate smooth commands, adaptively adjust the bandwidth of the extended state observer, and combine the sliding mode control law of the smooth saturation function to calculate the virtual control torque of the translational and rotational channels respectively. The virtual control torque is then mapped to physical thruster commands through a pseudo-inverse matrix to achieve adaptive disturbance rejection control of heterogeneous dynamics.
It reduces high-frequency jitter in the thruster, protects motor hardware, reduces system power consumption, achieves stability in the position channel and anti-interference capability in the attitude channel, avoids the singularity problem of traditional sliding mode control, and enhances the industrial application value of ROV.
Smart Images
Figure CN122363276A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of underwater robot motion control technology, and more specifically to an adaptive disturbance rejection method and system for underwater robots based on heterogeneous dynamics. Background Technology
[0002] Currently, remotely operated vehicles (ROVs) play a crucial role in deep-sea exploration and underwater operations. However, due to the complex ocean currents and disturbances in the real underwater environment, and the inherent high degree of nonlinear coupling and unmodeled dynamics of ROVs, traditional PID control struggles to meet the demands for high-precision hovering and trajectory tracking. Active disturbance rejection control (ADRC), with its powerful disturbance estimation and compensation capabilities, is widely used, but it still faces numerous bottlenecks in practical engineering applications.
[0003] Traditional ADRC linear extended state observers typically employ a fixed high bandwidth design. In environments containing real sensor white noise such as Doppler odometers and inertial navigation units, the fixed high bandwidth severely amplifies the differential terms of high-frequency noise, leading to severe high-frequency jitter in the thruster output, which can easily cause mechanical fatigue of the motor and overheating of the ESC.
[0004] Existing technologies typically employ a single control algorithm structure for the six degrees of freedom of an ROV. However, the translational channels of a large-mass ROV have high inertia, requiring smooth and continuous thrust; while the rotating channels are easily disrupted by water flow disturbances, necessitating extremely fast, finite-time correction capabilities. A single algorithm often fails to address both aspects, leading to unnecessary oscillations in the system.
[0005] Traditional terminal sliding mode control suffers from a control law singularity dead zone caused by negative fractional power operations when the error approaches zero, and the traditional sign function switching is prone to exacerbating damage to the physical actuator.
[0006] Therefore, the problems of high-frequency noise amplification leading to thruster chattering, heterogeneous dynamic conflicts between position and attitude, and singularities of traditional sliding mode control in underwater remotely operated vehicles are problems that urgently need to be solved by those skilled in the art. Summary of the Invention
[0007] In view of the above problems, the present invention is proposed to provide an adaptive disturbance rejection method and system for heterogeneous dynamics underwater robots that overcomes or at least partially solves the above problems.
[0008] To achieve the above objectives, the present invention adopts the following technical solution:
[0009] In a first aspect, embodiments of the present invention provide an adaptive disturbance rejection method for underwater robots based on heterogeneous dynamics, characterized in that it includes: S1. The original target command is discretized using a second-order linear tracking differential algorithm by a tracking differentiator to generate continuous and smooth desired position command, desired velocity command and desired acceleration command. S2. Based on the tracking error between the current position value output by the extended state observer and the desired position command, calculate the adaptive observer bandwidth in real time and update the gain matrix coefficients of the extended state observer. S3. Using the extended state observer with updated gain matrix coefficients, the actual position value, actual velocity state quantity, and lumped disturbance state quantity including unmodeled dynamics and external ocean current interference of the underwater robot are estimated based on the measurement values of the underlying sensors and the control output. S4. Based on different sliding mode control laws, using the desired position command, desired velocity command, desired acceleration command, the actual position value of the underwater robot, the actual velocity state quantity, and the lumped disturbance state quantity including unmodeled dynamics and external ocean current interference, the virtual control thrust in the translational channel and the virtual control torque in the rotational channel of the underwater robot are calculated respectively, and the virtual control thrust and virtual control torque are integrated into a six-degree-of-freedom virtual desired torque vector. S5. Based on the geometric layout matrix of the underwater robot thrusters, a pseudo-inverse matrix algorithm is used to map the six-degree-of-freedom virtual desired torque vector into thrust commands for each physical thruster, and output the commands to the thruster actuator.
[0010] Furthermore, the bandwidth of the adaptive observer in S2 The calculation formula is:
[0011] in For high bandwidth limits, The physical noise immunity lower limit is given by k, which is the sensitivity coefficient, and the tracking error is given by k. .
[0012] Furthermore, the control output in S3 is equal to the six-degree-of-freedom virtual desired torque vector calculated in the previous moment.
[0013] Furthermore, in S4, the sign function of the sliding mode control law is replaced with a smooth saturation function having a boundary layer thickness. ; The smooth saturation function is defined as:
[0014] Where s is the sliding surface function. The preset positive boundary layer thickness; The different sliding mode control laws are as follows: for translational channels, a linear sliding surface combined with an active disturbance rejection control law is used; for rotational channels, a non-singular terminal sliding surface combined with an active disturbance rejection control law is used.
[0015] Furthermore, the specific process for calculating the virtual control thrust in the translational channel in S4 is as follows: Calculate the linear sliding surface:
[0016] Virtual control thrust for calculating translational degrees of freedom:
[0017] in The slope of the linear sliding surface; This is an estimated value for the system control gain; The gain is linearly approaching. For robust nonlinear gain; It is a smooth saturation function; This represents the boundary layer thickness.
[0018] Furthermore, the specific process for calculating the virtual control torque in the rotation channel in S4 is as follows: Calculate the non-singular terminal synovial surface:
[0019] Calculate the virtual control torque for rotational degrees of freedom:
[0020] in The nonlinear weights of the sliding surface; For non-singular fractional order; sgn( ) represents the traditional hard-switching symbol function; This is an estimated value for the system control gain; The gain is linearly approaching. For robust nonlinear gain; It is a smooth saturation function; This represents the boundary layer thickness.
[0021] Furthermore, the calculation formula for the inverse matrix algorithm in S5 is as follows:
[0022] in, For physical thruster commands; thrust array matrix The Moore-Penrose generalized inverse matrix; This is a virtual desired torque vector with six degrees of freedom.
[0023] Secondly, embodiments of the present invention provide an adaptive disturbance rejection system for an underwater robot based on heterogeneous dynamics, characterized in that it includes: Tracking Differentiator Module: Used to smooth the original target command into continuously differentiable desired position command, desired velocity command, and desired acceleration command; Adaptive bandwidth expansion state observer module: used to calculate the adaptive observer bandwidth and update the gain coefficient in real time based on the tracking error, and then estimate the actual state estimate, actual speed estimate and lumped disturbance estimate based on the sensor measurement value and the control quantity of the previous cycle; Hybrid main controller module: includes translational channel controller and rotational channel controller. The translational channel controller uses a linear sliding surface combined with an active disturbance rejection law to generate virtual control force, and the rotational channel controller uses a non-singular terminal sliding surface combined with an active disturbance rejection law to generate virtual control torque. In addition, the sign function in the sliding control law is replaced with a smooth saturation function. Thrust distribution module: It is used to integrate the virtual control force and virtual control torque output from each channel into a six-degree-of-freedom virtual desired torque vector, and to map the six-degree-of-freedom vector into thrust commands for each physical thruster using a pseudo-inverse matrix algorithm based on the thruster geometry matrix.
[0024] The beneficial effects of the above-described technical solutions provided in the embodiments of the present invention include at least the following: This invention provides an adaptive anti-disturbance method and system for underwater robots based on heterogeneous dynamics. Through an adaptive variable bandwidth mechanism, this invention actively reduces the system's "auditory sensitivity" during hovering steady-state operation, significantly reducing the amplitude of ineffective high-frequency jitter in the thruster, thereby greatly protecting the motor and ESC hardware and reducing system power consumption. It breaks free from the constraints of traditional unified algorithms across all channels, maintaining smooth and stable position channels while endowing the attitude channels with extremely strong anti-disturbance lock-up capabilities, perfectly matching the physical and dynamic characteristics of real ROVs. The improved non-singular terminal sliding mode control structure not only guarantees absolute convergence within a finite time but also completely avoids the problem of zero denominator or complex number errors that are easily triggered by the underlying microprocessor when executing traditional terminal sliding mode algorithms, possessing extremely high industrial application value. Attached Figure Description
[0025] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.
[0026] Figure 1 This is an overall flowchart provided in the embodiments of the present invention; Figure 2 This describes the chattering situation of the thruster before and after the improvement provided in the embodiments of the present invention; Figure 3 This illustrates the accuracy of trajectory tracking of the underwater robot before and after improvement, as provided in this embodiment of the invention. Detailed Implementation The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. 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 are within the scope of protection of the present invention.
[0027] This invention discloses an adaptive disturbance rejection method and system for underwater robots based on heterogeneous dynamics, such as... Figure 1 As shown, it includes: S1, using a tracking differentiator to discretize the original target command using a second-order linear tracking differential algorithm to generate continuous and smooth desired position command, desired velocity command and desired acceleration command; S2. Based on the tracking error between the current position value output by the extended state observer and the desired position command, calculate the adaptive observer bandwidth in real time and update the gain matrix coefficients of the extended state observer. S3. Using the extended state observer with updated gain matrix coefficients, the actual position value, actual velocity state quantity, and lumped disturbance state quantity including unmodeled dynamics and external ocean current interference of the underwater robot are estimated based on the measurement values of the underlying sensors and the control output. S4. Based on different sliding mode control laws, using the desired position command, desired velocity command, desired acceleration command, the actual position value of the underwater robot, the actual velocity state quantity, and the lumped disturbance state quantity including unmodeled dynamics and external ocean current interference, the virtual control thrust in the translational channel and the virtual control torque in the rotational channel of the underwater robot are calculated respectively, and the virtual control thrust and virtual control torque are integrated into a six-degree-of-freedom virtual desired torque vector. S5. Based on the geometric layout matrix of the underwater robot thrusters, a pseudo-inverse matrix algorithm is used to map the six-degree-of-freedom virtual desired torque vector into thrust commands for each physical thruster, and output the commands to the thruster actuator.
[0028] The specific implementation of this invention is as follows: Step 1: The tracking differentiator generates a smooth desired trajectory. First, the original target command The input tracking differentiator uses a second-order discretization formula:
[0029] For acceleration Perform integration; this step outputs a smooth desired position command. Expected speed command and desired acceleration commands .
[0030] Step 2: Calculate the tracking error and adaptive observer bandwidth In each control cycle, the current sensor measurement value y is read, and the state estimate is obtained from the observation results of the previous cycle. Calculate the tracking error:
[0031] Substituting the tracking error into the adaptive bandwidth scheduling rule yields the dynamic observer bandwidth parameters that best meet the physical noise immunity requirements under the current control cycle. :
[0032] in, The maximum bandwidth allowed by the system. is the physical noise immunity lower limit, and k is the sensitivity coefficient.
[0033] Step 3: Update the gain and run the extended state observer The calculated dynamic bandwidth Converted to observer gain matrix coefficients: =3 , =3 2 , = 3 Construct a third-order linear extended state observer, and substitute the sensor measurement value y and the control output u of the previous cycle into the following state equation for iterative calculation:
[0034]
[0035]
[0036] Estimate the actual position of the underwater robot Actual velocity state quantity And the total lumped disturbance state variables including unmodeled dynamics and external ocean current disturbances. .
[0037] Step 4: Hybrid Master Controller Computation Calculate the virtual control torque according to the channel type.
[0038] 4.1: Calculation of linear sliding mode active disturbance rejection control for the translational channel (X,Y,Z): Calculate the linear sliding surface:
[0039] Using saturation function Smooth switching term, dynamic feedforward compensation, and calculation of virtual control thrust for translational degrees of freedom:
[0040] in The slope of the linear sliding surface; This is an estimated value for the system control gain; The gain is linearly approaching. For robust nonlinear gain; It is a smooth saturation function; This represents the boundary layer thickness.
[0041] The saturation function is defined as:
[0042] 4.2: Calculation of Non-Singular Terminal Sliding Membrane Active Disturbance Rejection Control for Rotation Channels (Roll, Pitch, Yaw) Calculate the non-singular terminal sliding surface (1<β<2):
[0043] Calculate the virtual control torque for the rotational degrees of freedom based on the finite-time convergence law:
[0044] Will and The system integrates and generates a six-degree-of-freedom virtual desired force / torque vector that includes disturbance compensation and anti-bounce processing. .
[0045] Step 5: Thrust Distribution The controller cannot directly output torque; it must be distributed to the physical motors. Based on the absolutely symmetrical vector layout of the underwater robot's 4 horizontal / 4 vertical thrusters in its center of gravity plane, the thrust conversion equation is established: .in It is a 6x8 geometric arrangement matrix. This represents the thrust vector of the eight physical thrusters. Because the system is an overdriven system, the layout matrix... Solve for its Mohr-Ponros generalized inverse matrix, B+. The final physical thrust command calculation formula is: .
[0046] The thrust command vectors are ultimately issued to the eight thrusters of the underlying electronic control actuator. This drives underwater robots to achieve high-precision, low-jitter tracking of desired trajectories, such as... Figure 2 The image shows the bounce of the thruster before and after the improvement, as follows: Figure 3 The image shows the accuracy of trajectory tracking before and after the improvement.
[0047] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since it corresponds to the method disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.
[0048] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Claims
1. An adaptive disturbance rejection method for underwater robots based on heterogeneous dynamics, characterized in that, include: S1. The original target command is discretized using a second-order linear tracking differentiation algorithm with a tracking differentiator to generate a continuous and smooth desired position command. Expected speed command and desired acceleration command ; S2, Current position value based on the output of the extended state observer. With the desired position command Tracking error Real-time calculation of adaptive observer bandwidth Update the gain matrix coefficients of the extended state observer; S3. Using the extended state observer with updated gain matrix coefficients, the actual position of the underwater robot is estimated based on the measurements from the underlying sensors and the control output. Actual velocity state quantity and the total lumped disturbance state variables that include unmodeled dynamics and external ocean current disturbances. ; S4. Based on different sliding mode control laws, utilize the desired position command. Expected speed command Desired acceleration command The actual position value of the underwater robot Actual velocity state quantity and the total lumped disturbance state variables that include unmodeled dynamics and external ocean current disturbances. For the translational and rotational channels of the underwater robot, the virtual control thrust in the translational channel and the virtual control torque in the rotational channel are calculated respectively, and the virtual control thrust and the virtual control torque are integrated into a six-degree-of-freedom virtual desired torque vector. S5. Based on the geometric layout matrix of the underwater robot thrusters, a pseudo-inverse matrix algorithm is used to map the six-degree-of-freedom virtual desired torque vector into thrust commands for each physical thruster, and output the commands to the thruster actuator.
2. The method as described in claim 1, characterized in that, The adaptive observer bandwidth in S2 The calculation formula is: in For high bandwidth limits, The physical noise immunity lower limit is given by k, which is the sensitivity coefficient, and the tracking error is given by k. .
3. The method as described in claim 1, characterized in that, The control output quantity in S3 is equal to the six-degree-of-freedom virtual desired torque vector calculated and output at the previous moment.
4. The method as described in claim 1, characterized in that, In S4, the sign function of the sliding mode control law is replaced with a smooth saturation function with boundary layer thickness. ; The smooth saturation function is defined as: Where s is the sliding surface function. The preset positive boundary layer thickness; The specific sliding mode control laws based on different sliding surfaces are as follows: for translational channels, a linear sliding surface combined with an active disturbance rejection control law is used; for rotational channels, a non-singular terminal sliding surface combined with an active disturbance rejection control law is used.
5. The method as described in claim 1, characterized in that, The specific process for calculating the virtual control thrust in the translational channel in S4 is as follows: Calculate the linear sliding surface: Virtual control thrust for calculating translational degrees of freedom: in The slope of the linear sliding surface; This is an estimated value for the system control gain; The gain is linearly approaching. For robust nonlinear gain; It is a smooth saturation function; This represents the boundary layer thickness.
6. The method as described in claim 1, characterized in that, The specific process for calculating the virtual control torque in the rotation channel in S4 is as follows: Calculate the non-singular terminal synovial surface: Calculate the virtual control torque for rotational degrees of freedom: in The nonlinear weights of the sliding surface; For non-singular fractional order; sgn( ) represents the traditional hard-switching symbol function; This is an estimated value for the system control gain; The gain is linearly approaching. For robust nonlinear gain; It is a smooth saturation function; This represents the boundary layer thickness.
7. The method as described in claim 1, characterized in that, The calculation formula for the inverse matrix algorithm in S5 is as follows: in, For physical thruster commands; thrust array matrix The Moore-Penrose generalized inverse matrix; This is a virtual desired torque vector with six degrees of freedom.
8. An adaptive disturbance rejection system for an underwater robot based on heterogeneous dynamics, characterized in that, include: Tracking Differentiator Module: Used to smooth the original target command into continuously differentiable desired position command, desired velocity command, and desired acceleration command; Adaptive bandwidth expansion state observer module: used to calculate the adaptive observer bandwidth and update the gain coefficient in real time based on the tracking error, and then estimate the actual state estimate, actual speed estimate and lumped disturbance estimate based on the sensor measurement value and the control quantity of the previous cycle; Hybrid main controller module: includes translational channel controller and rotational channel controller, wherein the translational channel controller uses a linear sliding surface combined with an active disturbance rejection law to generate virtual control force, and the rotational channel controller uses a non-singular terminal sliding surface combined with an active disturbance rejection law to generate virtual control torque, and the sign function in the sliding control law is replaced with a smooth saturation function; Thrust distribution module: It is used to integrate the virtual control force and virtual control torque output from each channel into a six-degree-of-freedom virtual desired torque vector, and to map the six-degree-of-freedom vector into thrust commands for each physical thruster using a pseudo-inverse matrix algorithm based on the thruster geometry matrix.