A robot path planning sampling method for joining candidate expansion queue

Abstract

The present invention discloses a robot path planning sampling method that incorporates a candidate expansion queue, comprising the following steps: obtaining a candidate expansion queue based on the positional relationship between a starting point and a target point; initializing a random tree; forming a guide path to the target point; searching the random tree for the node Qnearest closest to the expansion point; generating a new node; determining whether a collision occurs between nodes Qnear and Qnew and an obstacle; and determining whether the target point Qgoal or any other node in the guide path has been reached. Because the present invention incorporates a new candidate expansion queue, probabilistic expansion is no longer limited to the target point, resolving the problem with the original Goal-based RRT method of easily falling into a local optimum when approaching the target point. Because the present invention incorporates a new target point guide path, it can generate an initial path more quickly and ensure path quality.

Description

Technical Field

[0001] The present invention belongs to the technical field of mobile robot task planning, and in particular relates to a robot path planning sampling method for adding a candidate expansion queue. Background Art

[0002] Path planning is a core component of autonomous robot navigation. It involves generating a collision-free, feasible path from a starting point to a target location in a given environment. Common path planning methods include heuristic algorithms and sampling algorithms. The rapidly expanding random tree (RRT) is a classic sampling algorithm that uniformly searches the space to find a connected path. Essentially, the algorithm's random tree expansion tends to focus on the exploration area within the environment. It generates random points in the environment and finds the node closest to the random point in the random tree. The direction of the line connecting the two nodes is used as the direction for new node growth, and new nodes are generated with a fixed step size. This expansion process is repeated until the target point is added to the random tree, resulting in a complete path. However, the RRT algorithm has two unavoidable issues: first, the generated tree may find a node very close to the target, but it is not connected to the target due to the random nature of the sampling; second, it increases the number of calls to the node generation program, adding unnecessary branches to the tree. In order to improve these two defects of the RRT algorithm, someone proposed a Goal-biasRRT algorithm based on probability targets, which speeds up the algorithm by randomly selecting target points as expansion points with a certain probability.

[0003] Goal-bias RRT's strategy of selecting the target point as a sampling point with a certain probability not only guides the random tree toward the target but also prioritizes the growth of a branch, which facilitates rapid path planning. However, Goal-bias RRT's probability of selecting the target point as a sampling point is fixed. When the random tree falls into a local minimum, selecting the target point as an extension of the sampling point is generally ineffective, which wastes a lot of computing resources.

[0004] A complete path planning algorithm should meet the requirements of both path quality and algorithm performance. In terms of path quality, a short and sufficiently smooth path should be obtained to meet the robot's tracking requirements. Algorithm performance is reflected in the length of program running time. The probability-based Goal-biasRRT algorithm significantly improves algorithm performance compared to the RRT algorithm. However, the path quality generated by the Goal-biasRRT algorithm is mostly inferior to that of the RRT algorithm. In addition, the random tree of the Goal-biasRRT algorithm will fall into a local minimum in some scenarios, which has a huge impact on algorithm performance. Summary of the Invention

[0005] To solve the above problems existing in the prior art, the present invention aims to design a robot path planning sampling method for adding a candidate expansion queue that can not only improve the algorithm performance but also meet the path quality requirements.

[0006] To achieve the above object, the basic idea of the present invention is as follows: A set of candidate expansion queues (including the target point) are selected near the target point according to a certain rule. Each time when reselecting an expansion point, there is a certain probability P to select the head node in the candidate expansion queue as the expansion point, and then insert the node at the end of the queue. Correspondingly, each time when expanding, there is a certain probability 1 - P to use a random point as the expansion point.

[0007] The technical solution of the present invention is as follows: A robot path planning sampling method for adding a candidate expansion queue, comprising the following steps:

[0008] A. Obtain a candidate expansion queue according to the positional relationship between the starting point and the target point

[0009] Suppose the black solid line frame represents the given environment, the large dot at the lower left corner represents the starting point Qstart, the large dot at the upper right corner represents the target point Qgoal, and the length of the line segment L connecting the starting point and the target point is m; and with the target point Qgoal as an end point, extend a line segment outward at an interval of angle α on both sides of the line segment L, and a total of four line segments L1, L2, L3, L4 are extended; then use the target point Qgoal as the center of the circle, and make circles with radii R1 and R2 respectively, where R1 < R2, and the two circles intersect with the line segments L1, L2, L3, L4 to generate a total of 8 intersection points; use these 8 intersection points as candidate expansion points, and form candidate expansion point queues Q11, Q12, Q13, Q14, Q21, Q22, Q23, Q24, Qgoal with the target point Qgoal;

[0010] The coordinate representation of the candidate expansion points is as follows: Let the abscissa of the target point Qgoal in the natural coordinate system be Xg, and the ordinate be Yg, then the abscissa X1n and ordinate Y1n of the candidate expansion point Q1n are represented as follows:

[0011]

[0012]

[0013] The abscissa X2n and ordinate Y2n of the candidate expansion point Q2n are represented as follows:

[0014] <\(0000033\)><\(0000034\)><\(0000035\)><\(0000036\)><\(0000037\)><\(0000038\)>n = 1, 2, 3, 4 <\(0000039\)><\(0000040\)>B. Initialize the random tree <\(0000041\)>

[0018] B1. Initialize the task map, expand the obstacles, generate the map model, and define the points outside the obstacles and the expansion area as the feasible region. Initialize the search random tree Ts, initialize the starting point Qstart and the goal point Qgoal, and add the starting point to the search random tree Ts.

[0019] B2. Set the expansion step size to Lp, the candidate expansion selection probability to P0, and the target point connection determination distance to r0;

[0020] C. Forming a guiding path to the target point

[0021] The candidate expansion nodes generated by the same interval angle α are directly connected to form a guiding path. If the connected path collides with an obstacle, pruning is performed to remove points other than those between the target point and the obstacle. When the random search tree Ts grows to any node in these guiding paths whose distance is shorter than r0, it is directly connected to generate an initial path. After that, reverse pruning is performed to optimize the path. The specific method is: starting from the target point and tracing back in order of interval angle α from small to large, check whether the connection between nodes generated by the same interval angle α collides with an obstacle. If no collision occurs, the node is retained and the obstacle collision check is performed on this node and the previous node. If a collision occurs, all nodes except the node that was retained by the previous collision check are deleted.

[0022] The method of using Qexp to determine the expansion point using a random probability check is as follows:

[0023] Generate a random number Prand in the range of 0 to 1, and compare the random number Prand with the candidate expansion selection probability P0. If Prand>P0, then randomly generate an expansion point Qexp in the obstacle-free area; if Prand<=P0, then select the first node in the candidate queue as the expansion point Qexp, and at the same time remove this point from the queue and insert it at the end of the queue.

[0024] D. Find the node Qnearest closest to the expansion point in the random tree;

[0025] Traverse the search random tree Ts and calculate the node closest to Qexp in the random tree as Qnearest.

[0026] E. Generate new nodes

[0027] Expand the step length Lp from the node Qnearest to the node Qexp to generate a new node Qnew;

[0028] F. Determine whether nodes Qnear and Qnew collide with obstacles

[0029] Determine whether the nodes on the line connecting nodes Qnew and Qnearest collide with the obstacle and its expansion area; if the line connecting the nodes intersects the map expansion layer, it is determined that a collision has occurred, otherwise it is determined that no collision has occurred. If a collision has occurred, discard this node Qnew and return to step C; otherwise, add node Qnew to the search random tree;

[0030] G. Determine whether the target point Qgoal or any node in the guidance path has been reached

[0031] The new node Qnew in the search random tree is checked for relative distance with the points on the candidate expansion queue in turn. If the relative distance is less than r0, it is determined that the target point has been reached. Starting from the target point, the nearest nodes are checked in turn to generate the initial path. Otherwise, return to step B. The relative distance is calculated using the Euclidean distance calculation method. Assume that the horizontal and vertical coordinates of the node Qnew are Xnew and Ynew respectively, and the horizontal and vertical coordinates of the node to be checked Qopt in the candidate expansion queue are Xopt and Yopt respectively. The relative distance between Qnew and Qopt is:

[0032]

[0033] Furthermore, the interval angle α is 10-20 degrees; R2 is 2-4 times of R1; and R1 is 0.1-0.2 times of m.

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

[0035] 1. Since the present invention adds a new candidate expansion queue, the probability expansion is no longer limited to the target point, which solves the problem that the original Goal-based RRT is prone to falling into local optimality when approaching the target point;

[0036] 2. Since the present invention adds a new target point guidance path, the initial path can be generated more quickly and the path quality can be guaranteed. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 It is a schematic diagram of the candidate expansion point generation method of the present invention.

[0038] Figure 2 It is the overall flow chart of the present invention.

[0039] Figure 3 This is a flow chart of the expansion point Qexp selection of the present invention.

[0040] Figure 4 This is a schematic diagram of candidate expansion node pruning according to the present invention. DETAILED DESCRIPTION

[0041] The present invention will be further described below with reference to the accompanying drawings.

[0042] Figure 1 It represents the generation rule of candidate expansion queue nodes. In the figure, the black dot in the lower left corner represents the starting point position, and its horizontal and vertical coordinates are represented as Xstart and Ystart respectively. The black dot in the upper right corner represents the target position, and its horizontal and vertical coordinates are represented as Xgoal and Ygoal respectively. Taking the target point Qgoal as an endpoint, a line segment is extended outward at an interval of angle α on both sides of the line segment L. According to the scenario and performance requirements, n line segments L1, L2, L3, …, Ln are extended; in the figure, α is selected as 15°. Then, taking the target point Qgoal as the center of the circle, circles are drawn with radii R1 and R2 respectively, where R1 < R2, and the values of R1 and R2 are related to the distance L between Qstart and Qgoal. Figure 1 In it, R1 = L / 8 and R2 = L / 4. The two circles intersect with L1, L2, L3, and L4 to generate 8 intersection points in total; these 8 intersection points are used as candidate expansion points, and together with the target point Qgoal, they form a candidate expansion point queue Q11, Q12, Q13, Q14, Q21, Q22, Q23, Q24, Qgoal.

[0043] Figure 2 It is the overall process of the present invention. Figure 3 is Figure 2 the supplementary specific process of Qexp generation.

[0044] Figure 4 It is the pruning method for candidate expansion queue nodes. Figure 4 In it, an obstacle is added, and it is calculated and determined that the connecting lines between Q13, Q23 and Q14, Q24 pass through the obstacle area. At this time, Q23 and Q24 are removed from the candidate expansion queue.

[0045] The present invention is not limited to this embodiment. Any equivalent concept or change within the technical scope disclosed by the present invention shall be included in the protection scope of the present invention.

Claims

1. A robot path planning sampling method for adding a candidate expansion queue, characterized by: It includes the following steps: A. Obtain a candidate expansion queue according to the positional relationship between the starting point and the target point Suppose the black solid line frame represents the given environment, the large dot at the lower left corner represents the starting point Qstart, the large dot at the upper right corner represents the target point Qgoal, and the length of the line segment L connecting the starting point and the target point is m; Taking the target point Qgoal as an endpoint, extend a line segment outward at an interval angle α on both sides of the line segment L, and a total of four line segments L1, L2, L3, and L4 are extended; then, taking the target point Qgoal as the center of the circle, draw circles with radii R1 and R2 respectively, where R1 < R2, and the two circles intersect with the line segments L1, L2, L3, and L4 to generate a total of 8 intersection points; these 8 intersection points are used as candidate expansion points, and together with the target point Qgoal, they form a candidate expansion point queue Q11, Q12, Q13, Q14, Q21, Q22, Q23, Q24, Qgoal; The coordinate representation of the candidate expansion points is as follows: Let the abscissa of the target point Qgoal in the natural coordinate system be Xg, and the ordinate be Yg, then the abscissa X1n and ordinate Y1n of the candidate expansion point Q1n are represented as follows: The abscissa X2n and ordinate Y2n of the candidate expansion point Q2n are represented as follows: B. Initialize the random tree B1. Initialize the task map, inflate the obstacles to generate a map model, and define the points outside the obstacles and the inflated areas of the obstacles as the feasible areas; initialize the search random tree Ts, initialize the starting point Qstart and the target point Qgoal, and add the starting point to the search random tree Ts; B2. Set the expansion step size as Lp, the candidate expansion selection probability as P0, and the target point connection determination distance as r0; C. Form a guiding path to the target point Directly connect the candidate expansion nodes generated by the same interval angle α to form a guiding path. If the connected path collides with an obstacle, remove the points other than those between the target point and the obstacle through pruning. When the random search tree Ts grows to a distance shorter than r0 from any node in these guiding paths, directly connect to generate an initial path, and then perform reverse pruning to optimize the path; the specific method is as follows: in the order of the interval angle α from small to large, trace back from the target point, and check whether the connection between the nodes generated by the same interval angle α collides with an obstacle. If no collision occurs, retain this node, and check for obstacle collision between this node and the previous node. If a collision occurs, delete all nodes other than the nodes that passed the collision check and were retained previously; The method for determining the expansion point Qexp using a single random probability check is as follows: Generate a random number Prand within the range of 0 to 1, compare the random number Prand with the candidate expansion selection probability P0. If Prand > P0, randomly generate an expansion point Qexp in the obstacle-free area; if Prand <= P0, select the head node in the candidate queue as the expansion point Qexp, and at the same time move this point out of the queue and insert it at the end of the queue; D. Find the node Qnearest closest to the expansion point in the random tree Traverse the search random tree Ts and calculate the node closest to Qexp in the random tree as Qnearest; E. Generate new nodes Expand the step length Lp from the node Qnearest to the node Qexp to generate a new node Qnew; F. Determine whether nodes Qnear and Qnew collide with obstacles Determine whether the nodes on the line connecting nodes Qnew and Qnearest collide with the obstacle and its expansion area; if the line connecting the nodes intersects the map expansion layer, it is determined that a collision has occurred, otherwise it is determined that no collision has occurred; if a collision has occurred, discard this node Qnew and return to step C; otherwise, add node Qnew to the search random tree; G. Determine whether the target point Qgoal or any node in the guidance path has been reached The new node Qnew of the search random tree is checked for relative distance with the points on the candidate expansion queue in turn. If the relative distance is less than r0, it is determined that the target point has been reached. Starting from the target point, the nearest nodes are checked in turn to generate the initial path. Otherwise, return to step B. The relative distance is calculated using the Euclidean distance calculation method. Assume that the horizontal and vertical coordinates of the node Qnew are Xnew and Ynew respectively, and the horizontal and vertical coordinates of the node to be checked Qopt in the candidate expansion queue are Xopt and Yopt respectively. Then the relative distance between Qnew and Qopt is:

2. A robot path planning sampling method for adding a candidate expansion queue according to claim 1, characterized in that: The interval angle α is 10-20 degrees; R2 is 2-4 times of R1; and R1 is 0.1-0.2 times of m.

Images