CN114115271A - Robot path planning method and system for improving RRT - Google Patents

Abstract

The invention discloses a robot path planning method and a system for improving RRT (remote distance test). by generating an extended node in a self-adaptive manner and optimizing a path from a starting point to a newly generated extended node, the time complexity of an RRT algorithm in the path planning process is reduced; and the generated random expansion tree is pruned, and redundant nodes are removed, so that the convergence speed is increased, a better path is obtained, and the path planning efficiency is improved.

Description

Robot path planning method and system for improving RRT

Technical Field

The invention belongs to the technical field of robot path planning, and particularly relates to a robot path planning method and system capable of avoiding obstacles.

Background

Path planning refers to searching a safe, collision-free and feasible path from a starting point to an end point in a given area containing obstacles. A rapid exploration random tree (RRT) based on random point sampling is an algorithm widely applied to robot path planning at present. The RRT algorithm uses an initial point as a root node, generates a random expanded tree by increasing leaf nodes through random sampling, and can find a path from the initial point to a target point in the random tree when the leaf nodes in the random tree contain the target nodes or enter a target area.

There are some significant disadvantages: the method is insensitive to environment information, and when a feasible region is narrow and complex or a large number of irregular obstacles exist, the efficiency of the RRT algorithm for converging to the optimal solution is greatly reduced, and even the exploration fails.

Disclosure of Invention

The purpose of the invention is as follows: the invention provides a robot path planning method and system for improving RRT (route planning), which can solve the problems of low convergence speed and easiness in falling into local optimal solution when an RRT algorithm is adopted for path planning, and improve the efficiency of path planning.

The technical scheme is as follows: the invention discloses a robot path planning method for improving RRT (remote distance measurement) on one hand, which comprises the following steps:

s1, dividing a two-dimensional scene of robot activity into a passable area and an impassable obstacle, and establishing a two-dimensional grid map; acquiring initial position Q of robot in two-dimensional scene_startAnd destination position Q_goal；

If from Q_startTo Q_goalIf the straight path does not touch the obstacle, the planned path is Q_startTo Q_goalStraight line segment of (2); otherwise, Q is_startAs a root node of a random expansion tree in the RRT algorithm, initializing a target direction weight k to be 1, and determining a planned path according to steps S2-S7;

s2, generating random position points Q in the motion area of the robot_rand(ii) a If Q is_startAnd Q_randCrosses an obstacle, the Q is discarded_randAnd again generate a new Q_randUp to Q_startAnd Q_randThe wiring of (a) does not pass through the barrier;

s3, traversing the current random expansion tree, searching and Q_randClosest point Q_near；

S4, calculating Q_nearMinimum distance to obstacle D_obAnd calculating a repulsion factor h:

rho is the movement step length of the robot;

calculating Q_randTo Q_nearGravitation of (2)

Q_goalTo Q_nearGravitation of (2)

Obstacle pair Q_nearRepulsive force of

Calculating Q_nearResultant force of the process

Wherein

w is gravity weight and is preset (0, 1)]A constant within a range; q_obIs a distance Q_nearThe nearest obstacle position; i is the distance between two position points in the grid graph of the two-dimensional environment;

with Q_nearAs a parent node, according to the resultant force

Generating a new node Q_new：

Judging new node Q_newAnd Q_nearWhether there is a collision between the connecting line and the obstacle, or not, and if there is no collision, Q is set_newAdding the random position points to a random expansion tree, and jumping to S2 to regenerate random position points if collision exists;

s5 optimization starting point Q_startTo Q_newA path of (a);

s6, repeating the steps S3-S5, and when a new node Q_newInto a destination position Q_goalIn the neighborhood of or is Q_goalStopping the expansion of the random expansion tree when the random expansion tree is self; pruning the random expanded tree;

s7 from the destination position Q_goalStarting to trace back the father nodes in sequence until the starting point Q_startEnd, find a piece from Q_startTo Q_goalIs not obstructed.

Preferably, the step S4 further includes: according to F₁And F₂And updating k: if F₁＞F₂K is k + Δ k; otherwise k is k- Δ k; and deltak is a preset updating step length.

Specifically, the step S5 specifically includes:

s5.1 with Q_newAs a center of circle, R_nearCalculating neighborhood V for radius_near；R_nearA preset wiring length;

s5.2, locating in the neighborhood V in the random expansion tree_nearThe nodes in the set S_nearObtaining a set S_nearThe first external node of each node in the set S_pnear(ii) a The first external node of the node s is the random spanning tree, and the distance between the node s and the first external node is smaller than the first distance R₁A node of (2); s is as large as S_near，S_sum＝S_near∪S_pnear；

S5.3 traversing set S_sumNode in (1), judge Q_newParent node of (1) is replaced by S_sumAfter the node in (1), whether Q is shortened_startTo Q_newIf shortened, Q is performed_newThe parent node of (3) is replaced, otherwise, the parent node is not replaced;

s5.4, searching for Q_newSecond external node ofSet S of_pnewNode Q_newThe second external node of (2) is the AND Q in the random spanning tree_newIs less than the second distance R₂A node of (2);

s5.5, traversing S_nearIf the parent node is replaced by Q, the node in (1) is judged_newAnd S_pnewWhether or not the nodes in the union of (1) will later reduce Q_startTo the S_nearIf the path is reduced, the S is carried out_nearThe replacement of the node parent node in (1) and not otherwise.

Specifically, the pruning processing on the random spanning tree in the step S6 specifically includes:

root node Q from random spanning tree_startInitially, a destination position Q capable of collision-free connection is sought_goalThen, the intermediate point P1 is used as a starting point to find a destination position Q capable of connecting without collision by using P1 as a starting point_goalUntil Q is found, repeating the steps until the intermediate point P2_goalOr into the destination position Q_goalWithin a neighborhood of (c); q_startAnd all intermediate points form an optimized path in sequence.

On the other hand, the invention also discloses a robot path planning system for improving the RRT, which comprises:

the two-dimensional scene establishing module is used for dividing a two-dimensional scene of robot activity into a passable area and an impassable barrier and establishing a two-dimensional grid map; acquiring initial position Q of robot in two-dimensional scene_startAnd destination position Q_goal；

Random position point Q_randA generation module for randomly positioning the point Q_rand；

Closest point Q_nearA generating module for traversing the current random spanning tree, searching and Q_randClosest point Q_near；

New node Q_newA generation module for generating a new node Q_newThe generation steps are as follows:

calculating Q_nearMinimum distance to obstacle D_obAnd calculating a repulsion factor h:

rho is the movement step length of the robot;

calculating Q_randTo Q_nearGravitation of (2)

Q_goalTo Q_nearGravitation of (2)

Obstacle pair Q_nearRepulsive force of

Calculating Q_nearResultant force of the process

Wherein

w is gravity weight and is preset (0, 1)]A constant within a range; q_obIs a distance Q_nearThe nearest obstacle position; i is the distance between two position points in the grid graph of the two-dimensional environment;

with Q_nearAs a parent node, according to the resultant force

Generating a new node Q_new：

Judging new node Q_newAnd Q_nearWhether there is a collision between the connecting line and the obstacle, or not, and if there is no collision, Q is set_newAdding to random expansion tree, if there is collision, then randomly locating point Q_randThe generation module regenerates the random position points;

starting point Q_startTo Q_newFor optimizing the starting point Q_startTo Q_newA path of (a);

the expansion stopping judging module is used for judging whether the expansion of the random expansion tree is stopped or not; the judging method comprises the following steps: when the new node Q_newInto a destination position Q_goalIn the neighborhood of or is Q_goalStopping the expansion of the random expansion tree when the random expansion tree is self;

the pruning processing module is used for carrying out pruning processing on the random expanded tree;

a path generation module for generating a path from the destination position Q_goalStarting to trace back the father nodes in sequence until the starting point Q_startEnd, find a piece from Q_startTo Q_goalIs not obstructed.

Has the advantages that: the robot path planning method and the robot path planning system disclosed by the invention have the following advantages:

1. according to the invention, a gravitational method is introduced into Qnew expansion, and an adaptive function is designed to adjust the ratio of the random point gravitational force to the target point gravitational force in the new node expansion process, so that the RRT algorithm path search speed and obstacle avoidance capability are improved;

2. in the process of path optimization, an improved rewiring method is introduced, so that the time complexity is reduced, and the improved RRT algorithm has progressive optimization;

3. in the invention, in the processing of the nodes, a greedy algorithm is introduced for pruning, a large number of redundant nodes are filtered, and a better initial path and a faster convergence speed are obtained under the condition of keeping the time complexity and the space complexity the same.

Drawings

FIG. 1 is a flow chart of a robot path planning method disclosed in the present invention;

FIG. 2 is a schematic diagram of a two-dimensional scene;

FIG. 3 is a schematic diagram of the generation of a new node;

FIG. 4 is a schematic diagram of an optimization path;

FIG. 5 is a schematic illustration of a pruning process;

FIG. 6 is a schematic diagram of a robot path planning system according to the present disclosure;

FIG. 7 is a diagram showing simulation results in the example;

FIG. 8 is a schematic diagram of simulation results after pruning processing in the example;

fig. 9 is a schematic diagram of the robot movement pattern.

Detailed Description

The invention is further elucidated with reference to the drawings and the detailed description.

The invention discloses a robot path planning method for improving RRT, which comprises the following steps of:

s1, dividing a two-dimensional scene of robot activity into a passable area and an impassable obstacle, and establishing a two-dimensional grid map; acquiring initial position Q of robot in two-dimensional scene_startAnd destination position Q_goal；

In the embodiment, the point cloud data of the robot activity scene is extracted by using a laser radar and a binocular camera, the cloud data is imaged by combining a ros platform, the passable area and the barrier are distinguished, the constructed two-dimensional scene is shown in fig. 2, the barrier is a black part, and the rest white parts are the passable area of the robot.

If from Q_startTo Q_goalIf the straight path does not touch the obstacle, the planned path is Q_startTo Q_goalStraight line segment of (2); otherwise, Q is_startAs a root node of a random expansion tree in the RRT algorithm, initializing a target direction weight k to be 1, and determining a planned path according to steps S2-S7;

s2, generating random position points Q in the motion area of the robot_rand(ii) a If Q is_startAnd Q_randCrosses an obstacle, the Q is discarded_randAnd again generate a new Q_randUp to Q_startAnd Q_randThe wiring of (a) does not pass through the barrier;

s3, traversing the current random expansion tree, searching and Q_randClosest point Q_near；

S4, calculating Q_nearMinimum distance to obstacle D_obAnd calculating a repulsion factor h:

rho is the movement step length of the robot;

calculating Q_randTo Q_nearGravitation of (2)

Q_goalTo Q_nearGravitation of (2)

Obstacle pair Q_nearRepulsive force of

Calculating Q_nearResultant force of the process

Wherein

w is gravity weight and is preset (0, 1)]A constant within a range; q_obIs a distance Q_nearThe nearest obstacle position; calculating the distance between two position points in the two-dimensional environment grid graph;

in that

The calculation formula of the method is added with a self-adaptive function related to the distance of the obstacle, so that the proportion of repulsive force in resultant force is adjusted, the phenomenon that the random tree is not vibrated before encountering continuous obstacles is avoided, the random tree is diffused to a region far away from the obstacle as soon as the random tree approaches the obstacle region, the random tree is rapidly expanded to a target point when the random tree is not in the obstacle region, the planning time is shortened, and the number of sampling points is reduced.

In order to avoid oscillation when the random tree node reaches the target point, the present embodiment adjusts the target direction weight k according to the distance between the current node and the target point: if F₁＞F₂That is, the current position is farther from the target position, the value of k is increased, and k is k + Δ k; otherwise, reducing the value of k, wherein k is k-delta k;Δ k is a preset update step, and in this embodiment, Δ k is 0.1.

With Q_nearAs a parent node, according to the resultant force

Generating a new node Q_new：

As shown in FIG. 3, is a new node Q_newAnd combined force

A schematic diagram of (a);

judging new node Q_newAnd Q_nearWhether there is a collision between the connecting line and the obstacle, or not, and if there is no collision, Q is set_newAdding the random position points to a random expansion tree, and jumping to S2 to regenerate random position points if collision exists;

s5 optimization starting point Q_startTo Q_newA path of (a); the method specifically comprises the following steps:

s5.1 with Q_newAs a center of circle, R_nearCalculating neighborhood V for radius_near；R_nearA preset wiring length;

s5.2, locating in the neighborhood V in the random expansion tree_nearThe nodes in the set S_nearObtaining a set S_nearThe first external node of each node in the set S_pnear(ii) a The first external node of the node s is the random spanning tree, and the distance between the node s and the first external node is smaller than the first distance R₁A node of (2); s is as large as S_near；S_sum＝S_near∪S_pnear；

As shown in fig. 4, the small circles in the figure represent nodes in the random spanning tree, the numbers in the small circles are the numbers of the nodes, the lines between the nodes represent paths, and the numbers on the lines represent path lengths. In FIG. 4(a), node No. 1 is the starting point Q_startAnd the node 5 is the closest point Q found in the previous step S3_nearAnd generates a new node Q according to S4_newNode number 10. As shown in FIG. 4(b), with Q_newAs the center R_nearIs a radiusResulting neighborhood V_nearIs the inner area of the dotted circle in the figure; v_nearThe internal nodes have nodes numbered 5, 6, 7, 9 and 10, and form a set S_near(ii) a A first distance R with the nodes numbered 5, 6, 7, 9 and 10 as the circle center₁A circle is drawn for the radius, and the node inside the circle is the first outer node of the node at the circle center. In this example R₁Setting the number to 3, and then obtaining the number 6 first external node as the number 8 node; the first external node of the node No. 7 is a node No. 11; then S is obtained_pnearComposed of nodes numbered 8 and 11, and then a set S is obtained_nearAnd S_pnearIs a union of S_sum. Here by setting the first distance R₁Defined neighborhood V_nearThe outer node set of the inner nodes is to search for V uniformly_nearThe peripheral range reduces the probability of overlong searching time caused by the uncertainty of the expansion of the random point, reduces the searching time, accelerates the speed of path optimization and improves the efficiency of path searching.

S5.3 traversing set S_sumNode in (1), judge Q_newParent node of (1) is replaced by S_sumAfter the node in (1), whether Q is shortened_startTo Q_newIf shortened, Q is performed_newThe parent node of (3) is replaced, otherwise, the parent node is not replaced; as shown in FIG. 4(c), after traversal, Q is added_newThe father node of the node is replaced by a node No. 6;

s5.4, searching for Q_newOf the second external node S_pnewNode Q_newThe second external node of (2) is the AND Q in the random spanning tree_newIs less than the second distance R₂A node of (2); in this example, R is set₂4, as shown in FIG. 4(d), the solid line great circles 5, 6, 7, 9, 11 constitute a set S_pnew。

S5.5, traversing S_nearIf the parent node is replaced by Q, the node in (1) is judged_newAnd S_pnewWhether or not the nodes in the union of (1) will later reduce Q_startTo the S_nearIf the path is reduced, the S is carried out_nearThe replacement operation of the parent node of the node in (1),otherwise, it does not operate. As shown in fig. 4(e), the parent node of the node 7 is replaced with the node 11.

S6, repeating the steps S3-S5, and when a new node Q_newInto a destination position Q_goalIn the neighborhood of or is Q_goalStopping the expansion of the random expansion tree when the random expansion tree is self; pruning the random expanded tree; the pruning treatment specifically comprises the following steps:

root node Q from random spanning tree_startInitially, a destination position Q capable of collision-free connection is sought_goalThen, the intermediate point P1 is used as a starting point to find a destination position Q capable of connecting without collision by using P1 as a starting point_goalUntil Q is found, repeating the steps until the intermediate point P2_goalOr into the destination position Q_goalWithin a neighborhood of (c); q_startAnd all intermediate points form an optimized path in sequence. As shown in fig. 5, the path obtained from the random spanning tree with a circle as the starting point and a square as the destination point is the path shown as L1 in the figure, and the path after pruning is L2.

S7 from the destination position Q_goalStarting to trace back the father nodes in sequence until the starting point Q_startEnd, find a piece from Q_startTo Q_goalIs not obstructed.

The invention also discloses a system for realizing the robot path planning method, as shown in fig. 6, the method comprises the following steps:

the two-dimensional scene establishing module 1 is used for dividing a two-dimensional scene of robot activity into a passable area and an impassable barrier and establishing a two-dimensional grid map; acquiring initial position Q of robot in two-dimensional scene_startAnd destination position Q_goal；

Random position point Q_randA generation module 2 for randomly positioning the point Q_rand；

Closest point Q_near A generating module 3 for traversing the current random spanning tree, searching and Q_randClosest point Q_near；

New node Q_newA generating module 4 for generating a new node Q_newThe generation steps are as follows:

calculating Q_nearTo obstaclesMinimum distance D_obAnd calculating a repulsion factor h:

rho is the movement step length of the robot;

calculating Q_randGravity of pair F₁、Q_goalTo Q_nearGravitation F₂Barrier pair Q_nearRepulsive force F₃(ii) a Calculating Q_nearResultant force of the process

Wherein

w is gravity weight and is preset (0, 1)]A constant within a range; q_obIs a distance Q_nearThe nearest obstacle position; calculating the distance between two position points in the two-dimensional environment grid graph;

according to F₁And F₂And updating k: if F₁＞F₂K is k + Δ k; otherwise k is k- Δ k; and deltak is a preset updating step length.

With Q_nearAs a parent node, according to the resultant force

Generating a new node Q_new：

Judging new node Q_newAnd Q_nearWhether there is a collision between the connecting line and the obstacle, or not, and if there is no collision, Q is set_newAdding to random expansion tree, if there is collision, then randomly locating point Q_randThe generation module regenerates the random position points;

starting point Q_startTo Q_newThe path optimization module 5 of (1) for optimizing the starting point Q according to steps S5.1-S5.5_startTo Q_newA path of (a);

stopAn expansion judging module 6, configured to judge whether to stop expanding the random expansion tree; the judging method comprises the following steps: when the new node Q_newInto a destination position Q_goalIn the neighborhood of or is Q_goalStopping the expansion of the random expansion tree when the random expansion tree is self;

a pruning processing module 7, configured to perform pruning processing on the random expanded tree;

a path generation module 8 for generating a path from the destination position Q_goalStarting to trace back the father nodes in sequence until the starting point Q_startEnd, find a piece from Q_startTo Q_goalIs not obstructed.

The effect graph of the route planning of the improved RRT algorithm is shown in FIG. 7, the effect graph of the route planning after pruning processing is shown in FIG. 8, the thin line part in the graph is the step length of each expansion, the thick line part is the actual searched route, the starting point in the graph is (0, 30), and the end point in the graph is (46, 20). Compared with the path planned by the traditional RRT algorithm, the improved path has the advantages that the iteration times are reduced by 27%, the used time is reduced by 47%, the length of the planned path is reduced by 58%, and the number of corners is reduced by 69%.

In this embodiment, the motion mode of the robot is eight-neighborhood motion, that is, the robot can move forward, backward, leftward, rightward, leftward and forward by 45 degrees, rightward and forward by 45 degrees, leftward and rearward by 45 degrees, and rightward and rearward by 45 degrees, as shown in fig. 9.

Claims (8)

1. A robot path planning method for improving RRT is characterized by comprising the following steps:

s1, dividing a two-dimensional scene of robot activity into a passable area and an impassable obstacle, and establishing a two-dimensional grid map; acquiring initial position Q of robot in two-dimensional scene_startAnd destination position Q_goal；

If from Q_startTo Q_goalIf the straight path does not touch the obstacle, the planned path is Q_startTo Q_goalStraight line segment of (2); otherwise, Q is_startAs the root node of the random expansion tree in the RRT algorithm, initializing the target direction weightDetermining a planned path according to steps S2-S7 when k is 1;

s2, generating random position points Q in the motion area of the robot_rand(ii) a If Q is_startAnd Q_randCrosses an obstacle, the Q is discarded_randAnd again generate a new Q_randUp to Q_startAnd Q_randThe wiring of (a) does not pass through the barrier;

s3, traversing the current random expansion tree, searching and Q_randClosest point Q_near；

S4, calculating Q_nearMinimum distance to obstacle D_obAnd calculating a repulsion factor h:

rho is the movement step length of the robot;

calculating Q_randTo Q_nearGravitation of (2)

Q_goalTo Q_nearGravitation of (2)

Obstacle pair Q_nearRepulsive force of

Calculating Q_nearResultant force of the process

Wherein

w is gravity weight and is preset (0, 1)]A constant within a range; q_obIs a distance Q_nearMore recentAn obstacle position; calculating the distance between two position points in the two-dimensional environment grid graph;

with Q_nearAs a parent node, according to the resultant force

Generating a new node Q_new：

Judging new node Q_newAnd Q_nearWhether there is a collision between the connecting line and the obstacle, or not, and if there is no collision, Q is set_newAdding the random position points to a random expansion tree, and jumping to S2 to regenerate random position points if collision exists;

s5 optimization starting point Q_startTo Q_newA path of (a);

s6, repeating the steps S3-S5, and when a new node Q_newInto a destination position Q_goalIn the neighborhood of or is Q_goalStopping the expansion of the random expansion tree when the random expansion tree is self; pruning the random expanded tree;

s7 from the destination position Q_goalStarting to trace back the father nodes in sequence until the starting point Q_startEnd, find a piece from Q_startTo Q_goalIs not obstructed.

2. The robot path planning method according to claim 1, wherein the step S4 further includes: according to F₁And F₂And updating k: if F₁＞F₂K is k + Δ k; otherwise k is k- Δ k; and deltak is a preset updating step length.

3. The robot path planning method according to claim 1, wherein the step S5 specifically includes:

s5.1 with Q_newAs a center of circle, R_nearCalculating neighborhood V for radius_near；R_nearA preset wiring length;

s5.2, locating in the neighborhood V in the random expansion tree_nearThe nodes in the set S_nearObtaining a set S_nearThe first external node of each node in the set S_pnear(ii) a The first external node of the node s is the random spanning tree, and the distance between the node s and the first external node is smaller than the first distance R₁A node of (2); s is as large as S_near；，S_sum＝S_near∪S_pnear；

S5.3 traversing set S_sumNode in (1), judge Q_newParent node of (1) is replaced by S_sumAfter the node in (1), whether Q is shortened_startTo Q_newIf shortened, Q is performed_newThe parent node of (3) is replaced, otherwise, the parent node is not replaced;

s5.4, searching for Q_newOf the second external node S_pnewNode Q_newThe second external node of (2) is the AND Q in the random spanning tree_newIs less than the second distance R₂A node of (2);

s5.5, traversing S_nearIf the parent node is replaced by Q, the node in (1) is judged_newAnd S_pnewWhether or not the nodes in the union of (1) will later reduce Q_startTo the S_nearIf the path is reduced, the S is carried out_nearThe replacement of the node parent node in (1) and not otherwise.

4. The robot path planning method according to claim 1, wherein the pruning of the random spanning tree in step S6 specifically includes:

root node Q from random spanning tree_startInitially, a destination position Q capable of collision-free connection is sought_goalThen, the intermediate point P1 is used as a starting point to find a destination position Q capable of connecting without collision by using P1 as a starting point_goalUntil Q is found, repeating the steps until the intermediate point P2_goalOr into the destination position Q_goalWithin a neighborhood of (c); q_startAnd all intermediate points form an optimized path in sequence.

5. A system for improved RRT robotic path planning, comprising:

the two-dimensional scene establishing module is used for dividing a two-dimensional scene of robot activity into a passable area and an impassable barrier and establishing a two-dimensional grid map; acquiring initial position Q of robot in two-dimensional scene_startAnd destination position Q_goal；

Random position point Q_randA generation module for randomly positioning the point Q_rand；

Closest point Q_nearA generating module for traversing the current random spanning tree, searching and Q_randClosest point Q_near；

New node Q_newA generation module for generating a new node Q_newThe generation steps are as follows:

calculating Q_nearMinimum distance to obstacle D_obAnd calculating a repulsion factor h:

rho is the movement step length of the robot;

calculating Q_randTo Q_nearGravitation of (2)

Q_goalTo Q_nearGravitation of (2)

Obstacle pair Q_nearRepulsive force of

Calculating Q_nearResultant force of the process

Wherein

w is gravity weight and is preset (0, 1)]A constant within a range; q_obIs a distance Q_nearThe nearest obstacle position; calculating the distance between two position points in the two-dimensional environment grid graph;

with Q_nearAs a parent node, according to the resultant force

Generating a new node Q_new：

Judging new node Q_newAnd Q_nearWhether there is a collision between the connecting line and the obstacle, or not, and if there is no collision, Q is set_newAdding to random expansion tree, if there is collision, then randomly locating point Q_randThe generation module regenerates the random position points;

starting point Q_startTo Q_newFor optimizing the starting point Q_startTo Q_newA path of (a);

the expansion stopping judging module is used for judging whether the expansion of the random expansion tree is stopped or not; the judging method comprises the following steps: when the new node Q_newInto a destination position Q_goalIn the neighborhood of or is Q_goalStopping the expansion of the random expansion tree when the random expansion tree is self;

the pruning processing module is used for carrying out pruning processing on the random expanded tree;

a path generation module for generating a path from the destination position Q_goalStarting to trace back the father nodes in sequence until the starting point Q_startEnd, find a piece from Q_startTo Q_goalIs not obstructed.

6. A robot path planning system according to claim 5, characterized in that the new node Q_newThe generation module is further to: according to F₁And F₂And updating k: if F₁＞F₂K is k + Δ k; otherwise k is k- Δ k; and deltak is a preset updating step length.

7. A robot path planning system according to claim 5, characterized by a starting point Q_startTo Q_newThe optimizing by the path optimizing module specifically includes:

s5.1 with Q_newAs a center of circle, R_nearCalculating neighborhood V for radius_near；R_nearA preset wiring length;

s5.2, locating in the neighborhood V in the random expansion tree_nearThe nodes in the set S_nearObtaining a set S_nearThe first external node of each node in the set S_pnear(ii) a The first external node of the node s is the random spanning tree, and the distance between the node s and the first external node is smaller than the first distance R₁A node of (2); s is as large as S_near；，S_sum＝S_near∪S_pnear；

S5.3 traversing set S_sumNode in (1), judge Q_newParent node of (1) is replaced by S_sumAfter the node in (1), whether Q is shortened_startTo Q_newIf shortened, Q is performed_newThe parent node of (3) is replaced, otherwise, the parent node is not replaced;

s5.4, searching for Q_newOf the second external node S_pnewNode Q_newThe second external node of (2) is the AND Q in the random spanning tree_newIs less than the second distance R₂A node of (2);

s5.5, traversing S_nearIf the parent node is replaced by Q, the node in (1) is judged_newAnd S_pnewWhether or not the nodes in the union of (1) will later reduce Q_startTo the S_nearIf the path is reduced, the S is carried out_nearThe replacement of the node parent node in (1) and not otherwise.

8. The robot path planning system according to claim 5, wherein the pruning processing module specifically prunes the random spanning tree by:

root node Q from random spanning tree_startInitially, a destination position Q capable of collision-free connection is sought_goalThen, the intermediate point P1 is used as a starting point to find a destination position Q capable of connecting without collision by using P1 as a starting point_goalUntil Q is found, repeating the steps until the intermediate point P2_goalOr into the destination position Q_goalWithin a neighborhood of (c); q_startAnd all intermediate points form an optimized path in sequence.

Images