CN118672287A - Hybrid self-adaptive underwater robot track tracking control method - Google Patents

Abstract

The present invention provides a hybrid adaptive underwater robot trajectory tracking control method, comprising the following steps: S1, establishing a system motion control model based on kinematic equations and dynamic equations; S2, obtaining a nonlinear model predictive controller; S3, inputting the basic control input u _b output by the nonlinear model predictive controller into the L1 controller, and outputting the interference compensation u _L1 ; combining the interference compensation u _L1 with the basic control input u _b to obtain a hybrid adaptive control input input into the system motion control model, and considering unknown interference, the system motion control model outputs the posture state to the L1 controller and the nonlinear model predictive controller for optimization, and instantly compensates for the tracking accuracy error caused by unknown environmental interference and model parameter uncertainty, and finally realizes tracking control of the underwater robot trajectory. The present invention can ensure global stability when performing trajectory tracking motion control on an underwater vehicle.

Description

A hybrid adaptive underwater robot trajectory tracking control method

Technical Field

The present invention relates to the technical field of underwater robot motion control, and in particular to a hybrid adaptive underwater robot trajectory tracking control method.

Background Art

Advances in ocean exploration and increasing competition for marine resources have led to an increase in the detection capabilities and mission complexity of underwater unmanned platforms. Remotely operated underwater vehicles (ROVs) have several key advantages, including excellent safety, robust autonomy, fast search speed, superb maneuverability, and high modularity. However, ROVs usually perform tasks based on operator control commands transmitted via cables. ROVs that lack autonomous control capabilities often perform poorly in typical underwater operations. The complex marine environment also poses significant challenges to the development and application of underwater robots. Therefore, an advanced control system is required to ensure precise motion control in demanding underwater environments. The main obstacles faced by autonomous control systems in this scenario come from highly nonlinear motion, time-varying hydrodynamic effects, and external environmental interference.

Model Predictive Control (MPC) is an optimization-based control technique. This technique has the advantage of handling optimization problems subject to physical constraints. The system model is used to predict future states within a limited prediction horizon and find a series of inputs by defining a cost function. This process is repeated by moving the prediction horizon forward to form a closed-loop control. It can constrain the inputs and states of the control body based on a rolling optimization. Due to its real-time rolling optimization characteristics, it can handle real-time external disturbances to a certain extent. However, from a theoretical point of view, there is still room for improvement in handling disturbances, and it is difficult to achieve high control accuracy when the prediction model parameters are uncertain.

Summary of the invention

In view of this, the purpose of this paper is to propose a hybrid adaptive underwater robot trajectory tracking control method, aiming to overcome the challenges brought by unknown disturbances and model uncertainty. This method is based on in-depth dynamic and kinematic model construction, combined with nonlinear model predictive control (NMPC) and L1 adaptive control (L1AC) technology, to improve the accuracy and robustness of trajectory tracking.

The technical means adopted by the present invention are as follows:

A hybrid adaptive underwater robot trajectory tracking control method comprises the following steps:

S1. Establish a fixed coordinate system and a body coordinate system of the underwater robot; establish kinematic equations and dynamic equations of the underwater robot based on the fixed coordinate system and the body coordinate system; take the state of the underwater robot driving device as input, and establish a system motion control model based on the kinematic equations and dynamic equations;

S2. Combining the given reference trajectory and the system motion control model, the trajectory tracking problem is converted into a rolling optimization problem of a nonlinear model predictive control with input and state constraints, and a nonlinear model predictive controller is obtained;

S3. Input the position information and attitude information into the L1 controller, estimate the impact of unknown environmental interference and model uncertainty on vehicle control, and calculate the interference compensation uL1; combine the interference compensation uL1 with the basic nonlinear model predictive controller input ub to obtain a hybrid adaptive control input, which is input into the system motion control model, and considers unknown interference to calculate and output the vehicle position and attitude state, which are input into the L1 controller and the nonlinear model predictive controller as the initial value for the next iterative calculation, and instantly compensate for the tracking accuracy error caused by unknown environmental interference and model parameter uncertainty, and finally realize the tracking control of the underwater robot trajectory.

Furthermore, S1 specifically includes the following steps:

S101, establishing a fixed coordinate system of the underwater robot and a body coordinate system describing the body motion;

S102, converting between the fixed coordinate system and the body coordinate system by using a rotation matrix to describe the kinematic equation and dynamic equation of the underwater robot;

S103, using CFD method to solve the hydrodynamic coefficient, inertia matrix and Coriolis force matrix of the underwater robot;

S104. Substitute the hydrodynamic coefficient, inertia matrix, and Coriolis force matrix into the kinematic equation and the dynamic equation and construct the dynamic description equation of the system in the form of the Fossen model; add the input of the motion actuator to the dynamic description equation to establish the system motion control model.

Further, in S102, the rotation matrix is as follows:

Among them, Sθ represents sinθ, Cθ represents cosθ, and Tθ represents tanθ.

Furthermore, S2 specifically includes the following steps:

S201, given a reference trajectory, combined with the system motion control model in S1, establish a mathematical model of the model predictive control method;

S202, determine the predictive control parameter matrix of the mathematical model in S201, and establish a model predictive controller through a programming language in combination with the mathematical model in S201; the model predictive controller takes into account input and state constraints, optimizes the trajectory tracking control problem, and obtains a nonlinear model predictive controller.

Further, the nonlinear model predictive controller includes a nonlinear system model, a preset cost function and a numerical optimization solver;

The formula of the nonlinear system model is as follows:

Wherein, f represents the position and velocity information of the underwater vehicle, η represents the position and attitude of the underwater vehicle, and V represents the velocity information;

The formula of the preset cost function is as follows:

f(0)＝f(t ₀ )|u _b (t)|≤u _bMAX ,

in, u _b represents the output value of the underwater robot actuator, f = [η, V] ^T represents the actual system state, f _r = [η _r , V _r ] ^T represents the system state reference value, u _b is the predicted input, Q _p and _Qu are diagonal positive definite weight matrices;

V _r =[ _ur (t)v _r (t)w _r (t)p _r (t)q _r (t)r _r (t)] ^T

The formula of the numerical optimization solver is as follows:

stf(0)＝f(t ₀ )|u _b (t)|≤u _bMAX ,

k ₁ =g(t(n+i-1),f(n+i-1),u _b (n+i-1))

k ₄ =g(t _n+i-1 +h,f _n+i-1 +hk ₃ ,u _b (n+i-1))

Where h is the discrete time interval, f represents the state function of the underwater vehicle, the subscript represents the discrete time step, and k represents the intermediate coefficient of the fourth-order Runge-Kutta method;

The fourth-order Runge-Kutta method is used to determine the state function value of each time step in the prediction domain. After summing, a nonlinear optimization problem about the input value is constructed. The nonlinear optimization solver is used to solve the optimal input value in the prediction domain of the current time step. Through internal optimization, the optimal solution of the cost function is continuously sought within the predicted time interval to obtain the input sequence required for the next time step; this process is repeated until the goal of rolling optimization is achieved.

Furthermore, S3 specifically includes the following steps:

Introducing environmental interference and model uncertainty into the ROV motion model, the formula is as follows:

in, u _b is the input calculated by the nonlinear model predictive controller;

The dynamic equation with position disturbance is derived as follows:

The output value u _L1 of the L1 controller is input into the dynamic equation with position disturbance, as follows:

Wherein, u _L1 ∈R ^6×1 represents the cascade compensation value activated by the L1 controller.

Further, the L1 controller includes a state predictor, an adaptive law and a low-pass filter;

The state predictor formula is as follows:

in, represents the estimated value of the state, is the state error; A _s ∈ R ^6×6 is an adjustable diagonal Hurwitz matrix;

The formula of the adaptive law is as follows:

Where _Ts is the time step, Square matrices are reversible:

For i∈N:

Next, a first-order continuous-time filter C(s) is defined, and the L1AC control law is:

In practice, in discrete time, taking the kth time step as an example, the control rate can be defined as:

Where ω _co is the frequency limit of a properly chosen first-order filter, the L1AC observer in discrete time is given by:

The piecewise constant adaptive law can offset the error in state prediction at the following sample time (i+1)t;

The present invention also provides a storage medium, which includes a stored program, wherein when the program is run, any of the above-mentioned hybrid adaptive underwater robot trajectory tracking control methods is executed.

The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes any of the above-mentioned hybrid adaptive underwater robot trajectory tracking control methods through the operation of the computer program.

Compared with the prior art, the present invention has the following advantages:

The present invention forms an efficient hybrid adaptive control framework by combining the rolling optimization characteristics of nonlinear model predictive control (NMPC) and the instant adaptive adjustment capability of L1AC. Experimental simulation results show that the hybrid control scheme shows superior performance and robustness than the traditional NMPC method under various interference conditions, including ocean currents, wave activities, random interference and model inconsistency. Therefore, the present invention successfully solves the problem of underwater vehicle trajectory tracking in complex environments and demonstrates its great potential in practical applications.

The present invention proposes a new hybrid adaptive nonlinear model predictive controller for ROV trajectory tracking control. The present invention realizes real-time rolling optimization control of ROV trajectory tracking control through nonlinear model predictive control. The present invention uses a cascaded L1 adaptive controller to compensate for environmental disturbances and model uncertainties in real time, and significantly reduces the position error compared with NMPC under different operating conditions. Under the condition of model parameter mismatch, the tracking accuracy of the present invention is still better than that of the NMPC controller with accurate model parameters.

The present invention effectively improves the accuracy and robustness of underwater vehicle trajectory tracking in complex underwater environments, solves the performance limitations of traditional NMPC methods when faced with parameter mismatch and unknown environmental interference, and provides a new solution strategy for trajectory tracking control in complex underwater environments.

BRIEF DESCRIPTION OF THE DRAWINGS

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

FIG1 is a flow chart of the method of the present invention.

FIG. 2 is a coordinate system design diagram of the present invention.

FIG3 is a diagram showing the structure of the L1 controller of the present invention.

FIG. 4 is a diagram showing the trajectory tracking results of the three-dimensional sinusoidal numerical simulation of the present invention.

FIG5 is an X-Y plane projection diagram of the sinusoidal trajectory tracking result of the present invention.

FIG6 is a Z-Y plane projection diagram of the sinusoidal trajectory tracking result of the present invention.

FIG. 7 is an X-Z plane projection diagram of the sinusoidal trajectory tracking result of the present invention.

FIG8 is a graph showing the comparison error between the two control methods under the non-interference working condition of the present invention.

FIG. 9 is a graph showing the comparison error of the two control methods under random interference conditions of the present invention.

FIG. 10 is a graph showing the comparison error of the two control methods under the ocean current condition of the present invention.

FIG. 11 is a comparative error curve diagram of two control methods under wave conditions of the present invention.

FIG. 12 is a graph showing the error comparison between the two control methods under the condition of model uncertainty of the present invention.

FIG13 is a comparison diagram of the average errors of the two methods under various working conditions of the present invention.

DETAILED DESCRIPTION

In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

As shown in FIG1 , the present invention provides a hybrid adaptive underwater robot trajectory tracking control method, comprising the following steps:

S1. Establish system motion control model;

S101. Establishment of the vehicle kinematic equations:

Mathematical modeling of underwater vehicles is based on the hydrodynamic properties of rigid bodies and their corresponding motion characteristics. The modeling process involves static and dynamic studies, the former focusing on the balance of objects at rest or in uniform motion, and the latter focusing on the forces that cause motion. Dynamics is further subdivided into kinematics and dynamics. Kinematics studies more about the geometric relationships of the position, direction, etc. of a moving object, while dynamics focuses more on the analysis of the forces that cause the object to move. By accurately estimating the model parameters and constructing an accurate motion model and external forces, the motion of the ROV can be accurately predicted. The coordinate system of the vehicle is set up as shown in Figure 2. The BlueROV2 open source platform was used in the actual numerical simulation process.

The present invention uses six degrees of freedom to represent the overall motion of a remotely operated vehicle (ROV) and establishes a corresponding coordinate system for description. The above six degrees of freedom are usually used to define the posture information of the ROV in a specified coordinate system frame. The present invention treats the ROV as a rigid body in a mobile coordinate system for research. In order to prepare for further research, this paper defines motion and force as follows, as shown in Table 1:

Table 1 Sign conventions for motion, force and torque

The present invention defines the motion analysis of ROV in two coordinate systems, which are described as follows. First, a fixed earth-fixed coordinate system (NED) is defined, which can also be called an inertial coordinate system, which is a reference frame attached to the ground and moves with the vehicle. When the ROV speed is low, the movement of the earth has limited impact on the ROV. The present invention uses the following symbols to represent the above-mentioned coordinate system frames (Oe, Xe, Ye, Ze). The body-fixed reference frame (BODY) is a mobile reference coordinate system fixed to the ROV body, which is used to describe the motion of the ROV relative to the inertial reference system. The axes of the body-fixed reference frame are consistent with the inertial axes, as shown in Table 2.

Table 2 Coordinate system and speed state representation method

The position and orientation of a vehicle are usually expressed using a ground-fixed coordinate system, while its linear and angular velocities are usually expressed using a body-fixed coordinate system. These values are expressed using the standard notation shown in the table above. The relationship between the two reference frames is described by the coordinate transformation equation, as shown below:

Separating the angular velocity and linear velocity states, we obtain:

The rotation matrix from body coordinates to inertial frame:

Among them, Sθ represents sinθ, Cθ represents cosθ, and Tθ represents tanθ.

S102. Establishment of aircraft dynamics model:

Dynamics focuses on understanding the connection between the forces acting on a remotely operated vehicle (ROV) and its motion. Based on the Newton-Euler method, the dynamic equations for an ideal six-degree-of-freedom (6-DOF) ROV, without considering external disturbances or model uncertainties, are as follows:

Wherein, τ＝[τ ₁ ,τ ₂ ] ^T The above equation is the dynamic equation of ROV under ideal conditions and without considering external environmental interference. The present invention directly considers the uncertainty of thrust and torque in six degrees of freedom and introduces external environmental interference into the thrust and torque of each degree of freedom. Therefore, the equation can be rewritten as follows:

Where V = [v, ω] ^T . After separating the linear acceleration and angular acceleration, the above formula can be rewritten as:

Wherein, M＝ _MA + _MRB , _MRB is the rigid body inertia matrix of ROV, and is the additional mass matrix of the vehicle. Due to the assumptions about the position of the center of buoyancy and the motion characteristics of ROV in this paper, the present invention chooses to ignore the contribution of non-diagonal elements, so there will be:

in:

K ₁ (v)＝C ₁ (ω)ω+D ₁ (ν)ν+g ₁ (η) (10)

K ₂ (ν)＝C ₂ (ν,ω)[ν,ω] ^T +D ₂ (ω)ω+g ₂ (η) (11)

C(v)v is the Coriolis force of the spacecraft; D(v)v is the damping force of the spacecraft; is the environmental disturbance in linear acceleration; ζ = [ζ _x , ζ _y , ζ _z ] ^T is the environmental disturbance in angular acceleration; τ ₁ = [f _X , f _Y , f _Z ] ^T is the thrust provided by the thruster. The ROV in the present invention is a fully driven system that can provide linear acceleration along three coordinate axes and angular acceleration around three coordinate axes. By doing so, it can match the uncertainty and disturbance of the ROV degrees of freedom, defined as:

g(η) is the static water restoring force, assuming that the buoyancy is equal to the gravity. That is, B = mg, where g is the acceleration caused by gravity. In addition, assume that (x _b ,y _b ,z _b ) = (0,0,0) is the buoyancy center coordinate of the ROV in the vehicle frame, which coincides with the center of gravity coordinate. That is:

In general, it can be assumed that the ROV is performing uncoupled motion, then the approximate damping term on the diagonal can be expressed as:

C(v) represents the Coriolis force. It includes the rigid body Coriolis force term and the fluid dynamics Coriolis force term, namely:

C(ν)＝C _RB +C _A (14)

The following expression is obtained by Lagrange parameterization method:

S2. Nonlinear Model Predictive Control:

Nonlinear model predictive control (NMPC) is a control strategy based on numerical optimization. It contains a nonlinear system model, a preset cost function and a numerical optimization solver. By rolling to solve the minimum value of the cost function, the input of the system model is optimized, and the future control input and system response are predicted. The NMPC controller optimizes the future control input within each prediction time range and only applies the first step input to the actual model so that the nonlinear system response is close to the reference value. Without considering external interference and analyzing the dynamic equations of the ROV, the prediction model of the nonlinear system is defined as follows:

For simplicity, the compact mathematical form is described below:

As a prediction model, as mentioned above, the equation does not take into account the influence of the external environment or the uncertainty of the system (since subsequent studies need to consider the uncertainty of the model parameters, an uncertainty coefficient needs to be added). After constructing the system prediction model, it is necessary to construct a system cost function. In this study, the cost function is constructed by taking the difference between the predicted system response and the expected system output, as well as the size of the input value. By minimizing the cost function, the goal of the present invention is to minimize the tracking error and the input value, thereby finding the control input for the next N time steps that meets the constraints, and then applying the first step prediction value to the system control. The expected navigation trajectory is known in advance and is assumed to be smooth and bounded. The time parameter is introduced to define the absolute position of the trajectory. The form of the preset cost function is as follows:

f(0)＝f(t ₀ ),|u _b (t)|≤u _bMAX (18)

in, u _b represents the output value of the ROV actuator and has the same meaning as mentioned above. Here, u _b is expressed in a new form for ease of description; f = [η, V] ^T and f _r = [η _r , V _r ] ^T are the actual system state and reference value. It uses the differential equations given in equations (17) (18) to represent the dynamics of the unified maneuver model; u _b is the prediction input; Q _p and _Qu are diagonal positive definite weight matrices.

V _r =[ _ur (t)v _r (t)w _r (t)p _r (t)q _r (t)r _r (t)] ^T (19)

In the invention, the reference trajectory is pre-designed and expressed in the form of a function of time t to the x-y-z coordinate axis. The integration problem in the above equation is difficult to solve in actual programming. In order to solve the optimization problem in the code, numerical calculations are required. The present invention completes the entire solution process by summing the cost function in a discrete form in the prediction time domain. The present invention evenly divides M nodes in the prediction time domain and calculates the cost function at each time node. The optimal first prediction value is solved by finding the minimum value after summing all node values in the prediction time domain. The cost function J(n) at the nth sampling time can be written as:

stf(0)＝f(t ₀ )|u _b (t)|≤u _bMAX ,

k ₁ =g(t(n+i-1),f(n+i-1),u _b (n+i-1))

k ₄ =g(t _n+i-1 +h,f _n+i-1 +hk ₃ ,u _b (n+i-1)) (20)

Wherein, h is a discrete time interval. Here, the fourth-order Runge-Kutta method is used to determine the state function value at each time step in the prediction domain. After summing, a nonlinear optimization problem about the input value is constructed. Using a nonlinear optimization solver, the optimal input value is solved in the prediction domain of the current time step. Through internal optimization, the present invention continuously searches for the optimal solution of the cost function within the predicted time interval, thereby obtaining the input sequence required for the next time step. This process is repeated until the goal of rolling optimization is achieved.

S3.L1 adaptive control method integration, the composition of L1 controller is shown in Figure 3;

It is challenging to obtain accurate models in real-world situations, especially in complex underwater environments where the motion of the ROV is subject to uncertainties and disturbances, such as propeller dynamics and wind and waves. Therefore, the NMPC control model relies on an idealized description of the ROV motion. In order to take into account the uncertainties and unknown external disturbances of the actual system, the uncertainties are incorporated into the state space representation of the system. The method of the present invention handles these unknown factors by using L1AC. Consider the simplified state variables defined in the above equations (8) and (9). The derivative can be restated as:

in u _b is the input calculated by the nonlinear model predictive controller (NMPC). The environmental disturbance and model uncertainty are introduced into the system through the above formula, and the dynamic equation with position disturbance is derived:

The ROV studied in this paper is a fully driven system, so σ _m ∈ ^{R 6×1} is an uncertainty that can be matched.

L1AC consists of a state predictor, an adaptive law, and a low-pass filter (LPF), as shown in the figure. The state predictor mimics the system structure and replaces unknown uncertainties with estimated values. The output of L1AC is input into the uncertain dynamic equation:

Where u _L1 ∈ R ^6×1 represents the cascade compensation value initiated by L1 control. The predictor in L1AC is constructed as follows:

in, is the state error; A _s ∈ R ^6×6 is an adjustable diagonal Hurwitz matrix. L1 adaptive control is implemented using a nonlinear reference model to estimate the matching uncertainty.

A piecewise constant adaptation rate t∈[iT _s ,(i+1)T _s ) is used.

Where _Ts is the time step, Square matrices are reversible:

For i∈N:

Next, define a first-order continuous-time filter C(s). The L1AC control law is:

In practice, in discrete time, taking the kth time step as an example, the control rate can be defined as:

where ω _co is the frequency limit of a properly chosen first-order filter. The L1AC observer in discrete time is given by:

The piecewise constant adaptive law can offset the error in the state prediction at the following sample time (i+1)t.

According to the above formula, the composition of the L1 adaptive part is shown in Figure 2, which is divided into three parts: low-pass filter, state predictor, and adaptive law.

The present invention considers two types of disturbances: one is the unknown environmental disturbance acting directly on the ROV body, and the other is the model uncertainty acting on the nonlinear model predictive controller. The input initial conditions are calculated by the NMPC controller to obtain the basic control input u _b . Then, u _b is input to the adaptive control part to implement disturbance compensation, and finally a hybrid adaptive control input is obtained, which acts on the ROV model to calculate the attitude state and is provided to the next iteration.

u _all = u _b + u _L1 (33)

For five working conditions, namely, no interference, random interference, ocean current interference, wave interference and model uncertainty, numerical simulation tests were carried out using the trajectory tracking method involved in the present invention, and compared with the traditional NMPC method.

In the numerical simulation experiment, the three-dimensional sinusoidal trajectory set for trajectory tracking is:

Table 3 ROV parameter values

The ROV parameter values are shown in Table 3. The initial setting conditions of the numerical simulation are: Speed: v ₀ = [0, 0, 0, 0, 0] ^T . The first three values of the initial state vector are in meters per second, while the remaining three values are in rad per second. The prediction time horizon N = 5, and the sampling period is the nonlinear model predictive control (NMPC) control input constraints are:

u _max =[50,50,50,50,50,50] ^T

u _min =[-50,-50,-50,-50,-50,-50] ^T

NMPC position error weight matrix:

Q _P =2000×diag(1,1,1,1,1,1)

Input cost function matrix:

Q _u = diag(1,1,1,1,1,1)

In order to conduct the simulation experiment of sinusoidal curve trajectory tracking, the initial posture state of the underwater remote control (ROV) is: S ₀ = [1m, 1m, 1m, 0rad, 0rad, 0rad] ^T . The initial posture state of the underwater remote control (ROV) for the simulation experiment of spiral curve trajectory tracking is:

S ₀ =[1m,1m,9m,0rad,0rad,0rad] ^T .

In order to verify the effectiveness of the L1 and NMPC cascade hybrid adaptive controller, the following simulation conditions are set: no disturbance, current disturbance, random disturbance, wave disturbance and model uncertainty (the coefficients of the NMPC dynamic model are inaccurate). The coefficients of the L1 controller are set as: ω_co = [2, 2, 2, 2, 2, 2] ^T The coefficients of the L1 controller are set as: A _s = -diag (5, 5, 5, 10, 10, 10) where the units of the 6 values of the vector are rad/s.

In this study, all simulations were performed on a laptop equipped with an Intel Core i7-10750H CPU @ 2.60GHz 2.59GHz dual-core processor. The simulations were performed using a simulator based on the MATLAB R2018b platform. A three-dimensional sinusoidal expected trajectory was selected in the present invention. The simulations were performed with and without ocean disturbances. In the simulation experiment design, four types of disturbances were considered, three of which were environmental disturbances, and disturbances expressed as forces and torques were included in the dynamic equations of each degree of freedom. The four types of disturbances include: random disturbances, ocean current disturbances, wave disturbances, and uncertainty in model parameters.

Random interference:

Ocean current disturbance:

Wave disturbance:

The fourth operating condition is the uncertainty of model parameters, that is, there are errors in the control model parameters inside the NMPC. This paper considers the disturbance of 20% uncertainty in the ROV model parameters to the controller.

Based on the above set parameters, numerical simulation experiments were carried out. The trajectory tracking results are shown in Figure 4; Figures 5, 6, and 7 show the projections of the trajectory tracking results in different planes; Figures 8, 9, 10, 11, and 12 respectively show the position error time history curves of the method according to the present invention and the NMPC method under the conditions of no interference, random interference, ocean current interference, wave interference, and model uncertainty interference; Figure 13 shows the average error comparison between the method according to the present invention and the NMPC method under various conditions.

Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein by equivalents. However, these modifications or replacements 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 (9)

1. A hybrid adaptive underwater robot trajectory tracking control method, characterized in that it includes the following steps: S1. Establish a fixed coordinate system and a body coordinate system of the underwater robot; establish kinematic equations and dynamic equations of the underwater robot based on the fixed coordinate system and the body coordinate system; take the state of the underwater robot driving device as input, and establish a system motion control model based on the kinematic equations and dynamic equations; S2. Combining the given reference trajectory and the system motion control model, the trajectory tracking problem is converted into a rolling optimization problem of a nonlinear model predictive control with input and state constraints, and a nonlinear model predictive controller is obtained; S3. Input the position information and attitude information into the L1 controller, estimate the impact of unknown environmental interference and model uncertainty on the control of the vehicle, and calculate the interference compensation u _L1 ; combine the interference compensation u _L1 with the basic nonlinear model predictive controller input u _b to obtain a hybrid adaptive control input, which is input into the system motion control model, and considers unknown interference to calculate and output the vehicle position and attitude state, and input them into the L1 controller and the nonlinear model predictive controller as the initial value for the next iterative calculation, so as to immediately compensate for the tracking accuracy error caused by unknown environmental interference and model parameter uncertainty, and finally realize the tracking control of the underwater robot trajectory. 2. The hybrid adaptive underwater robot trajectory tracking control method according to claim 1 is characterized in that S1 specifically comprises the following steps: S101, establishing a fixed coordinate system of the underwater robot and a body coordinate system describing the body motion; S102, converting between the fixed coordinate system and the body coordinate system by using a rotation matrix to describe the kinematic equation and dynamic equation of the underwater robot; S103, using CFD method to solve the hydrodynamic coefficient, inertia matrix and Coriolis force matrix of the underwater robot; S104. Substitute the hydrodynamic coefficient, inertia matrix, and Coriolis force matrix into the kinematic equation and the dynamic equation and construct the dynamic description equation of the system in the form of the Fossen model; add the input of the motion actuator to the dynamic description equation to establish the system motion control model. 3. The hybrid adaptive underwater robot trajectory tracking control method according to claim 2, characterized in that, in S102, the rotation matrix is as follows: Among them, Sθ represents sinθ, Cθ represents cosθ, and Tθ represents tanθ. 4. The hybrid adaptive underwater robot trajectory tracking control method according to claim 1, characterized in that S2 specifically comprises the following steps: S201, given a reference trajectory, combined with the system motion control model in S1, establish a mathematical model of the model predictive control method; S202, determine the predictive control parameter matrix of the mathematical model in S201, and establish a model predictive controller through a programming language in combination with the mathematical model in S201; the model predictive controller takes into account input and state constraints, optimizes the trajectory tracking control problem, and obtains a nonlinear model predictive controller. 5. The hybrid adaptive underwater robot trajectory tracking control method according to claim 1, characterized in that the nonlinear model predictive controller includes a nonlinear system model, a preset cost function and a numerical optimization solver; The formula of the nonlinear system model is as follows: Wherein, f represents the position and velocity information of the underwater vehicle, η represents the position and attitude of the underwater vehicle, and V represents the velocity information; The formula of the preset cost function is as follows: f(0)＝f(t ₀ )，|u _b (t)|≤u _bMAX in, u _b represents the output value of the underwater robot actuator, f = [η, V] ^T represents the actual system state, f _r = [η _r , V _r ] ^T represents the system state reference value, u _b is the predicted input, Q _p and _Qu are diagonal positive definite weight matrices; V _r =[u _r (t) v _r (t) w _r (t) p _r (t) q _r (t) r _r (t)] ^T The formula of the numerical optimization solver used is as follows: stf(0)＝f(t ₀ )，|u _b (t)|≤u _bMAX k ₁ =g(t(n+i-1),f(n+i-1),u _b (n+i-1)) k ₄ =g(t _n+i-1 +h,f _n+i-1 +hk ₃ ,u _b (n+i-1)) Where h is the discrete time interval, f represents the state function of the underwater vehicle, the subscript represents the discrete time step, and k represents the intermediate coefficient of the fourth-order Runge-Kutta method; The fourth-order Runge-Kutta method is used to determine the state function value of each time step in the prediction domain. After summing, a nonlinear optimization problem about the input value is constructed. The nonlinear optimization solver is used to solve the optimal input value in the prediction domain of the current time step. Through internal optimization, the optimal solution of the cost function is continuously sought within the predicted time interval to obtain the input sequence required for the next time step; this process is repeated until the goal of rolling optimization is achieved. 6. The hybrid adaptive underwater robot trajectory tracking control method according to claim 1, characterized in that S3 specifically comprises the following steps: Introducing environmental interference and model uncertainty into the ROV motion model, the formula is as follows: in, u _b is the input calculated by the nonlinear model predictive controller; The dynamic equation with position disturbance is derived as follows: The output value u _L1 of the L1 controller is input into the dynamic equation with position disturbance, as follows: Wherein, u _L1 ∈R ^6×1 represents the cascade compensation value activated by the L1 controller. 7. The hybrid adaptive underwater robot trajectory tracking control method according to claim 6, characterized in that the L1 controller includes a state predictor, an adaptive law and a low-pass filter; The state predictor formula is as follows: in, represents the estimated value of the state, is the state error; A _s ∈ R ^6×6 is an adjustable diagonal Hurwitz matrix; The formula of the adaptive law is as follows: Where _Ts is the time step, Square matrices are reversible: For i∈N: Next, a first-order continuous-time filter C(s) is defined, and the L1AC control law is: In practice, in discrete time, taking the kth time step as an example, the control rate can be defined as: Where ω _co is the frequency limit of a properly chosen first-order filter, the L1AC observer in discrete time is given by: The piecewise constant adaptive law can offset the error in state prediction at the following sample time (i+1)t; 8. A storage medium, characterized in that the storage medium includes a stored program, wherein when the program is run, the hybrid adaptive underwater robot trajectory tracking control method described in any one of claims 1 to 7 is executed. 9. An electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the hybrid adaptive underwater robot trajectory tracking control method as described in any one of claims 1 to 7 through the computer program.