An unmanned surface vehicle safety formation control method based on a full-distributed observer

Abstract

The application discloses an unmanned ship safety formation control method based on a complete distributed observer, and establishes an adaptive complete distributed observer through the leader state in an unmanned ship formation system to obtain the estimated navigation system state of the unmanned ship; and establishes a distributed controller according to the estimated value of an unknown nonlinear function based on an adaptive fuzzy predictor, and combines a control barrier function to obtain the longitudinal and lateral optimization control input of the unmanned ship under the influence of the barrier, finally obtains the optimal longitudinal control input of the unmanned ship and the actual heading control input of the unmanned ship, and completes the control of the unmanned ship safety formation. Compared with the existing navigation system state estimation method, the application can estimate the signal of the navigation system under the condition that the network global information is unknown by using the limited local information and the self state, so that the unmanned ship formation control system is not greatly affected due to the failure of a node, and the unknown parameters of the navigation system can be estimated, and the reliability of the formation control of the unmanned ship is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned ship control, and particularly relates to an unmanned ship safety formation control method based on a completely distributed observer. BACKGROUND

[0002] With the rapid development of science and technology, unmanned ships 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 transportation and military operations. Unmanned ships can autonomously navigate in complex and dangerous water environments and perform tasks, avoiding the risks of personnel facing adverse sea conditions, dangerous areas, etc., while enabling 24-hour uninterrupted operation, greatly improving operational efficiency and task execution accuracy. For example, in marine environmental monitoring, unmanned ships can carry various sensors to monitor water quality, weather parameters, marine life, etc. for a long time and over a large area; in the field of maritime security, they can be used for border patrol, sea monitoring, and timely detection of abnormal situations such as illegal intrusions.

[0003] In many practical application scenarios, the capabilities of a single unmanned ship are often limited, and the formation of multiple unmanned ships working together can significantly improve the efficiency of task execution. For example, in large-area marine search and rescue tasks, a formation of multiple unmanned ships can quickly and comprehensively cover the search area, improving the success rate of rescue; in maritime material transportation, formation navigation can optimize the transportation path, improve transportation efficiency and reduce transportation cost. Currently, many scholars have proposed various unmanned ship formation control methods, but the existing control methods still have some problems:

[0004] First, most existing unmanned ship formation control methods rely on prior network global information related to the communication topology, and if a fault occurs in a certain unmanned ship in the formation, such as a communication device fault or a power system fault, its position in the communication topology will be missing. In this case, the control method relying 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 all unmanned ships know the system matrix of the leader system, which may not be desirable in some applications. In the case where the leader system parameters are known, estimating the state of the leader system will result in great limitations in the formation control method of the unmanned ship.

[0006] Third, in actual working scenarios, there may be a large number of dynamic or static obstacles (such as coral reef areas, marine biological aggregation areas, etc.) in complex marine ecological environments. When unmanned ships navigate in formation, they need to avoid collisions with other unmanned ships and obstacles in the ocean to ensure safe navigation. SUMMARY

[0007] The application provides a fully-distributed observer-based unmanned surface vehicle (USV) safe formation control method to overcome the above technical problems.

[0008] To achieve the above-mentioned purpose, the technical scheme of the application is as follows:

[0009] A fully-distributed observer-based USV safe formation control method comprises the following steps:

[0010] S1: establishing a model of an underactuated USV in a multi-USV formation system composed of underactuated USV models; and obtaining a leader state in the USV formation system;

[0011] S2: establishing an adaptive fully-distributed observer based on the model of the underactuated USV and the leader state in the USV formation system to obtain an estimated navigation system state of the USV;

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

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

[0014] S5: establishing a control barrier function to obtain longitudinal and lateral optimal control inputs of the USV considering the influence of obstacles based on the longitudinal and lateral control inputs of the USV in the earth coordinate system and the safety constraints of the USV formation system;

[0015] S6: obtaining optimal longitudinal control inputs of the USV and actual heading control inputs of the USV based on the adaptive fuzzy predictor and the longitudinal and lateral optimal control inputs of the USV considering the influence of obstacles, and completing the control of the safe formation of the USV based on the optimal longitudinal control inputs of the USV and the actual heading control inputs of the USV.

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

[0017]

[0018] wherein,

[0019]

[0020] In the formula, χ i represents an estimated navigation system state matrix of the i-th USV, wherein χ i ∈R 6 , R 6denotes a 6-dimensional Euclidean space, and χ0denotes a matrix related to the leader system state when i = 0; denotes χ i denotes the first derivative of χ i denotes the leader system parameter matrix estimated by the ith unmanned surface vehicle; ρ i (t) denotes the dynamic gain of the ith unmanned surface vehicle; i, j denote the index number of the unmanned surface vehicle; I denotes the total number of unmanned surface vehicles; a ij (t) denotes the communication coefficient between the ith unmanned surface vehicle and the jth unmanned surface vehicle; χ j denotes the leader system state estimated by the jth unmanned surface vehicle; denotes the first derivative of ρ i (t); γ i is an adaptive gain; l i is a scalar; denotes the sum of the difference between the leader system state estimated by the ith unmanned surface vehicle and the leader system state estimated by the unmanned surface vehicle in communication with the ith unmanned surface vehicle; φ i denotes the frequency of the leader system multi-tone sinusoidal signal estimated by the ith unmanned surface vehicle, col denotes a column vector, are the first and second elements of the column vector of the frequency of the multi-tone sinusoidal signal, respectively; denotes the first derivative of φ i ; block diag{·} denotes a block diagonal matrix; 0 2×2 denotes a square matrix with the 2nd order elements being zero; denotes the Kronecker product, and a denotes a known two-order matrix.

[0021] Further, in the S1, the model of the underactuated unmanned surface vehicle is established as follows:

[0022]

[0023] wherein: h i (t) denotes the position and direction of the ith unmanned surface vehicle in the earth coordinate system, wherein h i (t) = [x i (t), y i (t), ζ i (t)] T ∈ R 3 , x i (t) and y i (t) denote the horizontal and vertical coordinates of the position in the earth coordinate system, respectively, and ζ i (t) denotes the heading angle of the ith unmanned surface vehicle, wherein ζ i (t) ∈ (-π, π]; R 3 denotes a 3-dimensional Euclidean space; T denotes transposition; denotes a rotation matrix; where, diag(·) denotes a diagonal matrix; J i i (t)) denotes a 2x2 matrix formed by 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) denotes the heading angle of the ith USV, where ζ i (t) e (-π, π]; μ i (t) denotes the velocity of the ith USV in the vessel coordinate frame, where μ i (t) = [u i (t) v i (t) r i (t)] T e R 3 , u i (t) denotes the surge velocity, v i (t) denotes the sway velocity, and r i (t) denotes the yaw rate of the ith USV; denotes the first derivative of μ i (t); R 3 denotes the 3-dimensional Euclidean space; M i denotes the inertia matrix of the ith USV, where both denote parameters related to the mass of the USV; R 3×3 denotes the 3x3-dimensional Euclidean space; denotes the Coriolis force, centripetal force matrix, and nonlinear damping matrix; δ i (t) is the external disturbance considered for the ith USV due to complex air flows and extreme weather; σ i (t) denotes the control input of the ith USV, where σ i (t) = [σ i1 (t) 0 σ i3 (t)] T , σ i1 (t) is the longitudinal control input, and σ i3 (t) is the yaw control input; i denotes the index number of the USV, where i = {1... I}, and I denotes the total number of USVs;

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

[0025]

[0026] where: b i denotes the position vector of the i-th USV; denotes the first derivative of b i ; d i denotes the velocity of the i-th USV in the earth coordinate frame; denotes the first derivative of d i ; Γ i d (ζ i (t), μ i (t), t) denotes the unknown nonlinear disturbance to the USV position state; ζ i (t) denotes the heading angle of the i-th USV; t denotes time; denotes the USV generalized longitudinal and lateral control input in the earth coordinate frame; denotes the first derivative of ζ i (t); r i (t) denotes the angular velocity of the i-th USV; denotes the first derivative of r i (t); Γ i r (ζ i (t), μ i (t), t) denotes the unknown nonlinear disturbance to the USV heading state; μ i (t) denotes the velocity of the i-th USV in the body coordinate frame; denotes the USV generalized heading control input.

[0027] Further, in the S1, the leader state in the USV formation system is represented as follows:

[0028]

[0029] b0= F χ0

[0030]

[0031] where: χ0denotes a matrix related to the leader system state; denotes the first derivative of χ0; F ∈ R 2×6 is a constant matrix; R 2×6denotes a 2x6 dimensional Euclidean space; b0denotes the position information of the leader system consisting of a multi-tone sinusoidal signal; E ∈ R 6×6 denotes a parameter matrix of the leader system; R 6×6 denotes a 6x6 dimensional Euclidean space; block diag denotes a block diagonal matrix, 0 2×2 denotes a 2x2 matrix with zero elements, diag denotes a diagonal matrix, both denote the frequencies of the multi-tone sinusoidal signal, denotes a Kronecker product, a denotes a known second-order matrix.

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

[0033]

[0034] wherein: denotes μ i (t) is an estimation value of the velocity of the i-th USV, denotes a first-order derivative of H i is a weight matrix; is a H i estimation value; β i is a known activation function; σ i (t) denotes a control input of the i-th USV; K i denotes a predictor gain; denotes an error between the estimated velocity and the actual velocity; denotes an unknown nonlinear function of the USV i Γ i denotes an unknown nonlinear function of the USV; ε i denotes a function reconstruction error; is a first-order derivative of ; Λ denotes an adaptive gain; k w is a constant.

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

[0036]

[0037] b ic = Fχ i

[0038]

[0039] wherein: is a virtual control quantity of the i-th USV designed; J i denotes a 2x2 matrix formed by the first two rows and the first two columns of the rotation matrix; bic represents the position of the i-th USV estimated leadership system, represents the first derivative of b ic ; a is a given parameter; b i represents the position vector of the i-th USV; θ i represents the formation error of the i-th USV; F is a constant matrix; χ i represents the i-th USV estimated state of the navigation system; H is a given scalar; b id represents the filtered virtual control; represents the derivative of the filtered virtual control; represents the longitudinal and lateral control input of the i-th USV in the earth coordinate system; represents the position control gain; d i represents the velocity of the i-th USV in the earth coordinate system; represents the estimate of the unknown nonlinear function of the i-th USV first two dimensions, i.e., the position-related state, of the unknown nonlinear disturbance.

[0040] Further, in the S5, the control barrier function is established as follows:

[0041]

[0042] wherein: represents the control barrier function; T represents the transpose; is the first derivative of b ik ; a3(g ik (b)) is a continuous strictly increasing function with domain [0, ∞); ‖b ik ‖ represents the two-norm of b ik ;

[0043] wherein,

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

[0045] b ik = b i - o k

[0046] wherein: g ik (b) represents the relative position between the USV and the obstacle after adding a safety distance; k represents the number of the obstacle; b ik represents the distance between the USV and the obstacle, b i represents the position vector of the i-th USV, o k is the position vector of the obstacle; D krepresents the limit safety distance of the USV and the obstacle to avoid collision; ik ‖ represents b ik norm of b.

[0047] Further, in the S5, the formula used to obtain the longitudinal and lateral optimization control input of the USV considering the influence of the obstacle is as follows:

[0048]

[0049] In the formula: represents the longitudinal and lateral optimization control input of the i th USV considering the influence of the obstacle, that is, the control input capable of realizing the obstacle avoidance function; represents the longitudinal and lateral control input of the i th USV in the earth coordinate system, that is, the control input when only realizing the bottom layer control task; represents the longitudinal and lateral control input of the i th USV satisfying all safety constraints; represents the value of the function when the minimum value of the function is found in the real number space, represents the objective function of the quadratic programming.

[0050] Further, in the S6, the optimal longitudinal control input of the USV is obtained as follows:

[0051]

[0052] In the formula: represents the optimal longitudinal control input of the i th USV; is a parameter related to the mass of the USV; and respectively represent the virtual longitudinal control input and the virtual lateral control input of the USV in the earth coordinate system.

[0053] Further, in the S6, the actual heading control input of the USV is obtained as follows:

[0054]

[0055]

[0056] In the formula: r ic is the virtual control quantity of the i th USV designed; ζ ic represents the expected heading signal of the i th USV; α r is a normal number; ζ i (t) represents the actual heading of the i th USV; represents the virtual heading control input of the i th USV; r i(t) represents the yaw rate of the i-th USV; represents the heading control gain of the i-th USV; represents the estimated unknown nonlinear term of the i-th USV of the third dimension, i.e. the estimated value of the unknown nonlinear disturbance suffered by the direction-dependent state; atan2 represents the inverse tangent function;

[0057] then

[0058]

[0059] In the formula: represents the actual heading control input of the i-th USV; represents a parameter related to the mass of the USV.

[0060] Beneficial effects: the USV safe formation control method based on the complete distributed observer of the application, through the leader state in the USV formation system, an adaptive complete distributed observer is established to obtain the estimated pilot system state of the USV; and according to the estimated value of the unknown nonlinear function based on the adaptive fuzzy predictor, a distributed controller is established, and a control barrier function is combined to obtain the longitudinal and lateral optimization control input of the USV under the influence of the obstacle, and finally the optimal longitudinal control input of the USV and the actual heading control input of the USV are obtained, and the control of the USV safe formation is completed. The adaptive complete distributed observer is used to estimate the pilot system state, compared with the existing pilot system state estimation method, the limited local information and the self state are used to estimate the pilot system signal in the case that the network global information is unknown, so that the USV formation control system is not greatly affected by the failure of a node, and the unknown parameters of the pilot system can also be estimated, improving the reliability of the USV formation control. At the same time, when the USV safe formation control task is executed, even if the USV encounters dynamic and static obstacles, the formation control task can still be completed while ensuring the safe collision avoidance between the USVs and the obstacle avoidance. BRIEF DESCRIPTION OF DRAWINGS

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

[0062] Figure 1 Flow chart of the USV safe formation control method based on the complete distributed observer of the application;

[0063] Figure 2aAn error diagram between a navigation system state estimated by a full-distributed observer in an x direction of four unmanned ships in an embodiment of the present application and an actual navigation system state is shown;

[0064] Figure 2b An error diagram between a navigation system state estimated by a full-distributed observer in a y direction of four unmanned ships in an embodiment of the present application and an actual navigation system state is shown;

[0065] Figure 3a An error diagram between a navigation system state estimated by a full-distributed observer in an x direction of four unmanned ships in an embodiment of the present application and an actual navigation system state is shown;

[0066] Figure 3b An error diagram between a navigation system state estimated by a full-distributed observer in a y direction of four unmanned ships in an embodiment of the present application and an actual navigation system state is shown.

[0067] Figure 4a A trajectory diagram of four unmanned ships in an embodiment of the present application is shown;

[0068] Figure 4b An avoidance diagram of four unmanned ships in an embodiment of the present application is shown. DETAILED DESCRIPTION

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

[0070] The embodiment provides a safe formation control method for unmanned ships based on a full-distributed observer, as shown in the figure. Figure 1 The method comprises the following steps.

[0071] S1: a model of an underactuated unmanned ship in a multi-unmanned ship formation system composed of underactuated unmanned ship models is established; and a leader state in the unmanned ship formation system is obtained;

[0072] Specifically, the embodiment considers a multi-unmanned ship formation system composed of multiple underactuated unmanned ships, and the underactuated unmanned ship model is used to obtain position and speed information of the unmanned ship and position and speed information of an obstacle through a sensor.

[0073] Preferably, the model of the underactuated unmanned ship is established as follows.

[0074]

[0075] Where: h i (t) represents the position and orientation of the i-th unmanned surface vessel 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 x and y coordinates of the position in the Earth coordinate system, respectively, and ζ i (t) represents the heading angle of the i-th unmanned surface vessel, where ζ i (t)∈(-π,π];R 3 Represents 3D Euclidean space; T represents transpose; Denotes 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 surface vessel, where ζ i (t)∈(-π,π];μ i (t) represents the velocity of the i-th unmanned surface vessel 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 oscillation velocity, v i (t) represents the sway velocity, r i (t) represents the heading angular velocity of the i-th unmanned surface vessel; μ i The first derivative of (t); R 3 M represents 3-dimensional Euclidean space; i Let represent the inertial matrix of the i-th unmanned surface vessel, where All represent parameters related to the mass of the unmanned surface vessel; R 3×3represents a 3x3 dimensional Euclidean space; represents Coriolis force, centripetal force matrix and nonlinear damping matrix; δ i (t) is the external disturbance to the i-th USV considering complex air flow and extreme weather; σ i (t) represents the control input of the i-th USV, 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 USV, where i = {1...I}, I represents the total number of USVs;

[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 USV; represents the first derivative of b i ; d i represents the velocity of the i-th USV in the earth coordinate system; represents the first derivative of d i ; Γ i d (ζ i (t), μ i (t), t) represents the unknown nonlinear disturbance to the USV position state; ζ i (t) represents the heading angle of the i-th USV; t represents time; represents the USV generalized longitudinal and lateral control input in the earth coordinate system; represents the first derivative of ζ i (t); r i (t) represents the angular velocity of the i-th USV; represents the first derivative of r i (t); Γ i r (ζ i (t), μ i (t), t) represents the unknown nonlinear disturbance to the USV heading state; μ i(t) represents the velocity of the i-th USV in the ship coordinate system; represents the generalized heading control input of the USV;

[0079] wherein,

[0080] Specifically, Γ i d (ζ i (t), μ i (t), t) e R 2 and Γ i r (ζ i (t), μ i (t), t) e R are the first two dimensions and the third dimension of the unknown nonlinear disturbance Γ i (ζ i (t), μ i (t), t) respectively, wherein represents the unknown nonlinear disturbance of the USV; represents the first-order derivative of the rotation matrix; represents the Coriolis force, the centripetal force matrix and the nonlinear damping matrix; δ i (t) is the external disturbance considering the i-th USV is subjected to complex air flow and extreme weather; represents the inverse matrix operation of M i wherein and and represent the virtual longitudinal control input and the virtual lateral control input of the USV in the earth coordinate respectively, represents the actual longitudinal control input of the USV in the ship coordinate system.

[0081] Preferably, the leader state in the USV formation system is represented as follows:

[0082] The leader state involved in the embodiment is generated by the following system, wherein b0e R 2 is the desired position information; the specific form is as follows:

[0083]

[0084]

[0085] In the formula: χ0represents a matrix related to the leader system state; represents the first-order derivative of χ0; F e R 2×6 is a constant matrix; R 2×6 ​represents a 2x6 dimensional Euclidean space; b0represents the position information of the leader system composed of a multi-tone sinusoidal signal; E ∈ R 6×6 represents a parameter matrix of the leader system; R 6×6 represents a 6x6 dimensional Euclidean space; block diag represents a block diagonal matrix, 0 2×2 represents a 2-order square matrix with zero elements, diag represents a diagonal matrix, both represent the frequency of the multi-tone sinusoidal signal, represents a Kronecker product, a represents a known second-order matrix; wherein,

[0086] S2: according to the model of the underactuated unmanned ship and the state of the leader in the unmanned ship formation system, an adaptive completely distributed observer is established to obtain the estimated navigation system state χ i of the unmanned ship;

[0087] Specifically, the adaptive completely distributed observer in the embodiment obtains the state information of the neighbor unmanned ship (the estimated frequency of the navigation system multi-tone sinusoidal signal of the unmanned ship and the estimated navigation system state information) from the communication network, combines the state information of itself, estimates the unknown parameters of the navigation system through an adaptive method, and further estimates the navigation system state.

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

[0089]

[0090] wherein,

[0091]

[0092] In the formula: χ i represents the estimated navigation system state matrix of the i-th unmanned ship, wherein χ i ∈ R 6 , R 6 represents a 6-dimensional Euclidean space, and when i=0, χ0represents a matrix related to the state of the leader system; represents the first-order derivative of χ i ; E i represents the estimated leader system parameter matrix of the i-th unmanned ship; ρ i (t) represents the dynamic gain of the i-th unmanned ship; i and j both represent the index number of the unmanned ship; I represents the total number of the unmanned ships; a ij (t) represents the communication coefficient between the i-th unmanned ship and the j-th unmanned ship, that is, when the i-th unmanned ship can receive the information of the j-th unmanned ship, a ij is 1, and otherwise 0; χ jrepresents the jth USV's estimated navigation system state; represents the i th USV's estimated navigation system state and the difference between the i th USV's estimated navigation system state and the estimated navigation system state of the USV in communication with the i th USV; φ i represents the first derivative of (t); γ i is an adaptive gain; l i is a scalar; wherein, l i plays a role of "correction" to avoid parameter drift; represents the i th USV's estimated navigation system state and the difference between the i th USV's estimated navigation system state and the estimated navigation system state of the USV in communication with the i th USV; φ i represents the frequency of the i th USV's estimated navigation system multi-tone sinusoidal signal, col represents a column vector, are the first and second elements of the column vector of the frequency of the multi-tone sinusoidal signal, respectively; represents the first derivative of φ i ;

[0093] Specifically, in the embodiment, the follower USVs not directly connected with the navigation system do not need to know the parameter matrix E i of the navigation system in advance; each USV dynamically estimates the leader system parameter matrix based on local neighborhood communication information (i.e., the communication coefficient a ij (t)); meanwhile, the dynamic gain p i (t) related to the global network information is updated online through an adaptive law, and is calculated only through the neighborhood state difference without relying on the global network information. This mechanism enables the observer to run completely distributed, and even if some nodes fail or the communication topology changes, the system can still achieve accurate estimation of the parameter matrix and the state through local information. S3: Establish an adaptive fuzzy estimator to obtain an estimated value of the unknown nonlinear function Γ i

[0094] Specifically, after obtaining the state of the leader system, an adaptive fuzzy estimator is then used to estimate the unknown nonlinear function in the ship model to obtain an estimated value of the unknown nonlinear function Γ i , and the adaptive fuzzy estimator can also obtain an estimated value of the velocity derivative.

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

[0096]

[0097] In the formula: represents the estimated value of μ i (t), represents the first derivative of ; H i is a weight matrix; is the H i ​Estimated value; β i The activation function is known; σ i (t) represents the control input of the i-th unmanned surface vessel; K i Indicates the predictor gain; This indicates the error between the estimated speed and the actual speed. The unknown nonlinear function Γ represents the unmanned surface vessel. i The estimated value; Γ i ε represents an unknown nonlinear function of the unmanned surface vessel; i Indicates the function reconstruction error; for The first derivative; Λ represents the adaptive gain; k w It is a constant.

[0098] S4: Based on the estimated navigation system state χ of the unmanned surface vessel i and unknown nonlinear function Γ i The estimated value Establish a distributed controller to obtain the longitudinal and lateral control inputs of the unmanned surface vessel in the Earth coordinate system.

[0099] Specifically, in this embodiment, the distributed controller uses the navigation system state matrix χ obtained from a fully distributed observer. i Predictor estimates signal and adaptive fuzzy estimates (unknown nonlinear function Γ of the unmanned surface vessel) i (estimated value) Its control objective is to achieve the underlying control task of multiple unmanned surface vessels (USVs), namely, the control task of multiple USV formations. 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 =Fχ i

[0103]

[0104] In the formula: J represents the virtual control quantity for the i-th unmanned surface vessel in the design; i This represents a 2×2 matrix formed by the intersection of the first two rows and the first two columns of the rotation matrix; b ic This represents the estimated position of the leadership system for the i-th unmanned surface vessel. b ic The first derivative; α is a given parameter; b i θ represents the position vector of the i-th unmanned surface vessel; irepresents the formation error of the ith unmanned surface vehicle; F is a constant matrix; χ i represents the ith unmanned surface vehicle estimated leader system state; H is a given scalar; b id represents the filtered virtual control; represents the derivative of the filtered virtual control; represents the longitudinal and lateral control input of the ith unmanned surface vehicle in the earth coordinate frame; represents the position control gain; d i represents the velocity of the ith unmanned surface vehicle in the earth coordinate frame; represents the estimate of the unknown nonlinear function of the ith unmanned surface vehicle represents the estimate of the unknown nonlinear disturbance on the first two dimensions, i.e., the position related states, of the ith unmanned surface vehicle;

[0105] S5: Establish a control barrier function; and obtain the longitudinal and lateral optimal control input of the unmanned surface vehicle under the influence of the barrier according to the longitudinal and lateral control input of the unmanned surface vehicle in the earth coordinate frame S6: Perform quadratic programming on g

[0106] Specifically, the embodiment obtains the position of the barrier through a sensor, and uses the position to construct a control barrier function based on a collision avoidance mechanism to obtain an optimal control input vector under the influence of the barrier, so as to realize safe formation control of multiple unmanned surface vehicles. The embodiment uses system state information such as position and velocity to avoid obstacles by inputting the state into a safety control barrier function.

[0107] Specifically, the embodiment constructs a control barrier function based on g ik (b), and the control barrier function is established as follows:

[0108]

[0109] In the formula: represents the control barrier function; T represents transposition; is the first derivative of b ik ; α3(g ik (b)) is a continuous strictly increasing function with a domain of [0, ∞); ‖b ik ‖ represents the two-norm of b ik .

[0110] wherein,

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

[0112] b ik = b i ​-o k

[0113] where g ik (b) represents the relative position between the unmanned surface vehicle and the obstacle after adding a safety distance; k represents the number of the obstacle; b ik represents the distance between the unmanned surface vehicle and the obstacle, b i represents the position vector of the ith unmanned surface vehicle, o k is the position vector of the obstacle; D k represents the limit safety distance of the unmanned surface vehicle and the obstacle; ‖b ik ‖ represents the two-norm of b ik .

[0114] Specifically, the partial derivative of the control obstacle function is obtained, which is brought into the feasible safety set, and the collision avoidance parameter can be obtained.

[0115] Thus, the safety constraints of the multi-unmanned surface vehicle formation system can be obtained, including the unmanned surface vehicle collision avoidance safety constraint and the obstacle avoidance safety constraint. Then, by setting a quadratic programming problem, the safe formation control of the multi-unmanned surface vehicle is realized.

[0116] The safety constraints of the unmanned surface vehicle formation system of the embodiment include the unmanned surface vehicle collision avoidance safety constraint and the obstacle avoidance safety constraint;

[0117] The unmanned surface vehicle collision avoidance safety constraint is:

[0118]

[0119] The obstacle avoidance safety constraint is:

[0120]

[0121] The quadratic programming design for the unmanned surface vehicle system is as follows:

[0122]

[0123] In the formula: represents the longitudinal and lateral optimization control input of the ith unmanned surface vehicle considering the influence of the obstacle, that is, the control input capable of realizing the collision avoidance and obstacle avoidance function; represents the longitudinal and lateral control input of the ith unmanned surface vehicle in the earth coordinate system, that is, the control input when only realizing the bottom layer control task; represents the longitudinal and lateral control input of the ith unmanned surface vehicle satisfying all safety constraints; represents the value of the function when the minimum value of the function is obtained, represents the objective function of the quadratic programming; is the transpose of the position vector difference between the ith unmanned surface vehicle and the jth unmanned surface vehicle; is the collision avoidance parameter between the ith unmanned surface vehicle and the jth unmanned surface vehicle calculated by the control barrier function; is the transpose of the position vector difference between the ith unmanned surface vehicle and the obstacle; is the collision avoidance parameter between the ith unmanned surface vehicle and the obstacle calculated by the control barrier function.

[0124] S6: obtaining the optimal longitudinal control input of the unmanned surface vehicle and the actual heading control input of the unmanned surface vehicle considering the influence of the obstacle according to the adaptive fuzzy predictor and the longitudinal and lateral optimal control input of the unmanned surface vehicle obtaining the optimal longitudinal control input of the unmanned surface vehicle and the actual heading control input of the unmanned surface vehicle to complete the control of the safe formation of the unmanned surface vehicle.

[0125] Preferably, the optimal longitudinal control input of the unmanned surface vehicle is obtained as follows:

[0126]

[0127] wherein: denotes the optimal longitudinal control input of the ith unmanned surface vehicle; is a parameter related to the mass of the unmanned surface vehicle; and denote the virtual longitudinal control input and the virtual lateral control input of the unmanned surface vehicle in the earth coordinate, respectively.

[0128] wherein, and denote the first dimension and the second dimension of , respectively.

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

[0130] In order to generate the optimal heading control input, the following expected heading signal is considered:

[0131]

[0132] wherein: ζ ic denotes the expected heading signal of the ith unmanned surface vehicle; atan2 denotes the inverse tangent function;

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

[0134]

[0135] wherein: r​ic a virtual control variable for the ith unmanned vehicle; ζ ic denotes a desired heading signal for the ith unmanned vehicle; a r is a positive constant; ζ i (t) denotes the actual heading of the ith unmanned vehicle; denotes a virtual heading control input for the ith unmanned vehicle; r i (t) denotes the yaw rate of the ith unmanned vehicle; denotes a heading control gain for the ith unmanned vehicle; denotes the estimated unknown nonlinear term of the ith unmanned vehicle a third dimension, i.e., the estimated value of the unknown nonlinear disturbance suffered by the direction-dependent state.

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

[0137]

[0138] wherein: denotes the actual heading control input for the ith unmanned vehicle; denotes a parameter related to the mass of the unmanned vehicle.

[0139] The overall beneficial effects of the embodiment compared to the prior art are:

[0140] First, compared to most current distributed observers, the embodiment proposes an adaptive fully distributed observer that accurately estimates the leader system state without prior knowledge of the network global information.

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

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

[0143] In summary, the embodiment further optimizes the performance and function of the unmanned vehicle safe formation control problem through in-depth analysis and research, improves its operation ability in various environments and conditions, and better serves the needs of human exploration of the ocean, resource development, environmental monitoring, etc.

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

[0145] A multi-unmanned-boat formation system composed of four unmanned boats is taken as an example to further illustrate the technical solutions in this embodiment. The following parameters are in units of meters for position-related parameters and in units of radians for angle-related parameters. The model inertia matrix of each unmanned boat is as follows:

[0146]

[0147] The initial position is set as:

[0148]

[0149] wherein b(0) represents a position vector of the unmanned-boat formation;

[0150] The initial angle is set as:

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

[0152] wherein ζ(0) is a heading angle vector of the unmanned-boat formation;

[0153] The formation error is set as:

[0154]

[0155] wherein θ is an error vector of the unmanned-boat formation;

[0156] The leader 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] The static obstacle position is Obstacle1 = [50, 50],

[0161] The dynamic obstacle position is Obstacle2 = [56, 60 - 2t], wherein t represents time;

[0162] In this embodiment, the obstacle in the simulation implementation case 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 show the observer error diagrams of the four unmanned boats, from Figure 2a and Figure 2bIt is observed that the complete distributed observer proposed in this embodiment can accurately estimate the state of the pilot system; Figure 3a and Figure 3b The following shows the tracking error diagram of the four unmanned ships, from Figure 3a and Figure 3b It is observed that under the action of the control law proposed in this embodiment, the four unmanned ships can form a formation and complete the cooperative control task; Figure 4a and Figure 4b The following shows the trajectory and obstacle avoidance diagram of the four unmanned ships, clearly showing that they have successfully completed the collision avoidance between unmanned ships and obstacle avoidance during the entire obstacle avoidance process.

[0164] In summary, the unmanned ship safe formation control method based on a complete distributed observer of this embodiment, through the leader state in the unmanned ship formation system, an adaptive complete distributed observer is established to obtain the estimated state of the pilot system of the unmanned ship; and according to the estimated value of the unknown nonlinear function based on the adaptive fuzzy predictor, a distributed controller is established, and a control barrier function is combined to obtain the longitudinal and lateral optimization control input of the unmanned ship under the influence of the obstacle, and finally the optimal longitudinal control input of the unmanned ship and the actual heading control input of the unmanned ship are obtained, and the control of the safe formation of the unmanned ship is completed. The adaptive complete distributed observer is used to estimate the state of the pilot system, the unknown nonlinear function approximated by the fuzzy system of the adaptive fuzzy predictor and the error between the state and the estimated state of the pilot system are combined to design a distributed controller, and the control input of the unmanned ship in the earth coordinate system is obtained, and the optimal longitudinal control input of the unmanned ship and the actual heading control input of the unmanned ship are obtained considering the influence of the obstacle, so as to realize the safe control of the unmanned ship formation. Compared with the existing pilot system state estimation method, the local information and the state of the unmanned ship can be estimated in the case that the network global information is unknown, so that the unmanned ship formation control system will not be greatly affected by the failure of a node, and the unknown parameters of the pilot system can also be estimated, improving the reliability of the unmanned ship formation control. At the same time, when the unmanned ship safe formation control task is executed, even if the unmanned ship encounters dynamic and static obstacles, it can still continue to complete the formation control task while ensuring the safe collision avoidance between the unmanned ships and the collision avoidance of the obstacles. This embodiment can be applied to unmanned ships and can realize cooperative formation control. Through the complete distributed observer, the pilot signal can be accurately estimated, which is convenient for the design of the controller; and through the design of the safe optimization mechanism based on the control barrier function, the system can effectively complete the bottom formation control task while realizing the safe formation control, avoiding the loss caused by the collision of the unmanned ship during the task.

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

Claims

1. A method for safe formation control of unmanned surface vehicles based on a fully distributed observer, characterized in that, The method comprises the following steps: S1: establishing a model of an under-actuated unmanned surface vehicle in a multi-unmanned surface vehicle formation system composed of under-actuated unmanned surface vehicle models; and obtaining a leader state in the unmanned surface vehicle formation system; S2: establishing an adaptive full-distributed observer according to the model of the under-actuated unmanned surface vehicle and the leader state in the unmanned surface vehicle formation system, to obtain an estimated navigation system state of the unmanned surface vehicle; S3: establishing an adaptive fuzzy predictor to obtain an estimated value of an unknown nonlinear function; S4: establishing a distributed controller according to the estimated navigation system state of the unmanned surface vehicle and the estimated value of the unknown nonlinear function, to obtain longitudinal and lateral control inputs of the unmanned surface vehicle in an earth coordinate system; S5: establishing a control barrier function, to obtain longitudinal and lateral optimal control inputs of the unmanned surface vehicle considering the influence of obstacles based on the longitudinal and lateral control inputs of the unmanned surface vehicle in the earth coordinate system and the safety constraints of the unmanned surface vehicle formation system; S6: obtaining optimal longitudinal control inputs of the unmanned surface vehicle and actual heading control inputs of the unmanned surface vehicle according to the adaptive fuzzy predictor and the longitudinal and lateral optimal control inputs of the unmanned surface vehicle considering the influence of obstacles, to complete the control of the safe formation of the unmanned surface vehicle according to the optimal longitudinal control inputs of the unmanned surface vehicle and the actual heading control inputs of the unmanned surface vehicle; In the S6, the actual heading control inputs of the unmanned surface vehicle are obtained as follows: wherein: is a design parameter a virtual control of the unmanned surface vehicle; denotes the a desired heading signal of the unmanned surface vehicle; is a positive constant; denotes a true heading of the unmanned surface vehicle; denotes a virtual heading control input of the unmanned surface vehicle; denotes a yaw rate of the unmanned surface vehicle; denotes a heading control gain of the unmanned surface vehicle; denotes an estimated unknown nonlinear term of the unmanned surface vehicle a third dimension, i.e. a direction dependent state, of the unmanned surface vehicle is subjected to an unknown nonlinear disturbance; denotes an arctangent function; and denote a virtual longitudinal control input and a virtual lateral control input of the unmanned surface vehicle in earth coordinates, respectively; Then wherein: represents actual heading control input for the unmanned surface vehicle; represents a parameter related to the mass of the unmanned surface vehicle.

2. The unmanned surface vehicle safety formation control method based on a fully distributed observer according to claim 1, characterized in that, In the S2, the adaptive full-distributed observer is established as follows: wherein wherein: represents the estimated state of the lead system of the USV, , represents Euclidean space, when = 0, represents the matrix related to the state of the lead system; represents the first derivative of represents the estimated parameter matrix of the lead system of the USV; represents the dynamic gain of the USV; each of represents the total number of USVs; represents the connectivity coefficient of the USV with the USV; represents the estimated state of the lead system of the USV; represents the first derivative of is an adaptive gain; is a scalar; represents the sum of the estimated state of the lead system of the USV and the difference of the estimated state of the lead system of the USV connected with the USV; represents the frequency of the multi-tone sinusoidal signal estimated by the lead system of the USV, , represents a column vector, the first and second elements of the column vector of the frequency of the multi-tone sinusoidal signal, respectively; represents the first derivative of represents a block diagonal matrix; represents a square matrix whose elements of order represents the Kronecker product, represents a known second-order matrix.

3. The unmanned surface vehicle safety formation control method based on a fully distributed observer according to claim 1, wherein, In the S1, the model of the under-actuated unmanned surface vehicle is established as follows: In the formula: Represents the position and orientation of the i-th unmanned surface vessel in the Earth coordinate system, where , These represent the x-coordinate and y-coordinate of the position in the Earth coordinate system, respectively. express The bow angle of the unmanned surface vessel, among which, ; express Vioclimatic space; Indicates transpose; Denotes the rotation matrix; where, diag , diag Table diagonal matrix; This represents the intersection of the first two rows and the first two columns of the rotation matrix. The matrix, where ; express The bow angle of the unmanned surface vessel, among which, ; express The velocity of the unmanned surface vessel in the ship's coordinate system, where, , Indicates the oscillation velocity. Indicates sway speed, express The bow angular velocity of the unmanned surface vessel; express The first derivative; express Vioclimatic space; express The inertial matrix of the unmanned surface vessel, where , All of these represent parameters related to the mass of the unmanned surface vessel; express Vioclimatic space; Represents the Coriolis force, centripetal force matrix, and nonlinear damping matrix; For consideration Unmanned surface vessels are subject to external disturbances caused by complex air currents and extreme weather. Indicates the first The control inputs of the unmanned surface vessel, among which , For longitudinal control input, This is the yaw control input; Indicates the index number of the unmanned surface vessel, where Indicates the total number of unmanned surface vessels; Let then wherein: denotes a position vector of the unmanned surface vehicle; denotes a first derivative of denotes a velocity of the unmanned surface vehicle in the earth coordinate frame; denotes a first derivative of denotes an unknown nonlinear disturbance to the position state of the unmanned surface vehicle; denotes a heading angle of the unmanned surface vehicle; denotes time; denotes a generalized longitudinal and lateral control input of the unmanned surface vehicle in the earth coordinate frame; denotes a first derivative of denotes an angular velocity of the unmanned surface vehicle; denotes a first derivative of denotes an unknown nonlinear disturbance to the heading state of the unmanned surface vehicle; denotes a velocity of the unmanned surface vehicle in the body coordinate frame; denotes a generalized heading control input of the unmanned surface vehicle.

4. The unmanned surface vehicle safety formation control method based on a fully distributed observer of claim 1, wherein, In the S1, the leader state in the unmanned surface vehicle formation system is represented as follows: wherein: denotes a matrix related to the leader system state; denotes the first derivative of is a constant matrix; denotes a d-dimensional Euclidean space; denotes the position information of the leader system consisting of a multi-tone sinusoidal signal; denotes a parameter matrix of the leader system; denotes a d-dimensional Euclidean space; denotes a block-diagonal matrix, denotes a square matrix with zero elements of order denotes a diagonal matrix, all denote frequencies of the multi-tone sinusoidal signal, denotes the Kronecker product, denotes a known second-order matrix.

5. The unmanned surface vehicle safety formation control method based on a fully distributed observer according to claim 1, characterized in that, In the S3, the adaptive fuzzy predictor is established as follows: In the formula: express The estimated value, express The first derivative; This is the weight matrix; For Estimated value; The activation function is known. Indicates the first Control inputs for an unmanned surface vessel; Indicates the predictor gain; This indicates the error between the estimated speed and the actual speed. Unknown nonlinear function representing an unmanned surface vessel The estimated value; The unknown nonlinear function representing the unmanned surface vessel; Indicates the function reconstruction error; for The first derivative; Indicates adaptive gain; It is a constant.

6. The unmanned surface vehicle safety formation control method based on a fully distributed observer according to claim 1, characterized in that, In the S4, the distributed controller is established as follows: In the formula: For the design of the first Virtual control variables for unmanned surface vessels; This represents the intersection of the first two rows and the first two columns of the rotation matrix. Matrix; express The location of the leadership system estimated by the unmanned surface vessel. express The first derivative; For given parameters; express The position vector of the unmanned surface vessel; express Formation error of unmanned surface vessels; It is a constant matrix; express The estimated navigation system status of the unmanned surface vessel; For a given scalar; This represents the filtered virtual control quantity; This represents the derivative of the filtered virtual control quantity. express The longitudinal and lateral control inputs of the unmanned surface vessel in the Earth coordinate system; Indicates position control gain; express The speed of the unmanned surface vessel in the Earth coordinate system; Indicates the first Estimated value of unknown nonlinear function of unmanned surface vessel The first two dimensions are estimates of the unknown nonlinear disturbances experienced by the position-related state.

7. The unmanned surface vehicle safety formation control method based on a fully distributed observer according to claim 1, characterized in that, In the S5, the control barrier function is established as follows: In the formula: Represents the control barrier function; T Indicates transpose; for The first derivative; A function whose domain is [0,∞) is continuously and strictly increasing; express The second norm; Wherein, wherein: represents the relative position between the USV and the obstacle after adding a safety distance; represents the number of the obstacle; represents the distance between the USV and the obstacle, represents the position vector of the USV, is the position vector of the obstacle; represents the limit safety distance for collision avoidance between the USV and the obstacle; represents the two-norm of 8. The unmanned surface vehicle safety formation control method based on a fully distributed observer according to claim 1, wherein, In the S5, the formula used to obtain the longitudinal and lateral optimal control inputs of the unmanned surface vehicle considering the influence of obstacles is as follows: In the formula: represents the first The longitudinal and lateral optimization control input of the unmanned ship considering the influence of obstacles, that is, the control input that can realize the collision avoidance and obstacle avoidance function; represents The longitudinal and lateral control input of the unmanned ship in the earth coordinate system, that is, the control input when only the underlying control task is implemented; represents the first The longitudinal and lateral control input of the unmanned ship to meet all safety constraints; represents the value of the function when the minimum value is obtained represents the objective function of the quadratic programming.​ 9. The unmanned surface vehicle safety formation control method based on a fully distributed observer according to claim 1, wherein, In the S6, the optimal longitudinal control inputs of the unmanned surface vehicle are obtained as follows: wherein: denotes the optimal longitudinal control input of the unmanned surface vehicle; parameters related to the mass of the unmanned surface vehicle; and denote the virtual longitudinal control input and the virtual lateral control input of the unmanned surface vehicle in earth coordinates, respectively.

Images