A method, device, equipment and medium for unmanned aerial vehicle route planning

By obtaining the relevant parameters of the drone and building a threat vector field histogram, determining the contingency value and evasion cost model of the exploration nodes is solved, and the route planning problem of the drone in the case of sudden obstacles is achieved, and the optimal planning cost is achieved And the route avoids the indecision caused by the distance between drones and obstacles too close.

CN115031738BActive Publication Date: 2024-12-06AVIC (CHENGDU) UAS CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202210662519.0
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-06-13
Publication Date
2024-12-06
Estimated Expiration
2042-06-13

AI Technical Summary

Technical Problem

When facing sudden dynamic obstacles, the existing drone route planning method cannot ensure that the drone can quickly reach the target position at the optimal planning cost and optimal route, and the VFH algorithm is far from the distance between the drone and the obstacle. It is easy to hesitate in the near future. The VFH+ algorithm only makes the planning cost optimal but cannot guarantee the optimal route throughout the journey.

Method used

By obtaining relevant parameters of the drone, determine the target location and expected route length, determine the generation value of the exploration node based on the exploration distance and exploration depth, traverse and obtain the exploration node with the minimum generation value, judge whether it corresponds to the target location, and according to the threat Vector field histograms and cost models determine candidate evasion directions and heuristic functions, pruning and constructing evasion cost models to generate optimal routes.

Benefits of technology

In the case of obstacle interference, the drone can quickly reach the target position at the optimal planning cost and the optimal route, solving the problem of hesitation caused by the drone's distance from the obstacles, and ensuring that no one can be found The machine can safely reach the target position when encountering obstacles.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN115031738B_ABST
    Figure CN115031738B_ABST
Patent Text Reader

Abstract

The present application discloses a method, device, equipment and medium for UAV route planning, and relates to the field of UAV technology. Determine the target position and expected route length according to the current position to obtain the optimal route. Determine the cost value of each exploration node in the exploration node set according to the exploration distance and exploration depth; traverse and obtain the minimum cost value to obtain the optimal cost value. Determine whether the exploration node corresponds to the target position; if so, end the route planning; if not, determine whether the route length of the UAV from the current position to the target position corresponding to the exploration node is greater than the expected route length; if so, end the route planning. By continuously judging the relationship between the exploration node and the target position, when the exploration node is the target position, the target position can be reached according to the optimal route even when encountering obstacles; when the exploration distance is appropriate, the indecision caused by the close distance between the UAV and the obstacle is solved, and the optimal planning and optimal route are achieved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present application relates to the field of unmanned aerial vehicle (UAV) technology, and in particular to a method, device, equipment and medium for UAV route planning. Background Art

[0002] With the increasing development of intelligent warfare, drone technology is becoming more and more mature. Due to the advantages of drones such as low cost, simple take-off and landing, and flexible operation, their important position in the military field is increasing day by day. However, the environment of military operations is complex, and environmental threats are intertwined, which greatly reduces the flight path planning space of drones. Generally, there are two planning methods based on the degree of knowledge of the environment around the drone, namely global route planning and local route planning. Global route planning is to search for an optimal flight route that meets static constraints in a static environment; local route planning is to ensure that the drone can achieve real-time obstacle avoidance in an unknown environment, and enable the drone to travel to the target location according to the planned optimal route.

[0003] When the regional environment is fully perceived (there are no dynamic obstacles in the UAV's flight path), the A* algorithm (A_Star algorithm), genetic algorithm, etc. can be used for global route planning; but when the UAV faces sudden threats (such as the presence of dynamic obstacles), the UAV will not be able to safely reach the target location according to the pre-planned route. Existing local route planning methods can be vector field histogram (VFH) and VFH+, but the VFH algorithm often hesitates during the planning process due to the close distance between the UAV and the obstacle; the VFH+ algorithm only optimizes the planning cost, but cannot optimize the entire route of the UAV.

[0004] In view of the above-mentioned problems, finding out how to quickly reach the target location with the optimal planning cost and the optimal route in the presence of obstacle interference is a problem that technical personnel in this field are trying their best to solve. Summary of the invention

[0005] The purpose of this application is to provide a method, device, equipment and medium for UAV route planning, which is used to quickly reach the target location with the optimal planning cost and optimal route in the presence of obstacle interference.

[0006] In order to solve the above technical problems, the present application provides a UAV route planning method, comprising:

[0007] Obtain relevant parameters of the drone, which include at least one of the current position of the drone, threat information, flight safety distance, exploration distance, exploration depth, and route length;

[0008] Determine the target location and expected route length based on the current location;

[0009] Determine the cost value corresponding to each exploration node in the exploration node set according to the exploration distance and the exploration depth;

[0010] Traverse and obtain the exploration node corresponding to the minimum cost value;

[0011] Determine whether the exploration node corresponds to the target location;

[0012] If the explored node corresponds to the target location, the route planning ends;

[0013] If the exploration node does not correspond to the target location, determine whether the route length from the current location of the drone to the target location corresponding to the exploration node is greater than the expected route length;

[0014] If yes, the route planning ends;

[0015] If not, return to the step of obtaining relevant parameters of the drone.

[0016] Preferably, determining the cost value corresponding to each exploration node in the exploration node set according to the exploration distance and the exploration depth includes:

[0017] Determine the exploration step length based on the exploration distance and exploration depth;

[0018] Generate an exploration node set according to the exploration step length, the exploration node set includes multiple exploration nodes, and the exploration nodes correspond to the candidate avoidance directions one by one;

[0019] Determine the cost value corresponding to the exploration node.

[0020] Preferably, before determining the cost value corresponding to each exploration node in the exploration node set according to the exploration distance and the exploration depth, and after determining the target position and the expected route length according to the current position, the method further includes:

[0021] Construct a threat vector field histogram based on the current position and threat information;

[0022] Candidate avoidance directions are determined based on the threat vector field histogram and flight safety distance.

[0023] Preferably, after generating the exploration node set according to the exploration step length and before determining the cost value corresponding to the exploration node, the method further includes:

[0024] Prune the exploration nodes in the exploration node set.

[0025] Preferably, determining the cost value corresponding to the exploration node includes:

[0026] Determine the cost change factor weight based on the explored node and current position;

[0027] Construct an avoidance cost model based on the weights of cost change factors;

[0028] The cost value is determined based on the avoidance cost model.

[0029] Preferably, after determining the cost value corresponding to each exploration node in the exploration node set according to the exploration distance and the exploration depth, and before traversing and obtaining the exploration node corresponding to the minimum cost value, the method further includes:

[0030] Construct an estimated cost model based on the target exploration node and exploration node corresponding to the target location;

[0031] The heuristic function corresponding to each exploration node is determined according to the avoidance cost model and the estimated cost model.

[0032] Preferably, constructing a threat vector field histogram according to the current position and threat information includes:

[0033] Determine the activity window radius of the drone based on the flight radius of the drone and the distance between the drone and the current position;

[0034] Divide the area formed by the radius of the active window into a plurality of sector-shaped areas according to a preset resolution;

[0035] The threat vector field histogram is constructed based on the distance between the UAV and the obstacle in each sector area.

[0036] In order to solve the above technical problems, the present application also provides a UAV route planning device, comprising:

[0037] An acquisition module is used to acquire relevant parameters of the drone, and the relevant parameters include at least one of the current position of the drone, threat information, flight safety distance, exploration distance, exploration depth, and route length;

[0038] A first determination module, used to determine a target position and an expected route length according to a current position;

[0039] A second determination module is used to determine a cost value corresponding to each exploration node in the exploration node set according to the exploration distance and the exploration depth;

[0040] The traversal module is used to traverse and obtain the exploration node corresponding to the minimum cost value;

[0041] The first judgment module is used to judge whether the exploration node corresponds to the target location;

[0042] If the explored node corresponds to the target location, the route planning ends;

[0043] The second judgment module is used to judge whether the route length from the current position of the UAV to the target position corresponding to the exploration node is greater than the expected route length if the exploration node does not correspond to the target position;

[0044] If yes, the route planning ends;

[0045] If not, return to the step of obtaining relevant parameters of the drone.

[0046] In addition, the UAV route planning device also includes the following modules:

[0047] A third determination module is used to determine the exploration step length according to the exploration distance and the exploration depth;

[0048] A generation module, used for generating an exploration node set according to the exploration step length, wherein the exploration node set includes multiple exploration nodes, and the exploration nodes correspond to the candidate avoidance directions one by one;

[0049] The fourth determination module is used to determine the cost value corresponding to the exploration node.

[0050] A first building module is used to build a threat vector field histogram according to the current position and threat information;

[0051] The fifth determination module is used to determine a candidate avoidance direction according to the threat vector field histogram and the flight safety distance.

[0052] The pruning processing module is used to prune the exploration nodes in the exploration node set.

[0053] A sixth determination module, configured to determine a cost change factor weight according to the exploration node and the current position;

[0054] The second building module is used to build an avoidance cost model according to the weight of the cost change factor;

[0055] The seventh determination module is used to determine the cost value according to the avoidance cost model.

[0056] The third construction module is used to construct an estimated cost model according to the target exploration node and the exploration node corresponding to the target position;

[0057] The eighth determination module is used to determine the heuristic function corresponding to each exploration node according to the avoidance cost model and the estimated cost model.

[0058] a ninth determination module, configured to determine an activity window radius of the drone according to a flight radius of the drone and a distance between the drone and a current position;

[0059] A division module, used for dividing the area formed by the radius of the active window into a plurality of sector-shaped areas according to a preset resolution;

[0060] The fourth building module is used to build a threat vector field histogram according to the distance between the UAV and the obstacle in each sector area.

[0061] In order to solve the above technical problems, the present application also provides a UAV route planning device, including:

[0062] Memory for storing computer programs;

[0063] The processor is used to point to the computer program to implement the steps of the UAV route planning method.

[0064] In order to solve the above technical problems, the present application also provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of all the above-mentioned UAV route planning methods are implemented.

[0065] The present application provides a method for UAV route planning, which can obtain the optimal route of the UAV by obtaining the relevant parameters of the UAV; determining the target position and the expected route length according to the current position; determining the cost value corresponding to each exploration node in the exploration node set according to the exploration distance and exploration depth; traversing and obtaining the steps of the exploration node corresponding to the minimum cost value to obtain the optimal cost value; judging whether the exploration node corresponds to the target position; if the exploration node corresponds to the target position, the route planning is terminated; if the exploration node does not correspond to the target position, it is judged whether the route length of the UAV from the current position to the target position corresponding to the exploration node is greater than the expected route length; if so, the route planning is terminated; if not, it returns to the step of obtaining the relevant parameters of the UAV. By continuously judging the relationship between the exploration node and the target position, when the exploration node is the target position, it can fly to the target position according to the previously obtained optimal route, and can also reach the target position in the case of encountering obstacles; since the minimum cost value is obtained according to the exploration distance and exploration depth, when the exploration distance is appropriate, the phenomenon of indecision caused by the UAV being too close to the obstacle is also solved. At this time, the optimal planning cost can be obtained while the optimal route can be obtained.

[0066] The present application also provides a UAV route planning device, equipment and medium, with the same effect as above. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] In order to more clearly illustrate the embodiments of the present application, the following is a brief introduction to the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0068] Figure 1A flow chart of a method for planning a route for a UAV provided in an embodiment of the present application;

[0069] Figure 2 A flow chart of another UAV route planning method provided in an embodiment of the present application;

[0070] Figure 3 A threat vector field histogram provided by an embodiment of the present application;

[0071] Figure 4 A structural diagram of a UAV route planning device provided in an embodiment of the present application;

[0072] Figure 5 A structural diagram of a UAV route planning device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0073] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0074] The core of this application is to provide a UAV route planning method, device, equipment and medium, which can quickly reach the target location with the optimal planning cost and optimal route in the presence of obstacle interference.

[0075] In order to enable those skilled in the art to better understand the present application, the present application is further described in detail below in conjunction with the accompanying drawings and specific implementation methods.

[0076] In this application, for the global route planning of drones, A* algorithm or genetic algorithm, etc. are generally used. Among them, A* algorithm is the most effective direct search method for solving the shortest path in a static road network, and it is also an effective algorithm for solving many search problems. The closer the distance estimation value in the algorithm is to the actual value, the faster the final search speed. It can be intuitively understood that if an item is to be moved from point A to point B, but the two points are separated by a wall, the search area should be divided into square grids to simplify the search area. At this time, the search area is simplified to a two-dimensional array. Each item in the array represents a grid, and the state of each grid is divided into two types, namely walkable and unwalkable. By calculating which grids need to be walked from point A to point B, the corresponding path is found. Once the path is found, the item at point A can be moved from the center of one grid to the center of another grid until it reaches the destination point B.

[0077] However, when the drone faces sudden threats (such as dynamic obstacles), the drone will not be able to safely reach the target location according to the pre-planned route. At this time, local route planning is required for the path where dynamic obstacles appear. The local route planning methods for drones generally include VFH algorithm and VFH+ algorithm.

[0078] Among them, the VFH algorithm is represented by a histogram grid, but the uncertainty from sensor and modeling errors has a great impact. Unlike other obstacle avoidance algorithms, VFH takes into account the dynamics and shape of the drone and returns platform-specific steering instructions. The center of the VFH algorithm is to use a statistical representation of obstacles through a histogram grid. Such a representation is suitable for inaccurate sensor data and adapts to the fusion of multi-sensor readings.

[0079] The VFH+ algorithm is improved on the basis of the VFH algorithm, including the following aspects:

[0080] Threshold hysteresis: Hysteresis increases the smoothness of the planning trajectory;

[0081] Drone size: No need to manually adjust parameters through low-pass filter;

[0082] Obstacle forward vision: sectors blocked by obstacles are obscured, and the guidance angle will not directly obscure obstacles;

[0083] Cost Function: A cost function was added to better characterize the performance of the algorithm and to provide the possibility to switch the behavior by changing the cost function or its parameters.

[0084] However, the VFH algorithm often hesitates during the planning process because the distance between the drone and the obstacle is too close; the VFH+ algorithm only optimizes the planning cost, but cannot optimize the entire route of the drone. Therefore, the present application provides a drone route planning method as described in the following specific embodiments.

[0085] Figure 1 This is a flow chart of a method for planning a UAV route provided by an embodiment of the present application. Figure 1 As shown, the method comprises:

[0086] It should be noted that drones are generally equipped with laser radars for real-time perception of the surrounding environment. During the flight of a drone, when the laser radar finds that the flight route is blocked by sudden dynamic obstacles (threats), it will expand in the flyable direction according to the laser radar detection data and terrain data until it reaches the target location and then select the optimal path to replace the current part of the flight route.

[0087] S10: Obtain relevant parameters of the drone.

[0088] Related parameters generally include two categories, namely flight performance parameters and environmental parameters. Flight performance parameters generally include: the current position of the drone, flight safety distance, exploration distance, exploration depth, route length, maximum climb angle, maximum dive angle, maximum horizontal turning angle, maximum flight altitude, etc.; environmental parameters generally include terrain information, threat information, etc. In this embodiment, the above-mentioned related parameters are only some of the many parameters, and it does not mean that this application only includes the above parameters, and more parameters can also be included. The implementation method can be determined according to the specific implementation scenario.

[0089] The terrain information obtained is obtained by drawing the terrain using actual elevation data or simulating the terrain using the following formula:

[0090]

[0091] Among them, H i is the parameter that controls the height of the mountain. i , B i ) is the coordinate value of the center point of the i-th peak, K i is the parameter controlling the slope steepness of the i-th peak, N peak represents the number of mountain peaks, and h(x,y) is the terrain height value.

[0092] S11: Determine the target position and expected route length according to the current position.

[0093] The target position is related to the flight route and the laser radar detection range. In this embodiment, it can be set as the closest waypoint on the flight route that is 2-5 times the laser radar detection distance from the current position of the drone. The current position of the drone is recorded as q now , with the current UAV position as the center and 2-5 times the laser radar detection distance as the radius, select the intersection of the flight path and the arc as the target position, and record the target position as q target .

[0094] S12: Determine a cost value corresponding to each exploration node in the exploration node set according to the exploration distance and the exploration depth.

[0095] S13: traverse and obtain the exploration node corresponding to the minimum cost value.

[0096] S14: Determine whether the exploration node corresponds to the target location.

[0097] If the explored node corresponds to the target location, the route planning ends;

[0098] If the exploration node does not correspond to the target position, the process proceeds to step S15: determining whether the route length from the current position of the UAV to the target position corresponding to the exploration node is greater than the expected route length.

[0099] If yes, the route planning ends;

[0100] If not, return to step S10: obtain relevant parameters of the drone.

[0101] By obtaining the relevant parameters of the drone; determining the target position and the expected route length according to the current position, the optimal route of the drone can be obtained; determining the cost value corresponding to each exploration node in the exploration node set according to the exploration distance and exploration depth; traversing and obtaining the exploration node corresponding to the minimum cost value, the optimal cost value is obtained; by judging whether the exploration node corresponds to the target position; if the exploration node corresponds to the target position, the route planning is terminated; if the exploration node does not correspond to the target position, it is judged whether the route length from the current position of the drone to the target position corresponding to the exploration node is greater than the expected route length; if so, the route planning is terminated; if not, it returns to the step of obtaining the relevant parameters of the drone. By continuously judging the relationship between the exploration node and the target position, when the exploration node is the target position, it can fly to the target position according to the previously obtained optimal route, and can also reach the target position in the case of encountering obstacles; since the minimum cost value is obtained according to the exploration distance and exploration depth, when the exploration distance is appropriate, the indecision caused by the close distance between the drone and the obstacle is also solved. At this time, the optimal planning cost can be obtained while the optimal route can be obtained.

[0102] Based on the above embodiment, as a more preferred embodiment, determining the cost value corresponding to each exploration node in the exploration node set according to the exploration distance and the exploration depth includes:

[0103] The exploration step length is determined based on the exploration distance and exploration depth.

[0104] Generate an exploration node set according to the exploration step length. The exploration node set is denoted as Q next ,The exploration node set contains multiple exploration nodes, and the exploration nodes correspond one to one to the candidate avoidance directions;

[0105] Determine the cost value corresponding to the exploration node.

[0106] Let the exploration step length be d s , and record the exploration distance as d t , and record the exploration depth as n d The exploration step length is obtained by the following formula:

[0107]

[0108] By generating a set of exploration nodes and determining the cost value corresponding to each exploration node in the exploration node set, the indecisive phenomenon caused by the close distance between the drone and the obstacle is solved.

[0109] Based on the above embodiment, as a more preferred embodiment, after generating the exploration node set according to the exploration step length and before determining the cost value corresponding to the exploration node, the method further includes:

[0110] Prune the exploration nodes in the exploration node set.

[0111] When pruning, multiple constraints are required. The types of constraints and how to obtain the constraint relationships are as follows:

[0112] Maximum climb angle and maximum dive angle constraints: The maximum climb angle is γ min_climb , the maximum dive angle is γ min_dive , the dive angle is γ dive , the climbing angle is γ climb . Then the UAV starts from waypoint P n :(x n ,y n ,z n ) (can be understood as the current position) to waypoint P n+1 :(x n+1 ,y n+1 ,z n+1 ) (which can be understood as the target position) needs to satisfy the following angle relationship:

[0113]

[0114]

[0115] Maximum horizontal turning angle constraint: Between two adjacent route segments, the UAV’s horizontal turning ability is limited. The maximum horizontal turning angle of the UAV is ΔΨ max , let the horizontal turning angle of the drone between adjacent route segments be ΔΨ, then the three waypoints P between two adjacent route segments n :(x n ,y n ,z n ) (can be understood as the current position) to waypoint P n+1 :(x n+1 ,y n+1 ,z n+1 ) (can be understood as the target location) and then to waypoint P n+2 :(x n+2 ,y n+2 ,z n+2 ) (which can be understood as the middle position) needs to satisfy the following angle relationship:

[0116]

[0117] Flight altitude constraint: Due to the performance limitation of the drone, a certain altitude above the ground must be maintained. The minimum altitude above the ground is ΔZ.min , the maximum flight altitude of the drone is Z max , the terrain height during the flight of the drone is Z terrain , the rules need to satisfy the following relationship:

[0118] Z n -Z terrain ≥ΔZ min

[0119] Z n ≤Z max

[0120] The purpose of pruning is to prevent information that is no longer within the constraints from participating in subsequent operations, resulting in a waste of resources.

[0121] Based on the above embodiment, as a more preferred embodiment, determining the cost value corresponding to the exploration node includes:

[0122] The cost change factor weight is determined based on the explored node and the current position.

[0123] The cost change factor weights include the azimuth change cost factor weight, the pitch change cost factor weight, the height change cost factor, etc.

[0124] Construct an avoidance cost model based on the weights of cost change factors;

[0125] The cost value is determined based on the avoidance cost model.

[0126] The avoidance cost model is constructed according to the weight of the cost change factor as follows:

[0127] The avoidance cost model is recorded as g(i), then g(i) = g p +ω0·δ ψ +ω1·δ θ +ω2·δ h Among them, g p is the parent node, ω i is the factor weight, i=0, 1, 2; ω0 is the weight of the azimuth change cost factor, ω1 is the weight of the pitch change cost factor, ω2 is the weight of the height change cost factor, and the factor weights satisfy the relationship ω0+ω1+ω2=1. ψ is the cost factor of orientation change, δ θ is the pitch change cost factor, δ h is the height change cost factor. The formulas for the azimuth change cost factor, pitch change cost factor, and height change cost factor are as follows:

[0128]

[0129]

[0130]

[0131] Among them, ψ next To explore the node position, ψ now is the current position, θ next is the exploration node pitch, θ now is the pitch of the current position, h next is the exploration node height, h now is the current position height. In this embodiment, the weight of the azimuth change cost factor can be set to 0.6, the weight of the pitch change cost factor can be set to 0.3, and the weight of the height change cost factor can be set to 0.1.

[0132] By constructing an avoidance cost model based on the weights of the cost change factors, the optimal cost value is obtained, while solving the indecisiveness caused by the close distance between the drone and the obstacle.

[0133] Based on the above embodiment, as a more preferred embodiment, after determining the cost value corresponding to each exploration node in the exploration node set according to the exploration distance and the exploration depth, before traversing and obtaining the exploration node corresponding to the minimum cost value, the method further includes:

[0134] Construct an estimated cost model based on the target exploration node and exploration node corresponding to the target location;

[0135] Determine the heuristic function corresponding to each exploration node based on the avoidance cost model and the estimated cost model. Assign a cost value to each exploration node, and then obtain the heuristic function f(i) of each exploration node, f(i) = g(i) + h(i). Among them, h(i) is the estimated cost model, which represents the expected cost of the drone flying from the exploration node to the target location. The estimated cost model formula is as follows:

[0136]

[0137] Among them, ψ t is the relative position of the target location to the exploration node, ψ next To explore the node location.

[0138] By constructing an estimated cost model through the target exploration nodes and exploration nodes corresponding to the target position, the optimal cost value is obtained, while solving the indecisive phenomenon caused by the close distance between the drone and the obstacle.

[0139] Figure 2 This is another flow chart of a method for planning a route for a drone provided in an embodiment of the present application. Based on the above embodiment, as a more preferred embodiment, Figure 2As shown, before determining the cost value corresponding to each exploration node in the exploration node set according to the exploration distance and the exploration depth, after determining the target position and the expected route length according to the current position, it also includes:

[0140] S20: constructing a threat vector field histogram according to the current position and threat information;

[0141] S21: Determine candidate avoidance directions based on the threat vector field histogram and the flight safety distance.

[0142] The candidate avoidance direction is denoted as {(Ψ,θ) i}, where Ψ is the heading angle, θ is the pitch angle, and i represents the i-th candidate avoidance direction.

[0143] The specific steps of constructing the threat vector field histogram based on the current position and threat information are as follows:

[0144] Determine the activity window radius of the drone based on the flight radius of the drone and the distance between the drone and the current position;

[0145] Divide the area formed by the radius of the active window into a plurality of sector-shaped areas according to a preset resolution;

[0146] The threat vector field histogram is constructed based on the distance between the UAV and the obstacle in each sector area.

[0147] Determine the local environment range of the platform threat. Generally, there is a circular active window that moves with the movement of the drone. The grid information in the circular active window is the local environment information that the drone needs to consider when avoiding. Considering that the target position is inside the active window, it will lead to over-considering the impact of threat information and repeatedly moving near the target position. Divide the active window sector area into several sectors according to a fixed resolution α, and construct a drone threat vector field histogram in each sector. Figure 3 The threat vector field histogram provided in the embodiment of the present application is as follows: Figure 3 As shown. The radius of the active window is denoted as R VFH* , R is obtained by the following formula VFH* :

[0148] R VFH* =min(R safe ,D LOS )

[0149] Among them, R safe is the safe flight radius of the drone, D LOS is the horizontal distance between the UAV and the target position.

[0150] The threat impact on each sector is recorded as H(n), n = 0, 1, ..., And initialize H(n) to H(n) = R VFH* , which means there is no threat in the sector. Traverse all threats in the active window and update H(n) of the corresponding sector. If there is a threat in the grid, calculate the sector where the grid is located, as follows:

[0151]

[0152] Among them, δ ψcell is the position of the grid relative to the longitudinal axis of the drone. Then the impact value of the obstacle in the sector where the grid is located is updated, and the impact value is calculated according to the following formula:

[0153] H(n)=min{H(n),D cell},

[0154] in,

[0155] Among them, r cell is the grid side length, D cell is the distance between the grid and the drone. After going through all the grids in the active window, H(n) of all sectors can be obtained. Since the lidar measurement result is a spatial threat map, the active window is actually a 3D sector, and the threat impact of the corresponding sector should be H(m,n).

[0156] where m = 0, 1, ...,

[0157] It solves the indecisiveness caused by the UAV being too close to obstacles, and achieves the optimal planning cost while also obtaining the optimal route.

[0158] In the above embodiments, the UAV route planning method is described in detail, and the present application also provides an embodiment corresponding to the UAV route planning device. It should be noted that the present application describes the embodiments of the device part from two perspectives, one is based on the functional module perspective, and the other is based on the hardware perspective.

[0159] Figure 4 This is a structural diagram of a UAV route planning device provided in an embodiment of the present application. Figure 4 As shown, the present application also provides a UAV route planning device, comprising:

[0160] An acquisition module 40 is used to acquire relevant parameters of the drone, wherein the relevant parameters include at least one of the current position of the drone, threat information, flight safety distance, exploration distance, exploration depth, and route length;

[0161] A first determination module 41 is used to determine a target position and an expected route length according to a current position;

[0162] A second determination module 42, configured to determine a cost value corresponding to each exploration node in the exploration node set according to the exploration distance and the exploration depth;

[0163] A traversal module 43, used for traversing and obtaining the exploration node corresponding to the minimum cost value;

[0164] A first determination module 44 is used to determine whether the exploration node corresponds to the target location;

[0165] If the explored node corresponds to the target location, the route planning ends;

[0166] The second judgment module 45 is used to judge whether the route length from the current position of the UAV to the target position corresponding to the exploration node is greater than the expected route length if the exploration node does not correspond to the target position;

[0167] If yes, the route planning ends;

[0168] If not, return to the step of obtaining relevant parameters of the drone.

[0169] By obtaining the relevant parameters of the drone; determining the target position and the expected route length according to the current position, the optimal route of the drone can be obtained; determining the cost value corresponding to each exploration node in the exploration node set according to the exploration distance and exploration depth; traversing and obtaining the steps of the exploration node corresponding to the minimum cost value to obtain the optimal cost value; by judging whether the exploration node corresponds to the target position; if the exploration node corresponds to the target position, the route planning is terminated; if the exploration node does not correspond to the target position, it is judged whether the route length of the drone from the current position to the target position corresponding to the exploration node is greater than the expected route length; if so, the route planning is terminated; if not, it returns to the step of obtaining the relevant parameters of the drone. By continuously judging the relationship between the exploration node and the target position, when the exploration node is the target position, it can fly to the target position according to the previously obtained optimal route, and can also reach the target position in the case of encountering obstacles; since the minimum cost value is obtained according to the exploration distance and exploration depth, when the exploration distance is appropriate, the indecisive phenomenon caused by the close distance between the drone and the obstacle is also solved. At this time, it is achieved that the optimal route can be obtained while the optimal planning cost is obtained. The present application also provides a drone route planning device, equipment and medium, with the same effect as above.

[0170] In addition, the UAV route planning device also includes the following modules:

[0171] A third determination module is used to determine the exploration step length according to the exploration distance and the exploration depth;

[0172] A generation module, used for generating an exploration node set according to the exploration step length, wherein the exploration node set includes multiple exploration nodes, and the exploration nodes correspond to the candidate avoidance directions one by one;

[0173] The fourth determination module is used to determine the cost value corresponding to the exploration node.

[0174] A first building module is used to build a threat vector field histogram according to the current position and threat information;

[0175] The fifth determination module is used to determine a candidate avoidance direction according to the threat vector field histogram and the flight safety distance.

[0176] The pruning processing module is used to prune the exploration nodes in the exploration node set.

[0177] A sixth determination module, configured to determine a cost change factor weight according to the exploration node and the current position;

[0178] The second building module is used to build an avoidance cost model according to the weight of the cost change factor;

[0179] The seventh determination module is used to determine the cost value according to the avoidance cost model.

[0180] The third construction module is used to construct an estimated cost model according to the target exploration node and the exploration node corresponding to the target position;

[0181] The eighth determination module is used to determine the heuristic function corresponding to each exploration node according to the avoidance cost model and the estimated cost model.

[0182] a ninth determination module, configured to determine an activity window radius of the drone according to a flight radius of the drone and a distance between the drone and a current position;

[0183] A division module, used for dividing the area formed by the radius of the active window into a plurality of sector-shaped areas according to a preset resolution;

[0184] The fourth building module is used to build a threat vector field histogram according to the distance between the UAV and the obstacle in each sector area.

[0185] Since the embodiments of the apparatus part correspond to the embodiments of the method part, please refer to the description of the embodiments of the method part for the embodiments of the apparatus part, which will not be repeated here.

[0186] Figure 5 A structure diagram of a UAV route planning device provided in an embodiment of the present application, such as Figure 5 As shown, the UAV route planning equipment includes:

[0187] A memory 50, for storing computer programs;

[0188] The processor 51 is used to implement the steps of the UAV route planning method mentioned in the above embodiment when executing the computer program.

[0189] The drone route planning device provided in this embodiment may include but is not limited to a smart phone, a tablet computer, a laptop computer, or a desktop computer.

[0190] Among them, the processor 51 may include one or more processing cores, such as a 5-core processor, an 8-core processor, etc. The processor 51 can be implemented in at least one hardware form of digital signal processing (DSP), field-programmable gate array (FPGA), and programmable logic array (PLA). The processor 51 may also include a main processor and a coprocessor. The main processor is a processor for processing data in the awake state, also known as a central processing unit (CPU); the coprocessor is a low-power processor for processing data in the standby state. In some embodiments, the processor 51 may be integrated with a graphics processing unit (GPU), which is responsible for rendering and drawing the content to be displayed on the display screen. In some embodiments, the processor 51 may also include an artificial intelligence (AI) processor, which is used to process computing operations related to machine learning.

[0191] The memory 50 may include one or more computer-readable storage media, which may be non-transitory. The memory 50 may also include a high-speed random access memory, and a non-volatile memory, such as one or more disk storage devices, flash memory storage devices. In this embodiment, the memory 50 is at least used to store the following computer program, wherein, after the computer program is loaded and executed by the processor 51, it can implement the relevant steps of the UAV route planning method disclosed in any of the aforementioned embodiments. In addition, the resources stored in the memory 50 may also include an operating system and data, etc., and the storage method may be short-term storage or permanent storage. Among them, the operating system may include Windows, Unix, Linux, etc. The data may include but is not limited to the UAV route planning method, etc.

[0192] In some embodiments, the UAV route planning device may also include a display screen, an input and output interface, a communication interface, a power supply, and a communication bus.

[0193] Those skilled in the art will understand that Figure 5 The structure shown in does not constitute a limitation of the UAV route planning device and may include more or fewer components than shown in the figure.

[0194] The UAV route planning device provided in the embodiment of the present application includes a memory 50 and a processor 51. When the processor 51 executes the program stored in the memory 50, the UAV route planning method can be implemented.

[0195] Finally, the present application also provides an embodiment corresponding to a computer-readable storage medium. The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps recorded in the above method embodiment are implemented.

[0196] It is understandable that if the method in the above embodiment is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium to execute all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (Read-Only Memory), ROM, random access memory (RandomAccess Memory, RAM), disk or optical disk and other media that can store program code.

[0197] The above is a detailed introduction to a UAV route planning method, device, equipment and medium provided by the present application. The various embodiments in the specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same and similar parts between the embodiments can refer to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can refer to the method part description. It should be pointed out that for ordinary technicians in this technical field, without departing from the principles of this application, several improvements and modifications can be made to the present application, and these improvements and modifications also fall within the scope of protection of the claims of this application.

[0198] It should also be noted that, in this specification, 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 "comprise", "include" 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, an element defined by the statement "comprises a ..." does not exclude the presence of other identical elements in the process, method, article or device including the element.

Claims

1. A method for planning a route for a UAV, wherein when it is found that the flight route is blocked by a sudden dynamic obstacle during the flight of the UAV, the optimal path is selected to replace the current part of the flight route after the UAV expands in the flyable direction according to the detection data and terrain data until the target position is reached, and the method is characterized in that: include: Acquire relevant parameters of the drone, wherein the relevant parameters include at least one of the current position, threat information, flight safety distance, exploration distance, exploration depth, and route length of the drone; Determine a target location and an expected route length based on the current location; Determine a cost value corresponding to each exploration node in the exploration node set according to the exploration distance and the exploration depth; Traversing and obtaining the exploration node corresponding to the minimum cost value; Determining whether the exploration node corresponds to the target location; If the exploration node corresponds to the target location, then the route planning is terminated; If the exploration node does not correspond to the target location, determining whether the route length of the drone from the current location to the target location corresponding to the exploration node is greater than the expected route length; If yes, then route planning ends; If not, return to the step of obtaining the relevant parameters of the drone; Determining the cost value corresponding to each exploration node in the exploration node set according to the exploration distance and the exploration depth includes: Determine the exploration step length according to the exploration distance and the exploration depth; Generate the exploration node set according to the exploration step length, the exploration node set includes a plurality of the exploration nodes, and the exploration nodes correspond to candidate avoidance directions one by one; Determining the cost value corresponding to the exploration node; Before determining the cost value corresponding to each exploration node in the exploration node set according to the exploration distance and the exploration depth, and after determining the target position and the expected route length according to the current position, the method further includes: constructing a threat vector field histogram according to the current position and the threat information; determining the candidate avoidance direction according to the threat vector field histogram and the flight safety distance; Determining the cost value corresponding to the exploration node includes: Determining a cost change factor weight according to the exploration node and the current position; Constructing an avoidance cost model according to the weights of the cost change factors; Determining the cost value according to the avoidance cost model; Avoidance Cost Model The formula is as follows: ; in, is the parent node, is the weight of the cost factor of the orientation change, is the weight of the pitch change cost factor, is the height variation cost factor weight, ; is the cost factor of orientation change, is the pitch change cost factor, is the height change cost factor; the formulas for the azimuth change cost factor, pitch change cost factor, and height change cost factor are as follows: ; ; ; in, To explore the node location, is the current location, To explore the node pitch, is the current position pitch, To explore the node height, is the current position height; After determining the cost value corresponding to each exploration node in the exploration node set according to the exploration distance and the exploration depth, and before traversing and obtaining the exploration node corresponding to the minimum cost value, the method further includes: Constructing an estimated cost model according to the target exploration node corresponding to the target position and the exploration node; Determine the heuristic function corresponding to each of the exploration nodes according to the avoidance cost model and the estimated cost model; Heuristic function The formula is as follows: ; in, is the estimated cost model, which represents the expected cost of the drone flying from the exploration node to the target location; the estimated cost model formula is as follows: ; in, is the relative position of the target location to the exploration node, To explore the node location; The constructing a threat vector field histogram according to the current position and the threat information includes: Determining an activity window radius of the drone according to a flight radius of the drone and a distance between the drone and the current position; Dividing the area formed by the radius of the active window into a plurality of sector-shaped areas according to a preset resolution; The threat vector field histogram is constructed according to the distance between the UAV and the obstacle in each of the fan-shaped areas.

2. The UAV route planning method according to claim 1, characterized in that: After generating the exploration node set according to the exploration step length and before determining the cost value corresponding to the exploration node, the method further includes: Pruning is performed on the exploration nodes in the exploration node set.

3. A UAV route planning device, when it is found that the flight route is blocked by a sudden dynamic obstacle during the flight of the UAV, according to the detection data and terrain data, it expands in the flyable direction until it reaches the target position and then selects the optimal path to replace the current part of the flight route, characterized in that: include: An acquisition module, used to acquire relevant parameters of the drone, wherein the relevant parameters include at least one of the current position, threat information, flight safety distance, exploration distance, exploration depth, and route length of the drone; A first determination module, used to determine a target position and an expected route length according to the current position; A second determination module, configured to determine a cost value corresponding to each exploration node in the exploration node set according to the exploration distance and the exploration depth; A traversal module, used for traversing and obtaining the exploration node corresponding to the minimum cost value; A first judgment module, used to judge whether the exploration node corresponds to the target location; If the exploration node corresponds to the target location, then the route planning is terminated; A second judgment module is used to judge whether the route length of the UAV from the current position to the target position corresponding to the exploration node is greater than the expected route length if the exploration node does not correspond to the target position; If yes, then route planning ends; If not, return to the step of obtaining the relevant parameters of the drone; The second determining module comprises: A third determination module, configured to determine an exploration step length according to the exploration distance and the exploration depth; A generating module, configured to generate the exploration node set according to the exploration step length, wherein the exploration node set includes a plurality of the exploration nodes, and the exploration nodes correspond to candidate avoidance directions one by one; A fourth determination module, used to determine the cost value corresponding to the exploration node; Also includes: A first construction module, configured to construct a threat vector field histogram according to the current position and the threat information; a fifth determination module, configured to determine the candidate avoidance direction according to the threat vector field histogram and the flight safety distance; The fourth determination module comprises: a sixth determination module, configured to determine a cost change factor weight according to the exploration node and the current position; A second construction module is used to construct an avoidance cost model according to the cost change factor weight; A seventh determination module, configured to determine the cost value according to the avoidance cost model; Avoidance Cost Model The formula is as follows: ; in, is the parent node, is the weight of the cost factor of the orientation change, is the weight of the pitch change cost factor, is the height variation cost factor weight, ; is the cost factor of orientation change, is the pitch change cost factor, is the height change cost factor; the formulas for the azimuth change cost factor, pitch change cost factor, and height change cost factor are as follows: ; ; ; in, To explore the node location, is the current location, To explore the node pitch, is the current position pitch, To explore the node height, is the current position height; Also includes: A third construction module is used to construct an estimated cost model according to the target exploration node corresponding to the target position and the exploration node; an eighth determination module, configured to determine a heuristic function corresponding to each of the exploration nodes according to the avoidance cost model and the estimated cost model; Heuristic function The formula is as follows: ; in, is the estimated cost model, which represents the expected cost of the drone flying from the exploration node to the target location; the estimated cost model formula is as follows: ; in, is the relative position of the target location to the exploration node, To explore the node location; The first building block comprises: a ninth determination module, configured to determine an activity window radius of the drone according to a flight radius of the drone and a distance between the drone and the current position; A division module, used for dividing the area formed by the radius of the active window into a plurality of sector-shaped areas according to a preset resolution; The fourth construction module is used to construct the threat vector field histogram according to the distance between the UAV and the obstacle in each of the fan-shaped areas.

4. A UAV route planning device, characterized in that: include: Memory for storing computer programs; A processor is used to implement the steps of the UAV route planning method as described in claim 1 or 2 when executing the computer program.

5. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by the processor, the steps of the UAV route planning method as described in claim 1 or 2 are implemented.