Path planning method and mobile robot

Abstract

The embodiment of the present application relates to the technical field of mobile robots, and discloses a path planning method and a mobile robot, the method first acquires a starting point and an ending point of movement of the mobile robot, then sets a node expansion threshold based on the starting point and the ending point, and initializes an array in a Dijkstra algorithm, then expands nodes through the Dijkstra algorithm based on the node expansion threshold and the initialized array to obtain an expanded potential field, and finally plans a path based on the expanded potential field, the path planning method provided by the embodiment of the present application expands nodes to contain nodes located in narrow channels in the node expansion process, so that path planning of the Dijkstra algorithm in a narrow channel environment is realized, and the mobile robot is ensured to pass through the narrow channel without collision.

Description

TECHNICAL FIELD

[0001] Embodiments of the present application relate to the technical field of mobile robots, and in particular to a path planning method and a mobile robot. BACKGROUND

[0002] In the technical field of mobile robots, path planning is usually based on a grid map, in which the cost value of each grid ranges from 0 to 255, where 0 represents free, 254 represents occupied, and 255 represents unknown area. Generally, 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 indicates that the distance between the obstacle and the mobile robot is within the range of the inscribed circle radius of the mobile robot. If there is a narrow passage or other narrow area, the cost of the grid will usually be marked as 253.

[0003] In the process of implementing the embodiments of the present application, the inventors found that at least the following problems exist in the above related technologies: Dijkstra algorithm is a classic graph search path planning algorithm, and in the process of extending nodes, in order to ensure the safety of robot navigation, the algorithm will not expand the grid with a cost greater than or equal to 253, which will result in the algorithm being unable to realize path planning in a narrow passage environment. SUMMARY

[0004] Embodiments of the present application provide a path planning method and a mobile robot, which can realize path planning of Dijkstra algorithm in a narrow passage.

[0005] The purpose of the embodiments of the present application is achieved by the following technical solutions:

[0006] To solve the above technical problems, in a first aspect, a path planning method is provided in the embodiments of the present application, applied to a mobile robot, the method comprising: obtaining a starting point and an ending point of movement of the mobile robot; setting a node expansion threshold based on the starting point and the ending point, and initializing an array in Dijkstra algorithm; based on the node expansion threshold and the initialized array, expanding nodes by Dijkstra algorithm to obtain an expanded potential field, wherein the expanded nodes include nodes located in a narrow passage; and planning a path based on the expanded potential field.

[0007] In some embodiments, setting the node expansion threshold based on the starting point and the ending point comprises: obtaining the cost value of the starting point and the ending point in the grid map; determining whether the cost value of the starting point is greater than the cost value of the ending point; if yes, setting the cost value of the starting point as the node expansion threshold; and if no, setting the cost value of the ending point as the node expansion threshold.

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

[0009] In some embodiments, the initializing the open set and the closed set used for searching by the Dijkstra algorithm based on the start point and the end point comprises: initializing each element in the closed set as positive infinity; obtaining an index value of the start point in the grid map, and saving a cost value corresponding to the index value to the open set after setting the cost value as a first preset value, wherein each element in the open set is arranged in descending order of cost value.

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

[0011] In some embodiments, the traversing the eight nodes around the first element and expanding the nodes comprises: determining whether an index value of a current node to be expanded is less than a first preset value or greater than a grid number of the grid map minus a second preset value; if not, determining whether a cost value of the current node to be expanded in the closed set is less than positive infinity; if not, determining whether a cost value of a 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 cost values of grids within a collision detection range of the current node to be expanded are all less than a third preset value; if yes, 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, the updating the current node to be expanded and the first element to the array comprises: determining whether a cost value of the current node to be expanded is less than or equal to positive infinity; updating a cost threshold to the cost value of the current node to be expanded and saving to a cost array; saving an index value of the first element as a parent node of the current node to be expanded.

[0013] In some embodiments, the method further comprises: if the index value of the current node to be expanded is less than the first preset value or greater than the grid number of the grid map minus the second preset value, or, the cost value of the current node to be expanded in the 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 values of the grids within the collision detection range of the current node to be expanded are greater than or equal to the third preset value, determining that the current node to be expanded is 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 the third preset value, the method further comprises: setting a maximum width of a collision detection frame; determining whether a width of a channel where the current node to be expanded is located is greater than the maximum width of the collision detection frame; if yes, obtaining 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 according to the coordinates and the maximum width of the collision detection frame.

[0015] In some embodiments, the updating the cost threshold value to the cost value of the current node to be expanded and saving to the cost array comprises: obtaining a step cost, and determining the cost threshold value according to a cost value of a grid in which the current node to be expanded is located in a grid map and a proportionality coefficient of the cost value of the current node to be expanded in the grid map, and a 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 comprises: determining whether the node expansion threshold value is a fourth preset value; if yes, setting the proportionality coefficient to a value greater than a fifth preset value; if no, setting the proportionality coefficient to a sixth preset value.

[0016] In some embodiments, the planning the path based on the expanded potential field comprises: obtaining a search queue based on the index values in the closed set; initializing an index value of a current path point in the search to an index value of the terminal point, and saving a coordinate of a grid in which the current path point is located to an end of the search queue; determining a parent node corresponding to the current path point according to the parent node array; saving a grid coordinate of the parent node to the end of the search queue, and assigning an index value of a grid in which the parent node is located to the current path point to obtain an index value of a new 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, to obtain a path capable of passing through the narrow channel.

[0017] To solve the above technical problems, in a second aspect, embodiments of the present application provide 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 to enable the at least one processor to perform the method of the first aspect.

[0018] Compared with the prior art, the present application has the following beneficial effects: different from the prior art, the path planning method and the mobile robot provided in the embodiments of the present application first obtain a starting point and a terminal point of movement of the mobile robot, then set a node expansion threshold value based on the starting point and the terminal point, and initialize arrays in the Dijkstra algorithm, then expand nodes by the Dijkstra algorithm based on the node expansion threshold value and the initialized arrays to obtain an expanded potential field, and finally plan a path based on the expanded potential field. The path planning method provided in the embodiments of the present application expands nodes to contain nodes located in a narrow channel in the node expansion process, thereby realizing path planning of the Dijkstra algorithm in a narrow channel environment and ensuring that the mobile robot passes through the narrow channel without collision. Attached Figure Description

[0019] One or more embodiments are illustrated by way of example with reference to the accompanying drawings. These illustrations do not constitute a limitation 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 stated, the figures in the drawings do not constitute a limitation on scale.

[0020] Figure 1 This is a flowchart illustrating a path planning method provided in Embodiment 1 of the present invention;

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

[0022] Figure 3 yes Figure 1 A schematic diagram of another sub-process 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] Figure 9 yes Figure 1 A schematic diagram of a sub-process of step S400 in the path planning method shown;

[0029] Figure 10 This is a schematic diagram of the hardware structure of a mobile robot provided in Embodiment 2 of the present invention. Detailed Implementation

[0030] The application will be described in further detail below with reference to specific embodiments. The following examples will help those skilled in the art to further understand the application, but are not intended to limit the application in any form. It should be noted that those skilled in the art can make several modifications and improvements without departing from the concept of the application. These are all within the scope of protection of the application.

[0031] In order to make the purpose, technical scheme and advantages of the present application clearer, the present application will be further described in detail below in combination with the drawings and examples. It should be understood that the specific examples described herein are only used to explain the present application and are not intended to limit the present application.

[0032] It should be noted that the various features in the embodiments of the present application can be combined with each other without conflict, and are all within the scope of protection of the present application. In addition, although the functional modules are divided in the device schematic diagram, and the logical order is shown in the flowchart, in some cases, the steps shown or described can be different from the module division in the device or the order of execution in the flowchart.

[0033] Unless otherwise defined, all technical and scientific terms used in the specification have the same meaning as commonly understood by one of ordinary skill in the art to which the present application belongs. The terms used in the specification of the present application are only for the purpose of describing the specific embodiments and are not intended to limit the present application.

[0034] In addition, the technical features involved in each embodiment of the present application described below can be combined with each other as long as there is no conflict.

[0035] In order to solve the problem that the current Dijkstra algorithm cannot plan a path in a narrow channel and other narrow environments, limiting its application scenarios, an embodiment of the present application provides a path planning method, which expands the node to include the node located in the narrow channel in the node expansion process, and filters out the node that may collide, so as to realize the path planning of the Dijkstra algorithm in the narrow channel environment. Further, the further judgment on the cost of the grid where the expanded node is located can be used to ensure that the mobile robot will not collide when passing through the planned narrow channel path, and the planned path will not have the phenomenon of passing through the wall.

[0036] Specifically, the embodiments of the present application will be further described below in combination with the drawings.

[0037] Embodiment one

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

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

[0040] In the embodiment of the present application, firstly, the start point of the movement of the mobile robot needs to be obtained, and the end point of the movement of the mobile robot needs to be obtained. Generally, the start point is the current position of the mobile robot, and the end point is the target position of the movement of the mobile robot. Then, the path of the movement of the mobile robot from the start point to the end point is planned based on the start point and the end point.

[0041] Step S200: setting a node expansion threshold based on the start point and the end point, and initializing an array in the Dijkstra algorithm;

[0042] When planning the path, since there can be two cases of narrow channel and no narrow channel, for the case of narrow channel, the embodiment of the present application can cope with the case that the node to be expanded is in the narrow channel and is saved in the process of expanding the potential field by the Dijkstra algorithm, and then searches the path based on the expanded potential field. Based on this, the node expansion threshold needs to be set to expand the potential field, and the array in the Dijkstra algorithm needs to be initialized, so as to expand the node according to the node expansion threshold and update the array.

[0043] For the given start point and end point of the movement of the mobile robot, before performing step S200 to set the node expansion threshold and initialize the array in the Dijkstra algorithm, the Dijkstra algorithm can be first tried to be used for path planning. If the planning is successful, it means that there is no narrow channel, and the path can be directly planned, and the process ends. If the planning is not successful, it is further judged 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 at the start point or somewhere between the start point and the end point. If the cost is equal to 253, it means that the end point is in the narrow channel. Whether the cost is less than 253 or equal to 253, it means that there can be a narrow channel. At this time, steps S200 to S400 of the present application are performed to plan the path in the narrow channel. 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 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, and does not need to be limited by the embodiment of the present application.

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

[0045] Step S210: Obtain the generation value of the start point and the end point in the grid map;

[0046] Step S220: Determine whether the generation value of the start point is greater than the generation value of the end point; if yes, jump to step S230; if no, jump to step S240;

[0047] Step S230: Set the generation value of the start point as the node expansion threshold;

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

[0049] The grid map is composed of a plurality of grids, and the start point and the end point are located in the grid map. In the embodiment of the application, the node expansion threshold needs to be initialized and set as the greater one of the generation value of the start point and the generation value of the end point.

[0050] Further, when obtaining the generation value of the end point in the grid map, it is also necessary to determine whether the generation 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, ending the path planning process. If the generation 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, there is a narrow channel. If the generation 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 of Dijkstra algorithm needs to be initialized, please refer to Figure 3 which shows Figure 1 Another sub-process of step S200 in the path planning method shown, the array in Dijkstra algorithm is initialized based on the start point and the end point, including:

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

[0053] Step S260: Based on the start point and the end point, initialize the open set and the closed set used for searching in Dijkstra algorithm;

[0054] The cost array of the grid map stores the cost of each grid in the map, the cost value is represented as 0 to 255, the parent node array saves the mapping of each node and the parent node, the cost array saves 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 saves the mapping of the index value and the cost value of each node, the closed set saves the mapping of the index value and the cost value of each grid in the grid map, and one node corresponds to a grid.

[0055] Specifically, please refer to Figure 4 which shows Figure 3 A sub-process of step S260 in the path planning method shown, the open set and the closed set used for searching are initialized based on the starting point and the ending point, including:

[0056] Step S261: initializing each element in the closed set as positive infinity;

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

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

[0059] Step S300: based on the node expansion threshold and the initialized array, the node expansion is performed through the Dijkstra algorithm to obtain the expanded potential field;

[0060] In the embodiment of the application, after the node expansion threshold and the initialized array are determined, the node expansion can be performed through the Dijkstra algorithm, so as to plan one or more paths from the starting point to the ending point based on the expanded potential field, and because the nodes in the expanded potential field include the nodes located in the narrow channel, the paths obtained by planning can also include the paths through the narrow channel.

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

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

[0063] In the embodiment of the application, whether the node has been saved to the closed set can be determined by whether the value of 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 value of the node saved in the closed set is the minimum value, the node has been saved to the closed set as an expandable node, and the next step of judgment is not needed; since each element in the closed set is initialized as positive infinity when the closed set is initialized, if it is equal to positive infinity, it is indicated that the element has not been expanded, and the judgment of whether it is an expandable node can be performed, and at this time, the element is expanded in step S320.

[0064] Step S320: The value of the first element in the open set is saved to the closed set, and nodes in eight directions around the first element are traversed and expanded;

[0065] In the embodiment of the application, before the first element is expanded, the value of the first element is saved from the open set to the closed set, and then nodes in eight directions around the first element are sequentially traversed and expanded, wherein the nodes in eight directions around the first element are respectively a left node, a right node, an upper node, a lower node, a left upper node, a right upper node, a left lower node and a right lower node, and the sequential traversal can be clockwise traversal or counterclockwise traversal. For the judgment of whether each node traversed 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, the nodes in eight directions around the first element are traversed and expanded, including:

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

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

[0068] Step S322: determining whether the generation value of the current node to be expanded in the closed set is less than positive infinity; if not, jumping to step S323; if yes, jumping to step S326.

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

[0070] Step S323: determining whether the generation 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, jumping to step S324; if yes, jumping to step S326.

[0071] Then, after determining that the current node to be expanded is in the grid map and the generation value in the closed set is the initial positive infinity, it is also necessary to determine whether the generation 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 the nodes whose generation value is too large and whose distance from the obstacle is too close, and the mobile robot is likely to collide or penetrate the wall.

[0072] Step S324: determining whether the generation value of the grid in the collision detection range of the current node to be expanded is less than the third preset value; if yes, jumping to step S325; if not, jumping to step S326.

[0073] In the third preset value of the application, 254 is taken as an example. Then, after determining that the current node to be expanded is in the grid map and the cost value of the node in the closed set is the initial positive infinity and the node is not too close to the obstacle, it is further determined whether the cost values of the grids in the collision detection range of the current node to be expanded are all 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 expanded node is located is greater than or equal to 254, that is, equal to 254, the mobile robot will collide or cannot pass through when located at the node, and the node cannot be an expandable node; if the cost value of the grid where the expanded node is located is less than 254, the node is an expandable node that can pass through without collision.

[0074] Taking a square detection frame as an example, it is assumed that the range of the square collision detection frame is:

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

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

[0077] wherein next_x and next_y are the x and y coordinates of the current node to be expanded in the map. Then, any grid (x, y) in the range of the collision detection can be taken, that is, the grid with x taking any value in the interval [next_x-lower_bound, next_x+upper-bound] and y taking any value in the interval [next_y-lower_bound, next_y+upper_bound], and it is determined whether the cost values of the grids are all less than 254. If all the grids meet the requirement, it is indicated that the mobile robot can pass through without collision when located at the expanded node; if only one grid does not meet the requirement, it is indicated that the mobile robot is too close to the obstacle when located at the expanded node, and collision may occur, so the corresponding current node to be expanded is discarded.

[0078] wherein the determination of the range of the collision detection is described in Figure 7 which shows Figure 6 a sub-process of step S324 in the path planning method shown in the figure, before determining whether the cost values of the grids in the collision detection range of the current node to be expanded 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 various shapes such as rectangle, circle, ellipse, and polygon, and the specific shape 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 that pass 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 practical applications, the shape of the collision detection frame can be set according to actual needs and does not need to be limited to the limitations of the embodiments of this invention.

[0081] Furthermore, setting the maximum width of the collision detection frame specifically includes: determining the number of grid cells corresponding to the minimum width based on the minimum width of the passage that the mobile robot can pass through 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 grid cells. For example, when the minimum width of the passage that the mobile robot can pass through is 60cm and the resolution of the grid map is 0.05m (i.e., 5cm), dividing the minimum width of the passage that the mobile robot can pass through by the resolution of the grid map yields a grid cell number of 12. Considering mapping errors, a 60cm narrow passage may be narrower than 60cm 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 that the mobile robot can pass through 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. Therefore, the specific settings can be made according to actual needs and do not need to be limited to the limitations of this embodiment.

[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, proceed to step S3243.

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

[0084] Step S3243: Obtain the coordinates of the grid cell containing the node to be expanded in the grid map;

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

[0086] Step S3244: Determine the collision detection range of the node to be expanded based on 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 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 bounding box as an example, using the center coordinates (next_x, next_y) as the base point, the lower and upper limits of the collision detection range are determined according to the side length of the square collision detection bounding box, 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] Wherein, 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] Choosing the lower and upper limits of the collision detection range as set above ensures 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, which is the side length of the square collision detection box.

[0095] Step S325: Determine that the current node to be expanded is an expandable node, and update the array with the current node to be expanded and the first element;

[0096] Finally, after determining through steps S321 to S324—that is, after confirming that the node to be expanded is within the grid map, its cost value in the closed set is initially positive infinity, the cost value of the grid containing the node is less than the node expansion threshold, and the cost value of all grids within the collision detection range is less than 254—it can be determined as an expandable node, and the node to be expanded and the first element are updated in the array. Specifically, for the process of saving to the array, please refer to [link to relevant documentation]. Figure 8 It shows Figure 6 A sub-process of step S325 in the path planning method shown, which involves updating the current node to be expanded and the first element to the array, includes:

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

[0098] First, it is necessary to determine whether the value of the node to be expanded is less than or equal to positive infinity to determine whether it has been saved to the open set. If it is equal to positive infinity, it means that the node has not been added to the open set, and then proceed to step S3252 to 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: Update the cost threshold to the cost value of the current node to be expanded and save it to the cost array;

[0100] Specifically, the step-size cost is obtained, and the cost threshold is determined based on the cost value of the grid cell where the currently expanded node is located in the grid map, the proportion coefficient of the cost value of the currently expanded node in the grid map, the cost value of the first element, and the step-size cost. Further, it is necessary to determine whether the cost value stored for the node in the open set is the minimum value. Specifically, if the cost value stored for the node in the open set is less than or equal to the cost threshold, it means that the cost value stored in the open set is the minimum cost value; if the cost value stored in the open set is greater than the cost threshold, it means that a smaller cost has been obtained, and the cost threshold is updated to the cost value of the currently expanded node and stored in the cost array. The expression for the cost value of the node is as follows:

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

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

[0103] Furthermore, in this embodiment of the invention, it is also necessary to determine the proportional coefficient of the cost threshold. Before determining whether the cost value of the currently expanded node is less than or equal to positive infinity, the method further includes: determining whether the node expansion threshold is a fourth preset value; if yes, setting the proportional coefficient to a value greater than a fifth preset value, for example, it can be set to 1e6; if no, 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 the eight directions surrounding each of the first element through the above steps, and determining that at least one of the nodes is a collision-free, non-wall-penetrating, expandable node, the index value of the first element is saved to the parent node array.

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

[0107] In this embodiment of the invention, if step S321 is affirmative, 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 affirmative, 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 affirmative, 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 negative, 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 the current node to be expanded can be determined to be an unexpandable node.

[0108] Step S330: The first element is deleted from the open set and a new open set is obtained;

[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. The second element in the original open set is updated to the new first element in the new open set.

[0110] Step S340: The node expansion ends when the index value of the new first element is the same as the index value of the endpoint in the grid map.

[0111] Specifically, after saving the first element to the closed set, the first element is deleted from the open set to obtain a new open set. For the new first element in the new open set, the steps of obtaining the first element in the open set and determining whether the value of the first element in the closed set is less than positive infinity are repeated. If not, the value of the first element in the open set is saved to the closed set, and the nodes in the eight directions around the first element are traversed and the nodes are expanded. The node expansion ends when the index value of the new first element is the same as the index value of the endpoint in the grid map.

[0112] Step S400: Plan a path based on the extended potential field.

[0113] In this embodiment of the invention, while obtaining the extended potential field, the Dijkstra algorithm can be used to attempt to plan a path from the starting point to the ending point. The path planning algorithm provided in this embodiment of the invention can combine the case of narrow channels for judgment to obtain a path from the starting point to the ending point that is shortest, collision-free, and does not pass through walls. Furthermore, the final planned path may be a path that passes through the narrow channel or a path that does not pass through the narrow channel.

[0114] After completing all the steps S100 to S300 above and obtaining a closed set containing passable nodes and expandable nodes, path search can be performed based on the closed set containing the expanded nodes. For details, please refer to [link to relevant documentation]. Figure 9 It shows Figure 1 A sub-process of step S400 in the path planning method shown, wherein the path planning based on the extended potential field includes:

[0115] Step S410: Obtain the 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 to the index value of the destination, and save the coordinates of the grid where the current path point is located to the end of the search queue;

[0117] Step S430: Determine the parent node corresponding to the current path point based on the parent node array;

[0118] Step S440: Save the grid coordinates of the parent node to the end of the search queue, and assign the index value of the grid where the parent node is located to the current path point to obtain the 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 the narrow channel is obtained.

[0120] In this embodiment of the invention, firstly, a search queue is obtained sequentially according to the index values ​​in the closed set. Then, the index value of the current path point in the search queue path, current_index, is initialized to the index value of the destination, goal_index, and the coordinates of the grid where the current path point is located are stored at the end of the search queue path. Next, based on the parent node array, the parent node parent[current_index] corresponding to the current path point current_index is determined, and the grid coordinates where the parent node parent[current_index] is located are placed at the end of the search queue path. The index value of the grid where the parent node parent[current_index] is located is assigned to the index value of the current path point current_index. For a 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 of the new current path point current_index is equal to the index value of the starting point start_index. At this point, a path connecting the starting point and the destination is obtained. If the path point contains a narrow channel, the path is a path that can pass through the narrow channel.

[0121] Example 2

[0122] This invention also provides a mobile robot, please refer to [link / reference]. Figure 10 It demonstrates the ability to execute Figures 1 to 9 The hardware structure of the mobile robot using the path planning method.

[0123] The mobile robot 10 includes: at least one processor 11; and a memory 12 communicatively connected to the at least one processor 11. Figure 10 Taking a processor 11 as an example, the memory 12 stores instructions that can be executed by the at least one processor 11, which, when executed by the at least one processor 11, enable the at least one processor 11 to perform the aforementioned... Figures 1 to 9 The path planning method described above. The processor 11 and the memory 12 can be connected via a bus or other means. Figure 10 Taking the example of a connection between China and Israel via a bus.

[0124] The memory 12, as a non-volatile computer-readable storage medium, can be used to store non-volatile software programs, non-volatile computer-executable programs, and modules, such as the program instructions / modules corresponding to the path planning method in the embodiments of this 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, thereby implementing the path planning method in the above-described method embodiments.

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

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

[0127] The above-described product can perform the methods provided in the embodiments of this application, and has the corresponding functional modules and beneficial effects for performing the methods. Technical details not described in detail in this embodiment can be found in the methods provided in the embodiments of this application.

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

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

[0130] This invention provides a path planning method and a mobile robot. The method first obtains the starting and ending points of the mobile robot's movement. Then, based on the starting and ending points, it sets a node expansion threshold and initializes the array in the Dijkstra algorithm. Next, based on the node expansion threshold and the initialized array, it expands the nodes using the Dijkstra algorithm to obtain the expanded potential field. Finally, it plans the path based on the expanded potential field. The path planning method provided in this invention expands the nodes to include nodes located in narrow channels during the node expansion process, thereby realizing path planning using the Dijkstra algorithm in narrow channel environments and ensuring that the mobile robot passes through narrow channels without collisions.

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

[0132] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented using software and a general-purpose hardware platform, or of course, using hardware. Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. The storage medium can be a magnetic disk, optical disk, read-only memory (ROM), or 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, and not to limit them; under the concept of the present invention, the technical features of the above embodiments or different embodiments can also be combined, the steps can be implemented in any order, and there are many other variations of different aspects of the present invention as described above. For the sake of brevity, they are not provided in detail; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A path planning method characterized by, The method is applied to a mobile robot, and the method comprises: obtaining a start point and an end point of movement of the mobile robot; based on the start point and the end point, setting a node expansion threshold value, and initializing an array in a Dijkstra algorithm, comprising: obtaining a generation value of the start point and the end point in a grid map; determining whether the generation value of the start point is greater than the generation value of the end point; if yes, setting the generation value of the start point as the node expansion threshold value; if no, setting the generation value of the end point as the node expansion threshold value; initializing a parent node array and a cost array, wherein the parent node array saves a mapping of each node and a parent node, and the cost array saves a generation value of each node, and the generation value of each node in the initialized cost array is positive infinity; based on the start point and the end point, initializing an open set and a closed set used for searching by the Dijkstra algorithm, wherein the open set saves a mapping of an index value and a generation value of each node, and the closed set saves a mapping of an index value and a potential field value of each grid in the grid map, and one node corresponds to one grid; based on the node expansion threshold value and the initialized array, performing node expansion by the Dijkstra algorithm to obtain an expanded potential field; wherein the expanded node includes a node located in a narrow channel; planning a path based on the expanded potential field.

2. The path planning method according to claim 1, wherein the initializing the open set and the closed set used for searching by the Dijkstra algorithm based on the start point and the end point comprises: initializing each element in the closed set as positive infinity; obtaining an index value of the start point in the grid map, and saving a generation value corresponding to the index value to the open set after setting the generation value as a first preset value, wherein each element in the open set is arranged in descending order of the generation value.

3. The path planning method according to claim 1, wherein the performing node expansion by the Dijkstra algorithm based on the node expansion threshold value and the initialized array to obtain the expanded potential field comprises: obtaining a first element in the open set and determining whether a generation value corresponding to the first element in the closed set is less than positive infinity; if no, saving the generation value of the first element in the open set to the closed set, and performing node expansion on nodes in eight directions around the first element; wherein after saving the first element to the closed set, the open set deletes the first element and obtains a new open set, for a new first element in the new open set, repeating the steps of obtaining the first element in the open set and determining whether the generation value corresponding to the first element in the closed set is less than positive infinity, and the step of saving the generation value of the first element in the open set to the closed set and performing node expansion on nodes in eight directions around the first element, until the index value of the new first element is the same as an index value of the end point in the grid map, and ending the node expansion.

4. The path planning method according to claim 3, wherein The traversing the eight nodes around the first element and the node expansion include: 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 of the grid map minus a second preset value; if not, determining whether the heuristic value of the current node to be expanded in the closed set is less than positive infinity; if not, determining whether the heuristic value of the grid where the current node to be expanded is located in the grid map is greater than the node expansion threshold value; if not, determining whether the heuristic 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 yes, 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.

5. The path planning method of claim 4, wherein, The updating the current node to be expanded and the first element to the array includes: determining whether the heuristic value of the current node to be expanded is less than or equal to positive infinity; updating the cost threshold value to the heuristic value of the current node to be expanded and saving it to the cost array; saving the index value of the first element as the parent node of the current node to be expanded.

6. The path planning method of claim 4, wherein, The method further includes: if the index value of the current node to be expanded is less than the first preset value or greater than the number of grids of the grid map minus the second preset value, or the heuristic value of the current node to be expanded in the closed set is less than positive infinity, or the heuristic value of the grid where the current node to be expanded is located in the grid map is greater than the node expansion threshold value, or the heuristic values of the grids within the collision detection range of the current node to be expanded are greater than or equal to the third preset value, it is determined that the current node to be expanded is an inexpandable node.

7. The path planning method of claim 4, wherein, before determining whether the heuristic values of the grids within the collision detection range of the current node to be expanded are all less than the third preset value, the method further includes: setting a maximum width of the collision detection frame; 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 frame; if yes, obtaining the coordinates of the grid where the current node to be expanded is located in the grid map; determining the collision detection range of the current node to be expanded according to the coordinates and the maximum width of the collision detection frame.

8. The path planning method of claim 5, wherein, the updating the cost threshold value to the heuristic 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 value according to the heuristic value of the grid where the current node to be expanded is located in the grid map, a proportionality coefficient of the heuristic value of the current node to be expanded in the grid map, the heuristic value of the first element, and the step cost; before determining whether the heuristic 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 value is a fourth preset value; if yes, setting the proportionality coefficient to a value greater than a fifth preset value; if not, setting the proportionality coefficient to a sixth preset value.

9. The path planning method of any one of claims 1-8, wherein the planning a path based on the extended potential field comprises: obtaining a search queue based on the index values in the closed set; initializing an index value of a current path point in the search to an index value of the end point and saving a coordinate of a grid where the current path point is located to an end of the search queue; determining a parent node corresponding to the current path point according to the parent node array; saving a grid coordinate of the parent node to the end of the search queue and assigning an index value of a grid where the parent node is located to the current path point to obtain an index value of a new current path point; repeating the determining the parent node corresponding to the current path point according to the parent node array and the saving the grid coordinate of the parent node to the end of the search queue and the assigning the index value of the grid where the parent node is located to the current path point to obtain the index value of the new current path point for the index value of the new current path point until the index value of the new current path point is equal to an index value of the start point, and obtaining the path that can pass through the narrow passage. comprise:

10. A mobile robot, characterized by at least one processor; and a memory connected with the at least one processor in communication; wherein the memory stores instructions executable 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 path planning method of any one of claims 1-9. ​

Images