CN116242354B

CN116242354B - A robot path planning sampling method for joining candidate expansion queue

Info

Publication number: CN116242354B
Application number: CN202211584088.7A
Authority: CN
Inventors: 郭烈; 陈凯文; 余旭东; 殷广; 赵剑; 李刚
Original assignee: Dalian University of Technology; Ningbo Research Institute of Dalian University of Technology
Current assignee: Dalian University of Technology; Ningbo Research Institute of Dalian University of Technology
Priority date: 2022-12-09
Filing date: 2022-12-09
Publication date: 2025-08-26
Anticipated expiration: 2042-12-09
Also published as: CN116242354A

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

Robot path planning sampling method added into candidate expansion queue

Technical Field

The invention belongs to the technical field of mobile robot task planning, and particularly relates to a robot path planning sampling method for adding a candidate expansion queue.

Background

The path planning technology is a core part of the autonomous navigation technology of the robot, and the path planning technology refers to that a collision-free feasible path is generated from a starting position to a target position in a given environment. Common path planning methods are heuristic algorithms and sampling algorithms. The rapid expansion random tree (RRT) is a classical sampling algorithm, a communication path can be obtained by searching space uniformly, the expansion of the random tree in the algorithm is prone to be an exploration area in the environment space, the random points are generated in the environment, tree nodes closest to the random points in the random tree are searched, the two connecting line directions are used as new node growth directions, new nodes are generated by expansion with fixed step length, and the expansion process is repeated until target points are added into the random tree, so that a complete path is obtained. The RRT algorithm has two unavoidable problems, one is that the generated tree may have found nodes very close to the target, but not connected to the target due to the randomness of such sampling, and one is that this increases calls to the node generator, adding more unnecessary branches to the tree. To improve these two drawbacks of the RRT algorithm, a gol-biasRRT algorithm based on a probabilistic target has been proposed, and the running speed of the algorithm is accelerated by randomly selecting a target point as an expansion point with a certain probability.

The Goal-biasRRT can guide the random tree to grow towards the target by taking the target point as the sampling point according to a certain probability, and has the characteristic of preferentially growing a branch, which is beneficial to quickly planning a path. The probability P of gol-biasRRT to pick a target point as a sampling point is fixed, however, when the random tree falls into a local minimum, the extension of picking a target point as a sampling point is generally ineffective, which wastes a lot of computational resources.

The complete path planning algorithm can meet the requirements of path quality and algorithm performance, a short and smooth enough path needs to be obtained on the path quality to meet the requirements of robot tracking, the algorithm performance is embodied in the length of program running time, the algorithm performance is obviously improved by the Goal-biasRRT algorithm based on probability compared with the RRT algorithm, the path quality generated by the Goal-biasRRT algorithm is mostly inferior to that of the RRT algorithm, and the random tree of the Goal-biasRRT algorithm falls into local minimum values under certain scenes, so that the algorithm performance is greatly influenced.

Disclosure of Invention

In order to solve the problems in the prior art, the invention designs a robot path planning sampling method for adding candidate expansion queues, which can improve the algorithm performance and meet the path quality requirements.

In order to achieve the above purpose, the basic idea of the invention is that a group of candidate expansion queues (including target points) are selected in the vicinity of the target points according to a certain rule, each time an expansion point is reselected, a certain probability P is provided for selecting a head node in the candidate expansion queues as an expansion point, then the node is inserted into the tail of the queues, and correspondingly, each expansion has a certain probability 1-P for taking a random point as an expansion point.

The robot path planning sampling method for joining the candidate expansion queue comprises the following steps:

A. obtaining candidate expansion queues according to the position relation between the starting point and the target point

The black solid line frame is assumed to represent a given environment, the large dot at the lower left corner represents a starting point Qstart, the large dot at the upper right corner represents a target point Qgoal, the length of a line segment L connecting the starting point and the target point is m, the line segment is expanded outwards at intervals of an angle alpha at two sides of the line segment L by taking the target point Qgoal as an end point, four line segments L1, L2, L3 and L4 are expanded totally, R1 and R2 are taken as circles by taking the target point Qgoal as circle centers, R1 and R2 are taken as radiuses, and the two circles intersect with the line segments L1, L2, L3 and L4 to generate 8 intersection points totally, and the 8 intersection points are taken as candidate expansion points to form candidate expansion point queues Q11, Q12, Q13, Q14, Q21, Q22, Q23, Q24 and Qgoal with the target point Qgoal;

the coordinates of the candidate expansion point are expressed as follows, assuming that the abscissa of the target point Qgoal in the natural coordinate system is Xg and the ordinate is Yg, the abscissa X1n and the ordinate Y1n of the candidate expansion point Q1n are expressed as follows:

the abscissa X2n and the ordinate Y2n of the candidate expansion point Q2n are expressed as follows:

n=1、2、3、4

B. initializing a random tree

And B1, initializing a task map, expanding the obstacle to generate a map model, and defining points except the obstacle and the obstacle expansion area as a feasible area. The search random tree Ts is initialized, the starting point Qstart and the target point Qgoal are initialized, and the starting point is added to the search random tree Ts.

B2, setting the expansion step length as Lp, setting the candidate expansion selection probability as P0, and setting the target point connection judgment distance as r0;

C. forming a guide path for the target point

And directly connecting all candidate expansion nodes generated by the same interval angle alpha to form guide paths, removing points from the target points to the points except between the obstacles through pruning if the connected paths collide with the obstacles, directly connecting when a random search tree Ts grows to any node distance of the guide paths to be shorter than r0, generating an initial path, and then carrying out reverse pruning optimization paths. The specific method is that the connection between nodes generated by the same interval angle alpha is checked to see if the connection collides with an obstacle according to the sequence of the interval angle alpha from small to large from a target point, if the connection does not collide, the node is reserved, the obstacle collision check is carried out on the node and the previous node, and if the connection collides, all nodes except the node reserved by the collision check are deleted.

The method for determining the Qexp for the expansion point by using one random probability check is as follows:

Generating random numbers Prand in the range of 0-1, comparing the random numbers Prand with candidate expansion selection probability P0, randomly generating an expansion point Qexp in the barrier-free area if Prand > P0, selecting a queue head node from a candidate queue as the expansion point Qexp if Prand < = P0, and simultaneously removing the point from the queue and inserting the point into the queue tail.

D. Searching a node Qnearest closest to the expansion point in the random tree;

The random tree Ts is searched in a traversing mode, and the node closest to Qexp in the random tree is calculated to be Qnearest.

E. Generating new nodes

Expanding the step length Lp from the node Qnearest to the node Qexp direction to generate a new node Qnew;

F. judging whether collision occurs between the node Qnear and Qnew and the obstacle

Judging whether the nodes on the connecting lines of the nodes Qnew and Qnearest collide with the obstacle and the expansion area thereof, judging that the collision occurs if the connecting lines of the nodes intersect with the expansion layer of the map, and otherwise judging that the collision does not occur. C, if collision occurs, discarding the node Qnew, and returning to the step C, otherwise, adding the node Qnew into the search random tree;

G. determine whether to reach the target point Qgoal or any node in the guidance path

And (C) sequentially checking the relative distance between the new node Qnew searching the random tree and the point on the candidate expansion queue, judging that the new node reaches the target point if the relative distance is smaller than r0, sequentially checking the nearest node from the target point to generate an initial path, and otherwise, returning to the step (B). The relative distance is calculated by using the Euclidean distance calculation method, and if the abscissa of the node Qnew is Xnew and Ynew respectively and the ordinate of the node Qopt to be checked in the candidate expansion queue is Xopt and Yopt respectively, the relative distance between Qnew and Qopt is:

Further, the interval angle alpha=10-20 degrees, R2 is 2-4 times of R1, and R1 is 0.1-0.2 times of m.

Compared with the prior art, the invention has the following beneficial effects:

1. because a new candidate expansion queue is added, probability expansion is not limited to the target point any more, and the problem that the original Goal-basedRRT is easy to fall into local optimum when approaching the target point is solved;

2. the invention adds a new target point guiding path, which can generate an initial path faster and ensure the path quality.

Drawings

FIG. 1 is a schematic diagram of a candidate expansion point generation method of the present invention.

Fig. 2 is a general flow chart of the present invention.

Fig. 3 is a flowchart of the expansion point Qexp selection of the present invention.

Fig. 4 is a schematic pruning diagram of a candidate expansion node according to the present invention.

Detailed Description

The invention is further described below with reference to the accompanying drawings.

Fig. 1 shows a generation rule of candidate expansion queue nodes, wherein a black dot at the lower left corner in the diagram represents a starting point position, the abscissas and ordinates thereof are respectively represented as Xstart and Ystart, a black dot at the upper right corner in the diagram represents a target position, the abscissas and ordinates thereof are respectively represented as Xgoal and Ygoal, a target point Qgoal is used as an endpoint, a line segment is expanded outwards at intervals of an angle alpha at two sides of a line segment L, n line segments L1, L2 and L3 are expanded according to scene and performance requirements, and alpha is selected as 15 deg. Taking the target point Qgoal as a circle center, taking R1 and R2 as radiuses to make circles, wherein the values of R1 and R2 are related to the distance L of Qstart and Qgoal, R1 = L/8 and R2 = L/4 in figure 1, the two circles intersect with L1, L2, L3 and L4 to generate 8 intersection points, taking the 8 intersection points as candidate expansion points, and forming candidate expansion point queues Q11, Q12, Q13, Q14, Q21, Q22, Q23, Q24 and Qgoal with the target point Qgoal.

Fig. 2 is an overall flow of the present invention, and fig. 3 is a specific flow supplement of Qexp generation in fig. 2.

Fig. 4 is a method for pruning candidate expansion queue nodes, in fig. 4, an obstacle is added, and it is calculated and determined that the connection lines between Q13 and Q23 and Q14 and Q24 pass through the obstacle area, and at this time, Q23 and Q24 are removed from the candidate expansion queue.

The present invention is not limited to the present embodiment, and any equivalent concept or modification within the technical scope of the present invention is listed as the protection scope of the present invention.

Claims

1. A robot path planning sampling method for adding a candidate expansion queue, characterized by comprising the following steps:

A. Obtain candidate expansion queues based on the positional relationship between the starting point and the target point

Assume that the black solid box represents the given environment, the large circle in the lower left corner represents the starting point Qstart, the large circle in 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 the endpoint, a line segment is extended outward at an angle α on both sides of the line segment L, for a total of four line segments L1, L2, L3, and L4; then, with the target point Qgoal as the center, circles are drawn with R1 and R2 as radii, respectively, with R1 < R2. The two circles intersect with the line segments L1, L2, L3, and L4 to produce a total of 8 intersection points; these 8 intersection points are used as candidate extension points and form a candidate extension point queue Q11, Q12, Q13, Q14, Q21, Q22, Q23, Q24, and Qgoal with the target point Qgoal;

The coordinates of the candidate expansion points are expressed as follows: Let the horizontal coordinate of the target point Qgoal in the natural coordinate system be Xg and the vertical coordinate be Yg, then the horizontal coordinate X1n and vertical coordinate Y1n of the candidate expansion point Q1n are expressed as follows:

The horizontal coordinate X2n and vertical coordinate Y2n of the candidate extension point Q2n are expressed as follows:

B. Initialize random tree

B1. Initialize the task map, expand the obstacles, generate the map model, and define the points outside the obstacles and the expansion area of the obstacles as the feasible area; 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 to Lp, the candidate expansion selection probability to P0, and the target point connection determination distance to r0;

C. Forming a guiding path to the target point

Directly connect the candidate expansion nodes generated by the same interval angle α to form a guide path. If the connected path collides with an obstacle, prune the nodes other than the target point to the obstacle. When the random search tree Ts grows to any node in these guide paths whose distance is shorter than r0, directly connect them to generate an initial path, and then perform reverse pruning to optimize the path. The specific method is: start from the target point and trace 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, retain this node, and perform obstacle collision check on this node and the previous node. If a collision occurs, delete all nodes except the node that has been retained by the previous collision check.

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

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;

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 α = 10-20 degrees; R2 is 2-4 times R1; R1 is 0.1-0.2 times m.