Heuristic path planning method

Abstract

The invention discloses a heuristic path planning method, which comprises the following steps: acquiring an initial path, and initializing a sampling space and a path random tree according to the initial path; randomly generating a first sampling point or a second sampling point in the sampling space, and confirming a node closest to the first sampling point or the second sampling point in the path tree nodes as a first node; when the first sampling point is generated, generating an expansion path node under a specific deflection angle according to the position of the first node, the position of the first sampling point and the position of the target node; when the second sampling point is generated, generating an expansion path node in the direction of the target node based on the first node; calculating a cost function according to the expansion path node, and performing path optimization according to the cost function value; according to the method, a dynamic target offset deviation angle expansion strategy is adopted to solve the problem of poor guidance quality of a random tree.

Description

Technical Field

[0001] The present invention relates to the technical field of path optimization, and more particularly to a heuristic path planning method. Background Art

[0002] At present, the robot arm widely used in industrial manufacturing automation has become a widely used automation tool. The robot arm can accurately perform complex tasks in a constantly changing environment. Its flexibility and precision make it an important tool in various industries. The path planning of the robot arm is one of the core elements to achieve its efficient work.

[0003] Compared with the traditional RRT algorithm, the RRT* (Rapidly-exploring Random Trees Star, RRTs) algorithm can find a better path when searching a large space, and can gradually converge to the asymptotic optimal solution within a limited number of iterations. It uses the optimization objective to evaluate the nodes of the tree and tries to optimize the structure of the tree to obtain a better path. However, when the RRT* algorithm expands nodes, it has strong randomness and poor guidance. The path it plans is usually quite different from the optimal path. Relevant scholars have proposed many improved algorithms to solve the path optimization problem. Among them, the Informed-RRT* algorithm proposed by Gammell et al. is based on the improved RRT* algorithm. The main idea is to continue to optimize the initial path based on the initial path planned by the RRT* algorithm. Although it does not help the generation of the entire initial path, it can improve the efficiency of subsequent path planning. Therefore, improving the generation efficiency of the initial path of the RRT* algorithm can also improve the path planning efficiency of the Informed-RRT* algorithm in disguise.

[0004] Compared with the RRT* algorithm, the Informed-RRT* algorithm improves the secondary sampling space and retains the probability guarantee of completeness and asymptotic optimality by directly focusing the search on the subset of the sampling space, but it does not effectively solve the problem of low efficiency of random expansion caused by the strong randomness and poor guidance of the RRT* algorithm. To address these problems, a PSP-Informed-RRT* path planning method based on heuristic path encirclement contraction and dynamic target biased expansion is designed to plan an optimization path with lower cost.

[0005] Therefore, how to reduce the randomness of sampling and improve the poor guidance during optimization is an urgent problem that technicians in this field need to solve. Summary of the invention

[0006] In view of this, the present invention provides a heuristic path planning method, which can solve the problems of poor guidance and high randomness.

[0007] In order to achieve the above object, the present invention adopts the following technical solution:

[0008] A heuristic path planning method, comprising:

[0009] Obtaining an initial path, and initializing a sampling space and a path random tree according to the initial path;

[0010] Randomly generate a first sampling point or a second sampling point in the sampling space, and confirm the closest node to the first sampling point or the second sampling point in the path tree node as the first node;

[0011] When the first sampling point is generated, an extended path node is generated at a specific deflection angle according to the position of the first node, the position of the first sampling point, and the position of the target node;

[0012] When generating the second sampling point, generating an extended path node in the direction of the target node based on the first node;

[0013] A cost function is calculated according to the extended path nodes, and path optimization is performed according to the cost function value.

[0014] Preferably, when the first sampling point is used to generate the extended path node, collision detection is performed;

[0015] When the path between the extended path point and the first node does not collide with an obstacle, outputting the extended path point;

[0016] Otherwise, the deflection angle is adjusted to move away from the obstacle, and collision detection is performed again.

[0017] Preferably, when the first sampling point is used to generate the extended path node, the node position of the extended path is:

[0018]

[0019] Among them, X new is the expansion path node, X nearest is the first node, λ is the step length of each node expansion, X goal is the target node, X rand is the first sampling point, and β is the deflection angle.

[0020] Preferably, the deflection angle is adjusted according to the collision situation of the collision detection, specifically including:

[0021]

[0022] Wherein, formula (1) is the angle adjustment method when no collision occurs, formula (2) is the deflection angle adjustment method when collision occurs, k is the expansion round, j is the number of collisions, X start is the starting node, β init It is the preset angle deflection parameter.

[0023] Preferably, the first sampling point or the second sampling point is randomly generated in the sampling space, and the steps include:

[0024] Generate a temporary sampling point and generate a random number P;

[0025] The random number P and the random threshold parameter P bias A comparison is performed, and according to the comparison result, the temporary sampling point is confirmed to be the first sampling point or the second sampling point.

[0026] Preferably, the steps further include: when the extension path point cannot be generated, recording it as a sampling failure, resampling, and accumulating the number of sampling failures;

[0027] adjusting the probability according to the number of sampling failures, so that the probability of generating the first sampling point increases as the number of sampling failures increases;

[0028] When the final expansion path node is obtained, the number of sampling failures is reset.

[0029] Preferably, updating the probability of generating the first sampling point or the second sampling point according to the number of sampling failures includes:

[0030]

[0031] Among them, X sample represents a randomly generated temporary sampling point, X goal is the second sampling point, X rand is the first sampling point, P bias is the probability threshold of the current temporary sampling point category, P th is the initial value of the probability threshold; n is the number of sampling failures.

[0032] Preferably, the collision detection step includes:

[0033] Taking the path from the first node to the extended path node as an extended path;

[0034] Model obstacles as spherical bounding volumes;

[0035] Expanding the spherical bounding volume and obtaining a radius of the expanded model;

[0036] The distance from the center of the model sphere to the extended path is calculated and compared with the model radius to confirm whether a collision occurs.

[0037] Preferably, calculating a cost function according to the extended path nodes, and performing path optimization according to the cost function value, includes:

[0038] Generate an exploration area within a preset range based on the expansion path node;

[0039] Extracting a path tree node in the exploration domain as a second node;

[0040] Calculate the first path cost values ​​from the starting point to the expansion path point through each of the second nodes respectively, and obtain the second node corresponding to the minimum first path cost value as the parent node of the expansion path node;

[0041] The second node except the parent node is used as a third node; a path connecting the starting point, the parent node, the extended path node and the third node in sequence is used as a candidate path;

[0042] A second path cost value of each candidate path is calculated and compared with the original path cost value, and a path with a smaller cost value is retained.

[0043] Preferably, when the second sampling point is used to generate the extended path node, the node position of the extended path is:

[0044]

[0045] Among them, X new is the expansion path node, X nearest is the first node, λ is the step length of each node expansion, X goal is the target node.

[0046] It can be seen from the above technical solution that, compared with the prior art, the present invention discloses a path optimization method, which adopts a dynamic target bias angle expansion strategy to solve the problem of poor guidance of random trees; at the same time, the bias angle exploration is combined with collision detection, and the path range is further contracted according to the number of collisions, which can reduce sampling randomness, accelerate path convergence, and avoid falling into local optimality. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0048] Figure 1 A schematic diagram of a heuristic path planning method provided by the present invention;

[0049] Figure 2 Schematic diagram of elliptical sampling space in the implementation of the present invention;

[0050] Figure 3 Schematic diagram of target biased expansion node in an embodiment of the present invention;

[0051] Figure 4 is a schematic diagram comparing the path planned by the Informed-RRT* algorithm and the path planned by the embodiment of the present invention;

[0052] Figure 5 It is a schematic diagram comparing path planning in the robotic arm simulation environment at different angles. DETAILED DESCRIPTION

[0053] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0054] The embodiment of the present invention discloses a heuristic path planning method, comprising the following steps:

[0055] S1: Get the initial path, and initialize the sampling space and the path random tree according to the initial path, where the path random tree refers to the entire tree structure consisting of a series of path nodes and edges connecting these nodes.

[0056] S2: randomly generate a first sampling point or a second sampling point in the sampling space, and confirm the closest node to the first sampling point or the second sampling point in the path tree node as the first node;

[0057] S3: Generate expansion path nodes.

[0058] S31: when generating the first sampling point, generating an extended path node at a specific deflection angle according to the position of the first node, the position of the first sampling point and the position of the target node;

[0059] S32: when generating the second sampling point, generating an extended path node in the direction of the target node based on the first node;

[0060] S4: Calculate the cost function based on the expanded path nodes and optimize the path based on the cost function value

[0061] In one embodiment, the initial path includes at least a starting point and a target point, and may also be a feasible path.

[0062] In order to further implement the above technical solution, the specific method for confirming the sampling space is as follows:

[0063] When a feasible path cannot be identified, random sampling is performed in the free space until a feasible path is identified.

[0064] When a feasible path can be identified, the total cost length of the feasible path Cline is set to Cmax=2a, the distance between the starting point and the target point is set to Cmin, and the starting point and the target point are the two foci F1 and F2 of the ellipse. Therefore, the ellipse relationship can be obtained as follows:

[0065] |PF1|+|PF2|=2a

[0066] The obtained ellipse is used as the sampling space to avoid invalid sampling points outside the ellipse. The cost of the initial feasible path is used to form an ellipsoid to limit the size of the sampling space. Reducing the size of the sampling space can make sampling more efficient and explore the global asymptotic optimal path.

[0067] In order to further implement the above technical solution, the specific method for randomly generating the first sampling point and the second sampling point is as follows:

[0068] S21: Generate a temporary sampling point and generate a random number P.

[0069] S22: The random number P and the random threshold parameter P bias A comparison is performed, and according to the comparison result, the temporary sampling point is confirmed to be the first sampling point or the second sampling point.

[0070]

[0071] Among them, X sample represents a randomly generated temporary sampling point, X goal is the second sampling point, X rand is the first sampling point, P bias is the probability threshold that currently controls the category of the temporary sampling point.

[0072] Furthermore, when the first sampling point is used to generate an extended path node, a collision detection is performed; if no collision occurs, the deflection angle is adjusted to be progressive in the direction of the obstacle; if a collision occurs, the deflection angle is adjusted to be away from the obstacle; until a path node at a non-collision critical deflection angle is generated as the final extended path node. The node position of the generated extended path is represented as follows:

[0073]

[0074] Among them, X new X is the extended path node. nearest is the first node, λ is the step size of each node expansion, X goal is the target node, X rand is the first sampling point, and β is the deflection angle.

[0075] Furthermore, the deflection angle is adjusted according to the collision situation of the collision detection, specifically including:

[0076]

[0077] Wherein, formula (1) is the angle adjustment method when no collision occurs, formula (2) is the deflection angle adjustment method when collision occurs, k is the expansion round, j is the number of consecutive collisions, X start is the starting node, β init is the initial deflection angle.

[0078] In order to further implement the above technical solution, S3 also includes:

[0079] S33: After the final extended path point is generated, the probability of generating the first sampling point or the second sampling point is updated according to the number of sampling failures, so that the greater the number of collisions, the lower the probability of generating the second sampling point.

[0080] Specifically, the probability update method includes:

[0081]

[0082] Among them, P th is the initial value of the probability threshold; n is the number of sampling failures. Sampling failure refers to the inability to expand an expansion path node that satisfies collision detection. When an expansion path point cannot be generated, it is recorded as a sampling failure, and resampling is performed. The number of sampling failures is accumulated, and the probability is adjusted using the accumulated number of sampling failures; when the final expansion path node is obtained, the number of sampling failures is reset.

[0083] In this embodiment, after each layer of optimization, the updated probability is used to execute S2 in the next round of iteration.

[0084] In order to further implement the above technical solution, S4 includes: reselecting the parent node for the expanded new node through rewrite, adding the new node to the expanded tree, and then further rewiring through relink, searching for the child node that can make the total path cost less than the original path based on the new node and its parent node, and optimizing the random tree. The specific steps are as follows:

[0085] S41: Generate an exploration area within a preset range based on the expanded path node;

[0086] S42: extracting a path tree node in the exploration area as a second node;

[0087] S43: respectively calculating the first path cost values ​​from the starting point through each second node to the extended path point, and obtaining the corresponding second node when the first path cost value is the smallest as the parent node of the extended path node;

[0088] S44: the second node except the parent node is used as the third node; a path connecting the starting point, the parent node, the extended path node and the third node in sequence is used as a candidate path;

[0089] S45: Calculate the second path cost value of each candidate path and compare it with the original path cost value, and retain the path with the smaller cost value.

[0090] In this embodiment, after the candidate paths are obtained, it is necessary to perform a collision detection on the paths between the extended path node and the third node, and select the paths without collision as the candidate paths.

[0091] In order to further implement the above technical solution, when the second sampling point is used to generate the extended path node, the node position of the extended path is:

[0092]

[0093] Among them, X new X is the extended path node. nearest is the first node, λ is the step size of each node expansion, X goal is the target node.

[0094] In order to further implement the above technical solution, the step also includes, after the final path is generated, using a cubic B-spline curve interpolation method to smooth the tortuous path.

[0095] Figure 4 and Figure 5 The technical effects of the present invention are reflected:

[0096] from Figure 4 It can be seen that the path planned by the PSP-Informed-RRT* algorithm of the present invention has obvious advantages. PSP-Informed-RRT* optimizes the problems of strong randomness and poor guidance of sampling points. By combining the dynamic target bias and deflection angle expansion of the sampling points, the expansion direction of the random tree tends to the target area; at the same time, in order to optimize the path quality, the triangle inequality and the linear interpolation principle are integrated to reduce the redundant nodes and redundant path segments of the path, which greatly improves the path quality and is more conducive to the operation of the robot arm. Good path length, number of turning points and path planning time period.

[0097] Figure 5 The green route in the middle is the path planned by the PSP-Informed-RRT* algorithm, and the red route is the path planned by the Informed-RRT* algorithm. It can be seen that the path planned by the PSP-Informed-RRT* algorithm is obviously smoother, has fewer turning points and larger angles, and is more conducive to the movement of the robot arm.

[0098] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0099] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A heuristic path planning method, characterized in that: include: Obtaining an initial path, and initializing a sampling space and a path random tree according to the initial path; Randomly generate a first sampling point or a second sampling point in the sampling space, and confirm the closest node to the first sampling point or the second sampling point in the path tree node as the first node; When the first sampling point is generated, an extended path node is generated at a specific deflection angle according to the position of the first node, the position of the first sampling point, and the position of the target node; When generating the second sampling point, generating an extended path node in the direction of the target node based on the first node; A cost function is calculated according to the extended path nodes, and path optimization is performed according to the cost function value.

2. A heuristic path planning method according to claim 1, characterized in that: When the first sampling point is used to generate the extended path node, collision detection is performed; When the path between the extended path point and the first node does not collide with an obstacle, outputting the extended path point; Otherwise, the deflection angle is adjusted to move away from the obstacle, and collision detection is performed again.

3. A heuristic path planning method according to claim 2, characterized in that: When the first sampling point is used to generate the extended path node, the node position of the extended path is: Among them, X new is the expansion path node, X nearest is the first node, λ is the step length of each node expansion, X goal is the target node, X rand is the first sampling point, and β is the deflection angle.

4. A heuristic path planning method according to claim 3, characterized in that: The deflection angle is adjusted according to the collision situation of the collision detection, specifically including: Wherein, formula (1) is the angle adjustment method when no collision occurs, formula (2) is the deflection angle adjustment method when collision occurs, k is the expansion round, j is the number of collisions, X start is the starting node, β init It is the preset angle deflection parameter.

5. A heuristic path planning method according to claim 2, characterized in that: The steps of randomly generating a first sampling point or a second sampling point in the sampling space include: Generate a temporary sampling point and generate a random number P; The random number P and the random threshold parameter P bias A comparison is performed, and according to the comparison result, the temporary sampling point is confirmed to be the first sampling point or the second sampling point.

6. A heuristic path planning method according to claim 2 or 5, characterized in that: The steps also include: when the extended path point cannot be generated, recording it as a sampling failure, resampling, and accumulating the number of sampling failures; adjusting the probability according to the number of sampling failures, so that the probability of generating the first sampling point increases as the number of sampling failures increases; When the final expansion path node is obtained, the number of sampling failures is reset.

7. A heuristic path planning method according to claim 6, characterized in that: Updating the probability of generating the first sampling point or the second sampling point according to the number of sampling failures includes: Among them, X sample represents a randomly generated temporary sampling point, X goal is the second sampling point, X rand is the first sampling point, P bias is the probability threshold of the current temporary sampling point category, P th is the initial value of the probability threshold; n is the number of sampling failures.

8. A heuristic path planning method according to claim 2, characterized in that: The steps of collision detection include: Taking the path from the first node to the extended path node as an extended path; Model obstacles as spherical bounding volumes; Expanding the spherical bounding volume and obtaining a radius of the expanded model; The distance from the center of the model sphere to the extended path is calculated and compared with the model radius to confirm whether a collision occurs.

9. A heuristic path planning method according to claim 1, characterized in that: Calculating a cost function according to the extended path nodes, and performing path optimization according to the cost function value, including: Generate an exploration area within a preset range based on the expansion path node; Extracting a path tree node in the exploration domain as a second node; Calculate the first path cost values ​​from the starting point to the expansion path point through each of the second nodes respectively, and obtain the second node corresponding to the minimum first path cost value as the parent node of the expansion path node; The second node except the parent node is used as a third node; a path connecting the starting point, the parent node, the extended path node and the third node in sequence is used as a candidate path; A second path cost value of each candidate path is calculated and compared with the original path cost value, and a path with a smaller cost value is retained.

10. A heuristic path planning method according to claim 1, characterized in that: When the second sampling point is used to generate the node of the extended path, the node position of the extended path is: Among them, X new is the expansion path node, X nearest is the first node, λ is the step length of each node expansion, X goal is the target node.

Images