Formation transformation and trajectory planning method for safe crossing of multiple unmanned ships in limited environment

Abstract

The invention discloses a formation transformation and trajectory planning method for safe crossing of multiple unmanned ships in a limited environment. The method comprises the following steps: S1, initializing unmanned ship formation parameters; s2, designing a weighted cost function for the unmanned ship formation, and obtaining a passable piloting trajectory meeting environmental constraints and unmanned ship motion characteristics by minimizing the weighted cost function; s3, based on a multi-agent cooperative control strategy, the unmanned ship formation adaptively reconstructs a formation topological structure through a dynamic pilot node switching mechanism; and S4, when the passing width is limited, the unmanned ship formation adaptively selects a proper passing mode to pass through the narrow area and arrive at a terminal point. According to the invention, by providing a smooth switching method of a common formation and a pilot ship dynamic replacement mechanism, the formation structure of the unmanned ship formation can be flexibly adjusted according to task requirements and environment changes, and the maneuverability and adaptability of the formation are improved.

Description

Technical Field

[0001] The present invention relates to the field of unmanned boat control technology, and more specifically to a formation transformation and trajectory planning method for multiple unmanned boats to safely cross in a restricted environment. Background Art

[0002] Currently, model predictive control (MPC) is commonly used for trajectory planning of UAV formations. This method establishes a dynamic model of the UAVs, combines it with preset mission objectives and environmental constraints, and optimizes a control input sequence within a prediction time domain to achieve trajectory tracking and obstacle avoidance for the UAV formation. However, existing technologies mainly focus on how to achieve accurate trajectory planning in a known environment and a fixed formation structure, and do not fully consider the practical problems and challenges that may be faced in complex dynamic environments, such as local minima, formation width adaptability, and energy consumption optimization. Specifically:

[0003] (1) Limited navigability and poor adaptability of formation width: When the water surface width is limited, the traditional MPC method may not be able to reasonably plan the navigable width of the formation, resulting in difficulty for the formation to pass smoothly in narrow waters, and may even cause collisions or mission delays;

[0004] (2) Infeasible trajectory caused by local minimum: In complex environments, such as when there are multiple obstacles or narrow channels, the trajectory planned by MPC may fall into a local minimum, making it impossible for the unmanned vehicle to find a feasible route, which may lead to mission failure or safety risks;

[0005] (3) Insufficient integration of energy consumption, tasks and obstacle avoidance, and rigid decision-making models: Traditional methods often treat energy consumption control, task priority adjustment, surface obstacle avoidance, etc. as independent modules. There is a lack of a unified strategy that can comprehensively consider these factors, which may lead to conflicts between modules in actual tasks and affect the overall performance.

[0006] Therefore, it is necessary to propose a new solution to solve the above problems. Summary of the Invention

[0007] The purpose of the present invention is to overcome the deficiencies of the above-mentioned prior art and provide a method for formation transformation and trajectory planning of multiple unmanned boats that can safely cross in a restricted environment.

[0008] In order to achieve the above object, the present invention adopts the following technical solutions:

[0009] The formation transformation and trajectory planning method for multiple unmanned boats safely crossing in a constrained environment includes the following steps:

[0010] Step S1, initializing the parameters of the unmanned boat formation;

[0011] Step S2: designing a weighted cost function for the UAV formation, and obtaining a navigable pilotage trajectory that satisfies environmental constraints and UAV motion characteristics by minimizing the weighted cost function;

[0012] Step S3: Based on the multi-agent collaborative control strategy, the unmanned boat formation adaptively reconstructs the formation topology through a dynamic pilot node switching mechanism;

[0013] Step S4: When the passage width is limited, the unmanned boat formation adaptively selects a suitable passage method to pass through the narrow area and reach the destination.

[0014] Furthermore, the parameters for initializing the UAV formation include the target position coordinates of the UAV formation. .

[0015] Furthermore, when the unmanned boat formation falls into a local minimum dilemma during the trajectory planning process, a dynamic path reconstruction strategy driven by virtual obstacles is introduced to generate a new virtual obstacle at the dilemma location. At the same time, the unmanned boat formation retreats to a certain trajectory point at a certain moment and replans a safe arrival trajectory in the new scenario with the addition of virtual obstacles.

[0016] Furthermore, the conditions for judging whether the unmanned boat formation is in a local predicament are:

[0017] (1)

[0018] In formula (1), The unmanned boat formation from time step T to (T+ ), , is a natural number, indicating the number of states involved in the judgment; and is the judgment parameter;

[0019] If the conditions are met, then from the coordinates Re-plan the trajectory; is the re-planning parameter to obtain a new passable pilot trajectory , , is a natural number, indicating the subscript of the end state of the entire planned trajectory.

[0020] Furthermore, step S2 includes the following steps:

[0021] Step S201: define the desired state , defining the actual state ,exist Under control, the control error is ,in, is the left propeller speed; is the right propeller speed; represents transpose; is the desired heading angle of the unmanned boat; is the desired longitudinal speed of the unmanned boat; is the desired lateral speed of the USV; is the expected turning rate of the unmanned boat;

[0022] Step S202: The weighted cost function is J ,in, and is a positive deterministic symmetric weight indicator, indicating the forecast range; is the current time point; is the Nth time point;

[0023] Step S203: Under the environmental constraints, model and control constraints, the weighted cost function J Minimize the distance from the initial point to the end point to obtain a navigable pilot trajectory that meets environmental constraints and the motion characteristics of the unmanned boat , .

[0024] Furthermore, environmental constraints include environmental constraint variables and , the design is as follows:

[0025] (2)

[0026] In formula (2), is the effective diameter of the unmanned boat; is the effective diameter of the i-th obstacle; The set safety distance; is the coordinate of the i-th obstacle; Command for the width of the formation; is the number of obstacles;

[0027] In order to meet the requirements of formation width and avoid obstacles, the environmental constraint variables and The constraints are given by:

[0028] (3).

[0029] Furthermore, the model and control constraints are:

[0030] (4)

[0031] In formula (4), 、 and is the hydrodynamic function of the unmanned boat; is the thrust coefficient of the left propeller; is the thrust coefficient of the right propeller; is the water density; D is the propeller diameter; M is the mass of the unmanned boat hull; is the moment of inertia of the unmanned boat; B is the width of the unmanned boat;

[0032] Left propeller speed and right propeller speed The constraints are given by:

[0033] (5).

[0034] Furthermore, in step S3, for the horizontal formation, the required position of each unmanned boat is as follows:

[0035] (6)

[0036] In formula (6), is the trajectory coordinate of the jth unmanned boat, ; Expressed as an unmanned boat; Indicates the fixed distance between two adjacent unmanned boats in a horizontal formation.

[0037] Furthermore, in step S3, for the longitudinal formation, the required position of each unmanned boat is as follows:

[0038] (7)

[0039] In formula (7), ; Indicates the distance between two adjacent unmanned boats in longitudinal formation.

[0040] Furthermore, in step S4, when the unmanned boat formation considers energy consumption first, it will change its formation to pass through the narrow area and reach the end point; when considering formation first, it will maintain the original formation to bypass the narrow area and reach the end point through a trajectory of passable width.

[0041] The beneficial effects of the present invention are:

[0042] 1. The present invention proposes a smooth switching method for common formations and a dynamic replacement mechanism for pilot ships, so that the unmanned boat formation can flexibly adjust the formation structure according to mission requirements and environmental changes, thereby improving the maneuverability and adaptability of the formation.

[0043] 2. This invention introduces a dynamic path reconstruction strategy driven by virtual obstacles. When the UAV falls into a local minimum dilemma, a new virtual obstacle is generated at the dilemma location. At the same time, the UAV formation retreats to a certain trajectory point at a certain moment. In the new scenario with the addition of virtual obstacles, a new trajectory is replanned to reach the destination safely, thereby ensuring the global accessibility and safety of trajectory planning.

[0044] 3. The present invention comprehensively considers energy consumption, task priority and the width of navigable waters, and realizes the integration of dynamic formation switching and obstacle avoidance; when the navigable width is limited, the unmanned boat can intelligently decide whether to prioritize energy efficiency to pass through obstacles or to prioritize maintaining the formation, ensuring safe trajectory planning and improving the efficiency, stability and economy of the unmanned boat formation in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 This is a flow chart of a method for formation transformation and trajectory planning of multiple unmanned boats safely crossing a restricted environment in this embodiment;

[0046] Figure 2 This is a schematic diagram of a conversion of the lateral formation of unmanned boats in this embodiment;

[0047] Figure 3 A schematic diagram of a conversion of the longitudinal formation of unmanned boats in this embodiment;

[0048] Figure 4a A schematic diagram of the unmanned boat falling into a local minimum dilemma in this embodiment;

[0049] Figure 4b A schematic diagram of the unmanned boat generating a virtual obstacle at a difficult position in this embodiment;

[0050] Figure 4c A schematic diagram of replanning the trajectory of the unmanned boat in a difficult position in this embodiment;

[0051] Figure 5a A schematic diagram of the unmanned boat formation in this embodiment selecting energy consumption priority to pass through a narrow area;

[0052] Figure 5b A schematic diagram of the unmanned boat formation in this embodiment selecting a formation to bypass narrow areas first;

[0053] Figure 6a A schematic diagram of the unmanned boat formation sailing in a horizontal line in this embodiment;

[0054] Figure 6b A schematic diagram of the unmanned boat formation sailing in a longitudinal line in this embodiment;

[0055] Figure 6cA schematic diagram of the dynamic replacement of the pilot boat of the unmanned boat formation in this embodiment;

[0056] Figure 6d A schematic diagram of the unmanned boat formation in this embodiment returning to a horizontal in-line sailing state;

[0057] Figure 7a This is a schematic diagram of the unmanned boat in this embodiment directly passing through an obstacle;

[0058] Figure 7b This is a schematic diagram of the unmanned boat in this embodiment directly passing through the obstacle to reach the end point;

[0059] Figure 7c A schematic diagram of the unmanned boat bypassing an obstacle in this embodiment;

[0060] Figure 7d A schematic diagram of the unmanned boat in this embodiment bypassing an obstacle to reach the destination;

[0061] Figure 8a A schematic diagram of the departure of the unmanned boat formation in this embodiment;

[0062] Figure 8b This is a schematic diagram of the unmanned boat formation selecting energy consumption priority to pass through obstacles in this embodiment. DETAILED DESCRIPTION

[0063] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0064] Example: Formation transformation and trajectory planning method for multiple unmanned boats to safely cross in a restricted environment, such as Figure 1 As shown, the following steps are included:

[0065] Step S1: The parameters of the unmanned boat formation initialization include the target position coordinates of the unmanned boat formation ;

[0066] Step S2: designing a weighted cost function for the UAV formation, and obtaining a navigable pilotage trajectory that satisfies environmental constraints and UAV motion characteristics by minimizing the weighted cost function;

[0067] Furthermore, step S2 specifically includes the following steps:

[0068] Step S201: define the desired state , defining the actual state ,exist Under control, the control error is ,in, is the left propeller speed; is the right propeller speed; represents transpose; is the desired heading angle of the unmanned boat; is the desired longitudinal speed of the unmanned boat; is the desired lateral speed of the unmanned boat; is the expected turning rate of the unmanned boat;

[0069] Step S202: To drive the unmanned boat formation to the end point, the weighted cost function is J ,in, and is a positive deterministic symmetric weight indicator, indicating the forecast range; is the current time point; is the Nth time point;

[0070] Step S203: Under the environmental constraints, model and control constraints, the weighted cost function J Minimize the distance from the initial point to the final point.

[0071] Specifically, environmental constraints include environmental constraint variables and , the design is as follows:

[0072] (2)

[0073] In formula (2), is the effective diameter of the unmanned boat; is the effective diameter of the i-th obstacle; The safety distance is set; is the coordinate of the i-th obstacle; Command for the width of the formation; is the number of obstacles;

[0074] In order to meet the requirements of formation width and avoid obstacles, the environmental constraint variables and The constraints are given by:

[0075] (3).

[0076] The model and control constraints are:

[0077] (4)

[0078] In formula (4), 、 and is the hydrodynamic function of the unmanned boat; is the thrust coefficient of the left propeller; is the thrust coefficient of the right propeller; is the water density; D is the propeller diameter; M is the mass of the unmanned boat hull; is the moment of inertia of the unmanned boat; B is the width of the unmanned boat;

[0079] Left propeller speed and right propeller speed The constraints are given by:

[0080] (5).

[0081] Under the constraints of Equations (2) to (5), the weighted cost function J Minimize the distance from the initial point to the end point; in this way, a navigable pilot trajectory that meets the environmental constraints and the motion characteristics of the unmanned boat is obtained. , .

[0082] In step S3, based on a multi-agent collaborative control strategy, the UAV formation adaptively reconfigures its formation topology through a dynamic pilot node switching mechanism. This allows the formation to maintain geometric stability by autonomously switching to a suboptimal pilot node based on a preset priority or real-time performance evaluation algorithm when the pilot unit's energy reserves approach a critical threshold. This approach effectively addresses the energy balance issue for UAV formations during long-duration missions, significantly improving the continuity and robustness of swarm operations by reducing the risk of energy overload at a single node.

[0083] Specifically, for horizontal formations, such as Figure 2 As shown, the required position of each unmanned boat is as follows:

[0084] (6)

[0085] In formula (6), is the trajectory coordinate of the jth unmanned boat, ; Expressed as an unmanned boat; Indicates the fixed distance between two adjacent unmanned boats in a horizontal formation.

[0086] For longitudinal formations, such as Figure 3 As shown, the required position of each unmanned boat is as follows:

[0087] (7)

[0088] In formula (7), ; Indicates the distance between two adjacent unmanned boats in longitudinal formation.

[0089] Step S4: When the passage width is limited, the unmanned boat formation adaptively selects an appropriate passage method to pass through the narrow area and reach the destination. Specifically:

[0090] like Figure 5a As shown in Figure 2, when the unmanned boat formation considers energy consumption first, it will change its formation to pass through the narrow area and reach the end point; Figure 5b As shown in the figure, when considering formation priority, the original formation will be maintained to bypass the narrow area and reach the end point through a trajectory with a passable width.

[0091] In order to solve the problem that the unmanned boat formation may fall into the local minimum dilemma during the trajectory planning process, this embodiment also proposes a virtual obstacle-driven dynamic path reconstruction strategy to escape the dilemma. When the unmanned boat formation falls into the local minimum dilemma, a new virtual obstacle is generated at the dilemma position. At the same time, the unmanned boat formation returns to the previous state of T time steps and replans a safe arrival trajectory in the new scenario with the addition of virtual obstacles, such as Figure 4a-4c shown.

[0092] The conditions for judging whether the unmanned boat formation is in a local predicament are:

[0093] (1)

[0094] In formula (1), The unmanned boat formation from time step T to (T+ ), , is a natural number, indicating the number of states involved in the judgment; and is the judgment parameter;

[0095] If the conditions are met, then from the coordinates Re-plan the trajectory; is the re-planning parameter to obtain a new passable pilot trajectory , , is a natural number, indicating the subscript of the end state of the entire planned trajectory.

[0096] The following simulation verification is performed:

[0097] (1) Verification of dynamic transformation of unmanned boat formation based on improved MPC for channel navigability:

[0098] like Figure 6a-6d As shown, the unmanned boat formation transitions from a horizontal in-line to a vertical in-line, and then gradually recovers to a horizontal in-line through dynamic replacement of the pilot ship.

[0099] (2) Virtual obstacle generation and retrospective replanning verification:

[0100] like Figure 7a-7b As shown in , when the passable width meets the requirements, the unmanned boat passes directly and reaches the end point; Figure 7c-7d As shown in the figure, when the passable width does not meet the requirements, the dynamic path reconstruction strategy driven by virtual obstacles is used to replan the trajectory to reach the destination.

[0101] (3) Multi-objective collaborative optimization of unmanned boat formation crossing trajectory planning and decision-making

[0102] The unmanned boat formation sets off (e.g. Figure 8a As shown in the figure, when the passable width is limited, the unmanned boat formation can intelligently decide its crossing method, choosing energy consumption priority or formation priority; energy consumption priority means completing the crossing by changing the formation, that is, changing from a horizontal straight line to a vertical straight line, as shown in the figure. Figure 8b As shown; the team's priority is to maintain the formation and continue sailing by bypassing obstacles.

[0103] The above description is merely a preferred embodiment of the present invention. The scope of protection of the present invention is not limited to the above embodiment. All technical solutions based on the concept of the present invention are within the scope of protection of the present invention. It should be noted that for those skilled in the art, various improvements and modifications that do not depart from the principles of the present invention should also be considered within the scope of protection of the present invention.

Claims

1. A method for formation transformation and trajectory planning of multiple unmanned boats for safe crossing in a restricted environment, characterized by: The steps include: Step S1, initializing the parameters of the unmanned boat formation; Step S2: designing a weighted cost function for the UAV formation, and obtaining a navigable pilotage trajectory that satisfies environmental constraints and UAV motion characteristics by minimizing the weighted cost function; Step S3: Based on the multi-agent collaborative control strategy, the unmanned boat formation adaptively reconstructs the formation topology through a dynamic pilot node switching mechanism; Step S4: When the passage width is limited, the unmanned boat formation adaptively selects a suitable passage method to pass through the narrow area and reach the destination.

2. The method for formation transformation and trajectory planning of multiple unmanned boats for safe crossing in a restricted environment according to claim 1, characterized in that: The parameters for initializing the unmanned boat formation include the target position coordinates of the unmanned boat formation .

3. The method for formation transformation and trajectory planning of multiple unmanned boats for safe crossing in a restricted environment according to claim 1, characterized in that: When the unmanned boat formation falls into a local minimum dilemma during the trajectory planning process, a dynamic path reconstruction strategy driven by virtual obstacles is introduced to generate a new virtual obstacle at the dilemma location. At the same time, the unmanned boat formation retreats to a certain trajectory point at a certain moment and replans a safe arrival trajectory in the new scenario with the addition of virtual obstacles.

4. The method for formation transformation and trajectory planning of multiple unmanned boats for safe crossing in a restricted environment according to claim 3, characterized in that: The conditions for judging whether the unmanned boat formation is in a local predicament are: (1) In formula (1), The unmanned boat formation from time step T to (T+ ), , is a natural number, indicating the number of states involved in the judgment; and is the judgment parameter; If the conditions are met, then from the coordinates Re-plan the trajectory; is the re-planning parameter to obtain a new passable pilot trajectory , , is a natural number, indicating the subscript of the end state of the entire planned trajectory.

5. The method for formation transformation and trajectory planning of multiple unmanned boats for safe crossing in a restricted environment according to claim 1, characterized in that: Step S2 includes the following steps: Step S201: define the desired state , defining the actual state ,exist Under control, the control error is ,in, is the left propeller speed; is the right propeller speed; represents transpose; is the desired heading angle of the unmanned boat; is the desired longitudinal speed of the unmanned boat; is the desired lateral speed of the unmanned boat; is the expected turning rate of the unmanned boat; Step S202: The weighted cost function is J ,in, and is a positive deterministic symmetric weight indicator, indicating the forecast range; is the current time point; is the Nth time point; Step S203: Under the environmental constraints, model and control constraints, the weighted cost function J Minimize the distance from the initial point to the end point to obtain a navigable pilot trajectory that meets environmental constraints and the motion characteristics of the unmanned boat , .

6. The method for formation transformation and trajectory planning of multiple unmanned boats for safe crossing in a restricted environment according to claim 5, characterized in that: Environmental constraints include environmental constraint variables and , the design is as follows: (2) In formula (2), is the effective diameter of the unmanned boat; is the effective diameter of the i-th obstacle; The safety distance is set; is the coordinate of the i-th obstacle; Command for the width of the formation; is the number of obstacles; In order to meet the requirements of formation width and avoid obstacles, the environmental constraint variables and The constraints are given by: (3)。 7. The method for formation transformation and trajectory planning of multiple unmanned boats for safe crossing in a restricted environment according to claim 5, characterized in that: The model and control constraints are: (4) In formula (4), 、 and is the hydrodynamic function of the unmanned boat; is the thrust coefficient of the left propeller; is the thrust coefficient of the right propeller; is the water density; D is the propeller diameter; M is the mass of the unmanned boat hull; is the moment of inertia of the unmanned boat; B is the width of the unmanned boat; Left propeller speed and right propeller speed The constraints are given by: (5)。 8. The method for formation transformation and trajectory planning of multiple unmanned boats for safe crossing in a restricted environment according to claim 1, characterized in that: In step S3, for the horizontal formation, the required position of each unmanned boat is as follows: (6) In formula (6), is the trajectory coordinate of the jth unmanned boat, ; Expressed as an unmanned boat; Indicates the fixed distance between two adjacent unmanned boats in a horizontal formation.

9. The method for formation transformation and trajectory planning of multiple unmanned boats for safe crossing in a restricted environment according to claim 1, characterized in that: In step S3, for the longitudinal formation, the required position of each unmanned boat is as follows: (7) In formula (7), ; Indicates the distance between two adjacent unmanned boats in longitudinal formation.

10. The method for formation transformation and trajectory planning of multiple unmanned boats for safe crossing in a restricted environment according to claim 1, characterized in that: In step S4, when the unmanned boat formation considers energy consumption first, it will change its formation to pass through the narrow area and reach the end point; when considering formation first, it will maintain the original formation to bypass the narrow area and reach the end point through a trajectory of passable width.

Images