An optimal path planning method for UAV based on fully expanded graph

Abstract

A method for optimal path planning for a drone based on a fully expanded graph. The present invention relates to a method for optimal path planning for a drone based on a fully expanded graph, and the present invention belongs to the field of unmanned robot path planning. The purpose of the present invention is to solve the problem that existing Voronoi diagrams are difficult to use for path planning in three-dimensional scenes, and that Voronoi diagrams themselves are not designed for drone path planning, and the resulting route map is usually very tortuous and cannot be directly executed by drones. The specific process of the method for optimal path planning for a drone based on a fully expanded graph is as follows: Step 1: constructing an initial grid map; Step 2: constructing a cost map based on the initial grid map; Step 3: obtaining a map skeleton based on the cost map; Step 4: searching for an optimal path in the cost map.

Description

Technical Field

[0001] The invention relates to an optimal path planning method for an unmanned aerial vehicle (UAV) based on a complete expansion graph, and belongs to the field of unmanned robot path planning. Background Art

[0002] The path planning problem is a key issue in the field of robotics and even automation. The definition of the path planning problem is very simple: "finding a collision-free motion between an initial (starting point) and a final configuration (goal) in a specific environment." The simplest case is planning a path in a static and known environment; however, more generally, the path planning problem can be formulated as any robotic system subject to kinematic constraints in a dynamic and unknown environment. It has a wide range of applications in real life, including unmanned driving, logistics distribution, robot navigation and other fields. With the development of artificial intelligence and computer technology, path planning technology is also constantly being improved and applied.

[0003] Path planning algorithms can be divided into the following categories according to the method of generating geometric paths: roadmap techniques (Roadmap Techniques), cell decomposition algorithms (Cell Decomposition Algorithms), and artificial potential fields (Artificial Potential Methods).

[0004] The roadmap approach consists of two phases: construction and query. In the construction phase, the connectivity of the free-form configuration space is computed by defining a network of curves in the 3D environment. After the roadmap is constructed, the query phase solves for the initial and final configuration points. By connecting the curves, a path planning solution can be found between these points. Roadmap technology is used to handle path planning queries for UAVs moving in a static environment. The query computation time is completed in real time during the processing time, and the final path is smoothed using roadmap algorithms such as RRT, A*, and RRT*.

[0005] Region segmentation first divides free space into a set of distinct cells, or regions, to facilitate the computation of safe paths between two points in the same or adjacent cells. Once the cell decomposition of free space is computed, the cells can be organized sequentially. For UAV path planning in configuration space, different methods and algorithms can be employed depending on the cell type.

[0006] Artificial potential fields are a simple and lightweight approach to dynamic path planning. They represent the environment of a design object as a particle, which moves under the control of a potential field surrounding the configuration space. The UAV path is calculated from the resulting field from an initial point to a target point. However, conventional PFMs are limited to local minima, causing the UAV to become stuck before reaching the target point.

[0007] The above path planning methods all treat the drone as a point mass without volume, so the planned path may cause collisions during actual execution. The current solution is usually to inflate the volume of the obstacle based on the volume of the drone, treat the inflated obstacle as an obstacle in the map, and then treat the drone as a point mass for path planning. In addition, using the Voronoi diagram, it is possible to achieve the safest path planning in principle. On a specific plane, it divides different areas according to the distance between waypoints. The VD method defines that all points around obstacles are at equal distances from the obstacles. The path diagram generated by VD is very safe because the obstacles are far away from all edges of the path.

[0008] There are many methods for calculating Voronoi diagrams, including divide-and-conquer, sweep-line, and incremental algorithms. These algorithms are primarily designed for point, line, and polygonal sites, and are difficult to calculate for complex three-dimensional sites. Furthermore, Voronoi diagrams themselves are not designed for drone path planning; the resulting route maps are often very tortuous and cannot be directly executed by drones. Summary of the Invention

[0009] The purpose of the present invention is to solve the problems that the existing Voronoi diagram is difficult to use for path planning in three-dimensional scenes, and the Voronoi diagram itself is not designed for drone path planning. The resulting route map is usually very tortuous and cannot be directly executed by the drone. Instead, an optimal path planning method for drones based on a fully expanded graph is proposed.

[0010] The specific process of an optimal path planning method for UAV based on a fully expanded graph is as follows:

[0011] Step 1: Build the initial raster map;

[0012] Step 2: Build a cost map based on the initial grid map;

[0013] Step 3: Get the map skeleton based on the cost map;

[0014] Step 4: Search for an optimal path in the cost map.

[0015] The beneficial effects of the present invention are:

[0016] The present invention combines the technical ideas of obstacle expansion correction and Voronoi diagram, and proposes a path planning method based on complete expansion graph, which can be implemented in square time complexity O(n 2 ) to obtain a two-dimensional or three-dimensional route map with optimal security similar to the Voronoi diagram, and combine it with random sampling or graph search algorithm to obtain the path that the drone can execute.

[0017] The present invention adopts an expansion algorithm to pre-process a map containing obstacles, and obtains a route map with the farthest distance from obstacles, that is, a route map with optimal safety, at a lower time complexity.

[0018] The fully expanded map method proposed in the present invention can be used in two-dimensional and three-dimensional scenes, solving the defect that the Voronoi map is difficult to use for path planning in three-dimensional scenes.

[0019] The present invention uses the safest route map to guide the search process of the graph search to obtain a smooth path, which solves the problem that the Voronoi diagram itself is not designed for drone path planning and the resulting route map is usually very tortuous.

[0020] For static scenes, the cost map of the present invention can be obtained offline and directly used during path planning without increasing the path planning time; at the same time, the cost map reduces the search dimension of the graph search process and can increase the speed of the graph search process. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 Flowchart of the present invention;

[0022] Figure 2 Flowchart for constructing the initial raster map;

[0023] Figure 3 This is a flow chart of the full expansion method;

[0024] Figure 4 Flowchart for graph search based on safety cost map. DETAILED DESCRIPTION

[0025] Specific implementation method 1: This implementation method is a method for optimal path planning of a UAV based on a fully expanded graph. The specific process is as follows:

[0026] Step 1: Build the initial grid map;

[0027] Step 2: Build a cost map based on the initial grid map;

[0028] Step 3: Get the map skeleton based on the cost map;

[0029] Step 4: Search for a path with optimal security in the cost map.

[0030] Specific embodiment 2: This embodiment differs from specific embodiment 1 in that the initial grid map is constructed in step 1; the specific process is as follows:

[0031] Step 1: Given the scene's boundaries, grid size, the world coordinates of the grid map's origin, and a list of obstacle vertices (describing their shape and position, any number of vertices is acceptable);

[0032] The grid map is represented by a two-dimensional or three-dimensional array, and the value of each position in the array indicates whether the drone is allowed to pass through the point;

[0033] Step 1 and 2: Build a grid map based on the scene's boundaries, grid size, the world coordinates of the grid map's origin, and the list of obstacles' vertices.

[0034] The flowchart for this step is as follows Figure 1 shown.

[0035] Other steps and parameters are the same as those in the first embodiment.

[0036] Specific embodiment three: This embodiment differs from specific embodiments one or two in that in steps one and two, a grid map is constructed based on the scene boundary, grid size, coordinates of the grid map origin in the world coordinate system, and a vertex list of obstacles; the specific process is as follows:

[0037] Determine the size of the grid map (pixel size) based on the scene boundary and grid size, and determine the memory space size based on the size of the grid map;

[0038] Set the locations where obstacles are located in the grid as occupied and the rest of the locations as unoccupied.

[0039] Other steps and parameters are the same as those in the first or second embodiment.

[0040] Specific embodiment 4: This embodiment differs from any one of specific embodiments 1 to 3 in that the cost map is constructed based on the initial grid map in step 2; the specific process is as follows:

[0041] Step 21: Set the number of iterations k = 1; initialize the cost map and the expansion increment map based on the grid map;

[0042] Step 22: Set the number of iterations k = k + 1; set all elements in the dilated incremental map to 0, start from the origin of the cost map, traverse all grids in the cost map, examine the neighborhood of each grid, and if there is an occupied grid in the neighborhood, set the corresponding grid in the dilated incremental map to 1;

[0043] For the two-dimensional case, the neighborhood of each grid is 8;

[0044] For the three-dimensional case, the neighborhood of each grid is 26;

[0045] Step 23: Update the cost map;

[0046] Step 24: Repeat steps 22 to 24 until there is no element with a value of 1 in the expansion increment map.

[0047] The other steps and parameters are the same as those in the first to third embodiments.

[0048] Specific embodiment 5: This embodiment differs from any one of specific embodiments 1 to 4 in that in step 21, the number of iterations k is set to 1; the cost map and the dilated incremental map are initialized based on the grid map;

[0049] The specific process is:

[0050] Initialize the cost map size, origin, grid size and initial grid location Figure 1 To (initialize the cost map to the initial grid map, the grid information is different);

[0051] Initialize the size, origin, grid size and initial grid location of the expansion incremental map Figure 1 Therefore, all elements in the initial expansion increment map are set to 0 (the initial expansion increment map is the initial grid map, and the grid information is different).

[0052] The other steps and parameters are the same as those in the first to fourth embodiments.

[0053] Specific embodiment 6: This embodiment differs from any one of specific embodiments 1 to 5 in that the cost map is updated in steps 2 and 3; the process is:

[0054] According to the grid position with a value of 1 in the expansion increment map, the corresponding position in the initial grid map is set to occupied, and the corresponding position in the cost map is set to the current iteration number.

[0055] The other steps and parameters are the same as those in the first to fifth embodiments.

[0056] Specific embodiment seven: This embodiment differs from any one of specific embodiments one to six in that the map skeleton is obtained according to the cost map in step three; the specific process is as follows:

[0057] Step 31: Design a set of operators to extract the map skeleton;

[0058] Step 32: Perform a convolution operation on the cost map based on the operator designed in step 31, and record the positions where the convolution result is negative as the map skeleton.

[0059] The closest distance between all elements in the map skeleton and the obstacle is farther than other positions on the map. The map skeleton is approximately equivalent to the calculation result of the Voronoi diagram and can be directly used in the path planning method using the Voronoi diagram.

[0060] The flowchart of step 2 is as follows Figure 2 shown.

[0061] The other steps and parameters are the same as those in the first to sixth embodiments.

[0062] Specific embodiment eight: This embodiment differs from specific embodiments one to seven in that a set of operators is designed in step three-one to extract the map skeleton;

[0063] 1) Design a set of operators for 2D maps to extract map skeletons;

[0064] The specific form of the operator is expressed as:

[0065]

[0066]

[0067]

[0068]

[0069] Among them, L x,x , L y,y , L x,y , L x,-y Operators designed for two-dimensional maps;

[0070] For two-dimensional maps, this operator extracts the second-order derivatives of the cost map along the x-axis, y-axis, and diagonal directions. When used, a set of orthogonal directions can be selected according to the direction of travel of the drone.

[0071] 2) Design a set of operators for 3D maps to extract map skeletons;

[0072] The specific form of the operator is expressed as:

[0073]

[0074]

[0075]

[0076]

[0077]

[0078]

[0079]

[0080]

[0081]

[0082]

[0083]

[0084]

[0085]

[0086] Among them, L1, L2, L3, L4, L5, L6, L7, L8, L9, L10, L11, L12, and L13 are operators designed for three-dimensional maps.

[0087] For three-dimensional maps, this operator will extract the second-order derivatives of each direction in the 26-neighborhood. When used, a group of directions that are orthogonal can be selected according to the direction of the drone's movement. The specific form is a three-dimensional extension of the two-dimensional form.

[0088] The other steps and parameters are the same as those in the first to seventh embodiments.

[0089] Specific embodiment 9: This embodiment differs from any one of specific embodiments 1 to 8 in that in step 4, a path with optimal security is searched in the cost map; the specific process is as follows:

[0090] Step 41: Given the initial position, target position and cost map of the UAV;

[0091] The node data structure is used to represent the state of the grid of the cost map. The information contained in the node (the node corresponds to the grid of the cost map) includes: the position of the current node in the cost map, the position of the previous node in the cost map (the position of the previous node in the cost map), the safety cost G value of the current node position in the cost map, the estimated cost H value from the current node position to the target position in the cost map, and the estimated total cost F value in the cost map;

[0092] The safety cost G value is the inverse of the number of iterations of the corresponding position in the cost map;

[0093] The estimated cost H is the estimated cost from the current position to the target position, which is generally calculated using the Euclidean distance;

[0094] For the two-dimensional case:

[0095] For the three-dimensional case:

[0096] Among them, CostMap(x,y) represents the cost map for the two-dimensional case; G(x,y) represents the safety cost of the current node position in the cost map for the two-dimensional case; (x,y) represents the current node position in the cost map for the two-dimensional case; CostMap(x,y,z) represents the cost map for the three-dimensional case; G(x,y,z) represents the safety cost of the current node position in the cost map for the three-dimensional case; (x,y,z) represents the current node position in the cost map for the three-dimensional case; k is an adjustable parameter used to control the search path strategy; the larger the k value, the farther the search path is from obstacles;

[0097] For the two-dimensional case:

[0098] For the three-dimensional case:

[0099] Among them, H(x,y) represents the estimated cost from the current node position to the target position in the cost map for the two-dimensional case; (x,y) represents the current node position in the cost map for the two-dimensional case; (x goal ,y goal ) represents the target position in the cost map for the two-dimensional case; H(x,y,z) represents the estimated cost from the current node position to the target position in the cost map for the three-dimensional case; (x,y,z) represents the current node position in the cost map for the three-dimensional case; (x goal ,y goal ,z goal ) represents the target position in the cost map for the three-dimensional case;

[0100] F=G+H

[0101] Use open sets and closed sets to store nodes to be visited and nodes that have been visited respectively. Each time a node is added to the open set, the nodes contained in the open set are sorted in ascending order according to the F value.

[0102] Step 42: Construct the initial node (drone initial position) based on the input drone initial position, target position and cost map, add the initial node to the open set, and sort the nodes contained in the open set in ascending order of F value;

[0103] Step 43: Determine whether the open set is empty;

[0104] If the open set is empty, the search fails, indicating that the drone’s target location cannot be reached;

[0105] If the open set is not empty, take the node with the smallest total cost in the open set (i.e., the first node in the open set) as the current node; if the position of the current node is the same as the target position, end the graph search, backtrack and return the path; if the position of the current node is different from the target position, delete the current node from the open set and add it to the closed set; expand the adjacent nodes of the current node, and then add the neighbor nodes to the open set, and sort the nodes contained in the open set in ascending order according to the F value; repeat step 43 (repeat step 43 with 2 stop conditions: 1. If the open set is empty, the search fails, indicating that the drone's target position cannot be reached; 2. If the open set is not empty, take the node with the smallest total cost in the open set as the current node; if the position of the current node is the same as the target position, end the graph search, backtrack and return the path).

[0106] The other steps and parameters are the same as those in Specific Embodiments 1 to 8.

[0107] Specific embodiment 10: This embodiment differs from any one of specific embodiments 1 to 9 in that in step 43, the neighboring nodes of the current node are expanded, and then the neighboring nodes are added to the open set; the process is:

[0108] 1) Assume the current node's position is (x, y), the security cost is G, and traverse the neighborhood of the current node. Each neighbor will correspond to a cost G* value; record the neighbor node's position (x', y') and cost G', H', F', and add it to the neighbor node list;

[0109] For the two-dimensional case, the neighborhood is 8;

[0110] For the three-dimensional case, the neighborhood is 26;

[0111] For the two-dimensional case:

[0112] For the three-dimensional case:

[0113] Where (x, y, z) represents the position of the current node in the cost map for the three-dimensional case; (x′, y′, z′) represents the position of the neighbor node in the cost map for the three-dimensional case; G* represents the new cost generated by adding the neighbor node to the path; G′ represents the safety cost of the neighbor node position in the cost map, H′ represents the estimated cost from the neighbor node position in the cost map to the target position, and F′ represents the estimated total cost of the neighbor nodes in the cost map;

[0114] 2) For all nodes in the neighbor node list that are not occupied and not in the closed set;

[0115] If the i-th neighbor node is not in the open set, modify the G′ value of the i-th neighbor node, G′=G *The H′ value of the i-th neighbor node is obtained by calling the heuristic function; the F′ value of the i-th neighbor node is F′=G′+H′; the neighbor nodes are added to the open set, and the nodes included in the open set are sorted in ascending order according to the estimated total cost F value;

[0116] If the i-th neighbor node is in the open set and G*<G′, first remove the i-th node from the open set and update the G′ value of the i-th node, G′=G * Update the F′ value of the i-th neighbor node, F′=G′+H′; add the updated i-th node to the open set, and sort the nodes included in the open set in ascending order according to the estimated total cost F value; H is not updated, because H has been calculated in the branch that is not in the open set and will not change during the search process;

[0117] 3) No operation is performed in other cases (for all nodes in the neighbor node list that are in the closed set and whose neighbor nodes are in the open set but do not satisfy G*<G′).

[0118] The flowchart of the process is as follows Figure 3 shown.

[0119] The other steps and parameters are the same as those in Specific Embodiments 1 to 9.

[0120] The present invention may have many other embodiments. Without departing from the spirit and essence of the present invention, those skilled in the art may make various corresponding changes and modifications based on the present invention, but these corresponding changes and modifications should all fall within the scope of protection of the claims attached to the present invention.

Claims

1. A method for optimal path planning for unmanned aerial vehicles based on a fully expanded graph, characterized by: The specific process of the method is: Step 1: Build the initial grid map; Step 2: Build a cost map based on the initial grid map; Step 3: Get the map skeleton based on the cost map; Step 4: Search for an optimal path in the cost map; the specific process is: Step 41: Given the initial position, target position and cost map of the UAV; The node data structure is used to represent the state of the grid of the cost map. The information contained in the node includes: the position of the current node in the cost map, the position of the previous node in the cost map, the safety cost G value of the current node position in the cost map, the estimated cost H value from the current node position to the target position in the cost map, and the estimated total cost F value in the cost map; For the two-dimensional case: For the three-dimensional case: Among them, CostMap(x,y) represents the cost map for the two-dimensional case; G(x,y) represents the safety cost of the current node position in the cost map for the two-dimensional case; (x,y) represents the current node position in the cost map for the two-dimensional case; CostMap(x,y,z) represents the cost map for the three-dimensional case; G(x,y,z) represents the safety cost of the current node position in the cost map for the three-dimensional case; (x,y,z) represents the current node position in the cost map for the three-dimensional case; k is an adjustable parameter; For the two-dimensional case: For three-dimensional cases: Among them, H(x,y) represents the estimated cost from the current node position to the target position in the cost map for the two-dimensional case; (x,y) represents the current node position in the cost map for the two-dimensional case; (x goal ,y goal ) represents the target position in the cost map for the two-dimensional case; H(x,y,z) represents the estimated cost from the current node position to the target position in the cost map for the three-dimensional case; (x,y,z) represents the current node position in the cost map for the three-dimensional case; (x goal ,y goal ,z goal ) represents the target position in the cost map for the three-dimensional case; F=G+H Step 42: Construct the initial node based on the input drone initial position, target position and cost map, add the initial node to the open set, and sort the nodes contained in the open set in ascending order of F value; Step 43: Determine whether the open set is empty; If the open set is empty, the search fails, indicating that the drone’s target location cannot be reached; If the open set is not empty, take the node with the smallest total cost in the open set as the current node; if the position of the current node is the same as the target position, end the graph search, backtrack and return the path; if the position of the current node is different from the target position, delete the current node from the open set and add it to the closed set; expand the adjacent nodes of the current node, then add the neighboring nodes to the open set, and sort the nodes contained in the open set in ascending order according to the F value; repeat steps 4 and 3.

2. The optimal path planning method for a UAV based on a fully expanded graph according to claim 1, characterized in that: In step 1, an initial grid map is constructed; the specific process is as follows: Step 1: Given the scene's boundaries, grid size, the world coordinates of the grid map's origin, and a list of obstacles' vertices; Step 1 and 2: Build a grid map based on the scene's boundaries, grid size, the world coordinates of the grid map's origin, and the list of obstacles' vertices.

3. The optimal path planning method for a UAV based on a fully expanded graph according to claim 2, characterized in that: In steps 1 and 2, a grid map is constructed based on the scene boundary, grid size, coordinates of the grid map origin in the world coordinate system, and the vertex list of obstacles. The specific process is as follows: Determine the size of the grid map based on the scene boundary and grid size, and determine the memory space size based on the size of the grid map; Set the locations where obstacles are located in the grid as occupied and the rest of the locations as unoccupied.

4. The optimal path planning method for a UAV based on a fully expanded graph according to claim 3, characterized in that: In step 2, a cost map is constructed based on the initial grid map; the specific process is: Step 21: Set the number of iterations k = 1; initialize the cost map and the expansion increment map based on the grid map; Step 22: Set the number of iterations k = k + 1; set all elements in the dilated incremental map to 0, start from the origin of the cost map, traverse all grids in the cost map, examine the neighborhood of each grid, and if there is an occupied grid in the neighborhood, set the corresponding grid in the dilated incremental map to 1; For the two-dimensional case, the neighborhood of each grid is 8; For the three-dimensional case, the neighborhood of each grid is 26; Step 23: Update the cost map; Step 24: Repeat steps 22 to 24 until there is no element with a value of 1 in the expansion increment map.

5. The optimal path planning method for a UAV based on a fully expanded graph according to claim 4, characterized in that: In the step 21, the number of iterations k=1; initializing the cost map and the expansion increment map based on the grid map; The specific process is: The size, origin, and grid size of the initial cost map are consistent with the initial grid map; The size, origin, and grid size of the initial expansion incremental map are consistent with those of the initial grid map, and all elements in the initial expansion incremental map are set to 0.

6. The optimal path planning method for a UAV based on a fully expanded graph according to claim 5, characterized in that: In steps 2 and 3, the cost map is updated; the process is: According to the grid position with a value of 1 in the expansion increment map, the corresponding position in the initial grid map is set to occupied, and the corresponding position in the cost map is set to the current iteration number.

7. The optimal path planning method for a UAV based on a fully expanded graph according to claim 6, characterized in that: In step 3, a map skeleton is obtained according to the cost map; The specific process is: Step 31: Design a set of operators; Step 32: Perform a convolution operation on the cost map based on the operator designed in step 31, and record the positions where the convolution result is negative as the map skeleton.

8. The optimal path planning method for a UAV based on a fully expanded graph according to claim 7, characterized in that: In the step 31, a set of operators is designed; 1) Design a set of operators for two-dimensional maps; The specific form of the operator is expressed as: Among them, L x,x , L y,y , L x,y , L x,-y Operators designed for two-dimensional maps; 2) Design a set of operators for 3D maps; The specific form of the operator is expressed as: Among them, L1, L2, L3, L4, L5, L6, L7, L8, L9, L10, L11, L12, and L13 are operators designed for three-dimensional maps.

9. The optimal path planning method for a UAV based on a fully expanded graph according to claim 8, characterized in that: In step 43, the neighboring nodes of the current node are expanded, and then the neighboring nodes are added to the open set; the process is: 1) Assume the current node's position is (x, y), the security cost is G, and traverse the neighborhood of the current node. Each neighbor will correspond to a cost G* value; record the neighbor node's position (x', y') and cost G', H', F', and add it to the neighbor node list; For the two-dimensional case, the neighborhood is 8; For the three-dimensional case, the neighborhood is 26; For the two-dimensional case: For the three-dimensional case: Where (x, y, z) represents the position of the current node in the cost map for the three-dimensional case; (x′, y′, z′) represents the position of the neighbor node in the cost map for the three-dimensional case; G* represents the new cost generated by adding the neighbor node to the path; G′ represents the safety cost of the neighbor node position in the cost map, H′ represents the estimated cost from the neighbor node position in the cost map to the target position, and F′ represents the estimated total cost of the neighbor nodes in the cost map; 2) For all nodes in the neighbor node list that are not occupied and not in the closed set; If the i-th neighbor node is not in the open set, modify the G′ value of the i-th neighbor node, G′=G * The H′ value of the i-th neighbor node is obtained by calling the heuristic function; the F′ value of the i-th neighbor node is F′=G′+H′; the neighbor nodes are added to the open set, and the nodes included in the open set are sorted in ascending order according to the estimated total cost F value; If the i-th neighbor node is in the open set and G* < G′, first remove the i-th node from the open set, update the G′ value of the i-th node, G′ = G * ; update the F′ value of the i-th neighbor node, F′ = G′ + H′; add the updated i-th node to the open set, and sort the nodes contained in the open set in ascending order according to the estimated total cost F value; 3) No operation is performed on nodes in other situations.

Images