Unmanned aerial vehicle automatic homeward voyage method based on global topological map and graph search algorithm

Abstract

The invention discloses an unmanned aerial vehicle automatic homeward voyage method based on a global topological map and a graph search algorithm, which comprises a global passable area topological connected graph module, a homeward voyage path search module, a homeward voyage path optimization module and a homeward voyage path error processing module. According to the method, global path searching is carried out in combination with a Dijkstra algorithm, path smoothness, flight time and obstacle avoidance are optimized by using a B spline curve, the precision and speed of path planning are effectively improved, and meanwhile, the system can cope with changes in the environment by updating a topological connected graph and optimizing the path in real time, so that the path planning efficiency is improved. And secondly, the path safety and the flight stability are both considered, and an obstacle distance penalty term is introduced, so that the flight path can avoid the obstacle, and the collision risk is reduced.

Description

Technical Field

[0001] The present invention designs an autonomous return-to-home method for a multi-rotor unmanned aerial vehicle (UAV), and specifically relates to an automatic return-to-home method based on a global topological map and a graph search algorithm. Background Art

[0002] With the in-depth application of drones in logistics, agriculture, emergency rescue, surveying and mapping, the reliability and environmental adaptability of their autonomous return technology have become core issues restricting the development of the industry. Traditional return path planning methods have the following technical defects:

[0003] Limitations of the static environment assumption: Path planning techniques based on graph search algorithms such as A* and Dijkstra, while globally optimal in structured static environments, cannot respond in real time to dynamic obstacles and sudden environmental changes. For example, a Chinese patent document discloses a method, device, and storage medium for autonomous return-to-home decision-making for drones [Application Number: 202311135635.8, Publication Number: CN117075623A]. While this document proposes a hierarchical response mechanism for communication status, it does not address the issue of real-time avoidance of dynamic obstacles.

[0004] Inefficient topology modeling: Existing methods for constructing dynamic topology maps, such as the Chinese patent document "UAV Return to Home Method, Device, UAV, and Storage Medium" (Application No. 201980012461.1, Publication No. CN111742276A), rely on discretized raster map modeling, resulting in exponentially increasing computational complexity and making it difficult to meet the real-time path updating requirements in complex urban environments. Furthermore, topology nodes lack embedded modeling of the UAV's dynamic constraints, which can easily lead to the generation of unflyable paths.

[0005] Unbalanced multi-objective optimization: Traditional methods often use a single optimization objective, ignoring the coupled relationship between flight time, energy efficiency, and path smoothness. A Chinese patent document discloses a method and system for drone return-to-home (ROH) [Application No. 201510239862.4, Publication No. CN104881039A]. While the proposed safe return path selection mechanism incorporates historical trajectory safety range constraints, it fails to integrate battery degradation models and aerodynamic performance parameters, significantly reducing the RTH success rate in low-battery scenarios.

[0006] Insufficient sensor fusion depth: Existing technologies, including a Chinese patent document disclosing a drone return-to-home method, device, equipment, and storage medium [Application Number: 202210316184.7, Publication Number: CN114740885A], often rely on single GPS positioning, which can easily cause positioning drift in signal-blocked areas. While some solutions incorporate visual SLAM-assisted positioning, these solutions lack tightly coupled data fusion from multimodal sensors, resulting in insufficient environmental perception.

[0007] In response to the above technical bottlenecks, the industry urgently needs an autonomous return method that integrates dynamic environment perception, real-time topology optimization and multi-constraint decision-making. Summary of the Invention

[0008] A method for automatic return of drones based on a global topological map and a graph search algorithm, characterized by including the following modules:

[0009] A global traversable area topological connectivity graph module is used to generate a dynamically updated topological connectivity graph based on the probabilistic grid map. The topological connectivity graph is constructed by generating a convex hull and connecting the nodes of the overlapping area. The current position of the drone and the planned trajectory points are used as seeds to generate the constrained convex hull, and the geometric centers of the overlapping parts of the convex hulls are used as topological nodes.

[0010] a return path search module, configured to search for a globally optimal path from the current position of the UAV to the return point in the topologically connected graph using a graph search algorithm, wherein the graph search algorithm is the Dijkstra algorithm, including initializing a starting node, priority queue processing, and neighborhood expansion operations;

[0011] The return path optimization module is used to optimize the global optimal path into a smooth trajectory that meets the UAV's dynamic constraints, generate a B-spline trajectory by intercepting the local path, and optimize its smoothness, flight time and obstacle avoidance distance;

[0012] The return path error handling module is used to regenerate the convex hull, update the topological connectivity graph, and downgrade to the A* algorithm to search for an alternative path when a path collision is detected.

[0013] Preferably, the method for constructing the global traversable area topology connectivity graph module comprises the following steps:

[0014] Step 1: Determine the location constraints of obstacles and unknown areas based on the probability grid map;

[0015] Step 2: Generate the initial convex hull using the current position of the drone or the planned trajectory point as the seed;

[0016] Step 3: When the drone trajectory exceeds the current convex hull range, a new convex hull is generated using the new point on the trajectory as a seed, and the new and old convex hulls are ensured to have overlapping areas.

[0017] Step 4: Take the geometric center of the overlapping area as the topological node, and connect different overlapping nodes of the same convex hull through edges to form a dynamically expanded topological connectivity graph.

[0018] The above technical solution can dynamically update the topological connectivity graph to cope with the ever-changing environmental conditions during the flight of the UAV. Specifically, step 1 models obstacles and unknown areas based on the probabilistic grid map, thereby accurately identifying traversable areas and restricted areas, which provides basic data support for subsequent path planning and optimization. Step 2 generates an initial convex hull based on the UAV's current position or planned trajectory points, ensuring that the initial construction of the topological graph has high initial accuracy. Step 3 further optimizes the dynamic nature of the topological graph. When the UAV's flight trajectory changes and exceeds the range of the current convex hull, the new trajectory points will be used as seeds for generating a new convex hull. This strategy not only enhances the real-time and flexibility of path planning, but also ensures that there is an overlap between the old and new convex hulls, thereby maintaining the continuity and stability of the topological graph. Step 4 further expands the available area and coverage of the topological graph by using the geometric centers of the overlapping areas as topological nodes and establishing connecting edges. This process effectively avoids holes or dead ends in the path planning process and enables the topological connectivity graph to be expanded and adjusted in real time, thereby better adapting to dynamic obstacles or environmental changes that the UAV may encounter during flight.

[0019] In summary, the method for constructing the global traversable area topological connectivity graph module not only improves the path planning capability of the UAV in complex environments, but also enhances its adaptability to dynamic changes during flight, enabling the UAV to complete the return mission safely and quickly in unknown or constantly changing environments.

[0020] Preferably, the Dijkstra algorithm execution steps of the return path search module include:

[0021] Step 1: Initialization phase: The node closest to the current position of the UAV is set as the starting node, and its cumulative cost value g(start) = 0. The cost values ​​of the remaining nodes are infinite.

[0022] Step 2: Loop and extract the top node of the priority queue. If it is the target destination, backtrack to generate a path; otherwise, expand its adjacent nodes.

[0023] Step 3: Calculate temporary accumulative cost values ​​as the neighborhood expands:

[0024] g temp =g current +w (current,neighbor)

[0025] Among them, the edge weight w is the Euclidean distance between nodes;

[0026] Step 4: Update the cumulative cost values ​​and parent node pointers of adjacent nodes until a path is generated or the queue is exhausted.

[0027] Through the above technical solution, the return path search module can effectively realize the global optimal path planning from the current position of the drone to the return point. Specifically, the initialization stage of step 1 lays the foundation for the path search. By setting the node closest to the current position of the drone as the starting node, and setting the cumulative cost value of the starting node to 0, and the cost values ​​of other nodes to infinity, the starting point of the path search is ensured to be clear and accurate; step 2 uses the top node of the priority queue to perform cyclic extraction, and gradually approaches the target end point by continuously expanding the adjacent nodes. When the target end point is found, the algorithm will backtrack to generate a complete path. This process ensures the accuracy and optimality of the path. During the search process, if the current node cannot reach the end point directly, the algorithm will continue to expand the adjacent nodes. The continuity and feasibility of path planning are guaranteed. In step 3, during the neighborhood expansion process, the cost of the path can be accurately calculated by calculating the temporary cumulative cost value and combining it with the edge weight (i.e., the Euclidean distance between nodes). This provides a more accurate decision basis for path search. The introduction of this formula ensures that path planning can take into account the actual flight distance, thereby providing a more accurate and efficient return path. The update operation in step 4 gradually adjusts the direction of the path search by updating the cumulative cost values ​​and parent node pointers of adjacent nodes until a complete return path is generated or the queue is exhausted. Through this iterative method, the algorithm can adjust the path search strategy in real time and respond quickly in a dynamic environment, ensuring that the drone can find an optimal return path.

[0028] In summary, the return path search module realizes the optimal path search of the UAV in a complex environment through the algorithm, effectively solves the accuracy and real-time problems of path planning in a dynamic environment, and enables the UAV to return to the designated destination or take-off point safely and quickly.

[0029] Preferably, the specific optimization method of the return path optimization module is:

[0030] a) Intercept the 10-meter path segment starting from the node closest to the drone;

[0031] b) generating a minimum acceleration (minimum snap) trajectory using the path segment as an initial value;

[0032] c) Parameterize the trajectory into a B-spline curve and optimize its smoothness, total flight time, and obstacle distance penalty;

[0033] d) Introduce the dynamic constraints of the UAV's maximum speed, acceleration and turning radius for secondary optimization.

[0034] Through the above technical solution, the return path optimization module can effectively convert the global optimal path into a smooth trajectory that meets the dynamic constraints of the drone. Specifically, step a intercepts the 10-meter path segment closest to the current position of the drone as the initial path segment, which can ensure that the optimization process starts from a part of the trajectory in the actual flight, avoiding lengthy processing of the entire path and improving calculation efficiency; step b generates a minimum acceleration trajectory with the path segment as the initial value. This method can significantly reduce the oscillation and irregular motion in the path by minimizing the rate of change of acceleration, providing the drone with a smooth and stable flight trajectory. Through this optimization, the smoothness of the trajectory is improved, while reducing the sudden changes or large turns that the drone may encounter during flight, thereby improving flight safety and comfort; step c parameterizes the trajectory into a B-spline curve and further optimizes it. The purpose is to achieve the effect of optimizing the path by comprehensively considering objective functions such as smoothness, flight time and obstacle distance penalty. The introduction of B-spline curves makes the trajectory highly flexible and adjustable, and can avoid collisions with obstacles to the greatest extent without sacrificing flight efficiency. The optimization of flight time helps to improve the return efficiency, while the penalty term of obstacle distance further ensures the safety of flight. Step d introduces dynamic constraints such as the maximum speed, acceleration and turning radius of the drone for secondary optimization. The optimization at this stage ensures that the trajectory meets the physical performance limitations of the drone, avoids trajectory selection that exceeds the capability of the aircraft, and makes the trajectory more in line with the actual flight environment. For example, in the case of high-speed flight or sharp turns, the optimization algorithm can adjust the trajectory according to parameters such as the maximum speed and acceleration of the drone, thereby achieving safe and efficient flight.

[0035] In summary, by comprehensively utilizing minimum acceleration trajectory generation, B-spline curve smoothing, flight time and obstacle avoidance optimization, and secondary optimization of dynamic constraints, the return path optimization module can provide the UAV with a smooth and safe return trajectory that meets actual flight conditions in a dynamic flight environment, effectively improving the execution efficiency and flight safety of the return path.

[0036] Preferably, the specific steps of the return path error processing module include:

[0037] a) When a path collision is detected, the system enters emergency stop mode and deletes the topological edge of the convex hull where the collision point is located;

[0038] b) Regenerate the convex hull using the location near the collision point as a seed, ensuring that the new convex hull overlaps with the existing convex hull;

[0039] c) After updating the topological connectivity graph, re-execute the graph search. If it fails, switch to the A* algorithm to search for alternative paths in the grid map.

[0040] Through the above technical solution, the return path error handling module can effectively deal with the path collision problem encountered by the drone during flight and ensure that it can safely and quickly replan the return path. Specifically, step a immediately enters the emergency stop mode when a path collision is detected, ensuring that the drone can stop in time and avoid further damage or risks. During the emergency stop process, the topological edges of the convex hull where the collision point is located will be deleted. This step effectively eliminates the path connection that may cause a collision and creates conditions for subsequent path correction; step b regenerates a new convex hull with the position near the collision point as the seed. The new convex hull generation process ensures that there is an overlapping area between the new and old convex hulls. This measure ensures This verifies the connectivity of the topological graph and ensures that the drone can always remain in a known, traversable area when replanning the path. This method avoids path breakage or connection errors that may occur in path planning and enhances the stability and reliability of path repair. In step c, the updated topological connectivity graph will execute the graph search algorithm again to find a new optimal path. If the graph search cannot successfully find a feasible path, the system will switch to the A* algorithm and search for alternative paths based on the obstacle information in the grid map. This degradation strategy can ensure that in extreme cases, a usable return path can still be found for the drone, maximizing the robustness and adaptability of the system.

[0041] In summary, the return path error handling module provides an efficient and reliable path repair mechanism through emergency stop mode, convex hull regeneration, topology map updates, and A* algorithm backup path search. This technical solution can promptly respond to the drone's path collision and effectively replan a safe return path, ensuring that the drone maintains efficient and stable flight performance in complex and dynamic environments.

[0042] Preferably, the generation constraints of the convex hull include: using the current position of the drone as an input constraint to ensure that the overlapping area of ​​the new convex hull and the old convex hull covers the current position of the drone.

[0043] Through this technical solution, the convex hull generation constraint ensures that the drone always remains within the traversable area and can adapt to environmental changes in real time. Specifically, the drone's current position is used as an input constraint to ensure that the newly generated convex hull covers the current position. This constraint effectively avoids unsafe situations in path planning and improves path stability. When the drone's trajectory exceeds the current convex hull, a new convex hull is regenerated based on the current position, ensuring that the new and old convex hulls overlap. This avoids holes or breaks in path planning while ensuring the connectivity of the topological graph.

[0044] Preferably, the maintenance of the priority queue includes: when the temporary accumulated cost value of an adjacent node is less than its current value, updating the node and triggering an enqueue operation.

[0045] Through the above technical solution, the maintenance of the priority queue ensures that the graph search algorithm can efficiently select the optimal path during the path finding process. When the temporary cumulative cost value of an adjacent node is less than its current value, the node is updated and the enqueue operation is triggered. This ensures that the priority queue always maintains the latest optimal node. This strategy avoids unnecessary repeated calculations and improves the efficiency and accuracy of the algorithm.

[0046] Preferably, the optimization objective function of the B-spline trajectory includes the following terms:

[0047] a) The smoothness of the trajectory, quantified by the jerk integral;

[0048] b) Total flight time penalty;

[0049] c) The obstacle distance penalty is calculated using the Euclidean distance field in the obstacle grid map.

[0050] Through the above technical solution, the optimization objective function of the B-spline trajectory can balance multiple objectives to ensure that the return path of the UAV has good smoothness, short flight time, and avoids obstacles while meeting the dynamic constraints. Specifically, the trajectory smoothness index in step a is quantified by the integral of acceleration, aiming to optimize the smoothness of the trajectory. The change in acceleration will directly affect the stability and comfort of the flight. By integrating the acceleration, the oscillation and discontinuous changes in the trajectory can be quantified, and then the drastic changes in the path can be reduced by optimizing the objective function, ensuring that the acceleration changes of the UAV during flight are smooth, avoiding sudden acceleration or deceleration, and improving the stability of the flight; the total flight time penalty term in step b is considered The flight time of the return path is calculated. Flight time is an important factor in path optimization because in practical applications, drones usually need to complete tasks in a shorter time, especially in emergency situations. By setting a penalty item for the flight time, it can be ensured that the return path can shorten the flight time as much as possible while ensuring flight safety, thereby improving efficiency. The obstacle distance penalty item in step c is calculated through the Euclidean distance field in the obstacle grid map to ensure that the drone's flight path maintains a sufficient safe distance from obstacles. By calculating the distance between each path point and the obstacle and performing a penalty based on the distance, the optimization algorithm can enable path planning to avoid obstacles as much as possible, reduce collision risks, and improve flight safety.

[0051] In summary, the B-spline trajectory optimization objective function comprehensively considers multiple factors, including trajectory smoothness, flight time, and obstacle distance, to provide a safe, efficient, and stable return path for the drone. This optimization method ensures that the drone can adjust its path in real time in complex environments, while simultaneously minimizing flight time and maximizing flight safety, achieving comprehensive path optimization.

[0052] Preferably, when downgrading to the A* algorithm, a heuristic function is used in combination with obstacle information of the grid map, and Manhattan distance is used as the heuristic weight.

[0053] Through the above technical solution, when downgrading to Algorithm A, a heuristic function is used to combine obstacle information from the grid map and Manhattan distance as a heuristic weight, which helps improve the efficiency of path search. The heuristic function uses obstacle information to guide the search process, reducing ineffective path exploration and ensuring faster target discovery. Manhattan distance as a heuristic weight is simple to calculate and well-suited to grid maps. It can effectively guide Algorithm A in path planning, especially in scenarios with horizontal and vertical movement, avoiding redundant calculations.

[0054] Preferably, the topological connectivity graph, path search and optimization process are updated in real time during the flight of the UAV to cope with dynamic environmental changes.

[0055] Through this technical solution, the topological connectivity map, path search, and optimization processes can be updated in real time during the drone's flight, thereby responding to dynamic environmental changes. Specifically, the real-time updating of the topological connectivity map ensures that the drone can continuously adjust its flight path to respond to sudden obstacles or environmental changes. The path search and optimization modules also dynamically adjust based on the latest environmental data, ensuring that each planned path reflects the current flight status and environmental conditions.

[0056] Compared with the prior art, the present invention has the following advantages:

[0057] 1. This patent has efficient path planning and optimization capabilities. By combining the Dijkstra algorithm for global path search and using B-spline curves to optimize path smoothness, flight time and obstacle avoidance, this technical solution effectively improves the accuracy and speed of path planning. Especially in complex environments, it can quickly adapt to various changes and ensure the stability and safety of UAV flight.

[0058] 2. This patented system has strong adaptability to dynamic environments. By updating the topological connectivity map and optimizing the path in real time, the system can respond to changes in the environment, such as the appearance of obstacles or flight path deviations. In particular, when the drone encounters a path collision, the system can ensure the drone can return home smoothly in complex and constantly changing environments by regenerating the convex hull, updating the topological map, and using the A* algorithm to search for alternative paths, thereby improving system reliability.

[0059] 3. This patent takes into account both path safety and flight smoothness. By introducing an obstacle distance penalty term, it ensures that the flight path can avoid obstacles and reduce collision risks. At the same time, through minimum acceleration trajectory generation and B-spline curve optimization, the path smoothness is significantly improved. These optimization measures not only improve flight safety, but also reduce the instability factors caused by severe acceleration and deceleration during flight, ensuring that the drone can complete the return mission safely and smoothly in the shortest time. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 It is a schematic diagram of the process of the present invention;

[0061] Figure 2 This is a schematic diagram of the topological connectivity graph of the global traversable area of ​​the present invention;

[0062] Figure 3 It is a schematic diagram of the UAV mission execution scheme of the present invention;

[0063] Figure 4 This is a schematic diagram of the return path search for the UAV of the present invention;

[0064] Figure 5 This is a schematic diagram of the return path execution scheme of the UAV of the present invention;

[0065] Figure 6 This is a schematic diagram of new obstacles in the return path of the UAV of the present invention;

[0066] Figure 7 Schematic diagram of the return path update solution for the UAV of the present invention. DETAILED DESCRIPTION

[0067] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0068] Those skilled in the art will appreciate that embodiments of the present invention may be implemented as a system, apparatus, method, or computer program product. Therefore, the present invention may be implemented in the following forms: entirely in hardware, entirely in software (including firmware, resident software, microcode, etc.), or in a combination of hardware and software.

[0069] 1. Construct a topological connectivity graph of the global traversable area

[0070] Step 1: Generate a probability grid map

[0071] The drone collects real-time environmental data through onboard sensors and constructs a probabilistic grid map. The occupancy probability of each grid cell in the map indicates the likelihood of an obstacle, and unknown areas are marked as unexplored. This map serves as the basis for the subsequent construction of a topological connectivity graph.

[0072] Step 2: Dynamically generate the constrained convex hull

[0073] Initial convex hull generation: Using the current position of the drone as the seed point, combined with the constraints of obstacles and unknown areas in the grid map, an initial polygonal area is generated using a convex hull algorithm. This convex hull covers the traversable area around the current drone.

[0074] Dynamic Update Mechanism: When the drone's flight path exceeds the current convex hull, a new point on the trajectory is selected as a seed to generate a new convex hull. To ensure continuity between the old and new convex hulls, the new convex hull generation must include the drone's current position as a constraint, ensuring that there is overlap between the new and old convex hulls. The minimum overlap is determined through geometric calculations to ensure the connectivity of the topologically connected graph.

[0075] Topological Node and Edge Generation: The geometric center of the overlapping area of ​​the new and old convex hulls is used as a topological node. Nodes in different overlapping areas within the same convex hull are connected through edges, forming a dynamically expanding global topological connectivity graph. As the drone flies, the topological graph gradually covers a wider range of the feasible area.

[0076] 2. Global return path search

[0077] Step 1: Initialize search parameters

[0078] In the global topological connectivity graph, the topological node closest to the current position of the UAV is set as the starting node, its cumulative cost value is initialized to zero, and the remaining nodes are initialized to maximum values.

[0079] Create a priority queue and insert the start node into the queue.

[0080] Step 2: Execute Dijkstra's algorithm

[0081] Node extraction and determination: The node with the lowest cumulative cost is extracted from the priority queue as the current processing node. If the node is the return destination, a path node sequence is generated by backtracking to the parent node; otherwise, it is marked as a visited node.

[0082] Neighborhood expansion: Traverse all neighboring nodes of the current node and calculate the temporary cumulative cost from the current node to the neighboring node. The cost calculation is based on the Euclidean distance between the nodes. If the temporary cost is less than the current cumulative cost of the neighboring node, update its cumulative cost and parent node pointer, and insert it into the priority queue.

[0083] Termination condition: If the priority queue is exhausted and the end point is still not found, it is determined that the path does not exist; if the path is successfully generated, the node sequence path is output.

[0084] Step 3: Path output and verification

[0085] The generated path is in the form of a broken line, and it is necessary to verify whether it is completely within the flight corridor covered by the convex hull. If any node is located in an unknown area or near an obstacle, the path error handling module is triggered.

[0086] 3. Local optimization of the return path

[0087] Step 1: Intercept the local path segment

[0088] When the UAV flies along the global path, it intercepts a section of the path starting from the nearest node in real time as the local optimization input.

[0089] Step 2: Generate B-spline trajectory

[0090] The intercepted polyline path is used as the initial reference to generate a smooth trajectory that satisfies the UAV's dynamic constraints. Specifically, a B-spline curve parameterization method is used to ensure the continuity and differentiability of the trajectory.

[0091] During the optimization process, penalty terms for trajectory smoothness, total flight time, and obstacle avoidance distance are introduced. Smoothness is achieved by minimizing the high-order derivatives of the trajectory; obstacle avoidance distance is calculated based on the Euclidean distance field of the probability grid map to ensure that the trajectory stays away from obstacles.

[0092] Step 3: Dynamic Constraint Adjustment

[0093] Based on the flight characteristics of the drone, the maximum speed, acceleration, and turning radius of the trajectory are limited. The trajectory parameters are adjusted through iterative optimization to generate a final trajectory that meets the actual flight capabilities.

[0094] 4. Return path error handling

[0095] Step 1: Collision Detection and Emergency Response

[0096] When the optimized trajectory collides with a newly detected obstacle, the drone immediately enters emergency stop mode and records the location information of the collision point.

[0097] Step 2: Topology connectivity graph update and path replanning

[0098] Delete invalid topological edges: Remove the topological edges corresponding to the convex hull where the collision point is located, and disconnect it from the fault area.

[0099] Regenerate the convex hull: Use a safe position near the collision point as a seed point to generate a new convex hull and ensure that it overlaps with the existing convex hull.

[0100] Update the topology and re-search: Add the newly generated convex hull nodes to the topological connectivity graph and re-run the Dijkstra algorithm to search for a new path. If a new path is successfully generated, the drone continues its return flight.

[0101] Step 3: Degrade the search mechanism

[0102] If a feasible path still cannot be found after the topology map is updated, the system switches to the grid-based A algorithm. The A algorithm uses Manhattan distance as a heuristic function and combines real-time obstacle information to search for alternative paths, ensuring that the drone can return safely even in extreme situations.

[0103] Example 1

[0104] Step 1: Construct a global topological connectivity graph of the traversable area

[0105] Schematic diagram of the topological connectivity graph of the global traversable area, such as Figure 2 As shown in the figure, the circle is the starting point, the diamond is the node of the connected graph, and the dotted line is the edge of the connected graph;

[0106] Schematic diagram of UAV mission execution plan, such as Figure 3 As shown, the red track is the actual trajectory of the UAV performing the mission, the small orange dot is the position of the UAV when the new convex hull is generated, the gray polygon is the obstacle, the colored polygon is the generated convex hull, and the five-pointed star is the mission endpoint;

[0107] The location of obstacles in the environment is determined through a grid map. A convex hull is generated using the current position of the drone as a seed and the obstacles and unknown areas in the grid map as constraints. When the planned future trajectory of the drone is not within a certain range of the convex hull, a convex hull is generated again using a certain point on the planned future trajectory of the drone as a seed. To ensure that the newly generated convex hull overlaps with the old convex hull, the current position of the drone is also input as a constraint into the generation of the new convex hull. The geometric center points of the overlapping parts of the convex hulls are used as nodes in the topological graph. Different overlapping parts of the same convex hull are considered to be connected, thus forming a topologically connected graph that continues to expand with the range of the drone map.

[0108] Secondly, when the future trajectory planned by the drone exceeds the coverage of the current convex hull, the system needs to dynamically generate a new convex hull to ensure the continuity and safety of the path. Specifically, when the drone predicts through real-time sensor data that the closest distance from certain key points in the future trajectory (such as the position every 0.5 seconds) to the current convex hull boundary exceeds a preset safety threshold (for example, 1 meter), the trajectory is determined to be beyond the convex hull range. At this time, the system will select the point in the future trajectory that is farthest beyond the current convex hull as the seed point, giving priority to covering the unknown area at the end of the trajectory. If there are obstacles or high-density unknown areas around the seed point (detected by occupancy probability of the probabilistic grid map), the seed point is adjusted to the nearest safe point on the trajectory to ensure the feasibility of generating the new convex hull.

[0109] Step 2: Global return path search

[0110] Schematic diagram of the drone's return path, such as Figure 4 As shown in the figure, the red line is the final trajectory obtained by the search, the blue dotted line is the edge checked during the search process, the five-pointed star is the starting point when returning, the small orange dot is the end point when returning, the gray polygon is the obstacle, and the colored polygon is the generated convex hull.

[0111] When the drone wants to return, it will search the global topology map from its current position to the return point to find the most efficient return path. The graph search algorithm uses the Dijkstra algorithm.

[0112] 1. System initialization phase:

[0113] Find the node closest to the current position as the starting node;

[0114] Configure the initial cumulative cost value g of the starting node start =0, the cumulative cost values ​​of all other nodes are set to infinity;

[0115] Create a priority queue and insert the starting node into the queue.

[0116] 2. The path exploration cycle process includes the following sub-steps:

[0117] (1) When it is detected that the priority queue is not empty, extract the top element of the stack as the current processing node;

[0118] (2) Perform endpoint determination on the current processing node:

[0119] When it is identified as the target endpoint, a node sequence path is generated by recursively tracing back the parent node pointer;

[0120] When the end point is not reached, move the current node into the visited mark set;

[0121] (3) Neighborhood expansion operation, specifically implemented as follows:

[0122] Traverse all adjacent nodes of the currently processed node and continue processing only if the adjacent node is not marked as visited;

[0123] Calculate the temporary cumulative cost value for the adjacent nodes that are not marked as visited:

[0124] g temp =g current +w (current,neighbor)

[0125] The edge weight w is the Euclidean distance between two nodes;

[0126] Execution path optimization judgment: if and only if g temp <g neighbor hour:

[0127] Update the cumulative cost value of the adjacent node to g temp ; Reset the parent node pointer to point to the current processing node; trigger the queue operation when the adjacent node does not exist in the priority queue;

[0128] 3. Termination condition judgment:

[0129] When the priority queue is exhausted, it returns that the path does not exist; when the path is successfully generated, the total cost value and node sequence of the path are output synchronously;

[0130] Step 3: Global return path search

[0131] Schematic diagram of the UAV return path execution plan, as shown in Figure 5 As shown in the figure, the green trajectory is the optimized B-spline trajectory, the five-pointed star is the starting point of the return, the small orange dot is the end point of the return, the gray polygon is the obstacle, and the colored polygon is the generated convex hull.

[0132] The path obtained by graph search is a broken line path, containing only the topological geometry information required for return flight. It cannot be directly used for the drone's return flight and requires local optimization. The drone returns along the path obtained by the graph search module. Each time it optimizes, it first obtains the position of the node closest to it on the return path, and then proceeds along the path from this node, intercepting 10 meters for optimization. Using the intercepted 10-meter trajectory as the initial value, it generates a minimunsnap trajectory as the initial value for optimization and parameterizes it into a B-spline trajectory. Next, the B-spline trajectory is optimized for smoothness and total flight time, penalizing its distance to obstacles while taking into account the dynamic limitations of the drone. This enables the drone to execute a safe, smooth, and dynamic flight trajectory, fully utilizing its flight performance and improving flight speed and safety.

[0133] Step 4: Handling Return Path Errors

[0134] Schematic diagram of new obstacles on the drone's return path, such as Figure 6 As shown in the figure, the red track is the searched return path, the five-pointed star is the starting point of the return, the small orange dot is the end point of the return, the gray polygon is the obstacle, and the colored polygon is the generated convex hull.

[0135] Schematic diagram of the drone return path update scheme, as shown in Figure 7 As shown in the figure, the red track is the searched return path, the five-pointed star is the starting point of the return, the small orange dot is the end point of the return, the gray polygon is the obstacle, and the colored polygon is the generated convex hull.

[0136] When the drone detects a collision with a new obstacle on its return path, the system first triggers a dynamic response mechanism: the drone enters a decelerated hovering mode and, based on the current grid map, generates a local obstacle avoidance trajectory around the hovering position to maintain basic safety. If the obstacle distance falls below a preset danger threshold, the drone switches to a full emergency stop and activates an alarm. The system then deletes topological edges directly connected to the collision area in the convex hull of the collision point, while preserving the connectivity of other unaffected areas. The system then screens multiple candidate seed points within a safe zone with a radius of 2 meters around the collision point, prioritizing those closest to the collision point and with the lowest obstacle density as new seeds. When generating a new convex hull, it is mandatory to include the drone's current location and at least one known safe node area from the old convex hull to ensure natural overlap between the new and old convex hulls. If the overlap does not reach 20% of the old convex hull, the new convex hull boundary is extended along the drone's historical flight direction until the overlap requirement is met. When updating the topological connectivity graph, only the geometric center node and its connecting edges in the overlapping region of the new convex hull are added to avoid global reconstruction. Starting from the nearest unaffected node, the bidirectional Dijkstra algorithm is used to accelerate the path search. If the timeout fails, the A* algorithm combined with the Manhattan distance heuristic function is switched to quickly generate an alternative path. If the new path is valid, the drone continues to return and monitors environmental changes in real time. If two consecutive replanning failures occur, the autonomous landing protocol is triggered, landing in the nearest safe area and reporting the anomaly.

Claims

1. A method for automatic return of drones based on global topological maps and graph search algorithms, characterized in that: Includes the following modules: A global traversable area topological connectivity graph module is used to generate a dynamically updated topological connectivity graph based on the probabilistic grid map. The topological connectivity graph is constructed by generating a convex hull and connecting the nodes of the overlapping area. The current position of the drone and the planned trajectory points are used as seeds to generate the constrained convex hull, and the geometric centers of the overlapping parts of the convex hulls are used as topological nodes. a return path search module, configured to search for a globally optimal path from the current position of the UAV to the return point in the topologically connected graph using a graph search algorithm, wherein the graph search algorithm is the Dijkstra algorithm, including initializing a starting node, priority queue processing, and neighborhood expansion operations; The return path optimization module is used to optimize the global optimal path into a smooth trajectory that meets the UAV's dynamic constraints, generate a B-spline trajectory by intercepting the local path, and optimize its smoothness, flight time and obstacle avoidance distance; The return path error handling module is used to regenerate the convex hull, update the topological connectivity graph, and downgrade to the A* algorithm to search for an alternative path when a path collision is detected.

2. The method for automatic return of a drone based on a global topological map and a graph search algorithm according to claim 1, characterized in that: The method for constructing the global traversable area topology connectivity graph module comprises the following steps: Step 1: Determine the location constraints of obstacles and unknown areas based on the probability grid map; Step 2: Generate the initial convex hull using the current position of the drone or the planned trajectory point as the seed; Step 3: When the drone trajectory exceeds the current convex hull range, a new convex hull is generated using the new point on the trajectory as a seed, and the new and old convex hulls are ensured to have overlapping areas. Step 4: Take the geometric center of the overlapping area as the topological node, and connect different overlapping nodes of the same convex hull through edges to form a dynamically expanded topological connectivity graph.

3. The method for automatic return of a drone based on a global topological map and a graph search algorithm according to claim 1, characterized in that: The Dijkstra algorithm execution steps of the return path search module include: Step 1: Initialization phase: The node closest to the current position of the UAV is set as the starting node, and its cumulative cost value g(start) = 0. The cost values ​​of the remaining nodes are infinite. Step 2: Loop and extract the top node of the priority queue. If it is the target destination, backtrack to generate a path; otherwise, expand its adjacent nodes. Step 3: Calculate temporary accumulative cost values ​​as the neighborhood expands: g temp =g current +w (current,neighbor) Among them, the edge weight w is the Euclidean distance between nodes; Step 4: Update the cumulative cost values ​​and parent node pointers of adjacent nodes until a path is generated or the queue is exhausted.

4. The method according to claim 1, wherein The specific optimization method of the return path optimization module is: a) Intercept the 10-meter path segment starting from the node closest to the drone; b) generating a minimum acceleration (minimum snap) trajectory using the path segment as an initial value; c) Parameterize the trajectory into a B-spline curve and optimize its smoothness, total flight time, and obstacle distance penalty; d) Introduce the dynamic constraints of the UAV's maximum speed, acceleration and turning radius for secondary optimization.

5. The method for automatic return of a drone based on a global topological map and a graph search algorithm according to claim 1, characterized in that: The specific steps of the return path error processing module include: a) When a path collision is detected, the system enters emergency stop mode and deletes the topological edge of the convex hull where the collision point is located; b) Regenerate the convex hull using the location near the collision point as a seed, ensuring that the new convex hull overlaps with the existing convex hull; c) After updating the topological connectivity graph, re-execute the graph search. If it fails, switch to the A* algorithm to search for alternative paths in the grid map.

6. The method according to claim 2, characterized in that The generation constraints of the convex hull include: using the current position of the drone as an input constraint to ensure that the overlapping area of ​​the new convex hull and the old convex hull covers the current position of the drone.

7. The method according to claim 3, characterized in that The maintenance of the priority queue includes: when the temporary accumulated cost value of an adjacent node is less than its current value, updating the node and triggering an enqueue operation.

8. The method according to claim 4, characterized in that The optimization objective function of the B-spline trajectory includes the following terms: a) The smoothness of the trajectory, quantified by the jerk integral; b) Total flight time penalty; c) The obstacle distance penalty is calculated using the Euclidean distance field in the obstacle grid map.

9. The method according to claim 5, characterized in that When the algorithm is downgraded to the A* algorithm, a heuristic function is used in combination with the obstacle information of the grid map, and the Manhattan distance is used as the heuristic weight.

10. The method according to any one of claims 1 to 9, characterized in that: The topological connectivity graph, path search and optimization process are updated in real time during the flight of the UAV to cope with dynamic environmental changes.

Images