Unmanned aerial vehicle flight path planning method

Abstract

The invention discloses an unmanned aerial vehicle flight path planning method, and relates to the technical field of unmanned aerial vehicle navigation and control, and the method comprises the steps: guiding a random search tree to rapidly expand towards a target through a target deviation and artificial potential field gravitational potential field negative gradient deviation sampling strategy, achieving the heuristic intelligent sampling of a free space, achieving the rapid and efficient convergence of an RRT * algorithm, and achieving the rapid and efficient detection of the flight path of an unmanned aerial vehicle. And the optimal track is quickly and efficiently converged. Redundant nodes of a feasible track can be deleted, the number of track nodes is reduced, and the voyage is shortened.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle navigation and control technology, and in particular to a unmanned aerial vehicle track planning method based on heuristic intelligent sampling and redundant point deletion RRT* algorithm. Background Art

[0002] UAV trajectory planning is the basis of UAV navigation and control. UAV trajectory planning is to search for an optimal or suboptimal collision-free path from the starting state to the target state under comprehensive consideration of factors such as fuel consumption, threats, and environment. Path planning generally starts with establishing an environmental model, that is, constructing a connectivity graph of the traversable area of ​​the UAV based on its connectivity, called a road map, and then searching for a path that meets the conditions on the road map based on a graph search algorithm.

[0003] Sampling-based motion planning (SBMP) algorithm is an important graph search algorithm that can effectively solve high-dimensional trajectory planning problems with complex constraints. The SBMP algorithm discretizes the continuous state space through a large number of sample points, thereby decomposing the optimal trajectory planning problem into a series of relaxed and simplified two-point boundary value problems, and then connects these sample points in the form of a topological graph.

[0004] The SBMP algorithm mainly includes the Probabilistic Roadmap (PRM) algorithm and the Rapid-Exploring Random Tree (RRT) algorithm. Most of the existing SBMP algorithms are based on improvements of these two algorithms.

[0005] The RRT algorithm is widely used because it has high optimization performance, does not require geometric division of the mission area in advance, the computational complexity does not change significantly with the increase in the number of obstacles or threats, and can find feasible solutions for trajectory planning in complex environments. The RRT algorithm is probabilistically complete, but it cannot guarantee optimality. Karaman and Frazzoli gave the asymptotic optimality conditions of the SBMP algorithm and designed an optimal RRT algorithm with asymptotic optimality, called the RRT* algorithm.

[0006] The standard RRT* algorithm includes two stages: the construction of a random search tree and the reverse search of a feasible path through the random search tree. The construction of a random search tree uses the starting state as the root node of the tree and randomly spreads the nodes to grow leaf nodes from the root node until the leaf nodes reach the vicinity of the target position. In this way, the growth of the random search tree is completed. In the reverse search to generate a feasible path stage, the parent nodes are searched in reverse order to generate a path from the starting state to the target state.

[0007] Random sample points are the key to driving the RRT* algorithm to expand into space. Most RRT* algorithms in the prior art use a uniform sampling strategy, which has low information utilization efficiency and slow algorithm convergence. In addition, due to the randomness of RRT* algorithm sampling, the final track obtained by the reverse search of the random search tree contains redundant nodes that do not meet the performance constraints of the drone. Summary of the invention

[0008] In view of the above problems, the present invention provides a UAV trajectory planning method for overcoming the above problems or at least partially solving the above problems.

[0009] The present invention provides the following scheme:

[0010] A UAV trajectory planning method, comprising:

[0011] S1: Determine the starting node, target node, state space, obstacle space, free space and preset parameters;

[0012] S2: Initialize the random search tree and determine the vertex set and edge set of the tree;

[0013] S3: Determine the number of iterations and the target deviation rate;

[0014] S4: if it is determined that the number of iterations is greater than the maximum number of iterations or the distance between a node in the vertex set and the target point is less than the preset parameter, then jump to step S13;

[0015] S5: Heuristic intelligent sampling of free space is realized based on target bias and negative gradient offset of gravitational potential field of artificial potential field to generate random sample points;

[0016] S6: Execute nearest point search: search for the nearest neighbor node to the random sample point from the vertex set;

[0017] S7: Solve the two-point boundary value problem without trajectory constraints under the system dynamics constraints and controllability constraints, and obtain the local planning state node;

[0018] S8: Execute collision detection to determine whether a collision occurs when the neighboring node moves to the local planning state node. If a collision occurs, jump to step S5;

[0019] S9: Select the parent node of the neighboring node;

[0020] S10: adding the neighboring node to the vertex set;

[0021] S11: random tree reshaping, taking cost optimization as consideration, correcting the node affiliation near the neighboring node, so that the nearby nodes have a chance to reselect the parent node;

[0022] S12: Update the number of iterations and jump to step S4;

[0023] S13: adding the target node to the vertex set, and obtaining the parent node of the target node with the optimal cost as a consideration;

[0024] S14: Find a path from the starting node to the target node according to a random search tree;

[0025] S15: Delete redundant nodes on the path from the starting node to the target node.

[0026] Preferably: the step S5 is specifically as follows:

[0027] S501: Generate a pseudo-random number;

[0028] S502: If the pseudo-random number is less than the target deviation rate, determining that the probability of the random sampling point being set as the target node is the target deviation rate;

[0029] S503: If the pseudo-random number is greater than the target deflection rate, a spatial random point is generated in the free space; based on the artificial potential field model, the spatial random point moves along the negative gradient direction of the gravitational potential field, which is expressed by the following formula:

[0030] F att =(-2k att )*(q goal -q rand )

[0031]

[0032] In the formula, k att represents the gravitational coefficient, q goal represents the target node, q rand represents a random sample point, X rnode represents a random point in space, and λ represents the moving length in the direction of the negative gradient of the gravitational potential field.

[0033] Preferably: the step S6 is specifically as follows:

[0034] For the vertex set, the nearest neighbor node to the random sample point is searched using the Euclidean distance as the metric, that is:

[0035]

[0036] Where: q near represents the neighboring node, q rand represents a random sample point, and V represents a vertex set.

[0037] Preferably: the step S7 is specifically as follows:

[0038] Initially, the neighboring nodes and the random sample points are considered, and the dynamic constraints and control capability constraints of the motion system are considered to obtain the local planning state nodes, namely:

[0039]

[0040] In the formula, q new represents the local planning state node, η is the local planning step length, q rand represents a random sample point.

[0041] Preferably: the step S9 is specifically as follows:

[0042] According to the local planning state node and the vertex set, a neighboring node set of the local planning state node is found from the fixed point set, and a node is selected from the neighboring node set as a parent node of the local planning state node based on cost optimization.

[0043] Preferably: the step S15 is specifically as follows:

[0044] S1501: The initial planned track node sequence is P 1 ,P 2 ,…,P N ;

[0045] S1502: Set a node sequence set Γ, Γ = {P N};

[0046] S1503: Set i=1, j=N;

[0047] S1504: Determine P i With P j Does the line between them intersect with the obstacle? If so, i'=i+1; jump to step S1506

[0048] S1505: Γ=Γ∪{P i},j=i,i=1;

[0049] S1506: Determine whether j is equal to 1, if not, jump to step S1504;

[0050] S1507: Output node sequence set Γ.

[0051] According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0052] The embodiment of the present application provides a method for planning the trajectory of a drone. Through the sampling strategy of target bias and negative gradient offset of the artificial potential field gravitational potential field, the strategy guides the random search tree to quickly expand toward the target, realizes heuristic intelligent sampling of free space, and achieves fast and efficient convergence of the RRT* algorithm, and quickly and efficiently converges to the optimal trajectory. It can delete redundant nodes of feasible trajectories, reduce the number of trajectory nodes, and shorten the flight range.

[0053] Of course, any product implementing the present invention does not necessarily need to achieve all of the advantages described above at the same time. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] 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 are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention, and for ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0055] Figure 1 is a flow chart of a UAV trajectory planning method provided by an embodiment of the present invention;

[0056] Figure 2 This is a schematic diagram of the result of performing UAV trajectory planning using the method provided by an embodiment of the present invention;

[0057] Figure 3 This is a schematic diagram of the results of UAV trajectory planning using traditional methods. DETAILED DESCRIPTION

[0058] The technical scheme in the embodiment of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiment of the present invention. Obviously, the described embodiment is only a part of the embodiment of the present invention, not all of the embodiments. Based on the embodiment of the present invention, all other embodiments obtained by ordinary technicians in this field belong to the scope of protection of the present invention.

[0059] See also Figure 1 , is a UAV trajectory planning method provided by an embodiment of the present invention, such as Figure 1 As shown, the method may include:

[0060] S1: Determine the starting node, target node, state space, obstacle space, free space and preset parameters;

[0061] S2: Initialize the random search tree and determine the vertex set and edge set of the tree;

[0062] S3: Determine the number of iterations and the target deviation rate;

[0063] S4: if it is determined that the number of iterations is greater than the maximum number of iterations or the distance between a node in the vertex set and the target point is less than the preset parameter, then jump to step S13;

[0064] S5: realizing heuristic intelligent sampling of free space based on target deflection and negative gradient offset of gravitational potential field of artificial potential field to generate random sample points; the specific step S5 is:

[0065] S501: Generate a pseudo-random number;

[0066] S502: If the pseudo-random number is less than the target deviation rate, determining that the probability of the random sampling point being set as the target node is the target deviation rate;

[0067] S503: If the pseudo-random number is greater than the target deflection rate, a spatial random point is generated in the free space; based on the artificial potential field model, the spatial random point moves along the negative gradient direction of the gravitational potential field, which is expressed by the following formula:

[0068] F att =(-2k att )*(q goal -q rand )

[0069]

[0070] In the formula, set flag = 1, k att represents the gravitational coefficient, q goal represents the target node, q rand represents a random sample point, X rnode represents a random point in space, and λ represents the moving length in the direction of the negative gradient of the gravitational potential field.

[0071] S6: Execute the nearest point search; search for the nearest neighbor node to the random sample point from the vertex set; the step S6 is specifically as follows:

[0072] For the vertex set, the nearest neighbor node to the random sample point is searched using the Euclidean distance as the metric, that is:

[0073]

[0074] Where: q near represents the neighboring node, q rand represents a random sample point, and V represents a vertex set.

[0075] S7: Solve the two-point boundary value problem without trajectory constraints under the system dynamics constraints and controllability constraints to obtain the local planning state node; the step S7 is specifically:

[0076] Initially, the neighboring nodes and the random sample points are considered, and the dynamic constraints and control capability constraints of the motion system are considered to obtain the local planning state nodes, namely:

[0077]

[0078] In the formula, q new represents the local planning state node, η is the local planning step length, q rand represents a random sample point.

[0079] S8: Execute collision detection to determine whether a collision occurs when the neighboring node moves to the local planning state node. If a collision occurs, jump to step S5;

[0080] S9: Select the parent node of the neighboring node; the step S9 is specifically as follows:

[0081] According to the local planning state node and the vertex set, a neighboring node set of the local planning state node is found from the fixed point set, and a node is selected from the neighboring node set as a parent node of the local planning state node based on cost optimization.

[0082] S10: adding the neighboring node to the vertex set;

[0083] S11: random tree reshaping, taking cost optimization as consideration, correcting the node affiliation near the neighboring node, so that the nearby nodes have a chance to reselect the parent node;

[0084] S12: Update the number of iterations and jump to step S4;

[0085] S13: adding the target node to the vertex set, and obtaining the parent node of the target node with the optimal cost as a consideration;

[0086] S14: Find a path from the starting node to the target node according to a random search tree;

[0087] S15: Deleting redundant nodes on the path from the starting node to the target node. The specific steps of step S15 are:

[0088] S1501: The initial planned track node sequence is P 1 ,P 2 ,…,P N ;

[0089] S1502: Set a node sequence set Γ, Γ = {P N};

[0090] S1503: Set i=1, j=N;

[0091] S1504: Determine P i With P j Does the line between them intersect with the obstacle? If so:

[0092] i'=i+1;

[0093] Jump to step S1506

[0094] S1505: Γ=Γ∪{P i},j=i,i=1;

[0095] S1506: Determine whether j is equal to 1, if not, jump to step S1504;

[0096] S1507: Output node sequence set Γ.

[0097] The drone trajectory planning method provided in the embodiment of the present application adopts a sampling strategy of target bias and negative gradient offset of artificial potential field gravitational potential field for heuristic intelligent sampling. This strategy guides the random search tree to quickly expand toward the target, realizes heuristic intelligent sampling of free space, and achieves fast and efficient convergence of the RRT* algorithm. It can delete redundant nodes of feasible trajectories, reduce the number of trajectory nodes, and shorten the flight distance.

[0098] The embodiment of the present application provides an RRT* method for heuristic intelligent sampling and redundant point deletion, which can realize UAV track planning, realize heuristic intelligent sampling of free space through target deviation and artificial potential field gravitational potential field negative gradient offset sampling strategy, and quickly and efficiently converge to the optimal track. Redundant point deletion realizes the deletion of redundant nodes of feasible tracks, shortens the flight range, and can include the following steps when specifically implemented:

[0099] S1: Set the starting node q init , target node q goal , state space X, obstacle space X obs , Free Space X free , X free =X / X obs , preset parameter γ.

[0100] S2: Initialize a random search tree T = {V, E}, where the vertex set of the tree is V, V = {q init}, the edge set E of the tree is

[0101] S3: Set the number of iterations n s =1, target deviation rate σ AGB =0.1.

[0102] S4: If the number of iterations n sGreater than the maximum number of iterations n or there is a node in the vertex set V that is consistent with q goal If the distance is less than the preset parameter γ, jump to step S13.

[0103] S5: Heuristic intelligent sampling of free space is realized based on target bias and negative gradient offset of artificial potential field gravitational potential field to generate random sample points q rand .

[0104] S6: Nearest point search. Search for a random sample node q from the vertex set V. rand The nearest node, which is recorded as the neighboring node q near .

[0105] S7: Solve the two-point boundary value problem without trajectory constraints under the system dynamics constraints and controllability constraints, and obtain the local planning state node q new , where the local planning state node q new ∈X free , terminal state q rand ∈X free , and make the local planning state node q new As close as possible to the random sample node q rand .

[0106] S8: Collision detection. Determine the collision between neighboring nodes q near Move to the local planning state node q new Whether a collision occurs. If a collision occurs, jump to step S5.

[0107] S9: Select local planning state node q new The parent node of the local planning state node q new And vertex set V, find the local planning state node q from V new The set of neighboring nodes V near , taking the cost optimization as the consideration, from the neighboring node set V near Select a node as the local planning state node q new The parent node of parent .

[0108] S10: local planning state node q new Add to the vertex set V.

[0109] S11: Random tree reshaping, taking cost optimization as consideration, and correcting the local planning state node q new The nearby node affiliation gives these nodes a chance to reselect their parent node.

[0110] S12: Update the number of iterations n' s =n s+1, jump to step S4.

[0111] S13: Set the target node q goal Add to V and consider the optimal cost to find the target node q goal The parent node of .

[0112] S14: According to the random search tree T = {V, E}, find the node from the starting node q according to the random search tree init Depart to the target node q goal Path.

[0113] S15: Initial node q init Depart to the target node q goal Redundant nodes of the path are deleted.

[0114] Preferably, step S5 is specifically:

[0115] S501: Generate a pseudo-random number r∈(0,1)

[0116] S502: If r<σ AGB , then q rand =q goal , flag = 0, that is, the probability of setting the random sampling point as the target point is σ AGB .

[0117] S503: If r>σ AGB , then a random point X is generated in free space mode , so that X mode ∈X free Based on the artificial potential field model, a random point X mode Moving along the negative gradient direction of the gravitational potential field, we have:

[0118] F att =(-2k att )*(q goal -q rand )

[0119]

[0120] Set flag = 1. att is the gravitational coefficient.

[0121] Preferably, step S6 specifically comprises:

[0122] For the vertex set V, using Euclidean distance as the metric, search for random sample points q rand The nearest node q near ,Right now:

[0123]

[0124] Preferably, step S7 specifically includes:

[0125] The neighboring node q in the initial state near and a random sample point q of the terminal state rand , considering the dynamic constraints and control capability constraints of the motion system, the local planning state node q is obtained new ,Right now:

[0126]

[0127] Among them, η is the local planning step size.

[0128] Preferably, step S15 is specifically as follows:

[0129] S1501: The initial planned track node sequence is P 1 ,P 2 ,…,P N ;

[0130] S1502: Set a node sequence set Γ, Γ = {P N};

[0131] S1503: Set i=1, j=N;

[0132] S1504: Determine P i With P j Whether the line between them intersects with the obstacle, if so, i'=i+1, and jump to step S1506;

[0133] S1505: Γ=Γ∪{P i},j=i,i=1;

[0134] S1506: Determine whether j is equal to 1. If not, jump to step S1504

[0135] S1507: Output node sequence set Γ.

[0136] In summary, the UAV trajectory planning method provided by this application uses the target bias and artificial potential field gravitational potential field negative gradient offset sampling strategy, which guides the random search tree to quickly expand toward the target, realizes heuristic intelligent sampling of free space, and achieves fast and efficient convergence of the RRT* algorithm, and quickly and efficiently converges to the optimal trajectory. It can delete redundant nodes of feasible trajectories, reduce the number of trajectory nodes, and shorten the flight range.

[0137] The results of the method provided in the examples of this application are as follows Figure 2 As shown, the results of not using the method provided in the embodiment of the present application are as follows Figure 3 shown.

[0138] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0139] It can be known from the description of the above implementation methods that those skilled in the art can clearly understand that the present application can be implemented by means of software plus a necessary general hardware platform. Based on such an understanding, the technical solution of the present application can be essentially or partly embodied in the form of a software product that contributes to the prior art. The computer software product can be stored in a storage medium such as ROM / RAM, a magnetic disk, an optical disk, etc., and includes several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in the various embodiments of the present application or certain parts of the embodiments.

[0140] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can refer to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the system or system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can refer to the partial description of the method embodiment. The system and system embodiments described above are merely schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without creative work.

[0141] The above description is only a preferred embodiment of the present invention and is not intended to limit the protection scope of the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention are included in the protection scope of the present invention.

Claims

1. A UAV trajectory planning method, characterized in that: include: S1: Determine the starting node, target node, state space, obstacle space, free space and preset parameters; S2: Initialize the random search tree and determine the vertex set and edge set of the tree; S3: Determine the number of iterations and the target deviation rate; S4: if it is determined that the number of iterations is greater than the maximum number of iterations or the distance between a node in the vertex set and the target point is less than the preset parameter, then jump to step S13; S5: Heuristic intelligent sampling of free space is realized based on target bias and negative gradient offset of gravitational potential field of artificial potential field to generate random sample points; S6: Execute nearest point search: search for the nearest neighbor node to the random sample point from the vertex set; S7: Solve the two-point boundary value problem without trajectory constraints under the system dynamics constraints and controllability constraints, and obtain the local planning state node; S8: Execute collision detection to determine whether a collision occurs when the neighboring node moves to the local planning state node. If a collision occurs, jump to step S5; S9: Select the parent node of the neighboring node; S10: adding the neighboring node to the vertex set; S11: random tree reshaping, taking cost optimization as consideration, correcting the node affiliation near the neighboring node, so that the nearby nodes have a chance to reselect the parent node; S12: Update the number of iterations and jump to step S4; S13: adding the target node to the vertex set, and obtaining the parent node of the target node with the optimal cost as a consideration; S14: Find a path from the starting node to the target node according to a random search tree; S15: Delete redundant nodes on the path from the starting node to the target node.

2. The UAV trajectory planning method according to claim 1, characterized in that: The step S5 is specifically as follows: S501: Generate a pseudo-random number; S502: If the pseudo-random number is less than the target deviation rate, determining that the probability of the random sampling point being set as the target node is the target deviation rate; S503: If the pseudo-random number is greater than the target deflection rate, generating a spatial random point in the free space; Based on the artificial potential field model, the random point in space moves along the negative gradient direction of the gravitational potential field, which is expressed by the following formula: F att =(-2k att )*(q goal -q rand ) In the formula, k att represents the gravitational coefficient, q goal represents the target node, q rand represents a random sample point, X rnode represents a random point in space, and λ represents the moving length in the direction of the negative gradient of the gravitational potential field.

3. The UAV trajectory planning method according to claim 1, characterized in that: The step S6 is specifically as follows: For the vertex set, the nearest neighbor node to the random sample point is searched using the Euclidean distance as the metric, that is: Where: q near represents the neighboring node, q rand represents a random sample point, and V represents a vertex set.

4. The UAV trajectory planning method according to claim 1, characterized in that: The step S7 is specifically as follows: Initially, the neighboring nodes and the random sample points are considered, and the dynamic constraints and control capability constraints of the motion system are considered to obtain the local planning state nodes, namely: In the formula, q new represents the local planning state node, η is the local planning step length, q rand represents a random sample point.

5. The UAV trajectory planning method according to claim 1, characterized in that: The step S9 is specifically as follows: According to the local planning state node and the vertex set, a neighboring node set of the local planning state node is found from the fixed point set, and a node is selected from the neighboring node set as a parent node of the local planning state node based on cost optimization.

6. The UAV trajectory planning method according to claim 1, characterized in that: The step S15 is specifically as follows: S1501: The initial planned track node sequence is P1, P2, ..., P N ; S1502: Set a node sequence set Γ, Γ = {P N }; S1503: Set i=1, j=N; S1504: Determine P i With P j Does the line between them intersect with the obstacle? If so, i'=i+1; jump to step S1506 S1505:Γ=Γ∪{P i },j=i,i=1; S1506: Determine whether j is equal to 1, if not, jump to step S1504; S1507: Output node sequence set Γ.

Images