CN111752306B - Unmanned aerial vehicle route planning method based on fast-expanding random tree - Google Patents

Abstract

The invention relates to an unmanned aerial vehicle route planning method based on a fast-expanding random tree, and belongs to the technical field of unmanned aerial vehicles. Compared with the prior art, the method has the main improvement points that: and a random point is used as a new added point to replace a fixed step length of the traditional method to generate the new added point, and the local path is closer to the barrier by creating a father node for the new added node. The scheme of generating new added points by using random points as the new added points to replace the fixed step length of the traditional method greatly improves the expansion speed of a random tree, reduces the constraint strength of the dimensionality and the size of the environment on a planning algorithm, and can be better suitable for a three-dimensional scene of unmanned aerial vehicle work; the local path is closer to the barrier by creating the father node for the newly added node, so that the cost of the newly added node from the newly added node to the root node after being added to the random tree is reduced, and the speed of generating the unmanned aerial vehicle track is improved.

Description

Unmanned aerial vehicle route planning method based on fast-expansion random tree

Technical Field

The invention relates to the field of path planning, in particular to a path planning method based on a fast-expanding random tree, which is widely applied to the fields of unmanned driving, mechanical arm control, unmanned aerial vehicle route planning and the like.

Background

Path planning is always a hot topic in the field of intelligent robots, and aims to search a collision-free continuous path from a starting point to a target point in a task space. The fast expanding Random tree (RRT/rapid expanding Random tree) algorithm (see Laval S M. rapid-expanding Random Trees: ANew Tool for Path Planning [ Z ]. USA: Computer Science Dept, low State Univ,1998.) abandons the Path optimization characteristic, explores the space through a scheme of Random sampling in the space, has great advantages in environmental adaptability and solving speed, and the Random tree can be regarded as a safety zone explored in the space, and when the Random tree expands to a target point, a feasible safety Path can be generated. Because the structure of the random tree is closely related to the generation sequence of the random points, the path generated by the traditional RRT algorithm is circuitous and not globally optimal, and the feasible path generated by the RRT algorithm can not be used as the planning track of the unmanned aerial vehicle in consideration of the motion characteristics of the unmanned aerial vehicle. The RRT algorithm (see Karaman, S., & Frazzoli, E. (2011) Sampling-based algorithm for optimal movement planning, the International Journal of nodes Research,30(7),846 and 894.) adds a local reconstruction link of a random tree, so that the algorithm has the characteristic of gradual optimization, and adds a parent node reselection link of a newly added node, which reduces the cost from a starting point to a point path, thereby reducing the time required for the algorithm to converge to the optimal path. On the basis, how to quickly converge the algorithm to the Optimal Path becomes a research hotspot, and Inform-RRT utilizes the initial solution of RRT algorithm to construct an elliptical Sampling region (see Gamma J D, Srinivasa S, Barfoot D. Informated RRT: Optimal Sampling-based Path Planning Focused via Direct Sampling of an adaptive Ellippsoil Hearistic [ J ].2014.), thereby effectively improving the utilization rate of Sampling points, but when the constructed Sampling region exceeds the original Sampling space, the algorithm cannot play an acceleration role; RRT-Smart also optimizes on the initial solution of RRT algorithm (see Islam F, Nasir J, Malik U, et al. RRT-Smart: Rapid conversion estimation of RRT) and methods of optimization using node deletion and intelligent sampling to reduce the optimization time, but the performance of algorithm is easily affected by the initial solution, only the same optimization result as the initial solution type can be obtained; Quick-RRT increases the number of search nodes using the connection relationship between random tree vertices (see Jeong I B, Lee S J, Kim J H. Quick-RRT: Triangular Initial-based Implementation of RRT with Improved Initial Solution and conversion Rate [ J ]. Expert Systems with Applications,2019,123(JUN.):82-90.), also shortens the effect of generating path length, but increases the number of search nodes results in increased consumption time and is therefore often limited in practical Applications.

Because the sampling-based path planning scheme cannot obtain the optimal path within a limited time, the optimal path is not needed to be optimized in a practical application scene, and particularly for high-speed moving objects such as unmanned planes, the requirements on the air route planning time are high. Therefore, the time for obtaining a feasible path should also be used as an important index for measuring the route planning algorithm, and the above-mentioned route planning methods cannot achieve a good balance between the time and the path length.

Disclosure of Invention

Technical problem to be solved

The current sampling-based path planning algorithm focuses on how to improve the convergence rate, and the length of an initial feasible path obtained by the algorithm is not emphasized, so that an expected path meeting the flight requirement of the unmanned aerial vehicle is obtained, and extra time is needed for optimization. In order to improve the efficiency of a path planning algorithm and enable the path planning algorithm to meet the application scene of an unmanned aerial vehicle, the invention provides an improved path planning method based on a fast-expansion random tree, and the cost from a newly-added node to the random tree to a root node is further reduced, so that the length of a generated path is reduced, the time required by optimization time is shortened, and the requirement of the unmanned aerial vehicle on a track planning algorithm is rapidly met.

Technical scheme

An unmanned aerial vehicle route planning method based on a fast expansion random tree is characterized by comprising the following steps:

step 1: unmanned aerial vehicle route planning environment Map with known position and size of obstacle, starting point x_startTarget area X_goalEnvironment X ═ X_obs∪X_freeWherein X is_obsIs the area of the obstacle, X_freeIs an obstacle-free area, and therefore has an X_obs∩X_freePhi is defined as; simultaneously initializing parameters, including: maximum number of cycles N_maxThe variable n of the cycle count_count1 is ═ 1; local reconstruction radius D_rewire(ii) a Random tree G ═ (V, E), where V ← { x)_startE ← phi shows the expansion relationship between nodes;

step 2: judging the number of cycles n_countWhether the maximum number of cycles N is exceeded_max: when the number of cycles n_countExceeding a set maximum number of cycles N_maxIf yes, jumping to step 15; when the number of cycles n_countLess than or equal to the maximum number of cycles N_maxThen, entering step 3;

and step 3: number of cycles n_countIncreasing by 1, and then entering the step 4;

and 4, step 4: randomly generating new nodes x in barrier-free areas in environment Map_new(ii) a After the node is generated, entering step 5;

and 5: finding a new node x in a list of nodes V contained in a known random tree_newThe node with the closest spatial distance is marked as x_nearest(ii) a Entering step 6;

step 6: discrimination point x_nearestTo x_newWhether the straight line path between the two paths contains the obstacle or not is judged, if so, the step 2 is returned; if no obstacle exists, entering step 7;

and 7: along x_nearestThe branch carries out one-by-one detection to the root node of the random tree, and finds out one branch and x_newLocal path between is secure and its parent node and x_newNode between which there is an obstacle, if it is denoted as x_bestAnd its father node is marked as Parent (x)_best) They need to satisfy: { x_best-x_new}∈X_freeAnd is

Find x_bestThen, using dichotomy at x_best-Parent(x_best) To a point x on the line segment_allowSo that x is_allowTo x_newThere is no obstacle between them, i.e. { x_allow-x_new}∈X_free(ii) a Then again using the dichotomy at x_allow-x_newFinding a point x on the line segment_new-parentSo as to satisfy { Parent (x) }_best)-x_new-parent}∈X_freeSo that x can be made_newBy x_new-parentAttached to Parent (x)_best)；

And 8: judgment of x_new-parentWhether creation was successful, if x_new-parentNot equal to phi and x_new-parent≠x_bestThen, it represents x_new-parentThe creation is successful, and the step 9 is entered; otherwise, go to step 13;

and step 9: x is to be_new-parentAdding the node into a random tree G to make the Parent node of the random tree G be Parent (x)_best) I.e. x_new-parentAdd to node list V of the tree and join line { Parent (x)_best)-x_new-parentAdding E; then entering step 10;

step 10: x is to be_newAdding the node to a random tree G to make the parent node x_new-parentI.e. x_newJoin the node list V of the tree and connect the line { x }_new-parent-x_newAdding E; then entering step 11;

step 11: at x_newAs a center of circle, D_rewireFor local reconstruction of random trees in the region of the radius, i.e. for any node x in the region_nearLet's x_nearAlong the tree to a starting point x_startHas a path length of Cost (x)_near)，x_newAlong the tree to a starting point x_startHas a path length of Cost (x)_new) If Cost (x)_near)＞Cost(x_new)+Distance(x_near,x_new) Then x is_nearIs changed into x_new(ii) a Then entering step 12;

step 12: discriminating the newly added node x_newIs equal to the target area X_goalInner, if x_new∈X_goalIf yes, then a feasible path exists, and step 14 is entered; if it is

Returning to the step 2;

step 13: x is to be_newAdding the node to a random tree G to make the parent node x_bestI.e. x_newJoin the node list V of the tree and connect the line { x }_best-x_newAdding E; then entering step 11;

step 14: generating a feasible path according to the relation between the nodes of the random tree G, and then entering step 15;

step 15: if the unmanned aerial vehicle route planning is successful, outputting an unmanned aerial vehicle route path; if the failure occurs, the failure is returned.

Advantageous effects

The invention provides an unmanned aerial vehicle route planning method based on a fast expansion random tree, which adopts a scheme that random points are used as new adding points to replace fixed step length of a traditional algorithm to generate new adding points, thereby greatly improving the expansion speed of the random tree, reducing the constraint intensity of environment dimensionality and size on a planning algorithm, and being better suitable for a three-dimensional scene of unmanned aerial vehicle work; the local path is closer to the barrier by creating a father node for the newly added node, so that the cost of the newly added node from the newly added node to the root node after being added to the random tree is reduced, and the speed of generating the unmanned aerial vehicle track is increased; the invention provides an expansion scheme of a random tree, which can be combined with a sampling scheme and an optimization scheme, thereby further optimizing the convergence speed and the planning speed and having very important engineering application value.

Drawings

FIG. 1 is a flow chart of a fast spanning random tree improvement method designed by the present invention;

FIG. 2 is a flow chart of creating a parent node for a newly added node in the planning method of the present invention;

FIG. 3 is a schematic diagram of creating a parent node for a newly added node in the planning method of the present invention;

FIG. 4 is a schematic illustration of local reconstruction in the planning method of the present invention;

FIG. 5 is map environment information;

FIG. 6 is a path generated by the planning method after first exploring to a target point in a two-dimensional scene;

FIG. 7 is a graph of the generated path versus time of the present invention;

Detailed Description

The invention will now be further described with reference to the following examples and drawings:

the invention relates to a path planning scheme which is named as Fast-RRT (F-RRT), wherein the F-RRT algorithm is an improved version of the RRT algorithm, the main improvement is how new points are added into a random tree, the whole algorithm can be divided into 15 steps corresponding to a flow chart shown in figure 1, rectangles in the flow chart are execution steps, diamonds represent judgment steps, and function or variable processing processes in the judgment steps are names or contents corresponding to the steps. The specific steps of the algorithm are as follows:

step 1: acquiring required relevant parameters, including: starting point x_startTarget area X_goalEnvironment X ═ X_obs∪X_freeWherein X is_obsIs the area of the obstacle, X_freeIs an obstacle-free area, and therefore has X_obs∩X_freePhi is given. And simultaneously initializing parameters in the algorithm, including: maximum number of cycles N_maxThe variable n of the cycle count_count1, process parameter D of new node creation_a(ii) a Local reconstruction radius D_rewire(ii) a Random tree G ═ (V, E), in which V ← { x)_startAnd E ← phi represents the expansion relationship among the nodes.

Step 2: judging the number of cycles n_countWhether the maximum number of cycles N is exceeded_max. To avoid trapping dead loops, when the number of loops n_countExceeding a set maximum number of cycles N_maxIf yes, jumping to step 15; when the number of cycles n_countLess than or equal to the maximum number of cycles N_maxAnd (4) entering the step 3.

And step 3: number of loops n to prevent program from falling into a dead loop_countIncreasing by 1, and then entering the step 4;

and 4, step 4: at X_freeRandomly sampling in the region to obtain a new node x_newThe operation content of this step is expressed by a function SampleFree. x is the number of_newAfter generation, the process proceeds to step 5.

And 5: in a random tree packageFinding and new node x in node list V_newThe node with the closest spatial distance is marked as x_nearest. The operation content of this step is described by a function Nearest. Then step 6 is entered.

Step 6: discrimination x_nearestTo x_newWhether the local path between is safe, i.e. segment x_nearest-x_newSatisfy { x_nearest-x_new}∈X_freeIf so, the local path is considered to be safe, and the step 7 is carried out; on the contrary, if

Then an obstacle exists in the local path and the process returns to step 2. The content of the operation step is represented by a Collision free function in the flow chart, the input of the Collision free function is two space coordinates, and the return value is yes or no, which respectively corresponds to safe and unsafe.

And 7: along x_nearestThe branch carries out one-by-one detection to the root node of the random tree, and finds out one branch and x_newLocal path between is secure and its parent node and x_newNode between which there is an obstacle, if it is denoted as x_bestAnd its father node is marked as Parent (x)_best) They need to satisfy: { x_best-x_new}∈X_freeAnd is

Find x_bestThen, using dichotomy at x_best-Parent(x_best) To a point x on the line segment_allowSo that x is_allowTo x_newWith no obstacle in between, i.e. { x_allow-x_new}∈X_free(ii) a Then again using dichotomy at x_allow-x_newFinding a point x on the line segment_new-parentSo as to satisfy { Parent (x) }_best)-x_new-parent}∈X_freeThus x can be made_newBy x_new-parentAttached to Parent (x)_best) The contents of this step are represented in the flow chart using the function creatnoden. The input to the function CreatNode includes x_new、x_nearestAnd G; and (3) returning: x is a radical of a fluorine atom_bestAnd x_new-parent. FIG. 2 is a diagram corresponding to the implementation flow of CreatNode in a computer program, in which function Distance (x)_a,x_b) Return x_aAnd x_bThe distance between the two elements, fig. 3 can more visually represent the operation of step 7. Then step 8 is performed.

And 8: judgment of x_new-parentWhether creation was successful, if x_new-parentNot equal to phi and x_new-parent≠x_bestThen, it represents x_new-parentThe creation is successful, and the step 9 is entered; otherwise, go to step 13.

And step 9: x is to be_new-parentAdding the node into a random tree G to make the Parent node of the random tree G be Parent (x)_best) I.e. x_new-parentAdd to node list V of the tree and join line { Parent (x)_best)-x_new-parentAdd E. The process of representing the tree of node addition in the flow chart by using a function Addnode, the input parameters of which are: g, x_p，x_aThe execution content is x_aAdded to G and is present as x_pIs a parent node. Then step 10 is entered.

Step 10: x is to be_newAdding the node to a random tree G to make the parent node of the random tree X_new-parentI.e. x_newJoin the node list V of the tree and connect the line { x }_new-parent-x_newAdd E. Then step 11 is entered.

Step 11: at x_newAs a center of circle, D_rewireAnd local reconstruction is carried out on the random tree in the area of the radius. I.e. for any node x within the area_nearLet's x_nearAlong the tree to a starting point x_startHas a path length of Cost (x)_near)，x_newAlong the tree to a starting point x_startHas a path length of Cost (x)_new) If Cost (x)_near)＞Cost(x_new)+Distance(x_near,x_new) Then x is_nearIs changed into x_newThe content of the operation of step 11 is represented in the flow chart of fig. 1 by a function Rewire which changes the connection between the nodes in the tree GRelationships, so both inputs and returns are random trees. Fig. 4 may visually represent the operation result of step 11. Then entering step 12;

step 12: discriminating the newly added node x_newIs equal to the target area X_goalInner, if x_new∈X_goalIf yes, then a feasible path exists, and step 14 is entered; if it is

The procedure returns to step 2.

Step 13: x is to be_newAdding the node to a random tree G to make the parent node x_bestI.e. x_newJoin node list V of tree and connect line { x_best-x_newAdd E. Then step 11 is entered.

Step 14: and generating a feasible path according to the relation between the nodes of the random tree G, using a function Findpath in the flow chart to represent the operation content in the step 14, wherein the input of the function Findpath is the random tree, and returning the ordered discrete points to represent the planned path. Then step 15 is entered.

Step 15: if the path planning is successful, the path is output, and if the path planning is failed, the failure is returned.

And (3) adopting matlab as a simulation platform, compiling a two-dimensional path planning program according to an algorithm, and designing an experiment for verification.

1. Feasibility of path planning

The purpose of the experiment is as follows: and verifying the feasibility of the path planning algorithm.

The experimental process comprises the following steps: FIG. 1 shows a flow chart.

The experimental scheme is as follows: starting point x_start(5, 5); target point x_goal＝(45,45)；X_goalIs x_goalA circular area with the radius of 2 and the center of a circle; the map shown in FIG. 5 is used for simulation, where the black square is the obstacle X_obsThe white area is a safety area X_free(ii) a Maximum number of cycles N_max200 parts of a total weight; local reconstruction radius D _rewire4; iteration precision D of newly added node_a＝1.5；。

And (4) analyzing results: carrying out two times of simulation experiments in a two-dimensional scene to obtain a random tree and a generation track shown in fig. 6, wherein thin straight lines are connection relations among nodes of the random tree, namely, the exploration range of the random tree to the environment is represented, triangular points are space discrete points of a generation path, and a thickened dotted line part is a path; and the generated paths are close to the obstacles in an optimal mode, because a scheme of creating a father node for the newly added nodes is adopted.

2. Algorithm convergence speed contrast experiment

Purpose of the experiment: and comparing the convergence speed of the F-RRT algorithm with the convergence speed of the RRT algorithm, thereby highlighting the advantage of the F-RRT algorithm in terms of the convergence speed.

The experimental process comprises the following steps: the F-RRT algorithm does not stop after detection of a feasible path, i.e. step 12in fig. 1 is omitted, i.e. step 2 is entered directly from step 11.

The experimental scheme is as follows: starting point x_start(5, 5); target point x_goal＝(45,45)；X_goalIs x_goalA circular area with the radius of 2 and the center of a circle; the map shown in FIG. 5 is used for simulation, where the black square is the obstacle X_obsThe white area is a safety area X_free(ii) a Maximum number of cycles N_max80000; local reconstruction radius D _rewire4; iteration precision D of newly added node_a1.5; the RRT algorithm takes the same parameters. Static experiments were performed 50 times under these conditions and the path lengths were analyzed.

And (4) analyzing results: fig. 7 shows data results of 50 static experiments, where x-axis is time and y-axis is path length, and the curve represents the average length of the generated paths at the time point during the simulation of the algorithm 50, where the solid line represents the length of the generated paths of the F-RRT algorithm, and the dotted line corresponds to the experimental results of the RRT algorithm. As can be seen from the figure, as the planning time increases, the path lengths generated by both algorithms are becoming smaller, but the path length at the planning position of the F-RRT algorithm in the same time is obviously shorter than the path length at RRT, and reaches the optimum earlier.

Claims (1)

1. An unmanned aerial vehicle route planning method based on a fast-expanding random tree is characterized by comprising the following steps:

step 1: unmanned aerial vehicle route planning environment Map with known position and size of obstacle, starting point x_startTarget area X_goalEnvironment X ═ X_obs∪X_freeWherein X is_obsIs the area of the obstacle, X_freeIs an obstacle-free area, and therefore has an X_obs∩X_freePhi is defined as; simultaneously initializing parameters, including: maximum number of cycles N_maxOf a loop count variable n_count1 is ═ 1; local reconstruction radius D_rewire(ii) a Random tree G ═ (V, E), where V ← { x)_startE ← phi shows the expansion relationship between nodes;

step 2: judging the number of cycles n_countWhether the maximum number of cycles N is exceeded_max: when the number of cycles n_countExceeding a set maximum number of cycles N_maxIf yes, jumping to step 15; when the number of cycles n_countLess than or equal to the maximum number of cycles N_maxThen, entering step 3;

and step 3: number of cycles n_countIncreasing by 1, and then entering the step 4;

and 4, step 4: randomly generating new nodes x in barrier-free areas in environment Map_new(ii) a After the node is generated, entering step 5;

and 5: finding a new node x in a list of nodes V contained in a known random tree_newThe node with the closest spatial distance is marked as x_nearest(ii) a Entering step 6;

step 6: discrimination point x_nearestTo x_newWhether the straight line path between the two paths contains the obstacle or not is judged, if so, the step 2 is returned; if no obstacle exists, entering step 7;

and 7: along x_nearestThe branch carries out one-by-one detection to the root node of the random tree, and finds out one branch and x_newBetween the local paths is safe and itParent node and x_newNode between which there is an obstacle, if it is denoted as x_bestAnd its father node is marked as Parent (x)_best) They need to satisfy: { x_best-x_new}∈X_freeAnd is

Find x_bestThen, using dichotomy at x_best-Parent(x_best) To a point x on the line segment_allowSo that x is_allowTo x_newWith no obstacle in between, i.e. { x_allow-x_new}∈X_free(ii) a Then again using the dichotomy at x_allow-x_newFinding a point x on the line segment_new-parentSo as to satisfy { Parent (x) }_best)-x_new-parent}∈X_freeThus x can be made_newBy x_new-parentAttached to Parent (x)_best)；

And 8: judgment of x_new-parentWhether creation was successful, if x_new-parentNot equal to phi and x_new-parent≠x_bestThen, it represents x_new-parentThe creation is successful, and the step 9 is entered; otherwise, go to step 13;

and step 9: x is to be_new-parentAdding the node into a random tree G to make the Parent node of the random tree G be Parent (x)_best) I.e. x_new-parentAdd to the node list V of the tree and join { Parent (x) }_best)-x_new-parentAdding E; then entering step 10;

step 10: x is to be_newAdding the node to a random tree G to make the parent node x_new-parentI.e. x_newJoin node list V of tree and connect line { x_new-parent-x_newAdding E; then entering step 11;

step 11: at x_newAs a center of circle, D_rewireFor local reconstruction of random trees in the region of the radius, i.e. for any node x in the region_nearLet's x_nearAlong the tree to the starting point x_startHas a path length of Cost (x)_near)，x_newAlong the tree to the starting point x_startHas a path length of Cost (x)_new) If Cost (x)_near)＞Cost(x_new)+Distance(x_near,x_new) Then x is_nearIs changed into x_new(ii) a Then, entering step 12;

step 12: discriminating the newly added node x_newIs equal to the target area X_goalIn, if x_new∈X_goalIf yes, a feasible path exists, and the step 14 is entered; if it is

Returning to the step 2;

step 13: x is to be_newAdding the node to a random tree G to make the parent node x_bestI.e. x_newJoin node list V of tree and connect line { x_best-x_newAdding E; then entering step 11;

step 14: generating a feasible path according to the relation between the nodes of the random tree G, and then entering step 15;

step 15: if the unmanned aerial vehicle route planning is successful, outputting an unmanned aerial vehicle route path; if the failure occurs, the failure is returned.

Images