A ship formation system switching control method based on two-degree-of-freedom LQR

Abstract

The application discloses a ship formation system switching control method based on two-degree-of-freedom LQR, which comprises the following steps: establishing a ship mathematical model, defining the relative position vector between the position of each ship in the north-east coordinate system and the corresponding formation reference point; establishing a communication topology graph through a directed strongly connected graph between ships; obtaining a corresponding coordination controller according to an input expected state signal based on a designed ship cooperative formation controller; designing a cooperative smooth controller based on two-degree-of-freedom smooth LQR; obtaining a ship cooperative formation switching control law suitable for the ship based on the cooperative smooth controller; obtaining an expected state signal according to the control signal of the ship and the instruction of the commander by the cooperative formation controller, and switching different coordination controllers according to the expected state signal by the cooperative formation controller. The problem that the switching of different control systems in the ship formation of multiple control systems has a large time delay and error, and the ship formation controller is disturbed more greatly is solved.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of water surface coordination of multiple ships, and particularly to a ship formation system switching control method based on two-degree-of-freedom smooth LQR. BACKGROUND

[0002] With the rapid development of ship control technology, multi-water surface ship task execution has become a general trend, but ships often need to coordinate with each other during actual maritime operations, and are required to perform different coordination tasks according to operation requirements. Most of the current ship formations are single control systems. However, in a multi-control system ship formation, the switching of different control systems has a large time delay and error, which makes the ship formation controller more disturbed. SUMMARY

[0003] The present application provides a ship formation system switching control method based on two-degree-of-freedom smooth LQR to overcome the problem that in a multi-control system ship formation, the switching of different control systems has a large time delay and error, which makes the ship formation controller more disturbed.

[0004] In order to achieve the above purpose, the technical scheme of the present application is:

[0005] A ship formation system switching control method based on two-degree-of-freedom LQR, comprising the following steps:

[0006] Step S1: Establish a mathematical model of the ship, and define the relative position vector between the position of each ship in the north-east coordinate system and the corresponding formation reference point according to the specified formation reference point of each ship;

[0007] Step S2: A communication topology graph is established between the ships through a directed strongly connected graph;

[0008] A cooperative formation controller of the ship is designed, and the cooperative formation controller generates an input desired state signal according to the control signal of the ship and the instruction of the instructor, and obtains a coordination controller corresponding to the input desired state signal;

[0009] The input desired state signal includes a coordination tracking control signal and a coordination positioning control signal;

[0010] The coordination controller includes a coordination tracking controller and a coordination positioning controller;

[0011] Step S3: A general cooperative smooth controller is designed based on two-degree-of-freedom smooth LQR through the coordination tracking controller and the coordination positioning controller;

[0012] The general cooperative smooth controller is used to obtain a ship cooperative formation switching control law; the ship cooperative formation switching control law comprises a first transition control law and a second transition control law;

[0013] The first transition control law is a transition control law from a cooperative formation controller to a coordinated tracking controller;

[0014] The second transition control law is a transition control law from a coordinated trajectory tracking controller to a coordinated positioning controller;

[0015] The coordinated controller and the cooperative formation controller are smoothly transitioned through the ship cooperative formation switching control law;

[0016] Step S4: The cooperative formation controller updates an input expected state signal according to a control signal of the ship and an instruction of an instructor, and switches different coordinated controllers according to the updated input expected state signal;

[0017] If a coordinated trajectory tracking operation is performed first in the maritime operation, the cooperative formation controller is switched to the coordinated tracking controller to realize coordinated trajectory tracking of the ship through the transition control law from the cooperative formation controller to the coordinated tracking controller;

[0018] If a coordinated positioning operation is performed first, the coordinated tracking controller can be switched to the coordinated positioning controller to realize coordinated positioning of the ship directly through the transition control law from the coordinated trajectory tracking controller to the coordinated positioning controller, without passing through the cooperative formation controller.

[0019] Further, the kinematic mathematical model of the ship established in S1 is:

[0020]

[0021] In the formula, R(ψ) represents a conversion matrix; denotes the derivative of the position vector η of the ship; v denotes the derivative of the position vector η of the ship; M v denotes a system inertia matrix composed of hydrodynamic added mass; C v (v) denotes a Coriolis centripetal force matrix acting on the ship; D v (v) denotes a ship hydrodynamic damping coefficient matrix composed of potential damping, friction damping on the surface of the ship body, wave drift damping and vortex damping; τ v denotes an external force or torque input given by an actuator of the ship; η = [n, e, ψ] T denotes the north position, east position and heading of the ship in the north-east coordinate system; v = [u, v, r] T denotes the surge velocity, sway velocity and turning rate of the ship in the ship coordinate system;

[0022] The kinematic mathematical model of the ship is deformed to obtain a mathematical model equation in the North-East coordinate system as follows:

[0023]

[0024] wherein M(η)=R(ψ)M v R -1 (ψ),

[0025] τ=R(ψ)τ v ; R(ψ) represents a conversion matrix; represents a derivative of a position vector η i of the i th ship; represents a second derivative of a position vector η i of the i th ship; M(η) represents a system inertia matrix composed of a ship rigid body inertia and a water dynamic added mass; represents a Coriolis centripetal force matrix acting on the ship; D i (η i ) represents a ship water dynamic damping coefficient matrix; and τ represents a ship propeller torque.

[0026] Further, the relative position vector of each ship in the North-East coordinate system is defined according to the respective formation reference point of each ship in step S1, and specifically,

[0027] Step S1.1: assuming that n ships perform a cooperative formation operation, a corresponding state variable of each ship is represented by subscript i (i=1, 2, …, n);

[0028] First, the relative position vector between the position of each ship in the North-East coordinate system and the corresponding formation reference point is defined as follows by respectively specifying the respective formation reference point of each ship:

[0029] l i =[x oi y oi ψ oi ] T ,i=1,2,…,n.

[0030] η i (t) represents an actual position of the ship; x 0i represents a longitudinal position of the relative vector; y 0i represents a transverse position of the relative vector; and ψ0i represents a bow yaw angle of the relative vector;

[0031] Step S1.2: according to the defined relative position vector, the position of the formation reference point of all ships is represented as follows:

[0032] x i = η i (t) + l i , i = 1, 2, …, n.

[0033] When x1= x2= ··· = x n , all the ship formation reference points reach synchronization;

[0034] If the desired trajectory of the formation reference point is defined as η d (t), and satisfies x1= x2= … = x n = η d (t), the ship achieves coordinated tracking;

[0035] If the actual position of the formation reference point is defined as η d = [x d y d ψ d ] T , where x d , y d , ψ d are constants, and satisfies: x1= x2= … = x n = η d , the ship achieves coordinated positioning.

[0036] Further, the step S2 of designing the cooperative formation controller of the ship is specifically

[0037] Step S2.1: defining the position tracking error of the formation reference point of each ship as:

[0038]

[0039] wherein η d (t) represents the desired trajectory of the formation reference point; x i represents the position of the formation reference point.

[0040] Step S2.2: defining a new vector S i for each ship:

[0041]

[0042] wherein e represents the position tracking error of the formation reference point of each ship; e represents the derivative of the position tracking error of the formation reference point of each ship; λ i ∈ R 3×3 represents a positive diagonal matrix.

[0043] Step S2.3: defining the relative position error of the formation reference point of the ship as:

[0044] e ij = x j - x i (5)

[0045] where x j denotes the position of the ship, x i denotes the position of the formation reference point;

[0046] By calculation, we have:

[0047]

[0048] where s j denotes the actual error of the ship: denotes the position tracking error of the formation reference point of each ship; denotes the derivative of the position tracking error of the formation reference point of each ship; λ j ∈ R 3×3 is a positive definite diagonal matrix;

[0049] Step S2.4: Express the control input of the cooperative formation controller of each ship as:

[0050]

[0051] where R(ψ i ) denotes the transformation matrix, denotes the derivative of the position vector η i of the i-th ship, denotes the second derivative of the position vector η i of the i-th ship, denotes the first derivative of η d (t), denotes the second derivative of η d (t); M i (η i ) denotes the system inertia matrix composed of the rigid body inertia of the ship and the added mass of the hydrodynamic force, denotes the Coriolis centripetal force matrix acting on the ship, D i (η i ) denotes the ship hydrodynamic damping coefficient matrix; τ i = [τ ui τ vi τ ri ] T denotes the propeller torque of the i-th ship; τ ui denotes the ship longitudinal force; τ vi denotes the lateral force; τ ri denotes the torque, k i ∈ R 3×3is a positive definite diagonal matrix, f i is the coordinated control auxiliary input, and

[0052]

[0053] where l ij denotes the elements of the Laplacian matrix L representing the communication topology among the vessels; N i is defined as the neighborhood set of node i, i.e. the set of all nodes having a directed communication link pointing to node i.

[0054] Further, the control input of the coordinated tracking controller in step S2 is denoted as

[0055]

[0056] where τ ti denotes the control input of the coordinated tracking controller; f i denotes the coordinated control auxiliary input; s i denotes a new vector defined for each vessel; M i (η i ) denotes the system inertia matrix composed of the rigid body inertia and the hydrodynamic added mass of the vessels, denotes the Coriolis centripetal force matrix acting on the vessels, D i (η i ) denotes the hydrodynamic damping coefficient matrix of the vessels.

[0057] Further, the control input of the coordinated positioning controller in step S2 is denoted as

[0058]

[0059] where R(ψ i ) denotes the transformation matrix; denotes the derivative of the position vector η i of the i-th vessel; denotes the second derivative of the position vector η i of the i-th vessel; M i (η i ) denotes the system inertia matrix composed of the rigid body inertia and the hydrodynamic added mass of the vessels; denotes the Coriolis centripetal force matrix acting on the vessels; D i (η i ) denotes the hydrodynamic damping coefficient matrix of the vessels; τ i = [τ ui τ vi τ ri ] T denotes the propeller torque of the i-th vessel; k i ∈ R 3×3is a positive definite diagonal matrix; f i is the coordinated control auxiliary input.

[0060] Further, the control input of the ship to achieve the desired formation control is obtained according to the control input of the cooperative formation controller of each ship;

[0061] The control input of the ship to achieve the desired formation control is used to achieve the ship formation control before the coordinated tracking controller coordinates tracking;

[0062] The control input of the ship to achieve the desired formation control is expressed as

[0063]

[0064] In the formula, R (ψ i ) represents a transformation matrix; represents the derivative of the position vector η i of the i-th ship; represents the second derivative of the position vector η i of the i-th ship; M i (η i ) represents the system inertia matrix composed of the rigid body inertia of the ship and the added mass of the hydrodynamic force; represents the Coriolis centripetal force matrix acting on the ship; D i (η i ) represents the ship hydrodynamic damping coefficient matrix; τ i = [τ ui τ vi τ ri ] T represents the propeller torque of the i-th ship; k0∈R 3×3 is a positive definite diagonal matrix, f i is the coordinated control auxiliary input.

[0065] Further, the general cooperative smoothing controller is designed in step S3, specifically

[0066] Step S3.1: define the control objective function of the traditional LQR as:

[0067] E η (t) = η (t) - η d (t) (12)

[0068] E τ (t) = τ (t) - τ d (t) (13)

[0069] In the formula, E η (t) and E τ(t) is the actual position of the ship; η d (t) is the desired position of the ship; τ(t) is the actual power output of the ship controller; τ d (t) is the desired power output of the ship controller;

[0070] Step S3.2: Extracting the two-degree-of-freedom state space model of the offline controller, which is:

[0071]

[0072] u1=Cx+D1k+D2y1 (15)

[0073] wherein, denotes the first-order differential form of the state variable; A denotes an unknown coefficient matrix; B1 denotes an unknown coefficient matrix; B2 denotes an unknown coefficient matrix; x denotes the state variable; k is a constant; y1 denotes the system output; u1 denotes the system input; C denotes an unknown coefficient matrix; D1 denotes an unknown coefficient matrix; D2 denotes an unknown coefficient matrix;

[0074] Step S3.3: According to formula (14) and formula (15), the control objective function of the improved LQR is:

[0075]

[0076] wherein, η(t) denotes the actual position of the ship; η d (t) denotes the desired position of the ship; τ(t) denotes the actual power output of the ship controller; τ d (t) denotes the desired power output of the ship controller; T denotes the upper limit of integration, and the time period of the control process; u2(t) denotes the actual input of the system; W η denotes the weight coefficient matrix; τ(t) denotes the actual control force of the ship; τ d (t) denotes the desired control force of the ship;

[0077] Step S3.4: By means of the Lagrange multiplier λ(t)∈R n , formula (16) can be rewritten as:

[0078]

[0079] wherein, denotes the rewritten objective function; T0 denotes the rewritten upper limit of integration; λ(t) T denotes the transpose of λ(t)∈R n ; denotes the differential form of the actual state variable;

[0080] In formula (17), H(t) is a Hamiltonian, which can be expressed as:

[0081]

[0082] Step S3.5: According to the first-order optimality necessary condition of H(t), the following formula is obtained:

[0083]

[0084] In the formula, ∂H(t) / ∂x represents the derivative of H(t) with respect to x;

[0085] Then, the optimal solution of the Hamiltonian H(t) is:

[0086]

[0087] In the formula, D1 T is the transpose of the matrix D1; the Lagrange multiplier λ is λ=P x-g, p is the solution of the differential Riccati equation; g is a parameter for solving the equation with time invariance, and the differential Riccati equation can be expressed as:

[0088]

[0089] In formula (21), represents a constant matrix; represents a constant matrix; is a constant matrix;

[0090] Step S3.6: According to the optimal control theory, the approximate value of g can be obtained as:

[0091]

[0092] In the formula, B1 represents the transpose of ; B g represents a matrix about g; is a constant diagonal matrix;

[0093] By solving formula (21) and combining g in formula (22), λ can be obtained; by substituting λ into formula (20), the final form of k when the infinite solution is determined is:

[0094]

[0095] In the formula, B1 T is the transpose of the B1 matrix; W e ​​is the weight matrix, typically a diagonal matrix; M is a diagonal matrix; This is the output value; Input value; is the system disturbance matrix; SF is the state feedback matrix, which is a time-invariant matrix.

[0096] Step S3.7: Determine the state space of the offline controller, and determine the main characteristic values ​​of the offline controller's closed loop based on the state space of the offline controller;

[0097] The main characteristic value of the offline controller closed loop is determined by the output u1 of the offline controller. The overshoot percentage and rise time in the linear control criterion of the tracking ship cooperative formation controller u2 with minimum error tracking control can be used to formulate the expected characteristic value of the offline controller.

[0098] The general form of the offline controller closed-loop system can be expressed as:

[0099] A CI =A-B1k1 (24)

[0100]

[0101] In the formula, matrix W τ and W η The pole placement algorithm of the genetic algorithm can be used to achieve the ideal system response; the W τ and W η The determination principle is: to converge the error between the closed-loop pole location and the desired pole location of the offline controller to W. τ and W η A value, specifically:

[0102]

[0103] In formula (26), and These represent the desired pole value and the offline controller pole value, respectively; N represents a positive constant.

[0104] The penalty value ρ in formula (25) can be expressed as:

[0105] ρ=ρ ctr +ρ obs (27)

[0106] In the formula, ρ represents the system penalty value; ρ ctr ρ is a constant; obs It is a constant; if If it is controllable, then ρ ctr =0, otherwise ρ ctr =1; if If it is observable, then ρ obs= 0, otherwise p obs = 1.

[0107] Further, the transition control law from the cooperative formation controller to the coordinated trajectory tracking controller in step S3 is:

[0108] τ fti = (1 - a(e T Ae))τ fi + a(e T Ae)τ ti (28)

[0109] where τ ti denotes the control input of the controller; τ fi denotes the control input of the desired formation; e denotes the vector form of the position synchronization error between the reference points of all vessels; A denotes the adjacency matrix of the topological graph of the mutual communication between vessels; a denotes the defined weight function, and

[0110] Further, the transition control law from the coordinated trajectory tracking controller to the coordinated positioning controller in step S3 is:

[0111] τ tpi = σ(U)τ pi +(1 - σ(U))τ ti (29)

[0112] where τ ti denotes the control input of the controller; τ pi denotes the control input of the coordinated positioning; σ(U) is a function of U, u denotes the longitudinal velocity of the vessel, v denotes the lateral velocity of the vessel, and σ(U) = exp(-(2.5U) 10 ).

[0113] Beneficial effects: the application provides a ship formation system switching control method based on two-degree-of-freedom LQR, by respectively designing a formation controller, a coordinated positioning controller, a coordinated tracking controller and a general collaborative smoothing controller between the controllers, so that the ship activates the corresponding coordinated controller according to different operation requirements, and reduces the time and error of the control system switching process through the designed transition control law, so that the formation quickly reacts and adapts to the new control system, and in actual maritime affairs, it is often necessary to switch between different operation modes according to the corresponding task requirements, and the general collaborative smoothing controller is designed to realize the smooth switching between different coordinated operation modes, so as to ensure that the ship can call the corresponding coordinated controller according to the task, and ensure the smooth transition between the coordinated controllers, thereby realizing the stable, rapid and accurate reaction of the task-driven ship collaborative formation control. BRIEF DESCRIPTION OF DRAWINGS

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

[0115] Figure 1 The flow chart of the ship formation system switching control method based on two-degree-of-freedom LQR. DETAILED DESCRIPTION

[0116] In order to make the purpose, technical scheme and advantages of the embodiments of the present application more clear, the technical scheme in the embodiments of the present application will be described clearly and completely below in combination with the drawings in the embodiments of the present application. Obviously, the described embodiments are some embodiments of the present application, not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor are within the scope of protection of the present application.

[0117] The present embodiment provides a ship formation system switching control method based on two-degree-of-freedom LQR, as shown in Figure 1 The method comprises the following steps:

[0118] Step S1: establishing a mathematical model of the ship, according to the specified respective formation reference points of each ship, defining the relative position vector between the position of each ship in the north-east coordinate system and the corresponding formation reference point;

[0119] Step S2: the communication topology graph between the ships is established through a strongly connected graph; and the implementation method of the communication topology graph established through the strongly connected graph is a known technology and is not the point of the application, and will not be described here.

[0120] The cooperative formation controller of the ship is designed, and the cooperative formation controller obtains a coordinated controller corresponding to the input desired state signal according to the input desired state signal generated by the control signal of the ship and the instruction of the instructor;

[0121] The input desired state signal includes a coordinated tracking control signal and a coordinated positioning control signal;

[0122] The coordinated controller includes a coordinated tracking controller and a coordinated positioning controller;

[0123] Step S3: a general cooperative smooth controller is designed through the coordinated tracking controller and the coordinated positioning controller based on a two-degree-of-freedom smooth LQR;

[0124] A ship cooperative formation switching control law is obtained according to the general cooperative smooth controller; the ship cooperative formation switching control law includes a first transition control law and a second transition control law;

[0125] The first transition control law is a transition control law from the cooperative formation controller to the coordinated tracking controller;

[0126] The second transition control law is a transition control law from the coordinated trajectory tracking controller to the coordinated positioning controller;

[0127] The smooth transition control of the coordinated controller and the cooperative formation controller is realized through the ship cooperative formation switching control law;

[0128] Step S4: the input desired state signal is updated by the cooperative formation controller according to the control signal of the ship and the instruction of the instructor; and the cooperative formation controller switches different coordinated controllers according to the updated input desired state signal;

[0129] If the coordinated trajectory tracking operation is performed first in the maritime operation, the cooperative formation controller is switched to the coordinated tracking controller to realize the coordinated trajectory tracking of the ship through the transition control law from the cooperative formation controller to the coordinated tracking controller;

[0130] If the coordinated positioning operation is performed first, the coordinated tracking controller can be directly switched from the coordinated positioning controller to the coordinated tracking controller to realize the coordinated positioning of the ship through the transition control law from the coordinated trajectory tracking controller to the coordinated positioning controller, without passing through the cooperative formation controller.

[0131] By designing the formation controller, the coordinated positioning controller, the coordinated tracking controller and the general collaborative smoothing controller between the controllers respectively, the ship can activate the corresponding coordinated controller according to different operation requirements, and through the designed transition control law, the time and error of the control system switching process are reduced, so that the formation can quickly respond and adapt to the new control system. In actual maritime affairs, it is often necessary to switch between different operation modes according to the corresponding task requirements. The general collaborative smoothing controller is designed to realize the smooth switching between different coordinated operation modes, so as to ensure that the ship can call the corresponding coordinated controller according to the task and ensure the smooth transition between the coordinated controllers, thereby realizing the task-driven ship cooperative formation control. The smooth transition between the controllers is ensured, so as to realize the stable, rapid and accurate response of the task-driven ship cooperative formation control.

[0132] In specific embodiments, the kinematic mathematical model of the ship in S1 is:

[0133]

[0134] In the formula, R(ψ) represents a conversion matrix; denotes the derivative of the position vector η of the ship; v represents the derivative of the position vector η of the ship; M v denotes the system inertia matrix composed of hydrodynamic added mass; C v (v) denotes the Coriolis centripetal force matrix acting on the ship; D v (v) denotes the hydrodynamic damping coefficient matrix of the ship, mainly composed of potential damping, friction damping on the surface of the ship body, wave drift damping and vortex damping; τ v denotes the external force or torque input given by the actuator of the ship; η = [n, e, ψ] T denotes the north position, east position and heading of the ship in the north-east coordinate system; v = [u, v, r] T denotes the surge velocity, sway velocity and turning rate of the ship in the ship coordinate system;

[0135] The kinematic mathematical model of the ship is deformed to obtain the mathematical model equation in the north-east coordinate system:

[0136]

[0137] In the formula, M(η) = R(ψ)M v R -1 (ψ),

[0138] τ = R(ψ)τ v ; R(ψ) represents a conversion matrix; denotes the position vector η of the i-th shipi derivative of denotes the position vector of the i-th ship i second derivative of i M(η) denotes the system inertia matrix composed of ship rigid body inertia and hydrodynamic added mass; i D(η) denotes the ship hydrodynamic damping coefficient matrix; τ denotes the ship propeller torque.

[0139] In specific embodiments, in step S1, the position of each ship in the North-East coordinate system is defined as a position vector related to the formation according to assigning a respective formation reference point to each ship, specifically

[0140] Step S1.1: assuming that n ships perform cooperative formation operation, the corresponding state variable of each ship is represented by subscript i (i = 1, 2, …, n);

[0141] First, a respective formation reference point is assigned to each ship, and the relative position vector between the position of each ship in the North-East coordinate system and the corresponding formation reference point is defined as:

[0142] l i = [x oi y oi ψ oi ] T ,i = 1, 2, …, n.

[0143] η i (t) denotes the actual position of the ship; x 0i denotes the longitudinal position of the relative vector; y 0i denotes the transverse position of the relative vector; ψ 0i denotes the yaw angle of the relative vector;

[0144] Step S1.2: according to the defined relative position vector, the positions of the formation reference points of all ships can be represented as:

[0145] x i = η i (t) + l i ,i = 1, 2, …, n.

[0146] The position relationship between the formation reference points of the ships can directly reflect the formation structure between the ships, and when x1= x2= ··· = x n , the cooperative formation controller can make all the formation reference points reach synchronization and consistency;

[0147] If the expected trajectory of the formation reference point is defined as η d (t), and satisfies x1= x2= … = x​n = η d (t), the ship achieves coordinated tracking;

[0148] If the actual position of the formation reference point is defined as where x d , y d , ψ d are constants, and satisfy: x1=x2=…=x n = η d , the ship achieves coordinated positioning; obviously, coordinated positioning can be regarded as a special case of coordinated tracking.

[0149] In specific embodiments, the step of designing the cooperative formation controller of the ship in S2 specifically comprises

[0150] Step S2.1: define the position tracking error of the formation reference point of each ship as:

[0151]

[0152] where η d (t) represents the desired trajectory of the definition of the formation reference point; x i represents the position of the formation reference point;

[0153] Step S2.2: define a new vector S i for each ship:

[0154]

[0155] wherein represents the position tracking error of the formation reference point of each ship; represents the derivative of the position tracking error of the formation reference point of each ship; λ i ∈ R 3×3 represents a positive diagonal matrix.

[0156] Step S2.3: define the relative position error of the formation reference point of the ship as:

[0157] ei j = x j -x i (5)

[0158] wherein x j represents the position of the ship, and x i represents the position of the formation reference point;

[0159] Through calculation, it can be obtained that:

[0160]

[0161] wherein sj denotes the actual error of the ship; denotes the position tracking error of the formation reference point of each ship; denotes the derivative of the position tracking error of the formation reference point of each ship; j ∈ R 3×3 is a positive definite diagonal matrix;

[0162] Step S2.4: Express the control input of the cooperative formation controller of each ship as:

[0163]

[0164] where R(ψ i ) denotes the transformation matrix, denotes the derivative of the position vector η i of the i-th ship, denotes the second derivative of the position vector η i of the i-th ship, denotes the first derivative of η d (t), denotes the second derivative of η d (t); M i (η i ) denotes the system inertia matrix composed of the rigid body inertia of the ship and the hydrodynamic added mass, denotes the Coriolis centripetal force matrix acting on the ship, D i (η i ) denotes the ship hydrodynamic damping coefficient matrix; τ i = [τ ui τ vi τ ri ] T denotes the propeller torque of the i-th ship; τ ui denotes the ship longitudinal force; τ vi denotes the lateral force; τ ri denotes the moment, k i ∈ R 3×3 is a positive definite diagonal matrix, f i is the coordinated control auxiliary input, and

[0165]

[0166] where l ij denotes the elements of the Laplacian matrix L of the communication topology graph among the ships; N i is defined as the neighborhood of node i, i.e., the set of all nodes whose directed communication link points to node i.

[0167] In specific embodiments, when the desired state signal is chosen as a trajectory, i.e., ηd (t) = [x d (t) y d (t) ψ d (t)] T , then the control input of each ship to achieve the coordinated tracking control is represented as

[0168]

[0169] where τ ti represents the control input of the coordinated tracking controller; f i represents the coordinated control auxiliary input; s i represents a new vector defined for each ship; M i (η i ) represents the system inertia matrix composed of the ship rigid body inertia and the hydrodynamic added mass, represents the Coriolis centripetal force matrix acting on the ship; D i (η i ) represents the ship hydrodynamic damping coefficient matrix.

[0170] In a specific embodiment, when the desired reference signal is selected as a desired position, i.e., η d = [x d y d ψ d ] T , x d y d ψ d are constants, respectively, then the control input of each ship to achieve the ship coordinated positioning is represented as

[0171]

[0172] where R(ψ i ) represents the conversion matrix; represents the derivative of the position vector η i of the i-th ship; represents the second derivative of the position vector η i of the i-th ship; M i (η i ) represents the system inertia matrix composed of the ship rigid body inertia and the hydrodynamic added mass; represents the Coriolis centripetal force matrix acting on the ship; D i (η i ) represents the ship hydrodynamic damping coefficient matrix; τ i = [τ ui τ vi τ ri ] T represents the propeller torque of the i-th ship; ki ∈R 3×3 is a positive definite diagonal matrix, f i is a coordinated control auxiliary input.

[0173] In specific embodiments, the control input for the ships to achieve the desired formation control is obtained from the control input of the cooperative formation controller of each ship;

[0174] The control input for the ships to achieve the desired formation control is used to achieve the ship formation control before the coordinated tracking controller coordinates tracking;

[0175] The control input for the ships to achieve the desired formation control is expressed as

[0176]

[0177] In the formula, R(ψ i ) represents a transformation matrix; represents the derivative of the position vector η i of the i-th ship; represents the second derivative of the position vector η i of the i-th ship; M i (η i ) represents the system inertia matrix composed of the rigid body inertia of the ship and the hydrodynamic added mass; represents the Coriolis centripetal force matrix acting on the ship; D i (η i ) represents the ship hydrodynamic damping coefficient matrix; τ i = [τ ui τ vi τ ri ] T represents the propeller torque of the i-th ship; k0∈R 3×3 is a positive definite diagonal matrix, f i is a coordinated control auxiliary input.

[0178] In specific embodiments, the general cooperative smoothing controller is designed in step S3 according to the characteristics of the ship formation system, specifically

[0179] Step S3.1: define the control objective function of the traditional LQR as:

[0180] E η (t) = η(t) - η d (t) (12)

[0181] E τ (t) = τ(t) - τ d (t) (13)

[0182] In the formula, E η (t) and Eτ (t) is the actual position of the ship; η d (t) is the desired position of the ship; τ(t) is the actual power output of the ship controller; τ d (t) is the desired power output of the ship controller;

[0183] Step S3.2: Extracting the two-degree-of-freedom state space model of the offline controller, which is:

[0184]

[0185] u1=Cx+D1k+D2y1 (15)

[0186] wherein, denotes the first-order differential form of the state variable; A denotes an unknown coefficient matrix; B1 denotes an unknown coefficient matrix; B2 denotes an unknown coefficient matrix; x denotes a state variable; k is a constant value; y1 denotes a system output; u1 denotes a system input; C denotes an unknown coefficient matrix; D1 denotes an unknown coefficient matrix; D2 denotes an unknown coefficient matrix;

[0187] Step S3.3: According to formula (14) and formula (15), the control objective function of the improved LQR is:

[0188]

[0189] wherein, η(t) denotes the actual position of the ship; η d (t) denotes the desired position of the ship; τ(t) denotes the actual power output of the ship controller; τ d (t) denotes the desired power output of the ship controller; T denotes the upper limit of integration, and the time period of the control process; u2(t) denotes the actual input of the system; W η denotes a weight coefficient matrix; τ(t) denotes the actual control force of the ship; τ d (t) denotes the desired control force of the ship;

[0190] Step S3.4: By means of the Lagrange multiplier λ(t)∈R n , formula (16) can be rewritten as:

[0191]

[0192] wherein, denotes the rewritten objective function; T0 denotes the rewritten upper limit of integration; λ(t) T denotes the transpose of λ(t)∈R n ; and a differential form representing the actual state variable;

[0193] H(t) in equation (17) is the Hamiltonian, which can be expressed as:

[0194]

[0195] Step S3.5: According to the first-order optimality necessary condition of H(t), we have:

[0196]

[0197] where, denotes the derivative of H(t) with respect to x; denotes the derivative of H(t) with respect to λ;

[0198] The optimal solution of the Hamiltonian H(t) is

[0199]

[0200] where, D1 T is the transpose of matrix D1; vector K includes the state variables of the offline controller, the Lagrange multiplier, the system output, the output of the online controller, and the output set value of the offline controller. The Lagrange multiplier λ is λ = Px - g, and p is the solution of the differential Riccati equation; g is a parameter for solving the equation with time invariance. The differential Riccati equation can be expressed as:

[0201]

[0202] In equation (21), denotes a constant matrix; denotes a constant matrix; is a constant matrix;

[0203] Step S3.6: According to the optimal control theory, the approximate value of g can be obtained as:

[0204]

[0205] where, denotes the transpose of ; B g denotes the matrix with respect to g; is a constant diagonal matrix;

[0206] By solving equation (21) and combining g in equation (22), λ can be obtained. Substituting λ into equation (20) can determine the final form of k when the infinite solution is:

[0207]

[0208] In the formula, B1 T W is the transpose of matrix B1; e is the weight matrix, typically a diagonal matrix; M is a diagonal matrix; This is the output value; For input values; is the system disturbance matrix; SF is the state feedback matrix, which is a time-invariant matrix.

[0209] Step S3.7: Determine the state space of the offline controller. In order to achieve a smooth switching effect during mode switching, determine the main characteristic value of the offline controller closed loop based on the state space of the offline controller.

[0210] The main characteristic value of the offline controller closed loop is determined by the output u1 of the offline controller. The overshoot percentage and rise time in the linear control criterion of the tracking ship cooperative formation controller u2 with minimum error tracking control can be used to formulate the expected characteristic value of the offline controller.

[0211] The general form of the offline controller closed-loop system can be expressed as:

[0212] A CI =A-B1k1 (24)

[0213]

[0214] In the formula, matrix W e and W u It plays an important role in the system's transient response; matrix W τ and W η The pole placement algorithm of the genetic algorithm can be used to achieve the ideal system response; the W τ and W η The determination principle is: to converge the error between the closed-loop pole location and the desired pole location of the offline controller to W. τ and W η A very small value, specifically:

[0215]

[0216] In formula (26), and These represent the desired pole value and the offline controller pole value, respectively; N represents a sufficiently large positive constant, typically 10. 5 ;

[0217] To ensure the controllability and observability of the control system, the penalty value ρ in formula (25) can be expressed as:

[0218] ρ=ρ ctr +ρ obs(27)

[0219] where p represents the system penalty; p ctr is a constant; p obs is a constant; if controllable, p ctr = 0, otherwise p ctr = 1; if observable, p obs = 0, otherwise p obs = 1.

[0220] The general cooperative smooth controller designed according to the above analysis obtains a switching control law suitable for ship cooperative formation control. According to the coordinated controllers in different operation modes designed above, the coordinated controllers in multiple operation modes are integrated into an overall coordinated control system based on the idea of switching control. It is particularly emphasized that if the coordinated trajectory tracking operation is performed first in maritime operations, the mode is transitioned from the cooperative formation mode to the coordinated trajectory tracking mode, so that mutual collision between ships can be avoided; if the coordinated positioning mode is performed first, the mode is transitioned from the positioning mode to the coordinated trajectory tracking mode without passing through the cooperative formation mode. The supervisory module is responsible for selecting the coordinated controller in the corresponding operation mode according to the corresponding instructions or instructions directly issued by the DP operator, so as to complete the corresponding coordination task. The coordinated controllers in the two operation modes can complete the smooth transition of the operation mode through the design of an additional transition control law.

[0221] In specific embodiments, the transition control law of the cooperative formation controller to the coordinated trajectory tracking controller in step S3 is:

[0222] τ fti = (1 - a (e T Ae))τ fi + a (e T Ae)τ ti (28)

[0223] where τ ti represents the control input of the controller; τ fi represents the control input of the desired formation; e represents the vector form of the position synchronization error between the formation reference points of all ships; A represents the adjacency matrix of the topological graph of the mutual communication between the ships; a represents a defined weight function, and

[0224] In specific embodiments, the control law of the transition process of the coordinated trajectory tracking controller to the coordinated positioning controller in step S3 is:

[0225] τ tpi = σ (U)τ pi + (1 - σ (U))τ ti(29)

[0226] where τ ti represents the control input of the controller; τ pi represents the control input of the coordinated positioning controller; σ(U) is a function of U, u represents the longitudinal velocity of the ship, v represents the lateral velocity of the ship, and σ(U) = exp(-(2.5U) 10 ).

[0227] According to the general coordinated smooth controller design process designed according to the above analysis, the controller in each task mode is stable, and the transition process between the controllers is realized by designing a transition control law, so the controller is also stable in the transition process.

[0228] In severe sea conditions, the communication between multiple ships may fail, and in this case, global communication between ships cannot be guaranteed, or even the communication topology cannot be guaranteed to be a balanced graph. To solve this problem, considering that the inertia of the ship is relatively large, in actual maritime affairs, it is often necessary to switch between different operation modes according to the corresponding task requirements. By designing a smooth switching transition process between the coordinated controllers in different coordinated operation modes, the different coordinated controllers are switched according to the control signals of the ship and the instructions of the commander, and the control instruction task is completed. In maritime operations, if the coordinated trajectory tracking operation is performed first, the mode is transitioned from the coordinated formation mode to the coordinated trajectory tracking mode, which can avoid mutual collision between ships. If the coordinated positioning mode is performed first, it can be transitioned from the positioning mode to the coordinated trajectory tracking mode without passing through the coordinated formation mode. Thus, the ship can call the corresponding coordinated controller according to the task, and the smooth transition between the coordinated controllers is guaranteed, thereby realizing the coordinated formation control of the ship under the task driving.

[0229] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present application, and not to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacement to part or all of the technical features; 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 two-degree-of-freedom LQR-based ship formation system switching control method, characterized by, The method comprises the following steps: Step S1: establishing a mathematical model of the ships, defining a relative position vector between the position of each ship in a North-East coordinate system and the corresponding formation reference point according to the respective formation reference points designated for each ship; Step S2: establishing a communication topology graph between the ships through a directed strongly connected graph; A cooperative formation controller of the ships is designed, and the cooperative formation controller obtains a coordination controller corresponding to an input desired state signal according to the input desired state signal generated by the control signal of the ships and the instruction of the instructor; The input desired state signal comprises a coordinated tracking control signal and a coordinated positioning control signal; The coordination controller comprises a coordinated tracking controller and a coordinated positioning controller; Step S3: obtaining a general cooperative smooth controller based on a two-degree-of-freedom smooth LQR through the coordinated tracking controller and the coordinated positioning controller; A ship cooperative formation switching control law is obtained according to the general cooperative smooth controller; the ship cooperative formation switching control law comprises a first transition control law and a second transition control law; The first transition control law is a transition control law from the cooperative formation controller to the coordinated tracking controller; The second transition control law is a transition control law from the coordinated trajectory tracking controller to the coordinated positioning controller; And the smooth transition control of the coordinated controller and the cooperative formation controller is realized through the ship cooperative formation switching control law; Specifically, the control objective function of a traditional LQR is defined as: （12） （13） wherein and are a controller output position error of the marine vessel and a control input error of the marine vessel controller, respectively; is an actual position of the marine vessel; is a desired position of the marine vessel; is an actual power output of the marine vessel controller; is a desired power output of the marine vessel controller; A two-degree-of-freedom state space model of an offline controller is extracted, and the two-degree-of-freedom state space model of the offline controller is: （14） （15） wherein denotes a first derivative form of a state variable; denotes an unknown coefficient matrix; denotes an unknown coefficient matrix; denotes an unknown coefficient matrix; denotes a state variable; is a constant; denotes a system output; denotes a system input; denotes an unknown coefficient matrix; denotes an unknown coefficient matrix; denotes an unknown coefficient matrix; The state space of the offline controller is determined, and the main eigenvalue of the closed loop of the offline controller is determined according to the state space of the offline controller; The main eigenvalue of the offline controller closed loop is determined as the output of the offline controller With minimal error tracking control to track the ship cooperative formation controller The percentage of overshoot and the rise time in the linear control criterion can be used to formulate the desired eigenvalue of the offline controller; The general form of the closed loop system of the offline controller can be represented as: （24） （25） In the formula, the matrix The genetic algorithm pole placement algorithm can be used to achieve the ideal system response. The determination principle is to converge the error between the closed-loop pole position of the offline controller and the expected pole position to a value. The transition control law from the cooperative formation controller to the coordinated trajectory tracking controller is: （28） wherein denotes a control input of the controller; denotes a control input of the desired formation; denotes a vector form of the position synchronization error between the formation reference points of all vessels; denotes an adjacency matrix of the topological graph of the mutual communication between the vessels; denotes a defined weight function, and ; The control law of the transition process from the coordinated trajectory tracking controller to the coordinated positioning controller is: （29） wherein denotes a control input of the controller; denotes a control input of the coordinating positioner; with respect to the function , denotes a ship longitudinal velocity, denotes a ship lateral velocity, and ; Step S4: the cooperative formation controller updates the input desired state signal according to the control signal of the ships and the instruction of the instructor, and the cooperative formation controller switches different coordination controllers according to the updated input desired state signal; If the coordinated trajectory tracking operation is performed first in the maritime operation, the cooperative formation controller switches to the coordinated tracking controller to realize the coordinated trajectory tracking of the ships through the transition control law from the cooperative formation controller to the coordinated tracking controller; If the coordinated positioning operation is performed first, the coordinated tracking controller can switch to the coordinated tracking controller from the coordinated positioning controller to realize the coordinated positioning of the ships through the transition control law from the coordinated trajectory tracking controller to the coordinated positioning controller without passing through the cooperative formation controller.

2. The switching control method of a ship formation system based on two-degree-of-freedom LQR according to claim 1, characterized in that, The kinematic mathematical model of the ships in S1 is: （1） wherein represents a transformation matrix; represents a position vector of the ship derivative of represents a position vector of the ship derivative of represents a system inertia matrix composed of hydrodynamic added mass; represents a Coriolis centripetal force matrix acting on the ship; represents a ship hydrodynamic damping coefficient matrix composed of potential damping, hull surface friction damping, wave drift damping and vortex damping; represents an external force or moment input given by the ship actuators; represents the north position, east position and heading of the ship in the North-East coordinate system; represents the surge velocity, sway velocity and ship turning rate in the ship coordinate system; The kinematic mathematical model of the ships is deformed to obtain a mathematical model equation in the North-East coordinate system: （2） In the formulae, , , ; denotes the transformation matrix; denotes the position vector of the first ship ; denotes the derivative of the position vector of the first ship ; denotes the second derivative of the position vector of the first ship ; denotes the system inertia matrix composed of the ship rigid body inertia and the hydrodynamic added mass; denotes the Coriolis centripetal force matrix acting on the ship; denotes the ship hydrodynamic damping coefficient matrix; denotes the ship propeller force moment.

3. The switching control method of a ship formation system based on two-degree-of-freedom LQR according to claim 1, characterized in that, In step S1, the relative position vector between the position of each ship in the North-East coordinate system and the formation is defined according to the respective formation reference points designated for each ship, specifically Step S1.1: Assume that there are n vessels performing cooperative formation operations, and the corresponding state variable of each vessel is denoted by subscript ; First, the formation reference point of each ship is specified respectively, and the relative position vector between the position of each ship in the North-East coordinate system and the corresponding formation reference point is defined as: represents the actual position of the ship; represents the longitudinal position of the relative vector; represents the transverse position of the relative vector; represents the yaw angle of the relative vector; Step S1.2: According to the defined relative position vector, the position of the formation reference point of all ships is expressed as: When all the ships' formation reference points reach synchronization consistency; If the desired trajectory of the formation reference point is defined as and satisfies , the ship achieves coordinated tracking; If the actual position of the formation reference point is defined as where , , are constants, and satisfy: the ship achieves coordinated positioning.

4. The switching control method of a ship formation system based on two-degree-of-freedom LQR according to claim 1, characterized in that, Step S2: The cooperative formation controller of the ship is designed, specifically Step S2.1: The position tracking error of the formation reference point of each ship is defined as: （3） wherein represents a desired trajectory defining a formation reference point; represents a position of the formation reference point; Step S2.2: Defining a new vector for each ship : （4） wherein represents the position tracking error of the formation reference point of each ship; represents the derivative of the position tracking error of the formation reference point of each ship; represents a positive definite diagonal matrix; Step S2.3: The relative position error of the formation reference point of the ship is defined as: （5） In the formulae, denotes the position of the ship, denotes the position of the formation reference point; Through calculation, it can be obtained that: （6） wherein represents the actual error of the ship: , represents the position tracking error of the formation reference point of each ship; represents the derivative of the position tracking error of the formation reference point of each ship; is a positive definite diagonal matrix; Step S2.4: The control input of the cooperative formation controller of each ship is expressed as: （7） wherein represents a transformation matrix, represents the first derivative of the position vector of the th ship, represents the second derivative of the position vector of the th ship, represents the first derivative of the represents the second derivative of the represents the system inertia matrix composed of the ship rigid body inertia and the hydrodynamic added mass, represents the Coriolis centripetal force matrix acting on the ship, represents the ship hydrodynamic damping coefficient matrix; represents the th ship propeller torque, represents the ship longitudinal force, represents the lateral force, represents the moment, is a positive definite diagonal matrix, is the coordinated control auxiliary input, and （8） wherein represents the Laplacian matrix of the communication topology graph between the ships whose elements are given by is defined as the neighborhood of the node i.e. the set of all nodes to which there is a directed communication link pointing to node i.

5. The two-degree-of-freedom LQR-based ship formation system switching control method according to claim 1, characterized in that, The control input of the coordinated tracking controller in step S2 is expressed as （9） wherein represents the control input to the coordinated tracking controller; represents the coordinated control auxiliary input; represents a new vector defined for each ship; represents the system inertia matrix composed of the ship rigid body inertia and the hydrodynamic added mass, represents the Coriolis centripetal force matrix acting on the ship, represents the ship hydrodynamic damping coefficient matrix.

6. The two-degree-of-freedom LQR-based ship formation system switching control method according to claim 1, characterized in that, The control input of the coordinated positioning controller in step S2 is expressed as （10） wherein denotes the transformation matrix; denotes the position vector of the th ship; denotes the derivative of the position vector of the th ship; denotes the second derivative of the position vector of the th ship; denotes the system inertia matrix composed of the ship rigid body inertia and the hydrodynamic added mass; denotes the Coriolis centripetal force matrix acting on the ship; denotes the ship hydrodynamic damping coefficient matrix; denotes the propeller torque of the th ship; is a positive definite diagonal matrix; is the coordinated control auxiliary input.

7. The switching control method of a ship formation system based on two-degree-of-freedom LQR according to claim 4, characterized in that, The control input of the ship to achieve the desired formation control is obtained according to the control input of the cooperative formation controller of each ship; The control input of the ship to achieve the desired formation control is used to realize the ship formation control before the coordinated tracking controller is coordinated and tracked; The control input of the ship to achieve the desired formation control is expressed as （11） wherein denotes the transformation matrix; denotes the first derivative of the position vector of the th ship; denotes the second derivative of the position vector of the th ship; denotes the system inertia matrix composed of the ship rigid body inertia and the hydrodynamic added mass; denotes the Coriolis centripetal force matrix acting on the ship; denotes the hydrodynamic damping coefficient matrix of the ship; denotes the propeller torque of the th ship; is a positive definite diagonal matrix, is the coordinated control auxiliary input.

8. The two-degree-of-freedom LQR-based ship formation system switching control method according to claim 1, characterized in that, Step S3: The general cooperative smoothing controller is designed, specifically Step S3.1: The control objective function of the traditional LQR is defined; Step S3.2: The two-degree-of-freedom state space model of the offline controller is extracted; Step S3.3: According to formula (14) and formula (15), the control objective function of the improved LQR is obtained as: （16） wherein represents an actual position of the ship; represents a desired position of the ship; represents an actual power output of the ship controller; represents a desired power output of the ship controller; represents an upper limit of integration and a time period of the control process; represents an actual input of the system; represents a weight coefficient matrix; represents an actual control force of the ship; represents a desired control force of the ship; Step S3.4: The Lagrange multiplier Equation (16) can be rewritten as: （17） In the formula, denotes the rewritten objective function; denotes the rewritten upper limit of integration; denotes the transpose of denotes the differential form of the actual state variable;​ In equation (17), H is the Hamiltonian, which can be expressed as: （18） Step S3.5: According to the first order optimality necessary condition of , （19） wherein denotes derivative with respect to x; denotes derivative with respect to derivative with respect to The optimal solution for the Hamiltonian is then given by （20） wherein is the transpose of the matrix ; the Lagrange multiplier is , is the solution of the differential equation; is time-invariant for the parameters to solve the equation, the differential equation can be expressed as: （21） In formula (21), ; denotes a constant matrix; denotes a constant matrix; is a constant matrix; Step S3.6: According to the optimal control theory, the approximate value of g is obtained as: （22） wherein denotes the transpose of ; and denotes a matrix with respect to g; is a constant diagonal matrix; By solving equation (21) and combining equation (22) , we can get ; substituting into equation (20) can determine The final form in the infinite solution is: （23） wherein is the transpose of the matrix; is a weight matrix, typically a diagonal matrix; is a diagonal matrix; is the output value; is the input value; is the system disturbance matrix; SF is the state feedback matrix, which is a time-invariant matrix; Step S3.7: The state space of the offline controller is determined, and the main characteristic value of the offline controller closed loop is determined according to the state space of the offline controller. The main eigenvalue of the offline controller closed loop is determined as the output of the offline controller With minimal error tracking control to track the ship cooperative formation controller The percentage overshoot and the rise time in the linear control criterion can be used to formulate the desired eigenvalue of the offline controller; The The determination principle is: to converge the error between the closed-loop pole location and the desired pole location of the offline controller to... A value, specifically: （26） In equation (26), and are the desired pole values and the off-line controller pole values, respectively; denotes a positive constant; The penalty value in equation (25) may be expressed as: （27） wherein represents a system penalty; is a constant; is a constant; if (x , ) is controllable, then = 0, otherwise = 1; if (x , ) is observable, then = 0, otherwise = 1.

Images