Wave disturbance adaptive observation method for underwater robotic point-hold
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- OCEAN UNIV OF CHINA
- Filing Date
- 2026-06-23
- Publication Date
- 2026-07-21
Smart Images
Figure CN122431397A_ABST
Abstract
Description
Technical Field
[0001] This invention discloses an adaptive observation method for wave disturbances for stationary maintenance of underwater robots, belonging to the field of underwater robot stationary maintenance control technology. Background Technology
[0002] In current underwater robot stationary control methods, most treat waves, currents, and model uncertainties as a unified unknown lumped disturbance, compensating for them with ordinary disturbance observers or robust control. This approach fails to fully utilize the periodicity and narrow-band dominant frequency characteristics of wave disturbances, leading to phase lag in disturbance estimation and inaccurate peak-valley tracking. Some methods rely on external wave measurement equipment (such as wave height meters and pressure sensor arrays) to obtain wave parameters, but these methods face challenges in complex waters such as ports and nearshore areas, including difficulties in equipment deployment, high costs, and poor real-time performance. Furthermore, the free surface wave information measured externally differs spatially and dynamically from the equivalent wave disturbance actually experienced by the underwater robot, making it difficult to meet the requirements of real-time compensation control.
[0003] Another approach uses preset or empirical frequencies to establish a periodic disturbance model. When actual sea conditions change, causing a shift in the dominant wave frequency, the fixed-frequency model suffers from frequency mismatch, manifesting as increased phase deviation of the estimated signal, inaccurate amplitude reconstruction, and control compensation lag. Furthermore, underwater robots possess nonlinear and strongly coupled dynamics, and their motion response simultaneously includes control input, model errors, low-frequency environmental disturbances, and periodic wave components. This makes it difficult to extract effective wave equivalent dominant frequencies online from the robot's own information, thus limiting the accuracy and stationary performance of the disturbance observation model. Under shallow-water wave conditions and single-peak narrow-band waves, the vertical velocity response typically contains a significant dominant periodic component, which can therefore serve as an input signal for online identification of the equivalent dominant frequency. Summary of the Invention
[0004] The purpose of this invention is to provide an adaptive wave disturbance observation method for stationary maintenance of underwater robots, in order to solve the problem in the prior art of how to improve the estimation accuracy and real-time compensation of wave-induced periodic disturbances when stationary maintenance of underwater robots in shallow wave environments without relying on external wave measurement equipment.
[0005] Adaptive wave disturbance observation methods for stationary positioning of underwater robots include: S1. The vertical velocity response of the underwater robot is obtained in real time through the onboard sensors of the underwater robot; S2. Construct a frequency estimator state model based on the vertical velocity response of the underwater robot, use the equivalent dominant frequency estimation algorithm to predict the wave disturbance frequency, and extract the third component from the posterior state estimate as the frequency parameter of the internal harmonic model of the wave disturbance observer. S3. Construct a wave disturbance observer, update the internal harmonic model based on the frequency parameters, and output the total disturbance output by the wave disturbance observer; S4. The total disturbance output by the wave disturbance observer is used as a feedforward compensation amount and introduced into the underwater robot stationary controller for feedforward compensation control to generate the thruster control input. S5. Repeat steps S1 to S4 in each sampling period.
[0006] S1 includes airborne sensors including an inertial measurement unit, a velocity measurement device, a depth sensor, and a combined navigation system.
[0007] S2 includes S2.1, constructing the frequency estimator state model, including defining the frequency estimator state vector. : ; In the formula, For the vertical velocity response of underwater robots, For vertical acceleration, The equivalent dominant angular frequency to be estimated is... It is the transpose symbol; Based on the second-order oscillation model, the continuous state equation is established: ; ; In the formula, To determine the sign of the derivative, It is a nonlinear state function. This is the process noise vector. .
[0008] S2 includes S2.2, the equivalent dominant frequency estimation algorithm is an extended Kalman filter, and the process of predicting wave disturbance frequency includes, at the current sampling time Based on the posterior estimate of the previous time step Predict the current state : ; In the formula, The sampling period; Calculate the discrete state transition matrix : ; In the formula, It is a third-order identity matrix. The sign of the partial derivative; based on Update prediction error covariance : ; In the formula, The process noise covariance matrix is... This represents the covariance of the prediction error at the previous time step.
[0009] S2 includes S2.3, correcting the extended Kalman filter to obtain the vertical velocity response at the current moment. Calculate measurement error : ; In the formula, For measurement matrix; Calculate Kalman gain And correct the state to obtain the posterior estimate. : ; Update posterior error covariance : ; S2 includes, S2.4, from Extract the third component : ; Will Frequency parameters of the internal harmonic model of the wave disturbance observer : .
[0010] S3 includes, S3.1, constructing a wave disturbance observer: ; In the formula, For underwater robot pose estimation, For speed estimation, For wave-induced periodic disturbance estimation. This represents the derivative state of wave disturbance. For slow-varying composite perturbation estimation, For pose estimation error, For thruster control input, The inertia matrix, Here is the damping matrix. , , , , The observer gain matrix is... , , , , , For adjusting the bandwidth parameters of the wave disturbance observer, It is a sixth-order identity matrix. Let be the kinematic Jacobian matrix.
[0011] S3 includes S3.2, and the estimated value of the output wave-induced periodic disturbance. : ; Output Slowly Varying Composite Perturbation Estimation : ; Output wave disturbance observer output total disturbance estimate : .
[0012] S4 includes, will As a feedforward compensation factor, a stationary controller for the underwater robot is introduced for feedforward compensation control. The stationary controller ultimately forms the thruster control input. : ; In the formula, This is the output of the feedback control law.
[0013] S5 includes repeating steps S1 to S4 in each sampling period, where the frequency estimator is updated based on the latest vertical velocity. The wave disturbance observer updates the internal harmonic model, and the controller updates it according to... Estimated output .
[0014] Compared to existing technologies, this invention offers the following advantages: It eliminates the need for external wave measurement equipment, utilizing only the underwater robot's own vertical velocity response to acquire the equivalent dominant wave frequency online and update the internal harmonic model of the wave disturbance observer in real time. This effectively overcomes the frequency mismatch, phase lag, and amplitude tracking deviation problems caused by fixed-frequency disturbance observers when sea state changes. By jointly estimating and feedforward-compensating wave-induced periodic disturbances and slowly varying composite disturbances to the stationary controller, the invention significantly improves the underwater robot's position-keeping accuracy and attitude stability in shallow, wave-facing environments, while reducing system hardware costs and deployment complexity. It demonstrates excellent engineering practicality and environmental adaptability. Attached Figure Description
[0015] Figure 1 This is a flowchart of the technology of this invention; Figure 2 This is a structural diagram of the wave disturbance adaptive observation system provided by the present invention; Figure 3This is a flowchart of the online estimation process for the equivalent dominant wave disturbance frequency; Figure 4 This is a structural diagram of a frequency-adaptive wave disturbance observer; Figure 5 It is the compensation effect for the direction of wave-induced periodic disturbance heave; Figure 6 It is a compensation effect for the oscillation direction of wave-induced periodic disturbances. Detailed Implementation
[0016] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention are described clearly and completely below. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.
[0017] Adaptive wave disturbance observation methods for stationary positioning of underwater robots include: S1. The vertical velocity response of the underwater robot is obtained in real time through the onboard sensors of the underwater robot; S2. Construct a frequency estimator state model based on the vertical velocity response of the underwater robot, use the equivalent dominant frequency estimation algorithm to predict the wave disturbance frequency, and extract the third component from the posterior state estimate as the frequency parameter of the internal harmonic model of the wave disturbance observer. S3. Construct a wave disturbance observer, update the internal harmonic model based on the frequency parameters, and output the total disturbance output by the wave disturbance observer; S4. The total disturbance output by the wave disturbance observer is used as a feedforward compensation amount and introduced into the underwater robot stationary controller for feedforward compensation control to generate the thruster control input. S5. Repeat steps S1 to S4 in each sampling period.
[0018] S1 includes airborne sensors including an inertial measurement unit, a velocity measurement device, a depth sensor, and a combined navigation system.
[0019] S2 includes S2.1, constructing the frequency estimator state model, including defining the frequency estimator state vector. : ; In the formula, For the vertical velocity response of underwater robots, For vertical acceleration, The equivalent dominant angular frequency to be estimated is... It is the transpose symbol; Based on the second-order oscillation model, the continuous state equation is established: ; ; In the formula, To determine the sign of the derivative, It is a nonlinear state function. This is the process noise vector. .
[0020] S2 includes S2.2, the equivalent dominant frequency estimation algorithm is an extended Kalman filter, and the process of predicting wave disturbance frequency includes, at the current sampling time Based on the posterior estimate of the previous time step Predict the current state : ; In the formula, The sampling period; Calculate the discrete state transition matrix : ; In the formula, It is a third-order identity matrix. The sign of the partial derivative; based on Update prediction error covariance : ; In the formula, The process noise covariance matrix is... This represents the covariance of the prediction error at the previous time step.
[0021] S2 includes S2.3, correcting the extended Kalman filter to obtain the vertical velocity response at the current moment. Calculate measurement error : ; In the formula, For measurement matrix; Calculate Kalman gain And correct the state to obtain the posterior estimate. : ; Update posterior error covariance : ; S2 includes, S2.4, from Extract the third component : ; Will Frequency parameters of the internal harmonic model of the wave disturbance observer : .
[0022] S3 includes, S3.1, constructing a wave disturbance observer: ; In the formula, For underwater robot pose estimation, For speed estimation, For wave-induced periodic disturbance estimation. This represents the derivative state of wave disturbance. For slow-varying composite perturbation estimation, For pose estimation error, For thruster control input, The inertia matrix, Here is the damping matrix. , , , , The observer gain matrix is... , , , , , For adjusting the bandwidth parameters of the wave disturbance observer, It is a sixth-order identity matrix. Let be the kinematic Jacobian matrix.
[0023] S3 includes S3.2, and the estimated value of the output wave-induced periodic disturbance. : ; Output Slowly Varying Composite Perturbation Estimation : ; Output wave disturbance observer output total disturbance estimate : .
[0024] S4 includes, will As a feedforward compensation factor, a stationary controller for the underwater robot is introduced for feedforward compensation control. The stationary controller ultimately forms the thruster control input. : ; In the formula, This is the output of the feedback control law.
[0025] S5 includes repeating steps S1 to S4 in each sampling period, where the frequency estimator is updated based on the latest vertical velocity. The wave disturbance observer updates the internal harmonic model, and the controller updates it according to... Estimated output .
[0026] In this invention, the underwater robot (ROV) is considered a six-degree-of-freedom rigid body system. Position and attitude vectors. Defined as: ; In the formula, , , These represent the longitudinal, lateral, and vertical positions of the underwater robot in the geodetic coordinate system, respectively. , , These represent the roll angle, pitch angle, and yaw angle, respectively.
[0027] The underwater robot's velocity vector in volume coordinates The definition of is: ; In the formula, , , These represent the sway velocity, transverse velocity, and heave velocity, respectively. , , These represent the roll rate, pitch rate, and yaw rate, respectively. ; ; in, Let the inertia matrix include the rigid body mass and the added mass. The Coriolis force matrix, For hydrodynamic damping matrix, The restoring force term is caused by gravity and buoyancy. For the control force and control torque generated by the thruster, For wave-induced disturbance force and disturbance moment, The external lumped disturbance consists of ocean currents, model parameter uncertainties, thruster errors, and other unmodeled factors.
[0028] In shallow water hovering operations, underwater robots typically operate within a low-speed, small-attitude variation range. In this case, the Coriolis and centripetal terms have relatively limited impact on the observer design; simultaneously, if the distance between the underwater robot's center of gravity and center of buoyancy is small, the restoring torque near the nominal hovering attitude is relatively weak. Therefore, to facilitate the construction of a disturbance observer, the Coriolis term, restoring force term, ocean current disturbance, parameter uncertainties, and other unmodeled terms can be uniformly incorporated into the slowly varying lumped disturbance term. This leads to the control-guided dynamics model used for observer design: ; This model forms the basis for the subsequent design of wave disturbance observers.
[0029] In shallow, wave-facing conditions, the wave propagation direction is approximately aligned with the longitudinal axis of the underwater robot, and the wave action is mainly concentrated in the sway, heave, and pitch directions. In single-peak, narrow-band wave environments, wave energy is primarily concentrated around a dominant frequency, thus wave-induced disturbance forces and moments exhibit significant periodicity. Treating such wave disturbances as ordinary, unknown disturbances easily leads to phase lag and peak-valley mismatch in disturbance estimation. Therefore, this invention no longer relies solely on ordinary lumped disturbance estimation methods but explicitly incorporates the dominant frequency information of the wave disturbance.
[0030] In practical engineering applications, the equivalent wave disturbance frequency experienced by the underwater robot is not necessarily identical to the free surface wave frequency. After being transmitted through the underwater robot's structure, hydrodynamic characteristics, and closed-loop control system, the external wave excitation will manifest as an equivalent dominant frequency in the underwater robot's vertical velocity response. Therefore, this invention utilizes the underwater robot's own vertical velocity response for online frequency identification, without relying on external wave sensors. This equivalent dominant frequency is then embedded into the internal harmonic model of the harmonic state observer, enabling the periodic disturbance model of the harmonic state observer to be updated according to changes in the actual wave frequency.
[0031] This invention employs an equivalent dominant frequency estimation algorithm to estimate the equivalent dominant wave disturbance frequency online from the vertical velocity response of an underwater robot. The vertical velocity response has a higher signal-to-noise ratio for high-frequency wave excitation compared to the pose signal, and it can weaken the influence of integral drift, thus making it more suitable for frequency identification of narrowband periodic components. This frequency estimation algorithm is implemented using an extended Kalman filter. The state vector is defined as: ; in, For ROV vertical velocity response, Indicates vertical acceleration. Let represent the equivalent dominant angular frequency to be estimated. Under narrowband wave action, the dominant periodic component in the vertical velocity response can be approximated as: ; in, This represents the amplitude of the component corresponding to the dominant period of vertical velocity. For the initial phase, The dominant angular frequency. For Taking the first derivative, we get: ; Further differentiation yields: ; In the formula, For the second derivative; Based on the above second-order oscillation model, a continuous-time nonlinear state equation can be established: ; in, This is the process noise vector, used to characterize model error, unmodeled dynamics, and the slow changes in the equivalent dominant frequency. Nonlinear state function: ; Since the vertical velocity response of an underwater robot can be obtained from onboard sensors and velocity measurement devices, vertical velocity is used as the measurement quantity in the frequency estimation algorithm. In the... At each sampling time, the measurement equation is: ; in, For vertical velocity measurement signals, For the true vertical velocity response, For measuring noise; ; Measurement Matrix for: ; Let the sampling period be By discretizing the state equations between continuous intervals using the forward Euler method, we can obtain: ; Substituting the state function, we get: .
[0032] To simplify the representation, the discrete state equations are expressed as: ; Because the function in the discrete system contains nonlinear terms, the system is a nonlinear system. To perform predictive covariance updates, the nonlinear function is now locally linearized at each sampling time. The Jacobian matrix of the continuous-time state function... for: ; Discrete state transition matrix for: ; Expanding, we get: .
[0033] In addition, the initial state estimate, initial error covariance matrix, process noise covariance matrix, and measurement noise covariance matrix need to be provided. Initial State Estimation It can be set to: ; in, The initial vertical velocity measurement value. These are initial frequency estimates based on the typical wave periods of the operating sea area. Predicted state at each sampling time for: ; Its component form is: ; The prediction error covariance matrix is updated to : ; Predicted measurement value for: ; In obtaining the first After obtaining the vertical velocity at each sampling time, the measurement error is calculated: ; ; Obtain the Kalman gain matrix for: ; The posterior state estimation error is updated to : ; The posterior error covariance matrix is updated to : ; EKF completes the first After prediction and correction at each sampling time, the equivalent dominant angular frequency is extracted from the third component of the posterior state estimate: ; This estimated frequency is used as the frequency parameter of the internal harmonic model of the wave disturbance observer, denoted as: ; Therefore, the output of the equivalent dominant frequency estimation algorithm is connected to the frequency input of the frequency adaptive wave disturbance observation algorithm, forming a data transmission relationship of "vertical velocity response - equivalent dominant frequency - frequency adjustable period internal model".
[0034] This invention addresses narrow-band sea states with a single dominant frequency, where wave-induced disturbances exhibit a distinct periodic structure in shallow, wave-facing conditions. To enable frequency-adaptive wave disturbance observation algorithms to utilize this periodic structure, this invention models wave-induced disturbances as second-harmonic waves. The invention models wave-induced disturbances as second-harmonic waves as follows: ; in, For wave-induced disturbance forces and moments. This represents the derivative state of wave-induced disturbance. For the true equivalent dominant frequency, This refers to the harmonic model residuals caused by non-ideal narrowband waves, multi-frequency components, and model approximation errors.
[0035] Slow-varying integrated total disturbance The model is as follows: ; This slowly varying composite disturbance mainly includes ocean current influences, model parameter uncertainties, thruster errors, and other unmodeled hydrodynamic terms. The aforementioned modeling approach categorizes external disturbances into two types: the first type is wave-induced periodic disturbances with a dominant frequency, and the second type is composite disturbances with a slower rate of change. The frequency-adaptive wave disturbance observation algorithm estimates both types of disturbances simultaneously by setting different states.
[0036] By embedding the equivalent dominant frequency output by the frequency estimation algorithm into the periodic internal model of the wave disturbance observation algorithm, a frequency-adaptive wave disturbance observer can be constructed as follows: ; This formula shows that the equivalent dominant frequency output by the frequency estimation algorithm is directly fed into the internal harmonic model of the disturbance observer, which is the core technical feature that distinguishes this invention from ordinary disturbance observation methods.
[0037] Select the following parameters: ; in For HESO bandwidth adjustment parameters, It is a sixth-order identity matrix.
[0038] Therefore, by choosing The nominal observation error poles can be configured at Nearby. Larger. It can improve the convergence speed of the observer, but may also increase sensitivity to measurement noise; smaller This can reduce noise sensitivity, but it will slow down the disturbance estimation response speed. Therefore, in practical applications, the response speed can be adjusted according to the underwater robot's sensor noise level and the wave disturbance frequency range. .
[0039] Based on the observer state definition, the estimated value of wave-induced periodic disturbance is: ; The estimated total disturbance of the slow-varying integrated system is: ; Therefore, the total disturbance output by the wave disturbance observer is: .
[0040] The core of this invention lies in constructing a frequency-adaptive wave disturbance estimation method. In order to enable the disturbance estimation results to directly serve the underwater robot's stationary maintenance task, the disturbance estimates output by each observer can be used as feedforward compensation terms for the controller.
[0041] This invention also provides an adaptive wave disturbance observation system. A vertical velocity acquisition module is used to acquire the vertical velocity response of an underwater robot during its stationary position in shallow water facing waves, and sends the vertical velocity measurement signal to an equivalent dominant frequency estimation module. The equivalent dominant frequency estimation module takes the vertical velocity measurement signal as input and... The system estimates the equivalent dominant wave disturbance frequency online through a prediction and correction process, based on the state of the underwater robot. A frequency-adaptive wave disturbance observation module takes the underwater robot's position, attitude, velocity, control input, and the frequency estimator's output frequency as input, and uses a five-state observation structure to estimate wave-induced periodic disturbances and slowly varying lumped disturbances in real time. A disturbance estimation output module extracts the values from the wave disturbance observer's state. and The vertical velocity acquisition module receives the total disturbance estimate and uses it as a feedforward compensation term in the control input calculation, thereby reducing the impact of wave disturbance on the underwater robot's position and attitude stability. The connection between these modules is as follows: the output of the vertical velocity acquisition module is connected to the input of the equivalent dominant frequency estimation module; the output of the equivalent dominant frequency estimation module is connected to the frequency input of the frequency adaptive wave disturbance observation module; the output of the frequency adaptive wave disturbance observation module is connected to the disturbance estimation output module; the output of the disturbance estimation output module is connected to the fixed-point holding compensation control module; and the output of the fixed-point holding compensation control module is connected to the underwater robot's thruster actuator. Through this connection, the present invention realizes the technical path from vertical velocity response to dominant frequency estimation, and from dominant frequency to wave disturbance observation.
[0042] The following description, in conjunction with the accompanying drawings, provides further details. The flowchart of the method of this invention is shown below. Figure 1 As shown, a six-degree-of-freedom ROV dynamic model is established; ROV pose, velocity, and control input information are collected; vertical velocity information is extracted from the ROV velocity information; the vertical velocity information is processed to estimate the equivalent dominant disturbance frequency online; the equivalent dominant disturbance frequency is input into the harmonic disturbance internal model; a frequency-adaptive wave disturbance observer is constructed; wave-induced disturbances and slowly varying composite disturbances are estimated online; the disturbance estimates are introduced into the ROV fixed-point hold control law to achieve disturbance feedforward, and the above steps are repeated in the next sampling period.
[0043] This invention provides an adaptive wave disturbance observation system, such as... Figure 2 As shown, after receiving shallow-water wave disturbances, the system, through the six-degree-of-freedom ROV body system, airborne attitude and velocity sensor measurement module, vertical velocity response extraction module, equivalent dominant frequency estimation module, and wave disturbance adaptive observation module, outputs fixed-point control feedforward compensation. This feedforward compensation is input to the thruster allocation and execution module to control the six-degree-of-freedom ROV body. The equivalent dominant frequency estimation module obtains the equivalent dominant disturbance frequency online based on the ROV's own vertical velocity response. The wave disturbance adaptive observation module updates the harmonic internal model based on this frequency and outputs wave disturbance estimates and composite disturbance estimates.
[0044] The online estimation process of the equivalent dominant wave disturbance frequency of this invention is as follows: Figure 3 As shown, the ROV vertical velocity response is used as input. The velocity signal preprocessing module performs mean removal, amplitude limiting, and filtering operations, and then constructs a harmonic model containing frequency components. The state prediction module predicts the vertical velocity response state, the prediction error calculation module compares the measured value and the predicted value, the state correction module updates the frequency state estimate, and outputs the equivalent wave-dominant disturbance frequency.
[0045] The structure of the frequency-adaptive wave disturbance adaptive observer of this invention is as follows: Figure 4 As shown, a frequency-adaptive wave disturbance observer is constructed based on an adaptive frequency estimator. The ROV velocity signal, control input, and dynamic parameters are used as inputs. An error injection term is constructed through the frequency-adaptive wave disturbance observer and input into the error injection module. Then, the wave disturbance is estimated through the frequency-adaptive harmonic internal mode and the slow-varying composite disturbance is estimated through the error integral internal mode.
[0046] In this embodiment of the invention, a comparison is made between the traditional observer compensation method (GESO generalized extended state observer) and the method of the present invention to verify the compensation effect of the method of the present invention on wave-induced periodic disturbances. Figure 5 and Figure 6 As shown, compared to traditional observer compensation methods, the compensation method proposed in this invention exhibits better stability in both the surge and heave directions. Traditional methods show significant high-frequency oscillations and multiple abrupt changes in the compensation output, with the compensation amount fluctuating over a large range. In contrast, the method of this invention can quickly converge to a stable compensation range after a brief transition and remain near the desired compensation amount for a long period. Based on the steady-state fluctuation range estimation in the figure, the method of this invention can reduce the amplitude of the compensation fluctuation by more than 85% in the surge direction and more than 80% in the heave direction, significantly improving the smoothness and stability of the compensation output.
[0047] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.
Claims
1. An adaptive wave disturbance observation method for stationary maintenance of underwater robots, characterized in that, include: S1. The vertical velocity response of the underwater robot is obtained in real time through the onboard sensors of the underwater robot; S2. Construct a frequency estimator state model based on the vertical velocity response of the underwater robot, use the equivalent dominant frequency estimation algorithm to predict the wave disturbance frequency, and extract the third component from the posterior state estimate as the frequency parameter of the internal harmonic model of the wave disturbance observer. S3. Construct a wave disturbance observer, update the internal harmonic model based on the frequency parameters, and output the total disturbance output by the wave disturbance observer; S4. The total disturbance output by the wave disturbance observer is used as a feedforward compensation amount and introduced into the underwater robot stationary controller for feedforward compensation control to generate the thruster control input. S5. Repeat steps S1 to S4 in each sampling period.
2. The wave disturbance adaptive observation method for stationary maintenance of underwater robots according to claim 1, characterized in that, S1 includes airborne sensors including an inertial measurement unit, a velocity measurement device, a depth sensor, and a combined navigation system.
3. The wave disturbance adaptive observation method for stationary maintenance of underwater robots according to claim 2, characterized in that, S2 includes S2.1, constructing the frequency estimator state model, including defining the frequency estimator state vector. : ； In the formula, For the vertical velocity response of underwater robots, For vertical acceleration, The equivalent dominant angular frequency to be estimated is... It is the transpose symbol; Based on the second-order oscillation model, the continuous state equation is established: ； ； In the formula, For the sign of differentiation, It is a nonlinear state function. This is the process noise vector. .
4. The wave disturbance adaptive observation method for stationary maintenance of underwater robots according to claim 3, characterized in that, S2 includes S2.2, the equivalent dominant frequency estimation algorithm is an extended Kalman filter, and the process of predicting wave disturbance frequency includes, at the current sampling time Based on the posterior estimate of the previous time step Predict the current state : ； In the formula, The sampling period; Calculate the discrete state transition matrix : ； In the formula, It is a third-order identity matrix. The sign of the partial derivative; based on Update prediction error covariance : ； In the formula, The process noise covariance matrix is... This represents the covariance of the prediction error at the previous time step.
5. The wave disturbance adaptive observation method for stationary maintenance of underwater robots according to claim 4, characterized in that, S2 includes S2.3, correcting the extended Kalman filter to obtain the vertical velocity response at the current moment. Calculate measurement error : ； In the formula, For measurement matrix; Calculate Kalman gain And correct the state to obtain the posterior estimate. : ； Update posterior error covariance : ； S2 includes, S2.4, from Extract the third component : ； Will Frequency parameters of the internal harmonic model of the wave disturbance observer : 。 6. The wave disturbance adaptive observation method for stationary maintenance of an underwater robot according to claim 5, characterized in that, S3 includes, S3.1, constructing a wave disturbance observer: ； In the formula, For underwater robot pose estimation, For speed estimation, For wave-induced periodic disturbance estimation. This represents the derivative state of wave disturbance. For slow-varying composite perturbation estimation, For pose estimation error, For thruster control input, The inertia matrix, Here is the damping matrix. , , , , The observer gain matrix is... , , , , , For adjusting the bandwidth parameters of the wave disturbance observer, It is a sixth-order identity matrix. Let be the kinematic Jacobian matrix.
7. The wave disturbance adaptive observation method for stationary maintenance of an underwater robot according to claim 6, characterized in that, S3 includes S3.2, and the estimated value of the output wave-induced periodic disturbance. : ； Output Slowly Varying Composite Perturbation Estimation : ； Output wave disturbance observer output total disturbance estimate : 。 8. The wave disturbance adaptive observation method for stationary maintenance of an underwater robot according to claim 7, characterized in that, S4 includes, will As a feedforward compensation factor, a stationary controller for the underwater robot is introduced for feedforward compensation control. The stationary controller ultimately forms the thruster control input. : ； In the formula, This is the output of the feedback control law.
9. The wave disturbance adaptive observation method for stationary maintenance of an underwater robot according to claim 8, characterized in that, S5 includes repeating steps S1 to S4 in each sampling period, where the frequency estimator is updated based on the latest vertical velocity. The wave disturbance observer updates the internal harmonic model, and the controller updates it according to... Estimated output .