Multi-unmanned ship cooperative target tracking method based on model predictive control

Abstract

The invention discloses a multi-unmanned-ship cooperative target tracking method based on model predictive control, and the method comprises the steps: building a multi-unmanned-ship motion model, and defining the state variables and control input of unmanned ships, and the kinematics equation and kinetic equation of the unmanned ships; the unmanned ship comprises a pilot ship and a following ship; a pilot-follower formation structure is determined, and expected trajectories of a pilot ship and a following ship are solved; the expected angular velocity of the pilot ship is obtained through offset proportion navigation guidance; and multi-unmanned ship cooperative target tracking is carried out. According to the method, the expected angular velocity of a pilot ship is derived by using an offset proportion navigation guidance law, the expected angular velocity of the pilot ship is applied to the expected state of model prediction target tracking control, and then the expected state of each unmanned ship is tracked by using the model prediction target tracking control. The multiple unmanned ships can accurately position the target ship at the final moment, the calculation complexity in the tracking control process can be reduced, and the real-time performance of cooperative target tracking of the multiple unmanned ships is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned ship target tracking, and in particular to a multi-unmanned ship collaborative target tracking method based on model predictive control. Background Art

[0002] As the application fields of fully autonomous unmanned ships continue to expand, the operations of multiple unmanned ships are becoming more and more diverse. Tasks such as anti-ship, cluster warfare and maritime police water patrols are widely used. When multiple unmanned ships perform these tasks, they must track the target ship within a limited time. This gave birth to the target tracking method, which is one of the important supporting technologies for the autonomous operation of unmanned ships.

[0003] Most of the existing target tracking methods focus on single unmanned ships and track single target ships. However, when multiple unmanned ships are tracking targets, they need to maintain formation, avoid collisions with obstacles, and avoid collisions between ships. As a result, the existing methods cannot meet the task requirements. Therefore, a general collaborative control method needs to be developed for the control system of multiple unmanned ships. Summary of the invention

[0004] The purpose of the present invention is to overcome the deficiencies of the prior art and provide a multi-unmanned ship collaborative target tracking method based on model predictive control.

[0005] To achieve the above purpose, the technical solution provided by the present invention is:

[0006] A multi-unmanned ship cooperative target tracking method based on model predictive control, comprising:

[0007] Establish a multi-unmanned ship motion model, including defining the state variables, control inputs, kinematic equations and dynamic equations of the unmanned ships; the unmanned ships include pilot ships and follower ships;

[0008] Determine the leader-follower formation structure and find the desired trajectories of the leader and follower ships;

[0009] The desired angular velocity of the pilot ship is obtained through bias proportional navigation guidance;

[0010] Multi-unmanned ship collaborative target tracking is performed based on the state variables of the unmanned ships, control inputs, kinematic equations and dynamic equations of the unmanned ships, the expected trajectories of the lead ship and the following ship, and the expected angular velocity of the lead ship.

[0011] Furthermore, the process of establishing the multi-unmanned ship motion model includes:

[0012] Define that there are N unmanned ships participating in collaborative target tracking;

[0013] State variable definition: The state variable of the i-th unmanned ship is the position (xi ,y i ), bow angle Longitudinal speed u i , lateral velocity v i , bow angular velocity r i ;

[0014] Definition of control input: The control input of the i-th unmanned ship includes the longitudinal thrust τ ui , bow thrust τ ri ;

[0015] Define the kinematic equation of the i-th unmanned ship:

[0016]

[0017] is the speed of the i-th unmanned ship in the x direction; is the speed of the i-th unmanned ship in the y direction; is the bow angular velocity of the i-th unmanned ship;

[0018] Define the dynamic equation of the i-th unmanned ship, the inertia matrix M i =diag(m 1i ,m 2i ,m 3i ), m 1i 、m 2i and m 3i They represent the mass distribution of the i-th unmanned ship in the x-axis direction, the mass distribution of the i-th unmanned ship in the y-axis direction, and the moment of inertia of the i-th unmanned ship, and their values ​​are M i =diag(25.8,33.8,2.76), hydrodynamic damping coefficient matrix D i =diag(d 1i ,d 2i ,d 3i ), d 1i ,d 2i and d 3i They represent the hydrodynamic resistance coefficient of the i-th unmanned ship along the x-axis, the hydrodynamic resistance coefficient of the i-th unmanned ship along the y-axis, and the rotational resistance coefficient of the i-th unmanned ship, and their values ​​are D i =diag(12,17,0.5), the dynamic equation of the i-th unmanned ship is defined as follows:

[0019]

[0020] is the longitudinal acceleration of the i-th unmanned ship; is the lateral acceleration of the i-th unmanned ship; is the bow angular acceleration of the i-th unmanned ship.

[0021] Furthermore, the leader-follower formation structure is determined, including:

[0022] The expected trajectories of the pilot ship and the i-th following ship are defined as follows:

[0023]

[0024] The expected trajectory of the i-th following ship By placing the desired trajectory of the pilot boat Displacement d i ,θ i Angular deviation and heading angle σ i Rotate to get, The expected trajectory formula of the i-th following ship is as follows:

[0025]

[0026] represents the expected trajectory of the pilot ship The operator to make the adjustment.

[0027] Furthermore, the desired angular velocity is obtained through bias proportional navigation guidance, including:

[0028] Get the state information of the pilot ship and the target ship at time step t. The state variable of the pilot ship is x l ,y l , u l 、v l 、r l , the state variable of the target ship is x t ,y t , u t 、v t 、r t ;

[0029] According to the obtained status information, the relative position P of the pilot ship and the target ship is calculated:

[0030] P=P t -P l

[0031] P t =(x t ,y t ) is the position of the target ship, P l =(x l ,y l ) is the position of the pilot boat;

[0032] According to the obtained status information, the sight angle Ψ is calculated as follows:

[0033]

[0034] Calculate relative heading angle The formula is as follows:

[0035]

[0036] in, and are the velocity components of the target ship and the pilot ship in the inertial coordinate system, respectively, which are calculated from their respective longitudinal velocities u;

[0037] Calculate the relative velocity V r , the formula is as follows:

[0038]

[0039] Calculate the relative viewing angle Θ, the formula is as follows:

[0040]

[0041] Determine the proportional navigation coefficient K, which is 10; finally calculate the expected angular velocity r of the pilot ship at time step t d And return r d Value, r d The calculation formula is as follows:

[0042]

[0043] Furthermore, based on the state variables of the unmanned ships, the control input, the kinematic equations and dynamic equations of the unmanned ships, the expected trajectories of the pilot ship and the follower ship, and the expected angular velocity of the pilot ship, multiple unmanned ships are coordinated to track the target, including:

[0044] The expected state variable X of the i-th unmanned ship is constructed by combining the state variables of the unmanned ship, the expected trajectories of the pilot ship and the following ship, and the expected angular velocity of the pilot ship. di , and calculate the state variable deviation at time step t X i (t) is the actual state variable of the i-th unmanned ship at time step t;

[0045] According to the control input of the unmanned ship, the control input of the i-th unmanned ship at time step t is defined as U i (t) = [τ ui 0 τ ri ] T , τ ui is the longitudinal thrust of the i-th unmanned ship, τ ri is the bow thrust of the i-th unmanned ship;

[0046] Using the Euler method for the nonlinear motion model of the i-th unmanned ship at time step t, the state update equation is obtained:

[0047] X i (t+1)=X i (t)+f(X i (t))Δt

[0048] Where Δt is the duration of each time step; f(X i (t)) is a motion model combining the kinematic equation and dynamic equation of the unmanned ship;

[0049] Create a model prediction target tracking control objective function and set constraints for the model prediction target tracking control objective function;

[0050] Solve the model prediction target tracking control objective function in combination with the constraint conditions to obtain the optimal control input result;

[0051] The optimal control input result is applied to the unmanned ship and its state is updated, thereby performing collaborative target tracking of multiple unmanned ships.

[0052] Furthermore, the created model predicts the target tracking control objective function as follows:

[0053]

[0054] in, is the state variable deviation of the unmanned ship at time step t; U(t) is the control input of the unmanned ship at time step t; Ts is the prediction time domain; M, W, F are all set weight matrices; ξ dist (t) is the position tracking error penalty term of the unmanned ship at time step t; ξ pc (t) is the preset performance constraint penalty term; ξ oa (t) is the obstacle avoidance penalty term, which includes the obstacle avoidance constraint L ik (t);

[0055] The constraints are as follows:

[0056] Control increment constraint [-10 0 -5] T ≤ΔU(t)≤[10 0 5] T ,

[0057] Control variable constraints [0 0 -40] T ≤U(t)≤[100 0 40] T ,

[0058] Obstacle avoidance constraint L ik (t)≥5.

[0059] Furthermore, the position tracking error penalty term ξ of the unmanned ship at time step t is dist The calculation formula of (t) is as follows:

[0060]

[0061] Among them, F zi (t) is the position tracking error at time step t, and its calculation process includes:

[0062] First calculate the unmanned ship at time step t x i ,y i and The tracking deviation F xi (t), F yi (t) and Then calculate according to the Euclidean distance formula; x i and i is the coordinate of the i-th unmanned ship, is the bow angle of the i-th unmanned ship.

[0063] Furthermore, the performance constraint penalty term ξ is preset pc The calculation formula of (t) is as follows:

[0064]

[0065] F ji (t) is the preset performance constraint, j=x,y,φ;

[0066] in, is the maximum tracking deviation at time step t;

[0067] is the minimum tracking deviation at time step t;

[0068] is the maximum tracking deviation on the x-axis at time step t;

[0069] is the minimum tracking deviation on the x-axis at time step t;

[0070] is the maximum tracking deviation on the y-axis at time step t;

[0071] is the minimum tracking deviation on the y-axis at time step t.

[0072] Furthermore, the calculation formula of the obstacle avoidance penalty term is as follows:

[0073]

[0074] ∈ is a positive number used to prevent numerical instability when the denominator approaches zero;

[0075] Obstacle avoidance constraint L ik (t) is the distance from the i-th unmanned ship to the k-th unmanned ship with a radius of r ok The Euclidean distance of the obstacle is calculated as follows:

[0076]

[0077] Among them, x k (t) and y k (t) represents the position of the kth obstacle on the x-axis and y-axis at time step t; n o is the total number of obstacles.

[0078] Furthermore, when performing collaborative target tracking of multiple unmanned ships, it also includes determining that the tracking is successful when the distance between the pilot ship and the target ship is less than a preset capture radius, and stopping control and collaborative operation.

[0079] Compared with the prior art, the principles and advantages of this technical solution are as follows:

[0080] 1. Use the biased proportional navigation guidance law to derive the expected angular velocity of the pilot ship, and apply the expected angular velocity of the pilot ship to the expected state of the model-predicted target tracking control. Then use the model-predicted target tracking control to track the expected state of each unmanned ship. In this way, multiple unmanned ships can accurately locate the target ship at the last moment, and can reduce the computational complexity of the tracking control process, thereby improving the real-time performance of collaborative target tracking of multiple unmanned ships.

[0081] 2. Add position tracking error penalty, preset performance constraint penalty and obstacle avoidance penalty to the created model prediction target tracking control objective function, so that each unmanned ship can safely bypass obstacles, ensure the stability of the multi-unmanned ship formation, and minimize the risk of collision. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0083] Figure 1 The present invention is a principle flow chart of a method for cooperative target tracking of multiple unmanned ships based on model predictive control;

[0084] Figure 2This is a principle flow chart of a multi-unmanned ship collaborative target tracking method based on model predictive control in the present invention, which performs multi-unmanned ship collaborative target tracking based on the state variables of the unmanned ships, control inputs, kinematic equations and dynamic equations of the unmanned ships, the expected trajectories of the pilot ship and the follower ship, and the expected angular velocity of the pilot ship. DETAILED DESCRIPTION

[0085] The present invention will be further described below in conjunction with specific embodiments:

[0086] like Figure 1 As shown, the multi-unmanned ship collaborative target tracking method based on model predictive control described in this embodiment includes the following steps:

[0087] S1. Establish a multi-unmanned ship motion model, including defining the state variables, control inputs, kinematic equations and dynamic equations of the unmanned ships; the unmanned ships include pilot ships and follower ships;

[0088] Specifically, this step includes:

[0089] Define that there are N unmanned ships participating in collaborative target tracking;

[0090] State variable definition: The state variable of the i-th unmanned ship is the position (x i ,y i ), bow angle Longitudinal speed u i , lateral velocity v i , bow angular velocity r i ;

[0091] Definition of control input: The control input of the i-th unmanned ship includes the longitudinal thrust τ ui , bow thrust τ ri ;

[0092] Define the kinematic equation of the i-th unmanned ship:

[0093]

[0094]

[0095] is the speed of the i-th unmanned ship in the x direction; is the speed of the i-th unmanned ship in the y direction; is the bow angular velocity of the i-th unmanned ship;

[0096] Define the dynamic equation of the i-th unmanned ship, the inertia matrix M i =diag(m 1i ,m 2i ,m 3i ), m 1i、m 2i and m 3i They represent the mass distribution of the i-th unmanned ship in the x-axis direction, the mass distribution of the i-th unmanned ship in the y-axis direction, and the moment of inertia of the i-th unmanned ship, and their values ​​are M i =diag(25.8,33.8,2.76), hydrodynamic damping coefficient matrix D i =diag(d 1i ,d 2i ,d 3i ), d 1i d 2i and d 3i They represent the hydrodynamic resistance coefficient of the i-th unmanned ship along the x-axis, the hydrodynamic resistance coefficient of the i-th unmanned ship along the y-axis, and the rotational resistance coefficient of the i-th unmanned ship, and their values ​​are D i =diag(12,17,0.5), the dynamic equation of the i-th unmanned ship is defined as follows:

[0097]

[0098] is the longitudinal acceleration of the i-th unmanned ship; is the lateral acceleration of the i-th unmanned ship; is the bow angular acceleration of the i-th unmanned ship;

[0099] Combining the kinematic equation and the dynamic equation, the motion model of multiple unmanned ships is obtained as follows:

[0100]

[0101] S2, determine the leader-follower formation structure and obtain the expected trajectories of the leader ship and the follower ship;

[0102] Specifically, this step includes:

[0103] The expected trajectories of the pilot ship and the i-th following ship are defined as follows:

[0104]

[0105] The expected trajectory of the i-th following ship By placing the desired trajectory of the pilot boat Displacement d i ,θ i Angular deviation and heading angle σ i Rotate to get, The expected trajectory formula of the i-th following ship is as follows:

[0106]

[0107] represents the expected trajectory of the pilot ship The operator to make the adjustment.

[0108] S3, obtaining the desired angular velocity of the pilot ship through bias proportional navigation guidance;

[0109] Specifically, this step includes:

[0110] Get the state information of the pilot ship and the target ship at time step t. The state variable of the pilot ship is x l ,y l , u l 、v l 、r l , the state variable of the target ship is x t ,y t , u t 、v t 、r t ;

[0111] According to the obtained status information, the relative position P of the pilot ship and the target ship is calculated:

[0112] P=P t -P l

[0113] P t =(x t ,y t ) is the position of the target ship, P l =(x l ,y l ) is the position of the pilot boat;

[0114] According to the obtained status information, the sight angle Ψ is calculated as follows:

[0115]

[0116] Calculate relative heading angle The formula is as follows:

[0117]

[0118] in, and are the velocity components of the target ship and the pilot ship in the inertial coordinate system, respectively, which are calculated from their respective longitudinal velocities u;

[0119] Calculate the relative velocity V r , the formula is as follows:

[0120]

[0121] Calculate the relative viewing angle Θ, the formula is as follows:

[0122]

[0123] Determine the proportional navigation coefficient K, which is 10; finally calculate the expected angular velocity r of the pilot ship at time step t d And return r d Value, r d The calculation formula is as follows:

[0124]

[0125] S4. Perform multi-unmanned ship collaborative target tracking based on the state variables of the unmanned ships, control inputs, kinematic equations and dynamic equations of the unmanned ships, the expected trajectories of the lead ship and the follower ship, and the expected angular velocity of the lead ship.

[0126] Specifically, Figure 2 As shown, this step includes:

[0127] S4-1. Construct the expected state variable X of the i-th unmanned ship by combining the state variables of the unmanned ship, the expected trajectories of the pilot ship and the following ship, and the expected angular velocity of the pilot ship. di , and calculate the state variable deviation at time step t is the actual state variable of the i-th unmanned ship at time step t;

[0128] S4-2. According to the control input of the unmanned ship, define the control input of the i-th unmanned ship at time step t as U i (t) = [τ ui 0 τ ri ] T , τ ui is the longitudinal thrust of the i-th unmanned ship, τ ri is the bow thrust of the i-th unmanned ship;

[0129] S4-3, according to the motion model of multiple unmanned ships f(X i ) It can be seen that the multi-unmanned ship system has significant nonlinear characteristics. In order to simplify the nonlinear multi-unmanned ship system, the Euler method is used for the nonlinear motion model of the i-th unmanned ship at time step t to obtain the state update equation:

[0130] X i (t+1)=X i (t)+f(X i (t))Δt

[0131] Where Δt is the duration of each time step; f(X i(t)) is a motion model combining the kinematic equation and dynamic equation of the unmanned ship;

[0132] S4-4, creating a model prediction target tracking control objective function, and setting constraints of the model prediction target tracking control objective function;

[0133] The created model predicts the target tracking control objective function as follows:

[0134]

[0135] in, is the state variable deviation of the unmanned ship at time step t; U(t) is the control input of the unmanned ship at time step t; Ts is the prediction time domain; M, W, F are all set weight matrices; ξ dist (t) is the position tracking error penalty term of the unmanned ship at time step t; ξ pc (t) is the preset performance constraint penalty term; ξ oa (t) is the obstacle avoidance penalty term, which includes the obstacle avoidance constraint L ik (t);

[0136] The constraints are as follows:

[0137] Control increment constraint [-10 0 -5] T ≤ΔU(t)≤[10 0 5] T ,

[0138] Control variable constraints [0 0 -40] T ≤U(t)≤[100 0 40] T ,

[0139] Obstacle avoidance constraint L ik (t)≥5.

[0140] in,

[0141] 1) The position tracking error penalty term is used to avoid collision between the pilot ship and the following ship;

[0142] The position tracking error penalty term ξ of the unmanned ship at time step t dist The calculation formula of (t) is as follows:

[0143]

[0144] Among them, F zi (t) is the position tracking error at time step t, and its calculation process includes:

[0145] First calculate the unmanned ship at time step t x i ,y i and The tracking deviation Fxi (t), F yi (t) and Then calculate according to the Euclidean distance formula; x i and i is the coordinate of the i-th unmanned ship, is the bow angle of the i-th unmanned ship.

[0146] 2) The preset performance constraint penalty term is used to ensure that the system error at each time step converges within the preset dynamic range and improves formation stability;

[0147] Preset performance constraint penalty term ξ pc The calculation formula of (t) is as follows:

[0148]

[0149] F ji (t) is the preset performance constraint,

[0150] in, is the maximum tracking deviation at time step t;

[0151] is the minimum tracking deviation at time step t;

[0152] is the maximum tracking deviation on the x-axis at time step t;

[0153] is the minimum tracking deviation on the x-axis at time step t;

[0154] is the maximum tracking deviation on the y-axis at time step t;

[0155] is the minimum tracking deviation on the y-axis at time step t.

[0156] 3) The obstacle avoidance penalty term is used to achieve real-time obstacle avoidance control of the multi-unmanned ship system and avoid the local minimum problem;

[0157] The calculation formula of the obstacle avoidance penalty term is as follows:

[0158]

[0159] ∈ is a positive number used to prevent numerical instability when the denominator approaches zero;

[0160] Obstacle avoidance constraint L ik (t) is the distance from the i-th unmanned ship to the k-th unmanned ship with a radius of r ok The Euclidean distance of the obstacle is calculated as follows:

[0161]

[0162] Among them, x k (t) and y k (t) represents the position of the kth obstacle on the x-axis and y-axis at time step t; n o is the total number of obstacles.

[0163] S4-5. Use the sequential least squares programming (SLSQP) in the scipy.optimize library of Python to solve the model prediction target tracking control objective function in combination with the constraints to obtain the optimal control input result;

[0164] S4-6. Apply the optimal control input results (longitudinal thrust, bow thrust) to the unmanned ship and update its status, thereby performing collaborative target tracking of multiple unmanned ships.

[0165] S4-7. When the distance between the pilot ship and the target ship is less than the preset capture radius, it is determined that the tracking is successful, and the control and coordinated operation are stopped.

[0166] The embodiments described above are only preferred embodiments of the present invention and are not intended to limit the scope of implementation of the present invention. Therefore, all changes made according to the shape and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A multi-unmanned ship collaborative target tracking method based on model predictive control, characterized in that: include: Establish a multi-unmanned ship motion model, including defining the state variables, control inputs, kinematic equations and dynamic equations of the unmanned ships; the unmanned ships include pilot ships and follower ships; Determine the leader-follower formation structure and find the desired trajectories of the leader and follower ships; The desired angular velocity of the pilot ship is obtained through bias proportional navigation guidance; Multi-unmanned ship collaborative target tracking is performed based on the state variables of the unmanned ships, control inputs, kinematic equations and dynamic equations of the unmanned ships, the expected trajectories of the lead ship and the following ship, and the expected angular velocity of the lead ship.

2. The method for collaborative target tracking of multiple unmanned ships based on model predictive control according to claim 1 is characterized in that: The process of establishing a multi-unmanned ship motion model includes: Define that there are N unmanned ships participating in collaborative target tracking; State variable definition: The state variable of the i-th unmanned ship is the position (x i ,y i ), bow angle Longitudinal speed u i , lateral velocity v i , bow angular velocity r i ; Definition of control input: The control input of the i-th unmanned ship includes the longitudinal thrust τ ui , bow thrust τ ri ; Define the kinematic equation of the i-th unmanned ship: is the speed of the i-th unmanned ship in the x direction; is the speed of the i-th unmanned ship in the y direction; is the bow angular velocity of the i-th unmanned ship; Define the dynamic equation of the i-th unmanned ship, the inertia matrix M i =diag(m 1i ,m 2i ,m 3i ), m 1i 、m 2i and m 3i They represent the mass distribution of the i-th unmanned ship in the x-axis direction, the mass distribution of the i-th unmanned ship in the y-axis direction, and the moment of inertia of the i-th unmanned ship, and their values ​​are M i =diag(25.8,33.8,2.76), hydrodynamic damping coefficient matrix D i =diag(d 1i ,d 2i ,d 3i ), d 1i ,d 2i and d 3i They represent the hydrodynamic resistance coefficient of the i-th unmanned ship along the x-axis, the hydrodynamic resistance coefficient of the i-th unmanned ship along the y-axis, and the rotational resistance coefficient of the i-th unmanned ship, and their values ​​are D i =diag(12,17,0.5), the dynamic equation of the i-th unmanned ship is defined as follows: is the longitudinal acceleration of the i-th unmanned ship; is the lateral acceleration of the i-th unmanned ship; is the bow angular acceleration of the i-th unmanned ship.

3. The method for collaborative target tracking of multiple unmanned ships based on model predictive control according to claim 1 is characterized in that: Determine the leader-follower formation structure, including: The expected trajectories of the pilot ship and the i-th following ship are defined as follows: The expected trajectory of the i-th following ship By placing the desired trajectory of the pilot boat Displacement d i ,θ i Angular deviation and heading angle σ i Rotate to get, The expected trajectory formula of the i-th following ship is as follows: represents the expected trajectory of the pilot ship The operator to make the adjustment.

4. The method for collaborative target tracking of multiple unmanned ships based on model predictive control according to claim 1 is characterized in that: The desired angular velocity is obtained through bias proportional navigation guidance, including: Get the state information of the pilot ship and the target ship at time step t. The state variable of the pilot ship is x l ,y l , 、u l 、v l 、r l , the state variable of the target ship is x t ,y t , u t 、v t 、r t ; According to the obtained status information, the relative position P of the pilot ship and the target ship is calculated: P=P t -P l P t =(x t ,y t ) is the position of the target ship, P l =(x l ,y l ) is the position of the pilot boat; According to the obtained status information, the sight angle Ψ is calculated as follows: Calculate relative heading angle The formula is as follows: in, and are the velocity components of the target ship and the pilot ship in the inertial coordinate system, respectively, which are calculated from their respective longitudinal velocities u; Calculate the relative velocity V r , the formula is as follows: Calculate the relative viewing angle Θ, the formula is as follows: Determine the proportional navigation coefficient K, which is 10; finally calculate the expected angular velocity r of the pilot ship at time step t d And return r d Value, r d The calculation formula is as follows:

5. The method for cooperative target tracking of multiple unmanned ships based on model predictive control according to claim 1 is characterized in that: Based on the state variables of the unmanned ships, control inputs, kinematic equations and dynamic equations of the unmanned ships, the expected trajectories of the pilot ship and the follower ship, and the expected angular velocity of the pilot ship, multiple unmanned ships are coordinated to track targets, including: The expected state variable X of the i-th unmanned ship is constructed by combining the state variables of the unmanned ship, the expected trajectories of the pilot ship and the following ship, and the expected angular velocity of the pilot ship. di , and calculate the state variable deviation at time step t X i (y) is the actual state variable of the i-th unmanned ship at time step t; According to the control input of the unmanned ship, the control input of the i-th unmanned ship at time step t is defined as U i (t) = [τ ui 0τ ri ] T , τ ui is the longitudinal thrust of the i-th unmanned ship, τ ri is the bow thrust of the i-th unmanned ship; Using the Euler method for the nonlinear motion model of the i-th unmanned ship at time step t, the state update equation is obtained: X i (t+1)=X i (t)+f(X i (t))Δt Where Δt is the duration of each time step; f(X i (t)) is a motion model combining the kinematic equation and dynamic equation of the unmanned ship; Create a model prediction target tracking control objective function and set constraints for the model prediction target tracking control objective function; Solve the model prediction target tracking control objective function in combination with the constraint conditions to obtain the optimal control input result; The optimal control input result is applied to the unmanned ship and its state is updated, thereby performing collaborative target tracking of multiple unmanned ships.

6. The method for collaborative target tracking of multiple unmanned ships based on model predictive control according to claim 5 is characterized in that: The created model predicts the target tracking control objective function as follows: in, is the state variable deviation of the unmanned ship at time step t; U(t) is the control input of the unmanned ship at time step t; Ts is the prediction time domain; M, W, F are all set weight matrices; ξ dist (t) is the position tracking error penalty term of the unmanned ship at time step t; ξ pc (t) is the preset performance constraint penalty term; ξ oa (t) is the obstacle avoidance penalty term, which includes the obstacle avoidance constraint L ik (t); The constraints are as follows: Control increment constraint [-10 0 -5] T ≤ΔU(t)≤[10 0 5] T , Control variable constraints [0 0 -40] T ≤U(t)≤p100 0 40] T , Obstacle avoidance constraint L ik (t)≥5.

7. The method for cooperative target tracking of multiple unmanned ships based on model predictive control according to claim 5 is characterized in that: The position tracking error penalty term ξ of the unmanned ship at time step t dist The calculation formula of (t) is as follows: Among them, F zi (t) is the position tracking error at time step t, and its calculation process includes: First calculate the unmanned ship at time step t x i ,y i and The tracking deviation F xi (t), F yi (t) and Then calculate according to the Euclidean distance formula; x i and i is the coordinate of the i-th unmanned ship, is the bow angle of the i-th unmanned ship.

8. The method for cooperative target tracking of multiple unmanned ships based on model predictive control according to claim 5 is characterized in that: Preset performance constraint penalty term ξ pc The calculation formula of (t) is as follows: F ji (t) is the preset performance constraint, in, is the maximum tracking deviation at time step t; F ji (t) is the minimum tracking deviation at time step t; is the maximum tracking deviation on the x-axis at time step t; F x (t)= F xi (t) = (1-0.1) exp (-0.05t) + 0.1, which is the minimum tracking deviation on the x-axis at time step t; is the maximum tracking deviation on the y-axis at time step t; F y (t)= F yi (t) = (1.5-0.1)exp(-0.04t) + 0.1, which is the minimum tracking deviation on the y-axis at time step t.

9. The method for cooperative target tracking of multiple unmanned ships based on model predictive control according to claim 5 is characterized in that: The calculation formula of the obstacle avoidance penalty term is as follows: ∈ is a positive number used to prevent numerical instability when the denominator approaches zero; Obstacle avoidance constraint L ik (t) is the distance from the i-th unmanned ship to the k-th unmanned ship with a radius of r ok The Euclidean distance of the obstacle is calculated as follows: Among them, x k (t) and y k (t) represents the position of the kth obstacle on the x-axis and y-axis at time step t; n o is the total number of obstacles.

10. The method for cooperative target tracking of multiple unmanned ships based on model predictive control according to claim 5, characterized in that: When performing collaborative target tracking of multiple unmanned ships, it also includes determining that the tracking is successful when the distance between the pilot ship and the target ship is less than a preset capture radius, and stopping control and collaborative operations.

Images