Path planning method based on adaptive multi-resolution A* algorithm based on octree map

Abstract

The present invention discloses a path planning method based on an adaptive multi-resolution A* algorithm of an octree map, comprising: 1) obtaining octree map information and publishing it to a path planning node; 2) adjusting the current map depth and performing path planning in the octree map using an improved adjacent node search strategy at the current depth to obtain a path T1; 3) selecting the end point of a second path planning from the path planned for the first time according to a variable resolution threshold, deleting the path points between the starting point and the end point of the second path planning from the path, adjusting the current map depth again, performing a second path planning at a higher resolution, and obtaining a path T h ;4) Compare the obtained path T1 with the path T h The images are stitched together to obtain a multi-resolution path T with a higher resolution at close range and a lower resolution at long range. The method of the present invention can significantly increase the running speed of the algorithm and can generate a safe path suitable for mobile robots to travel more quickly than existing algorithms.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robot navigation, and in particular to a path planning method of an adaptive multi-resolution A* algorithm based on an octree map. Background Art

[0002] With the development of manufacturing, the increasing demand for manufacturing flexibility, rising labor costs, and the growing demand for environmental and human safety trends are all requiring manufacturing and logistics scenarios to possess rapid adaptability and higher efficiency. Autonomous mobile robots, highly automated and flexible handling equipment, will be the major direction of future industrial automation upgrades. Currently, over 60% of industrial applications in the Chinese market require the ability of mobile robots to navigate independently. Path planning algorithms, as one of the core technologies in robot navigation systems, have become a hot topic of research for scholars both domestically and internationally.

[0003] Currently, most mobile robots use two-dimensional maps for path planning. Since two-dimensional maps only contain two-dimensional environmental information at the same level or multiple levels of environmental information, when encountering complex environmental conditions, the robot may collide with obstacles of different heights that are not marked in the map, resulting in path planning failure.

[0004] The three-dimensional A* algorithm is commonly used for path planning in drones, and is typically based on a three-dimensional grid map. Three-dimensional grids offer fast indexing, but excessive grid resolution can lead to an excessive number of grid cells, increasing runtime memory usage. Excessively large grid cell side lengths can cause path planning failures due to the grid map's low resolution. Octree maps, on the other hand, use a tree structure to store maps, saving storage space. Furthermore, because the map's resolution can be adjusted by adjusting the depth, searches at different resolutions can be performed during path planning, thus avoiding these issues. Based on this, the present invention proposes a path planning method using an adaptive multi-resolution A* algorithm based on an octree map. Summary of the Invention

[0005] The present invention aims to address the shortcomings of existing technologies by providing a path planning method based on an adaptive multi-resolution A* algorithm using an octree map. By improving the adjacent node search strategy, the A* algorithm increases its search and expansion speed for adjacent nodes. Furthermore, the algorithm can restrict the path to a two-dimensional plane while acquiring three-dimensional environmental information, making the generated path more suitable for mobile robot travel. Furthermore, the adaptive multi-resolution A* algorithm proposed in this invention significantly increases its operating speed, generating safe paths suitable for mobile robot travel more quickly than existing algorithms.

[0006] To achieve the above object, the present invention adopts the following technical solutions:

[0007] A path planning method based on an adaptive multi-resolution A* algorithm of an octree map comprises the following steps:

[0008] Step S1, obtain the octree map information and publish it to the path planning node through ROS;

[0009] Step S2: Adjust the current map depth and use the improved adjacent node search strategy to perform path planning in the octree map at the current depth to obtain the path T l ;

[0010] Step S3: Adjust the current map depth again and perform a second path planning at a higher resolution at the current depth to obtain the path T h ;

[0011] Step S4: The lower resolution planned path T obtained in step S2 is l The higher resolution planning path T obtained in step S3 h The multi-resolution path T is obtained by splicing, with higher resolution in close distance and lower resolution in long distance. The path is converted into ROS messages and published to the subsequent execution units of the mobile robot.

[0012] Specifically, the octree map is obtained in step S1 and published to the path planning node through ROS. The specific process is as follows:

[0013] The octree map information is read through the octomap library, and then the octree map is converted into ROS message format and published to the path planning node through the ROS message publisher.

[0014] Specifically, in step S2, the improved adjacent node search strategy is used to perform path planning in the octree map at the current depth. The specific content of the improved adjacent node search strategy is as follows:

[0015] Step S21: Since the mobile robot cannot move directly above or below, the search for nodes directly above and below is removed, reducing the number of nodes in 26 directions to 24.

[0016] Step S22: Add judgment during the search, change the search in 24 directions to the search in the surrounding 8 columnar areas, and divide the search into three layers: upper, middle and lower, with 8 nodes in each layer, that is, 8 horizontal directions;

[0017] Step S23: During the search, when a node in one direction in one layer is occupied by an obstacle, it is determined that the columnar area in that direction is occupied by an obstacle; when all nodes in the same direction in the three layers are not occupied by obstacles, it is determined that the direction is passable.

[0018] Specifically, step S3 performs a second path planning at a higher resolution at the current depth. The second path planning generates a path based on an adaptive multi-resolution A* algorithm. The specific contents of the adaptive multi-resolution A* algorithm are as follows:

[0019] Step S31: Set the lower limit d of the adaptive depth l and upper limit d h Used to adjust the depth of the path node during path planning and initialize the current depth d c The value is equal to d l ;

[0020] Step S32: Set a resolution change threshold t to determine where in the path the resolution should be changed; set a path identifier P and set its initial state to false to determine whether to perform secondary path planning to achieve a locally higher-resolution path and to combine paths with different resolutions;

[0021] Step S33: Before running the adaptive multi-resolution A* algorithm, add a loop judgment on the depth and the judgment on the path identifier, that is, when the path planning fails at the current depth, adjust the current depth to d c +Δd1, then judge d c Is it greater than the upper depth limit d? h , if d c >d h , then determine whether the path identifier state is false. If it is false, it means that the path planning failed, and if it is true, it means that the first path planning was successful and the path is output directly; if d c ≤d h , then continue with path planning based on the current depth;

[0022] Step S3: At the current depth d c Next, we use the improved adjacent node search strategy to plan the path in the octree map. First, we set the starting point (x start ,y start ,z start ) and the target point ((x goal ,y goal ,z goal )), search the 24 adjacent nodes according to the improved adjacent node search strategy, and put the horizontal nodes that are not judged as obstacles into the open list Openlist, and then find the node with the smallest cost value F(n) from the Openlist. The cost value formula is:

[0023] F(n)=g(n)+h(n) (1)

[0024] Where n represents a node in the path search process, g(n) represents the minimum cost from the starting point to the nth node, and h(n) represents the estimated cost from the nth node to the target node. g(n) is usually the accumulation of single-step Euclidean distances, and the formula can be expressed as:

[0025]

[0026] In the formula, (x n ,y n ,z n ) is the coordinate of the current point, (x n-1 ,y n-1 ,y n-1 ) is the coordinate of the previous node;

[0027] h(n) is usually expressed by the Manhattan distance or Euclidean distance from the current point to the target point, but the Euclidean distance is more in line with the distance representation in real life. Therefore, in this paper, h(n) is expressed by the Euclidean distance between the current point and the target point. The formula is:

[0028]

[0029] Set the node with the smallest cost value F(n) as the current node, delete it from the Openlist, and add it to the Closelist; cyclically use the improved adjacent node search strategy to search for adjacent points until the target point coordinates are found, and finally generate a path T with a lower resolution l , and adjust the current depth to d c +Δd2;

[0030] Step 35: If path planning fails at the current depth, adjust the current depth to d c +Δd1, and then repeat the contents of step S33 until the path is found and proceed to the next step;

[0031] Step 36: Add secondary path planning to implement multi-resolution path planning, that is, when d c <d h , and the path identifier is true, select the node with changed resolution according to the variable resolution threshold t, and use this node as the end point of the secondary path planning. c Path planning at +Δd2 depth and obtaining a path T at a higher resolution h .

[0032] Compared with the prior art, the present invention has the following beneficial effects:

[0033] 1. The present invention uses ROS to transmit map data and path data, so it can be easily used on mobile robots.

[0034] 2. The present invention improves the adjacent node search strategy, thereby increasing the speed of the A* algorithm in searching and expanding adjacent nodes, and enabling the algorithm to restrict the path to a two-dimensional plane while obtaining three-dimensional environmental information, making the generated path more suitable for mobile robot travel.

[0035] 3. The adaptive multi-resolution A* algorithm proposed in this invention can significantly improve the running speed of the algorithm and can generate a safe path suitable for mobile robots to travel more quickly than existing algorithms. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0037] Figure 1 This is a general workflow diagram of a path planning method of the present invention based on an adaptive multi-resolution A* algorithm with an octree map;

[0038] Figure 2 This is a flowchart of the adaptive multi-resolution A* algorithm described in the present invention. DETAILED DESCRIPTION

[0039] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without creative work are within the scope of protection of the present invention.

[0040] like Figure 1 As shown, the present invention provides a path planning method based on an adaptive multi-resolution A* algorithm of an octree map, comprising the following steps:

[0041] Step S1, obtain the octree map information and publish it to the path planning node through ROS;

[0042] Step S2: Adjust the current map depth and use the improved adjacent node search strategy to perform path planning in the octree map at the current depth to obtain the path T l ;

[0043] Step S3: Adjust the current map depth again and perform a second path planning at a higher resolution at the current depth to obtain the path Th ;

[0044] Step S4: The lower resolution planned path T obtained in step S2 is l The higher resolution planning path T obtained in step S3 h The multi-resolution path T is obtained by splicing, with higher resolution in close distance and lower resolution in long distance. The path is converted into ROS messages and published to the subsequent execution units of the mobile robot.

[0045] Specifically, the octree map is obtained in step S1 and published to the path planning node through ROS. The specific process is as follows:

[0046] The octree map information is read through the octomap library, and then the octree map is converted into ROS message format and published to the path planning node through the ROS message publisher.

[0047] Specifically, in step S2, the improved adjacent node search strategy is used to perform path planning in the octree map at the current depth. The specific content of the improved adjacent node search strategy is as follows:

[0048] Step S21: Since the mobile robot cannot move directly above or below, the search for nodes directly above and below is removed, reducing the number of nodes in 26 directions to 24.

[0049] Step S22: Add judgment during the search, change the search in 24 directions to the search in the surrounding 8 columnar areas, and divide the search into three layers: upper, middle and lower, with 8 nodes in each layer, that is, 8 horizontal directions;

[0050] Step S23: During the search, when a node in one direction in one layer is occupied by an obstacle, it is determined that the columnar area in that direction is occupied by an obstacle; when all nodes in the same direction in the three layers are not occupied by obstacles, it is determined that the direction is passable.

[0051] Specifically, such as Figure 2 As shown, step S3 performs a second path planning at a higher resolution at the current depth. The second path planning generates a path based on the adaptive multi-resolution A* algorithm. The specific content of the adaptive multi-resolution A* algorithm is as follows:

[0052] Step S31: Set the lower limit d of the adaptive depth l and upper limit d h Used to adjust the depth of the path node during path planning and initialize the current depth d c The value is equal to d l ;

[0053] Step S32: Set a resolution change threshold t to determine where in the path the resolution should be changed; set a path identifier P and set its initial state to false to determine whether to perform secondary path planning to achieve a locally higher-resolution path and to combine paths with different resolutions;

[0054] Step S33: Before running the adaptive multi-resolution A* algorithm, add a loop judgment on the depth and the judgment on the path identifier, that is, when the path planning fails at the current depth, adjust the current depth to d c +Δd1, then judge d c Is it greater than the upper depth limit d? h , if d c >d h , then determine whether the path identifier state is false. If it is false, it means that the path planning failed, and if it is true, it means that the first path planning was successful and the path is output directly; if d c ≤d h , then continue with path planning based on the current depth;

[0055] Step S3: At the current depth d c Next, we use the improved adjacent node search strategy to plan the path in the octree map. First, we set the starting point (x start ,y start ,z start ) and the target point ((x goal ,y goal ,z goal )), search the 24 adjacent nodes according to the improved adjacent node search strategy, and put the horizontal nodes that are not judged as obstacles into the open list Openlist, and then find the node with the smallest cost value F(n) from the Openlist. The cost value formula is:

[0056] F(n)=g(n)+h(n) (1)

[0057] Where n represents a node in the path search process, g(n) represents the minimum cost from the starting point to the nth node, and h(n) represents the estimated cost from the nth node to the target node. g(n) is usually the accumulation of single-step Euclidean distances, and the formula can be expressed as:

[0058]

[0059] In the formula, (x n ,y n ,z n ) is the coordinate of the current point, (x n-1 ,y n-1 ,y n-1) is the coordinate of the previous node;

[0060] h(n) is usually expressed by the Manhattan distance or Euclidean distance from the current point to the target point, but the Euclidean distance is more in line with the distance representation in real life. Therefore, in this paper, h(n) is expressed by the Euclidean distance between the current point and the target point. The formula is:

[0061]

[0062] Set the node with the smallest cost value F(n) as the current node, delete it from the Openlist, and add it to the Closelist; cyclically use the improved adjacent node search strategy to search for adjacent points until the target point coordinates are found, and finally generate a path T with a lower resolution l , and adjust the current depth to d c +Δd2;

[0063] Step 35: If path planning fails at the current depth, adjust the current depth to d c +Δd1, and then repeat the contents of step S33 until the path is found and proceed to the next step;

[0064] Step 36: Add secondary path planning to implement multi-resolution path planning, that is, when d c <d h , and the path identifier is true, select the node with changed resolution according to the variable resolution threshold t, and use this node as the end point of the secondary path planning. c Path planning at +Δd2 depth and obtaining a path T at a higher resolution h .

[0065] In summary, this invention improves the A* algorithm's search and expansion speed for adjacent nodes by improving its adjacent node search strategy. Furthermore, the algorithm can constrain the path to a two-dimensional plane while acquiring three-dimensional environmental information, making the generated path more suitable for mobile robot navigation. Furthermore, the adaptive multi-resolution A* algorithm proposed in this invention significantly increases its operating speed, generating safe paths suitable for mobile robot navigation more quickly than existing algorithms.

[0066] 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 it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A path planning method based on an adaptive multi-resolution A* algorithm with an octree map, characterized in that: The following steps are involved: Step S1, obtain the octree map information and publish it to the path planning node through ROS; Step S2: Adjust the current map depth and use the improved adjacent node search strategy to perform path planning in the octree map at the current depth to obtain the path T l ; Step S3: Adjust the current map depth again and perform a second path planning at a higher resolution at the current depth to obtain the path T h ; Step S4: The lower resolution planned path T obtained in step S2 is l The higher resolution planning path T obtained in step S3 h Perform stitching to obtain a multi-resolution path T with higher resolution in close distance and lower resolution in long distance. The path is converted into ROS messages and published to the subsequent execution units of the mobile robot. The resolution change threshold t is set to determine where in the path the resolution should be changed; Set the path identifier P and set the initial state to false to determine whether to perform secondary path planning to achieve local higher-resolution path planning and splicing of paths with different resolutions; Before running the adaptive multi-resolution A* algorithm, a loop judgment on the depth and the judgment on the path identifier are added. That is, when the path planning fails at the current depth, the current depth is adjusted to d c +Δd1, then judge d c Is it greater than the upper depth limit d? h , if d c >d h , then determine whether the path identifier state is false. If it is false, it means that the path planning failed, and if it is true, it means that the first path planning was successful and the path is output directly; if d c ≤d h , then continue with path planning based on the current depth.

2. The path planning method based on the adaptive multi-resolution A* algorithm of octree map according to claim 1 is characterized in that: The octree map is obtained in step S1 and published to the path planning node through ROS. The specific process is as follows: The octree map information is read through the octomap library, and then the octree map is converted into ROS message format and published to the path planning node through the ROS message publisher.

3. The path planning method based on the adaptive multi-resolution A* algorithm of octree map according to claim 1, characterized in that: In step S2, the improved adjacent node search strategy is used to perform path planning in the octree map at the current depth. The specific content of the improved adjacent node search strategy is as follows: Step S21: Since the mobile robot cannot move directly above or below, the search for nodes directly above and below is removed, reducing the number of nodes in 26 directions to 24. Step S22: Add judgment during the search, change the search in 24 directions to the search in the surrounding 8 columnar areas, and divide the search into three layers: upper, middle and lower, with 8 nodes in each layer, that is, 8 horizontal directions; Step S23: During the search, when a node in one direction in one layer is occupied by an obstacle, the direction columnar area is determined to be occupied by an obstacle; when all nodes in the same direction in the three layers are not occupied by obstacles, the direction is determined to be passable.

4. The path planning method based on the adaptive multi-resolution A* algorithm of octree map according to claim 1, characterized in that: Step S3 performs a second path planning at a higher resolution at the current depth. The second path planning generates a path based on an adaptive multi-resolution A* algorithm. The specific contents of the adaptive multi-resolution A* algorithm are as follows: Step S31: Set the lower limit d of the adaptive depth l and upper limit d h Used to adjust the depth of the path node during path planning and initialize the current depth d c The value is equal to d l ; Step S32: At the current depth d c Next, we use the improved adjacent node search strategy to plan the path in the octree map. First, we set the starting point (x start ,y start ,z start ) and the target point ((x goal ,y goal ,z goal )), search the 24 adjacent nodes according to the improved adjacent node search strategy, and put the horizontal nodes that are not judged as obstacles into the open list Openlist, and then find the node with the smallest cost value F(n) from the Openlist. The cost value formula is: F(n)=g(n)+h(n) (1) Where n represents a node in the path search process, g(n) represents the minimum cost from the starting point to the nth node, h(n) represents the estimated cost from the nth node to the target node; g(n) is the accumulation of the single-step Euclidean distance, and the formula is expressed as: In the formula, (x n ,y n ,z n ) is the coordinate of the current point, (x n-1 ,y n-1 ,y n-1 ) is the coordinate of the previous node; h(n) is represented by the Manhattan distance or Euclidean distance from the current point to the target point, but the Euclidean distance is more in line with the distance representation in real life. Therefore, h(n) is represented by the Euclidean distance between the current point and the target point. The formula is: Set the node with the smallest cost value F(n) as the current node, delete it from the Openlist, and add it to the Closelist; cyclically use the improved adjacent node search strategy to search for adjacent points until the target point coordinates are found, and finally generate a path T with a lower resolution l , and adjust the current depth to d c +Δd2; Step 33: If path planning fails at the current depth, adjust the current depth to d c +Δd1, until the path is found, proceed to the next step; Step 34: Add secondary path planning to implement multi-resolution path planning, that is, when d c <d h , and the path identifier is true, select the node with changed resolution according to the variable resolution threshold t, and use this node as the end point of the secondary path planning. c Path planning at +Δd2 depth and obtaining a path T at a higher resolution h .

Images