Unmanned cluster trajectory planning method based on tangent bug algorithm and real-time virtual pipeline

Abstract

The application 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. The method manages the switching of three navigation modes through a state machine. After perceiving the environment in real time, if a narrow channel is identified, the special mode is entered, the pipeline center line is aligned with the channel axis and the radius is contracted, and the cluster is guided in the direction of the center line; if the narrow channel is not identified, the switching between the border following mode and the target seeking mode is performed, the obstacle avoidance tangent direction or the direct approaching direction is calculated based on the tangent bug algorithm, and the pipeline radius remains a standard value. Finally, the virtual pipeline constructed dynamically is taken as a unified space reference, the motion trajectories of the units are solved in real time according to a preset formation configuration, and the trajectories are output. The application effectively solves the problems of fixed pipeline radius, low narrow channel passing efficiency and insufficient formation cooperation in the prior art, and realizes the safe, efficient and cooperative motion of the unmanned cluster in a complex environment.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of unmanned swarm cooperative control and trajectory planning, and particularly relates to a multi-agent autonomous navigation and obstacle avoidance method in a complex dynamic environment, more particularly to an unmanned swarm trajectory planning method based on a Tangent Bug algorithm and a real-time adaptive virtual pipe. BACKGROUND

[0002] With the wide application of intelligent agents such as unmanned aerial vehicles, mobile robots and autonomous vehicles in fields such as surveying, search and rescue and monitoring, unmanned swarm cooperative control has become a frontier research direction in robotics. Among them, the virtual pipe technology can effectively simplify the obstacle avoidance decision by constructing a spatial safety area to guide the formation movement. However, the existing research results still have the following obvious deficiencies in the virtual pipe construction related technology.

[0003] Firstly, the radius of the virtual pipe is mostly fixed, which is difficult to adapt to environmental changes. In a narrow channel, a fixed radius that is too large is easy to cause collision between the robot and the obstacle; in an open area, a radius that is too large leads to loose swarm formation, which not only affects the information exchange efficiency, but also may affect the task execution effect due to out-of-control formation.

[0004] Secondly, in the narrow channel scene, the existing method often regards other units in the swarm as dynamic obstacles, introduces a large number of redundant constraints, and leads to complex optimization problems and difficult solutions. This makes it easy for robots to have path conflicts, and often causes the swarm formation to split in typical narrow areas such as bridges and valleys, and some robots cannot plan a feasible path and thus leave the formation.

[0005] In addition, the existing method often independently performs the swarm formation layout design and trajectory planning in steps, and lacks collaborative optimization. The single planner or the potential field-based method is often used to plan the path of each unit in the trajectory planning, and the influence of the overall configuration of the formation on the path feasibility is not fully considered, which leads to the problems of path intersection, collision or formation tearing among units in narrow or complex environments. When adjusting the formation layout, static configuration library matching or simple feedback control based on relative position constraints is often used, which lacks collaborative constraints on the coordinated consistency and overall safety of the trajectories of each unit in the continuous motion of the swarm, which makes the motion of each unit in the swarm difficult to synchronize, and the formation stability is insufficient, which is difficult to meet the high-precision cooperative task requirements. SUMMARY

[0006] The purpose of the present application is to solve the problems of fixed pipe radius, low efficiency in narrow channel and insufficient formation cooperation in the existing unmanned swarm trajectory planning, and to propose a real-time trajectory planning method combining dynamic virtual pipe and state machine navigation strategy to improve the overall task execution capability of the unmanned swarm in complex environments.

[0007] To achieve the above object, the technical solution provided by the present application is:

[0008] An unmanned cluster trajectory planning method based on Tangent Bug algorithm and real-time virtual pipeline is provided, the method manages the switching of navigation modes through a state machine, and comprises the following steps:

[0009] Step 1, environment initialization and parameter configuration: defining the task environment map, the starting position and global target position of the unmanned cluster, obstacle information, configuring sensor parameters, and initializing the radius parameter of the virtual pipeline and the initial navigation mode of the state machine;

[0010] Step 2, real-time environment perception, mode decision and virtual pipeline dynamic construction: based on the real-time environment information obtained by the sensor, the obstacle distribution and channel structure are detected, and navigation mode decision and virtual pipeline construction are performed accordingly; if a narrow channel meeting the passing condition is identified, the narrow channel navigation mode is entered, the center line position of the virtual pipeline is adjusted to align with the center axis of the narrow channel, and the pipeline radius is adjusted according to the channel width; if no narrow channel is identified, switching is performed between the boundary following mode and the target approaching mode according to the obstacle detection result, and the center line position of the virtual pipeline is determined according to the corresponding mode, and the radius of the virtual pipeline remains the preset standard radius;

[0011] Step 3, local motion planning: according to the navigation mode and the corresponding virtual pipeline center line position decided in step 2, the extension guide direction of the pipeline center line is determined; in the narrow channel navigation mode, the center axis direction of the channel is taken as the guide direction of the cluster; in the boundary following mode, the tangent direction along the obstacle boundary is calculated as the obstacle avoidance motion direction of the cluster based on the Tangent Bug algorithm; in the target approaching mode, the direction from the current position to the global target position is calculated as the approaching motion direction of the cluster based on the Tangent Bug algorithm;

[0012] Step 4, cluster formation trajectory generation: taking the virtual pipeline center line determined in step 2 as the reference, taking the extension guide direction determined in step 3 as the forward direction, and according to the preset formation configuration, the positions of each unit in the pipeline are calculated in real time to generate the original motion trajectory of each unit;

[0013] Step 5, trajectory output: outputting the motion trajectory generated in step 4 for cluster motion control.

[0014] Further, before the motion trajectory is output in step 5, the step of smoothing and filtering the original motion trajectory of each unit generated in step 4 is further included.

[0015] Further, in step 2, a narrow passage meeting the passing condition is identified, specifically: when the gap width between two obstacles is detected to meet the condition that , , is the minimum radius of the virtual tube, and is the maximum radius of the virtual tube. The tube radius is adjusted according to the passage width in the narrow passage navigation mode, specifically: the radius of the virtual tube is adjusted to the passing radius , , wherein is a preset safety margin.

[0016] Further, in step 2, in the narrow passage navigation mode, the center line position of the virtual tube is adjusted to align with the central axis of the narrow passage, specifically including: based on the first obstacle center position vector and the first obstacle radius, the second obstacle center position vector and the second obstacle radius, the position vector of the first obstacle surface point and the position vector of the second obstacle surface point are calculated respectively; the midpoint of the line connecting and is taken as the point through which the virtual tube center line should pass in the passage, and the direction perpendicular to the line connecting and is taken as the central axis direction of the narrow passage.

[0017] Further, in step 2, the preset switching condition for exiting the boundary following mode includes any one of the following:

[0018] The sensor detects that there is no obstacle in front, or detects that it is about to enter an identified narrow passage;

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

[0020] Further, in step 3, in the boundary following mode, the tangent direction along the obstacle boundary is calculated as the obstacle avoidance motion direction of the swarm, specifically including:

[0021] According to the detected obstacle radius , the current radius of the virtual tube , and the distance from the current position to the obstacle center , the tangent angle is calculated, and 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 the present application are:

[0034] 1. The unmanned cluster trajectory planning method based on Tangent Bug algorithm and real-time virtual pipeline provided by the present application intelligently switches and uniformly manages three navigation modes of narrow channel navigation, boundary following and target seeking through a state machine, effectively overcoming the problems of fixed pipeline radius, low narrow channel passing efficiency and insufficient formation coordination in existing methods. After perceiving the environment in real time, if a narrow channel is identified, the method enters a dedicated navigation mode (narrow channel navigation mode) and aligns the virtual pipeline with the channel axis and shrinks the radius, thereby providing a safe and central passing path for the cluster, significantly improving the passing safety and efficiency in the restricted space. For general scenarios of non-narrow channels, the state machine switches between boundary following and target seeking modes, and real-time obstacle avoidance or approaching direction is planned based on the Tangent Bug algorithm. At the same time, by taking a unified and dynamic virtual pipeline as a spatial reference and calculating the trajectories of each unit in real time according to the preset formation configuration, the inherent consistency and motion synchronization of the cluster in complex maneuvers are ensured, fundamentally realizing the deep coordination of formation layout and trajectory planning.

[0035] 2. The present application effectively eliminates high-frequency jitter and sharp turns in the motion instructions by real-time smoothing and optimization of the generated trajectory, generating a smooth trajectory that meets the dynamics constraints, thereby improving the continuity, control stability and energy efficiency of the cluster motion. BRIEF DESCRIPTION OF DRAWINGS

[0036] The above and / or other features and advantages of the present application will become more apparent by describing in following reference to the accompanying drawings, in which:

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

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

[0039] Figure 3 is a trajectory planning result schematic diagram in an obstacle environment of the present application embodiment;

[0040] Figure 4 is a change curve diagram of the adaptive adjustment of the virtual pipeline radius in the present application embodiment;

[0041] Figure 5 is a motion trajectory result diagram of each unit in the cluster in the present application embodiment;

[0042] Figure 6 is the speed change curve of each unit in the cluster in the embodiment of the application. DETAILED DESCRIPTION

[0043] The application will be described in detail below with reference to the accompanying drawings and exemplary embodiments of the application. It should be pointed out that the following detailed description of the application is for illustrative purposes only and is not limiting on the application.

[0044] Referring to Figure 1 , the unmanned cluster trajectory planning method based on Tangent Bug algorithm and real-time virtual pipeline as an exemplary embodiment of the application intelligently manages the switching between three navigation modes, i.e., narrow channel navigation mode, boundary following mode and target-oriented mode, through a state machine. The method closely couples environment perception, navigation decision and virtual pipeline construction, and guides the unmanned cluster to move from the starting position to the target position safely, efficiently and cooperatively. The following will be described in detail according to the flow steps.

[0045] Step S1, environment initialization and parameter configuration

[0046] First, the initialization definition of the running environment is performed. The range of the two-dimensional task map is set to 100m*100m, and it is clear that the unmanned cluster needs to move from the starting position to the global target position . The obstacle information in the initialized environment is described by the center coordinates and the radius of each obstacle in this embodiment. At the same time, the sensor parameters carried on the cluster unit are configured, and the detection range is set to 15m, and the field of view angle is set to .

[0047] Next, the parameters of the virtual pipeline are initialized: the standard radius is 6m, the minimum allowable radius is 2m, the maximum allowable radius is 6m, . At the same time, the initial navigation mode of the state machine is set, which is usually set to the target-oriented mode. This step establishes a complete running framework and initial state for the entire planning system, as shown in Figure 2 , the initial scene includes the starting point, the target point, the obstacle and the initial virtual pipeline constructed with the standard radius.

[0048] Step S2, real-time environment perception, mode decision and dynamic construction of virtual pipeline

[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 connection as the central axis direction :

[0056]

[0057]

[0058]

[0059] This series of operations can ensure that the cluster can safely pass through the narrowest area in the most centered posture, and the radius of the pipeline can be adaptively contracted and restored with the width of the passage, avoiding the collision or loose formation problem caused by fixed radius.

[0060] In the conventional scene where no narrow passage is identified, such as an open area or only isolated obstacles exist, the system switches between the boundary-following mode and the target-oriented mode, and adjusts the center line position of the virtual pipeline in the corresponding navigation mode, while the radius of the virtual pipeline remains the preset standard radius of the initial configuration .

[0061] When the sensor detects that there is an obstacle in front of the path to the target, it switches to the boundary-following mode. At this time, the radius of the virtual pipeline remains the standard radius , and the center line position of the pipeline is adjusted according to the obstacle avoidance requirement to ensure that it can extend along the safe tangent direction of the obstacle.

[0062] When there is no obstacle in the sensor detection range, the system enters the target-oriented mode. At this time, the radius of the virtual pipeline also remains the standard radius , and the center line of the pipeline is oriented towards the target point, and its position is determined to ensure that the whole formation maintains a safe distance from the possible lateral obstacles in the environment.

[0063] The switching conditions between modes are carefully designed. For example, the conditions for exiting the boundary-following mode include two parallel situations, either of which can trigger mode switching: one situation is that the sensor detects that there is no obstacle in front, or it is about to enter the identified narrow passage. This mechanism ensures that the robot can respond immediately when the obstacle threat disappears or enters a new scene; the other situation is that the straight-line path from the current position to the global target position is obstacle-free, 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. This situation is a heuristic optimization that ensures the robot can leave the boundary and head for the target as soon as it finds a better direct path than the historical detour path. This mechanism significantly improves the global near-optimality of the path under the premise of ensuring safe obstacle avoidance, and is the key to ensuring the near-optimality of the Tangent Bug algorithm.

[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 converting 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. is a unit vector pointing from the current position to the global target position, is an adjustable weight coefficient and , The greater the value, the more the motion direction focuses on obstacle avoidance. The smaller the value, the more the motion direction focuses on the global target, which is 0.4 in this embodiment.

[0076] In the target-seeking mode, the "move-to-target" behavior is also based on the Tangent Bug algorithm. In this mode, the system calculates the direction vector directly pointing to the global target position , based on the direction vector and the dynamically adjusted local target distance , determines a local target point , and takes the local target point and the direction vector as the approaching motion direction of the cluster:

[0077]

[0078]

[0079] wherein is a very small positive number used to prevent division by zero errors, which is in this embodiment.

[0080] Step S4, cluster formation trajectory generation

[0081] This step aims to convert the unified pipeline guide into the motion trajectory of each unit in the cluster. As shown in Figure 5 , although there are individual differences in each unit trajectory, the overall trajectory is constrained within the unified virtual pipeline. Specifically, the virtual pipeline center line constructed in step S2 is used as the spatial reference, and the guide direction calculated in step S3 is used as the forward direction. According to the preset formation geometry (in this embodiment, the inscribed square geometry), the exact position of each unit in the pipeline cross section is calculated in real time. Taking the inscribed square as an example, the system takes the current position of the pipeline center as the reference point, calculates the coordinate offset of each unit relative to the center according to the current radius of the pipeline and the square vertex distribution model, and determines the absolute position of each unit. For a typical formation consisting of four units, their positions can be calculated by the following formulas:

[0082]

[0083]

[0084]

[0085]

[0086] In this configuration, the distance between any two adjacent units satisfies . Regardless of the dynamic change of the pipeline radius , this geometric model-based solving mechanism can automatically maintain the preset relative position relationship between units, such as the initial configuration shown in Figure 2 . This ensures that the cluster can naturally and stably maintain the formation during complex maneuvers, thereby maintaining reliable communication links and cooperative perception capabilities between units and enhancing the robustness of the overall operation.

[0087] Step S5, trajectory output

[0088] This step is the final link of the planning process, and its task is to output the trajectory information generated in step S4 for describing the future motion of each unit in the cluster for execution by the underlying motion controller. The data stream or instruction set output can directly drive each unmanned platform to control its motion along the planned path, realizing a closed loop from planning to execution. After the output is completed, the system will return to step S2 to perform the perception-planning-output cycle of the next period, thereby realizing online real-time navigation of the cluster in a dynamic environment.

[0089] In order to further improve the quality of the final execution trajectory, the original trajectory generated in step S4 can be smoothed and optimized before output. For this purpose, the parameters required for trajectory smoothing, such as the size of the smoothing window , need to be set in step S1.

[0090] The smoothing process can adopt a hierarchical optimization strategy. First, to avoid distortion caused by too few data points at the beginning of the trajectory, forward window dynamic smoothing is used for preprocessing. For the th point in the trajectory sequence, its smoothed position is calculated from the points within the forward window as follows:

[0091]

[0092] where is the actual forward window size, is the total number of current trajectory points, and are the horizontal and vertical coordinates of the th original trajectory point within the window, respectively.

[0093] Subsequently, real-time smoothing is performed on the preprocessed trajectory point sequence, and a fixed window (in this embodiment, the smoothing window 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 cluster motion.

[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 features mentioned and / or shown in the above description of exemplary embodiments of the application can be combined with one or more other embodiments in the same or analogous manner as the features of the other embodiments are combined, or as the corresponding features of the other embodiments are replaced. The technical solutions thus obtained by combination or replacement should also be regarded as included in the scope of protection of the application.

Claims

1. An unmanned cluster trajectory planning method based on Tangent Bug algorithm and real-time virtual pipeline, characterized in that, The method manages the switching of navigation modes by a state machine, and comprises the following steps: Step 1, environment initialization and parameter configuration: defining the task environment map, the starting position and global target position of the unmanned cluster, obstacle information, configuring sensor parameters, and initializing the radius parameter of the virtual pipeline and the initial navigation mode of the state machine; Step 2, real-time environment perception, mode decision and dynamic construction of virtual pipeline: based on the real-time environment information obtained by the sensor, the obstacle distribution and channel structure are detected, and navigation mode decision and virtual pipeline construction are performed accordingly; if a narrow channel meeting the passing condition is identified, the narrow channel navigation mode is entered, the center line position of the virtual pipeline is adjusted to align with the center axis of the narrow channel, and the pipeline radius is adjusted according to the channel width; if no narrow channel is identified, switching is performed between the boundary following mode and the target oriented mode according to the obstacle detection result, and the center line position of the virtual pipeline is determined according to the corresponding mode, and the radius of the virtual pipeline remains the preset standard radius; Step 3, local motion planning: according to the navigation mode and the corresponding virtual pipeline center line position decided in step 2, the extension guide direction of the pipeline center line is determined; in the narrow channel navigation mode, the center axis direction of the channel is taken as the guide direction of the cluster; in the boundary following mode, the tangent direction along the obstacle boundary is calculated as the obstacle avoidance motion direction of the cluster based on the Tangent Bug algorithm; in the target oriented mode, the direction from the current position to the global target position is calculated as the approaching motion direction of the cluster based on the Tangent Bug algorithm; Step 4, cluster formation trajectory generation: taking the virtual pipeline center line determined in step 2 as the reference, and taking the extension guide direction determined in step 3 as the forward direction, the original motion trajectories of each unit in the cluster are generated according to the preset formation configuration, and the original motion trajectories of each unit in the cluster are generated according to the preset formation configuration; Step 5, trajectory output: outputting the motion trajectories generated in step 4 for cluster motion control. 2.The unmanned cluster trajectory planning method of claim 1, wherein, Before the motion trajectories are output in step 5, a step of smoothing and filtering the original motion trajectories of each unit generated in step 4 is further included. 3.The unmanned cluster trajectory planning method of claim 1 or 2, wherein, In Step 2, a narrow passage that meets the passage condition is identified, specifically: when the gap width between two obstacles is detected to be less than or equal to the minimum radius of the virtual tube satisfies the condition , it is determined to be a narrow passage, where is the minimum radius of the virtual tube, is the maximum radius of the virtual tube; In the narrow channel navigation mode, the radius of the virtual pipeline is adjusted according to the channel width, specifically, the radius of the virtual pipeline is adjusted to the passing radius , wherein is a preset safety margin. 4.The unmanned cluster trajectory planning method of claim 1 or 2, wherein, 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 cluster trajectory planning method of claim 1 or 2, wherein, In step 2, the preset switching condition for exiting the boundary following mode includes any one of the following: The sensor detects that there is no obstacle in front, or detects that it is about to enter an identified narrow channel; The straight line path from the current position to the global target position is confirmed to be obstacle-free, and the Euclidean distance from the current position to the global target point is less than the minimum historical distance recorded during boundary following. 6.The unmanned cluster trajectory planning method of claim 1 or 2, wherein, In step 3, in the boundary following mode, the tangent direction along the obstacle boundary is calculated as the obstacle avoidance motion direction of the cluster, specifically including: According to the detected obstacle radius , the virtual pipe current radius , and the distance from the current position to the center of the obstacle , the tangent angle is calculated, the calculation formula is: ; According to the tangent angle and the vector direction from the current position to the center of the obstacle, the unit vectors of two candidate tangent directions are determined; Selecting a candidate tangent direction with a larger dot product with a unit vector pointing to the global target direction as the current obstacle avoidance motion direction.

7. The trajectory planning method for unmanned swarm according to claim 6, wherein, In step 3, a direction mixing mechanism is introduced when determining the actual moving direction of the cluster in the border-following mode, and the actual moving direction vector is calculated by the following formula : wherein is a unit vector in the selected tangent direction, is a unit vector pointing from the current position to the global target position, is an adjustable weight coefficient and . 8.The unmanned cluster trajectory planning method of claim 1 or 2, wherein, In step 3, in the target oriented mode, the direction from the current position to the global target position is calculated as the approaching motion direction of the cluster, specifically including: Compute from current position Direction vector pointing to global target position with the formula where is a very small positive number for preventing 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 the approach movement direction basis for the cluster. 9.The unmanned cluster trajectory planning method of claim 1 or 2, wherein, In step 4, the preset formation configuration is a circumscribed square configuration; The position of each unit in the pipeline is determined by taking the virtual pipeline center line as a reference point, calculating the coordinate offset of each unit relative to the reference point according to the vertex distribution model of the inscribed square, and combining the current virtual pipeline radius to obtain the absolute position of each unit. 10.The unmanned cluster trajectory planning method of claim 2, wherein, The smoothing filtering process comprises pre-processing by using a forward window dynamic smoothing algorithm, and then real-time smoothing by using a fixed window moving average algorithm.

Images