WO2017173990A1

WO2017173990A1 - Method for planning shortest path in robot obstacle avoidance

Info

Publication number: WO2017173990A1
Application number: PCT/CN2017/079488
Authority: WO
Inventors: 王玉亮; 王晓刚; 乔涛; 王巍; 薛林
Original assignee: 北京进化者机器人科技有限公司
Priority date: 2016-04-07
Filing date: 2017-04-05
Publication date: 2017-10-12
Also published as: CN105716613B; CN105716613A

Abstract

Provided is a method for planning a shortest path in robot obstacle avoidance, comprising the following steps: using a convolution operation to obtain a probability grid map by; improving an original A* algorithm and obtaining a cost function of the improved A* algorithm; using the improved A* algorithm to search for, on the basis of the probability grid map, the shortest path from a starting point to an end point. The method of the present invention for planning a shortest path in robot obstacle avoidance improves an original grid map to obtain a probability grid map, and uses an improved A* algorithm to search for, on the basis of the probability grid map, a shortest path, so that a robot can avoid an obstacle by a certain distance and move more safely and reliably on a path obtained after planning.

Description

Shortest path planning method for robot obstacle avoidance

This application claims priority to Chinese Patent Application No. CN201610213569.5, entitled "A Shortest Path Planning Method for Robotic Obstacle Avoidance", filed on April 7, 2016, the entire contents of which are hereby incorporated by reference. Combined in this application.

Technical field

The invention belongs to the field of robot obstacle avoidance technology, and particularly relates to a shortest path planning method in robot obstacle avoidance.

Background technique

Robot obstacle avoidance path planning refers to selecting a path from the starting point to the target point under given environmental obstacle conditions, so that the robot can pass all obstacles safely and without collision. This method of autonomously avoiding obstacles and completing work tasks is an important part of robot research and application. At present, in order to quickly find an optimal or shortest path from the starting point to the target point, researchers have developed many different algorithms, such as A* algorithm, Dijkstra algorithm, genetic algorithm, particle swarm algorithm and artificial potential field. Law and so on.

Compared with the A* algorithm, Dijkstra algorithm, genetic algorithm and particle swarm algorithm all have the disadvantages of complex data and large computational complexity. The A* algorithm is the best priority algorithm based on the Dijkstra algorithm. The heuristic information related to the problem is added to the search to guide the search in the most promising direction. The shortest path for searching the state space. This is theoretically faster than the non-directional Dijkstra algorithm. However, the optimal path planned by the A* algorithm tends to be close to obstacles and there is no buffer zone between the robots. People prefer to increase the length of the path slightly so that the robot avoids the surrounding obstacles and makes the robot move safely and reliably.

The artificial potential field method is one of the few robot obstacle avoidance path planning methods that considers safety issues. The path planned by the artificial potential field method is generally smooth and safe, but this method has local optimization, that is, the problem of local convergence is easy to occur; and when the positions of the two obstacles are relatively close, according to According to the rules of artificial potential field method, the channel between them cannot pass. Therefore, the path planning using the artificial potential field method at this time either causes the planning to fail due to the obstacles being too close, or it is necessary to surround the obstacles, which leads to planning. The path is too long. In addition, the paths planned by the artificial potential field method are mostly irregular curves, which do not conform to the robot's exercise habits.

Summary of the invention

In order to solve the above problems existing in the prior art, the present invention provides a shortest path planning method in robot obstacle avoidance.

To achieve the above objective, the present invention adopts the following technical solution: a shortest path planning method in robot obstacle avoidance, which includes the following steps:

Obtain a probability raster map using a convolution operation;

The original A* algorithm is improved, and the cost function of the improved A* algorithm is:

f(n)=k ₁ *g(n)+k ₂ *h(n)+k ₃ *k(n),

Where g(n) represents the actual cost from the starting point S to the node n, representing the preferential trend of the search breadth; h(n) represents the estimated cost of the best path from the node n to the target point D, Contains the heuristic information in the search; k(n) represents the probability value corresponding to the grid in the probability raster map; k ₁ is the weight corresponding to the cost g(n), and k ₂ is the weight corresponding to the cost h(n), k ₃ is the weight corresponding to the probability value k(n), where 0<k _i <1(i=1, 2, 3);

Based on the probability raster map, the improved A* algorithm is used to search for the shortest path from the start point to the end point.

Further, the process of obtaining a probability raster map by using a convolution operation is:

(1) Defining a raster map using a two-dimensional array, which includes:

The raster types in the custom raster map are: FREE, OBSTACLE, START, END, and PROBABILITY; FREE means no obstacles at all, OBSTACLE means there are obstacles, START means starting point, END means end point; PROBABILITY is high to low Indicates the likelihood of the presence of an obstacle. The greater the value of PROBABILITY, the greater the likelihood that an obstacle will be present;

The data types of custom 2D arrays are as follows:

Where s_x and s_y represent the coordinates of a grid in the map, s_g and s_h represent the two cost distances in the A* algorithm plan, s_style represents the raster type; struct AStarNode*s_parent defines a pointer of the same type; S_is_in_closetable and int s_is_in_opentable indicate whether a raster has been traversed during the search. If it has been traversed, s_is_in_closetable=1, s_is_in_opentable=0; otherwise, s_is_in_closetable=0, s_is_in_opentable=1;

According to the data type of the custom two-dimensional array, the two-dimensional array is represented as:

AStarNode map_maze[ROW][COLUMN];

(2) Convolution operation on the grid map to obtain a probability raster map, the specific process is:

For raster probabilization of non-edge parts in a raster map, the specific process is:

Select a weight matrix whose number of rows and columns is m and m is an odd number as the convolution kernel. The convolution kernel is expressed as:

In the original raster map, select a matrix G with the number of rows and the number of columns being m and m being an odd number:

The probability value of the center raster of the probability raster map is:

For the raster probability of the edge part of the raster map, the specific process is:

First, the original raster map is expanded, that is, it is expanded outward from the original raster map.

ring;

Secondly, the probability value of each raster in the boundary part of the original raster map is obtained by the same method as the non-edge part raster probability in the raster map.

Further, the process of performing the shortest path search from the start point to the end point using the improved A* algorithm is:

The search positions of the upper, lower, left, right, upper left, lower left, upper right, and lower right directions are specified on the probability raster map. For each search position, the cost function of the improved A* algorithm is used to calculate the generation. Value, the position corresponding to the minimum generation value is defined as the position of the next moment;

For a m*m probability raster map, there are 8 adjacent grids around the center grid z; the 8 adjacent grids are divided into two categories, one is a horizontal or vertical grid, one The class is a diagonal grid; look for a grid b with the smallest distance from the center grid in the eight adjacent grids around the center grid z, and mark the center grid z as the crossed grid To draw a path from the center grid z to the next grid b, and then repeat the above process with the grid b as the parent node;

When performing the next raster search, first determine whether the sum of the distance from the parent node of the current raster to the current grid and the distance from the parent node of the last raster to the last raster is less than the parent node of the last raster to the current The distance of the raster, if it is, the last node of the last raster to the current raster to the current raster as the shortest path, otherwise the shortest path needs to be re-planned; the parent of the current raster is the last raster.

Due to the adoption of the above technical solution, the beneficial effects of the present invention are: the present invention improves the original raster map to obtain a probability raster map, and based on the probability raster map, adopts an improved A* algorithm. By performing the shortest path search, the path obtained by the planning of the present invention enables the robot to avoid a certain distance of the obstacle and make the robot move safely and reliably.

DRAWINGS

1 is a flowchart of a shortest path planning method in a robot obstacle avoidance provided in an embodiment of the present invention;

2 is a schematic diagram of a convolution kernel in a raster map;

3 is a schematic diagram showing the relative positional relationship between the starting point S, the node n, the target point D, and the obstacle;

4 is a schematic diagram showing the relative positional relationship between the center grid z and its surrounding grid;

Figure 5 is a schematic diagram showing the relative positional relationship between the center grid z and the obstacle;

Figure 6 is a schematic diagram of a robot path obtained by using the original A* algorithm and the improved A* algorithm; wherein, (a) is a schematic diagram of the robot path planned by the original A* algorithm; and (b) is an improved Schematic diagram of the robot path obtained by the A* algorithm.

detailed description

The technical solution of the present invention will be described in detail below with reference to the accompanying drawings and embodiments.

As shown in FIG. 1 , the present invention provides a shortest path planning method in robot obstacle avoidance, which includes the following steps:

1) Using a convolution operation to obtain a probability raster map, the specific process is:

(1) Defining a raster map using a two-dimensional array, which includes:

The raster types in the custom raster map are: FREE, OBSTACLE, START, END, and PROBABILITY. Among them, FREE means no obstacles at all, OBSTACLE means that there are obstacles, START means starting point, END means end point; PROBABILITY indicates the possibility of obstacles from high to low, and the larger value of PROBABILITY indicates the possibility of obstacles. The greater the sex.

The data types of custom 2D arrays are as follows:

Where s_x and s_y represent the coordinates of a grid in the map, s_g and s_h represent the two cost distances in the A* algorithm plan, and s_style represents the grid type. Struct AStarNode*s_parent defines a pointer of the same type, so that you can define a linked list to represent the nodes to be stored, which is convenient for computer memory management. Int s_is_in_closetable and int s_is_in_opentable indicates whether a raster has been traversed during the search. If it has been traversed, s_is_in_closetable=1, s_is_in_opentable=0; otherwise, s_is_in_closetable=0, s_is_in_opentable=1.

AStarNode map_maze[ROW][COLUMN].

The probability value of the center raster of the probability raster map is:

For example, as shown in FIG. 2, a weight matrix having a row number and a column number of m=3 is selected as a convolution kernel, and the convolution kernel is expressed as:

In the original raster map, select a matrix G with a row number and a column number of m=3:

The probability value of the center raster of the probability raster map is:

First, the original raster map is expanded, that is, it is expanded outward around the original raster map.

ring. For example, if the original raster map has 90 rows and the number of columns is 100, that is, 90X100, the expanded raster map has 92 rows and the number of columns is 102, that is, 92X102.

Through the above convolution operation on the initial raster map, the values of each raster in the raster map are changed from the original FREE or OBSTACLE values to PROBABILITY (including FREE and OBSTACLE), and the values of the grids in the probability raster map. Not only can it indicate whether there is an obstacle in a certain grid, but also the possibility that there is an obstacle in the grid. The larger the value of the grid, the greater the probability that the grid has obstacles.

2) The original A* algorithm is improved, and the cost function of the improved A* algorithm is:

f(n)=k ₁ *g(n)+k ₂ *h(n)+k ₃ *k(n),

In the formula, as shown in FIG. 3, g(n) represents the actual cost from the starting point S to the node n, and represents a preferential trend of the search breadth. h(n) represents the estimated cost of the best path from node n to target point D, including the heuristic information in the search. k(n) represents the probability value corresponding to the raster in the probability raster map. k ₁ is the weight corresponding to the cost g(n), k ₂ is the weight corresponding to the cost h(n), and k ₃ is the weight corresponding to the probability value k(n), where 0<k _i <1(i=1, 2 , 3).

Simultaneous adjustment of the three weights k ₁ , k ₂ and k ₃ can more effectively adjust the planned robot path. For example, it is possible to control whether the planned route is the shortest distance, whether it is kept at a certain distance from the obstacle, and the robot is safer, etc., as needed. The two generation values of g(n) and h(n) mainly determine whether the planned path is the shortest, and k(n) mainly determines whether the planned robot path keeps a certain distance from the obstacle. Therefore, to obtain the shortest path, adjusting the value of k ₁ and k _2, k ₁ and k ₂ so that both slightly larger than k _3; you want to get away from the obstacle farther path, adjusting the value of k ₁ and k _2, k ₁ so and k ₂ are slightly smaller than k _3.

3) Based on the probability raster map, the improved A* algorithm is used to search for the shortest path from the start point to the end point.

The improved A* algorithm is a raster map-based search algorithm. After converting a raster map into a probability raster map, each raster type includes a start point, an end point, a completely barrier-free area, an obstacle area, and a suspected Obstacle area.

The search positions of the upper, lower, left, right, upper left, lower left, upper right, and lower right directions are specified on the probability raster map. For each search position, the cost function of the improved A* algorithm is used to calculate the generation. Value, the position corresponding to the minimum generation value is defined as the position of the next moment.

During the search, for a m*m probability raster map, there are 8 adjacent grids around the center grid z. The eight adjacent grids are divided into two categories, one is a horizontal or vertical grid, and the other is a diagonal grid. Find a grid b with the smallest distance between the center grid and the center grid z, mark the center grid z as the crossed grid, and draw the center grid. Repeat the above process from grid z to the path between the next grid b and then with grid b as the parent node.

When performing the next raster search, first determine whether the distance between the parent node of the current raster (that is, the last raster) to the current raster and the distance from the parent node of the last raster to the last raster is less than the last time. Grid The distance of the parent node to the current raster. If it is, the parent node of the last raster to the last raster to the current raster is the shortest path, otherwise the shortest path needs to be re-planned.

The above description is exemplified below.

As shown in FIG. 4, during the search process, there are 8 adjacent grids around the grid z, and 8 adjacent grids are divided into two categories, one of which is a horizontal or vertical grid, and the current grid The grid has a distance of 10 from the horizontal or vertical grid; the other is a diagonal grid with a distance of 14 from the current grid to its diagonal grid. Look for a grid with the shortest distance value, such as grid 1, among the eight adjacent grids around the grid z. Then, mark the grid z as the crossed grid, draw the path from the grid z to the next grid 1, and then repeat the process with the grid 1 as the parent node.

As shown in FIG. 5, starting from the grid z, the black part is an obstacle, and the generation value of the grid 1 is the smallest in the first search, but the obstacle on the right side of the grid 1 is the obstacle in the second search. Can not cross, can only move up or down from the grid 1, for example, choose to move down to the grid 8, the cost value from the grid z through the grid 1 to the grid 8 is 10 + 10 = 20. The cost value directly from grid z to grid 8 is 14. Obviously, 14<20, therefore, the path from grid z to grid 1 to grid 8 is not the shortest path and the shortest path needs to be re-planned.

Although the original A* algorithm can guarantee the search for the global optimal path, this is the desired result of finding the shortest path in path planning in the robot field, but considering that the robot should avoid obstacles during the movement, and the robot itself The body has a certain width, and when the robot walks along the shortest path, it often faces the situation of colliding with obstacles. In addition, when the robot passes through a narrow passage, people often want the robot to walk along the road in the middle of the passage, but the actual planned path may move closely along the wall on one side, which often brings unpredictable danger.

As shown in FIG. 6 , FIG. a is a schematic diagram of a robot path planned by using the original A* algorithm, and it can be seen from FIG. a that the planned path is always along the obstacle. Figure b is a schematic diagram of the robot path obtained by the improved A* algorithm. It can be seen from Figure b that the robot path obtained by the improved A* algorithm avoids the problem that the path always follows the obstacle and gives the robot motion. A certain safety distance is set in the process to make the robot movement safer.

The present invention is not limited to the above-described preferred embodiments, and those skilled in the art can derive other various forms of products under the suggestion of the present invention, but with any changes in shape or structure, The technical solutions that are the same as or similar to the present application are all within the scope of the present invention.

Claims

A shortest path planning method in robot obstacle avoidance, characterized in that the shortest path planning method comprises the following steps:

Obtain a probability raster map using a convolution operation;

The original A* algorithm is improved, and the cost function of the improved A* algorithm is:

f(n)=k 1 *g(n)+k 2 *h(n)+k 3 *k(n),

Where g(n) represents the actual cost from the starting point S to the node n, representing the preferential trend of the search breadth; h(n) represents the estimated cost of the best path from the node n to the target point D, Contains the heuristic information in the search; k(n) represents the probability value corresponding to the grid in the probability raster map; k 1 is the weight corresponding to the cost g(n), and k 2 is the weight corresponding to the cost h(n), k 3 is the weight corresponding to the probability value k(n), where 0<k i <1(i=1, 2, 3);

Based on the probability raster map, the improved A* algorithm is used to search for the shortest path from the start point to the end point.
The shortest path planning method for obstacle avoidance in a robot according to claim 1, wherein the process of obtaining a probability raster map by using a convolution operation is:

(1) Defining a raster map using a two-dimensional array, which includes:

The raster types in the custom raster map are: FREE, OBSTACLE, START, END, and PROBABILITY; FREE means no obstacles at all, OBSTACLE means there are obstacles, START means starting point, END means end point; PROBABILITY is high to low Indicates the likelihood of the presence of an obstacle. The greater the value of PROBABILITY, the greater the likelihood that an obstacle will be present;

The data types of custom 2D arrays are as follows:

Where s_x and s_y represent the coordinates of a grid in the map, s_g and s_h represent the two cost distances in the A* algorithm plan, s_style represents the raster type; struct AStarNode*s_parent defines a pointer of the same type; S_is_in_closetable and int s_is_in_opentable indicate whether a raster has been traversed during the search. If it has been traversed, s_is_in_closetable=1, s_is_in_opentable=0; otherwise, s_is_in_closetable=0, s_is_in_opentable=1;

According to the data type of the custom two-dimensional array, the two-dimensional array is represented as:

AStarNode map_maze[ROW][COLUMN];

(2) Convolution operation on the grid map to obtain a probability raster map, the specific process is:

For raster probabilization of non-edge parts in a raster map, the specific process is:

Select a weight matrix whose number of rows and columns is m and m is an odd number as the convolution kernel. The convolution kernel is expressed as:

In the original raster map, select a matrix G with the number of rows and the number of columns being m and m being an odd number:

The probability value of the center raster of the probability raster map is:

For the raster probability of the edge part of the raster map, the specific process is:

First, the original raster map is expanded, that is, it is expanded outward from the original raster map.
ring;

Secondly, the probability value of each raster in the boundary part of the original raster map is obtained by the same method as the non-edge part raster probability in the raster map.
The shortest path planning method in robot obstacle avoidance according to claim 1 or 2, wherein the process of using the improved A* algorithm to perform the shortest path search from the start point to the end point is:

The search positions of the upper, lower, left, right, upper left, lower left, upper right, and lower right directions are specified on the probability raster map. For each search position, the cost function of the improved A* algorithm is used to calculate the generation. Value, the position corresponding to the minimum generation value is defined as the position of the next moment;

For a m*m probability raster map, there are 8 adjacent grids around the center grid z; the 8 adjacent grids are divided into two categories, one is a horizontal or vertical grid, one The class is a diagonal grid; look for a grid b with the smallest distance from the center grid in the eight adjacent grids around the center grid z, and mark the center grid z as the crossed grid To draw a path from the center grid z to the next grid b, and then repeat the above process with the grid b as the parent node;

When performing the next raster search, first determine whether the sum of the distance from the parent node of the current raster to the current grid and the distance from the parent node of the last raster to the last raster is less than the parent node of the last raster to the current The distance of the raster, if it is, the last node of the last raster to the current raster to the current raster as the shortest path, otherwise the shortest path needs to be re-planned; the parent of the current raster is the last raster.