Unmanned surface vehicle safe formation control method based on completely distributed observer

Abstract

According to the unmanned surface vehicle safe formation control method based on the fully distributed observer, the adaptive fully distributed observer is established through the leader state in an unmanned surface vehicle formation system so as to acquire the estimated navigation system state of the unmanned surface vehicle; establishing a distributed controller according to an estimated value of an unknown nonlinear function based on a self-adaptive fuzzy predictor, and combining with a control obstacle function to obtain longitudinal and transverse optimization control input of the unmanned ship under the condition of considering the influence of an obstacle, and finally obtaining the optimal longitudinal control input of the unmanned ship and the actual course control input of the unmanned ship. And the control of the safe formation of the unmanned surface vehicles is completed. Compared with an existing navigation system state estimation method, the method has the advantages that the signals of the navigation system can be estimated under the condition that global information of a network is unknown by utilizing limited local information and the state of the navigation system, so that an unmanned ship formation control system cannot be greatly influenced due to a fault of a certain node, unknown parameters of the navigation system can be estimated, and the reliability of the navigation system is improved. And the reliability of formation control of the unmanned surface vehicles is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned boat control, and in particular to an unmanned boat safety formation control method based on a fully distributed observer. Background Art

[0002] With the rapid development of science and technology, unmanned boats, as a new type of intelligent water equipment, have shown great application potential in many fields such as marine scientific research, marine resource exploration, maritime security patrol, port logistics and transportation, and military operations. Unmanned boats can autonomously navigate and perform tasks in complex and dangerous water environments, avoiding the risks of personnel facing bad sea conditions and dangerous areas. At the same time, they can achieve 24-hour uninterrupted operation, greatly improving the operation efficiency and the accuracy of task execution. For example, in marine environmental monitoring, unmanned boats can be equipped with various sensors to conduct long-term and large-scale monitoring of seawater quality, meteorological parameters, marine life, etc.; in the field of maritime security, they can be used for border patrols and sea area monitoring to promptly detect abnormal situations such as illegal intrusion boats.

[0003] In many practical application scenarios, the capabilities of a single unmanned boat are often limited. The collaborative work of multiple unmanned boats in a formation can significantly improve the efficiency of mission execution. For example, in large-scale ocean search and rescue missions, a formation of multiple unmanned boats can quickly and comprehensively cover the search area and improve the success rate of rescue; when transporting materials at sea, formation sailing can optimize the transportation route, improve transportation efficiency and reduce transportation costs. At present, many scholars have proposed a variety of unmanned boat formation control methods, but the existing control methods still have some problems:

[0004] First, most of the existing unmanned boat formation control methods rely on prior network global information related to the communication topology. If a certain unmanned boat in the formation fails, such as a communication device failure or a power system failure, its position in the communication topology will be lost. In this case, the control method that relies on prior network global information will be greatly affected by the lack of information about this node.

[0005] Second, some existing control methods usually assume that the system matrix of the pilot system is known for all unmanned boats, which may be undesirable in some applications. When the parameters of the pilot system are known, estimating the state of the pilot system will lead to great limitations in the formation control method of unmanned boats.

[0006] Third, in actual work scenarios, the complex marine ecological environment may have a large number of dynamic or static obstacles (such as coral reef areas, marine biological gathering areas, etc.). When unmanned boats are sailing in formation, they need to avoid collisions with other unmanned boats and obstacles in the ocean to ensure navigation safety. Summary of the invention

[0007] The present invention provides an unmanned boat safety formation control method based on a fully distributed observer to overcome the above technical problems.

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

[0009] A safe formation control method for unmanned boats based on a fully distributed observer comprises the following steps:

[0010] S1: Establish a model of an underactuated unmanned boat in a multi-unmanned boat formation system composed of underactuated unmanned boat models; and simultaneously obtain the leader status in the unmanned boat formation system;

[0011] S2: According to the model of the underactuated unmanned boat and the leader state in the unmanned boat formation system, an adaptive fully distributed observer is established to obtain the estimated pilot system state of the unmanned boat;

[0012] S3: Establish an adaptive fuzzy predictor to obtain the estimated value of the unknown nonlinear function;

[0013] S4: establishing a distributed controller according to the estimated state of the navigation system of the unmanned boat and the estimated value of the unknown nonlinear function to obtain the longitudinal and lateral control inputs of the unmanned boat in the earth coordinate system;

[0014] S5: Establishing a control obstacle function; based on the longitudinal and lateral control inputs of the unmanned boat in the earth coordinate system and the safety constraints of the unmanned boat formation system, obtaining the longitudinal and lateral optimized control inputs of the unmanned boat under the influence of obstacles;

[0015] S6: According to the adaptive fuzzy predictor and the longitudinal and lateral optimized control inputs of the unmanned boat considering the influence of obstacles, the optimal longitudinal control input of the unmanned boat and the actual heading control input of the unmanned boat are obtained, so as to complete the control of the unmanned boat safety formation according to the optimal longitudinal control input of the unmanned boat and the actual heading control input of the unmanned boat.

[0016] Furthermore, in S2, the adaptive fully distributed observer is established as follows:

[0017]

[0018] in,

[0019]

[0020] Where: i represents the estimated navigation system state matrix of the i-th unmanned boat, where χ i ∈R 6 , R 6 represents a 6-dimensional Euclidean space. When i = 0, χ0 Represents the matrix related to the leadership system status; Represents χ i The first derivative of E i represents the estimated leadership system parameter matrix of the i-th unmanned boat; ρ i (t) represents the dynamic gain of the i-th unmanned boat; i and j represent the index number of the unmanned boat; I represents the total number of unmanned boats; a ij (t) represents the connectivity coefficient between the i-th unmanned boat and the j-th unmanned boat; χ j represents the estimated state of the navigation system of the jth unmanned boat; Represents ρ i The first derivative of (t); γ i is the adaptive gain; l i is a scalar; represents the sum of the differences between the estimated navigation system state of the i-th unmanned boat and the estimated navigation system state of the unmanned boat connected to the i-th unmanned boat; φ i represents the frequency of the multi-tone sinusoidal signal of the navigation system estimated by the i-th unmanned boat, col represents a column vector, The first and second elements of the column vector of the frequencies of the multi-tone sinusoidal signal are respectively; Represents φ i The first derivative of ; block diag{·} represents a block diagonal matrix; 0 2×2 Represents a square matrix whose 2nd-order elements are zero; represents the Kronecker product, and a represents a known second-order matrix.

[0021] Furthermore, in S1, the model of the underactuated unmanned boat is established as follows:

[0022]

[0023] Where: h i (t) represents the position and direction of the i-th unmanned boat in the earth coordinate system, where h i (t) = [x i (t),y i (t),ζ i (t)] T ∈R 3 , x i (t) and y i (t) represent the horizontal and vertical coordinates of the position in the earth coordinate system, ζ i (t) represents the heading angle of the i-th unmanned boat, where ζ i (t)∈(-π,π]; R 3 represents 3-dimensional Euclidean space; T represents transpose; represents the rotation matrix; where diag(·) represents a diagonal matrix; J i (ζ i (t)) represents a 2×2 matrix formed by the intersection of the first two rows and the first two columns of the rotation matrix, where J i (ζ i (t)) = [cos(ζ i (t)) -sin(ζ i (t)) sin(ζ i (t)) cos(ζ i (t))];ζ i (t) represents the heading angle of the i-th unmanned boat, where ζ i (t)∈(-π,π]; μ i (t) represents the speed of the i-th unmanned boat in the ship coordinate system, where μ i (t)=[u i (t) v i (t) r i (t)] T ∈R 3 ,u i (t) represents the longitudinal velocity, v i (t) represents the sway velocity, r i (t) represents the bow angular velocity of the i-th unmanned boat; Represents μ i The first derivative of (t); R 3 represents 3-dimensional Euclidean space; M i represents the inertia matrix of the i-th unmanned boat, where All represent parameters related to the quality of unmanned boats; R 3×3 represents a 3×3 dimensional Euclidean space; represents the Coriolis force, centripetal force matrix and nonlinear damping matrix; δ i (t) is the external disturbance caused by complex airflow and extreme weather to the i-th unmanned boat; σ i (t) represents the control input of the i-th unmanned boat, where σ i (t)=[σ i1 (t) 0 σ i3 (t)] T , σ i1 (t) is the longitudinal control input, σ i3 (t) is the yaw control input; i represents the index number of the unmanned boat, where i = {1...I}, and I represents the total number of unmanned boats;

[0024] Let d i =R i(ζ i (t))[u i (t),v i (t)] T ,but

[0025]

[0026] Where: b i represents the position vector of the i-th unmanned boat; Indicates b i The first derivative of i represents the speed of the i-th unmanned boat in the earth coordinate system; Indicates d i The first derivative of Γ i d (ζ i (t),μ i (t), t) represents the unknown nonlinear disturbance to the position state of the unmanned boat; ζ i (t) represents the heading angle of the i-th unmanned boat; t represents time; represents the generalized longitudinal and lateral control inputs of the unmanned vehicle in the earth coordinate system; Indicates i The first derivative of (t); r i (t) represents the angular velocity of the i-th unmanned boat; Represents r i The first derivative of (t); Γ i r (ζ i (t),μ i (t), t) represents the unknown nonlinear disturbance to the heading state of the unmanned boat; μ i (t) represents the speed of the i-th unmanned boat in the ship coordinate system; Represents the generalized heading control input of the unmanned boat.

[0027] Furthermore, in S1, the leader status in the unmanned boat formation system is expressed as follows:

[0028]

[0029] b 0 =Fx 0

[0030]

[0031] Where: 0 Represents the matrix related to the leadership system status; Represents χ 0 The first derivative of; F∈R 2×6 is a constant matrix; R2×6 represents 2×6 dimensional Euclidean space; b 0 Represents the position information of the leader system composed of multi-tone sinusoidal signals; E∈R 6×6 Represents the parameter matrix of the leadership system; R 6×6 represents a 6×6 dimensional Euclidean space; block diag represents a block diagonal matrix, 0 2×2 represents a square matrix with 2nd-order elements being zero, diag represents a diagonal matrix, Both represent the frequency of multi-tone sinusoidal signals. represents the Kronecker product, and a represents a known second-order matrix.

[0032] Further, in S3, the adaptive fuzzy predictor is established as follows:

[0033]

[0034] Where: Represents μ i The estimated value of (t), express The first derivative of i is the weight matrix; H i Estimated value; β i is a known activation function; σ i (t) represents the control input of the i-th unmanned boat; K i represents the estimator gain; It represents the error between the estimated speed and the actual speed; Represents the unknown nonlinear function Γ of the unmanned boat i The estimated value of Γ i represents the unknown nonlinear function of the unmanned boat; ε i represents the function reconstruction error; for The first-order derivative of; Λ represents the adaptive gain; k w is a constant.

[0035] Further, in S4, the distributed controller is established as follows:

[0036]

[0037] b ic =Fx i

[0038]

[0039] Where: is the virtual control quantity of the designed i-th unmanned boat; J irepresents a 2×2 matrix consisting of the first two rows and the first two columns of the rotation matrix; b ic represents the estimated position of the leadership system of the i-th unmanned boat, Indicates b ic The first derivative of; α is a given parameter; b i represents the position vector of the i-th unmanned boat; θ i represents the formation error of the i-th unmanned boat; F is a constant matrix; χ i represents the estimated navigation system state of the i-th unmanned boat; H is a given scalar; b id Represents the virtual control quantity after filtering; Represents the derivative of the virtual control quantity after filtering; represents the longitudinal and lateral control input of the i-th unmanned boat in the earth coordinate system; represents the position control gain; d i represents the speed of the i-th unmanned boat in the earth coordinate system; Represents the estimated value of the unknown nonlinear function of the i-th unmanned boat The first two dimensions of are the estimates of the unknown nonlinear disturbances to which the position-dependent state is subjected.

[0040] Furthermore, in S5, the control obstacle function is established as follows:

[0041]

[0042] Where: represents the control barrier function; T represents transpose; for b ik The first derivative of 3 (g ik (b)) is a continuous and strictly increasing function with a domain of [0,∞); ‖b ik ‖ represents b ik The second norm of ;

[0043] in,

[0044] g ik (b)=‖b ik ‖-D k

[0045] b ik =b i -o k

[0046] Where: g ik (b) indicates the relative position between the unmanned boat and the obstacle after adding the safety distance; k indicates the number of the obstacle; b ik represents the distance between the unmanned boat and the obstacle, b irepresents the position vector of the i-th unmanned boat, o k is the position vector of the obstacle; D k represents the maximum safe distance for the unmanned boat to avoid collision with obstacles; ‖b ik ‖ represents b ik The second norm of .

[0047] Furthermore, in S5, the formula used to obtain the longitudinal and lateral optimization control inputs of the unmanned boat considering the influence of obstacles is as follows:

[0048]

[0049] Where: represents the longitudinal and lateral optimized control input of the i-th unmanned boat considering the influence of obstacles, that is, the control input that can achieve the collision and obstacle avoidance function; represents the longitudinal and lateral control input of the i-th unmanned boat in the earth coordinate system, that is, the control input when only the underlying control task is realized; represents the longitudinal and lateral control inputs of the i-th unmanned boat that satisfies all safety constraints; Represents searching for a function in real number space When the minimum value is obtained The value of represents the objective function of quadratic programming.

[0050] Furthermore, in S6, the optimal longitudinal control input of the unmanned boat is obtained as follows:

[0051]

[0052] Where: represents the optimal longitudinal control input of the i-th unmanned boat; Parameters related to the quality of unmanned boats; and They represent the virtual longitudinal control input and virtual lateral control input of the unmanned boat in the earth coordinates respectively.

[0053] Furthermore, in S6, the actual heading control input of the unmanned boat is obtained as follows:

[0054]

[0055]

[0056] Where: r ic is the virtual control quantity of the designed i-th unmanned boat; ζ ic represents the expected heading signal of the i-th unmanned boat; α r is a positive constant; i (t) represents the actual heading of the i-th unmanned boat; represents the virtual heading control input of the i-th unmanned boat; r i (t) represents the bow angular velocity of the i-th unmanned boat; represents the heading control gain of the i-th unmanned boat; Represents the unknown nonlinear term estimated for the i-th unmanned boat The third dimension of is the estimated value of the unknown nonlinear disturbance to the direction-dependent state; atan2 represents the inverse tangent function;

[0057] but

[0058]

[0059] Where: represents the actual heading control input of the i-th unmanned boat; Represents parameters related to the mass of the unmanned boat.

[0060] Beneficial effects: The invention discloses a method for controlling the safe formation of unmanned boats based on a fully distributed observer. An adaptive fully distributed observer is established through the leader state in the unmanned boat formation system to obtain the estimated state of the pilot system of the unmanned boat; and a distributed controller is established based on the estimated value of the unknown nonlinear function based on the adaptive fuzzy predictor, and the control obstacle function is combined to obtain the longitudinal and lateral optimized control inputs of the unmanned boat under the influence of obstacles, and finally the optimal longitudinal control input of the unmanned boat and the actual heading control input of the unmanned boat are obtained to complete the control of the safe formation of the unmanned boat. The invention adopts an adaptive fully distributed observer to estimate the state of the pilot system. Compared with the existing method for estimating the state of the pilot system, the invention uses limited local information and its own state to estimate the signal of the pilot system when the global information of the network is unknown, so that the unmanned boat formation control system will not be greatly affected by the failure of a certain node, and the unknown parameters of the pilot system can be estimated, thereby improving the reliability of the formation control of the unmanned boat. At the same time, when the unmanned boat safety formation control task is executed, even if the unmanned boat encounters a dynamic or static obstacle, the formation control task can still be completed while ensuring the safe collision avoidance between the unmanned boats and the collision avoidance of the obstacle. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] 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.

[0062] Figure 1It is a flow chart of the unmanned boat safety formation control method based on a fully distributed observer of the present invention;

[0063] Figure 2a A schematic diagram showing the error between the navigation system state estimated by the fully distributed observer and the actual navigation system state of four unmanned boats in the embodiment of the present invention in the x-direction in the earth coordinate system;

[0064] Figure 2b A schematic diagram showing the error between the navigation system state estimated by the fully distributed observer and the actual navigation system state of four unmanned boats in the y direction in the earth coordinate system in an embodiment of the present invention;

[0065] Figure 3a A schematic diagram showing the states of four unmanned boats in the x-direction in the earth coordinate system and the following error of the actual navigation system state in an embodiment of the present invention is shown;

[0066] Figure 3b A schematic diagram showing the status of four unmanned boats in the y direction in the earth coordinate system and the following error of the actual navigation system state in an embodiment of the present invention is shown.

[0067] Figure 4a A schematic diagram of the trajectories of four unmanned boats in an embodiment of the present invention is shown;

[0068] Figure 4b A schematic diagram of collision and obstacle avoidance of four unmanned boats in an embodiment of the present invention is shown. DETAILED DESCRIPTION

[0069] 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.

[0070] This embodiment provides a method for controlling a safe formation of unmanned boats based on a fully distributed observer. Figure 1 As shown; including the following steps:

[0071] S1: Establish a model of an underactuated unmanned boat in a multi-unmanned boat formation system composed of underactuated unmanned boat models; and simultaneously obtain the leader status in the unmanned boat formation system;

[0072] Specifically, this embodiment considers a multi-unmanned boat formation system composed of multiple under-actuated unmanned boats, and the under-actuated unmanned boat model is used to obtain the position and speed information of the unmanned boats and the position and speed information of obstacles through sensors.

[0073] Preferably, the model of the underactuated unmanned boat is established as follows:

[0074]

[0075] Where: h i (t) represents the position and direction of the i-th unmanned boat in the earth coordinate system, where h i (t) = [x i (t),y i (t),ζ i (t)] T ∈R 3 , x i (t) and y i (t) represent the horizontal and vertical coordinates of the position in the earth coordinate system, ζ i (t) represents the heading angle of the i-th unmanned boat, where ζ i (t)∈(-π,π]; R 3 represents 3-dimensional Euclidean space; T represents transpose; represents the rotation matrix; where diag(·) represents a diagonal matrix; J i (ζ i (t)) represents a 2×2 matrix formed by the intersection of the first two rows and the first two columns of the rotation matrix, where J i (ζ i (t)) = [cos(ζ i (t)) -sin(ζ i (t)) sin(ζ i (t)) cos(ζ i (t))];ζ i (t) represents the heading angle of the i-th unmanned boat, where ζ i (t)∈(-π,π]; μ i (t) represents the speed of the i-th unmanned boat in the ship coordinate system, where μ i (t)=[u i (t) v i (t) r i (t)] T ∈R 3 ,u i (t) represents the longitudinal velocity, v i (t) represents the sway velocity, r i (t) represents the bow angular velocity of the i-th unmanned boat; Represents μ i The first derivative of (t); R 3 represents 3-dimensional Euclidean space; M i represents the inertia matrix of the i-th unmanned boat, where All represent parameters related to the quality of unmanned boats; R 3×3 represents a 3×3 dimensional Euclidean space; represents the Coriolis force, centripetal force matrix and nonlinear damping matrix; δ i (t) is the external disturbance caused by complex airflow and extreme weather to the i-th unmanned boat; σ i (t) represents the control input of the i-th unmanned boat, where σ i (t)=[σ i1 (t) 0 σ i3 (t)] T , σ i1 (t) is the longitudinal control input, σ i3 (t) is the yaw control input; i represents the index number of the unmanned boat, where i = {1...I}, and I represents the total number of unmanned boats;

[0076] Let d i =R i (ζ i (t))[u i (t),v i (t)] T , then the above formula is converted to:

[0077]

[0078] Where: b i represents the position vector of the i-th unmanned boat; Indicates b i The first derivative of i represents the speed of the i-th unmanned boat in the earth coordinate system; Indicates d i The first derivative of Γ i d (ζ i (t),μ i (t), t) represents the unknown nonlinear disturbance to the position state of the unmanned boat; ζ i (t) represents the heading angle of the i-th unmanned boat; t represents time; represents the generalized longitudinal and lateral control inputs of the unmanned vehicle in the earth coordinate system; Indicates i The first derivative of (t); r i (t) represents the angular velocity of the i-th unmanned boat; Represents r i The first derivative of (t); Γ i r (ζ i (t),μi (t), t) represents the unknown nonlinear disturbance to the heading state of the unmanned boat; μ i (t) represents the speed of the i-th unmanned boat in the ship coordinate system; represents the generalized heading control input of the unmanned boat;

[0079] in,

[0080] Specifically, Γ i d (ζ i (t),μ i (t),t)∈R 2 and Γ i r (ζ i (t),μ i (t),t)∈R are the unknown nonlinear disturbances Γ i (ζ i (t),μ i (t),t), where represents the unknown nonlinear disturbance of the unmanned boat; represents the first-order derivative of the rotation matrix; represents the Coriolis force, centripetal force matrix and nonlinear damping matrix; δ i (t) To consider the external disturbances caused by complex airflow and extreme weather to the i-th unmanned boat; Indicates M i Inverse matrix operation of ; in and and They represent the virtual longitudinal control input and virtual lateral control input of the unmanned boat in the earth coordinates, Represents the actual longitudinal control input of the unmanned boat in the ship coordinate system.

[0081] Preferably, the leader status in the unmanned boat formation system is expressed as follows:

[0082] The leader state involved in this embodiment is generated by the following system. 0 ∈R 2 is the expected location information; its specific form is as follows:

[0083]

[0084]

[0085] Where: 0Represents the matrix related to the leadership system status; Represents χ 0 The first derivative of; F∈R 2×6 is a constant matrix; R 2×6 represents 2×6 dimensional Euclidean space; b 0 Represents the position information of the leader system composed of multi-tone sinusoidal signals; E∈R 6×6 Represents the parameter matrix of the leadership system; R 6×6 represents a 6×6 dimensional Euclidean space; block diag represents a block diagonal matrix, 0 2×2 represents a square matrix with 2nd-order elements being zero, diag represents a diagonal matrix, Both represent the frequency of multi-tone sinusoidal signals. represents the Kronecker product, a represents a known second-order matrix; where,

[0086] S2: Based on the model of the underactuated unmanned boat and the leader state in the unmanned boat formation system, an adaptive fully distributed observer is established to obtain the estimated pilot system state χ of the unmanned boat i ;

[0087] Specifically, the adaptive fully distributed observer in this embodiment obtains the status information of the neighboring unmanned boat from the communication network (the frequency of the multi-tone sinusoidal signal of the navigation system estimated by the unmanned boat and the estimated status information of the navigation system), combines its own status information, estimates the unknown parameters of the navigation system through an adaptive method, and further estimates the status of the navigation system.

[0088] Preferably, the adaptive fully distributed observer is established as follows:

[0089]

[0090] in,

[0091]

[0092] Where: i represents the estimated navigation system state matrix of the i-th unmanned boat, where χ i ∈R 6 , R 6 represents a 6-dimensional Euclidean space. When i = 0, χ 0 Represents the matrix related to the leadership system status; Represents χ i The first derivative of E i represents the estimated leadership system parameter matrix of the i-th unmanned boat; ρ i (t) represents the dynamic gain of the i-th unmanned boat; i and j represent the index number of the unmanned boat; I represents the total number of unmanned boats; aij (t) represents the connectivity coefficient between the i-th unmanned boat and the j-th unmanned boat, that is, when the i-th unmanned boat can receive the information of the j-th unmanned boat, a ij is 1, otherwise it is 0; j represents the estimated state of the navigation system of the jth unmanned boat; Represents ρ i The first derivative of (t); γ i is the adaptive gain; l i is a scalar; where l i Plays a "correction" role to avoid parameter drift; represents the sum of the differences between the estimated navigation system state of the i-th unmanned boat and the estimated navigation system state of the unmanned boat connected to the i-th unmanned boat; φ i represents the frequency of the multi-tone sinusoidal signal of the navigation system estimated by the i-th unmanned boat, col represents a column vector, The first and second elements of the column vector of the frequencies of the multi-tone sinusoidal signal are respectively; Represents φ i The first derivative of ;

[0093] Specifically, in this embodiment, the following unmanned boat that is not directly connected to the pilot system does not need to know the parameter matrix E of the pilot system in advance. i , each unmanned boat is based on the local neighborhood communication information (i.e., the connectivity coefficient a ij (t)) dynamically estimates the leader system parameter matrix; at the same time, the dynamic gain ρ associated with the global network information i (t) is updated online through the adaptive law and is calculated only through the neighborhood state difference, without relying on global network information. This mechanism enables the observer to run completely distributed. Even if some nodes fail or the communication topology changes, the system can still accurately estimate the parameter matrix and state through local information. S3: Establish an adaptive fuzzy predictor to obtain the unknown nonlinear function Γ i Estimated value of

[0094] Specifically, after obtaining the state of the leadership system, an adaptive fuzzy predictor is then used to estimate the unknown nonlinear function in the ship model to obtain the unknown nonlinear function Γ i The estimated value of the velocity derivative can also be obtained by using the adaptive fuzzy predictor.

[0095] Preferably, the adaptive fuzzy predictor is established as follows:

[0096]

[0097] Where: Represents μi The estimated value of (t), express The first derivative of i is the weight matrix; H i Estimated value; β i is a known activation function; σ i (t) represents the control input of the i-th unmanned boat; K i represents the estimator gain; It represents the error between the estimated speed and the actual speed; Represents the unknown nonlinear function Γ of the unmanned boat i The estimated value of Γ i represents the unknown nonlinear function of the unmanned boat; ε i represents the function reconstruction error; for The first-order derivative of; Λ represents the adaptive gain; k w is a constant.

[0098] S4: According to the estimated navigation system state x of the unmanned boat i and the unknown nonlinear function Γ i Estimated value of Build a distributed controller to obtain the longitudinal and lateral control inputs of the unmanned vehicle in the earth coordinate system

[0099] Specifically, the distributed controller of this embodiment uses the pilot system state matrix χ obtained by the fully distributed observer i , the estimator estimates the signal and adaptive fuzzy estimation value (the unknown nonlinear function Γ of the unmanned boat i Estimated value of The control goal is to achieve the underlying control task of multiple unmanned boats, that is, the control task of multiple unmanned boats formation. The following fully distributed control law in the earth coordinate system is proposed.

[0100] Preferably, the distributed controller is established as follows:

[0101]

[0102] b ic =Fx i

[0103]

[0104] Where: is the virtual control quantity of the designed i-th unmanned boat; J i represents a 2×2 matrix consisting of the first two rows and the first two columns of the rotation matrix; b icrepresents the estimated position of the leadership system of the i-th unmanned boat, Indicates b ic The first derivative of; α is a given parameter; b i represents the position vector of the i-th unmanned boat; θ i represents the formation error of the i-th unmanned boat; F is a constant matrix; χ i represents the estimated navigation system state of the i-th unmanned boat; H is a given scalar; b id Represents the virtual control quantity after filtering; Represents the derivative of the virtual control quantity after filtering; represents the longitudinal and lateral control input of the i-th unmanned boat in the earth coordinate system; represents the position control gain; d i represents the speed of the i-th unmanned boat in the earth coordinate system; Represents the estimated value of the unknown nonlinear function of the i-th unmanned boat The first two dimensions of , i.e., estimates of the unknown nonlinear disturbances to which the position-dependent state is subjected;

[0105] S5: Establish the control obstacle function; according to the longitudinal and lateral control inputs of the unmanned boat in the earth coordinate system right It performs secondary planning to obtain the longitudinal and lateral optimized control input of the unmanned boat considering the influence of obstacles.

[0106] Specifically, this embodiment obtains the position of the obstacle through the sensor, which is used to control the obstacle function based on the collision avoidance mechanism, and obtains the optimized control input vector considering the influence of the obstacle, so as to realize the safe formation control of multiple unmanned boats. This embodiment uses system state information: such as position, speed, etc., to avoid obstacles by inputting the state safety control obstacle function.

[0107] Specifically, this embodiment uses g ik (b) Based on the above, a control obstacle function is constructed, and the control obstacle function is established as follows:

[0108]

[0109] Where: represents the control barrier function; T represents transpose; for b ik The first derivative of 3 (g ik (b)) is a continuous and strictly increasing function with a domain of [0,∞); ‖b ik ‖ represents b ik The second norm of .

[0110] in,

[0111] g ik (b)=‖b ik ‖-D k

[0112] b ik =b i -o k

[0113] Where: g ik (b) indicates the relative position between the unmanned boat and the obstacle after adding the safety distance; k indicates the number of the obstacle; b ik represents the distance between the unmanned boat and the obstacle, b i represents the position vector of the i-th unmanned boat, o k is the position vector of the obstacle; D k represents the maximum safe distance for the unmanned boat to avoid collision with obstacles; ‖b ik ‖ represents b ik The second norm of .

[0114] Specifically, for the control barrier function By taking the partial derivative and substituting it into the feasible safety set, we can get the collision avoidance parameters.

[0115] In this way, the safety constraints of the multi-unmanned boat formation system can be obtained, including the safety constraints of collision avoidance between unmanned boats and the safety constraints of obstacle avoidance. Then, by setting a quadratic programming problem, the safe formation control of multiple unmanned boats can be realized.

[0116] The safety constraints of the unmanned boat formation system of this embodiment include safety constraints for collision avoidance between unmanned boats and safety constraints for obstacle avoidance;

[0117] Among them, the safety constraints for collision avoidance between unmanned boats are:

[0118]

[0119] The safety constraints for obstacle avoidance are:

[0120]

[0121] The design of the secondary planning of the unmanned boat system is as follows:

[0122]

[0123] Where: represents the longitudinal and lateral optimized control input of the i-th unmanned boat considering the influence of obstacles, that is, the control input that can achieve the collision and obstacle avoidance function; represents the longitudinal and lateral control input of the i-th unmanned boat in the earth coordinate system, that is, the control input when only the underlying control task is realized; represents the longitudinal and lateral control inputs of the i-th unmanned boat that satisfies all safety constraints; Represents searching for a function in real number space When the minimum value is obtained The value of represents the objective function of quadratic programming; is the transpose of the difference between the position vectors of the i-th unmanned boat and the j-th unmanned boat; is the collision avoidance parameter between the i-th unmanned boat and the j-th unmanned boat calculated by controlling the obstacle function; is the transpose of the difference between the position vectors of the i-th unmanned boat and the obstacle; are the collision avoidance parameters between i unmanned boats and obstacles calculated by controlling the obstacle function.

[0124] S6: Longitudinal and lateral optimization control inputs based on adaptive fuzzy predictor and unmanned boat considering obstacles Obtaining the optimal longitudinal control input for the unmanned vehicle and the actual heading control input of the unmanned boat To complete the control of the unmanned boat safety formation.

[0125] Preferably, the optimal longitudinal control input of the unmanned boat is Get as follows:

[0126]

[0127] Where: represents the optimal longitudinal control input of the i-th unmanned boat; Parameters related to the quality of unmanned boats; and They represent the virtual longitudinal control input and virtual lateral control input of the unmanned boat in the earth coordinates respectively.

[0128] in, and Respectively The first and second dimensions.

[0129] Preferably, the actual heading control input of the unmanned boat is obtained as follows:

[0130] To generate the optimal heading control input, consider the following desired heading signal:

[0131]

[0132] Where: ic represents the expected heading signal of the i-th unmanned boat; atan2 represents the inverse tangent function;

[0133] Specifically, after obtaining the desired heading signal, the unknown nonlinear value in the adaptive fuzzy predictor is used to obtain the estimated value of the angular velocity derivative. The heading distributed controller is designed as follows:

[0134]

[0135] Where: r ic is the virtual control quantity of the designed i-th unmanned boat; ζ ic represents the expected heading signal of the i-th unmanned boat; α r is a positive constant; i (t) represents the actual heading of the i-th unmanned boat; represents the virtual heading control input of the i-th unmanned boat; r i (t) represents the bow angular velocity of the i-th unmanned boat; represents the heading control gain of the i-th unmanned boat; Represents the unknown nonlinear term estimated for the i-th unmanned boat The third dimension of , which is an estimate of the unknown nonlinear disturbance to which the direction-dependent state is subjected.

[0136] Based on the above results, the following actual heading control input is obtained:

[0137]

[0138] Where: represents the actual heading control input of the i-th unmanned boat; Represents parameters related to the mass of the unmanned boat.

[0139] The overall beneficial effects of this embodiment compared with the prior art are as follows:

[0140] First, compared with most of the current distributed observers, this embodiment proposes an adaptive fully distributed observer, which can accurately estimate the state of the pilot system without knowing the global network information in advance.

[0141] Second, the adaptive fully distributed observer proposed in this embodiment only assumes that the follower unmanned boats in the subsystem of the leader system can receive the system matrix information of the leader system, and other unmanned boats can estimate the system matrix information of the leader system using local information and their own states.

[0142] Third, compared with most current unmanned boat formation control strategies, during the execution of the multi-unmanned boat safe formation control task proposed in this embodiment, even if the multi-unmanned boat system faces dynamic or static obstacles, it can still ensure safe avoidance between unmanned boats and continue to complete the formation control task.

[0143] In summary, this embodiment further optimizes the performance and functions of unmanned boats through in-depth analysis and research on the problem of safe formation control of unmanned boats, and improves their ability to operate in various environments and conditions, so as to better serve human needs in ocean exploration, resource development, environmental monitoring, etc.

[0144] A specific embodiment of the present invention is as follows:

[0145] Taking a multi-unmanned boat formation system consisting of four unmanned boats as an example, the technical solution in this embodiment is further explained. The following parameters are in meters for position-related parameters and in radians for angle-related parameters. The model inertia matrix of each unmanned boat is as follows:

[0146]

[0147] The initial position is set to:

[0148]

[0149] Among them, b(0) represents the position vector of the unmanned boat formation;

[0150] The initial angle is set to:

[0151] ζ(0)=[-1.326 -1.965 -1.515 -1.633]

[0152] Among them, ζ(0) is the heading angle vector of the unmanned boat formation;

[0153] The formation error is set as:

[0154]

[0155] Among them, θ is the error vector of the unmanned boat formation;

[0156] The leadership system parameters are as follows:

[0157] F=[1 0 1 0 0 0; 0 1 0 0 0 1],

[0158] χ 0 =[30 30 0 30 0 30],

[0159] χ 0 =[0.15 0.15] T ,

[0160] Static obstacle position: Obstacle1 = [50,50],

[0161] Dynamic obstacle position: Obstacle2 = [56, 60-2t], t represents time;

[0162] The obstacle in the simulation implementation case of this embodiment is set as a circular obstacle, and the radius of the obstacle is 1 meter.

[0163] In addition, in the simulation results of this case, Figure 2a and Figure 2b The schematic diagram of the observer error of four unmanned boats is shown. Figure 2a and Figure 2b It is observed that the fully distributed observer proposed in this embodiment can accurately estimate the state of the pilot system; Figure 3a and Figure 3b The following error diagram of four unmanned boats is shown. Figure 3a and Figure 3b It is observed that under the control law proposed in this embodiment, the four unmanned boats can form a formation and complete the collaborative control task; Figure 4a and Figure 4b It shows the trajectories and collision and obstacle avoidance diagrams of the four unmanned boats, clearly demonstrating that they successfully completed collision avoidance between unmanned boats and dynamic and static obstacle avoidance during the entire obstacle avoidance process.

[0164] In summary, the present embodiment is a method for controlling the safe formation of unmanned boats based on a fully distributed observer. An adaptive fully distributed observer is established through the leader state in the unmanned boat formation system to obtain the estimated state of the navigation system of the unmanned boat; and a distributed controller is established based on the estimated value of the unknown nonlinear function based on the adaptive fuzzy predictor, and combined with the control obstacle function to obtain the longitudinal and lateral optimized control inputs of the unmanned boat under the influence of obstacles, and finally the optimal longitudinal control input of the unmanned boat and the actual heading control input of the unmanned boat are obtained to complete the control of the safe formation of the unmanned boat. The present invention uses an adaptive fully distributed observer to estimate the state of the navigation system, combines the unknown nonlinear function approximated by the fuzzy system of the adaptive fuzzy predictor and the error between its own state and the estimated state of the navigation system, designs a distributed controller, obtains the control input of the unmanned boat in the earth coordinates, and then considers the influence of obstacles to obtain the optimal longitudinal control input of the unmanned boat and the actual heading control input of the unmanned boat, so as to achieve safe control of the unmanned boat formation. Compared with the existing state estimation method of the navigation system, by using limited local information and its own state, the signal of the navigation system can be estimated when the global information of the network is unknown, so that the unmanned boat formation control system will not be greatly affected by the failure of a certain node, and the unknown parameters of the navigation system can be estimated, which improves the reliability of the formation control of the unmanned boat. At the same time, when the unmanned boat safety formation control task is executed, even if the unmanned boat encounters dynamic and static obstacles, the formation control task can still be completed while ensuring the safe collision avoidance between unmanned boats and the avoidance of obstacles. This embodiment can be applied to unmanned boats and can realize collaborative formation control. Through a fully distributed observer, the navigator signal can be accurately estimated, which is convenient for the design of the controller; and through the design based on the safety optimization mechanism of the control obstacle function, it can effectively realize the safe formation control while the system completes the underlying formation control task, avoiding the loss of the unmanned boat caused by collision during the mission.

[0165] 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 formation control method for unmanned boats based on a fully distributed observer, characterized in that: The steps include: S1: Establish a model of an underactuated unmanned boat in a multi-unmanned boat formation system composed of underactuated unmanned boat models; and simultaneously obtain the leader status in the unmanned boat formation system; S2: According to the model of the underactuated unmanned boat and the leader state in the unmanned boat formation system, an adaptive fully distributed observer is established to obtain the estimated pilot system state of the unmanned boat; S3: Establish an adaptive fuzzy predictor to obtain the estimated value of the unknown nonlinear function; S4: establishing a distributed controller according to the estimated state of the navigation system of the unmanned boat and the estimated value of the unknown nonlinear function to obtain the longitudinal and lateral control inputs of the unmanned boat in the earth coordinate system; S5: Establishing a control obstacle function; based on the longitudinal and lateral control inputs of the unmanned boat in the earth coordinate system and the safety constraints of the unmanned boat formation system, obtaining the longitudinal and lateral optimized control inputs of the unmanned boat under the influence of obstacles; S6: According to the adaptive fuzzy predictor and the longitudinal and lateral optimized control inputs of the unmanned boat considering the influence of obstacles, the optimal longitudinal control input of the unmanned boat and the actual heading control input of the unmanned boat are obtained, so as to complete the control of the unmanned boat safety formation according to the optimal longitudinal control input of the unmanned boat and the actual heading control input of the unmanned boat.

2. The unmanned boat safety formation control method based on a fully distributed observer according to claim 1 is characterized in that: In S2, the adaptive fully distributed observer is established as follows: in, Where: i represents the estimated navigation system state matrix of the i-th unmanned boat, where χ i ∈R 6 , R 6 represents a 6-dimensional Euclidean space. When i = 0, χ0 represents the matrix associated with the state of the leadership system; Represents χ i The first derivative of E i represents the estimated leadership system parameter matrix of the i-th unmanned boat; ρ i (t) represents the dynamic gain of the i-th unmanned boat; i and j represent the index number of the unmanned boat; I represents the total number of unmanned boats; a ij (t) represents the connectivity coefficient between the i-th unmanned boat and the j-th unmanned boat; χ j represents the estimated state of the navigation system of the jth unmanned boat; Represents ρ i The first derivative of (t); γ i is the adaptive gain; l i is a scalar; represents the sum of the differences between the estimated navigation system state of the i-th unmanned boat and the estimated navigation system state of the unmanned boat connected to the i-th unmanned boat; φ i represents the frequency of the multi-tone sinusoidal signal of the navigation system estimated by the i-th unmanned boat, col represents a column vector, The first and second elements of the column vector of the frequencies of the multi-tone sinusoidal signal are respectively; Represents φ i The first-order derivative of ; block diag{·} represents a block diagonal matrix; 0 2×2 Represents a square matrix whose 2nd-order elements are zero; represents the Kronecker product, and a represents a known second-order matrix.

3. The unmanned boat safety formation control method based on fully distributed observer according to claim 1 is characterized in that: In S1, the model of the underactuated unmanned boat is established as follows: Where: h i (t) represents the position and direction of the i-th unmanned boat in the earth coordinate system, where h i (t) = [x i (t),y i (t),ζ i (t)]T∈R 3 , x i (t) and y i (t) represent the horizontal and vertical coordinates of the position in the earth coordinate system, ζ i (t) represents the heading angle of the i-th unmanned boat, where ζ i (t)∈(-π,π]; R 3 represents 3-dimensional Euclidean space; T represents transpose; represents the rotation matrix; where diag(·) represents a diagonal matrix; J i (ζ i (t)) represents a 2×2 matrix formed by the intersection of the first two rows and the first two columns of the rotation matrix, where J i (ζ i (t)) = [cos(ζ i (t)) -sin(ζ i (t)) sin(ζ i (t)) cos(ζ i (t))];ζ i (t) represents the heading angle of the i-th unmanned boat, where ζ i (t)∈(-π,π]; μ i (t) represents the speed of the i-th unmanned boat in the ship coordinate system, where μ i (t)=[u i (t)v i (t)r i (t)] T ∈R 3 ,u i (t) represents the longitudinal velocity, v i (t) represents the sway velocity, r i (t) represents the bow angular velocity of the i-th unmanned boat; Represents μ i The first derivative of (t); R 3 represents 3-dimensional Euclidean space; M i represents the inertia matrix of the i-th unmanned boat, where All represent parameters related to the quality of unmanned boats; R 3×3 represents a 3×3 dimensional Euclidean space; represents the Coriolis force, centripetal force matrix and nonlinear damping matrix; δ i (t) is the external disturbance caused by complex airflow and extreme weather to the i-th unmanned boat; σ i (t) represents the control input of the i-th unmanned boat, where σ i (t)=[σ i1 (t) 0 σ i3 (t)] T , σ i1 (t) is the longitudinal control input, σ i3 (t) is the yaw control input; i represents the index number of the unmanned boat, where i = {1...I}, and I represents the total number of unmanned boats; Let d i =R i (ζ i (t))[u i (t),v i (t)] T ,but Where: b i represents the position vector of the i-th unmanned boat; Indicates b i The first derivative of i represents the speed of the i-th unmanned boat in the earth coordinate system; Indicates d i The first derivative of ; Indicates the unknown nonlinear disturbance to the position state of the unmanned boat; ζ i (t) represents the heading angle of the i-th unmanned boat; t represents time; represents the generalized longitudinal and lateral control inputs of the unmanned vehicle in the earth coordinate system; Indicates i The first derivative of (t); r i (t) represents the angular velocity of the i-th unmanned boat; Represents r i The first derivative of (t); Indicates the unknown nonlinear disturbance to the heading state of the unmanned boat; μ i (t) represents the speed of the i-th unmanned boat in the ship coordinate system; Represents the generalized heading control input of the unmanned boat.

4. The method for controlling a safe formation of unmanned boats based on a fully distributed observer according to claim 1, characterized in that: In S1, the leader status in the unmanned boat formation system is expressed as follows: b0=Fχ0 Where: χ0 represents the matrix related to the state of the leadership system; represents the first-order derivative of χ0; F∈R 2×6 is a constant matrix; R 2×6 represents a 2×6-dimensional Euclidean space; b0 represents the position information of the leader system composed of multi-tone sinusoidal signals; E∈R 6×6 Represents the parameter matrix of the leadership system; R 6×6 represents a 6×6 dimensional Euclidean space; block diag represents a block diagonal matrix, 0 2×2 represents a square matrix with 2nd-order elements being zero, diag represents a diagonal matrix, Both represent the frequency of multi-tone sinusoidal signals. represents the Kronecker product, and a represents a known second-order matrix.

5. The method for controlling a safe formation of unmanned boats based on a fully distributed observer according to claim 1, characterized in that: In S3, the adaptive fuzzy predictor is established as follows: Where: Represents μ i The estimated value of (t), express The first derivative of i is the weight matrix; H i Estimated value; β i is a known activation function; σ i (t) represents the control input of the i-th unmanned boat; K i represents the estimator gain; It represents the error between the estimated speed and the actual speed; Represents the unknown nonlinear function Γ of the unmanned boat i The estimated value of Γ i represents the unknown nonlinear function of the unmanned boat; ε i represents the function reconstruction error; for The first-order derivative of; Λ represents the adaptive gain; k w is a constant.

6. The method for controlling a safe formation of unmanned boats based on a fully distributed observer according to claim 1, characterized in that: In S4, the distributed controller is established as follows: b ic =Fχ i Where: is the virtual control quantity of the designed i-th unmanned boat; J i represents a 2×2 matrix consisting of the first two rows and the first two columns of the rotation matrix; b ic represents the estimated position of the leadership system of the i-th unmanned boat, Indicates b ic The first derivative of; α is a given parameter; b i represents the position vector of the i-th unmanned boat; θ i represents the formation error of the i-th unmanned boat; F is a constant matrix; χ i represents the estimated navigation system state of the i-th unmanned boat; H is a given scalar; b id Represents the virtual control quantity after filtering; Represents the derivative of the virtual control quantity after filtering; represents the longitudinal and lateral control input of the i-th unmanned boat in the earth coordinate system; Represents position control gain; d i represents the speed of the i-th unmanned boat in the earth coordinate system; Represents the estimated value of the unknown nonlinear function of the i-th unmanned boat The first two dimensions of are the estimates of the unknown nonlinear disturbances to which the position-dependent state is subjected.

7. The method for controlling a safe formation of unmanned boats based on a fully distributed observer according to claim 1, characterized in that: In S5, the control obstacle function is established as follows: Where: represents the control barrier function; T represents transpose; for b ik The first derivative of α3(g ik (b)) is a continuous and strictly increasing function with a domain of [0,∞); ‖b ik ‖ represents b ik The second norm of ; in, g ik (b)=‖b ik ‖-D k b ik =b i -o k Where: g ik (b) indicates the relative position between the unmanned boat and the obstacle after adding the safety distance; k indicates the number of the obstacle; b ik represents the distance between the unmanned boat and the obstacle, b i represents the position vector of the i-th unmanned boat, o k is the position vector of the obstacle; D k represents the maximum safe distance for the unmanned boat to avoid collision with obstacles; ‖b ik ‖ represents b ik The second norm of .

8. The method for controlling a safe formation of unmanned boats based on a fully distributed observer according to claim 1, characterized in that: In S5, the formula used to obtain the longitudinal and lateral optimization control inputs of the unmanned boat considering the influence of obstacles is as follows: Where: represents the longitudinal and lateral optimized control input of the i-th unmanned boat considering the influence of obstacles, that is, the control input that can achieve the collision and obstacle avoidance function; represents the longitudinal and lateral control input of the i-th unmanned boat in the earth coordinate system, that is, the control input when only the underlying control task is realized; represents the longitudinal and lateral control inputs of the i-th unmanned boat that satisfies all safety constraints; Represents searching for a function in real number space When the minimum value is obtained The value of represents the objective function of quadratic programming.

9. The method for controlling a safe formation of unmanned boats based on a fully distributed observer according to claim 1, characterized in that: In S6, the optimal longitudinal control input of the unmanned boat is obtained as follows: Where: represents the optimal longitudinal control input of the i-th unmanned boat; Parameters related to the quality of unmanned boats; and They represent the virtual longitudinal control input and virtual lateral control input of the unmanned boat in the earth coordinates respectively.

10. The method for controlling a safe formation of unmanned boats based on a fully distributed observer according to claim 1, characterized in that: In S6, the actual heading control input of the unmanned boat is obtained as follows: Where: r ic is the virtual control quantity of the designed i-th unmanned boat; ζ ic represents the expected heading signal of the i-th unmanned boat; α r is a positive constant; i (t) represents the actual heading of the i-th unmanned boat; represents the virtual heading control input of the i-th unmanned boat; r i (t) represents the bow angular velocity of the i-th unmanned boat; represents the heading control gain of the i-th unmanned boat; Represents the unknown nonlinear term estimated for the i-th unmanned boat The third dimension of is the estimated value of the unknown nonlinear disturbance to the direction-dependent state; atan2 represents the inverse tangent function; but Where: represents the actual heading control input of the i-th unmanned boat; Represents parameters related to the mass of the unmanned boat.

Images