A method and system for controlling anti-disturbance motion of an autonomous underwater robot

Through the combination of the two-layer model predictive control framework and perturbation observer, the control problem of autonomous underwater robots under unknown bounded disturbances is solved, the control accuracy and robustness are improved, and the system operates stably in complex marine environments.

CN114967714BActive Publication Date: 2024-11-01HUNAN UNIV
View PDF 5 Cites 0 Cited by

Patent Information

Application Number
CN202210728589.1
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-06-24
Publication Date
2024-11-01
Estimated Expiration
2042-06-24

AI Technical Summary

Technical Problem

When autonomous underwater robots face unknown bounded disturbances, existing control methods are difficult to ensure control accuracy and robustness. Especially under complex conditions in marine environments, model uncertainty and external interference are difficult to effectively deal with.

Method used

Using a two-layer model prediction control framework, simplified linear models by establishing autonomous underwater robot dynamics and kinematics, constructing perturbation observers and nominal model prediction controllers, designing auxiliary prediction controllers, using perturbation observers to estimate perturbation and optimize them The problem is solved by optimum control input to ensure that the system state is maintained within the constraints.

Benefits of technology

Effectively dealing with the impact of uncertainty improves control accuracy and robustness, allowing autonomous underwater robots to strictly meet constraints under uncertain circumstances and achieve better motion control effects.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114967714B_ABST
    Figure CN114967714B_ABST
Patent Text Reader

Abstract

The present invention discloses an anti-disturbance motion control method for an autonomous underwater robot, comprising the following steps: step 1, establishing a simplified linear model of dynamics and kinematics of the autonomous underwater robot; step 2, establishing a nominal model of the system; step 3, constructing a disturbance observer; step 4, designing a nominal model predictive controller; step 5, designing an auxiliary model predictive controller; step 6, measuring the system state at the next moment, taking the moment as the new current moment, and returning to step 3. The anti-disturbance motion control method for an autonomous underwater robot of the present invention establishes a double-layer model predictive control framework, can effectively cope with the influence of uncertainty and track reference values, and has a better control effect.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous underwater robot control, and in particular relates to an anti-disturbance motion control method and system for an autonomous underwater robot. Background Art

[0002] An underwater robot is an underwater device that replaces humans to complete various complex operations in the marine environment. Autonomous underwater robots are increasingly being used in marine scientific investigations, rescue and salvage, and seabed resource surveys due to their flexibility, autonomy, and intelligence. In various application scenarios, the ability of autonomous underwater robots to achieve high-quality posture control such as depth determination, diving, floating, and hovering positioning is a necessary condition for them to complete tasks accurately and efficiently.

[0003] The control technology of autonomous underwater robots is their core technology. Due to the characteristics of autonomous underwater robots such as high nonlinearity and strong coupling, their application in motion control is more difficult. In addition, common and complex environments in the ocean environment, such as dynamic ocean currents, wind and waves, and deep-water pressure, will greatly affect the motion control of underwater robots. Based on the above characteristics, it is difficult to establish an accurate control model of autonomous underwater robots. Some unmodeled dynamics are often ignored and simplified models are established, resulting in the established model not matching the actual situation, thus affecting the control accuracy.

[0004] At present, the research on motion control methods of autonomous underwater robots mainly includes various methods such as PID control, fuzzy control, sliding mode control, neural network control and adaptive control. There are also technologies that combine two or more control methods, such as fuzzy sliding mode variable structure control and adaptive neural network control. Among them, the patent "A method and system for controlling the depth interval of underwater robots" owned by Huazhong University of Science and Technology (patent number: 202010310143.8) discloses a method and system for controlling the depth interval of underwater robots under the constraints of rudder angle and rudder speed, which is conducive to reducing the frequency and amplitude of ruddering. However, this method does not consider disturbances in the optimization process. It only relies on the robustness of the controller itself, and may not guarantee the control effect when there are large disturbances. The patent "A method for tracking the path of underwater robots based on model predictive control" owned by Tsinghua University (patent number: 201811383664.5) discloses a method for tracking the path of underwater robots based on model predictive control. The model predictive control method is realized through online numerical optimization and rolling time domain, and handles the constraints of control input. It has certain robustness, but it is still not enough to handle model uncertainty and external interference. Therefore, it is necessary to propose a robust predictive control method that can effectively handle disturbances to ensure that the autonomous underwater robot system can strictly meet the constraints under uncertainties and effectively improve the system robustness. Summary of the invention

[0005] In view of the deficiencies in the prior art, the purpose of the present invention is to solve the motion control problem of an autonomous underwater robot under unknown bounded disturbances, and to propose a robust control method and system.

[0006] To achieve the above object, the present invention provides the following technical solution: an autonomous underwater robot anti-disturbance motion control method, comprising the following steps:

[0007] Step 1, establish a simplified linear model of the dynamics and kinematics of the autonomous underwater robot, transform it into a continuous time model in the state space, and discretize it to obtain a discrete time model of the system;

[0008] Step 2, establish a system nominal model, which is used as a prediction equation of a nominal model predictive controller, and transforms the system state and control input constraints into tightened state and control input constraints of the nominal system;

[0009] Step 3, construct a disturbance observer, regard the disturbance variable as an extended state quantity, estimate the system disturbance value, and send the estimated value to the auxiliary predictive controller;

[0010] Step 4, design a nominal model predictive controller, construct a cost function, solve the finite time domain optimization problem, and obtain the nominal optimal control input and the corresponding nominal state;

[0011] Step 5, design an auxiliary model predictive controller, receive the nominal predicted input and output signals and the disturbance signal provided by the disturbance observer, solve the optimal control for the actual system model, and apply the first control quantity of the control sequence to the autonomous underwater robot system;

[0012] Step 6, measure the system state at the next moment, take that moment as the new current moment, and return to step 3.

[0013] As a further improvement of the present invention, the tightened state and control input constraints of the nominal system in step 2 are specifically expressed as:

[0014]

[0015]

[0016] Among them, α and γ represent the contraction coefficients in the range of (0,1).

[0017] As a further improvement of the present invention, the specific steps of constructing the disturbance observer in step 3 are as follows:

[0018] Step 31, treat the disturbance as a newly added state quantity and add it to the uncertain system, and establish the following linear state observer:

[0019]

[0020] The state estimate Output Estimates L, H are the observer gain matrices, is the disturbance estimate, β is an auxiliary variable;

[0021] Step 32, calculate the state estimation error and disturbance error:

[0022]

[0023]

[0024] Then for e x The derivative is:

[0025]

[0026] Get the first derivative of the perturbation estimate:

[0027]

[0028] where the estimate of the derivative of the disturbance is Step 33, the state estimation error and the disturbance estimation error are combined into the following estimation error system:

[0029]

[0030] Where G = [e x T e w ] T , represents the estimated error of the derivative of the disturbance.

[0031] As a further improvement of the present invention, the specific steps of designing a nominal model predictive controller, constructing a cost function, solving a finite time domain optimization problem, and obtaining a nominal optimal control input and a corresponding nominal state in step 4 are as follows:

[0032] Step 41, design the optimal control input for each sampling moment, drive the nominal system state to the target point and minimize the cost function. The constructed cost function is as follows:

[0033]

[0034] where x ref is the target state vector, N p is the prediction time domain, Q and R represent the positive definite weighting matrices of the prediction state and control input, respectively, and P is the terminal weight matrix, which is obtained by solving the Riccati equation;

[0035] Step 42, satisfying the nominal prediction model constraints as well as the nominal state constraints and the nominal control constraints, wherein the nominal state and the nominal control are respectively constrained within a subset of the actual constraints:

[0036]

[0037]

[0038] The state constraints that the model must satisfy in the last prediction interval within the prediction time domain:

[0039]

[0040] Terminal Constraints satisfy Among them, A K =A+BK, is a robust invariant set;

[0041] Step 43, express the optimal control problem at any time k in a finite time domain:

[0042]

[0043]

[0044]

[0045]

[0046]

[0047] in, The linear feedback gain K is obtained by a linear quadratic regulator.

[0048] As a further improvement of the present invention, the method of solving the optimal control for the actual system model in step 5 is to solve the following optimization problem:

[0049]

[0050]

[0051]

[0052]

[0053]

[0054] in It is estimated by the disturbance observer.

[0055] As a further improvement of the present invention, the specific steps of establishing a simplified linear model of the dynamics and kinematics of the autonomous underwater robot in step 1, converting it into a continuous time model in the state space, and discretizing it to obtain a discrete time model of the system are as follows:

[0056] Step 11, define the required coordinate systems: an inertial coordinate system and a body coordinate system, the inertial coordinate system is fixed to the ground, and the body coordinate system is fixed to the autonomous underwater robot, where E-ξηζ represents the inertial coordinate system and O-xyz represents the body coordinate system;

[0057] Step 12, define the speed V of the autonomous underwater robot in the body coordinate system in six degrees of freedom = [uv lp qr] T and the position and attitude angle η relative to the fixed coordinate system = [xyz φ θ ψ] T , the motion equation of the autonomous underwater robot is as follows:

[0058] MV+C(V)V+D(V)V+g(η)=τ

[0059]

[0060] Where M is the sum of the rigid body mass and the additional mass matrix, C(V) is the Coriolis force and centripetal force matrix, D(V) is the damping matrix, g(η) is the restoring moment, τ is the propulsion system input matrix, and J(η) is the Jacobian transformation matrix between the inertial coordinate system and the body coordinate system;

[0061] Step 13, assuming that the relevant state quantities of the horizontal plane, the lateral velocity v, the heading angular velocity p, the rolling angular velocity r, and the rolling angle φ are all approximately 0, the change of the longitudinal velocity u is gentle and stable, and u is approximately a constant. Only the movement of the autonomous underwater robot in the vertical xz plane is considered, the model is decoupled, and a simplified vertical third-order depth control model is established as follows:

[0062]

[0063] Where z is the vertical depth in the inertial coordinate system, θ is the pitch angle, q is the pitch angular velocity, δ is the rudder angle, I y is the moment of inertia about the y-axis, M q 、M δ , is the additional mass matrix, W is gravity, B1 is buoyancy, Δ q represents the uncertain part of the model, τ q represents the external time-varying disturbance caused by ocean currents and wind waves;

[0064] Step 14, using sensors to collect the position information and posture information of the autonomous underwater robot, taking z, θ, q as state quantities, and the rudder angle δ as the control input, x = [z, θ, q] T Used to describe the depth control problem of autonomous underwater robots. Convert the above model into state space equations:

[0065]

[0066] Among them, the state transfer matrix Input Matrix Discretizing the continuous-time state space equation according to the sampling time ΔT yields:

[0067] x k+1 =Ax k +Bδ k +w k

[0068] Among them, the status Control Input As a further improvement of the present invention, in step 14, it is assumed that the perturbation is bounded, and the bounded set is a convex set, which is expressed as: where w max is the upper bound of disturbance.

[0069] On the other hand, the present invention provides a system for applying the above method, including a data acquisition device, a computing unit, an actuator and a driving mechanism. The computing unit is used to carry out the above method and control the actuator and the driving mechanism after executing the method. The data acquisition device includes a posture sensor and a depth sensor. The posture sensor is used to collect underwater information, robot posture and inertial navigation information, and perform mean filtering on the information. The depth sensor is used to measure the height distance to the water surface.

[0070] Beneficial effects of the present invention:

[0071] (1) The present invention establishes a two-layer model predictive control framework, which can effectively deal with the impact of uncertainty and track reference values, and has better control effects;

[0072] (2) The present invention uses a disturbance observer to estimate the prediction error caused by the uncertainty of the autonomous underwater robot system and compensates for the estimated disturbance, so that the actual state trajectory is close to the nominal system state trajectory, thereby improving the control accuracy;

[0073] (3) The present invention explicitly processes state and control constraints and strictly ensures constraint satisfaction, and keeps the state of the autonomous underwater robot system in a pipeline centered on the nominal state trajectory, thereby improving the robustness of system control. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] Figure 1 The inertial coordinate system and the body coordinate system of the autonomous underwater robot in the present invention;

[0075] Figure 2 A framework diagram of the depth control of the autonomous underwater robot in the embodiment provided by the present invention. DETAILED DESCRIPTION

[0076] The present invention will be further described below in detail with reference to the embodiments shown in the accompanying drawings.

[0077] Taking the depth control of an autonomous underwater robot as an example, the specific steps are as follows:

[0078] Step 1: Establish a simplified prediction model based on the kinematic and dynamic models of the autonomous underwater vehicle.

[0079] First, define the required coordinate systems: inertial coordinate system and body coordinate system. The inertial coordinate system is fixed to the ground, and the body coordinate system is fixed to the autonomous underwater robot, such as Figure 1 As shown, E-ξηζ represents the inertial coordinate system and O-xyz represents the body coordinate system.

[0080] Then define the speed V of the six degrees of freedom of the autonomous underwater robot in the body coordinate system as [uvlp qr] T and the position and attitude angle η relative to the fixed coordinate system = [xyz φ θ ψ] T , the motion equation of the autonomous underwater robot is as follows:

[0081] MV+C(V)V+D(V)V+g(η)=τ

[0082]

[0083] Among them, M is the sum of the rigid body mass and the additional mass matrix, C(V) is the Coriolis force and centripetal force matrix, D(V) is the damping matrix, g(η) is the restoring torque, τ is the propulsion system input matrix, and J(η) is the Jacobian transformation matrix between the inertial coordinate system and the body coordinate system.

[0084] Assuming that the relevant state quantities of the horizontal plane, the lateral velocity v, the heading angular velocity p, the rolling angular velocity r, and the rolling angle φ are all approximately 0, the change of the longitudinal velocity u is gentle and stable, and u is approximately a constant. Only the motion of the autonomous underwater robot in the vertical xz plane is considered, the model is decoupled, and a simplified vertical third-order depth control model is established as follows:

[0085]

[0086] Where z is the vertical depth in the inertial coordinate system, θ is the pitch angle, q is the pitch angular velocity, δ is the rudder angle, Iy is the moment of inertia about the y-axis, M q 、M δ , is the additional mass matrix, W is gravity, B1 is buoyancy, Δ q represents the uncertain part of the model, τ q Represents external time-varying disturbances caused by ocean currents and wind waves.

[0087] The unmodeled dynamics of the system are internal disturbances, and the various uncertainties caused by ocean currents, wind and waves are external disturbances. q , external disturbance τ q are all regarded as bounded disturbances and are equivalent to the total disturbance Δ of the system. w .

[0088] The position and attitude information of the autonomous underwater robot are collected by sensors, z, θ, q are used as state quantities, and the rudder angle δ is the control input, x = [z, θ, q] T Used to describe the depth control problem of autonomous underwater robots. Convert the above model into state space equations:

[0089]

[0090] Among them, the state transfer matrix Input Matrix

[0091] Discretizing the continuous-time state space equation according to the sampling time ΔT yields:

[0092] x k+1 =Ax k +Bδ k +w k

[0093] Among them, the status Control Input

[0094] Due to the complexity and time-varying nature of the underwater environment, it is difficult to obtain the exact value of the disturbance through the model or sensor. It can be assumed that the disturbance is bounded, and the bounded set is a convex set, which can be expressed as: where w max is the upper bound of disturbance.

[0095] Step 2: Establish the nominal model of the autonomous underwater robot system and calculate the nominal constraints.

[0096] First, set the state and control input constraints according to the actual physical conditions of the system and the saturation constraints of the mechanism. The diving speed of the autonomous underwater robot is related to the pitch angle and torque. Larger pitch and torque make it dive quickly, but not too large. In order to ensure safety and stability during movement, certain pitch constraints must be met to avoid rolling and other phenomena. Therefore, the given state constraints include constraints on the pitch angle and pitch angular velocity, and the control input constraints refer to the range of allowed input rudder angle changes:

[0097]

[0098]

[0099] Among them, the collection are all compact convex sets including the origin, x min 、x max ,δ min ,δ max is a known constant.

[0100] The nominal model refers to a virtual but precisely known model established in the control system that ignores all uncertainties of the system and is written in state space form:

[0101]

[0102] in, is the nominal state at time k, is the nominal control input at time k.

[0103] The difference between the actual model and the nominal model is:

[0104]

[0105] The state and input constraints of the nominal system are calculated based on the state and control constraints of the autonomous underwater robot system. Since the system uncertainty is ignored, the nominal system has more compact constraints than the actual system. The calculation of the nominal state constraints and nominal control constraints is tightened on the basis of the actual constraints, which are expressed as:

[0106]

[0107]

[0108] Among them, α and γ represent the contraction coefficients in the range of (0,1).

[0109] Step 3: Use the disturbance observer to estimate the system disturbance. First, treat the disturbance as an extended state quantity and add it to the uncertain system, and establish the following linear state observer:

[0110]

[0111] The state estimate Output Estimates L, H are the observer gain matrices, is the disturbance estimate, and β is an auxiliary variable.

[0112] The state estimation error and disturbance estimation error are as follows:

[0113]

[0114]

[0115] Then for e x The derivative is:

[0116]

[0117] Then the first-order derivative of the disturbance estimate can be expressed as:

[0118]

[0119] where the estimate of the derivative of the disturbance is

[0120] The state estimation error and disturbance estimation error form the following estimation error system:

[0121]

[0122] Where G = [e x T e w ] T , represents the estimated error of the derivative of the disturbance.

[0123] By designing an appropriate observer gain matrix, the eigenvalues ​​of the error system are all located in the left half complex plane, so that the estimated error converges to zero, thereby ensuring the stability of the disturbance observer. The obtained disturbance estimate will be fed back to the auxiliary predictive controller to offset the impact of the disturbance on the controlled object as much as possible.

[0124] Step 4: Design a nominal model predictive controller and calculate the optimal nominal control input in the prediction time domain.

[0125] Design the optimal control input for each sampling moment, drive the nominal system state to the target point and minimize the cost function. The control goal is to design the nominal input rudder angle without considering system uncertainty and unknown time-varying external disturbances, so that the error between the nominal state in the prediction time domain and the given reference state value is minimized. The cost function reflects the system's control requirements for the state and input of each prediction stage, and adopts the following cost function:

[0126]

[0127] where x ref is the target state vector, N p is the prediction time domain, Q and R represent the positive definite weight matrices of the prediction state and control input, respectively. P is the terminal weight matrix, which is obtained by solving the Riccati equation.

[0128] Solving the optimal problem requires satisfying the nominal prediction model constraints as well as the nominal state constraints and nominal control constraints. In a finite time domain, the optimal control problem at any time k is expressed as:

[0129]

[0130]

[0131]

[0132]

[0133]

[0134] in, The linear feedback gain K is obtained by a linear quadratic regulator.

[0135] The nominal states and nominal control inputs are each constrained within a subset of the actual constraints:

[0136]

[0137]

[0138] The state constraints that the model must satisfy in the last prediction interval within the prediction time domain:

[0139]

[0140] Terminal Constraints satisfy Among them, A K =A+BK, is a robust invariant set.

[0141] This optimization problem hopes that the predicted state is as close to the target state value as possible and the input rudder angle is as small as possible. Solving the above optimization problem gives the nominal optimal control sequence and the corresponding nominal state sequence And sent to the lower-level controller as a reference instruction for the auxiliary predictive controller.

[0142] Step 5: Design an auxiliary predictive controller and design a control strategy based on the actual prediction model.

[0143] Since the autonomous underwater robot system is affected by disturbances, the future state trajectory may be different from the nominal predicted trajectory. In order to offset the impact of disturbances, an auxiliary predictive controller is designed to make the system state and control input close to the nominal sequence. Solve the following optimization problem:

[0144]

[0145]

[0146]

[0147]

[0148]

[0149] in It is estimated by the disturbance observer.

[0150] The final control signal It is obtained by solving two optimization problems in steps 4 and 5. It acts as a rudder angle signal to the controlled object.

[0151] Step 6: Measure the system state at the next moment, take that moment as the new current moment, and return to step 3.

[0152] On the other hand, this embodiment provides a system, including a data acquisition device, a computing unit, an actuator and a driving mechanism. The computing unit is used to carry the above method and control the actuator and the driving mechanism after executing the method. The data acquisition device includes a posture sensor and a depth sensor. The posture sensor is used to collect underwater information, robot posture and inertial navigation information, and perform mean filtering on the information. The depth sensor is used to measure the height distance to the water surface. Considering the unknown disturbances caused by unmodeled dynamics and dynamic ocean currents, wind and waves, etc., a system model that meets the state and control constraints is established, and a two-layer model predictive controller is designed in combination with a disturbance observer. The upper layer designs a nominal predictive controller for the nominal model, and calculates the nominal input and the nominal state so that the error between the nominal state and the reference value is minimized. The lower layer designs an auxiliary predictive controller for the actual model, and solves the control input actually acting on the autonomous underwater robot to compensate for the error between the nominal state and the actual state. This method can ensure that the state of the autonomous underwater robot system is strictly kept in a small neighborhood of the target state to accurately achieve motion control.

[0153] The above is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention. It should be pointed out that for ordinary technicians in this technical field, some improvements and modifications without departing from the principle of the present invention should also be regarded as the protection scope of the present invention.

Claims

1. A method for controlling the anti-disturbance motion of an autonomous underwater robot, characterized in that: The steps include: Step 1, establish a simplified linear model of the dynamics and kinematics of the autonomous underwater robot, transform it into a continuous time model in the state space, and discretize it to obtain a discrete time model of the system; Step 2, establish a system nominal model, which is used as a prediction equation of a nominal model predictive controller, and transforms the system state and control input constraints into tightened state and control input constraints of the nominal system; Step 3, construct a disturbance observer, regard the disturbance variable as an extended state quantity, estimate the system disturbance value, and send the estimated value to the auxiliary predictive controller; Step 4, design a nominal model predictive controller, construct a cost function, solve the finite time domain optimization problem, and obtain the nominal optimal control input and the corresponding nominal state; Step 5, design an auxiliary model predictive controller, receive the nominal predicted input and output signals and the disturbance signal provided by the disturbance observer, solve the optimal control for the actual system model, and apply the first control quantity of the control sequence to the autonomous underwater robot system; Step 6, measure the system state at the next moment, take the moment as the new current moment, and return to step 3; the specific steps of constructing the disturbance observer in step 3 are as follows: Step 31, treat the disturbance as a newly added state quantity and add it to the uncertain system, and establish the following linear state observer: The state estimate Output estimate L and H are observer gain matrices, is the disturbance estimate, β is an auxiliary variable; Step 32, calculate the state estimation error and disturbance error: Then for e x The derivative is: Get the first derivative of the perturbation estimate: where the estimate of the derivative of the disturbance is Step 33, the state estimation error and the disturbance estimation error are combined into the following estimation error system: Where G = [e x T e w ] T , represents the estimated error of the derivative of the disturbance; The specific steps of designing a nominal model predictive controller, constructing a cost function, solving a finite time domain optimization problem, and obtaining a nominal optimal control input and a corresponding nominal state in step 4 are as follows: Step 41, design the optimal control input for each sampling moment, drive the nominal system state to the target point and minimize the cost function. The constructed cost function is as follows: where x ref is the target state vector, N p is the prediction time domain, Q and R represent the positive definite weighting matrices of the prediction state and control input, respectively, and P is the terminal weight matrix, which is obtained by solving the Riccati equation; Step 42, satisfying the nominal prediction model constraints as well as the nominal state constraints and the nominal control constraints, wherein the nominal state and the nominal control are respectively constrained within a subset of the actual constraints: The state constraints to be satisfied in the last prediction interval in the prediction horizon: Terminal Constraints satisfy These conditions, among which A K =A+BK, is a robust invariant set; Step 43, express the optimal control problem at any time k in a finite time domain: in, The linear feedback gain K is obtained by a linear quadratic regulator.

2. The anti-disturbance motion control method of an autonomous underwater robot according to claim 1, characterized in that: The tightened state and control input constraints of the nominal system in step 2 are specifically expressed as: Among them, α and γ represent the contraction coefficients in the range of (0,1).

3. The anti-disturbance motion control method of an autonomous underwater robot according to claim 2, characterized in that: In step 5, the following optimization problem is solved by solving the optimal control method for the actual system model: in It is estimated by the disturbance observer.

4. The anti-disturbance motion control method of an autonomous underwater robot according to claim 3, characterized in that: The specific steps of establishing a simplified linear model of the dynamics and kinematics of the autonomous underwater robot in step 1, converting it into a continuous time model in the state space, and discretizing it to obtain a discrete time model of the system are as follows: Step 11, define the required coordinate systems: an inertial coordinate system and a body coordinate system, the inertial coordinate system is fixed to the ground, and the body coordinate system is fixed to the autonomous underwater robot, where E-ξηζ represents the inertial coordinate system and O-xyz represents the body coordinate system; Step 12, define the speed V of the autonomous underwater robot in the body coordinate system in six degrees of freedom = [uvlp qr] T and the position and attitude angle η relative to the fixed coordinate system = [xyz φ θ ψ] T , the motion equation of the autonomous underwater robot is as follows: MV+C(V)V+D(V)V+g(η)=τ Where M is the sum of the rigid body mass and the additional mass matrix, C(V) is the Coriolis force and centripetal force matrix, D(V) is the damping matrix, g(η) is the restoring moment, τ is the propulsion system input matrix, and J(η) is the Jacobian transformation matrix between the inertial coordinate system and the body coordinate system; Step 13, assuming that the relevant state quantities of the horizontal plane, the lateral velocity v, the heading angular velocity p, the rolling angular velocity r, and the rolling angle φ are all approximately 0, the change of the longitudinal velocity u is gentle and stable, and u is approximately a constant. Only the movement of the autonomous underwater robot in the vertical xz plane is considered, the model is decoupled, and a simplified vertical third-order depth control model is established as follows: Where z is the vertical depth in the inertial coordinate system, θ is the pitch angle, q is the pitch angular velocity, δ is the rudder angle, I y is the moment of inertia about the y-axis, M q 、M δ , is the additional mass matrix, W is gravity, B1 is buoyancy, Δ q represents the uncertain part of the model, τ q represents the external time-varying disturbance caused by ocean currents and wind waves; Step 14, using sensors to collect the position information and posture information of the autonomous underwater robot, taking z, θ, q as state quantities, and the rudder angle δ as the control input, x = [z, θ, q] T Used to describe the depth control problem of autonomous underwater robots, the above model is converted into a state space equation: Among them, the state transfer matrix Input Matrix Discretizing the continuous-time state space equation according to the sampling time ΔT yields: x k+1 =Ax k +Bδ k +w k Among them, the status Control Input 5. The anti-disturbance motion control method of an autonomous underwater robot according to claim 4, characterized in that: In step 14, it is assumed that the perturbation is bounded, and the bounded set is a convex set, which is expressed as: where w max is the upper bound of disturbance.

6. A system using the anti-disturbance motion control method of an autonomous underwater robot according to any one of claims 1 to 5, characterized in that: It includes a data acquisition device, a computing unit, an actuator and a driving mechanism. The computing unit is used to carry the above method and control the actuator and the driving mechanism after executing the method. The data acquisition device includes a posture sensor and a depth sensor. The posture sensor is used to collect underwater information, robot posture and inertial navigation information, and perform mean filtering on the information. The depth sensor is used to measure the height distance to the water surface.

Citation Information

Patent Citations

  • Underwater robot path tracking control method based on model predictive control

    CN109343350A

  • Underwater robot depth interval control method and system

    CN111506090A

  • Ocean current disturbance-resistant autonomous underwater robot dynamic positioning method and system

    CN113485390A

  • Method and system for determining a climb profile

    EP3598262A1

  • Underwater sailing body, underwater sailing body controller, and underwater sailing body control method

    JP2015016826A