Robot navigation method, storage medium, program product and robot

Abstract

The invention provides a robot navigation method, a storage medium, a program product and a robot. The navigation method for the robot comprises the following steps: a distance conversion step: carrying out distance conversion on a map; in the pre-planning step, the heuristic cost is calculated based on the heuristic distance, path pre-planning is carried out in the mode that the node with the small heuristic cost is selected each time, and the heuristic cost represents the shortest path from the current node to the target node plus the heuristic distance; a visual detection step: carrying out visual detection by using a line-of-sight algorithm so as to update the local father node and the remote father node; an updating step: updating the remote father node by adopting a lazy updating mode; a proceeding step: after the pre-planning is completed, proceeding from the starting node to the target node and arriving at the target node along the remote father node, and if an unknown obstacle is encountered on the way, carrying out re-planning; and an iteration step: repeating the visual detection step, the updating step and the advancing step until the target position is reached.

Description

Technical Field

[0001] The present invention relates to the field of industrial robots, and in particular to a robot navigation method, a storage medium, a program product and a robot. Background Art

[0002] Robot autonomous navigation and intelligent obstacle avoidance are key components of modern robotics technology, which enable robots to self-locate, plan paths and avoid obstacles in unknown or dynamically changing environments to reach a predetermined target location. This technology has a wide range of applications in service robots, industrial automation, unmanned vehicles and other fields. The core of robot autonomous navigation is the robot's simultaneous localization and mapping technology (SLAM), which allows the robot to determine its own position while building a map in an unknown environment. On this basis, the robot completes the autonomous navigation task through path planning methods, including a global path planning method that finds the best path from the starting point to the end point in a known environment, and a local path planning method that adjusts the path based on real-time perception information to cope with unknown obstacles or dynamically changing environments.

[0003] The D* algorithm is an existing path planning algorithm that can quickly update the path without recalculating from scratch, especially when the obstacles or traversable areas in the environment change. Compared with the original A* algorithm, D* uses previously calculated information to quickly update the path, which is more efficient when dealing with environmental changes. It searches from the starting point and the end point at the same time, and uses a two-way search strategy to accelerate the path update process. However, the D* algorithm is more difficult to implement than the A* algorithm because it involves more state management and two-way search mechanisms. In large-scale environments, especially when the environment changes frequently, such as changes in the position or number of obstacles, changes in lighting conditions, changes in meteorological conditions, etc., each environmental change requires recalculation of the affected part of the path, which has a high computational cost, and the searched path is not smooth. The actual distance of the path increases, which is not conducive to the operation of the robot entity. Additional data structures and memory space are required to maintain the state information of the nodes. The initial path planning stage takes a long time, and a complete search is required from the target node to establish the initial path. Summary of the invention

[0004] The purpose of the present invention is to provide an efficient and real-time SLAM-based path planning and intelligent obstacle avoidance algorithm in view of the deficiencies in the prior art.

[0005] According to one aspect of the present invention, a navigation method for a robot is provided, which comprises: a distance transformation step (S200), performing distance transformation on a map, and calculating the heuristic distance of each node by making the closer the current node is to an obstacle in the map, the larger its heuristic distance; a pre-planning step (S300), calculating a heuristic path based on the heuristic distance, and performing path pre-planning by selecting a node with a smaller heuristic cost each time, wherein the heuristic path represents the shortest path from the current node to the target node plus the heuristic distance; a visual detection step (S400), using a line of sight algorithm for Visual detection is used to update the local parent node and the remote parent node. The local parent node is the next node that can be reached through the minimum heuristic cost, and the remote parent node is the final visible node of the current node; the update step (S500) uses a lazy update method to update the remote parent node; the moving step (S600) moves from the starting node to the target node after the pre-planning is completed, and reaches the target node along the remote parent node. If an unknown obstacle is encountered on the way, re-planning is performed; the iteration step (S700) repeats the visual detection step, the update step and the moving step until the target position is reached.

[0006] As a preferred solution, the navigation method further includes a node marking step (S100), in which the current node is marked as s and the starting node is marked as S start , and the target node is recorded as S goal .

[0007] As a preferred solution, in the navigation method according to the present invention, in the pre-planning step, the heuristic distance dist(s) is calculated by the following formula: Among them, r represents the threshold of the distance between the current node and the obstacle, Indicates the distance between the current node and the obstacle s, And, the heuristic cost rhs(s) is calculated by the following formula: Among them, s represents the successor node of node s, represents the Euclidean distance from node s to node s′, g(s ) indicates that from the starting node S start The total distance to node s′, dist(s ) represents node s The inspiration distance.

[0008] As a preferred solution, in the navigation method according to the present invention, in the visual detection step, the local parent node lp(s) is updated when calculating the heuristic cost rhs(s) of each node, and the remote parent node rp(s) is updated using the line of sight algorithm. Wherein, lp(s) is calculated by the following formula: , The following formula is used to update the remote parent node rp(s): , Among them, c(rp(s ), s) represents node s The remote parent node rp(s ) to node s, and rhs(rp(s )) represents node s The remote parent node rp(s )’s inspiration cost.

[0009] As a preferred solution, in the navigation method according to the present invention, when rhs(s) = g(s), it indicates that the node is in a consistent state and is passable, otherwise, it indicates that the node is in an inconsistent state and needs to be further updated.

[0010] As a preferred solution, in the navigation method according to the present invention, the distance transformation step further includes the following steps: Initialization step (S210), initializing the distance values ​​of all free area nodes of the map to infinity, and initializing the distance values ​​of obstacle nodes to 0; The first calculation step (S220) traverses all nodes of the map from left to right and from top to bottom, and calculates the minimum distance value among the four nodes to the upper left of the node as the distance value of the middle node; In the second calculation step (S230), all nodes of the map are traversed from bottom to top and from right to left, and the minimum distance value between the four nodes to the right below the node and the one obtained in the first calculation step is calculated as the distance value of the middle node.

[0011] As a preferred solution, in the navigation method according to the present invention, in the update step, a priority queue is used. The priority queue adopts the first-in-first-out principle. When a node is popped out of the priority queue, a line of sight algorithm is used to determine whether the current node and its remote parent node are visible, that is, there are no obstacles blocking it. If the remote parent node is not visible, the remote parent node of node s is set as the local parent node.

[0012] As a preferred solution, in the navigation method according to the present invention, in the updating step, when the surrounding nodes and the parent node of the current node are all in a visible state, it is calculated whether it is closer to the target node through the remote parent of the current node in a visible state; if it is closer to the target node through the remote parent of the current node in a visible state, the remote parent node of the current node is updated.

[0013] As a preferred solution, in the navigation method according to the present invention, in the moving step, when encountering an unknown obstacle on the way, a local distance transformation is performed around the obstacle node and the heuristic distance is updated to replan the path so that the path is away from the unknown obstacle.

[0014] According to a second aspect of the present invention, there is provided a non-transitory storage medium storing a computer program, which can implement the navigation method according to the first aspect of the present invention when executed by a processor.

[0015] According to a third aspect of the present invention, a computer program product is provided. When the computer program is executed by a computer, the computer program performs the navigation method according to the first aspect of the present invention.

[0016] According to a third aspect of the present invention, there is provided an autonomous navigation robot, which executes the navigation method according to the first aspect of the present invention.

[0017] The beneficial effects of the present invention are: first, through the distance transformation step and the specific heuristic distance and heuristic cost calculation method, the path of the robot from the starting node to the target node can be planned more scientifically, thereby improving the rationality and efficiency of path planning; second, the visual detection step uses the line of sight algorithm to update the local parent node and the remote parent node, which helps the robot to better perceive the environment and adjust the direction of travel; third, the lazy update method is used to update the remote parent node, which can save computing resources to a certain extent; fourth, when encountering unknown obstacles in the travel step, re-planning can be performed, so that the path can flexibly avoid obstacles, thereby enhancing the adaptability and reliability of robot navigation; fifth, the navigation method can be implemented and applied in the form of storage media, program products, etc., and can be widely used in robots with autonomous navigation functions to improve the overall navigation performance of the robot.

[0018] Further features of the present invention will become apparent from the following description of exemplary embodiments with reference to the attached drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 is a flow chart showing a navigation method of a robot according to an embodiment of the present invention; Figure 2 is a flow chart illustrating a distance transformation operation according to an embodiment of the present invention; Figure 3is a schematic diagram showing a method of updating a remote parent node according to an embodiment of the present invention; Figure 4 is a flow chart illustrating a line of sight algorithm according to an embodiment of the present invention. DETAILED DESCRIPTION

[0020] The exemplary embodiment is described in detail below with reference to the accompanying drawings. Note that all combinations of features described in the exemplary embodiment are not necessarily essential to the solving means of the present invention.

[0021] The navigation method for the robot of the present invention can be implemented by the processor in the robot executing a computer program stored in the memory of the robot or a program downloaded from the cloud. As an alternative, the robot can also communicate with a server or a charging base station, and the processor in the server or the charging base station executes the computer program stored in the server or the charging base station and feeds back the program execution results to the robot in real time.

[0022] First, refer to Figure 1 A navigation method for a robot according to an embodiment of the present invention is described.

[0023] First, in step S100, node marking is performed, and the current node is s and the starting node is S start , the target node is S goal , then, the processing of the navigation method enters S200.

[0024] In step S200, distance transformation is performed. Distance transformation (DistanceTransform) means that a distance transformation is performed on the map within a range r around the node s, and the node undergoing distance transformation takes the larger heuristic distance as the final heuristic distance. Where s is the detected obstacle node.

[0025] The following will refer to Figure 2 , a distance transformation operation according to an exemplary embodiment of the present invention is described in detail.

[0026] First, in step S210, all free area node distance values ​​of the map are initialized to infinity, and obstacle node distance values ​​are initialized to zero.

[0027] Next, the distance transformation operation enters step S220. In step S220, all nodes of the map are traversed from left to right and from top to bottom, and the minimum distance value among the four nodes to the upper left of the node is calculated as the distance value of the middle node.

[0028] Next, the distance transformation operation enters step S230. In step S230, all nodes of the map are traversed from bottom to top and from right to left, and the minimum distance value between the four nodes below the right of the node and the obtained in step S220 is calculated as the distance value of the middle node, and then the distance transformation operation ends.

[0029] Next, the processing of the navigation method enters step S300.

[0030] In step S300, pre-planning is performed.

[0031] Specifically, the heuristic distance of each node is calculated by the following formula (1), where the closer the node s is to the obstacle in the map, the larger its heuristic distance dist(s) will be. The heuristic distance is used as part of the heuristic cost (heuristic cost) rhs(s). Since the node with the smaller heuristic cost rhs(s) is selected each time to plan the path, the path will maintain a certain distance from the obstacle during pre-planning.

[0032] The inspiration distance dist(s) is calculated by the following formula (1): (1) The shortest path from the current node s to the target node plus the shortest path from the starting node S start The total distance g(s) to node s′ is obtained from the successor node s′ of node s, and the heuristic cost from node s to the target node is rhs(s). The heuristic cost is calculated by the following formula (2): (2) In the above formula (2), represents the cost from node s to s′, Defined as the Euclidean distance between two nodes (node ​​s and node s'); g(s) is the distance from the starting node S start The total distance to node s′. When the heuristic cost rhs(s) = g(s), it means that the node is in a consistent state and is passable. Otherwise, it means that the node is in an inconsistent state and needs further update.

[0033] Assume that p(s) is the local parent node of node s, and rp(s) is the remote parent node of node s. The local parent node lp(s) is obtained by calculating the minimum heuristic distance of the successor node, and the remote parent node rp(s) is obtained by performing the line of sight algorithm on node s and its initial remote parent node rp(s).

[0034] Specifically, refer to Figure 2 Describes how the remote parent node rp(s) is updated.

[0035] Figure 2(a) shows how the remote parent node is updated in the absence of obstacles. Figure 2 As shown in (a), when expanding the nodes around node A4, since the surrounding nodes are all in the neighborhood of node A4, lp(s) and rp(s) are the same. In addition, for node A3's neighbor node B2, node B2 is visible to node A4, so the remote parent node of node B2 is node A4, and the local parent node of node B2 is node A3.

[0036] Figure 2 (b) shows how the remote parent node is updated when there are obstacles between nodes. Figure 2 As shown in (b), since the C1 node and the A4 node are not visible, the remote parent node is set as the local parent node.

[0037] Next, the processing of the navigation method enters step S400. In S400, visual detection is performed using the line of sight algorithm. The core of visual detection is the update of the local parent node lp(s) and the remote parent node rp(s). The local parent node lp(s) is updated when calculating the heuristic cost rhs(s) of each node. The next node that can be reached through the minimum heuristic cost is the local parent node lp(s). The specific expression of the local parent node is the following formula (3): (3) The update method of the remote parent node is as follows (4): (4) Among them, c(rp(s ), s) represents the slave node s The remote parent node rp(s ) to node s, and rhs(rp(s )) represents node s The remote parent node rp(s )’s inspiration cost.

[0038] Next, the processing of the navigation method enters step S500. In step S500, the parent node is updated. The remote parent node rp(s) is the final visible node of node s, and the remote parent node rp(s) is updated by the line of sight algorithm. When the line of sight algorithm is introduced, a lazy update method is adopted, that is, when the current node is set to a consistent state, it is assumed that the surrounding nodes and the parent node of the current node are all in a visible state, and it is calculated whether it is closer to reach this node through the remote parent of the current node in a visible state. If it is closer to reach this node through the remote parent node of the current node, the remote parent node of this node is updated. When a node is popped out from the priority queue U, the line of sight algorithm is used to determine whether the current node and its remote parent node are visible. When the current node and its remote parent node are not visible, the remote parent node of node s is set as the local parent node. Since the initial remote parent node of each node is empty, after the local parent node is updated, if the remote parent node is empty, the remote parent node is set as the local parent node.

[0039] Next, the processing of the navigation method enters step S600. In step S600, after the pre-planning is completed, it is necessary to move from the starting node to the target node. At this time, the target node can be reached along the remote parent node. If an unknown obstacle is encountered on the way, re-planning is required. At this time, a local distance transformation is performed around the obstacle node and the heuristic distance is updated, and then re-planning is performed, and the path will be away from the unknown obstacle.

[0040] Next, the process of the navigation method enters step S700. In step S700, iteration is performed, that is, step S400 (visual detection), step S500 (update parent node) and step S600 (travel) are repeated until the robot reaches the target position.

[0041] Next, we will refer to Figure 4 To describe the specific process of the line of sight algorithm.

[0042] Assume that Bad(x0, y0) means that there is an obstacle in the grid at (x0, y0) or the heuristic distance is not 0, (x0, y0) and (x1, y1) are the coordinates of the two grids respectively, dx and dy are the differences between the horizontal and vertical coordinates of the two, sx and sy represent the steps of the x-axis and y-axis, which represent the difference between the current shorter axis and the longer axis.

[0043] First, determine whether y0 is equal to y1. If y0=y1, return true (i.e., indicating that the target node has been reached), and if y0 is not equal to y1, make f=f+dx.

[0044] Then, determine whether f is greater than dy. When f is less than or equal to dy, determine whether there is an obstacle in the grid at (x0, y0) or the heuristic distance is not 0. If there is an obstacle in the grid at (x0, y0) or the heuristic distance is not 0, return false (i.e., indicating that the target node has not been reached). If there is no obstacle in the grid at (x0, y0) and the heuristic distance is 0, make x0=x0+sx,ff-dy. Then, determine whether f!=0 and there is an obstacle in the grid at (x0, y0) or the heuristic distance is not 0. If so, return false. If not, determine whether dx=0 and there is an obstacle in the grid at (x0, y0) or the heuristic distance is not 0 and there is an obstacle in the grid at (x0, y0-1) or the heuristic distance is not 0. If so, return false. If not, return true.

[0045] In summary, according to the present invention, in a large-scale environment, especially when the environment changes frequently, the computational cost is reduced, so that the searched path is smoother, which is beneficial to the operation of the robot entity. The time required for the initial path planning stage is shortened, and there is no need to conduct a complete search from the target node to establish the initial path, so that path planning and intelligent obstacle avoidance can be performed efficiently and in real time.

[0046] Other embodiments The embodiments of the present invention may also be implemented by reading out and executing computer executable instructions (e.g., one or more programs) recorded on a storage medium (which may also be more completely referred to as a "non-transitory computer-readable storage medium") to perform one or more functions in the above-mentioned embodiments, and / or a computer of a system or device including one or more circuits (e.g., an application-specific integrated circuit (ASIC)) for performing one or more functions in the above-mentioned embodiments, and the embodiments of the present invention may be implemented by, for example, a method in which the computer of the system or device reads out and executes the computer executable instructions from the storage medium to perform one or more functions in the above-mentioned embodiments, and / or controls the one or more circuits to perform one or more functions in the above-mentioned embodiments. The computer may include one or more processors (e.g., a central processing unit (CPU), a microprocessing unit (MPU)), and may include a network of separate computers or separate processors to read out and execute the computer executable instructions. The computer executable instructions may be provided to the computer, for example, from a network or the storage medium. The storage medium may include, for example, one or more of a hard disk, a random access memory (RAM), a read-only memory (ROM), a memory of a distributed computing system, an optical disk (such as a compact disc (CD), a digital versatile disc (DVD), or a Blu-ray Disc (BD)™), a flash memory device, a memory card, and the like.

[0047] The embodiments of the present invention may also be implemented by providing software (program) for executing the functions of the above embodiments to a system or device via a network or various storage media, and a computer or a central processing unit (CPU) or a microprocessing unit (MPU) of the system or device reads and executes the program.

[0048] While the present invention has been described with reference to exemplary embodiments, it is to be understood that the invention is not limited to the disclosed exemplary embodiments.The scope of the following claims is to be accorded the broadest interpretation so as to encompass all such modifications and equivalent structures and functions.

Claims

1. A navigation method for a robot, comprising: A distance transformation step (S200) performs distance transformation on the map, and calculates the heuristic distance of each node by making the closer the current node is to the obstacle in the map, the larger its heuristic distance; A pre-planning step (S300) is to calculate a heuristic path based on the heuristic distance, and to perform path pre-planning by selecting a node with a smaller heuristic cost each time, wherein the heuristic path represents the shortest path from the current node to the target node plus the heuristic distance; A visual detection step (S400) uses a sight line algorithm to perform visual detection to update a local parent node and a remote parent node, wherein the local parent node is the next node reachable through a minimum heuristic cost, and the remote parent node is the final visible node of the current node; An updating step (S500) uses a lazy update method to update the remote parent node; A moving step (S600), after the pre-planning is completed, moving from the starting node to the target node, along the remote parent node to reach the target node, and re-planning is performed if an unknown obstacle is encountered on the way; The iteration step ( S700 ) repeats the visual detection step, the updating step, and the moving step until the target position is reached.

2. The navigation method according to claim 1, wherein: The navigation method further comprises a node marking step (S100), in which the current node is marked as s and the starting node is marked as S start , and the target node is recorded as S goal .

3. The navigation method according to claim 1 or 2, wherein: In the pre-planning step, the heuristic distance dist(s) is calculated by the following formula: Among them, r represents the threshold of the distance between the current node and the obstacle, Indicates the distance between the current node and the obstacle s, And, the heuristic cost rhs(s) is calculated by the following formula: Among them, s represents the successor node of node s, represents the Euclidean distance from node s to node s′, g(s ) indicates that from the starting node S start The total distance to node s′, dist(s ) represents node s The inspiration distance.

4. The navigation method according to claim 3, wherein: In the visual detection step, the local parent node lp(s) is updated when calculating the heuristic cost rhs(s) of each node, and the remote parent node rp(s) is updated using the line of sight algorithm. Wherein, lp(s) is calculated by the following formula: ， The following formula is used to update the remote parent node rp(s): ， Among them, c(rp(s ), s) represents node s The remote parent node rp(s ) to node s, and rhs(rp(s )) represents node s The remote parent node rp(s )’s inspiration cost.

5. The navigation method according to claim 3, wherein: When rhs(s) = g(s), it means that the node is in a consistent state and is passable. Otherwise, it means that the node is in an inconsistent state and needs to be further updated.

6. The navigation method according to claim 1 or 2, wherein: The distance transformation step further comprises the following steps: Initialization step (S210), initializing the distance values ​​of all free area nodes of the map to infinity, and initializing the distance values ​​of obstacle nodes to 0; The first calculation step (S220) traverses all nodes of the map from left to right and from top to bottom, and calculates the minimum distance value among the four nodes to the upper left of the node as the distance value of the middle node; In the second calculation step (S230), all nodes of the map are traversed from bottom to top and from right to left, and the minimum distance value between the four nodes to the right below the node and the one obtained in the first calculation step is calculated as the distance value of the middle node.

7. The navigation method according to claim 1 or 2, wherein: In the update step, a priority queue is used. The priority queue adopts the first-in-first-out principle. When a node is popped out of the priority queue, a line of sight algorithm is used to determine whether the current node and its remote parent node are visible, that is, there are no obstacles blocking it. If the remote parent node is not visible, the remote parent node of node s is set as the local parent node.

8. The navigation method according to claim 1 or 2, wherein: In the updating step, when the surrounding nodes and the parent node of the current node are all in a visible state, it is calculated whether it is closer to the target node through the remote parent of the current node in a visible state. If it is closer to the target node through the remote parent of the current node in a visible state, the remote parent node of the current node is updated.

9. The navigation method according to claim 1 or 2, wherein: In the traveling step, when encountering an unknown obstacle on the way, a local distance transformation is performed around the obstacle node and the heuristic distance is updated to replan the path so that the path is away from the unknown obstacle.

10. A non-transitory storage medium storing a computer program, which, when executed by a processor, can implement the navigation method according to any one of claims 1 to 9.

11. A computer program product, wherein when the computer program is executed by a computer, the computer program performs the navigation method according to any one of claims 1 to 9.

12. A robot capable of autonomous navigation, the robot comprising at least one processor and a storage medium, wherein a computer program is stored on the storage medium, and when the computer program is executed by the at least one processor, the navigation method according to any one of claims 1 to 9 can be implemented.

Images