Autonomous underwater vehicle dense cluster motion planning method, device and system

Abstract

The application relates to the technical field of autonomous underwater robots, and particularly discloses an autonomous underwater robot dense cluster motion planning method, device and system, which comprises the following steps: obtaining AUV individual expected trajectory information issued by an upper computer; analyzing the AUV individual expected trajectory information; performing AUV individual motion according to an AUV individual initial motion trajectory, and monitoring the formation transformation of an AUV group in real time; when it is determined that the formation transformation of the AUV group is the formation transformation of the AUV group to a known obstacle, keeping the AUV individual initial motion trajectory unchanged; and when it is determined that the formation transformation of the AUV group is the formation transformation of the AUV group to an unknown obstacle, performing autonomous obstacle avoidance motion according to an AUV group formation transformation scene. The autonomous underwater robot dense cluster motion planning method can improve the defects of poor global performance, low fault tolerance and easy deviation transmission of the motion planning of the autonomous underwater robot dense cluster.

Description

Technical Field

[0001] This invention relates to the field of autonomous underwater robot technology, and in particular to a method, device, and system for planning motion in dense swarms of autonomous underwater robots. Background Technology

[0002] Autonomous Underwater Vehicles (AUVs) are advanced marine exploration equipment characterized by their wide detection range and high degree of autonomy.

[0003] To further expand the operational range of autonomous underwater vehicles (AUVs), swarm operations of multiple AUVs are now the mainstream application. Based on the spatial distribution and degree of cooperation among the robots, AUV swarms can be divided into loose swarms and dense swarms. In loose swarms, the distance between individual robots is relatively large (usually more than 100 times their own length), communication and cooperation are relatively weak, and the requirements for swarm formation precision are not high. For example, in large-scale marine environmental monitoring tasks, robots in a loose swarm can be dispersed in different areas to collect parameters such as seawater temperature, salinity, and dissolved oxygen, and then periodically summarize the data. This swarm method is highly flexible, requires strong autonomy from individual robots, and is suitable for large-area, low-precision collaborative tasks. Dense swarms, on the other hand, have robots that are spatially closely distributed (usually 1 to 10 times their own length) and achieve strong coupling and cooperation through frequent and efficient communication. Dense swarms are advantageous when performing tasks with extremely high requirements for collaborative precision, such as detailed mapping of seabed targets and collaborative operations in confined spaces.

[0004] Currently, motion planning and control methods suitable for dense swarms of autonomous underwater vehicles (AUVs) are mainly divided into three types: behavior-based methods, leader-follower methods, and virtual structure methods. Among them, the core of behavior-based swarm control is to decompose the complex swarming task into multiple basic behavioral rules. By weighting or prioritizing these behaviors, cooperative movement of multiple AUVs is achieved. Its advantage lies in the absence of a central controller; each AUV makes autonomous decisions based solely on local information and the state of its neighbors, reducing reliance on global communication and computation. The drawbacks of this behavior-based method are that its global performance is difficult to guarantee, and formation errors may accumulate. Furthermore, instructions between different behaviors may contradict each other, requiring complex fusion strategies to resolve these issues. The leader-follower method, on the other hand, designates one or more AUVs in the swarm as "leaders," with the remaining AUVs acting as "followers." These follow preset rules to track the leader's position / trajectory, thus determining the swarm's position and formation. Its advantage is that the overall swarm operation depends on the leader's path planning, while the followers only focus on local relative positions, requiring no global information. The followers have a single control objective, clear algorithm logic, and are easy to implement in engineering. The drawbacks of the pilot-follower method are twofold: firstly, a failure of the pilot can disrupt the entire swarm's spatial and temporal stability, resulting in poor fault tolerance; secondly, followers need to obtain the pilot's status in real time, and communication delays or interruptions can lead to formation collapse. The virtual structure method treats the entire swarm as a rigid or semi-rigid "virtual overall structure," with each AUV abstracted as a "node" on this structure. Formation positioning is achieved by maintaining the relative positions of nodes within the virtual structure. Its advantages include using the virtual structure as a unified formation benchmark, with all AUVs sharing the same reference coordinate system, eliminating the need to distinguish between pilots and followers; and allowing for flexible setting of the swarm's navigation level by adjusting virtual structure parameters (such as node spacing and geometric constraints). However, this virtual structure method has drawbacks: firstly, deviations from individual AUVs can be transmitted through the virtual structure, affecting overall stability; localized failures are prone to propagation; and secondly, it requires real-time global status of the virtual structure or precise positions of neighboring nodes, with information delays leading to decreased control performance.

[0005] Given the shortcomings of existing motion planning and control methods for dense swarms of autonomous underwater robots, how to improve the poor global performance, low fault tolerance, and easy propagation of deviations in motion planning for dense swarms of autonomous underwater robots has become a technical problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0006] This invention provides a motion planning method, a motion planning device, and a motion planning system for dense swarms of autonomous underwater robots, solving the problems of poor global performance, low fault tolerance, and easy propagation of first-order deviations in motion planning for dense swarms of autonomous underwater robots in related technologies.

[0007] As a first aspect of the present invention, a method for motion planning in a dense swarm of autonomous underwater vehicles (AUVs) is provided, wherein each individual AUV in the AUV swarm is capable of communicating with a host computer in the initial state of the AUV swarm, the initial state of the AUV swarm being when the AUV swarm is floating on the water surface and has not yet entered the water. The method for motion planning in a dense swarm of autonomous underwater vehicles includes:

[0008] The host computer obtains the expected trajectory information of individual AUVs from the host computer. The host computer can perform group trajectory planning to obtain virtual trajectory information of the group based on the initial position of each individual AUV in the group, the geometric center point of the group, the number of individual AUVs in the group, and preset task-related information. Based on the virtual trajectory information of the group, the host computer performs trajectory planning for individual AUVs to obtain the expected trajectory information of individual AUVs.

[0009] Analyze the expected trajectory information of individual AUVs to obtain the initial movement trajectory of individual AUVs. The initial movement trajectory of individual AUVs includes at least the position of individual AUVs in the formation of AUV groups and the movement speed of individual AUVs.

[0010] The movement of individual AUVs is performed based on their initial movement trajectories, and the formation changes of the AUV group during the movement of individual AUVs are monitored in real time. The formation changes of the AUV group include at least the formation changes of the AUV group in response to known obstacles and the formation changes of the AUV group in response to unknown obstacles.

[0011] When the formation change of the AUV swarm is determined to be a formation change of the AUV swarm in response to a known obstacle, the initial trajectory of each AUV remains unchanged.

[0012] When the formation change of the AUV swarm is determined to be a formation change of the AUV swarm in response to unknown obstacles, autonomous obstacle avoidance movement is performed according to the formation change scenario of the AUV swarm, and the initial movement trajectory of the individual AUV is adjusted according to the feedback of the autonomous obstacle avoidance movement to obtain the updated movement trajectory of the individual AUV.

[0013] Furthermore, when the formation change of the AUV swarm is determined to be a formation change performed by the AUV swarm in response to an unknown obstacle, autonomous obstacle avoidance maneuvers are performed based on the AUV swarm formation change scenario, including:

[0014] When it is determined that the formation change of the AUV group is a formation change of the AUV group in response to an unknown obstacle, the current AUV group formation change scenario is determined. The AUV group formation change scenario includes at least: the overall formation remains unchanged, the basic formation remains unchanged but the formation spacing changes, and the formation change itself.

[0015] If the current AUV group formation change scenario is such that the overall formation remains unchanged, then autonomous obstacle avoidance is achieved through trajectory replanning.

[0016] If the current AUV group formation change scenario is that the basic formation remains unchanged and the formation spacing changes and / or the formation changes, then the trajectory replanning result of the AUV group's decision-maker is received. The decision-maker of the AUV group is the AUV individual in the AUV group that is close to the geometric center point of the AUV group. The decision-maker of the AUV group can change and adjust the current formation according to the target formation to achieve trajectory replanning.

[0017] Furthermore, the current AUV group formation change scenario is determined, including:

[0018] The scene type of the current AUV formation change is determined based on the obstacle core parameters, the relative relationship between the AUV group and the obstacle, and the state of the AUV group itself. The obstacle core parameters include at least the obstacle's three-dimensional coordinates, obstacle volume, obstacle quantity, and obstacle motion state.

[0019] If the obstacle core parameters, the relative relationship between the AUV group and the obstacle, and the AUV group's own state satisfy the first constraint, then the scenario type of the current AUV group formation change is determined to be "overall formation unchanged". The first constraint includes simultaneously satisfying the first constraint condition of the obstacle core parameters, the first constraint condition of the relative relationship between the AUV group and the obstacle, and the first constraint condition of the AUV group's own state. Specifically, the first constraint condition of the obstacle core parameters includes the diameter of an AUV individual whose projected area on the flight path is ≤1.5 times that of the obstacle when it is a single obstacle, and the cruising speed of an AUV individual when the obstacle is static or dynamic and its speed is ≤0.5 times that of the obstacle. The first constraint condition of the relative relationship between the AUV group and the obstacle includes the width of the AUV group whose shortest distance between the obstacle and the AUV group is ≥1.5 times that of the obstacle group, and the expected trajectory information of AUV individuals whose obstacle coverage area is ≤2 AUV individuals. The first constraint condition of the AUV group's own state includes maintaining the actual distance between adjacent AUV individuals within the effective communication range after the AUV individual affected by the obstacle autonomously adjusts its local trajectory.

[0020] If the obstacle's core parameters, the relative relationship between the AUV group and the obstacle, and the AUV group's own state all simultaneously satisfy the second constraint, then the scenario type of the current AUV group formation change is determined to be: basic formation unchanged, formation spacing changing. The second constraint includes simultaneously satisfying the second constraint condition of the obstacle's core parameters, the second constraint condition of the relative relationship between the AUV group and the obstacle, and the second constraint condition of the AUV group's own state. The second constraint condition of the obstacle's core parameters includes the sum of the diameters of individual AUVs whose projected area on the flight path is ≤ M / 2, where M represents the number of AUVs in the AUV group. The number of individuals; whether the obstacle is static or dynamic and its trajectory is predictable; the second constraint on the relative relationship between the AUV swarm and the obstacle includes the shortest distance between the obstacle and the AUV swarm being between 0.5 and 1.5 times the width of the AUV swarm, and the expected trajectory information of the AUV individuals for the obstacle coverage area ≤ M / 2 units; the second constraint on the state of the AUV swarm itself includes the ability to completely avoid obstacles by adjusting the spacing between AUV individuals while maintaining the current formation, and the adjusted spacing between AUV individuals being greater than or equal to the minimum safe distance and less than or equal to the effective communication distance.

[0021] If the obstacle core parameters, the relative relationship between the AUV group and the obstacle, and the AUV group's own state satisfy the third constraint, then the scenario type of the current AUV group formation change is determined to be a formation change. The third constraint includes satisfying one of the following: the third constraint condition of the obstacle core parameters, the third constraint condition of the relative relationship between the AUV group and the obstacle, and the third constraint condition of the AUV group's own state. Specifically, the third constraint condition of the obstacle core parameters includes: the projected area of ​​the obstacle on the flight path > the sum of the diameters of M / 2 AUV individuals; there are multiple obstacles and the distance between them is < the minimum adjustable width of the current formation; the obstacle is static or dynamic and its speed is > 0.5 times the cruising speed of the individual AUVs; the third constraint condition of the relative relationship between the AUV group and the obstacle includes: the shortest distance between the obstacle and the AUV group is less than 0.5 times the width of the AUV group; and the third constraint condition of the AUV group's own state includes: the AUV group after the formation change can completely avoid the obstacle.

[0022] Furthermore, the decision-maker of the AUV swarm can adjust the current formation according to the target formation to achieve trajectory replanning, including:

[0023] The geometric center point of the new formation and the new target position of each AUV in the new formation are generated based on the target formation. The transition reference point position of each AUV in the current formation is determined. The transition reference point position of each AUV is located at 1 / 2 offset from the line connecting the current position of each AUV and the new target position of that AUV.

[0024] The transition path of each AUV is generated according to the order from its current position to its transition reference point and from its transition reference point to its new target position, and the trajectory replanning result of each AUV is obtained.

[0025] Trajectory replanning results of any two AUVs in the AUV swarm are traversed to pre-verify the risk of spatiotemporal collision;

[0026] Once the space-time collision risk verification is passed, the trajectory replanning result is sent to each individual AUV in the AUV group at once. The trajectory replanning result includes at least the path parameters, velocity curve and safety threshold of the individual AUV.

[0027] Furthermore, the trajectory replanning results of any two AUVs in the AUV swarm are iterated to pre-verify the risk of spatiotemporal collision, including:

[0028] Determine whether the minimum distance between the two paths of any two AUVs is greater than or equal to the preset safety distance, or determine whether the time difference of the approach segment of any two AUVs is greater than or equal to the preset safety time difference.

[0029] If the conditions are not met, the new target velocity in the trajectory replanning result of the current AUV individual will be adjusted until all verifications pass.

[0030] Furthermore, when the formation change of the AUV swarm is determined to be a formation change performed by the AUV swarm in response to an unknown obstacle, the autonomous obstacle avoidance movement based on the AUV swarm formation change scenario also includes:

[0031] Autonomous path following is performed based on the trajectory replanning results issued by the decision-makers of the AUV group;

[0032] Determine whether the new target position has been reached;

[0033] If a new target location is reached, switch to formation hold mode.

[0034] Furthermore, the movement of the AUV individual is performed based on its initial trajectory, including:

[0035] Using the expected location in the expected trajectory information of the AUV individual as the center, the tracking area of ​​the AUV individual is divided to obtain a consistency zone, an adjustment zone, and a warning zone. The radius of the consistency zone is smaller than the radius of the adjustment zone, and the radius of the adjustment zone is smaller than the radius of the warning zone.

[0036] Different feedback tracking controls are applied to different tracking areas.

[0037] Furthermore, different feedback tracking controls are implemented for different tracking areas, including:

[0038] The comparison is made based on the interval between the actual current position and the expected current position of the AUV individual, and the area radius of each tracking region.

[0039] If the distance between the actual position of the AUV at the current moment and its expected position at the current moment is not greater than the radius of the consistency zone, then the tracking effect of the AUV at the current moment is determined to be good.

[0040] If the distance between the actual position and the expected position of the AUV at the current moment is greater than the radius of the consistency zone and less than or equal to the radius of the adjustment zone, then it is determined that the current tracking effect of the AUV is poor, and a heading / pitch / speed adjustment command is generated.

[0041] If the distance between the actual position and the expected position of the AUV at the current moment is greater than the radius of the adjustment zone and less than or equal to the radius of the warning zone, a fault detection command is generated to detect whether the AUV has malfunctioned.

[0042] If the distance between the actual position and the expected position of an AUV at the current moment is greater than the radius of the warning zone, the AUV stops its current task and reports the fault information to the decision-maker. If the current decision-maker of the AUV group fails, the current decision-maker terminates its task, and the AUV closest to the geometric center of the AUV group becomes the new decision-maker. The new decision-maker can move from its original position to the current decision-maker's position to take over the current decision-maker's task.

[0043] As another aspect of the present invention, a motion planning device for dense swarms of autonomous underwater vehicles (AUVs) is provided, wherein each AUV in the AUV swarm is capable of communicating with a host computer in the initial state of the AUV swarm, which is the state in which the AUV swarm floats on the water surface and has not yet entered the water. The motion planning device for dense swarms of autonomous underwater vehicles includes:

[0044] The acquisition module is used to acquire the expected trajectory information of individual AUVs sent by the host computer. The host computer can perform group trajectory planning to obtain virtual trajectory information of the group based on the initial position of each individual AUV in the group, the geometric center point of the group, the number of individual AUVs in the group, and preset task-related information. Based on the virtual trajectory information of the group, the host computer can perform trajectory planning for individual AUVs to obtain the expected trajectory information of individual AUVs.

[0045] The parsing module is used to parse the expected trajectory information of individual AUVs and obtain the initial movement trajectory of individual AUVs. The initial movement trajectory of individual AUVs includes at least the position of individual AUVs in the formation of AUV groups and the movement speed of individual AUVs.

[0046] The individual motion control module is used to control the movement of the AUV individual based on the initial movement trajectory of the AUV individual, and to monitor the formation changes of the AUV group in real time during the movement of the AUV individual. The formation changes of the AUV group include at least the formation changes of the AUV group in response to known obstacles and the formation changes of the AUV group in response to unknown obstacles.

[0047] The maintenance module is used to maintain the initial trajectory of individual AUVs when the formation change of the AUV group is determined to be a formation change of the AUV group in response to a known obstacle.

[0048] The autonomous obstacle avoidance module is used to perform autonomous obstacle avoidance movements based on the formation change scenario of the AUV group when it is determined that the formation change of the AUV group is a formation change of the AUV group in response to unknown obstacles. The module also adjusts the initial movement trajectory of the individual AUVs based on the feedback of the autonomous obstacle avoidance movement to obtain the updated movement trajectory of the individual AUVs.

[0049] As another aspect of the present invention, an autonomous underwater robot dense swarm motion planning system is provided, comprising: a host computer and an AUV swarm, the AUV swarm comprising multiple AUV individuals, each AUV individual being able to communicate with the host computer within a preset communication range, any two adjacent AUV individuals in the AUV swarm being able to communicate with each other within an effective communication distance, and each AUV individual including the autonomous underwater robot dense swarm motion planning device described above.

[0050] The host computer can generate the expected trajectory information of individual AUVs in the initial state of the AUV group, wherein the initial state of the AUV group is the state when the AUV group is floating on the water surface and has not yet entered the water;

[0051] Each AUV's autonomous underwater vehicle (AUV) swarm motion planning device is capable of moving the AUV according to its desired trajectory information and autonomously avoiding obstacles when the formation of the AUV swarm changes.

[0052] The autonomous underwater vehicle (AUV) dense swarm motion planning method provided by this invention analyzes the expected trajectory information of individual AUVs sent by a host computer to obtain the initial motion trajectory of each AUV. Based on the initial motion trajectory of each AUV, the AUVs move, and the formation changes of the AUV group are monitored during the movement of individual AUVs. For formation changes involving known obstacles, the initial motion trajectory of the individual AUVs remains unchanged, while for formation changes involving unknown obstacles, autonomous obstacle avoidance movement is performed, thereby updating the initial motion trajectory of each AUV. This method for planning the motion of dense swarms of autonomous underwater vehicles (AUVs) involves virtual trajectory planning for the AUV swarm via a host computer during its initial state, followed by autonomous obstacle avoidance during underwater operations. This approach enhances fault tolerance while ensuring the overall motion planning of the AUV swarm. Furthermore, because each AUV has its own trajectory and can change formations, deviations are not propagated. Therefore, this method not only achieves overall motion planning for the AUV swarm but also effectively improves fault tolerance and prevents deviation propagation, thereby enhancing the accuracy and efficiency of motion planning for dense swarms of underwater robots and making it suitable for complex underwater operating environments. Attached Figure Description

[0053] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used together with the following detailed description to explain the invention, but do not constitute a limitation thereof.

[0054] Figure 1 The flowchart shows the motion planning method for dense swarms of autonomous underwater robots provided by this invention.

[0055] Figure 2 This invention provides a flowchart for the movement of an AUV individual based on its initial trajectory.

[0056] Figure 3 The flowchart provided by this invention illustrates different feedback tracking controls for different tracking regions.

[0057] Figure 4 This is a flowchart of autonomous obstacle avoidance movement based on AUV group formation changes, provided by the present invention.

[0058] Figure 5 This invention provides a flowchart for determining the current AUV group formation transformation scenario.

[0059] Figure 6 This invention provides a flowchart for transforming and adjusting the current formation based on the target formation to achieve trajectory replanning.

[0060] Figure 7 The structural block diagram of the autonomous underwater robot dense swarm motion planning device provided by the present invention.

[0061] Figure 8 The structural block diagram of the autonomous underwater robot dense swarm motion planning system provided by the present invention is shown. Detailed Implementation

[0062] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0063] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0064] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate for the embodiments of the invention described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0065] This embodiment provides a motion planning method for dense swarms of autonomous underwater vehicles (AUVs), applied to each individual AUV in the swarm. Each individual AUV can communicate with a host computer in the initial state of the swarm, which is when the swarm is floating on the water surface and has not yet entered the water. Figure 1 This is a flowchart of a method for planning the dense swarm motion of autonomous underwater robots according to an embodiment of the present invention, such as... Figure 1 As shown, the method for planning the motion of dense swarms of autonomous underwater robots includes:

[0066] S100. Obtain the expected trajectory information of individual AUVs issued by the host computer. The host computer can perform group trajectory planning to obtain virtual trajectory information of the group based on the initial position of each individual AUV in the AUV group, the geometric center point of the AUV group, the number of AUVs in the AUV group, and preset task-related information. Based on the virtual trajectory information of the group, the host computer can perform trajectory planning for individual AUVs to obtain the expected trajectory information of individual AUVs.

[0067] In this embodiment of the invention, when the AUV swarm is in its initial state, each AUV in the swarm can communicate with the host computer. The initial state of the AUV swarm can be understood as all AUVs floating on the water surface and not yet operating underwater. At this time, the communication distance between the AUVs in the swarm and the host computer is within a preset communication range. Therefore, the host computer can obtain data such as the initial position of each AUV, the geometric center point of the AUV swarm, the number of AUVs in the swarm, and preset task-related information. Based on this information, the host computer performs swarm trajectory planning to obtain swarm virtual trajectory information. Based on the swarm virtual trajectory information, the host computer performs trajectory planning for each AUV to obtain the desired trajectory information for each AUV. The host computer then sends the desired trajectory information of each AUV in the swarm to the corresponding AUV, so that each AUV obtains the desired trajectory information that matches its own.

[0068] In this embodiment of the invention, the individual expected trajectory includes at least the geometric center point of the AUV group, the formation information of the AUV group, the position of the AUV individual in the AUV group formation, and the trajectory information of each AUV individual.

[0069] S200. Analyze the expected trajectory information of individual AUVs to obtain the initial movement trajectory of individual AUVs. The initial movement trajectory of individual AUVs includes at least the position of individual AUVs in the formation of AUV groups and the movement speed of individual AUVs.

[0070] In this embodiment of the invention, the expected trajectory information of the AUV individual is parsed to obtain the initial motion trajectory of the AUV individual. The initial motion trajectory of the AUV individual includes not only the position of the AUV individual in the AUV group formation, but also the movement speed of the AUV individual, and may also include the expected posture and expected position of the AUV individual, etc.

[0071] S300. Perform AUV individual movement based on the initial movement trajectory of the AUV individual, and monitor the formation change of the AUV group in real time during the movement of the AUV individual. The formation change of the AUV group includes at least the formation change of the AUV group in response to known obstacles and the formation change of the AUV group in response to unknown obstacles.

[0072] It should be understood that, based on the above-mentioned initial movement trajectory of individual AUVs, the movement process of individual AUVs is controlled, and the formation changes of the AUV group are monitored in real time during the movement of individual AUVs. It should be noted that the formation changes of the AUV group include at least the formation changes of the AUV group in response to known obstacles and the formation changes of the AUV group in response to unknown obstacles.

[0073] S400. When the formation change of the AUV group is determined to be a formation change of the AUV group in response to a known obstacle, the initial movement trajectory of the individual AUV remains unchanged.

[0074] It should be understood that the known obstacles are those that the host computer has already known in advance and have been planned accordingly in the expected trajectory of the group. Therefore, the formation change for the known obstacles is part of the pre-planning, so the initial movement trajectory of the corresponding AUV individuals can be kept unchanged and the movement can continue.

[0075] S500: When it is determined that the formation change of the AUV group is a formation change of the AUV group in response to an unknown obstacle, autonomous obstacle avoidance movement is performed according to the formation change scenario of the AUV group, and the initial movement trajectory of the AUV individual is adjusted according to the feedback of the autonomous obstacle avoidance movement to obtain the updated movement trajectory of the AUV individual.

[0076] It should be understood that formation changes for unknown obstacles can be achieved through autonomous obstacle avoidance. After autonomous obstacle avoidance, the initial trajectory of the AUV is adjusted based on the autonomous obstacle avoidance movement, thereby obtaining the updated trajectory of the AUV. Subsequently, the AUV moves according to the updated trajectory of the AUV.

[0077] Therefore, the autonomous underwater vehicle (AUV) dense swarm motion planning method provided by this invention analyzes the expected trajectory information of individual AUVs sent by the host computer to obtain the initial motion trajectory of each AUV. Based on the initial motion trajectory of each AUV, the AUVs move, and the formation changes of the AUV group are monitored during the movement of individual AUVs. For formation changes involving known obstacles, the initial motion trajectory of the individual AUVs remains unchanged, while for formation changes involving unknown obstacles, autonomous obstacle avoidance movement is performed, thereby updating the initial motion trajectory of each AUV. This method for planning the motion of dense swarms of autonomous underwater vehicles (AUVs) involves virtual trajectory planning for the AUV swarm via a host computer during its initial state, followed by autonomous obstacle avoidance during underwater operations. This approach enhances fault tolerance while ensuring the overall motion planning of the AUV swarm. Furthermore, because each AUV has its own trajectory and can change formations, deviations are not propagated. Therefore, this method not only achieves overall motion planning for the AUV swarm but also effectively improves fault tolerance and prevents deviation propagation, thereby enhancing the accuracy and efficiency of motion planning for dense swarms of underwater robots and making it suitable for complex underwater operating environments.

[0078] In this embodiment of the invention, before the mission begins, i.e. when the AUV swarm is in its initial state, the AUV swarm floats on the water surface, and each AUV is connected to the host computer. The host computer first obtains the positions of the individual AUVs, and then calculates the geometric center point of the AUV swarm by combining the positions of each individual AUV in the swarm. Afterwards, using the geometric center point of the AUV swarm, the number of AUVs, mission instructions (mission type, formation information), motion constraints, and known obstacle points, a swarm trajectory is planned to generate a three-dimensional virtual trajectory with swarm width and time information, i.e., the swarm virtual trajectory information.

[0079] Specifically, the host computer can calculate the geometric center point of the AUV swarm based on the initial position of each individual AUV. The calculation process is as follows: Since the initial state of the AUVs is floating on the water surface and they are basically in the same plane, the position of each individual AUV relative to the reference point is first obtained and calculated. Then, determine the vertices of the convex polygon: use the Graham scan method or the monotonic chain method to select the vertices of the convex polygon from each coordinate point. Taking the monotonic chain method as an example, sort by the x-coordinate and construct the convex polygon in two chains, upper and lower. For example, if the AUV coordinate points are (1,3), (2,1), (3,2), (4,4), (0,2), its convex polygon vertices are arranged in the order of (0,2), (2,1), (4,4), (1,3). Based on the above coordinate information, calculate the geometric center point (centroid coordinates) of the AUV group. Specifically, the vertex coordinates of the polygon are recorded clockwise or counterclockwise as follows: Its centroid coordinates The calculation formula is:

[0080] ,

[0081] ,

[0082] in, The area of ​​a polygon is expressed by the following formula: ; (A closed polygon, where the last vertex is connected to the first vertex).

[0083] After obtaining the geometric center point of the AUV swarm through the above calculations, the swarm width is calculated. In this embodiment of the invention, the formation of the AUV swarm may specifically include a triangular formation, a transverse formation, a longitudinal formation, and a rhomboid formation. Specifically, the swarm width of a triangular formation is the sum of the distance between the two outermost AUV individuals and the diameter of the warning zone; the swarm width of a transverse formation is the sum of the distance between the two outermost AUV individuals and the diameter of the warning zone; the swarm width of a longitudinal formation is the same as the diameter of the warning zone; and the swarm width of a rhomboid formation is the sum of the distance between the two outermost AUV individuals and the diameter of the warning zone.

[0084] Finally, the virtual trajectory of the group is planned.

[0085] a. Problem definition and constraint setting. Speed ​​constraint, maximum speed. minimum speed Acceleration constraint: linear acceleration angular acceleration Attitude constraints: Maximum pitch angle Maximum yaw angle Minimum turning radius The path is continuously differentiable: position, velocity, and acceleration are continuously differentiable.

[0086] b. Input for trajectory planning. Starting point and target point: 3D coordinates and ,in Depth information; Environmental information: 3D coordinates and dimensions of obstacles (including safety buffer zones); Mission and group information: Formation type and spacing; Speed ​​reference: Cruising speed. (Cruising speed referenced during formation operations).

[0087] c. Geometric Path Generation. A 3D A* algorithm is used to search for discrete keypoint paths from the starting point to the target point in 3D space. Obstacle Avoidance Logic: The trajectory width is set according to the "maximum formation width during task execution" to ensure the entire group passes through without colliding with obstacles. Trajectory Smoothing: For the generated "initial geometric path" composed of a series of discrete 3D coordinate points, a Bézier curve is used to fit the initial discrete path to eliminate path inflection points and ensure that the path meets the requirements of continuous differentiability of position, velocity, and acceleration, thus obtaining a smooth "continuous geometric path". ,in This represents the path arc length parameter, containing only spatial information.

[0088] It should be noted that, in addition to using the 3D A* algorithm to generate discrete paths, the 3D RRT* (Fast Explore Random Tree Star algorithm) can also be used. RRT, by randomly sampling spatial nodes and optimizing path costs, can also meet obstacle avoidance requirements and motion constraints such as speed and acceleration, and is more adaptable to scenarios with complex obstacle distributions. Ultimately, it can generate discrete key point paths that are functionally equivalent to the A* algorithm.

[0089] Alternatively, B-spline curves can be used to achieve trajectory smoothing. B-spline curves are defined by control vertices and node vectors, which can also ensure the continuous differentiability of position, velocity, and acceleration, and have better local adjustability (modifying a single control vertex does not affect the overall curve), thus achieving the same trajectory smoothing effect.

[0090] d. Velocity planning and time parameterization. Using geometric paths... Based on this, design the velocity profile Among them, velocity profile On the one hand, it satisfies all the above dynamic constraints, and on the other hand, it controls the cruising speed. The target speed is used as the velocity profile; the velocity profile is integrated, and the arc length parameter is... Convert to time parameters ,Right now ;Will Substitute geometric path To obtain the complete three-dimensional spatiotemporal trajectory This complete three-dimensional spatiotemporal trajectory is the virtual trajectory information of the group.

[0091] Based on the obtained virtual trajectory information of the group, the expected trajectory information of individual AUVs is generated. Specifically, firstly, the group trajectory is discretized according to the formation type, the number of AUVs, and the spacing between AUVs, based on the virtual trajectory of the group; secondly, the trajectory is allocated according to the initial position information of the AUVs in the group and the starting position information of the cluster formation to obtain a collision-free trajectory plan from the initial position to the starting position. Finally, each AUV has a complete trajectory information from the initial position to the task position; finally, the geometric center point of the AUV group, the formation information, the position of the AUV in the formation, and the trajectory information of each AUV are sent to the designated AUV.

[0092] More specifically, 1) Trajectory discretization. In the generated virtual trajectories with a group width, the trajectory is discretized according to the task type, the number of AUV individuals, and the spacing between individuals to generate a number of sub-trajectories equal to the number of AUV individuals. 2) Point allocation. By establishing an energy consumption evaluation function, optimal energy is achieved when transforming from the initial position to the preset position. The specific steps are as follows:

[0093] a. Analysis and Modeling. If there are M individuals in the population, the initial positions of the M AUV individuals are denoted as:

[0094] ,

[0095] in, Indicates the first The location coordinates of each AUV individual.

[0096] The starting positions of the M desired trajectories generated by discretization are denoted as:

[0097] ,

[0098] in, Indicates the first The initial position of the desired trajectory.

[0099] Construct mapping function , Indicates the first Individual movement to the first The allocation scheme for the starting positions of the trajectory exists; therefore, there exists an optimal allocation scheme. This allows all individuals to... Move to The total energy consumption is minimized.

[0100] b. Construction of the energy consumption evaluation function. If using... Let represent the energy consumed by a single composite object moving from point A to point B. Then the total energy consumed is... ,in,

[0101] ,

[0102] in, Indicates individual quality; This represents the Euclidean distance from point A to point B; This represents the individual's movement speed; the target speed set above is used here. It means "directly proportional";

[0103] The energy consumption evaluation function can then be expressed as follows:

[0104] ,

[0105] This energy consumption evaluation function can well reflect that the greater the distance traveled, the faster the speed, and the greater the mass, the higher the energy consumption.

[0106] It should be noted that a water flow resistance factor can also be added to the above energy consumption evaluation function, i.e. , ( (representing the water flow resistance coefficient), this function also reflects the core principle that "the farther the distance, the faster the speed, and the greater the mass, the higher the energy consumption," and is more in line with the actual underwater environment, achieving the same optimal energy consumption allocation goal.

[0107] c. Optimal Allocation Scheme Determining the optimal allocation scheme is essentially an assignment problem, which can be solved efficiently using the Hungarian algorithm.

[0108] First, construct the cost matrix. ( A matrix representing the number of AUV individuals in an AUV population. Matrix elements Indicates the first The expected trajectory is assigned to the first The cost of an individual AUV , where the matrix The expression is:

[0109] ,

[0110] Cost matrix Substituting this into the Hungarian algorithm yields an assignment scheme with the minimum total cost, which is... .

[0111] It should be noted that, in addition to the Hungarian algorithm, a genetic algorithm can also be used. The genetic algorithm searches for the optimal allocation scheme through operations such as encoding, crossover, and mutation, and can also minimize total energy consumption. It is suitable for scenarios with a large number of AUVs and is functionally equivalent to the Hungarian algorithm, differing only in its computational logic.

[0112] 3) After the mission begins, the movement trajectories of AUVs in the AUV group from their initial positions to the starting position of the formation are planned to ensure no collisions.

[0113] Specifically, the initial positions of all AUVs are planned according to cubic spline curves. To the starting position of the formation If the paths intersect, check if the time difference between the intersections is large enough. If it is, depart simultaneously; if not, fine-tune the speed of one of the AUVs. ~ Within a certain range, they are staggered in the time dimension at the intersection point to ensure collision-free movement. Specific steps include:

[0114] a. Define known parameters of the AUV: maximum linear acceleration Target speed Minimum speed Minimum turning radius Spatial safety distance (Can be defined as twice the AUV width), calculate critical time Critical distance .

[0115] b. Define three cubic spline control points for each AUV individual: starting point Right now ,end Right now Intermediate control points ,lie in and On the perpendicular bisector of the line, the initial offset distance The offset direction is the direction away from the group center point or the vertical direction of the initial orientation.

[0116] c. Generate a cubic spline curve based on 3 control points, and verify that the radius of curvature of all points on the curve is ≥ If the conditions are not met, increase the offset distance. .

[0117] It should be noted that, in addition to cubic spline curves, embodiments of the present invention can also employ spiral curves. The curvature of a spiral curve changes linearly with the arc length, better matching the motion characteristics of an AUV transitioning from straight-line travel to uniform-speed turning, and can also satisfy the minimum turning radius constraint. The collision-free core still employs "spatiotemporal staggered peaks," only the path curve type differs, achieving the same collision-free motion effect.

[0118] d. Calculate the total arc length of the path and path intersections Corresponding arc length segment Calculate arrival time based on different scenarios actual time ;when hour, ;when hour, .

[0119] e. Traverse all paths. If the paths have no intersection, AUV will start simultaneously; if there is an intersection, verification will be performed. ( If the conditions are not met, the acceleration or constant velocity is finely adjusted to ensure that there are no collisions in spacetime.

[0120] In this embodiment of the invention, if the conditions are not met, the duration of the acceleration phase can be adjusted to ensure that there is no collision in spacetime.

[0121] 4) Send specific information to the designated AUV. As mentioned above, this is done after obtaining the optimal allocation scheme. After that, the The AUV will be assigned to the first The desired trajectory, during which the AUV is internally numbered, will yield a unique result. Then, the group's center coordinates, formation type, and desired trajectory can be sent to the designated AUV.

[0122] After each AUV receives the expected trajectory information from the host computer, it parses the expected trajectory information to obtain the initial motion trajectory of the AUV.

[0123] In this embodiment of the invention, as can be seen from the above, the trajectory of an AUV group can be described as follows: Then the first The expected trajectory of an individual AUV is represented as follows:

[0124] ,

[0125] in, This indicates the desired location of the AUV individual; This represents the desired pose of the AUV individual, where To indicate the roll angle, generally let ; Indicates pitch angle, , Indicates the yaw angle. ,in , as well as Both represent linear velocity, and , as well as .

[0126] In this embodiment of the invention, the movement of the AUV is performed based on the initial movement trajectory of the AUV, such as... Figure 2 As shown, it includes:

[0127] S310. Using the expected point location in the expected trajectory information of the AUV individual as the center, the tracking area of ​​the AUV individual is divided to obtain a consistency zone, an adjustment zone, and a warning zone. The radius of the consistency zone is smaller than the radius of the adjustment zone, and the radius of the adjustment zone is smaller than the radius of the warning zone.

[0128] It should be understood that during the individual motion control process of AUV, the tracking area is divided into three types based on the desired location and different radii: the consistency zone, the adjustment zone, and the warning zone.

[0129] S320: Different feedback tracking control is performed for different tracking areas.

[0130] It should be understood that, for different tracking areas, different feedback tracking control results can be obtained by comparing the distance between the actual position of the AUV individual at the current moment and its expected position at the current moment, and the size relationship between the area radius of different tracking areas.

[0131] Specifically, different feedback tracking controls are implemented for different tracking areas, such as... Figure 3 As shown, it includes:

[0132] S321. Compare the actual position of the AUV at the current moment with the expected position at the current moment and the area radius of each tracking area.

[0133] In this embodiment of the invention, the interval between the actual position and the desired position of the AUV at the current moment can specifically be the Euclidean distance between the actual position and the desired position of the AUV at the current moment. Specifically, using... Indicates the first The actual current location of each AUV individual, using Indicates the first The expected position of each AUV at the current moment, using express and The Euclidean distance between them, using The radius of the uniform region is represented by... Indicates the radius of the adjustment area, using Indicates the radius of the warning zone.

[0134] S322. If the distance between the actual position of the AUV at the current moment and the expected position at the current moment is not greater than the radius of the consistent area, then it is determined that the current tracking effect of the AUV is good.

[0135] Specifically, when At that time, it was believed that the first Individual AUV tracking performed well and required no adjustments.

[0136] S323. If the distance between the actual position of the AUV at the current moment and the expected position at the current moment is greater than the radius of the consistency zone and less than or equal to the radius of the adjustment zone, then it is determined that the current tracking effect of the AUV is poor, and a heading / pitch / speed adjustment command is generated.

[0137] Specifically, when At that time, it was believed that the first The tracking performance of the individual AUV is not good, and adjustments to the heading / pitch / speed are needed to bring the individual AUV into the alignment zone.

[0138] S324. If the distance between the actual position of the AUV at the current moment and the expected position at the current moment is greater than the radius of the adjustment zone and less than or equal to the radius of the warning zone, a fault detection command is generated to detect whether the AUV has malfunctioned.

[0139] Specifically, when At that time, the first If an AUV is suspected of malfunctioning, a fault diagnosis program must be run to check the condition of its hardware (such as thrusters, servos, navigation hardware, etc.). If a malfunction is found, the fault information is uploaded to the decision-maker, and the AUV's mission is terminated. Depending on the fault situation, a return route may be planned, or the AUV may surface, send its real-time position, and await salvage. If no hardware malfunction is found, the heading / pitch / speed adjustments are increased. If the AUV remains within the warning zone for 10 seconds (adjusted according to mission requirements), the AUV's mission is terminated.

[0140] In embodiments of the present invention, fault diagnosis can also be triggered by a combination of speed and position. For example, when the AUV is in the adjustment zone ( (Continued for 3 seconds and the speed deviation is >) When fault diagnosis is triggered, it can accurately identify hardware faults or serious path deviations, avoiding misjudgments.

[0141] S325. If the distance between the actual position of an AUV at the current moment and its expected position at the current moment is greater than the radius of the warning zone, then the current task is stopped and the fault information is reported to the decision-maker. If the current decision-maker of the AUV group fails, the current decision-maker terminates the task, and the AUV individual in the AUV group that is closest to the geometric center of the AUV group becomes the new decision-maker. The new decision-maker can move from its original position to the current decision-maker's position to take over the task of the current decision-maker.

[0142] Specifically, when When this happens, the AUV immediately terminates its mission and uploads fault information to the decision-maker. It should be understood that when the current decision-maker of the AUV group malfunctions and cannot perform trajectory tracking, the current decision-maker's mission terminates, and the AUV closest to the center point of the group becomes the new decision-maker. To maintain formation integrity, the new decision-maker moves from its old position to a new position (the original decision-maker's location), and the same applies to other AUVs.

[0143] It should be understood that, in the embodiments of the present invention, the decision-maker is specifically an AUV individual located near the geometric center of the AUV group.

[0144] In this embodiment of the invention, in order to achieve the above-described trajectory tracking process, each AUV, after obtaining the trajectory analysis results, analyzes the navigation sensor data to obtain the actual state of the AUV, that is, to obtain the actual position and actual speed of the AUV at the current moment.

[0145] ,

[0146] The position error can be obtained based on the current actual position and the current expected position. And the speed error can be obtained based on the actual speed and the expected speed at the current moment. ,

[0147] .

[0148] Based on the aforementioned position and velocity errors, a hierarchical controller approach can be implemented. Specifically, the first step is to design the upper-level trajectory tracking controller, which calculates the desired velocity required to eliminate the position / attitude errors. ,in This represents the desired longitudinal velocity, corresponding to the longitudinal position error. By adjusting make Tracking ;in This represents the desired pitch angular velocity, corresponding to the depth error. By adjusting make Tracking ;in This represents the desired angular velocity of the heading, corresponding to the heading error. By adjusting make Tracking Secondly, there is the lower-level speed / torque controller, which calculates the thrust, pitch control torque, and yaw control torque required by the actuators (such as the stern thrusters and rudder) of the AUV based on the desired speed output from the upper level. Finally, there is the bottom-level actuator control, which calculates the thruster speed based on the thruster efficiency, calculates the rudder angle based on the rudder efficiency parameters, and ultimately achieves trajectory tracking.

[0149] In addition to being implemented based on a hierarchical controller, embodiments of the present invention can also be implemented using model predictive control. Model predictive control (MPC) optimizes the control input over a future period by rolling optimization, taking into account the AUV dynamics model and external disturbances (such as water flow). It can also achieve high-precision trajectory tracking and can predict deviations in advance, which is equivalent to the function of a hierarchical controller and can meet the accuracy requirements of a three-zone tracking mechanism.

[0150] In this embodiment of the invention, the formation change of the AUV group during movement is divided into two cases. One is the formation change performed before the AUV group starts moving in response to a known obstacle. In this case, the AUV group can move to the designated position and follow the preset trajectory. The other is that the AUV group needs to change its formation to avoid the obstacle when it detects an unknown obstacle after starting to move, or it needs to change its formation after receiving a formation change command from the host computer.

[0151] It should be understood that in order to achieve the above functions, it is necessary to implement how to change formations. Specifically, a formation library is preset within the AUV group, and the AUV group changes formations according to the mission scenario and obstacle avoidance requirements.

[0152] Specifically, the formation library may include:

[0153] a. Triangular formation: Characterized by a shallow formation depth, it is highly mobile and suitable for patrol or guard duty.

[0154] b. Lateral formation: Its characteristic is that it can greatly increase the scanning area of ​​the cluster side-scan sonar, which is suitable for ocean exploration.

[0155] c. Longitudinal formation: Characterized by a narrow formation width, suitable for traversing narrow and long environments.

[0156] d. Diamond formation: Characterized by good stability and flexibility, it is suitable for tracking and encircling specific targets.

[0157] For formation changes involving known obstacles, the initial trajectory of each AUV remains unchanged.

[0158] In this embodiment of the invention, when it is determined that the formation change of the AUV swarm is a formation change performed by the AUV swarm in response to an unknown obstacle, autonomous obstacle avoidance movement is performed according to the AUV swarm formation change scenario, such as... Figure 4 As shown, it includes:

[0159] S510. When it is determined that the formation change of the AUV group is a formation change of the AUV group in response to an unknown obstacle, the current AUV group formation change scenario is determined. The AUV group formation change scenario includes at least: the overall formation remains unchanged, the basic formation remains unchanged and the formation spacing changes, and the formation change.

[0160] It should be noted that autonomous obstacle avoidance during individual AUV movement includes three scenarios: Scenario 1: the overall formation remains unchanged, and the individual autonomously avoids obstacles; Scenario 2: the basic formation remains unchanged, and obstacle avoidance is achieved by changing the spacing between formations; Scenario 3: obstacle avoidance is achieved by changing the formation.

[0161] Before performing autonomous obstacle avoidance, it is first necessary to determine which transformation scenario the current AUV group formation change belongs to.

[0162] Specifically, determine the current AUV group formation change scenario, such as... Figure 5 As shown, it includes:

[0163] S511. Determine the scene type of the current AUV formation change based on the obstacle core parameters, the relative relationship between the AUV group and the obstacle, and the state of the AUV group itself. The obstacle core parameters include at least the obstacle's three-dimensional coordinates, obstacle volume, obstacle quantity, and obstacle motion state.

[0164] In this embodiment of the invention, the determination of different scenarios depends on the following three key pieces of information:

[0165] 1) Core parameters of obstacles: three-dimensional coordinates, volume, number, and motion state (static / dynamic). This information depends on the sonar, cameras and other sensors carried on the AUV.

[0166] 2) Relative relationship between AUV swarm and obstacles: the shortest distance between the obstacle and the AUV swarm, and the extent of the overlap between the obstacle and the swarm's expected trajectory (the number of AUVs covered).

[0167] 3) AUV group status: current formation type, current individual spacing, adjustable spacing range of formation (minimum safe spacing, maximum effective communication spacing) and group width, etc.

[0168] S512. If the obstacle core parameters, the relative relationship between the AUV group and the obstacle, and the AUV group's own state satisfy the first constraint, then the scenario type of the current AUV group formation change is determined to be "overall formation unchanged". The first constraint includes simultaneously satisfying the first constraint condition of the obstacle core parameters, the first constraint condition of the relative relationship between the AUV group and the obstacle, and the first constraint condition of the AUV group's own state. Specifically, the first constraint condition of the obstacle core parameters includes the diameter of an AUV individual whose projected area on the flight path is ≤1.5 times that of the obstacle when it is a single obstacle, and the cruising speed of an AUV individual when the obstacle is static or dynamic and its speed is ≤0.5 times that of the obstacle. The first constraint condition of the relative relationship between the AUV group and the obstacle includes the width of the AUV group whose shortest distance between the obstacle and the AUV group is ≥1.5 times that of the obstacle group, and the expected trajectory information of AUV individuals whose obstacle coverage area is ≤2 AUV individuals. The first constraint condition of the AUV group's own state includes that when an AUV individual affected by an obstacle autonomously adjusts its local trajectory, the actual distance between it and adjacent AUV individuals remains within the effective communication range.

[0169] In this embodiment of the invention, entering scenario one (overall formation unchanged, individual units autonomously avoid obstacles) requires the following constraints to be met simultaneously:

[0170] 1) Obstacle physical characteristics constraints: The obstacle is a single obstacle and its projected area on the flight path is ≤1.5 times the diameter of a single AUV; the obstacle is static or dynamic but its speed is ≤0.5 times the AUV's cruising speed (the purpose is to ensure that the AUV can quickly avoid it).

[0171] 2) Obstacle and AUV group position constraints: The shortest distance between the obstacle and the group is ≥ 1.5 times the group width (the purpose is not to affect the overall path of the group); and the obstacle only covers the expected trajectory information of ≤ 2 AUV individuals;

[0172] 3) Formation adaptability constraint: After the affected AUVs autonomously adjust their local trajectories, the actual distance between them and the adjacent AUVs is still within the effective communication range.

[0173] It should be noted here that the effective communication range refers to the maximum distance at which individual AUVs can stably transmit status data via sonar when the AUV group is operating underwater. It is a fundamental constraint parameter for determining local coordination and status feedback.

[0174] S513. If the obstacle core parameters, the relative relationship between the AUV group and the obstacle, and the state of the AUV group simultaneously satisfy the second constraint, then the scenario type of the current AUV group formation change is determined to be a basic formation unchanged with a change in formation spacing. The second constraint includes simultaneously satisfying the second constraint condition of the obstacle core parameters, the second constraint condition of the relative relationship between the AUV group and the obstacle, and the second constraint condition of the AUV group's own state. The second constraint condition of the obstacle core parameters includes the sum of the diameters of individual AUVs whose projected area on the flight path is ≤ M / 2 units, where M represents the number of AUVs in the AUV group. The number of AUV individuals; whether the obstacle is static or dynamic and its trajectory is predictable; the second constraint on the relative relationship between the AUV group and the obstacle includes the shortest distance between the obstacle and the AUV group being between 0.5 and 1.5 times the width of the AUV group, and the expected trajectory information of the AUV individuals for the obstacle coverage area ≤ M / 2 units; the second constraint on the state of the AUV group itself includes the ability to completely avoid obstacles by adjusting the spacing between AUV individuals while maintaining the current formation, and the adjusted spacing between AUV individuals being greater than or equal to the minimum safe distance and less than or equal to the effective communication distance.

[0175] In this embodiment of the invention, entering scenario two (basic formation unchanged, obstacle avoidance by changing formation spacing) requires the following constraints to be met simultaneously:

[0176] 1) Obstacle physical characteristic constraint: The projected area of ​​the obstacle on the flight path ≤ The sum of the diameters of individual AUVs; the obstacle is static or dynamic but the trajectory is predictable;

[0177] 2) Obstacle and AUV group position constraints: The shortest distance between the obstacle and the AUV group is between 0.5 and 1.5 times the group width; and the obstacle coverage is ≤ AUV individual's expected trajectory information;

[0178] 3) Formation adaptability constraints: Maintain the current formation and only adjust the spacing between individuals to completely avoid obstacles; the adjusted spacing is greater than or equal to the minimum safe spacing and less than or equal to the effective communication distance.

[0179] S514. If the obstacle core parameters, the relative relationship between the AUV group and the obstacle, and the AUV group's own state satisfy the third constraint, then the scenario type of the current AUV group formation change is determined to be a formation change. The third constraint includes satisfying one of the following: the third constraint condition of the obstacle core parameters, the third constraint condition of the relative relationship between the AUV group and the obstacle, and the third constraint condition of the AUV group's own state. Specifically, the third constraint condition of the obstacle core parameters includes: the projected area of ​​the obstacle on the flight path > the sum of the diameters of M / 2 AUV individuals; there are multiple obstacles and the distance between them is < the minimum adjustable width of the current formation; the obstacle is static or dynamic and its speed is > 0.5 times the cruising speed of the AUV individuals; the third constraint condition of the relative relationship between the AUV group and the obstacle includes: the shortest distance between the obstacle and the AUV group is less than 0.5 times the width of the AUV group; and the third constraint condition of the AUV group's own state includes: the AUV group after the formation change can completely avoid the obstacle.

[0180] In this embodiment of the invention, entering scenario three (formation change obstacle avoidance) requires satisfying any of the following constraints:

[0181] 1) Obstacle physical characteristic constraints: The projected area of ​​the obstacle on the flight path > The sum of the individual diameters of a single AUV; there are multiple obstacles and the distance between them is less than the minimum adjustable width of the current formation; the speed of the obstacles is greater than 0.5 times the cruising speed of the AUV when they are static or dynamic;

[0182] 2) Obstacle and AUV group position constraints: The shortest distance between the obstacle and the AUV group is less than 0.5 times the group width;

[0183] 3) Formation adaptability constraint: After changing the formation, it can pass through obstacles that can be completely avoided.

[0184] S520. If the current AUV group formation change scenario is such that the overall formation remains unchanged, then autonomous obstacle avoidance is performed through trajectory replanning.

[0185] S530. If the current AUV group formation change scenario is that the basic formation remains unchanged and the formation spacing changes and / or the formation changes, then the trajectory replanning result of the AUV group decision-maker is received. The decision-maker of the AUV group is the AUV individual in the AUV group that is close to the geometric center point of the AUV group. The decision-maker of the AUV group can change and adjust the current formation according to the target formation to achieve trajectory replanning.

[0186] It should be understood that trajectory replanning is only performed when individuals autonomously avoid obstacles; when changes in formation spacing or formation are required, trajectory replanning is uniformly performed by the decision-maker.

[0187] Specifically, the decision-maker of the AUV swarm can adjust the current formation according to the target formation to achieve trajectory replanning, such as... Figure 6 As shown, it includes:

[0188] S531. Generate the geometric center point of the new formation and the new target position of each AUV individual in the new formation based on the target formation, and determine the transition reference point position of each AUV individual in the current formation, wherein the transition reference point position of each AUV individual is at 1 / 2 offset of the line connecting the current position of each AUV individual and the new target position of the AUV individual.

[0189] In this embodiment of the invention, the decision-maker AUV generates a new formation geometric center based on the target formation. and new target locations for each AUV individual Set a transition reference point for each AUV (This transitional benchmark) Located at the current location and Connection Offset To ensure that the minimum spacing between all paths is ≥ ).

[0190] S532. Generate the transition path of each AUV individual according to the order from the current position of each AUV individual to the transition reference point position of the AUV individual, and from the transition reference point position of the AUV individual to the new target position of the AUV individual, and obtain the trajectory replanning result of each AUV individual.

[0191] Specifically, decision-makers based on Generate cubic spline paths and verify that the curve radius of curvature is ≥ , Rate of change of curvature ≤ This ensures a smooth and easy-to-follow path.

[0192] S533. Trajectory replanning results of any two AUV individuals in the AUV group are traversed to pre-verify the risk of spatiotemporal collision.

[0193] Specifically, decision-makers traverse all AUV pairs to pre-check the risk of spatiotemporal collisions.

[0194] In this embodiment of the invention, the trajectory replanning results of any two AUVs in the AUV swarm are traversed to pre-verify the risk of spatiotemporal collision, including:

[0195] 1) Determine whether the minimum distance between the two paths of any two AUVs is greater than or equal to the preset safe distance, or determine whether the time difference of the approach segment of any two AUVs is greater than or equal to the preset safe time difference.

[0196] 2) If not satisfied, adjust the new target velocity in the trajectory replanning result of the current AUV individual until all verifications pass.

[0197] It should be understood that the minimum distance between any two paths is ≥ or the time difference of the adjacent segments is ≥ ( If the conditions are not met, make minor adjustments. Continue until all checks pass.

[0198] S534. After the space-time collision risk verification is passed, the trajectory replanning result is issued to each AUV in the AUV group at one time. The trajectory replanning result includes at least the path parameters, speed curve and safety threshold of the AUV.

[0199] It should be understood that once the space-time collision risk verification is passed, the decision-maker will issue a parameter package of "path parameters + speed curve + safety threshold" to each AUV in one go, and no further real-time verification or instruction adjustment will be performed after the issuance.

[0200] It should be noted that, in addition to using a one-time planning approach by the decision-maker for trajectory replanning, a distributed pre-negotiation mechanism can also be adopted. Before the transformation, the AUV exchanges its initial position, target position, and motion parameters through short-range communication (non-real-time). After each AUV has preliminarily calculated its path, it feeds it back to the decision-maker. The decision-maker only arbitrates and adjusts conflicting paths, without needing to plan the entire path. This also avoids the dependence on real-time communication and achieves collision-free transformation.

[0201] Alternatively, decision-makers can choose to use a region-based approach for trajectory replanning. The decision-maker divides the obstacle avoidance area into sub-regions based on the number of AUVs. Each AUV performs trajectory replanning only within its own sub-region. By pre-setting sub-region boundaries to avoid path intersections, collision-free replanning can still be achieved for the three types of obstacle avoidance scenarios, while reducing the computational burden on the decision-maker.

[0202] In addition to formation changes / space adjustments, speed-based obstacle avoidance strategies can also be used for obstacle avoidance. For dynamic obstacles, the AUV speed can be adjusted according to the obstacle's speed (low-speed obstacles → maintain). High-speed obstacle → Descend By using speed differences to create obstacle avoidance distances, obstacle avoidance can be achieved without changing the formation, making it suitable for scenarios where the speed of obstacles is unstable.

[0203] In this embodiment of the invention, when it is determined that the formation change of the AUV swarm is a formation change performed by the AUV swarm in response to an unknown obstacle, the autonomous obstacle avoidance movement based on the AUV swarm formation change scenario further includes:

[0204] (1) Perform autonomous path following based on the trajectory replanning results issued by the decision-makers of the AUV group;

[0205] Specifically, after receiving the parameter packet, the AUV autonomously follows its own path based on its sensors, executing two criteria in the process: First, it detects that the distance between adjacent AUVs is < Immediately reduce speed to (0.8 * current preset speed), spacing > (1.1 * ... After recovery; secondly, during the transformation process, continuously monitor its own path deviation. If the deviation lasts for 5 seconds > (0.5 * If the deviation is <0.3*, then autonomous correction will be initiated. Then it enters normal tracking mode.

[0206] Additionally, it should be noted that after receiving the parameter packet, if the distance between adjacent AUVs is less than [a certain value], [the system will detect this]. Alternatively, autonomous path following can be achieved through small path offsets. Specifically, when the AUV detects an adjacent spacing of < At that time, guided by the local sensor, the path is slightly deviated away from the adjacent AUV (the deviation amount is ≤0.3*). (Without changing the target position), it can also quickly create a safe distance without relying on real-time communication.

[0207] (2) Determine whether the new target position has been reached;

[0208] (3) If a new target location is reached, switch to formation maintenance mode.

[0209] It should be understood that when the AUV autonomously determines that it has reached... Switch to formation hold mode to complete the transformation.

[0210] In summary, the autonomous underwater vehicle (AUV) dense swarm motion planning method provided by this invention analyzes the expected trajectory information of individual AUVs sent by the host computer to obtain the initial motion trajectory of each AUV. Based on the initial motion trajectory, the AUVs move, and the formation changes of the AUV group are monitored during the movement of individual AUVs. For formation changes involving known obstacles, the initial motion trajectory of the individual AUVs remains unchanged, while for formation changes involving unknown obstacles, autonomous obstacle avoidance movement is performed, thereby updating the initial motion trajectory of each AUV. This autonomous underwater vehicle (AUV) dense swarm motion planning method boasts outstanding collision-free performance and adaptability to dense swarm scenarios. Through cubic spline path planning and spatiotemporal peak-shifting verification, it ensures collision-free initial movement and formation changes for the AUVs. It eliminates reliance on real-time underwater communication, allowing decision-makers to plan and issue parameters once, with the AUVs autonomously executing and correcting their course locally. Its strong fault tolerance effectively prevents formation collapse. It adapts to AUV hardware characteristics, ensuring minimum turning radius, maximum acceleration, and maximum speed meet individual AUV hardware performance requirements, and utilizes the Hungarian algorithm for optimal energy allocation. It exhibits broad task adaptability, with pre-set multi-formation libraries and layered obstacle avoidance scenarios to handle complex underwater environments. High trajectory tracking accuracy is achieved through layered control and a three-zone tracking mechanism, ensuring formation stability and keeping errors within a reasonable range. It also boasts low engineering implementation cost and strong compatibility.

[0211] As another embodiment of the present invention, a motion planning device for dense swarms of autonomous underwater vehicles (AUVs) is provided. This device is applied to each individual AUV in the AUV swarm, and each AUV is able to communicate with a host computer in the initial state of the AUV swarm, which is the state when the AUV swarm is floating on the water surface and has not yet entered the water. Figure 7 As shown, the autonomous underwater robot dense swarm motion planning device 100 includes:

[0212] The acquisition module 110 is used to acquire the expected trajectory information of individual AUVs sent by the host computer. The host computer can perform group trajectory planning to obtain virtual trajectory information of the group based on the initial position of each individual AUV in the AUV group, the geometric center point of the AUV group, the number of AUVs in the AUV group, and preset task-related information. Based on the virtual trajectory information of the group, the host computer can perform trajectory planning for individual AUVs to obtain the expected trajectory information of individual AUVs.

[0213] The parsing module 120 is used to parse the expected trajectory information of individual AUVs and obtain the initial movement trajectory of individual AUVs. The initial movement trajectory of individual AUVs includes at least the position of individual AUVs in the formation of AUV groups and the movement speed of individual AUVs.

[0214] The individual motion control module 130 is used to control the movement of the AUV individual according to the initial movement trajectory of the AUV individual, and to monitor the formation change of the AUV group in real time during the movement of the AUV individual. The formation change of the AUV group includes at least the formation change of the AUV group in response to known obstacles and the formation change of the AUV group in response to unknown obstacles.

[0215] The module 140 is used to maintain the initial trajectory of each AUV individual when it is determined that the formation change of the AUV group is a formation change of the AUV group in response to a known obstacle.

[0216] The autonomous obstacle avoidance module 150 is used to perform autonomous obstacle avoidance movements according to the formation change scenario of the AUV group when it is determined that the formation change of the AUV group is a formation change of the AUV group in response to an unknown obstacle, and to adjust the initial movement trajectory of the AUV individual based on the feedback of the autonomous obstacle avoidance movement to obtain the updated movement trajectory of the AUV individual.

[0217] The autonomous underwater vehicle (AUV) dense swarm motion planning device provided by this invention analyzes the expected trajectory information of individual AUVs sent by the host computer to obtain the initial motion trajectory of each AUV. Based on the initial motion trajectory of each AUV, the device performs movement of the individual AUVs and monitors the formation changes of the AUV group during the movement of the individual AUVs. For formation changes involving known obstacles, the initial motion trajectory of the individual AUVs remains unchanged, while for formation changes involving unknown obstacles, the device performs autonomous obstacle avoidance movement, thereby updating the initial motion trajectory of each AUV. This autonomous underwater vehicle (AUV) swarm motion planning device performs virtual trajectory planning for the AUV swarm via a host computer in its initial state. While the AUV swarm is operating underwater, it autonomously avoids obstacles. This improves fault tolerance while ensuring overall AUV swarm motion planning. Furthermore, because each AUV has its own trajectory and can change formation, there is no propagation of deviations. Therefore, this device not only achieves overall AUV swarm motion planning but also effectively improves fault tolerance and prevents deviation propagation, thereby enhancing the accuracy and efficiency of underwater robot swarm motion planning and adapting to complex underwater operating environments.

[0218] It should be noted that the acquisition module 110 in this embodiment of the invention can acquire the expected trajectory information of individual AUVs issued by the host computer when the AUV group is in the initial state, and when the AUV group is in the working state (i.e., when the AUV group has entered the underwater working state), the acquisition module 110 can also acquire the trajectory replanning results issued by the decision-maker of the AUV group.

[0219] The specific working principle and process of the autonomous underwater robot dense swarm motion planning device of the present invention can be referred to the description of the autonomous underwater robot dense swarm motion planning method above, and will not be repeated here.

[0220] As another embodiment of the present invention, an autonomous underwater robot dense swarm motion planning system 1 is provided, wherein, as Figure 8 As shown, it includes: a host computer 10 and an AUV swarm 20. The AUV swarm 20 includes multiple AUV individuals 21. Each AUV individual 21 can communicate with the host computer 10 within a preset communication range. Any two adjacent AUV individuals 21 in the AUV swarm 20 can communicate with each other within an effective communication distance. Each AUV individual 21 includes the autonomous underwater robot dense swarm motion planning device 100 described above.

[0221] The host computer 10 can generate the expected trajectory information of individual AUVs in the initial state of the AUV group, wherein the initial state of the AUV group is the state when the AUV group is floating on the water surface and has not yet entered the water;

[0222] Each AUV individual 21 has an autonomous underwater robot dense swarm motion planning device 100 that can perform AUV individual movement according to the expected trajectory information of each AUV individual, and perform autonomous obstacle avoidance when the formation of the AUV group changes.

[0223] In this embodiment of the invention, the specific process of the host computer generating the expected trajectory information of individual AUVs can be referred to the relevant description of the autonomous underwater robot dense swarm motion planning method mentioned above.

[0224] The autonomous underwater vehicle (AUV) dense swarm motion planning system provided by this invention analyzes the expected trajectory information of individual AUVs sent by the host computer to obtain the initial motion trajectory of each AUV. Based on the initial motion trajectory of each AUV, the system performs movement of the individual AUVs and monitors the formation changes of the AUV group during the movement of individual AUVs. For formation changes involving known obstacles, the initial motion trajectory of the individual AUVs remains unchanged, while for formation changes involving unknown obstacles, the system performs autonomous obstacle avoidance movement, thereby updating the initial motion trajectory of each AUV. This autonomous underwater robot (AUV) swarm motion planning system performs virtual trajectory planning for the AUV swarm via a host computer during the initial state of the swarm. While the AUVs are working underwater, they autonomously avoid obstacles. This improves fault tolerance while ensuring global motion planning for the AUV swarm. Furthermore, because each AUV has its own trajectory and can change formations, there is no propagation of deviations. Therefore, this system not only achieves global motion planning for the AUV swarm but also effectively improves fault tolerance and prevents deviation propagation, thereby enhancing the accuracy and efficiency of motion planning for dense underwater robot swarms and adapting to complex underwater operating environments.

[0225] The specific working principle and process of the autonomous underwater robot dense swarm motion planning system of the present invention can be referred to the description of the autonomous underwater robot dense swarm motion planning method above, and will not be repeated here.

[0226] It is understood that the above embodiments are merely exemplary implementations used to illustrate the principles of the present invention, and the present invention is not limited thereto. For those skilled in the art, various modifications and improvements can be made without departing from the spirit and essence of the present invention, and these modifications and improvements are also considered to be within the scope of protection of the present invention.

Claims

1. A method for planning the motion of dense swarms of autonomous underwater robots, characterized in that, The method applies to each individual AUV in an AUV swarm, where each AUV can communicate with the host computer in the initial state of the swarm, which is when the swarm is floating on the water surface and has not yet entered the water. The method includes: The host computer obtains the expected trajectory information of individual AUVs from the host computer. The host computer can perform group trajectory planning to obtain virtual trajectory information of the group based on the initial position of each individual AUV in the group, the geometric center point of the group, the number of individual AUVs in the group, and preset task-related information. Based on the virtual trajectory information of the group, the host computer performs trajectory planning for individual AUVs to obtain the expected trajectory information of individual AUVs. Analyze the expected trajectory information of individual AUVs to obtain the initial movement trajectory of individual AUVs. The initial movement trajectory of individual AUVs includes at least the position of individual AUVs in the formation of AUV groups and the movement speed of individual AUVs. The movement of individual AUVs is performed based on their initial movement trajectories, and the formation changes of the AUV group during the movement of individual AUVs are monitored in real time. The formation changes of the AUV group include at least the formation changes of the AUV group in response to known obstacles and the formation changes of the AUV group in response to unknown obstacles. When the formation change of the AUV swarm is determined to be a formation change of the AUV swarm in response to a known obstacle, the initial movement trajectory of the individual AUVs remains unchanged; When the formation change of the AUV group is determined to be the formation change of the AUV group in response to unknown obstacles, autonomous obstacle avoidance movement is performed according to the formation change scenario of the AUV group, and the initial movement trajectory of the AUV individual is adjusted according to the feedback of the autonomous obstacle avoidance movement to obtain the updated movement trajectory of the AUV individual. When the formation change of the AUV swarm is determined to be a formation change performed by the AUV swarm in response to an unknown obstacle, autonomous obstacle avoidance maneuvers are performed based on the AUV swarm formation change scenario, including: When it is determined that the formation change of the AUV group is a formation change of the AUV group in response to an unknown obstacle, the current AUV group formation change scenario is determined. The AUV group formation change scenario includes at least: the overall formation remains unchanged, the basic formation remains unchanged but the formation spacing changes, and the formation change itself. If the current AUV group formation change scenario is such that the overall formation remains unchanged, then autonomous obstacle avoidance is achieved through trajectory replanning. If the current AUV group formation change scenario is that the basic formation remains unchanged and the formation spacing changes and / or the formation changes, then the trajectory replanning result of the AUV group's decision-maker is received. The decision-maker of the AUV group is the AUV individual in the AUV group that is close to the geometric center point of the AUV group. The decision-maker of the AUV group can change and adjust the current formation according to the target formation to achieve trajectory replanning.

2. The method for planning the dense swarm motion of autonomous underwater robots according to claim 1, characterized in that, Determine the current AUV group formation change scenario, including: The scene type of the current AUV formation change is determined based on the obstacle core parameters, the relative relationship between the AUV group and the obstacle, and the state of the AUV group itself. The obstacle core parameters include at least the obstacle's three-dimensional coordinates, obstacle volume, obstacle quantity, and obstacle motion state. If the obstacle core parameters, the relative relationship between the AUV group and the obstacle, and the AUV group's own state satisfy the first constraint, then the scenario type of the current AUV group formation change is determined to be "overall formation unchanged". The first constraint includes simultaneously satisfying the first constraint condition of the obstacle core parameters, the first constraint condition of the relative relationship between the AUV group and the obstacle, and the first constraint condition of the AUV group's own state. Specifically, the first constraint condition of the obstacle core parameters includes the diameter of an AUV individual whose projected area on the flight path is ≤1.5 times that of the obstacle when it is a single obstacle, and the cruising speed of an AUV individual when the obstacle is static or dynamic and its speed is ≤0.5 times that of the obstacle. The first constraint condition of the relative relationship between the AUV group and the obstacle includes the width of the AUV group whose shortest distance between the obstacle and the AUV group is ≥1.5 times that of the obstacle group, and the expected trajectory information of AUV individuals whose obstacle coverage area is ≤2 AUV individuals. The first constraint condition of the AUV group's own state includes maintaining the actual distance between adjacent AUV individuals within the effective communication range after the AUV individual affected by the obstacle autonomously adjusts its local trajectory. If the obstacle's core parameters, the relative relationship between the AUV group and the obstacle, and the AUV group's own state all simultaneously satisfy the second constraint, then the scenario type of the current AUV group formation change is determined to be: basic formation unchanged, formation spacing changing. The second constraint includes simultaneously satisfying the second constraint condition of the obstacle's core parameters, the second constraint condition of the relative relationship between the AUV group and the obstacle, and the second constraint condition of the AUV group's own state. The second constraint condition of the obstacle's core parameters includes the sum of the diameters of individual AUVs whose projected area on the flight path is ≤ M / 2, where M represents the number of AUVs in the AUV group. The number of individuals; whether the obstacle is static or dynamic and its trajectory is predictable; the second constraint on the relative relationship between the AUV swarm and the obstacle includes the shortest distance between the obstacle and the AUV swarm being between 0.5 and 1.5 times the width of the AUV swarm, and the expected trajectory information of the AUV individuals for the obstacle coverage area ≤ M / 2 units; the second constraint on the state of the AUV swarm itself includes the ability to completely avoid obstacles by adjusting the spacing between AUV individuals while maintaining the current formation, and the adjusted spacing between AUV individuals being greater than or equal to the minimum safe distance and less than or equal to the effective communication distance. If the obstacle core parameters, the relative relationship between the AUV group and the obstacle, and the AUV group's own state satisfy the third constraint, then the scenario type of the current AUV group formation change is determined to be a formation change. The third constraint includes satisfying one of the following: the third constraint condition of the obstacle core parameters, the third constraint condition of the relative relationship between the AUV group and the obstacle, and the third constraint condition of the AUV group's own state. Specifically, the third constraint condition of the obstacle core parameters includes: the projected area of ​​the obstacle on the flight path > the sum of the diameters of M / 2 AUV individuals; there are multiple obstacles and the distance between them is < the minimum adjustable width of the current formation; the obstacle is static or dynamic and its speed is > 0.5 times the cruising speed of the individual AUVs; the third constraint condition of the relative relationship between the AUV group and the obstacle includes: the shortest distance between the obstacle and the AUV group is less than 0.5 times the width of the AUV group; and the third constraint condition of the AUV group's own state includes: the AUV group after the formation change can completely avoid the obstacle.

3. The method for planning the dense swarm motion of autonomous underwater robots according to claim 1, characterized in that, The decision-maker of the AUV swarm can adjust the current formation according to the target formation to achieve trajectory replanning, including: The geometric center point of the new formation and the new target position of each AUV in the new formation are generated based on the target formation. The transition reference point position of each AUV in the current formation is determined. The transition reference point position of each AUV is located at 1 / 2 offset from the line connecting the current position of each AUV and the new target position of that AUV. The transition path of each AUV is generated according to the order from its current position to its transition reference point and from its transition reference point to its new target position, and the trajectory replanning result of each AUV is obtained. Trajectory replanning results of any two AUVs in the AUV swarm are traversed to pre-verify the risk of spatiotemporal collision; Once the space-time collision risk verification is passed, the trajectory replanning result is sent to each individual AUV in the AUV group at once. The trajectory replanning result includes at least the path parameters, velocity curve and safety threshold of the individual AUV.

4. The method for planning the dense swarm motion of autonomous underwater robots according to claim 3, characterized in that, Trajectory replanning results for any two AUVs in the AUV swarm are iterated to pre-verify the risk of spatiotemporal collisions, including: Determine whether the minimum distance between the two paths of any two AUVs is greater than or equal to the preset safety distance, or determine whether the time difference of the approach segment of any two AUVs is greater than or equal to the preset safety time difference. If the conditions are not met, the new target velocity in the trajectory replanning result of the current AUV individual will be adjusted until all verifications pass.

5. The method for dense swarm motion planning of autonomous underwater robots according to claim 1, characterized in that, When the formation change of the AUV swarm is determined to be a formation change performed by the AUV swarm in response to an unknown obstacle, the autonomous obstacle avoidance movement based on the AUV swarm formation change scenario also includes: Autonomous path following is performed based on the trajectory replanning results issued by the decision-makers of the AUV group; Determine whether the new target position has been reached; If a new target location is reached, switch to formation hold mode.

6. The method for dense swarm motion planning of autonomous underwater robots according to claim 1, characterized in that, The movement of the AUV is based on its initial trajectory, including: Using the expected location in the expected trajectory information of the AUV individual as the center, the tracking area of ​​the AUV individual is divided to obtain a consistency zone, an adjustment zone, and a warning zone. The radius of the consistency zone is smaller than the radius of the adjustment zone, and the radius of the adjustment zone is smaller than the radius of the warning zone. Different feedback tracking controls are applied to different tracking areas.

7. The method for planning the dense swarm motion of autonomous underwater robots according to claim 6, characterized in that, Different feedback tracking controls are applied to different tracking areas, including: The comparison is made based on the interval between the actual current position and the expected current position of the AUV individual, and the area radius of each tracking region. If the distance between the actual position of the AUV at the current moment and its expected position at the current moment is not greater than the radius of the consistency zone, then the tracking effect of the AUV at the current moment is determined to be good. If the distance between the actual position and the expected position of the AUV at the current moment is greater than the radius of the consistency zone and less than or equal to the radius of the adjustment zone, then it is determined that the current tracking effect of the AUV is poor, and a heading / pitch / speed adjustment command is generated. If the distance between the actual position of the AUV at the current moment and the expected position at the current moment is greater than the radius of the adjustment zone and less than or equal to the radius of the warning zone, a fault detection command is generated to detect whether the AUV has malfunctioned. If the distance between the actual position and the expected position of an AUV at the current moment is greater than the radius of the warning zone, the AUV stops its current task and reports the fault information to the decision-maker. If the current decision-maker of the AUV group fails, the current decision-maker terminates its task, and the AUV closest to the geometric center of the AUV group becomes the new decision-maker. The new decision-maker can move from its original position to the current decision-maker's position to take over the current decision-maker's task.

8. A motion planning device for dense swarms of autonomous underwater robots, used to implement the motion planning method for dense swarms of autonomous underwater robots as described in any one of claims 1 to 7, characterized in that, The autonomous underwater vehicle (AUV) swarm motion planning device is applied to each individual AUV in the AUV swarm, and each AUV can communicate with the host computer in the initial state of the AUV swarm, which is when the AUV swarm is floating on the water surface and has not yet entered the water. The device includes: The acquisition module is used to acquire the expected trajectory information of individual AUVs sent by the host computer. The host computer can perform group trajectory planning to obtain virtual trajectory information of the group based on the initial position of each individual AUV in the group, the geometric center point of the group, the number of individual AUVs in the group, and preset task-related information. Based on the virtual trajectory information of the group, the host computer can perform trajectory planning for individual AUVs to obtain the expected trajectory information of individual AUVs. The parsing module is used to parse the expected trajectory information of individual AUVs and obtain the initial movement trajectory of individual AUVs. The initial movement trajectory of individual AUVs includes at least the position of individual AUVs in the formation of AUV groups and the movement speed of individual AUVs. The individual motion control module is used to control the movement of the AUV individual based on the initial movement trajectory of the AUV individual, and to monitor the formation changes of the AUV group in real time during the movement of the AUV individual. The formation changes of the AUV group include at least the formation changes of the AUV group in response to known obstacles and the formation changes of the AUV group in response to unknown obstacles. The maintenance module is used to maintain the initial trajectory of individual AUVs when the formation change of the AUV group is determined to be a formation change of the AUV group in response to a known obstacle. The autonomous obstacle avoidance module is used to perform autonomous obstacle avoidance movements based on the formation change scenario of the AUV group when it is determined that the formation change of the AUV group is a formation change of the AUV group in response to unknown obstacles. The module also adjusts the initial movement trajectory of the individual AUVs based on the feedback of the autonomous obstacle avoidance movement to obtain the updated movement trajectory of the individual AUVs.

9. A motion planning system for dense swarms of autonomous underwater robots, characterized in that, include: The host computer and the AUV swarm, wherein the AUV swarm includes multiple AUV individuals, each of which can communicate with the host computer within a preset communication range, and any two adjacent AUV individuals in the AUV swarm can communicate with each other within an effective communication distance, and each AUV individual includes the autonomous underwater robot dense swarm motion planning device as described in claim 8. The host computer can generate the expected trajectory information of individual AUVs in the initial state of the AUV group, wherein the initial state of the AUV group is the state when the AUV group is floating on the water surface and has not yet entered the water; Each AUV's autonomous underwater vehicle (AUV) swarm motion planning device is capable of moving the AUV according to its desired trajectory information and autonomously avoiding obstacles when the formation of the AUV swarm changes.

Images