Underactuated underwater vehicle three-dimensional robust formation control method based on super-spiral observer

Abstract

The invention provides an underactuated underwater vehicle three-dimensional robust formation control method based on a superhelix observer. The method comprises the following steps: step 1, establishing a kinematics model and a dynamics model of an underactuated AUV formation; step 2, defining a directed graph used for describing a communication topological relation among multiple AUVs in the under-actuated AUV formation; 3, based on the fluid coordinate system, designing a distributed cooperative guidance law on the basis of the above steps; 4, introducing a superhelix observer to estimate the composite disturbance of the under-actuated AUV formation; and 5, designing a superhelix dynamic control law based on the compensation corresponding to the composite disturbance, and controlling the AUV formation. According to the method, a distributed cooperative guidance law is designed to realize the cooperation of each AUV on path parameters and expected speed, a superhelix observer is introduced to estimate composite disturbance, a superhelix dynamic control law is designed based on disturbance compensation, and the dynamics is controlled in finite time. Compared with a traditional control method, the method has implementation value in ocean engineering.

Description

Technical Field

[0001] This invention belongs to the field of underwater vehicle technology, specifically relating to a three-dimensional robust formation control method for underactuated underwater vehicles based on a superhelical observer. Background Technology

[0002] With the continuous development of marine technology in various countries, Automatic Underwater Vehicles (AUVs) have been applied to many fields such as underwater target detection, deep-sea resource surveys, and seabed mapping. The developed AUVs possess advantages such as high maneuverability and good stealth. However, due to the limited capabilities of a single AUV, the recently developed technology of utilizing multiple AUVs in formation to perform missions can significantly improve the detection performance of multiple AUVs.

[0003] Multiple AUVs operating in convoy typically encounter two types of disturbances: the first is external environmental interference, such as the impact of wind, waves, and currents on the AUVs; the second is internal AUV disturbances, such as disturbances within the AUV convoy caused by unmodeled dynamics and parameter uncertainties. Since the hydrodynamic models of AUVs are based on commercial fluid dynamics calculation software or tank experiments, model uncertainties are unavoidable. Furthermore, although these two types of disturbances may not coexist, they both negatively impact the stability and effectiveness of the control system, and their values ​​are often impossible to measure directly using sensors. These adverse factors directly affect the accuracy of multi-AUV convoy motion control; therefore, these disturbances need to be addressed in AUV control design.

[0004] In complex marine environments, anti-interference control is of significant engineering importance for the normal navigation of AUVs. Extended state observers (ESAs), as an effective method for estimating and compensating for external disturbances and internal uncertainties, work by treating system uncertainties and disturbances as extended states of the system. The extended state observer then provides a rapid and accurate estimate of these extended states, thereby compensating for the system state. However, because extended state observers cannot adequately adapt to the control of multiple AUVs in marine environments, further optimization of control performance and practicality is needed in current research on anti-interference control for AUVs. Summary of the Invention

[0005] The purpose of this invention is to address the shortcomings of existing AUV anti-interference control technologies by providing a three-dimensional robust formation control method for underactuated underwater vehicles based on a superhelical observer. This method designs a distributed cooperative guidance law based on a fluid coordinate system, combining line-of-sight guidance and a cooperative path parameter update law to achieve coordination among AUVs in path parameters and desired velocities, providing a kinematic basis for AUV formation cooperative path tracking. Furthermore, it introduces a superhelical observer to estimate the composite disturbance composed of external disturbances and model uncertainties within a finite time, and designs a superhelical dynamic control law based on disturbance compensation to achieve finite-time dynamic control. Compared to traditional control methods, this invention exhibits better convergence performance and control accuracy, making it valuable for implementation in marine engineering.

[0006] To achieve the above objectives, the technical solution provided by this invention is:

[0007] A three-dimensional robust formation control method for underactuated underwater vehicles based on a superhelical observer includes:

[0008] Step 1: Establish the kinematic and dynamic models of the underactuated AUV formation; wherein, the kinematic and dynamic models are established by considering the combined disturbances of the external environment and model uncertainties.

[0009] Step 2: Define a directed graph to describe the communication topology among multiple AUVs in an underactuated AUV platoon, represented as:

[0010] Formula (1)

[0011] In formula (1), This represents a directed graph. This represents a set of nodes, where each node represents an AUV in the formation. Represents the set of edges between nodes. Represents the adjacency matrix;

[0012] Step 3: Based on the fluid coordinate system, design a distributed cooperative guidance law based on the above steps;

[0013] Step 4: Introduce a superspiral observer to estimate the composite perturbations of the underactuated AUV formation;

[0014] Step 5: Design a superspiral dynamic control law based on the compensation for the corresponding composite disturbance, and control the AUV formation.

[0015] As a further limitation of the present invention, step one includes:

[0016] A dynamic model of multiple AUVs containing composite perturbations is constructed based on the parameter information of multiple AUVs in the global coordinate system. The composite perturbations are composed of unknown external environmental perturbations and unknown internal uncertainties of the model.

[0017] Based on the first in the body coordinate system A five-degree-of-freedom (DOF) three-dimensional motion model is constructed using the parameter information of an underactuated AUV. The parameter information includes: position data, velocity data, and attitude data of the multiple AUVs; the five degrees of freedom include: based on a fixed coordinate system... To position vector , To position vector , To position vector Pitch angle and yaw angle ;

[0018] Based on the The parameter information of the underdriven AUV is used to simplify the dynamic model and the three-dimensional motion model in the established fluid coordinate system, resulting in kinematic and dynamic models.

[0019] As a further limitation of the present invention, the expression for the multi-AUV dynamics model is as follows:

[0020] Formula (2)

[0021] In formula (2), This indicates the longitudinal displacement of the AUV. This represents the longitudinal velocity of the AUV in the body coordinate system. This indicates the yaw angle of the AUV. This indicates the pitch angle of the AUV. This represents the lateral velocity of the AUV in the body coordinate system. This represents the vertical velocity of the AUV in the body coordinate system. This indicates the lateral displacement of the AUV. This indicates the vertical displacement of the AUV. This represents the pitch angular velocity of the AUV in the body coordinate system; This represents the yaw rate of the AUV in the body coordinate system; This represents the derivative of the AUV's displacement with respect to time. This represents the derivative of the lateral displacement of the AUV with respect to time. This represents the derivative of the vertical and lateral displacement of the AUV with respect to time. Indicates the actual pitch angle of the AUV The derivative with respect to time, This represents the derivative of the AUV's yaw angle with respect to time.

[0022] Formula (3)

[0023] In formula (3), This represents the combined mass and additional mass of the aircraft in its first direction. This represents the derivative of the longitudinal velocity with respect to time. express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances to the channel. This represents the combined mass and additional mass of the aircraft in the second direction. Indicates lateral velocity The derivative with respect to time, express The internal dynamics of the channel This indicates external, unknown environmental disturbances affecting the channel. This represents the combined third-party mass and added mass of the aircraft. Represents vertical velocity The derivative with respect to time, express The internal dynamics of the channel express Unknown external environmental disturbances to the channel. This represents the combined mass and additional mass of the aircraft in the fifth direction. This represents the derivative of the vehicle's pitch rate with respect to time. express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances to the channel. This represents the combined mass and additional mass of the aircraft in the sixth direction. Indicates the yaw rate of the aircraft The derivative with respect to time, express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances affecting the channel;

[0024] The expression for a five-degree-of-freedom three-dimensional motion model is as follows:

[0025] Formula (4)

[0026] In formula (4), Indicates the first The derivative of the displacement of the AUV with respect to time, Represents the first in the moving coordinate system The total speed of the AUV This represents the pitch angle of the AUV in a fixed coordinate system. This represents the yaw angle of the AUV in a fixed coordinate system. Indicates the first The derivative of the lateral displacement of an AUV with respect to time, Indicates the first The derivative of the vertical and lateral displacement of an AUV with respect to time. This represents the reciprocal of the AUV's angle of attack with respect to time. Represents the first in the moving coordinate system The pitch rate of the AUV, Indicates the sideslip angle The derivative with respect to time, Represents the first in the moving coordinate system The yaw rate of an AUV Represents the first in the moving coordinate system The pitch angle of an AUV;

[0027] No. The dynamic model expression for an AUV is:

[0028] Formula (5)

[0029] In formula (5), This represents the combined mass and additional mass of the aircraft in its first direction. Represents the first in the moving coordinate system The derivative of the total speed of the AUV with respect to time, express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances to the channel. Indicates quality parameters, Represents the first in the moving coordinate system Angle of attack of an AUV, Represents the first in the moving coordinate system The sideslip angle of an AUV Represents the first in the moving coordinate system The derivative of the angle of attack of an AUV with respect to time. Represents the first in the moving coordinate system The derivative of the sideslip angle of an AUV with respect to time. This represents the combined mass and additional mass of the aircraft in the fifth direction. This represents the derivative of the vehicle's pitch rate with respect to time. express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances to the channel. This represents the combined mass and additional mass of the aircraft in the sixth direction. Represents the first in the moving coordinate system The yaw rate of an AUV This represents the derivative of the vehicle's yaw rate with respect to time. express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances to the channel.

[0030] As a further limitation of the present invention, step two includes:

[0031] Based on the information of neighboring AUVs, the coordination error is defined, the formation of multiple AUVs is pre-designed and described, and a directed graph of the communication topology between multiple AUVs in the formation is obtained.

[0032] As a further limitation of the present invention, step three includes:

[0033] Based on the fluid coordinate system, the kinematic model and dynamic model established in step one, and the directed graph defined in step two, combined with the cooperative path parameter update law in the line-of-sight guidance and cooperative tracking error system, a distributed cooperative guidance law is designed by constructing a Lyapunov function, and then a cascaded stable cooperative guidance law subsystem is designed; and, based on the directed graph, a cooperative path parameter update law is designed, thus obtaining the cooperative tracking error system;

[0034] Specifically, the method for obtaining the cooperative guidance law subsystem includes:

[0035] (1) Based on the first The three-dimensional spatial reference coordinate system of the underactuated AUVs describes the three-dimensional kinematic model of the underactuated AUV formation, defining the first... The tangent angle in a given path of an underdriven AUV, and the three-dimensional path tracking error are determined based on data information obtained from coordinate transformations between different coordinate systems; wherein, the three-dimensional spatial reference coordinate system includes the Serret-Fernet coordinate system, the fluid coordinate system, and the fixed coordinate system;

[0036] (2) Based on the dynamic three-dimensional path tracking error, design a three-dimensional LOS guidance law corresponding to the distributed cooperative guidance law used in the underactuated AUV path tracking control system, and design guidance command parameters; specifically, the guidance command parameters include: synthesized velocity. Pitch angular velocity yaw rate track angle and azimuth .

[0037] As a further limitation of the present invention, the expression for the tangent angle in a given path is:

[0038] Formula (6)

[0039] In formula (6), Indicates path parameters, Indicates the first The desired pitch angle of an underpowered AUV. Indicates the first The derivative of the desired path vertical displacement of an AUV with respect to parameters. Indicates the first The derivative of the expected path lateral displacement of an AUV with respect to parameters. Indicates the first The derivative of the longitudinal displacement of the AUV along the desired path with respect to the parameters. Indicates the first The expected yaw angle of an underpowered AUV;

[0040] The expression for the 3D path tracking error is:

[0041] Formula (7)

[0042] In formula (7), Indicates the first The derivative of the longitudinal error of a spacecraft with respect to time, Indicates the first Longitudinal error of an AUV Represents the first in the moving coordinate system The total speed of the AUV This represents the yaw angle representing the transformation from the fluid coordinate system to the Serret-Fernet coordinate system. This represents the pitch angle representing the transformation from the fluid coordinate system to the Serret-Fernet coordinate system; Indicates the desired path pitch angle. Indicates the first The derivative of the pitch angle of the expected path of the aircraft with respect to time. The yaw angle represents the desired path. Indicates the first The derivative of the lateral error of a spacecraft with respect to time. Indicates the first The derivative of the lateral error of a spacecraft with respect to time. Indicates the first Lateral error of a ship Indicates the first The derivative of the vertical error of a spacecraft with respect to time. Indicates the first Vertical error of a vessel;

[0043] The expression for the three-dimensional LOS guidance law is:

[0044] Formula (8)

[0045] In formula (8), Indicates the synthesis rate. This represents the control gain corresponding to the first observer. Indicates the first Longitudinal error of an AUV Indicates correspondence Auxiliary parameters of the channel, Represents the first in the moving coordinate system The total speed of the AUV This represents the pitch angle representing the transformation from the fluid coordinate system to the Serret-Fernet coordinate system. Indicates the estimated speed. Indicates the desired speed. Indicates azimuth. The yaw angle represents the desired path. This represents the yaw angle representing the transformation from the fluid coordinate system to the Serret-Fernet coordinate system. Indicates pitch angular velocity, This represents the control gain corresponding to the second observer. Indicates the track angle, Indicates correspondence Auxiliary parameters of the channel, Indicates the track angle, Indicates the desired path pitch angle. Indicates pitch angular velocity error. Indicates acceleration. Indicates the first The yaw rate of an AUV This represents the control gain corresponding to the third observer. Indicates the error in azimuth angle. Indicates correspondence Auxiliary parameters of the channel, Indicates the yaw rate error. Indicates the sideslip angle The derivative with respect to time, Indicates the first The derivative of the azimuth angle of an AUV with respect to time. Indicates path parameters, Indicates a positive constant reference velocity. This indicates the variables to be designed next. Represents the constants for setting related path parameters. Represents auxiliary variables. This represents the constant value set for the auxiliary variables. Indicates path parameter error;

[0046] The expression for the 3D path tracking error is:

[0047] Formula (9)

[0048] In formula (9), Indicates the first The derivative of the longitudinal error of a spacecraft with respect to time, Indicates the first Longitudinal error of an AUV This represents the control gain corresponding to the first observer. Indicates the desired speed. Indicates the first The derivative of the lateral error of a spacecraft with respect to time. This indicates the variables to be designed next. The yaw angle represents the desired path. Indicates correspondence Auxiliary parameters of the channel, Indicates the first The derivative of the pitch angle of the expected path of the aircraft with respect to time. Indicates the first The derivative of the lateral error of a spacecraft with respect to time. Represents the first in the moving coordinate system The total speed of the AUV Indicates correspondence Auxiliary parameters of the channel, Indicates the desired path pitch angle. Indicates the first The derivative of the vertical error of a spacecraft with respect to time. Indicates the first Vertical error of a vessel Indicates the first Lateral error of a ship Indicates correspondence Auxiliary parameters of the channel, This represents the pitch angle representing the transformation from the fluid coordinate system to the Serret-Fernet coordinate system. express The estimation error of the channel observer, Indicates the first The derivative of the trajectory angle error of a vessel with respect to time. Indicates the error in the track angle. This represents the control gain corresponding to the second observer. Indicates correspondence Auxiliary parameters of the channel, express The estimation error of the channel observer, Indicates the first The derivative of the azimuth error of a vessel with respect to time. Indicates azimuth error. This represents the control gain corresponding to the third observer. Indicates correspondence Auxiliary parameters of the channel, express The estimation error of the channel observer, Represents the first in the moving coordinate system The pitch angle of an AUV.

[0049] As a further limitation of the present invention, step four includes:

[0050] Based on the The dynamic model described in step one is rewritten based on the total disturbance experienced by the underactuated AUV. The error dynamics of the superspiral observer and the superspiral observer estimation subsystem are designed with the estimation of the estimated values ​​within a finite time as a condition, and the composite disturbance encountered by the underactuated AUV formation control is obtained by introducing the superspiral observer to estimate the composite disturbance.

[0051] As a further limitation of the present invention, the rewritten dynamic model is in the following form:

[0052] Formula (10)

[0053] In formula (10), Represents the first in the moving coordinate system The derivative of the total speed of the AUV with respect to time, Indicates that AUV is affected Total disturbance of the channel, express Channel control input, This represents the derivative of the vehicle's pitch rate with respect to time. Indicates that AUV is affected Total disturbance of the channel, express Channel control input, Indicates the yaw rate of the aircraft The derivative with respect to time, Indicates that AUV is affected Total disturbance of the channel, express Channel control input;

[0054] The expression for the superhelical observer is:

[0055] Formula (11)

[0056] In formula (11), The derivative of the error in the total velocity of the AUV with respect to time. express The first observer gain of the channel, express The estimation error of the channel observer Indicates that AUV is affected The derivative of the estimated total channel disturbance with respect to time. This represents the combined mass and additional mass of the aircraft in its first direction. express Channel control input, express The second observer gain of the channel, Represents the first in the moving coordinate system The derivative of the pitch rate of an AUV with respect to time. express The first observer gain of the channel, express The estimation error of the channel observer Indicates that AUV is affected The estimated total disturbance of the channel. Indicates that AUV is affected The time derivative of the estimate of the total channel disturbance. This represents the combined mass and additional mass of the aircraft in the fifth direction. express Channel control input, express The first observer gain of the channel, Represents the first in the moving coordinate system The derivative of the error in the yaw rate of an AUV with respect to time. express The first observer gain of the channel, express The estimation error of the channel observer, Indicates that AUV is affected The estimated total disturbance of the channel. express Channel control input, Indicates that AUV is affected The derivative of the total disturbance estimate of the channel with respect to time. express The second observer gain of the channel, This refers to the combined mass and additional mass of the aircraft in the sixth direction.

[0057] The error dynamics of the superspiral observer estimation subsystem are expressed as follows:

[0058] Formula (12)

[0059] In formula (12), express The observation error ensemble term for the channel. express Auxiliary constants of the channel, express The derivative with respect to time, express The system auxiliary matrix of the channel, express The input auxiliary matrix of the channel, express Auxiliary items for the channel;

[0060] Formula (13)

[0061] In formula (13), express The observation error ensemble term for the channel. express The derivative with respect to time, express Auxiliary constants of the channel, express The system auxiliary matrix of the channel, express The input auxiliary matrix of the channel, express Auxiliary items for the channel;

[0062] Formula (14)

[0063] In formula (14), express The observation error ensemble term for the channel. express The derivative with respect to time, express Auxiliary constants of the channel, express The system auxiliary matrix of the channel, express The input auxiliary matrix of the channel, express Auxiliary items for the channel.

[0064] As a further limitation of the present invention, step five includes:

[0065] The compensation for the composite disturbance is determined based on the stability of the sliding surface during the sliding mode control process. Based on this compensation, the superhelical dynamics control law and the error dynamics of the superhelical dynamics control subsystem are designed.

[0066] The above steps are combined to control the AUV formation; specifically, this includes:

[0067] (1) Based on the stability requirements of the closed-loop cascade system composed of the cooperative guidance law subsystem, the superspiral observer estimation subsystem and the superspiral dynamic control subsystem, the desired path of the underactuated AUV formation is given;

[0068] (2) Set external disturbances, configure cooperative path parameter update laws, and obtain compensation for the corresponding composite disturbances, and use the closed-loop cascaded system to allow the first An underdriven AUV starts from a given starting point and moves toward a desired trajectory. During the process of coinciding with the desired trajectory, it controls its actual path to track the desired path well, thereby ensuring that the underdriven AUV formation sails in the desired formation in the presence of external interference and internal disturbances.

[0069] As a further limitation of the present invention, the expression of the superspiral dynamic control law is as follows:

[0070] Formula (15)

[0071] In formula (15), express Channel control input, This represents the combined mass and additional mass of the aircraft in its first direction. Indicates the first AUV itself The first control law parameter of the channel, express Channel speed estimation error, express Uncertainty estimation term for the channel, Indicates the first AUV itself Auxiliary state variables of the channel control law express Desired control parameters for the channel Indicates the first AUV itself The second control law parameter of the channel, express Channel control input, This represents the combined mass and additional mass of the aircraft in the fifth direction. Indicates the first AUV itself The first control law parameter of the channel, Indicates the first AUV itself Uncertainty estimation term for the channel, express Uncertainty estimation term for the channel, express Desired control parameters for the channel Indicates the first AUV itself Auxiliary state variables of the channel control law express The second control law parameter of the channel, express Channel control input, This represents the combined mass and additional mass of the aircraft in the sixth direction. Indicates the first AUV itself The first control law parameter of the channel, express Uncertainty estimation term for the channel, express Desired control parameters for the channel Indicates the first AUV itself Auxiliary state variables of the channel control law Indicates the first AUV itself The second control law parameter of the channel, Indicates the first AUV itself Uncertainty estimation term for the channel;

[0072] The error dynamic expression of the superspiral dynamic control subsystem is:

[0073] Formula (16)

[0074] In formula (16), express The dynamic error integration term of the channel, express The derivative with respect to time, express Related to the passage Auxiliary constants of the term, express Auxiliary parameter matrix of the channel;

[0075] Formula (17)

[0076] In formula (17), express The dynamic error integration term of the channel, express The derivative with respect to time, express Related to the passage Auxiliary constants of the term, express Auxiliary parameter matrix of the channel;

[0077] Formula (18)

[0078] In formula (18), express The dynamic error integration term of the channel, express The derivative with respect to time, express Related to the passage Auxiliary constants of the term, express Auxiliary parameter matrix of the channel.

[0079] The advantages of this invention are:

[0080] This invention designs a distributed cooperative guidance law based on a fluid coordinate system. It combines line-of-sight guidance with a cooperative path parameter update law to achieve coordination among AUVs in path parameters and desired velocities, providing a kinematic basis for AUV formation cooperative path tracking. This invention also introduces a superhelical observer to estimate a composite disturbance composed of external disturbances and model uncertainties within a finite time. Based on this composite disturbance compensation, a superhelical dynamic control law is designed to achieve finite-time dynamic control. Compared with traditional control methods, this invention has better convergence performance and control accuracy, and has practical value in marine engineering.

[0081] Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0082] The above and / or additional aspects and advantages of the present invention will become apparent and readily understood from the description of the embodiments taken in conjunction with the following drawings, in which:

[0083] Figure 1 The present invention provides a flowchart of a three-dimensional robust formation control method for underactuated underwater vehicles based on a superhelical observer;

[0084] Figure 2 The coordinate system definition diagram of the underwater vehicle provided by this invention;

[0085] Figure 3 The present invention provides a diagram illustrating the motion coordinate system of an underwater vehicle.

[0086] Figure 4 The present invention provides a three-dimensional formation control structure diagram for AUVs.

[0087] Figure 5 The AUV communication topology diagram provided by this invention;

[0088] Figure 6 The control design parameter table provided by this invention;

[0089] Figure 7 The present invention provides a three-dimensional formation motion trajectory diagram of AUVs;

[0090] Figure 8 The present invention provides a horizontal motion trajectory diagram of AUV formations.

[0091] Figure 9 The present invention provides a longitudinal plane motion trajectory diagram of AUV formations;

[0092] Figure 10 The path parameter variation diagram provided by this invention;

[0093] Figure 11The disturbance estimation error diagram provided by this invention;

[0094] Figure 12 The AUV position error diagram provided by this invention;

[0095] Figure 13 The AUV velocity error diagram provided by this invention;

[0096] Figure 14 The AUV control input diagram provided by this invention;

[0097] Figure 15 The simulation experiment comparison diagram provided by this invention;

[0098] Figure 16 Comparison of tracking errors of the control algorithm provided by this invention Figure 1 ;

[0099] Figure 17 Comparison of unknown disturbance estimation errors provided by this invention Figure 1 ;

[0100] Figure 18 Comparison of tracking errors of the control algorithm provided by this invention Figure 2 ;

[0101] Figure 19 Comparison of unknown disturbance estimation errors provided by this invention Figure 2 . Detailed Implementation

[0102] The embodiments of the present invention are described in detail below. These embodiments are exemplary and intended to explain the present invention, and should not be construed as limiting the present invention.

[0103] Please see Figure 1 This invention provides a three-dimensional robust formation control method for underactuated underwater vehicles based on a superhelical observer, comprising:

[0104] Step 1: Establish the kinematic and dynamic models of the underactuated AUV formation; the kinematic and dynamic models take into account the complex disturbances consisting of unknown external disturbances and internal model uncertainties.

[0105] Specifically, step one of the embodiments of the present invention includes:

[0106] A dynamic model of multiple AUVs containing composite disturbances is constructed based on the parameter information of multiple AUVs in the global coordinate system. The composite disturbances are composed of unknown external environmental disturbances and unknown internal uncertainties of the model.

[0107] Based on the first in the body coordinate system A five-DOF (degrees of freedom) 3D motion model is constructed using the parameter information of an underactuated AUV. The parameter information includes: position data, velocity data, and attitude data of the multiple AUVs; the five degrees of freedom include: based on a fixed coordinate system... To position vector , To position vector , To position vector track angle and azimuth ;

[0108] Based on the The parameter information of the underdriven AUV is used to simplify the dynamic model and three-dimensional motion model in the established fluid coordinate system, resulting in kinematic and dynamic models.

[0109] For more details, please refer to Figure 2 The coordinate system and vehicle model shown are for the underwater vehicle. Figure 2 In this context, O-xyz is the global coordinate system, with its origin typically fixed at a point on the water surface. The Ox and Oy axes point due east and due south respectively, and are perpendicular to each other. The Oz axis points in the same direction as gravity. This indicates the longitudinal displacement of the AUV. This indicates the lateral displacement of the AUV. This indicates the vertical displacement of the AUV. Indicates the roll angle of the AUV. This indicates the pitch angle of the AUV. Indicates the yaw angle of the AUV; O B -x B y B z B The coordinate system is the body coordinate system, with its origin coinciding with the center of mass of the vehicle. B x B The axis points towards the bow of the vehicle, O B y B The axis points to the starboard side of the aircraft, O B z B Shaft and O B x B Shaft and O B y B The axes form a right-handed coordinate system. This represents the roll rate of the AUV in the body coordinate system. This represents the pitch angle of the AUV in the body coordinate system. This represents the yaw rate of the AUV in the body coordinate system. This represents the longitudinal velocity of the AUV in the body coordinate system. This represents the lateral velocity of the AUV in the body coordinate system. This represents the vertical velocity of the AUV in the body coordinate system.

[0110] To simplify the control model, it is assumed that the structure of each AUV is symmetrical about the principal axis of symmetry. Therefore, this embodiment ignores the roll motion of the AUV, i.e. This leads to the five-degree-of-freedom motion equations of the underactuated AUV in three-dimensional space. In the global coordinate system, using vectors... Indicates the position of the AUV in the global coordinate system, using This represents the attitude angle of the AUV in the global coordinate system. (Vector) and These represent the linear velocity and angular velocity of the AUV in the body coordinate system, respectively.

[0111] Therefore, the expression for the multi-AUV dynamics model described above in this embodiment of the invention is:

[0112] (1)

[0113] In equation (1), This indicates the longitudinal displacement of the AUV. This represents the longitudinal velocity of the AUV in the body coordinate system. This indicates the yaw angle of the AUV. This indicates the pitch angle of the AUV. This represents the lateral velocity of the AUV in the body coordinate system. This represents the vertical velocity of the AUV in the body coordinate system. This indicates the lateral displacement of the AUV. This indicates the vertical displacement of the AUV. This represents the pitch angular velocity of the AUV in the body coordinate system; This represents the yaw rate of the AUV in the body coordinate system; This represents the derivative of the AUV's displacement with respect to time. This represents the derivative of the lateral displacement of the AUV with respect to time. This represents the derivative of the vertical and lateral displacement of the AUV with respect to time. Indicates the actual pitch angle of the AUV The derivative with respect to time, This represents the derivative of the AUV's yaw angle with respect to time.

[0114] (2)

[0115] In equation (2), This represents the combined mass and additional mass of the aircraft in its first direction. This represents the derivative of the longitudinal velocity with respect to time. express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances to the channel. This represents the combined mass and additional mass of the aircraft in the second direction. Indicates lateral velocity The derivative with respect to time, express The internal dynamics of the channel This indicates external, unknown environmental disturbances affecting the channel. This represents the combined third-party mass and added mass of the aircraft. Represents vertical velocity The derivative with respect to time, express The internal dynamics of the channel express Unknown external environmental disturbances to the channel. This represents the combined mass and additional mass of the aircraft in the fifth direction. This represents the derivative of the vehicle's pitch rate with respect to time. express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances to the channel. This represents the combined mass and additional mass of the aircraft in the sixth direction. Indicates the yaw rate of the aircraft The derivative with respect to time, express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances affecting the channel;

[0116] The expression for the above five-degree-of-freedom three-dimensional motion model is as follows:

[0117] (3)

[0118] In equation (3), Indicates the first The derivative of the displacement of the AUV with respect to time, Represents the first in the moving coordinate system The total speed of the AUV This represents the pitch angle of the AUV in a fixed coordinate system. This represents the yaw angle of the AUV in a fixed coordinate system. Indicates the first The derivative of the lateral displacement of an AUV with respect to time, Indicates the first The derivative of the vertical and lateral displacement of an AUV with respect to time. This represents the reciprocal of the AUV's angle of attack with respect to time. Represents the first in the moving coordinate system The pitch rate of the AUV, Indicates the sideslip angle The derivative with respect to time, Represents the first in the moving coordinate system The yaw rate of an AUV Represents the first in the moving coordinate system The pitch angle of an AUV;

[0119] The above-mentioned The dynamic model expression for an AUV is:

[0120] (4)

[0121] In equation (4), This represents the combined mass and additional mass of the aircraft in its first direction. Represents the first in the moving coordinate system The derivative of the total speed of the AUV with respect to time, express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances to the channel. Indicates quality parameters, Represents the first in the moving coordinate system Angle of attack of an AUV, Represents the first in the moving coordinate system The sideslip angle of an AUV Represents the first in the moving coordinate system The derivative of the angle of attack of an AUV with respect to time. Represents the first in the moving coordinate system The derivative of the sideslip angle of an AUV with respect to time. This represents the combined mass and additional mass of the aircraft in the fifth direction. This represents the derivative of the vehicle's pitch rate with respect to time. express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances to the channel. This represents the combined mass and additional mass of the aircraft in the sixth direction. Represents the first in the moving coordinate system The yaw rate of an AUV This represents the derivative of the vehicle's yaw rate with respect to time. express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances to the channel.

[0122] Step 2: Define a directed graph to describe the communication topology among multiple AUVs in the formation, represented as:

[0123] (5)

[0124] In equation (5), This represents a directed graph. This represents a set of nodes, where each node represents an AUV in the formation. Represents the set of edges between nodes. This represents the adjacency matrix.

[0125] Specifically, step two of this embodiment of the invention includes:

[0126] Based on the information of neighboring AUVs, the coordination error is defined, the formation of multiple AUVs is pre-designed and described, and a directed graph of the communication topology between multiple AUVs in the formation is obtained.

[0127] Step 3: Based on the fluid coordinate system, and on the kinematic and dynamic models established in Step 1 and the directed graph defined in Step 2, a distributed cooperative guidance law is designed by combining line-of-sight guidance and cooperative path parameter update law. Among them, the three-dimensional line-of-sight cooperative guidance law realizes the coordination of each AUV in terms of path parameters and desired velocity, providing a kinematic basis for realizing AUV formation cooperative path tracking.

[0128] Specifically, step three in this embodiment of the invention includes:

[0129] Based on the fluid coordinate system, and on the kinematic and dynamic models established in step one and the directed graph defined in step two, combined with the cooperative path parameter update law in the line-of-sight guidance and cooperative tracking error system, a distributed cooperative guidance law is designed by constructing a Lyapunov function, and then a cascaded stable cooperative guidance law subsystem is designed; and, based on the directed graph, a cooperative path parameter update law is designed, thus obtaining the cooperative tracking error system.

[0130] Specifically, the methods for obtaining the cooperative guidance law subsystem include:

[0131] (1) Based on the first The three-dimensional spatial reference coordinate system of the underactuated AUVs describes the three-dimensional kinematic model of the underactuated AUV formation, defining the first... The tangent angle in a given path of an underdriven AUV, and the three-dimensional path tracking error are determined based on data information obtained from coordinate transformations between different coordinate systems; wherein, the three-dimensional spatial reference coordinate system includes the Serret-Fernet coordinate system, the fluid coordinate system, and the fixed coordinate system;

[0132] (2) Based on the dynamic three-dimensional path tracking error, design a three-dimensional LOS guidance law corresponding to the distributed cooperative guidance law used in the underactuated AUV path tracking control system, and design guidance command parameters; specifically, the guidance command parameters include: synthesized velocity. Pitch angular velocity yaw rate track angle and azimuth .

[0133] More specifically, Figure 3 The following is the first A three-dimensional spatial reference coordinate system diagram of an underactuated AUV. This embodiment of the invention uses the Serret-Fernet coordinate system. Fluid coordinate system and fixed coordinate system A three-dimensional motion mathematical model for an underactuated AUV is described.

[0134] Definition of the first The parameterization path for an underdriven AUV is as follows ,in Indicates path parameters. Yaw angle is considered. Pitch angle Around Z respectively E axis and Y E With axis rotation, in this embodiment of the invention, the tangent angle in the given path is represented as:

[0135] (6)

[0136] In equation (6), Indicates path parameters, Indicates the first The desired pitch angle of an underpowered AUV. Indicates the first The derivative of the desired path vertical displacement of an AUV with respect to parameters. Indicates the first The derivative of the expected path lateral displacement of an AUV with respect to parameters. Indicates the first The derivative of the longitudinal displacement of the AUV along the desired path with respect to the parameters. Indicates the first The expected yaw angle of an underpowered AUV;

[0137] This invention defines the coordinate system {F} from the Serret-Fernet coordinate system. i} to a fixed coordinate system {I i The rotation matrix of} is The expression for the above three-dimensional path tracking error is:

[0138] (7)

[0139] In equation (7), Indicates the first The derivative of the longitudinal error of a spacecraft with respect to time, Indicates the first Longitudinal error of an AUV Represents the first in the moving coordinate system The total speed of the AUV This represents the yaw angle representing the transformation from the fluid coordinate system to the Serret-Fernet coordinate system. This represents the pitch angle representing the transformation from the fluid coordinate system to the Serret-Fernet coordinate system; Indicates the desired path pitch angle. Indicates the first The derivative of the pitch angle of the expected path of the aircraft with respect to time. The yaw angle represents the desired path. Indicates the first The derivative of the lateral error of a spacecraft with respect to time. Indicates the first The derivative of the lateral error of a spacecraft with respect to time. Indicates the first Lateral error of a ship Indicates the first The derivative of the vertical error of a spacecraft with respect to time. Indicates the first Vertical error of a vessel;

[0140] The expression for the above three-dimensional LOS guidance law is:

[0141] (8)

[0142] In equation (8), Indicates the synthesis rate. This represents the control gain corresponding to the first observer. Indicates the first Longitudinal error of an AUV Indicates correspondence Auxiliary parameters of the channel, Represents the first in the moving coordinate system The total speed of the AUV This represents the pitch angle representing the transformation from the fluid coordinate system to the Serret-Fernet coordinate system. Indicates the estimated speed. Indicates the desired speed. Indicates azimuth. The yaw angle represents the desired path. This represents the yaw angle representing the transformation from the fluid coordinate system to the Serret-Fernet coordinate system. Indicates pitch angular velocity, This represents the control gain corresponding to the second observer. Indicates the track angle, Indicates correspondence Auxiliary parameters of the channel, Indicates the track angle, Indicates the desired path pitch angle. Indicates pitch angular velocity error. Indicates acceleration. Indicates the first The yaw rate of an AUV This represents the control gain corresponding to the third observer. Indicates the error in azimuth angle. Indicates correspondence Auxiliary parameters of the channel, Indicates the yaw rate error. Indicates the sideslip angle The derivative with respect to time, Indicates the first The derivative of the azimuth angle of an AUV with respect to time Indicates path parameters, Indicates a positive constant reference velocity. This indicates the variables to be designed next. Represents the constants for setting related path parameters. Represents auxiliary variables. This represents the constant value set for the auxiliary variables. Indicates path parameter error;

[0143] The expression for the above three-dimensional path tracking error is:

[0144] (9)

[0145] In equation (9), Indicates the first The derivative of the longitudinal error of a spacecraft with respect to time, Indicates the first Longitudinal error of an AUV This represents the control gain corresponding to the first observer. Indicates the desired speed. Indicates the first The derivative of the lateral error of a spacecraft with respect to time. This indicates the variables to be designed next. The yaw angle represents the desired path. Indicates correspondence Auxiliary parameters of the channel, Indicates the first The derivative of the pitch angle of the expected path of the aircraft with respect to time. Indicates the first The derivative of the lateral error of a spacecraft with respect to time. Represents the first in the moving coordinate system The total speed of the AUV Indicates correspondence Auxiliary parameters of the channel, Indicates the desired path pitch angle. Indicates the first The derivative of the vertical error of a spacecraft with respect to time. Indicates the first Vertical error of a ship Indicates correspondence Auxiliary parameters of the channel, This represents the pitch angle representing the transformation from the fluid coordinate system to the Serret-Fernet coordinate system. Indicates the first Lateral error of a ship express The estimation error of the channel observer Indicates the first The derivative of the trajectory angle error of a vessel with respect to time. Indicates the error in the track angle. This represents the control gain corresponding to the second observer. Indicates correspondence Auxiliary parameters of the channel, express The estimation error of the channel observer Indicates the first The derivative of the azimuth error of a vessel with respect to time. Indicates azimuth error. This represents the control gain corresponding to the third observer. Indicates correspondence Auxiliary parameters of the channel, express The estimation error of the channel observer Represents the first in the moving coordinate system The pitch angle of an AUV.

[0146] Step 4: Introduce a superspiral observer to estimate the complex perturbation of the kinematic and dynamic models, which consists of external disturbances and model uncertainties. The superspiral observer can quickly and accurately complete the complex perturbation consisting of external disturbances and internal uncertainties. The control algorithm based on the superspiral observer can bring the perturbation observation error to zero at a faster speed.

[0147] Step four of this embodiment of the invention includes:

[0148] Based on the The dynamic model of step one is rewritten based on the total disturbance experienced by the underactuated AUV. The error dynamics of the superspiral observer and the superspiral observer estimation subsystem are designed with the estimation of the estimated values ​​within a finite time as a condition. The composite disturbance encountered by the formation control of the underactuated AUV is estimated by introducing the superspiral observer.

[0149] Specifically, the overall structural diagram of the embodiment of the present invention is as follows: Figure 3 As shown, to achieve the pre-designed formation cluster, a cooperative error is defined based on information from neighboring AUVs, and its matrix form is represented using directed graph theory. Within this context, a path parameter update law is designed. for In the formula, Indicates a positive constant reference velocity. This indicates the variables that will be designed next.

[0150] The rewritten dynamic model described above in this embodiment of the invention is in the following form:

[0151] (10)

[0152] In equation (10), Represents the first in the moving coordinate system The derivative of the total speed of the AUV with respect to time, Indicates that AUV is affected Total disturbance of the channel, express Channel control input, This represents the derivative of the vehicle's pitch rate with respect to time. Indicates that AUV is affected Total disturbance of the channel, express Channel control input, Indicates the yaw rate of the aircraft The derivative with respect to time, Indicates that AUV is affected Total disturbance of the channel, express Channel control input.

[0153] The expression for the superhelical observer mentioned above is:

[0154] (11)

[0155] In equation (11), The derivative of the error in the total velocity of the AUV with respect to time. Indicates the total speed of the AUV. express The first observer gain of the channel, express The estimation error of the channel observer, Indicates that AUV is affected The derivative of the estimated total channel disturbance with respect to time. This represents the combined mass and additional mass of the aircraft in its first direction. express Channel control input, express The second observer gain of the channel, Represents the first in the moving coordinate system The derivative of the pitch rate of an AUV with respect to time. express The first observer gain of the channel, express The estimation error of the channel observer, Indicates that AUV is affected The estimated total disturbance of the channel. Indicates that AUV is affected The time derivative of the estimate of the total channel disturbance. This represents the combined mass and additional mass of the aircraft in the fifth direction. express Channel control input, express The first observer gain of the channel, Represents the first in the moving coordinate system The derivative of the error in the yaw rate of an AUV with respect to time. express The first observer gain of the channel, express The estimation error of the channel observer, Indicates that AUV is affected The estimated total disturbance of the channel. express Channel control input, Indicates that AUV is affected The derivative of the total disturbance estimate of the channel with respect to time. express The second observer gain of the channel, This represents the combined mass and additional mass of the aircraft in the sixth direction.

[0156] The error dynamics of the superspiral observer estimation subsystem are expressed as follows:

[0157] (12)

[0158] In equation (12), express The observation error ensemble term for the channel. express Auxiliary constants of the channel, express The derivative with respect to time, express The system auxiliary matrix of the channel, express The input auxiliary matrix of the channel, express Auxiliary items for the channel.

[0159] (13)

[0160] In equation (13), express The observation error ensemble term for the channel. express The derivative with respect to time, express Auxiliary constants of the channel, express The system auxiliary matrix of the channel, express The input auxiliary matrix of the channel, express Auxiliary items for the channel.

[0161] (14)

[0162] In equation (14), express The observation error ensemble term for the channel. express The derivative with respect to time, express Auxiliary constants of the channel, express The system auxiliary matrix of the channel, express The input auxiliary matrix of the channel, express Auxiliary items for the channel.

[0163] Step 5: Design a superspiral dynamics control law based on the compensation of the corresponding composite disturbance, and control the AUV formation through superspiral dynamics.

[0164] Specifically, step five in this embodiment of the invention includes:

[0165] Based on the stability of the sliding surface during sliding mode control, the compensation for the corresponding composite disturbance is determined, and the superhelical dynamics control law and the error dynamics of the superhelical dynamics control subsystem are designed based on the compensation.

[0166] The above steps are combined to control the AUV formation; specifically, this includes:

[0167] (1) Based on the stability requirements of the closed-loop cascade system composed of the cooperative guidance law subsystem, the superspiral observer estimation subsystem and the superspiral dynamic control subsystem, the desired path of the underactuated AUV formation is given;

[0168] (2) Set external disturbances, configure the cooperative path parameter update law, and obtain the compensation for the corresponding composite disturbances. Then, through a closed-loop cascaded system, allow the first... An underdriven AUV starts from a given starting point and moves toward a desired trajectory. During the process of coinciding with the desired trajectory, it controls its actual path to track the desired path well, thereby ensuring that the underdriven AUV formation sails in the desired formation in the presence of external interference and internal disturbances.

[0169] More specifically, the expression for the superspiral dynamic control law is as follows:

[0170] (15)

[0171] In equation (15), express Channel control input, This represents the combined mass and additional mass of the aircraft in its first direction. Indicates the first AUV itself The first control law parameter of the channel, express Channel speed estimation error, express Uncertainty estimation term for the channel, Indicates the first AUV itself Auxiliary state variables of the channel control law express Desired control parameters for the channel Indicates the first AUV itself The second control law parameter of the channel, express Channel control input, This represents the combined mass and additional mass of the aircraft in the fifth direction. Indicates the first AUV itself The first control law parameter of the channel, Indicates the first AUV itself Uncertainty estimation term for the channel, express Uncertainty estimation term for the channel, express Desired control parameters for the channel Indicates the first AUV itself Auxiliary state variables of the channel control law express The second control law parameter of the channel, express Channel control input, This represents the combined mass and additional mass of the aircraft in the sixth direction. Indicates the first AUV itself The first control law parameter of the channel, express Uncertainty estimation term for the channel, express Desired control parameters for the channel Indicates the first AUV itself Auxiliary state variables of the channel control law Indicates the first AUV itself The second control law parameter of the channel, Indicates the first AUV itself Uncertainty estimation term for the channel.

[0172] The error dynamic expression of the superspiral dynamic control subsystem is:

[0173] (16)

[0174] In equation (16), express The dynamic error integration term of the channel, express The derivative with respect to time, express Related to the passage Auxiliary constants of the term, express Auxiliary parameter matrix of the channel.

[0175] (17)

[0176] In equation (17), express The dynamic error integration term of the channel, express The derivative with respect to time, express Related to the passage Auxiliary constants of the term, express Auxiliary parameter matrix of the channel.

[0177] (18)

[0178] In equation (18), express The dynamic error integration term of the channel, express The derivative with respect to time, express Related to the passage Auxiliary constants of the term, express Auxiliary parameter matrix of the channel.

[0179] The stability analysis of the closed-loop cascaded system in the above embodiments is as follows:

[0180] The stability of the closed-loop cascaded system consisting of the three-dimensional cooperative guidance law subsystem, the superspiral observer subsystem, and the superspiral dynamic control law subsystem is given by the following theorem.

[0181] Theorem 1: Under the premise of satisfying Assumption 1, consider the kinematic and dynamic models of an underactuated AUV, the cooperative guidance law, the superhelical observer, and the superhelical dynamic control law, and the input-state stability of the closed-loop cascaded control system.

[0182] Proof: By applying the stability of a closed-loop cascaded system, we can obtain the system's... Channel observer estimation error , Channel observer estimation error , Channel observer estimation error , Channel auxiliary items , Channel auxiliary items , Channel auxiliary items As input, Channel observation error ensemble , Channel observation error ensemble , Channel observation error ensemble As a state, the input-state of the closed-loop cascaded control system is in a steady state. - The following conditions should be met at any time:

[0183] (19)

[0184] In equation (19), express The set of states of each channel within a given time period. express Class function, express Class function, express The set of inputs from each channel within a given time period; if the inputs of the closed-loop cascaded control system are bounded, then all error signals within the closed-loop cascaded control system are ultimately bounded.

[0185] In this embodiment of the invention, a system consisting of three AUVs is preferably selected. The initial conditions for each AUV are set as follows: the initial position of AUV1 is set to... The initial velocity of AUV1 under initial conditions is set as follows: The initial position of AUV2 under initial conditions is set as follows: The initial velocity of AUV2 under initial conditions is set as follows: The initial position of AUV3 under initial conditions is set to The initial velocity of AUV3 under initial conditions is set as follows: The total simulation time of the system is set to 800s, and the simulation step size is set to 0.01s.

[0186] The expected path for a given formation is as follows: , , , .

[0187] Based on the desired path of the formation given above, the parameters of the control law are set as follows in the system simulation: Figure 6 .according to Figure 4 and Figure 5 AUV formation communication Laplace matrix defined in the communication topology diagram ,in: This represents the AUV formation communication Laplace matrix defined in the communication topology diagram. The degree matrix defined by the communication topology graph is represented as: , The adjacency matrix defined by the communication topology graph is represented as: ,but: ;

[0188] Further define the external disturbance in the system simulation as:

[0189] (20)

[0190] In equation (20), express Unknown external disturbances to the channel. express Unknown external disturbances to the channel. express Unknown external disturbances to the channel. express Unknown external disturbances to the channel. express Unknown external disturbances to the channel.

[0191] In the simulation embodiment of the above system of the present invention, see Figures 7-9 As shown, Figure 7 This is a diagram showing the longitudinal plane motion trajectory of the AUV formation. Figure 8 This is a graph showing the changes in path parameters. Figure 9 This is a diagram showing the perturbation estimation error. Figures 7-9 The document presents the actual navigation paths of the AUV formation in three-dimensional space, the horizontal plane, and the vertical plane. The actual navigation paths also show the position of the AUV formation at 100s, 300s, 500s, and 700s. Figures 7-9As can be seen from the simulation, the three AUVs start from a given starting point and move towards the desired trajectory until all three AUVs coincide with the desired trajectory. The actual paths of the three AUVs can all track the desired path well. According to the simulation results, it can be seen that under the condition of a combined disturbance consisting of external disturbances and internal uncertainties, the three-dimensional robust formation control method designed in this embodiment of the invention can enable the AUV formation to track the given path in the desired formation, thereby realizing the AUV formation sailing according to the desired path.

[0192] from Figure 10 As can be seen from the present invention, the cooperative path parameter synchronization update law designed in the embodiments of the present invention is still effective in AUV three-dimensional formation cooperative tasks, which can make the error of the three-dimensional robust formation control parameters of each AUV according to the desired path approach zero, and provide the desired formation position reference for AUV formation tasks in practical applications.

[0193] from Figure 11 As can be seen from the present invention, the superspiral observer designed in this embodiment can quickly and accurately complete the processing of complex disturbances composed of external disturbances and internal uncertainties. It provides corresponding dynamic compensation terms for AUVs in the presence of complex disturbances composed of external disturbances and internal uncertainties, thereby realizing the cooperative path tracking task of AUV formation under complex disturbances and effectively improving the robustness of the AUV formation system.

[0194] from Figure 12 As can be seen, the initial position errors of each AUV are relatively large because the AUVs are far from the initial positions of the desired trajectory. However, as the control law runs, the tracking errors of each AUV quickly converge to zero, and from... Figure 9 As can be seen, the path parameters tend to be consistent over time, which indicates that each AUV will reach the desired position and achieve formation sailing.

[0195] Embodiments of the present invention Figure 13 The speed tracking error curve of the AUV is shown. Because the AUV's initial speed is zero and it is far from the desired trajectory, a large speed error occurs at the beginning of the simulation experiment. However, as the control law runs, the speed error gradually decreases. Figure 13 It can be seen that the velocity error converges to zero in a relatively short time, thus enabling the AUV to reach the desired guidance speed and complete the formation mission.

[0196] Embodiments of the present invention Figure 14The control input of the AUV is given. As can be seen from the figure, the control input is relatively large in the initial stage. This is because the initial velocity of the AUV is zero, which is significantly different from the guidance velocity. The control law generates a large control input to quickly bring the AUV's velocity to the guidance velocity. After the tracking speed error converges, the input curve tends to smooth out, but small fluctuations still exist. This is because a disturbance compensation term for the super-helical observer is added to the control law to counteract the combined disturbances of external disturbances and internal uncertainties on the AUV formation navigation. The simulation results demonstrate the rationality and effectiveness of the designed control law.

[0197] To further verify the advancement of the control algorithm based on the superspiral observer designed in this embodiment of the invention, a simulation experiment was conducted to compare it with the control algorithm based on the Extended State Observer (ESO). In the experiment, to make the experimental results clear, AUV1 was selected as the experimental object, and the motion paths of other AUVs were omitted in the figure. The simulation conditions for AUV1 were the same as the experimental conditions described above.

[0198] 1) Simulation comparison experiment under disturbance: From Figures 15-17 Experimental results show that the control algorithm designed in this embodiment of the invention has a stronger anti-interference ability against complex disturbances than the ESO algorithm, avoids frequent swaying of AUVs under complex disturbances, improves the robustness of the AUV formation control system, and provides strong support for the normal formation navigation mission of AUVs in three-dimensional space.

[0199] 2) Simulation Comparison Experiment Without External Disturbance: To further verify the advancement of the control algorithm designed in this invention and to make the experimental results clearer, this simulation comparison experiment verifies the method designed in the embodiments of this invention and the simulation comparison experiment based on the ESO algorithm under the condition of no external disturbance and only internal disturbance composed of model uncertainty.

[0200] from Figures 18-19 Simulation results show that the control algorithm designed in this embodiment of the invention has a stronger convergence ability for disturbance estimation compared to the Active Disturbance Rejection Algorithm (ESO). Compared to ESO, the super-spiral observer designed in this embodiment can converge the disturbance observation error to zero more quickly, improving the speed of AUV system compensation for unknown disturbances under disturbed conditions and providing support for normal formation navigation missions of AUVs in three-dimensional space.

[0201] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the scope of the technology disclosed in the present invention, and such modifications or substitutions should all be covered within the scope of protection of the present invention.

Claims

1. A three-dimensional robust formation control method for underactuated underwater vehicles based on a superhelical observer, characterized in that, include: Step 1: Establish the kinematic and dynamic models of the underactuated AUV formation; wherein, the kinematic and dynamic models are established by considering the combined disturbances of the external environment and model uncertainties. Step 2: Define a directed graph to describe the communication topology among multiple AUVs in an underactuated AUV platoon, represented as: Formula (1) In formula (1), This represents a directed graph. This represents a set of nodes, where each node represents an AUV in the formation. Represents the set of edges between nodes. Represents the adjacency matrix; Step 3: Based on the fluid coordinate system, design a distributed cooperative guidance law based on the above steps; Step 4: Introduce a superspiral observer to estimate the composite perturbations of the underactuated AUV formation; Step 5: Design a superspiral dynamic control law based on the compensation for the corresponding composite disturbance, and control the AUV formation.

2. The three-dimensional robust formation control method for an underactuated underwater vehicle based on a superhelical observer according to claim 1, characterized in that, Step one includes: A dynamic model of multiple AUVs containing composite perturbations is constructed based on the parameter information of multiple AUVs in the global coordinate system. The composite perturbations are composed of unknown external environmental perturbations and unknown internal uncertainties of the model. Based on the first in the body coordinate system A five-degree-of-freedom (DOF) three-dimensional motion model is constructed using the parameter information of an underactuated AUV. The parameter information includes: position data, velocity data, and attitude data of the multiple AUVs; the five degrees of freedom include: based on a fixed coordinate system... To position vector , To position vector , To position vector Pitch angle and yaw angle ; Based on the The parameter information of the underdriven AUV is used to simplify the dynamic model and the three-dimensional motion model in the established fluid coordinate system, resulting in kinematic and dynamic models.

3. The three-dimensional robust formation control method for an underactuated underwater vehicle based on a superhelical observer according to claim 1, characterized in that, The expression for the multi-AUV dynamics model is: Formula (2) In formula (2), This indicates the longitudinal displacement of the AUV. This represents the longitudinal velocity of the AUV in the body coordinate system. This indicates the yaw angle of the AUV. This indicates the pitch angle of the AUV. This represents the lateral velocity of the AUV in the body coordinate system. This represents the vertical velocity of the AUV in the body coordinate system. This indicates the lateral displacement of the AUV. This indicates the vertical displacement of the AUV. This represents the pitch angular velocity of the AUV in the body coordinate system; This represents the yaw rate of the AUV in the body coordinate system; This represents the derivative of the AUV's displacement with respect to time. This represents the derivative of the lateral displacement of the AUV with respect to time. This represents the derivative of the vertical and lateral displacement of the AUV with respect to time. Indicates the actual pitch angle of the AUV The derivative with respect to time, This represents the derivative of the AUV's yaw angle with respect to time. Formula (3) In formula (3), This represents the combined mass and additional mass of the aircraft in its first direction. This represents the derivative of the longitudinal velocity with respect to time. express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances to the channel. This represents the combined mass and additional mass of the aircraft in the second direction. Indicates lateral velocity The derivative with respect to time, express The internal dynamics of the channel This indicates external, unknown environmental disturbances affecting the channel. This represents the combined third-party mass and added mass of the aircraft. Represents vertical velocity The derivative with respect to time, express The internal dynamics of the channel express Unknown external environmental disturbances to the channel. This represents the combined mass and additional mass of the aircraft in the fifth direction. This represents the derivative of the vehicle's pitch rate with respect to time. express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances to the channel. This represents the combined mass and additional mass of the aircraft in the sixth direction. Indicates the yaw rate of the aircraft The derivative with respect to time, express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances affecting the channel; The expression for a five-degree-of-freedom three-dimensional motion model is as follows: Formula (4) In formula (4), Indicates the first The derivative of the displacement of the AUV with respect to time, Represents the first in the moving coordinate system The total speed of the AUV This represents the pitch angle of the AUV in a fixed coordinate system. This represents the yaw angle of the AUV in a fixed coordinate system. Indicates the first The derivative of the lateral displacement of an AUV with respect to time, Indicates the first The derivative of the vertical and lateral displacement of an AUV with respect to time. This represents the reciprocal of the AUV's angle of attack with respect to time. Represents the first in the moving coordinate system The pitch rate of the AUV, Indicates the sideslip angle The derivative with respect to time, Represents the first in the moving coordinate system The yaw rate of an AUV Represents the first in the moving coordinate system The pitch angle of an AUV; No. The dynamic model expression for an AUV is: Formula (5) In formula (5), This represents the combined mass and additional mass of the aircraft in its first direction. Represents the first in the moving coordinate system The derivative of the total speed of the AUV with respect to time, express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances to the channel. Indicates quality parameters, Represents the first in the moving coordinate system Angle of attack of an AUV, Represents the first in the moving coordinate system The sideslip angle of an AUV Represents the first in the moving coordinate system The derivative of the angle of attack of an AUV with respect to time. Represents the first in the moving coordinate system The derivative of the sideslip angle of an AUV with respect to time. This represents the combined mass and additional mass of the aircraft in the fifth direction. This represents the derivative of the vehicle's pitch rate with respect to time. express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances to the channel. This represents the combined mass and additional mass of the aircraft in the sixth direction. Represents the first in the moving coordinate system The yaw rate of an AUV This represents the derivative of the vehicle's yaw rate with respect to time. express The internal dynamics of the channel express Channel control input, express Unknown external environmental disturbances to the channel.

4. The three-dimensional robust formation control method for an underactuated underwater vehicle based on a superhelical observer according to claim 1, characterized in that, Step two includes: Based on the information of neighboring AUVs, the coordination error is defined, the formation of multiple AUVs is pre-designed and described, and a directed graph of the communication topology between multiple AUVs in the formation is obtained.

5. The three-dimensional robust formation control method for an underactuated underwater vehicle based on a superhelical observer according to claim 1, characterized in that, Step three includes: Based on the fluid coordinate system, the kinematic model and dynamic model established in step one, and the directed graph defined in step two, combined with the cooperative path parameter update law in the line-of-sight guidance and cooperative tracking error system, a distributed cooperative guidance law is designed by constructing a Lyapunov function, and then a cascaded stable cooperative guidance law subsystem is designed; and, based on the directed graph, a cooperative path parameter update law is designed, thus obtaining the cooperative tracking error system; Specifically, the method for obtaining the cooperative guidance law subsystem includes: (1) Based on the first The three-dimensional spatial reference coordinate system of the underactuated AUVs describes the three-dimensional kinematic model of the underactuated AUV formation, defining the first... The tangent angle in a given path of an underdriven AUV, and the three-dimensional path tracking error are determined based on data information obtained from coordinate transformations between different coordinate systems; wherein, the three-dimensional spatial reference coordinate system includes the Serret-Fernet coordinate system, the fluid coordinate system, and the fixed coordinate system; (2) Based on the dynamic three-dimensional path tracking error, design a three-dimensional LOS guidance law corresponding to the distributed cooperative guidance law used in the underactuated AUV path tracking control system, and design guidance command parameters; specifically, the guidance command parameters include: synthesized velocity. Pitch angular velocity yaw rate track angle and azimuth .

6. The three-dimensional robust formation control method for an underactuated underwater vehicle based on a superhelical observer according to claim 5, characterized in that, Given the tangent angle in the path, the expression is: Formula (6) In formula (6), Indicates path parameters, Indicates the first The desired pitch angle of an underpowered AUV. Indicates the first The derivative of the desired path vertical displacement of an AUV with respect to parameters. Indicates the first The derivative of the expected path lateral displacement of an AUV with respect to parameters. Indicates the first The derivative of the longitudinal displacement of the AUV along the desired path with respect to the parameters. Indicates the first The expected yaw angle of an underpowered AUV; The expression for the 3D path tracking error is: Formula (7) In formula (7), Indicates the first The derivative of the longitudinal error of a spacecraft with respect to time, Indicates the first Longitudinal error of an AUV Represents the first in the moving coordinate system The total speed of the AUV This represents the yaw angle representing the transformation from the fluid coordinate system to the Serret-Fernet coordinate system. This represents the pitch angle representing the transformation from the fluid coordinate system to the Serret-Fernet coordinate system; Indicates the desired path pitch angle. Indicates the first The derivative of the pitch angle of the expected path of the aircraft with respect to time. The yaw angle represents the desired path. Indicates the first The derivative of the lateral error of a spacecraft with respect to time. Indicates the first The derivative of the lateral error of a spacecraft with respect to time. Indicates the first Lateral error of a ship Indicates the first The derivative of the vertical error of a spacecraft with respect to time. Indicates the first Vertical error of a vessel; The expression for the three-dimensional LOS guidance law is: Formula (8) In formula (8), Indicates the synthesis rate. This represents the control gain corresponding to the first observer. Indicates the first Longitudinal error of an AUV Indicates correspondence Auxiliary parameters of the channel, Represents the first in the moving coordinate system The total speed of the AUV This represents the pitch angle representing the transformation from the fluid coordinate system to the Serret-Fernet coordinate system. Indicates the estimated speed. Indicates the desired speed. Indicates azimuth. The yaw angle represents the desired path. This represents the yaw angle representing the transformation from the fluid coordinate system to the Serret-Fernet coordinate system. Indicates pitch angular velocity, This represents the control gain corresponding to the second observer. Indicates the track angle, Indicates correspondence Auxiliary parameters of the channel, Indicates the track angle, Indicates the desired path pitch angle. Indicates pitch angular velocity error. Indicates acceleration. Indicates the first The yaw rate of an AUV This represents the control gain corresponding to the third observer. Indicates the error in azimuth angle. Indicates correspondence Auxiliary parameters of the channel, Indicates the yaw rate error. Indicates the sideslip angle The derivative with respect to time, Indicates the first The derivative of the azimuth angle of an AUV with respect to time. Indicates path parameters, Indicates a positive constant reference velocity. This indicates the variables to be designed next. Represents the constants for setting related path parameters. Represents auxiliary variables. This represents the constant value set for the auxiliary variables. Indicates path parameter error; The expression for the 3D path tracking error is: Official(9) In formula (9), Indicates the first The derivative of the longitudinal error of a spacecraft with respect to time, Indicates the first Longitudinal error of an AUV This represents the control gain corresponding to the first observer. Indicates the desired speed. Indicates the first The derivative of the lateral error of a spacecraft with respect to time. This indicates the variables to be designed next. The yaw angle represents the desired path. Indicates correspondence Auxiliary parameters of the channel, Indicates the first The derivative of the pitch angle of the expected path of the aircraft with respect to time. Indicates the first The derivative of the lateral error of a spacecraft with respect to time. Represents the first in the moving coordinate system The total speed of the AUV Indicates correspondence Auxiliary parameters of the channel, Indicates the desired path pitch angle. Indicates the first The derivative of the vertical error of a spacecraft with respect to time. Indicates the first Vertical error of a vessel Indicates correspondence Auxiliary parameters of the channel, This represents the pitch angle representing the transformation from the fluid coordinate system to the Serret-Fernet coordinate system. Indicates the first Lateral error of a ship express The estimation error of the channel observer, Indicates the first The derivative of the trajectory angle error of a vessel with respect to time. Indicates the error in the track angle. This represents the control gain corresponding to the second observer. Indicates correspondence Auxiliary parameters of the channel, express The estimation error of the channel observer, Indicates the first The derivative of the azimuth error of a vessel with respect to time. Indicates azimuth error. This represents the control gain corresponding to the third observer. Indicates correspondence Auxiliary parameters of the channel, express The estimation error of the channel observer, Represents the first in the moving coordinate system The pitch angle of an AUV.

7. A three-dimensional robust formation control method for an underactuated underwater vehicle based on a superhelical observer according to claim 1, characterized in that, Step four includes: Based on the The dynamic model described in step one is rewritten based on the total disturbance experienced by the underactuated AUV. The error dynamics of the superspiral observer and the superspiral observer estimation subsystem are designed with the estimation of the estimated values ​​within a finite time as a condition, and the composite disturbance encountered by the underactuated AUV formation control is obtained by introducing the superspiral observer to estimate the composite disturbance.

8. A three-dimensional robust formation control method for an underactuated underwater vehicle based on a superhelical observer according to claim 7, characterized in that, The rewritten dynamic model is in the following form: Official(10) In formula (10), Represents the first in the moving coordinate system The derivative of the total speed of the AUV with respect to time, Indicates that AUV is affected Total disturbance of the channel, express Channel control input, This represents the derivative of the vehicle's pitch rate with respect to time. Indicates that AUV is affected Total disturbance of the channel, express Channel control input, Indicates the yaw rate of the aircraft The derivative with respect to time, Indicates that AUV is affected Total disturbance of the channel, express Channel control input; The expression for the superhelical observer is: Official(11) In formula (11), The derivative of the error in the total velocity of the AUV with respect to time. Indicates the total speed of the AUV. express The first observer gain of the channel, express The estimation error of the channel observer, Indicates that AUV is affected The derivative of the estimated total channel disturbance with respect to time. This represents the combined mass and additional mass of the aircraft in its first direction. express Channel control input, express The second observer gain of the channel, Represents the first in the moving coordinate system The derivative of the pitch rate of an AUV with respect to time. express The first observer gain of the channel, express The estimation error of the channel observer, Indicates that AUV is affected The estimated total disturbance of the channel. Indicates that AUV is affected The time derivative of the estimate of the total channel disturbance. This represents the combined mass and additional mass of the aircraft in the fifth direction. express Channel control input, express The first observer gain of the channel, Represents the first in the moving coordinate system The derivative of the error in the yaw rate of an AUV with respect to time. express The first observer gain of the channel, express The estimation error of the channel observer, Indicates that AUV is affected The estimated total disturbance of the channel. express Channel control input, Indicates that AUV is affected The derivative of the total disturbance estimate of the channel with respect to time. express The second observer gain of the channel, This refers to the combined mass and additional mass of the aircraft in the sixth direction. The error dynamics of the superspiral observer estimation subsystem are expressed as follows: Formula(12) In formula (12), express The observation error ensemble term for the channel. express Auxiliary constants of the channel, express The derivative with respect to time, express The system auxiliary matrix of the channel, express The input auxiliary matrix of the channel, express Auxiliary items for the channel; Official(13) In formula (13), express The observation error ensemble term for the channel. express The derivative with respect to time, express Auxiliary constants of the channel, express The system auxiliary matrix of the channel, express The input auxiliary matrix of the channel, express Auxiliary items for the channel; Official(14) In formula (14), express The observation error ensemble term for the channel. express The derivative with respect to time, express Auxiliary constants of the channel, express The system auxiliary matrix of the channel, express The input auxiliary matrix of the channel, express Auxiliary items for the channel.

9. A three-dimensional robust formation control method for an underactuated underwater vehicle based on a superhelical observer according to claim 1, characterized in that, Step five includes: The compensation for the composite disturbance is determined based on the stability of the sliding surface during the sliding mode control process. Based on this compensation, the superhelical dynamics control law and the error dynamics of the superhelical dynamics control subsystem are designed. The above steps are combined to control the AUV formation; specifically, this includes: (1) Based on the stability requirements of the closed-loop cascade system composed of the cooperative guidance law subsystem, the superspiral observer estimation subsystem and the superspiral dynamic control subsystem, the desired path of the underactuated AUV formation is given; (2) Set external disturbances, configure cooperative path parameter update laws, and obtain compensation for the corresponding composite disturbances, and use the closed-loop cascaded system to allow the first An underdriven AUV starts from a given starting point and moves toward a desired trajectory. During the process of coinciding with the desired trajectory, it controls its actual path to track the desired path well, thereby ensuring that the underdriven AUV formation sails in the desired formation in the presence of external interference and internal disturbances.

10. A three-dimensional robust formation control method for an underactuated underwater vehicle based on a superhelical observer according to claim 9, characterized in that, The expression for the superspiral dynamic control law is: Official(15) In formula (15), express Channel control input, This represents the combined mass and additional mass of the aircraft in its first direction. Indicates the first AUV itself The first control law parameter of the channel, express Channel speed estimation error, express Uncertainty estimation term for the channel, Indicates the first AUV itself Auxiliary state variables of the channel control law express Desired control parameters for the channel Indicates the first AUV itself The second control law parameter of the channel, express Channel control input, This represents the combined mass and additional mass of the aircraft in the fifth direction. Indicates the first AUV itself The first control law parameter of the channel, Indicates the first AUV itself Uncertainty estimation term for the channel, express Uncertainty estimation term for the channel, express Desired control parameters for the channel Indicates the first AUV itself Auxiliary state variables of the channel control law express The second control law parameter of the channel, express Channel control input, This represents the combined mass and additional mass of the aircraft in the sixth direction. Indicates the first AUV itself The first control law parameter of the channel, express Uncertainty estimation term for the channel, express Desired control parameters for the channel Indicates the first AUV itself Auxiliary state variables of the channel control law Indicates the first AUV itself The second control law parameter of the channel, Indicates the first AUV itself Uncertainty estimation term for the channel; The error dynamic expression of the superspiral dynamic control subsystem is: Official(16) In formula (16), express The dynamic error integration term of the channel, express The derivative with respect to time, express Related to the passage Auxiliary constants of the term, express Auxiliary parameter matrix of the channel; Official(17) In formula (17), express The dynamic error integration term of the channel, express The derivative with respect to time, express Related to the passage Auxiliary constants of the term, express Auxiliary parameter matrix of the channel; Official(18) In formula (18), express The dynamic error integration term of the channel, express The derivative with respect to time, express Related to the passage Auxiliary constants of the term, express Auxiliary parameter matrix of the channel.

Images