A safe tracking control method for a distributed self-organizing unmanned surface vehicle cluster

Abstract

The application discloses a kind of safety tracking control methods of distributed self-organizing unmanned ship cluster, S1: design group aggregation energy function, for ensuring each unmanned ship in unmanned ship cluster is distributed around the set virtual center point;S2: design collision avoidance potential function and obstacle avoidance potential function, so as to avoid collision between adjacent unmanned ship, between each unmanned ship and external obstacle when unmanned ship gathers;S3: design bow consistent function, for ensuring the bow consistency of all unmanned ship;S4: design the expected guidance speed of unmanned ship cluster;S5: design distributed extended state observer, real-time estimate the unknown system dynamics and external disturbance of each unmanned ship;So as to obtain unmanned ship actual speed, combine the unmanned ship actual speed with expected guidance speed to construct speed tracking error function;S6: based on speed tracking error function and using adaptive dynamic programming algorithm design optimal control law, and based on optimal control law control unmanned ship cluster stable navigation.The application can realize the safe and efficient cooperative control of unmanned ship cluster, ensure that unmanned ship cluster can efficiently, safely, consistently execute tracking task in complex environment, significantly improve the overall performance of unmanned ship cluster.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned ship cluster tracking control, and particularly relates to a safe tracking control method for a distributed self-organizing unmanned ship cluster. BACKGROUND

[0002] The unmanned ship cluster has shown unprecedented importance in performing complex and diverse maritime tasks due to its concealment, autonomy and scalability. In modern naval warfare, for example, the trajectory tracking technology of the unmanned ship cluster is an indispensable key link in performing tasks such as tracking enemy targets and pursuing illegal objects. However, these task environments are highly dynamic and complex, and the unmanned ship cluster often encounters external obstacles such as static or dynamic obstacles formed by non-combat ships or other non-scheduled targets during tracking, which poses a serious challenge to the tracking action.

[0003] Current unmanned ship cluster cooperative control strategies, such as graph-based, virtual structure method, leader-following method and behavior method, have promoted the realization of autonomous control of unmanned ship formation to a certain extent. However, these methods generally have certain limitations: they usually rely on preset fixed formation structure and formation constraints, and cannot flexibly respond to changing navigation conditions. In particular, in a highly dynamic environment, the rigidity of the formation structure and the high sensitivity to initial conditions limit the autonomous decision-making ability and flexibility of the unmanned ship cluster. In addition, when unmanned ships perform tasks in real complex marine environments, they will encounter a large number of unpredictable external disturbances, including but not limited to time-varying, unknown dynamic disturbances caused by natural factors such as sea wind, sea wave and sea current, and water dynamic parameter uncertainty caused by differences between actual navigation state and ideal model, which seriously challenge the precise trajectory tracking control ability of the unmanned ship cluster. Furthermore, given that the unmanned ship cluster is a nonlinear time-varying strongly coupled system, how to ensure that all unmanned ships in the cluster not only maintain their stability, but also achieve optimal attitude consistency to achieve efficient and accurate trajectory tracking targets under conditions of model information uncertainty and unknown external disturbances is also one of the key problems to be solved in this field. SUMMARY

[0004] The present application provides a safe tracking control method for a distributed self-organizing unmanned ship cluster to overcome the technical problems of insufficient flexibility of formation structure, serious environmental disturbance and difficulty in guaranteeing optimal attitude consistency of self-organizing cluster in existing unmanned ship cluster cooperative control.

[0005] To achieve the above-mentioned purpose, the technical solution of the present application is:

[0006] A safe tracking control method for a distributed self-organizing unmanned ship cluster, comprising the following specific steps:

[0007] S1: establishing a ship motion model, and designing a group gathering energy function based on actual trajectory positions and reference trajectory positions of the ship motion model, so as to ensure that each unmanned ship in the unmanned ship cluster is distributed around a set virtual center point;

[0008] S2: designing a collision avoidance potential function and an obstacle avoidance potential function for adjacent unmanned ships and external obstacles, so as to avoid collisions between adjacent unmanned ships and between each unmanned ship and external obstacles when the unmanned ships gather;

[0009] S3: designing a consistent heading function, so as to ensure the consistency of the headings of all unmanned ships;

[0010] S4: designing a desired guidance speed of the unmanned ship cluster based on the group gathering energy function, the collision avoidance potential function, the obstacle avoidance potential function, and the consistent heading function;

[0011] S5: designing a distributed extended state observer, and using the distributed extended state observer to estimate unknown system dynamics and external disturbances of each unmanned ship in real time; obtaining an actual speed of the unmanned ship based on the unknown system dynamics and the external disturbances of the unmanned ship, and constructing a speed tracking error function by combining the actual speed of the unmanned ship with the desired guidance speed;

[0012] S6: designing an optimal control law based on the speed tracking error function and using an adaptive dynamic programming algorithm, and controlling the unmanned ship cluster to stably sail based on the optimal control law.

[0013] Further, in S1, the process of establishing a ship motion model and designing a group gathering energy function based on actual trajectory positions and reference trajectory positions of the ship motion model is as follows:

[0014] Suppose that an unmanned ship cluster is formed by n unmanned ships, and a ship motion equation of the i-th unmanned ship in a horizontal plane is defined as follows:

[0015]

[0016] wherein η i = [x i , y i , ψ i ] T , x i , y i , and ψ i are the horizontal and vertical coordinates and the heading of the i-th unmanned ship in an inertial coordinate system, i = 1, 2, …, n, and υ i = [u i , v i , r i ] T , u i , v i , and ri Let τ represent the forward, lateral, and bow velocities of the i-th unmanned surface vessel in the attached coordinate system. i τ is the control input for the i-th unmanned surface vessel. i =[τ ui ,τ vi ,τ ri ] T d i =[d ui ,d vi ,d ri ] T d ui ,d vi ,d ri All are external disturbances of the i-th unmanned surface vessel, M = M0 + ΔM, C = C0 + ΔC, D = D0 + ΔD, where M0, C0, and D0 are modeling matrices, and ΔM, ΔC, and ΔD are uncertainty matrices, R(ψ i ) is the rotation matrix;

[0017] For ease of design, let ω i =R(ψ) i )υ i The equation of motion for the ship in equation (1) can be rewritten as:

[0018]

[0019] Among them, f δi (η i ,ω i ) is a lumped unknown term consisting of the dynamics of the unknown system and external disturbances, f δi (η i ,ω i )=f(η i ,ω i )+δ i ;f(η i ,ω i ) = C o (η i ,ω i )ω i +D o (η i ,ω i )ω i δ i =RM -1 d i μ i =RM -1 τ i C o (η i ,ω i )=-RM -1 C(R -1 ωi )R -1 , R is R(ψ i );

[0020] The reference trajectory position of the ship motion model is defined as follows:

[0021]

[0022] wherein: η d = [x d , y d , ψ d ] T , x d , y d , ψ d are the horizontal and vertical coordinates and the heading of the reference trajectory, respectively; υ d = [u d , v d , r d ] T , u d , v d , r d are the forward, lateral and yaw velocities of the reference trajectory position in the body coordinate system, respectively; ω d is the desired velocity vector in the inertial coordinate system, ω d = R d (ψ d )υ d , R d (ψ d ) is the desired rotation matrix; μ d is the control input of the reference trajectory position, μ d = R d (ψ d )M -1 τ d ;

[0023] When defining the trajectory tracking, the reference trajectory position at each time is set as the virtual center point of the actual trajectory position of the unmanned ship cluster, and the distance of each unmanned ship to the virtual center point is controlled;

[0024] The position and velocity vector of the i-th unmanned ship in the world coordinate system are defined as p i = [x i , y i ] T and v i = [ω ix , ω iy ] T , respectively; the reference trajectory position and the velocity vector in the world coordinate system are defined as p d = [x d , yd ] T and v d = [w dx , w dy ] T ;

[0025] To control the size of the whole USV swarm, i.e. the dispersion of USVs in the swarm, a dispersion function σ of the USV swarm is defined as:

[0026]

[0027] where l id is the distance between the ith USV and the reference trajectory position, l id = [(x i - x d ) 2 + (y i - y d ) 2 ] 12 , and σ d is the set desired aggregation quantization parameter;

[0028] Based on the dispersion function σ, an error expression between the actual distribution and the desired distribution of the USV swarm aggregation is defined as follows:

[0029] σ e = σ - σ d (5)

[0030] A group aggregation energy function is designed using the Lyapunov stability theory-based method, which is expressed as:

[0031]

[0032] The gradient of v i is obtained by formula (6), which is:

[0033]

[0034] where g σi represents a dispersion factor, and its expression is:

[0035]

[0036] Further, in S2, the collision avoidance potential function and the obstacle avoidance potential function are designed for the adjacent USVs and external obstacles, including:

[0037] S21: The collision avoidance potential function is designed for the adjacent USVs in the USV swarm, and the process is:

[0038] The radii of r r and Rr The circular forbidden zone and buffer zone are designed and the collision avoidance potential function is expressed as:

[0039]

[0040] where U rij represents the repulsion vector of the jth USV to the ith USV, l ij is the distance between the ith USV and the jth USV, l ij = [(x i - x j ) 2 + (y i - y j ) 2 ] 1 / 2 ;

[0041] The partial derivative of v i with respect to (9) is taken, and the repulsion of other USVs in the USV swarm to the ith USV is obtained, and the following is obtained:

[0042]

[0043] where g rij represents the collision avoidance repulsion factor, and the expression is as follows:

[0044]

[0045] S22: The obstacle avoidance potential function is designed for the external obstacles of the USV swarm, and the process is as follows:

[0046] Suppose that the positions of the external obstacles are known, and the number of the external obstacles is m, the coordinates of the oth obstacle are defined as p o = (x o , y o ) T , and the preliminary form of the obstacle avoidance potential function can be designed as follows:

[0047]

[0048] where l io represents the straight-line distance between the ith USV and the oth external obstacle, l io = [(x i - x o ) 2 + (y i - y o ) 2 ] 12 , R o and r o represent the maximum and minimum safety distances for avoiding external obstacles, respectively;

[0049] By taking the partial derivative of v i , we get:

[0050]

[0051] where F ai represents the repulsive force vector of all obstacles to the ith unmanned surface vehicle, g aio represents the repulsion factor, which is expressed as:

[0052]

[0053] Further, in S3, the process of designing the heading consistency function is:

[0054] To achieve heading consistency, the heading dynamic error of the unmanned surface vehicle in the unmanned surface vehicle cluster is defined as:

[0055] ψ ei = ψ i - ψ d (15)

[0056] Based on the heading dynamic error, the Lyapunov function of the heading adjustment, i.e., the heading consistency function, is constructed, which is expressed as:

[0057]

[0058] By taking the gradient of the ith unmanned surface vehicle's yaw rate r i in the appendage coordinate system with respect to formula (16), the heading control force is obtained, which is expressed as:

[0059]

[0060] Further, in S4, based on the group aggregation energy function, the collision avoidance potential function, the obstacle avoidance potential function, and the heading consistency function, the process of designing the desired guidance velocity of the unmanned surface vehicle cluster is:

[0061] The group aggregation energy function, the collision avoidance potential function, and the obstacle avoidance potential function designed by formula (6), formula (9), and formula (12) are superimposed, thereby designing the position planning energy function of the unmanned surface vehicle cluster, which is expressed as:

[0062]

[0063] By taking the gradient of v i with respect to formula (18), and formula (7), (8), (10), (11), (13), and (14), we get:

[0064]

[0065] where k σ , kr , k a and k ψ are positive parameters to be designed,

[0066] Meanwhile, in order to control the consistency of the sailing direction of each USV, a USV swarm heading planning energy function is designed based on the heading consistency function, which is expressed as:

[0067]

[0068] The gradient of r i is obtained by formula (20) as:

[0069]

[0070] where g ψi represents a heading adjustment factor, g ψi = k ψ ψ ei ;

[0071] According to the Lyapunov stability criterion, in order to make the results of formula (19) and formula (21) less than 0, a USV swarm decision expected speed signal is designed, which is expressed as:

[0072]

[0073] where Δ ν , Δ ψ , k1 and k2 are positive parameters to be designed;

[0074] Combined with the ship motion model and based on the USV swarm decision expected speed signal, the expected guidance speed of the USV swarm is defined as:

[0075]

[0076] Further, in S5, a distributed extended state observer is designed, and the unknown system dynamics and external disturbances of each USV are estimated in real time using the distributed extended state observer; the actual speed of the USV is obtained based on the unknown system dynamics and external disturbances of the USV, and the process of constructing a speed tracking error function by combining the actual speed of the USV with the expected guidance speed is:

[0077] S51: the process of designing a distributed extended state observer is:

[0078] Define the state variable Then formula (2) can be expressed as:

[0079]

[0080] where h ifor the unknown derivative of the extended state i = [h ui , h vi , h ri ] T ;

[0081] A distributed extended state observer is designed to observe the lumped unknown term in equation (2), i.e., the unknown system dynamics and external disturbances of the USV, and the lumped unknown term is regarded as the extended state. The specific form of the distributed extended state observer is designed as follows:

[0082]

[0083] wherein are the estimated values of the system state , L1, L2, and L3 are the parameter matrices of the distributed extended state observer, respectively;

[0084] is represented as:

[0085]

[0086] The speed tracking error function of the i-th USV in the USV cluster is constructed in combination with the actual speed ω i of the USV and the expected guidance speed ω di :

[0087] ω ei = ω i - ω di (27).

[0088] Further, the process of designing the optimal control law based on the speed tracking error function and using the adaptive dynamic programming algorithm is as follows:

[0089] The cost function of the speed tracking error of the i-th USV is defined based on the speed tracking error function as:

[0090]

[0091] wherein

[0092] According to equation (28), the optimal cost function is defined as:

[0093]

[0094] wherein Ψ(Ω i ) represents the set of all control laws;

[0095] ​According to equation (2), equation (26) and the optimal control theory, the optimal Hamilton-Jacobi-Bellman (HJB) equation is defined as:

[0096]

[0097] The gradient descent method is used, i.e. The ideal optimal control law is designed as:

[0098]

[0099] In order to obtain the available control input, the Actor-Critic (i.e. critic and actor reinforcement learning neural network structure) is used to estimate the cost function V i * and the ideal optimal control law μ i * , respectively, to obtain:

[0100]

[0101] wherein, represents the estimate of V i * ; and are the critic and actor neural network weights, respectively;

[0102] Since the estimated optimal control law is depicted in the world coordinate system in equation (2), the control input of the ship motion equation in equation (1) is obtained as:

[0103]

[0104] Substituting equation (35) into equation (30), the estimated HJB equation is obtained, which is expressed as:

[0105]

[0106] The error between the estimated HJB equation and the optimal HJB equation is designed as:

[0107]

[0108] A positive definite function is defined as:

[0109]

[0110] For the above equation (39), the gradient descent method is used to calculate the weight update law of the critic neural network as:

[0111]

[0112] wherein, γ ci is the learning rate of the i-th USV, γ ci > 0.

[0113] Similarly, the weight update law of the actuator neural network is:

[0114]

[0115] wherein, γ ai is the learning rate of the i-th USV, γ ai > 0.

[0116] Beneficial effects: The present application ensures that the USV cluster is stably distributed around the virtual center point by designing a group aggregation energy function, and simultaneously introduces a collision avoidance potential function and an obstacle avoidance potential function to avoid collisions between adjacent USVs and between each USV and external obstacles, and maintains the heading consistency of all USVs through a consistent heading function. On this basis, the expected guidance speed of the USV cluster is planned by fusing the above functions to guide the speed adjustment of the USV individual. In addition, the present application also designs a distributed extended state observer to estimate the unknown system dynamics and external disturbances in real time, and obtains the actual speed of the USV based on the unknown system dynamics and external disturbances of the USV, and constructs a speed tracking error function by combining the actual speed of the USV with the expected guidance speed. Based on the speed tracking error function and using an adaptive dynamic programming algorithm, an optimal control law is designed, and based on the optimal control law, the USV cluster is controlled to stably sail, thereby effectively overcoming the problems of insufficient flexibility of the formation structure, serious environmental disturbance and difficult to guarantee the attitude consistency of the self-organizing cluster in the prior art, realizing safe and efficient cooperative control of the USV cluster, ensuring that the USV cluster can efficiently, safely and consistently perform tracking tasks in complex environments, and significantly improving the overall performance of the USV cluster. BRIEF DESCRIPTION OF DRAWINGS

[0117] In order to more clearly illustrate the technical solutions of the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiment or prior art description. Obviously, the drawings in the following description are some embodiments of the present application, and those skilled in the art can also obtain other drawings according to these drawings without creative labor.

[0118] Figure 1 A flow chart of a distributed self-organizing USV cluster safety tracking control method in the present application;

[0119] Figure 2A safety tracking control flow chart of a distributed self-organizing unmanned surface vehicle cluster in an embodiment of the present application;

[0120] Figure 3 A trajectory tracking effect diagram of an unmanned surface vehicle cluster in an obstacle environment in an embodiment of the present application;

[0121] Figure 4 A pose change curve diagram of an unmanned surface vehicle cluster in an embodiment of the present application;

[0122] Figure 5 A speed change curve diagram of an unmanned surface vehicle cluster in an embodiment of the present application;

[0123] Figure 6 A speed tracking error curve diagram of an unmanned surface vehicle cluster in an embodiment of the present application;

[0124] Figure 7 A relative distance curve diagram between each unmanned surface vehicle in an unmanned surface vehicle cluster in an embodiment of the present application. DETAILED DESCRIPTION

[0125] To make the objectives, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be described below in connection with the drawings in the embodiments of the present application. Obviously, the described embodiments are some but not all of the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without creative work fall within the protection scope of the present application.

[0126] The present embodiment provides a safety tracking control method for a distributed self-organizing unmanned surface vehicle cluster, as shown in Figure 1 and Figure 2 , the specific steps include:

[0127] S1: establishing a ship motion model, and designing a group aggregation energy function based on an actual trajectory position and a reference trajectory position of the ship motion model, for ensuring that each unmanned surface vehicle in the unmanned surface vehicle cluster is distributed around a set virtual center point;

[0128] In specific embodiments, in S1, the process of establishing a ship motion model and designing a group aggregation energy function based on an actual trajectory position and a reference trajectory position of the ship motion model is as follows:

[0129] It is assumed that an unmanned surface vehicle cluster is composed of n unmanned surface vehicles, and a ship motion equation of the i-th unmanned surface vehicle in a horizontal plane is defined as follows:

[0130]

[0131] wherein η i = [x i , yi ,ψ i ] T x i ,y i ,ψ i Let x, y, and y be the x, y, and y coordinates of the i-th unmanned surface vessel in the inertial coordinate system, respectively, i = 1, 2, ..., n, and υ be the x, y, and y coordinates of the i-th unmanned surface vessel in the inertial coordinate system. i =[u i ,v i ,r i ] T u i ,v i ,r i Let τ represent the forward, lateral, and bow velocities of the i-th unmanned surface vessel in the attached coordinate system. i τ is the control input for the i-th unmanned surface vessel. i =[τ ui ,τ vi ,τ ri ] T d i =[d ui ,d vi ,d ri ] T d ui ,d vi ,d ri All are external disturbances of the i-th unmanned surface vessel, M = M0 + ΔM, C = C0 + ΔC, D = D0 + ΔD, where M0, C0, and D0 are modeling matrices, and ΔM, ΔC, and ΔD are uncertainty matrices, R(ψ i ) is the rotation matrix;

[0132] For ease of design, let ω i =R(ψ) i )υ i The equation of motion for the ship in equation (1) can be rewritten as:

[0133]

[0134] Among them, f δi (η i ,ω i ) is a lumped unknown term consisting of the dynamics of the unknown system and external disturbances, f δi (η i ,ω i )=f(η i ,ω i )+δ i ;f(η i ,ω i ) = C o (η i ,ω i )ω i+D o (η i ,ω i )ω i ,δ i =RM -1 d i ,μ i =RM -1 τ i ,C o (η i ,ω i )=-RM -1 C(R -1 ω i )R -1 , R is R(ψ i );

[0135] The reference trajectory position of the ship motion model is defined as follows:

[0136]

[0137] wherein η d =[x d ,y d ,ψ d ] T , x d , y d , ψ d are the horizontal and vertical coordinates and the heading of the reference trajectory, respectively; υ d =[u d ,v d ,r d ] T , u d , v d , r d are the forward, lateral and yaw velocities of the reference trajectory position in the body coordinate system, respectively; ω d is the desired velocity vector in the inertial coordinate system, ω d =R d (ψ d )υ d , R d (ψ d ) is the desired rotation matrix; μ d is the control input of the reference trajectory position, μ d =R d (ψ d )M -1 τ d ;

[0138] In the definition of trajectory tracking, the reference trajectory position at each time is set as the virtual center point of the actual trajectory position of the USV cluster, and the distance of each USV to the virtual center point is controlled.

[0139] Let p i and v i be the position and velocity vector of the ith USV in the world coordinate system, respectively, defined as i T and i ix iy T Let p d and v d be the position and velocity vector of the reference trajectory in the world coordinate system, respectively, defined as d T and d dx dy T

[0140] To control the size of the USV swarm, i.e., the dispersion of the USVs in the swarm, a dispersion function σ of the USV swarm is defined as

[0141]

[0142] where l id is the distance between the ith USV and the reference trajectory position, l id = [(x i - x d ) 2 + (y i - y d ) 2 ] 12 , and σ d is the desired aggregation quantization parameter;

[0143] Based on the dispersion function σ, an error expression between the actual distribution and the desired distribution of the USV swarm is defined as

[0144] σ e = σ - σ d (5)

[0145] To make the USV swarm reach the desired range, a group aggregation energy function is designed based on the Lyapunov stability theory, which is expressed as

[0146]

[0147] By taking the gradient of v i in equation (6), we get

[0148]

[0149] where g​​​​​​​​​σi represents a discrete factor, and its expression form is:

[0150]

[0151] Specifically, in the embodiment, for the cluster composed of n unmanned ships, the average distance between each unmanned ship and the virtual center point is ensured to be maintained at a preset value by calculating the average value of the sum of the distances from each unmanned ship to the virtual center point, so that flexible and stable cluster aggregation is realized.

[0152] S2: collision avoidance potential functions and obstacle avoidance potential functions are designed for adjacent unmanned ships and external obstacles, so as to avoid collision between adjacent unmanned ships and between each unmanned ship and external obstacles when the unmanned ships are aggregated;

[0153] In specific embodiments, in S2, the collision avoidance potential functions and the obstacle avoidance potential functions designed for adjacent unmanned ships and external obstacles include:

[0154] S21: collision avoidance potential functions are designed for adjacent unmanned ships inside the unmanned ship cluster, and the process is:

[0155] A circular forbidden zone and a buffer area with radii of r r and R r are respectively set around the unmanned ship, and a collision avoidance potential function is designed, which is expressed as:

[0156]

[0157] wherein U rij represents the repulsion vector of the jth unmanned ship to the ith unmanned ship, l ij is the distance between the ith unmanned ship and the jth unmanned ship, l ij = [(x i -x j ) 2 +(y i -y j ) 2 ] 1 / 2 ;

[0158] The essence of formula (9) is to establish a ring-shaped repulsion potential field in the area with r r <l ij <R r , so that other unmanned ships appearing in the area will be subjected to a radial virtual repulsion, and under the condition that the control parameters are determined, the size and direction of the virtual repulsion are only related to the relative position of the nearby unmanned ship. Therefore, by taking the partial derivative of v i with respect to formula (9), the repulsion of other unmanned ships in the unmanned ship cluster to the ith unmanned ship can be obtained, and the following formula is obtained:

[0159]

[0160] wherein g rij represents the repulsion factor, and its expression is:

[0161]

[0162] S22: Designing the obstacle avoidance potential function for the external obstacles of the unmanned vehicle cluster, the process is:

[0163] Suppose the positions of the external obstacles are known, and the number is m, the coordinates of the oth obstacle are defined as p o = (x o , y o ) T , the preliminary form of the obstacle avoidance potential function can be designed as follows:

[0164]

[0165] wherein l io represents the straight-line distance between the ith unmanned vehicle and the oth external obstacle, l io = [(x i -x o ) 2 +(y i -y o ) 2 ] 1 / 2 , R o and r o represent the maximum and minimum safety distances for avoiding external obstacles respectively;

[0166] By taking the partial derivative of v i with respect to formula (12), we get:

[0167]

[0168] wherein F ai represents the repulsive force vector of all obstacles on the ith unmanned vehicle, g aio represents the repulsion factor, and its expression is:

[0169]

[0170] Specifically, the embodiment designs the corresponding repulsive potential function for static obstacles, dynamic obstacles and adjacent unmanned vehicles by constructing a comprehensive collision avoidance strategy. According to the relative position, distance and motion state of the unmanned vehicle and the obstacle, these functions calculate and apply appropriate repulsive force to ensure that the cluster safely avoids various obstacles during cooperative navigation and ensures external safety.

[0171] S3: Design a heading consistency function to ensure heading consistency for all unmanned surface vessels;

[0172] In a specific embodiment, the process of designing the forward consensus function in S3 is as follows:

[0173] To achieve heading consistency, the heading dynamic error of unmanned surface vessels (USVs) in a USV swarm is defined as:

[0174] ψ ei =ψ i -ψ d (15)

[0175] Based on the aforementioned heading dynamic error, a Lyapunov function for heading adjustment, namely the heading consistency function, is constructed, expressed as:

[0176]

[0177] The yaw rate r of the i-th unmanned surface vessel in the attached coordinate system is obtained by equation (16). i The gradient is calculated to obtain the heading control force, which is expressed as:

[0178]

[0179] Specifically, in order to achieve heading consistency, this embodiment defines a heading dynamic error and designs a heading consistency function based on it, so that each unmanned surface vessel gradually corrects its heading while approaching the desired trajectory, ensuring that the entire cluster maintains heading consistency during navigation.

[0180] S4: Based on the aforementioned swarm aggregation energy function, collision avoidance potential function, obstacle avoidance potential function, and heading consistency function, design the desired guidance velocity of the unmanned surface vessel swarm;

[0181] In a specific embodiment, in S4, the process of designing the desired guidance velocity of the unmanned surface vessel swarm based on the swarm aggregation energy function, collision avoidance potential function, obstacle avoidance potential function, and bow alignment function is as follows:

[0182] The energy functions for swarm aggregation, collision avoidance, and obstacle avoidance designed by equations (6), (9), and (12) are superimposed to design the energy function for unmanned surface vessel swarm location planning, which is expressed as:

[0183]

[0184] Through equation (18) for ν i Finding the gradient, and using equations (7), (8), (10), (11), (13), and (14), we get:

[0185]

[0186] Where, kσ , k r , k a , and k ψ are positive parameters to be designed,

[0187] Meanwhile, in order to control the consistency of the sailing direction of each USV, a USV cluster heading planning energy function is designed based on the heading consistency function, which is expressed as:

[0188]

[0189] The gradient of r i is obtained by formula (20), which is:

[0190]

[0191] Wherein, g ψi represents the heading adjustment factor, g ψi =k ψ ψ ei ;

[0192] According to the Lyapunov stability criterion, in order to make the results of formula (19) and formula (21) less than 0, a USV cluster decision expected speed signal is designed, which is expressed as:

[0193]

[0194] Wherein, Δ ν , Δ ψ , k1 and k2 are positive parameters to be designed;

[0195] Combined with the ship motion model and based on the USV cluster decision expected speed signal, the expected guidance speed of the USV cluster is defined as:

[0196]

[0197] Specifically, in this embodiment, based on the group aggregation energy function, the collision avoidance potential function, the obstacle avoidance potential function and the heading consistency function, the self-organizing USV cluster decision expected speed signal is planned, which serves as a unified instruction to guide each USV to maintain the consistency of the cooperative heading while performing its own task (such as aggregation, obstacle avoidance), ensuring that the whole cluster completes the sailing task in an orderly and coordinated manner.

[0198] S5: A distributed extended state observer is designed, and the unknown system dynamics and external disturbances of each USV are estimated in real time by using the distributed extended state observer; the actual speed of the USV is obtained based on the unknown system dynamics and external disturbances of the USV, and a speed tracking error function is constructed by combining the actual speed of the USV with the expected guidance speed;

[0199] In specific embodiments, in S5, a distributed extended state observer is designed, and the unknown system dynamics and external disturbances of each USV are estimated in real time using the distributed extended state observer; the actual speed of the USV is obtained based on the unknown system dynamics and external disturbances of the USV, and a speed tracking error function is constructed by combining the actual speed of the USV with the expected guidance speed, and the process is as follows:

[0200] S51: the process of designing a distributed extended state observer is as follows:

[0201] Define the state variable Then, formula (2) can be expressed as:

[0202]

[0203] Wherein, h i is the unknown derivative of the extended state , h i = [h ui , h vi , h ri ] T ;

[0204] In order to effectively estimate the unknown system dynamics and external disturbance terms in the ship motion model and improve the robustness of the USV cluster, a distributed extended state observer is designed to observe the lumped unknown term in formula (2), i.e. the unknown system dynamics and external disturbances of the USV, and the lumped unknown term is regarded as an extended state. The distributed extended state observer can estimate the unknown system dynamics and external disturbances of each USV in real time, providing real-time and accurate information support for accurate control. The specific form of the distributed extended state observer is designed as follows:

[0205]

[0206] Wherein, are the estimated values of the system state , L1, L2, L3 are the parameter matrices of the distributed extended state observer, respectively;

[0207] According to the existing finite time extended state observer theorem, if the system state satisfies the condition of formula (25), the state variables of the system can be stabilized and converged to the equilibrium point in a finite time. Therefore, the accurate observation of f δi can be realized in a finite time by , which is expressed as:

[0208]

[0209] ​Specifically, in this embodiment, the distributed extended state observer estimates the lumped unknown term f in formula (2). δi (η i ,ω i (Including unknown system dynamics and external disturbances), combined with the optimal control law, the rate of change of velocity in formula (2) can be obtained, that is By obtaining the initial velocity and using the obtained rate of change of velocity, the actual velocity of the unmanned surface vessel at the next moment can be calculated. The actual velocity of the unmanned surface vessel at each subsequent moment is updated according to the updated rate of change of velocity.

[0210] Combined with the actual speed ω of the unmanned surface vessel i With the desired guidance velocity ω di The velocity tracking error function of the i-th unmanned surface vessel in the constructed unmanned surface vessel swarm is:

[0211] ω ei =ω i -ω di (27).

[0212] In a specific embodiment, the process of designing the optimal control law based on the speed tracking error function and using an adaptive dynamic programming algorithm is as follows:

[0213] Based on the aforementioned speed tracking error function, the cost function for the speed tracking error of the i-th unmanned surface vessel is defined as follows:

[0214]

[0215] in,

[0216] According to equation (28), the optimal cost function is defined as:

[0217]

[0218] Among them, Ψ(Ω) i () represents the set of all control laws;

[0219] Based on equations (2) and (26) and optimal control theory, the optimal Hamilton-Jacobi-Bellman (HJB) equation is defined as:

[0220]

[0221] Gradient descent method is used, i.e. The ideal optimal control law is:

[0222]

[0223] Since it is impossible to obtain the value in equation (31) the value of V

[0224]

[0225] where β i is a designed normal number,

[0226] The ideal neural network approximation is applied to formula (32) to obtain:

[0227]

[0228] where W i * is the ideal neural network weight, n ω is the number of neurons, S i (ω ei ) is the basis function vector, ε i (ω ei ) is the neural network approximation error, represents n ω dimensional Euclidean space;

[0229] Specifically, since the optimal cost function cannot be directly solved, and the cost function is coupled with the optimal control law. Therefore, in this embodiment, two neural networks of critic and actor are introduced to replace the original ideal neural network form to learn the cost function and the optimal control law respectively. The introduced neural network structure is the neural network in the adaptive dynamic programming algorithm.

[0230] Based on formula (33), V i * (ω ei ) and μ i * are re-expressed as:

[0231]

[0232] Since the ideal neural network weight W i * is unknown, the ideal optimal control law μ i * cannot be directly used, therefore, in order to obtain the available control input, the Actor-Critic, i.e. the critic and actor reinforcement learning neural network structure, is used to estimate the cost function V i * and the ideal optimal control law μ i * respectively, to obtain:

[0233]

[0234] wherein, represents V i * an estimate of; and are critic and actor neural network weights, respectively;

[0235] In particular, the optimal control law can be dynamically adjusted according to real-time environmental conditions and system states, ensuring that the unmanned vehicle cluster can still achieve optimal attitude consistency control in complex environments.

[0236] Since the estimated optimal control law is depicted in the world coordinate system in equation (2), the control input of the ship motion equation in equation (1) is obtained as:

[0237]

[0238] In order to obtain the update law of the critic and actor neural network weights in the cost function and the optimal control law, equation (35) is substituted into equation (30) to obtain the estimated HJB equation, denoted as:

[0239]

[0240] The error between the estimated HJB equation and the optimal HJB equation is designed as:

[0241]

[0242] A positive definite function is defined as:

[0243]

[0244] For the above equation (39), the gradient descent method is used to calculate the weight update law of the critic neural network as:

[0245]

[0246] wherein, γ ci is the critic learning rate of the i-th unmanned vehicle, and γ ci > 0;

[0247] Similarly, the weight update law of the actor neural network is:

[0248]

[0249] wherein, γ ai is the actor learning rate of the i-th unmanned vehicle, and γai > 0.

[0250] S6: Designing an optimal control law based on the speed tracking error function and using an adaptive dynamic programming algorithm, and controlling the unmanned surface vehicle cluster to stably sail based on the optimal control law.

[0251] Specifically, the present application aims to address the multiple challenges of unknown system dynamics, external disturbances and complex environmental obstacles to the sailing of the unmanned surface vehicle cluster. In order to solve the safety problem of the unmanned surface vehicle cluster in the dynamic tracking process, an innovative self-organizing cluster mechanism is constructed, so that the unmanned surface vehicle cluster can adaptively adjust its formation according to the real-time environmental dynamics, and ensure that the unmanned surface vehicle cluster can effectively avoid collision and keep the heading consistent during tracking. In view of the problem of unknown system dynamics and external disturbances of the unmanned surface vehicle, the distributed extended state observer is combined with the adaptive dynamic programming algorithm, which can estimate and compensate the unknown dynamics of the unmanned surface vehicle cluster and external disturbances in real time, and ensure that the posture of the self-organizing cooperative tracking motion of the unmanned surface vehicle cluster reaches the optimal state. The present application can accurately identify and reconstruct the unknown dynamic characteristics of the unmanned surface vehicle cluster, so as to realize efficient self-organizing cooperative tracking in complex dynamic environment while avoiding dynamic and static obstacles.

[0252] In order to verify the effectiveness of the method proposed in the embodiment, simulation test is carried out on the unmanned surface vehicle cluster, and the results are shown in Figures 3 to 7 . Figure 3 The trajectory tracking performance of the unmanned surface vehicle cluster composed of 6 unmanned surface vehicles (USV1-USV6) is shown, and the results show that the method proposed in the embodiment can ensure that the unmanned surface vehicle cluster sails along the preset reference trajectory, while keeping the distance between the internal members of the unmanned surface vehicle cluster as the preset distance, avoiding collision, and effectively dealing with static and dynamic obstacles. Figure 4 and Figure 5 respectively depict the attitude change curve and the speed change curve of each member of the self-organizing unmanned surface vehicle cluster in the process of tracking the reference trajectory. As can be seen from the figure, when the behavior of the unmanned surface vehicle cluster is adjusted, the attitude and speed of part of the unmanned surface vehicle will fluctuate significantly, but as the behavior of the unmanned surface vehicle cluster tends to be stable, these fluctuations will subside and enter the stable change interval; since the core of the embodiment is the design of the distributed cluster expected guidance speed, Figure 6 the speed tracking error curve of the unmanned surface vehicle cluster is shown, which indicates that the speed tracking error of each unmanned surface vehicle can converge to a stable state in a short time. Figure 7 The distance change curve between the members of the unmanned surface vehicle cluster is shown, and from the figure it can be seen that the distance between any two unmanned surface vehicles in the unmanned surface vehicle cluster is always kept above 10 meters during the whole sailing process, which shows that the embodiment can effectively ensure the sailing safety and distance stability between the internal members.

[0253] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present application, and are not intended to limit the present application; although the present application has been described in detail with reference to the above embodiments, those skilled in the art should understand that the technical solutions recorded in the above embodiments can be modified, or some or all of the technical features can be replaced by equivalents; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present application.

Claims

1. A method for secure tracking control of a distributed self-organizing unmanned surface vehicle (USV) swarm, the method comprising: The specific steps include: S1: establishing a ship motion model, and designing a group gathering energy function based on actual trajectory positions and reference trajectory positions of the ship motion model, for ensuring that each unmanned ship in the unmanned ship cluster is distributed around a set virtual center point; S2: designing a collision avoidance potential function and an obstacle avoidance potential function for adjacent unmanned ships and external obstacles, so as to avoid collisions between adjacent unmanned ships and between each unmanned ship and external obstacles when the unmanned ships gather; S3: designing a heading consistency function for ensuring the heading consistency of all unmanned ships; S4: designing an expected guidance speed of the unmanned ship cluster based on the group gathering energy function, the collision avoidance potential function, the obstacle avoidance potential function, and the heading consistency function; S5: designing a distributed extended state observer, and using the distributed extended state observer to estimate unknown system dynamics and external disturbances of each unmanned ship in real time; obtaining an actual speed of the unmanned ship based on the unknown system dynamics and the external disturbances of the unmanned ship, and constructing a speed tracking error function by combining the actual speed of the unmanned ship with the expected guidance speed; S6: designing an optimal control law based on the speed tracking error function and using an adaptive dynamic programming algorithm, and controlling the unmanned ship cluster to stably sail based on the optimal control law.

2. The method of claim 1, wherein, In S1, the process of establishing a ship motion model and designing a group gathering energy function based on actual trajectory positions and reference trajectory positions of the ship motion model is as follows: An unmanned ship cluster composed of n unmanned ships is set, and a ship motion equation of the i-th unmanned ship in the horizontal plane is defined as follows: where η i = [x i , y i , ψ i ] T , x i , y i , ψ i are the lateral, longitudinal coordinates and the heading of the ith USV in the inertial frame, i = 1, 2,..., n, respectively, v i = [u i , v i , r i ] T , u i , v i , r i are the surge, sway and yaw velocities of the ith USV in the body frame, τ i is the control input of the ith USV, τ i = [τ ui , τ vi , τ ri ] T , d i = [d ui , d vi , d ri ] T d ui , d vi , d ri are the external disturbances of the ith USV, M = M0+ ΔM, C = C0+ ΔC, D = D0+ ΔD, M0, C0and D0are the nominal matrices, ΔM, ΔC and ΔD are the uncertain matrices, and R(ψ i ) is the rotation matrix. For the convenience of design, let ω i = R(ψ i )υ i , the ship motion equation in formula (1) is rewritten as: where f δi (η i ,ω i ) is a lumped unknown term composed of unknown system dynamics and external disturbances, f δi (η i ,ω i ) = f(η i ,ω i ) + δ i ; f(η i ,ω i ) = C o (η i ,ω i )ω i + D o (η i ,ω i )ω i , δ i = RM -1 d i , μ i = RM -1 τ i , C o (η i ,ω i ) = -RM -1 C(R -1 ω i )R -1 , R is R(ψ i ); A reference trajectory position of the ship motion model is defined as follows: where: η d = [x d , y d , ψ d ] T , x d , y d , ψ d are the reference trajectory's horizontal and vertical coordinates and heading, respectively; υ d = [u d , v d , r d ] T , u d , v d , r d are the reference trajectory's position's forward, lateral and yaw velocities in the body frame; ω d is the desired velocity vector in the inertial frame, ω d = R d (ψ d ) υ d , R d (ψ d ) is the desired rotation matrix; μ d is the control input to the reference trajectory's position, μ d = R d (ψ d ) M -1 τ d ; In trajectory tracking, the reference trajectory position at each time is set as a virtual center point of an actual trajectory position of the unmanned ship cluster, and the distance of each unmanned ship to the virtual center point is controlled; Define the position and velocity vectors of the i-th unmanned surface vessel in the world coordinate system as p i =[x i ,y i ] T and ν i =[ω ix ,ω iy ] T Define the reference trajectory position and the velocity vector in the world coordinate system as p, respectively. d =[x d ,y d ] T and ν d =[ω dx ,ω dy ] T ; In order to control the size of the entire unmanned ship cluster, that is, the dispersion degree of the unmanned ships in the cluster, a dispersion degree function σ of the unmanned ship cluster is defined as follows: wherein, l id is the distance between the ith unmanned surface vehicle and the reference trajectory position, l id = [(x i - x d ) 2 + (y i - y d ) 2 ] 12 , σ d is a set desired aggregation quantization parameter; Based on the dispersion degree function σ, an error expression between the actual distribution and the expected distribution of the unmanned ship cluster is defined as follows: σ e = σ - σ d (5) A group gathering energy function is designed by using a method based on Lyapunov stability theory, and is expressed as follows: Through equation (6) for ν i Calculating the gradient, we get: where g σi represents a dispersion factor, expressed as:

3. The method of claim 2, wherein, In S2, the process of designing a collision avoidance potential function and an obstacle avoidance potential function for adjacent unmanned ships and external obstacles includes: S21: the process of designing a collision avoidance potential function for adjacent unmanned ships in the unmanned ship cluster is as follows: A circular forbidden zone and a buffer zone with radius r r and R r are set around the USV respectively, and a collision avoidance potential function is designed, which is expressed as: wherein, U rij represents the repulsion vector of the jth unmanned surface vehicle to the ith unmanned surface vehicle, l ij is the distance between the ith unmanned surface vehicle and the jth unmanned surface vehicle, l ij = [(x i - x j ) 2 + (y i - y j ) 2 ] 1 / 2 ; By taking the partial derivative of v i The repulsive force of other USVs in the USV swarm to the ith USV can be obtained by taking the partial derivative of v where g rij represents the collision avoidance repulsion factor, which is expressed as: S22: the process of designing an obstacle avoidance potential function for external obstacles of the unmanned ship cluster is as follows: Assuming the positions of external obstacles are known, and the number of obstacles is m, let the coordinates of the oth obstacle be defined as p o = (x o , y o ) T Then a preliminary form of the obstacle avoidance potential function can be designed as follows: wherein, l io represents the straight-line distance between the ith unmanned surface vehicle and the oth external obstacle, l io = [(x i -x o ) 2 + (y i -y o ) 2 ] 1 / 2 , R o and r o represent the maximum and minimum safety distances for avoiding external obstacles, respectively. By taking the partial derivative of v with respect to t i we obtain: where F ai represents the repulsive force vector of all obstacles to the ith unmanned surface vehicle, g aio represents the repulsion factor, which is expressed as:

4. The method of claim 3, wherein, In S3, the process of designing a heading consistency function is as follows: In order to achieve heading consistency, a heading dynamic error of the unmanned ships in the unmanned ship cluster is defined as follows: ψ ei = ψ i - ψ d (15) A Lyapunov function for heading adjustment, that is, a heading consistency function, is constructed based on the heading dynamic error, and is expressed as follows: The yaw velocity r of the ith USV in the body coordinate system is given by equation (16) i The gradient is calculated to obtain the heading control force, denoted as:

5. The method of claim 4, wherein, In S4, the process of designing an expected guidance speed of the unmanned ship cluster based on the group gathering energy function, the collision avoidance potential function, the obstacle avoidance potential function, and the heading consistency function is as follows: The group gathering energy function, the collision avoidance potential function, and the obstacle avoidance potential function designed by formula (6), formula (9), and formula (12) are superimposed, so as to design a position planning energy function of the unmanned ship cluster, and is expressed as follows: Using equation (18) for ν i Finding the gradient, and using equations (7), (8), (10), (11), (13), and (14), we get: wherein k σ , k r , k a and k ψ are positive parameters to be designed, Meanwhile, in order to control the consistency of the sailing direction of each USV, a USV cluster heading planning energy function is designed based on the heading consistency function, denoted as: By r i Taking the gradient gives: where g ψi represents a bow adjustment factor, g ψi = k ψ ψ ei ; According to the Lyapunov stability criterion, in order to make the results of formula (19) and formula (21) less than 0, a USV cluster decision expected speed signal is designed, denoted as: wherein Δ ν , Δ ψ , k1 and k2 are positive parameters to be designed. Combining the ship motion model and based on the USV cluster decision expected speed signal, the expected guidance speed of the USV cluster is defined as:

6. The method of claim 5, wherein, In S5, a distributed extended state observer is designed, and the unknown system dynamics and external disturbances of each USV are estimated in real time by using the distributed extended state observer; the actual speed of the USV is obtained based on the unknown system dynamics and external disturbances of the USV, and the process of constructing a speed tracking error function by combining the actual speed of the USV with the expected guidance speed is as follows: S51: the process of designing a distributed extended state observer is as follows: Defining state variables Equation (2) can then be expressed as: where h i is the unknown derivative of the expansion state h i = [h ui , h vi , h ri ] T ; A distributed extended state observer is designed to observe the lumped unknown term in formula (2), i.e. the unknown system dynamics and external disturbances of the USV, and the lumped unknown term is regarded as an extended state, and the specific form of the distributed extended state observer is designed as follows: wherein, are the estimated values of the system states L1, L2, L3 are the parameter matrices of the distributed extended state observer, respectively; is represented by: The actual speed ω of the unmanned vehicle i The desired guidance speed ω di The speed tracking error function of the i-th unmanned vehicle in the constructed unmanned vehicle cluster is: ω ei = ω i - ω di (27).

7. The method of claim 6, wherein, Based on the speed tracking error function and by using an adaptive dynamic programming algorithm, the process of designing an optimal control law is as follows: Based on the speed tracking error function, the cost function of the speed tracking error of the i-th USV is defined as: wherein According to formula (28), the optimal cost function is defined as: where Ψ(Ω i ) represents the set of all control laws. According to formula (2), formula (26) and the optimal control theory, the optimal Hamilton-Jacobi-Bellman (HJB) equation is defined as: The gradient descent method is used, i.e. The ideal optimal control law is designed as To obtain the usable control input, an Actor-Critic, i.e. critic and actor reinforcement learning neural network structure is used, which estimates the cost function V i * and the ideal optimal control law μ i * is estimated to obtain: wherein, represents an estimate of and are the critic and actor neural network weights, respectively; Due to the estimated optimal control law is depicted in the world coordinate system, the control input to the ship motion equation in equation (1) is obtained as: By substituting formula (35) into formula (30), the estimated HJB equation is obtained, denoted as: The error between the estimated HJB equation and the optimal HJB equation is designed as: A positive definite function is defined as: For the above formula (39), the gradient descent method is used to calculate the weight update law of the critic neural network as: wherein, γ ci is the learning rate of the i-th USV evaluator, γ ci > 0; Similarly, the weight update law of the actuator neural network is as follows: wherein γ ai is the learning rate of the i-th USV actuator, γ ai > 0.

Images