Under-actuated AUV and USV formation trajectory tracking control method

Abstract

The invention discloses an under-actuated AUV and USV formation trajectory tracking control method, and relates to the technical field of under-actuated AUV and USV three-dimensional trajectory tracking control, and the method comprises the steps: building a dynamic model of an under-actuated underwater robot-unmanned ship heterogeneous multi-agent system; establishing a specified time control system; designing a distributed specified time state observer for each follower agent; defining a system trajectory tracking error; lOS error conversion is carried out on the under-actuated underwater robot; designing a Lyapunov function, and designing a specified time sliding mode surface of the system by using a backstepping method; designing a specified time interference observer for each follower agent; and redesigning a Lyapunov function, and designing a system control rate by using a backstepping method. The problems that the formation control precision is limited and the convergence time for reaching the stable state cannot be accurately estimated when an existing control method is applied are solved.

Description

Technical Field

[0001] The present invention relates to the technical field of three-dimensional trajectory tracking control of underactuated AUVs and USVs, and particularly relates to a formation trajectory tracking control method for underactuated AUVs and USVs. Background Technique

[0002] With the increasing investment in the research of marine resource development by countries around the world, the applications of underwater and surface unmanned systems are becoming increasingly widespread. Autonomous underwater vehicles (AUVs) have a high degree of autonomy and play a crucial role in underwater detection, monitoring, and operations. Unmanned surface vehicles (USVs) have characteristics such as high-speed mobility and easy recovery, and have great potential in maritime patrol, rescue, and military operations. Different from single-agent vehicles, the construction of multi-agent vehicle formation systems promotes information exchange, collaborative task completion, and formation control algorithm research among different entities, improves work efficiency, and increases fault tolerance and flexibility. However, due to the heterogeneity of underactuated AUVs and USVs and the complex and changeable marine environment, there are still many problems to be solved in such heterogeneous multi-agent systems, such as external interference, model uncertainty, communication limitations, etc. Affected by these factors, it is particularly difficult to design a formation trajectory tracking control strategy suitable for heterogeneous multi-agent systems. Therefore, under various uncertain factors, studying the high-precision formation control of underactuated AUV-USV heterogeneous multi-agent systems in the sea area has important theoretical and practical significance.

[0003] The research on underactuated AUV-USV heterogeneous multi-agents is in an advanced and active stage at home and abroad. Existing control methods mainly include adaptive control, model predictive control, and sliding mode control, etc. Among them, for adaptive control, its parameters need to be dynamically adjusted according to the actual working conditions, which sometimes causes system instability; model predictive control can handle complex system dynamics and nonlinear characteristics, but its calculation is relatively complex and time-consuming, and has high requirements for hardware; sliding mode control has good robustness, but overshoot and chattering phenomena may occur. To ensure stability, reasonable design of controller parameters is required. Currently, in the field of tracking control, the convergence forms of the system include asymptotic time convergence, finite-time convergence, fixed-time convergence, predefined-time convergence, etc. However, the convergence speeds of the controllers in these convergence forms are relatively slow, and the convergence time is difficult to estimate. At this time, the fixed-time convergence form begins to be well-known, and its convergence time can be set by itself and is not affected by the initial values of the system.

[0004] The complexity of the underactuated AUV-USV model indicates that obtaining an accurate model for the control system design is an important challenge. Due to the complexity of the underwater and ocean current environment, the underactuated AUV-USV heterogeneous system is subject to various disturbances from the surrounding environment, such as wind, waves, and currents. At the same time, each agent or sensor device in the system will experience wear or aging phenomena, and these inevitable phenomena make the model contain various uncertainties. To ensure that the agent vehicles can remain stable under these uncertainties, existing technologies usually solve this problem by using disturbance observers or neural networks. However, when using neural networks, it is difficult to determine the number of neurons, the convergence speed of existing observers is relatively slow, and the convergence time is difficult to estimate. Summary of the Invention

[0005] Aiming at the above deficiencies in the prior art, a formation trajectory tracking control method for underactuated AUV and USV provided by the present invention solves the problems existing in the application of existing control methods, such as limited formation control accuracy, external environmental disturbances in heterogeneous multi-agent systems, model uncertainties, and the inability to accurately estimate the convergence time to reach the stable state.

[0006] To achieve the above invention purpose, the technical solution adopted by the present invention is: a formation trajectory tracking control method for underactuated AUV and USV, including the following steps:

[0007] S1: Establish the dynamic model of the underactuated underwater robot-unmanned surface vehicle heterogeneous multi-agent system;

[0008] S2: Establish the prescribed-time control system of the underactuated underwater robot-unmanned surface vehicle heterogeneous multi-agent system;

[0009] S3: Design a distributed prescribed-time state observer for each follower agent;

[0010] S4: Based on the dynamic model, the prescribed-time control system, and the distributed prescribed-time state observer, define the trajectory tracking error of the underactuated underwater robot-unmanned surface vehicle heterogeneous multi-agent system;

[0011] S5: Based on the trajectory tracking error, perform LOS error conversion on the underactuated underwater robot;

[0012] S6: Design a Lyapunov function and use the backstepping method to design the prescribed-time sliding mode surface of the underactuated underwater robot-unmanned surface vehicle heterogeneous multi-agent system;

[0013] S7: Based on the prescribed-time sliding mode surface, design a prescribed-time disturbance observer for each follower agent;

[0014] S8: Based on the above steps, the Lyapunov function is redesigned, and the control law of the underactuated AUV-USV heterogeneous multi-agent system is designed using the backstepping method to complete the formation trajectory tracking control of the underactuated AUV and USV.

[0015] Furthermore, the dynamic model in S1 includes the dynamic model of the USV, the dynamic model of the underactuated AUV, and the dynamic equation of the virtual leader;

[0016] The dynamic model of the k-th USV is:

[0017]

[0018] where represents the position and heading angle of the k-th USV, ν sk = [u sk , v sk , r sk T represents the velocity and angular acceleration of the k-th USV, represents the control input of the k-th USV, M k represents the inertia matrix, C(·) represents the Coriolis and centripetal force matrix, D(·) represents the damping matrix, J(·) represents the rotation matrix, f sk represents the random disturbance in the ocean environment;

[0019] Due to the unpredictability of external environmental disturbances, it is difficult to accurately obtain the model parameters of the USV, and the following model matrix representation is constructed:

[0020] M k = M k ′ + ΔM k

[0021] C(ν sk ) = C′(ν sk ) + ΔC(ν sk )

[0022] D(ν sk ) = D′(ν sk ) + ΔD(ν sk )

[0023] where M' k , C'(·) and D'(·) are nominal matrices, and ΔM k , ΔC(·) and ΔD(·) are matrices of unmodeled dynamics;

[0024]

[0025] Define χ sk,1 = η sk ​, The comprehensive dynamic model of the k-th unmanned boat is expressed as:

[0026]

[0027] where χ sk,1 and χ sk,2 are state variables, and the superscript · represents the first derivative, and are assignment parameters, v sk is the speed and angular acceleration of the k-th unmanned boat, is the total uncertainty disturbance of the k-th unmanned boat, J(·) is the rotation matrix, η sk is the position and heading angle of the k-th unmanned boat, f sk is the random disturbance in the marine environment, and the superscript ·· represents the second derivative, N is the number of unmanned boats, is the heading angle of the k-th unmanned boat, m 11 , m 22 , m 23 , m 32 , m 33 , c 13,k , c 23,k , c 31,k , c 32,k , d 11,k , d 22,k , d 23,k , d 32,k and d 33,k are all configuration parameters;

[0028] The dynamic model of the underactuated underwater robot is:

[0029]

[0030]

[0031] where (x Am , y Am , z Am ) is the position of the m-th underactuated underwater robot, is the pitch angle and heading angle of the m-th underactuated underwater robot, u Am is the surge speed, v Am is the sway speed, w Am is the roll speed, q Am is the pitch speed, r Am is the yaw speed, m i,Am (i = 1, 2, 3, 5, 6) are the mass and inertia parameters of the m-th underactuated underwater robot, d i,Am (i = 1, 2, 3, 5, 6) are the hydrodynamic coefficients, is the control force and moment of the m-th underactuated underwater vehicle, G m is an assigned parameter, ρ is the density of seawater, g is the acceleration due to gravity, is the seawater discharge volume, is the longitudinal center height, M is the number of underactuated underwater vehicles, is the uncertainty perturbation;

[0032]

[0033] where, Δ mi,Am , Δ di,Am and Δ G,m respectively represent the model uncertainties caused by hydrodynamics and inaccurate parameters, i = (1, 2, 3, 5, 6), f n,Am is the unknown time-varying perturbation of the ocean environment, n = (u, v, w, q, r);

[0034] The dynamic equation of the virtual leader is:

[0035]

[0036] where, χ L,1 is the position information and angle information of the virtual leader, χ L,2 is the velocity information of the virtual leader, U L is a bounded value.

[0037] Furthermore, the time control system specified in S2 is:

[0038]

[0039] where, g(·) is the system state variable, F(·) is a nonlinear function, R n is the n-dimensional Euclidean space, t is the time;

[0040] g(0) = g 0 , F(g): R n →R n is a nonlinear function, and there exists a radially unbounded continuous positive definite function V(x): R n , satisfying:

[0041]

[0042] The time-varying proportional function is expressed as:

[0043]

[0044] where, g 0$g(·)$ is the initial value of the system state variable, $V(·)$ is a radially unbounded continuous function, $x$ is the state variable, $\theta$ is a time-varying proportional function, $p$ and $q$ are constants, and $p\gt0$, $q\gt0$, $H\gt0$ is a user-defined positive parameter, and $T$ is the convergence time.

[0045] Furthermore, the distributed prescribed-time state observer in S3 is:

[0046]

[0047] where and respectively represent the estimation of the position $\chi$ L,1 and velocity $\chi$ L,2 information of the $i$-th agent with respect to the virtual leader, $g$ k,i and $h$ k,i represent a positive constant, $k=(1,2,3)$, $N$ is the number of unmanned surface vehicles, $M$ is the number of underactuated underwater vehicles, $a$ ij is the information exchange between the $i$-th agent and the $j$-th agent. If there is information exchange between the $i$-th agent and the $j$-th agent, then $a$ ij $\gt0$, otherwise, $a$ ij $=0$, $b$ il is the information exchange between the $i$-th agent and the virtual leader. If there is information exchange between the $i$-th agent and the virtual leader, then $b$ il $=1$, otherwise, $b$ il $=0$, $\chi$ i,1 is the position information of the $i$-th agent, $\chi$ j,1 is the position information of the $j$-th agent, $\chi$ i,2 is the velocity information of the $i$-th agent, $\chi$ j,2 is the velocity information of the $j$-th agent.

[0048] Furthermore, the trajectory tracking error of the underactuated underwater vehicle-unmanned surface vehicle heterogeneous multi-agent system in S4 includes the trajectory tracking error of the unmanned surface vehicle and the trajectory tracking error of the underactuated underwater vehicle;

[0049] The trajectory tracking error of the unmanned surface vehicle is:

[0050]

[0051] where $j = 1,2$, $e$ sk,1 and $e$ sk,2 are respectively the position error and velocity error of the $k$-th unmanned surface vehicle, and satisfy is the estimated value of the state information of the $k$-th unmanned surface vehicle with respect to the virtual leader, $\beta$ sk,j is the relative value maintained by the $k$-th unmanned surface vehicle with respect to the virtual leader in terms of position and angle, and satisfies

[0052] The trajectory tracking error of the underactuated underwater vehicle is:

[0053]

[0054] where x e,Am 、y e,Am and z e,Am are the tracking errors of the underactuated underwater vehicle in the x, y, and z directions, represents the estimated value of the position state information of the virtual leader by the m-th underactuated underwater vehicle, is the relative position maintained by the m-th underactuated underwater vehicle and the virtual leader;

[0055] Taking the derivative of it gives:

[0056]

[0057] where, is the estimated value of the velocity state information of the virtual leader by the m-th underactuated underwater vehicle.

[0058] Furthermore, in S5, the underactuated underwater vehicle performs LOS error conversion based on the tracking position error ρ L,Am , pitch angle error θ L,Am and heading angle error :

[0059]

[0060] Taking the derivative of it gives:

[0061]

[0062] where, ζ 1d,Am 、ζ 2d,Am and ζ 3d,Am are all assigned parameters;

[0063] Continuing to take the derivative gives:

[0064]

[0065]

[0066] where, and are all assigned parameters.

[0067] Furthermore, in S6, the Lyapunov function is designed as:

[0068]

[0069] Among them, and are the designed Lyapunov functions, is ρ L,Am , θ L,Am or

[0070] Taking the derivative of it gives:

[0071]

[0072] The specified-time sliding surface is:

[0073]

[0074] where s sk and are the designed specified-time sliding surfaces, k sk and are the designed positive parameters, p sk and q sk are the designed positive parameters; and are the designed positive parameters;

[0075] Taking the derivative of it gives:

[0076]

[0077] B uAm = m 2,Am v Am r Am - m 3,Am w Am q Am - d 1,Am u Am

[0078] B qAm =(m 3,Am - m 1,Am )u Am w Am - G m - d 5,Am q Am

[0079] B rAm =(m 1,Am - m 2,Am )u Am v Am - d 6,Am r Am

[0080] where τ sk is the control input of the kth unmanned boat, E sk and are both assigned parameters, is the control force and moment of the m-th underactuated underwater robot, and are both assigned parameters.

[0081] Furthermore, the prescribed-time disturbance observer designed for each follower agent in S7 is:

[0082]

[0083] where, and are the designed prescribed-time disturbance observers, p 2,sk , q 2,sk , and are positive design parameters, is the sliding-mode surface error, L i is a positive design parameter, and s is the state variable.

[0084] Furthermore, the Lyapunov function redesigned in S8 is:

[0085]

[0086] where, and are the redesigned Lyapunov functions;

[0087] Taking the derivative of it gives:

[0088]

[0089] The control law of the underactuated underwater robot-unmanned surface vehicle heterogeneous multi-agent system designed using the backstepping method is:

[0090]

[0091] where, k 1,sk , p 1,sk , q 1,sk , and are positive design parameters, is the disturbance estimation value.

[0092] The beneficial effects of the present invention are:

[0093] (1) The present invention estimates the state information of the virtual leader through a prescribed-time distributed state observer designed for each follower agent. Compared with the prior art, this state observer can make the observation error reach stability within a prescribed time, and this distributed state observer can meet the control requirements of heterogeneous multi-agent systems.

[0094] (2) The present invention designs a prescribed-time sliding mode surface by designing a Lyapunov function and adopting sliding mode control technology, enabling the heterogeneous system error to converge within a prescribed time. Compared with the prior art, the designed sliding mode surface has a shorter convergence time and higher control accuracy.

[0095] (3) The present invention designs a prescribed-time disturbance observer for the external environmental disturbances and model uncertainties existing in heterogeneous multi-agent systems. Compared with the existing technologies, the present invention has higher observation accuracy and faster observation speed.

[0096] (4) Under the conditions of external environmental disturbances and model uncertainties, the present invention proposes a prescribed-time formation trajectory tracking control design method for an underactuated AUV-USV heterogeneous multi-agent system. While taking into account the heterogeneity of the heterogeneous multi-agent system, this control method also realizes precise control of the system. And this control method can make the system converge within a prescribed time, and the convergence time does not depend on the system initial value. Finally, the effectiveness of the proposed algorithm is proved by Lyapunov. Description of the Drawings

[0097] Figure 1 It is a flow chart of a formation trajectory tracking control method for an underactuated AUV and USV.

[0098] Figure 2 It is a communication topology diagram of an underactuated AUV-USV heterogeneous multi-agent system.

[0099] Figure 3 It is a three-dimensional trajectory tracking diagram of an underactuated AUV-manipulator heterogeneous multi-agent system.

[0100] Figure 4 It is a two-dimensional trajectory tracking diagram of an underactuated AUV-manipulator heterogeneous multi-agent system.

[0101] Figure 5 It is a tracking error diagram of the USV.

[0102] Figure 6 It is a tracking error diagram of the underactuated AUV.

[0103] Figure 7 It is an observation error diagram of the position information of the USV.

[0104] Figure 8Observation error diagram of the position information of the underactuated underwater robot.

[0105] Figure 9 Observation error diagram of the speed information of the unmanned surface vehicle.

[0106] Figure 10 Observation error diagram of the speed information of the underactuated underwater robot.

[0107] Figure 11 Observation error diagram of the disturbance of USV1.

[0108] Figure 12 Observation error diagram of the disturbance of USV2.

[0109] Figure 13 Observation error diagram of the disturbance of USV3.

[0110] Figure 14 Observation error diagram of the disturbance of USV4.

[0111] Figure 15 Observation error diagram of the disturbance of AUV. Specific implementation manners

[0112] The present invention will be further described below with reference to the accompanying drawings and specific embodiments.

[0113] As Figure 1 shown, a formation trajectory tracking control method for an underactuated AUV and USV includes the following steps:

[0114] S1: Establish the dynamic model of the underactuated underwater robot - unmanned surface vehicle heterogeneous multi-agent system;

[0115] S2: Establish the prescribed-time control system of the underactuated underwater robot - unmanned surface vehicle heterogeneous multi-agent system;

[0116] S3: Design a distributed prescribed-time state observer for each follower agent;

[0117] S4: Define the trajectory tracking error of the underactuated underwater robot - unmanned surface vehicle heterogeneous multi-agent system based on the dynamic model, the prescribed-time control system, and the distributed prescribed-time state observer;

[0118] S5: Perform LOS error conversion on the underactuated underwater robot based on the trajectory tracking error;

[0119] S6: Design a Lyapunov function and use the backstepping method to design the prescribed-time sliding mode surface of the underactuated underwater robot - unmanned surface vehicle heterogeneous multi-agent system;

[0120] S7: Design a prescribed-time disturbance observer for each follower agent based on the prescribed-time sliding mode surface;

[0121] S8: In the above steps, the Lyapunov function is redesigned, and the backstepping method is used to design the control law of the underactuated AUV-USV heterogeneous multi-agent system, so as to complete the formation trajectory tracking control of the underactuated AUV and USV.

[0122] The dynamic model in S1 includes the dynamic model of the USV, the dynamic model of the underactuated AUV, and the dynamic equation of the virtual leader.

[0123] In an embodiment of the present invention, considering that there are N USVs in the underactuated AUV-USV heterogeneous multi-agent system, the dynamic model of the k-th USV can be described as follows:

[0124]

[0125] where M k ∈R 3N×3N denotes the inertia matrix, C(·) is the Coriolis and centripetal force matrix, D(·) is the damping matrix, denotes the position and heading angle of the k-th USV, denotes the control input of the k-th USV, f sk is the random perturbation in the marine environment. Due to the unpredictability of the external environmental interference, it is difficult to accurately obtain the model parameters of the USV. To solve the uncertainty of this model, the following model matrix representation is constructed in this embodiment:

[0126] M k = M k '+ ΔM k

[0127] C(ν sk ) = C'(ν sk ) + ΔC(ν sk )

[0128] D(ν sk ) = D'(ν sk ) + ΔD(ν sk )

[0129] where M' k , C'(·) and D'(·) are nominal matrices, ΔM k , ΔC(ν sk ) and ΔD(ν sk ) represent the matrices of the unmodeled dynamics, and the formula is:

[0130]

[0131] Define χ sk,1 = η sk , The comprehensive dynamic model of the k-th unmanned boat is expressed as follows:

[0132] The dynamic model of the unmanned boat is:

[0133]

[0134] where χ sk,1 and χ sk,2 are state variables, and the superscript · represents the first derivative. and are assignment parameters, v sk is the speed and angular acceleration of the k-th unmanned boat. is the total uncertainty disturbance of the k-th unmanned boat, J(·) is the rotation matrix, η sk is the position and heading angle of the k-th unmanned boat, f sk is the random disturbance in the marine environment, and the superscript ·· represents the second derivative. N is the number of unmanned boats. is the heading angle of the k-th unmanned boat, m 11 , m 22 , m 23 , m 32 , m 33 , c 13,k , c 23,k , c 31,k , c 32,k , d 11,k , d 22,k , d 23,k , d 32,k and d 33,k are all configuration parameters;

[0135] The dynamic model of the underactuated underwater robot is:

[0136]

[0137]

[0138] where (x Am , y Am , z Am ) is the position of the m-th underactuated underwater robot, is the pitch angle and heading angle of the m-th underactuated underwater robot, u Am is the surge speed, v Am is the sway speed, w Am is the roll speed, q Am is the pitch speed, r Am is the yaw speed, m i,Am (i = 1, 2, 3, 5, 6) are the mass and inertia parameters of the m-th underactuated underwater robot, di,Am (i = 1, 2, 3, 5, 6) are hydrodynamic coefficients, is the control force and moment of the m-th underactuated underwater vehicle, G m is an assignment parameter, ρ is the seawater density, g is the acceleration due to gravity, is the seawater discharge, is the longitudinal center height, M is the number of underactuated underwater vehicles, is the uncertainty disturbance;

[0139]

[0140] where, Δ mi,Am , Δ di,Am and Δ G,m respectively represent the model uncertainties caused by hydrodynamics and inaccurate parameters, i = (1, 2, 3, 5, 6), f n,Am is the unknown time-varying disturbance of the ocean environment, n = (u, v, w, q, r);

[0141] The dynamic equation of the virtual leader is:

[0142]

[0143] where, χ L,1 is the position information and angle information of the virtual leader, χ L,2 is the velocity information of the virtual leader, U L is a bounded value.

[0144] The time control system specified in S2 is:

[0145]

[0146] where, g(·) is the system state variable, F(·) is a nonlinear function, R n is the n-dimensional Euclidean space, t is the time;

[0147] g(0) = g 0 , F(g): R n →R n is a nonlinear function, and there exists a radially unbounded continuous positive definite function V(x): R n that satisfies:

[0148]

[0149] The time-varying proportional function is expressed as:

[0150]

[0151] where, g 0is the initial value of the system state variable g(·), V(·) is a radially unbounded continuous function, x is the state variable, θ is the time-varying proportional function, p and q are constants, and p>0, q>0, then the system is globally stable within the specified time, H>0 is a custom positive parameter, and T is the convergence time.

[0152] In this embodiment, the matrix H=L+B, L is the Laplacian matrix, L=[l ij ]∈R (N×N) , l ij =-a ij , i≠j, B=diag(b 1l ,b 2l ,...,b Nl ), P is a positive definite matrix, and its eigenvalues ​​are described as follows. There exists a positive definite matrix Q = PH + H T P.

[0153]

[0154] Among them, p 1 ,p 2 ,…,p n are the diagonal components of the P matrix.

[0155] Not all followers can directly know the state information of the virtual leader. Based on this, a time-limited distributed state observer is designed for each follower agent to estimate the state information of the virtual leader. The time-varying proportional function is expressed as follows:

[0156]

[0157] Here H>0 is a custom positive parameter;

[0158] The first and second derivatives of the time-varying proportional function θ(t) are expressed as:

[0159]

[0160] The distributed time-limited state observer in S3 is:

[0161]

[0162] in, and They represent the position of the virtual leader χ of the i-th intelligent agent L,1 and speed χ L,2 Estimation of information, g k,i and h k,i represents a positive constant, k = (1, 2, 3), N is the number of unmanned boats, M is the number of underactuated underwater robots, aij is the information exchange between the i-th agent and the j-th agent. If there is information exchange between the i-th agent and the j-th agent, then a ij > 0; otherwise, a ij = 0, and b il is the information exchange between the i-th agent and the virtual leader. If there is information exchange between the i-th agent and the virtual leader, then b il = 1; otherwise, b il = 0, and χ i,1 is the position information of the i-th agent, and χ j,1 is the position information of the j-th agent, and χ i,2 is the velocity information of the i-th agent, and χ j,2 is the velocity information of the j-th agent, and sgn(·) is the sign function.

[0163] It is stipulated that the position and velocity errors observed by the time-distributed state observer are respectively expressed as and

[0164] Construct a Lyapunov equation:

[0165]

[0166] where X i,1 is the diagonal element of the positive definite diagonal matrix X, and ψ i,1 is the assigned parameter;

[0167] Taking the derivative of it, we get:

[0168]

[0169] The trajectory tracking error of the underactuated AUV-USV heterogeneous multi-agent system in S4 includes the trajectory tracking error of the USV and the trajectory tracking error of the underactuated AUV;

[0170] The trajectory tracking error of the USV is:

[0171]

[0172] where j = 1, 2, and e sk,1 and e sk,2 are respectively the position error and velocity error of the k-th USV, and satisfy is the estimated value of the state information of the k-th USV with respect to the virtual leader, and β sk,j is the relative value maintained by the k-th USV with respect to the virtual leader in terms of position and angle, and satisfies

[0173] The trajectory tracking error of the underactuated underwater vehicle is as follows:

[0174]

[0175] where x e,Am , y e,Am and z e,Am are the tracking errors of the underactuated underwater vehicle in the x, y, and z directions, represents the estimated value of the virtual leader's position and state information by the m-th underactuated underwater vehicle, is the relative position maintained by the m-th underactuated underwater vehicle and the virtual leader;

[0176] Taking the derivative of it gives:

[0177]

[0178] where is the estimated value of the virtual leader's velocity and state information by the m-th underactuated underwater vehicle.

[0179] In step S5, the underactuated underwater vehicle performs LOS error conversion based on the tracking position error ρ L,Am , pitch angle error θ L,Am and heading angle error as follows:

[0180]

[0181] Taking the derivative of it gives:

[0182]

[0183] where ζ 1d,Am , ζ 2d,Am and ζ 3d,Am are all assigned parameters;

[0184] Continuing to take the derivative gives:

[0185]

[0186] where and are all assigned parameters.

[0187] In step S6, the Lyapunov function is designed as:

[0188]

[0189] where and are the designed Lyapunov functions, is ρL,Am , θ L,Am or

[0190] Deriving the derivative with respect to it gives:

[0191]

[0192] The specified-time sliding surface is:

[0193]

[0194] where s sk and is the designed specified-time sliding surface, k sk and are designed positive parameters, p sk and q sk are designed positive parameters, and are designed positive parameters;

[0195] Deriving the derivative with respect to it gives:

[0196]

[0197] B uAm = m 2,Am v Am r Am - m 3,Am w Am q Am - d 1,Am u Am

[0198] B qAm =(m 3,Am - m 1,Am )u Am w Am - G m - d 5,Am q Am

[0199] B rAm =(m 1,Am - m 2,Am )u Am v Am - d 6,Am r Am

[0200] where τ sk is the control input of the k-th unmanned surface vehicle, E sk and are both assigned parameters, is the control force and moment of the m-th underactuated underwater vehicle, and are both assignment parameters.

[0201] The specified-time disturbance observer designed for each follower agent in S7 is as follows:

[0202]

[0203] where and are the designed specified-time disturbance observers, p 2,sk , q 2,sk , and are positive design parameters, is the sliding-mode surface error, L i is a positive design parameter, and s is the state variable.

[0204] At this time, a new Lyapunov function is constructed as follows:

[0205]

[0206] Taking the derivative of it gives:

[0207]

[0208] The Lyapunov function redesigned in S8 is as follows:

[0209]

[0210] where and are the redesigned Lyapunov functions;

[0211] Taking the derivative of it gives:

[0212]

[0213] The control law of the underactuated AUV-USV heterogeneous multi-agent system is designed using the backstepping method as follows:

[0214]

[0215] where k 1,sk , p 1,sk , q 1,sk , and are positive design parameters, is the disturbance estimation value.

[0216] Based on steps S6 - S8, the proof of the prescribed - time sliding - mode formation trajectory tracking control method for the under - actuated underwater robot - unmanned surface vehicle heterogeneous multi - agent system designed by the present invention includes three parts: the stability proof of the prescribed - time distributed state observer, the stability proof of the prescribed - time disturbance observer, and the stability proof of the designed controller.

[0217] (1) Stability proof of the prescribed - time distributed state observer

[0218] Definition:

[0219]

[0220] Among them, is an assignment parameter;

[0221] Substitute the prescribed - time distributed state observer and into to get:

[0222]

[0223] Define When ψ i,1 < 0, there is That is, Ψ i,1 ≤ 0;

[0224] When ψ i,1 > 0, there is That is, Ψ i,1 ≤ 0; when ψ i,1 = 0, there is Ψ i,1 = 0. So, through the above analysis, Ψ i,1 is always less than or equal to zero. Therefore:

[0225]

[0226] Here Q = PH + H T P is a positive - definite matrix. That is proved Similarly, it can be proved Therefore, it can be proved that the prescribed - time distributed state observer is stable within the prescribed time T ob .

[0227] (2) Stability proof of the prescribed - time disturbance observer

[0228] Substitute and into to get:

[0229]

[0230] Among them, p0 > 0, q 0 > 0 is a positive design parameter;

[0231] According to the established specified-time control system, the specified-time disturbance observer can reach stability within the specified time T Ro and reach stability within.

[0232] (3) Proof of the stability of the designed controller

[0233] When s sk = 0 and then:

[0234]

[0235] According to the established specified-time control system, when s sk = 0 and the tracking errors of the underactuated underwater vehicle and the unmanned surface vehicle and e sk,1 reach stability within the specified time T ob + T Ro + T s and reach stability within.

[0236] Substitute the derivative formula and the control law formula of the designed specified-time sliding surface into the formula to obtain:

[0237]

[0238] Therefore, the designed control law can reach stability within the specified time T ob + T Ro + T s and reach stability within.

[0239] In an embodiment of the present invention, in order to verify the effectiveness of the method of the present invention, a simulation experiment was carried out, which is specifically as follows:

[0240] The underactuated underwater vehicle - unmanned surface vehicle system architecture consists of 1 virtual leader agent and 5 follower agents. Among them, the 5 follower agents consist of 1 underactuated underwater vehicle and 4 unmanned surface vehicles. The underactuated underwater vehicle - unmanned surface vehicle heterogeneous multi-agent system uses a communication topology graph for information exchange, and its topological structure is as Figure 2As shown. In this figure, number 1 represents an underactuated underwater robot, numbers 2-5 represent four unmanned boats, and number 0 represents a virtual leader. The dotted line represents a one-way transmission path of the state information variable, and the solid line represents a two-way transmission path of the state information variable. The formation is a square composed of four unmanned boats, the virtual leader is located at the center of the square, and the underactuated underwater robot and the virtual leader are located at the same position. The system parameters are shown in Tables 1 and 2, and the controller parameters are shown in Table 3. The parameters in Table 3 are detailed descriptions of the hydrodynamic parameters in the system parameters in Table 2:

[0241] Table 1 Parameter information of underactuated underwater robot model

[0242]

[0243]

[0244] Table 2 Model parameter information of unmanned boat

[0245]

[0246] Table 3 Hydrodynamic parameter information of the model

[0247]

[0248] Figure 3 and Figure 4 The formation trajectory tracking effect diagrams of the under-actuated underwater robot-unmanned boat heterogeneous multi-agent in 3D environment and 2D environment are shown respectively. Among them, the formation formation and trajectory tracking of the under-actuated underwater robot-unmanned boat heterogeneous multi-agent at 30s and 190s are shown. It can be seen that under the control of the designed controller, the under-actuated underwater robot-unmanned boat heterogeneous multi-agent system can achieve the expected formation tracking effect and can track the trajectory according to the expected formation formation. Figure 5 and Figure 6 The tracking errors of the unmanned vehicle and the underactuated underwater vehicle under the designed controller are shown. Figure 7 and Figure 8 represents the observation error value of the position state information of the unmanned boat and the underactuated underwater robot, Figure 9 and Figure 10 Represents the velocity state information observation error value of the unmanned vehicle and the underactuated underwater robot. Figures 11 - 15 is the lumped disturbance observation error value of the unmanned boat and the underactuated underwater robot. It can be seen that both the tracking error and the observation error can converge to a region close to zero within the specified time. It verifies that the proposed algorithm has good robustness, fast convergence speed, and the convergence time is not affected by the initial value of the system.

[0249] Those skilled in the art will appreciate that the embodiments described herein are intended to help readers understand the principles of the present invention, and should be understood that the protection scope of the present invention is not limited to such specific statements and embodiments. Those skilled in the art can make various other specific variations and combinations that do not deviate from the essence of the present invention based on the technical revelations disclosed by the present invention, and these variations and combinations are still within the protection scope of the invention.

Claims

1. A formation trajectory tracking control method for underactuated AUVs and USVs, characterized in that: The following steps are involved: S1: Establish the dynamic model of heterogeneous multi-agent system of underactuated underwater robot and unmanned boat; S2: Establish a time-limited control system for heterogeneous multi-agent systems of underactuated underwater robots and unmanned boats; S3: Design a distributed time-limited state observer for each follower agent; S4: Based on the dynamic model, the prescribed time control system and the distributed prescribed time state observer, the trajectory tracking error of the underactuated underwater robot-unmanned vehicle heterogeneous multi-agent system is defined; S5: Based on the trajectory tracking error, LOS error conversion is performed on the underactuated underwater robot; S6: Design Lyapunov function and use backstepping method to design the prescribed time sliding surface of the heterogeneous multi-agent system of underactuated underwater robot and unmanned boat; S7: Based on the specified time sliding surface, a specified time disturbance observer is designed for each follower agent; S8: Based on the above steps, the Lyapunov function is redesigned, and the control rate of the underactuated underwater robot-unmanned boat heterogeneous multi-agent system is designed using the backstepping method to complete the formation trajectory tracking control of the underactuated AUV and USV.

2. The formation trajectory tracking control method of underactuated AUV and USV according to claim 1, characterized in that: The dynamic model in S1 includes the unmanned boat dynamic model, the underactuated underwater robot dynamic model and the dynamic equation of the virtual leader; The dynamic model of the kth unmanned boat is: in, represents the position and heading angle of the kth unmanned boat, ν sk =[u sk ,v sk ,r sk ] T represents the speed and angular acceleration of the kth unmanned boat, represents the control input of the kth unmanned boat, M k represents the inertia matrix, C(·) represents the Coriolis and centripetal force matrices, D(·) represents the damping matrix, J(·) represents the rotation matrix, and f sk represents random disturbances in the marine environment; Due to the unpredictability of external environmental interference, it is difficult to accurately obtain the model parameters of the unmanned boat. The following model matrix representation is constructed: M k =M′ k +ΔM k C(n sk )=C′(ν sk )+ΔC(ν sk ) D(n sk )=D′(ν sk )+ΔD(ν sk ) Among them, M' k , C'(·) and D'(·) are nominal matrices, ΔM k , ΔC(·), and ΔD(·) are matrices of unmodeled dynamics; Definition of χ sk,1 =η sk , The comprehensive dynamic model of the kth unmanned boat is expressed as: Among them, χ sk,1 and χ sk,2 is the state variable, the superscript · indicates the first-order derivative. and is the assignment parameter, v sk is the speed and angular acceleration of the kth unmanned boat, is the total uncertainty interference of the kth unmanned boat, J(·) is the rotation matrix, η sk is the position and heading angle of the kth unmanned boat, f sk is the random disturbance in the marine environment, the superscript ·· indicates the second-order derivative, N is the number of unmanned boats, is the heading angle of the kth unmanned boat, m 11 、m 22 、m 23 、m 32 、m 33 、c 13,k 、c 23,k 、c 31,k 、c 32,k ,d 11,k ,d 22,k ,d 23,k ,d 32,k and d 33,k These are configuration parameters; The dynamic model of the underactuated underwater robot is: Among them, (x Am ,y Am ,z Am ) is the position of the mth underactuated underwater vehicle, is the pitch angle and heading angle of the mth underactuated underwater robot, u Am is the surge velocity, v Am is the swing speed, w Am is the rolling velocity, q Am is the pitch velocity, r Am is the yaw speed, m i,Am (i=1,2,3,5,6) are the mass and inertia parameters of the mth underactuated underwater robot, d i,Am (i=1,2,3,5,6) is the hydrodynamic coefficient, is the control force and torque of the mth underactuated underwater robot, G m is the assigned parameter, ρ is the seawater density, g is the gravitational acceleration, is the seawater discharge, is the longitudinal center height, M is the number of underactuated underwater robots, (n=u,v,w,q,r) is the uncertainty disturbance; Among them, Δ mi,Am , Δ di,Am and Δ G,m denote the model uncertainty caused by fluid mechanics and imprecise parameters, i = (1, 2, 3, 5, 6), f n,Am is the unknown time-varying disturbance of the ocean environment, n=(u,v,w,q,r); The dynamic equation of the virtual leader is: Among them, χ L,1 is the position and angle information of the virtual leader, χ L,2 Speed ​​information for virtual leaders, U L is a bounded value.

3. The formation trajectory tracking control method of underactuated AUV and USV according to claim 2, characterized in that: The time control system specified in S2 is: Among them, g(·) is the system state variable, F(·) is the nonlinear function, R n is n-dimensional Euclidean space, t is time; g(0)=g0,F(g):R n →R n is a nonlinear function, there exists a radially unbounded continuous positive definite function V(x):R n ,satisfy: The time-varying scaling function is expressed as: Where g0 is the initial value of the system state variable g(·), V(·) is a radially unbounded continuous function, x is the state variable, θ is the time-varying proportional function, p and q are constants, and p>0, q>0, H>0 is a custom positive parameter, and T is the convergence time.

4. The formation trajectory tracking control method of underactuated AUV and USV according to claim 3, characterized in that: The distributed time-limited state observer in S3 is: in, and They represent the position of the virtual leader χ of the i-th intelligent agent L,1 and speed χ L,2 Estimation of information, g k,i and h k,i represents a positive constant, k = (1, 2, 3), N is the number of unmanned boats, M is the number of underactuated underwater robots, a ij is the information exchange between the i-th intelligent agent and the j-th intelligent agent. If there is information exchange between the i-th intelligent agent and the j-th intelligent agent, then a ij > 0, otherwise, a ij =0, b il is the information exchange between the ith intelligent agent and the virtual leader. If there is information exchange between the ith intelligent agent and the virtual leader, then b il =1, otherwise, b il =0, χ i,1 is the position information of the i-th intelligent agent, χ j,1 is the position information of the jth intelligent agent, χ i,2 is the speed information of the i-th intelligent agent, χ j,2 is the speed information of the j-th intelligent agent.

5. The formation trajectory tracking control method of underactuated AUV and USV according to claim 4, characterized in that: The trajectory tracking error of the underactuated underwater robot-unmanned boat heterogeneous multi-agent system in S4 includes the trajectory tracking error of the unmanned boat and the trajectory tracking error of the underactuated underwater robot; The trajectory tracking error of the unmanned boat is: Where j = 1, 2, e sk,1 and e sk,2 are the position error and speed error of the kth unmanned boat, and satisfy is the estimated value of the state information of the kth unmanned boat to the virtual leader, β sk,j is the relative value of the kth unmanned boat in position and angle to the virtual leader, and satisfies The trajectory tracking error of the underactuated underwater robot is: Among them, x e,Am ,y e,Am and z e,Am is the tracking error of the underactuated underwater robot in the x, y, and z directions, It is represented as the estimated value of the position state information of the virtual leader by the mth underactuated underwater robot, is the relative position between the mth underactuated underwater robot and the virtual leader; Taking its derivative we get: in, is the estimated value of the velocity state information of the virtual leader by the mth underactuated underwater vehicle.

6. The formation trajectory tracking control method of underactuated AUV and USV according to claim 5, characterized in that: In S5, the underactuated underwater robot is tracked based on the position error ρ L,Am , pitch angle error θ L,Am and heading angle error Perform LOS error conversion: Taking its derivative we get: in, ζ 1d,Am , 2d,Am and 3d,Am All are assigned parameters; Continue to derive: in, and All are assigned parameters.

7. The formation trajectory tracking control method of underactuated AUV and USV according to claim 6, characterized in that: The Lyapunov function designed in S6 is: in, and is the designed Lyapunov function, is L,Am ,θ L,Am or Taking its derivative we get: The specified time sliding surface is: Among them, s sk and is the designed sliding surface at specified time, k sk and is the positive parameter of the design, p sk and q sk is the positive parameter of the design, and is the positive parameter of the design; Taking its derivative we get: B uAm =m 2,Am v Am r Am -m 3,Am w Am q Am -d 1,Am u Am B qAm =(m 3,Am -m 1,Am )u Am w Am -G m -d 5,Am q Am B rAm =(m 1,Am -m 2,Am )u Am v Am -d 6,Am r Am Among them, τ sk is the control input of the kth unmanned boat, E sk and All are assigned parameters. is the control force and torque of the mth underactuated underwater robot, and All are assigned parameters.

8. The formation trajectory tracking control method of underactuated AUV and USV according to claim 7, characterized in that: In S7, a time interference observer is designed for each follower agent as follows: in, and is the designed time-limited disturbance observer, p 2,sk ,q 2,sk , and is the design positive parameter, is the sliding surface error, L i is the design positive parameter and s is the state variable.

9. The formation trajectory tracking control method of underactuated AUV and USV according to claim 8, characterized in that: The Lyapunov function is redesigned in S8 as follows: in, and It is a redesigned Lyapunov function; Taking its derivative we get: The control efficiency of the heterogeneous multi-agent system of underactuated underwater robot and unmanned boat designed by backstepping method is: Among them, k 1,sk 、p 1,sk ,q 1,sk , and is the design positive parameter, is the disturbance estimate.

Images