Safety tracking control method for distributed self-organizing unmanned ship cluster

Abstract

The invention discloses a safety tracking control method for a distributed self-organizing unmanned ship cluster, and the method comprises the steps: S1, designing a group gathering energy function which is used for guaranteeing that all unmanned ships in the unmanned ship cluster are distributed around a set virtual center point; s2, designing a collision avoidance potential function and an obstacle avoidance potential function so as to avoid collision between adjacent unmanned ships and collision between each unmanned ship and an external obstacle when the unmanned ships gather; s3, designing a heading consistency function for ensuring heading consistency of all the unmanned ships; s4, designing an expected guidance speed of the unmanned ship cluster; s5, designing a distributed extended state observer, and estimating unknown system dynamics and external disturbance of each unmanned ship in real time; the actual speed of the unmanned ship is obtained, and a speed tracking error function is constructed in combination with the actual speed of the unmanned ship and the expected guidance speed; and S6, designing an optimal control law based on the speed tracking error function by adopting a self-adaptive dynamic programming algorithm, and controlling the unmanned ship cluster to stably sail based on the optimal control law. According to the invention, safe and efficient cooperative control of the unmanned ship cluster can be realized, it is ensured that the unmanned ship cluster can efficiently, safely and consistently execute a tracking task in a complex environment, and the overall performance of the unmanned ship cluster is significantly improved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned boat cluster tracking control, and in particular to a distributed self-organizing unmanned boat cluster safety tracking control method. Background Art

[0002] Unmanned boat swarms, due to their advantages of concealment, autonomy and scalability, have shown unprecedented importance in performing complex and diverse maritime missions. For example, in modern naval warfare, the trajectory tracking technology of unmanned boat swarms is an indispensable key link in performing tasks such as tracking hostile targets and pursuing illegal objects. These mission environments are highly dynamic and complex and changeable, especially when they frequently encounter external obstacles such as static or dynamic obstacles formed by non-combat ships or other non-predetermined targets during tracking, which poses a severe challenge to tracking operations.

[0003] The current cooperative control strategies of unmanned boat swarms, such as those based on graph theory, virtual structure method, leader-follower method and behavior method, have promoted the realization of autonomous control of unmanned boat formations to a certain extent, but these methods generally have certain limitations: they usually rely on preset fixed formation structures and formation constraints, and cannot flexibly respond to changing navigation conditions. Especially in high dynamic environments, the rigidity of the formation structure and its high sensitivity to initial conditions limit the autonomous decision-making ability and flexible mobility of the unmanned boat swarm; in addition, when performing tasks in a real and complex marine environment, unmanned boats will encounter a large number of unpredictable external interferences, including but not limited to time-varying and unknown dynamic disturbances caused by natural factors such as sea breeze, waves, and currents, as well as the uncertainty of hydrodynamic parameters caused by the difference between the actual navigation state and the ideal model, which seriously challenges the accurate trajectory tracking control ability of the unmanned boat swarm; furthermore, given that the unmanned boat swarm is a nonlinear time-varying strongly coupled system, how to ensure that all unmanned boats in the swarm not only maintain their own stability but also achieve optimal attitude consistency in the case of uncertain model information and unknown external disturbances, so as to achieve efficient and accurate trajectory tracking goals, is also one of the key issues to be solved in this field. Summary of the invention

[0004] The present invention provides a safe tracking control method for a distributed self-organizing unmanned boat cluster, so as to overcome the technical problems existing in the existing cooperative control of unmanned boat clusters, such as insufficient flexibility of the formation structure, serious influence of environmental disturbances, and difficulty in ensuring the optimal posture consistency of the self-organizing cluster.

[0005] In order to achieve the above object, the technical solution of the present invention is:

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

[0007] S1: Establish a ship motion model, and design a group aggregation energy function based on the actual trajectory position and reference trajectory position of the ship motion model to ensure that each unmanned boat in the unmanned boat cluster is distributed around the set virtual center point;

[0008] S2: Design collision avoidance potential functions and obstacle avoidance potential functions for adjacent unmanned boats and external obstacles, so as to avoid collisions between adjacent unmanned boats and between each unmanned boat and external obstacles when the unmanned boats gather;

[0009] S3: Design a heading consistency function to ensure the heading consistency of all unmanned boats;

[0010] S4: Designing the expected guidance speed of the unmanned boat cluster based on the group aggregation energy function, collision avoidance potential function, obstacle avoidance potential function and heading consistency function;

[0011] S5: Design a distributed extended state observer, and use the distributed extended state observer to estimate the unknown system dynamics and external disturbances of each unmanned boat in real time; obtain the actual speed of the unmanned boat based on the unknown system dynamics and external disturbances of the unmanned boat, and construct a speed tracking error function by combining the actual speed of the unmanned boat with the expected guidance speed;

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

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

[0014] Assume that an unmanned boat cluster is composed of n unmanned boats, and define the ship motion equation of the i-th unmanned boat in the horizontal plane as follows:

[0015]

[0016] Among them, η i =[x i ,y i ,ψ i ] T , x i ,y i ,ψ i are the horizontal and vertical coordinates and heading of the i-th unmanned boat in the inertial coordinate system, i = 1, 2, ..., n, υ i =[u i ,v i ,r i ] T ,u i ,v i ,ri are the forward, drift and bow pitch speeds of the i-th unmanned boat in the appendage coordinate system, τ i is the control input of the i-th unmanned boat, τ i =[τ ui ,τ vi ,τ ri ] T , d i =[d ui ,d vi ,d ri ] T d ui ,d vi ,d ri are all external disturbances of the i-th unmanned boat, M = M 0 +ΔM,C=C 0 +ΔC,D=D 0 +ΔD,M 0 , C 0 and D 0 are all modeling matrices, ΔM, ΔC and ΔD are all uncertainty matrices, R(ψ i ) is the rotation matrix;

[0017] For the convenience of design, let ω i =R(ψ i ) i , rewrite the ship motion equation in equation (1) as:

[0018]

[0019] Among them, f δi (η i ,ω i ) is the lumped unknown term consisting of the 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 , Co (η 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] Where: η d =[x d ,y d ,ψ d ] T , x d ,y d ,ψ d are the horizontal and vertical coordinates and heading of the reference trajectory respectively; d =[u d ,v d ,r d ] T ,u d ,v d ,r d are the forward, drift and bow speeds of the reference trajectory position in the appendage coordinate system; ω 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 trajectory tracking, the reference trajectory position at each moment is set as the virtual center point of the actual trajectory position of the unmanned boat cluster, and the distance from each unmanned boat to the virtual center point is controlled;

[0024] The position and velocity vector of the i-th unmanned boat in the world coordinate system are defined 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 d =[x d ,y d ] T and ν d =[ω dx ,ω dy ] T ;

[0025] In order to control the size of the entire unmanned boat cluster, that is, the dispersion of the unmanned boats in the cluster, the dispersion function σ of the unmanned boat cluster is defined as:

[0026]

[0027] Among them, l id is the distance between the i-th unmanned boat and the reference trajectory position, l id =[(x i -x d ) 2 +(y i -y d ) 2 ] 12 , σ d Quantize the parameters for the desired aggregation set;

[0028] Based on the dispersion function σ, the error expression between the actual distribution and the expected distribution of the unmanned boat cluster is defined as follows:

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

[0030] The group aggregation energy function is designed based on the Lyapunov stability theory, which is expressed as:

[0031]

[0032] Through formula (6) i Find the gradient and get:

[0033]

[0034] Among them, g σi represents a discrete factor, which is expressed as:

[0035]

[0036] Furthermore, in S2, the collision avoidance potential function and obstacle avoidance potential function designed for adjacent unmanned boats and external obstacles include:

[0037] S21: Design a collision avoidance potential function for adjacent unmanned boats within the unmanned boat cluster. The process is as follows:

[0038] The radius around the unmanned boat is set to r r and R r The circular restricted area and buffer area are designed, and the collision avoidance potential function is expressed as:

[0039]

[0040] Among them, U rij represents the repulsive force vector of the j-th unmanned boat on the i-th unmanned boat, l ij is the distance between the i-th unmanned boat and the j-th unmanned boat, l ij =[(x i -x j ) 2 +(y i -y j ) 2 ] 1 / 2 ;

[0041] Through formula (9) i By finding the partial derivative, we can find the repulsive force of other unmanned boats in the unmanned boat cluster on the i-th unmanned boat, and we get:

[0042]

[0043] Among them, g rij represents the collision avoidance factor, which is expressed as:

[0044]

[0045] S22: Design an obstacle avoidance potential function for the external obstacles of the unmanned boat cluster. The process is as follows:

[0046] Assuming that the positions of 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:

[0047]

[0048] Among them, l io represents the straight-line distance between the i-th unmanned boat and the o-th external obstacle, l io =[(x i -x o ) 2 +(y i -y o ) 2] 12 , R o With r o They represent the maximum and minimum safe distances to avoid external obstacles respectively;

[0049] Through formula (12) i Taking partial derivatives, we get:

[0050]

[0051] Among them, F ai Represents the repulsive force vector of all obstacles on the i-th unmanned boat, g aio Represents the rejection factor, which is expressed as:

[0052]

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

[0054] In order to achieve heading consistency, the heading dynamic error of the unmanned boat in the unmanned boat cluster is defined as:

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

[0056] Based on the heading dynamic error, the Lyapunov function of heading adjustment, namely the heading consistency function, is constructed and expressed as:

[0057]

[0058] The bow speed r of the i-th unmanned boat in the appendage coordinate system is calculated by formula (16): i Find the gradient and get the heading control force, 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 expected guidance speed of the unmanned boat cluster is:

[0061] The group aggregation energy function, collision avoidance potential function and obstacle avoidance potential function designed by equations (6), (9) and (12) are superimposed to design the energy function of the position planning of the unmanned boat cluster, which is expressed as:

[0062]

[0063] Through formula (18) i Finding the gradient, and equations (7), (8), (10), (11), (13) and (14) yields:

[0064]

[0065] Among them, k σ , k r , k a and k ψ is the positive parameter to be designed,

[0066] At the same time, in order to control the consistency of the navigation direction of each unmanned boat, the heading planning energy function of the unmanned boat cluster is designed based on the heading consistency function, which is expressed as:

[0067]

[0068] Through formula (20) i Find the gradient and get:

[0069]

[0070] Among them, g ψi represents the heading adjustment factor, g ψi =k ψ ψ ei ;

[0071] According to the Lyapunov stability criterion, in order to make the results of equations (19) and (21) less than 0, the unmanned boat cluster decision-making expected speed signal is designed, which is expressed as:

[0072]

[0073] Among them, Δ ν , Δ ψ , k 1 and k 2 All are positive parameters to be designed;

[0074] In combination with the ship motion model and based on the unmanned boat swarm decision expected speed signal, the expected guidance speed of the unmanned boat swarm is defined as:

[0075]

[0076] Furthermore, in S5, a distributed extended state observer is designed, and the distributed extended state observer is used to estimate the unknown system dynamics and external disturbances of each unmanned boat in real time; the actual speed of the unmanned boat is obtained based on the unknown system dynamics and external disturbances of the unmanned boat, and the process of constructing a speed tracking error function by combining the actual speed of the unmanned boat with the expected guidance speed is:

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

[0078] Defining state variables Then formula (2) can be expressed as:

[0079]

[0080] Among them, h i In expansion state The unknown derivative of h i =[h ui ,h vi ,h ri ] T ;

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

[0082]

[0083] in, System status The estimated value of L 1 , L 2 , L 3 are the parameter matrices of the distributed extended state observer respectively;

[0084] It is expressed as:

[0085]

[0086] Combined with the actual speed of the unmanned boat ω i and the desired guidance speed ω di The speed tracking error function of the i-th unmanned boat in the constructed unmanned boat cluster is:

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

[0088] Furthermore, 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:

[0089] Based on the speed tracking error function, the cost function of the speed tracking error of the i-th unmanned boat is defined as:

[0090]

[0091] in,

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

[0093]

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

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

[0096]

[0097] Using the gradient descent method, The ideal optimal control law is designed as:

[0098]

[0099] In order to obtain usable control input, the Actor-Critic and the actuator reinforcement learning neural network structure are used to respectively i * and the ideal optimal control law μ i * Estimation yields:

[0100]

[0101] in, Indicates V i * estimates; and They are the neural network weights of the judge and the actuator respectively;

[0102] Since the estimated optimal control law It is described in equation (2) based on the world coordinate system. Therefore, the control input of the ship motion equation in equation (1) is:

[0103]

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

[0105]

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

[0107]

[0108] Define a positive definite function as:

[0109]

[0110] For the above formula (39), the weight update law of the judge neural network is calculated by the gradient descent method as follows:

[0111]

[0112] in, γ ci is the learning rate of the judge of the i-th unmanned boat, γ ci >0;

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

[0114]

[0115] Among them, γ ai is the actuator learning rate of the i-th unmanned boat, γ ai >0.

[0116] Beneficial effects: The present invention ensures that the unmanned boat cluster is stably distributed around the virtual center point by designing a group aggregation energy function, and at the same time introduces a collision avoidance potential function and an obstacle avoidance potential function to avoid collisions between adjacent unmanned boats and between each unmanned boat and external obstacles, and maintains the heading consistency of all unmanned boats through a heading consistency function. On this basis, the expected guidance speed of the unmanned boat cluster is planned by integrating the above functions, and the individual unmanned boats are guided to adjust the speed. In addition, the present invention also designs a distributed extended state observer to estimate the unknown system dynamics and external disturbances in real time, obtain the actual speed of the unmanned boat based on the unknown system dynamics and external disturbances of the unmanned boat, and construct a speed tracking error function by combining the actual speed of the unmanned boat and the expected guidance speed; based on the speed tracking error function and using an adaptive dynamic programming algorithm to design the optimal control law, and based on the optimal control law, control the unmanned boat cluster to sail stably, thereby effectively overcoming the problems of insufficient flexibility of the formation structure, serious impact of environmental disturbances, and difficulty in ensuring the consistency of the self-organizing cluster posture in the prior art, realizing safe and efficient collaborative control of the unmanned boat cluster, ensuring that the unmanned boat cluster can perform tracking tasks efficiently, safely and consistently in a complex environment, and significantly improving the overall performance of the unmanned boat cluster. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0118] Figure 1 It is a flow chart of a safe tracking control method of a distributed self-organizing unmanned boat cluster in the present invention;

[0119] Figure 2 This is a flow chart of the safety tracking control of a distributed self-organizing unmanned boat cluster in an embodiment of the present invention;

[0120] Figure 3 This is a trajectory tracking effect diagram of a cluster of unmanned boats in an obstacle environment according to an embodiment of the present invention;

[0121] Figure 4 is a graph showing the change in posture of the unmanned boat cluster in an embodiment of the present invention;

[0122] Figure 5 It is a speed variation curve diagram of the unmanned boat cluster in an embodiment of the present invention;

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

[0124] Figure 7 It is a relative distance curve diagram between the unmanned boats in the unmanned boat cluster in an embodiment of the present invention. DETAILED DESCRIPTION

[0125] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0126] This embodiment provides a distributed self-organizing unmanned boat swarm safety tracking control method, such as Figure 1 and Figure 2 As shown, the specific steps include:

[0127] S1: Establish a ship motion model, and design a group aggregation energy function based on the actual trajectory position and reference trajectory position of the ship motion model to ensure that each unmanned boat in the unmanned boat cluster is distributed around the set virtual center point;

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

[0129] Assume that an unmanned boat cluster is composed of n unmanned boats, and define the ship motion equation of the i-th unmanned boat in the horizontal plane as follows:

[0130]

[0131] Among them, η i =[x i ,y i ,ψ i ] T , x i ,y i ,ψ i are the horizontal and vertical coordinates and heading of the i-th unmanned boat in the inertial coordinate system, i = 1, 2, ..., n, υ i =[u i ,v i ,r i ] T ,u i ,v i ,r i are the forward, drift and bow pitch speeds of the i-th unmanned boat in the appendage coordinate system, τ i is the control input of the i-th unmanned boat, τ i =[τ ui ,τ vi ,τ ri ] T , d i =[d ui ,d vi ,d ri ] T d ui ,d vi ,d ri are all external disturbances of the i-th unmanned boat, M = M 0 +ΔM,C=C 0 +ΔC,D=D 0 +ΔD,M 0 , C 0 and D 0 are all modeling matrices, ΔM, ΔC and ΔD are all uncertainty matrices, R(ψ i ) is the rotation matrix;

[0132] For the convenience of design, let ω i =R(ψ i ) i , rewrite the ship motion equation in equation (1) as:

[0133]

[0134] Among them, f δi (η i ,ω i ) is the lumped unknown term consisting of the 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 );

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

[0136]

[0137] Where: η d =[x d ,y d ,ψ d ] T , x d ,y d ,ψ d are the horizontal and vertical coordinates and heading of the reference trajectory respectively; d =[u d ,v d ,r d ] T ,u d ,v d ,r d are the forward, drift and bow speeds of the reference trajectory position in the appendage coordinate system; ω 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] When defining trajectory tracking, the reference trajectory position at each moment is set as the virtual center point of the actual trajectory position of the unmanned boat cluster, and the distance from each unmanned boat to the virtual center point is controlled;

[0139] The position and velocity vector of the i-th unmanned boat in the world coordinate system are defined 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 d =[x d ,y d ] T and ν d =[ω dx ,ω dy ] T ;

[0140] In order to control the size of the entire unmanned boat cluster, that is, the dispersion of the unmanned boats in the cluster, the dispersion function σ of the unmanned boat cluster is defined as:

[0141]

[0142] Among them, l id is the distance between the i-th unmanned boat and the reference trajectory position, l id =[(x i -x d ) 2 +(y i -y d ) 2 ] 12 , σ d Quantize the parameters for the desired aggregation set;

[0143] Based on the dispersion function σ, the error expression between the actual distribution and the expected distribution of the unmanned boat cluster is defined as follows:

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

[0145] In order to make the unmanned boat cluster reach the desired range, the group aggregation energy function is designed based on the Lyapunov stability theory, which is expressed as:

[0146]

[0147] Through formula (6) i Find the gradient and get:

[0148]

[0149] Among them, g σi represents a discrete factor, which is expressed as:

[0150]

[0151] Specifically, in this embodiment, for a cluster composed of n unmanned boats, the average value of the sum of the distances from each unmanned boat to the virtual center point is calculated to ensure that the average distance between each unmanned boat and the virtual center point is always maintained at a preset value, thereby achieving flexible and stable cluster aggregation.

[0152] S2: Design collision avoidance potential functions and obstacle avoidance potential functions for adjacent unmanned boats and external obstacles, so as to avoid collisions between adjacent unmanned boats and between each unmanned boat and external obstacles when the unmanned boats gather;

[0153] In a specific embodiment, in S2, designing a collision avoidance potential function and an obstacle avoidance potential function for adjacent unmanned boats and external obstacles includes:

[0154] S21: Design a collision avoidance potential function for adjacent unmanned boats within the unmanned boat cluster. The process is as follows:

[0155] The radius around the unmanned boat is set to r r and R r The circular restricted area and buffer area are designed, and the collision avoidance potential function is expressed as:

[0156]

[0157] Among them, U rij represents the repulsive force vector of the j-th unmanned boat on the i-th unmanned boat, l ij is the distance between the i-th unmanned boat and the j-th unmanned boat, l ij =[(x i -x j ) 2 +(y i -y j ) 2 ] 1 / 2 ;

[0158] The essence of formula (9) is to use r r <l ij <R rA circular repulsive potential field is established in the area, so that other unmanned boats in the area will be subject to a radial virtual repulsive force. When the control parameters are determined, the magnitude and direction of the virtual repulsive force are only related to the relative position of the nearby unmanned boats. Therefore, ν is obtained by formula (9). i By finding the partial derivative, we can find the repulsive force of other unmanned boats in the unmanned boat cluster on the i-th unmanned boat, and we get:

[0159]

[0160] Among them, g rij represents the collision avoidance factor, which is expressed as:

[0161]

[0162] S22: Design an obstacle avoidance potential function for the external obstacles of the unmanned boat cluster. The process is as follows:

[0163] Assuming that the positions of 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] Among them, l io represents the straight-line distance between the i-th unmanned boat and the o-th external obstacle, l io =[(x i -x o ) 2 +(y i -y o ) 2 ] 1 / 2 , R o With r o They represent the maximum and minimum safe distances to avoid external obstacles respectively;

[0166] Through formula (12) i Taking partial derivatives, we get:

[0167]

[0168] Among them, F ai Represents the repulsive force vector of all obstacles on the i-th unmanned boat, g aio Represents the rejection factor, which is expressed as:

[0169]

[0170] Specifically, this embodiment constructs an all-round collision avoidance strategy and designs corresponding repulsive potential functions for static obstacles, dynamic obstacles and adjacent unmanned boats. These functions calculate and apply appropriate repulsive forces based on factors such as the relative position, distance, and motion state of the unmanned boat and the obstacle, ensuring that the cluster can safely avoid various obstacles during collaborative navigation and ensure external safety.

[0171] S3: Design a heading consistency function to ensure the heading consistency of all unmanned boats;

[0172] In a specific embodiment, in S3, the process of designing the heading consistency function is:

[0173] In order to achieve heading consistency, the heading dynamic error of the unmanned boat in the unmanned boat cluster is defined as:

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

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

[0176]

[0177] The bow speed r of the i-th unmanned boat in the appendage coordinate system is calculated by formula (16): i Find the gradient and get 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 this, so as to prompt each unmanned boat to gradually correct its heading while approaching the desired trajectory, ensuring that the entire cluster maintains heading consistency during navigation.

[0180] S4: Designing the expected guidance speed of the unmanned boat cluster based on the group aggregation energy function, collision avoidance potential function, obstacle avoidance potential function and heading consistency function;

[0181] In a specific embodiment, in S4, based on the group aggregation energy function, collision avoidance potential function, obstacle avoidance potential function and heading consistency function, the process of designing the expected guidance speed of the unmanned boat cluster is:

[0182] The group aggregation energy function, collision avoidance potential function and obstacle avoidance potential function designed by equations (6), (9) and (12) are superimposed to design the energy function of the position planning of the unmanned boat cluster, which is expressed as:

[0183]

[0184] Through formula (18) i Finding the gradient, and equations (7), (8), (10), (11), (13) and (14) yields:

[0185]

[0186] Among them, k σ , k r , k a and k ψ is the positive parameter to be designed,

[0187] At the same time, in order to control the consistency of the navigation direction of each unmanned boat, the heading planning energy function of the unmanned boat cluster is designed based on the heading consistency function, which is expressed as:

[0188]

[0189] Through formula (20) i Find the gradient and get:

[0190]

[0191] Among them, 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 equations (19) and (21) less than 0, the unmanned boat cluster decision-making expected speed signal is designed, which is expressed as:

[0193]

[0194] Among them, Δ ν , Δ ψ , k 1 and k 2 All are positive parameters to be designed;

[0195] In combination with the ship motion model and based on the unmanned boat swarm decision expected speed signal, the expected guidance speed of the unmanned boat swarm is defined as:

[0196]

[0197] Specifically, in this embodiment, based on the group aggregation energy function, collision avoidance potential function, obstacle avoidance potential function and heading consistency function, the expected speed signal for self-organizing unmanned boat cluster decision-making is planned. This signal serves as a unified instruction to guide each boat to maintain the consistency of the cluster's coordinated heading while performing its respective tasks (such as aggregation and obstacle avoidance), thereby ensuring that the cluster as a whole completes the navigation task in an orderly and coordinated manner.

[0198] S5: Design a distributed extended state observer, and use the distributed extended state observer to estimate the unknown system dynamics and external disturbances of each unmanned boat in real time; obtain the actual speed of the unmanned boat based on the unknown system dynamics and external disturbances of the unmanned boat, and construct a speed tracking error function by combining the actual speed of the unmanned boat with the expected guidance speed;

[0199] In a specific embodiment, in S5, a distributed extended state observer is designed, and the distributed extended state observer is used to estimate the unknown system dynamics and external disturbances of each unmanned boat in real time; the actual speed of the unmanned boat is obtained based on the unknown system dynamics and external disturbances of the unmanned boat, and the process of constructing a speed tracking error function by combining the actual speed of the unmanned boat with the expected guidance speed is:

[0200] S51: The process of designing a distributed extended state observer is:

[0201] Defining state variables Then formula (2) can be expressed as:

[0202]

[0203] Among them, h i In expansion state The unknown derivative of h i =[h ui ,h vi ,h ri ] T ;

[0204] In order to effectively estimate the unknown system dynamics and external disturbances in the ship motion model and improve the robustness of the unmanned boat cluster, a distributed extended state observer is designed to observe the lumped unknown terms in equation (2), namely the unknown system dynamics and external disturbances of the unmanned boats, and regard the lumped unknown terms as extended states. The distributed extended state observer can be used to estimate the unknown system kinetic energy and external disturbances of each unmanned boat in real time, providing real-time and accurate information support for precise control. The specific form of the distributed extended state observer is designed as follows:

[0205]

[0206] in, System status The estimated value of L 1 , L 2 , L 3 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 equation (25), the state variables of the system can be stabilized and converge to the equilibrium point in a finite time. It can realize f δi The precise observation of It 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 speed change rate in formula (2) can be obtained, that is, The initial speed is obtained, and based on the obtained speed change rate, the actual speed of the unmanned boat at the next moment can be calculated. The actual speed of the unmanned boat at each subsequent moment is updated according to the updated speed change rate.

[0210] Combined with the actual speed of the unmanned boat ω i and the desired guidance speed ω di The speed tracking error function of the i-th unmanned boat in the constructed unmanned boat cluster 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 speed tracking error function, the cost function of the speed tracking error of the i-th unmanned boat is defined as:

[0214]

[0215] in,

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

[0217]

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

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

[0220]

[0221] Using the gradient descent method, The ideal optimal control law is designed as:

[0222]

[0223] Since it is impossible to obtain Therefore, the optimal cost function of formula (29) is redesigned as:

[0224]

[0225] Among them, β i is a designed positive constant,

[0226] Using an ideal neural network approximation for equation (32), we get:

[0227]

[0228] Among them, W i * are the ideal neural network weights, n ω is the number of neurons, S i (ω ei ) is the basis function vector, ε i (ω ei ) is the neural network approximation error, Indicates n ω dimensional Euclidean space;

[0229] Specifically, since the optimal cost function cannot be solved directly, and the cost function is coupled with the optimal control law, this embodiment introduces two neural networks, the judge and the actuator, to replace the original ideal neural network form to learn the cost function and the optimal control law separately. 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 * Re-expressed as:

[0231]

[0232] Since the ideal neural network weight W i * is unknown, so the ideal optimal control law μ i * It cannot be used directly. Therefore, in order to obtain usable control input, the Actor-Critic, i.e., the judge and the actuator reinforcement learning neural network structure, is used to respectively i * and the ideal optimal control law μ i * Estimation yields:

[0233]

[0234] in, Indicates V i * estimates; and They are the neural network weights of the judge and the actuator respectively;

[0235] Specifically, the optimal control law can be dynamically adjusted according to real-time environmental conditions and system status to ensure that the unmanned boat cluster can still achieve optimal attitude consistency control in a complex environment.

[0236] Since the estimated optimal control law It is described in equation (2) based on the world coordinate system. Therefore, the control input of the ship motion equation in equation (1) is:

[0237]

[0238] In order to obtain the update law of the weights of the neural network of the judge and the actuator in the cost function and the optimal control law, substitute equation (35) into equation (30) to obtain the estimated HJB equation, which is expressed as:

[0239]

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

[0241]

[0242] Define a positive definite function as:

[0243]

[0244] For the above formula (39), the weight update law of the judge neural network is calculated by the gradient descent method as follows:

[0245]

[0246] in, γ ci is the learning rate of the judge of the i-th unmanned boat, γ ci >0;

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

[0248]

[0249] Among them, γ ai is the actuator learning rate of the i-th unmanned boat, γ ai >0.

[0250] S6: Design an optimal control law based on the speed tracking error function and using an adaptive dynamic programming algorithm, and control the unmanned boat cluster to navigate stably based on the optimal control law.

[0251] Specifically, the present invention aims to cope with the multiple challenges of unknown system dynamics, external disturbances and complex environmental obstacles to the navigation of unmanned boat clusters. In order to solve the safety problems of unmanned boat clusters in the dynamic tracking process, an innovative self-organizing cluster mechanism is constructed, so that the unmanned boat cluster can dynamically and adaptively adjust its formation according to the real-time environment, ensuring that the unmanned boat cluster can effectively avoid collisions and maintain a consistent bow direction during the tracking process. In view of the problem of unknown system dynamics and external disturbances in unmanned boats, the distributed extended state observer is combined with an adaptive dynamic programming algorithm to ensure that the posture of the self-organized collaborative tracking movement of the unmanned boat cluster is optimized while estimating and compensating for the unknown dynamics and external interference inside the unmanned boat cluster in real time. The present invention can accurately identify and reconstruct the unknown dynamic characteristics of the unmanned boat cluster, thereby achieving efficient self-organized collaborative tracking in a complex dynamic environment while avoiding dynamic and static obstacles.

[0252] In order to verify the effectiveness of the method proposed in this embodiment, a simulation test of the unmanned boat cluster was carried out. The results are as follows: Figures 3 to 7 shown. Figure 3 The trajectory tracking performance of an unmanned boat cluster consisting of 6 unmanned boats (USV1-USV6) was demonstrated. The results showed that the method proposed in this embodiment can ensure that the unmanned boat cluster navigates along a preset reference trajectory, while maintaining the expected spacing between members within the unmanned boat cluster to avoid collisions, and can effectively deal with static and dynamic obstacles. Figure 4 and Figure 5The attitude change curve and speed change curve of each member of the self-organizing unmanned boat cluster in the process of tracking the reference trajectory are depicted respectively. It can be seen from the figure that when the behavior of the unmanned boat cluster is adjusted, the attitude and speed of some unmanned boats will fluctuate significantly, but as the behavior of the unmanned boat cluster tends to stabilize, these fluctuations will also subside and enter a stable change range; given that the core of this embodiment is the design of the expected guidance speed of the distributed cluster, Figure 6 The speed tracking error curve of the unmanned boat cluster is displayed, which shows that the speed tracking error of each unmanned boat can converge to a stable state in a short time. Figure 7 The distance change curve between the members of the unmanned boat cluster is shown. It can be seen from the figure that during the entire navigation process, the distance between any two unmanned boats in the unmanned boat cluster is always maintained above 10 meters, indicating that this embodiment can effectively ensure the navigation safety and spacing stability between internal members.

[0253] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A safe tracking control method for a distributed self-organizing unmanned boat swarm, characterized in that: The specific steps include: S1: Establish a ship motion model, and design a group aggregation energy function based on the actual trajectory position and reference trajectory position of the ship motion model to ensure that each unmanned boat in the unmanned boat cluster is distributed around the set virtual center point; S2: Design collision avoidance potential functions and obstacle avoidance potential functions for adjacent unmanned boats and external obstacles, so as to avoid collisions between adjacent unmanned boats and between each unmanned boat and external obstacles when the unmanned boats gather; S3: Design a heading consistency function to ensure the heading consistency of all unmanned boats; S4: Designing the expected guidance speed of the unmanned boat cluster based on the group aggregation energy function, collision avoidance potential function, obstacle avoidance potential function and heading consistency function; S5: Design a distributed extended state observer, and use the distributed extended state observer to estimate the unknown system dynamics and external disturbances of each unmanned boat in real time; obtain the actual speed of the unmanned boat based on the unknown system dynamics and external disturbances of the unmanned boat, and construct a speed tracking error function by combining the actual speed of the unmanned boat with the expected guidance speed; S6: Design an optimal control law based on the speed tracking error function and using an adaptive dynamic programming algorithm, and control the unmanned boat cluster to navigate stably based on the optimal control law.

2. The safe tracking control method of the distributed self-organizing unmanned boat swarm according to claim 1 is characterized in that: In S1, the process of establishing a ship motion model and designing a group aggregation energy function based on the actual trajectory position and reference trajectory position of the ship motion model is as follows: Assume that an unmanned boat cluster is composed of n unmanned boats, and define the ship motion equation of the i-th unmanned boat in the horizontal plane as follows: Among them, η i =[x i ,y i ,ψ i ] T , x i ,y i ,ψ i are the horizontal and vertical coordinates and heading of the i-th unmanned boat in the inertial coordinate system, i = 1, 2, ..., n, υ i =[u i ,v i ,r i ] T ,u i ,v i ,r i are the forward, drift and bow pitch speeds of the i-th unmanned boat in the appendage coordinate system, τ i is the control input of the i-th unmanned boat, τ i =[τ ui ,τ vi ,τ ri ] T , d i =[d ui ,d vi ,d ri ] T d ui ,d vi ,d ri are all external disturbances of the i-th unmanned boat, M=M0+ΔM, C=C0+ΔC, D=D0+ΔD, M0, C0 and D0 are all modeling matrices, ΔM, ΔC and ΔD are all uncertainty matrices, R(ψ i ) is the rotation matrix; For the convenience of design, let ω i =R(ψ i ) i , rewrite the ship motion equation in equation (1) as: Among them, f δi (η i ,ω i ) is the lumped unknown term consisting of the 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 ); The 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 horizontal and vertical coordinates and heading of the reference trajectory respectively; d =[u d ,v d ,r d ] T ,u d ,v d ,r d are the forward, drift and bow speeds of the reference trajectory position in the appendage coordinate system; ω 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 ; When defining trajectory tracking, the reference trajectory position at each moment is set as the virtual center point of the actual trajectory position of the unmanned boat cluster, and the distance from each unmanned boat to the virtual center point is controlled; The position and velocity vector of the i-th unmanned boat in the world coordinate system are defined 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 d =[x d ,y d ] T and ν d =[ω dx ,ω dy ] T ; In order to control the size of the entire unmanned boat cluster, that is, the dispersion of the unmanned boats in the cluster, the dispersion function σ of the unmanned boat cluster is defined as: Among them, l id is the distance between the i-th unmanned boat and the reference trajectory position, l id =[(x i -x d ) 2 +(y i -y d ) 2 ] 12 , σ d Quantize the parameters for the desired aggregation set; Based on the dispersion function σ, the error expression between the actual distribution and the expected distribution of the unmanned boat cluster is defined as follows: s e =s-s d (5) The group aggregation energy function is designed based on the Lyapunov stability theory, which is expressed as: Through formula (6) i Finding the gradient, we get: Among them, g σi represents a discrete factor, which is expressed as:

3. The safe tracking control method of the distributed self-organizing unmanned boat swarm according to claim 2 is characterized in that: In S2, the collision avoidance potential function and obstacle avoidance potential function designed for adjacent unmanned boats and external obstacles include: S21: Design a collision avoidance potential function for adjacent unmanned boats within the unmanned boat cluster. The process is as follows: The radius around the unmanned boat is set to r r and R r The circular restricted area and buffer area are designed, and the collision avoidance potential function is expressed as: Among them, U rij represents the repulsive force vector of the j-th unmanned boat on the i-th unmanned boat, l ij is the distance between the i-th unmanned boat and the j-th unmanned boat, l ij =[(x i -x j ) 2 +(y i -y j ) 2 ] 1 / 2 ; Through formula (9) i By finding the partial derivative, we can find the repulsive force of other unmanned boats in the unmanned boat cluster on the i-th unmanned boat, and we get: Among them, g rij represents the collision avoidance factor, which is expressed as: S22: Design an obstacle avoidance potential function for the external obstacles of the unmanned boat cluster. The process is as follows: Assuming that the positions of 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: Among them, l io represents the straight-line distance between the i-th unmanned boat and the o-th external obstacle, l io =[(x i -x o ) 2 +(y i -y o ) 2 ] 1 / 2 , R o With r o They represent the maximum and minimum safe distances to avoid external obstacles respectively; Through formula (12) i Taking partial derivatives, we get: Among them, F ai Represents the repulsive force vector of all obstacles on the i-th unmanned boat, g aio Represents the rejection factor, which is expressed as:

4. The safe tracking control method of the distributed self-organizing unmanned boat swarm according to claim 3 is characterized in that: In S3, the process of designing the heading consistency function is: In order to achieve heading consistency, the heading dynamic error of the unmanned boat in the unmanned boat cluster is defined as: ψ ei =ψ i -ψ d (15) Based on the heading dynamic error, the Lyapunov function of heading adjustment, namely the heading consistency function, is constructed and expressed as: The bow speed r of the i-th unmanned boat in the appendage coordinate system is calculated by formula (16): i Find the gradient and get the heading control force, which is expressed as:

5. The safe tracking control method of the distributed self-organizing unmanned boat swarm according to claim 4 is characterized in that: In S4, based on the group aggregation energy function, collision avoidance potential function, obstacle avoidance potential function and heading consistency function, the process of designing the expected guidance speed of the unmanned boat cluster is: The group aggregation energy function, collision avoidance potential function and obstacle avoidance potential function designed by equations (6), (9) and (12) are superimposed to design the energy function of the position planning of the unmanned boat cluster, which is expressed as: Through formula (18) i Finding the gradient, and equations (7), (8), (10), (11), (13) and (14) yields: Among them, k σ , k r , k a and k ψ is the positive parameter to be designed, At the same time, in order to control the consistency of the navigation direction of each unmanned boat, the heading planning energy function of the unmanned boat cluster is designed based on the heading consistency function, which is expressed as: Through formula (20) i Find the gradient and get: Among them, g ψi represents the heading adjustment factor, g ψi =k ψ ψ ei ; According to the Lyapunov stability criterion, in order to make the results of equations (19) and (21) less than 0, the unmanned boat cluster decision-making expected speed signal is designed, which is expressed as: Among them, Δ ν , Δ ψ , k1 and k2 are all positive parameters to be designed; In combination with the ship motion model and based on the unmanned boat swarm decision expected speed signal, the expected guidance speed of the unmanned boat swarm is defined as:

6. The safe tracking control method of the distributed self-organizing unmanned boat swarm according to claim 5 is characterized in that: In S5, a distributed extended state observer is designed, and the distributed extended state observer is used to estimate the unknown system dynamics and external disturbances of each unmanned boat in real time; the actual speed of the unmanned boat is obtained based on the unknown system dynamics and external disturbances of the unmanned boat, and the process of constructing a speed tracking error function by combining the actual speed of the unmanned boat with the expected guidance speed is as follows: S51: The process of designing a distributed extended state observer is: Defining state variables Then formula (2) can be expressed as: Among them, h i In expansion state The unknown derivative of h i =[h ui ,h vi ,h ri ] T ; A distributed extended state observer is designed to observe the lumped unknown items in equation (2), i.e., the unknown system dynamics and external disturbances of the unmanned boat, and the lumped unknown items are regarded as extended states. The specific form of the distributed extended state observer is designed as follows: in, System status The estimated value of , L1, L2, L3 are the parameter matrices of the distributed extended state observer; It is expressed as: Combined with the actual speed of the unmanned boat ω i and the desired guidance speed ω di The speed tracking error function of the i-th unmanned boat in the constructed unmanned boat cluster is: oh ei =ω i -oh di (27)。 7. The safe tracking control method of the distributed self-organizing unmanned boat swarm according to claim 6 is characterized in that: 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: Based on the speed tracking error function, the cost function of the speed tracking error of the i-th unmanned boat is defined as: in, According to formula (28), the optimal cost function is defined as: Among them, Ψ(Ω i ) represents the set of all control laws; According to equation (2), equation (26) and optimal control theory, the optimal Hamilton-Jacobi-Bellman (HJB) equation is defined as: Using the gradient descent method, The ideal optimal control law is designed as: In order to obtain usable control input, the Actor-Critic and the actuator reinforcement learning neural network structure are used to respectively i * and the ideal optimal control law μ i * Estimation yields: in, express estimates; and They are the neural network weights of the judge and the actuator respectively; Since the estimated optimal control law It is described in equation (2) based on the world coordinate system. Therefore, the control input of the ship motion equation in equation (1) is: Substituting equation (35) into equation (30), we obtain the estimated HJB equation, which is expressed as: The error between the estimated HJB equation and the optimal HJB equation is designed to be: Define a positive definite function as: For the above formula (39), the weight update law of the judge neural network is calculated by the gradient descent method as follows: in, γ ci is the learning rate of the judge of the i-th unmanned boat, γ ci >0; Similarly, the weight update law of the actuator neural network is: Among them, γ ai is the actuator learning rate of the i-th unmanned boat, γ ai >0.

Images