A Probabilistic Atlas Path Planning Method Combining Artificial Potential Field

Abstract

The present invention discloses a path planning method combining artificial potential field and probability map. This method uses the method of virtual potential field to represent the target environment with a quantifiable potential field map, so as to clearly express the obstacle information in the environment. Then, a sampling strategy based on partitioning is proposed, which improves the number of sampling points in the dense obstacle area while maintaining the uniform distribution of sampling points. Finally, a new eight-direction detection method is used to identify and screen some key points, and the success rate of path planning is effectively improved by adding key points. This method can effectively increase the number of sampling points in narrow channels and the success rate of path planning, and the experimental results in MATLAB prove the effectiveness of this method.

Description

Technical Field

[0001] The present invention belongs to the field of path planning, and particularly relates to path planning applicable to a static environment containing narrow channels. Background Art

[0002] In recent years, autonomous mobile robots have been widely used in many fields, such as cargo transportation, search and rescue, underwater cruising, etc. Path planning is one of the most critical technologies that enables a robot to autonomously plan a feasible collision-free path from a starting position to a destination. It can ensure that the planned path is optimal under certain conditions, such as the minimum working cost, the shortest walking route, the shortest walking time, etc. In the past few decades, many methods for robot path planning have been proposed. Among them, the Probabilistic Roadmap Method (PRM) has been widely applied due to its good performance. PRM can transform the path planning problem in a continuous complex environment into a path planning problem in a discrete space, reducing the complexity of path planning. At the same time, through the method of random sampling, it can effectively avoid the influence of the spatial dimension on the algorithm and has probabilistic completeness. At the same time, because of its random sampling characteristics, the sampling probability of each area in the environment is the same. Therefore, when there are narrow channels in the task environment, there will usually be a situation where path planning fails due to insufficient sampling points inside the channels. This is also known as the narrow channel problem. Therefore, how to solve the narrow channel problem and increase the number of effective sampling points in the channel has become the main direction for improving the probabilistic roadmap method.

[0003] To solve the narrow channel problem faced by PRM and improve the effectiveness of sampling points in the channel, the present invention proposes an improved probabilistic roadmap algorithm based on the potential field, which overcomes the narrow channel problem by improving the effectiveness of sampling points and reducing the calculation time. The main contributions of this work are in three aspects. First, using the method of artificial potential field, the target environment is represented by a quantifiable potential field map, thus clearly expressing the obstacle information. Second, a sampling strategy based on partitioning is proposed, which increases the number of sampling points in the dense obstacle area while maintaining the uniform distribution of sampling points. Finally, a new eight-direction detection method is proposed to identify and screen out key points, and by adding some key points, the success rate of path planning is effectively improved. Summary of the Invention

[0004] The object of the present invention is to propose a method that can identify key sampling points inside a narrow channel, increase the number of effective sampling points in the narrow channel by sampling key points, and improve the success rate of path planning, in combination with the idea of artificial potential field for the traditional probabilistic roadmap method. To achieve this object, the steps adopted by the present invention are as follows:

[0005] Step 1: Establish the force field map. According to the known environmental information, the set repulsive force field coefficient η and the repulsive force range D 0 , combined with the repulsive force field formula in the artificial potential field method, calculate the repulsive force field value at each spatial configuration point in the environment to obtain a repulsive force field map of the environment;

[0006] Step 2: Screen key points. Based on the obtained repulsive force field map, first set a maximum detection distance threshold r max , calculate the repulsive force field threshold value rep max at a distance r threshold from the obstacle according to the repulsive force field formula, and screen the spatial configuration points in the environment to initially obtain a set of key points to be screened p cri ; for each point in p cri , further detect and screen it using the eight-direction detection method according to the detection radius r d to obtain three key point sets inside the narrow channel, namely the key point set q passage inside the channel, the key point set q door. at the channel entrance, and the key point set q corner. at the channel corner;

[0007] Step 3: Learning stage. Divide the environmental force field map equally into n x and n y parts in the x and y directions, and calculate the total potential field value P tot (i, j) of each region; define P m as the median of P tot (i, j), and divide all grids into two categories: high potential field value and low potential field value; according to the total number of sampling points required, determine the number of sampling points Nodenum(i, j) required for each region; perform sampling point compensation for the regions completely covered by obstacles; sample sampling points for each divided region respectively, and add key points to the environment at the same time; finally connect all the sampling points to form a connected graph, also called a roadmap.

[0008] Step 4: Query stage. According to the obtained roadmap, try to add the starting point and the target point to the roadmap. First add the starting point and calculate the distances from all nodes in the environment to the starting point. The points within the neighborhood distance D whose distances to the starting point attempt to be connected to the starting point through the local planner. If all points that meet the conditions cannot be connected to the starting point, it means there is no feasible path between the starting point and the target point. The addition of the target point is the same as above. If the starting point and the target point are successfully added to the roadmap, use the A* algorithm to calculate the optimal path on the roadmap.

[0009] The improved probabilistic roadmap method proposed by the present invention has been verified through specific cases implemented in MATLAB. In the simulation experiment, the total number of sampling points set is 80 and 100, and 100 repeated trials are carried out in each case, and the average value of each test index is taken. Attached Figure 1 shows the flowchart of the method proposed by the present invention. Attached Figure 2 shows the schematic diagram of key points for different channel conditions of the present invention. According to the method proposed by the present invention, these key points can be identified and distinguished. Attached Figure 3 The experimental environment model of the present invention, where the dot is the starting point and the diamond point is the ending point. Attached Figure 4 is the comparison result of the success rate achieved by the present invention. Attached Figure 5 is the comparison result of the average path length achieved by the present invention. Attached Figure 6 is the comparison result of the average algorithm time achieved by the present invention. Brief Description of the Drawings

[0010] Figure 1 is the experimental flowchart of the present invention;

[0011] Figure 2 is the schematic diagram of key points for different channel conditions of the present invention;

[0012] Figure 3 is the environmental model of the experiment of the present invention;

[0013] Figure 4 is the comparison result of the success rate between the present invention and other path planning methods;

[0014] Figure 5 is the comparison result of the average path length between the present invention and other path planning methods;

[0015] Figure 6 is the comparison result of the average algorithm time between the present invention and other path planning methods. Detailed Description of the Invention

[0016] The present invention will be further described in detail below with reference to the drawings and embodiments.

[0017] In the following description, this specification abbreviates the path planning method of the improved artificial potential field-based probabilistic roadmap proposed by the present invention as PCPRM (An improved Probabilistic Roadmap Method combined with Potential field and Critical point for path planning). PCPRM first sets the following scenario parameters:

[0018] 1. Total number of sampling points: N. It is the number of environmental sampling points q set for path planning within the working area.

[0019] 2. Domain length: D. It is the maximum length that can form a connected path between two adjacent neighbor nodes when constructing the roadmap.

[0020] 3. Repulsive force field coefficient: η. Also known as repulsive force gain, it is a constant.

[0021] 4. Range of repulsive force action: D 0 . It is the influence range of the repulsive force field generated by the obstacle. Only when the object is less than D away from the obstacle 0 will it be affected by the repulsive force field generated by the obstacle.

[0022] 5. Key point set and classification in narrow channels: q passage represents the set of key points in the channel, q door. represents the set of key points at the channel entrance, q corner. represents the set of key points at the channel corner.

[0023] Based on the above conditions, the PCPRM proposed in the present invention has implemented a specific case in MATLAB, and the implementation results prove the effectiveness of the method. The specific implementation steps of PCPRM are as follows:

[0024] Step 1: Construct the environmental potential field map.

[0025] Since only the environmental information is needed to reasonably distribute the sampling points in the subsequent sampling stage, only the repulsive force field map needs to be constructed in the environment. The repulsive force field formula is

[0026]

[0027] In the formula, η represents the repulsive force field coefficient, also known as the repulsive force gain, which is a constant; D(q) represents the closest distance from the spatial configuration point q to the nearby obstacles; D 0 represents the range of action of the repulsive force generated by the obstacle. Only when the distance between the spatial configuration point q and the obstacle is less than D 0 will the obstacle generate a repulsive force field at the position q. When the spatial configuration point q is outside the action range of the obstacle, that is, when D(q) is greater than D 0 , the magnitude of the repulsive force field generated by the obstacle at its position is 0.

[0028] Step 2: Selection of key points

[0029] 1. Preliminary screening of key points in the environment

[0030] First, set a maximum distance r maxAs the detection threshold distance, according to formula (1), the repulsive field threshold value rep at the distance r from the obstacle is calculated max from the obstacle threshold . Because in the repulsive field map, the repulsive field value at the obstacle is the maximum value, that is

[0031]

[0032] Therefore, the magnitude of the repulsive field rep at the key points in the narrow passage cri satisfies

[0033]

[0034] The set of key points that satisfy this part of the conditions is denoted as p cri .

[0035] 2. Further screen the key points through the eight-direction detection method

[0036] According to the schematic diagram of the key points in different narrow passage cases shown in the appendix Figure 2 , it can be seen that the currently screened key points are the points in the gray area, so further screening is needed to obtain the key points

[0037] (1) Set the detection radius of the eight-direction detection method as r d .

[0038] (2) For each point q in p cri , detect it in eight directions: up, down, left, right, upper left, lower left, upper right, and lower right respectively

[0039] For the point q to be detected, by detecting it in each direction respectively, judge whether an obstacle is detected within the range of length R from the point q d . If an obstacle is detected, record the value of the detection flag corresponding to this detection direction as 1, indicating that an obstacle is detected in this direction, otherwise record it as 0

[0040] (3) After all eight directions are detected, each direction has its corresponding identification value. First, judge the identifications of the four directions of upper left, lower left, upper right, and lower right. The judgment condition is

[0041] left_up&&right_down&&left_down&&right_up = 1 (4)

[0042] The points that satisfy this condition are denoted as q corner. , and at the same time, give a judgment identifier flag to record that this point satisfies formula (4)

[0043] (4) Because at this time q corner.The points in it not only include the points at the corners, but also include some points in the channels. Therefore, it is necessary to judge the direction identifiers in the up, down, left, and right directions, and the judgment condition is

[0044] up&&down&&left&&right = 1 (5)

[0045] Through formula (5), the points q in the channel can be further screened out passage and the points q at the channel entrance door. .

[0046] Step 3: Learning stage. Select sampling points and construct paths.

[0047] (1) Divide the map. Divide the obtained potential field map equally into n x and n y parts in the x and y directions. In different environments, when constructing the roadmap, the number of divided areas can be adjusted according to the environmental characteristics and the neighborhood distance.

[0048] (2) Calculate the total potential field value of each area. For each grid (a total of n x multiplied by n y ), calculate the total potential field of all spatial configuration points in each grid and record it as P tot (i, j).

[0049] (3) Determine the number of sampling points in each area. Define P m as the median of P tot (i, j). Divide all grids into two categories: high potential field value and low potential field value. The grids where P tot (i, j) is greater than P m are recorded as high potential field areas, and the grids where P tot (i, j) is less than P m are recorded as low potential field areas. Then, calculate the number of sampling points in different areas through (6).

[0050]

[0051] Among them, V represents the total number of set sampling points, and a and b are sampling point distribution coefficients, satisfying

[0052] a = 1 + k, b = 1 - k (0 < k < 1) (7)

[0053] (4) Sampling point compensation. According to the total potential field value P tot (i, j) of each area, find the areas completely covered by obstacles, and accumulate the sampling points allocated in these areas to obtain the total number of required compensated sampling points, that is

[0054] Nodenumcomp = ∑ (i，j)∈obstacle area Nodenum(i, j) (8)

[0055] Then evenly disperse the compensation points to the remaining area not covered by obstacles.

[0056] (5) Sampling point sampling. First, calculate a potential field average value P LocalM (i, j) as the threshold value. Then start sampling point sampling within the range of area (i, j). When the potential field value rep at the spatial configuration point q q is less than P LocalM (i, j), add it as a valid sampling point to the sampling point set. When the number of sampling points in an area reaches the specified number, stop sampling in this area and start sampling in the next area.

[0057] (6) Add key points. Assume that the obtained key point set contains N key points, and randomly sample λN key points into the sampling point set. Where λ is a constant coefficient. Because in the previous step of ordinary sampling point sampling, it can ensure that valid sampling points can also be obtained in areas with high obstacle density, that is to say, there are already a certain number of configuration points in the free space inside the narrow channel. So relatively speaking, we need more points located at the entrances and exits of the narrow channel and at the corners of the narrow channel as key points to add. Therefore, here, the key point set is the point q at the entrance and exit of the narrow channel door. and the point q at the corner of the narrow channel corner. .

[0058] (7) Path construction. Connect all sampling points to form a connected graph. The specific method is the same as the traditional PRM.

[0059] Step 4: Query phase. On the obtained roadmap, use the A* algorithm for optimal path planning.

[0060] Similar to the query phase of the traditional PRM algorithm, according to the obtained roadmap, try to add the start point and the target point to the roadmap. First, add the start point and calculate the distances from all nodes in the environment to the start point. The points within the neighborhood distance D whose distances to the start point attempt to be connected to the start point through the local planner. If all points that meet the conditions cannot be connected to the start point, it means there is no feasible path between the start point and the target point. The addition of the target point is the same as above. If the start point and the target point are successfully added to the roadmap, use the A* algorithm to calculate the best path on the roadmap.

[0061] The content not described in detail in this invention application belongs to the prior art well-known to those skilled in the art.

Claims

1. A path planning method based on a probability map combined with an artificial potential field, and the steps adopted are as follows: Step 1: Establish a force field map; according to the known environmental information, the set repulsive force field coefficient η and the repulsive force range D 0 , combined with the repulsive force field formula in the artificial potential field method, calculate the repulsive force field value at each spatial configuration point in the environment to obtain a repulsive force field map of the environment ； Combined with the artificial potential field method, the specific method of quantifying complex and abstract environmental information into specific potential field value information by using obstacle information in the environment is as follows: Because only environmental information is needed in the subsequent sampling stage to reasonably distribute sampling points, only a repulsive force field map needs to be constructed in the environment, and the repulsive force field formula is In the formula, η represents the repulsive force field coefficient, also known as the repulsive force gain, which is a constant; D(q) represents the closest distance from the spatial configuration point q to the nearby obstacles; D 0 represents the action range of the repulsive force generated by the obstacles. Only when the distance between the spatial configuration point q and the obstacles is less than D 0 will the obstacles generate a repulsive force field at the position q. When the spatial configuration point q is outside the action range of the obstacles, that is, when D(q) is greater than D 0 the magnitude of the repulsive force field generated by the obstacles at their positions is 0; Step 2: Filter key points; Based on the repulsive field map obtained, first set a maximum detection distance threshold r max According to the repulsive field formula, the distance from the obstacle r is calculated. max The repulsive field threshold value rep threshold , the spatial configuration points in the environment are screened to initially obtain the key point set p to be screened cri ; For p cri For each point in the d The eight-direction detection method is used to further detect and screen it, and three key point sets inside the narrow channel are obtained, namely, the key point set q in the channel passage , the key point set q at the channel entrance door. , the key point set q at the channel corner corner. ; The specific method of using the eight-direction detection method to screen the key point set of the narrow channel is: (1) Initially screen key points in the environment First, set a maximum distance r max As the detection threshold distance, according to formula (1), calculate the repulsive force field threshold value rep at a distance r from the obstacle max from the obstacle threshold , because in the repulsive force field map, the repulsive force field value at the obstacle is the maximum value, that is Therefore, the magnitude of the repulsive force field rep at the key points in the narrow channel cri satisfies The set of key points that meet the conditions of this part is denoted as p cri ; (2) Further screen key points by the eight-direction detection method According to the schematic diagram of key points in different narrow channel situations shown in Figure 2, the currently screened key points are the points in the gray area, so further screening is needed to obtain key points; ①Set the detection radius of the eight-direction detection method to r d ; ②For each point q in p cri perform detections in eight directions: up, down, left, right, upper left, lower left, upper right, and lower right respectively; For the point q to be detected, by detecting in each direction of the point q respectively, it is judged whether an obstacle is detected within the range of the length R_d from the point q. If an obstacle is detected, the value of the detection flag corresponding to this detection direction is recorded as 1, indicating that an obstacle is detected in this direction, otherwise it is recorded as 0; ③ After all eight directions are detected, each direction has its corresponding flag value; first, judge the flags of the four directions of upper left, lower left, upper right, and lower right, and the judgment condition is left_up&&right_down&&left_down&&right_up=1 (4) The points that satisfy this condition are denoted as q corner. , and at the same time, a judgment identifier flag is given to record that this point satisfies the formula (4); ④ Since at this time q corner. contains not only the points at the corners but also some points in the channels, it is necessary to judge the direction identifiers in the up, down, left, and right directions, and the judgment condition is up&&down&&left&&right=1 (5) Through formula (5), the point q in the channel can be further screened out passage and the point q at the channel opening door. ; Step 3: Learning stage; Divide the environmental potential field map into n equal parts in the x and y directions x and n y parts, and calculate the total potential field value P tot (i, j); Define P m as the median of P tot (i, j), and divide all grids into two categories: high potential field value and low potential field value; According to the total number of required sampling points, determine the number of sampling points Nodenum(i, j) required for each area; Perform sampling point compensation for the areas completely covered by obstacles; Sample the sampling points for each divided area respectively, and add key points to the environment at the same time; Finally, connect all the sampling points to form a connected graph, also called a roadmap; Step 4: Query stage; According to the obtained roadmap, try to add the starting point and the target point to the roadmap; first add the starting point, and calculate the distances from all nodes in the environment to the starting point; the points within the neighborhood distance D from the starting point that attempt to be connected to the starting point through the local planner; if all points that meet the conditions cannot be connected to the starting point, it means that there is no feasible path between the starting point and the target point; the addition of the target point is the same as above; If the starting point and the target point are successfully added to the roadmap, use the A* algorithm to calculate the optimal path on the roadmap.

2. A path planning method based on a probability map combined with an artificial potential field according to claim 1, characterized in that The specific method of adopting a strategy based on regional sampling for sampling point compensation and key point addition is as follows: (1) Divide the map; equally divide the obtained potential field map into n parts in the x and y directions. x and n y parts. In different environments, when constructing the roadmap, adjust the number of divided regions according to the environmental characteristics and neighborhood distance. (2) Calculate the total potential field value of each region; for each grid, a total of n x Multiply by n y , calculate the total potential field of all spatial configuration points in each grid, and record it as P tot (i, j); (3) Determine the number of sampling points in each region; Let P m be defined as the median of P tot (i, j), and divide all grids into two categories: high potential field values and low potential field values; Grids where P tot (i, j) is greater than P m are denoted as high potential field regions, and grids where P tot (i, j) is less than P m are denoted as low potential field regions, and then calculate the number of sampling points in different regions through (6); Among them, V represents the total number of sampling points set, and a and b are sampling point distribution coefficients, satisfying a = 1 + k, b = 1 - k (0 < k < 1) (7) (4) Sampling point compensation; according to the total potential field value P of each area tot (i, j), find the areas completely covered by obstacles, and accumulate the sampling points allocated in these areas to obtain the total number of required compensated sampling points, that is Node number comp = ∑ (i，j)∈obstacle area Node number(i, j) (8) Then evenly disperse the compensation points to the remaining areas not covered by obstacles; (5) Sampling at sampling points; first, calculate an average potential field value P for each region (i, j), use P(i, j) as the threshold value, and then start sampling at sampling points within the region (i, j); when the potential field value rep at the spatial configuration point q is less than P(i, j), add it to the sampling point set as a valid sampling point; when the number of sampling points in a region reaches the specified number, stop sampling in that region and start sampling in the next region. LocalM (i, j) is used as the threshold value, and then sampling at sampling points starts within the region (i, j); when the potential field value rep at the spatial configuration point q q is less than P LocalM (i, j), add it to the sampling point set as a valid sampling point; when the number of sampling points in a region reaches the specified number, stop sampling in that region and start sampling in the next region; (6) Add key points; assume that there are N key points in the obtained key point set, and randomly sample λN key points into the sampling point set, where λ is a constant coefficient; because in the previous step of ordinary sampling point sampling, it can be ensured that effective sampling points are also obtained in areas with high obstacle density, that is to say, there are already a certain number of configuration points in the free space inside the narrow channel; so relatively speaking, points located at the entrances and exits of the narrow channel and at the corners of the narrow channel are more needed to be added as key points; so here, the key point set is the point q at the entrance and exit of the narrow channel door. and the point q at the corner of the narrow channel corner. .

Images