Unmanned cluster trajectory planning method based on Tangent Bug algorithm and real-time virtual pipeline

Abstract

The invention discloses an unmanned cluster trajectory planning method based on a Tangent Bug algorithm and a real-time virtual pipeline, and belongs to the field of unmanned cluster cooperative control and trajectory planning. According to the method, switching of three navigation modes is managed through a state machine. After the environment is sensed in real time, if a narrow channel is recognized, entering a special mode, aligning the central line of the pipeline with the central axis of the channel and shrinking the radius, and guiding the cluster in the central line direction; and if the narrow channel is not identified, switching between a boundary following mode and a target approaching mode is carried out, an obstacle avoidance tangent direction or a direct approaching direction is calculated based on a Tangent Bug algorithm, and the radius of the pipeline is kept at a standard value. And finally, by taking the dynamically constructed virtual pipeline as a unified space reference, calculating the motion trail of each unit in real time according to a preset formation configuration, and outputting the motion trail. According to the method, the problems of fixed pipeline radius, low passing efficiency of a narrow channel and insufficient formation collaboration in an existing method are effectively solved, and safe, efficient and collaborative movement of the unmanned cluster in a complex environment is realized.

Description

Technical Field

[0001] This invention belongs to the field of unmanned swarm cooperative control and trajectory planning technology, specifically involving an autonomous navigation and obstacle avoidance method for multiple agents in complex dynamic environments, and more specifically, an unmanned swarm trajectory planning method based on the Tangent Bug algorithm and real-time adaptive virtual pipeline. Background Technology

[0002] With the widespread application of intelligent agents such as drones, mobile robots, and autonomous vehicles in surveying, search and rescue, and monitoring, unmanned swarm cooperative control has become a cutting-edge research direction in robotics. Among these, virtual pipeline technology, by constructing safe spatial zones to guide formation movement, can effectively simplify obstacle avoidance decisions. However, existing research on virtual pipeline construction technologies still has the following significant shortcomings.

[0003] First, the radius of virtual pipelines is often fixed, making it difficult to adapt to environmental changes. In narrow passages, an excessively large fixed radius can easily cause robots to collide with obstacles; in open areas, an excessively large radius can lead to loose cluster formation, affecting not only the efficiency of information exchange but also the effectiveness of task execution due to loss of formation control.

[0004] Secondly, in narrow passage scenarios, existing methods often treat other units in the cluster as dynamic obstacles, introducing a large number of redundant constraints, which makes the optimization problem complex and difficult to solve. This makes it easy for path conflicts to occur between robots, often causing cluster formation splits in typical narrow areas such as bridges and canyons, with some robots breaking away from the formation because they cannot plan a feasible path.

[0005] Furthermore, existing methods often separate cluster formation layout design and trajectory planning into independent steps, lacking collaborative optimization. Trajectory planning often employs individual planners or potential field-based methods to plan paths cell by cell, without fully considering the impact of the overall formation configuration on path feasibility. This leads to problems such as inter-cell path intersections, collisions, or formation tearing in narrow or complex environments. On the other hand, formation layout adjustments often rely on static configuration library matching or simple feedback control based on relative position constraints, lacking collaborative constraints on the coordination and consistency of individual cell trajectories and overall safety during continuous cluster movement. This results in difficulty synchronizing the movement of individual cells within the cluster, insufficient formation stability, and an inability to meet the requirements of high-precision collaborative tasks. Summary of the Invention

[0006] The purpose of this invention is to solve the problems of fixed pipeline radius, low passage efficiency in narrow passages, and insufficient formation coordination in existing unmanned swarm trajectory planning. It proposes a real-time trajectory planning method that combines dynamic virtual pipelines and state machine navigation strategies to improve the overall task execution capability of unmanned swarms in complex environments.

[0007] To achieve the above objectives, the technical solution provided by this invention is:

[0008] This paper presents a method for unmanned cluster trajectory planning based on the Tangent Bug algorithm and real-time virtual pipeline. This method manages the switching of navigation modes through a state machine and includes the following steps:

[0009] Step 1, Environment Initialization and Parameter Configuration: Define the task environment map, the starting position and global target position of the unmanned swarm, obstacle information, configure sensor parameters, and initialize the radius parameters of the virtual pipeline and the initial navigation mode of the state machine;

[0010] Step 2, Real-time Environmental Perception, Pattern Decision-Making, and Dynamic Construction of Virtual Pipelines: Based on real-time environmental information acquired by sensors, obstacle distribution and channel structure are detected, and navigation mode decisions and virtual pipeline construction are made accordingly. If a narrow channel that meets the passage conditions is identified, the narrow channel navigation mode is entered, and the centerline of the virtual pipeline is adjusted to align with the central axis of the narrow channel, while the pipeline radius is adjusted according to the channel width. If no narrow channel is identified, the system switches between boundary following mode and target approach mode based on the obstacle detection results, and the centerline position of the virtual pipeline is determined according to the corresponding mode, while the radius of the virtual pipeline remains at a preset standard radius.

[0011] Step 3, Local Motion Planning: Based on the navigation mode decided in Step 2 and the corresponding virtual pipeline centerline position, determine the extension guidance direction of the pipeline centerline; in the narrow passage navigation mode, the direction of the central axis of the passage is used as the guidance direction of the cluster; in the boundary following mode, based on the Tangent Bug algorithm, the tangent direction along the obstacle boundary is calculated as the obstacle avoidance motion direction of the cluster; in the target approach mode, based on the Tangent Bug algorithm, the direction from the current position to the global target position is calculated as the approach motion direction of the cluster.

[0012] Step 4, Cluster Formation Trajectory Generation: Using the virtual pipeline centerline determined in Step 2 as the reference and the extension guidance direction determined in Step 3 as the forward direction, the position of each unit in the cluster within the pipeline is calculated in real time according to the preset formation configuration, and the original motion trajectory of each unit is generated.

[0013] Step 5, Trajectory Output: Output the motion trajectory generated in Step 4 for cluster motion control.

[0014] Furthermore, before outputting the motion trajectory in step 5, a step of smoothing and filtering the original motion trajectories of each unit generated in step 4 is also included.

[0015] Furthermore, in step 2, a narrow passage that meets the passage conditions is identified, specifically: when the width of the gap between two obstacles is detected. Meet the conditions At that time, it was determined to be a narrow passage, in which The minimum radius of the virtual pipe. This represents the maximum radius of the virtual pipeline. In narrow passage navigation mode, the pipeline radius is adjusted according to the passage width; specifically, the radius of the virtual pipeline is adjusted to the passage radius. , ,in This is a preset safety margin.

[0016] Further, in step 2, in the narrow passage navigation mode, the centerline position of the virtual pipeline is adjusted to be aligned with the central axis of the narrow passage. Specifically, this includes: calculating the surface points of the first obstacle based on the center position vector of the first obstacle and the radius of the first obstacle, and the center position vector of the second obstacle and the radius of the second obstacle. Position vector and second obstacle surface point Position vector; and The midpoint of the connecting line is taken as the point through which the virtual pipeline centerline should pass within the channel, and perpendicular to... and The direction of the connecting line serves as the central axis of the narrow passage.

[0017] Furthermore, in step 2, the preset switching conditions for exiting the boundary-following mode include any of the following:

[0018] The sensor detects that there are no obstacles blocking the way ahead, or that it is about to enter a recognized narrow passage;

[0019] The straight path from the current position to the global target position is confirmed to be unobstructed, and the Euclidean distance from the current position to the global target point is less than the minimum historical distance recorded during the boundary following process.

[0020] Furthermore, in step 3, the tangential direction along the obstacle boundary is calculated as the obstacle avoidance motion direction of the cluster in the boundary-following mode, specifically including:

[0021] Based on the detected obstacle radius Current radius of virtual pipeline and the distance from the current position to the center of the obstacle. Calculate the tangent angle The calculation formula is: ;

[0022] According to the tangent angle And the vector direction from the current position to the center of the obstacle, to determine the unit vector of the two candidate tangent directions;

[0023] Select the candidate tangent direction that has the larger dot product with the unit vector pointing to the global target direction as the current obstacle avoidance motion direction.

[0024] Furthermore, in step 3, when determining the actual movement direction of the cluster in the boundary-following mode, a direction mixing mechanism is introduced, and the actual movement direction vector is calculated using the following formula. :

[0025]

[0026] In the formula, Let be the unit vector of the selected tangent direction. Let be a unit vector pointing from the current position to the global target position. The weighting coefficients are adjustable and .

[0027] Furthermore, in step 3, the direction from the current position to the global target position is calculated as the approaching motion direction of the cluster in the target-oriented mode, specifically including:

[0028] Calculate from the current position Point to global target location Direction vector The calculation formula is: ,in For the smallest positive number used to prevent division by zero errors;

[0029] Based on direction vector and dynamically adjusted local target distance Determine the local target point The calculation formula is: ;

[0030] With local target point and direction vector As a basis for the approaching direction of the cluster.

[0031] Furthermore, in step 4, the preset formation configuration is an inscribed square configuration; the position of each unit in the pipeline is determined in the following way: taking the center line of the virtual pipeline as the reference point, according to the vertex distribution model of the inscribed square, combined with the current virtual pipeline radius, the coordinate offset of each unit relative to the reference point is calculated, and then the absolute position of each unit is obtained.

[0032] Furthermore, the smoothing filtering process includes preprocessing using a forward window dynamic smoothing algorithm, followed by real-time smoothing using a fixed window moving average algorithm.

[0033] The advantages of this invention are:

[0034] 1. The unmanned swarm trajectory planning method based on the Tangent Bug algorithm and real-time virtual pipelines provided by this invention intelligently switches and manages three navigation modes—narrow passage navigation, boundary following, and target approach—through a state machine. This effectively overcomes the problems of fixed pipeline radius, low passage efficiency in narrow passages, and insufficient formation coordination in existing methods. After real-time environmental perception, if a narrow passage is identified, the method enters a dedicated navigation mode (narrow passage navigation mode) and aligns the virtual pipeline with the central axis of the passage and shrinks its radius, thereby providing the swarm with a centered and safe passage path, significantly improving the safety and efficiency of passage in confined spaces. For general scenarios without narrow passages, the state machine switches between boundary following and target approach modes, and plans obstacle avoidance or approach directions in real time based on the Tangent Bug algorithm. Simultaneously, by using a unified and dynamic virtual pipeline as a spatial reference and calculating the trajectory of each unit in real time according to a preset formation configuration, the inherent consistency and motion synchronization of the swarm formation during complex maneuvers are ensured, fundamentally achieving deep coordination between formation layout and trajectory planning.

[0035] 2. This invention effectively eliminates high-frequency jitter and sharp turns in motion commands by performing real-time smoothing optimization on the generated trajectory, generating a smooth trajectory that conforms to dynamic constraints, thereby improving the coherence, control stability and energy efficiency of cluster motion. Attached Figure Description

[0036] The above and / or other features and advantages of the present invention will become more readily understood from the following description with reference to the accompanying drawings, in which:

[0037] Figure 1 This is the overall control flowchart of the unmanned cluster trajectory planning method based on the Tangent Bug algorithm and real-time virtual pipeline according to an embodiment of the present invention;

[0038] Figure 2 This is a schematic diagram of the virtual pipeline and unmanned cluster configuration in the initial state of an embodiment of the present invention, showing the initial distribution relationship of the virtual pipeline, obstacles, starting position and each unit in the cluster;

[0039] Figure 3 This is a schematic diagram of the trajectory planning results in an obstacle environment according to an embodiment of the present invention;

[0040] Figure 4 This is a graph showing the change in the adaptive adjustment of the virtual pipeline radius in an embodiment of the present invention;

[0041] Figure 5 This is a diagram showing the motion trajectory results of each unit within the cluster in this embodiment of the invention;

[0042] Figure 6 This is a graph showing the speed change of each unit within the cluster in an embodiment of the present invention. Detailed Implementation

[0043] The present invention will now be described in detail with reference to the accompanying drawings and exemplary embodiments thereof. It should be noted that the following detailed description of the present invention is for illustrative purposes only and is not intended to limit the scope of the invention.

[0044] Reference Figure 1 As an exemplary embodiment of the present invention, the unmanned swarm trajectory planning method based on the Tangent Bug algorithm and real-time virtual pipelines intelligently manages the switching between three navigation modes through a state machine. These three navigation modes are narrow passage navigation mode, boundary following mode, and target approach mode. The method tightly couples environmental perception, navigation decision-making, and virtual pipeline construction to guide the unmanned swarm to move safely, efficiently, and collaboratively from the starting position to the target position. The following will describe the process steps in detail.

[0045] Step S1: Environment Initialization and Parameter Configuration

[0046] First, the runtime environment is initialized and defined. The 2D task map is set to 100m × 100m, and it is determined that the unmanned swarm needs to start from the initial position. Move to the global target position Initialize obstacle information in the environment. In this embodiment, four static circular obstacles are used as examples. Each obstacle is represented by its center coordinates. and radius Describe the process. Simultaneously configure the sensor parameters mounted on the cluster unit, and set the detection range. 15m, field of view for .

[0047] Next, initialize the parameters of the virtual pipeline: standard radius. Minimum allowable radius of 6m The maximum allowable radius is 2m. It is 6m. Simultaneously, the initial navigation mode of the state machine is set, typically to the goal-oriented mode. This step establishes a complete operational framework and initial state for the entire planning system, such as... Figure 2 As shown, the initial scene includes a starting point, a target point, obstacles, and an initial virtual pipeline constructed with a standard radius.

[0048] Step S2: Real-time environmental perception, pattern decision-making, and dynamic construction of virtual pipelines

[0049] During swarm movement, each unit uses sensors to acquire real-time information about its surrounding environment. Based on this sensory data, the system detects the distribution of obstacles and identifies specific structures in the environment, such as narrow passages formed by two obstacles that allow the swarm to pass. According to the currently perceived environmental characteristics, the system dynamically adjusts the shape of the virtual corridor. The core function of the virtual corridor is to construct a continuous, safe corridor for the entire swarm's movement.

[0050] When the system identifies a narrow passage that meets the passage conditions, the pipe adjustment strategy is key to improving passage safety and efficiency in this invention. In this embodiment, when a pair of visible obstacles is detected, the gap width between the surfaces of the two obstacles is calculated. An area is considered a passable narrow passage when the following conditions are met:

[0051]

[0052] At this point, the state machine directly decides to enter the narrow passage navigation mode and simultaneously executes the adaptive construction of the virtual pipeline to enable the cluster to pass safely and centrally. Specifically, the virtual pipeline makes two key adjustments: First, it adjusts the position of the pipeline's centerline to align it with the central axis of the narrow passage. This central axis direction is determined by calculating the key points of the relative surfaces of the obstacles on both sides. , First, obtain the perpendicular direction of the connecting line; second, adjust the pipe radius according to the channel width to shrink it to a safer optimal passage radius. The passage radius can be calculated, for example, using the following formula:

[0053]

[0054] in As a preset safety margin, 0.45 is used in this embodiment.

[0055] Central axis direction of narrow passage The specific calculation method is as follows: based on the center position vector of the first obstacle and the radius of the first obstacle The center position vector of the second obstacle and the radius of the second obstacle Calculate the points on the surface of the first obstacle respectively. position vector Second obstacle surface point position vector ;by and The midpoint of the connecting line is taken as the point through which the virtual pipeline centerline should pass within the channel, and perpendicular to... and The direction of the connecting line is used as the direction of the central axis. :

[0056]

[0057]

[0058]

[0059] This series of operations ensures that the cluster can safely pass through the narrowest areas in the most centered position, and the pipeline radius can adaptively shrink and recover with the channel width, avoiding collisions or loose formation problems that may occur due to a fixed radius.

[0060] In typical scenarios where narrow passages are not detected, such as open areas or areas with only isolated obstacles, the system switches between boundary-following mode and target-approaching mode, adjusting the centerline position of the virtual pipeline in the corresponding navigation mode while maintaining the preset standard radius of the virtual pipeline. .

[0061] When the sensor detects an obstacle blocking the path to the target, it switches to boundary-following mode. In this mode, the radius of the virtual pipeline remains at the standard radius. The position of the pipeline centerline is adjusted according to obstacle avoidance requirements to ensure that it can extend along the safe tangent direction of the obstacle.

[0062] When there are no obstacles within the sensor's detection range, the system enters target-oriented mode. At this time, the radius of the virtual pipeline also remains at the standard radius. The centerline of the pipeline is oriented toward the target point, and its position is determined based on the principle of ensuring that the entire formation maintains a safe distance from any lateral obstacles that may exist in the environment.

[0063] The switching conditions between different modes are carefully designed. For example, the conditions for exiting the boundary-following mode include two parallel scenarios, either of which triggers a mode switch: one scenario is that the sensor detects no obstacles ahead, or that the robot is about to enter a recognized narrow passage. This mechanism ensures an immediate response when the obstacle threat disappears or a new scene is entered. The other scenario is that the straight path from the current position to the global target position is confirmed to be unobstructed, and the Euclidean distance from the current position to the global target point is less than the minimum historical distance recorded during boundary-following. This scenario is a heuristic optimization that ensures the robot can immediately leave the boundary and turn towards the target once it finds a better direct path than the historical detour path. This mechanism significantly improves the global approximate optimality of the path while ensuring safe obstacle avoidance, which is the key to the Tangent Bug algorithm's ability to guarantee that the path is close to optimal.

[0064] Step S3, Local Motion Planning

[0065] Based on the current navigation mode determined in step S2 and the corresponding virtual pipeline centerline position, the system calculates the guiding direction in which the virtual pipeline centerline should extend. This is the step of translating high-level decisions into specific motion commands.

[0066] In narrow passage navigation mode, the system directly adopts the direction of the passage centerline calculated in step S2. This serves as a guide for unmanned swarms. It allows the swarm to be reliably guided to the widest central area of ​​the passage, significantly improving the success rate and safety of passage through confined spaces.

[0067] In boundary-following mode, the system performs real-time obstacle avoidance planning based on the Tangent Bug algorithm. Specifically, it plans the obstacle avoidance based on the obstacle radius. Current radius of the pipeline and the distance from the current position to the center of the obstacle. Calculate the tangent angle :

[0068]

[0069] Next, based on the vector from the current position to the center of the obstacle... azimuth ( and Representing vectors respectively (Two components in a two-dimensional coordinate system) and the calculated tangent angle Determine the unit vectors of the two candidate tangent directions, as follows:

[0070]

[0071]

[0072] Finally, by comparing the dot product of these two directions with the global target direction, the direction that is more aligned with the target is selected as the obstacle avoidance motion direction.

[0073] To further improve path smoothness and global guidance, a direction blending mechanism can be introduced. This involves weighted fusion of the obstacle avoidance direction and the target direction using the following formula. This allows the robot to maintain its tendency to approach the target while navigating obstacles, preventing formation chaos or collisions caused by sudden changes in direction. The actual movement direction vector of the robot formation is... The calculation is as follows:

[0074]

[0075] In the formula, Let be the unit vector of the selected tangent direction. Let be a unit vector pointing from the current position to the global target position. The weighting coefficients are adjustable and , The larger the value, the more the movement direction emphasizes obstacle avoidance; The smaller the value, the more the movement direction tends to move towards the global target; in this embodiment, 0.4 is used.

[0076] In the target-oriented mode, which also uses the Tangent Bug algorithm for the "move to target" behavior, the system calculates the target from the current position at the center of the pipeline. Directly points to the global target location Direction vector Based on this direction vector and dynamically adjusted local target distance Determine a local target point and local target points and direction vector As a basis for the approaching direction of the cluster:

[0077]

[0078]

[0079] in In this embodiment, the value is taken as an extremely small positive number to prevent division by zero errors. .

[0080] Step S4: Generation of Cluster Formation Trajectory

[0081] This step aims to translate the unified pipeline guidance into the motion trajectory of each unit within the cluster. For example... Figure 5 As shown, although the trajectories of each unit have individual differences, they are all constrained within a unified virtual pipeline. Specifically, using the centerline of the virtual pipeline constructed in step S2 as the spatial reference and the guiding direction calculated in step S3 as the forward direction, the system calculates in real time the precise position each unit should occupy within the pipeline cross-section based on the preset formation geometry (an inscribed square configuration in this embodiment). Taking the inscribed square as an example, the system uses the current position of the pipeline center... Using this as a reference point, based on the current radius of the pipeline Using a square vertex distribution model, the coordinate offset of each element relative to the center is calculated to determine the absolute position of each element. For a typical formation consisting of four elements, their positions can be calculated using the following formulas:

[0082]

[0083]

[0084]

[0085]

[0086] In this configuration, the distance between any two adjacent units All meet Regardless of pipe radius Regardless of dynamic changes, this geometric model-based solution mechanism can automatically maintain the preset relative positional relationships between each element, such as... Figure 2 As shown in the initial configuration. This ensures that the cluster can maintain its formation naturally and stably during complex maneuvers, thereby maintaining reliable communication links and collaborative awareness between units and enhancing the overall operational robustness.

[0087] Step S5, Trajectory Output

[0088] This step is the final stage of the planning process. Its task is to output the trajectory information generated in step S4, which describes the future movement of each unit within the cluster, for execution by the underlying motion controller. The output data stream or instruction set can directly drive each unmanned platform, controlling its movement along the planned path, thus achieving a closed loop from planning to execution. After the output is completed, the system will return to step S2 to begin the next cycle of perception-planning-output, thereby enabling online real-time navigation of the cluster in a dynamic environment.

[0089] To further improve the quality of the final execution trajectory, the original trajectory generated in step S4 can be smoothed and optimized before output. To do this, step S1 needs to set the parameters required for trajectory smoothing, such as the smoothing window. Size.

[0090] Smoothing can employ a hierarchical optimization strategy. First, to avoid smoothing distortion due to insufficient data points at the beginning of the trajectory, forward window dynamic smoothing is used for preprocessing. For the first data point in the trajectory sequence... The position of each point after smoothing. Calculated from the points within the forward window:

[0091]

[0092] in, This is the actual size of the forward window. This represents the total number of points on the current trajectory. and The first in the window The x and y coordinates of the original trajectory points.

[0093] Subsequently, the preprocessed trajectory point sequence is smoothed in real time. This stage uses a fixed window (the smoothing window in this embodiment). The moving average algorithm is set to 7). For the... points ( ), its smooth position From window The points within are calculated to obtain:

[0094]

[0095] During the calculation process, the system identifies outliers within the window. For example, if the distance between a point and its neighboring points exceeds a preset threshold, these outliers are removed when calculating the window average. In this embodiment, points whose distance to their neighboring points is less than or equal to twice the step size are retained, thereby enhancing the robustness of the algorithm. To further improve the trajectory quality, this embodiment also constructs and iteratively optimizes an objective function containing smoothness and curvature terms, performing several (e.g., three) iterations on the trajectory to ensure that the final trajectory is smooth while maintaining curvature continuity.

[0096] After smoothing, the system outputs the final smoothed trajectory, which is used to directly control the movement of each unit in the cluster. For example... Figure 6 As shown, the smoothed velocity curve is continuous and gentle, without abrupt changes. This indicates that the change in motion acceleration is gentle, which helps to reduce actuator load, save energy, and significantly improve the smoothness and stability of the overall motion of the cluster.

[0097] To verify the effectiveness of the method proposed in this invention, experiments were conducted in a simulation environment. Throughout the simulation, the system switched navigation modes in real time according to the environment. The final planning result is as follows. Figure 3 As shown, the centerline of the virtual pipe can smoothly bypass obstacles and automatically contract in narrow passages, accurately passing through the center of the passage along the central axis. Figure 4 The adaptive change process of the pipe radius throughout the entire journey is clearly demonstrated. When passing through narrow areas, the radius dynamically shrinks from the standard value of 6m to a smaller safe value according to the channel width, and then recovers after passing through, which intuitively reflects the adaptive capability of the present invention.

[0098] Figure 5 The final movement paths of the four units within the cluster (represented by trajectories of different colors) are shown. All units successfully reached the target area and maintained good formation throughout the process without collision. Analysis of the simulation data shows that the total time for the entire task was 64 seconds, the total length of the planned trajectory of the cluster was 109.96 meters, and the average position error upon reaching the target point was 0.97 meters. These quantitative indicators, combined with the smooth velocity curve ( Figure 6 The results comprehensively demonstrate the excellent performance of the method of the present invention in terms of path length optimization, traffic efficiency, formation maintenance accuracy, and motion smoothness.

[0099] Finally, it should be noted that the features mentioned and / or shown in the above description of exemplary embodiments of the present invention can be combined in the same or similar manner with one or more other embodiments, combined with or substituted for corresponding features in other embodiments. These combined or substituted technical solutions should also be considered to be included within the scope of protection of the present invention.

Claims

1. A method for unmanned cluster trajectory planning based on the Tangent Bug algorithm and real-time virtual pipeline, characterized in that, The method manages the switching of navigation modes through a state machine and includes the following steps: Step 1, Environment Initialization and Parameter Configuration: Define the task environment map, the starting position and global target position of the unmanned swarm, obstacle information, configure sensor parameters, and initialize the radius parameters of the virtual pipeline and the initial navigation mode of the state machine; Step 2, Real-time Environmental Perception, Pattern Decision-Making, and Dynamic Construction of Virtual Pipelines: Based on real-time environmental information acquired by sensors, obstacle distribution and channel structure are detected, and navigation mode decisions and virtual pipeline construction are made accordingly. If a narrow channel that meets the passage conditions is identified, the narrow channel navigation mode is entered, and the centerline of the virtual pipeline is adjusted to align with the central axis of the narrow channel, while the pipeline radius is adjusted according to the channel width. If no narrow channel is identified, the system switches between boundary following mode and target approach mode based on the obstacle detection results, and the centerline position of the virtual pipeline is determined according to the corresponding mode, while the radius of the virtual pipeline remains at a preset standard radius. Step 3, Local Motion Planning: Based on the navigation mode decided in Step 2 and the corresponding virtual pipeline centerline position, determine the extension guidance direction of the pipeline centerline; wherein, in the narrow passage navigation mode, the direction of the central axis of the passage is used as the guidance direction of the cluster; in the boundary following mode, based on the Tangent Bug algorithm, the tangent direction along the obstacle boundary is calculated as the obstacle avoidance motion direction of the cluster; in the target approach mode, based on the Tangent Bug algorithm, the direction from the current position to the global target position is calculated as the approach motion direction of the cluster. Step 4, Cluster Formation Trajectory Generation: Using the virtual pipeline centerline determined in Step 2 as the reference and the extension guidance direction determined in Step 3 as the forward direction, the position of each unit in the cluster within the pipeline is calculated in real time according to the preset formation configuration, and the original motion trajectory of each unit is generated. Step 5, Trajectory Output: Output the motion trajectory generated in Step 4 for cluster motion control.

2. The unmanned swarm trajectory planning method according to claim 1, characterized in that, Before outputting the motion trajectory in step 5, the process also includes a step of smoothing and filtering the original motion trajectories of each unit generated in step 4.

3. The unmanned swarm trajectory planning method according to claim 1 or 2, characterized in that, In step 2, narrow passages that meet the passage conditions are identified, specifically: when the gap width between two obstacles is detected. Meet the conditions At that time, it was determined to be a narrow passage, in which The minimum radius of the virtual pipe. The maximum radius of the virtual pipe; In narrow passage navigation mode, the pipe radius is adjusted according to the passage width; specifically, the radius of the virtual pipe is adjusted to the passage radius. , ,in This is a preset safety margin.

4. The unmanned swarm trajectory planning method according to claim 1 or 2, characterized in that, In step 2, in the narrow passage navigation mode, the centerline of the virtual pipeline is adjusted to align with the central axis of the narrow passage. Specifically, this includes: calculating the surface points of the first obstacle based on the center position vector of the first obstacle and the radius of the first obstacle, and the center position vector of the second obstacle and the radius of the second obstacle. Position vector and second obstacle surface point Position vector; and The midpoint of the connecting line is taken as the point through which the virtual pipeline centerline should pass within the channel, and perpendicular to... and The direction of the connecting line serves as the central axis of the narrow passage.

5. The unmanned swarm trajectory planning method according to claim 1 or 2, characterized in that, In step 2, the preset switching conditions for exiting the boundary-following mode include any of the following: The sensor detects that there are no obstacles blocking the way ahead, or that it is about to enter a recognized narrow passage; The straight path from the current position to the global target position is confirmed to be unobstructed, and the Euclidean distance from the current position to the global target point is less than the minimum historical distance recorded during the boundary following process.

6. The unmanned swarm trajectory planning method according to claim 1 or 2, characterized in that, In step 3, the tangent direction along the obstacle boundary is calculated as the obstacle avoidance movement direction of the cluster in the boundary-following mode. Specifically, this includes: Based on the detected obstacle radius Current radius of virtual pipeline and the distance from the current position to the center of the obstacle. Calculate the tangent angle The calculation formula is: ; According to the tangent angle And the vector direction from the current position to the center of the obstacle, to determine the unit vector of the two candidate tangent directions; Select the candidate tangent direction that has the larger dot product with the unit vector pointing to the global target direction as the current obstacle avoidance motion direction.

7. The unmanned swarm trajectory planning method according to claim 6, characterized in that, In step 3, when determining the actual movement direction of the cluster in the boundary-following mode, a direction mixing mechanism is introduced, and the actual movement direction vector is calculated using the following formula. : In the formula, Let be the unit vector of the selected tangent direction. Let be a unit vector pointing from the current position to the global target position. The weighting coefficients are adjustable and .

8. The unmanned swarm trajectory planning method according to claim 1 or 2, characterized in that, In step 3, the direction from the current position to the global target position is calculated as the cluster's approach direction in the target-oriented mode. This specifically includes: Calculate from the current position Point to global target location Direction vector The calculation formula is: ,in For the smallest positive number used to prevent division by zero errors; Based on the direction vector and dynamically adjusted local target distance Determine the local target point The calculation formula is: ; With the local target point and the direction vector As a basis for the approaching direction of the cluster.

9. The unmanned swarm trajectory planning method according to claim 1 or 2, characterized in that, In step 4, the preset formation configuration is an inscribed square configuration; The position of each unit within the pipeline is determined as follows: using the centerline of the virtual pipeline as the reference point, and based on the vertex distribution model of the inscribed square, combined with the current virtual pipeline radius, the coordinate offset of each unit relative to the reference point is calculated, thereby obtaining the absolute position of each unit.

10. The unmanned swarm trajectory planning method according to claim 2, characterized in that, The smoothing filtering process includes preprocessing using a forward window dynamic smoothing algorithm, followed by real-time smoothing using a fixed window moving average algorithm.

Images