An underwater swarm trajectory planning method, device, equipment and storage medium

Abstract

The present disclosure provides an underwater cluster path planning method and device, equipment and a storage medium, the method comprising: obtaining a reference time for a first sub-device to reach a target point after being launched from a first initial point; determining a first path for a launching device to travel and a second path for a second sub-device to travel based on a preset launch point of the second sub-device; determining a first time based on a first speed of the launching device and the first path, determining a second time based on a second speed of the second sub-device and the second path, and determining an estimated time based on the first time and the second time; determining a first parameter based on the estimated time and the reference time, weighting the first path and the second path based on the first parameter, updating the preset launch point, and obtaining a target launch point.

Description

Technical Field

[0001] This disclosure relates to the field of unmanned system swarm control, and in particular to an underwater swarm trajectory planning method, apparatus, equipment and storage medium. Background Technology

[0002] With the increasing complexity of marine unmanned system missions, heterogeneous swarm systems composed of unmanned surface vehicles (USVs) and autonomous underwater vehicles (AUVs) have shown great potential in areas such as marine exploration and collaborative monitoring. In a typical operational process, a surface mother ship usually releases multiple underwater robots sequentially during its voyage, and each robot must then arrive at a specific three-dimensional spatial location on time to form a mission formation. However, existing planning methods have significant shortcomings in handling such problems.

[0003] First, dynamic deployment presents a significant challenge in terms of spatiotemporal coupling. The continuous movement of the surface mother ship results in a non-uniform dynamic distribution of the deployment starting points for each underwater robot, making precise time synchronization control difficult within a unified framework. Second, while existing random sampling algorithms can handle spatial obstacles, they incur enormous computational costs and exhibit unpredictable randomness when meeting strict kinematic constraints and high-precision timing requirements. Furthermore, traditional obstacle avoidance strategies often rely on drastic changes in heading, which not only disrupts the smoothness of the trajectory but also increases the difficulty of time compensation. Therefore, developing a collaborative trajectory planning method that balances three-dimensional curvature constraints, dynamic obstacle avoidance, and precise time alignment is crucial for improving the operational efficiency of unmanned system swarms, addressing the deterministic planning and high-precision collaboration requirements of heterogeneous systems in dynamic deployment environments. Summary of the Invention

[0004] This disclosure provides an underwater swarm trajectory planning method, apparatus, device, and storage medium to at least solve the above-mentioned technical problems existing in the prior art.

[0005] According to a first aspect of this disclosure, an underwater swarm trajectory planning method is provided, applied to a system consisting of deployment equipment and sub-equipment, the method comprising: Obtain the reference time for the first sub-device to reach the target point after being deployed from the first initial point; Based on the preset delivery point of the second sub-device, determine the first path of the delivery device and the second path of the second sub-device; A first time is determined based on the first speed and the first path of the delivery device; a second time is determined based on the second speed and the second path of the second sub-device; and an estimated time is determined based on the first time and the second time. A first parameter is determined based on the estimated time and the reference time. The first path and the second path are weighted based on the first parameter, and the preset delivery point is updated to obtain the target delivery point.

[0006] In one possible implementation, obtaining the reference time for the first sub-device to reach the target point after being deployed from the first initial point includes: Obtain the first state information of the first sub-device, and determine the minimum turning radius of the first sub-device based on the first state information; Obtain environmental information of the target area, input the first state information, the environmental information, the minimum turning radius, the first initial point and the target point into the first path planning model, and obtain the first reference path output by the first path planning model; Based on the kinematic constraints of the first sub-device, the first reference path is mapped to a three-dimensional reference path; The reference time is determined based on the preset speed of the first sub-device and the three-dimensional reference path.

[0007] In one possible implementation, determining the first path of the delivery device and the second path of the second sub-device based on the preset delivery point of the second sub-device includes: The preset delivery point is the intersection of the first path and the second path; Determine the second initial point at which the delivery device accelerates to the target speed after delivering the first sub-device; The environmental information, the second initial point, the preset delivery point, and the second status information of the delivery device are input into the first path planning model to obtain the first path output by the first path planning model. Obtain the third state information of the second sub-device, input the preset delivery point, the target point, the environmental information, the minimum turning radius and the third state information into the first path planning model, and obtain the second predicted path output by the first path planning model; The second predicted path is mapped to the second path based on the kinematic constraints of the second sub-device.

[0008] In one possible implementation, after mapping the first reference path to a three-dimensional reference path based on the kinematic constraints of the first sub-device, the method further includes: Based on the environmental information, determine the coordinate information and equivalent radius of the obstacle; After determining the interference of the obstacle with the three-dimensional reference path based on the coordinate information and the equivalent radius, the first distance for the first sub-device to avoid the obstacle is determined based on the equivalent radius. The obstacle avoidance path type of the first sub-device is determined based on the coordinate information of the obstacle and the three-dimensional reference path. The starting position of the obstacle avoidance path is determined based on the preset relative azimuth angle, the first spacing, the equivalent radius, and the minimum turning radius. The central angle of the obstacle avoidance arc is determined based on the starting position, the relative azimuth angle, the equivalent radius, and the minimum turning radius; The three-dimensional reference path is updated based on the central angle of the obstacle avoidance arc and the starting position; The reference time is determined based on the preset speed of the first sub-device and the updated three-dimensional reference path.

[0009] In one possible implementation, determining the first parameter based on the expected time and the reference time includes: Based on the time ratio of the expected time and the reference time, if the time ratio is determined to be greater than a first threshold, the first algorithm is used to calculate the time ratio to determine the iterative value of the first parameter. Alternatively, if the time ratio is determined to be less than a first threshold, the second algorithm is used to calculate the time ratio to determine the iterative value of the first parameter; The first parameter is determined based on the preset value of the first parameter and the iterative value of the first parameter.

[0010] In one possible implementation, after updating the preset delivery point, the method further includes: The first path and the second path are updated based on the updated delivery points; The estimated time is updated based on the updated first path and second path to obtain the updated time; The current time synchronization accuracy is determined based on the update time and the reference time. In response to the current time synchronization accuracy being lower than the time synchronization accuracy threshold, the updated delivery point is used as the target delivery point. Alternatively, in response to the current time synchronization accuracy being higher than the time synchronization accuracy threshold, the iterative value of the first parameter is updated based on the updated delivery point; The updated delivery point is iteratively updated based on the updated first parameter iteration value until the current time synchronization accuracy is lower than the time synchronization accuracy threshold.

[0011] In one possible implementation, mapping the first reference path to a three-dimensional reference path based on the kinematic constraints of the first sub-device includes: The three-dimensional reference path is divided into transition segments, correction segments, and alignment segments; The starting point of the transition segment is set as the starting point of the first reference path, and the ending point of the transition segment is set as the projection point of the starting point of the first reference path at the target depth. The starting point of the correction segment is the ending point of the transition segment, and the ending point of the correction segment is when the first sub-device travels to a position where the first angle is zero based on the projection of the first reference path at the target depth. The starting point of the alignment segment is the ending point of the correction segment, and the ending point of the alignment segment is the target point; A spatial spiral is determined based on the kinematic constraints of the first sub-device, the start point of the transition segment, and the end point of the transition segment; and the transition segment is determined based on the spatial spiral. The correction segment is determined based on the starting point of the correction segment, the ending point of the correction segment, and the first angle; The first reference path, excluding the portions corresponding to the transition segment and the correction segment, is copied as the alignment segment within the plane at the target depth.

[0012] According to a second aspect of this disclosure, an underwater swarm trajectory planning device is provided, the device comprising: The reference time determination unit is used to obtain the reference time of the first sub-device arriving at the target point after being deployed from the first initial point; The path determination unit is used to determine a first path for the delivery device and a second path for the second sub-device based on a preset delivery point of the second sub-device. A time generation unit is configured to determine a first time based on a first speed and a first path of the delivery device, determine a second time based on a second speed and a second path of the second sub-device, and determine an estimated time based on the first time and the second time. The target delivery point update unit is used to determine a first parameter based on the estimated time and the reference time, weight the first path and the second path based on the first parameter, update the preset delivery point, and obtain the target delivery point.

[0013] According to a third aspect of this disclosure, an electronic device is provided, comprising: At least one processor; and A memory communicatively connected to the at least one processor; wherein, The memory stores instructions that can be executed by the at least one processor to enable the at least one processor to perform the methods described in this disclosure.

[0014] According to a fourth aspect of this disclosure, a non-transitory computer-readable storage medium is provided storing computer instructions for causing a computer to perform the methods described in this disclosure.

[0015] This disclosure discloses an underwater swarm trajectory planning method, apparatus, device, and storage medium. It uses the arrival time of the first deployed sub-device at the target point based on the path output by a first path planning model as the base time. Homotopy parameters are used as weighting coefficients between the paths of the next deployed sub-device and the deployed device. The homotopy parameters are updated based on the base time and the expected time to achieve the update of the deployment point. Beneficial technical effects include: significantly improving the spatiotemporal coordination capability of heterogeneous unmanned system swarms in dynamic mission scenarios; ensuring that heterogeneous swarms can still form predetermined formations with extremely high time synchronization accuracy under complex dynamic initial constraints; improving computational efficiency; reducing dependence on real-time communication bandwidth of the swarm; and allowing the system to flexibly add or remove swarm members or adjust the target configuration during the mission, exhibiting strong mission adaptability and system robustness.

[0016] It should be understood that the description in this section is not intended to identify key or essential features of the embodiments of this disclosure, nor is it intended to limit the scope of this disclosure. Other features of this disclosure will become readily apparent from the following description. Attached Figure Description

[0017] The above and other objects, features, and advantages of this disclosure will become readily apparent from the following detailed description of exemplary embodiments, taken in conjunction with the accompanying drawings. Several embodiments of this disclosure are illustrated in the drawings by way of example and not limitation, in which: In the accompanying drawings, the same or corresponding reference numerals indicate the same or corresponding parts.

[0018] Figure 1 This illustration shows the implementation flow of an underwater swarm trajectory planning method according to an embodiment of the present disclosure. Figure 1 ; Figure 2 This illustration shows the implementation flow of an underwater swarm trajectory planning method according to an embodiment of the present disclosure. Figure 2 ; Figure 3 A schematic diagram of a deployment scenario according to an embodiment of this disclosure is shown; Figure 4 This diagram illustrates the path weight partitioning and 3D mapping logic based on homotopy parameters according to an embodiment of this disclosure. Figure 5 A geometric relationship diagram of the variable radius Durbins curve obstacle avoidance strategy according to an embodiment of this disclosure is shown; Figure 6 The diagram shows the convergence curves of the arrival time deviation of each underwater unmanned vehicle according to the embodiments of this disclosure as a function of the number of iterations; Figure 7 A schematic diagram of the overall process of an underwater swarm trajectory planning method according to an embodiment of the present disclosure is shown; Figure 8 A schematic diagram of the composition structure of an electronic device according to an embodiment of the present disclosure is shown; Figure 9 A schematic diagram of an underwater swarm trajectory planning device according to an embodiment of the present disclosure is shown. Detailed Implementation

[0019] To make the objectives, features, and advantages of this disclosure more apparent and understandable, the technical solutions in the embodiments of this disclosure will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this disclosure, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of this disclosure without creative effort are within the scope of protection of this disclosure.

[0020] Figure 1 This illustration shows the implementation flow of an underwater swarm trajectory planning method according to an embodiment of the present disclosure. Figure 1 ,like Figure 1 As shown, an underwater swarm trajectory planning method according to an embodiment of this disclosure includes the following process: Step 101: Obtain the reference time for the first sub-device to reach the target point after being deployed from the first initial point.

[0021] In this embodiment of the disclosure, a system consisting of a delivery device and sub-devices is applied, wherein the delivery device can be a USV and the sub-devices can be AUVs. Specifically, the system acquires first state information of the first sub-device, determines the minimum turning radius of the first sub-device based on the first state information, wherein the first state information includes: the pitch angle, longitudinal velocity, lateral velocity, vertical velocity, and yaw rate of the AUV; acquires environmental information of the target area; inputs the first state information, the environmental information, the minimum turning radius, the first initial point, and the target point into a first path planning model; and acquires a first reference path output by the first path planning model, wherein the environmental information includes the coordinates and dimensions of obstacles in the environment where the sub-devices and the delivery device are located, the first initial point is the point where the first sub-device is delivered, the target point is the driving target position of all sub-devices, the first path planning model is a Dubins path planning model, and the first reference path is a two-dimensional planar path output by the Dubins path planning model.

[0022] In this embodiment of the disclosure, the first reference path is mapped to a three-dimensional reference path based on the kinematic constraints of the first sub-device. Specifically, the three-dimensional reference path is divided into a transition segment, a correction segment, and an alignment segment. The transition segment is used for the first sub-device to spiral down to the target depth according to the pitch angle corresponding to the arc set by the kinematic constraints. The correction segment is used for the first sub-device to correct attitude deviations so that its pitch angle is zero. The alignment segment is used for the first sub-device to travel along the path to the target point. The starting point of the transition segment is set as the starting point of the first reference path, and the ending point of the transition segment is set as the projection point of the starting point of the first reference path at the target depth. The starting point of the correction segment is the ending point of the transition segment, and the ending point of the correction segment is the position where the first sub-device travels to a position with a first angle of zero based on the projection of the first reference path at the target depth, where the first angle is the pitch angle. The starting point of the alignment segment is the ending point of the correction segment, and the ending point of the alignment segment is the target point. A spatial spiral is determined based on the kinematic constraints of the first sub-device, the starting point of the transition segment, and the ending point of the transition segment. The transition segment is determined based on the spatial spiral, where the kinematic constraints are the AUV's own kinematic performance, including the AUV's maximum pitch angle limit and minimum radius of curvature limit. The correction segment is determined based on the starting point of the correction segment, the ending point of the correction segment, and the first angle. The first reference path, excluding the corresponding portions of the transition segment and the correction segment, is copied into the alignment segment in the plane at the target depth. The reference time is determined based on the preset speed of the first sub-device and the three-dimensional reference path.

[0023] In this embodiment of the disclosure, when the interference between the third-dimensional reference path and an obstacle is determined based on environmental information, the path needs to be adjusted to achieve obstacle avoidance. Specifically, the coordinate information and equivalent radius of the obstacle are determined based on the environmental information; after determining the interference of the obstacle with the three-dimensional reference path based on the coordinate information and the equivalent radius, a first distance for the first sub-device to avoid the obstacle is determined based on the equivalent radius; the obstacle avoidance path type of the first sub-device is determined based on the coordinate information of the obstacle and the three-dimensional reference path, wherein the path type is divided into: right-turn arc - Left-turn arc-right-turn arc (RLR) and left-turn arc-right-turn arc-left-turn arc (LRL); the starting position of the obstacle avoidance path is determined based on the preset relative azimuth angle, the first spacing, the equivalent radius, and the minimum turning radius; the central angle of the obstacle avoidance arc of the obstacle avoidance path is determined based on the starting position, the relative azimuth angle, the equivalent radius, and the minimum turning radius; the three-dimensional reference path is updated based on the central angle of the obstacle avoidance arc and the starting position; the reference time is determined based on the preset speed of the first sub-device and the updated three-dimensional reference path.

[0024] Step 102: Based on the preset delivery point of the second sub-device, determine the first path of the delivery device and the second path of the second sub-device.

[0025] In this embodiment, the preset deployment point is the intersection of the first path and the second path. For example, a USV carrying an AUV to be deployed travels along the first path on the water surface to the preset deployment point, deploys the AUV, and then the AUV travels from the preset deployment point along the second path to the target point. Since the USV is at low speed or stationary when deploying the AUV, it needs to accelerate to the target speed for normal travel. A second initial point is determined when the deployment device accelerates to the target speed after deploying the first sub-device. The environmental information, the second initial point, the preset deployment point, and the second state information of the deployment device are input into the first path planning model to obtain the first path output by the first path planning model. The second state information refers to the state of the USV. The system acquires state information, including longitudinal velocity, lateral velocity, heading angle, heading angular velocity, and yaw angular velocity; it obtains the third state information of the second sub-device, inputs the preset deployment point, the target point, the environmental information, the minimum turning radius, and the third state information into the first path planning model, and obtains the second predicted path output by the first path planning model, wherein the third state information is the same as the first state information; based on the kinematic constraints of the second sub-device, it maps the second predicted path to the second path, wherein the second predicted path is a two-dimensional path and the second path is a three-dimensional path, and the method of mapping the second predicted path to the second path is the same as the method of mapping the first reference path to the three-dimensional reference path.

[0026] Step 103: Determine a first time based on the first speed and the first path of the delivery device, determine a second time based on the second speed and the second path of the second sub-device, and determine an estimated time based on the first time and the second time.

[0027] In this embodiment of the disclosure, the first speed of the delivery device is the aforementioned target speed, and the second speed is the combined speed determined based on the lateral speed, longitudinal speed, and vertical speed of the second sub-device.

[0028] Step 104: Determine a first parameter based on the estimated time and the reference time; weight the first path and the second path based on the first parameter; update the preset delivery point; and obtain the target delivery point.

[0029] In this embodiment of the disclosure, based on the time ratio of the estimated time and the reference time, if the time ratio is determined to be greater than a first threshold, a first algorithm is used to calculate the time ratio to determine a first parameter iteration value; or, if the time ratio is determined to be less than the first threshold, a second algorithm is used to calculate the time ratio to determine the first parameter iteration value; wherein, the first threshold is a preset value, greater than the first threshold indicates that the travel time needs to be shortened, and less than the first threshold indicates that the travel distance needs to be extended. The application condition of the first algorithm is that the product of the time ratio and the first parameter iteration value is less than zero, and the application condition of the second algorithm is that the product of the time ratio and the first parameter iteration value is not less than zero. Specifically, the first algorithm and the second algorithm are as follows:

[0030] in, The first parameter iteration value, The time ratio is used. The first parameter is determined based on its preset value and its iterative value.

[0031] In this embodiment, the first path and the second path are updated based on the updated delivery point; the estimated time is updated based on the updated first path and the second path according to the preset speed of the delivery device and the speed of the sub-device, to obtain the updated time; the current time synchronization accuracy is determined based on the updated time and the reference time; in response to the current time synchronization accuracy being lower than the time synchronization accuracy threshold, the updated delivery point is used as the target delivery point; or, in response to the current time synchronization accuracy being higher than the time synchronization accuracy threshold, the iterative value of the first parameter is updated based on the updated delivery point; the updated delivery point is iteratively updated based on the iterative value of the updated first parameter until the current time synchronization accuracy is lower than the time synchronization accuracy threshold. Specifically, when the current time synchronization accuracy is higher than the time synchronization accuracy threshold, steps 101-104 are repeated until the current time synchronization accuracy is lower than the time synchronization accuracy threshold, indicating that the sub-delivery device can reach the designated location while meeting the conditions.

[0032] Figure 2 This illustration shows the implementation flow of an underwater swarm trajectory planning method according to an embodiment of the present disclosure. Figure 2 ,like Figure 2 As shown, an underwater swarm trajectory planning method according to an embodiment of this disclosure includes the following steps: Step 201: Construct dynamic models of USV and AUV.

[0033] In this embodiment, a collaborative system for dynamically deploying multiple unmanned underwater vehicles (AUVs) from a surface unmanned surface vessel (USV) is first established. Based on the continuous navigation trajectory of the USV during the AUV deployment process, the coordinates of the deployment start point of each AUV are determined. As the real-time position of the USVs changes dynamically, they exhibit non-uniform dynamic distribution characteristics. Based on the preset mission formation configuration, the system accurately calculates the target position of each AUV in three-dimensional space, thereby determining the dynamic initial state and final rendezvous target point for each AUV with independent kinematic constraints.

[0034] The kinematic equations of the USV are expressed as follows:

[0035] The kinematic equations of an AUV are expressed as follows:

[0036] The USV kinematic equations and AUV kinematic equations are used to describe the motion variables in the inertial coordinate system. This represents the lateral displacement of the USV. This represents the longitudinal displacement of the USV. The yaw angle of the USV. The oscillation velocity of the USV, Indicates the sway velocity of the USV. This indicates the yaw rate of the USV; This refers to the lateral displacement of the AUV. This represents the longitudinal displacement of the AUV. This represents the vertical displacement of the AUV. The pitch angle of the AUV. The yaw angle of the AUV. The oscillation velocity of the AUV, The sway speed of the AUV, The yaw speed of the AUV. This is the rotational angular velocity (tilt rate) of the AUV about the y-axis. This is the rotational angular velocity (yaw rate) of the AUV about the z-axis.

[0037] Step 202: Deploy the first unmanned aerial vehicle (UAV) and solve for the UAV's path and reference time.

[0038] In this embodiment of the disclosure, when determining the reference time, the system selects the first AUV deployed in the deployment sequence as the reference. By performing trajectory geometry extrapolation between its initial position and target location, the system calculates the time required for the first AUV to reach different positions in the formation, selects the minimum time as the time limit T, and simultaneously determines the target point of the first AUV. Specifically, to accurately calculate its shortest distance, it is first necessary to calculate the minimum turning radius of the AUV. Considering the dynamic limits of the aircraft, the formula for determining its minimum turning radius is:

[0039]

[0040] The first AUV is released from the starting position, combined with... Figure 3 The AUV first plans a Dubins path on the horizontal plane and maps the obtained path to three-dimensional space. The starting point of this path is the AUV's initial position, and the ending point is the projection of the target point onto the horizontal plane. By adjusting the initial angle of the starting point, different paths are obtained, and the shortest path is selected as the reference path. The turning radius of the Dubins path must be set greater than the minimum turning radius. The obtained path is mapped to three-dimensional space according to its composition, including the first turning segment of the Dubins path mapped as a spiral descent segment, the second segment mapped as a pitch angle adjustment segment, and the final turning segment mapped as a circular arc segment to finally reach the target point. If the three-dimensional path encounters obstacles, it is then... Figure 4 (a) The path is replanned. The AUV determines the obstacle avoidance path type as RLR or LRL based on its positional relationship with the obstacle. If the obstacle center is to the left of the AUV's heading, it chooses to avoid the obstacle to the right to reduce the obstacle avoidance path length, resulting in an RLR path; otherwise, it chooses an LRL path. For example, the radius of the middle section is The starting position for obstacle avoidance and the distance to the obstacle have the following geometric relationship:

[0041] in The linear distance between the obstacle avoidance starting point and the obstacle. Removal between the centers of two circular arcs ( The incremental length of the line segment outside of ) The equivalent safety envelope radius of the obstacle. Let be the actual turning radius, and α be the reference azimuth angle between the AUV's heading at the obstacle avoidance starting point and the line connecting the obstacle's center. Therefore, we can calculate:

[0042] The angles of the three arcs that make up the obstacle avoidance path are respectively , , , It is the central angle of a single small arc. According to... Figure 4 The geometric relationship in (b) is expressed as:

[0043] Based on the baseline rate of the first deployed AUV (determined by taking the square root of the sum of the squares of the AUV's sway, pitch, and yaw rates; the system collects these values ​​in real-time or derives them in real-time from the AUV's equations of motion), a new path including the Dubins path is calculated between the initial position and the target location. The time is then recalculated, and this new path becomes the baseline time. .

[0044] Step 203: Determine the path of the next unmanned vehicle and the path of the surface unmanned vessel, as well as the running time.

[0045] In this embodiment of the disclosure, the reference time is obtained. The USV then selects the nearest point as the next deployment point based on the positions of the remaining AUVs in the formation. For example... Figure 5 As shown by the dashed line, the USV first accelerates to the target speed, and this position is taken as the starting point of the USV path. The deployment point of the next AUV is taken as the ending point of the USV path and the starting point of the AUV path, and the target point is taken as the ending point of the AUV path. Homotopy parameters are set. A weighted parameter is assigned to the path, with a value range of [0, 1], to divide the paths of the USV and AUV. Based on the starting and ending positions mentioned above, and following the method in step 202, the Dubins paths of the USV and AUV are first constructed in the horizontal plane. The AUV's path needs to be mapped to three-dimensional space, including the spiral descent section, the pitch angle adjustment section, and the end arc section. If obstacles are encountered, dynamic obstacle avoidance with variable radius is required. The mapping method is as follows: Figure 5 As shown, the pitch angle corresponding to the first arc is set based on the Dubins path obtained from the AUV's horizontal plane and the AUV's own kinematic performance to complete the spiral descent. When the AUV approaches the target depth and reaches the projection point corresponding to the first arc of the Dubins path, it begins a parabolic descent. At this point, the pitch angle changes with depth in the following manner:

[0046] in, The set diving pitch angle, For target depth, The current real-time depth, The target depth is set to initiate the parabolic dive. Upon reaching the target depth, the vehicle moves to the target position in a horizontal plane following the Dubins path at that depth. This ensures that each generated candidate track meets the AUV's maximum pitch angle and minimum radius of curvature limits. Setting a homotopy parameter yields a set of paths with different deployment positions, thus obtaining the time. This includes the runtime of the USV segment and the runtime of the 3D path after AUV mapping; specifically, the USV's speed. Rate of AUV Represented as:

[0047] in, and The variables in the above USV and AUV motion equations are... and The target speed for normal driving is as described above. and The motion equations of the AUV are derived in real time or the current speed is automatically collected by the system.

[0048] time Represented as:

[0049] in, Indicates the USV path. Indicates the AUV path. Indicates the speed of the USV. This indicates the speed of movement of the AUV.

[0050] Step 204: Update the homotopy parameters and determine the final delivery point.

[0051] In this embodiment of the disclosure, due to Since it is a random variable within the interval [0,1], it can generate an infinite number of path groups, leading to a large computational overhead. To improve computational efficiency, a time approximation adjustment strategy is adopted. Optimize accordingly. Specifically, calculate the current time ratio:

[0052] Wherein, time scaling factor This indicates that travel time must be shortened by increasing... To shorten the travel distance of the AUV. Conversely, the time scaling factor. This means that we need to extend the exercise time and reduce the time spent exercising. To extend the driving range of the AUV. Therefore, setting... The method for iterating over values ​​is expressed as:

[0053] in, Until the convergence criteria are met , This is a preset time synchronization accuracy threshold. Once the convergence condition is met, the USV and AUV segment paths are updated based on the updated homotopy parameters, and the final deployment point is determined based on the updated paths. Subsequent AUVs are iteratively planned using the same method.

[0054] In this embodiment of the disclosure, simulation experiments were conducted on 3AUV, 4AUV, and 5AUV formations to verify effectiveness. The time synchronization accuracy threshold was set to 0.02 seconds. Figure 6 The graph shows the convergence curve of the arrival time deviation of each AUV as a function of the number of iterations in the case of a 5AUV formation. The error reaches the specified time synchronization accuracy threshold after a maximum of 25 iterations.

[0055] This invention significantly enhances the spatiotemporal collaboration capabilities of heterogeneous unmanned system clusters in dynamic mission scenarios by constructing an integrated collaborative deployment and simultaneous arrival trajectory planning framework. For the first time, it integrates the dynamic deployment process of surface unmanned vessels with the navigation mission of underwater robots into a unified parameterized path partitioning model. Homotopy parameters are used to automatically adjust the path weights at different stages, effectively solving the initial state deviation problem caused by the disconnect between deployment and navigation in traditional planning. This ensures that heterogeneous clusters can still form predetermined formations with extremely high time synchronization accuracy under complex dynamic initial constraints, laying a solid synchronization foundation for subsequent collaborative operations.

[0056] This invention demonstrates significant technical advantages in terms of computational efficiency and system determinism. Employing a deterministic planning approach based on analytical geometry, it directly solves for the geometric analytical solution of the path, fundamentally avoiding the extensive spatial random sampling and redundant collision detection calculations inherent in traditional sampling-based algorithms. This planning method not only ensures high repeatability of the trajectory output under the same constraints but also reduces the computational overhead of the algorithm by several orders of magnitude. This enables the system to achieve millisecond-level real-time trajectory replanning on low-power embedded processors, greatly enhancing the rapid response capability and real-time performance of unmanned swarms in dynamically changing environments.

[0057] This invention also significantly improves upon safety and compliance with kinematic constraints. By introducing a variable radius Dubins strategy, it overcomes the limitations of traditional obstacle avoidance algorithms that require drastic changes in heading or the addition of discrete path points to avoid obstacles. By dynamically adjusting the curvature radius of turning segments, it maintains the geometric continuity of the trajectory while ensuring a safe obstacle avoidance envelope. Combined with a three-dimensional spatial decoupling mapping mechanism, the generated trajectory naturally meets the strict minimum turning radius, pitch angle, and depth variation constraints of underwater robots, completely eliminating the risk that conventional algorithms' generated trajectories cannot be accurately executed in practical engineering control due to abrupt curvature changes or excessive slope.

[0058] This invention boasts superior parameter convergence speed and system scalability. The designed fast-convergence parameter iteration strategy introduces a time-ratio-based adaptive step-size adjustment mechanism, which can rapidly approximate the reference time based on the predicted time deviation using monotonicity characteristics, achieving minimal time alignment error within a very small number of iterations. This implicit collaborative architecture based on a time reference ensures that the planning processes of each subsystem are logically independent, effectively reducing dependence on real-time cluster communication bandwidth and allowing the system to flexibly add or remove cluster members or adjust the target configuration during tasks, exhibiting strong task adaptability and system robustness.

[0059] Figure 7 This diagram illustrates the overall flow of an underwater swarm trajectory planning method according to an embodiment of the present disclosure, as follows: Figure 7 As shown, based on the motion parameters, environmental information, and target configuration of the AUV and USV, a cooperative system for the AUV and USV is established, and the target location and initial dynamic deployment state of the AUV are calculated. The first AUV is deployed, its dubins path is solved, and its travel time is calculated. The travel time of the first AUV is used as the baseline time. When the path is determined to pass an obstacle, dynamic obstacle avoidance with variable radius is implemented, and the path and baseline time are updated. When the path is determined not to pass an obstacle, or after updating the path and baseline time, the target point of the next deployed AUV is selected, and the initial values ​​of the homotopy parameters are set. Dubins paths are constructed for both the USV and AUV, and the AUV paths are mapped to three-dimensional space. The time required for this is calculated. Similarly, when the path passes an obstacle, dynamic obstacle avoidance with variable radius is implemented, and the path and baseline time are updated. When the path is determined not to pass an obstacle, or after updating the path and baseline time, the convergence criteria are used. Determine whether the homotopy parameter has been updated to the optimal value. If the determination condition is met, output the final deployment point determined based on the optimal homotopy parameter.

[0060] According to embodiments of this disclosure, this disclosure also provides an electronic device and a readable storage medium.

[0061] Figure 8 A schematic block diagram of an example electronic device 800 that can be used to implement embodiments of the present disclosure is shown. The electronic device is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device may also represent various forms of mobile devices, such as personal digital processors, cellular phones, smartphones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely illustrative and are not intended to limit the implementation of the present disclosure described and / or claimed herein.

[0062] like Figure 8 As shown, the electronic device 800 includes a computing unit 801, which can perform various appropriate actions and processes based on a computer program stored in a read-only memory (ROM) 802 or a computer program loaded from a storage unit 808 into a random access memory (RAM) 803. The RAM 803 may also store various programs and data required for the operation of the electronic device 800. The computing unit 801, ROM 802, and RAM 803 are interconnected via a bus 804. An input / output (I / O) interface 805 is also connected to the bus 804.

[0063] Multiple components in electronic device 800 are connected to I / O interface 805, including: input unit 806, such as keyboard, mouse, etc.; output unit 807, such as various types of displays, speakers, etc.; storage unit 808, such as disk, optical disk, etc.; and communication unit 809, such as network card, modem, wireless transceiver, etc. Communication unit 809 allows electronic device 800 to exchange information / data with other devices through computer networks such as the Internet and / or various telecommunications networks.

[0064] The computing unit 801 can be a variety of general-purpose and / or special-purpose processing components with processing and computing capabilities. Some examples of the computing unit 801 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various special-purpose artificial intelligence (AI) computing chips, various computing units running machine learning model algorithms, a digital signal processor (DSP), and any suitable processor, controller, microcontroller, etc. The computing unit 801 performs the various methods and processes described above, such as an underwater swarm trajectory planning method. For example, in some embodiments, an underwater swarm trajectory planning method can be implemented as a computer software program tangibly contained in a machine-readable medium, such as storage unit 808. In some embodiments, part or all of the computer program can be loaded and / or installed on the electronic device 800 via ROM 802 and / or communication unit 809. When the computer program is loaded into RAM 803 and executed by the computing unit 801, one or more steps of the underwater swarm trajectory planning method described above can be performed. Alternatively, in other embodiments, computing unit 801 may be configured to perform an underwater swarm trajectory planning method by any other suitable means (e.g., by means of firmware).

[0065] Figure 9 A schematic diagram of an underwater swarm trajectory planning device according to an embodiment of the present disclosure is shown, as follows: Figure 9 As shown, an underwater swarm trajectory planning device according to an embodiment of this disclosure includes: The reference time determination unit 901 is used to obtain the reference time of the first sub-device arriving at the target point after being deployed from the first initial point; The path determination unit 902 is used to determine a first path for the delivery device and a second path for the second sub-device based on a preset delivery point of the second sub-device. The time generation unit 903 is used to determine a first time based on the first speed and the first path of the delivery device, determine a second time based on the second speed and the second path of the second sub-device, and determine an estimated time based on the first time and the second time; The target delivery point update unit 904 is used to determine a first parameter based on the estimated time and the reference time, weight the first path and the second path based on the first parameter, update the preset delivery point, and obtain the target delivery point.

[0066] The obstacle avoidance unit 905 is configured to: determine the coordinate information and equivalent radius of an obstacle based on the environmental information; determine the interference of the obstacle with the three-dimensional reference path based on the coordinate information and the equivalent radius; determine a first distance for the first sub-device to avoid the obstacle based on the equivalent radius; determine the obstacle avoidance path type of the first sub-device based on the coordinate information of the obstacle and the three-dimensional reference path; determine the starting position of the obstacle avoidance path based on a preset relative azimuth angle, the first distance, the equivalent radius, and the minimum turning radius; determine the central angle of the obstacle avoidance arc of the obstacle avoidance path based on the starting position, the relative azimuth angle, the equivalent radius, and the minimum turning radius; update the three-dimensional reference path based on the central angle of the obstacle avoidance arc and the starting position; and determine the reference time based on the preset speed of the first sub-device and the updated three-dimensional reference path.

[0067] The parameter update unit 906 is configured to update the first path and the second path based on the updated delivery point; update the estimated time based on the updated first path and the second path to obtain an updated time; determine the current time synchronization accuracy based on the updated time and the reference time; and, in response to the current time synchronization accuracy being lower than the time synchronization accuracy threshold, use the updated delivery point as the target delivery point; or, in response to the current time synchronization accuracy being higher than the time synchronization accuracy threshold, update the first parameter iteration value based on the updated delivery point; and iteratively update the updated delivery point based on the updated first parameter iteration value until the current time synchronization accuracy is lower than the time synchronization accuracy threshold.

[0068] In an exemplary embodiment, the reference time determination unit 901, path determination unit 902, time generation unit 903, target delivery point update unit 904, obstacle avoidance unit 905, parameter update unit 906, etc., can be implemented by one or more central processing units (CPU), graphics processing units (GPU), application specific integrated circuits (ASIC), DSPs, programmable logic devices (PLD), complex programmable logic devices (CPLD), field-programmable gate arrays (FPGA), general-purpose processors, controllers, micro controller units (MCU), microprocessors, or other electronic components.

[0069] Regarding the apparatus in the above embodiments, the specific manner in which each module and unit performs its operations has been described in detail in the embodiments related to the method, and will not be elaborated upon here.

[0070] Various embodiments of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), systems-on-a-chip (SoCs), payload-programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various embodiments may include implementations in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor, capable of receiving data and instructions from a storage system, at least one input device, and at least one output device, and transmitting data and instructions to the storage system, the at least one input device, and the at least one output device.

[0071] The program code used to implement the methods of this disclosure may be written in any combination of one or more programming languages. This program code may be provided to a processor or controller of a general-purpose computer, special-purpose computer, or other programmable data processing apparatus, such that when executed by the processor or controller, the program code causes the functions / operations specified in the flowcharts and / or block diagrams to be implemented. The program code may be executed entirely on a machine, partially on a machine, as a standalone software package partially on a machine and partially on a remote machine, or entirely on a remote machine or server.

[0072] In the context of this disclosure, a machine-readable medium can be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, apparatus, or device. A machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium can be, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination of the foregoing. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0073] To provide interaction with a user, the systems and techniques described herein can be implemented on a computer having: a display device for displaying information to the user (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor); and a keyboard and pointing device (e.g., a mouse or trackball) through which the user provides input to the computer. Other types of devices can also be used to provide interaction with the user; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including sound input, voice input, or tactile input).

[0074] The systems and technologies described herein can be implemented in computing systems that include backend components (e.g., as a data server), or computing systems that include middleware components (e.g., an application server), or computing systems that include frontend components (e.g., a user computer with a graphical user interface or web browser through which a user can interact with implementations of the systems and technologies described herein), or any combination of such backend, middleware, or frontend components. The components of the system can be interconnected via digital data communication of any form or medium (e.g., a communication network). Examples of communication networks include local area networks (LANs), wide area networks (WANs), and the Internet.

[0075] Computer systems can include clients and servers. Clients and servers are generally located far apart and typically interact via communication networks. Client-server relationships are created by computer programs running on the respective computers and having a client-server relationship with each other. Servers can be cloud servers, servers in distributed systems, or servers incorporating blockchain technology.

[0076] It should be understood that the various forms of processes shown above can be used to rearrange, add, or delete steps. For example, the steps described in this disclosure can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution of this disclosure can be achieved, and this is not limited herein.

[0077] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this disclosure, "a plurality of" means two or more, unless otherwise explicitly specified.

[0078] The above description is merely a specific embodiment of this disclosure, but the scope of protection of this disclosure is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this disclosure should be included within the scope of protection of this disclosure. Therefore, the scope of protection of this disclosure should be determined by the scope of the claims.

Claims

1. An underwater swarm trajectory planning method, applied to a system consisting of deployment equipment and sub-equipment, characterized in that, The method includes: Obtain the reference time for the first sub-device to reach the target point after being deployed from the first initial point; Based on the preset delivery point of the second sub-device, determine the first path of the delivery device and the second path of the second sub-device; A first time is determined based on the first speed and the first path of the delivery device; a second time is determined based on the second speed and the second path of the second sub-device; and an estimated time is determined based on the first time and the second time. A first parameter is determined based on the estimated time and the reference time. The first path and the second path are weighted based on the first parameter, and the preset delivery point is updated to obtain the target delivery point.

2. The method according to claim 1, characterized in that, The step of obtaining the reference time for the first sub-device to reach the target point after being deployed from the first initial point includes: Obtain the first state information of the first sub-device, and determine the minimum turning radius of the first sub-device based on the first state information; Obtain environmental information of the target area, input the first state information, the environmental information, the minimum turning radius, the first initial point and the target point into the first path planning model, and obtain the first reference path output by the first path planning model; Based on the kinematic constraints of the first sub-device, the first reference path is mapped to a three-dimensional reference path; The reference time is determined based on the preset speed of the first sub-device and the three-dimensional reference path.

3. The method according to claim 2, characterized in that, The determination of the first path for the delivery device and the second path for the second sub-device based on the preset delivery point of the second sub-device includes: The preset delivery point is the intersection of the first path and the second path; Determine the second initial point at which the delivery device accelerates to the target speed after delivering the first sub-device; The environmental information, the second initial point, the preset delivery point, and the second status information of the delivery device are input into the first path planning model to obtain the first path output by the first path planning model. Obtain the third state information of the second sub-device, input the preset delivery point, the target point, the environmental information, the minimum turning radius and the third state information into the first path planning model, and obtain the second predicted path output by the first path planning model; The second predicted path is mapped to the second path based on the kinematic constraints of the second sub-device.

4. The method according to claim 2, characterized in that, After mapping the first reference path to a three-dimensional reference path based on the kinematic constraints of the first sub-device, the method further includes: Based on the environmental information, determine the coordinate information and equivalent radius of the obstacle; After determining the interference of the obstacle with the three-dimensional reference path based on the coordinate information and the equivalent radius, the first distance for the first sub-device to avoid the obstacle is determined based on the equivalent radius. The obstacle avoidance path type of the first sub-device is determined based on the coordinate information of the obstacle and the three-dimensional reference path. The starting position of the obstacle avoidance path is determined based on the preset relative azimuth angle, the first spacing, the equivalent radius, and the minimum turning radius. The central angle of the obstacle avoidance arc is determined based on the starting position, the relative azimuth angle, the equivalent radius, and the minimum turning radius; The three-dimensional reference path is updated based on the central angle of the obstacle avoidance arc and the starting position; The reference time is determined based on the preset speed of the first sub-device and the updated three-dimensional reference path.

5. The method according to claim 1, characterized in that, Determining the first parameter based on the expected time and the reference time includes: Based on the time ratio of the expected time and the reference time, if the time ratio is determined to be greater than a first threshold, the first algorithm is used to calculate the time ratio to determine the iterative value of the first parameter. Alternatively, if the time ratio is determined to be less than a first threshold, the second algorithm is used to calculate the time ratio to determine the iterative value of the first parameter; The first parameter is determined based on the preset value of the first parameter and the iterative value of the first parameter.

6. The method according to claim 5, characterized in that, After updating the preset delivery point, the method further includes: The first path and the second path are updated based on the updated delivery points; The estimated time is updated based on the updated first path and second path to obtain the updated time; The current time synchronization accuracy is determined based on the update time and the reference time. In response to the current time synchronization accuracy being lower than the time synchronization accuracy threshold, the updated delivery point is used as the target delivery point. Alternatively, in response to the current time synchronization accuracy being higher than the time synchronization accuracy threshold, the iterative value of the first parameter is updated based on the updated delivery point; The updated delivery point is iteratively updated based on the updated first parameter iteration value until the current time synchronization accuracy is lower than the time synchronization accuracy threshold.

7. The method according to claim 2, characterized in that, The kinematic constraints based on the first sub-device map the first reference path to a three-dimensional reference path, including: The three-dimensional reference path is divided into transition segments, correction segments, and alignment segments; The starting point of the transition segment is set as the starting point of the first reference path, and the ending point of the transition segment is set as the projection point of the starting point of the first reference path at the target depth. The starting point of the correction segment is the ending point of the transition segment, and the ending point of the correction segment is when the first sub-device travels to a position where the first angle is zero based on the projection of the first reference path at the target depth. The starting point of the alignment segment is the ending point of the correction segment, and the ending point of the alignment segment is the target point; A spatial spiral is determined based on the kinematic constraints of the first sub-device, the start point of the transition segment, and the end point of the transition segment; and the transition segment is determined based on the spatial spiral. The correction segment is determined based on the starting point of the correction segment, the ending point of the correction segment, and the first angle; The first reference path, excluding the portions corresponding to the transition segment and the correction segment, is copied as the alignment segment within the plane at the target depth.

8. An underwater cluster trajectory planning device, characterized in that, The device includes: The reference time determination unit is used to obtain the reference time of the first sub-device arriving at the target point after being deployed from the first initial point; The path determination unit is used to determine a first path for the delivery device and a second path for the second sub-device based on a preset delivery point of the second sub-device. A time generation unit is configured to determine a first time based on a first speed and a first path of the delivery device, determine a second time based on a second speed and a second path of the second sub-device, and determine an estimated time based on the first time and the second time. The target delivery point update unit is used to determine a first parameter based on the estimated time and the reference time, weight the first path and the second path based on the first parameter, update the preset delivery point, and obtain the target delivery point.

9. An electronic device, characterized in that, include: At least one processor; as well as A memory communicatively connected to the at least one processor; wherein, The memory stores instructions that can be executed by the at least one processor to enable the at least one processor to perform the method of any one of claims 1-7.

10. A non-transitory computer-readable storage medium storing computer instructions, characterized in that, The computer instructions are used to cause the computer to perform the method according to any one of claims 1-7.

Images