Unmanned aerial vehicle cluster trajectory planning method and system under unreliable communication

Abstract

The invention discloses an unmanned aerial vehicle cluster trajectory planning method and system under unreliable communication, and the method comprises the steps: firstly building and updating a three-dimensional map based on an airborne sensor of an unmanned aerial vehicle, then building a local map based on the data of the sensor at the current moment, and recognizing an idle area and an occupied area; generating a track for avoiding an obstacle by combining time and space constraints; the track and environment information are shared through communication, and collision is avoided through coordination; an MPC module is used to predict a trajectory when trajectory information is lacked, and collision check is performed. And in the feasible trajectory execution process, environment changes are coped with through timing collision detection, and safe navigation is ensured. According to the method, collision of the unmanned aerial vehicles due to absence of cluster real-time trajectory information for a long time is avoided by optimizing a communication network and an MPC prediction trajectory; and an MPC module is added to generate a prediction track when the interaction track information is lost so as to check whether the newly generated track is collided or not.

Description

Technical Field

[0001] The present invention belongs to the field of robotics and automation, and specifically relates to a UAV cluster trajectory planning method and system based on model predictive control (MPC), aiming to solve the problems of UAV coordination and collision avoidance under unreliable communication conditions. Background Art

[0002] With the rapid development of drone technology, it has been widely used in military, civil, agricultural, logistics and other fields. In particular, with the increase in mission complexity, multi-drone collaborative operations have attracted more and more attention due to their efficiency, flexibility and cost-effectiveness. Among them, trajectory planning is one of the core technologies of drone clusters, which will directly affect the efficiency, safety and reliability of the cluster's mission completion.

[0003] At present, the research on trajectory planning of drone swarms still faces many challenges. For example, ensuring that drones avoid collisions with other drones, obstacles and no-fly zones during flight; drones need to maintain effective communication to achieve information sharing and task coordination; drone trajectories need to be able to adjust dynamically to cope with environmental changes and emergencies; the trajectory planning problem of multiple drones is a high-dimensional, multi-constrained complex optimization problem that requires efficient algorithms to solve, etc.

[0004] In view of the above-mentioned problems and challenges, the existing research methods for multi-UAV trajectory planning can be mainly divided into the following categories. The centralized method relies on a central control unit, which uniformly plans the trajectory according to the status and mission requirements of all UAVs. The advantage of this method is that it can obtain the global optimal solution, but its disadvantage is that as the number of UAVs increases, the computational and communication burdens increase dramatically, and the risk of single point failure is high. The distributed method is that each UAV autonomously plans the trajectory and achieves the mission goal through local communication and collaboration. This method has good scalability and robustness, but it is difficult to guarantee the global optimal. Planning methods based on heuristics and intelligent optimization, including genetic algorithms, particle swarm optimization, ant colony algorithms, etc., search for the optimal trajectory by simulating evolution or group behavior in nature, and can provide efficient solutions in more complex environments, but this method is computationally complex and has strong parameter dependence, and poor real-time performance. Planning methods based on learning. In recent years, deep learning and reinforcement learning have shown great potential in UAV trajectory planning, but the calculation requires a large amount of computation, which is difficult for the onboard equipment of small UAVs to cope with. These methods can achieve autonomous planning and optimization in dynamic and uncertain environments by learning historical data and environmental characteristics. In view of the shortcomings of the above methods, the present invention proposes a path planning method with strong real-time performance, moderate computational complexity and not prone to falling into local optimum. Summary of the invention

[0005] The present invention aims to solve the collision risk caused by unreliable communication in the trajectory planning of drone clusters and provide a trajectory planning method for drone clusters under unreliable communication.

[0006] The specific technical solutions are as follows:

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

[0008] Step (1): Create and update a three-dimensional map based on the drone’s onboard sensors to ensure accurate positioning.

[0009] Consider a scenario where a swarm of quadcopters starts from a starting point, flies through a map with several obstacles to reach a given target point, collects data about the surrounding environment through the drone's onboard sensors, and then uses the collected sensor data to create and continuously update a three-dimensional map of the environment. At the same time, the accurate positioning of the drone in the map is determined during the map construction process.

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

[0011] The drone builds a local map of its surroundings based on current sensor data, identifies free spaces in the local map that can be safely navigated, and distinguishes between occupied and free areas.

[0012] Step (3): Generate a trajectory that avoids obstacles by combining temporal and spatial constraints.

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

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

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

[0016] Step (6): During the execution of the feasible trajectory, timed collision detection is performed to respond to environmental changes and ensure safe navigation.

[0017] Furthermore, the entire A* path search algorithm process implements A* path search given the starting and ending states under the premise of a known map. The specific operations are as follows:

[0018] First, sampling is performed based on the three-dimensional map and the local map information generated in step (2) to discretely obtain the trajectory points and the speed and acceleration of the starting point; then the algorithm search parameters are set, such as the maximum speed, maximum acceleration, search range and other conditional parameters; the three-dimensional positions of the starting point and the target point are converted to the grid map, and the trajectory points sampled from the starting point are used as the nodes for subsequent use, and the nodes are expanded and pruned; finally, the expanded nodes are searched in an iterative loop to determine whether the current node can calculate a curve directly to the target point. If so, the path node is returned as the initial path, otherwise, the node expansion and pruning are continued.

[0019] After the initial path is obtained, the initial trajectory is fitted by the uniform B-spline method to obtain the control points. During the fitting process, soft constraint optimization is performed based on smoothness, distance from obstacles, and speed and acceleration exceeding the limit to obtain the control points of the optimized trajectory. Then, the non-uniform B-spline method is used to iteratively adjust the control points of the optimized trajectory in time until the speed and acceleration in the optimized trajectory meet the constraints and a feasible trajectory is obtained.

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

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

[0022] The drones communicate based on dynamic communication groups, which are dynamically adjusted according to the distance between the drones. Communication is performed when the distance between the two drones is within a certain range.

[0023] When a UAV receives the trajectory information of other UAVs, 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 a new collision-free trajectory is regenerated using the method of step (3).

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

[0025] If the shared trajectory and environment information is 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.

[0026] In the MPC module, a least squares problem is constructed using the existing historical trajectory information, and a high-order regularization constraint of acceleration is added to solve the polynomial expression of the trajectory to predict the future trajectory positions of the remaining UAVs in 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 a new collision-free trajectory is regenerated using the method of step (3).

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

[0029] During the 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 UAV cluster trajectory planning system for unreliable communication, comprising:

[0031] The map generation module is used to process the depth image data, odometer and point cloud data and construct a three-dimensional map, while updating the occupancy status of the raster map to provide the UAV 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 three-dimensional grid map.

[0033] First, the map information obtained by the map generation module is sampled to discretely obtain the trajectory points and the speed and acceleration of the starting point; then the algorithm search parameters are set, such as the maximum speed, maximum acceleration, search range and other conditional parameters; the three-dimensional positions of the starting point and the target point are converted to the grid map, and the trajectory points sampled from the starting point are used as the nodes for subsequent use, and the nodes are expanded and pruned; finally, the expansion node is searched in an iterative loop to determine whether the current node can calculate a curve directly to the target point. If so, the path node is returned as the initial path, otherwise, the node expansion and pruning are continued.

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

[0035] After the initial path is obtained, the initial trajectory is fitted by the uniform B-spline method to obtain the control points. During the fitting process, soft constraint optimization is performed based on smoothness, distance from obstacles, and speed and acceleration exceeding the limit to obtain the control points of the optimized trajectory. Then, the non-uniform B-spline method is used to iteratively adjust the control points of the optimized trajectory in time until the speed and acceleration in the optimized trajectory meet the constraints and a feasible trajectory is obtained.

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

[0037] First, the path search module and the B-spline optimization module generate a trajectory to avoid obstacles based on the map information generated by the map generation module and combined with time and space constraints. Then, the robot communication module shares the trajectory and environmental information, performs collision detection, and coordinates to avoid collisions. When trajectory information is missing, the MPC module is used to predict the trajectory and perform collision checks. At the same time, during the execution of the feasible trajectory, regular collision detection is used to respond to environmental changes and ensure safe navigation.

[0038] When a collision risk is detected, the planning 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 the B-spline optimization module to regenerate a new collision-free trajectory.

[0039] Robot communication module, which uses TCP and UDP protocols to communicate between drones, sets up TCP and UDP connections, and handles 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 through the communication network. The UAVs communicate according to the dynamic communication group, and the group is dynamically adjusted according to the distance between the UAVs. When the distance between the two UAVs is within a certain range, communication is carried out.

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

[0042] The MPC module provides a method to predict future trajectories based on historical time period information, and achieves collision avoidance by combining the predicted trajectories of multiple drones. This module is based on an event-triggered method, which enables drones to achieve real-time collision detection when there is a lack of shared trajectory information from other drones, thereby reducing the collision risk caused by unreliable communication.

[0043] Furthermore, the method of predicting future trajectories based on the historical time period information in the MPC module is specifically operated as follows:

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

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

[0046] Where A and B are the state transfer matrix and control input matrix of the UAV, x k represents the state at step k, u k represents the control input at step k. According to the above dynamic model, the optimization problem of the MPC module is expressed as:

[0047]

[0048] where x ref,k is the reference state at the kth step, Q and R are the weighted matrices of the state error and control input, respectively, and λ acc is the weight of acceleration regularization, λ jerk is the weight of jerk regularization, is the acceleration at step k, dotx k is the jerk at the kth step. The calculation formula of the important jerk regularization term is:

[0049]

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

[0051] This optimization problem is solved by a quadratic programming (QP) method to generate a smooth prediction trajectory including jerk regularization.

[0052] Beneficial effects of the present invention:

[0053] The present invention mainly solves the problem that under unreliable communication conditions, UAVs may collide due to lack of mutual interactive information. The trajectory planning method proposed in the present invention avoids collisions between UAVs due to long-term lack of cluster real-time trajectory information by optimizing the communication network and MPC predicted trajectory. Optimizing the communication network can not only reduce the communication burden, but also reduce unnecessary trajectory information and reduce the amount of calculation during trajectory planning. The MPC module is added to generate a predicted trajectory when the interactive trajectory information is missing to check whether the newly generated trajectory will collide. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0055] Figure 1 Schematic diagram of the system architecture of an embodiment of the present invention.

[0056] Figure 2 This is the process of generating a plurality of {p, v} pairs of control points through the trajectory of an obstacle in an embodiment of the present invention.

[0057] Figure 3 The figure is a calculation flow chart of the MPC module according to an embodiment of the present invention.

[0058] Figure 4 The following are experimental results of the embodiments of the present invention.

[0059] Figure 5 The figures are the results of comparative experiments of the embodiments of the present invention. DETAILED DESCRIPTION

[0060] In order to enable relevant personnel to more clearly understand the technical content of the present invention, the following is a description with reference to the accompanying drawings:

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

[0062] The method mainly includes the following steps:

[0063] Step (1): The quadrotor drone cluster starts from the starting point and flies through obstacles to reach the end point.

[0064] Consider a scenario where a cluster of quadrotor drones starts from a starting point, flies through a map with several obstacles to reach a given end point. The drones collect data on the surrounding environment through onboard sensors, and then use the collected sensor data (mainly depth image data taken by a depth camera) to establish a three-dimensional point cloud map based on the depth image to create and continuously update the three-dimensional map of the environment.

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

[0066] Based on the current sensor data (mainly lidar) {x n ,z n}, where x n and z n Represent the location information and observation data of the object respectively, n represents the time series, and the grid mapping method based on lidar is used to construct a local map of its surrounding environment to solve the possible map:

[0067]

[0068] Among them, m represents the state of the map, m * represents the optimal map state, that is, the map state with the maximum posterior probability given the observation data and position data; Indicates that among all possible map states, the map state that maximizes the posterior probability P is selected; P(m|x 1:t ,z 1:t ) represents the posterior probability of the map state m, that is, given the position information x from t = 1 to the current time t 1:t and observation data z 1:tThe probability of map state m in the case of . Use the OccupancyGrid Algorithm to update the free space in the local grid map that can be safely navigated, distinguishing the occupied area p(m i )=1, free area p(m i )=0, unknown area p(m i )=0.5. Each grid in the map is independent, and the mathematical expression is:

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

[0070] After deriving the formula for solving the optimal map state, we get:

[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 the occupancy information from the sensor; l(m i |x 1:t ,z 1:t ) represents the inverse observation model of the laser radar, which is unknown; l(m i |x 1:t-1 ,z 1:t-1 ) represents the grid m i The state at time t-1, the recursive term, is known; l(m i ) represents the grid m i The prior value of , which is the same for all grids, is given and known at the initialization.

[0073] Solving the above formula completes the optimal map state m * Solve and get the grid map.

[0074] Step (3): Generate the initial trajectory by combining the A* search algorithm and use the cost function to optimize the trajectory, considering smoothness, collision, dynamic feasibility and terminal progress.

[0075] Considering the constraints of time and space, and combining the trajectory and environmental information shared by other drones, a trajectory is generated from the current position of the drone to the target position while avoiding obstacles. First, sampling is performed based on the three-dimensional map and the grid map information generated in step (2), and 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 starting point and the end point are initialized, and the priority queue openSet_ is used to store the nodes to be processed. Then, the search is iterated. According to the current node, the Pontryagin principle is used to solve the heuristic function as the control cost. Then, the minimum cost node is found based on the obtained control cost, and the node state is updated and placed in openSet_; finally, if the target point is reached, the path is backtracked to obtain the initial path.

[0076] The control cost is mainly based on the Pontryagin principle to select nodes according to the following function conditions:

[0077]

[0078] where r = {s, c, d, t}, represents 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 and generate {p,v} pairs to maximize the obstacle distance and ensure smoothness and feasibility.

[0080] The trajectory is optimized based on the B-spline method to ensure smoothness, feasibility, and compliance with dynamic constraints. Figure 2 As shown, the trajectory points of the initial trajectory are used as the control points Q for the B-spline optimization method. The present invention estimates the obstacle distance using the environmental information independently owned by each control point Q, and obtains the {p, v} pair according to the vertical obstacle section of the control point Q, where p represents the anchor point on the obstacle surface and v represents the safe direction pointing from the inside of the obstacle to the outside. Define the i th Control Point Q i To j th The distance of the obstacle d ij for:

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

[0082] The general process of {p,v} pair generation and trajectory optimization is to obtain a simple initial trajectory Φ without considering collision based on the initial trajectory generated in step (3), first search for a safe path Г connecting the two ends of the collision segment of Φ, then generate a vector v from Φ to Г, and define p on the obstacle surface. In the process of generating {p,v} pairs, maximize d ij, and soft constraint optimization is performed based on smoothness, distance from obstacles, and speed and acceleration exceeding limits to obtain the control points of the optimized trajectory. Then, the non-uniform B-spline method is used to iteratively adjust the control points of the optimized trajectory in time until the speed and acceleration in the optimized trajectory meet the constraints and a feasible trajectory is obtained.

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

[0084] At the same time, each drone communicates with each other, shares feasible trajectories and environmental information, and coordinates their trajectories based on this information to avoid collisions. At the same time, the communication grouping is adjusted based on the distance between drones in the latest environmental information. When the distance between two drones is greater than a given distance threshold, the drone that exceeds the distance threshold is deleted from the communication group, and the drone whose distance is less than the distance threshold is added to update the members of different communication groups.

[0085] The coordinated collision avoidance process is that when a UAV receives the trajectory information of other UAVs, it stores it and immediately performs collision detection. If a collision risk is detected, a new collision-free trajectory is regenerated.

[0086] Step (6): Perform collision check to detect static and dynamic obstacle collision risks and calculate the group collision avoidance penalty function using soft obstacle constraints.

[0087] The generated trajectory is checked for potential collisions with dynamic or static obstacles. Collision detection for dynamic obstacles mainly detects whether there is a collision with another UAV’s trajectory. If the distance between the two trajectories at a certain point in time is less than the set threshold, a collision is considered to have occurred. At the same time, the group collision avoidance penalty function J of UAV k is applied to the group collision avoidance penalty function J of UAV k. w,k Expressed as a soft obstacle constraint:

[0088]

[0089] Among them, t s and t e is the trajectory Φ k (t) The global start and end time within the time span, C is the custom proxy cleanup. E: = diag(1,1,1 / c), c>1 is to convert the Euclidean distance into an ellipsoid distance with a shorter main axis on the z-axis to mitigate the risk of undershoot. d k,i (t) represents the cost distance between trajectories k and i at time t, which is 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 checking and cost function optimization.

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

[0092] In the MPC module, a least squares problem is constructed using the existing historical trajectory information, and the high-order regularization constraint of acceleration and the Jerk regularization constraint are added to solve the polynomial expression of the trajectory to predict the future trajectory positions of the remaining drones in the communication network. The calculation process of the MPC module is shown in the attached figure. Figure 3 As shown, the steps to calculate the predicted trajectory are:

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

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

[0095] Where A and B are the state transfer matrix and control input matrix of the UAV, x k represents the state at step k, u k represents the control input at step k. According to the above dynamic model, the optimization problem of the MPC module is expressed as:

[0096]

[0097] where x ref,k is the reference state at the kth step, Q and R are the weighted matrices of state error and control input, respectively, and λ acc is the weight of acceleration regularization, λ jerk is the weight of jerk regularization, is the acceleration at step k, dotx k is the jerk at the kth step. The calculation formula of the important jerk regularization term is:

[0098]

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

[0100] This optimization problem is solved by a quadratic programming (QP) method to generate a smooth prediction trajectory including 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 regenerated.

[0102] The predicted trajectory obtained by the MPC module is checked for collision, and a new group collision avoidance penalty function J is calculated based on the newly obtained predicted trajectory. w Adding the cost function, the formula is as follows:

[0103]

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

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

[0106] During the trajectory execution process, 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 of steps (3) and (4) to ensure continuous and safe navigation.

[0107] In one embodiment, the present invention provides a UAV cluster 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 three-dimensional grid map.

[0109] First, the map information obtained by the map generation module is sampled to discretely obtain the trajectory points and the speed and acceleration of the starting point; then the algorithm search parameters are set, such as the maximum speed, maximum acceleration, search range and other conditional parameters; the three-dimensional positions of the starting point and the target point are converted to the grid map, and the trajectory points sampled from the starting point are used as the nodes for subsequent use, and the nodes are expanded and pruned; finally, the expansion node is searched in an iterative loop to determine whether the current node can calculate a curve directly to the target point. If so, the path node is returned as the initial path, otherwise, the node expansion and pruning are continued.

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

[0111] After the initial path is obtained, the initial trajectory is fitted by the uniform B-spline method to obtain the control points. During the fitting process, soft constraint optimization is performed based on smoothness, distance from obstacles, and speed and acceleration exceeding the limit to obtain the control points of the optimized trajectory. Then, the non-uniform B-spline method is used to iteratively adjust the control points of the optimized trajectory in time until the speed and acceleration in the optimized trajectory meet the constraints and a feasible trajectory is obtained.

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

[0113] First, the path search module and the B-spline optimization module generate a trajectory to avoid obstacles based on the map information generated by the map generation module and combined with time and space constraints. Then, the robot communication module shares the trajectory and environmental information, performs collision detection, and coordinates to avoid collisions. When trajectory information is missing, the MPC module is used to predict the trajectory and perform collision checks. At the same time, during the execution of the feasible trajectory, regular collision detection is used to respond to environmental changes and ensure safe navigation.

[0114] When a collision risk is detected, the planning 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 the B-spline optimization module to regenerate a new collision-free trajectory.

[0115] Robot communication module, which uses TCP and UDP protocols to communicate between drones, sets up TCP and UDP connections, and handles 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 through the communication network. The UAVs communicate according to the dynamic communication group, and the group is dynamically adjusted according to the distance between the UAVs. When the distance between the two UAVs is within a certain range, communication is carried out.

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

[0118] The MPC module provides a method to predict future trajectories based on historical time period information, and achieves collision avoidance by combining the predicted trajectories of multiple drones. This module is based on an event-triggered method, which enables drones to achieve real-time collision detection when there is a lack of shared trajectory information from other drones, thereby reducing the collision risk caused by unreliable communication.

[0119] Based on the method of predicting future trajectories using historical time period information, the specific operations are as follows:

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

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

[0122] Where A and B are the state transfer matrix and control input matrix of the UAV, x k represents the state at step k, u k represents the control input at step k. According to the above dynamic model, the optimization problem of the MPC module is expressed as:

[0123]

[0124] where x ref,k is the reference state at the kth step, Q and R are the weighted matrices of state error and control input, respectively, and λ acc is the weight of acceleration regularization, λ jerk is the weight of jerk regularization, is the acceleration at step k, dotx k is the jerk at the kth step. The calculation formula of the important jerk regularization term is:

[0125]

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

[0127] This optimization problem is solved by a quadratic programming (QP) method to generate a smooth prediction trajectory including jerk regularization.

[0128] The present invention solves the risk of UAV collision under unreliable communication by optimizing communication grouping and MPC predicted trajectory. Optimizing communication grouping reduces the communication burden, and the MPC module generates a predicted trajectory alternative when trajectory information is missing, ensuring the safety and effectiveness of trajectory planning.

[0129] The experiment of the present invention is simulated and compared based on the ROS system on Ubuntu 18.04. In the simulation, re-planning or prediction of whether a collision will occur is triggered every second. In the simulation experiment, the benefit results of the obstacle avoidance performance and path planning results of the system under different broadcast frequencies are tested. At the same time, it is compared with the existing path planning method that does not use MPC predicted trajectory, the path planning method that does not use dynamic broadcast grouping, and the path planning method that broadcasts path information but does not consider path prediction. The benefit optimization results are shown in the attached figure. Figure 4 The comparison results are shown in the attached Figure 5 ,In various scenarios, the effectiveness of this method is better than the other three methods.

[0130] It should be known that the parts not elaborated in detail in this specification belong to the prior art. Relevant technical personnel should understand that the above embodiments are only 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 replacements made on the basis of the present invention are within the protection scope of the rights of the present invention.

Claims

1. A method for trajectory planning of drone clusters under unreliable communication, characterized in that: The following steps are involved: Step (1): Create and update a three-dimensional map based on the drone’s onboard sensors; The drone's onboard sensors collect data about the surrounding environment, and then use the collected sensor data to create and continuously update a three-dimensional map of the environment; while determining the drone's accurate positioning in the map during the map construction process; Step (2): construct a local map based on the sensor data at the current moment and identify the free and occupied areas; The drone builds a local map of its surroundings based on current sensor data, identifies free spaces in the local map that can be safely navigated, and distinguishes between occupied and free areas; Step (3): Generate a trajectory to avoid obstacles by combining time and space constraints; Considering the time and space constraints, an initial path from the current position of the UAV to the target position is obtained through the A* path search algorithm; Step (4): Share trajectory and environment information through communication and coordinate to avoid collision; Step (5): Use the MPC module to predict the trajectory and perform collision checks when trajectory information is missing; Step (6): During the execution of the feasible trajectory, timed collision detection is performed to respond to environmental changes and ensure safe navigation.

2. According to claim 1, a method for UAV cluster trajectory planning under unreliable communication is characterized in that: The entire A* path search algorithm process is to implement A* path search given the starting and ending states under the premise of a known map; the specific operations are as follows: First, sampling is performed based on the three-dimensional map and the local map information generated in step (2), and the trajectory points and the speed and acceleration of the starting point are discretely obtained; then the algorithm search parameters are set; the three-dimensional positions of the starting point and the target point are converted to the grid map, and the trajectory points sampled from the starting point are used as the nodes for subsequent use, and the nodes are expanded and pruned; finally, the expanded nodes are searched in an iterative loop to determine whether the current node can calculate a curve directly to the target point. If so, the path node is returned as the initial path, otherwise, the node expansion and pruning are continued; After the initial path is obtained, 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 speed and acceleration exceeding limits to obtain the control points of the optimized trajectory. Then, the non-uniform B-spline method is used to iteratively adjust the control points of the optimized trajectory in time until the velocity and acceleration in the optimized trajectory meet the constraints and a feasible trajectory is obtained.

3. A method for UAV cluster trajectory planning under unreliable communication according to claim 1 or 2, characterized in that: The specific method of step (4) is as follows: After obtaining a feasible trajectory, the UAV transmits the feasible trajectory information and environmental information to other UAVs in the communication group through the communication network; The drones communicate according to dynamic communication groups, which are dynamically adjusted according to the distance between the drones. When the distance between the two drones is within a certain range, communication is performed; When a UAV receives the trajectory information of other UAVs, 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 a new collision-free trajectory is regenerated using the method of step (3).

4. A method for planning the trajectory of a drone cluster under unreliable communication according to claim 1 or 2, characterized in that: The specific method of step (5) is as follows: If the shared trajectory and environment information is 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 the existing historical trajectory information, and a high-order regularization constraint of acceleration is added to solve the polynomial expression of the trajectory to predict the future trajectory positions of the remaining drones in 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 a new collision-free trajectory is regenerated using the method of step (3).

5. The method for trajectory planning of drone clusters under unreliable communication according to claim 1, characterized in that: The specific method of step (6) is as follows: During the trajectory execution process, 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 and safe navigation.

6. The UAV cluster trajectory planning system according to claim 4, characterized in that: The specific operations of the MPC module are as follows: First, initialize the parameters and define the dynamic model of the drone as: x k+1 =Ax k +Bu k Where A and B are the state transfer matrix and control input matrix of the UAV, x k represents the state at step k, u k represents the control input at step k; according to the above dynamic model, the optimization problem of the MPC module is expressed as: where x ref,k is the reference state at the kth step, Q and R are the weighted matrices of the state error and control input, respectively, and λ acc is the weight of acceleration regularization, λ jerk is the weight of jerk regularization, is the acceleration at step k, dotx k is the jerk at the kth step; the calculation formula of the important jerk regularization term is: Where Δt is the time step initially defined; This optimization problem is solved by the quadratic programming (QP) method to generate a smooth prediction trajectory including jerk regularization.

7. A UAV cluster trajectory planning system for unreliable communication, characterized in that: include: The map generation module is used to process the depth image data, odometer and point cloud data and build a 3D map, while updating the occupancy status of the grid map to provide 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 three-dimensional grid map; B-spline optimization module, based on the initial trajectory generated by the A* path search method, uses uniform B-spline method and non-uniform B-spline method to optimize and obtain a feasible trajectory; The planning management module coordinates the path planning and execution of the aircraft by establishing a finite state machine; based on the feasible trajectory information and environmental information of the UAV sent by the robot communication module, the map generation module, the path search module, the B-spline optimization module and the MPC module coordinate their work to complete the trajectory planning of the UAV cluster; Robot communication module, which uses TCP and UDP protocols to communicate between drones, sets up TCP and UDP connections, and handles message serialization and deserialization; A path visualization module is used to visualize different data in path planning, including target point, global path, initial path and optimal path; The MPC module provides a method to predict future trajectories based on the use of historical time period information, and combines the predicted trajectories of multiple UAVs to achieve collision avoidance.

8. The UAV cluster trajectory planning system for unreliable communication according to claim 7, characterized in that: The method in the MPC module to predict future trajectories based on the use of historical time period information is as follows: First, initialize the parameters and define the dynamic model of the drone as: x k+1 =Ax k +Bu k Where A and B are the state transfer matrix and control input matrix of the UAV, x k represents the state at step k, u k represents the control input at step k; according to the above dynamic model, the optimization problem of the MPC module is expressed as: where x ref,k is the reference state at the kth step, Q and R are the weighted matrices of the state error and control input, respectively, and λ acc is the weight of acceleration regularization, λ jerk is the weight of jerk regularization, is the acceleration at step k, dotx k is the jerk at the kth step; the calculation formula of the important jerk regularization term is: Where Δt is the time step initially defined; This optimization problem is solved by a quadratic programming method to generate a smooth prediction trajectory including jerk regularization.

Images