A method and system for UAV cluster trajectory planning under unreliable communication

Abstract

This invention discloses a method and system for drone swarm trajectory planning under unreliable communication conditions. First, a 3D map is created and updated based on the drone's onboard sensors. Then, a local map is constructed based on the current sensor data, identifying idle and occupied areas. Next, a trajectory avoiding obstacles is generated by combining time and space constraints. Trajectory and environmental information are shared through communication to coordinate collision avoidance. An MPC module is used to predict trajectories and perform collision checks when trajectory information is lacking. Furthermore, periodic collision detection is used during the execution of feasible trajectories to cope with environmental changes and ensure safe navigation. This invention optimizes the communication network and MPC-predicted trajectories to prevent drone collisions due to prolonged lack of real-time swarm trajectory information. The addition of an MPC module to generate predicted trajectories when interactive trajectory information is missing checks whether newly generated trajectories will cause collisions.

Description

Technical Field

[0001] This invention belongs to the field of robotics and automation, specifically relating to a method and system for drone swarm trajectory planning based on model predictive control (MPC), aiming to solve the problems of drone cooperation and collision avoidance under unreliable communication conditions. Background Technology

[0002] With the rapid development of drone technology, it has been widely applied in military, civilian, agricultural, and logistics fields. Especially with the increasing complexity of tasks, multi-drone collaborative operations are attracting more and more attention due to their efficiency, flexibility, and cost-effectiveness. Trajectory planning, as one of the core technologies of drone swarms, directly affects the swarm's mission completion efficiency, safety, and reliability.

[0003] Current research on drone swarm trajectory planning still faces many challenges. For example, ensuring that drones avoid collisions with other drones, obstacles, and no-fly zones during flight; maintaining effective communication between drones to achieve information sharing and task coordination; the ability to dynamically adjust drone trajectories to cope with environmental changes and emergencies; and the fact that trajectory planning for multiple drones is a complex optimization problem with high dimensions and multiple constraints, requiring efficient algorithms to solve.

[0004] To address the aforementioned problems and challenges, existing multi-UAV trajectory planning methods can be broadly categorized as follows: Centralized methods rely on a central control unit that plans trajectories uniformly based on the status and mission requirements of all UAVs. The advantage of this method is its ability to obtain a globally optimal solution, but its disadvantages include a sharp increase in computational and communication burdens as the number of UAVs increases, and a high risk of single-point failures. Distributed methods involve each UAV autonomously planning its trajectory, achieving mission objectives through local communication and cooperation. This method offers good scalability and robustness, but struggles to guarantee global optimality. Heuristic and intelligent optimization-based planning methods, including genetic algorithms, particle swarm optimization, and ant colony optimization, search for optimal trajectories by simulating natural evolution or group behavior, providing efficient solutions in complex environments. However, these methods are computationally complex, highly parameter-dependent, and have poor real-time performance. Learning-based planning methods, particularly deep learning and reinforcement learning, have demonstrated significant potential in UAV trajectory planning in recent years. However, they require substantial computational resources, which is beyond the capabilities of small UAV onboard equipment. These methods learn from historical data and environmental characteristics, enabling autonomous planning and optimization in dynamic and uncertain environments. To address the shortcomings of the above methods, this invention proposes a path planning method that is highly real-time, has a moderate computational load, and is less prone to getting trapped in local optima. Summary of the Invention

[0005] This invention aims to address the collision risk caused by unreliable communication in UAV swarm trajectory planning, and provides a method for UAV swarm trajectory planning under unreliable communication conditions.

[0006] The specific technical solution adopted is as follows:

[0007] In one aspect, the present invention provides a method for planning the trajectory of a drone swarm under unreliable communication, comprising the following steps:

[0008] Step (1): Create and update a 3D map based on the UAV’s onboard sensors to ensure accurate positioning.

[0009] Consider a scenario where a swarm of quadcopter drones starts from a starting point, flies through a map with several obstacles, and reaches a given target location. The drones collect data about their surroundings using onboard sensors, and then use this data to create and continuously update a 3D map of the environment. Simultaneously, the precise location of the drones within the map is determined during the map construction process.

[0010] Step (2): Construct a local map based on the sensor data at the current moment and identify idle and occupied areas.

[0011] The drone constructs a local map of its surroundings based on current sensor data, identifies vacant spaces in the local map where it can navigate safely, and distinguishes between occupied and vacant areas.

[0012] Step (3): Combine time and space constraints to generate a trajectory that avoids obstacles.

[0013] Considering the constraints of time and space, an initial path from the UAV's current position to the target position is obtained through the A* path search algorithm.

[0014] Step (4): Share trajectory and environmental information through communication to coordinate and avoid collisions.

[0015] Step (5): Use the MPC module to predict the trajectory when trajectory information is missing and perform collision checks.

[0016] Step (6): During the execution of the feasible trajectory, periodic collision detection is used to cope with environmental changes and ensure safe navigation.

[0017] Furthermore, the entire A* pathfinding algorithm process is based on a known map, implementing A* pathfinding given the start and end point states. The specific operations are as follows:

[0018] First, based on the 3D map and the local map information generated in step (2), sampling is performed to discretely obtain the trajectory points and the velocity and acceleration of the starting point; then, the algorithm search parameters are set, such as maximum velocity, maximum acceleration, search range and other conditional parameters; the 3D positions of the starting point and the target point are converted to the raster map, and the trajectory points obtained from the sampling of the starting point are used as nodes for subsequent use to expand and prune the nodes; finally, the expansion nodes are iteratively searched and expanded, and it is determined whether the current node can calculate a curve that leads directly to the target point. If it can, the path node is returned as the initial path; otherwise, the node expansion and pruning continue.

[0019] After obtaining the initial path, control points are obtained by fitting the initial trajectory using a uniform B-spline method. During the fitting process, soft constraints are applied to optimize the trajectory based on smoothness, distance from obstacles, and velocity acceleration exceeding limits, resulting in control points for the optimized trajectory. Then, a non-uniform B-spline method is used to iteratively adjust the control points of the optimized trajectory over time until the velocity acceleration in the optimized trajectory meets the constraints, thus obtaining a feasible trajectory.

[0020] Furthermore, step (4) is as follows:

[0021] After obtaining a feasible trajectory, the drone transmits the feasible trajectory information and environmental information to other drones in the communication group through the communication network.

[0022] The drones communicate using dynamic communication packets, which are dynamically adjusted based on the distance between the drones. Communication occurs when the distance between two drones is within a certain range.

[0023] When a drone receives trajectory information from other drones, it stores it and immediately performs collision detection. If a collision risk is detected, the map information is updated according to the collision risk, and the method in step (3) is used to regenerate a new collision-free trajectory.

[0024] Furthermore, the specific method for step (5) is as follows:

[0025] If shared trajectory and environmental information is lacking, the MPC module is enabled to calculate the predicted trajectory based on the current state of the drone and perform collision checks on the predicted trajectory.

[0026] In the MPC module, a least squares problem is constructed using existing historical trajectory information, and a high-order regularization constraint on acceleration is added to solve the polynomial expression of the trajectory, thereby enabling the prediction of the future trajectory positions of other UAVs within the communication network.

[0027] Collision detection is performed based on the predicted trajectory information. If a collision risk is detected, the map information is updated based on the collision risk, and the method in step (3) is used to regenerate a new collision-free trajectory.

[0028] Furthermore, step (6) is as follows:

[0029] During trajectory execution, collision detection is scheduled at regular intervals. When a collision is detected or the environment changes, the map information is updated according to the collision risk and the trajectory is replanned using the method in step (3) to ensure continuous safe navigation.

[0030] In another aspect, the present invention provides a drone swarm trajectory planning system for unreliable communication, comprising:

[0031] The map generation module processes depth image data, odometry and point cloud data to build a 3D map, while updating the occupancy status of the raster map, providing the drone with more accurate and real-time environmental perception.

[0032] The path search module uses the A* path planning algorithm to search for the initial path from the starting point to the target point in the 3D grid map.

[0033] First, the map generation module samples the map information it acquires, discretely obtaining trajectory points and the velocity and acceleration of the starting point. Then, it sets algorithm search parameters, such as maximum velocity, maximum acceleration, and search range. The 3D positions of the starting and target points are converted into a raster map, and the trajectory points sampled from the starting point are used as nodes for subsequent expansion and pruning. Finally, it iteratively searches for expansion nodes, determining whether the current node can calculate a curve directly to the target point. If so, it returns a path node as the initial path; otherwise, it continues node expansion and pruning.

[0034] The B-spline optimization module uses uniform and non-uniform B-spline methods to process and optimize the initial trajectory generated by the A* path search method to obtain a feasible trajectory. The specific operations are as follows:

[0035] After obtaining the initial path, control points are obtained by fitting the initial trajectory using a uniform B-spline method. During the fitting process, soft constraints are applied to optimize the trajectory based on smoothness, distance from obstacles, and velocity acceleration exceeding limits, resulting in control points for the optimized trajectory. Then, a non-uniform B-spline method is used to iteratively adjust the control points of the optimized trajectory over time until the velocity acceleration in the optimized trajectory meets the constraints, thus obtaining a feasible trajectory.

[0036] The planning and management module establishes a finite state machine to coordinate the path planning and execution of the aircraft. Based on the feasible trajectory information and environmental information of the UAV sent by the robot communication module, it coordinates the map generation module, path search module, B-spline optimization module, and MPC module to complete the UAV swarm trajectory planning. The specific operations are as follows:

[0037] First, the path search module and B-spline optimization module generate an obstacle-avoiding trajectory based on map information from the map generation module, combined with time and space constraints. Then, the robot communication module shares trajectory and environmental information, performs collision detection, and coordinates collision avoidance. When trajectory information is lacking, the MPC module predicts the trajectory and performs collision checks. Simultaneously, during the execution of a feasible trajectory, timed collision detection is used to address environmental changes and ensure safe navigation.

[0038] When a collision risk is detected, the planning and management module controls the map generation module to update the map information according to the collision risk, and then uses the path search module and B-spline optimization module to regenerate a new collision-free trajectory.

[0039] The robot communication module uses TCP and UDP protocols to communicate between drones, setting up TCP and UDP connections and handling message serialization and deserialization.

[0040] After obtaining a feasible trajectory, the UAV transmits the feasible trajectory information and environmental information to other UAVs in the communication group via the communication network. The UAVs communicate based on dynamic communication groups, which are dynamically adjusted according to the distance between the UAVs. Communication occurs when the distance between two UAVs is within a certain range.

[0041] The path visualization module is used to visualize different data in path planning, including information such as target point, global path, initial path and optimal path.

[0042] The MPC module provides a method for predicting future trajectories using historical time-based information, and combines multi-drone trajectory prediction to achieve collision avoidance. This module uses an event-triggered approach, enabling real-time collision detection for drones when there is a lack of shared trajectory information with other drones, thereby mitigating the collision risks associated with unreliable communication.

[0043] Furthermore, the method in the MPC module for predicting future trajectories based on historical time period information operates as follows:

[0044] First, initialize the parameters and define the dynamic model of the drone as follows:

[0045] x k+1 =Ax k +Bu k

[0046] Where A and B are the state transition matrix and control input matrix of the UAV, respectively, x k U represents the state at step k. k This represents the control input at step k. Based on the dynamic model described above, the optimization problem of the MPC module can be expressed as:

[0047]

[0048] Where x ref,k Let Q be the reference state at step k, and let R be the weighted matrices of the state error and the control input, respectively. Let λ be the reference state at step k. acc The weights for acceleration regularization, λ jerk These are the weights of the Jerk regularization. For the acceleration at step k, dotx k Let be the jerk at step k. The important jerk regularization term is calculated using the following formula:

[0049]

[0050] Where Δt is the initially defined time step.

[0051] This optimization problem is solved using a quadratic programming (QP) method to generate smooth predictive trajectories with jerk regularization.

[0052] The beneficial effects of this invention are:

[0053] This invention primarily addresses the problem of potential collisions between drones due to a lack of interactive information in unreliable communication environments. The proposed trajectory planning method avoids collisions caused by prolonged absence of real-time cluster trajectory information by optimizing the communication network and using MPC (Multi-Process Controller) to predict trajectories. Optimizing the communication network reduces communication overhead and decreases computation by minimizing unnecessary trajectory information during trajectory planning. The inclusion of an MPC module generates predicted trajectories when interactive trajectory information is missing, and then checks whether these newly generated trajectories will lead to collisions. Attached Figure Description

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

[0055] Figure 1 This is a schematic diagram of the system architecture according to an embodiment of the present invention.

[0056] Figure 2 This is the process of generating several {p,v} control points based on the trajectory of obstacles in an embodiment of the present invention.

[0057] Figure 3 This is a flowchart illustrating the calculation process of the MPC module in an embodiment of the present invention.

[0058] Figure 4 These are the experimental results of an embodiment of the present invention.

[0059] Figure 5 The results are from comparative experiments in the embodiments of the present invention. Detailed Implementation

[0060] To enable those skilled in the art to more clearly understand the technical content of this invention, the following description is provided in conjunction with the accompanying drawings:

[0061] The purpose of this invention is to address the collision risk caused by unreliable communication in the scenario of UAV swarm trajectory planning, and to propose a UAV swarm trajectory planning method based on MPC.

[0062] The main steps of this method are as follows:

[0063] Step (1): The quadcopter drone swarm starts from the starting point, flies over obstacles, and reaches the destination.

[0064] Consider a scenario where a swarm of quadcopter drones starts from a starting point, flies through a map with several obstacles, and arrives at a given endpoint. The drones collect data about the surrounding environment through onboard sensors, and then use the collected sensor data (mainly depth images taken by depth cameras) to create and continuously update a 3D map of the environment based on the depth images.

[0065] Step (2): The UAV constructs a local grid map based on sensor data and the 3D map established in step (1) to identify a safe navigation space.

[0066] Based on sensor data (mainly LiDAR) at the current moment {x n ,z n}, where x n and z n Let represent the object's location information and observation data, respectively, and 'n' represent the time series. A local map of the object's surrounding environment is constructed using a LiDAR-based grid mapping method, and possible maps are solved.

[0067]

[0068] Where m represents the state of the map, m * This represents the optimal map state, which is the map state with the highest posterior probability given the observation data and location data. This represents selecting the map state that maximizes the posterior probability P from all possible map states; P(m|x 1:t ,z 1:t ) represents the posterior probability of map state m, that is, given the location information x from t=1 to the current time t. 1:t and observation data z 1:tGiven the given conditions, determine the probability of map state m. Use the OccupancyGrid Algorithm to update the free space in the local raster map that allows for safe navigation, distinguishing occupied areas p(m). i ) = 1, free region p(m) i ) = 0, unknown region p(m) i ) = 0.5. Each grid cell in the map is independent, and the mathematical expression is:

[0069] p(m)=∏p(m i )

[0070] The formula for solving the optimal map state is derived as follows:

[0071] l(m i |x 1:t ,z 1:t )=l(m i |x t ,z t )+l(m i |x 1:t-1 ,z 1:t-1 )-l(m i )

[0072] Where l represents the logarithmic probability form of the grid occupancy probability p, which can more effectively update and fuse occupancy information from the sensor; l(m i |x 1:t ,z 1:t ) represents the inverse observation model of the lidar, which is unknown; l(m) i |x 1:t-1 ,z 1:t-1 ) represents the raster m i The state at time t-1, the recursive term, are known; l(m) i ) represents the raster m i The prior value, which is the same for all rasters, is given during initialization and is known.

[0073] Solving the above formula yields the optimal map state m. * Solving for the raster map yields the raster map.

[0074] Step (3): Generate an initial trajectory using the A* search algorithm, optimize the trajectory using a cost function, and consider smoothness, collision, dynamic feasibility and terminal progress.

[0075] Considering time and space constraints, and combining trajectory and environmental information shared by other UAVs, a trajectory from the UAV's current position to the target position is generated, while avoiding obstacles. First, based on the 3D map and the raster map information generated in step (2), the trajectory points are discretely obtained as nodes to be processed. The A* path search algorithm is used to generate the initial trajectory. First, the start and end points are initialized, and the nodes to be processed are stored in the priority queue openSet_. Then, the search is iterative. Based on the current node, the heuristic function is obtained using the Pontryagin principle as the control cost. Then, based on the obtained control cost, the node with the minimum cost is found, the node state is updated and put into openSet_. Finally, if the target point is reached, the path is backtracked to obtain the initial path.

[0076] The control cost is primarily based on Pontryagin's principle, selecting nodes according to the following functional conditions:

[0077]

[0078] Where r = {s, c, d, t}, representing smoothness, collision, dynamic feasibility, and terminal progress, respectively, and λ represents the corresponding weight.

[0079] Step (4): Use the B-spline method to optimize the initial trajectory, generate {p,v} pairs, maximize the obstacle distance, and ensure smoothness and feasibility.

[0080] The trajectory is optimized using the B-spline method to ensure smoothness, feasibility, and compliance with dynamic constraints. For example... Figure 2 As shown, the trajectory points of the initial trajectory are used as control points Q for the B-spline optimization method. This invention utilizes the environmental information independently possessed by each control point Q to estimate obstacle distances. Based on the perpendicular cross-section of the obstacle from control point Q, {p,v} pairs are obtained, where p represents the anchor point on the obstacle surface, and v represents the safe direction from the inside of the obstacle to the outside. Define the i-th... th Control point Q i up to the jth th The distance d of the obstacle ij for:

[0081] d ij =(Q i -p ij )·v ij

[0082] The general process of generating and optimizing the {p,v} pairs is as follows: Based on the initial trajectory generated in step (3), a naive initial trajectory Φ is obtained without considering collisions. First, a safe path Г connecting the two ends of the collision segment of Φ is searched. Then, a vector v is generated from Φ to Г, and p is defined on the obstacle surface. During the generation of the {p,v} pairs, d is maximized. ijThe system then performs soft constraint optimization based on smoothness, distance from obstacles, and excessive velocity acceleration to obtain control points for the optimized trajectory. A non-uniform B-spline method is then used to iteratively adjust the control points over time until the velocity acceleration in the optimized trajectory meets the constraints, thus obtaining a feasible trajectory.

[0083] Step (5): Each drone shares its trajectory and environmental information and coordinates its trajectory to avoid collisions.

[0084] Simultaneously, the drones communicate with each other, sharing feasible trajectories and environmental information to coordinate their paths and avoid collisions. Communication groups are adjusted based on the latest environmental information and the distances between drones. When the distance between two drones exceeds a given distance threshold, the drone exceeding the threshold is removed from the communication group, while drones with a distance less than the threshold are added, updating the membership of different communication groups.

[0085] The coordinated collision avoidance process involves storing the trajectory information received by one drone from another drone and immediately performing collision detection. If a collision risk is detected, a new collision-free trajectory is regenerated.

[0086] Step (6): Perform a collision check to detect the risk of collision with static and dynamic obstacles, and calculate the group collision avoidance penalty function using soft obstacle constraints.

[0087] The generated trajectories undergo collision checks to detect potential collisions with dynamic or static obstacles. Collision detection for dynamic obstacles primarily checks for collisions with the trajectory of another drone; a collision is considered to have occurred if the distance between the two trajectories at a given time point is less than a set threshold. Simultaneously, a group collision avoidance penalty function J is applied to drone k. w,k Described as a soft obstacle constraint:

[0088]

[0089] Among them, t s and t e For trajectory Φ k (t) Global start and end times within the time span, where C is a custom agent cleanup. E:=diag(1,1,1 / c), c>1 converts the Euclidean distance to an ellipsoidal distance with a shorter principal axis on the z-axis to mitigate underslip risk. d k,i (t) represents the cost distance between trajectories k and i at time t, used to solve the group collision avoidance penalty function J. w,k .

[0090] Step (7): Enable the MPC module to predict the trajectory, perform collision checks and cost function optimization.

[0091] If shared trajectory and environmental information is lacking, the MPC module is enabled to calculate the predicted trajectory based on the current state of the drone.

[0092] The MPC module utilizes existing historical trajectory information to construct a least-squares problem, incorporating high-order regularization constraints on acceleration and Jerk regularization constraints to solve for the polynomial expression of the trajectory, thereby predicting the future trajectory positions of other UAVs within the communication network. The computational flow of the MPC module is shown in the attached figure. Figure 3 As shown, the steps for calculating the predicted trajectory are as follows:

[0093] First, initialize the parameters and define the dynamic model of the drone as follows:

[0094] x k+1 =Ax k +Bu k

[0095] Where A and B are the state transition matrix and control input matrix of the UAV, respectively, x k U represents the state at step k. k This represents the control input at step k. Based on the dynamic model described above, the optimization problem of the MPC module can be expressed as:

[0096]

[0097] Where x ref,k Let Q be the reference state at step k, and let R be the weighted matrices of the state error and the control input, respectively. Let λ be the reference state at step k. acc The weights for acceleration regularization, λ jerk These are the weights of the Jerk regularization. For the acceleration at step k, dotx k Let be the jerk at step k. The important jerk regularization term is calculated using the following formula:

[0098]

[0099] Where Δt is the initially defined time step.

[0100] This optimization problem is solved using a quadratic programming (QP) method to generate smooth predictive trajectories with jerk regularization.

[0101] Collision detection is performed based on the predicted trajectory information. If a collision risk is detected, a new collision-free trajectory is generated.

[0102] Collision checks are performed on the predicted trajectories obtained from the MPC module, and a new group collision avoidance penalty function J is calculated based on the newly obtained predicted trajectories. w Incorporating the cost function, the formula is as follows:

[0103]

[0104] Where J is the cost of trajectory selection obtained from the new solution, and λ r J r λ represents the cost calculated based on smoothness, collision, dynamic feasibility, and terminal progress. w J w The soft cost constraints and weights are calculated based on the predicted trajectory.

[0105] Step (8): During the execution of the feasible trajectory, real-time collision detection is used to cope with environmental changes and ensure safe navigation.

[0106] During trajectory execution, collision detection is scheduled at regular intervals. When a collision is detected or the environment changes, the map information is updated according to the collision risk, and the trajectory is replanned using the methods in steps (3) and (4) to ensure continuous safe navigation.

[0107] In one embodiment, the present invention provides a drone swarm trajectory planning system, comprising:

[0108] The path search module uses the A* path planning algorithm to search for the initial path from the starting point to the target point in the 3D grid map.

[0109] First, the map generation module samples the map information it acquires, discretely obtaining trajectory points and the velocity and acceleration of the starting point. Then, it sets algorithm search parameters, such as maximum velocity, maximum acceleration, and search range. The 3D positions of the starting and target points are converted into a raster map, and the trajectory points sampled from the starting point are used as nodes for subsequent expansion and pruning. Finally, it iteratively searches for expansion nodes, determining whether the current node can calculate a curve directly to the target point. If so, it returns a path node as the initial path; otherwise, it continues node expansion and pruning.

[0110] The B-spline optimization module uses uniform and non-uniform B-spline methods to process and optimize the initial trajectory generated by the A* path search method to obtain a feasible trajectory. The specific operations are as follows:

[0111] After obtaining the initial path, control points are obtained by fitting the initial trajectory using a uniform B-spline method. During the fitting process, soft constraints are applied to optimize the trajectory based on smoothness, distance from obstacles, and velocity acceleration exceeding limits, resulting in control points for the optimized trajectory. Then, a non-uniform B-spline method is used to iteratively adjust the control points of the optimized trajectory over time until the velocity acceleration in the optimized trajectory meets the constraints, thus obtaining a feasible trajectory.

[0112] The planning and management module establishes a finite state machine to coordinate the path planning and execution of the aircraft. Based on the feasible trajectory information and environmental information of the UAV sent by the robot communication module, it coordinates the map generation module, path search module, B-spline optimization module, and MPC module to complete the UAV swarm trajectory planning. The specific operations are as follows:

[0113] First, the path search module and B-spline optimization module generate an obstacle-avoiding trajectory based on map information from the map generation module, combined with time and space constraints. Then, the robot communication module shares trajectory and environmental information, performs collision detection, and coordinates collision avoidance. When trajectory information is lacking, the MPC module predicts the trajectory and performs collision checks. Simultaneously, during the execution of a feasible trajectory, timed collision detection is used to address environmental changes and ensure safe navigation.

[0114] When a collision risk is detected, the planning and management module controls the map generation module to update the map information according to the collision risk, and then uses the path search module and B-spline optimization module to regenerate a new collision-free trajectory.

[0115] The robot communication module uses TCP and UDP protocols to communicate between drones, setting up TCP and UDP connections and handling message serialization and deserialization.

[0116] After obtaining a feasible trajectory, the UAV transmits the feasible trajectory information and environmental information to other UAVs in the communication group via the communication network. The UAVs communicate based on dynamic communication groups, which are dynamically adjusted according to the distance between the UAVs. Communication occurs when the distance between two UAVs is within a certain range.

[0117] The path visualization module is used to visualize different data in path planning, including information such as target point, global path, initial path and optimal path.

[0118] The MPC module provides a method for predicting future trajectories using historical time-based information, and combines multi-drone trajectory prediction to achieve collision avoidance. This module uses an event-triggered approach, enabling real-time collision detection for drones when there is a lack of shared trajectory information with other drones, thereby mitigating the collision risks associated with unreliable communication.

[0119] The method for predicting future trajectories based on historical time period information is as follows:

[0120] First, initialize the parameters and define the dynamic model of the drone as follows:

[0121] x k+1 =Ax k +Bu k

[0122] Where A and B are the state transition matrix and control input matrix of the UAV, respectively, x k U represents the state at step k. k This represents the control input at step k. Based on the dynamic model described above, the optimization problem of the MPC module can be expressed as:

[0123]

[0124] Where x ref,k Let Q be the reference state at step k, and let R be the weighted matrices of the state error and the control input, respectively. Let λ be the reference state at step k. acc The weights for acceleration regularization, λ jerk These are the weights of the Jerk regularization. For the acceleration at step k, dotx k Let be the jerk at step k. The important jerk regularization term is calculated using the following formula:

[0125]

[0126] Where Δt is the initially defined time step.

[0127] This optimization problem is solved using a quadratic programming (QP) method to generate smooth predictive trajectories with jerk regularization.

[0128] This invention addresses the risk of drone collisions under unreliable communication by optimizing communication packets and using the MPC (Multi-Purpose Controller) trajectory prediction module. Optimized communication packets reduce the communication burden, while the MPC module generates a predicted trajectory alternative when trajectory information is missing, ensuring the safety and effectiveness of trajectory planning.

[0129] This invention's experiments were conducted on Ubuntu 18.04 using a ROS system for simulation and comparison. During the simulation, replanning or collision prediction was triggered every second. The simulation experiments tested the obstacle avoidance performance and path planning results at different broadcast frequencies. Simultaneously, comparisons were made with existing path planning methods that do not use MPC to predict trajectories, do not use dynamic broadcast groups, and broadcast path information without considering path prediction. The optimization results are attached. Figure 4 As shown in the attached figure, the comparison results are as follows. Figure 5 In various scenarios, this method is more effective than the other three methods.

[0130] It should be understood that any parts not described in detail in this specification belong to the prior art. Those skilled in the art should understand that the above embodiments are merely to help readers understand the principles and implementation methods of the present invention, and the scope of protection of the present invention is not limited to such embodiments. All equivalent substitutions made based on the present invention are within the scope of protection of the present invention.

Claims

1. A method for planning the trajectory of unmanned aerial vehicle (UAV) swarms under unreliable communication, characterized in that, Includes the following steps: Step (1): Create and update a 3D map based on the UAV's onboard sensors; The drone collects data about the surrounding environment using its onboard sensors, and then uses the collected sensor data to create and continuously update a 3D map of the environment; at the same time, the drone's accurate location in the map is determined during the map building process. Step (2): Construct a local map based on the sensor data at the current moment and identify idle and occupied areas; The drone constructs a local map of its surrounding environment based on current sensor data, identifies free space in the local map where it can navigate safely, and distinguishes between occupied and free areas; Step (3): Generate a trajectory that avoids obstacles by combining time and space constraints; Considering the constraints of time and space, an initial path from the UAV's current position to the target position is obtained through the A* path search algorithm; Step (4): Share trajectory and environmental information through communication to coordinate and avoid collisions; After obtaining a feasible trajectory, the drone transmits the feasible trajectory information and environmental information to other drones in the communication group through the communication network. The drones communicate based on dynamic communication packets, which are dynamically adjusted according to the distance between the drones. Communication occurs when the distance between two drones is within a certain range. When a drone receives trajectory information from other drones, it stores it and immediately performs collision detection. If a collision risk is detected, the map information is updated according to the collision risk and the method in step (3) is used to regenerate a new collision-free trajectory. Step (5): Use the MPC module to predict the trajectory when trajectory information is missing and perform collision checks; Step (6): During the execution of the feasible trajectory, periodic collision detection is used to cope with environmental changes and ensure safe navigation.

2. The method for planning the trajectory of a drone swarm under unreliable communication as described in claim 1, characterized in that, The entire A* pathfinding algorithm process is based on a known map, and implements A* pathfinding given the start and end points. The specific operations are as follows: First, based on the 3D map and the local map information generated in step (2), sampling is performed to discretely obtain the trajectory points and the velocity and acceleration of the starting point; then, the algorithm search parameters are set; the 3D positions of the starting point and the target point are converted to the raster map, and the trajectory points obtained from the sampling of the starting point are used as nodes for subsequent use to expand and prune the nodes; finally, iteratively search for expanded nodes, and determine whether the current node can calculate a curve that leads directly to the target point. If it can, the path node is returned as the initial path; otherwise, node expansion and pruning continue. After obtaining the initial path, the control points are obtained by fitting the initial trajectory using the uniform B-spline method. During the fitting process, soft constraint optimization is performed based on smoothness, distance from obstacles, and velocity acceleration exceeding limits to obtain the control points of the optimized trajectory. Then, the control points of the optimized trajectory are iteratively adjusted over time using the non-uniform B-spline method until the velocity and acceleration in the optimized trajectory meet the constraints, thus obtaining a feasible trajectory.

3. A method for planning the trajectory of a drone swarm under unreliable communication as described in claim 1 or 2, characterized in that, The specific method for step (5) is as follows: If the shared trajectory and environmental information are missing, the MPC module is enabled to calculate the predicted trajectory based on the current state of the drone and perform collision checks on the predicted trajectory. In the MPC module, a least squares problem is constructed using existing historical trajectory information, and a high-order regularization constraint on acceleration is added to solve the polynomial expression of the trajectory, thereby enabling the prediction of the future trajectory positions of other UAVs within the communication network. Collision detection is performed based on the predicted trajectory information. If a collision risk is detected, the map information is updated based on the collision risk, and the method in step (3) is used to regenerate a new collision-free trajectory.

4. The method for unmanned aerial vehicle (UAV) swarm trajectory planning under unreliable communication as described in claim 1, characterized in that, The specific method for step (6) is as follows: During trajectory execution, collision detection is scheduled at regular intervals. When a collision is detected or the environment changes, the map information is updated according to the collision risk and the trajectory is replanned using the method in step (3) to ensure continuous safe navigation.

5. The method for planning the trajectory of a drone swarm under unreliable communication as described in claim 3, characterized in that, The specific operation of the MPC module is as follows: First, initialize the parameters and define the dynamic model of the drone as follows: Where A and B are the state transition matrix and control input matrix of the UAV. This represents the state at step k. This represents the control input at step k; based on the dynamic model above, the optimization problem of the MPC module can be expressed as: in Let Q and R be the reference state at step k, respectively, and let Q and R be the weighted matrices of the state error and the control input. These are the weights for acceleration regularization. These are the weights of the Jerk regularization. Let the acceleration be at step k. Let jerk be the value at step k; the important jerk regularization term is calculated using the following formula: in The initial time step; This optimization problem is solved using the quadratic programming (QP) method, generating a smooth predicted trajectory with Jerk regularization.

6. A trajectory planning system for unmanned aerial vehicle (UAV) swarms under unreliable communication, characterized in that, include: The map generation module is used to process depth image data, odometry and point cloud data and build a 3D map, while updating the occupancy status of the raster map, providing the drone with more accurate and real-time environmental perception. The path search module uses the A* path planning algorithm to search for the initial path from the starting point to the target point in the 3D grid map; The B-spline optimization module uses uniform B-spline and non-uniform B-spline methods to process and optimize the initial trajectory generated by the A* path search method to obtain a feasible trajectory. The planning and management module establishes a finite state machine to coordinate the path planning and execution of the aircraft; based on the feasible trajectory information and environmental information of the UAV sent by the robot communication module, it controls the map generation module, path search module, B-spline optimization module and MPC module to coordinate their work and complete the UAV swarm trajectory planning. After obtaining a feasible trajectory, the drone transmits the feasible trajectory information and environmental information to other drones in the communication group through the communication network. The drones communicate based on dynamic communication packets, which are dynamically adjusted according to the distance between the drones. Communication occurs when the distance between two drones is within a certain range. When a drone receives trajectory information from other drones, it stores it and immediately performs collision detection. If a collision risk is detected, the map information is updated based on the collision risk and a new collision-free trajectory is regenerated. The robot communication module uses TCP and UDP protocols to communicate between drones, setting up TCP and UDP connections and handling message serialization and deserialization; The path visualization module is used to visualize different data in path planning, including the target point, global path, initial path, and optimal path. The MPC module provides a method for predicting future trajectories based on historical time period information, and combines multi-UAV trajectory prediction to achieve collision avoidance.

7. A drone swarm trajectory planning system for unreliable communication as described in claim 6, characterized in that, The method in the MPC module for predicting future trajectories based on historical time period information operates as follows: First, initialize the parameters and define the dynamic model of the drone as follows: Where A and B are the state transition matrix and control input matrix of the UAV. This represents the state at step k. This represents the control input at step k; based on the dynamic model above, the optimization problem of the MPC module can be expressed as: in Let Q and R be the reference state at step k, respectively, and let Q and R be the weighted matrices of the state error and the control input. These are the weights for acceleration regularization. These are the weights of the Jerk regularization. Let the acceleration be at step k. Let jerk be the value at step k; the important jerk regularization term is calculated using the following formula: in The initial time step; This optimization problem is solved using a quadratic programming method to generate a smooth predicted trajectory with Jerk regularization.

Images