Missing formation collision avoidance method, program and device based on navigation UUV local coordination planning and storage medium

Abstract

The invention belongs to the technical field of multi-UUV formation collision avoidance, and particularly relates to a careful formation collision avoidance method based on navigation UUV local coordination planning, a program, equipment and a storage medium. Firstly, parameters and task target points are initialized; the navigation UUV carries out obstacle detection and sensing and construction of a rolling probability occupied grid local map by using a multi-beam sonar installed at the bow of the navigation UUV; performing map rolling updating according to the formation scale; calculating a local optimal sub-target point based on the virtual potential field; formation constraint-oriented improved IRRT * path planning is carried out, formation constraint collision detection is completed, and a local optimal path is obtained; and performing path execution and formation coordination control so as to realize formation coordination collision avoidance and complete a task. The multi-UUV formation collision avoidance method realizes safer, more real-time and more coordinated multi-UUV formation collision avoidance, is especially suitable for an underwater unknown complex environment with limited resources and difficult communication, and has important practical application value.

Description

Technical Field

[0001] This invention belongs to the field of multi-UUV formation collision avoidance technology, specifically relating to a deliberate formation collision avoidance method, program, device and storage medium based on local coordination planning of the lead UUV. Background Technology

[0002] With the increasing complexity of underwater exploration and operations, unmanned underwater vehicle (UUV) technology is gradually evolving from single-platform to multi-platform collaborative formations. At the single UUV level, key technologies such as autonomous navigation, path planning, and collision avoidance have matured and are widely used in practical engineering tasks. Currently, research focus is shifting towards multi-UUV collaborative formation systems, aiming to improve mission execution efficiency, area coverage, and overall system robustness through formation cooperation. Among these, the leader-follower type multi-UUV formation has become the most widely used collaborative architecture in engineering practice due to its clear control structure and ease of implementation.

[0003] Whether for a single UUV or multiple UUV systems, the ability to avoid collisions with unknown obstacles is crucial for ensuring navigational safety. For multiple UUV formations, the collision avoidance problem is even more complex: not only must the safety of each individual UUV be ensured, but the overall formation configuration and motion coordination must also be maintained. In actual underwater missions, limitations in communication bandwidth, energy, and payload capacity often mean that only the lead UUV is equipped with forward-looking sonar or other obstacle detection equipment, while follower UUVs lack independent obstacle perception capabilities. Under these conditions, formation collision avoidance faces severe challenges: the lead UUV must plan a safe and formation-compliant path online for the entire formation in an unknown obstacle environment. Furthermore, many collaborative missions require rigid formation maintenance, meaning that the preset formation geometry must be maintained during obstacle avoidance, further increasing the difficulty of formation collision avoidance. Therefore, developing an efficient and reliable online collaborative formation collision avoidance method for the typical application scenario where only the lead UUV possesses perception capabilities and must maintain formation rigidity has significant engineering practical value.

[0004] Regarding existing patented technologies, patent ZL202310946057.X describes an adaptive method for multi-UUV formation switching and obstacle avoidance. This patent, under a 3D map, allows multiple UUVs to achieve formation and obstacle avoidance through the interaction of potential forces such as attraction and repulsion. This patent requires each UUV to have perception and decision-making capabilities, and there is a high demand for communication among formation members, requiring the exchange of UUV position information to calculate mutual repulsion and achieve coordination. The core of this invention is the path of "centralized perception and deliberate planning by the navigator UUV." Only the navigator has perception capabilities, and planning is performed by constructing a rolling grid map and using an improved IRRT* algorithm, along with formation constraint collision detection. These are completely different technical approaches. This patent and this invention differ fundamentally in their core concepts, technical paths, system architecture, and final effects. The patent cannot simultaneously guarantee collision avoidance and rigid formation under complex obstacles, and can only compromise or switch. The solution of this invention provides an efficient and reliable approach not seen in current literature for solving the high-value, high-difficulty scenario of "only the navigator perceives, and rigid formation must be maintained." Summary of the Invention

[0005] The purpose of this invention is to address the limitations of existing technologies by providing a deliberate formation collision avoidance method, program, equipment, and storage medium based on local coordination planning of the lead UUV, under the condition that only the lead UUV is equipped with obstacle detection sonar and no complex communication interaction is required within the formation. This invention achieves safe collision-free navigation while maintaining formation rigidity by continuously updating environmental awareness and dynamically generating collision avoidance paths.

[0006] A deliberate formation collision avoidance method based on local coordination planning of a lead UUV includes the following steps:

[0007] The pilot UUV emits beams horizontally through the bow multibeam sonar to obtain the detection distance of each beam to the obstacle. Based on the position and heading of the pilot UUV at the current control time, the position of each obstacle detection point is calculated.

[0008] Calculate the distance between the current position of the pilot UUV at the control moment and the center of the map of the scroll probability grid partial map updated at the previous control moment. If the distance is greater than the distance threshold of the map window scroll, the current position of the pilot UUV at the control moment is used as the updated map center; otherwise, the updated map center position remains unchanged.

[0009] Based on the updated map center, update the scroll probability occupancy grid partial map; for grids that exceed the area before the update, initialize the posterior probability that the grid is occupied by an obstacle; for grids in the overlapping area between the updated and the area before the update, determine the obstacle detection point closest to the grid center, calculate the distance difference and angle difference between the grid and the corresponding beam of the obstacle detection point, and then update the posterior probability that the grid is occupied by an obstacle.

[0010] Based on the posterior probability of each grid cell in the updated scroll probability occupancy grid partial map being occupied by obstacles, determine the state of each grid cell, including whether it is occupied by obstacles or not.

[0011] Using the map center of the updated scroll probability-occupied grid local map as the center, candidate points are uniformly sampled from the map center towards the global target point based on the search radius of the candidate point subset, constructing a candidate point set. For each candidate point, based on the virtual potential field method, the gravitational potential field guiding the UUV formation towards the global target point and the total repulsive potential field of all grids occupied by obstacles are calculated. The sum of the gravitational potential field and the total repulsive potential field is taken as the synthetic potential field. The candidate point with the smallest synthetic potential field in the candidate point set is taken as the local optimal sub-target point.

[0012] An improved IRRT* path planning method is adopted to plan a path from the current position of the lead UUV to the local optimal sub-target point; the path planning result is used as the desired trajectory, and the UUV formation performs path tracking until the next control time;

[0013] Repeat the above steps until the UUV formation reaches the influence radius of the global target point.

[0014] Furthermore, the improved IRRT* path planning method is used to plan the path from the current control position of the pilot UUV to the locally optimal sub-target point, specifically as follows:

[0015] Construct a random tree. In each iteration, generate random sampling points located in grids that are not occupied by obstacles. Take the node closest to the random sampling point from the node set of the random tree and call it the neighboring node. Obtain the grid that is closest to the neighboring node and is occupied by an obstacle and call it the neighboring grid. Based on the distance from the neighboring node to the neighboring grid, determine the expansion step size and generate a new node from the neighboring node towards the random sampling point with the expansion step size.

[0016] Calculate the heading change of the lead UUV as it moves from a neighboring node to a new node. Based on the formation constraints of the UUV formation, determine the expected positions of each following UUV when the lead UUV is at the new node. For the path segment of the lead UUV from a neighboring node to the new node, and the path segment of each following UUV from its current position to its expected position, traverse all the grids traversed by each path segment. If all grids are in a state where they are not occupied by obstacles, then add the new node to the node set of the random tree, and add the path segment of the lead UUV from a neighboring node to the new node as an edge to the edge set of the random tree.

[0017] Repeat the above steps until the distance between the new node and the local optimal sub-target point is less than the radius of influence. At this point, start from the new node and backtrack along the random tree until the starting node is reached, forming a path from the position of the pilot UUV at the current control time to the local optimal sub-target point.

[0018] Furthermore, the step of using the path planning result as the desired trajectory and the UUV formation performing path tracking specifically involves:

[0019] The pilot UUV calculates the desired forward speed and desired heading based on the path planning results, and constructs the pilot UUV's current position, desired forward speed, and desired heading into an information packet.

[0020] The lead UUV broadcasts information packets to all following UUVs via an underwater acoustic communication module;

[0021] After receiving the information packet, each following UUV will take the desired heading of the lead UUV as its own desired heading. Based on the formation constraints of the UUV formation and its own position at the current control moment, it will calculate the propulsion force and rudder angle control commands to drive its own movement, thereby achieving coordinated movement and formation maintenance with the lead UUV.

[0022] Furthermore, the distance threshold for scrolling the map window. Combining the scale of the UUV formation with the maximum detection range setting of the multibeam sonar:

[0023]

[0024] in, The width of the UUV formation; The length of the UUV formation; This represents the farthest detection range for multibeam sonar.

[0025] Furthermore, the pilot UUV emits beams horizontally via a bow multibeam sonar to obtain the detection distance of each beam to obstacles. Based on the current control position of the pilot UUV With bow Calculate the position of each obstacle detection point. :

[0026]

[0027] in, For the first The angle of the beam relative to the heading of the pilot UUV;

[0028] According to the updated map center Update the rolling probability occupancy grid partial map. After the update, the center position of each grid cell in the rolling probability occupancy grid partial map is... :

[0029]

[0030] Among them, the rolling probability occupies the composition of the grid in the partial map as follows: ; To occupy the first grid pattern in the map with rolling probability Line number Index of the column grid; The side length of the grid;

[0031] The grid in the overlapping area between the updated and unupdated regions. Determine the obstacle detection point closest to the center of the grid. Calculate the beam corresponding to the grid relative to the obstacle detection point. Distance difference and angle difference :

[0032]

[0033]

[0034] This updates the posterior probability that the grid cell is occupied by an obstacle. :

[0035]

[0036] in, To update the probability of scrolling within the grid in the partial map, the grid is occupied. The posterior probability of being blocked by an obstacle; For grid The likelihood probability of being occupied by obstacles. For grid The likelihood probability that the area is not occupied by obstacles;

[0037]

[0038]

[0039] in, , , These are the weighting coefficients. ; and For uncertainty parameters in angle and distance measurements; System error compensation for sonar distance measurement.

[0040] Furthermore, the updated rolling probability occupies the grid pattern partial map. map center Using the candidate point subset as the center, search radius From the center of the map Point to global target point Starting from the direction, uniformly sample candidate points to construct a candidate point set. Candidate point location for:

[0041]

[0042]

[0043] in, Center of the map Point to global target point Direction and UUV heading at the current control moment The included angle; The search angle interval;

[0044] For each candidate point, the gravitational potential field guiding the UUV formation toward the global target point is calculated based on the virtual potential field method. :

[0045]

[0046] in, This is the gravitational gain coefficient;

[0047] Calculate the total repulsive potential field of the candidate point on all grid cells whose state is occupied by obstacles. :

[0048]

[0049]

[0050] in, Represents a grid The state is occupied by an obstacle; This is the repulsive force gain coefficient; The threshold value is affected by the repulsive potential field. For the first Candidate points and grid Distance from the center.

[0051] Furthermore, the neighbor-based node... To the adjacent grid distance Determine the expansion step size :

[0052]

[0053] in, This is the maximum step size; Minimum step size; The step size is used to switch the distance threshold;

[0054] From adjacent nodes To random sampling points Direction, to expand step size Generate new nodes :

[0055]

[0056] The calculation of the navigation UUV from the nearest node Move to new node Change of course :

[0057]

[0058] Based on the formation constraints of the UUV formation, the lead UUV is located at the new node. The desired position of each UUV at the time :

[0059]

[0060]

[0061] in, For the first The relative distance between the following UUV and the lead UUV; For the first The relative positions of the following UUV and the leading UUV.

[0062] A computer device includes a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the above-described deliberate formation collision avoidance method based on local coordination planning of a navigation UUV.

[0063] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the above-described deliberate formation collision avoidance method based on local coordinated planning of a pilot UUV.

[0064] A computer program product includes computer instructions that, when executed by a processor, implement the steps of the aforementioned deliberate formation collision avoidance method based on local coordination planning of a pilot UUV.

[0065] The beneficial effects of this invention are as follows:

[0066] 1. This invention addresses a typical application scenario where only the lead UUV possesses sensing capabilities and needs to maintain a rigid formation. It employs an architecture where the lead UUV handles centralized sensing and decision-making, while the following UUVs merely maintain formation. Only the lead UUV needs high-performance multi-beam sonar and a powerful computing unit, while the following UUVs do not require expensive collision avoidance sensors or complex planning processors, only basic communication and trajectory tracking capabilities. This significantly reduces the overall hardware cost, power consumption, and size of multi-UUV formation systems, facilitating large-scale deployment and application.

[0067] 2. The present invention eliminates the need for complex communication interactions within the formation. The lead UUV broadcasts its own motion status information packets (position, heading, speed) to all following UUVs at a fixed frequency via an underwater acoustic communication module. This greatly reduces the dependence on and pressure of the low-bandwidth, high-latency underwater acoustic communication network, and improves the reliability and feasibility of the system under real underwater communication constraints.

[0068] 3. During path planning, this invention uses a "node generation strategy based on formation motion constraints" to check whether all paths following the UUV from the current position to the new desired position conflict with obstacles. The generated path is a safe "corridor" that the entire formation entity (not just the leader point) can pass through, fundamentally avoiding the collision risk caused by formation size or "blindly following" the UUV, and achieving true formation-level collision avoidance safety.

[0069] 4. This invention combines a rolling probability occupancy grid local map with an improved IRRT* path planning algorithm oriented towards formation constraints. It can quickly generate and update safe and smooth local paths in unknown and complex environments, balancing computational efficiency and planning quality, and meeting the real-time requirements of UUV formation dynamic collision avoidance.

[0070] 5. Through innovative system architecture, environmental modeling methods, and path planning strategies, this invention achieves safer, more real-time, and more coordinated multi-UUV formation collision avoidance while reducing system costs and communication burden. It is particularly suitable for resource-constrained and communication-difficult underwater unknown and complex environments, and has significant practical application value. Attached Figure Description

[0071] Figure 1 This is a schematic diagram of a deliberate formation collision avoidance method based on local coordination planning of a pilot UUV.

[0072] Figure 2 This is a flowchart of a deliberate formation collision avoidance method based on local coordination planning of pilot UUVs.

[0073] Figure 3 This is a diagram illustrating the areas where different types of rasters are located during map updates.

[0074] Figure 4 A schematic diagram of selecting sub-target points for a raster map.

[0075] Figure 5 A diagram illustrating the process of maintaining a navigator-follower UUV formation.

[0076] Figure 6 This is an overall effect diagram of the local coordination planning of the navigation UUV based on the improved IRRT* under the raster map.

[0077] Figure 7 The local coordination planning diagram of the navigation UUV based on the improved IRRT* under the grid map at T=50s.

[0078] Figure 8 This is a local coordination planning diagram of the navigation UUV based on the improved IRRT* under the raster map at T=100s. Detailed Implementation

[0079] The present invention will now be further described with reference to the accompanying drawings.

[0080] This invention provides a deliberate formation collision avoidance method based on local coordination planning of a pilot UUV, comprising the following steps:

[0081] Step 1: The UUV formation includes a lead UUV and... The UUV that follows the lead UUV is equipped with a multi-beam sonar at the bow.

[0082] Determine the formation constraints of the UUV formation, including the first The relative distance between the following UUV and the lead UUV and relative orientation Formation width Formation length ; ;

[0083] Determine the global target point and the target's radius of influence ;

[0084] Set map side length Map grid composition Map grid side length The distance threshold for map window scrolling is set by combining the scale of the UUV formation and the maximum detection range of multibeam sonar. :

[0085]

[0086] in, This represents the maximum detection range for multibeam sonar.

[0087] Set a safety margin Candidate point search radius Search angle interval ;

[0088] Set the gravity gain coefficient Repulsion gain coefficient The threshold of repulsive potential field influence (Usually larger than the safety radius);

[0089] Set maximum step size Minimum step size Step size switching distance threshold ;

[0090] Initialize control count ;

[0091] Initialize the posterior probability that each grid cell is occupied by an obstacle. ;

[0092] Initialize the rolling probability occupying grid pattern partial map The center of the map is ;

[0093] Step 2: At the current control moment, i.e. At all times, obtain the location of the lead UUV. With bow Obtain the position of each following UUV The pilot UUV emits signals horizontally via a bow multibeam sonar. Each beam is used to obtain the detection distance of the obstacle detected by each beam. Calculate the position of each obstacle detection point. ;

[0094]

[0095] in, For the first The angle of the beam relative to the heading of the pilot UUV; ;

[0096] Step 3: Calculate the position of the pilot UUV at the current control moment. The rolling probability occupancy grid map updated with the previous control time. map center distance ;

[0097]

[0098] like Greater than the map window scrolling distance threshold The current position of the pilot UUV at the moment of control will be used as the center of the updated map. Otherwise, the center position of the updated map remains unchanged. ;

[0099] According to the updated map center Update the rolling probability of occupying the grid pattern map After the update, the scrolling probability occupies part of the grid pattern map. The center position of each grid is ;

[0100]

[0101] in, To occupy the first grid pattern in the map with rolling probability Line number Index of the column grid;

[0102] Step 4: For the updated rolling probability occupies the grid pattern partial map The probability of scrolling in the map before the update occupies the grid pattern part of the map. The grid is initialized with the posterior probability that the grid is occupied by an obstacle, i.e. ;

[0103] The updated rolling probability occupies the grid pattern part of the map. Compared with the previous rolling probability occupying grid pattern map For grid cells in overlapping regions, update the posterior probability that the grid cell is occupied by an obstacle. :

[0104] Step 4.1: For each grid cell Identify the obstacle detection point closest to its center and obtain the beam index corresponding to that obstacle detection point. Calculate the distance difference between the grid and the beam. and angle difference :

[0105]

[0106]

[0107] Step 4.2: Calculate the likelihood probability that the grid cell is occupied by an obstacle. Likelihood probability of being idle ;

[0108]

[0109]

[0110] in, , , Let be the weighting coefficient, satisfying ; and For uncertainty parameters in angle and distance measurements; System error compensation for sonar distance measurement;

[0111] Step 4.3: Update the posterior probability that the grid cell is occupied by an obstacle. :

[0112]

[0113] in, The previous control moment, i.e. The probability of rolling at any time occupies the grid pattern map Middle grid The posterior probability of being blocked by an obstacle;

[0114] Step 5: Based on the updated rolling probability, occupy the grid pattern partial map. The posterior probability that each grid cell is occupied by an obstacle Determine the state of each grid cell. ;

[0115]

[0116] in, Represents a grid A grid that is occupied by an obstacle can be called an occupied grid. Represents a grid A grid cell that is not occupied by an obstacle can be called a free grid cell. Represents a grid Status unknown; The hysteresis threshold can be set to 0.05 to enhance state stability.

[0117] Step 6: Combining Figure 4 Introduction: Solving for local optimal sub-target points based on virtual potential fields. ;

[0118] Step 6.1: Occupy the grid pattern partial map with the updated rolling probability. map center Using the candidate point subset as the center, the search radius is... The radius is from the center of the map. Point to global target point Starting from the direction, with search angle intervals... Uniformly sample candidate points to construct a candidate point set;

[0119] No. Candidate point location for:

[0120]

[0121]

[0122] in, Center of the map Point to global target point Direction and UUV heading at the current control moment The included angle; , ;

[0123] Step 6.2: Calculate the gravitational potential field at each candidate point to guide the UUV formation toward the target point. ;

[0124]

[0125] Step 6.3: For the updated rolling probability occupancy grid partial map medium state The grid is used to calculate the value of the grid for candidate points. repulsive potential field ;

[0126]

[0127] in, ;

[0128] All states The grid for candidate points The superposition of repulsive potential fields yields guiding candidate points. Total repulsive potential field away from the obstacle ;

[0129]

[0130] Step 6.4: For each candidate point gravitational potential field With total repulsive potential field The sum of these can be used as the composite potential field. ;

[0131]

[0132] Take the composite potential field from the candidate point set The smallest candidate point is selected as the locally optimal sub-target point. ;

[0133] Step 7: Using the improved IRRT* path planning method, plan the position from the current control time of the pilot UUV. To the local optimal sub-target point The path;

[0134] Step 7.1: Initialize the node set With edge set It is an empty set;

[0135] The current control position of the navigation UUV As the starting node, add it to the node set. ;

[0136] Constructing a random tree ;

[0137] Step 7.2: Generate random sampling points And the random sampling point Located in the updated scrolling probability grid pattern map medium state Within the grid;

[0138] Possible methods:

[0139] Step 7.2.1: Generate random sampling candidate points ;

[0140]

[0141]

[0142] in, and The updated scrolling probability occupies the grid pattern map. Map boundary coordinates; , It is an interval Uniformly distributed independent random numbers;

[0143] Step 7.2.2: If random sampling of candidate points Located in the updated scrolling probability grid pattern map medium state Within the grid, the receiving point is a random sampling point, that is... ;

[0144] Otherwise, discard the random sampling candidate point and return to step 7.2.1;

[0145] Step 7.3: Retrieve the node set Mid-range random sampling points The nearest node is denoted as the neighboring node. ;

[0146] After the update, the scrolling probability occupies the grid pattern partial map. Inside, from the state Take the nearest node in the grid The nearest grid cell is denoted as the neighboring grid cell. Get neighboring nodes To the adjacent grid distance ;

[0147] neighboring nodes To the adjacent grid distance With step size switching distance threshold Compare and determine the expansion step size ;

[0148]

[0149] From adjacent nodes To random sampling points Direction, to expand step size Generate new nodes ;

[0150]

[0151] Combination Figure 5 Introduction: Formation-constrained collision detection; Compared to path planning for a single UUV, path planning for a UUV formation is more complex. In single UUV path planning, only the size of the UUV itself and the collision with obstacles need to be considered. However, in UUV formation collision avoidance, it is necessary not only to ensure that the lead UUV can avoid obstacles, but also to consider the scale of the entire formation to ensure that the following UUVs do not collide during the collision avoidance process.

[0152] Step 7.4: Calculate the pilot UUV's movement from neighboring nodes Move to new node Change of course :

[0153]

[0154] Based on the formation constraints of the UUV formation, the calculation is performed when the lead UUV is at the new node. At that time, the desired position of each following UUV ;

[0155]

[0156]

[0157] Step 7.5: Set the navigation UUV route segment as follows Each UUV path segment is ;

[0158] For each path segment, iterate through all the grids that the path segment passes through. If the state of all grids is... If so, the path segment is determined to be collision-free;

[0159] If the UUV is piloting the route segment and all UUV-following path segments If there are no collisions, then the new node will be... Add to node set , path segment As an edge set Proceed to step 7.6;

[0160] Otherwise, abandon the new node. Return to step 7.2;

[0161] Step 7.6: If the new node With the local optimal sub-target point The distance is less than the radius of influence of the target point. Then from the new node Start, along the random tree By tracing back sequentially to the starting node, the position from the current control time of the pilot UUV can be determined. To the local optimal sub-target point path ;

[0162] Otherwise, return to step 7.2;

[0163] Step 8: The pilot UUV will navigate the route As the desired trajectory, it guides the UUV formation to perform path tracking;

[0164] Leading UUVs will take the path As the desired trajectory, the path tracking controller calculates the desired forward velocity of the pilot UUV. With expected heading And drive its own movement;

[0165] The lead UUV broadcasts its motion status information to all following UUVs at a fixed frequency via an underwater acoustic communication module. ;

[0166] After receiving the information transmitted by the lead UUV, each following UUV determines its desired position according to the formation constraints of the UUV formation and sets the desired heading of the lead UUV. As its desired heading, based on its current position at the moment of control. It calculates the propulsion force and rudder angle control commands used to drive its own motion, and achieves coordinated movement and formation maintenance with the lead UUV;

[0167] Step 9: The UUV formation performs path tracking until the next control moment, i.e. At any time, if the UUV formation has not reached the target point radius of influence If the result is within the specified range, return to step 2.

[0168] While a single UUV can plan its path using only current forward-looking sonar information, in formation collision avoidance, information from a single moment might mean the navigator has avoided an obstacle while the followers have failed to do so in sync. To ensure formation coordination, local path planning needs to integrate historical environmental data; however, recording all detection information would significantly increase the data volume. Therefore, a rolling probabilistic occupancy grid map is adopted, achieving efficient environmental data management through dynamic updates, maintaining high resolution while reducing storage pressure.

[0169] Figure 6 This demonstrates the overall effectiveness of the pilot UUV's local coordination planning based on the improved IRRT* algorithm, taking into account the overall formation, in an environment with unknown obstacles. The image clearly shows that the formation maintained a stable and compact formation throughout its movement and ultimately reached the target point without collision.

[0170] Figure 7 This figure shows the local coordination planning results of a navigation UUV based on the improved IRRT* algorithm in a grid map environment at time T=50s. The figure shows that the sub-target point in the local grid map falls on an obstacle in the global map because the current sonar did not detect the obstacle due to its detection range. Figure 8 As the pilot UUV moves, the grid map begins to scroll, local sub-target points are updated again, and the path continues to change.

[0171] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A deliberate formation collision avoidance method based on local coordination planning of a navigation UUV, characterized in that: The pilot UUV emits beams horizontally through the bow multibeam sonar to obtain the detection distance of each beam to the obstacle. Based on the position and heading of the pilot UUV at the current control time, the position of each obstacle detection point is calculated. Calculate the distance between the current position of the pilot UUV at the control moment and the center of the map of the scroll probability grid partial map updated at the previous control moment. If the distance is greater than the distance threshold of the map window scroll, the current position of the pilot UUV at the control moment is used as the updated map center; otherwise, the updated map center position remains unchanged. Based on the updated map center, update the scroll probability occupancy grid partial map; for grids that exceed the area before the update, initialize the posterior probability that the grid is occupied by an obstacle; for grids in the overlapping area between the updated and the area before the update, determine the obstacle detection point closest to the grid center, calculate the distance difference and angle difference between the grid and the corresponding beam of the obstacle detection point, and then update the posterior probability that the grid is occupied by an obstacle. Based on the posterior probability of each grid cell in the updated scroll probability occupancy grid partial map being occupied by obstacles, determine the state of each grid cell, including whether it is occupied by obstacles or not. Using the map center of the updated scroll probability-occupied grid local map as the center, candidate points are uniformly sampled from the map center towards the global target point based on the search radius of the candidate point subset, constructing a candidate point set. For each candidate point, based on the virtual potential field method, the gravitational potential field guiding the UUV formation towards the global target point and the total repulsive potential field of all grids occupied by obstacles are calculated. The sum of the gravitational potential field and the total repulsive potential field is taken as the synthetic potential field. The candidate point with the smallest synthetic potential field in the candidate point set is taken as the local optimal sub-target point. An improved IRRT* path planning method is adopted to plan a path from the current position of the lead UUV to the local optimal sub-target point; the path planning result is used as the desired trajectory, and the UUV formation performs path tracking until the next control time; Repeat the above steps until the UUV formation reaches the influence radius of the global target point.

2. The deliberate formation collision avoidance method based on local coordination planning of a navigation UUV as described in claim 1, characterized in that: The improved IRRT* path planning method is used to plan the path from the current control position of the pilot UUV to the local optimal sub-target point, specifically as follows: Construct a random tree. In each iteration, generate random sampling points located in grid cells that are not occupied by obstacles. Take the node closest to the random sampling point from the node set of the random tree and call it the neighboring node. Obtain the grid cell that is occupied by obstacles closest to the neighboring node and call it the neighboring grid cell. Based on the distance from neighboring nodes to neighboring grids, the expansion step size is determined, and new nodes are generated from the neighboring nodes toward the random sampling points with the expansion step size. Calculate the heading change of the lead UUV as it moves from a neighboring node to a new node. Based on the formation constraints of the UUV formation, determine the expected positions of each following UUV when the lead UUV is at the new node. For the path segment of the lead UUV from a neighboring node to the new node, and the path segment of each following UUV from its current position to its expected position, traverse all the grids traversed by each path segment. If all grids are in a state where they are not occupied by obstacles, then add the new node to the node set of the random tree, and add the path segment of the lead UUV from a neighboring node to the new node as an edge to the edge set of the random tree. Repeat the above steps until the distance between the new node and the local optimal sub-target point is less than the radius of influence. At this point, start from the new node and backtrack along the random tree until the starting node is reached, forming a path from the position of the pilot UUV at the current control time to the local optimal sub-target point.

3. The deliberate formation collision avoidance method based on local coordination planning of a navigation UUV as described in claim 1, characterized in that: The process of using the path planning result as the desired trajectory and the UUV formation performing path tracking is as follows: The pilot UUV calculates the desired forward speed and desired heading based on the path planning results, and constructs the pilot UUV's current position, desired forward speed, and desired heading into an information packet. The lead UUV broadcasts information packets to all following UUVs via an underwater acoustic communication module; After receiving the information packet, each following UUV will take the desired heading of the lead UUV as its own desired heading. Based on the formation constraints of the UUV formation and its own position at the current control moment, it will calculate the propulsion force and rudder angle control commands to drive its own movement, thereby achieving coordinated movement and formation maintenance with the lead UUV.

4. The deliberate formation collision avoidance method based on local coordination planning of a pilot UUV as described in claim 1, characterized in that: The distance threshold for scrolling the map window Combining the scale of the UUV formation with the maximum detection range setting of the multibeam sonar: in, The width of the UUV formation; The length of the UUV formation; This represents the farthest detection range for multibeam sonar.

5. The deliberate formation collision avoidance method based on local coordination planning of a pilot UUV as described in claim 1, characterized in that: The pilot UUV emits beams horizontally via a bow multibeam sonar to obtain the detection range of obstacles detected by each beam. Based on the current control position of the pilot UUV With bow Calculate the position of each obstacle detection point. : in, For the first The angle of the beam relative to the heading of the pilot UUV; According to the updated map center Update the rolling probability occupancy grid partial map. After the update, the center position of each grid cell in the rolling probability occupancy grid partial map is... : Among them, the rolling probability occupies the composition of the grid in the partial map as follows: ; To occupy the first grid pattern in the map with rolling probability Line number Index of the column grid; The side length of the grid; The grid in the overlapping area between the updated and unupdated regions. Determine the obstacle detection point closest to the center of the grid. Calculate the beam corresponding to the grid relative to the obstacle detection point. Distance difference and angle difference : This updates the posterior probability that the grid cell is occupied by an obstacle. : in, To update the probability of scrolling within the grid in the partial map, the grid is occupied. The posterior probability of being blocked by an obstacle; For grid The likelihood probability of being occupied by obstacles. For grid The likelihood probability that the area is not occupied by obstacles; in, , , These are the weighting coefficients. ; and For uncertainty parameters in angle and distance measurements; System error compensation for sonar distance measurement.

6. The deliberate formation collision avoidance method based on local coordination planning of a navigation UUV as described in claim 1, characterized in that: The updated rolling probability occupies the grid pattern partial map. map center Using the candidate point subset as the center, search radius From the center of the map Point to global target point Starting from the direction, uniformly sample candidate points to construct a candidate point set. Candidate point location for: in, Center of the map Point to global target point Direction and UUV heading at the current control moment The included angle; The search angle interval; For each candidate point, the gravitational potential field guiding the UUV formation toward the global target point is calculated based on the virtual potential field method. : in, This is the gravitational gain coefficient; Calculate the total repulsive potential field of the candidate point on all grid cells whose state is occupied by obstacles. : in, Represents a grid The state is occupied by an obstacle; This is the repulsive force gain coefficient; The threshold value is affected by the repulsive potential field. For the first Candidate points and grid Distance from the center.

7. The deliberate formation collision avoidance method based on local coordination planning of a pilot UUV as described in claim 2, characterized in that: Based on neighboring nodes To the adjacent grid distance Determine the expansion step size : in, This is the maximum step size; Minimum step size; The step size is used to switch the distance threshold; From adjacent nodes To random sampling points Direction, to expand step size Generate new nodes : The calculation of the navigation UUV from the nearest node Move to new node Change of course : Based on the formation constraints of the UUV formation, the lead UUV is located at the new node. The desired position of each UUV at the time : in, For the first The relative distance between the following UUV and the lead UUV; For the first The relative positions of the following UUV and the leading UUV.

8. A computer device, comprising a memory, a processor, and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 7.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When executed by a processor, the computer program implements the steps of the method according to any one of claims 1 to 7.

10. A computer program product comprising computer instructions, characterized in that: When executed by a processor, the computer instructions implement the steps of the method according to any one of claims 1 to 7.

Images