Unmanned aerial vehicle cluster collaborative trajectory planning method

Abstract

The invention discloses an unmanned aerial vehicle cluster collaborative trajectory planning method, and belongs to the technical field of unmanned aerial vehicles, and the method comprises the steps: enabling four unmanned aerial vehicles to form a formation to form a three-dimensional ideal tetrahedron, and carrying out the real-time obstacle avoidance and trajectory optimization through the combination of a dynamic safety ball and a convex polyhedron, and automatic takeover when the pilot aircraft or the wing aircraft is lost is realized through heartbeat detection and a replacement priority table, so that the technical problems of dynamic obstacle avoidance, collision detection, trajectory optimization and automatic replacement of failure of the pilot aircraft when the unmanned aerial vehicle cluster cooperatively flies in a three-dimensional space are solved. According to the scheme, the maximum allowable deformation range of the whole formation is achieved, dynamic obstacle avoidance is achieved, automatic taking-over of unmanned aerial vehicles and updating of formation safety convex polyhedrons are achieved through a virtual safety ball and a replacement priority table according to the condition that wing planes or pilot planes are out of communication, and task continuity is guaranteed.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of unmanned aerial vehicles, and particularly relates to a method for cooperative trajectory planning of an unmanned aerial vehicle cluster. BACKGROUND

[0002] With the rapid development of unmanned aerial vehicle technology, multi-unmanned aerial vehicle cooperative operation is increasingly widely applied in the fields of monitoring, logistics, search and rescue, and military affairs. An unmanned aerial vehicle cluster can realize task division, efficiency improvement, and task coverage expansion through cooperative flight. However, when the cluster flies in a complex environment, there are problems such as collision risk between multiple unmanned aerial vehicles, great difficulty in obstacle avoidance, and loss of control of the entire formation due to failure of the lead vehicle. Therefore, how to plan a safe and controllable trajectory for the unmanned aerial vehicle cluster in three-dimensional space and realize real-time collision detection and dynamic adjustment during flight is a technical problem that needs to be solved for cooperative operation of the unmanned aerial vehicle cluster.

[0003] Disadvantages of the prior art: Trajectory planning is mostly for single unmanned aerial vehicle or two-dimensional planarization: existing methods mostly target single unmanned aerial vehicle or two-dimensional trajectory planning, and it is difficult to directly extend to three-dimensional cluster formation, and the overall safety of the cluster cannot be fully guaranteed.

[0004] Lack of dynamic collision detection and constraint mechanism: existing cluster trajectory planning is usually based on fixed spacing or static safety zones, and it is difficult to adapt to obstacles or speed changes during flight, and collision risks are likely to occur.

[0005] The risk of lead vehicle failure is not fully addressed: if the lead vehicle loses contact or fails, most existing solutions rely on manual intervention or the entire formation to stop flying, and lack of automatic replacement and safety mechanism, which affects the continuity and reliability of the task. SUMMARY

[0006] The purpose of the present application is to provide a method for cooperative trajectory planning of an unmanned aerial vehicle cluster, which solves the technical problems of dynamic obstacle avoidance, collision detection, trajectory optimization, and automatic replacement of lead vehicle failure when the unmanned aerial vehicle cluster cooperatively flies in three-dimensional space.

[0007] To achieve the above purpose, the application adopts the following technical solutions: A method for cooperative trajectory planning of an unmanned aerial vehicle cluster, comprising the following steps: Step 1: The ground control station generates a preliminary mission plan according to the task area, obstacle information, number of UAVs and initial positions entered by the operator; the UAVs are arranged in a formation of four per group, and the ideal tetrahedron formation structure is determined, in which the four UAVs are the four vertices, denoted as U1, U2, U3 and U4; U1 is designated as the lead aircraft and is the top vertex of the ideal tetrahedron, and U2, U3 and U4 are all wingmen and are the bottom vertices of the ideal tetrahedron; the preliminary trajectory is generated using the ideal tetrahedron, and a replacement priority table for replacing the lead aircraft is preset; the ground control station presets the safe flight range of each UAV to generate the safety sphere radius of each UAV; The ground control station sends the obstacle information, preliminary trajectory, safety sphere radius, model of ideal tetrahedron and replacement priority table to the UAVs in the formation; Step 2: During formation flight, the wingmen periodically report their own positions, speeds and flight states to the lead aircraft; the lead aircraft dynamically adjusts the safety sphere radius of each UAV according to the obstacle information; Step 3: The lead aircraft generates a safety sphere for each UAV in the formation according to their real-time positions and safety sphere radii, and discretizes the safety spheres to form surface points, thereby constructing a dynamic safety convex polyhedron, which represents the maximum allowable deformation range of the formation and serves as a spatial constraint for collision detection; Step 4: The lead aircraft performs collision detection using the safety convex polyhedron, and optimizes and adjusts the UAV trajectories under the condition of ensuring the spatial constraint when potential collisions are found; Step 5: When a wingman loses contact, the lead aircraft generates a virtual reference position based on the wingman's position and speed before the loss of contact, generates a virtual safety sphere, and updates the safety convex polyhedron; when the lead aircraft loses contact, the wingmen select a new lead aircraft according to the replacement priority table, and the new lead aircraft takes over the generation of the safety convex polyhedron, collision detection and trajectory optimization.

[0008] Preferably, when step 1 is performed, it specifically includes the following steps: Step 1-1: The operator enters a task into the ground control station, which includes region coordinates, obstacle information, number of UAVs and initial positions; Step 1-2: The ground control station arranges the UAVs in a formation of four per group according to the number of UAVs, and generates the vertices P ideal，i of the ideal tetrahedron for each formation, where P is a coordinate identifier; the four UAVs are arranged at the vertices of the ideal tetrahedron, denoted as U1, U2, U3 and U4, wherein U1 is at the top vertex of the ideal tetrahedron and is the lead aircraft, and U2, U3 and U4 are all wingmen; The model of the ideal tetrahedron is as follows: P ideal，1 = (x1, y1, z1); P ideal，2 = (x2, y2, z2); P ideal，3 = (x3, y3, z3); P ideal，4 = (x4, y4, z4); Among them, P ideal，i P represents the vertex position of the ideal tetrahedron, where i represents the UAV index, taking values ​​1, 2, 3, and 4 corresponding to U1, U2, U3, and U4; ideal，1 P is the top vertex. ideal，2 P ideal，3 and P ideal，4 All are vertices of the base triangle; x i y i , z i These are the position coordinates of the i-th drone; Steps 1-3: The ground control station generates trajectory sequences for each UAV along the vertices of the ideal tetrahedron, obtaining the preliminary trajectory T for each UAV. i : T i ={P ideal，i (t0),P ideal，i (t1),…,P ideal，i (t n )}; Among them, t k This represents the time node of the trajectory, where k represents the index of the time, ranging from 0 to n; P ideal，i (t k ) indicates that the i-th drone is in t k The vertex position; Steps 1-4: The ground control station presets a priority table for wingman takeover based on the wingman's serial number. Priority = [U2, U3, U4]; In Priority, the sorting number of the drone determines the priority order; Steps 1-5: The operator enters the flight safety radius in the ground control station. The ground control station presets the flight safety radius as the flight error radius of each UAV and generates the safety sphere radius for each UAV. Steps 1-6: The ground control station transmits obstacle information, the ideal tetrahedron model, and the preliminary trajectory T. i After the data is packaged with the priority table, it is sent to U1, U2, U3 and U4 respectively. U1, U2, U3 and U4 all receive the data packets sent by the ground control station and store them locally. Steps 1-7: Complete the initialization settings for each formation according to the methods in steps 1-2 to 1-6.

[0009] Preferably, step 2 includes the following steps: Step 2-1: Wingmen U2, U3, and U4 use their own flight control system sensors to collect their own status data and report the status data to the navigator U1 at fixed time intervals; the status data includes the actual position P. real，i (t k ), speed V real，i (t k ) and flight status: P real，i (t k )=(x real，i (t k ),y real，i (t k ),z real，i (t k )); Where, x real，i (t k ),y real，i (t k ),z real，i (t k ) The i-th drone at t k The actual coordinates of the time; k represents the time index; at this time, i takes values ​​of 2, 3, and 4; P real，i (t k ) indicates that the i-th drone is in t k The actual location at that moment; V real，i (t k )=(v xi (t k ),v yi (t k ),v zi (t k )); Among them, v xi (t k ) represents the i-th drone in t k The velocity on the x-axis at time t, v yi (t k ) represents the i-th drone in t k The velocity on the y-axis at time t, v zi (t k ) represents the i-th drone in t k The velocity on the z-axis at time t; V real，i (t k ) indicates that the i-th drone is in t k The actual velocity vector at any given moment; Step 2-2: The navigator U1 receives status data reported by wingmen U2, U3, and U4, and dynamically adjusts the safety sphere radius r of each UAV based on obstacle information. i (t k The specific formula is as follows: r i (t k )=r init，i ×(1+αD obs (t k ))×(1+β||V real，i (t k )||). Where, r i (t k ) represents the i-th drone in t k The radius of the safety sphere at any given time; r init，i The initial safety sphere radius, whose value is sent and set by the ground control station; D obs (t k The information refers to obstacle density, specifically the obstacle density factor, which represents the proportion of obstacles within a certain range around the drone. Its value ranges from 0 to 1; a larger value indicates a more complex environment. real，i (t k )|| indicates that the drone is at t k The velocity modulus at time t is α, which is the obstacle sensitivity coefficient used to control the influence of obstacle density on the radius of the safety ball; α>0; β is the velocity sensitivity coefficient used to control the influence of velocity magnitude on the radius of the safety ball; β>0.

[0010] Preferably, step 3 includes the following steps: Step 3-1: Navigator U1 receives the real-time position P from U2, U3, and U4. real，i (t k ), t k This represents the time node of the trajectory, where k represents the index of the time, ranging from 0 to n; P ideal，i (t k ) indicates that the i-th drone is in t k The vertex position at any given moment; The navigator U1 utilizes a dynamic safety sphere radius r i (t k Generate a safety sphere S for each drone. i (t k ): S i (t k )={X∈R 3 :||X−P ideal，i (t k )||≤r i (tk )}; Among them, S i (t k ) indicates that the i-th drone is in t k The safety sphere at time X represents any point in space, with position coordinates in three dimensions; i takes values ​​of 1, 2, 3, and 4. Step 3-2: Navigator U1 uses the fixed grid method to arrange each safety ball S i (t k Discretize into N s surface point Q i,j : Q i,j ∈SurfacePoints(S i (t k )); Where j takes values ​​from 1 to N s N s Q represents the number of discretization points for each safety ball; i,j Indicates the safety ball S i (t k Discretized boundary points on ) Step 3-3: Use the Convex Hull algorithm to generate a dynamic, safe convex polyhedron C(t). k ): ; Where ConvexHull(•) is the convex hull algorithm function; C(t) k ) represents time t k A safe convex polyhedron is used to represent the maximum permissible deformation boundary of the entire formation and to serve as a spatial constraint for collision detection. Steps 3-4: The navigator U1 sends the safety convex polyhedron C(t) to the ground control station in real time. k The ground control station will use the safety convex polyhedron C(t) k Add to task monitoring logs.

[0011] Preferably, step 4 includes the following steps: Step 4-1: The navigator U1 generates a safe convex polyhedron C(t). k ), thus obtaining the set of vertices V of the convex polyhedron. d (t k ), and the safety convex polyhedron C(t) k ) are considered spatial constraints; Step 4-2: The navigator U1 uses its own sensors to detect the real-time location information O(t) of surrounding obstacles. k ), for obstacles and safety convex polyhedra C(t) kPerform collision detection and obtain the collision detection results: Collision(t k )=1(∃v∈V d (t k ) st v∈O(t k )), Where O(t) k Specifically, it represents the set of spaces occupied by obstacles; 1 (•) represents an indicator function, which is 1 if true and 0 otherwise; Collision(t) k )=1 indicates that a potential collision has been detected, and the trajectory needs to be optimized; Step 4-3: When a collision occurs, optimize the reference trajectory of each UAV while satisfying the safety sphere and convex polyhedron constraints: ; ; in, For the i-th drone at time t k The optimized reference position sequence points; The i-th drone at time t k The safety ball; The safe convex polyhedron is obtained by updating the safe sphere based on the optimized reference position; Cost() represents the objective function for trajectory optimization, as follows: P ref，new，i (t k )=P ref，i (t k )+ΔP i (t k ); ||ΔP i (t k )||≤r i (t k ); Where, ΔP i (t k ) represents the trajectory correction vector; Step 4-4: Navigator U1 will optimize trajectory P ref，new，i (t k The data is sent to wingmen U2, U3, and U4 respectively, and wingmen U2, U3, and U4 execute the optimized trajectory P. ref，new，i (t k ); Steps 4-5: Navigator U1 will optimize trajectory P ref，new，i (t k The collision detection results are sent to the ground control station, which generates and saves the mission monitoring record.

[0012] Preferably, step 5 includes the following steps: Step 5-1: The current navigator Ua maintains communication with each wingman through a heartbeat mechanism; if the navigator does not receive a heartbeat packet from a certain wingman Uq within several consecutive heartbeat cycles, it is determined that it has lost contact; the current navigator Ua is initially navigator U1. At this moment, the navigator Ua is in the ideal position P of the lost wingman Uq within the ideal tetrahedral structure. ideal,q (t k A virtual substitution is performed to generate its virtual security sphere S. virt，q (t k ): S virt，q (t k )={X∈R 3 :||X−P ideal,q (t k )||≤r q (t k )}; Where q represents the index number of the missing drone, and the value can be 1, 2, 3 or 4; And based on the safety spheres of other online drones and the virtual safety sphere, an updated dynamic safety convex polyhedron C is constructed. new (t k ): C new (t k =ConvexHull({S j (t k )} j∈A ∪{S virt，q (t k )} q∈L ); Where j represents the index of the drone that has not lost contact, A represents the set of drones that have not lost contact, and L represents the set of drones that have lost contact. The navigator sends the updated safety convex polyhedron to the ground control station for recording and monitoring; Step 5-2: Each wingman receives the heartbeat packet from the current navigator's Ua in real time; if a wingman does not receive the navigator's heartbeat within the specified timeout period, while the other wingmen communicate normally, the navigator takeover process begins. In the preset replacement priority table, the online wingman Um with the highest priority is automatically promoted to the new navigator and broadcasts the replacement notice to other wingmen and the ground control station; Based on the principle, the new navigator Um is positioned P in the tetrahedron. ideal,a (t k A virtual replacement is created for the missing navigator, generating its virtual safety sphere: Svirt，a (t k )={X∈R 3 :||X−P ideal,a (t k )||≤r a (t k )}; Construct a new safe convex polyhedron C by combining the currently online wingman safety sphere. new (t k ): C new (t k =ConvexHull({S j (t k )} j∈A ∪{S virt，a (t k )}); The new lead aircraft sends it to the ground control station to update the mission log; Step 5-3: After receiving the updated safety convex polyhedron, the ground control station immediately records the loss of contact event, generates a loss of contact alarm message, and stores it in the mission monitoring database for future event analysis and retrospection.

[0013] This invention discloses a collaborative trajectory planning method for UAV swarms, which solves the technical problems of dynamic obstacle avoidance, collision detection, trajectory optimization, and automatic takeover when the wingman fails during collaborative flight in three-dimensional space. The invention uses four UAVs to form a three-dimensional ideal tetrahedron, achieving a stable spatial layout and trajectory planning for the swarm. Each UAV generates a real-time safety sphere, and a safe convex polyhedron is constructed using a convex hull algorithm to achieve the maximum allowable deformation range of the entire formation, providing precise spatial constraints for collision detection. During flight, the wingman dynamically adjusts the radius of the safety sphere based on obstacle information and UAV status to optimize the trajectory and achieve dynamic obstacle avoidance. In the event of a wingman or wingman losing contact, this solution uses a virtual safety sphere and a takeover priority table to achieve automatic UAV takeover and update of the formation's safe convex polyhedron, ensuring mission continuity. Attached Figure Description

[0014] Figure 1 This is the main flowchart of the present invention; Figure 2 This is a flowchart of the formation initialization and task issuance process of the present invention; Figure 3 This is a flowchart of the formation flight status acquisition, safety sphere dynamic adjustment, and dynamic safety convex polyhedron generation and updating process of the present invention. Figure 4 This is a flowchart of the trajectory optimization and collision detection process of the present invention; Figure 5This is a flowchart of the process for handling lost drones and taking over from the navigator, as described in this invention. Detailed Implementation

[0015] Depend on Figures 1-5 The method for collaborative trajectory planning of a drone swarm shown includes the following steps: Step 1: The ground control station generates a preliminary mission plan based on the mission area, obstacle information, number of UAVs, and initial positions entered by the operator; the UAVs are grouped into formations of four, forming an ideal tetrahedron structure, where the four UAVs are designated as the four vertices, U1, U2, U3, and U4; U1 is designated as the navigator and serves as the top vertex of the ideal tetrahedron, while U2, U3, and U4 are designated as wingmen and serve as the bottom vertices; a preliminary trajectory is generated using the ideal tetrahedron, and a priority table for replacing the navigator is preset; the ground control station presets the safe flight range for each UAV and generates the safety sphere radius for each UAV. The ground control station sends obstacle information, preliminary trajectory, safety sphere radius, ideal tetrahedron model, and succession priority table to the drones in the formation; Step 1 includes the following steps: Step 1-1: The operator enters the task into the ground control station. The task includes area coordinates, obstacle information, number of drones, and initial position. Steps 1-2: The ground control station groups the drones into squadrons of four based on the number of drones. For each squadron, it generates the vertices P of an ideal tetrahedron. ideal，i P is the coordinate identifier; four drones are set to be located at the vertices of the ideal tetrahedron, denoted as U1, U2, U3 and U4. U1 is located at the top vertex of the ideal tetrahedron and is the navigator, while U2, U3 and U4 are wingmen. The model of an ideal tetrahedron is as follows: P ideal，1 = (x1, y1, z1); P ideal，2 = (x2, y2, z2); P ideal，3 = (x3, y3, z3); P ideal，4 = (x4, y4, z4); Among them, P ideal，i P represents the vertex position of the ideal tetrahedron, where i represents the UAV index, taking values ​​1, 2, 3, and 4 corresponding to U1, U2, U3, and U4; ideal，1 P is the top vertex. ideal，2 P ideal，3 and P ideal，4 All are vertices of the base triangle; x iy i , z i These are the position coordinates of the i-th drone; In this embodiment, the specific method for the ground control station to generate the ideal tetrahedron vertices for each formation is as follows: An ideal tetrahedron is the smallest convex hull structure formed by four points in space, which can ensure that any three wingmen and the navigator form a triangular face, thus ensuring stable flight spacing.

[0016] Each vertex P ideal,i The coordinates are generated by the following constraints: The top vertex (navigator) is located at the highest z-coordinate to ensure line-of-sight and communication coverage; the three vertices of the base form an equilateral triangle (or isosceles triangle) to ensure spatial symmetry and safe spacing; the Euclidean distance between any two vertices is ≥ d. safe (Minimum safety distance, the ground control station uses a preset value), the formula is as follows: ||P ideal,i -P ideal,j ||≥d safe ; i,j∈{1,2,3,4},i≠j.

[0017] Steps 1-3: The ground control station generates trajectory sequences for each UAV along the vertices of the ideal tetrahedron, obtaining the preliminary trajectory T for each UAV. i : T i ={P ideal，i (t0),P ideal，i (t1),…,P ideal，i (t n )}; Among them, t k This represents the time node of the trajectory, where k represents the index of the time, ranging from 0 to n; P ideal，i (t k ) indicates that the i-th drone is in t k The vertex position; In this embodiment, the generation of the initial trajectory Ti should consider that the trajectory nodes are evenly distributed over the time interval [t0, t1]. n [This ensures smooth flight; vertex P] ideal,i (t k Trajectory can be generated through linear interpolation; if the task area is complex, the trajectory can be fitted using B-spline curves.

[0018] The linear interpolation algorithm is as follows: P ideal,i (t k )=P start,i +[(t) k −t0) / (t n −t0)]×(Pend,i -P start,i ); Among them, P start,i and P end,i These are the coordinates of the drone's start and end points, respectively.

[0019] The B-spline curve fitting trajectory is shown in the following formula: ; in, For B-spline basis functions, These are the coordinates of the control points.

[0020] The B-spline curve fitting trajectory is existing technology, so it will not be described in detail.

[0021] Steps 1-4: The ground control station presets a priority table for wingman takeover based on the wingman's serial number. Priority = [U2, U3, U4]; In Priority, the sorting number of the drone determines the priority order; The priority table is mainly used for drones to take over from the lead drone in the event of multiple drones losing contact. Its sorting rule is based on the smaller number + online status, which is used to ensure that when the lead drone loses contact, the drone can automatically select a new lead drone to achieve takeover and flight safety.

[0022] Steps 1-5: The operator enters the flight safety radius in the ground control station. The ground control station presets the flight safety radius as the flight error radius of each UAV and generates the safety sphere radius for each UAV. Steps 1-6: The ground control station transmits obstacle information, the ideal tetrahedron model, and the preliminary trajectory T. i After the data is packaged with the priority table, it is sent to U1, U2, U3 and U4 respectively. U1, U2, U3 and U4 all receive the data packets sent by the ground control station and store them locally. Steps 1-7: Complete the initialization settings for each formation according to the methods in steps 1-2 to 1-6.

[0023] After each formation is initialized, the following data structure will be formed: Ideal tetrahedron vertex coordinates P ideal,i ; Preliminary trajectory T for each drone i ; safety sphere radius r i ; A communication link has been established between the ground control station and the UAV.

[0024] Step 2: During formation flight, the wingman collects its own position, speed, and flight status in real time and reports them to the navigator periodically; the navigator dynamically adjusts the safety sphere radius of each drone based on obstacle information; Step 2 includes the following steps: Step 2-1: Wingmen U2, U3, and U4 use their own flight control system sensors to collect their own status data and report the status data to the navigator U1 at fixed time intervals; the status data includes the actual position P. real，i (t k ), speed V real，i (t k ) and flight status: P real，i (t k )=(x real，i (t k ),y real，i (t k ),z real，i (t k )); Where, x real，i (t k ),y real，i (t k ),z real，i (t k ) The i-th drone at t k The actual coordinates of the time; k represents the time index; at this time, i takes values ​​of 2, 3, and 4; P real，i (t k ) indicates that the i-th drone is in t k The actual location at that moment; Actual location P real，i (t k The data is mainly collected through the drone's internal positioning system and can be obtained through GPS / IMU fusion.

[0025] V real，i (t k )=(v xi (t k ),v yi (t k ),v zi (t k )); Among them, v xi (t k ) represents the i-th drone in t k The velocity on the x-axis at time t, v yi (t k ) represents the i-th drone in t k The velocity on the y-axis at time t, v zi (tk ) represents the i-th drone in t k The velocity on the z-axis at time t; V real，i (t k ) indicates that the i-th drone is in t k The actual velocity vector at any given moment; V real，i (t k This is obtained from the flight control system of the drone.

[0026] Step 2-2: The navigator U1 receives status data reported by wingmen U2, U3, and U4, and dynamically adjusts the safety sphere radius r of each UAV based on obstacle information. i (t k The specific formula is as follows: r i (t k )=r init，i ×(1+αD obs (t k ))×(1+β||V real，i (t k )||). Where, r i (t k ) represents the i-th drone in t k The radius of the safety sphere at any given time; r init，i The initial safety sphere radius, whose value is sent and set by the ground control station; D obs (t k The information refers to obstacle density, specifically the obstacle density factor, which represents the proportion of obstacles within a certain range around the drone. Its value ranges from 0 to 1; a larger value indicates a more complex environment. real，i (t k )|| indicates that the drone is at t k The velocity modulus at time t is α, which is the obstacle sensitivity coefficient used to control the influence of obstacle density on the radius of the safety ball; α>0; β is the velocity sensitivity coefficient used to control the influence of velocity magnitude on the radius of the safety ball; β>0.

[0027] D obs (t k The information pertains to obstacles, specifically calculated by the ground control station. This is typically achieved using a local voxel grid to statistically analyze the proportion of obstacle points. For example: D obs (t k )=N obs (t k ) / N total ; Where, N obs (t kN represents the number of obstacle points within a unit area calculated / statistically by the ground control station. total The total number of obstacles in the entire area is represented by the voxel grid size, which can be determined by the UAV's sensing range or the mission area scale.

[0028] Step 3: The navigator generates a safety ball for each UAV based on the real-time position and safety ball radius of each UAV in the formation, and discretizes the safety ball to form surface points, constructing a dynamic safety convex polyhedron. The safety convex polyhedron represents the maximum allowable deformation range of the formation, which serves as the spatial constraint for collision detection. Step 3 includes the following steps: Step 3-1: Navigator U1 receives the real-time position P from U2, U3, and U4. real，i (t k ), t k This represents the time node of the trajectory, where k represents the index of the time, ranging from 0 to n; P ideal，i (t k ) indicates that the i-th drone is in t k The vertex position at any given moment; The navigator U1 utilizes a dynamic safety sphere radius r i (t k Generate a safety sphere S for each drone. i (t k ): S i (t k )={X∈R 3 :||X−P ideal，i (t k )||≤r i (t k )}; Among them, S i (t k ) indicates that the i-th drone is in t k The safety sphere at time X represents any point in space, with position coordinates in three dimensions; i takes values ​​of 1, 2, 3, and 4. Step 3-2: Navigator U1 uses the fixed grid method to arrange each safety ball S i (t k Discretize into N s surface point Q i,j : Q i,j ∈SurfacePoints(S i (t k )); Where j takes values ​​from 1 to N s N s Q represents the number of discretization points for each safety ball;i,j Indicates the safety ball S i (t k Discretized boundary points on ) This embodiment uses the fixed grid method, uniformly sampling N on the spherical surface. s Q points i,j Points are generated by evenly dividing latitude / longitude angles to approximate a sphere: ; The fixed mesh method is an existing technology. When using it, in order to reduce the number of points and thus reduce the computational load, while ensuring the accuracy of the convex hull approximation, N is typically used. s Take 50-200.

[0029] Step 3-3: Use the Convex Hull algorithm to generate a dynamic, safe convex polyhedron C(t). k ): ; The ConvexHull algorithm is used to generate safe convex polyhedra from discrete boundary points. Its principle is to find the minimum convex set that encloses all points, so that the maximum allowable deformation space of the UAV formation can be represented as a convex polyhedron. It is used to determine whether any vertex of the formation has crossed the obstacle boundary during collision detection.

[0030] Where ConvexHull(•) is the convex hull algorithm function; C(t) k ) represents time t k A safe convex polyhedron is used to represent the maximum permissible deformation boundary of the entire formation and to serve as a spatial constraint for collision detection. Steps 3-4: The navigator U1 sends the safety convex polyhedron C(t) to the ground control station in real time. k The ground control station will use the safety convex polyhedron C(t) k Add to task monitoring logs.

[0031] Step 4: The navigator uses a safety convex polyhedron for collision detection. When a potential collision is detected, the drone's trajectory is optimized and adjusted while ensuring spatial constraints. Step 4 includes the following steps: Step 4-1: The navigator U1 generates a safe convex polyhedron C(t). k ), thus obtaining the set of vertices V of the convex polyhedron. d (t k ), and the safety convex polyhedron C(t) k ) are considered spatial constraints; Step 4-2: The navigator U1 uses its own sensors to detect the real-time location information O(t) of surrounding obstacles. k ), for obstacles and safety convex polyhedra C(t) kPerform collision detection and obtain the collision detection results: Collision(t k )=1(∃v∈V d (t k ) st v∈O(t k )); Where O(t) k Specifically, it represents the set of spaces occupied by obstacles; 1 (•) represents an indicator function, which is 1 if true and 0 otherwise; Collision(t) k )=1 indicates that a potential collision has been detected, and the trajectory needs to be optimized; Collision(tk) is mainly used for intersection detection of the set of vertices Vd of a safe convex polyhedron and the set of obstacles O. Its principle is that if any vertex enters the space occupied by an obstacle, there is a potential collision. This reduces the amount of computation by using the vertices of the convex polyhedron and does not detect the entire volume of the UAV point by point.

[0032] Step 4-3: When a collision occurs, optimize the reference trajectory of each UAV while satisfying the safety sphere and convex polyhedron constraints: ; ; in, For the i-th drone at time t k The optimized reference position sequence points; The i-th drone at time t k The safety ball; The safe convex polyhedron is obtained by updating the safe sphere based on the optimized reference position; Cost() represents the objective function for trajectory optimization, as follows: P ref，new，i (t k )=P ref，i (t k )+ΔP i (t k ); ||ΔP i (t k )||≤r i (t k ); Where, ΔP i (t k ) represents the trajectory correction vector; In this embodiment, ΔP i (t kThe algorithm calculates the trajectory based on collision detection results between the current trajectory and obstacle positions using the navigator. Specifically, while ensuring the drone remains within its safety sphere, the algorithm fine-tunes the drone's reference position based on detected potential collisions, guiding it to avoid obstacles and maintain a safe distance from other drones in the formation. This correction vector is calculated in real-time and can be based on optimization objectives (such as minimum offset or trajectory smoothness), thus enabling dynamic adjustment and optimization of the trajectory.

[0033] ΔP i (t k Gradient descent or constraint optimization algorithms can be used to achieve minimum trajectory offset and smoothness.

[0034] Step 4-4: Navigator U1 will optimize trajectory P ref，new，i (t k The data is sent to wingmen U2, U3, and U4 respectively, and wingmen U2, U3, and U4 execute the optimized trajectory P. ref，new，i (t k ); Steps 4-5: Navigator U1 will optimize trajectory P ref，new，i (t k The collision detection results are sent to the ground control station, which generates and saves the mission monitoring record.

[0035] Step 5: When the wingman loses contact, the navigator makes a short-term prediction based on the wingman's position and speed before losing contact, generates a virtual reference position, generates a virtual safety sphere, and updates the safety convex polyhedron; when the wingman loses contact, the wingman selects a new navigator according to the replacement priority table, and the new navigator takes over the generation of the safety convex polyhedron, collision detection, and trajectory optimization.

[0036] Step 5 includes the following steps: Step 5-1: The current navigator Ua maintains communication with each wingman through a heartbeat mechanism; if the navigator does not receive a heartbeat packet from a certain wingman Uq within several consecutive heartbeat cycles, it is determined that it has lost contact; the current navigator Ua is initially navigator U1. At this moment, the navigator Ua is in the ideal position P of the lost wingman Uq within the ideal tetrahedral structure. ideal,q (t k A virtual substitution is performed to generate its virtual security sphere S. virt，q (t k ): S virt，q (t k )={X∈R 3 :||X−P ideal,q (t k )||≤r q (t k)}; Where q represents the index number of the missing drone, and the value can be 1, 2, 3 or 4; And based on the safety spheres of other online drones and the virtual safety sphere, an updated dynamic safety convex polyhedron C is constructed. new (t k ): C new (t k =ConvexHull({S j (t k )} j∈A ∪{S virt，q (t k )} q∈L ); Where j represents the index of the drone that has not lost contact, A represents the set of drones that have not lost contact, and L represents the set of drones that have lost contact. The navigator sends the updated safety convex polyhedron to the ground control station for recording and monitoring; Step 5-2: Each wingman receives the heartbeat packet from the current navigator's Ua in real time; if a wingman does not receive the navigator's heartbeat within the specified timeout period, while the other wingmen communicate normally, the navigator takeover process begins. In the preset replacement priority table, the online wingman Um with the highest priority is automatically promoted to the new navigator and broadcasts the replacement notice to other wingmen and the ground control station; Based on the principle, the new navigator Um is positioned P in the tetrahedron. ideal,a (t k A virtual replacement is created for the missing navigator, generating its virtual safety sphere: S virt，a (t k )={X∈R 3 :||X−P ideal,a (t k )||≤r a (t k )}; Construct a new safe convex polyhedron C by combining the currently online wingman safety sphere. new (t k ): C new (t k =ConvexHull({S j (t k )} j∈A ∪{S virt，a (t k )}); The new lead aircraft sends it to the ground control station to update the mission log; Step 5-3: After receiving the updated safety convex polyhedron, the ground control station immediately records the loss of contact event, generates a loss of contact alarm message, and stores it in the mission monitoring database for future event analysis and retrospection.

[0037] This invention discloses a collaborative trajectory planning method for UAV swarms, which solves the technical problems of dynamic obstacle avoidance, collision detection, trajectory optimization, and automatic takeover when the wingman fails during collaborative flight in three-dimensional space. The invention uses four UAVs to form a three-dimensional ideal tetrahedron, achieving a stable spatial layout and trajectory planning for the swarm. Each UAV generates a real-time safety sphere, and a safe convex polyhedron is constructed using a convex hull algorithm to achieve the maximum allowable deformation range of the entire formation, providing precise spatial constraints for collision detection. During flight, the wingman dynamically adjusts the radius of the safety sphere based on obstacle information and UAV status to optimize the trajectory and achieve dynamic obstacle avoidance. In the event of a wingman or wingman losing contact, this solution uses a virtual safety sphere and a takeover priority table to achieve automatic UAV takeover and update of the formation's safe convex polyhedron, ensuring mission continuity.

Claims

1. A method for cooperative trajectory planning of unmanned aerial vehicle (UAV) swarms, characterized in that: Includes the following steps: Step 1: The ground control station generates a preliminary mission plan based on the mission area, obstacle information, number of UAVs, and initial positions entered by the operator; the UAVs are grouped into formations of four, forming an ideal tetrahedron structure, where the four UAVs are designated as the four vertices, U1, U2, U3, and U4; U1 is designated as the navigator and serves as the top vertex of the ideal tetrahedron, while U2, U3, and U4 are designated as wingmen and serve as the bottom vertices; a preliminary trajectory is generated using the ideal tetrahedron, and a priority table for replacing the navigator is preset; the ground control station presets the safe flight range for each UAV and generates the safety sphere radius for each UAV. The ground control station sends obstacle information, preliminary trajectory, safety sphere radius, ideal tetrahedron model, and succession priority table to the drones in the formation; Step 2: During formation flight, the wingman collects its own position, speed, and flight status in real time and reports them to the navigator periodically; the navigator dynamically adjusts the safety sphere radius of each drone based on obstacle information; Step 3: The navigator generates a safety ball for each UAV based on the real-time position and safety ball radius of each UAV in the formation, and discretizes the safety ball to form surface points, constructing a dynamic safety convex polyhedron. The safety convex polyhedron represents the maximum allowable deformation range of the formation, which serves as the spatial constraint for collision detection. Step 4: The navigator uses a safety convex polyhedron for collision detection. When a potential collision is detected, the drone's trajectory is optimized and adjusted while ensuring spatial constraints. Step 5: When the wingman loses contact, the navigator makes a short-term prediction based on the wingman's position and speed before losing contact, generates a virtual reference position, generates a virtual safety sphere, and updates the safety convex polyhedron; when the wingman loses contact, the wingman selects a new navigator according to the replacement priority table, and the new navigator takes over the generation of the safety convex polyhedron, collision detection, and trajectory optimization.

2. The method for collaborative trajectory planning of unmanned aerial vehicle (UAV) swarms as described in claim 1, characterized in that: Step 1 includes the following steps: Step 1-1: The operator enters the task into the ground control station. The task includes area coordinates, obstacle information, number of drones, and initial position. Steps 1-2: The ground control station groups the drones into squadrons of four based on the number of drones. For each squadron, it generates the vertices P of an ideal tetrahedron. ideal，i P is the coordinate identifier; four drones are set to be located at the vertices of the ideal tetrahedron, denoted as U1, U2, U3 and U4. U1 is located at the top vertex of the ideal tetrahedron and is the navigator, while U2, U3 and U4 are wingmen. The model of an ideal tetrahedron is as follows: P ideal，1 = (x1, y1, z1); P ideal，2 = (x2, y2, z2); P ideal，3 =(x3,y3,z3); P ideal，4 = (x4, y4, z4); Among them, P ideal，i P represents the vertex position of the ideal tetrahedron, where i represents the UAV index, taking values ​​1, 2, 3, and 4 corresponding to U1, U2, U3, and U4; ideal，1 P is the top vertex. ideal，2 P ideal，3 and P ideal，4 All are vertices of the base triangle; x i y i , z i These are the position coordinates of the i-th drone; Steps 1-3: The ground control station generates trajectory sequences for each UAV along the vertices of the ideal tetrahedron, obtaining the preliminary trajectory T for each UAV. i : T i ={P ideal，i (t0),P ideal，i (t1),…,P ideal，i (t n )}; Among them, t k This represents the time node of the trajectory, where k represents the index of the time, ranging from 0 to n; P ideal，i (t k ) indicates that the i-th drone is in t k The vertex position; Steps 1-4: The ground control station presets a priority table for wingman takeover based on the wingman's serial number. Priority = [U2, U3, U4]; In Priority, the sorting number of the drone determines the priority order; Steps 1-5: The operator enters the flight safety radius in the ground control station. The ground control station presets the flight safety radius as the flight error radius of each UAV and generates the safety sphere radius for each UAV. Steps 1-6: The ground control station transmits obstacle information, the ideal tetrahedron model, and the preliminary trajectory T. i After the data is packaged with the priority table, it is sent to U1, U2, U3 and U4 respectively. U1, U2, U3 and U4 all receive the data packets sent by the ground control station and store them locally. Steps 1-7: Complete the initialization settings for each formation according to the methods in steps 1-2 to 1-6.

3. The method for collaborative trajectory planning of unmanned aerial vehicle (UAV) swarms as described in claim 1, characterized in that: Step 2 includes the following steps: Step 2-1: Wingmen U2, U3, and U4 use their own flight control system sensors to collect their own status data and report the status data to the navigator U1 at fixed time intervals; the status data includes the actual position P. real，i (t k ), speed V real，i (t k ) and flight status: P real，i (t k )=(x real，i (t k ),y real，i (t k ),z real，i (t k )); Where, x real，i (t k ),y real，i (t k ),z real，i (t k ) The i-th drone at t k The actual coordinates of the time; k represents the time index; at this time, i takes values ​​of 2, 3, and 4; P real，i (t k ) indicates that the i-th drone is in t k The actual location at that moment; V real，i (t k )=(v xi (t k ),v yi (t k ),v zi (t k )); Among them, v xi (t k ) represents the i-th drone in t k The velocity on the x-axis at time t, v yi (t k ) represents the i-th drone in t k The velocity on the y-axis at time t, v zi (t k ) represents the i-th drone in t k The velocity on the z-axis at time t; V real，i (t k ) indicates that the i-th drone is in t k The actual velocity vector at any given moment; Step 2-2: The navigator U1 receives status data reported by wingmen U2, U3, and U4, and dynamically adjusts the safety sphere radius r of each UAV based on obstacle information. i (t k The specific formula is as follows: r i (t k )=r init，i ×(1+αD obs (t k ))×(1+β||V real，i (t k )||); Where, r i (t k ) represents the i-th drone in t k The radius of the safety sphere at any given time; r init，i The initial safety sphere radius, whose value is sent and set by the ground control station; D obs (t k The information refers to obstacle density, specifically the obstacle density factor, which represents the proportion of obstacles within a certain range around the drone. Its value ranges from 0 to 1; a larger value indicates a more complex environment. real，i (t k )|| indicates that the drone is at t k The velocity modulus at time t is α, which is the obstacle sensitivity coefficient used to control the influence of obstacle density on the radius of the safety ball; α>0; β is the velocity sensitivity coefficient used to control the influence of velocity magnitude on the radius of the safety ball; β>0.

4. The method for collaborative trajectory planning of unmanned aerial vehicle (UAV) swarms as described in claim 1, characterized in that: Step 3 includes the following steps: Step 3-1: Navigator U1 receives the real-time position P from U2, U3, and U4. real，i (t k ), t k This represents the time node of the trajectory, where k represents the index of the time, ranging from 0 to n; P ideal，i (t k ) indicates that the i-th drone is in t k The vertex position at any given moment; The navigator U1 utilizes a dynamic safety sphere radius r i (t k Generate a safety sphere S for each drone. i (t k ): S i (t k )={X∈R 3 :||X−P ideal，i (t k )||≤r i (t k )}; Among them, S i (t k ) indicates that the i-th drone is in t k The safety sphere at time X represents any point in space, with position coordinates in three dimensions; i takes values ​​of 1, 2, 3, and 4. Step 3-2: Navigator U1 uses the fixed grid method to arrange each safety ball S i (t k Discretize into N s surface point Q i,j : Q i,j ∈SurfacePoints(S i (t k )); Where j takes values ​​from 1 to N s N s Q represents the number of discretization points for each safety ball; i,j Indicates the safety ball S i (t k Discretized boundary points on ) Step 3-3: Use the Convex Hull algorithm to generate a dynamic, safe convex polyhedron C(t). k ): ； Where ConvexHull(•) is the convex hull algorithm function; C(t) k ) represents time t k A safe convex polyhedron is used to represent the maximum permissible deformation boundary of the entire formation and to serve as a spatial constraint for collision detection. Steps 3-4: The navigator U1 sends the safety convex polyhedron C(t) to the ground control station in real time. k The ground control station will use the safety convex polyhedron C(t) k Add to task monitoring logs.

5. The method for collaborative trajectory planning of unmanned aerial vehicle (UAV) swarms as described in claim 4, characterized in that: Step 4 includes the following steps: Step 4-1: The navigator U1 generates a safe convex polyhedron C(t). k ), thus obtaining the set of vertices V of the convex polyhedron. d (t k ), and the safety convex polyhedron C(t) k ) are considered spatial constraints; Step 4-2: The navigator U1 uses its own sensors to detect the real-time location information O(t) of surrounding obstacles. k ), for obstacles and safety convex polyhedra C(t) k Perform collision detection and obtain the collision detection results: Collision(t k )=1(∃v∈V d (t k ) s.t. v∈O(t k )), Where O(t) k Specifically, it represents the set of spaces occupied by obstacles; 1 (•) represents an indicator function, which is 1 if true and 0 otherwise; Collision(t) k )=1 indicates that a potential collision has been detected, and the trajectory needs to be optimized; Step 4-3: When a collision occurs, optimize the reference trajectory of each UAV while satisfying the safety sphere and convex polyhedron constraints: ； ； in, For the i-th drone at time t k The optimized reference position sequence points; The i-th drone at time t k The safety ball; The safe convex polyhedron is obtained by updating the safe sphere based on the optimized reference position; Cost() represents the objective function for trajectory optimization, as follows: P ref，new，i (t k )=P ref，i (t k )+ΔP i (t k ); ||ΔP i (t k )||≤r i (t k ); Where, ΔP i (t k ) represents the trajectory correction vector; Step 4-4: Navigator U1 will optimize trajectory P ref，new，i (t k The data is sent to wingmen U2, U3, and U4 respectively, and wingmen U2, U3, and U4 execute the optimized trajectory P. ref，new，i (t k ); Steps 4-5: Navigator U1 will optimize trajectory P ref，new，i (t k The collision detection results are sent to the ground control station, which generates and saves the mission monitoring record.

6. The method for collaborative trajectory planning of unmanned aerial vehicle (UAV) swarms as described in claim 4, characterized in that: Step 5 includes the following steps: Step 5-1: The current navigator Ua maintains communication with each wingman through a heartbeat mechanism; if the navigator does not receive a heartbeat packet from a certain wingman Uq within several consecutive heartbeat cycles, it is determined that it has lost contact; the current navigator Ua is initially navigator U1. At this moment, the navigator Ua is in the ideal position P of the lost wingman Uq within the ideal tetrahedral structure. ideal,q (t k A virtual substitution is performed to generate its virtual security sphere S. virt，q (t k ): S virt，q (t k )={X∈R 3 :||X−P ideal,q (t k )||≤r q (t k )}; Where q represents the index number of the missing drone, and the value can be 1, 2, 3 or 4; And based on the safety spheres of other online drones and the virtual safety sphere, an updated dynamic safety convex polyhedron C is constructed. new (t k ): C new (t k )=ConvexHull({S j (t k )} j∈A ∪{S virt，q (t k )} q∈L ); Where j represents the index of the drone that has not lost contact, A represents the set of drones that have not lost contact, and L represents the set of drones that have lost contact. The navigator sends the updated safety convex polyhedron to the ground control station for recording and monitoring; Step 5-2: Each wingman receives the heartbeat packet from the current navigator's Ua in real time; if a wingman does not receive the navigator's heartbeat within the specified timeout period, while the other wingmen communicate normally, the navigator takeover process begins. In the preset replacement priority table, the online wingman Um with the highest priority is automatically promoted to the new navigator and broadcasts the replacement notice to other wingmen and the ground control station; Based on the principle, the new navigator Um is positioned P in the tetrahedron. ideal,a (t k A virtual replacement is created for the missing navigator, generating its virtual safety sphere: S virt，a (t k )={X∈R 3 :||X−P ideal,a (t k )||≤r a (t k )}; Construct a new safe convex polyhedron C by combining the currently online wingman safety sphere. new (t k ): C new (t k )=ConvexHull({S j (t k )} j∈A ∪{S virt，a (t k )}); The new lead aircraft sends it to the ground control station to update the mission log; Step 5-3: After receiving the updated safety convex polyhedron, the ground control station immediately records the loss of contact event, generates a loss of contact alarm message, and stores it in the mission monitoring database for future event analysis and retrospection.

Images