CN115617076A - Trajectory planning and dynamic obstacle avoidance method for near-earth search UAV - Google Patents

Abstract

The invention discloses a track planning and dynamic obstacle avoidance method for a near-field search unmanned aerial vehicle, which comprises the following steps: s1, establishing an octree data structure discretization model aiming at a three-dimensional space, and performing three-dimensional expansion on a Jump Point Search (JPS) algorithm to enable an unmanned aerial vehicle to generate a collision-free path based on a static environment; s2, a path length optimization rule is provided based on a Lazy-Theta algorithm, so that multi-angle path finding of the JPS algorithm is realized, and the actual planned path of the unmanned aerial vehicle is further shortened; s3, performing smoothness processing on the optimized path through a seven-order polynomial interpolation method based on a minimun snap to ensure the continuity of the path nodes; and S4, improving an Artificial Potential Field (APF) method through a virtual gravitational field and a three-dimensional Brayton Hamm line algorithm, and realizing autonomous obstacle avoidance of the unmanned aerial vehicle in a dynamic threat conflict environment. The invention verifies the performance of the fusion scheme through simulation experiments. Simulation results show that the fusion scheme provided by the invention can effectively solve the problems of path planning and autonomous obstacle avoidance of the unmanned aerial vehicle in the low-altitude flight task.

Description

Trajectory planning and dynamic obstacle avoidance method for near-earth search UAV

technical field

The invention belongs to the field of UAV path planning and trajectory optimization, in particular to a method for trajectory planning and dynamic obstacle avoidance of a near-earth search UAV.

Background technique

Initially, due to its good concealment, relatively low cost, easy operation, and no fear of casualties, UAVs have been valued by the military industries of various countries. Today, the use of drones has gradually spread from the military to education, film and television, agriculture and service industries. Drones are also known as flying robots or unmanned aerial vehicles. Most of these craft are used for observation, search and careful planning. In recent years, with the advancement of drone technology, it has become possible for drones to perform near-ground search and rescue (SAR) in complex terrain environments such as cities and mountains. During a rescue mission in Oregon, for example, drones were used to confirm fatalities in slot canyons and eliminated the need for rescuers to make dangerous abseiling at night. This technology can greatly reduce search time, improve rescue efficiency, and provide guidance for teams in areas where manual patrols are difficult and time-consuming. For now, the airspace activities of drones in some areas are still very limited. Flight missions in no-fly zones require authorization and permission from relevant departments. However, this will far limit the scope of UAV search and rescue, and cannot give full play to the advantages and working standards of UAVs. Reasonable establishment and planning of a UAV-based search and rescue system framework, and the correct use of UAVs in cities and mountainous areas to perform near-ground search tasks will provide great help to rescue.

Unlike other commercial aircraft, UAVs take off and land vertically in short distances, which increases the adaptability of UAVs for near-ground search in complex environments. Nevertheless, unmanned aerial vehicles still face great challenges in the process of performing search and rescue missions near the ground. Currently, UAV-based near-terrestrial search and rescue typically has a similar set of constraints. First, the search time is tight, and the UAV needs to arrive at the mission location as quickly as possible, and any delay may lead to irreversible serious consequences; second, the working environment is not friendly, and the UAV is operating at low altitude. There are many complex scenes such as buildings and forests, resulting in an excessive number of turning angles in the path generated by the UAV. For unmanned aerial vehicles, this not only consumes energy, but also increases the search time; third, the dynamic threat that occurs during the execution of the mission, the unmanned aerial vehicle needs to keep a safe distance from the working environment and other dynamic obstacles to prevent the occurrence of channel conflicts Phenomenon. These unfavorable limiting factors make it difficult for UAVs to achieve flight tasks in low-altitude airspace in cities and mountainous areas.

Contents of the invention

In order to solve the above problems, the present invention proposes a trajectory planning and dynamic obstacle avoidance design method for near-earth search UAVs, through the efficient and fast path search ability of the three-dimensional JPS search algorithm based on the octree discretization map. Realize the autonomous navigation of UAVs when performing near-earth missions.

The technical scheme adopted in the present invention is: the trajectory planning and dynamic obstacle avoidance design method of near-earth search UAV, comprising the following steps:

S1, discretize the 3D working space of the UAV through the octree data structure, and perform 3D expansion for the jump point search algorithm (JPS), so that the UAV can generate a collision-free path based on the 3D static environment;

S2, based on the multi-angle pathfinding idea of the Lazy-Theta* algorithm, a path length optimization rule is proposed, and the multi-angle pathfinding of the JPS algorithm is realized, which further shortens the planned path of the UAV;

S3, smoothing the optimized path through the seventh-order polynomial interpolation method based on minimun snap to ensure the continuity at the path nodes;

S4, the artificial potential field (APF) method is improved through the virtual gravitational field and the three-dimensional Bresenham line algorithm to realize the autonomous obstacle avoidance of the UAV in the dynamic threat conflict environment.

Further, in the step S1, the establishment of an octree map based on the three-dimensional workspace of the drone and the three-dimensional expansion of the JPS algorithm specifically include:

S11, using the 3D working space of the UAV as the root node of the octree structure, divide the flight space into 8 or 0 parts in turn, and express obstacles through black, gray and white color blocks.

S12. Use a 7-element structure to mark the divided child nodes, and store the positional relationship between the current node and neighbor nodes, and the distance information from the current node to the root node.

S13, based on the three-dimensional discrete octree map model, the pruning neighbor rule of the JPS algorithm is corrected and expanded;

S14, based on the three-dimensional discrete octree map model, the forced neighbors and jump point selection methods of the JPS algorithm were corrected;

S15, after several experiments, the expansion direction of the original JPS algorithm is expanded and modified, making it expand from a two-dimensional plane to a three-dimensional workspace.

Further, the map of the octree structure is established:

The octree map regards the 3D workspace as a whole and sets it as the root node of the octree structure, and divides the scene into 8 or 0 parts in turn. The octree nodes are represented by three colors, which are: black means that the current node is occupied by obstacles, and the drone cannot pass; gray means that the number of obstacles in the current node is not clear; white means that the current node is a passable node, Drones can safely pass through this node. In the present invention, the number of layers of the octree map depends on the volume of the drone, that is, the volume of the smallest node of the octree should be greater than or equal to 1.5 times the volume of the drone.

Further, the specific process of modifying the pruning neighbor rule of the JPS algorithm is as follows:

(1) When the parent node parent(x) of the current node x is in the same plane as the current node, it satisfies the two-dimensional pruning rule and does not involve other planes. It is necessary to prune any neighbor node that meets the following constraints on the same plane.

len(<parent(x),....,n>\x)≤len(<parent(x),x,n>)

The len function represents the distance between nodes, <parent(x),....,n>\x represents all nodes that reach any neighbor node n from its parent node parent(x) without including the current node x gather.

(2) If the parent node parent(x) of the current node x is in the same plane as the current node and extends diagonally, it also satisfies the two-dimensional pruning rule and does not involve other planes. That is, any neighbor node n that meets the following constraints needs to be pruned on the same plane:

len(<parent(x),....,n>\x)<len(<parent(x),x,n>)

(3) If the parent node parent(x) of the current node x and the current node are expanded diagonally, it is necessary to expand to the upper layer along the diagonal direction and satisfy the above two formulas at the same time.

Further, the JPS algorithm's forced neighbors and jump point selection methods include:

Assume that in a minimum set with the current node x as the core, the set of neighbors to be pruned is P=<x ₁ , x ₂ , x ₃ . . . , x _n >. There is an obstacle x _ob ∈ P, if there is any neighbor node x _i ∈ P of x _ob , and _xi ≠ x _ob satisfies the following conditions, then _xi is called the forced neighbor of the current node x. Meanwhile, x is called a jump point.

len(<parent(x),x,n>)<len(<parent(x),...,n>\x)

Further, the extension direction of the 3D JPS algorithm is:

The JPS path is generated in the three-dimensional space formed by the octree, following the expansion method from the straight line of the same plane to the diagonal of the same plane, and then to the diagonal of the body. During the expansion process of the 3D JPS, based on the current node, it first expands for three straight line search directions parallel to the x-axis, y-axis and z-axis. Only when no jump point is found in the straight line direction, the same-plane diagonal expansion is performed, and the same-plane expansion follows the two-dimensional JPS algorithm expansion rule. If the jump point is still not found, then finally carry out the diagonal expansion of the volume, that is, expand a grid along the diagonal of the map volume and continue the straight line and plane search.

Further, the path length optimization rule proposed in the step S2 based on the multi-angle pathfinding idea of the Lazy-Theta* algorithm specifically includes:

S21, performing full path completion for the search nodes of the JPS algorithm, so that any two nodes on the path are directly connected;

S22. Perform parent node preset for all path nodes, and conduct overall management and design of the parent nodes of all nodes;

S23. Re-selecting a suitable parent node for the path node through line-of-sight (LOS) reachability detection, and deleting redundant path discontinuities to realize path length optimization.

Further, in the step S3, the seventh-order polynomial interpolation method based on minimun snap performs smoothness processing on the optimized path, specifically including:

S31, in order to ensure the degree of fitting between the optimized curve and the original path and the setting of the safe flight corridor, this method performs node interpolation processing on the node set of the three-dimensional JPS algorithm according to a fixed step size;

S32, performing path segmentation according to the number of nodes in the path node set generated by the three-dimensional JPS algorithm;

S33, setting the minimum snap as the optimization target, and setting the equality constraint condition according to the information of the position, velocity, acceleration and jerk of the path node and the interpolation point;

S34, using the space expansion strategy, adding the setting of the UAV flight corridor as an inequality constraint to the constraint matrix, and using quadratic programming to solve the optimization result.

Further, in the step S4, the artificial potential field (APF) method is improved through the virtual gravitational field and the three-dimensional Bresenham line algorithm to realize the autonomous obstacle avoidance of the UAV in a dynamic threat conflict environment, specifically including:

S41, the present invention further expands the Bresenham line algorithm in the three-dimensional space, and uses step size sampling to complete the detection of whether two points are directly reachable;

S42, using the method of adding a virtual target point around the obstacle that generates the local optimum to prompt the UAV to escape;

S43, classify the situation that the UAV falls into a local optimum and cannot reach the path target point during the dynamic obstacle avoidance process, and use the three-dimensional Bresenham line algorithm and the virtual target point for optimization.

Advantage of the present invention and beneficial effect are as follows:

For UAVs performing near-ground search and rescue, the complex environment, the large number of obstacles, and the existence of dynamic threats all pose great challenges to UAV path planning. Based on the above problems, the main points of the present invention are summarized as follows:

(1) For the static obstacles in the working environment of the UAV, the present invention expands the JPS algorithm in three dimensions through the octree structure map, so as to guide the UAV to perform flight tasks in the three-dimensional space. Through algorithm comparison experiments, it can be found that the three-dimensional JPS algorithm has good performance in time complexity and space complexity. Among them, the three-dimensional JPS algorithm reduces the calculation time by 88.45% to 30.18% compared with the three-dimensional A* algorithm, and reduces the calculation time by 34.83% to 4.75% compared to the RRT algorithm. The 3D JPS algorithm meets the requirements for high work efficiency in UAV search and rescue missions.

(2) For the generation path of the three-dimensional JPS algorithm, the present invention adopts an arbitrary-angle pathfinding strategy based on the Lazy-Theta* algorithm to optimize the generation path of the JPS algorithm. By changing the parent-child node relationship chain among the path nodes to eliminate redundant turning points, the multi-angle pathfinding of the 3D JPS algorithm is realized. Secondly, the present invention introduces the seventh-order polynomial interpolation optimization based on minimumsnap to further smooth the smoothness and continuity of the path generated by the three-dimensional JPS algorithm. Experiments show that the path optimized based on the parent node transfer rule is 4.8% to 8.1% shorter than the path obtained by the 3D JPS algorithm, and the total turning angle of the path is reduced by 51.6% to 93.2%. The overall optimization effect of the path is obvious. At the same time, the time complexity and space complexity of the algorithm caused by the parent node transfer rule and polynomial interpolation optimization have increased, and the operation time has increased by 2.5% to 5.4% compared with the three-dimensional JPS algorithm, and the memory usage has increased by 0.0014% to 0.0018%. .

(3) Finally, through the improved artificial potential field method based on the obstacle avoidance strategy, the dynamic threat that may appear in the low-altitude flight operation of the UAV is avoided. Firstly, the present invention improves the problem of falling into the local optimum in the artificial potential field method, and promotes the UAV to realize local optimum escape by adding a virtual target gravitational field and a three-dimensional Bresenham line algorithm. Secondly, through the combination of the three-dimensional JPS algorithm and the improved artificial potential field method, obstacle avoidance is carried out for dynamic threats entering the warning range. The experiments show that the dynamic obstacle avoidance strategy based on the improved artificial potential field method successfully completes the obstacle avoidance task in the case of a single intruder, a reverse intruder on the same trajectory and multiple intruders.

Description of drawings

Fig. 1 is a flowchart of the present invention;

Fig. 2 is the map building method based on octree data structure;

Figure 3 is the trajectory optimization process framework;

Figure 4 is the classification of the situation where the UAV falls into a local optimum;

Fig. 5 is the first experimental simulation result figure;

Fig. 6 is the second experimental result display figure;

Fig. 7 is a diagram showing the results of the third experiment.

detailed description

The technical solutions in the embodiments of the present invention will be described clearly and in detail below with reference to the drawings in the embodiments of the present invention. The described embodiments are only some of the embodiments of the invention.

Referring to Fig. 1, the concrete scheme that the present invention adopts is:

S1, discretize the 3D working space of the UAV through the octree data structure, and perform 3D expansion for the jump point search algorithm (JPS), so that the UAV can generate a collision-free path based on the 3D static environment:

S11, the octree map regards the three-dimensional workspace as a whole and sets it as the root node of the octree structure, and divides the scene into 8 or 0 shares in turn. The octree nodes are represented by three colors, which are: black means that the current node is occupied by obstacles, and the drone cannot pass; gray means that the number of obstacles in the current node is not clear; white means that the current node is a passable node, Drones can safely pass through this node. In the present invention, the number of layers of the octree map depends on the volume of the drone, that is, the volume of the smallest node of the octree should be greater than or equal to 1.5 times the volume of the drone.

S13, the present invention makes further amendments to the pruning neighbor rule of the three-dimensional JPS algorithm, and the specific amendments are as follows:

(1) When the parent node parent(x) of the current node x is in the same plane as the current node, it satisfies the two-dimensional pruning rule and does not involve other planes. It is necessary to prune any neighbor node that meets the following constraints on the same plane.

len(<parent(x),....,n>\x)≤len(<parent(x),x,n>)

The len function represents the distance between nodes, <parent(x),....,n>\x represents all nodes that reach any neighbor node n from its parent node parent(x) without including the current node x gather.

(2) If the parent node parent(x) of the current node x is in the same plane as the current node and extends diagonally, it also satisfies the two-dimensional pruning rule and does not involve other planes. That is, any neighbor node n that meets the following constraints needs to be pruned on the same plane:

len(<parent(x),....,n>\x)<len(<parent(x),x,n>)

(3) If the parent node parent(x) of the current node x and the current node are expanded diagonally, it is necessary to expand to the upper layer along the diagonal direction and satisfy the above two formulas at the same time.

S14, the present invention makes further amendments to the forced neighbor judgment standard and jump point selection of the 3D JPS algorithm. In the 3D JPS algorithm, forced neighbors and jump points are a set of relative concepts. Forcing the number and location of neighbors has an intuitive effect on hop points. We assume that in a minimum set with the current node x as the core, the pruned neighbor set is P=<x ₁ ,x ₂ ,x ₃ ...,x _n >. There is an obstacle x _ob ∈ P If there is any neighbor node x _i ∈ P of x _ob , and x _i ≠ x _ob satisfies the following conditions, then x _i is called the forced neighbor of the current node x. Meanwhile, x is called a jump point.

len(<parent(x),x,n>)<len(<parent(x),...,n>\x)

S15, generate a JPS path in the three-dimensional space formed by the octree, and follow the expansion method from the straight line on the same plane to the diagonal on the same plane, and then to the diagonal on the body. During the expansion process of the 3D JPS, based on the current node, it first expands for three straight line search directions parallel to the x-axis, y-axis and z-axis. Only when no jump point is found in the straight line direction, the same-plane diagonal expansion is performed, and the same-plane expansion follows the two-dimensional JPS algorithm expansion rule. If the jump point is still not found, then finally carry out the diagonal expansion of the volume, that is, expand a grid along the diagonal of the map volume and continue the straight line and plane search.

S16, the pathfinding steps of the three-dimensional JPS algorithm based on the octree map are as follows

(1) Initialize the static space environment and construct an octree grid map model to discretize the flight area of the UAV.

添加到openlist列表当中。(2) Set the drone's initial position x _start t and target point x _end . Will

Add it to the openlist list.

(3) Select the neighbor pruning rule according to the position information of the current node and its parent node, and calculate the expansion direction of the current node.

(4) Find jumping points by forcing neighbors' positions and add them to openlist.

(5) Calculate each node in the openlist through a heuristic function, and select the node with the smallest calculation value as the jump point and the parent node in the next search process.

(6) Add the node that acts as a parent node in the search process to the closelist list, and detect whether there is a target point in the closelist. If there is a target point, exit the search and form a search path through the closelist list. If there is no target point, repeat step (3) to step (6).

S2, as shown in Figure 3, the present invention proposes a path length optimization rule based on the multi-angle pathfinding idea of the Lazy-Theta* algorithm, and realizes the multi-angle pathfinding of the JPS algorithm with this, further shortening the unmanned aerial vehicle. Plan your path. The specific process is as follows:

(1) First, the present invention completes the path nodes of the 3D JPS algorithm, so that each node passed by the 3D JPS algorithm path is included in the node set. Let the set of nodes be pathlist=<x _end , x _n , x _n-1 ,..., x _i ,..., x ₁ , x _start >.

(2) Set the parent node of all nodes in the node set as the initial node, that is, all nodes have direct visibility to the initial node by default.

(3) Since the parent-child relationship between node x ₁ and x _start has not been modified, it is only necessary to perform LOS reachability detection with x _start from node x ₂ to node x _end in order to verify whether the visibility between nodes is established.

(4) If the visibility is established, there is no need to change the parent node relationship of the current node, and the node between the current node and x _start is deleted and added to the deletelist list.

(5) If the visibility is not established, it is necessary to re-find the parent node for the current node in the deletelist, and replace the parent node of the remaining nodes that have not undergone LOS detection with the parent node of the current node to continue detection.

(6) When the transfer of the parent node of x _end is completed, the algorithm ends and the shortest path is output.

S3, as shown in Figure 4, the present invention is based on the seventh-order polynomial interpolation method of minimun snap and has carried out smoothness processing to optimized path, and specific process is as follows:

(1) In order to ensure the degree of fitting between the optimized curve and the original path and the setting of the flight corridor, firstly, the present invention performs node interpolation processing for the node set of the three-dimensional JPS algorithm according to a fixed step.

(2) Segment according to the number of nodes in the path node set generated by the three-dimensional JPS algorithm. Assuming that there are n+1 nodes in the node set, the overall path is divided into n segments. The polynomial expression for n-segment paths is as follows.

Among them, p _m,i represents the i-th polynomial coefficient of the m-th path, t represents the flight time of the UAV, and T ₀ to T _n represent the moment when the UAV passes through the endpoint of each path. t ⁱ represents the i power of time t, where i∈[0,k]. k represents the total order of the polynomial.

(3) By performing multiple derivative calculations on f(t), the velocity, acceleration, jerk and snap functions of the corresponding path segment can be obtained. The present invention uses the minimum snap as the optimization objective formula of the polynomial as follows.

where T represents the total flight time of the UAV. By extending the formula J _n , the quadratic programming solution equation of the PQ problem can be obtained, as shown in the following expression.

Among them, J _n path represents the polynomial optimization objective function, P represents the position matrix of path nodes, and Q _n represents the parameter diagonal matrix.

(4) The position (P=f(t)), velocity (v=f'(t)), acceleration (a=f”(t)) and jerk function (jerk=f) of the node in the path process ⁽³⁾ (t)) and the snap function (snap=f ⁽⁴⁾ (t)) are used as the equality constraints in the quadratic programming. The expansion interval of each node (the expansion radius is r) is used as the inequality constraint to solve, the formula is as follows Shown, the set of smooth trajectory nodes can be obtained.

S4, the present invention adjusts the flight trajectory of the UAV in the path collision area by introducing the artificial potential field method, so as to avoid the collision phenomenon caused by the dynamic threat. During the path planning process of the UAV, the gravitational force generated by the target point on the UAV is equal to the repulsion force generated by the obstacle on the UAV, and the direction is opposite, that is, the force balance of the UAV at a non-target point Phenomenon. When the search falls into the force balance phenomenon of the UAV, that is, the UAV falls into a local optimum, it will cause the UAV to be unable to continue path planning, and eventually cannot reach the target point. In the present invention, the situation that the artificial potential field will produce a local optimum is divided into three types,

(1) When the obstacle is directly in front of the target point and the UAV is on the same channel, and the current gravity and repulsion force on the UAV are the same in magnitude and opposite in direction, as shown in Figure 4(1).

(2) When the obstacles are on both sides of the UAV, and the resultant direction of the repulsive force is opposite to the direction of the gravitational force, the magnitude is the same. As shown in Figure 4(2).

(3) When the obstacle is behind the target point, and the repulsive force is much greater than the attractive force, as shown in Figure 4(3).

Therefore, when the UAV is in a local optimal situation, the situation is divided into two according to whether there is direct visibility between the UAV and the target point. First of all, when the obstacle is between the UAV and the target point, it is found through the 3D LOS visibility detection that the UAV cannot pass through the obstacle directly to reach it, and it must be affected by unequal external forces to break the force balance of the UAV. Therefore, when the UAV falls into a local optimal solution and there is no direct visibility between the UAV and the target point, the present invention promotes the UAV to escape by adding a virtual target gravitational field around the local optimal point. Secondly, when the UAV falls into the local optimal solution, but there is direct visibility, the three-dimensional Bresenham line algorithm can be used to test whether the straight-line channel between the UAV and the target can be reached directly. Just connect the current UAV and the target point directly.

S5, the present invention adopts maps with a grid size of 8000 in the design of the randomness map, and the proportion of obstacles is 20%, 30% and 40%. The calculation time, space occupancy rate, number of path nodes, path length and total turning angle of the 3D JPS algorithm, 3D A* algorithm, RRT algorithm and Theta* algorithm are summarized and analyzed respectively. In the simulation experiment, the present invention uniformly selects the Euclidean distance formula as the distance calculation of the heuristic function. The experimental results are the average of 10 experiments. The simulation results are partially shown in Figure 5 below. Figure 5(1) shows the experimental results when the obstacle ratio is 20%. Figure 5(2) shows the experimental results when the obstacle ratio is 30%. Figure 5(3) shows the experimental results when the obstacle ratio is 40%.

S6, the present invention uses London, Boston, New York city center partial area maps and mountain area maps from the public database of Moving AI Lab (https://movingai.com/), respectively for the three-dimensional JPS algorithm and the path length optimization rule And the result path optimized by polynomial interpolation is used for comparative experiments. In order to verify the effect and authenticity of trajectory optimization based on the path generated by the 3D JPS algorithm. For the convenience of display, the present invention abbreviates the path of the path length optimization method as PNP-JPS, and the path optimized by polynomial interpolation is abbreviated as OP-PNP-JPS. Some experimental results are shown in Figure 6.

S7, the present invention conducts a simulation test for a single dynamic threat, a same-trajectory reverse threat, and multiple dynamic threats based on the map of a part of the central area of Shanghai in the public data set. In the design of this experiment, the warning range of the UAV is selected based on the spherical enclosing circle formed by the average value (20m) of the front-sight and side-sight precise detectable range of the laboratory UAV test platform DJI Mavic 2. When the dynamic threat enters the warning range, the artificial potential field method is adopted for dynamic obstacle avoidance. In the artificial potential field method, the range of the repulsive force field generated by obstacles is set to 5m, which is the average value of the precise measurement range distance of the forward and side infrared sensors of the UAV test platform DJI Mavic 2. When the distance between the drone and the obstacle is less than or equal to 5m, it will have a repulsive effect on the drone. The experimental results are now shown in Figure 7. Among them, Dynamic Threat represents a dynamic threat obstacle object, and Obstacle avoidance path represents a UAV obstacle avoidance path.

Claims (10)

1. The trajectory planning and dynamic obstacle avoidance method of near-earth search unmanned aerial vehicle, it is characterized in that, comprising the following steps: S1, discretize the 3D working space of the UAV through the octree data structure, and carry out 3D expansion for JPS, so that the UAV can generate a collision-free path based on the 3D static environment; S2, based on the Lazy-Theta* algorithm's multi-angle pathfinding idea to build a path length optimization rule, and realize the JPS algorithm multi-angle pathfinding; S3, smoothing the optimized path through the seventh-order polynomial interpolation method based on minimun snap; S4, through the virtual gravitational field and the three-dimensional Bresenham line algorithm, the artificial potential field method is improved to realize the autonomous obstacle avoidance of the UAV in the dynamic threat conflict environment. 2. according to the trajectory planning and the dynamic obstacle avoidance method of the described near ground search unmanned aerial vehicle of claim 1, it is characterized in that: described step S1 specifically comprises: S11, take the 3D working space of the UAV as the root node of the octree structure, divide the flight space into 8 or 0 parts in turn, and express obstacles through black, gray and white color blocks; S12, using a 7-element structure to mark the divided child nodes, and storing the positional relationship between the current node and neighbor nodes, and the distance information from the current node to the root node; S13, based on the three-dimensional discrete octree map model, the pruning neighbor rule of the JPS algorithm is corrected and expanded; S14, based on the three-dimensional discrete octree map model, the forced neighbor and jump point selection methods of the JPS algorithm are corrected; S15, expand and modify the extension direction of the original JPS algorithm, so that it expands from a two-dimensional plane to a three-dimensional work space. 3. According to the trajectory planning and dynamic obstacle avoidance method of the said near-ground search UAV according to claim 2, it is characterized in that: said black represents that the current node is occupied by obstacles, and the UAV is impassable; gray represents how many nodes the current node has. Obstacles are not clear; white indicates that the current node is a passable node, and the drone can safely pass through this node. 4. according to the track planning of claim 2 and the method for dynamic obstacle avoidance of near ground search unmanned aerial vehicle, it is characterized in that: the number of layers of described octree map model depends on the size of unmanned aerial vehicle volume, i.e. octree The volume of the smallest node is greater than or equal to 1.5 times the volume of the drone. 5. according to the trajectory planning and the dynamic obstacle avoidance method of the described near-ground search unmanned aerial vehicle of claim 2, it is characterized in that: the correction process of the pruning neighbor rule of the JPS algorithm is: (1) When the parent node parent(x) of the current node x is in the same plane as the current node, it satisfies the two-dimensional pruning rule and does not involve other planes. Any one that meets the following constraints needs to be pruned on the same plane Neighbor nodes of : len(<parent(x),....,n>\x)≤len(<parent(x),x,n>) The len function represents the distance between nodes, <parent(x),....,n>\x represents all nodes that reach any neighbor node n from its parent node parent(x) without including the current node x gather. (2) If the parent node parent(x) of the current node x is on the same plane as the current node, it also satisfies the two-dimensional pruning rule and does not involve other planes, that is, it needs to prune any A neighbor node n that satisfies the following constraints: len(<parent(x),....,n>\x)<len(<parent(x),x,n>) (3) If the parent node parent(x) of the current node x and the current node are expanded diagonally, it is necessary to expand to the upper layer along the diagonal direction and satisfy the above two formulas at the same time. 6. according to the trajectory planning of claim 2 near ground search unmanned aerial vehicle and dynamic obstacle avoidance method, it is characterized in that: the forced neighbor of described JPS algorithm and jump point selection mode, specifically comprise: Assume that in a minimum set with the current node x as the core, the pruned neighbor set is P=<x ₁ ,x ₂ ,x ₃ ...,x _n ＞, there is an obstacle x _ob ∈P, if there is Any neighbor node x _i ∈ P of x _ob , x _i ≠ x _ob satisfies the following conditions, then x _i is called the forced neighbor of the current node x, and x is called the jump point: len(<parent(x),x,n>)<len(<parent(x),...,n>\x). 7. According to any one of claim 2-6, the trajectory planning and the dynamic obstacle avoidance method of the near-ground search unmanned aerial vehicle, it is characterized in that: the extension direction of the JPS algorithm is: in the three-dimensional space formed by the octree The JPS path generated in the middle follows the expansion method from the straight line of the same plane to the diagonal of the same plane, and then to the diagonal of the body. During the expansion process of the 3D JPS, the current node is used as the reference, and the first point is parallel to the x-axis and y-axis. and the three straight-line search directions of the z-axis to expand. Only when no jump point is found in the straight line direction, the same-plane diagonal expansion is performed and the same-plane expansion follows the two-dimensional JPS algorithm expansion rule. If no jump point is found, then Finally, the volume diagonal expansion is performed, that is, a grid is expanded along the diagonal of the map volume and the line and plane search is continued. 8. according to the trajectory planning and the dynamic obstacle avoidance method of the described near ground search UAV of claim 1, it is characterized in that: described step S2 specifically comprises: S21, performing full path completion for the search nodes of the JPS algorithm, so that any two nodes on the path are directly connected; S22. Perform parent node preset for all path nodes, and conduct overall management and design of the parent nodes of all nodes; S23, re-selecting a suitable parent node for the path node through line-of-sight reachability detection, and deleting redundant path discontinuities to realize path length optimization. 9. According to the trajectory planning and the dynamic obstacle avoidance method of the described near ground search UAV of claim 1, it is characterized in that: described step S3 specifically comprises: S31, for the node set of the three-dimensional JPS algorithm, perform node interpolation processing according to a fixed step size; S32, performing path segmentation according to the number of nodes in the path node set generated by the three-dimensional JPS algorithm; S33, setting the minimum snap as the optimization target, and setting the equality constraint condition according to the information of the position, velocity, acceleration and jerk of the path node and the interpolation point; S34, using the space expansion strategy, adding the setting of the UAV flight corridor as an inequality constraint to the constraint matrix, and using quadratic programming to solve the optimization result. 10. The trajectory planning and dynamic obstacle avoidance method of the near-ground search UAV according to claim 1, characterized in that: said step S4 specifically comprises: S41, further expand the Bresenham line algorithm in the three-dimensional space, and use step size sampling to complete the detection of whether two points are directly reachable; S42, the method of adding a virtual target point around the obstacle that generates the local optimum prompts the UAV to escape; S43, classify the situation that the UAV falls into a local optimum and cannot reach the path target point during the dynamic obstacle avoidance process, and use the three-dimensional Bresenham line algorithm and the virtual target point for optimization.

Images