Path planning method and device, equipment and storage medium
By using the map of the first resolution to determine the obstacle between the starting point and the end point when planning paths in a complex environment, and continuing to explore the paths on a higher resolution map, the problem of path planning failure at low resolutions in the prior art is solved, and more efficient path planning is achieved.
Patent Information
- Application Number
- CN202311492232.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2023-11-09
- Publication Date
- 2025-05-09
AI Technical Summary
When the path planning in a complex environment, when the path exploration at low resolution fails, the resolution is increased and the calculation content of the previous steps is wasted, and the path length and exploration range are not effectively shortened, and the calculation efficiency is reduced.
By determining an obstacle between the start point and the end point in a map of the first resolution, a first path is determined and an intermediate point is determined based on the path. Then, on a higher resolution map, the second path continues to be explored based on the intermediate points, and finally merges the first and second paths to determine the target path.
It effectively shortens the path length and exploration range for exploration after obstacles appear, improves computing efficiency, and avoids waste of resources after path planning failure at low resolution.
Smart Images
Figure CN119958581A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of navigation technology, and in particular to a path planning method, device, equipment and storage medium. Background Art
[0002] In the related technologies, intelligent robot research usually involves the field of navigation. In order to solve the navigation problem of pan-robots, path planning algorithms are usually involved. For example, given the starting point and end point of the robot, in the face of a complex environment, the robot can complete the task independently without external assistance. The path planning algorithm is the key link in achieving this task. It is necessary to enable it to perfectly avoid obstacles in a complex environment, plan a path with a low flight cost, and complete the robot's preset task. In the related technology, if the path exploration fails at a low resolution, the resolution is increased for recalculation, wasting the calculation content of the previous step. There is currently no effective solution to this problem. Summary of the invention
[0003] In view of this, the embodiments of the present application provide a path planning method, apparatus, device and storage medium, which aim to effectively shorten the path length and exploration range to be explored after an obstacle appears, and further improve computing efficiency.
[0004] The technical solution of the embodiment of the present application is implemented as follows:
[0005] The present application provides a path planning method, including:
[0006] If it is determined that there is a first obstacle between the first starting point and the first end point in the first map with the first resolution, determining a first path based at least on the first obstacle, and the first starting point and / or the first end point;
[0007] Determining a first intermediate point based on the first path and the first obstacle;
[0008] Determine a second path based on the first intermediate point and a second map with a second resolution; the first map and the second map are maps representing the same path area to be planned; and the second resolution is higher than the first resolution;
[0009] A target path is determined according to the first path and the second path.
[0010] In the above solution, the first intermediate point includes a second starting point or a second end point; and determining the first intermediate point based on the first path and the first obstacle includes:
[0011] Based on the first path and the first obstacle, determining a second starting point or a second end point; the first path represents a path from the first starting point or the first end point to the first obstacle;
[0012] Correspondingly, determining the second path based on the first intermediate point and the second map with the second resolution includes:
[0013] The second path is determined based on the second map and the second starting point or the second end point; the second path represents a path from the second starting point to the first end point or a path from the second end point to the first starting point.
[0014] In the above solution, the first intermediate point includes a second starting point and a second end point; the first path includes a first sub-path and a second sub-path; the first sub-path represents a path from the first starting point to the first obstacle; the second sub-path represents a path from the first end point to the first obstacle; the method further includes:
[0015] Determine the second starting point based on the first sub-path and the first obstacle;
[0016] Determining the second end point based on the second sub-path and the first obstacle;
[0017] Based on the second map, the second starting point and the second end point, the second path is determined; the second path represents a path from the second starting point to the second end point.
[0018] In the above solution, the method further includes:
[0019] Determine at least one path point according to the target path;
[0020] If it is determined that the first path point and the third path point are passable, deleting the second path point in the target path;
[0021] Among them, the first path point is any path point among the at least one path point; the second path point is the next path point of the first path point among the at least one path point; and the third path point is the next path point of the second path point among the at least one path point.
[0022] In the above solution, the method further includes:
[0023] If it is determined that there is no first obstacle between the first starting point and the first end point of the path in the preset map of the first resolution, the target path is determined based on the first starting point and the first end point.
[0024] The present application provides a path planning device, including:
[0025] A first determination module, configured to determine a first path based at least on the first obstacle, and the first starting point and / or the first end point if it is determined that a first obstacle exists between the first starting point and the first end point in the first map with a first resolution;
[0026] A second determining module, configured to determine a first intermediate point based on the first path and the first obstacle;
[0027] A third determination module is used to determine a second path based on the first intermediate point and a second map of a second resolution; the first map and the second map are maps representing the same path area to be planned; the second resolution is higher than the first resolution;
[0028] The fourth determining module is used to determine a target path according to the first path and the second path.
[0029] In the above scheme, the device also includes:
[0030] a fifth determining module, configured to determine at least one path point according to the target path;
[0031] a deleting module, configured to delete the second path point in the target path if it is determined that the first path point and the third path point are passable;
[0032] Among them, the first path point is any path point among the at least one path point; the second path point is the next path point of the first path point among the at least one path point; and the third path point is the next path point of the second path point among the at least one path point.
[0033] In the above scheme, the device also includes:
[0034] The first determination module is further configured to determine the target path based on the first starting point and the first end point if it is determined that there is no first obstacle between the first starting point and the first end point of the path in the preset map of the first resolution.
[0035] The present application embodiment provides a path planning device, comprising: a processor and a memory for storing a computer program that can be run on the processor, wherein:
[0036] The processor is used to execute the steps of the above-mentioned path planning method when running the computer program.
[0037] An embodiment of the present application provides a storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the above-mentioned path planning method are implemented.
[0038] The embodiment of the present application provides a path planning method, device, equipment and storage medium. The method includes: if it is determined that there is a first obstacle between the first starting point and the first end point in the first map of the first resolution, then determine the first path based on at least the first obstacle, and the first starting point and / or the first end point; determine the first intermediate point based on the first path and the first obstacle; determine the second path based on the first intermediate point and the second map of the second resolution; the first map and the second map are maps representing the same path area to be planned; the second resolution is higher than the first resolution; determine the target path according to the first path and the second path. By adopting the technical solution of the embodiment of the present application, when there is a first obstacle between the first starting point and the first end point, the first intermediate point can be determined in the first path, and the first intermediate point and the second map with higher resolution can be used to continue to explore and obtain the second path, and the target path can be determined according to the first path and the second path explored last time, which effectively shortens the path length and exploration range to be explored after the obstacle appears, and further improves the calculation efficiency. BRIEF DESCRIPTION OF THE DRAWINGS
[0039] Figure 1 This is a schematic diagram of the process flow of implementing the path planning method of the embodiment of the present application;
[0040] Figure 2 This is a schematic diagram of another path planning method implementation flow in an embodiment of the present application;
[0041] Figure 3 A schematic diagram of a flow chart of another path planning method implementation in an embodiment of the present application;
[0042] Figure 4 A schematic diagram of a flow chart of another path planning method implementation in an embodiment of the present application;
[0043] Figure 5 This is a schematic diagram of the implementation process of the path planning method in an application example of this application;
[0044] Figure 6 This is a schematic diagram of obstacles appearing during the exploration of the path planning method in an application example of this application;
[0045] Figure 7 This is a schematic diagram of doubling the map resolution using a path planning method in an application example of this application;
[0046] Figure 8 This is a schematic diagram of the structure of the path planning device according to an embodiment of the present application;
[0047] Fig. 9 A schematic diagram of a hardware entity structure of a path planning device according to an embodiment of the present application. DETAILED DESCRIPTION
[0048] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art to which this application belongs. The terms used herein in the specification of this application are only for the purpose of describing specific embodiments and are not intended to limit this application.
[0049] The embodiments of the present application provide a path planning method, apparatus, device and storage medium, which can effectively shorten the path length and exploration range to be explored after an obstacle appears, and further improve computing efficiency.
[0050] In the related art, there are A-Star (A*) algorithm and Dijkstra algorithm based on heuristic path planning algorithms.
[0051] In the related technology, the heuristic path planning algorithm is basically based on a single map, and there are few solutions for path planning using multi-resolution maps. In the related solution, in order to improve the calculation efficiency, first obtain the high-resolution terrain digital elevation model (Digital Elevation Model, DEM) data of the area to be planned; select the grid unit mapping mode, build the low-resolution terrain DEM data according to the grid unit mapping mode, and obtain the mapping relationship between the high-resolution terrain DEM data and the low-resolution terrain DEM data; calculate the low-resolution shortest path between the specified starting point and the target point in the low-resolution terrain DEM data; according to the low-resolution shortest path and the mapping relationship, use the parallel computing strategy in the high-resolution terrain DEM data to simultaneously calculate the local shortest path between each adjacent grid; connect each local shortest path in turn to obtain the global shortest path of the high-resolution terrain DEM data.
[0052] In the related technology, the path planning is first obtained at low resolution, and then the adjacent points of the plan are re-solved at high resolution to improve the calculation efficiency. This method has the following problems: 1. It is possible that a successful planning path cannot be obtained at low resolution; 2. The successful path obtained at low resolution may also be the optimal path. For example, there are no obstacles between the starting point and the end point. At this time, re-planning the path at high resolution will reduce the calculation efficiency; 3. After the path exploration fails at low resolution, the resolution is increased for recalculation, which wastes the calculation content of the previous steps; 4. Due to the characteristics of the grid map or octree map itself, the path planned by A star or Dijkstra is not the optimal path, and the related technical solution does not adopt corresponding measures.
[0053] The present application embodiment provides a path planning method, such as Figure 1 As shown, the method includes:
[0054] Step 101: If it is determined that a first obstacle exists between a first starting point and a first end point in a first map with a first resolution, a first path is determined based at least on the first obstacle and the first starting point and / or the first end point.
[0055] Here, the path planning method can be a planning method for unidirectional exploration from the first starting point to the first end point; it can also be a planning method for unidirectional exploration from the first end point to the first starting point; it can also be a planning method for bidirectional exploration from the first starting point to the first end point and from the first end point to the first starting point.
[0056] Exemplarily, the first map used in the planning method can be determined according to actual conditions, which is not limited here. As an example, the first map can be a grid map and / or an octree map used for an exploratory path planning map.
[0057] Exemplarily, the process of exploring the first starting point and the first end point by the path planning method can be determined according to actual conditions, and is not limited here. As an example, the A* algorithm and / or the Dijkstra algorithm can be used.
[0058] It can be understood that at least one first exploration point and / or at least one second exploration point can be determined in the process of exploring the first starting point and the first end point; the first exploration point represents the exploration point obtained by exploring from the first starting point to the first end point; the second exploration point represents the exploration point obtained by exploring from the first end point to the first starting point.
[0059] Exemplarily, the presence of a first obstacle between the first starting point and the first end point can indicate that the first starting point and the first end point cannot meet smoothly. At this time, the exploration of path planning is terminated and the first path is determined, wherein the first path can represent the intermediate path before the first obstacle appears. The first path can be determined based on at least one first exploration point and the first starting point; and / or the first path can be determined based on at least one second exploration point and the first end point. Specifically, the first path is obtained by connecting the first starting point and the first exploration point close to the first obstacle among the at least one first exploration point; and / or the first path is obtained by connecting the first end point and the second exploration point close to the first obstacle among the at least one second exploration point.
[0060] Step 102: Determine a first intermediate point based on the first path and the first obstacle.
[0061] Exemplarily, the first intermediate point may be determined based on at least one first exploration point and the first obstacle in the first path; and / or the first intermediate point may be determined based on at least one second exploration point and the first obstacle in the first path. Specifically, the first exploration point close to the first obstacle in the at least one first exploration point is used as the first intermediate point; and / or the second exploration point close to the first obstacle in the at least one second exploration point is used as the first intermediate point.
[0062] Step 103: Determine a second path based on the first intermediate point and a second map with a second resolution; the first map and the second map are maps representing the same path area to be planned; and the second resolution is higher than the first resolution.
[0063] Exemplarily, the second map may be determined according to actual conditions, which is not limited herein. As an example, the second map may be a grid map and / or an octree map for an exploratory path planning map corresponding to the first map.
[0064] Exemplarily, if it is determined that there is no obstacle between the first intermediate point and the first end point in the second map, and / or there is no obstacle between the first intermediate point and the first starting point, a second path is determined based on the first intermediate point and the first end point and / or the first starting point, wherein the second path can represent an intermediate path after the first obstacle appears.
[0065] It can be understood that at least one third exploration point can be determined in the process of exploring the first intermediate point and the first end point, and / or at least one fourth exploration point can be determined in the process of exploring the first intermediate point and the first starting point; the third exploration point represents the exploration point obtained by exploring from the first intermediate point to the first end point; the fourth exploration point represents the exploration point obtained by exploring from the first intermediate point to the first starting point.
[0066] For example, if there is no obstacle between the first intermediate point and the first end point, it can be indicated that the first intermediate point and the first end point can meet smoothly, and the exploration of path planning is terminated at this time, and the second path can be determined according to the first intermediate point and the first end point. Specifically, the first intermediate point and the first end point are connected to obtain the second path.
[0067] Exemplarily, there is no obstacle between the first intermediate point and the first starting point, which indicates that the first intermediate point and the first starting point can meet smoothly, and the exploration of path planning is terminated at this time, and the second path can be determined according to the first intermediate point and the first starting point. Specifically, the first intermediate point and the first starting point are connected to obtain the second path.
[0068] Step 104: Determine a target path according to the first path and the second path.
[0069] Exemplarily, the first path and the second path before the first obstacle appears may be merged to obtain the target path.
[0070] In an application example, the method further includes:
[0071] determining at least one waypoint according to the target path;
[0072] If it is determined that the first path point and the third path point are passable, the second path point is deleted from the target path;
[0073] Among them, the first path point is any path point in at least one path point; the second path point is the next path point of the first path point in at least one path point; and the third path point is the next path point of the second path point in at least one path point.
[0074] Exemplarily, at least one path point can be determined during the exploration of the first starting point and the first end point. Specifically, at least one path point can be determined based on the first path and / or the second path. The path point may come from the first path or the second path, which is not limited here.
[0075] Exemplarily, whether two path points are passable can be determined based on the Floyd path smoothing algorithm (FLOYD), which at least includes: the first step, removing adjacent collinear path points, and calculating whether the directions of the two vectors are the same by traversing the path; the second step, removing redundant turning path points, and calculating whether the directions of the two vectors are the same by traversing the path; all path points can be traversed, the path points between two path points that can be directly passed can be removed, and a straight line drawing algorithm can be used to find two path points as endpoints to draw a line. If the grids passed through first are all passable, then the points between the two path points in the path are considered redundant.
[0076] Here, due to the characteristics of the grid map or octree map, the path planned by A-star or Dijkstra is not a smooth path. This embodiment can smooth the path points in the target path to obtain a target path with better performance.
[0077] In other application examples, if it is determined that there is a second obstacle between the first intermediate point and the first end point in the second map with a second resolution, and / or there is a second obstacle between the first intermediate point and the first starting point, then a third path is determined based on the second obstacle and the first intermediate point; based on the third path and the second obstacle, the second intermediate point is determined; based on the second intermediate point and the third map with a third resolution, a fourth path is determined; the first map, the second map and the third map are maps representing the same path area to be planned; the third resolution is higher than the second resolution; and the target path is determined based on the first path, the third path and the fourth path.
[0078] It should be noted that the path planning method in the present application is an iterative process. If there are still obstacles between the intermediate point and the first end point and / or the first starting point, the intermediate point is re-determined based on the obstacle, the resolution of the map is further improved, and the intermediate path is re-determined based on the map with a higher resolution; until there are no obstacles between the intermediate point and the first end point and / or the first starting point, the target path is determined based on the first path and the intermediate path.
[0079] In an application example, the method further includes:
[0080] If it is determined that there is no first obstacle between the first starting point and the first end point of the path in the preset map of the first resolution, the target path is determined based on the first starting point and the first end point.
[0081] For example, if there is no first obstacle between the first starting point and the first end point, it can be indicated that the first starting point and the first end point can meet smoothly, and the exploration of path planning is terminated at this time, and the target path can be determined according to the first starting point and the first end point. Specifically, the first starting point and the first end point are connected to obtain the target path.
[0082] Here, after the embodiment of the present application obtains the target path at a first resolution with a lower resolution, the path planning is directly ended to avoid wasting computing resources; the second resolution with a higher resolution will not be used to re-plan the path. At this time, re-planning the path at a high resolution will reduce the computing efficiency.
[0083] The present application provides another path planning method, which includes:
[0084] Step 201: If it is determined that there is a first obstacle between a first starting point and a first end point in a first map with a first resolution, a first path is determined based on the first obstacle and the first starting point; the first path represents a path from the first starting point to the first obstacle.
[0085] For example, the first path may represent an intermediate path explored between the first starting point and the first obstacle when a one-way exploration is performed from the first starting point to the first end point and a first obstacle appears between the first starting point and the first end point.
[0086] Step 202: Determine a second starting point based on the first path and the first obstacle.
[0087] Exemplarily, the second starting point may be determined according to at least one first exploration point and the first obstacle in the first path. Specifically, the first exploration point close to the first obstacle in the at least one first exploration point is used as the second starting point.
[0088] Step 203: Determine a second path based on the second map and the second starting point; the second path represents a path from the second starting point to the first end point.
[0089] It is understandable that at least one third exploration point can be determined in the process of exploring the second starting point and the first end point; the third exploration point represents an exploration point obtained by exploring from the second starting point to the first end point.
[0090] Exemplarily, if it is determined that there are no obstacles between the second starting point and the first end point in the second map, a second path is determined based on the second starting point and the first end point, where the second path can represent an intermediate path obtained by exploring between the second starting point and the first end point when a one-way exploration is performed from the second starting point to the first end point and there are no obstacles between the second starting point and the first end point.
[0091] For example, if there is no obstacle between the second starting point and the first end point, it can be indicated that the second starting point and the first end point can meet smoothly, and the exploration of path planning is terminated at this time, and the second path can be determined according to the second starting point and the first end point. Specifically, the second starting point and the first end point are connected to obtain the second path.
[0092] Step 204: Determine a target path according to the first path and the second path.
[0093] The present application provides another path planning method, which includes:
[0094] Step 301: If it is determined that there is a first obstacle between a first starting point and a first end point in a first map with a first resolution, a first path is determined based on the first obstacle and the first end point; the first path represents a path from the first end point to the first obstacle.
[0095] For example, the first path may represent an intermediate path obtained by exploring between the first end point and the first obstacle when a one-way exploration is performed from the first end point to the first starting point and a first obstacle appears between the first end point and the first starting point.
[0096] Step 302: Determine a second end point based on the first path and the first obstacle.
[0097] Exemplarily, the second end point may be determined based on at least one second exploration point and the first obstacle in the first path. Specifically, a second exploration point close to the first obstacle among the at least one second exploration point is used as the second end point.
[0098] Step 303: Determine a second path based on the second map and the second end point; the second path represents a path from the second end point to the first starting point.
[0099] It is understandable that at least one fourth exploration point can be determined in the process of exploring the second end point and the first starting point; the fourth exploration point represents an exploration point obtained by exploring from the second end point to the first starting point.
[0100] Exemplarily, if it is determined that there is no obstacle between the second end point and the first starting point in the second map, a second path is determined based on the second end point and the first starting point, where the second path can represent an intermediate path obtained by exploring between the second end point and the first starting point when a one-way exploration is performed from the second end point to the first starting point and there is no obstacle between the second end point and the first starting point.
[0101] For example, if there is no obstacle between the second end point and the first starting point, it can be indicated that the second end point and the first starting point can meet smoothly, and the exploration of path planning is terminated at this time, and the second path can be determined according to the second end point and the first starting point. Specifically, the second end point and the first starting point are connected to obtain the second path.
[0102] Step 304: Determine a target path according to the first path and the second path.
[0103] The present application provides another path planning method, which includes:
[0104] Step 401: If it is determined that there is a first obstacle between the first starting point and the first end point in the first map of the first resolution, a first sub-path is determined based on the first obstacle and the first starting point, and a second sub-path is determined based on the first obstacle and the first end point; the first sub-path represents a path from the first starting point to the first obstacle; and the second sub-path represents a path from the first end point to the first obstacle.
[0105] Exemplarily, the first subpath may represent an intermediate path explored between the first starting point and the first obstacle when bidirectional exploration is performed from the first starting point to the first end point and from the first end point to the first starting point, and when a first obstacle appears between the first starting point and the first end point.
[0106] Exemplarily, the second subpath may represent a bidirectional exploration from the first starting point to the first end point, and from the first end point to the first starting point; and when a first obstacle appears between the first end point and the first starting point, an intermediate path obtained by exploring between the first end point and the first obstacle.
[0107] Step 402: Determine a second starting point based on the first sub-path and the first obstacle;
[0108] Exemplarily, the second starting point may be determined according to at least one first exploration point and the first obstacle in the first subpath. Specifically, the first exploration point close to the first obstacle in the at least one first exploration point is used as the second starting point.
[0109] Step 403: Determine a second end point based on the second sub-path and the first obstacle;
[0110] Exemplarily, the second end point may be determined according to at least one second exploration point in the second subpath and the first obstacle. Specifically, a second exploration point close to the first obstacle among the at least one second exploration point is used as the second end point.
[0111] Step 404: Determine a second path based on the second map, the second starting point, and the second end point; the second path represents a path from the second starting point to the second end point.
[0112] It is understandable that at least one third exploration point can be determined in the process of exploring the second starting point and the first end point; the third exploration point represents the exploration point obtained by exploring from the second starting point to the first end point. At least one fourth exploration point can be determined in the process of exploring the second end point and the first starting point; the fourth exploration point represents the exploration point obtained by exploring from the second end point to the first starting point.
[0113] Exemplarily, if it is determined that there are no obstacles between the second starting point and the second end point in the second map, a second path is determined based on the second starting point and the second end point, where the second path can represent a two-way exploration from the first starting point to the first end point, and from the first end point to the first starting point; and when there are no obstacles between the second starting point and the second end point, an intermediate path is explored between the second starting point and the second end point.
[0114] For example, if there is no obstacle between the second starting point and the second end point, it can be indicated that the second starting point and the second end point can meet smoothly, and the exploration of path planning is terminated at this time, and the second path can be determined according to the second starting point and the second end point. Specifically, the second starting point and the second end point are connected to obtain the second path.
[0115] Step 405: Determine a target path according to the first path and the second path.
[0116] The path planning method of the embodiment of the present application is illustrated below with reference to an application example.
[0117] This application proposes a drone path planning method based on multi-resolution maps. First, two-way exploration is performed from the starting point and the end point towards each other on a low-resolution map. If no obstacles are encountered on the way, that is, the encounter is successful, the drone path planning is completed, and its corresponding path is the line connecting the starting point and the end point.
[0118] If an obstacle is encountered on the way, the exploration point near the obstacle close to the starting point is recorded in the bidirectional list as the new starting point (i.e., the starting point), and the exploration point near the obstacle close to the target point (i.e., the end point) is recorded as the new target point; at the same time, the resolution of the above low-resolution map is doubled, and a bidirectional A* or Dijkstra path planning algorithm is used on the new high-resolution map to explore the path.
[0119] Continue to perform bidirectional A* or Dijkstra path planning algorithm on the new map to explore the path until success. After obtaining the above successfully planned path, the path is clipped on the original highest resolution map.
[0120] like Figure 5 As shown, the specific process of this application method is as follows:
[0121] Step 501: Build a map.
[0122] This application can be applied to maps of most formats, such as building maps using raster maps and octree maps of exploratory path planning maps.
[0123] Step 502: On the low-resolution map, two-way exploration is performed from the starting point and the end point toward each other simultaneously, wherein the specific method and steps of the exploration are as follows:
[0124] First, an open table and a closed table are established respectively, and the start point / end point is added to the corresponding open table; the open table is used to store the currently unexpanded nodes, and the close table is used to store the expanded nodes.
[0125] Next, repeat the following steps:
[0126] Step a. Traverse the open table to find the node with the smallest cost function F value, and use the smallest node as the current node to be processed.
[0127] Step b. Move the current minimum node to the close table.
[0128] Step c. Perform the following processing on all neighbor directions of the current minimum node:
[0129] 1) If the neighbor node is unreachable or in the close table, ignore it; otherwise, continue to determine whether the neighbor node is in the open table.
[0130] 2) If the neighbor node is not in the open table, add the neighbor node to the open table, set the current minimum node as the father of the neighbor node, and record the cost function F value, movement cost G value and estimated cost H value of the neighbor node.
[0131] 3) If it is already in the open list, check if this path is better. If so, recalculate its cost and reorder the open list.
[0132] Repeat steps a, b, and c above until the distance between the new starting point and the target point is less than the threshold.
[0133] Step 503: Determine whether the encounter is successful.
[0134] If no obstacles are encountered during the two-way exploration, that is, the encounter is successful, the drone path planning ends, and its corresponding path is the connection between the starting point and the end point obtained by the exploration. This step ensures that there are no obstacles between the two points, which not only ensures the optimal path, but also ensures speed.
[0135] Step 504: Set a new starting point and target point, and double the previous map resolution.
[0136] If an obstacle is encountered during the two-way exploration, the exploration point near the obstacle in the direction of the starting point is recorded in the two-way list as the new starting point, and the exploration point near the obstacle in the direction of the target point is recorded in the two-way list as the new target point. Under the new starting point and the new end point, the map resolution in step 21 above is doubled to obtain a new map.
[0137] For example, at the current low resolution, although only a certain area of the middle grid has obstacles, at this resolution, the grid is considered unreachable. Figure 6 As shown in the figure, the exploration points near the obstacle in the direction of the starting point and the exploration points near the obstacle in the direction of the target point are unreachable, that is, the path cannot be found. Here, the exploration points near the obstacle in the direction of the target point are set as the new starting point position, and the exploration points near the obstacle in the direction of the target point are set as the new end point position, and then go to step 4.
[0138] Step 505: Perform a bidirectional A* or Dijkstra path planning algorithm to explore the path.
[0139] like Figure 7 As shown, if the above-mentioned bidirectional A* or Dijkstra fails to find the path on the map in step 21, the most recently added value in the bidirectional list is extracted as the starting point and the target point, where the most recently added value can be the value recorded after exploration. The starting point and the target point are converted into coordinates in the new grid coordinate system, and the path is explored again. This step can avoid the waste of computing resources, because the exploration before the failure is still valid and can be used as an intermediate path.
[0140] Step 506: Determine whether the path planning is successful.
[0141] Repeat steps 504 to 505 until the path planning is successful.
[0142] Step 507: Merge the above explored path points.
[0143] Step 508: Cropping.
[0144] Path clipping is done on the original highest resolution map.
[0145] Due to the grid map or octree map, the path points obtained by the above path planning are relatively tortuous, which increases the flight time and resource consumption of the drone, and the path points need to be screened out. Assuming that the path points S0, S1, S2...Sn are obtained, select the interval points to check whether they are passable, such as S0 and S2, S3 and S5, etc. If S0 and S2 are passable, delete S1 until no new path points can be deleted. This step can smooth the above planned path.
[0146] The present application embodiment provides a path planning device, such as Figure 8 As shown, the path planning device 800 includes:
[0147] A first determination module 801 is configured to determine a first path based at least on the first obstacle and the first starting point and / or the first end point if it is determined that there is a first obstacle between the first starting point and the first end point in the first map with a first resolution;
[0148] A second determination module 802 is used to determine a first intermediate point based on the first path and the first obstacle;
[0149] A third determination module 803 is used to determine a second path based on the first intermediate point and a second map of a second resolution; the first map and the second map are maps representing the same path area to be planned; and the second resolution is higher than the first resolution;
[0150] The fourth determining module 804 is configured to determine a target path according to the first path and the second path.
[0151] In some embodiments, the first intermediate point includes a second starting point or a second end point; the path planning device 800 includes:
[0152] The second determination module 802 is further used to determine a second starting point or a second end point based on the first path and the first obstacle; the first path represents a path from the first starting point or the first end point to the first obstacle;
[0153] The third determination module 803 is further used to determine a second path based on the second map and the second starting point or the second end point; the second path represents a path from the second starting point to the first end point or a path from the second end point to the first starting point.
[0154] In some embodiments, the first intermediate point includes a second starting point and a second end point; the first path includes a first sub-path and a second sub-path; the first sub-path represents a path from the first starting point to the first obstacle; the second sub-path represents a path from the first end point to the first obstacle; the path planning device 800 includes:
[0155] The second determination module 802 is further configured to determine a second starting point based on the first sub-path and the first obstacle; and determine a second end point based on the second sub-path and the first obstacle;
[0156] The third determination module 803 is further configured to determine a second path based on the second map, the second starting point, and the second end point; the second path represents a path from the second starting point to the second end point.
[0157] In some embodiments, the path planning device 800 further includes:
[0158] A fifth determination module, configured to determine at least one path point according to the target path;
[0159] A deletion module, configured to delete the second path point in the target path if it is determined that the first path point and the third path point are passable;
[0160] Among them, the first path point is any path point in at least one path point; the second path point is the next path point of the first path point in at least one path point; and the third path point is the next path point of the second path point in at least one path point.
[0161] In some embodiments, the path planning device 800 further includes:
[0162] The first determination module is further used to determine the target path based on the first starting point and the first end point if it is determined that there is no first obstacle between the first starting point and the first end point of the path in the preset map of the first resolution.
[0163] It should be noted that: when the path planning device provided in the above embodiment is controlled, only the division of the above program modules is used as an example. In actual applications, the above processing can be assigned to different program modules as needed, that is, the internal structure of the device is divided into different program modules to complete all or part of the processing described above. In addition, the path planning device provided in the above embodiment and the aforementioned path planning method embodiment belong to the same concept. The specific implementation process is detailed in the method embodiment and will not be repeated here.
[0164] Based on the hardware implementation of the above program modules, and in order to implement the method of the embodiment of the present application, the embodiment of the present application also provides a path planning device. Fig. 9 Only an exemplary structure of the path planning device is shown, not all structures, and can be implemented as needed. Fig. 9 Partial or complete structure shown.
[0165] like Fig. 9 As shown, the path planning device 900 provided in the embodiment of the present application includes: at least one processor 901, a memory 902 and a user interface 903. The various components in the path planning device 900 are coupled together through a bus system 904. It can be understood that the bus system 904 is used to realize the connection and communication between these components. In addition to the data bus, the bus system 904 also includes a power bus, a control bus and a status signal bus. However, for the sake of clarity, in Fig. 9 Various buses are labeled as bus system 904.
[0166] The user interface 903 may include a display, a keyboard, a mouse, a trackball, a click wheel, keys, buttons, a touch pad or a touch screen.
[0167] The memory 902 in the embodiment of the present application is used to store various types of data to support the operation of the control device. Examples of such data include: any computer program used to operate on the control device.
[0168] The path planning method disclosed in the embodiment of the present application can be applied to the processor 901, or implemented by the processor 901. The processor 901 may be an integrated circuit chip with signal processing capabilities. In the implementation process, each step of the path planning method can be completed by the hardware integrated logic circuit or software instructions in the processor 901. The above-mentioned processor 901 can be a general-purpose processor, a digital signal processor (DSP, Digital Signal Processor), or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. The processor 901 can implement or execute the various methods, steps and logic block diagrams disclosed in the embodiment of the present application. The general-purpose processor can be a microprocessor or any conventional processor, etc. In combination with the steps of the method disclosed in the embodiment of the present application, it can be directly embodied as a hardware decoding processor to execute, or it can be executed by a combination of hardware and software modules in the decoding processor. The software module can be located in a storage medium, which is located in the memory 902, and the processor 901 reads the information in the memory 902, and completes the steps of the path planning method provided in the embodiment of the present application in combination with its hardware.
[0169] In an exemplary embodiment, the path planning device can be implemented by one or more application specific integrated circuits (ASIC), DSP, programmable logic device (PLD), complex programmable logic device (CPLD), field programmable gate array (FPGA), general processor, controller, microcontroller (MCU), microprocessor, or other electronic components to execute the aforementioned method.
[0170] It can be understood that the memory 902 can be a volatile memory or a non-volatile memory, and can also include both volatile and non-volatile memories. Among them, the non-volatile memory can be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), a magnetic random access memory (FRAM), a flash memory, a magnetic surface memory, an optical disk, or a compact disc read-only memory (CD-ROM); the magnetic surface memory can be a disk memory or a tape memory. The volatile memory can be a random access memory (RAM), which is used as an external cache. By way of example and not limitation, many forms of RAM are available, such as static random access memory (SRAM), synchronous static random access memory (SSRAM), dynamic random access memory (DRAM), synchronous dynamic random access memory (SDRAM), double data rate synchronous dynamic random access memory (DDRSDRAM), enhanced synchronous dynamic random access memory (ESDRAM), synchronous link dynamic random access memory (SLDRAM, SyncLink Dynamic Random Access Memory), and direct RAM bus random access memory (DRRAM, Direct Rambus Random Access Memory).The memories described in the embodiments of the present application are intended to include, but are not limited to, these and any other suitable types of memories.
[0171] In an exemplary embodiment, the present application also provides a storage medium, namely a computer storage medium, which can be a computer-readable storage medium, for example, a memory 902 storing a computer program, and the computer program can be executed by a processor 901 of the path planning device to complete the steps described in the method of the present application embodiment. The computer-readable storage medium can be a memory such as a ROM, a PROM, an EPROM, an EEPROM, a Flash Memory, a magnetic surface memory, an optical disk, or a CD-ROM.
[0172] It should be noted that: "first", "second", etc. are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence.
[0173] In addition, the technical solutions described in the embodiments of the present application can be combined arbitrarily without conflict.
[0174] The above is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto. Any technician familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.
Claims
1. A path planning method, characterized in that: include: If it is determined that there is a first obstacle between the first starting point and the first end point in the first map with the first resolution, determining a first path based at least on the first obstacle, and the first starting point and / or the first end point; Determining a first intermediate point based on the first path and the first obstacle; Determine a second path based on the first intermediate point and a second map with a second resolution; the first map and the second map are maps representing the same path area to be planned; The second resolution is higher than the first resolution; A target path is determined according to the first path and the second path.
2. The method according to claim 1, characterized in that: The first intermediate point includes a second starting point or a second end point; The determining a first intermediate point based on the first path and the first obstacle includes: Based on the first path and the first obstacle, determining a second starting point or a second end point; the first path represents a path from the first starting point or the first end point to the first obstacle; Correspondingly, determining the second path based on the first intermediate point and the second map with the second resolution includes: The second path is determined based on the second map and the second starting point or the second end point; the second path represents a path from the second starting point to the first end point or a path from the second end point to the first starting point.
3. The method according to claim 1, characterized in that The first intermediate point includes a second starting point and a second end point; the first path includes a first sub-path and a second sub-path; the first sub-path represents a path from the first starting point to the first obstacle; The second sub-path represents a path from the first end point to the first obstacle; The method further comprises: Determine the second starting point based on the first sub-path and the first obstacle; Determining the second end point based on the second sub-path and the first obstacle; Based on the second map, the second starting point and the second end point, the second path is determined; the second path represents a path from the second starting point to the second end point.
4. The method according to any one of claims 1 to 3, characterized in that: The method further comprises: Determine at least one path point according to the target path; If it is determined that the first path point and the third path point are passable, deleting the second path point in the target path; Among them, the first path point is any path point among the at least one path point; the second path point is the next path point of the first path point among the at least one path point; and the third path point is the next path point of the second path point among the at least one path point.
5. The method according to any one of claims 1 to 3, characterized in that: The method further comprises: If it is determined that there is no first obstacle between the first starting point and the first end point of the path in the preset map of the first resolution, the target path is determined based on the first starting point and the first end point.
6. A path planning device, characterized in that: include: A first determination module, configured to determine a first path based at least on the first obstacle, and the first starting point and / or the first end point if it is determined that a first obstacle exists between the first starting point and the first end point in the first map with a first resolution; A second determining module, configured to determine a first intermediate point based on the first path and the first obstacle; A third determination module, configured to determine a second path based on the first intermediate point and a second map with a second resolution; The first map and the second map are maps representing the same path area to be planned; The second resolution is higher than the first resolution; The fourth determining module is used to determine a target path according to the first path and the second path.
7. The device according to claim 6, characterized in that The device also includes: a fifth determining module, configured to determine at least one path point according to the target path; a deleting module, configured to delete the second path point in the target path if it is determined that the first path point and the third path point are passable; Among them, the first path point is any path point among the at least one path point; the second path point is the next path point of the first path point among the at least one path point; and the third path point is the next path point of the second path point among the at least one path point.
8. The device according to claim 6, characterized in that The device also includes: The first determination module is further configured to determine the target path based on the first starting point and the first end point if it is determined that there is no first obstacle between the first starting point and the first end point of the path in the preset map of the first resolution.
9. A path planning device, characterized in that: include: A processor and a memory for storing a computer program that can be executed on the processor, wherein: The processor is used to execute the steps of the method according to any one of claims 1 to 5 when running a computer program.
10. A storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 5 are implemented.