CN105716613A - Method for planning shortest path in robot obstacle avoidance - Google Patents

Abstract

The invention relates to a method for planning the shortest path in robot obstacle avoidance. The method comprises steps as follows: a probability grid map is acquired through convolution operation; an original A*algorithm is improved, and a cost function of the improved A*algorithm is obtained; on the basis of the probability grid map, the improved A*algorithm is adopted for shortest path searching from the starting point to the end point. The probability grid map is obtained through improvement of the original grid map, and the shortest path searching is performed in the improved A*algorithm on the basis of the probability grid map. According to a path planned with the method, a robot can avoid an obstacle for a certain distance and can more safely and reliably move.

Description

A kind of shortest path planning method in robot obstacle-avoiding

Technical field

The invention belongs to robot obstacle-avoiding technical field, be specifically related to the shortest path planning method in a kind of robot obstacle-avoiding.

Background technology

Robot obstacle-avoiding route planning refers to when given Environment Obstacles, selects a path from starting point to impact point, makes robot can pass through all of obstacle safely, without collision.The method of this independently avoiding barrier the task that fulfils assignment, is an important content in robot research and application.At present, in order to be quickly found an optimum from starting point to impact point or shortest path, researcheres have been developed over much different algorithms, for instance A* algorithm, dijkstra's algorithm, genetic algorithm, particle cluster algorithm and Artificial Potential Field Method etc..

Compared with A* algorithm, all there is the shortcomings such as data are complicated, computationally intensive in dijkstra's algorithm, genetic algorithm and particle cluster algorithm.A* algorithm is the best-first search algorithm having added constraints on dijkstra's algorithm basis, add the enlightening information relevant with problem in the search, guidance search carries out towards most promising direction, for the shortest path in search condition space, this is rapider in theory than direction-free dijkstra's algorithm.But, adopt the optimal path that A* algorithmic rule goes out often adjacent with barrier, and between robot, lack buffer zone.People are more desirable to slightly increase path and make robot avoid peripheral obstacle, make robot move more safely and reliably.

Artificial Potential Field Method is few in number robot obstacle-avoiding route planning method considering safety problem.Adopt the path that Artificial Potential Field Method is planned out to be usually smoother and safe, but this method exists local optimum, problem local convergence namely easily occur；And when two Obstacle Positions relatively time, according to Artificial Potential Field Method rule, passage between them is intransitable, thus now utilize Artificial Potential Field Method carry out path planning or excessively closely cause planning failure due to barrier, will go the long way round along barrier periphery, cause that the path planning out is long.Additionally, adopt the path that Artificial Potential Field Method is planned out mostly to be irregular curve, do not meet the motor habit of robot.

Summary of the invention

In order to solve the problems referred to above that prior art exists, the invention provides the shortest path planning method in a kind of robot obstacle-avoiding.

For achieving the above object, the present invention takes techniques below scheme: the shortest path planning method in a kind of robot obstacle-avoiding, and it comprises the following steps:

Convolution algorithm is adopted to obtain Probabilistic Cell map；

Original A* algorithm is improved, and the cost function of the A* algorithm after being improved is:

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

In formula, g (n) represents from the actual cost starting point S to node n, represents the preferential trend of search range；H (n) represents the estimate cost from the optimal path node n to impact point D, contains the heuristic information in search；K (n) represents the probit that in Probabilistic Cell map, grid is corresponding；K₁For the weight that cost g (n) is corresponding, k₂For the weight that cost h (n) is corresponding, k₃For the weight that probit k (n) is corresponding, wherein 0 < k_i< 1 (i=1,2,3)；

On the basis of Probabilistic Cell map, the A* algorithm after improving is adopted to carry out the Shortest Path Searching from origin-to-destination.

Further, the process of described employing convolution algorithm acquisition Probabilistic Cell map is:

(1) two-dimensional array definition grating map is utilized, comprising:

Lattice types in self-defined grating map is: FREE, OBSTACLE, START, END and PROBABILITY；FREE represents and represents entirely without barrier, and OBSTACLE represents and determines there is barrier, and START represents starting point, and END represents terminal；PROBABILITY represents that barrier there is a possibility that size from high to low, and the more big expression barrier of the value of PROBABILITY there is a possibility that more big；

The data type of self-defined two-dimensional array is as follows:

Wherein, s_x and s_y represents certain grid coordinate in map, and s_g and s_h represents two cost distances in A* algorithmic rule, and s_style represents lattice types；StructAStarNode*s_parent defines a same kind of pointer；Ints_is_in_closetable and ints_is_in_opentable represents whether some grid had stepped through in search procedure, if had stepped through, then and s_is_in_closetable=1, s_is_in_opentable=0；Otherwise, s_is_in_closetable=0, s_is_in_opentable=1；

Data type according to self-defining two-dimensional array, is expressed as two-dimensional array:

AStarNodemap_maze [ROW] [COLUMN]；

(2) grating map being done convolution algorithm, obtain Probabilistic Cell map, its detailed process is:

Carrying out grid randomization for non-edge part in grating map, its detailed process is:

Select a line number and columns to be weight matrix that m and m is odd number is as convolution kernel, and convolution kernel is expressed as:

$R=\left[\begin{array}{cccc}{R}_{11}& R_{12}& \mathrm{...}& R_{1m}\\ R_{21}& R_{22}& \mathrm{...}& R_{2m}\\ .& & & \\ .& & & \\ .& & & \\ R_{m1}& R_{m2}& \mathrm{...}& R_{mm}\end{array}\right],$

In original grating map, one line number of corresponding selection and columns are the matrix G that m and m is odd number:

$G=\left[\begin{array}{cccc}{G}_{11}& G_{12}& \mathrm{...}& G_{1m}\\ G_{21}& G_{22}& \mathrm{...}& G_{2m}\\ .& & & \\ .& & & \\ .& & & \\ G_{m1}& G_{m2}& \mathrm{...}& G_{mm}\end{array}\right],$

Then the probit of the center grates of Probabilistic Cell map is:

$R_{\frac{m+1}{2}\frac{m+1}{2}}^{*}=R_{11}{G}_{11}+R_{12}{G}_{12}+...+R_{\frac{m+1}{2}\frac{m+1}{2}}{G}_{\frac{m+1}{2}\frac{m+1}{2}}+...+R_{mm}{G}_{mm};$

Carrying out grid randomization for marginal portion in grating map, its detailed process is:

First, original grating map is done expansion process, namely outwards expand in the surrounding of original grating mapCircle；

Secondly, the method identical with non-edge part grid randomization in grating map is adopted to obtain the probit of each grid in the boundary member of original grating map.

Further, the process that the described A* algorithm adopted after improving carries out the Shortest Path Searching from origin-to-destination is:

Probabilistic Cell map specifies the searching position in upper and lower, left and right, upper left, lower-left, eight directions of upper right and bottom right, the searching position in each direction is all used to the cost function calculation cost value of the A* algorithm after improvement, the position corresponding to minimum cost value is decided to be the position of subsequent time；

For the Probabilistic Cell map of a m*m, around its center grates z, there are 8 adjacent grids；These 8 adjacent grids are divided into two classes, and a class is horizontally or vertically grid, and a class is diagonal angle grid；8 adjacent grids around center grates z are found the grid b that between and center grates, distance value is minimum, center grates z is labeled as the grid passed by, draw from the path center grates z to next grid b, then using grid b as father node, repeat said process；

Carry out next time raster search time, first determine whether the father node of current grid to current grid distance with last time grid father node to last time grid distance sum whether less than last time grid father node to the distance of current grid, if, then using last time grid father node to last time grid to current grid as shortest path, otherwise need again to plan shortest path；The father node of current grid is grid last time.

Owing to adopting above technical scheme, the invention have the benefit that original grating map is improved by the present invention and obtain Probabilistic Cell map, and on the basis of Probabilistic Cell map, the A* algorithm improved is adopted to carry out Shortest Path Searching, adopt the path that present invention planning obtains can make robot avoiding obstacles certain distance, make robot move more safely and reliably.

Accompanying drawing explanation

Fig. 1 is the flow chart of the shortest path planning method in the robot obstacle-avoiding provided in one embodiment of the invention；

Fig. 2 is the schematic diagram of convolution kernel in grating map；

Fig. 3 is starting point S, node n, relative position relation schematic diagram between impact point D and barrier；

Fig. 4 is center grates z and relative position relation schematic diagram between grid about；

Fig. 5 is the relative position relation schematic diagram between center grates z and barrier；

Fig. 6 is the robot path schematic diagram that the A* algorithmic rule after adopting original A* algorithm and improving obtains；In, figure (a) is the robot path schematic diagram adopting original A* algorithmic rule to obtain；Figure (b) is the robot path schematic diagram adopting the A* algorithmic rule after improving to obtain.

Detailed description of the invention

Below in conjunction with drawings and Examples, technical scheme is described in detail.

As it is shown in figure 1, the shortest path planning method that the invention provides in a kind of robot obstacle-avoiding, it comprises the following steps:

1) adopting convolution algorithm to obtain Probabilistic Cell map, its detailed process is:

(1) two-dimensional array definition grating map is utilized, comprising:

Lattice types in self-defined grating map is: FREE, OBSTACLE, START, END and PROBABILITY.Wherein, FREE represents and represents entirely without barrier, and OBSTACLE represents and determines there is barrier, and START represents starting point, and END represents terminal；PROBABILITY represents that barrier there is a possibility that size from high to low, and the more big expression barrier of the value of PROBABILITY there is a possibility that more big.

The data type of self-defined two-dimensional array is as follows:

Wherein, s_x and s_y represents certain grid coordinate in map, and s_g and s_h represents two cost distances in A* algorithmic rule, and s_style represents lattice types.StructAStarNode*s_parent defines a same kind of pointer, such that it is able to one chained list of definition represents the node to store, facilitates calculator memory to manage.Ints_is_in_closetable and ints_is_in_opentable represents whether some grid had stepped through in search procedure, if had stepped through, then and s_is_in_closetable=1, s_is_in_opentable=0；Otherwise, s_is_in_closetable=0, s_is_in_opentable=1.

Data type according to self-defining two-dimensional array, is expressed as two-dimensional array:

AStarNodemap_maze[ROW][COLUMN]。

(2) grating map being done convolution algorithm, obtain Probabilistic Cell map, its detailed process is:

Carrying out grid randomization for non-edge part in grating map, its detailed process is:

Select a line number and columns to be weight matrix that m and m is odd number is as convolution kernel, and convolution kernel is expressed as:

$R=\left[\begin{array}{cccc}{R}_{11}& R_{12}& \mathrm{...}& R_{1m}\\ R_{21}& R_{22}& \mathrm{...}& R_{2m}\\ .& & & \\ .& & & \\ .& & & \\ R_{m1}& R_{m2}& \mathrm{...}& R_{mm}\end{array}\right],$

In original grating map, one line number of corresponding selection and columns are the matrix G that m and m is odd number:

$G=\left[\begin{array}{cccc}{G}_{11}& G_{12}& \mathrm{...}& G_{1m}\\ G_{21}& G_{22}& \mathrm{...}& G_{2m}\\ .& & & \\ .& & & \\ .& & & \\ G_{m1}& G_{m2}& \mathrm{...}& G_{mm}\end{array}\right],$

Then the probit of the center grates of Probabilistic Cell map is:

$R_{\frac{m+1}{2}\frac{m+1}{2}}^{*}=R_{11}{G}_{11}+R_{12}{G}_{12}+...+R_{\frac{m+1}{2}\frac{m+1}{2}}{G}_{\frac{m+1}{2}\frac{m+1}{2}}+...+R_{mm}{G}_{mm}.$

Such as, as in figure 2 it is shown, select a line number and columns to be the weight matrix of m=3 as convolution kernel, convolution kernel is expressed as:

$R=\left[\begin{array}{ccc}{R}_1& R_2& R_3\\ R_4& R_5& R_6\\ R_7& R_8& R_9\end{array}\right],$

In original grating map, one line number of corresponding selection and columns are the matrix G of m=3:

$G=\left[\begin{array}{ccc}{G}_1& G_2& G_3\\ G_4& G_5& G_6\\ G_7& G_8& G_9\end{array}\right],$

Then the probit of the center grates of Probabilistic Cell map is:

$R_5^{*}=R_1{G}_1+R_2{G}_2+R_3{G}_3+R_4{G}_4+R_5{G}_5+R_6{G}_6+R_7{G}_7+R_8{G}_8+R_9{G}_9.$

Carrying out grid randomization for marginal portion in grating map, its detailed process is:

First, original grating map is done expansion process, namely in each outside expansion of the surrounding of original grating mapCircle.Such as, the line number of original grating map is 90, and columns is 100, i.e. 90X100, then the line number of the grating map after expanding is 92, and columns is 102, i.e. 92X102.

Secondly, the method identical with non-edge part grid randomization in grating map is adopted to obtain the probit of each grid in the boundary member of original grating map.

By the above-mentioned convolution algorithm to initial map, in grating map, the value of each grid is become PROBABILITY (including FREE and OBSTACLE) from original two values of FREE or OBSTACLE, in Probabilistic Cell map, the value of each grid can not only represent that certain grid is either with or without barrier, can also represent that this grid exists the probability of barrier, the value of grid is more big, represents that this grid exists the probability of barrier more big.

2) original A* algorithm being improved, the cost function of the A* algorithm after being improved is:

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

In formula, as it is shown on figure 3, g (n) represents from the actual cost starting point S to node n, represent the preferential trend of search range.H (n) represents the estimate cost from the optimal path node n to impact point D, contains the heuristic information in search.K (n) represents the probit that in Probabilistic Cell map, grid is corresponding.K₁For the weight that cost g (n) is corresponding, k₂For the weight that cost h (n) is corresponding, k₃For the weight that probit k (n) is corresponding, wherein 0 < k_i< 1 (i=1,2,3).

Regulate k simultaneously₁、k₂And k₃Three weights can the robot path cooked up of significantly more efficient adjustment.For example, it is possible to whether the path controlling as required to cook up is that distance is the shortest, if keep certain distance with barrier, make robot safer.Whether the path that wherein g (n) and two cost value major decisions of h (n) are planned is the shortest, and whether the robot path that k (n) major decision is planned keeps certain distance with barrier.Therefore, if expecting shortest path, regulate k₁And k₂Value, make k₁And k₂All slightly larger than k₃；If expecting from barrier path farther out, regulate k₁And k₂Value, make k₁And k₂All it is slightly less than k₃。

3) on the basis of Probabilistic Cell map, the A* algorithm after improving is adopted to carry out the Shortest Path Searching from origin-to-destination.

A* algorithm after improvement is based on the searching algorithm of grating map, after grating map is transformed into Probabilistic Cell map, the kind of each grid just comprise starting point, terminal, entirely without barrier zone, determine barrier region and doubtful barrier region.

Probabilistic Cell map specifies the searching position in upper and lower, left and right, upper left, lower-left, eight directions of upper right and bottom right, the searching position in each direction is all used to the cost function calculation cost value of the A* algorithm after improvement, the position corresponding to minimum cost value is decided to be the position of subsequent time.

In search procedure, for the Probabilistic Cell map of a m*m, around its center grates z, there are 8 adjacent grids.These 8 adjacent grids are divided into two classes, and a class is horizontally or vertically grid, and a class is diagonal angle grid.8 adjacent grids around center grates z are found the grid b that between and center grates, distance value is minimum, center grates z is labeled as the grid passed by, draw from the path center grates z to next grid b, then using grid b as father node, repeat said process.

Carry out next time raster search time, first determine whether the father node (i.e. grid last time) of current grid to current grid distance with last time grid father node to last time grid distance sum whether less than last time grid father node to the distance of current grid, if, then using last time grid father node to last time grid to current grid as shortest path, otherwise need again to plan shortest path.

Below above description is illustrated.

As shown in Figure 4, in search procedure, having 8 adjacent grids around grid z, 8 adjacent grids are divided into two big classes, and one type is horizontally or vertically grid, and current grid is 10 to the distance of grid horizontal or vertical with it；Another kind of for diagonal angle grid, current grid is 14 to the distance of its diagonal angle grid.8 adjacent grids around grid z are found a grid that distance value is the shortest with it, for instance grid 1.Then, grid z is labeled as the grid passed by, draws the path from grid z to next grid 1, then with grid 1 for father node, repeat this process.

As shown in Figure 5, search is started from grid z, black part is divided into barrier, in first time search procedure, the cost value of grid 1 is minimum, but during second time search, the right side of grid 1 is barrier, it is impossible to passes through, upwardly or downwardly can only move from grid 1, such as select be moved downwardly to grid 8, now from the grid z cost value arriving grid 8 through grid 1 be 10+10=20.And directly from grid z to grid, the cost value of 8 is 14.Obviously, 14 < 20, therefore, from grid z to grid, 1 is not shortest path to the path of grid 8 again, it is necessary to again plan shortest path.

Although original A* algorithm ensure that and searches global optimum path, this also finds the result that shortest path is desired just in robot field during path planning, it is contemplated that robot wants avoidance in motor process, and the car body of robot own has one fixed width, often it is faced with situation about colliding with barrier when robot walks along shortest path.Additionally, when robot is by slype, people are often desirable to robot and can walk along the road in the middle of passage, but the path of physical planning is likely to move closely along the wall of certain side, and this often brings unpredictable danger.

As shown in Figure 6, wherein, figure a is the robot path schematic diagram adopting original A* algorithmic rule to obtain, it can be seen that the path cooked up is all along barrier from figure a.Figure b is the robot path schematic diagram adopting the A* algorithmic rule after improving to obtain, can be seen that from figure b adopts the robot path that the A* algorithmic rule after improving obtains to avoid path all along this problem of barrier, to robot kinematics leaves certain safe distance, make robot motion safer.

The present invention is not limited to above-mentioned preferred forms; those skilled in the art can draw other various forms of products under the enlightenment of the present invention; no matter but in its shape or structure, do any change; every have same or like with the application like technical scheme, all fall within protection scope of the present invention.

Claims (3)

1. the shortest path planning method in robot obstacle-avoiding, it comprises the following steps:

Convolution algorithm is adopted to obtain Probabilistic Cell map；

Original A* algorithm is improved, and the cost function of the A* algorithm after being improved is:

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

In formula, g (n) represents from the actual cost starting point S to node n, represents the preferential trend of search range；H (n) represents the estimate cost from the optimal path node n to impact point D, contains the heuristic information in search；K (n) represents the probit that in Probabilistic Cell map, grid is corresponding；K₁For the weight that cost g (n) is corresponding, k₂For the weight that cost h (n) is corresponding, k₃For the weight that probit k (n) is corresponding, wherein 0 < k_i< 1 (i=1,2,3)；

On the basis of Probabilistic Cell map, the A* algorithm after improving is adopted to carry out the Shortest Path Searching from origin-to-destination.

2. the shortest path planning method in a kind of robot obstacle-avoiding as claimed in claim 1, it is characterised in that: described employing convolution algorithm obtains the process of Probabilistic Cell map and is:

(1) two-dimensional array definition grating map is utilized, comprising:

Lattice types in self-defined grating map is: FREE, OBSTACLE, START, END and PROBABILITY；FREE represents and represents entirely without barrier, and OBSTACLE represents and determines there is barrier, and START represents starting point, and END represents terminal；PROBABILITY represents that barrier there is a possibility that size from high to low, and the more big expression barrier of the value of PROBABILITY there is a possibility that more big；

The data type of self-defined two-dimensional array is as follows:

Wherein, s_x and s_y represents certain grid coordinate in map, and s_g and s_h represents two cost distances in A* algorithmic rule, and s_style represents lattice types；StructAStarNode*s_parent defines a same kind of pointer；Ints_is_in_closetable and ints_is_in_opentable represents whether some grid had stepped through in search procedure, if had stepped through, then and s_is_in_closetable=1, s_is_in_opentable=0；Otherwise, s_is_in_closetable=0, s_is_in_opentable=1；

Data type according to self-defining two-dimensional array, is expressed as two-dimensional array:

AStarNodemap_maze [ROW] [COLUMN]；

(2) grating map being done convolution algorithm, obtain Probabilistic Cell map, its detailed process is:

Carrying out grid randomization for non-edge part in grating map, its detailed process is:

Select a line number and columns to be weight matrix that m and m is odd number is as convolution kernel, and convolution kernel is expressed as:

$R=\left[\begin{array}{cccc}{R}_{11}& R_{12}& ...& R_{1m}\\ R_{21}& R_{22}& ...& R_{2m}\\ \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}\\ \\ \end{array}& \begin{array}{c}\\ \\ \end{array}& \begin{array}{c}\\ \\ \end{array}\\ R_{m1}& R_{m2}& ...& R_{mm}\end{array}\right],$

In original grating map, one line number of corresponding selection and columns are the matrix G that m and m is odd number:

$G=\left[\begin{array}{cccc}{G}_{11}& G_{12}& \mathrm{...}& G_{1m}\\ G_{21}& G_{22}& \mathrm{...}& G_{2m}\\ .& & & \\ .& & & \\ .& & & \\ G_{m1}& G_{m2}& \mathrm{...}& G_{mm}\end{array}\right],$

Then the probit of the center grates of Probabilistic Cell map is:

$R_{\frac{m+1}{2}\frac{m+1}{2}}^{*}=R_{11}{G}_{11}+R_{12}{G}_{12}+...+R_{\frac{m+1}{2}\frac{m+1}{2}}{G}_{\frac{m+1}{2}\frac{m+1}{2}}+...+R_{mm}{G}_{mm};$

Carrying out grid randomization for marginal portion in grating map, its detailed process is:

First, original grating map is done expansion process, namely outwards expand in the surrounding of original grating mapCircle；

Secondly, the method identical with non-edge part grid randomization in grating map is adopted to obtain the probit of each grid in the boundary member of original grating map.

3. the shortest path planning method in a kind of robot obstacle-avoiding as claimed in claim 1 or 2, it is characterised in that: the process that the described A* algorithm adopted after improving carries out the Shortest Path Searching from origin-to-destination is:

Probabilistic Cell map specifies the searching position in upper and lower, left and right, upper left, lower-left, eight directions of upper right and bottom right, the searching position in each direction is all used to the cost function calculation cost value of the A* algorithm after improvement, the position corresponding to minimum cost value is decided to be the position of subsequent time；

For the Probabilistic Cell map of a m*m, around its center grates z, there are 8 adjacent grids；These 8 adjacent grids are divided into two classes, and a class is horizontally or vertically grid, and a class is diagonal angle grid；8 adjacent grids around center grates z are found the grid b that between and center grates, distance value is minimum, center grates z is labeled as the grid passed by, draw from the path center grates z to next grid b, then using grid b as father node, repeat said process；

Carry out next time raster search time, first determine whether the father node of current grid to current grid distance with last time grid father node to last time grid distance sum whether less than last time grid father node to the distance of current grid, if, then using last time grid father node to last time grid to current grid as shortest path, otherwise need again to plan shortest path；The father node of current grid is grid last time.