Path planning method and mobile robot

By setting node expansion thresholds and initializing arrays of Dixtra algorithms, expanding nodes to nodes containing narrow channels solves the problem that Dixtra algorithm cannot plan paths in narrow channels, and the security of mobile robots in narrow areas is realized pass.

CN118936493AActive Publication Date: 2024-11-12GUANGZHOU SHIYUAN ELECTRONICS CO LTD +1
View PDF 6 Cites 0 Cited by

Patent Information

Application Number
CN202310520986.4
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2023-05-09
Publication Date
2024-11-12
Estimated Expiration
2043-05-09

AI Technical Summary

Technical Problem

The Dixtra algorithm cannot effectively perform path planning in a narrow channel environment, resulting in mobile robots being unable to pass through narrow areas safely.

Method used

Path planning is achieved by setting node expansion thresholds and initializing an array of Dixtra algorithms, expanding nodes to nodes containing narrow channels, and filtering out nodes that may collide during the expansion process.

Benefits of technology

In a narrow channel environment, the path planning of mobile robots without collision is realized, avoiding the phenomenon of wall penetration, and expanding the application scenario of Dixtra algorithm.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118936493A_ABST
    Figure CN118936493A_ABST
Patent Text Reader

Abstract

The embodiment of the invention relates to the technical field of mobile robots, and discloses a path planning method and a mobile robot, and the method comprises the steps: firstly obtaining a starting point and an ending point of the movement of the mobile robot, then setting a node expansion threshold value based on the starting point and the ending point, and initializing an array in a Dikstra algorithm, the path planning method comprises the steps that firstly, a node expansion threshold value is obtained, then, node expansion is carried out through a Dikstra algorithm on the basis of the node expansion threshold value and an initialized array to obtain an expanded potential field, and finally, a path is planned on the basis of the expanded potential field. Therefore, path planning of the Dikstra algorithm in the narrow channel environment is realized, and collision-free passing of the mobile robot in the narrow channel is ensured.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The embodiments of the present invention relate to the technical field of mobile robots, and in particular to a path planning method and a mobile robot. Background Art

[0002] In the field of mobile robot technology, path planning is usually based on grid maps. In the grid map, the cost value of each grid varies from 0 to 255, where 0 represents idle, 254 represents occupied, and 255 represents unknown area. Usually, there is no obstacle in the grid with a cost of 0, and the mobile robot can pass freely; there is an obstacle in the grid with a cost of 254, and the mobile robot cannot pass; and the grid with a cost of 253 means that the distance between the obstacle and the mobile robot is within the radius of the inscribed circle of the mobile robot. If there is a narrow area such as a narrow passage, the cost of the grid is usually marked as 253.

[0003] In the process of implementing the embodiments of the present invention, the inventors found that there are at least the following problems in the above-mentioned related technologies: Dijkstra algorithm is a classic graph search path planning algorithm. At present, in the process of expanding nodes, in order to ensure the safety of robot navigation, the algorithm will not expand the grid with a grid cost greater than or equal to 253, which will cause the algorithm to be unable to implement path planning in a narrow channel environment. Summary of the invention

[0004] The embodiments of the present application provide a path planning method and a mobile robot, which can implement the path planning of the Dijkstra algorithm in a narrow channel.

[0005] The purpose of the embodiment of the present invention is achieved through the following technical solutions:

[0006] To solve the above technical problems, in the first aspect, a path planning method is provided in an embodiment of the present invention, which is applied to a mobile robot, and the method includes: obtaining a starting point and an end point of the movement of the mobile robot; based on the starting point and the end point, setting a node expansion threshold, and initializing an array in the Dijkstra algorithm; based on the node expansion threshold and the initialized array, performing node expansion through the Dijkstra algorithm to obtain an expanded potential field, wherein the expanded nodes include nodes located in narrow channels; and planning a path based on the expanded potential field.

[0007] In some embodiments, setting a node expansion threshold based on the starting point and the end point includes: obtaining cost values ​​of the starting point and the end point in a grid map; determining whether the cost value of the starting point is greater than the cost value of the end point; if so, setting the cost value of the starting point as the node expansion threshold; if not, setting the cost value of the end point as the node expansion threshold.

[0008] In some embodiments, the array in the Dijkstra algorithm is initialized based on the starting point and the end point, including: initializing a parent node array and a cost array, wherein the parent node array stores a mapping between each node and the parent node, and the cost array stores a cost value of each node, and the cost value of each node in the initialized cost array is positive infinity; based on the starting point and the end point, initializing an open set and a closed set used for searching by the Dijkstra algorithm, wherein the open set stores a mapping between the index value and the cost value of each node, and the closed set stores a mapping between the index value and the potential field value of each grid in the grid map, and one node corresponds to being located in one grid.

[0009] In some embodiments, the open set and closed set used for searching by the Dijkstra algorithm are initialized based on the starting point and the end point, including: initializing each element in the closed set to positive infinity; obtaining the index value of the starting point in the grid map, and setting the cost value corresponding to the index value to a first preset value and saving it in the open set, wherein the elements in the open set are arranged in descending order according to the cost value.

[0010] In some embodiments, the node expansion is performed by Dijkstra algorithm based on the node expansion threshold and the initialized array to obtain the expanded potential field, including: obtaining the first element in the open set and determining whether the cost value corresponding to the first element in the closed set is less than positive infinity; if not, saving the cost value of the first element in the open set to the closed set, and traversing the nodes in eight directions around the first element and performing node expansion; wherein, after saving the first element to the closed set, the open set deletes the first element and obtains a new open set, and for the new first element in the new open set, repeating the steps of determining whether the index value is less than positive infinity and performing node expansion, until the index value of the new first element is the same as the index value of the end point in the grid map, the node expansion is terminated.

[0011] In some embodiments, traversing the nodes in eight directions around the first element and performing node expansion includes: determining whether the index value of the current node to be expanded is less than a first preset value or greater than the number of grids in the grid map minus a second preset value; if not, determining whether the cost value of the current node to be expanded in the closed set is less than positive infinity; if not, determining whether the cost value of the grid where the current node to be expanded is located in the grid map is greater than the node expansion threshold; if not, determining whether the cost values ​​of the grids within the collision detection range of the current node to be expanded are all less than a third preset value; if so, determining that the current node to be expanded is an expandable node, and updating the current node to be expanded and the first element to the array.

[0012] In some embodiments, updating the current node to be expanded and the first element into the array includes: determining whether the cost value of the current node to be expanded is less than or equal to positive infinity; updating the cost threshold to the cost value of the current node to be expanded and saving it into the cost array; saving the index value of the first element as the parent node of the current node to be expanded.

[0013] In some embodiments, the method also includes: if the index value of the current node to be expanded is less than a first preset value or greater than the number of grids in the grid map minus a second preset value, or the cost value of the current node to be expanded in a closed set is less than positive infinity, or the cost value of the grid where the current node to be expanded is located in the grid map is greater than the node expansion threshold, or the cost value of the grid within the collision detection range of the current node to be expanded is greater than or equal to a third preset value, then the current node to be expanded is determined to be an unexpandable node.

[0014] In some embodiments, before determining whether the cost values ​​of the grids within the collision detection range of the current node to be expanded are all less than a third preset value, the method further includes: setting the maximum width of the collision detection box; determining whether the width of the channel where the current node to be expanded is located is greater than the maximum width of the collision detection box; if so, obtaining the coordinates of the grid where the current node to be expanded is located in the grid map; and determining the collision detection range of the current node to be expanded based on the coordinates and the maximum width of the collision detection box.

[0015] In some embodiments, the cost threshold is updated to the cost value of the current node to be expanded and saved in the cost array, including: obtaining the step cost, and determining the cost threshold according to the cost value of the grid where the current node to be expanded is located in the grid map and the proportional coefficient of the cost value of the current node to be expanded in the grid map, as well as the cost value of the first element and the step cost; before determining whether the cost value of the current node to be expanded is less than or equal to positive infinity, the method also includes: determining whether the node expansion threshold is a fourth preset value; if so, setting the proportional coefficient to a value greater than the fifth preset value; if not, setting the proportional coefficient to a sixth preset value.

[0016] In some embodiments, the path planning based on the expanded potential field includes: obtaining a search queue based on the index value in the closed set; initializing the index value of the current path point in the search to the index value of the end point, and saving the coordinates of the grid where the current path point is located to the end of the search queue; determining the parent node corresponding to the current path point according to the parent node array; saving the grid coordinates of the parent node to the end of the search queue, and assigning the index value of the grid where the parent node is located to the current path point to obtain a new index value of the current path point; for the index value of the new current path point, repeating the steps of determining the parent node and assigning the index value of the parent node to the new current path point, until the index value of the new current path point is equal to the index value of the starting point, and a path that can pass through a narrow channel is obtained.

[0017] To solve the above technical problems, in a second aspect, an embodiment of the present invention provides a mobile robot, comprising: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the method described in the first aspect above.

[0018] Compared with the prior art, the beneficial effects of the present invention are as follows: Different from the prior art, a path planning method and a mobile robot are provided in an embodiment of the present invention. The method first obtains the starting point and the end point of the mobile robot's movement, then sets a node expansion threshold based on the starting point and the end point, and initializes an array in the Dijkstra algorithm. Then, based on the node expansion threshold and the initialized array, the node is expanded by the Dijkstra algorithm to obtain an expanded potential field. Finally, a path is planned based on the expanded potential field. The path planning method provided in an embodiment of the present invention expands the nodes to include nodes located in narrow channels during the node expansion process, thereby realizing the path planning of the Dijkstra algorithm in a narrow channel environment and ensuring that the mobile robot passes through the narrow channel without collision. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] One or more embodiments are exemplarily described by pictures in the corresponding drawings, and these exemplified descriptions do not constitute limitations on the embodiments. Elements / modules and steps with the same reference numerals in the drawings are represented as similar elements / modules and steps. Unless otherwise specified, the figures in the drawings do not constitute proportional limitations.

[0020] Figure 1 is a flow chart of a path planning method provided in Embodiment 1 of the present invention;

[0021] Figure 2 yes Figure 1A schematic diagram of a sub-process of step S200 in the path planning method shown;

[0022] Figure 3 yes Figure 1 Another sub-process diagram of step S200 in the path planning method shown;

[0023] Figure 4 yes Figure 3 A schematic diagram of a sub-process of step S260 in the path planning method shown;

[0024] Figure 5 yes Figure 1 A schematic diagram of a sub-process of step S300 in the path planning method shown;

[0025] Figure 6 yes Figure 5 A schematic diagram of a sub-process of step S320 in the path planning method shown;

[0026] Figure 7 yes Figure 6 A schematic diagram of a sub-process of step S324 in the path planning method shown;

[0027] Figure 8 yes Figure 6 A schematic diagram of a sub-process of step S325 in the path planning method shown;

[0028] Fig. 9 yes Figure 1 A schematic diagram of a sub-process of step S400 in the path planning method shown;

[0029] Fig.10 It is a schematic diagram of the hardware structure of a mobile robot provided in Embodiment 2 of the present invention. DETAILED DESCRIPTION

[0030] The present invention is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several variations and improvements may be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0031] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0032] It should be noted that, if there is no conflict, the various features in the embodiments of the present invention can be combined with each other, and all are within the scope of protection of this application. In addition, although the functional modules are divided in the device schematic diagram 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.

[0033] Unless otherwise defined, all technical and scientific terms used in this specification have the same meaning as those commonly understood by those skilled in the art of the present invention. The terms used in this specification are only for the purpose of describing specific embodiments and are not intended to limit the present invention.

[0034] In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0035] In order to solve the problem that the current Dijkstra algorithm cannot plan paths in narrow environments such as narrow channels, which limits its application scenarios, a path planning method is provided in an embodiment of the present invention. In the node expansion process, the method expands the nodes to include nodes located in narrow channels, and filters out nodes that may collide, thereby realizing the path planning of the Dijkstra algorithm in a narrow channel environment. Furthermore, by further judging the cost of the grid where the expanded node is located, it can be ensured that the mobile robot will not collide when passing through the planned narrow channel path, and the planned path will not pass through the wall.

[0036] Specifically, the embodiments of the present invention are further described below in conjunction with the accompanying drawings.

[0037] Embodiment 1

[0038] The present invention provides a path planning method. Figure 1 , which shows the process of a path planning method provided by an embodiment of the present invention. The method can be applied in a mobile robot. The path planning method includes but is not limited to the following steps:

[0039] Step S100: obtaining the starting point and the end point of the movement of the mobile robot;

[0040] In the embodiment of the present invention, first, it is necessary to obtain the starting point of the mobile robot and the end point of the mobile robot. Usually, the starting point is the current position of the mobile robot, and the end point is the target position of the mobile robot. Then, based on the starting point and the end point, the path of the mobile robot moving from the starting point to the end point is planned.

[0041] Step S200: Based on the starting point and the end point, a node expansion threshold is set, and an array in the Dijkstra algorithm is initialized;

[0042] When planning a path, since there may be two situations: a narrow channel and no narrow channel, for the situation where there is a narrow channel, the embodiment of the present invention can handle and save the situation where the node to be expanded is in a narrow channel during the process of expanding the potential field through the Dijkstra algorithm, and then search for the path based on the expanded potential field. Based on this, it is necessary to first set a node expansion threshold to expand the potential field, and initialize the array in the Dijkstra algorithm, so as to expand the node according to the node expansion threshold and update the array.

[0043] Among them, for the given starting point and end point of the movement of the mobile robot, before executing step S200 to set the node expansion threshold and initialize the array in the Dijkstra algorithm, you can first try to use the Dijkstra algorithm for path planning. If the planning is successful, it means that there is no narrow channel, and the path can be directly planned to end the process; if the planning is unsuccessful, it is further determined whether the cost of the grid where the end point is located is less than 253 or equal to 253. If the cost is less than 253, it means that there is a narrow channel somewhere between the starting point or the seven points to the end point. If the cost is equal to 253, it means that the end point is in the narrow channel. Regardless of whether the cost is less than 253 or the cost is equal to 253, it means that a narrow channel may exist. At this time, execute steps S200 to S400 of this application to plan a path in the narrow channel. Among them, in the process of trying to use the Dijkstra algorithm for path planning, the node expansion threshold can be first set to a value less than 253, for example, set to 130. If the cost value of the grid where the node to be expanded is located is less than the node expansion threshold, it means that the mobile robot can pass through the node to be expanded without collision. Specifically, the node expansion threshold can be selected according to actual needs without being restricted to the limitations of the embodiments of the present invention.

[0044] Specifically, on the one hand, the node expansion threshold needs to be initialized, see Figure 2 , which shows Figure 1 A sub-process of step S200 in the path planning method, wherein the node expansion threshold is set based on the starting point and the end point, includes:

[0045] Step S210: Obtain cost values ​​of the starting point and the end point in the grid map;

[0046] Step S220: Determine whether the cost value of the starting point is greater than the cost value of the end point; if so, jump to step S230; if not, jump to step S240;

[0047] Step S230: setting the cost value of the starting point as the node expansion threshold;

[0048] Step S240: setting the cost value of the end point as the node expansion threshold.

[0049] The grid map is composed of a plurality of grids, and the starting point and the end point are located in the grids of the grid map. In an embodiment of the present invention, the node expansion threshold needs to be initialized to the larger one of the cost value of the starting point and the cost value of the end point.

[0050] Furthermore, when obtaining the cost value of the end point in the grid map, it is also necessary to determine whether the cost value of the end point is equal to 254. If it is equal to 254, it means that there is an obstacle at the end point, and the mobile robot cannot reach the end point, and the path planning process ends; if the cost value of the grid where the end point is located is equal to 253 and the end point is in a narrow channel, that is, the end point is located in a narrow area and there is a narrow channel; if the cost value of the grid where the end point is located is less than 253, it means that there is no obstacle at the end point and the end point can be reached.

[0051] On the other hand, the search array for the Dijkstra algorithm needs to be initialized. For details, see Figure 3 , which shows Figure 1 Another sub-process of step S200 in the path planning method shown, initializing an array in the Dijkstra algorithm based on the starting point and the end point, includes:

[0052] Step S250: Initialize the parent node array and the cost array;

[0053] Step S260: Initializing the open set and the closed set used for searching by the Dijkstra algorithm based on the starting point and the end point;

[0054] The cost array of the grid map stores the cost of each grid in the map. The cost value is expressed as 0 to 255. The parent node array stores the mapping of each node to the parent node. The cost array stores the cost value of each node in the potential field expansion process. It should be noted that the cost value in the cost array is not 0 to 255. The cost value of each node in the initialized cost array is positive infinity; the open set stores the mapping of the index value and the cost value of each node, and the closed set stores the mapping of the index value and the cost value of each grid in the grid map. One node corresponds to one grid.

[0055] Specifically, see Figure 4 , which shows Figure 3 A sub-process of step S260 in the path planning method shown, wherein the step of initializing the open set and the closed set for searching by the Dijkstra algorithm based on the starting point and the end point includes:

[0056] Step S261: Initialize each element in the closed set to positive infinity;

[0057] Step S262: obtaining the index value of the starting point in the grid map, setting the cost value corresponding to the index value to a first preset value and saving it in an open set;

[0058] Among them, the present application takes the first preset value of 0 as an example, and the elements in the open set are arranged in descending order according to the cost value, so as to ensure that the cost value of the first element in the queue of the open set remains at the minimum value, thereby completing the initialization of the open set and the closed set, and then judging in turn according to the index order of the elements whether each node in the open set is an expandable node, that is, a path that the mobile robot can pass through, and ending the node expansion when the open set is empty.

[0059] Step S300: Based on the node expansion threshold and the initialized array, perform node expansion by using the Dijkstra algorithm to obtain an expanded potential field;

[0060] Among them, the nodes in the expanded potential field include nodes located in narrow channels; in an embodiment of the present invention, after determining the node expansion threshold and the initialized array, the node expansion can be performed using the Dijkstra algorithm, so as to plan one or more paths from the starting point to the end point based on the expanded potential field, and since the nodes in the expanded potential field include nodes located in narrow channels, the planned paths can also include paths passing through narrow channels.

[0061] See also Figure 5 , which shows Figure 1 A sub-process of step S300 in the path planning method shown, wherein the node expansion is performed by Dijkstra algorithm based on the node expansion threshold and the initialized array to obtain an expanded potential field, includes:

[0062] Step S310: Get the first element in the open set and determine whether the cost value of the first element in the closed set is less than positive infinity; if not, jump to step S320;

[0063] In an embodiment of the present invention, whether the node has been saved in the closed set can be determined by whether the cost value corresponding to the first element in the closed set is less than positive infinity. If it is less than positive infinity, it is determined that the node has been added to the closed set, and the cost value of the node saved in the closed set is the minimum value. The node has been saved in the closed set as an expandable node and no further judgment is required. Since each element is initialized to positive infinity when the closed set is initialized, if it is equal to positive infinity, it means that the element has not been expanded, and a judgment can be made as to whether it is an expandable node. At this time, jump to step S320 to expand the node of the element.

[0064] Step S320: Save the cost value of the first element in the open set in the closed set, and traverse the nodes in eight directions around the first element and perform node expansion;

[0065] In the embodiment of the present invention, before expanding the first element, its cost value is first saved from the open set to the closed set, and then the nodes in the eight directions around the first element are sequentially traversed for expansion, wherein the nodes in the eight directions around the first element are respectively the left node, the right node, the upper node, the lower node, the upper left node, the upper right node, the lower left node, and the lower right node, and the sequential traversal can be performed in a clockwise or counterclockwise manner. Specifically, for the judgment of whether each traversed node can be saved as an expandable node, please refer to Figure 6 , which shows Figure 5 A sub-process of step S320 in the path planning method shown, traversing the nodes in eight directions around the first element and performing node expansion, includes:

[0066] Step S321: determine whether the index value of the node to be expanded is less than a first preset value or greater than the number of grids in the grid map minus a second preset value; if not, jump to step S322; if yes, jump to step S326;

[0067] In the embodiment of the present application, taking the first preset value as 0 and the second preset value as 1 as an example, first, nodes that exceed the boundary of the grid map need to be excluded. Specifically, the index value corresponds to the data corresponding to the coordinate values ​​of each node, and each grid corresponds to the index value of a node, and the maximum index value is equal to the number of grids in the grid map - 1; therefore, if the index value of the current node to be expanded is less than 0, or greater than the number of grids in the grid map minus 1, it is determined that the expanded node exceeds the boundary of the map and is not in the grid map, and jumps to step S326 to determine it as a non-expandable node; if it is greater than or equal to zero and less than or equal to the number of grids in the grid map minus 1, it continues to determine whether the node meets other expansion conditions.

[0068] Step S322: Determine whether the cost value of the node to be expanded in the closed set is less than positive infinity; if not, jump to step S323; if yes, jump to step S326;

[0069] Secondly, after determining that the node is a node in the grid map, it is also necessary to determine whether the node has been saved in the closed set, and exclude the nodes that have been added to the closed set. If the cost value of the node to be expanded in the closed set is less than positive infinity, that is, it is not the initial value, it means that the node has been added to the closed set and does not need to be judged as an expandable node in the next step; if the cost value of the node to be expanded in the closed set is equal to positive infinity, then continue to determine whether the node meets other expansion conditions.

[0070] Step S323: determine whether the cost value of the grid where the node to be expanded is located in the grid map is greater than the node expansion threshold; if not, jump to step S324; if yes, jump to step S326;

[0071] Next, when it is determined that the current node to be expanded is in the grid map and the cost value in the closed set is the initial positive infinity, it is also necessary to judge whether the cost value of the grid where the current node to be expanded is located in the grid map is greater than the node expansion threshold, so as to exclude nodes with too large cost value, too close to obstacles, and the mobile robot is likely to collide or pass through walls.

[0072] Step S324: determine whether the cost values ​​of the grids of the node to be expanded within the collision detection range are all less than the third preset value; if so, jump to step S325; if not, jump to step S326;

[0073] Among them, the third preset value of the present application takes 254 as an example. Then, after determining that the current node to be expanded is in the grid map, and the cost value in the closed set is initially positive infinity, and it is not too close to the obstacle, it is also necessary to determine whether the cost value of the grid within the collision detection range of the current node to be expanded is less than 254, so as to exclude the nodes that are too close to the obstacle in the narrow channel. Specifically, if the cost value of the grid where the expansion node is located is greater than or equal to 254, that is, equal to 254, then the mobile robot will collide or cannot pass when it is located at the node, and cannot be used as an expandable node; if the cost value of the grid where the expansion node is located is less than 254, it is an expandable node that can pass without collision.

[0074] Taking the square detection box as an example, assume that the range of the square collision detection box is:

[0075] x∈[next_x-lower_bound, next_x+upper-bound]

[0076] y∈[next_y-lower_bound, next_y+upper_bound]

[0077] Among them, next_x and next_ are the x and y coordinates of the current node to be expanded in the map. Specifically, any grid (x, y) within the range of the collision detection can be taken, that is, any value in the interval [next_x-lower_bound, next_x+upper-bound] for x and any value in the interval [next_y-lower_bound, next_y+upper_bound] for y, to determine whether the cost value of the grid (x, y) is less than 254. If all grids meet the requirements, it means that when the mobile robot is located at the expansion node, it meets the collision detection constraints and can pass without collision; if only one grid does not meet the requirements, it means that when the mobile robot is located at the expansion node, it is too close to the obstacle and may collide, and the corresponding current node to be expanded is discarded.

[0078] For the determination of the collision detection range, please refer to Figure 7 , which shows Figure 6 In a sub-process of step S324 in the path planning method shown, before determining whether the cost values ​​of the grids of the current node to be expanded within the collision detection range are all less than the third preset value, the method further includes:

[0079] Step S3241: setting the maximum width of the collision detection frame;

[0080] The collision detection frame can be in various shapes such as rectangle, circle, ellipse, polygon, etc., which can be determined according to the actual shape of the robot. The maximum width of the collision detection frame is the maximum width of all straight lines connecting the edges of the detection frame through the geometric center of the collision detection frame. Preferably, it can be a square collision detection frame, in which case the maximum width of the collision detection frame is the side length of the square collision detection frame. In actual applications, the shape of the collision detection frame can be set according to actual needs, without being restricted to the limitations of the embodiments of the present invention.

[0081] Furthermore, the setting of the maximum width of the collision detection frame specifically includes: determining the number of grids corresponding to the minimum width according to the minimum width of the passage through which the mobile robot can pass and the resolution of the grid map, and then setting the maximum width of the collision detection frame to a value less than the number of grids. For example, when the minimum width of the passage through which the mobile robot can pass is 60 cm, and the resolution of the grid map is 0.05 m, that is, 5 cm, the minimum width of the passage through which the mobile robot can pass is divided by the resolution of the grid map, and the number of grids corresponding to the minimum width is 12. Considering the mapping error, a narrow passage of 60 cm may be narrower than 60 cm after mapping, so the maximum width of the collision detection frame can be set to a value less than 12, such as 10. In practical applications, the minimum width of the passage through which the mobile robot can pass is related to factors such as the size and shape of the mobile robot, and the resolution of the grid map is related to factors such as the sensors mounted on the mobile robot, so it can be set according to actual needs, without being restricted to the limitations of the embodiments of the present invention.

[0082] Step S3242: Determine whether the width of the channel where the node to be expanded is located is greater than the maximum width of the collision detection box; if so, jump to step S3243;

[0083] After setting the maximum width of the collision detection frame, it is also necessary to determine whether the channel width where the current node is located is greater than the maximum width of the collision detection frame. If so, it means that the grid in the collision detection frame exists in the channel and can pass through. The node can be expanded and saved in the array of the algorithm; if not, it means that there may be a channel boundary in the collision detection frame, and collision will occur after expansion. At this time, expansion is not allowed. For example, when there are narrow channels with a width of 10 grids and 6 grids in the environment at the same time, if the maximum width of the collision detection frame is set to 11, the widths of the two narrow channels are both less than the maximum width of the collision detection frame, and there is no path that the mobile robot can pass through; if the side length of the collision detection frame is set to 8, the mobile robot can pass through the narrow channel of 10 grids, but cannot pass through the narrow channel of 6 grids; if the maximum width of the collision detection frame is set to 4, the mobile robot can pass through the narrow channel of 10 grids and 6 grids.

[0084] Step S3243: Obtain the coordinates of the grid where the node to be expanded currently is located in the grid map;

[0085] After determining that the current node to be expanded is an expandable node, and before calculating the collision detection range of the current node to be expanded, it is also necessary to obtain the coordinates (next_x, next_y) of the grid where the current node to be expanded is located in the grid map, and determine the coordinates (next_x, next_y) of the expansion node in the grid map as the center coordinates of the collision detection range of the expansion node.

[0086] Step S3244: Determine the collision detection range of the node to be expanded according to the coordinates and the maximum width of the collision detection box.

[0087] After obtaining the coordinates (next_x, next_y) of the grid where the current node to be expanded is located in the grid map, the boundary of the collision detection box is determined according to the maximum width of the collision detection box, and the longest boundary of the collision detection box is less than or equal to the maximum width of the collision detection box.

[0088] For example, taking a square collision detection frame as an example, taking the center coordinates (next_x, next_y) as the base point, the lower limit and upper limit of the collision detection range are determined according to the side length of the square collision detection frame, where:

[0089] The lower limit of the collision detection range satisfies the following formula:

[0090] lower_bound=collission_box_range / 2

[0091] The upper limit of the collision detection range satisfies the following formula:

[0092] upper_bound=collission_box_range / 2+collission_box_range%2-1

[0093] Among them, lower_bound represents the lower limit of the collision detection range, upper_bound represents the upper limit of the collision detection range, and collision_box_range represents the side length of the square collision detection box.

[0094] Selecting the collision detection range with the lower and upper limits set as above can ensure that the side length of the square collision detection box enclosed by the coordinate intervals [next_x-lower_bound, next_x+upper-bound] and [next_y-lower_bound, next_y+upper_bound] is exactly equal to the maximum width of the collision detection box, that is, the side length of the square collision detection box.

[0095] Step S325: determining that the current node to be expanded is an expandable node, and updating the current node to be expanded and the first element into the array;

[0096] Finally, after the judgment from step S321 to step S324, that is, after determining that the current node to be expanded is in the grid map, the cost value in the closed set is the initial positive infinity, the cost value of the grid where the node is located is less than the node expansion threshold, and the cost values ​​of the grids within the collision detection range are all less than 254, it can be determined as an expandable node, and the current node to be expanded and the first element are updated to the array. For details, please refer to the process of saving to the array. Figure 8 , which shows Figure 6 A sub-process of step S325 in the path planning method shown, wherein updating the current node to be expanded and the first element into the array includes:

[0097] Step S3251: Determine whether the cost value of the node to be expanded is less than or equal to positive infinity;

[0098] First, it is necessary to determine whether the cost value of the node to be expanded is less than or equal to positive infinity to determine whether it has been saved in the open set. If it is equal to positive infinity, it means that the node has not been added to the open set, then enter step S3252 and add the node to the open set for the first time; if it is less than or equal to positive infinity, it means that the node has been added to the open set.

[0099] Step S3252: updating the cost threshold to the cost value of the node to be expanded and saving it to the cost array;

[0100] Specifically, the step cost is obtained, and the cost threshold is determined according to the cost value of the grid where the node to be expanded is located in the grid map, the proportional coefficient of the cost value of the node to be expanded in the grid map, the cost value of the first element and the step cost. Furthermore, it is also necessary to determine whether the cost value saved by the node in the open set is the minimum value. Specifically, if the cost value saved by the node in the open set is less than or equal to the cost threshold, it means that the cost value saved in the open set is the minimum cost value; and if the cost value saved in the open set is greater than the cost threshold, it means that a smaller cost is obtained, and the cost threshold is updated to the cost value of the node to be expanded and saved in the cost array. Among them, the expression of the cost value of the node is as follows:

[0101] top.cost+k*map_cost[index]+neural_cost

[0102] Among them, top.cost represents the cost value of the first element, k represents the scale factor, map_cost[index] represents the cost value of the grid where the node to be expanded is located in the grid map, and neural_cost represents the step cost.

[0103] Furthermore, in an embodiment of the present invention, it is also necessary to determine the proportional coefficient of the cost threshold. Before determining whether the cost value of the current node to be expanded is less than or equal to positive infinity, the method further includes: determining whether the node expansion threshold is a fourth preset value; if so, setting the proportional coefficient to a value greater than the fifth preset value, for example, it can be set to 1e6; if not, setting the proportional coefficient to a sixth preset value. In this application, the fourth preset value is 253, the fifth preset value is 100, and the sixth preset value is 1 as an example.

[0104] Step S3253: Save the index value of the first element as the parent node of the current node to be expanded.

[0105] After traversing all nodes in eight directions around each of the first elements through the above steps and determining that there is at least one node that is an expandable node without collision and without penetrating walls, the index value of the first element is saved in the parent node array.

[0106] Step S326: Determine that the node to be expanded is a non-expandable node.

[0107] In an embodiment of the present invention, if step S321 is judged as yes, that is, if the index value of the current node to be expanded is less than 0 or greater than the number of grids in the grid map minus 1; or, if step S322 is judged as yes, that is, the cost value of the current node to be expanded in the closed set is less than positive infinity; or, if step S323 is judged as yes, that is, the cost value of the grid where the current node to be expanded is located in the grid map is greater than the node expansion threshold; or, if step S323 is judged as no, that is, the cost value of the grid within the collision detection range of the current node to be expanded is greater than or equal to 254, then it can be determined that the current node to be expanded is a non-expandable node.

[0108] Step S330: deleting the first element from the open set and obtaining a new open set;

[0109] After completing the node expansion of the first element of the open set, the first element is deleted, thereby updating the new open set, and the second element in the original open set is updated to the new first element in the new open set.

[0110] Step S340: End node expansion when the index value of the new first element is the same as the index value of the end point in the grid map.

[0111] Among them, after the first element is saved in the closed set, the open set deletes the first element and obtains a new open set. For the new first element in the new open set, the steps of determining whether the index value is less than positive infinity and performing node expansion are repeated until the index value of the new first element is the same as the index value of the end point in the grid map, and the node expansion is ended.

[0112] Step S400: planning a path based on the expanded potential field.

[0113] In an embodiment of the present invention, while obtaining the extended potential field, the Dijkstra algorithm can be used to try to plan a path from the starting point to the end point. The path planning algorithm provided by the embodiment of the present invention can combine the situation of the narrow channel for judgment, and obtain a path from the starting point to the end point with the shortest distance and no collision or wall penetration, and the final planned path may be a path that passes through a narrow channel or a path that does not pass through a narrow channel.

[0114] After all the steps from step S100 to step S300 are completed to obtain a closed set of passable nodes and expandable nodes, a path search may be performed based on the closed set of expanded nodes. For details, see Fig. 9 , which shows Figure 1 A sub-process of step S400 in the path planning method shown, wherein the path planning based on the expanded potential field includes:

[0115] Step S410: obtaining a search queue based on the index value in the closed set;

[0116] Step S420: Initialize the index value of the current path point in the search as the index value of the end point, and save the coordinates of the grid where the current path point is located to the end of the search queue;

[0117] Step S430: determining the parent node corresponding to the current path point according to the parent node array;

[0118] Step S440: saving the grid coordinates of the parent node to the end of the search queue, and assigning the index value of the grid where the parent node is located to the current path point to obtain a new index value of the current path point;

[0119] Step S450: When the index value of the new current path point is equal to the index value of the starting point, a path that can pass through a narrow channel is obtained.

[0120] In an embodiment of the present invention, first, a search queue is obtained in sequence according to the size of the index value in the closed set, and then the index value current_index of the current path point in the search queue path is initialized to the index value goal_index of the destination, and the coordinates of the grid where the current path point is located are stored at the end of the search queue path; then, the parent node parent[current_index] corresponding to the current path point current_index is determined according to the parent node array, so that the grid coordinate point where the parent node parent[current_index] is located is placed at the end of the search queue path, and the index value of the grid where the parent node parent[current_index] is located is assigned to the index value current_index of the current path point, and for the new index value of the current path point, the steps of determining the parent node and assigning the index value of the parent node to the new current path point are repeated until the index value current_index of the new current path point is equal to the index value start_index of the starting point, and a path connecting the starting point and the destination is obtained, and when the path point contains a narrow channel, the path is a path that can pass through the narrow channel.

[0121] Embodiment 2

[0122] The present invention also provides a mobile robot. Fig.10 , which shows that it is possible to execute Figures 1 to 9 The hardware structure of the mobile robot of the path planning method.

[0123] The mobile robot 10 includes: at least one processor 11; and a memory 12 in communication with the at least one processor 11. Fig.10 The memory 12 stores instructions that can be executed by the at least one processor 11, and the instructions are executed by the at least one processor 11 so that the at least one processor 11 can perform the above Figures 1 to 9 The processor 11 and the memory 12 may be connected via a bus or other means. Fig.10 The example of connecting through bus is taken in the following.

[0124] The memory 12 is a non-volatile computer-readable storage medium that can be used to store non-volatile software programs, non-volatile computer executable programs and modules, such as program instructions / modules corresponding to the path planning method in the embodiment of the present application. The processor 11 executes various functional applications and data processing of the server by running the non-volatile software programs, instructions and modules stored in the memory 12, that is, the path planning method of the above method embodiment is implemented.

[0125] The memory 12 may include a program storage area and a data storage area, wherein the program storage area may store an operating system and an application required by at least one function; the data storage area may store data created according to the use of the path planning device of the narrow channel, etc. In addition, the memory 12 may include a high-speed random access memory, and may also include a non-volatile memory, such as at least one disk storage device, a flash memory device, or other non-volatile solid-state storage device. In some embodiments, the memory 12 may optionally include a memory remotely arranged relative to the processor 11, and these remote memories may be connected to the path planning device of the narrow channel via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0126] The one or more modules are stored in the memory 12, and when executed by the one or more processors 11, execute the path planning method in any of the above method embodiments, for example, execute the above described Figures 1 to 9 method steps.

[0127] The above-mentioned product can execute the method provided in the embodiment of the present application, and has the functional modules and beneficial effects corresponding to the execution method. For technical details not fully described in this embodiment, please refer to the method provided in the embodiment of the present application.

[0128] The present application also provides a non-volatile computer-readable storage medium, wherein the computer-readable storage medium stores computer-executable instructions, and the computer-executable instructions are executed by one or more processors, for example, to execute the above-described Figures 1 to 9 method steps.

[0129] The present application also provides a computer program product, including a computer program stored on a non-volatile computer-readable storage medium, wherein the computer program includes program instructions, and when the program instructions are executed by a computer, the computer executes the path planning method in any of the above method embodiments, for example, executing the above described Figures 1 to 9 method steps.

[0130] A path planning method and a mobile robot are provided in an embodiment of the present invention. The method first obtains a starting point and an end point of the mobile robot's movement, then sets a node expansion threshold based on the starting point and the end point, and initializes an array in the Dijkstra algorithm. Then, based on the node expansion threshold and the initialized array, the Dijkstra algorithm is used to expand the nodes to obtain an expanded potential field. Finally, a path is planned based on the expanded potential field. The path planning method provided in an embodiment of the present invention expands the nodes to include nodes located in narrow channels during the node expansion process, thereby realizing the path planning of the Dijkstra algorithm in a narrow channel environment and ensuring that the mobile robot passes through the narrow channel without collision.

[0131] It should be noted that the device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0132] Through the description of the above implementation methods, ordinary technicians in this field can clearly understand that each implementation method can be implemented by means of software plus a general hardware platform, and of course, it can also be implemented by hardware. Ordinary technicians in this field can understand that all or part of the processes in the above-mentioned embodiment method can be completed by instructing related hardware through a computer program, and the program can be stored in a computer-readable storage medium. When the program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, the storage medium can be a disk, an optical disk, a read-only memory (ROM) or a random access memory (RAM), etc.

[0133] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them. Under the concept of the present invention, the technical features in the above embodiments or different embodiments may also be combined, the steps may be implemented in any order, and there are many other changes in different aspects of the present invention as described above, which are not provided in detail for the sake of simplicity. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A path planning method, characterized in that: Applied to a mobile robot, the method comprises: Obtaining the starting point and end point of the mobile robot's movement; Based on the starting point and the end point, a node expansion threshold is set, and an array in the Dijkstra algorithm is initialized; Based on the node expansion threshold and the initialized array, performing node expansion by Dijkstra algorithm to obtain an expanded potential field; A path is planned based on the expanded potential field.

2. The path planning method according to claim 1, characterized in that: The step of setting a node expansion threshold based on the starting point and the end point includes: Obtain cost values ​​of the starting point and the end point in the grid map; Determine whether the cost value of the starting point is greater than the cost value of the end point; If so, setting the cost value of the starting point as the node expansion threshold; If not, the cost value of the endpoint is set as the node expansion threshold.

3. The path planning method according to claim 1, characterized in that: Initializing an array in the Dijkstra algorithm based on the starting point and the end point includes: Initialize a parent node array and a cost array, wherein the parent node array stores a mapping between each node and the parent node, and the cost array stores a cost value of each node, and the cost value of each node in the initialized cost array is positive infinity; Based on the starting point and the end point, the open set and the closed set used for searching by the Dijkstra algorithm are initialized, wherein the open set stores the mapping between the index value and the cost value of each node, and the closed set stores the mapping between the index value and the potential field value of each grid in the grid map, and one node corresponds to one grid.

4. The path planning method according to claim 3, characterized in that: Initializing the Dijkstra algorithm for searching an open set and a closed set based on the starting point and the end point includes: Initialize each element in the closed set to positive infinity; The index value of the starting point in the grid map is obtained, and the cost value corresponding to the index value is set to a first preset value and then saved in an open set, wherein the elements in the open set are arranged in descending order according to the cost value.

5. The path planning method according to claim 3, characterized in that: The node expansion is performed based on the node expansion threshold and the initialized array by using the Dijkstra algorithm to obtain an expanded potential field, including: Obtaining the first element in the open set and determining whether the cost value corresponding to the first element in the closed set is less than positive infinity; If not, save the cost value of the first element in the open set in the closed set, and traverse the nodes in eight directions around the first element and perform node expansion; Among them, after the first element is saved in the closed set, the open set deletes the first element and obtains a new open set. For the new first element in the new open set, the steps of determining whether the index value is less than positive infinity and performing node expansion are repeated until the index value of the new first element is the same as the index value of the end point in the grid map, and the node expansion is ended.

6. The path planning method according to claim 5, characterized in that: The traversing the nodes in eight directions around the first element and performing node expansion includes: Determine whether the index value of the node to be expanded is less than a first preset value or greater than the number of grids in the grid map minus a second preset value; If not, determining whether the cost value of the current node to be expanded in the closed set is less than positive infinity; If not, determining whether the cost value of the grid where the node to be expanded currently is located in the grid map is greater than the node expansion threshold; If not, determining whether the cost values ​​of the grids of the node to be expanded within the collision detection range are all less than a third preset value; If so, determine that the current node to be expanded is an expandable node, and update the current node to be expanded and the first element into the array.

7. The path planning method according to claim 6, characterized in that: The updating the current node to be expanded and the first element into the array includes: Determine whether the cost value of the current node to be expanded is less than or equal to positive infinity; Update the cost threshold to the cost value of the node to be expanded and save it in the cost array; The index value of the first element is saved as the parent node of the current node to be expanded.

8. The path planning method according to claim 6, characterized in that: The method further comprises: If the index value of the current node to be expanded is less than a first preset value or greater than the number of grids in the grid map minus a second preset value, or the cost value of the current node to be expanded in a closed set is less than positive infinity, or the cost value of the grid where the current node to be expanded is located in the grid map is greater than the node expansion threshold, or the cost value of the grid within the collision detection range of the current node to be expanded is greater than or equal to a third preset value, then the current node to be expanded is determined to be an unexpandable node.

9. The path planning method according to claim 6, characterized in that: Before determining whether the cost values ​​of the grids of the node to be expanded within the collision detection range are all less than a third preset value, the method further includes: Set the maximum width of the collision detection box; Determine whether the width of the channel where the node to be expanded is located is greater than the maximum width of the collision detection box; If so, obtain the coordinates of the grid where the node to be expanded is located in the grid map; The collision detection range of the node to be expanded is determined according to the coordinates and the maximum width of the collision detection box.

10. The path planning method according to claim 7, characterized in that: The updating of the cost threshold to the cost value of the current node to be expanded and saving it to the cost array includes: Obtaining a step cost, and determining the cost threshold according to the cost value of the grid where the node to be expanded is located in the grid map, the proportional coefficient of the cost value of the node to be expanded in the grid map, the cost value of the first element and the step cost; Before determining whether the cost value of the current node to be expanded is less than or equal to positive infinity, the method further includes: Determining whether the node expansion threshold is a fourth preset value; If so, setting the proportionality factor to a value greater than a fifth preset value; If not, the proportionality factor is set to a sixth preset value.

11. The path planning method according to any one of claims 3 to 10, characterized in that: The planning of a path based on the expanded potential field includes: Based on the index value in the closed set, obtaining a search queue; Initialize the index value of the current path point in the search to the index value of the end point, and save the coordinates of the grid where the current path point is located to the end of the search queue; Determine the parent node corresponding to the current path point according to the parent node array; The grid coordinates of the parent node are saved to the end of the search queue, and the index value of the grid where the parent node is located is assigned to the current path point to obtain a new index value of the current path point; For the index value of the new current path point, repeat the steps of determining the parent node and assigning the index value of the parent node to the new current path point until the index value of the new current path point is equal to the index value of the starting point, thereby obtaining a path that can pass through a narrow channel.

12. A mobile robot, characterized in that: include: at least one processor; as well as, a memory communicatively connected to the at least one processor; wherein, The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to perform the method according to any one of claims 1 to 11.

Citation Information

Patent Citations

  • Robot path planning method in complex narrow environment

    CN110083165A

  • Path planning method and device with obstacle avoidance weight and electronic equipment

    CN110595482A

  • Mobile robot

    CN110621449A

  • Low-energy-consumption mobile robot path planning method

    CN115903816A

  • Global path planning method and mobile robot

    CN118896603A