Path planning method, robot and computer readable storage medium

Abstract

The embodiment of the invention relates to the technical field of path planning, in particular to a path planning method, a robot and a computer readable storage medium. The method comprises the steps of obtaining a quadtree map, determining a starting point and a target point, executing multiple potential field expansion operations on the quadtree map according to a preset path planning algorithm, the starting point and the target point to obtain a plurality of path points, and generating a target path according to the starting point, the plurality of path points and the target point. According to the embodiment of the invention, on-line path searching can be carried out based on the quadtree map according to the starting point and the target point, so that the problem of unreliability caused by off-line path planning can be avoided, the path searched on line can reflect the current road condition in real time, and reliable and safe navigation of the robot is facilitated. In addition, the quadtree map can compress the state information of each node, the robot does not need to open up a large memory to store the quadtree map, and the application range of the robot is widened.

Description

Technical Field

[0001] The embodiments of the present application relate to the field of path planning technology, and in particular to a path planning method, a robot, and a computer-readable storage medium. Background Art

[0002] Robots provided by related technologies typically perform path planning and navigation based on grid maps. Grid maps record the status information of all grid cells, and maps often contain a large number of grid cells. This requires the robot to allocate a large amount of memory to store the status information of each grid cell in the grid map. Later, when a path needs to be planned, the robot uses this information to perform path planning. Furthermore, robots provided by related technologies often use an offline path planning method on grid maps, which can make the robot's planned paths unreliable due to uncertainties such as dynamic obstacles. Summary of the Invention

[0003] One purpose of the embodiments of the present application is to provide a path planning method, a robot, and a computer-readable storage medium to solve the technical problem that the paths planned by related technologies are not reliable enough.

[0004] In a first aspect, an embodiment of the present application provides a path planning method, comprising:

[0005] Acquire a quadtree map, wherein the quadtree map includes idle leaf nodes, and the idle leaf nodes are leaf nodes in an idle state;

[0006] Determine the starting point and destination point;

[0007] According to the preset path planning algorithm, starting point and target point, multiple potential field expansion operations are performed on the quadtree map to obtain multiple path points;

[0008] generating a target path according to the starting point, the plurality of path points and the target point;

[0009] Among them, the last potential field expansion operation acts on at least one neighbor node adjacent to the central node, and the neighbor node that meets the preset constraints is determined as the central node of the next potential field expansion operation. The central node of the first potential field expansion operation is the starting point, and one of the neighbor nodes of the last potential field expansion operation is the target point. The central node and at least one of the neighbor nodes are both idle leaf nodes.

[0010] Optionally, the last potential field expansion operation is performed on at least one neighboring node adjacent to the central node, and determining the neighboring node that meets a preset constraint condition as the central node for the next potential field expansion operation includes:

[0011] Determine at least one neighbor node adjacent to the central node;

[0012] Calculating a distance search cost extending from the central node to the neighboring nodes;

[0013] If the distance search cost of the target neighbor node is the minimum distance cost among the distance search costs of the multiple neighbor nodes, it is determined that the target neighbor node meets the preset constraint condition, and the target neighbor node is a neighbor node among the multiple neighbor nodes;

[0014] The target neighbor node is set as the central node of the next potential field expansion operation.

[0015] Optionally, the quadtree map is configured with an original grid, and determining at least one neighbor node adjacent to the central node includes:

[0016] Determine a target idle grid adjacent to the central node, where the target idle grid is an original grid that is in an idle state and adjacent to the central node;

[0017] At least one neighbor node is determined according to at least one target idle grid.

[0018] Optionally, determining at least one neighbor node according to at least one target idle grid includes:

[0019] Determining a target idle leaf node including the target idle grid;

[0020] The target idle leaf node is set to the at least one neighbor node.

[0021] Optionally, determining the target idle leaf node including the target idle grid includes:

[0022] Determine a target position of the target idle grid in the quadtree map;

[0023] The quadtree map is traversed to find an idle leaf node containing the target position, and the idle leaf node containing the target position is the target idle leaf node.

[0024] Optionally, the quadtree map is configured with a coordinate system, and traversing the quadtree map to find an idle leaf node containing the target position includes:

[0025] If the horizontal coordinate of the target position is greater than or equal to the horizontal coordinate of the idle leaf node and less than the sum of the horizontal coordinate of the idle leaf node and the width of the idle leaf node, and the vertical coordinate of the target position is greater than or equal to the vertical coordinate of the idle leaf node and less than the sum of the vertical coordinate of the idle leaf node and the height of the idle leaf node, then it is determined that the idle leaf node contains the target position.

[0026] Optionally, determining a target idle grid adjacent to the central node includes:

[0027] Determine respectively a first idle grid set, a second idle grid set, a third idle grid set, and a fourth idle grid set adjacent to four sides of the central node, each idle grid set including at least one target idle grid;

[0028] The first diagonal idle grid, the second diagonal idle grid, the third diagonal idle grid and the fourth diagonal idle grid that are in the diagonal direction of the central node and adjacent to the four vertices of the central node are respectively determined, and the first diagonal idle grid, the second diagonal idle grid, the third diagonal idle grid and the fourth diagonal idle grid are all target idle grids.

[0029] Optionally, obtaining a quadtree map includes:

[0030] Obtaining an original map, wherein the original map is configured with a root node;

[0031] Set the root node as the current expansion node;

[0032] Determine whether the current expansion node is a leaf node;

[0033] If yes, then divide the area corresponding to the current expansion node into multiple child nodes according to the quadtree partitioning method, and set each child node as the current expansion node, and return to the step of determining whether the current expansion node is a leaf node;

[0034] If not, the determination operation on the current expansion node is terminated.

[0035] In a second aspect, an embodiment of the present application provides a robot comprising a memory and a processor, wherein the memory is connected to the processor, and the processor is used to execute one or more computer programs stored in the memory. When the processor executes the one or more computer programs, the robot implements the above-mentioned method.

[0036] In a third aspect, an embodiment of the present application provides a computer-readable storage medium, characterized in that the computer-readable storage medium stores a computer program, the computer program includes program instructions, and when the program instructions are executed by a processor, the processor executes the above method.

[0037] The present invention can achieve the following technical effects: In the path planning method provided in the embodiment of the present invention, a quadtree map is obtained, wherein the quadtree map includes idle leaf nodes, where the idle leaf nodes are leaf nodes in an idle state, each leaf node includes at least one grid, and the state of all grids contained in the leaf node is the same, a starting point and a target point are determined, and according to a preset path planning algorithm, the starting point and the target point, multiple potential field expansion operations are performed on the quadtree map to obtain multiple path points, and a target path is generated based on the starting point, the multiple path points and the target point, wherein the last potential field expansion operation acts on at least one neighboring node adjacent to the center node, and the neighboring node that meets the preset constraint conditions is determined as the center node of the next potential field expansion operation, the center node of the first potential field expansion operation is the starting point, and one of the neighboring nodes of the last potential field expansion operation is the target point, and the center node and at least one neighboring node are all idle leaf nodes. The present embodiment can perform online path search based on the quadtree map according to the starting point and the target point, thereby avoiding the unreliability problem caused by offline path planning. The path searched online can reflect the current road conditions in real time, which is conducive to reliable and safe navigation of the robot. In addition, the quadtree map can compress the status information of each node, so the robot does not need to allocate a large amount of memory to store the quadtree map, which is beneficial to improving the applicability of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following briefly introduces the drawings required for use in the description of the embodiments of the present application. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0039] Figure 1 A schematic diagram of a flow chart of a path planning method provided in an embodiment of the present application;

[0040] Figure 2 A schematic diagram of a grid map provided in an embodiment of the present application;

[0041] Figure 3 A schematic diagram of a route planned using a grid map provided by relevant technologies;

[0042] Figure 4A schematic diagram of a route planned using a quadtree map according to an embodiment of the present application;

[0043] Figure 5a 、 Figure 5b 、 Figure 5c and Figure 5d Schematic diagrams of the process of generating a quadtree map by using a quadtree segmentation method according to an embodiment of the present application;

[0044] Figure 6 A position state diagram of the central node and adjacent original grids provided in an embodiment of the present application;

[0045] Figure 7 A schematic diagram of the structure of a path planning device provided in an embodiment of the present application;

[0046] Figure 8 A schematic structural diagram of a robot provided in an embodiment of the present application. DETAILED DESCRIPTION

[0047] In order to make the purpose, technical solutions and advantages of this application more clearly understood, the present application is further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are only used to explain this application and are not intended to limit this application. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of this application.

[0048] It should be noted that, if there is no conflict, the various features in the embodiments of the present application can be combined with each other and are all within the scope of protection of the present application. In addition, although the functional modules are divided in the device schematic and the logical order is shown in the flow chart, in some cases, the steps shown or described can be performed in a different order than the module division in the device or the order in the flow chart. Furthermore, the words "first", "second", "third", etc. used in this application do not limit the data and execution order, but only distinguish between the same items or similar items with basically the same functions and effects.

[0049] The present application provides a first related technology that performs path planning based on a grid map, wherein the grid map is composed of multiple grids of the same size. When the grid map is large, due to the small size of the grids, the grid map contains a large number of grids. When the robot performs path planning, it needs to continuously search for smaller grids on the grid map. This path planning method is time-consuming and has low path planning efficiency.

[0050] The embodiment of the present application provides a second related technology, which adopts an offline method to plan the path, wherein the robot needs to plan the path based on the pre-stored neighbor information of each node. This will cause the robot to need to open up a large memory to store the neighbor information of each node. This offline path planning method is only suitable for high-cost robots with large memory, and is not suitable for low-cost robots with small memory.

[0051] The embodiments of the present application can generate a quadtree map based on a grid map. The size of each node in the quadtree map is larger than the grid size of the grid map. Then, based on the quadtree map and the A* algorithm or the Dijkstra algorithm, online path planning is performed, thereby improving path planning efficiency. Furthermore, since there is no need to allocate a large amount of memory to pre-store neighbor information for each node, path planning operations can be performed with low memory usage. This helps to expand the scope of application of the path planning method provided by the embodiments of the present application. Without requiring a large amount of memory for path planning, path planning operations can be performed on low-cost robots.

[0052] This application embodiment provides a path planning method. Figure 1 , the path planning method includes the following steps:

[0053] S11: Get the quadtree map.

[0054] In this step, the quadtree map is a map obtained by dividing the original map into a quadtree. The quadtree map includes a plurality of idle leaf nodes, where the idle leaf nodes are leaf nodes in an idle state. It is understood that the sizes of the plurality of idle leaf nodes may be the same or different.

[0055] Each leaf node contains at least one grid, and all grids contained in a leaf node have the same state. Grid states include idle, occupied, and unknown. Idle is when there are no obstacles occupying it, occupied is when there are obstacles occupying it, and unknown is when the robot has not yet detected an area. When a grid is idle, it is considered an idle grid. When it is occupied, it is considered an occupied grid. When it is unknown, it is considered an unknown grid.

[0056] See also Figure 2 ,like Figure 2 As shown, idle grids are represented by white, occupied grids are represented by black, and unknown grids are represented by gray.

[0057] The quadtree map is configured with multiple nodes, each of which may include one grid or more than two grids. As previously mentioned, since the states of all grids contained in a leaf node are the same, this embodiment may define a leaf node as: when the states of all grids within the node are the same, the node is a leaf node.

[0058] Leaf nodes include idle leaf nodes, occupied leaf nodes and unknown leaf nodes. Idle leaf nodes are leaf nodes where all grids are in idle state. Occupied leaf nodes are leaf nodes where all grids are in occupied state. Unknown leaf nodes are leaf nodes where all grids are in unknown state. In some embodiments, this embodiment may use different cost values to represent idle state, occupied state and unknown state respectively. For example, 0 represents the cost value of an idle grid, 254 represents the cost value of an occupied grid, and 255 represents the cost value of an unknown grid. Therefore, the cost value of the grids contained in the idle leaf nodes is 0, the cost value of the grids contained in the occupied leaf nodes is 254, and the cost value of the grids contained in the unknown leaf nodes is 255.

[0059] S12: Determine the starting point and the target point.

[0060] In this step, the starting point is the starting point of the robot, and the target point is where the robot expects to arrive.

[0061] Determining the starting point and the destination point includes: obtaining a path planning request received by the robot, the path planning request carrying the starting point and the destination point, parsing the path planning request, and obtaining the starting point and the destination point. It is understood that the path planning request can be sent by a user to the robot via a remote control, by a user operating a physical button on the robot, or by a server sending the request to the robot.

[0062] In some embodiments, the method also includes: determining whether the starting point or the target point is within an idle leaf node; if so, generating correct information about the starting point configuration, or generating correct information about the target point configuration; if not, generating incorrect information about the starting point configuration, or generating incorrect information about the target point configuration.

[0063] The quadtree map is configured with a coordinate system, and determining whether the starting point is within an idle leaf node includes: traversing the quadtree map to obtain each idle leaf node, determining whether the horizontal coordinate of the starting point is greater than or equal to the horizontal coordinate of the idle leaf node and less than the sum of the horizontal coordinate of the idle leaf node and the width of the idle leaf node, and whether the vertical coordinate of the starting point is greater than or equal to the vertical coordinate of the idle leaf node and less than the sum of the vertical coordinate of the idle leaf node and the height of the idle leaf node; if so, determining that the starting point is within the idle leaf node; if not, determining that the starting point is not within the idle leaf node.

[0064] Determining whether a target point is within an idle leaf node includes: traversing a quadtree map to obtain each idle leaf node, and determining whether the abscissa of the target point is greater than or equal to the abscissa of the idle leaf node and less than the sum of the abscissa of the idle leaf node and the width of the idle leaf node, and whether the ordinate of the target point is greater than or equal to the ordinate of the idle leaf node and less than the sum of the ordinate of the idle leaf node and the height of the idle leaf node. If so, it is determined that the target point is within the idle leaf node; if not, it is determined that the target point is not within the idle leaf node.

[0065] For example, the following formula is used to determine whether the position point p is located in an idle leaf node. The formula is as follows: If p.x >= node.x and p.x < node.x + node.w and p.y >= node.y and p.y < node.y + node.w, it means that the position point p is located in the idle leaf node; otherwise, it means that the position point p is not located in the idle leaf node. Here, p.x is the abscissa of the position point p, node.x is the abscissa of the idle leaf node, node.w is the width of the idle leaf node, p.y is the ordinate of the position point p, and node.y is the ordinate of the idle leaf node.

[0066] S13: According to a preset path planning algorithm, a starting point, and a target point, perform multiple potential field expansion operations on the quadtree map to obtain multiple path points.

[0067] In this step, the preset path planning algorithm can be the A* algorithm or the Dijkstra algorithm, etc. The potential field expansion operation is an expansion operation for finding the shortest path or the safest path. The previous potential field expansion operation includes determining, among at least one neighbor node adjacent to the central node, the neighbor node that meets the preset constraint conditions as the central node for the next potential field expansion operation. The central node of the first potential field expansion operation is the starting point, one of the neighbor nodes of the last potential field expansion operation is the target point, and both the central node and at least one neighbor node are idle leaf nodes.

[0068] For example, the central node of the first potential field expansion operation T0 is the starting point k0. The nodes adjacent to the starting point k0 are k1, k2, k3, k4. However, only the neighbor node k3 meets the preset constraint conditions. Therefore, the neighbor node k3 is the central node for the next potential field expansion operation T1. In the potential field expansion operation T1, the central node is the neighbor node k3. The nodes adjacent to the neighbor node k3 are k5, k6, k7, k8. However, only the neighbor node k7 meets the preset constraint conditions. Therefore, the neighbor node k7 is the central node for the next potential field expansion operation T2. And so on, which will not be elaborated here.

[0069] S14: Generate a target path according to the starting point, multiple path points, and the target point.

[0070] See also Figure 3 ,Related technology plans a target path 33 on an original map 30 according to a given starting point 31 and a target point 32.

[0071] See also Figure 4 In this embodiment, a target path 33 is planned on a quadtree map 40 based on a given starting point 31 and a target point 32. This embodiment can perform online path search based on the quadtree map according to the starting point and the target point, thereby avoiding the unreliability problem caused by offline path planning. The path searched online can reflect the current road conditions in real time, which is conducive to the robot's reliable and safe navigation. In addition, the quadtree map can compress the status information of each node, and the robot does not need to allocate a large memory to store the quadtree map, which is conducive to improving the robot's applicability. In addition, since the grid of the quadtree map is relatively large, the number of grids in the quadtree map will be relatively less than the number of grids in the original map. When planning a path, this embodiment will relatively reduce the number of excessive grids searched, thereby improving the efficiency of path planning and thus improving the robot's operating efficiency.

[0072] In some embodiments, obtaining a quadtree map includes the following steps: obtaining an original map, the original map is configured with a root node, setting the root node as the current expansion node, determining whether the current expansion node is a leaf node, if not, dividing the area corresponding to the current expansion node into multiple child nodes according to the quadtree partitioning method, and setting each child node as the current expansion node, returning to the step of determining whether the current expansion node is a leaf node, and if so, ending the judgment operation on the current expansion node. For example, after this embodiment ends the judgment operation on the current expansion node, it selects a node from multiple nodes that have not undergone the judgment operation as the current expansion node, and returns to the step of determining whether the current expansion node is a leaf node.

[0073] Setting the root node as the current expansion node includes configuring expansion level information and root location information for the root node. Configuring the expansion level information for the root node includes initializing the expansion level information to 0, adding the expansion level information to the natural number 1, and obtaining the expansion level information for the root node. The quadtree map is configured with a coordinate system, and configuring the root location information for the root node includes setting the origin of the coordinate system as the root location information for the root node.

[0074] For example, initialize the root node of the quadtree map as root_. Define the root node of the quadtree as root_. Define the extended level information node_id and initialize the extended level information node_id to 0. Set the id_ of root_ to node_id, and then add node_id to the natural number 1 to obtain the extended level information of the root node. The extended level information node_id is a global variable. The x_ and y_ coordinates of root_ are taken as 0, the width w_ of the root node is 2^level, and root_ is represented as the current extended node. Here, level is the number of layers of the quadtree map, and the value of level is: 2^level >= max_size and 2^(level - 1) < max_size, where max_size is the maximum value of the width and height of the original map.

[0075] Please refer to Figure 5a , the original map is configured with the root node Root, and in this embodiment, the root node Root is set as the current extended node. Since the current extended node is a mixed node, and a mixed node is a node that contains at least two grid states. For example, the root node Root contains free grids and occupied grids.

[0076] Please refer to Figure 5b , in this embodiment, according to the quadtree splitting method, the area corresponding to the root node Root (the current extended node) is split into 4 child nodes. The 4 child nodes are node A, node B, node C, and node D respectively. Node A and node B are both mixed nodes. Node C is an occupied leaf node, that is, all the grids contained in node C are occupied grids. Node D is a free leaf node, that is, all the grids contained in node D are free grids. Since node C and node D are both leaf nodes, in this embodiment, node C or node D will not be split further according to the quadtree splitting method.

[0077] In this embodiment, node A and node B are sequentially taken as the current extended node. For each current extended node, in this embodiment, it is necessary to return to the step of judging whether the current extended node is a leaf node.

[0078] Please refer to Figure 5c , taking node A as an example, since node A is a mixed node, that is, node A is not a leaf node, in this embodiment, according to the quadtree splitting method, the area corresponding to node A (the current extended node) is split into 4 child nodes. The 4 child nodes are node E, node F, node G, and node H respectively. Node E, node F, and node G are all free leaf nodes, and node H is an occupied leaf node. Since node E, node F, node G, and node H are all leaf nodes, in this embodiment, node E, node F, node G, and node H will not be split further according to the quadtree splitting method.

[0079] Similarly, taking node B as an example, since node B is a mixed node, that is, node B is not a leaf node, this embodiment divides the area corresponding to node B (the current expansion node) into 4 child nodes according to the quadtree segmentation method. The 4 child nodes are node I, node J, node K and node L. Node I and node L are both idle leaf nodes, node J is an occupied leaf node, and node K is a mixed node. Since node I, node J and node L are all leaf nodes, this embodiment will not continue to segment nodes I, node J and node L according to the quadtree segmentation method. However, node K is a mixed node, and this embodiment also needs to use the quadtree segmentation method to segment the area corresponding to node K into 4 child nodes, which will not be repeated here.

[0080] See also Figure 5d In general, the root node Root can be divided into node A, node B, node C and node D according to the quadtree partitioning method. Then node A and node B are mixed nodes, and node C and node D are leaf nodes. Therefore, node A can be divided into node E, node F, node G and node H according to the quadtree partitioning method, and node B can be divided into node I, node J, node K and node L according to the quadtree partitioning method.

[0081] After dividing the area corresponding to the current expansion node into a plurality of child nodes in a quadtree partitioning manner, the method further includes: configuring child position information and child level information for each child node.

[0082] Configuring child position information for each child node includes: calculating the child position information of the child node according to the position of the child node within the current expanded node. Configuring child level information for each child node includes: adding a natural number 1 to the current expanded level information to obtain the child level information.

[0083] For example, this embodiment defines the following variables:

[0084] Id_: used to describe the id of each node.

[0085] x_: represents the x coordinate of the lower left corner of each node in the original map.

[0086] y_: represents the y coordinate of the lower left corner of each node in the original map.

[0087] w_: represents the width of each node.

[0088] nw_: indicates the child node at the bottom left of the current expansion node. Figure 5b In the figure, the lower left child node nw_ is node C.

[0089] ne_: indicates the child node at the bottom right of the current expanded node. Figure 5bIn the figure, the lower right child node ne_ is node D.

[0090] sw_: indicates the child node on the upper left of the current expanded node. Figure 5b In the figure, the upper left child node sw_ is node A.

[0091] se_: represents the child node to the upper right of the current expanded node. Figure 5b In the figure, the upper right child node se_ is node B.

[0092] The id_ of the lower left child node nw_ is assigned to the child's node_id, which is then incremented by 1. The child's position information is as follows: the x_ of the lower left child node nw_ is assigned to the x_ of the current expanded node, the y_ is assigned to the y_ of the current expanded node, and the width w_ is assigned to the w_ / 2 of the current expanded node.

[0093] The id_ of the child node ne_ in the lower right corner is assigned the node_id of the child, which is then incremented by 1. The child's position information is as follows: the x_ of the child node ne_ in the lower right corner is assigned the value of the current expanded node's x_ + the current expanded node's w_ / 2, the y_ is assigned the value of the current expanded node's y_, and the width w_ is assigned half of the current expanded node's w_.

[0094] The id_ of the top-left child node sw_ is assigned the node_id of the child hierarchy, and then the node_id of the child hierarchy is incremented by 1. The child's position information is: the x_ of the top-left child node sw_ is assigned the x_ of the current expanded node, the y_ is assigned the y_ of the current expanded node + the w_ / 2 of the current node, and the width w_ is assigned half of the w_ of the current expanded node.

[0095] The id_ of the top-right child node se_ is assigned the node_id of the child hierarchy, and then the node_id of the child hierarchy is incremented by 1. The child's position information is: the x_ of the top-right child node se_ is assigned the value of the current node's x_ + the current expanded node's w_ / 2, the y_ is assigned the value of the current node's y_ + the current node's w_ / 2, and the width w_ is assigned half of the current expanded node's w_.

[0096] This embodiment decomposes all nodes in the quadtree map in the above manner until there are no more decomposable nodes in the entire quadtree map, and the construction of the quadtree map is completed.

[0097] In some embodiments, the path planning method based on the quadtree map further includes: detecting whether a node in the quadtree map is an occupied leaf node or an unknown leaf node, and deleting the occupied leaf node or the unknown leaf node. For example, if the lower left child node nw_ is a leaf node and is not an idle leaf node, it means that the lower left child node nw_ is an occupied leaf node or an unknown leaf node. Since the lower left child node nw_ is not helpful for path planning, this embodiment can delete the lower left child node nw_ to avoid memory usage, thereby improving path planning efficiency.

[0098] To facilitate online path planning, this embodiment defines the following variables before describing the specific operations of path planning:

[0099] Map_cost_: represents the cost of the node in the original map, where the cost is the average of all grid costs in the node.

[0100] f_cost: represents the distance cost between the current node and the starting point during the expansion of the potential field.

[0101] g_cost: represents the distance cost between the current node and the target point during the expansion of the potential field.

[0102] total_cost: The sum of f_cost and g_cost, which represents the total cost of the current node during the potential field expansion process.

[0103] closed_set: Indicates whether the current node is in a closed set during the potential field expansion process. True indicates that it is in a closed set.

[0104] father_node: represents the parent node of the current node during the potential field expansion process.

[0105] Before performing the first potential field expansion operation, this embodiment performs the following steps:

[0106] (1) Initialization: Initialize the path search process. Define the priority queue queue, which is used to store nodes, and the priority queue queue can be sorted in ascending order according to the total_cost of each node (the first element in the priority queue queue is the node with the smallest total_cost). During the path search process, first set the path search cost f_cost of the node start_node (starting point) to 0, and put the node start_node into the queue queue. Define the cycle count variable cycle and initialize it to 0. Define the cycle count threshold as cycle_threshold and initialize it to the number of quadtree nodes n.

[0107] (2) If the priority queue queue is not empty and the cycle is less than the cycle_threshold, the value of the cycle is increased by 1 and the following steps are executed

[0108] (3) Assign the first element in the priority queue queue to the variable top. Also, remove the first element from the priority queue queue. If the variable closed_cost of top is true, it means that top is in the closed set (meaning the shortest path connecting the starting point start_node and top has been found), then return to step (2). Otherwise, set top's closed_cost to false and proceed to step (4).

[0109] (4) If top is equal to the node goal_node, it means that the shortest path connecting the starting point start_node and the target point goal_node has been found, then the loop ends and enters the path connection step six, otherwise it enters the step of determining at least one neighbor node adjacent to the center node.

[0110] In some embodiments, among at least one neighbor node adjacent to the central node, determining a neighbor node that meets preset constraints as the central node for the next potential field expansion operation includes the following steps: determining at least one neighbor node adjacent to the central node, calculating the distance search cost extending from the central node to the neighbor node, if the distance search cost of the target neighbor node is the minimum distance cost among the distance search costs of multiple neighbor nodes, then determining that the target neighbor node meets the preset constraints, the target neighbor node is one of the multiple neighbor nodes, setting the target neighbor node as the central node for the next potential field expansion operation, if the distance search cost of the target neighbor node is not the minimum distance cost among the distance search costs of multiple neighbor nodes, then determining that the target neighbor node does not meet the preset constraints, and selecting another neighbor node from the multiple neighbor nodes as the target neighbor node.

[0111] In some embodiments, before calculating the distance search cost extending from the central node to the neighboring node, the method also includes: determining whether the target neighboring node is in a preset closed set; if so, selecting another neighboring node from multiple neighboring nodes as the target neighboring node; if not, entering the calculation of the distance search cost extending from the central node to the neighboring node.

[0112] Calculating the distance search cost extending from the central node to the neighboring nodes includes: calculating the first distance cost from the central node to the starting point, calculating the second distance cost from the central node to the neighboring nodes, determining the third cost of the neighboring nodes on the original map, and adding the first distance cost, the second distance cost and the third cost to obtain the distance search cost.

[0113] For example, this embodiment calculates the distance search cost from the center node to neighbor nodes according to the following formula: top_vj_cost = top.f_cost + dis + vj.map_cost, where top_vj_cost is the distance search cost from the center node to the neighbor nodes, top.f_cost is the first distance cost from the center node to the starting point, dis is the second distance cost from the center node to the neighbor nodes, and vj.map_cost is the third cost of the neighbor nodes on the original map. The second distance cost can be calculated using the Euclidean distance algorithm or the Manhattan distance algorithm.

[0114] If top_vj_cost is less than the f_cost of neighbor node vj, it means that the neighbor node vj can obtain a smaller f_cost by expanding from the central node top to the neighbor node vj. In this case, the f_cost of vj is set to top_vj_cost. At the same time, this embodiment calculates the distance cost between neighbor node vj and the goal_node of the target point and assigns it to the g_cost of neighbor node vj. The total_cost of neighbor node vj is updated to vj.total_cost = vj.f_cost + vj.g_cost. The parent node of neighbor node vj is set to top, and the neighbor node vj is placed in the priority queue queue.

[0115] Based on the potential field expansion operation of the above embodiment, this embodiment can obtain multiple path points connecting start_node and goal_node. That is, after the above potential field expansion operation, the shortest path connecting start_node and goal_node has been found. Generating a path based on the starting point, multiple path points and the target point includes the following steps: defining the final path as P, first placing the target point goal_pose into P, then placing the target point goal_node into P, using the target point goal_node as the current node, and looping through the following operations: taking the parent node j of the current node i, placing the x and y coordinates of node j into P, and using node j as the current node, continuing the loop until the parent node is start_node, and finally placing the coordinates of the starting point start_pose into P, and rearranging all the path points contained in P in reverse order to obtain the path.

[0116] In some embodiments, the quadtree map is configured with an original grid, and determining at least one neighbor node adjacent to the central node includes the following steps: determining a target idle grid adjacent to the central node, the target idle grid being an original grid that is in an idle state and adjacent to the central node, and determining at least one neighbor node based on at least one target idle grid.

[0117] In some embodiments, the central node is in a regular shape such as a rectangle or a square.

[0118] Determining the target idle grids adjacent to the central node includes the following steps: respectively determining the first idle grid set, the second idle grid set, the third idle grid set, and the fourth idle grid set adjacent to the four sides of the central node, each idle grid set including at least one target idle grid, respectively determining the first diagonal idle grid, the second diagonal idle grid, the third diagonal idle grid, and the fourth diagonal idle grid in the diagonal direction of the central node and adjacent to the four vertices of the central node, the first diagonal idle grid, the second diagonal idle grid, the third diagonal idle grid, and the fourth diagonal idle grid are all target idle grids.

[0119] See also Figure 6 , the shape of the central node is a rectangle, where Figure 6 As shown, the central node is represented by a white idle grid, and the target idle grids are all represented by light gray or dark gray idle grids.

[0120] The first idle grid set is a set of at least one target idle grid adjacent to the bottom of the central node, such as Figure 6 As shown, the first set of free grids includes grids 1 / 2 / 3 / 4 / 5 / 6.

[0121] This embodiment configures corresponding location information for each target idle grid in the first idle grid set. For example, the location information of each target idle grid in the first idle grid set is (top.x+i+0.5, top.y-0.5), top.x is the horizontal coordinate of the center node, top.y is the vertical coordinate of the center node, and the coordinates of the lower left corner of the center node are (top.x, top.y). Usually, the width and height of the original grid of the original map are both 1, and 0.5 is half the width or height of the original grid. i is used to represent the order of the original grid in the corresponding idle grid set. The value of i is an integer, the maximum value of i is top.w, and the minimum value is 0.

[0122] In this embodiment, according to the coordinates (top.x+i+0.5, top.y-0.5), from the value i=0 to top.w, the coordinates (top.x+i+0.5, top.y-0.5) are put into the neighbor array neighbor_xy, and the first free grid set can also be obtained.

[0123] The second idle grid set is a set of at least one target idle grid adjacent to the upper side of the central node, such as Figure 6 As shown, the second set of free grids includes grids 7 / 8 / 9 / 10 / 11 / 12.

[0124] In this embodiment, corresponding position information is configured for each target idle grid in the second idle grid set. For example, the position information of each target idle grid in the second idle grid set is (top.x+i+0.5, top.y+top.w+0.5), where top.w is the width of the center node.

[0125] In this embodiment, according to the coordinates (top.x+i+0.5, top.y+top.w+0.5), from the value i=0 to top.w, the coordinates (top.x+i+0.5, top.y+top.w+0.5) are put into the neighbor array neighbor_xy, and a second free grid set can also be obtained.

[0126] The third idle grid set is a set of at least one target idle grid adjacent to the left side of the central node, such as Figure 6 As shown, the third set of free grids includes grids 13 / 14 / 15 / 16 / 17 / 18.

[0127] In this embodiment, corresponding position information is configured for each target idle grid in the third idle grid set. For example, the position information of each target idle grid in the third idle grid set is (top.x-0.5, top.y+i+0.5).

[0128] In this embodiment, according to the coordinates (top.x-0.5, top.y+i+0.5), from the value i=0 to top.w, the coordinates (top.x-0.5, top.y+i+0.5) are put into the neighbor array neighbor_xy, and the third free grid set can also be obtained.

[0129] The fourth idle grid set is a set of at least one target idle grid adjacent to the right side of the central node, such as Figure 6 As shown, the fourth set of free grids includes grids 19 / 20 / 21 / 22 / 23 / 24.

[0130] In this embodiment, corresponding position information is configured for each target idle grid in the fourth idle grid set. For example, the position information of each target idle grid in the fourth idle grid set is (top.x+top.w+0.5, top.y+i+0.5).

[0131] In this embodiment, according to the coordinates (top.x+top.w+0.5, top.y+i+0.5), from the value i=0 to top.w, the coordinates (top.x+top.w+0.5, top.y+i+0.5) are put into the neighbor array neighbor_xy, and the third free grid set can also be obtained.

[0132] The first diagonal free grid is the free grid adjacent to the center node and at the lower left corner of the center node, such as Figure 6 As shown, the first diagonal free grid is grid 25. In this embodiment, corresponding location information is configured for each target free grid in the first diagonal free grid. For example, if the location information of the first diagonal free grid is (top.x-0.5, top.y-0.5), the coordinates (top.x-0.5, top.y-0.5) are placed in the neighbor array neighbor_xy.

[0133] The second diagonal free grid is the free grid adjacent to the center node and at the lower right corner of the center node, such as Figure 6 As shown, the second diagonal free grid is grid 26. In this embodiment, corresponding location information is configured for each target free grid in the second diagonal free grid. For example, the location information of the second diagonal free grid is (top.x+top.w+0.5, top.y-0.5). The coordinates (top.x+top.w+0.5, top.y-0.5) are placed in the neighbor array neighbor_xy.

[0134] The third diagonal free grid is the free grid adjacent to the center node and at the upper right corner of the center node, such as Figure 6 As shown, the third diagonal free grid is grid 27. For example, the position information of the third diagonal free grid is (top.x+top.w+0.5, top.y+top.w+0.5), and the coordinates (top.x+top.w+0.5, top.y+top.w+0.5) are put into the neighbor array neighbor_xy.

[0135] The fourth diagonal free grid is the free grid adjacent to the central node and at the upper left corner of the central node, such as Figure 6 As shown, the fourth diagonal free grid is grid 28. For example, the position information of the fourth diagonal free grid is (top.x-0.5, top.y+top.w+0.5), and the coordinates (top.x-0.5, top.y+top.w+0.5) are put into the neighbor array neighbor_xy.

[0136] In some embodiments, determining at least one neighbor node according to at least one target idle grid includes the following steps: determining a target idle leaf node including the target idle grid, and setting the target idle leaf node as a neighbor node.

[0137] Determining a target idle leaf node containing a target idle grid includes the following steps: determining a target position of the target idle grid in a quadtree map, traversing the quadtree map to find an idle leaf node containing the target position, and the idle leaf node containing the target position is the target idle leaf node.

[0138] Traversing the quadtree map to find an idle leaf node containing a target position includes the following steps: if the horizontal coordinate of the target position is greater than or equal to the horizontal coordinate of the idle leaf node and is less than the sum of the horizontal coordinate of the idle leaf node and the width of the idle leaf node, and the vertical coordinate of the target position is greater than or equal to the vertical coordinate of the idle leaf node and is less than the sum of the vertical coordinate of the idle leaf node and the height of the idle leaf node, then it is determined that the idle leaf node contains the target position.

[0139] For example, this embodiment determines the target idle leaf node including the target idle grid as follows:

[0140] (1) Take the root node of the quadtree map as the current node and go to step (2);

[0141] (2) For the current node, if the current node is an idle leaf node and the target position is within the range of the current node, the target position is considered to be within the current node and the judgment ends; otherwise: for the current node; traverse the child nodes of the current node, for any child node i, if the target position is within the range of node i, then node i is regarded as the current node and return to step (2).

[0142] (3) Step (2) is a recursive process. If no free leaf node is found in the end so that the target position is within the range of the node, it means that the input target position coordinates are incorrect and an error is returned.

[0143] Overall, this embodiment enables online path search based on a quadtree map, based on the starting and destination points. Because the grid cells in a quadtree map are relatively large, and therefore smaller than those in the original map, this embodiment reduces the number of cells that must be searched when planning a path. This improves path planning efficiency, thereby increasing the robot's operational efficiency. Furthermore, compared to traditional dense grid maps, this embodiment can increase path planning speed by an order of magnitude, addressing the slow computational speed inherent in service robots, which is often caused by the low cost and limited computing power of their computing units.

[0144] It should be noted that, in each of the above-mentioned embodiments, there is not necessarily a certain order between the above-mentioned steps. A person skilled in the art can understand, based on the description of the embodiments of this application, that in different embodiments, the above-mentioned steps may have different execution orders, that is, they may be executed in parallel, or may be executed interchangeably, etc.

[0145] As another aspect of the present invention, an embodiment of the present invention provides a path planning device. The path planning device may be a software module comprising a plurality of instructions stored in a memory, which a processor may access and execute to implement the path planning methods described in the various embodiments above.

[0146] In some embodiments, the path planning device can also be constructed from hardware devices. For example, the path planning device can be constructed from one or more chips, and the chips can work in coordination with each other to complete the path planning methods described in the above embodiments. For another example, the path planning device can also be constructed from various logic devices, such as a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA), a single-chip microcomputer, an ARM (Acorn RISC Machine) or other programmable logic device, discrete gate or transistor logic, discrete hardware components, or any combination of these components.

[0147] See also Figure 7 The path planning device 700 includes a map acquisition module 71 , a planning configuration module 72 , a potential field expansion module 73 and a path generation module 74 .

[0148] The map acquisition module 71 is used to acquire a quadtree map, wherein the quadtree map includes idle leaf nodes, which are leaf nodes in an idle state. Each leaf node includes at least one grid, and the states of all grids included in the leaf node are the same.

[0149] The planning configuration module 72 is used to determine the starting point and the target point.

[0150] The potential field expansion module 73 is used to perform multiple potential field expansion operations on the quadtree map according to a preset path planning algorithm, a starting point, and a target point to obtain multiple path points.

[0151] The path generation module 74 is used to generate a target path according to a starting point, a plurality of path points and a target point.

[0152] Among them, the last potential field expansion operation acts on at least one neighbor node adjacent to the central node, and the neighbor node that meets the preset constraints is determined as the central node of the next potential field expansion operation. The central node of the first potential field expansion operation is the starting point, and one of the neighbor nodes of the last potential field expansion operation is the target point. The central node and at least one neighbor node are all idle leaf nodes.

[0153] This embodiment uses a quadtree map to perform online path search based on the starting and destination points. This avoids the unreliability associated with offline path planning. The resulting online path reflects current road conditions in real time, facilitating reliable and safe robot navigation. Furthermore, the quadtree map compresses the state information of each node, eliminating the need for the robot to allocate large amounts of memory to store the quadtree map, thus broadening the robot's applicability.

[0154] In some embodiments, the potential field expansion module 73 is specifically used to: determine at least one neighbor node adjacent to the central node, calculate the distance search cost extending from the central node to the neighbor node, if the distance search cost of the target neighbor node is the minimum distance cost among the distance search costs of multiple neighbor nodes, then determine that the target neighbor node meets the preset constraints, the target neighbor node is a neighbor node among multiple neighbor nodes, and set the target neighbor node as the central node of the next potential field expansion operation.

[0155] In some embodiments, the quadtree map is configured with original grids, and the potential field expansion module 73 is further specifically used to: determine a target idle grid adjacent to the central node, the target idle grid is an original grid that is in an idle state and adjacent to the central node, and determine at least one neighbor node based on at least one target idle grid.

[0156] In some embodiments, the potential field expansion module 73 is further specifically used to: determine a target idle leaf node containing a target idle grid, and set the target idle leaf node as a neighbor node.

[0157] In some embodiments, the potential field expansion module 73 is further specifically used to: determine the target position of the target idle grid in the quadtree map, traverse the quadtree map to find the idle leaf node containing the target position, and the idle leaf node containing the target position is the target idle leaf node.

[0158] In some embodiments, the quadtree map is configured with a coordinate system, and the potential field expansion module 73 is also specifically used to: if the horizontal coordinate of the target position is greater than or equal to the horizontal coordinate of the idle leaf node and is less than the sum of the horizontal coordinate of the idle leaf node and the width of the idle leaf node, and the vertical coordinate of the target position is greater than or equal to the vertical coordinate of the idle leaf node and is less than the sum of the vertical coordinate of the idle leaf node and the height of the idle leaf node, then it is determined that the idle leaf node contains the target position.

[0159] In some embodiments, the potential field expansion module 73 is further specifically used to: respectively determine the first idle grid set, the second idle grid set, the third idle grid set and the fourth idle grid set adjacent to the four sides of the central node, each idle grid set includes at least one target idle grid, and respectively determine the first diagonal idle grid, the second diagonal idle grid, the third diagonal idle grid and the fourth diagonal idle grid in the diagonal direction of the central node and adjacent to the four vertices of the central node, the first diagonal idle grid, the second diagonal idle grid, the third diagonal idle grid and the fourth diagonal idle grid are all target idle grids.

[0160] In some embodiments, the map acquisition module 71 is specifically used to: obtain the original map, the original map is configured with a root node, set the root node as the current expansion node, determine whether the current expansion node is a leaf node, if so, then according to the quadtree splitting method, divide the area corresponding to the current expansion node into multiple child nodes, and set each child node as the current expansion node, return to the step of determining whether the current expansion node is a leaf node, if not, select another node as the current expansion node, and determine whether the current expansion node is a leaf node.

[0161] It should be noted that the above-mentioned path planning device 700 can execute the path planning method provided in the embodiment of this application, and has the corresponding functional modules and beneficial effects of the execution method. For technical details not fully described in the embodiment of the path planning device 700, please refer to the path planning method provided in the embodiment of this application.

[0162] See also Figure 8 , Figure 8 Schematic diagram of a robot according to an embodiment of the present invention. The robot includes one or more processors 81 and a memory 82. The memory 82 is connected to the one or more processors 81, for example, via a bus.

[0163] The processor 81 is configured to support the robot in executing the corresponding functions of the method in the above method embodiment. The processor can be a central processing unit (CPU), a network processor (NP), a hardware chip, or any combination thereof. The hardware chip can be an application specific integrated circuit (ASIC), a programmable logic device (PLD), or a combination thereof. The PLD can be a complex programmable logic device (CPLD), a field-programmable gate array (FPGA), a generic array logic (GAL), or any combination thereof.

[0164] The memory 82 is used to store program code, etc. The memory 82 may include volatile memory (VM), such as random access memory (RAM); non-volatile memory (NVM), such as read-only memory (ROM), flash memory, hard disk drive (HDD), or solid-state drive (SSD); or a combination of the aforementioned types of memory.

[0165] Memory 82 can be used to store non-volatile software programs, non-volatile computer executable programs, and modules, such as the program instructions / modules corresponding to the path planning method in the embodiments of the present application. The processor executes the non-volatile software programs, instructions, and modules stored in memory to perform various functional applications and data processing of the path planning method and path planning device, thereby implementing the functions of the path planning method and the various modules or units of the path planning device provided in the above-mentioned method embodiments.

[0166] The memory may include a program storage area and a data storage area. The program storage area may store an operating system and application programs required for at least one function. The data storage area may store data generated based on the use of the path planning device. In some embodiments, the memory may optionally include a memory remote from the processor, and such remote memory may be connected to the path planning device via a network. Examples of such networks include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0167] The one or more modules are stored in the memory, and when executed by the one or more processors, execute the path planning device in any of the above method embodiments, for example, execute the method steps described in the above method embodiments, and realize the functions of the modules described in the above device embodiments.

[0168] An embodiment of the present application further provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, wherein the computer program includes program instructions, and when the program instructions are executed by a computer, the computer executes the method as described in the above embodiment.

[0169] Those skilled in the art will appreciate that all or part of the processes in the above-described method embodiments can be implemented by instructing related hardware through a computer program. The program can be stored in a computer-readable storage medium, and when executed, the program can include the processes in the above-described method embodiments. The storage medium can be a magnetic disk, an optical disk, a read-only memory (ROM), or a random access memory (RAM).

[0170] The above disclosure is only a preferred embodiment of the present application, and certainly cannot be used to limit the scope of rights of the present application. Therefore, equivalent changes made according to the claims of the present application are still within the scope covered by the present application.

Claims

1. A path planning method, characterized in that: include: Acquire a quadtree map, wherein the quadtree map includes idle leaf nodes, and the idle leaf nodes are leaf nodes in an idle state; Determine the starting point and destination point; According to a preset path planning algorithm, the starting point and the target point, multiple potential field expansion operations are performed on the quadtree map to obtain multiple path points; generating a target path according to the starting point, the plurality of path points and the target point; Among them, the last potential field expansion operation acts on at least one neighbor node adjacent to the central node, and the neighbor node that meets the preset constraints is determined as the central node of the next potential field expansion operation. The central node of the first potential field expansion operation is the starting point, and one of the neighbor nodes of the last potential field expansion operation is the target point. The central node and at least one of the neighbor nodes are both idle leaf nodes.

2. The method according to claim 1, characterized in that The last potential field expansion operation acts on at least one neighboring node adjacent to the central node, and determining the neighboring node that meets the preset constraint condition as the central node for the next potential field expansion operation includes: Determine at least one neighbor node adjacent to the central node; Calculating a distance search cost extending from the central node to the neighboring nodes; If the distance search cost of the target neighbor node is the minimum distance cost among the distance search costs of the multiple neighbor nodes, it is determined that the target neighbor node meets the preset constraint condition, and the target neighbor node is a neighbor node among the multiple neighbor nodes; The target neighbor node is set as the central node of the next potential field expansion operation.

3. The method according to claim 2, characterized in that The quadtree map is configured with an original grid, and determining at least one neighbor node adjacent to the central node includes: Determine a target idle grid adjacent to the central node, where the target idle grid is an original grid that is in an idle state and adjacent to the central node; At least one neighbor node is determined according to at least one target idle grid.

4. The method according to claim 3, characterized in that Determining at least one neighbor node according to at least one target idle grid includes: Determining a target idle leaf node including the target idle grid; The target idle leaf node is set to the at least one neighbor node.

5. The method according to claim 4, characterized in that Determining the target idle leaf node including the target idle grid includes: Determine a target position of the target idle grid in the quadtree map; The quadtree map is traversed to find an idle leaf node containing the target position, and the idle leaf node containing the target position is the target idle leaf node.

6. The method according to claim 5, characterized in that The quadtree map is configured with a coordinate system, and traversing the quadtree map to find an idle leaf node containing the target location includes: If the horizontal coordinate of the target position is greater than or equal to the horizontal coordinate of the idle leaf node and less than the sum of the horizontal coordinate of the idle leaf node and the width of the idle leaf node, If the vertical coordinate of the target position is greater than or equal to the vertical coordinate of the idle leaf node and is less than the sum of the vertical coordinate of the idle leaf node and the height of the idle leaf node, it is determined that the idle leaf node contains the target position.

7. The method according to claim 3, characterized in that The determining of the target idle grid adjacent to the central node comprises: Determine respectively a first idle grid set, a second idle grid set, a third idle grid set, and a fourth idle grid set adjacent to four sides of the central node, each idle grid set including at least one target idle grid; The first diagonal idle grid, the second diagonal idle grid, the third diagonal idle grid and the fourth diagonal idle grid that are in the diagonal direction of the central node and adjacent to the four vertices of the central node are respectively determined, and the first diagonal idle grid, the second diagonal idle grid, the third diagonal idle grid and the fourth diagonal idle grid are all target idle grids.

8. The method according to any one of claims 1 to 7, characterized in that The obtaining of the quadtree map comprises: Obtaining an original map, wherein the original map is configured with a root node; Set the root node as the current expansion node; Determine whether the current expansion node is a leaf node; If not, then divide the area corresponding to the current expansion node into multiple child nodes according to the quadtree partitioning method, and set each child node as the current expansion node, and return to the step of determining whether the current expansion node is a leaf node; If so, the determination operation on the current expansion node ends.

9. A robot, characterized in that: The robot comprises a memory and a processor, wherein the memory is connected to the processor, and the processor is used to execute one or more computer programs stored in the memory. When the processor executes the one or more computer programs, the robot implements the method according to any one of claims 1 to 8.

10. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a computer program, wherein the computer program includes program instructions. When the program instructions are executed by a processor, the processor is caused to perform the method according to any one of claims 1 to 8.

Images