A distributed method and system for local replanning and real-time obstacle avoidance of unmanned aerial vehicles

Abstract

The application relates to the technical field of unmanned aerial vehicle path planning, in particular to a distributed unmanned aerial vehicle local re-planning and real-time obstacle avoidance method and system, which comprises the following steps: real-time detection of obstacles, triggering of local trajectory re-planning when an expanded obstacle region intersects with a global guide trajectory of the unmanned aerial vehicle; independent generation of a local re-planning trajectory based on an improved time elastic band algorithm and broadcasting of the local re-planning trajectory to a decision set for distributed conflict detection; construction of a constraint tree by each conflict unmanned aerial vehicle in a conflict set through a high-level node splitting method based on enhanced conflict search according to a fixed time window synchronization mechanism; calculation of the total cost of each sub-node of the constraint tree through the same cost function and calculation rule, selection of an optimal sub-node according to the total cost of each sub-node; and repeated selection of the optimal sub-node to obtain a conflict-free executable trajectory. The application solves the deadlock problem caused by close-range unmanned aerial vehicle re-planning trajectory conflicts in a dense obstacle environment.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) path planning technology, and in particular to a distributed UAV local replanning and real-time obstacle avoidance method and system. Background Technology

[0002] Drones may encounter sudden dynamic obstacles while executing a global trajectory, but the trajectories generated by traditional distributed and reactive obstacle avoidance algorithms are not smooth and are prone to deadlock in dense scenes.

[0003] Traditional obstacle avoidance algorithms for multiple drones are mainly divided into two categories: global planning and local planning. Global planning methods rely on complete prior environmental information for centralized offline computation. Their core advantage lies in achieving optimal or suboptimal path solutions. However, they are highly dependent on accurate environmental models, making it difficult to adapt to dynamic and unknown obstacles. Furthermore, their computational complexity increases sharply with scale, resulting in real-time bottlenecks. Urban environments contain sudden and dynamic obstacles, requiring obstacle avoidance algorithms to meet real-time and feasibility requirements. Therefore, local planning methods are more suitable for real-time obstacle avoidance. These methods generally rely on distributed architectures. Traditional distributed methods, such as the Artificial Potential Field (APF) method, guide drones through virtual force fields, but are prone to getting trapped in local minima and oscillations. ORCA directly solves for collision-free velocities through geometric calculations. Due to its computational efficiency and theoretical completeness, it is suitable for local obstacle avoidance. However, its model is an omnidirectional motion model, which is insufficient for handling drone dynamic constraints and is prone to causing group deadlock in dense scenes. Distributed Model Predictive Control (DMPC) constructs the obstacle avoidance problem as a constrained local optimization problem in the rolling time domain. It can explicitly handle state and input constraints and generate flatter trajectories, but its online computational burden is heavy and it is highly sensitive to model accuracy.

[0004] In the field of local optimization, the Timed Elastic Band (TEB) algorithm represents the robot's trajectory as a series of time-varying posture sequences (i.e., "elastic bands"). Under reasonable constraints, the algorithm simultaneously adjusts the posture sequences and their time intervals. However, its computational complexity increases with the number of optimization variables and relies on initial guesses, potentially leading to local optima or the inability to solve in real time. Local obstacle avoidance and replanning for unmanned aerial vehicles (UAVs) require real-time and accurate perception of surrounding obstacles using limited onboard sensor information, and the rapid generation of safe, smooth, and dynamically consistent local trajectories within the constraints of computational resources and flight stability. However, UAVs are susceptible to signal shielding and various electromagnetic interferences in urban environments, making long-term, long-distance communication difficult. Distributed obstacle avoidance algorithms are prone to local optima and deadlock, and the lack of a global perspective results in low group collaboration efficiency; the generated trajectories often lack theoretical guarantees in terms of safety and smoothness. Although distributed methods offer significant advantages in handling dynamic problems, ensuring millisecond-level response while properly addressing issues such as deadlock oscillations caused by local decisions, dynamic feasibility, and rigorous safety verification remains a core bottleneck that urgently needs to be overcome in this field. To achieve the advantages of both centralized and distributed algorithms, local replanning needs to be combined with global guidance trajectories to seamlessly connect and smoothly transition local emergency obstacle avoidance actions with predetermined global task objectives and top-level constraints. This is the key to solving this type of problem.

[0005] Global trajectory planning can only adapt to known environments and is difficult to cope with dynamic environmental changes. Traditional distributed obstacle avoidance algorithms mostly rely on local perception and real-time communication. In dense obstacle environments, if multiple UAVs encounter sudden obstacles and simultaneously complete trajectory replanning, data interaction will reveal that the replanned trajectories conflict with each other. This will cause the UAVs to be unable to make effective decisions and generate feasible trajectories. Existing methods have fixed priorities, which can easily lead to the loss of optimality of solutions, while high-frequency communication places extremely high demands on computing power. Summary of the Invention

[0006] This invention aims to at least solve one of the technical problems existing in related technologies. To this end, this invention provides a distributed UAV local replanning and real-time obstacle avoidance method and system, which quickly generates smooth and feasible replanning trajectories to meet real-time requirements; it solves the problems caused by deadlock, unsmooth trajectories, insufficient dynamic feasibility, low collaborative efficiency due to lack of global vision, and high computing power requirements of high-frequency communication caused by close-range UAV replanning trajectory conflicts in dense obstacle environments, thus ensuring the safety and stability of multi-UAV collaborative operations.

[0007] This invention provides a distributed unmanned aerial vehicle (UAV) local replanning and real-time obstacle avoidance method, comprising: S1: Multiple low-altitude logistics delivery drones fly according to a pre-planned global guidance trajectory and detect obstacles in real time. When the obstacle avoidance safety boundary after the obstacle expands and the state node on the future global guidance trajectory of the drone are spatially intersected at the same time, local trajectory replanning is triggered. S2: The UAV that triggers local trajectory replanning independently generates a local replanning trajectory based on the improved time elastic band algorithm; S3: Each UAV will broadcast its generated local replanning trajectory to the decision set for distributed conflict detection; if there is a conflict in the replanning trajectory of the UAV, then the UAV will be included in the conflict set. S4: Based on the synchronization mechanism of the fixed time window, each conflicting UAV in the conflict set constructs a constraint tree through the high-level node splitting method based on Enhanced Conflict-Based Search (ECBS); the total cost of each child node of the constraint tree is calculated using the same cost function and calculation rules, and the child node with the minimum total cost is selected as the optimal child node; S5: If there is a trajectory conflict among the UAVs in the optimal child node, repeat step S4 until the replanning trajectories of all conflicting UAVs are conflict-free, and obtain a conflict-free executable trajectory.

[0008] Furthermore, the obstacle avoidance safety boundary after the obstacle expands includes the expanded static obstacle area and the expanded dynamic obstacle area; Expand the original boundary of the static obstacle outward to the diameter of the UAV itself to obtain the expanded static obstacle avoidance safety boundary. The expanded static obstacle avoidance safety boundary encloses the expanded static obstacle region. The original boundary of the static obstacle is the convex hull boundary after point cloud clustering. Expand the original boundary of the dynamic obstacle outward to the diameter of the UAV itself to obtain the expanded dynamic obstacle avoidance safety boundary. The expanded dynamic obstacle avoidance safety boundary encloses the expanded dynamic obstacle region. The original boundary of the dynamic obstacle is the circumscribed geometry of the current position of the dynamic obstacle. If there is at least one state node on the future global guidance trajectory, and the drone and the expanded static obstacle area or the expanded dynamic obstacle area have spatial intersection at the same time, then the drone and the obstacle are at risk of collision, triggering local trajectory replanning.

[0009] Furthermore, the improved temporal elastic band algorithm constructs a spatiotemporal elastic band model that includes three-dimensional position, attitude, and time. The spatiotemporal elastic band model connects the starting point and target point state of the local replanning trajectory with a trajectory having multiple control points, and designs a hierarchical weighted quadratic multi-objective cost function based on the distance between the control points and obstacles. The multi-objective cost function is then solved quickly using a sparse matrix solver.

[0010] Furthermore, the hierarchical weighted quadratic multi-objective cost function based on the distance between the control point and the obstacle is a weighted sum of squares of trajectory matching cost, obstacle avoidance cost, smoothness cost, and speed cost; the weights of trajectory matching cost, obstacle avoidance cost, smoothness cost, and speed cost are set hierarchically according to the distance between the control point and the obstacle. Trajectory fitting cost is used to measure the degree of fit between the replanned trajectory and the global trajectory; Obstacle avoidance cost is used to measure the proximity of the elastic band control point to the obstacle expansion safety boundary; Smoothness cost is used to constrain the continuity and smoothness of the trajectory; Speed ​​cost is used to constrain speed continuity and time intervals.

[0011] Furthermore, the node splitting rule of the high-level node splitting method based on enhanced conflict search is as follows: Will The root node of the conflict drone splits into Each child node has only one drone executing the previously replanned trajectory, while the rest... The drone was used for local trajectory replanning.

[0012] Furthermore, step S4 includes: Construct a root node containing the first replanning trajectories of all UAVs in the conflict set; Each UAV in the conflict set uses a synchronization mechanism based on a fixed time window to generate a second replanning trajectory, constrained by the first replanning trajectory of other UAVs, and broadcasts the second replanning trajectory to other UAVs. If the second replanning trajectory of a UAV does not conflict with other UAVs, then the UAV is removed from the conflict set; the removed UAV executes the corresponding second replanning trajectory. After removing conflict-free drones, drones in the conflict set generate child nodes according to the node splitting rules. Calculate the total cost of each child node, and select the optimal child node based on the total cost of each child node.

[0013] Furthermore, the total cost of each child node is a weighted sum of the trajectory's own cost and the conflict cost, where the conflict cost is the number of all trajectory conflicts within the child node.

[0014] Furthermore, the synchronization mechanism of the fixed timing window includes: Set system time to a preset cycle Segmentation, defining replanning and decision-making time within a preset period. ; exist Internally, it collects replanning state information of conflict-affected drones and constructs a conflict set, in which... This marks the start of the current replanning cycle; exist At that time, the drones in the conflict set are replanned, and the replanning results are broadcast. exist Inside, complete the replanning of all drones, clear the conflict set, and obtain conflict-free executable trajectories.

[0015] Furthermore, if there is a trajectory conflict among the drones in the current optimal child node, the node splitting and replanning will continue to be performed with the optimal child node as the new root node until a child node that meets the conflict-free condition appears. In each node split, at least one drone's trajectory will not collide with the trajectories of other drones in that node, so as to ensure that the conflict set is cleared after at most N-1 splits, where N is the number of conflicting drones.

[0016] This invention also provides a distributed UAV local replanning and real-time obstacle avoidance system for executing the aforementioned distributed UAV local replanning and real-time obstacle avoidance method, comprising: The replanning trigger module is used for multiple low-altitude logistics delivery drones to fly according to a pre-planned global guidance trajectory and detect obstacles in real time. When the obstacle avoidance safety boundary after the obstacle expands and the state node on the future global guidance trajectory of the drone are spatially intersected at the same time, local trajectory replanning is triggered. A replanning trajectory generation module is used to trigger local trajectory replanning of the UAV, which independently generates a local replanning trajectory based on an improved time elastic band algorithm. The conflict detection module is used by each UAV to broadcast its generated local replanning trajectory to a decision set for distributed conflict detection; if the replanning trajectory of a UAV is in conflict, then the UAV is included in the conflict set. The optimal child node selection module is used to construct a constraint tree for each conflicting UAV in the conflict set according to the synchronization mechanism of a fixed time window, through a high-level node splitting method based on enhanced conflict search; calculate the total cost of each child node of the constraint tree through the same cost function and calculation rules, and select the child node with the minimum total cost as the optimal child node. The conflict-free trajectory acquisition module is used to iteratively process the optimal sub-node selection module if there is a trajectory conflict among the UAVs in the optimal sub-node, until the replanning trajectories of all conflicting UAVs are conflict-free, thus obtaining a conflict-free executable trajectory.

[0017] The above-described one or more technical solutions in the embodiments of the present invention have at least one of the following technical effects: This invention can quickly generate smooth and feasible replanning trajectories to meet real-time requirements; it solves problems such as deadlock, uneven trajectory, insufficient dynamic feasibility, low collaborative efficiency due to lack of global vision, and high computing power requirements for high-frequency communication caused by replanning trajectory conflicts of close-range UAVs in dense obstacle environments, thus ensuring the safety and stability of multi-UAV collaborative operations.

[0018] Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0019] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0020] Figure 1 This is a flowchart illustrating a distributed UAV local replanning and real-time obstacle avoidance method provided by the present invention.

[0021] Figure 2 This is a timing diagram of the synchronization mechanism with a fixed timing window provided in an embodiment of the present invention.

[0022] Figure 3 This is a schematic diagram of the structure of a distributed unmanned aerial vehicle (UAV) local replanning and real-time obstacle avoidance system provided by the present invention.

[0023] Figure 4 This is the trajectory curve provided in the embodiments of the present invention, wherein Figure 4 Figure (a) shows the trajectory curve generated by the APF algorithm. Figure 4 Figure (b) shows the trajectory curve generated by the ORCA algorithm. Figure 4 Figure (c) shows the trajectory curves of the five drones generated by this invention.

[0024] Figure 5 This is a comparison chart of the planned trajectory and actual tracking results of the first UAV provided in this embodiment of the invention. Figure 5 Figure (a) shows the first drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 5 Figure (b) shows the first drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 5 Image (c) shows the first drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 5 Figure (d) shows the yaw angle of the first UAV. A comparison chart of the planned trajectory and the actual tracking trajectory.

[0025] Figure 6 This is a comparison diagram of the planned trajectory and the actual tracking trajectory of the second UAV provided in this embodiment of the invention. Figure 6 Figure (a) shows the second drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 6 Figure (b) shows the second drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 6 Figure (c) shows the second drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 6 Figure (d) shows the yaw angle of the second drone. A comparison chart of the planned trajectory and the actual tracking trajectory.

[0026] Figure 7 This is a comparison diagram of the planned trajectory and the actual tracking trajectory of the third UAV provided in this embodiment of the invention. Figure 7 Figure (a) shows the third drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 7 Figure (b) shows the third drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 7 Figure (c) shows the third drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 7 Figure (d) shows the yaw angle of the third drone. A comparison chart of the planned trajectory and the actual tracking trajectory.

[0027] Figure 8 This is a comparison diagram of the planned trajectory and the actual tracking trajectory of the fourth UAV provided in this embodiment of the invention. Figure 8 Figure (a) shows the fourth drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 8 Figure (b) shows the fourth drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 8 Figure (c) shows the fourth drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 8 Figure (d) shows the yaw angle of the fourth UAV. A comparison chart of the planned trajectory and the actual tracking trajectory.

[0028] Figure 9 This is a comparison chart of the planned trajectory and the actual tracking trajectory of the fifth UAV provided in this embodiment of the invention. Figure 9 Figure (a) shows the fifth drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 9 Figure (b) shows the fifth drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 9 Figure (c) shows the fifth drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 9 Figure (d) shows the yaw angle of the fifth drone. A comparison chart of the planned trajectory and the actual tracking trajectory.

[0029] Figure label: 101. Replanning Trigger Module; 102. Replanning Trajectory Generation Module; 103. Conflict Detection Module; 104. Optimal Child Node Selection Module; 105. Conflict-Free Trajectory Acquisition Module. Detailed Implementation

[0030] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention. The following embodiments are used to illustrate this invention but cannot be used to limit the scope of this invention.

[0031] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., refer to specific features, structures, materials, or characteristics described in connection with that embodiment or example, which are included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0032] The following is combined with Figures 1 to 9 This invention describes a distributed unmanned aerial vehicle (UAV) local replanning and real-time obstacle avoidance method and system.

[0033] like Figure 1As shown, a distributed UAV local replanning and real-time obstacle avoidance method includes: S1: Multiple low-altitude logistics delivery drones fly according to a pre-planned global guidance trajectory and detect obstacles in real time. When the obstacle avoidance safety boundary after the obstacle expands and the state node on the future global guidance trajectory of the drone are spatially intersected at the same time, local trajectory replanning is triggered. In a distributed multi-UAV system, each UAV acts as an independent intelligent agent, employing a common hierarchical architecture of perception, planning, and decision-making. Static obstacles are pre-stored in the onboard map, and the perception layer only needs to detect unmodeled static and dynamic obstacles, and only those centered on the UAV's current position with a radius of... Obstacles within the area; to ensure sufficient time for replanning. Determined by the drone's maximum speed and average replanning response time, the calculation expression is: in, The maximum speed of the drone, To plan the average response time; To ensure obstacle avoidance safety, the detected obstacles need to be expanded and modeled. The obstacle avoidance safety boundary after the obstacle is expanded includes the expanded static obstacle avoidance safety boundary and the expanded dynamic obstacle avoidance safety boundary. Expand the original boundary of the static obstacle outward to the diameter of the UAV itself to obtain the expanded static obstacle avoidance safety boundary. The original boundary of the static obstacle is the convex hull boundary after point cloud clustering. Expand the original boundary of the dynamic obstacle outward to the diameter of the drone itself to obtain the expanded dynamic obstacle avoidance safety boundary. The original boundary of the dynamic obstacle is the circumscribed geometry of the dynamic obstacle at its current position. Let the original boundary of the detected static obstacle be... The original boundary of the dynamic obstacle is , Employing the convex hull boundary after point cloud clustering, Use the circumscribed geometry of its current location.

[0034] First, expand the boundary of the static obstacle outwards to the diameter of the drone itself. The expanded static obstacle avoidance safety boundary is obtained. The calculation expression is: in, For any point in three-dimensional space, For three-dimensional space, The expanded static obstacle region is the point cloud of a static obstacle. for The enclosed area.

[0035] The position and shape of dynamic obstacles change dynamically over time. Let the position prediction model for dynamic obstacles be: in, for The position of the obstacle is constantly changing. for The dynamic x-axis coordinates of the obstacle at any given time. for The dynamic y-axis coordinates of the obstacle at any given time. for The z-axis coordinate of the obstacle at any given time.

[0036] The dynamic obstacle avoidance safe zone for dynamic obstacles can be obtained. for: in, To find the minimum value of the function.

[0037] The expanded dynamic barrier region is The enclosed area.

[0038] When no obstacles are detected, the drone follows a global trajectory. Assume the drone's current position is... Then the future The position within each time step is , The obstacle avoidance trigger condition is as follows: if there is at least one state node on the future global guidance trajectory, and the UAV and the expanded static obstacle area or the expanded dynamic obstacle area have spatial intersection at the same time, then the UAV and the obstacle are at risk of collision, triggering local trajectory replanning.

[0039] When determining a collision, the center of mass of the drone is taken as the reference point and the circumscribed radius is... The sphere represents the drone's own area.

[0040] S2: The UAV that triggers local trajectory replanning independently generates a local replanning trajectory based on the improved time elastic band algorithm; The core idea of ​​the Temporal Elastic Band (TEB) algorithm is to abstract the local trajectory as an "elastic band" containing spatiotemporal information. Through multi-objective optimization and efficient solution using sparse matrices, it achieves real-time obstacle avoidance and trajectory smoothing based on a known global reference trajectory. TEB, based on the global trajectory, uses a sparse matrix to represent the starting and target states of the replanned trajectory. The trajectories of the control points are connected; each control point contains not only trajectory position information but also the robot's posture information, and motion time is defined between them. Because control points and motion time can control the shape of the trajectory, this deformable trajectory is likened to an elastic band. Furthermore, the algorithm constructs an optimization problem using control points, obstacles, and the original trajectory. Solving this optimization problem can rely on a large-scale sparse matrix, resulting in high computational efficiency and suitability for real-time applications.

[0041] However, the TEB algorithm is designed for ground robots, and its kinematic constraints differ significantly from the three-dimensional dynamics of UAVs. Ground robots primarily consider planar motion and nonholonomic constraints, while UAVs need to cope with omnidirectional motion in three-dimensional space, higher velocity and acceleration constraints, and stricter obstacle avoidance safety boundaries. Therefore, the original TEB algorithm needs to be specifically improved to adapt to the operational requirements of UAVs.

[0042] The improved TEB constructs a spatiotemporal elastic band model that includes three-dimensional position, attitude, and time. The spatiotemporal elastic band model connects the starting point and target point state of the local replanning trajectory with a trajectory having multiple control points, and designs a hierarchical weighted quadratic multi-objective cost function based on the distance between the control points and obstacles. The multi-objective cost function is solved quickly by a sparse matrix solver.

[0043] The constraints that must be met in low-altitude logistics delivery missions include environmental constraints, aircraft constraints, collision avoidance constraints, and time constraints.

[0044] Assuming drones in low-altitude logistics delivery missions The coordinates of the pickup point are The delivery point is .

[0045] Environmental constraints include static environmental constraints and dynamic environmental constraints. Static environmental constraints require all UAVs to avoid nodes occupied by static obstacles and not to enter no-fly zones. Dynamic environmental constraints require UAVs to obtain the position and speed of dynamic obstacles through real-time sensing and communication, and adjust their flight trajectories according to the position and speed of the dynamic obstacles.

[0046] The aircraft constraints require that the drone's speed must not exceed the maximum speed specified for the drone.

[0047] Time constraints require low-altitude logistics delivery to be completed within a certain time. Satisfy the upper limit constraint: in, This is an adjustable parameter used to flexibly control the strictness of task timing. This represents the distance value corresponding to the drone with the furthest Manhattan distance between the pickup and delivery points in the system. Time constraints ensure that the actual execution time of the task is controllable, avoiding a decrease in system efficiency due to delays caused by individual drones.

[0048] Metrics for measuring trajectory quality include trajectory smoothness and trajectory length.

[0049] Trajectory smoothness is a core physical quantity used to measure the drastic changes in control commands, and different quantification methods are employed at different planning levels. In actual UAV control, the second derivative of position is related to the UAV's total lift, the third derivative can be mapped to the rate of change of lift and torque, and the fourth derivative is related to energy. Therefore, the average equivalent acceleration or discrete differential acceleration of the trajectory is used as the quantification basis for trajectory smoothness. The smaller this value, the smoother the maneuvering of the UAV during emergency avoidance, and the smaller the instantaneous impact on the motors and ESCs.

[0050] The trajectory length is the actual length of the three-dimensional spatial curve traversed by the drone as it flies from the pickup point to the delivery point. A global algorithm uses short trajectories with fixed time intervals; adjusting the length of these trajectory segments indirectly controls the average speed without introducing additional velocity or energy terms into the cost function.

[0051] S21: Construct a spatiotemporal elastic band model that includes three-dimensional position, attitude, and time; The starting control point for the replanned trajectory is selected as the first vertex in the set of all global trajectory vertices contained after the obstacles are expanded multiple times. definition To expand the volume of the obstacle All contained after doubling A set of global trajectory vertices: , For drones No. vertex coordinates . This is the obstacle avoidance trigger coefficient. The larger the value, the greater the obstacle avoidance trigger distance for the drone, resulting in better obstacle avoidance performance. However, this also leads to a longer replanning trajectory and increases the algorithm's runtime.

[0052] drones The first replanned trajectory pose of control points The calculation expression is: in, To replan the trajectory of the first The location of each control point, and satisfying , Trajectories generated by the improved TEB algorithm, For drones The flight time interval from the starting control point to the first control point. For drones From the From the first control point to the... Flight time interval between control points; When it is 0, , For drones No. vertex coordinates for hour, , For drones No. vertex coordinates To replan the trajectory of the first The attitude angles of each control point.

[0053] Set of time intervals between control points on the replanned trajectory for: in, For the first From the first control point to the... The time interval between control points satisfies , To replan the total duration of the trajectory, i.e. The time difference between the first and last vertices.

[0054] After the UAV replans its trajectory, when it returns to execute the remaining global trajectory, it still retains the time information of the global trajectory, ensuring no conflict with other UAVs. The optimization target is to merge the pose and time interval sequence of the control points of the replanned trajectory.

[0055] S22: Design a hierarchical weighted quadratic multi-objective cost function based on the distance between control points and obstacles; The hierarchical weighted quadratic multi-objective cost function based on the degree of proximity of control points to obstacles It is the weighted sum of squares of trajectory matching cost, obstacle avoidance cost, smoothness cost, and speed cost; the weights of trajectory matching cost, obstacle avoidance cost, smoothness cost, and speed cost are set in layers according to the distance between the control point and the obstacle; The calculation expression is: in, The cost of trajectory alignment To avoid the cost of obstacles, For the sake of smoothness, For the sake of speed; For trajectory matching cost weights, To weigh the cost of obstacle avoidance, For smoothness cost weights, The speed cost weight.

[0056] Used to measure how well the replanned trajectory matches the global trajectory, preventing the trajectory from deviating excessively from the globally optimal trajectory. Definition and The Euclidean distance is The calculation expression is: The cost of trajectory matching is: in, The trajectory-fitting penalty coefficient is determined by the degree to which global vertices are close to obstacles, and its calculation expression is: in, For the first The distance from the control point to the nearest obstacle on the individual replanning trajectory. , The distance threshold between global vertices and obstacles is such that the closer the global vertex is to the obstacle, the smaller the trajectory matching cost, so that the objective function focuses on the obstacle avoidance cost. At this time, a slight trajectory deviation is allowed to prepare for obstacle avoidance.

[0057] set up Let be the Euclidean distance from the elastic band control point to the static obstacle expansion boundary. The obstacle avoidance cost is defined as the sum of the penalty terms corresponding to each control point and the obstacle expansion safety boundary on the reprogrammed trajectory, and its calculation expression is: in, The penalty weight for static obstacles, The penalty weight for dynamic obstacles, The obstacle avoidance penalty coefficient, The number of static obstacles in the environment. For the first The obstacle avoidance safety boundary after the static obstacle expands. For the first A dynamic obstacle Dynamic obstacle avoidance safety zone at all times. To calculate Euclidean distance, The number of dynamic obstacles in the environment; The calculation expression is: When an obstacle is close to a vertex of the global trajectory, The larger the value, the greater the distance penalty for the corresponding control point to the obstacle. This strategy ensures obstacle avoidance flexibility while preventing excessive trajectory deviation.

[0058] To ensure the continuity and smoothness of the trajectory, the second-order difference of the constrained position and the second-order difference of the attitude angle are constructed at a smoothness cost. for: in, For the position smoothing term weight, For the attitude smoothing term weights, To replan the trajectory of the first Attitude angles of each control point To replan the trajectory of the first Attitude angles of each control point To replan the trajectory of the first Attitude angles of each control point; To replan the trajectory of the first The location of each control point To replan the trajectory of the first Location of each control point To replan the trajectory of the first The location of each control point.

[0059] To avoid abrupt changes in the elastic band state, the velocity cost needs to simultaneously constrain velocity continuity and time intervals, thus replanning the total trajectory duration. The total time must be consistent with the corresponding segment of the global trajectory; otherwise, after replanning, when continuing to execute the globally planned trajectory, collision-free operation cannot be guaranteed. The velocity cost function is: in, To replan the trajectory The speed of each control point To replan the trajectory The speed of each control point For the first From the first control point to the... The time interval between control points.

[0060] S23: Fast solution of multi-objective cost functions using a sparse matrix solver; Each term in the objective function is a quadratic square term, and the optimization problem is a convex programming problem, which can be solved quickly using the classic sparse matrix solver in TEB. The final replanned trajectory will cover the previous global trajectory vertex set. .

[0061] The replanning of the trajectory considers multiple indicators such as trajectory smoothness, trajectory length, and attitude constraints. To reduce complexity, when performing global motion planning for the UAV, only the trajectory position information is used as the optimization objective, and it is assumed that the UAV adopts a tangential flight mode, that is, throughout the entire flight, the yaw angle is assumed to always be along the tangent direction of the velocity vector. The reasons are as follows: First, in the dynamics of a quadcopter, the yaw channel and the position channel are weakly coupled. Changes in the yaw angle will slightly affect the values ​​of roll and pitch angles through the attitude matrix, thus causing a slight disturbance to position control, but this disturbance can be effectively compensated by the closed-loop control law; Second, from a kinematic point of view, the nose along the flight direction conforms to the UAV's normal flight attitude, and the offline planned UAV trajectory aims to minimize the fourth derivative of the position. Mapping the yaw to a function of the first derivative of the position can avoid abrupt yaw changes. Therefore, setting the yaw as the tangent direction of the trajectory is feasible. In addition, this flight mode can effectively reduce the frontal area during flight and reduce aerodynamic drag.

[0062] First, the trajectory position is listed as a parameterized function with respect to time. Assume a drone... The first in the world coordinate system The short trajectory polynomial is: in, for Time Drone No. The three-dimensional position vector on the segment trajectory, , For the first The coefficient vector of an order polynomial , For the first polynomial of order Axial direction coefficient vector, For the first polynomial of order Axial direction coefficient vector, For the first polynomial of order Axial direction coefficient vector, For time vectors, Define the UAV trajectory state vector for: in, for Time Drone No. Velocity on the segment trajectory for Time Drone No. Acceleration on the trajectory segment for Time Drone No. jerk on the trajectory segment , for The first derivative, , for The second derivative, , for The third derivative. If we disregard... Given the kinematic constraints, the free state space of the trajectory... It can be represented as: The flight process of a drone is subject to state constraints: , The maximum speed of the drone, , The maximum acceleration of the drone, , For the maximum jerk of the drone, the feasible state space for: Given input , , for The fourth derivative, the state equation is: in, for The first derivative, For the system matrix, For the input matrix, , , for 3D identity matrix for Zero-dimensional matrix.

[0063] According to the minimum-snap theory, energy optimality during UAV flight is mainly characterized by high-order trajectory smoothness. Therefore, modeling the global trajectory as a high-order polynomial is primarily to satisfy indicators such as energy optimality, dynamic constraints, and path length. However, in real-time local obstacle avoidance, the primary issue faced by UAVs is safe obstacle avoidance, not energy optimality, and the response time requirement for online local planning is in the millisecond range. Therefore, when using the TEB algorithm for real-time local obstacle avoidance, the discrete pose sequence and time intervals are constructed as a hypergraph optimization problem. Under the condition of satisfying the UAV's kinematic constraints, a least-squares problem is solved to generate a smooth local replanning trajectory.

[0064] S3: Each UAV will broadcast its generated local replanning trajectory to the decision set for distributed conflict detection; if there is a conflict in the replanning trajectory of the UAV, then the UAV will be included in the conflict set. In a typical distributed dynamic environment, let's consider unmanned aerial vehicles (UAVs). After detecting the obstacle, the first replanning trajectory generated is Multiple drones simultaneously broadcast their replanned trajectories and received replanned trajectories from other drones, and among them... The replanned trajectories of existing methods often conflict with each other. Drones need to decide whether to continue with the planned trajectory. If multiple drones simultaneously perform secondary replanning, conflict-free trajectories cannot be guaranteed because the changed states of other drones cannot be predicted; therefore, two drones will remain waiting and deadlocked. To address the problems of existing methods, such as loss of feasible solutions, reliance on high-frequency communication, and susceptibility to deadlock, this paper applies the high-level node splitting and child node selection process to the distributed decision-making of drones.

[0065] Set up drones and The trajectories for the first replanning are as follows: in, For drones The first re-planned trajectory, For drones The first re-planned trajectory, For drones The time required for replanning and decision-making For drones The time required for replanning and decision-making Here are the x-coordinates of the control points. The ordinate of the control point. The vertical coordinates of the control points; If a time exists satisfy: The two trajectories are then determined to be in conflict, denoted as... ,in, For drones The first replanning and decision-making time, For drones The time required for replanning and decision-making for Time Drone The trajectory position, for Time Drone The trajectory position, For a safe distance, , Let the radius of the circumscribed sphere of the drone be . To find the function with the maximum value, This is the conflict determination function.

[0066] This invention primarily addresses an extreme scenario: multiple drones simultaneously broadcast their replanning information, resulting in conflicting replanning trajectories. If, under these conditions, no executable trajectory is generated within the latest collision time, causing the drones to wait indefinitely, the system is determined to be deadlocked.

[0067] S4: Based on the synchronization mechanism of the fixed time window, each conflicting UAV in the conflict set constructs a constraint tree through a high-level node splitting method based on enhanced conflict search; the total cost of each child node of the constraint tree is calculated using the same cost function and calculation rules, and the child node with the smallest total cost is selected as the optimal child node; To avoid inconsistencies in decision-making caused by asynchronous communication and replanning runtime delays in UAVs, a synchronization mechanism with a fixed timing window is designed.

[0068] like Figure 2 As shown, the system time is set at a preset period. Segmentation, defining replanning and decision-making time within a preset period. ,and ; exist Internally, it collects replanning state information of conflict-affected drones and constructs a conflict set, in which... The conflict set is the starting point of the current replanning cycle. Used to store conflict drones; exist At that time, the drones in the conflict set are replanned, and the replanning results are broadcast. like The replanned trajectories of any two drones in the dataset satisfy the following conditions: Each drone directly executes its own replanned trajectory; if drones meet This triggers node splitting decisions and multiple replanning processes.

[0069] exist Inside, complete the replanning of all drones, clear the conflict set, and obtain conflict-free executable trajectories.

[0070] Drones that detect sudden obstacles in the environment and need to be replanned Will set its own planning status Set to 1 and broadcast its status to other drones. If there are conflicts in the replanned trajectory, these drones will be classified as... This set only stores drones with a conflict state of 1.

[0071] The node splitting rule of the high-level node splitting method based on enhanced conflict search is as follows: Will The root node of the conflict drone splits into Each child node has only one drone executing the previously replanned trajectory, while the rest... The drone was used for local trajectory replanning.

[0072] S41: Construct a root node containing the first replanning trajectories of all UAVs in the conflict set; root node Defined as: in, For drones The first re-planned trajectory, For drones Re-planning and decision-making time Here are the x-coordinates of the control points. The ordinate of the control point. The vertical coordinates of the control points; it contains only the vertical coordinates of the control points. The first replanning of the trajectory for the Chinese drone resulted in the root node being unable to be directly executed due to conflicts.

[0073] S42: Each UAV in the conflict set uses a synchronization mechanism based on a fixed time window to generate a second replanning trajectory constrained by the first replanning trajectory of other UAVs and broadcasts the second replanning trajectory to other UAVs. If the second replanning trajectory of a certain UAV does not conflict with other UAVs, then the UAV is removed from the conflict set; the removed UAV executes the corresponding second replanning trajectory. S43: After removing conflict-free drones, the drones in the conflict set generate child nodes according to the node splitting rules. The calculation expression is: in, For drones child nodes, For drones The trajectory of the second replanning; S44: Calculate the total cost of each child node, and select the optimal child node based on the total cost of each child node; The total cost of each child node is a weighted sum of the trajectory's own cost and the conflict cost, where the conflict cost is the number of trajectory conflicts within the child node. The expression for calculating the total cost is: in, The total cost of the child nodes. The cost of the child node's trajectory itself. As the cost weight of the trajectory itself, As the conflict cost weight, The conflict cost of a child node is represented by the number of existing conflicts among all trajectories within that node.

[0074] Taking the second split as an example, drones child nodes The trajectory itself has a cost for: in, The trajectory is replanned a second time for the first child node. The trajectory is replanned for the first time for the second child node. For the first Each child node replans its trajectory a second time; if... If the value is minimized, then all drones will execute the decision corresponding to this node, that is: Execute immediately and will from It was launched in China.

[0075] S5: If there is a trajectory conflict among the UAVs in the optimal child node, repeat step S4 until the replanning trajectories of all conflicting UAVs are conflict-free, and obtain a conflict-free executable trajectory.

[0076] If there is a trajectory conflict among the drones in the current optimal child node, then the optimal child node is used as the new root node to continue to perform node splitting and replanning until a child node that meets the conflict-free condition appears. In each node split, at least one drone's trajectory does not collide with the trajectories of other drones in that node, so as to ensure that the conflict set is cleared after at most N-1 splits, where N is the number of conflicting drones.

[0077] While tracking its global trajectory, the drone continuously monitors the risk of collisions along its future path. When the distance between a vertex of the global trajectory and the safety boundary of the obstacle expansion is detected to meet the collision determination condition, trajectory replanning will be triggered. The final feasible reprogramming trajectory is: , ,generate Previously, multiple replanning trajectories might have been generated: , wait.

[0078] like Figure 3 As shown, a distributed UAV local replanning and real-time obstacle avoidance system is used to execute the aforementioned distributed UAV local replanning and real-time obstacle avoidance method, including: The replanning trigger module 101 is used for multiple low-altitude logistics delivery drones to fly according to a pre-planned global guidance trajectory and detect obstacles in real time. When the obstacle avoidance safety boundary after the obstacle expands and the state node on the future global guidance trajectory of the drone are spatially intersected at the same time, local trajectory replanning is triggered. The replanning trajectory generation module 102 is used by the UAV that triggers local trajectory replanning to independently generate local replanning trajectories based on an improved time elastic band algorithm. The conflict detection module 103 is used for each UAV to broadcast the generated local replanning trajectory to the decision set for distributed conflict detection; if there is a conflict in the replanning trajectory of the UAV, the UAV is included in the conflict set. The optimal child node selection module 104 is used to construct a constraint tree for each conflicting UAV in the conflict set through a high-level node splitting method based on enhanced conflict search; and to calculate the total cost of each child node of the constraint tree through a synchronization mechanism with a fixed time window and the same cost function and calculation rules, and select the child node with the minimum total cost as the optimal child node. The conflict-free trajectory acquisition module 105 is used to iteratively process the optimal child node selection module if there is a trajectory conflict among the UAVs in the optimal child node, until the replanning trajectories of all conflicting UAVs are conflict-free, and obtain a conflict-free executable trajectory.

[0079] Through the collaborative work of the above modules, smooth and feasible replanning trajectories can be generated quickly to meet real-time requirements; the deadlock problem caused by replanning trajectory conflicts of close-range UAVs in dense obstacle environments is solved, ensuring the safety and stability of multi-UAV collaborative operations.

[0080] To verify the comprehensive superiority of DTE in complex multi-drone cooperative environments, this invention selects classic algorithms applicable to local sudden obstacle avoidance for quantitative comparison. To control experimental variables, all comparative algorithms were tested in a unified environment: assuming all drones initially flew normally, obstacles of the same size and spatial location were placed between the start and end points or on the global reference trajectory to trigger swarm obstacle avoidance behavior. Furthermore, the configuration and physical constraint parameters of all algorithms were identical. The number of drones was 5, and the speed limit was [missing information]. The drone's field of view radius is The control cycle is .

[0081] The following is an introduction to each comparison algorithm and its parameter configuration. Common parameters should be kept consistent, while unique parameters should be set to appropriate values.

[0082] Artificial Potential Field (APF) method: This method virtualizes the environment as a physical potential field. The target point generates an attractive potential field, while obstacles and nearby drones generate repulsive potential fields. The resultant force is calculated to drive the robot's motion. The attraction and repulsion gains are set to 1.2 and 15, respectively; the obstacle influence range is set to... .

[0083] Optimal Reciprocal Collision Avoidance (ORCA) is a classic multi-agent distributed obstacle avoidance algorithm based on velocity barrier theory, commonly used for collision avoidance control in high-density clusters. The obstacle avoidance radius of each agent is set to... Time window set to Neighbor search distance set to .

[0084] Based on considerations of experimental fairness, DTE settings And the above comparative experiment was conducted, with the locations traversed by the drone in the experiment being... The step size is discretized and visualized as a trajectory, such as Figure 4 As shown, Figure 4 Figure (a) shows the trajectory curve generated by the APF algorithm, which exhibits obvious non-linear curvature. When approaching the obstacle sphere, the UAV is subjected to a strong repulsive field, causing it to circle the obstacle sphere significantly; furthermore, the jagged trend at some inflection points in the figure indicates that the trajectory oscillates at the edge of the obstacle. Figure 4 Figure (b) shows the trajectory curve generated by the ORCA algorithm. The trajectory is relatively flat compared to the APF. However, in order to meet the speed obstacle constraint, the speed vector of the UAV changes abruptly, resulting in an unsmooth trajectory. In actual UAV flight, this behavior will bring a large instantaneous load to the motor, making it difficult to achieve accurate tracking under high dynamic conditions. Figure 4 Figure (c) shows the trajectories of the five drones generated by this invention. All trajectories are relatively smooth, continuous curves, consistent with the physical characteristics of drones. The trajectories not only bypass obstacles but also maintain a stable, safe distance from other drones while flying towards each other.

[0085] The algorithm performance is quantitatively evaluated by obstacle avoidance success rate, algorithm running time, and trajectory smoothness.

[0086] The percentage of successful global planning algorithms or local planning and obstacle avoidance algorithms out of the total number of experiments during simulation. Successful experiments require the following conditions: no collisions; compliance with system and machine constraints; the system not getting stuck in a spatial deadlock or local optimum; and successful code execution.

[0087] In a single simulation, all five drones successfully avoided obstacles, including the percentage of collisions between drones, collisions between drones and obstacles, no obviously infeasible poses, no getting trapped in local minima, and no getting trapped in deadlock.

[0088] In global planning, algorithm runtime is defined as the offline computation time for solving the initial collision-free feasible trajectory of the entire multi-machine system. In local obstacle avoidance, it is defined as the total time consumed in a single obstacle avoidance attempt, i.e., the total time from triggering obstacle avoidance at the bottom layer to completely resolving the conflict and outputting the final feasible replanning trajectory. To ensure the scientific rigor of the comparative evaluation, for APF and ORCA, the total time consumed in a single obstacle avoidance attempt refers to the sum of the computation time of instantaneous control commands within all discrete control cycles from the start to the end of obstacle avoidance. For this invention, it is the total time consumed in solving the final collision-free feasible trajectory in a single attempt, encompassing the time consumed by multiple replanning iterations and conflict search tree node splitting that may be involved in the solution process. This metric directly reflects the algorithm's real-time response capability.

[0089] Trajectory smoothness is a core physical quantity used to measure the drastic changes in control commands, and different quantification methods are employed at different planning levels. In actual UAV control, the second derivative of position is related to the UAV's total lift, the third derivative can be mapped to the rate of change of lift and torque, and the fourth derivative is related to energy. Therefore, the average equivalent acceleration or discrete differential acceleration of the trajectory is used as the quantification basis for trajectory smoothness. The smaller this value, the smoother the maneuvering of the UAV during emergency avoidance, and the smaller the instantaneous impact on the motors and ESCs.

[0090] For this invention, average acceleration is used to represent smoothness. For APF and ORCA, smoothness is represented by the sum of the ratios of the velocity difference output between every two adjacent control time steps to the sampling time, i.e., discrete differential acceleration. .

[0091] The combined performance data of the three algorithms in 50 random tests are shown in Table 1.

[0092] Table 1 Overall Performance Data

[0093] The results show that the present invention significantly outperforms traditional APF and ORCA in overall performance. The present invention achieves the highest obstacle avoidance success rate, while the APF and ORCA algorithms perform poorly. In dense scenarios containing sudden obstacles, APF is prone to getting trapped in local minima, causing oscillations or even stagnation, leading to task timeouts or being deemed infeasible, resulting in the lowest obstacle avoidance success rate. Although the ORCA algorithm has high theoretical completeness in handling multi-agent collision avoidance, its essence is a linear constraint partitioning of the velocity space. In scenarios with complex static obstacles, the ORCA algorithm is prone to becoming unsolvable due to excessive compression of the feasible velocity space, leading to deadlock in the UAV swarm. The present invention, on the one hand, utilizes an improved TEB spatiotemporal elastic band model for nonlinear optimization, enabling UAVs to avoid obstacles in advance under complex constraints; on the other hand, it combines an ECBS-inspired distributed coordination framework to effectively resolve trajectory conflicts when multiple UAVs meet, avoiding deadlock and maintaining a very high obstacle avoidance success rate.

[0094] Regarding algorithm runtime, the APF and ORCA algorithms have lower overall time consumption for a single obstacle avoidance process, while the total time consumption of this invention is relatively higher. This invention requires nonlinear optimization based on an improved spatiotemporal elastic band model. When handling dense scenarios with five intersecting machines, it necessitates constructing a conflict search tree and splitting nodes to eliminate conflicts, thus taking a slightly longer time. APF and ORCA must strictly adhere to... The single-step algorithm output must be completed within the control cycle; otherwise, gaps in the issued control commands will occur, threatening the flight safety of the UAV. Furthermore, high-frequency communication is required during distributed planning. This invention can directly output a continuous feasible trajectory covering a long future time domain in a single calculation, without being limited to updating the planning results within the control cycle, and without high-frequency communication. Only a limited number of communications are required when replanning trajectory conflicts occur, making it suitable for urban environments where signals are easily interfered with.

[0095] The trajectory smoothness index obtained by APF is not ideal. Because its repulsive force increases exponentially with decreasing distance, it causes drastic state changes when the UAV approaches obstacles. ORCA avoids obstacles by changing speed; this first-order discontinuity directly leads to pulsed peaks in the second derivative, which is detrimental to the UAV's motor load and attitude control. This invention, while sacrificing some computational efficiency, achieves a significant improvement in obstacle avoidance success rate and robustness of spatial maneuvering.

[0096] To verify the real-time replanning and obstacle avoidance capabilities of drones in complex urban environments, simulation experiments were conducted in the laboratory. The experimental scenario was constructed using the RflySim simulation platform, simulating typical urban obstacles such as tall buildings, utility poles, and trees. Figure 5 The image shown is a comparison between the planned trajectory and the actual tracking results of the first drone. Figure 5Figure (a) shows the first drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 5 Figure (b) shows the first drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 5 Image (c) shows the first drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 5 Figure (d) shows the yaw angle of the first UAV. A comparison chart of the planned trajectory and the actual tracking trajectory. (See attached image.) Figure 6 As shown in the image, this is a comparison chart of the planned trajectory and the actual tracking trajectory of the second drone. Figure 6 Figure (a) shows the second drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 6 Figure (b) shows the second drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 6 Figure (c) shows the second drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 6 Figure (d) shows the yaw angle of the second drone. A comparison chart of the planned trajectory and the actual tracking trajectory. (See attached image.) Figure 7 The image shown is a comparison between the planned trajectory and the actual tracking trajectory of the third drone. Figure 7 Figure (a) shows the third drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 7 Figure (b) shows the third drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 7 Figure (c) shows the third drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 7 Figure (d) shows the yaw angle of the third drone. A comparison chart of the planned trajectory and the actual tracking trajectory. (See attached image.) Figure 8 The image shown is a comparison between the planned trajectory and the actual tracking trajectory of the fourth drone. Figure 8 Figure (a) shows the fourth drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 8 Figure (b) shows the fourth drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 8 Figure (c) shows the fourth drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 8 Figure (d) shows the yaw angle of the fourth UAV. A comparison chart of the planned trajectory and the actual tracking trajectory. (See attached image.) Figure 9 The image shown is a comparison between the planned trajectory and the actual tracking trajectory of the fifth drone. Figure 9 Figure (a) shows the fifth drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 9 Figure (b) shows the fifth drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 9 Figure (c) shows the fifth drone in... A comparison chart of the planned trajectory and the actual tracking trajectory at the axis position. Figure 9 Figure (d) shows the yaw angle of the fifth drone. A comparison chart of the planned trajectory and the actual tracking trajectory. At 32 seconds, the 4th drone was the first to detect the obstacle on its global guidance trajectory; at 42 seconds, the 5th drone executed a replanned trajectory and finally returned to the global guidance trajectory.

[0097] Each drone , , The planned trajectory and the actual tracking trajectory in the three-axis position exhibit a high degree of consistency. Experimental data shows that the real-time tracking error in the three-axis position remains consistently within an extremely low range (the maximum error is typically less than...). No significant lag or oscillation was observed, verifying that the underlying PID controller combined with the DTE algorithm can achieve accurate trajectory tracking. Furthermore, the yaw angle variation curve reflects the UAV's attitude adjustment under complex maneuvers. The yaw angle adjusts in real time according to position commands, with fluctuations remaining relatively stable within a certain range. Within this range, and rapidly converges to [a value] after completing the replanning action. .

[0098] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A distributed unmanned aerial vehicle (UAV) local replanning and real-time obstacle avoidance method, characterized in that, include: S1: Multiple low-altitude logistics delivery drones fly according to a pre-planned global guidance trajectory and detect obstacles in real time. When the obstacle avoidance safety boundary after the obstacle expands and the state node on the future global guidance trajectory of the drone are spatially intersected at the same time, local trajectory replanning is triggered. S2: The UAV that triggers local trajectory replanning independently generates a local replanning trajectory based on the improved time elastic band algorithm; S3: Each UAV will broadcast its generated local replanning trajectory to the decision set for distributed conflict detection; if there is a conflict in the replanning trajectory of the UAV, then the UAV will be included in the conflict set. S4: Based on the synchronization mechanism of the fixed time window, each conflicting UAV in the conflict set constructs a constraint tree through a high-level node splitting method based on enhanced conflict search; The total cost of each child node in the constraint tree is calculated using the same cost function and calculation rules, and the child node with the minimum total cost is selected as the optimal child node. The node splitting rule of the high-level node splitting method based on enhanced conflict search is as follows: Will The root node of the conflict drone splits into Each child node has only one drone executing the previously replanned trajectory, while the rest... The drone was used for local trajectory replanning. Step S4 includes: Construct a root node containing the first replanning trajectories of all UAVs in the conflict set; Each UAV in the conflict set uses a synchronization mechanism based on a fixed time window to generate a second replanning trajectory, constrained by the first replanning trajectory of other UAVs, and broadcasts the second replanning trajectory to other UAVs. If the second replanning trajectory of a UAV does not conflict with other UAVs, then the UAV is removed from the conflict set; the removed UAV executes the corresponding second replanning trajectory. After removing conflict-free drones, drones in the conflict set generate child nodes according to the node splitting rules. Calculate the total cost of each child node, and select the optimal child node based on the total cost of each child node; S5: If there is a trajectory conflict among the UAVs in the optimal child node, repeat step S4 until the replanning trajectories of all conflicting UAVs are conflict-free, and obtain a conflict-free executable trajectory.

2. The distributed UAV local replanning and real-time obstacle avoidance method according to claim 1, characterized in that, The obstacle avoidance safety boundary after the obstacle expands includes the expanded static obstacle avoidance safety boundary and the expanded dynamic obstacle avoidance safety boundary. Expand the original boundary of the static obstacle outward to the diameter of the UAV itself to obtain the expanded static obstacle avoidance safety boundary. The expanded static obstacle avoidance safety boundary encloses the expanded static obstacle region. The original boundary of the static obstacle is the convex hull boundary after point cloud clustering. Expand the original boundary of the dynamic obstacle outward to the diameter of the UAV itself to obtain the expanded dynamic obstacle avoidance safety boundary. The expanded dynamic obstacle avoidance safety boundary encloses the expanded dynamic obstacle region. The original boundary of the dynamic obstacle is the circumscribed geometry of the current position of the dynamic obstacle. If there is at least one state node on the future global guidance trajectory, and the drone and the expanded static obstacle area or the expanded dynamic obstacle area have spatial intersection at the same time, then the drone and the obstacle are at risk of collision, triggering local trajectory replanning.

3. The distributed UAV local replanning and real-time obstacle avoidance method according to claim 1, characterized in that, The improved temporal elastic band algorithm constructs a spatiotemporal elastic band model that includes three-dimensional position, attitude, and time. The spatiotemporal elastic band model connects the starting point and target point state of the local replanning trajectory with a trajectory having multiple control points, and designs a hierarchical weighted quadratic multi-objective cost function based on the distance between the control points and obstacles. The multi-objective cost function is solved quickly by a sparse matrix solver.

4. The distributed UAV local replanning and real-time obstacle avoidance method according to claim 3, characterized in that, The hierarchical weighted quadratic multi-objective cost function based on the distance between the control point and the obstacle is a weighted sum of squares of trajectory matching cost, obstacle avoidance cost, smoothness cost, and speed cost; the weights of trajectory matching cost, obstacle avoidance cost, smoothness cost, and speed cost are set hierarchically according to the distance between the control point and the obstacle. Trajectory fitting cost is used to measure the degree of fit between the replanned trajectory and the global trajectory; Obstacle avoidance cost is used to measure the proximity of the elastic band control point to the obstacle expansion safety boundary; Smoothness cost is used to constrain the continuity and smoothness of the trajectory; Speed ​​cost is used to constrain speed continuity and time intervals.

5. The distributed UAV local replanning and real-time obstacle avoidance method according to claim 1, characterized in that, The total cost of each sub-node is the weighted sum of the trajectory's own cost and the conflict cost, where the conflict cost is the number of all trajectory conflicts within the sub-node.

6. The distributed UAV local replanning and real-time obstacle avoidance method according to claim 1, characterized in that, The synchronization mechanism of the fixed timing window includes: Set system time to a preset cycle Segmentation, defining replanning and decision-making time within a preset period. ; exist Internally, it collects replanning state information of conflict-affected drones and constructs a conflict set, in which... This marks the start of the current replanning cycle; exist At that time, the drones in the conflict set are replanned, and the replanning results are broadcast. exist Inside, complete the replanning of all drones, clear the conflict set, and obtain conflict-free executable trajectories.

7. The distributed UAV local replanning and real-time obstacle avoidance method according to claim 1, characterized in that, If there is a trajectory conflict among the drones in the current optimal child node, then the optimal child node is used as the new root node to continue to perform node splitting and replanning until a child node that meets the conflict-free condition appears. In each node split, at least one drone's trajectory does not collide with the trajectories of other drones in that node, so as to ensure that the conflict set is cleared after at most N-1 splits, where N is the number of conflicting drones.

8. A distributed unmanned aerial vehicle (UAV) local replanning and real-time obstacle avoidance system, characterized in that, To perform a distributed UAV local replanning and real-time obstacle avoidance method as described in any one of claims 1 to 7, comprising: The replanning trigger module is used for multiple low-altitude logistics delivery drones to fly according to a pre-planned global guidance trajectory and detect obstacles in real time. When the obstacle avoidance safety boundary after the obstacle expands and the state node on the future global guidance trajectory of the drone are spatially intersected at the same time, local trajectory replanning is triggered. A replanning trajectory generation module is used to trigger local trajectory replanning of the UAV, which independently generates a local replanning trajectory based on an improved time elastic band algorithm. The conflict detection module is used by each UAV to broadcast its generated local replanning trajectory to a decision set for distributed conflict detection; if the replanning trajectory of a UAV is in conflict, then the UAV is included in the conflict set. The optimal child node selection module is used to construct a constraint tree for each conflicting UAV in the conflict set according to the synchronization mechanism of a fixed time window, through a high-level node splitting method based on enhanced conflict search; calculate the total cost of each child node of the constraint tree through the same cost function and calculation rules, and select the child node with the minimum total cost as the optimal child node. The conflict-free trajectory acquisition module is used to iteratively process the optimal sub-node selection module if there is a trajectory conflict among the UAVs in the optimal sub-node, until the replanning trajectories of all conflicting UAVs are conflict-free, thus obtaining a conflict-free executable trajectory.

Images