A method for UAV trajectory planning

Abstract

This invention discloses a UAV trajectory planning method, relating to the field of UAV navigation and control technology. Through a target bias and artificial potential field gravitational potential field negative gradient offset sampling strategy, this method guides a random search tree to rapidly expand towards the target, achieving heuristic intelligent sampling of free space and achieving fast and efficient convergence of the RRT* algorithm to the optimal trajectory. It can also remove redundant nodes from feasible trajectories, reducing the number of trajectory nodes and shortening the flight range.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) navigation and control technology, and in particular to a UAV trajectory planning method based on the heuristic intelligent sampling and redundant point deletion RRT* algorithm. Background Technology

[0002] Unmanned aerial vehicle (UAV) trajectory planning is the foundation of UAV navigation and control. It involves searching for an optimal or near-optimal collision-free path from the initial state to the target state, taking into account factors such as fuel consumption, threats, and the environment. Path planning typically begins by establishing an environmental model, constructing a connected graph (also known as a road map) of the UAV's traversable areas based on their connectivity. Then, a graph search algorithm is used to search for suitable paths on this road map.

[0003] Sampling-based motion planning (SBMP) is an important type of graph search algorithm that can effectively solve high-dimensional trajectory planning problems with complex constraints. SBMP discretizes the continuous state space using a large number of sample points, thus decomposing the optimal trajectory planning problem into a series of relaxed and simplified two-point boundary value problems. These sample points are then connected 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 existing SBMP algorithms are improvements based on these two algorithms.

[0005] The Regression-Range Optimization (RRT) algorithm is widely used due to its high optimization performance, the fact that it does not require prior geometric partitioning of the task area, its computational complexity does not change significantly with the increase in the number of obstacles or threats, and its ability to find feasible solutions for trajectory planning in complex environments. While the RRT algorithm is probabilistically complete, it cannot guarantee optimality. Karaman and Frazzoli provided the asymptotic optimality conditions for the SBMP algorithm and designed an optimal RRT algorithm with asymptotic optimality, called the RRT* algorithm.

[0006] The standard RRT* algorithm consists of two phases: constructing the random search tree and reverse-searching the random search tree to generate a feasible path. The random search tree is constructed by using the initial state as the root node and randomly scattering nodes to grow leaf nodes from the root node until a leaf node reaches near the target position. The reverse-searching phase generates a feasible path by sequentially searching the parent nodes in reverse, thus generating a path from the initial state to the target state.

[0007] Random sample points are crucial for driving the spatial expansion of the RRT* algorithm. Most existing RRT* algorithms employ a uniform sampling strategy, which suffers from low information utilization efficiency and slow convergence. Furthermore, due to the randomness of RRT* sampling, the final track obtained through back-search of the random search tree contains redundant nodes that do not meet the performance constraints of the UAV. Summary of the Invention

[0008] In view of the above problems, the present invention provides a method for planning the flight path of an unmanned aerial vehicle (UAV) to overcome or at least partially solve the above problems.

[0009] This invention provides the following solution:

[0010] A method for planning the trajectory of an unmanned aerial vehicle (UAV) includes:

[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 set of vertices and edges of the tree;

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

[0014] S4: If it is determined that the number of iterations is greater than the maximum number of iterations or that there is a node in the vertex set whose distance from the target node is less than the preset parameter, then proceed to step S13;

[0015] S5: Based on the target bias and the negative gradient shift of the gravitational potential field of the artificial potential field, heuristic intelligent sampling in free space is realized to generate random sample points;

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

[0017] S7: Solve the two-point boundary value problem without trajectory constraints under system dynamics constraints and control capability constraints to obtain the local planning state nodes;

[0018] S8: Perform collision detection to determine whether a collision occurs when moving from the nearest node to the local planning state node. If a collision occurs, proceed to step S5.

[0019] S9: Select the parent node of the nearest neighbor node;

[0020] S10: Add the nearest neighbor node to the vertex set;

[0021] S11: Random tree reshaping, with cost optimization as a consideration, corrects the membership relationship of the nearest neighbor node so that the nearby node has a chance to reselect the parent node.

[0022] S12: Update the iteration count, skip to step S4;

[0023] S13: Add the target node to the vertex set, and determine the parent node of the target node with the consideration of optimal cost;

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

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

[0026] Preferably, step S5 specifically comprises:

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

[0028] S502: If the pseudo-random number is less than the target bias rate, then the probability of determining the random sample point as the target node is the target bias rate;

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

[0030]

[0031] In the formula, Indicates the gravitational coefficient. Indicates the target node. Represents random sample points. Represents random points in space. This represents the length moved along the negative gradient direction of the gravitational potential field.

[0032] Preferably, step S6 specifically comprises:

[0033] For the set of vertices, using Euclidean distance as a metric, the nearest neighbor node to the random sample point is searched, i.e.:

[0034]

[0035] In the formula: Indicates the nearest neighbor node. Represents random sample points. Represents the set of vertices.

[0036] Preferably, step S7 specifically includes:

[0037] Initially, considering the nearest neighbor nodes and the random sample points, and taking into account the dynamic constraints and control capability constraints of the motion system, the local planning state nodes are obtained, namely:

[0038]

[0039] In the formula, This represents a local planning state node, where η is the local planning step size. This represents a random sample point.

[0040] Preferably, step S9 specifically includes:

[0041] Based on the local planning state node and the vertex set, find the nearest neighbor node set of the local planning state node from the vertex set, and select a node from the nearest neighbor node set as the parent node of the local planning state node with the consideration of optimal cost.

[0042] Preferably, step S15 specifically includes:

[0043] S1501: The initial planned track node sequence is as follows ;

[0044] S1502: Set the node sequence set , ;

[0045] S1503: Place ;

[0046] S1504: Judgment and Does the line connecting the two objects intersect with any obstacle? If it intersects, then... Jump to step S1506;

[0047] S1505: ;

[0048] S1506: Judgment Is it equal to 1? If not equal to 1, proceed to step S1504.

[0049] S1507: Output node sequence set .

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

[0051] This application provides a UAV trajectory planning method that employs a target bias and artificial potential field gravitational potential field negative gradient offset sampling strategy. This strategy guides a random search tree to rapidly expand towards the target, achieving heuristic intelligent sampling of free space and enabling fast and efficient convergence of the RRT* algorithm to the optimal trajectory. It also allows for the removal of redundant nodes from feasible trajectories, reducing the number of trajectory nodes and shortening the flight distance.

[0052] Of course, any product implementing this invention does not necessarily need to achieve all of the advantages described above at the same time. Attached Figure Description

[0053] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly described below. Obviously, the drawings described below are merely some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without any creative effort.

[0054] Figure 1 This is a flowchart of a UAV trajectory planning method provided in an embodiment of the present invention;

[0055] Figure 2 This is a schematic diagram showing the result of UAV trajectory planning using the method provided in the embodiments of the present invention;

[0056] Figure 3 This is a schematic diagram showing the results of drone trajectory planning using traditional methods. Detailed Implementation

[0057] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention are within the scope of protection of the present invention.

[0058] See Figure 1 This invention provides a method for planning the trajectory of an unmanned aerial vehicle (UAV), such as... Figure 1 As shown, the method may include:

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

[0060] S2: Initialize the random search tree and determine the set of vertices and edges of the tree;

[0061] S3: Determine the number of iterations and the target bias rate;

[0062] S4: If it is determined that the number of iterations is greater than the maximum number of iterations or that there is a node in the vertex set whose distance from the target node is less than the preset parameter, then proceed to step S13;

[0063] S5: Based on the target bias and the negative gradient shift of the gravitational potential field of the artificial potential field, heuristic intelligent sampling in free space is achieved to generate random sample points; the specific steps of S5 are as follows:

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

[0065] S502: If the pseudo-random number is less than the target bias rate, then the probability of determining the random sample point as the target node is the target bias rate;

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

[0067]

[0068] In the formula, place , Indicates the gravitational coefficient. Indicates the target node. Represents random sample points. Represents random points in space. This represents the length moved along the negative gradient direction of the gravitational potential field.

[0069] S6: Perform nearest neighbor search; search for the nearest neighbor node to the random sample point from the vertex set; step S6 specifically involves:

[0070] For the set of vertices, using Euclidean distance as a metric, the nearest neighbor node to the random sample point is searched, i.e.:

[0071]

[0072] In the formula: Indicates the nearest neighbor node. Represents random sample points. Represents the set of vertices.

[0073] S7: Solve the two-point boundary value problem without trajectory constraints under system dynamics constraints and control capability constraints to obtain the local planning state nodes; step S7 specifically involves:

[0074] Initially, considering the nearest neighbor nodes and the random sample points, and taking into account the dynamic constraints and control capability constraints of the motion system, the local planning state nodes are obtained, namely:

[0075]

[0076] In the formula, Represents the local planning state node. For local planning step size, This represents a random sample point.

[0077] S8: Perform collision detection to determine whether a collision occurs when moving from the nearest node to the local planning state node. If a collision occurs, proceed to step S5.

[0078] S9: Select the parent node of the nearest neighbor node; step S9 specifically involves:

[0079] Based on the local planning state node and the vertex set, find the nearest neighbor node set of the local planning state node from the vertex set, and select a node from the nearest neighbor node set as the parent node of the local planning state node with the consideration of optimal cost.

[0080] S10: Add the nearest neighbor node to the vertex set;

[0081] S11: Random tree reshaping, with cost optimization as a consideration, corrects the node membership relationship near the nearest node so that the nearby node has a chance to reselect the parent node.

[0082] S12: Update the iteration count, skip to step S4;

[0083] S13: Add the target node to the vertex set, and determine the parent node of the target node with the consideration of optimal cost;

[0084] S14: Find the path from the starting node to the target node based on the random search tree;

[0085] S15: Delete redundant nodes on the path from the starting node to the target node. Step S15 specifically involves:

[0086] S1501: The initial planned track node sequence is as follows ;

[0087] S1502: Set the node sequence set , ;

[0088] S1503: Place ;

[0089] S1504: Judgment and Does the line connecting the two objects intersect with any obstacle? If it intersects, then... ;

[0090] Proceed to step S1506;

[0091] S1505: ;

[0092] S1506: Judgment Is it equal to 1? If not equal to 1, proceed to step S1504.

[0093] S1507: Output node sequence set .

[0094] The UAV trajectory planning method provided in this application employs a heuristic intelligent sampling strategy using target bias and negative gradient offset sampling from an artificial potential field and a gravitational potential field. This strategy guides a random search tree to rapidly expand towards the target, achieving heuristic intelligent sampling of free space and achieving the fast and efficient convergence of the RRT* algorithm. It can also delete redundant nodes in feasible trajectories, reducing the number of trajectory nodes and shortening the flight distance.

[0095] This application provides a heuristic intelligent sampling and redundant point removal RRT* method, which can realize UAV trajectory planning. Through a target bias and artificial potential field gravitational potential field negative gradient offset sampling strategy, it achieves heuristic intelligent sampling of free space, quickly and efficiently converging to the optimal trajectory. Redundant point removal removes redundant nodes from feasible trajectories, shortening the flight distance. Specifically, its implementation may include the following steps:

[0096] S1: Set the starting node Target node State space X, obstacle space Free space Preset parameters .

[0097] S2: Initialize the random search tree The set of vertices of the tree , The set of edges of a tree For Ø.

[0098] S3: Set the number of iterations Target bias .

[0099] S4: If the number of iterations Greater than the maximum number of iterations or vertex set There exists a node and The distance is less than the preset parameter If so, proceed to step S13.

[0100] S5: Heuristic intelligent sampling in free space is achieved based on target bias and negative gradient shift of artificial potential field and gravitational potential field to generate random sample points. .

[0101] S6: Closest point search. From the set of vertices. Searching for random sample nodes The nearest node is denoted as the nearest neighbor node. .

[0102] S7: Solve the two-point boundary value problem without trajectory constraints under system dynamics and control capability constraints to obtain the local programming state nodes. Among them, local planning state nodes Terminal status And make the local planning state nodes As close as possible to the random sample node .

[0103] S8: Collision Detection. Determines if a collision occurs due to a nearest neighbor node. Move to the local planning state node Did a collision occur? If a collision occurred, proceed to step S5.

[0104] S9: Select local planning state nodes The parent node. Based on the local planning state node. and vertex set ,from Find the local planning state nodes The set of nearest neighbors Taking cost optimization as a consideration, the nearest neighbor set is selected. Select a node as the local planning state node. The parent node, denoted as .

[0105] S10: Set the local planning state nodes Add to vertex set .

[0106] S11: Random tree reshaping, with cost optimization as a consideration, corrects the local programming state nodes. The dependency relationship between nearby nodes gives these nodes a chance to reselect their parent node.

[0107] S12: Number of update iterations Skip to step S4.

[0108] S13: Set the target node Add to And, taking cost optimization as a consideration, the target node is obtained. The parent node.

[0109] S14: Based on the random search tree Find the starting node using a random search tree. Departure to target node The path.

[0110] S15: Initial Node Departure to target node Redundant nodes in the path are deleted.

[0111] Preferably, step S5 specifically includes:

[0112] S501: Generate a pseudo-random number

[0113] S502: If ,but The probability of setting a random sample point as the target node is... .

[0114] S503: If This generates random points in free space. , making Based on the artificial potential field model, random points Moving along the direction of the negative gradient of the gravitational potential field, we have:

[0115]

[0116] juxtaposition .in, It is the gravitational coefficient.

[0117] Preferably, step S6 specifically includes:

[0118] For the vertex set Using Euclidean distance as a metric, the distance to random sample points is retrieved. The nearest node ,Right now:

[0119] .

[0120] Preferably, step S7 specifically includes:

[0121] Nearest neighbor nodes in the initial state Random sample points of terminal state Considering the dynamic constraints and control capability constraints of the motion system, the local programming state nodes are obtained. ,Right now:

[0122] .

[0123] in, This is for local planning step size.

[0124] Preferably, step S15 specifically includes:

[0125] S1501: The initial planned track node sequence is as follows ;

[0126] S1502: Set the node sequence set , ;

[0127] S1503: Place ;

[0128] S1504: Judgment and Does the line connecting the two objects intersect with any obstacle? If it intersects, then... Proceed to step S1506;

[0129] S1505: ;

[0130] S1506: Judgment If the value is not equal to 1, proceed to step S1504.

[0131] S1507: Output node sequence set .

[0132] In summary, the UAV trajectory planning method provided in this application employs a target bias and artificial potential field gravitational potential field negative gradient offset sampling strategy. This strategy guides the random search tree to rapidly expand towards the target, achieving heuristic intelligent sampling of free space and achieving fast and efficient convergence of the RRT* algorithm, ultimately converging to the optimal trajectory. It also enables the removal of redundant nodes from feasible trajectories, reducing the number of trajectory nodes and shortening the flight distance.

[0133] The results of using the method provided in the embodiments of this application are as follows: Figure 2 As shown, the results of not using the method provided in the embodiments of this application are as follows: Figure 3 As shown.

[0134] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0135] As can be seen from the above description of the embodiments, those skilled in the art can clearly understand that this application can be implemented by means of software plus necessary general-purpose hardware platforms. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in various embodiments or some parts of the embodiments of this application.

[0136] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to mutually. Each embodiment focuses on describing the differences from other embodiments. In particular, for system or system embodiments, since they are basically similar to method embodiments, the description is relatively simple, and relevant parts can be referred to the descriptions in the method embodiments. The systems and system embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without creative effort.

[0137] The above description is merely a preferred embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention are included within the scope of protection of the present invention.

Claims

1. A method for planning the trajectory of an unmanned aerial vehicle (UAV), 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 set of vertices and edges of the tree; S3: Determine the number of iterations and the target bias rate; S4: If it is determined that the number of iterations is greater than the maximum number of iterations or that there is a node in the vertex set whose distance from the target node is less than the preset parameter, then proceed to step S13; S5: Based on the target bias and the negative gradient shift of the gravitational potential field of the artificial potential field, heuristic intelligent sampling of free space is realized to generate random sample points; S6: Perform nearest neighbor 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 system dynamics constraints and control capability constraints to obtain the local planning state nodes; S8: Perform collision detection to determine whether a collision occurs when moving from the nearest node to the local planning state node. If a collision occurs, proceed to step S5. S9: Select the parent node of the nearest neighbor node; S10: Add the nearest neighbor node to the vertex set; S11: Random tree reshaping, with cost optimization as a consideration, corrects the node membership relationship near the nearest node so that the nearby node has a chance to reselect the parent node. S12: Update the iteration count, skip to step S4; S13: Add the target node to the vertex set, and determine the parent node of the target node with the consideration of optimal cost; S14: Find the path from the starting node to the target node based on the 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, Step S5 specifically involves: S501: Generate a pseudo-random number; S502: If the pseudo-random number is less than the target bias rate, then the probability of determining the random sample point as the target node is the target bias rate; S503: If the pseudo-random number is greater than the target bias rate, generate a spatial random point in free space; Based on the artificial potential field model, the spatial random point moves along the negative gradient direction of the gravitational potential field, as expressed by the following equation: In the formula, Indicates the gravitational coefficient. Indicates the target node. Represents random sample points. Represents random points in space. This represents the length moved along the negative gradient direction of the gravitational potential field.

3. The UAV trajectory planning method according to claim 1, characterized in that, Step S6 specifically involves: For the set of vertices, using Euclidean distance as a metric, the nearest neighbor node to the random sample point is searched, i.e.: In the formula: Indicates the nearest neighbor node. Represents random sample points. Represents the set of vertices.

4. The UAV trajectory planning method according to claim 1, characterized in that, Step S7 specifically involves: Initially, considering the nearest neighbor nodes and the random sample points, and taking into account the dynamic constraints and control capability constraints of the motion system, the local planning state nodes are obtained, namely: In the formula, This represents a local planning state node, where η is the local planning step size. This represents a random sample point.

5. The UAV trajectory planning method according to claim 1, characterized in that, Step S9 specifically involves: Based on the local planning state node and the vertex set, find the nearest neighbor node set of the local planning state node from the vertex set, and select a node from the nearest neighbor node set as the parent node of the local planning state node with the consideration of optimal cost.

6. The UAV trajectory planning method according to claim 1, characterized in that, Step S15 specifically involves: S1501: The initial planned track node sequence is as follows ; S1502: Set the node sequence set , ; S1503: Place ; S1504: Judgment and Does the line connecting the two objects intersect with any obstacle? If it intersects, then... Jump to step S1506; S1505： ; S1506: Judgment Is it equal to 1? If not equal to 1, proceed to step S1504. S1507: Output node sequence set .

Images