A three-dimensional robust formation control method for underactuated underwater vehicles based on super-twisting observer

Abstract

The application provides a kind of three-dimensional robust formation control method of underactuated underwater vehicle based on super-spiral observer, comprising: step one, the kinematic model and the dynamic model of underactuated AUV formation are established;Step two, define the directed graph for describing the communication topological relationship between multiple AUVs in underactuated AUV formation;Step three, based on the fluid coordinate system, design a distributed cooperative guidance law based on the above steps;Step four, introduce super-spiral observer to estimate the complex disturbance of underactuated AUV formation;Step five, based on the compensation of the complex disturbance, design a super-spiral dynamics control law, and control the AUV formation.The application designs a distributed cooperative guidance law to realize the cooperation of each AUV in path parameters and expected speed, introduces a super-spiral observer to estimate the complex disturbance, and designs a super-spiral dynamics control law based on disturbance compensation to realize control in limited time.Compared with the traditional control method, it has implementation value in ocean engineering.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of underwater vehicles, and particularly relates to a three-dimensional robust formation control method for underactuated underwater vehicles based on a super-spiral observer. BACKGROUND

[0002] With the continuous development of marine technology in various countries, automatic underwater vehicles (AUVs) have been applied to underwater target detection, deep-sea resource investigation, and seabed surveying and mapping in multiple fields. The developed AUVs have the advantages of maneuverability, flexibility and good concealment. Since the capacity of a single AUV is limited, the technology of using multiple AUVs to form a formation to perform tasks has been developed recently, which can fully improve the detection performance of multiple AUVs.

[0003] During the process of performing tasks by multiple AUVs, two types of disturbances are usually encountered, including: the first type is external environmental disturbance, such as the impact force of wind, sea waves, ocean currents and the like on the AUV; and the second type is internal disturbance of the AUV, such as the internal disturbance of the AUV formation caused by unmodeled dynamics and parameter uncertainty. Since the hydrodynamic model of the AUV is obtained based on commercial fluid calculation software or pool experiment measurement, the model uncertainty cannot be avoided. Moreover, although the above two types of disturbances may not exist at the same time, they will cause negative effects on the stability and effectiveness of the control system, and often cannot be directly measured by sensors. Based on the above factors, the precision of the motion control of the multiple AUV formation will be directly affected, and therefore the above disturbances need to be handled in the AUV control design.

[0004] In a complex marine environment, anti-disturbance control has important engineering significance for the normal navigation of the AUV. As an effective method for estimating and compensating external disturbances and internal uncertainties, the core idea of the extended state observer is to regard the uncertainties and disturbances of the system as an extended state of the system, and then use the extended state observer to quickly and accurately estimate the extended state, so as to realize the compensation of the system state. Since the extended state observer cannot well adapt to the control of multiple AUVs in the marine environment, the current research on the anti-disturbance control of the AUV still needs to further optimize the control in terms of control performance and practicability. SUMMARY

[0005] The present application aims at solving the deficiencies of the prior art AUV anti-interference control, and provides a three-dimensional robust formation control method for underactuated underwater vehicles based on a super-spiral observer, which is based on a fluid coordinate system to design a distributed cooperative guidance law, which combines a line-of-sight guidance method and a cooperative path parameter updating law to realize the cooperation of each AUV in path parameters and expected speed, and provides a kinematic basis for AUV formation cooperative path tracking. A super-spiral observer is also introduced to estimate the composite disturbance composed of external disturbance and model uncertainty in a limited time, and a super-spiral dynamics control law is designed based on disturbance compensation to realize limited-time dynamics control. Compared with the traditional control method, the present application has better convergence performance and control accuracy, and has implementation value in ocean engineering.

[0006] To achieve the above-mentioned purpose, the technical solution provided by the present application is:

[0007] A three-dimensional robust formation control method for underactuated underwater vehicles based on a super-spiral observer, comprising:

[0008] Step one, establishing a kinematic model and a dynamic model of the underactuated AUV formation; wherein the kinematic model and the dynamic model consider the composite disturbance composed of external environmental disturbance and model uncertainty during the establishment process;

[0009] Step two, defining a directed graph for describing the communication topological relationship among the multiple AUVs in the underactuated AUV formation, represented as:

[0010] Formula (1)

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

[0012] Step three, based on the fluid coordinate system, a distributed cooperative guidance law is designed based on the above step;

[0013] Step four, a super-spiral observer is introduced to estimate the composite disturbance of the underactuated AUV formation;

[0014] Step five, a super-spiral dynamics control law is designed based on the compensation corresponding to the composite disturbance, and the AUV formation is controlled.

[0015] As a further limitation of the present application, the step one comprises:

[0016] The multi-AUV dynamics model containing compound disturbance is constructed based on parameter information of the multi-AUV in a global coordinate system, wherein the compound disturbance is composed of unknown external environmental disturbance and unknown model internal uncertainty.

[0017] The five-degree-of-freedom three-dimensional motion model of the first underactuated AUV is constructed based on parameter information of the multi-AUV in a body coordinate system, wherein the parameter information comprises position data, velocity data and attitude data of the multi-AUV; and the five-degree-of-freedom comprises longitudinal displacement, lateral displacement, vertical displacement, pitch angle and yaw angle of the multi-AUV based on a fixed coordinate system. The position vector , The position vector , The position vector , pitch angle and yaw angle .

[0018] The dynamics model and the three-dimensional motion model are simplified based on parameter information of the first underactuated AUV in the established fluid coordinate system, so as to obtain kinematics and dynamics models.

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

[0020] Formula (2)

[0021] In formula (2), represents longitudinal displacement of the AUV, represents longitudinal velocity of the AUV in the body coordinate system, represents yaw angle of the AUV, represents pitch angle of the AUV, represents lateral velocity of the AUV in the body coordinate system, represents vertical velocity of the AUV in the body coordinate system, represents lateral displacement of the AUV, represents vertical displacement of the AUV, represents pitch angular velocity of the AUV in the body coordinate system; represents yaw angular velocity of the AUV in the body coordinate system; represents derivative of displacement of the AUV with respect to time, represents derivative of lateral displacement of the AUV with respect to time, represents derivative of vertical displacement of the AUV with respect to time, represents derivative of actual pitch angle of the AUV with respect to time, represents derivative of yaw angle of the AUV with respect to time;

[0022] Equation (3)

[0023] In Equation (3), denotes the combined mass of the vehicle in the first direction and the added mass, denotes the derivative of the longitudinal velocity with respect to time, denotes the internal dynamics of the channel, denotes the control input of the channel, denotes the external unknown environmental disturbance of the channel, denotes the combined mass of the vehicle in the second direction and the added mass, denotes the lateral velocity the derivative with respect to time, denotes the internal dynamics of the channel, denotes the external unknown environmental disturbance of the channel, denotes the combined mass of the vehicle in the third direction and the added mass, denotes the vertical velocity the derivative with respect to time, denotes the internal dynamics of the channel, denotes the external unknown environmental disturbance of the channel, denotes the combined mass of the vehicle in the fifth direction and the added mass, denotes the derivative of the vehicle pitch angle velocity with respect to time, denotes the internal dynamics of the channel, denotes the control input of the channel, denotes the external unknown environmental disturbance of the channel, denotes the combined mass of the vehicle in the sixth direction and the added mass, denotes the derivative of the vehicle yaw angle velocity with respect to time, denotes the internal dynamics of the channel, denotes the control input of the channel, denotes the external unknown environmental disturbance of the channel.

[0024] A three-dimensional motion model of five degrees of freedom, expressed as:

[0025] Equation (4)

[0026] In Equation (4), denotes the derivative of the displacement of the denotes the total velocity of the denotes the denotes the derivative of the lateral displacement of the denotes the derivative of the vertical displacement of the denotes the denotes the denotes the denotes the denotes the derivative of the sideslip angle denotes the denotes the denotes the denotes the

[0027] denotes the The dynamics model of the

[0028] Equation (5)

[0029] In Equation (5), denotes the denotes the derivative of the total velocity of the denotes the internal dynamics of the channel, denotes the control input of the channel, denotes the external unknown environmental disturbance of the channel, denotes the denotes the denotes the denotes the denotes the denotes the denotes the 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... A tangent angle in a given path of the underactuated AUV, and a three-dimensional path tracking error determined according to data information obtained through coordinate transformation between different coordinate systems; wherein the three-dimensional space reference coordinate system comprises a Serret-Fernet coordinate system, a fluid coordinate system and a fixed coordinate system;

[0036] (2) According to the dynamic three-dimensional path tracking error, a three-dimensional LOS guidance law applied in the underactuated AUV path tracking control system and corresponding to the distributed cooperative guidance law is designed, and a guidance instruction parameter is designed; specifically, the guidance instruction parameter comprises: a synthetic speed , a pitch angle speed , a yaw angle speed , a track angle and an azimuth angle .

[0037] As a further limitation of the application, the tangent angle in the given path is expressed as:

[0038] Formula (6)

[0039] In formula (6), denotes a path parameter, denotes an expected pitch angle of the first underactuated AUV, denotes a derivative of the expected path vertical displacement of the first AUV with respect to the parameter, denotes a derivative of the expected path lateral displacement of the first AUV with respect to the parameter, denotes a derivative of the expected path longitudinal displacement of the first AUV with respect to the parameter, denotes an expected yaw angle of the first underactuated AUV;

[0040] The expression of the three-dimensional path tracking error is:

[0041] Formula (7)

[0042] In formula (7), denotes a derivative of the longitudinal error of the first vehicle with respect to time, denotes a longitudinal error of the first AUV, denotes a total speed of the first AUV in a moving coordinate system, denotes a conversion yaw angle from the fluid coordinate system to the Serret-Fernet coordinate system, a conversion pitch angle of the fluid coordinate system to the Serret-Fernet coordinate system; a desired path pitch angle, a desired path pitch angle of the first a derivative of the desired path pitch angle of the first a desired path yaw angle, a desired path yaw angle of the first a derivative of the lateral error of the first a derivative of the lateral error of the first a derivative of the lateral error of the first a lateral error of the first a derivative of the vertical error of the first a derivative of the vertical error of the first a derivative of the vertical error of the first a vertical error of the first a vertical error of the first

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

[0044] Equation (8)

[0045] In Equation (8), a resultant velocity, a control gain corresponding to the first observer, a longitudinal error of the first a longitudinal error of the first AUV, an auxiliary parameter corresponding to a channel, a total velocity of the first AUV in the moving coordinate system, a conversion pitch angle of the fluid coordinate system to the Serret-Fernet coordinate system, an estimated velocity, a desired velocity, an azimuth angle, a desired path yaw angle, a conversion yaw angle of the fluid coordinate system to the Serret-Fernet coordinate system, a pitch angle velocity, a control gain corresponding to the second observer, a track angle, an auxiliary parameter corresponding to a channel, a track angle, a desired path pitch angle, a pitch angle velocity error, an 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, denotes the derivative with respect to time of the error in the yaw rate of the AUV, denotes the derivative with respect to time of the error in the yaw rate of the AUV, denotes the first observer gain of the channel, denotes the estimation error of the channel observer, denotes the total disturbance to which the AUV is subjected, the estimation of the total disturbance of the channel, denotes the control input of the channel, the estimation of the total disturbance of the channel, the derivative with respect to time of the estimation of the total disturbance of the channel, denotes the second observer gain of the channel, denotes the combined term of the mass and the added mass of the sixth direction of the vehicle;

[0057] The error dynamic of the super-twisting observer estimation subsystem is represented as:

[0058] Equation (12)

[0059] In Equation (12), denotes the observation error integral term of the channel, denotes the auxiliary constant of the channel, denotes the derivative with respect to time, denotes the system auxiliary matrix of the channel, denotes the input auxiliary matrix of the channel, denotes the auxiliary term of the channel;

[0060] Equation (13)

[0061] In Equation (13), denotes the observation error integral term of the channel, denotes the derivative with respect to time, denotes the auxiliary constant of the channel, denotes the system auxiliary matrix of the channel, denotes the input auxiliary matrix of the channel, denotes the auxiliary term of 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 first control law parameter of the channel, denotes velocity estimation error of the channel, denotes uncertainty estimation term of the channel, denotes the AUV itself auxiliary state variable of the channel control law, denotes desired control parameter of the channel, denotes the AUV itself second control law parameter of the channel, denotes control input of the channel, denotes the AUV itself first control law parameter of the channel, denotes the AUV itself uncertainty estimation term of the channel, denotes uncertainty estimation term of the channel, denotes desired control parameter of the channel, denotes the AUV itself auxiliary state variable of the channel control law, denotes second control law parameter of the channel, denotes control input of the channel, denotes the AUV itself first control law parameter of the channel, denotes uncertainty estimation term of the channel, denotes desired control parameter of the channel, denotes the AUV itself auxiliary state variable of the channel control law, denotes the AUV itself second control law parameter of the channel, denotes the AUV itself first control law parameter of the channel, denotes the an uncertainty estimate term for the channel;

[0072] The error dynamic expression of the super-spiral dynamics control subsystem is:

[0073] Equation (16)

[0074] In Equation (16), represents a dynamic error integration term for the channel, represents a derivative with respect to time, represents an auxiliary constant for the channel related term, represents an auxiliary parameter matrix for the channel;

[0075] Equation (17)

[0076] In Equation (17), represents a dynamic error integration term for the channel, represents a derivative with respect to time, represents an auxiliary constant for the channel related term, represents an auxiliary parameter matrix for the channel;

[0077] Equation (18)

[0078] In Equation (18), represents a dynamic error integration term for the channel, represents a derivative with respect to time, represents an auxiliary constant for the channel related term, represents an auxiliary parameter matrix for the channel.

[0079] The advantages of the present application are:

[0080] The application designs a distributed cooperative guidance law based on fluid coordinates, which combines the line-of-sight guidance method and the cooperative path parameter updating law to realize the cooperation of each AUV in path parameters and expected speed, and provides a kinematic basis for AUV formation cooperative path tracking. The application also introduces a super-spiral observer to estimate the composite disturbance composed of external disturbance and model uncertainty in a limited time, and designs a super-spiral dynamics control law based on the composite disturbance compensation to realize limited-time dynamics control. Compared with the traditional control method, the application has better convergence performance and control accuracy, and has implementation value in ocean engineering.

[0081] Additional aspects and advantages of the application will be set forth in part in the description which follows, and in part will become apparent to those skilled in the art upon examination of the following and / or can be learned by practice of the application. BRIEF DESCRIPTION OF DRAWINGS

[0082] The above and / or additional aspects and advantages of the application will become apparent and be readily understood by considering the following detailed description, from which the singular aspects become apparent.

[0083] Figure 1 The application provides a three-dimensional robust formation control method for underactuated underwater vehicles based on a super-spiral observer.

[0084] Figure 2 The application provides a coordinate system definition diagram of an underwater vehicle.

[0085] Figure 3 The application provides a motion coordinate system diagram of an underwater vehicle.

[0086] Figure 4 The application provides a three-dimensional formation control structure diagram of an AUV.

[0087] Figure 5 The application provides an AUV communication topology diagram.

[0088] Figure 6 The application provides a control design parameter table.

[0089] Figure 7 The application provides a three-dimensional formation motion trajectory diagram of an AUV.

[0090] Figure 8 The application provides a horizontal plane motion trajectory diagram of an AUV formation.

[0091] Figure 9 The application provides a longitudinal plane motion trajectory diagram of an AUV formation.

[0092] Figure 10 The application provides a path parameter change diagram.

[0093] Figure 11The disturbance estimation error diagram is provided in the application;

[0094] Figure 12 The AUV position error diagram is provided in the application;

[0095] Figure 13 The AUV velocity error diagram is provided in the application;

[0096] Figure 14 The AUV control input diagram is provided in the application;

[0097] Figure 15 The simulation experiment comparison diagram is provided in the application;

[0098] Figure 16 The control algorithm tracking error comparison is provided in the application Figure 1 ;

[0099] Figure 17 The unknown disturbance estimation error comparison is provided in the application Figure 1 ;

[0100] Figure 18 The control algorithm tracking error comparison is provided in the application Figure 2 ;

[0101] Figure 19 The unknown disturbance estimation error comparison is provided in the application Figure 2 . DETAILED DESCRIPTION

[0102] The embodiments of the application are described in detail below, which are exemplary and are intended to explain the application, and cannot be understood as a limitation of the application.

[0103] Please refer to Figure 1 The embodiment of the application provides a three-dimensional robust formation control method of an underactuated underwater vehicle based on a super-helix observer, which comprises the following steps:

[0104] Step 1, a kinematics and dynamics model of the underactuated AUV formation is established; wherein the kinematics and dynamics model considers a composite disturbance composed of unknown external disturbances and internal model uncertainties.

[0105] Specifically, the above step 1 of the embodiment of the application comprises:

[0106] A multi-AUV dynamics model containing a composite disturbance is constructed based on the parameter information of the multi-AUV in a global coordinate system, wherein the composite disturbance is a disturbance composed of unknown external environmental disturbances and unknown model internal uncertainties.

[0107] The first 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 indicates 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. denotes the vertical velocity of the AUV in the body-fixed frame.

[0110] To simplify the control model, it is assumed that the structure of each AUV is symmetric about the main symmetry axis, so the embodiment ignores the roll motion of the AUV, i.e. Thus, the five-degree-of-freedom motion equation of the under-actuated AUV in three-dimensional space is obtained. In the global coordinate system, the position of the AUV in the global coordinate system is denoted by the vector , and the attitude angle of the AUV in the global coordinate system is denoted by . The linear velocity and angular velocity of the AUV in the body-fixed frame are respectively denoted by vectors and .

[0111] Therefore, the expression of the above-mentioned multi-AUV dynamics model of the embodiment of the present application is:

[0112] (1)

[0113] In formula (1), denotes the longitudinal displacement of the AUV, denotes the longitudinal velocity of the AUV in the body-fixed frame, denotes the yaw angle of the AUV, denotes the pitch angle of the AUV, denotes the lateral velocity of the AUV in the body-fixed frame, denotes the vertical velocity of the AUV in the body-fixed frame, denotes the lateral displacement of the AUV, denotes the vertical displacement of the AUV, denotes the pitch angular velocity of the AUV in the body-fixed frame; denotes the yaw angular velocity of the AUV in the body-fixed frame; denotes the derivative of the displacement of the AUV with respect to time, denotes the derivative of the lateral displacement of the AUV with respect to time, denotes the derivative of the vertical displacement of the AUV with respect to time, denotes the actual pitch angle of the AUV with respect to time, denotes the derivative of the yaw angle of the AUV with respect to time.

[0114] (2)

[0115] In formula (2), denotes the combined item of the mass and the added mass in the first direction of the vehicle, denotes the derivative of the longitudinal velocity with respect to time, denotes the internal dynamics component of the channel, denotes control input to the channel, denotes external unknown environmental disturbance to the channel, denotes the combined mass and added mass of the vehicle in the second direction, denotes the lateral velocity derivative with respect to time, denotes internal dynamics component of the channel, denotes external unknown environmental disturbance to the channel, denotes the combined mass and added mass of the vehicle in the third direction, denotes the vertical velocity derivative with respect to time, denotes internal dynamics component of the channel, denotes external unknown environmental disturbance to the channel, denotes the combined mass and added mass of the vehicle in the fifth direction, denotes the derivative with respect to time of the pitch angle of the vehicle, denotes internal dynamics component of the channel, denotes control input to the channel, denotes external unknown environmental disturbance to the channel, denotes the combined mass and added mass of the vehicle in the sixth direction, denotes the derivative with respect to time of the yaw angle of the vehicle derivative with respect to time, denotes internal dynamics component of the channel, denotes control input to the channel, denotes external unknown environmental disturbance to the channel;

[0116] the above five degrees of freedom three-dimensional motion model is expressed as:

[0117] (3)

[0118] in equation (3), denotes the first derivative with respect to time of the displacement of the first AUV, denotes the total velocity of the first AUV in the moving coordinate system, denotes the pitch angle of the AUV in the 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, represents external unknown environmental disturbances of the channel, represents the combined term of mass and added mass of the sixth direction of the vehicle, represents the first derivative of the yaw rate of the AUV, represents the derivative of the yaw rate of the vehicle with respect to time, represents internal dynamics of the channel, represents control input of the channel, represents external unknown environmental disturbances of the channel.

[0122] Step two, define a directed graph for describing the communication topological relationship between the multiple AUVs in the formation, denoted as:

[0123] (5)

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

[0125] Specifically, step two of the embodiment of the present application comprises:

[0126] Define a cooperative error according to the information of the neighbor AUV, pre-design and describe the formation of the multiple AUVs, and obtain the directed graph of the communication topological relationship between the multiple AUVs in the formation.

[0127] Step three, based on the fluid coordinate system, design a distributed cooperative guidance law on the basis of the kinematic and dynamic models established in step one and the directed graph defined in step two, in combination with the line-of-sight guidance and the cooperative path parameter update law; wherein the three-dimensional line-of-sight cooperative guidance law realizes the cooperation of each AUV in the path parameter and the expected speed, and provides a kinematic basis for realizing the cooperative path tracking of the AUV formation.

[0128] Specifically, the above-mentioned step three of the embodiment of the present application comprises:

[0129] Based on the fluid coordinate system, design a distributed cooperative guidance law on the basis of the kinematic model and the dynamic model established in step one and the directed graph defined in step two, in combination with the line-of-sight guidance and the cooperative path parameter update law in the cooperative tracking error system, and further design a cascade stable cooperative guidance law subsystem by constructing a Lyapunov function; and design a cooperative path parameter update law based on the directed graph, and further obtain a cooperative tracking error system.

[0130] Specifically, the method for obtaining the cooperative guidance law subsystem comprises:

[0131] (1) based on the first AUV three-dimensional space reference coordinate system describes the three-dimensional kinematic model of the underactuated AUV formation, defines the tangent angle in the given path of the first AUV, and determines the three-dimensional path tracking error according to the data information obtained through the coordinate transformation between different coordinate systems; wherein the three-dimensional space reference coordinate system comprises Serret-Fernet coordinate system, fluid coordinate system and fixed coordinate system;

[0132] (2) according to the dynamic three-dimensional path tracking error, design three-dimensional LOS guidance law applied in the path tracking control system of underactuated AUV and corresponding distributed cooperative guidance law, and design guidance instruction parameters; specifically, the guidance instruction parameters include: synthetic speed , pitch angle velocity , yaw angle velocity , track angle and azimuth angle .

[0133] More specifically, Figure 3 Fig. 1 shows the three-dimensional space reference coordinate system of the first underactuated AUV, and the embodiment of the application adopts Serret-Fernet coordinate system , fluid coordinate system and fixed coordinate system to describe the three-dimensional motion mathematical model of the underactuated AUV.

[0134] The parameterized path of the first underactuated AUV is defined as , wherein represents the path parameter. Considering the yaw angle and the pitch angle rotating around the Z E axis and the Y E axis respectively, the embodiment of the application represents the tangent angle in the given path as:

[0135] (6)

[0136] In formula (6), represents the path parameter, represents the expected pitch angle of the first underactuated AUV, represents the derivative of the expected path vertical displacement of the first AUV with respect to the parameter, 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. K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer, K1represents a control gain corresponding to the first observer,

[0143] The expression of the 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 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. denotes the corresponding auxiliary parameters of the channel, denotes the estimation error of the channel observer, denotes the pitch angle of the AUV.

[0146] Step four, introducing a super-spiral observer to estimate the compound disturbance composed of external disturbance and model uncertainty of the kinematic and dynamic model; wherein the super-spiral observer quickly and accurately completes the observation of the compound disturbance composed of external disturbance and internal uncertainty, and based on the control algorithm of the super-spiral observer, the disturbance observation error can be converged to zero at a faster speed.

[0147] The above step four of the embodiment of the present application comprises:

[0148] Based on the total disturbance suffered by the AUV, the dynamic model of step one is rewritten, and the super-spiral observer and the error dynamics of the super-spiral observer estimation subsystem are designed based on the estimation value in the limited time, so as to obtain the super-spiral observer for estimating the compound disturbance encountered in the formation control of the underactuated AUV.

[0149] Specifically, the overall structure diagram of the embodiment of the present application is shown in Figure 3 In order to realize the pre-designed formation cluster, the cooperative error is defined according to the information of the neighbor AUV, and the matrix form of the cooperative error is represented based on the directed graph theory. The path parameter updating law is designed as , wherein, denotes a positive constant reference speed, denotes a variable to be designed next.

[0150] The rewritten dynamic model of the embodiment of the present application is as follows:

[0151] (10)

[0152] In formula (10), denotes the derivative of the total speed of the AUV with respect to time, denotes the total disturbance suffered by the AUV, the channel, denotes the control input of the channel, denotes the derivative of the pitch angle of the vehicle with respect to time, denotes the total disturbance suffered by the AUV, the channel, denotes the control input of the channel, yaw rate of the vehicle derivative with respect to time, total disturbance to the AUV total disturbance to the channel, control input to the channel.

[0153] The expression of the super-twisting observer is:

[0154] (11)

[0155] In equation (11), derivative with respect to time of the error in the total velocity of the AUV, total velocity of the AUV, first observer gain for the channel, estimation error of the channel observer, total disturbance to the AUV derivative with respect to time of the estimated value of the total disturbance to the channel, combined term of mass and added mass in the first direction of the vehicle, control input to the channel, second observer gain for the channel, derivative with respect to time of the error in the pitch rate of the first AUV in the moving coordinate system, first observer gain for the channel, estimation error of the channel observer, estimated value of the total disturbance to the AUV estimated value of the total disturbance to the AUV derivative with respect to time of the estimated value of the total disturbance to the channel, combined term of mass and added mass in the fifth direction of the vehicle, control input to the channel, first observer gain for the channel, derivative with respect to time of the error in the yaw rate of the first AUV in the moving coordinate system, first observer gain for the channel, estimation error of the channel observer,​​​​​​​​​​​​​​ an estimate of the total disturbance to the AUV by the channel, an estimate of the total disturbance to the AUV by the channel, an estimate of the total disturbance to the AUV by the channel, an estimate of the total disturbance to the AUV by the channel, an estimate of the total disturbance to the AUV by

[0156] The error dynamics of the super-twisting observer estimation subsystem are represented as:

[0157] (12)

[0158] In equation (12), an estimate of the total disturbance to the AUV by the channel, an estimate of the total disturbance to the AUV by the channel, an estimate of the total disturbance to the AUV by the channel, an estimate of the total disturbance to the AUV by the channel, an estimate of the total disturbance to the AUV by the channel, an estimate of the total disturbance to the AUV by the channel.

[0159] (13)

[0160] In equation (13), an estimate of the total disturbance to the AUV by the channel, an estimate of the total disturbance to the AUV by the channel, an estimate of the total disturbance to the AUV by the channel, an estimate of the total disturbance to the AUV by the channel, an estimate of the total disturbance to the AUV by the channel, an estimate of the total disturbance to the AUV by the channel.

[0161] (14)

[0162] In equation (14), an estimate of the total disturbance to the AUV by the channel, an estimate of the total disturbance to the AUV by the channel, denotes auxiliary constant of the channel, denotes system auxiliary matrix of the channel, denotes input auxiliary matrix of the channel, denotes auxiliary term of the channel.

[0163] Step five, design the super-spiral dynamics control law based on the compensation of the corresponding compound disturbance, and control the AUV formation through the super-spiral dynamics.

[0164] Specifically, the step five of the embodiment of the present application comprises:

[0165] According to the stability of the sliding surface in the process of the sliding mode control, the compensation of the corresponding compound disturbance is determined, and the super-spiral dynamics control law and the error dynamics of the super-spiral dynamics control subsystem are designed based on the compensation.

[0166] The AUV formation is controlled according to the above steps; specifically, it comprises:

[0167] (1) based on the stability requirement of the closed-loop cascade system composed of the cooperative guidance law subsystem, the super-spiral observer estimation subsystem and the super-spiral dynamics control subsystem, the desired path of the underactuated AUV formation is given;

[0168] (2) set the external disturbance, configure the cooperative path parameter update law, and obtain the compensation of the corresponding compound disturbance, so that the first underactuated AUV moves from the given starting point to the desired trajectory through the closed-loop cascade system, and the actual path coincides with the desired trajectory during the process of tracking the desired path, so as to ensure that the underactuated AUV formation sails in the desired formation under the condition of existing external disturbance and internal disturbance.

[0169] More specifically, the expression of the super-spiral dynamics control law is:

[0170] (15)

[0171] In formula (15), denotes control input of the channel, denotes the combined term of the mass and the added mass in the first direction of the vehicle, denotes the first control law parameter of the AUV itself channel, denotes denotes velocity estimation error of the channel, denotes uncertainty estimation term of the channel,​ represents the first AUV itself auxiliary state variable of the channel control law, represents the first desired control parameter of the channel, represents the first AUV itself second control law parameter of the channel, represents the first control input of the channel, represents the combined term of mass and added mass in the fifth direction of the vehicle, represents the first AUV itself first control law parameter of the channel, represents the first AUV itself uncertainty estimation term of the channel, represents the first uncertainty estimation term of the channel, represents the first desired control parameter of the channel, represents the first AUV itself auxiliary state variable of the channel control law, represents the first second control law parameter of the channel, represents the first control input of the channel, represents the combined term of mass and added mass in the sixth direction of the vehicle, represents the first AUV itself first control law parameter of the channel, represents the first uncertainty estimation term of the channel, represents the first desired control parameter of the channel, represents the first AUV itself auxiliary state variable of the channel control law, represents the first AUV itself second control law parameter of the channel, represents the first AUV itself uncertainty estimation term of the channel.

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

[0173] (16)

[0174] in equation (16), denotes a power error integral term for the channel, denotes a derivative with respect to time, denotes a channel-dependent auxiliary constant for the term, denotes an auxiliary parameter matrix for the channel.

[0175] (17)

[0176] in equation (17), denotes a power error integral term for the channel, denotes a derivative with respect to time, denotes a channel-dependent auxiliary constant for the term, denotes an auxiliary parameter matrix for the channel.

[0177] (18)

[0178] in equation (18), denotes a power error integral term for the channel, denotes a derivative with respect to time, denotes a channel-dependent auxiliary constant for the term, denotes an auxiliary parameter matrix for the channel.

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

[0180] The stability of the closed-loop cascade system composed of the three-dimensional cooperative guidance law subsystem, the hyper-spiral observer subsystem and the hyper-spiral dynamics control law subsystem is given by the following theorem.

[0181] Theorem 1: Under the premise of satisfying the assumption 1, considering the kinematics and dynamics model of the under-actuated AUV, the cooperative guidance law, the hyper-spiral observer, the hyper-spiral dynamics control law, the input-state stability of the closed-loop cascade control system composed of the above is considered.

[0182] Proof: The stability of the closed-loop cascade system is applied to obtain that the system is the estimation error of the channel observer , Estimation error of channel observers , Estimation error of channel observers , Auxiliary term of channel , Auxiliary term of channel , Auxiliary term of channel As input, the observation error integration term of channel is Observation error integration term of channel , Observation error integration term of channel , Observation error integration term of channel As state, the input-state of the closed-loop cascade control system composed of the above is in stable state, which should meet the following condition at time -

[0183] (19)

[0184] In formula (19), represents the set of states of each channel at time represents the function of represents the function of represents the set of inputs of each channel at time If the input of the closed-loop cascade control system is bounded, it can be obtained that all error signals in the closed-loop cascade control system are ultimately bounded.

[0185] The embodiment of the application preferentially selects a system composed of three AUVs. The initial conditions of each AUV are set as follows: the initial position of AUV1 is set as , the initial velocity of AUV1 is set as ; the initial position of AUV2 is set as , the initial velocity of AUV2 is set as , the initial position of AUV3 is set as , and the initial velocity of AUV3 is set as . The total simulation time of the system is set as 800s, and the simulation step of the system is set as 0.01s.

[0186] The expected path of the given formation is as follows: , , ,​​​​ .

[0187] According to the expected path of the platoon given above, the parameters of the control law in the simulation of the system are set as Figure 6 . According to the AUV platoon communication Laplacian matrix defined by the communication topology graph in Figure 4 and Figure 5 wherein: represents the AUV platoon communication Laplacian matrix defined by the communication topology graph, represents the degree matrix defined by the communication topology graph, and is represented as: , represents the adjacency matrix defined by the communication topology graph, and is represented as: , then: ;

[0188] Further, the external disturbance in the simulation of the system is set as:

[0189] (20)

[0190] In formula (20), represents the external unknown disturbance of the channel, represents the external unknown disturbance of the channel, represents the external unknown disturbance of the channel, represents the external unknown disturbance of the channel, represents the external unknown disturbance of the channel.

[0191] In the simulation embodiment of the system of the present application, as shown in Figures 7-9 , Figure 7 is a longitudinal plane motion trajectory diagram of the AUV platoon, Figure 8 is a path parameter change diagram, Figure 9 is a disturbance estimation error diagram, Figures 7-9 the actual navigation path of the AUV platoon in the three-dimensional space, the horizontal plane and the longitudinal plane is given in Figures 7-9 ​As can be seen from the simulation, the three AUVs in the simulation move from the given starting point to the desired trajectory until the three AUVs coincide with the desired trajectory, and the actual paths of the three AUVs can track the desired path well. According to the simulation results, it can be seen that in the case of the composite disturbance composed of external disturbance and internal uncertainty, the three-dimensional robust formation control method designed in the embodiment of the application 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 It can be known that the cooperative path parameter synchronous updating law designed in the embodiment of the application is still effective in the AUV three-dimensional formation cooperative task, and can enable each AUV to approach to zero according to the error of the three-dimensional robust formation control parameter of the desired path, thereby providing a desired formation position reference for the AUV formation task in actual application.

[0193] From Figure 11 It can be known that the super-spiral observer designed in the embodiment of the application can quickly and accurately complete the composite disturbance composed of external disturbance and internal uncertainty. The corresponding dynamic compensation term is provided for the AUV in the case of the composite disturbance composed of external disturbance and internal uncertainty, thereby realizing the AUV formation cooperative path tracking task under the composite disturbance, and effectively improving the robustness of the AUV formation system.

[0194] From Figure 12 As can be seen from the simulation, the position error of each AUV is large because the initial position distance of the AUV from the desired trajectory is far. However, with the operation of the control law, the tracking error of each AUV converges to zero quickly, and it can be known from Figure 9 that the path parameters also tend to be consistent with time, so that each AUV will reach the desired position and realize formation sailing.

[0195] The embodiment of the application Figure 13 The velocity tracking error curve of the AUV is shown, and because the initial velocity of the AUV is zero and the distance from the desired trajectory is far, a large velocity error is generated in the initial stage of the simulation experiment, but with the operation of the control law, it can be seen from Figure 13 that the velocity error converges to zero in a relatively short time, so that the AUV reaches the desired guidance velocity and completes the formation task.

[0196] The embodiment of the application Figure 14The control input of the AUV is given, and it can be seen from the figure that the control input is large in the initial stage, which is because the initial speed of the AUV is zero and the difference between the initial speed and the guide speed is large, and a large control input is generated according to the control law to make the speed of the AUV quickly reach the guide speed. When the tracking speed error converges, the input curve tends to be smooth, but there are still small fluctuations, because the disturbance compensation term of the super-spiral observer is added in the control law to resist the influence of the combined disturbance composed of external disturbance and internal uncertainty on the AUV formation navigation. The above simulation results show the rationality and effectiveness of the designed control law.

[0197] In order to further verify the advancement of the control algorithm based on the super-spiral observer designed in the embodiment of the present application, simulation experiments are compared with the control algorithm based on the extended state observer (ESO). In the experiment, in order to make the experimental results clear and visible, AUV1 is selected as the experimental object, and the motion paths of other AUVs are omitted in the figure, and the simulation conditions of AUV1 are the same as the above experimental conditions.

[0198] 1) Simulation comparison experiment under disturbance: from Figures 15-17 It can be found from the experimental results that the anti-interference ability of the control algorithm designed in the embodiment of the present application to the combined disturbance is stronger than that of the ESO algorithm, the frequent swing of the AUV under the combined disturbance is avoided, the robustness of the AUV formation control system is improved, and strong support is provided for the normal formation navigation task of the AUV in the three-dimensional space.

[0199] 2) Simulation comparison experiment without external disturbance: in order to further verify the advancement of the control algorithm designed in the present application, make the experimental results more clear and visible. The simulation comparison experiment verifies the comparison simulation experiment between the method designed in the embodiment of the present application and the ESO algorithm under the condition of no external disturbance and only internal disturbance composed of model uncertainty.

[0200] From the simulation experiment results of Figures 18-19 It can be found from the simulation experiment results that the control algorithm designed in the embodiment of the present application has stronger convergence ability for disturbance estimation than the active disturbance rejection algorithm ESO. Compared with the active disturbance rejection algorithm ESO, the super-spiral observer designed in the embodiment of the present application can make the disturbance observation error converge to zero at a faster speed, improve the rapidity of the AUV system in compensating for unknown disturbance under disturbance conditions, and provide support for the normal formation navigation task of the AUV in the three-dimensional space.

[0201] The above is only a specific embodiment of the present application, but the protection scope of the present application is not limited thereto, and any person skilled in the art can easily think of various equivalent modifications or replacements within the technical range disclosed in the present application, and these modifications or replacements should be covered within the protection scope of the present application.

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 considering a composite disturbance consisting of external environmental disturbances and model uncertainties; specifically including: A multi-AUV dynamic model containing composite perturbations is constructed based on the parameter information of multiple AUVs in the global coordinate system. The composite perturbation is composed of unknown external environmental perturbations and unknown internal model uncertainties. The kinematic model and the dynamic model are constructed by considering the composite perturbation composed of external environmental perturbations and internal model uncertainties. 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 underactuated AUV is used to simplify the dynamic model of the multi-AUV and the three-dimensional motion model in the fluid coordinate system to obtain the kinematic and dynamic models; Step 2: Define a directed graph to describe the communication topology among multiple AUVs in an underactuated AUV platoon, represented as: Official (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; specifically including: 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. Step 3: Design a distributed cooperative guidance law based on the fluid coordinate system; specifically including: 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-Frenet 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 ; Step 4: Introduce a superspiral observer to estimate the composite perturbations of the underactuated AUV formation; specifically including: Based on the The total disturbance experienced by the underactuated AUV is rewritten from the dynamic model described in step one, and the first step is estimated in a finite time interval. The estimated value in the motion of an underdriven AUV is used to design the error dynamics of the superspiral observer and the superspiral observer estimation subsystem, and to obtain the estimated composite disturbance of the underdriven AUV formation by introducing the superspiral observer. 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, The expression for the multi-AUV dynamics model is: Official (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. Official (3) In formula (3), This represents the combined mass and added mass in the first direction of the AUV. This represents the derivative of the longitudinal velocity with respect to time. Indicates and The longitudinal velocity variable corresponds to the internal dynamic composition of the channel. express Channel control input, Indicates and The longitudinal velocity variable corresponds to the unknown external environmental disturbances of the channel. This represents the combined mass and added mass in the second direction of the AUV. Indicates lateral velocity The derivative with respect to time, Indicates and The lateral velocity variable corresponds to the internal dynamics of the channel. This indicates unknown external environmental disturbances affecting the channel. This represents the combined term of AUV third-party quality and added quality. Represents vertical velocity The derivative with respect to time, Indicates and The vertical velocity variable corresponds to the internal dynamics of the channel. Indicates and The vertical velocity variable corresponds to the unknown external environmental disturbances of the channel. This represents the combined mass and added mass in the fifth direction of the AUV. This represents the derivative of the AUV's pitch velocity with respect to time. Indicates and The pitch angular velocity variable corresponds to the internal dynamics of the channel. Indicates and The control input for the channel corresponding to the pitch angular velocity variable. Indicates and The pitch angular velocity variable corresponds to the unknown external environmental disturbances of the channel. This represents the combined mass and added mass in the sixth direction of the AUV. Indicates the yaw rate of the AUV The derivative with respect to time, Indicates and The yaw rate variable corresponds to the internal dynamics of the channel. Indicates and The control input for the corresponding channel is the yaw rate variable. Indicates and The yaw rate variable corresponds to the unknown external environmental disturbances in the channel; The expression for a five-degree-of-freedom three-dimensional motion model is as follows: Official (4) In formula (4), Indicates the first The derivative of the longitudinal 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 yaw angle of the AUV in a fixed coordinate system. This represents the pitch 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: Official (5) In formula (5), This represents the combined mass and added mass in the first direction of the AUV. Represents the first in the moving coordinate system The derivative of the total speed of the AUV with respect to time, Indicates and The longitudinal velocity variable corresponds to the internal dynamic composition of the channel. Indicates and The longitudinal velocity variable corresponds to the control input of the channel. Indicates and The longitudinal velocity variable corresponds to the unknown external environmental disturbances of 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 added mass in the fifth direction of the AUV. This represents the derivative of the AUV's pitch velocity with respect to time. Indicates and The pitch angular velocity variable corresponds to the internal dynamics of the channel. Indicates and The control input for the channel corresponding to the pitch angular velocity variable. Indicates and The pitch angular velocity variable corresponds to the unknown external environmental disturbances of the channel. This represents the combined mass and added mass in the sixth direction of the AUV. Represents the first in the moving coordinate system The yaw rate of an AUV This represents the derivative of the AUV's yaw rate with respect to time. Indicates and The yaw rate variable corresponds to the internal dynamics of the channel. Indicates and The control input for the corresponding channel is the yaw rate variable. Indicates and The yaw rate variable corresponds to the unknown external environmental disturbances of the channel.

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, Given the tangent angle in the path, the expression is: Official (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: Official (7) In formula (7), Indicates the first The derivative of the longitudinal error of an AUV 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-Frenet coordinate system. This represents the pitch angle representing the transformation from the fluid coordinate system to the Serret-Frenet coordinate system; Indicates the desired path pitch angle. Indicates the first The derivative of the desired path pitch angle of an AUV with respect to time. Indicates the desired path yaw angle. Indicates the first The derivative of the expected path yaw angle of an AUV with respect to time. Indicates the first The derivative of the lateral error of an AUV with respect to time. Indicates the first Lateral error of an AUV Indicates the first The derivative of the vertical error of an AUV with respect to time. Indicates the first Vertical error of an AUV; The expression for the three-dimensional LOS guidance law is: Official (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 and The longitudinal velocity variable corresponds to the 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-Frenet 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-Frenet coordinate system. Indicates pitch angular velocity, This represents the control gain corresponding to the second observer. Indicates the track angle, Indicates and The pitch angular velocity variable corresponds to the 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 and The auxiliary parameters of the corresponding channel are the yaw rate variable. 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 an AUV with respect to time. Indicates the first Longitudinal error of an AUV This represents the control gain corresponding to the first observer. This represents the derivative of the desired velocity with respect to time. Indicates the first The derivative of the lateral error of an AUV with respect to time. The yaw angle represents the desired path. Indicates correspondence Auxiliary parameters of the channel, Indicates the first The derivative of the desired path pitch angle of an AUV with respect to time. Indicates the first The derivative of the lateral error of an AUV 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 an AUV with respect to time. Indicates the first Vertical error of an AUV Indicates correspondence Auxiliary parameters of the channel, This represents the pitch angle representing the transformation from the fluid coordinate system to the Serret-Frenet coordinate system. Indicates the first Lateral error of an AUV express The estimation error of the channel observer Indicates the first The derivative of the AUV's trajectory angle error 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 AUV's azimuth error 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.

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, 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, This represents the time derivative of the combined term of mass and added mass in the first direction of the AUV. express Channel control input, This represents the derivative of the AUV's pitch velocity with respect to time. Indicates that AUV is affected Total disturbance of the channel, The time derivative of the combined term of mass and added mass in the fifth direction of the AUV. express Channel control input, Indicates the yaw rate of the AUV The derivative with respect to time, Indicates that AUV is affected Total disturbance of the channel, The time derivative of the combined term for the mass and added mass in the sixth direction of the AUV. 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. Indicates and The total velocity variable corresponds to the first observer gain of the channel. Indicates and The total velocity variable corresponds to the estimation error of the channel observer. Indicates that AUVs are affected by The derivative of the estimated total perturbation of the channel with respect to time for the total velocity variable. This represents the combined mass and added mass in the first direction of the AUV. Indicates and The total speed variable corresponds to the control input of the channel. 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. Indicates and The gain of the first observer in the channel corresponding to the pitch angular velocity variable. Indicates and The estimation error of the channel observer corresponding to the pitch angular velocity variable. Indicates that AUVs are affected by The pitch angular velocity variable corresponds to the estimated total disturbance of the channel. Indicates that AUVs are affected by The derivative of the estimated total channel disturbance with respect to time for the pitch angular velocity variable. This represents the combined mass and added mass in the fifth direction of the AUV. Indicates and The control input for the channel corresponding to the pitch angular velocity variable. Indicates and The gain of the first observer in the channel corresponding to the pitch angular velocity variable. Represents the first in the moving coordinate system The derivative of the error in the yaw rate of an AUV with respect to time. Indicates and The gain of the first observer in the corresponding channel for the yaw rate variable. Indicates and The estimation error of the channel observer corresponding to the yaw rate variable. Indicates that AUVs are affected by The yaw rate variable corresponds to an estimate of the total disturbance in the channel. Indicates and The control input for the corresponding channel is the yaw rate variable. Indicates that AUVs are affected by The derivative of the total disturbance estimate of the corresponding channel with respect to time for the yaw rate variable. Indicates and The gain of the second observer in the corresponding channel for the yaw rate variable. This represents the combined mass and additional mass in the sixth direction of the AUV. The error dynamics of the superspiral observer estimation subsystem are expressed as follows: Official (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.

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 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. Controlling AUV formations specifically 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 environmental disturbances, configure the cooperative path parameter update law, 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 when there are external environmental disturbances and internal model uncertainties.

6. A three-dimensional robust formation control method for an underactuated underwater vehicle based on a superhelical observer according to claim 5, characterized in that, The expression for the superspiral dynamic control law is: Official (15) In formula (15), Indicates and The longitudinal velocity variable corresponds to the control input of the channel. This represents the combined mass and added mass in the first direction of the AUV. Indicates the first AUV itself and The longitudinal velocity variable corresponds to the first control law parameter of the channel. Indicates and The total velocity variable corresponds to the velocity estimation error of the channel. Indicates and The uncertainty estimate term for the total velocity variable corresponding to the channel. Indicates the first AUV itself and The longitudinal velocity variable corresponds to the auxiliary state variable of the channel control law. Indicates and The total speed variable corresponds to the desired control parameters of the channel. Indicates the first AUV itself and The longitudinal velocity variable corresponds to the second control law parameter of the channel. Indicates and The control input for the channel corresponding to the pitch angular velocity variable. This represents the combined mass and added mass in the fifth direction of the AUV. Indicates the first AUV itself and The pitch angular velocity variable corresponds to the first control law parameter of the channel. Indicates the first AUV itself and The uncertainty estimation term for the channel corresponding to the pitch angular velocity variable. Indicates and The uncertainty estimation term for the channel corresponding to the pitch angular velocity variable. Indicates and The pitch angular velocity variable corresponds to the desired control parameter of the channel. Indicates the first AUV itself and The pitch angular velocity variable corresponds to the auxiliary state variable of the channel control law. Indicates and The pitch angular velocity variable corresponds to the second control law parameter of the channel. Indicates and The control input for the corresponding channel is the yaw rate variable. This represents the combined mass and added mass in the sixth direction of the AUV. Indicates the first AUV itself and The yaw rate variable corresponds to the first control law parameter of the channel. Indicates and The uncertainty estimate term for the yaw rate variable in the corresponding channel. Indicates and The yaw rate variable corresponds to the desired control parameter of the channel. Indicates the first AUV itself and The yaw rate variable corresponds to the auxiliary state variable of the channel control law. Indicates the first AUV itself and The yaw rate variable corresponds to the second control law parameter of the channel. Indicates the first AUV itself and Uncertainty estimation term for the yaw rate variable in the corresponding channel; The error dynamic expression of the superspiral dynamic control subsystem is: Official (16) In formula (16), Indicates and The total speed variable corresponds to the dynamic error integrated term of the channel. express The derivative with respect to time, Indicates and The total speed variable is related to the corresponding channel. Auxiliary constants of the term, Indicates and The auxiliary parameter matrix of the channel corresponding to the total velocity variable; Official (17) In formula (17), Indicates and The dynamic error integration term for the channel corresponding to the pitch angular velocity variable. express The derivative with respect to time, Indicates and Pitch angular velocity variables are related to the corresponding channels. Auxiliary constants of the term, Indicates and Auxiliary parameter matrix for the channel corresponding to pitch angular velocity variable; Official (18) In formula (18), Indicates and The dynamic error integration term for the corresponding channel of the yaw rate variable. express The derivative with respect to time, Indicates and The yaw rate variable is related to the corresponding channel. Auxiliary constants of the term, Indicates and The auxiliary parameter matrix for the corresponding channel of yaw rate variable.

Images