Unmanned ship cluster cooperation pre-defined time angle-distance guidance algorithm

Abstract

The application discloses a kind of unmanned ship cluster cooperation predefinition time package angle-distance guidance algorithm, and the present application relates to the field of underactuated multi-unmanned ship formation control, comprising the following steps: step (1): establish unmanned ship cluster cooperation movement three degrees of freedom mathematical model;Step (2): define the package angle and distance between the first follower and the pilot;Step (3): the desired position of the first unmanned ship and tracking error are constructed;Step (4): the error tracking dynamic of the actual position of the first unmanned ship relative to the reference position is calculated;Step (5): the predefinition time reference heading of the first unmanned ship is designed;Step (6): the predefinition time reference speed of the first unmanned ship is designed;Step (7): position error predefinition time convergence proof;Step (8): numerical simulation verification.

Description

Technical Field

[0001] This invention relates to the field of multi-unmanned surface vessel (USV) cooperative guidance and control technology, and in particular to a predefined time envelope-range guidance algorithm for USV swarm cooperative guidance. Background Technology

[0002] With the increasing demand for marine development and defense, underactuated unmanned surface vessels (USVs) have attracted widespread attention as efficient platforms for performing tasks such as marine environmental monitoring, resource exploration, and search and rescue patrols. To compensate for the shortcomings of single-vessel operations in terms of perception range, mission carrying capacity, and system redundancy, formation systems composed of multiple USVs, through collaborative operations, can demonstrate superior mission execution efficiency and higher system reliability, and have become a research hotspot in the field of unmanned surface systems.

[0003] In multi-unmanned surface vessel (USV) formation operations, formation guidance algorithms are crucial for achieving formation, maintenance, and transformation. Existing formation guidance strategies mainly include the pilot-follower method, the virtual structure method, and behavior-based methods. While these traditional guidance methods can achieve basic formation motion, they often have limitations in terms of convergence time, a key performance indicator. Specifically, asymptotically stable guidance algorithms can only guarantee that the system converges to the desired state as time approaches infinity, failing to meet the needs of mission scenarios with strict timeliness requirements. Finite-time guidance algorithms can achieve convergence within a finite time, but their upper bound on convergence time heavily depends on the initial positions and attitudes of the formation members. When the initial error is large, the convergence time will be significantly prolonged, lacking consistent time predictability. Fixed-time guidance algorithms overcome the dependence on the initial state, providing an upper bound on convergence time independent of initial conditions. However, this upper bound is usually determined by complex controller parameters, making it difficult to intuitively and accurately preset a minimum convergence time that meets mission requirements.

[0004] In view of the above problems, in order to achieve precise and intuitive control of formation convergence time while ensuring the flexibility and robustness of the distributed guidance structure, this invention proposes a predefined time envelope-range guidance algorithm for unmanned surface vessel (USV) swarms. This method directly integrates a clear and arbitrarily specifyable convergence time as a control parameter into the guidance law design, enabling the swarm system to stably complete formation generation and adjustment within a pre-set time limit. This effectively improves the time response accuracy and collaborative operation efficiency of multi-USV swarms in complex mission environments. Summary of the Invention

[0005] The present invention relates to a predefined time envelope-range guidance algorithm for unmanned surface vessel swarm collaboration.

[0006] The objective of this invention is achieved as follows:

[0007] A predefined time envelope-range guidance algorithm for unmanned surface vessel swarm collaboration includes the following steps:

[0008] Step (1): Establish a three-degree-of-freedom mathematical model for the cooperative motion of unmanned surface vessel swarms:

[0009] This study investigates the motion of unmanned surface vessels (USVs) on the horizontal plane, examining their pitch, sway, and bow motion in both a fixed coordinate system and a ship's coordinate system.

[0010] The mathematical model for the horizontal motion of a rocker with three degrees of freedom includes: a three-degree-of-freedom kinematic model and a three-degree-of-freedom dynamic model.

[0011] Three-degree-of-freedom horizontal plane kinematic model of multiple unmanned surface vessels in a fixed coordinate system:

[0012] ,

[0013] Three-degree-of-freedom horizontal plane dynamics model of multiple unmanned surface vessels in hull coordinate system:

[0014] ,

[0015] in: , , Representing the first The northward and eastward positions and bow angle of an unmanned surface vessel in a fixed coordinate system; , , Representing the first The pitch speed, sway speed, and bow roll rate of the unmanned surface vessel; , , , and These represent the mass and moment of inertia of the unmanned surface vessel, respectively. , These represent sway control and yaw control, respectively.

[0016] Step (2): Define the first The angle between the follower and the navigator and distance :

[0017] Corner Definition: From the first The angle between the line of sight of the follower pointing towards the navigator and the direction of the navigator's bow;

[0018] distance Definition: The first The straight-line Euclidean distance between the center of gravity of the follower and the center of gravity of the navigator.

[0019] Step (3): Construct the first The expected position and tracking error of the unmanned surface vessel:

[0020] Based on the leader-follower structure, the first... The expected position of the follower , Define the tracking error along the path of the follower. and cross-tracking error Define the heading angle error of the follower. .

[0021] definition Indicates the first The desired position of a follower is specifically represented as follows:

[0022] ,

[0023] In the above formula, and Navigator unmanned surface vessel (USV) The actual position of (=0), Indicates the navigator and the first The formation distance between the follower ships This indicates the actual bow angle of the Navigator unmanned surface vessel. Indicates the navigator and the first The formation angle between the follower ships.

[0024] Step (4): Calculate the first... Dynamic tracking of the error between the actual position of an unmanned surface vessel and its reference position:

[0025] ,

[0026] in: For the first The ship expects to follow the combined velocity of its sway and roll speeds. For the first The bow roll rate of the follower ship, and They are the first The expected sway speed and lateral sway speed of the ship's followers It is the first The sideslip angle of the ship that expects to follow.

[0027] Step (5): Design the first Predefined time reference heading for the unmanned surface vessel:

[0028] Regarding the first The yaw and yaw systems of an unmanned surface vessel (USV) are used to construct the desired heading angle. :

[0029] ,

[0030] in: It is the first The expected heading angle of an unmanned surface vessel. It is the first The heading angle of the unmanned surface vessel, It is the actual heading angle of the Navigator unmanned surface vessel. , , It is a constant. For the first The positional error of the unmanned surface vessel It is a symbolic function.

[0031] Step (6): Design the first Predefined time reference speed for an unmanned surface vessel:

[0032] Based on the mathematical model established in step (1), for the first The tumble motion of an unmanned surface vessel is designed with a predefined time-defined tumble guidance velocity. Construct a predefined time-based sway guidance law for multi-unmanned surface vessel formations;

[0033] ,

[0034] in: , , It is a constant. For the first The positional error of the unmanned surface vessel It is a symbolic function.

[0035] Step (7): Proof of convergence of position error in predefined time:

[0036] According to the predefined time stability lemma, for Lyapunov functions... The conclusion obtained by differentiation is consistent with the predefined time stability lemma, thus the proof of predefined time stability is complete.

[0037] Step (8): Numerical simulation verification:

[0038] The unmanned surface vessel (USV) swarm collaborative predefined time envelope angle-distance guidance algorithm designed in steps (1) to (7) was simulated on a certain type of USV swarm.

[0039] The present invention has the following beneficial effects:

[0040] 1. The unmanned surface vessel (USV) swarm collaborative predefined time envelope-distance guidance algorithm described in this invention solves the problem that the convergence time in traditional formation guidance methods depends on the initial state of the system or complex controller parameters. By directly incorporating preset time parameters into the guidance law design, the formation system can complete formation and reconstruction within any user-specified time limit, significantly improving the time predictability and planning flexibility of formation mission execution.

[0041] 2. The unmanned surface vessel (USV) swarm collaborative predefined time envelope-range guidance algorithm described in this invention designs a desired heading angle guidance law and a desired longitudinal velocity guidance command based on predefined time theory. Compared with existing asymptotically stable or fixed-time stable guidance strategies, this algorithm can strictly guarantee that the formation tracking error converges precisely to zero within a preset stable time window, thereby effectively suppressing transient overshoot in the early stage of formation formation and improving the path tracking smoothness and overall formation stability of the formation system under complex sea conditions. Attached Figure Description

[0042] Figure 1 This is a flowchart of the steps described in this invention;

[0043] Figure 2 This is a structural diagram of the predefined time unmanned surface vessel formation control system of the present invention;

[0044] Figure 3 A multi-unmanned surface vessel (USV) formation tracking curve diagram for a certain type of USV under the present invention;

[0045] Figure 4 The diagram shows the dynamic variable curve of the position error of a certain type of unmanned surface vessel under the present invention.

[0046] Figure 5 This is a dynamic variable curve of the speed error of a certain type of unmanned surface vessel under the present invention. Detailed Implementation

[0047] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described below with reference to the accompanying drawings:

[0048] Example 1

[0049] like Figure 1 A predefined time envelope-range guidance algorithm for unmanned surface vessel swarm collaboration includes the following steps:

[0050] Step (1): Establish a three-degree-of-freedom mathematical model for the cooperative motion of unmanned surface vessel swarms:

[0051] To study the motion of unmanned surface vessels on the horizontal plane, mathematical models of their horizontal motion with three degrees of freedom—swell, roll, and yaw—are established in a fixed coordinate system and a hull coordinate system. These models include a three-degree-of-freedom kinematic model and a three-degree-of-freedom dynamic model.

[0052] Three-degree-of-freedom horizontal plane kinematic model of multiple unmanned surface vessels in a fixed coordinate system:

[0053] ,

[0054] Three-degree-of-freedom horizontal plane dynamics model of multiple unmanned surface vessels in hull coordinate system:

[0055] ,

[0056] in: , , Representing the first The northward and eastward positions and bow angle of an unmanned surface vessel in a fixed coordinate system; , , Representing the first The pitch speed, sway speed, and bow roll rate of the unmanned surface vessel; , , , and These represent the mass and moment of inertia of the unmanned surface vessel, respectively. , Representing vertical

[0057] Sway control force and bow roll control force.

[0058] Step (2): Define the first The angle between the follower and the navigator and distance :

[0059] Corner Definition: From the first The angle between the line of sight of the follower pointing towards the navigator and the direction of the navigator's bow;

[0060] distance Definition: The first The straight-line Euclidean distance between the center of gravity of the follower and the center of gravity of the navigator.

[0061] Step (3): Construct the first The expected position and tracking error of the unmanned surface vessel:

[0062] Based on the leader-follower structure, the first... The expected position of the follower , Define the tracking error along the path of the follower. and cross-tracking error Define the heading angle error of the follower. .

[0063] definition Indicates the first The desired position of a follower is specifically represented as follows:

[0064] ,

[0065] in: and The leader is an unmanned surface vessel (USV). The actual position of (=0), Indicates the navigator and the first The formation distance between the follower ships Navigator unmanned surface vessel (USV) The actual heading angle (=0). Indicates the navigator and the first The formation angle between the follower ships.

[0066] Step (4): Calculate the first... Dynamic tracking of the error between the actual position of an unmanned surface vessel and its reference position:

[0067] Based on step (3), and combining the kinematic relationship with the derivative of the desired trajectory, the error dynamics equation is obtained, and the first equation is defined. The combined velocity of the expected follower's sway and roll speeds and the The expected side slip angle of the ship .

[0068] From step (3) The expected position expression of the follower with respect to time Differentiation yields:

[0069] ,

[0070] Definition of error (in Earth-fixed coordinate system):

[0071] ,

[0072] Multiply by the rotation matrix on the left and transfer to the ship's coordinate system:

[0073] ,

[0074] Therefore The positional error and heading angle error of the follower are shown below:

[0075] ,

[0076] In the above formula, and These are the tracking error along the path and the cross-tracking error of the follower. It is the heading angle error of the follower.

[0077] Organize the above formula and adjust the time. The derivative yields the error dynamics equation described in step (3):

[0078] ,

[0079] in: For the first The ship expects to follow the combined velocity of its sway and roll speeds. and They are the first The expected sway speed and lateral sway speed of the ship's followers It is the first The sideslip angle of the ship that expects to follow.

[0080] Step (5): Design the first Predefined time reference heading for the unmanned surface vessel:

[0081] Regarding the first The yaw and yaw systems of an unmanned surface vessel (USV) are used to construct the desired heading angle. :

[0082] ,

[0083] in: It is the first The expected heading angle of an unmanned surface vessel. It is the first The heading angle of the unmanned surface vessel, It is the actual heading angle of the Navigator unmanned surface vessel. , , It is a constant. For the first The positional error of the unmanned surface vessel It is a symbolic function.

[0084] Step (6): Design the first Predefined time reference speed for an unmanned surface vessel:

[0085] Based on the mathematical model established in step (1), for the first The tumble motion of an unmanned surface vessel is designed with a predefined time-defined tumble guidance velocity. Construct a predefined time-based sway guidance law for multi-unmanned surface vessel formations;

[0086] ,

[0087] in: , , It is a constant. For the first The positional error of the unmanned surface vessel It is a symbolic function.

[0088] Step (7): Proof of convergence of position error in predefined time, as follows.

[0089] Prove the stability of the follower's tracking error and cross-tracking error along the way:

[0090] Consider the Lyapunov function:

[0091] ,

[0092] Taking its derivative, we get:

[0093] ,

[0094] Depend on And guide the predefined time oscillation speed. Substituting into the derivative of the Lyapunov function, we get:

[0095] ,

[0096] in, ,but:

[0097] ,

[0098] Combining the results, we get: Let , ,but:

[0099] ,

[0100] For any ,have: ,Pick ,again ,but: ,

[0101] Therefore:

[0102] ,

[0103] Similarly, take ,but:

[0104] ,

[0105] Substituting, we get:

[0106] ,

[0107] Matching predefined time lemma forms:

[0108] ,

[0109] Right now:

[0110]

[0111] Therefore, we choose:

[0112] ,

[0113] In summary:

[0114] .

[0115] According to the predefined time stability theory, the above equation shows that the tracking error along the path and the cross-tracking error of the follower satisfy the predefined time convergence property.

[0116] Step (8): Simulation verification:

[0117] Based on steps (1) to (7), the predefined time-expected heading angle, predefined time-swell guidance velocity, and LOS-based predefined time guidance law are designed; the simulation environment is set as follows:

[0118] Environmental parameter settings: The scalar distances of the 1st, 2nd, and 3rd actual unmanned surface vessels (USVs) relative to the virtual USVs are respectively The corresponding deflection angles are respectively The initial heading angle of most unmanned surface vessels is... The initial bow roll angular velocity is The initial oscillation velocity is The initial positions of the first, second, and third actual unmanned surface vessels are [7.07, 2.07], [7.07, -7.07], and [0, 0], respectively; the initial position of the virtual navigator is [0, 0].

[0119] Control parameter settings: The control parameters for the multi-unmanned surface vessel (USV) swarm system are as follows: , , , , , , , , , .

[0120] Using the designed unmanned surface vessel (USV) swarm collaborative predefined time envelope angle-range guidance algorithm, and taking a certain type of USV as the simulation object, the algorithm based on the predefined time envelope angle-range guidance algorithm of the present invention is simulated and verified.

Claims

1. A predefined time envelope-range guidance algorithm for unmanned surface vessel swarm collaboration, characterized in that: Includes the following steps: Step (1): Establish a three-degree-of-freedom mathematical model for the cooperative motion of unmanned surface vessel swarms: To study the motion of unmanned surface vessels (USVs) on the horizontal plane, a mathematical model of their horizontal motion is established in a fixed coordinate system and a hull coordinate system, including a kinematic model and a dynamic model, encompassing their three degrees of freedom: pitch, sway, and yaw. Step (2): Define the first The angle between the follower and the navigator and distance ; Step (3): Construct the first The expected position and tracking error of the unmanned surface vessel: Based on the leader-follower structure, the first... The expected northward position of the ship's followers and eastward Define the northbound tracking error of the follower. and eastward tracking error Define the heading angle error of the follower. ; Step (4): Calculate the first... Dynamic tracking of the error between the actual position of an unmanned surface vessel and its reference position; Step (5): Design the first A predefined time reference heading for an unmanned surface vessel; Step (6): Design the first The predefined time reference speed of the unmanned surface vessel; Step (7): Proof of convergence of position error in predefined time: According to the predefined time stability lemma, for Lyapunov functions... The conclusion obtained by differentiation is consistent with the predefined time stability lemma, thus the proof of the predefined time stability is complete; Step (8): Numerical simulation verification: The unmanned surface vessel (USV) swarm collaborative predefined time envelope angle-distance guidance algorithm designed in steps (1) to (7) was simulated on a certain type of USV swarm.

2. The unmanned surface vessel swarm collaborative predefined time envelope-range guidance algorithm according to claim 1, characterized in that, The definition described in step (2) is as follows: The angle between the follower and the navigator and distance The details are as follows: Corner Definition: From the first The angle between the line of sight of the follower pointing towards the navigator and the direction of the navigator's bow; distance Definition: The first The straight-line Euclidean distance between the center of gravity of the follower and the navigator.

3. The unmanned surface vessel swarm collaborative predefined time envelope-range guidance algorithm according to claim 1, characterized in that, The construction of the desired formation position in step (3) is as follows: definition Indicates the first The desired position of a follower is specifically represented as follows: ， in: and The leader is an unmanned surface vessel (USV). The actual position of (=0), Indicates the navigator and the first The formation distance between the follower ships Navigator unmanned surface vessel (USV) The actual heading angle (=0). Indicates the navigator and the first The formation angle between the follower ships.

4. The unmanned surface vessel swarm collaborative predefined time envelope-range guidance algorithm according to claim 1, characterized in that, The error dynamics equation mentioned in step (4) is as follows: From step (3) The expected position expression of the follower with respect to time Differentiation yields: ， Therefore The positional error and heading angle error of the follower are shown below: ， in: and These are the tracking error along the path and the cross-tracking error of the follower. It is the heading angle error of the follower.

5. Organize the above formula and adjust the time. The derivative yields the error dynamics equation described in step (4): ， in: For the first The ship expects to follow the combined velocity of its sway and roll speeds. For the first The bow roll rate of the follower ship, and They are the first The expected sway speed and lateral sway speed of the ship's followers It is the first The sideslip angle of the ship that expects to follow.

6. The unmanned surface vessel swarm collaborative predefined time envelope-range guidance algorithm according to claim 1, characterized in that, The step (5) described for the first The yaw and yaw systems of an unmanned surface vessel (USV) are used to construct the desired heading angle. The details are as follows: To stabilize the cross-tracking error of the followers , for the first The follower design predefined expected heading angle at a given time: ， in: It is the first The expected heading angle of an unmanned surface vessel. It is the first The heading angle of the unmanned surface vessel, It is the actual heading angle of the Navigator unmanned surface vessel. , , It is a constant. For the first The eastward position error of the unmanned surface vessel. It is a symbolic function.

7. The unmanned surface vessel swarm cooperative predefined time envelope-range guidance algorithm according to claim 1, characterized in that, The step (6) described for the first The tumble motion of an unmanned surface vessel is designed with a predefined time-defined tumble guidance velocity. A predefined time-based multi-unmanned surface vessel (USV) formation sway guidance law is constructed as follows: To stabilize the tracking error of the follower unmanned surface vessel along its route Assuming Design oscillation speed: ， in: , , It is a constant. For the first The northward position error of the unmanned surface vessel. It is a symbolic function.

Images