Threat area multi-unmanned aerial vehicle path planning method and device based on MUBCR-RRT algorithm

Abstract

The invention provides a threat area multi-unmanned aerial vehicle path planning method and device based on an MUBCR-RRT algorithm, and the method comprises the following steps: dividing a flight environment into grid units, and constructing a threat environment model; calculating a threat value of each grid unit according to the threat environment model; generating path points of the multiple unmanned aerial vehicles according to the threat values of the grid units; and performing trajectory optimization on the path points of the multiple unmanned aerial vehicles by adopting Dubins. A threat area and a building area are effectively distinguished, and the space in which the unmanned aerial vehicle can fly is increased. The unnecessary path points are reduced, and the path quality is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle (UAV) path planning, and in particular to a method and device for planning multiple UAV paths in a threat area based on a MUBCR-RRT algorithm. Background Art

[0002] Due to their advantages in flexibility, cost-effectiveness and casualties, drones play an important role in commerce, agriculture, military and other fields. Compared with a single drone, a drone swarm has the characteristics of high efficiency, strong robustness and good scalability, and can perform complex tasks that a single drone cannot complete, including reconnaissance, surveillance, and strike. However, the flight missions of drone swarms are becoming increasingly complex, and finding a set of optimal flight trajectories that meet flight constraints is still a great challenge.

[0003] In military wars, cities have always been the focus of contention between the two sides, and urban warfare is an important factor affecting the outcome of both sides. Urban environments are often more complex, including tall and dense buildings, and there are often enemy fire points at the edges of buildings. These fire points will generate larger threat areas in urban environments. This has a great impact on the completion of drone penetration missions. In a complex urban environment considering threat areas, planning a set of feasible and conflict-free flight paths is crucial to completing the penetration mission. At present, in the face of urban environments that consider threat areas, urban building areas are confused with threat areas, and both are directly constructed as inaccessible areas. It is difficult to distinguish between areas that are still passable despite threats and areas that are completely prohibited from passing. Planning drone swarm paths in a complex urban environment considering threat areas is still a relatively difficult problem.

[0004] In summary, the existing technology has the following problems: the path planning of the drone swarm cannot distinguish the threat force of the flight area, and its path planning is imperfect. Summary of the invention

[0005] The purpose of the present invention is to solve the problem of how to distinguish the threat force of the flight path of a group of drones to optimize their path planning.

[0006] To this end, on the one hand, an embodiment of the present invention provides a method for multi-UAV path planning in a threat area based on the MUBCR-RRT algorithm, the method comprising the following steps:

[0007] Divide the flight environment into grid units and construct a threat environment model;

[0008] Calculating a threat value for each grid cell according to the threat environment model;

[0009] According to the threat value of the threat unit, the environment space is divided into safe passable space, threatening passable space and impassable space;

[0010] Generate multi-drone flight path points in safe navigable spaces and threatening navigable spaces;

[0011] Dubins is used to optimize the trajectory of multiple UAV flight path points.

[0012] On the other hand, an embodiment of the present invention further provides a multi-UAV path planning device in a threat area based on the MUBCR-RRT algorithm, comprising:

[0013] A construction unit is used to divide the flight environment into grid units and construct a threat environment model;

[0014] A calculation unit, used for calculating the threat value of each grid unit according to the threat environment model;

[0015] Divide the units, and divide the environment space into safe passable space, threatening passable space, and impassable space according to the threat value of the threat unit;

[0016] A planning unit for generating multi-UAV flight path points in safe navigable spaces and navigable spaces with threats;

[0017] The optimization unit is used to perform trajectory optimization of multiple UAV flight path points using Dubins.

[0018] The above technical solution has the following beneficial effects: the present invention can effectively distinguish between threat areas and building areas by dividing the entire environmental space into grid units. This distinction provides an important basis for the subsequent multi-UAV flight path planning. In order to meet the challenges of multi-UAV trajectory planning, the present invention proposes a safety detection mechanism, expands the application scope of the Rapidly-exploring Random Tree star algorithm to the field of multi-UAV trajectory planning, and proposes node generation strategies, candidate node evaluation strategies, and path reconnection mechanisms to improve the RRT* algorithm. These measures are intended to reduce planning time and improve path quality, thereby optimizing the trajectory planning performance of the multi-UAV system.

[0019] The present invention adopts the Dubins algorithm to smooth the trajectory, ensuring that the generated path not only meets the dynamic characteristics of the UAV, but also can achieve smooth and continuous flight. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 is a flow chart of a method for multi-UAV path planning in a threat area based on the MUBCR-RRT algorithm provided by an embodiment of the present invention;

[0021] Figure 2It is a structural schematic diagram of a multi-UAV path planning device in a threat area based on the MUBCR-RRT algorithm provided by an embodiment of the present invention;

[0022] Figure 3 This is a flow chart of the first implementation method of the method for multi-UAV path planning in a threat area based on the MUBCR-RRT algorithm provided in an embodiment of the present invention. DETAILED DESCRIPTION

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

[0024] In urban warfare, drones have become the representative of new domains and new types of combat forces. How to perform multi-drone path planning in an urban environment taking into account threat areas is an important issue. The key elements and constraints of this issue are as follows:

[0025] Environmental problem: An environmental modeling method is needed to transform it from a real environment into a problem space where drones can perform path planning. The urban environment that takes into account the threat area requires a new environmental modeling method to distinguish the threat area from the building area. This invention proposes a threat environment grid method to solve this problem.

[0026] Number of drones: In multi-drone path planning, multiple drones take off from different starting points and fly to different destinations. In this process, it is necessary to consider not only avoiding collisions between drones, but also the obstruction of buildings, threats from fire points, etc.

[0027] In the embodiment of the present invention, Figure 1 , a multi-UAV path planning method in a threat area based on the MUBCR-RRT algorithm is provided, the method comprising the following steps:

[0028] S101: Divide the flight environment into grid units and construct a threat environment model;

[0029] The present invention proposes a threat environment grid method, which can effectively distinguish between threat areas and building areas, and increase the space in which drones can fly.

[0030] Drone path planning first requires modeling of the environment. Through environmental modeling, the actual environment is converted into a mathematical representation that can be processed by a computer, providing an operating space for the path planning algorithm. The environmental grid method is to rasterize the selected area and evenly divide the drone flight area into a polygonal area. The present invention proposes a threat grid method, which first divides the urban environment of the target area into equal-sized cubic units. The space within these grid cells is abstracted as a point, and the position of each unit is located by the index of the three-dimensional matrix. Compared with the environmental grid method, the threat grid method proposed in the present invention proposes an innovative method to represent the threat level of each grid cell, and quantifies the threat level of each grid cell by the element value in the three-dimensional matrix, and the size of the value is proportional to the degree of threat.

[0031] The threat matrix is ​​shown in formula (1):

[0032]

[0033] t(x,y,z) represents the threat level of the grid cell (x,y,z). When the value of t(x,y,z) is equal to 1, it means that the grid is impassable. In the present invention, the range of the environmental space is determined by the maximum value x on the three coordinate axes. max ,y max ,z max Definition, representing the limit boundaries of space in the x-axis, y-axis and z-axis directions. Any path of the drone must be planned within the specified range.

[0034] There are two main sources of threats, as shown in Table 1. One is buildings, and the other is fire points arranged in the city. Drones cannot pass through building areas, but can pass through areas covered by fire points. In areas covered by fire points, drones are still threatened. Considering these two types of threat sources, for the grid unit P(x, y, z), its threat level is defined as follows:

[0035]

[0036] in, represents the threat value of building i to grid (x, y, z), and Nb represents the number of buildings. It represents the threat value of firepower point i to (x, y, z), and Nf represents the number of firepower points.

[0037] Table 1 Threat types

[0038]

[0039] For buildings, as long as a grid cell contains a building, the threat level of the grid cell is set to 1. Considering that the drone needs to maintain a certain safety distance from obstacles during flight, the safety distance for the obstacle is set to 1. If a grid contains an obstacle, in order to ensure the safety of the drone, the threat levels of the eight adjacent grids around it, namely, top, bottom, left, right, front, back, upper left, and lower left, are also set to 1.

[0040] In the threat grid method, the firing coverage of the firing point is simulated as a cone-like shape. The cone area is the firing coverage area. The closer the grid is to the firing point, the greater the degree of threat. The firing point has the following attributes as shown in Table 2.

[0041] Table 2 Fire Point Attributes

[0042]

[0043]

[0044] For a grid point P (x, y, z), the firepower threat it receives is as follows:

[0045]

[0046] Where P f Indicates the grid unit where the firing point is located, v f Indicates the direction angle of the shooting. If the grid (x, y, z) is within the firing range angle and the firing distance of the firing point, then it is within the coverage of the firing point. The closer the grid point is to the firing point, the greater the threat value; the farther it is from the firing point, the smaller the threat value.

[0047] In addition, when analyzing the coverage of the firepower point, the present invention also takes into account the shielding of buildings. Specifically, even if a grid is within the shooting range, if it is shielded by a building, the firepower point cannot cover this grid point.

[0048] S102: Calculating the threat value of each grid unit according to the threat environment model;

[0049] The threat value of a grid cell is greater than or equal to 0 and less than or equal to 1. When the threat value of a grid cell is 1, it is not passable. The greater the threat value of a grid cell, the higher the probability that the flight will be attacked by fire.

[0050] S103: Dividing the environment space into a safe passable space, a threatening passable space, and an impassable space according to the threat value of the threat unit;

[0051] S104: Generate multi-drone flight path points in safe traversable space and threatening traversable space;

[0052] First, initialize the vertex set, edge set, path set and flag. When initializing the vertex set, put the initial nodes of all drones into the vertex set as different sets. Next, enter the loop to perform multi-drone path planning. If all drones reach the end point, exit the loop. If the drone has reached the target point, jump into the path search for the next drone. The node_generation function is a new node generation strategy proposed by the present invention. The node_generation function can only generate a new node and the x_nearest corresponding to the new node. The find_near function searches for all points within a certain range in the extension tree of the drone. The rewire function is responsible for checking the nodes within a certain range around the new node, and evaluating whether the path can be optimized by using it as the parent node of these nodes.

[0053] After that, the probability_testing function proposed in the present invention is used to detect whether the drone can connect to the destination. If so, the flight path of the drone is generated. Next, after finding all the drone paths, the path points are reconnected through the path_reconnection function to reduce unnecessary path points and improve the path quality.

[0054] S105: Use Dubins to optimize the trajectory of multiple UAV flight path points.

[0055] Threat environment model, specifically:

[0056]

[0057] Among them, t(x,y,z) represents the threat level of the grid cell (x,y,z), x max Represents the limit boundary value of the grid cell space in the x-axis direction, y max Represents the limit boundary value of the grid cell space in the y-axis direction, z max Represents the limit boundary value of the grid unit space in the z-axis direction.

[0058] In the threat environment model:

[0059]

[0060] in, represents the threat value of building i to grid unit (x, y, z), Nb represents the number of buildings, represents the threat value of firepower point i to (x, y, z), and Nf represents the number of firepower points;

[0061] in,

[0062]

[0063] Where P is a grid point, P f Indicates the location of the firing point; v f Indicates the direction angle of the shooting;

[0064] θ represents the angle between the grid unit and the firing point, that is, v f and The angle between , in degrees;

[0065] dis is distance.

[0066] Generate path points for multiple drones based on the threat value of the grid cells, including:

[0067] The MUBCR-RRT* algorithm is used to add the starting point of the drone to the corresponding expansion tree;

[0068] In the process of executing multi-UAV trajectory planning, in order to avoid intersection collision of UAV routes, it is necessary to ensure that the flight trajectories of different UAVs cannot occupy the same grid cells. Specifically, in the process of building a UAV expansion tree, every time a new edge is generated, it is necessary to determine whether this new edge occupies the same grid as any existing edge of the remaining UAV expansion tree. If they occupy the same grid, this new edge cannot be added to the expansion tree.

[0069] Find the flight node in the expansion tree until all expansion trees reach the end;

[0070] Connecting the starting point of the drone, the flight node, and the end point of the drone to obtain a flight path;

[0071] The flight path is optimized using the Dubins curve.

[0072] The present invention also provides a multi-UAV path planning device in a threat area based on the MUBCR-RRT algorithm, such as Figure 2 As shown, including:

[0073] A construction unit 21, used to divide the flight environment into grid units and construct a threat environment model;

[0074] A calculation unit 22, configured to calculate a threat value of each grid unit according to the threat environment model;

[0075] A division unit 23 divides the environment space into a safe passable space, a threatening passable space, and an impassable space according to the threat value of the threat unit;

[0076] A planning unit 24, for generating multi-UAV flight path points in a safe traversable space and a threatening traversable space;

[0077] The optimization unit 25 is used to optimize the trajectory of multiple UAV flight path points using Dubins.

[0078] Threat environment model, specifically:

[0079]

[0080] Among them, t(x,y,z) represents the threat level of the grid cell (x,y,z), x max Represents the limit boundary value of the grid cell space in the x-axis direction, y max Represents the limit boundary value of the grid cell space in the y-axis direction, z max Represents the limit boundary value of the grid unit space in the z-axis direction.

[0081] In the threat environment model:

[0082]

[0083] in, represents the threat value of building i to grid unit (x, y, z), Nb represents the number of buildings, represents the threat value of firepower point i to (x, y, z), and Nf represents the number of firepower points;

[0084] in,

[0085]

[0086] Where P is a grid point, P f Indicates the location of the firing point; v f Indicates the direction angle of the shooting;

[0087] θ represents the angle between the grid unit and the firing point, that is, v f and The angle between , in degrees;

[0088] dis is distance.

[0089] The threat value of the grid cell is greater than or equal to 0 and less than or equal to 1.

[0090] The planning unit specifically includes:

[0091] Used to add the starting point of the drone to the corresponding expansion tree using the MUBCR-RRT* algorithm;

[0092] Used to find flight nodes in the expansion tree until all expansion trees reach the end point;

[0093] Used to connect the starting point of the drone, the flight node and the end point of the drone to obtain a flight path;

[0094] Used to optimize the flight path using the Dubins curve.

[0095] The advantage of the present invention is that the present invention can effectively distinguish between threat areas and building areas by dividing the entire environmental space into grid units. This distinction provides an important basis for subsequent multi-UAV flight path planning. In order to meet the challenges of multi-UAV trajectory planning, the present invention proposes a safety detection mechanism, expands the application scope of the Rapidly-exploring Random Tree star algorithm to the field of multi-UAV trajectory planning, and proposes node generation strategies, candidate node evaluation strategies, and path reconnection mechanisms to improve the RRT* algorithm. These measures are intended to reduce planning time and improve path quality, thereby optimizing the trajectory planning performance of the multi-UAV system.

[0096] The present invention adopts the Dubins algorithm to smooth the trajectory, ensuring that the generated path not only meets the dynamic characteristics of the UAV, but also can achieve smooth and continuous flight.

[0097] The above technical solution of the embodiment of the present invention is described in detail below in conjunction with specific application examples. For technical details not introduced during the implementation process, please refer to the relevant description in the previous text.

[0098] Embodiment 1:

[0099] The present invention provides a multi-UAV path planning method in a threat area based on the MUBCR-RRT algorithm. In order to effectively realize the flight path planning of multiple UAVs in a crowded urban environment with a threat area, the present invention first proposes a threat environment grid method to grid the urban environment with a threat area. The threat value of each grid represents the threat size of the UAV passing through this grid. This method can distinguish between the threat area and the prohibited area. Next, based on this environment modeling, the present invention proposes a Multi-Unmanned Aerial Vehicle Goal-Biased CandidateEvaluation Strategy with Reconnection Rapidly-exploring Random Tree star (MUBCR-RRT*) algorithm, which comprehensively considers the collision avoidance strategy of multiple UAVs, the flight motion characteristics of UAVs, and the simultaneous planning of the flight paths of multiple UAVs.

[0100] Multiple drones successfully cross from the starting position to the target position to perform tasks in an urban environment with a threat area. There are multiple buildings and multiple enemy firing points distributed in the environment. The drone cannot fly through the buildings, but can pass through the coverage range of the firing points and the areas without threats. However, there is a risk of being shot down when passing through the coverage range of the firing points. The closer to the firing point, the higher the risk of being shot down and the higher the threat level. The present invention proposes an environmental threat model for urban environments with threat areas, which can distinguish different threat levels of urban environments.

[0101] Environmental Threat Generation Model:

[0102] The first step in drone path planning is to model the environment. Through environmental modeling, the actual environment is converted into a mathematical representation that can be processed by a computer, providing an operating space for the path planning algorithm. The environmental grid method is to rasterize the selected area and evenly divide the drone flight area into a polygonal area. The present invention proposes an innovative threat grid method, which first divides the urban environment of the target area into equal-sized cubic units.

[0103] The space within these grid cells is abstracted as a point, and the position of each cell is located by the index of the three-dimensional matrix. Compared with the environmental grid method, the threat grid method proposed in this invention proposes an innovative method to represent the threat level of each grid cell. The threat level of each grid cell is quantified by the element value in the three-dimensional matrix, and the value is proportional to the degree of threat. The threat matrix is ​​shown in formula (1):

[0104]

[0105] t(x,y,z) represents the threat level of the grid cell (x,y,z). When the value of t(x,y,z) is equal to 1, it means that the grid is impassable. In the present invention, the range of the environmental space is determined by the maximum value x on the three coordinate axes. max ,y max ,z max Definition, representing the limit boundaries of space in the x-axis, y-axis and z-axis directions. Any path of the drone must be planned within the specified range.

[0106] There are two main sources of threats, as shown in Table 1. One is buildings, and the other is fire points arranged in the city. Drones cannot pass through building areas, but can pass through areas covered by fire points. In areas covered by fire points, drones are still threatened. Considering these two types of threat sources, for the grid unit P(x, y, z), its threat level is defined as follows:

[0107]

[0108] in, represents the threat value of building i to grid (x, y, z), and Nb represents the number of buildings. It represents the threat value of firepower point i to (x, y, z), and Nf represents the number of firepower points.

[0109] For buildings, as long as a grid cell contains a building, the threat level of the grid cell is set to 1. Considering that the drone needs to maintain a certain safety distance from obstacles during flight, the safety distance for the obstacle is set to 1. If a grid contains an obstacle, in order to ensure the safety of the drone, the threat level of the eight adjacent grids (i.e., top, bottom, left, right, front, back, top left, and bottom left) is also set to 1.

[0110] In the threat grid method, the firing coverage of the firing point is simulated as a cone-like shape. The cone area is the firing coverage area. The closer the grid is to the firing point, the greater the degree of threat. The firing point has the following attributes as shown in Table 2.

[0111]

[0112] Where P f Indicates the grid unit where the firing point is located, v f Indicates the direction angle of the shooting. If the grid (x, y, z) is within the firing range angle and the firing distance of the firing point, then it is within the coverage of the firing point. The closer the grid point is to the firing point, the greater the threat value; the farther it is from the firing point, the smaller the threat value.

[0113] In addition, when analyzing the coverage of the firepower point, the present invention also takes into account the shielding of buildings. Specifically, even if a grid is within the shooting range, if it is shielded by a building, the firepower point cannot cover this grid point.

[0114] Path representation:

[0115] The parametric expression of UAV paths is of great significance for the study of UAV path planning. For a UAV, its path can be expressed as:

[0116] path={P0,P1,P2,…P k ,…,P n ,P n+1}(6)

[0117] Where P_0 represents the starting point coordinates, P_(n+1) represents the end point. P_k=(x_k, y_k, z_k) represents the intermediate path points that the drone passes during flight. These path points are discrete in the present invention.

[0118] The current path expression has certain shortcomings. For example, the path constructed by directly connecting the path points may result in an uneven flight trajectory, which may limit the performance of the drone. There are two ways to smooth the path. One method is to use the B-Spline curve to smooth the broken line connecting the path points. This method can make the rigid broken line smooth. However, this smooth curve method may not meet the restrictions of the drone's own azimuth and pitch angle, and may change the path the drone passes through, causing it to collide with obstacles. Another method is to use the Dubins algorithm to construct a three-dimensional path between two obstacle-free points. This method can not only meet the performance constraints of the drone itself, but also keep the flight trajectory of the drone as unchanged as possible.

[0119] The present invention adopts the second method to connect the obstacle-free points of the UAV flight trajectory. The original Dubins curve was proposed to find the shortest path connecting two points on a two-dimensional plane while satisfying the curvature constraint of the vehicle. This ensures that the vehicle maintains a stable state during driving and can smoothly cross the path between the two points. In three-dimensional space, in addition to being constrained by the azimuth angle, the UAV is also restricted by the pitch angle. There are currently many methods for generating three-dimensional Dubins paths, such as adding intermediate arcs to the path or using nonlinear methods to generate curves. This study adopts an accurate algorithm proposed by Hota et al., which can generate a path for the UAV in three-dimensional space that satisfies the curvature and angle constraints. Given the starting point P_start = (x_0, y_0, z_0) and the end point P_end = (x_1, y_1, z_1) and the minimum turn R of the UAV, only the azimuth angle and pitch angle of the starting point and the end point need to be provided to determine the optimal flight path between the two points.

[0120] The azimuth of the drone represents its steering ability in the horizontal plane, that is, the lateral deviation angle of the drone relative to the forward direction, represented by the symbol γ. In the assumption of the Dubins curve, the drone is restricted to flying only in the direction of its forward movement. Therefore, the range of the azimuth is Correspondingly, the pitch angle of the drone describes its attitude adjustment in the vertical plane, that is, the upward or downward tilt angle relative to the horizontal plane, represented by ψ. Similarly, the range of the pitch angle is also

[0121] There is a continuous path point P on a path k , P k+1 , the drone is at the path point P k The flight direction is calculated as follows:

[0122]

[0123] In the above formula, V k Indicates that the drone is in P k In particular, the UAV is at the end point P n+1 The flight direction of the path remains the same as the flight direction of the previous path point.

[0124] Assume V k =(v xk ,v yk ,v zk ), according to the three-dimensional dynamic model of the UAV, the vector relationship between the direction angle and the pitch angle is as follows:

[0125] v xk =||V k ||cosγcosψ(8)

[0126] v yk =||V k ||cosγsinψ(9)

[0127] v zk =||V k ||sinγ(10)

[0128] Therefore, the calculation formula for the direction angle and pitch angle can be obtained:

[0129]

[0130] Multi-UAV path planning model:

[0131] Multi-UAV 3D path planning refers to finding flight paths that meet the conditions for multiple UAVs in 3D space, taking into account multiple constraints and mission requirements. Multi-UAV path planning is a constrained optimization problem (COP). In general, the basic form of a constrained optimization problem is as follows:

[0132] minf(x)(13)

[0133]

[0134] where x=(x1,x2,…x D )εΩ is the decision variable, Ω is the decision space formed by the decision vector. f(x) is the objective function, which maps the decision space Ω to the objective space π. j is an inequality constraint, and h j is an equality constraint. i and L i is the decision variable x i The upper and lower bounds of . And q is the total number of constraints.

[0135] For the multi-UAV path planning problem in the threat area of ​​the urban environment, the goal is to minimize the length of the UAV flight path and give priority to grid points with lower threat levels. The length of the Dubins curve is mainly composed of two parts, the first part is the turning distance, and the second part is the straight line distance. In general, the turning radius of the rotary-wing UAV is very small, and the length of the first part is much smaller than the length of the second part. In this regard, the present invention uses Euclidean distance to replace the length of the Dubins curve, and the objective function can be expressed as:

[0136]

[0137]

[0138] Where i is an index from 1 to Nu, where Nu represents the number of drones. j is a discrete point index from 0 to N, representing the sequence number of the drone path point. ij is the path point P ij The threat value is defined as the distance from the drone to the grid point P ij Fly to P ij+1 The average threat value of a series of grids in the process. k represents a value from 1 to Np ij The index of Np ij Indicates that the i-th UAV starts from the path point P ij Fly to P ij+1 The number of grids passed in the process.

[0139] In addition to defining the objective function, it is also important to properly define the flight constraints of the drones. When drones are performing low-altitude flight missions, they are generally required to dive in from low altitudes, so an upper limit on the altitude is set, called the maximum altitude limit, which is expressed in z. max In addition, the height of the drone needs to be greater than the ground height. So the height constraint of the drone is as follows:

[0140] 0≤z ijk ≤z max (18)

[0141] Multi-UAV path planning generally searches for feasible paths in a specified area, so the coordinate range of the grid points that the UAV passes through satisfies the constraints:

[0142] 0≤x ijk ≤x max (19)

[0143] 0≤y ijk ≤y max (20)

[0144] The drone needs to avoid buildings during flight, that is, the threat value of the grid that the drone passes cannot be 1. For the i-th drone, its planned path has two path points P ij , P ij+1 . Drone from P ij Fly to P ij+1 During the process, it will pass through a series of grids. The anti-collision constraints between the drone and the building are as follows:

[0145]

[0146] Among them, Np ij Indicates that the i-th UAV starts from the path point P ij Fly to P ij+1 The grid index passed in the process, t(x ijk ,y ijk ,z ijk) represents the grid point (x ijk ,y ijk ,z ijk ) is the threat value. Nu represents the number of drones, N i Represents the number of path points on the trajectory of the i-th UAV.

[0147] The drone needs to meet its own performance constraints. This model mainly considers the azimuth and pitch angle of the drone during flight, which should not exceed the defined maximum range. Therefore, these two constraints are defined as follows:

[0148] g1=γ-γ max (twenty four)

[0149] g2=ψ-ψ max (25)

[0150] γ max represents the maximum direction angle, ψ max Indicates the maximum pitch angle. Finally, for UAV formation flight, in the path planning process, the collision avoidance problem between UAVs needs to be considered. The present invention designs the UAV collision constraint as follows:

[0151]

[0152] In the above formula, cum ij Indicates that the i-th UAV starts from the path point P ij Fly to P ij+1 In the process of detecting collisions with other drone paths.

[0153] Based on the above discussion, the multi-UAV path planning model can be described as follows:

[0154] minf=f(28)

[0155]

[0156] 0≤x ijk ≤x max (30)

[0157] 0≤y ijk ≤y max (31)

[0158] 0≤z ijk ≤z max (32)

[0159] Obviously, this is a constrained optimization problem. If the UAV trajectory can satisfy the constraints (29)-(32), a set of required paths can be generated.

[0160] Based on the RRT* algorithm, the present invention proposes a Multi-Unmanned Aerial Vehicle Goal-Biased Candidate Evaluation Strategy with Reconnection Rapidly-exploring Random Tree star (MUBCR-RRT*). The overall framework of MUBCR-RRT* is introduced below. The pseudo code of the algorithm is shown in Algorithm 1 in Table 3.

[0161] First, initialize the vertex set, edge set, path set and flag, see line 1-7 for details. When initializing the vertex set, put the initial nodes of all drones into the vertex set as different sets. Next, enter the loop, see line 10 for details, and perform multi-drone path planning. If all drones reach the end point, exit the loop, see lines 11-12 for details. If the drone has reached the target point, jump into the path search for the next drone, see lines 13-15 for details. The node_generation function is a new node generation strategy proposed by the present invention. For specific steps, see Algorithm 3. It should be noted that the node_generation function can only generate a new node and the x_nearest corresponding to the new node x_new, see line 16 for details. The find_near function searches for all points within a certain range of x_new in the extended tree of the drone, see line 18 for details. The rewire function is responsible for checking the nodes within a certain range around the new node x_new, and evaluating whether the path can be optimized by using x_new as the parent node of these nodes, see line 21 for details.

[0162] After that, the probability_testing function proposed in the present invention is used to detect whether the drone can connect to the destination. If it can, the flight path of the drone is generated. For details, see lines 23-25. The specific steps of the probability_testing function are shown in Algorithm 5. Next, after finding all the drone paths, the path points are reconnected through the path_reconnection function. For details, see line 30 to reduce unnecessary path points and improve path quality. For specific steps, see Algorithm 6.

[0163] Table 3 Algorithm 1

[0164]

[0165]

[0166] Compared with the traditional RRT* algorithm, MUBCR-RRT* has made the following improvements:

[0167] First, the algorithm plans the paths of multiple drones simultaneously, that is, it generates multiple expansion trees at the same time. Figure 3 As shown in the figure, in the node connection stage, a safety detection mechanism is introduced. This mechanism performs collision detection on the newly generated path segment and other existing expansion trees based on obstacles and fire points to ensure that the drones can effectively avoid each other during flight and ensure flight safety. This mechanism is applied in the connection stage of new nodes, the probability detection mechanism, and the path reconnection strategy.

[0168] Second, a node generation strategy is proposed. Specifically, multiple sampling points are first generated, and the sampling bias strategy is used to offset the sampling points. Then, multiple candidate points are obtained through expansion. After that, the candidate point evaluation strategy is used to evaluate and sort the candidate points, and the optimal nodes that can ensure safety are selected and added to the expansion.

[0169] Third, drawing on the principle of artificial potential field method, a sampling bias strategy is proposed. An attractive potential field is set for the end point to attract the sampling points to approach the end point, aiming to improve the efficiency of path search. By guiding the sampling points to shift to more potential areas, the convergence speed of path planning is accelerated.

[0170] Fourth, we draw on and optimize the fitness function in the A* algorithm, propose a candidate point evaluation strategy, and design a new fitness function. This strategy evaluates candidate nodes and takes into account the positions and paths of other drones. This strategy improves path quality and further accelerates algorithm convergence.

[0171] Fifth, a probabilistic detection mechanism is introduced to determine whether the path between the end point and the nearest node in the current tree is passable. If the path satisfies the safety detection mechanism, the algorithm will directly connect the nearest node to the end point, thus skipping the process of generating other nodes in between.

[0172] Sixth, a path reconnection mechanism is proposed. After obtaining a feasible path, the paths of multiple drones are reconnected. On the basis of ensuring safety, unnecessary nodes are reduced and the path quality is improved.

[0173] Seventh, for the final path, the Dubins curve is used to optimize the trajectory so that it meets the UAV's self-generated performance constraints.

[0174] Safety detection mechanism:

[0175] In the process of executing multi-UAV trajectory planning, in order to avoid intersection collision of UAV routes, it is necessary to ensure that the flight trajectories of different UAVs cannot occupy the same grid cells. Specifically, in the process of building a UAV expansion tree, every time a new edge is generated, it is necessary to determine whether this new edge occupies the same grid as any existing edge of the remaining UAV expansion tree. If they occupy the same grid, this new edge cannot be added to the expansion tree.

[0176] In view of the above problems, the present invention proposes a safety detection mechanism, and its pseudo code is shown in Algorithm 2 in Table 4. First, the mechanism performs a passability check on the two points to be connected (see line 1-2), and the pass_free function is used to ensure that there are no obstacles between the two points and that they can pass. Next, the present invention introduces a distance parameter check_dis, see line 4, which is directly related to the distance between x1 and x2. Then, all nodes whose distance to x1 is less than check_dis are searched in different expansion trees, see line 9. For these nodes, the present invention will perform a cross-check, see line 10. Specifically, it is to determine whether the lines between these nodes and their parent nodes and child nodes occupy the same grid as the new lines to be generated. Finally, it can be determined whether the two points can be connected safely.

[0177] Table 4 Algorithm 2

[0178]

[0179] Node generation strategy:

[0180] Although the RRT* algorithm effectively solves the path planning problem, its nature based on random sampling leads to a slow convergence speed and the risk of falling into a local optimal solution. Therefore, the present invention designs a node generation strategy to generate new nodes for the random expansion tree. The specific steps of this strategy are detailed in Algorithm 3 in Table 5. First, in the sampling point generation stage of the algorithm, not only one sampling point is generated, but multiple sampling points are generated, specifically see line 3-8, and a sampling bias strategy is designed to bias these sampling points, specifically see line 4, to obtain multiple bias points, and then multiple candidate points are generated according to these bias points, specifically see line 6, and the steer function represents the expansion number from x_nearest to the sampling point x_rand direction to expand the step length step to obtain a new candidate point x_new. For these candidate points, the present invention designs a candidate point evaluation strategy, evaluates and ranks these candidate points, specifically see line 9, and finally selects the optimal candidate point that can pass the safety detection mechanism as the new node of this tree, specifically see line 11-13.

[0181] Table 5 Algorithm 3

[0182]

[0183] Sampling bias strategy:

[0184] The present invention draws on the idea of ​​the APF algorithm and constructs an attractive potential field for the end point. When the random sampling point is generated, it is guided by the potential field to obtain a bias point. The artificial potential field function set by the present invention is shown in formula (34), where ζ is the gravitational potential field coefficient.

[0185]

[0186] The present invention combines conical and quadratic potential fields. At long distances, the conical potential field provides a fixed, large attraction, allowing the agent to move quickly toward the target. When close, the quadratic potential field provides an attraction that decreases as the distance decreases, helping the object to approach the target more accurately. Combining these two potential fields, a potential field can be created that provides sufficient attraction when the object is far away from the target, while providing more precise control when the agent approaches the target, thereby avoiding the overshooting problem.

[0187] The gradient corresponding to the potential field is shown in formula (35).

[0188]

[0189] in, is the boundary distance between the two potential fields, when When , the gradients of the quadratic potential field and the conical potential field are equal. The step size is set to three times and ζ is set to 1 / 3.

[0190] Next, the gravitational force on the random sampling point is calculated based on the random sampling point and the end point position. The calculation formula is as follows:

[0191]

[0192] Candidate point evaluation strategy:

[0193] The present invention innovatively proposes a candidate point evaluation strategy, which aims to evaluate and rank a large number of candidate tree nodes. The strategy uses an optimized heuristic function to evaluate the value of each candidate node. After the evaluation is completed, all candidate tree nodes will be arranged in order from low to high according to their fitness values.

[0194] In the A* algorithm, its heuristic function is defined as shown in formula (37):

[0195] f(x)=h(x)+g(x)(36)

[0196] Among them, f(x) represents the value information of the current node x, h(x) represents the estimated cost from the current node to the end point, and g(x) represents the actual cost from the starting point to the current node.

[0197] The present invention proposes an improved heuristic function, which is specifically defined as shown in formula (37). Assuming there are Nu drones, there are Nu expansion trees. For an expansion tree, its heuristic function is:

[0198]

[0199] Where F(x new ) represents the candidate node x new The fitness value of x parent Represents x new The parent node of t(x parent ,x new ) indicates that from x parent to x new The threat value of is shown in formula (38). new ) means x in the current tree new The specific calculation formula is shown in formula (39). In order to comprehensively evaluate the advantages and disadvantages of existing nodes, ρ(x new ), which means x new For other tree values, the specific calculation formula is shown in formula (40). At the same time, the weight α is introduced to adjust f1(x new ) and ρ(x new ) in the node selection process.

[0200] t(x parent ,x new )=min(1,δt passnode / num passnode )(38)

[0201] In formula (38), δ represents the weight coefficient of the grid threat value, which is used to quantify the importance attached to the threat area. passnode Indicates that from x parent to x new The sum of the threat values ​​of each grid passed on the path, num passnode Indicates the number of grids passed.

[0202]

[0203] In formula (39), ρ(x new ) represents the cumulative fitness value of x_new in other trees. new) is used to quantify the fitness value of x_new in other trees. The specific calculation is shown in formula (41).

[0204] f1(x new ) = g(x parent )+βh(x new )+dis(x parent ,x new ) (40)

[0205] In formula (40), f1(x new ) consists of three parts. The first part is from the starting point to x parent The path length consumed is given by g(x parent ) represents; the second part is from the candidate node x new The distance to the end point is calculated using the Euclidean distance, using h(x new ) is represented by the third part. parent to x new The distance is also calculated using the Euclidean distance, using dis(x parent ,x new ) is represented by . β is the balancer of this function, which coordinates the dependence on the historical path and the response to the target proximity.

[0206]

[0207] Formula (41) introduces lastx new Parameter, which stores the nodes newly added by different expansion trees during the previous iteration. represents the nodes newly added in the last round of the j-th expansion tree. So f2(x new ) by replacing f1(x new ) parent Replace with This introduces the value of the current node x_new to other expansion trees. At the same time, in order to make f2(x new )for With x new The distance between them is more sensitive, and f2(x new ), the first half of r safe Indicates the safe distance of the drone. represents the Euclidean distance between the current candidate node and the newly added node in the previous round, and γ is a weight coefficient, which weighs the weight between the two. The present invention sets it to 1.

[0208] The pseudo code of the candidate point evaluation strategy is shown in Algorithm 4 in Table 6. First, the find_near function is used to search for all points within a certain range from x_new in the current expansion tree. Next, the parent node for x_new is found again, see line 3 for details, and the role of the chooseparent function is to filter each node in x_near_list to determine whether there is a node that can be the parent node of x_new, thereby optimizing the path length from the starting point to x_new. Next, the fitness value of each candidate node is calculated, see lines 4-6 for details, and they are sorted from small to large, see line 9 for details, and finally the sorted list is returned.

[0209] Table 6 Algorithm 4

[0210]

[0211]

[0212] Probabilistic Detection Strategy:

[0213] The present invention improves the termination condition of the algorithm and introduces a probabilistic detection mechanism. Specifically, for each tree, the strategy detects whether the node closest to the target point can pass the safety test through probabilistic evaluation. Once the test passes, the algorithm will directly connect the point to the target point, thereby significantly improving the convergence efficiency of the algorithm.

[0214] The specific steps are shown in Algorithm 5 in Table 7. First, a probability judgment is made, see line 1 for details. After the probability judgment, the node closest to the end point is found, see line 2 for details. Next, it is determined whether the nearest node and the end point can pass the safety check line 3. If so, the final path is found, see line 4 for details, and the path is returned, see line 5 for details.

[0215] Table 7 Algorithm 5

[0216]

[0217] Path reconnection mechanism:

[0218] The present invention proposes a path reconnection machine to reduce unnecessary nodes on the path, thereby improving the path quality. The specific operation is shown in Algorithm 6 in Table 8. Traverse all paths one by one, see line 2 for details, starting from the starting point of each path, check whether it can pass the security check and connect to the node after it one by one, and directly cross some nodes in the middle. If a node that fails the security check is encountered, the x_parent node will jump to the previous node of the node, and repeat the above operation, see line 4-15 for details, thereby realizing local optimization of the path. This process will continue along the entire path until all nodes are evaluated and possibly reconnected.

[0219] Table 8 Algorithm 6

[0220]

[0221] The present invention can effectively distinguish between threat areas and building areas by dividing the entire environmental space into grid units. This distinction provides an important basis for subsequent multi-UAV flight path planning. In order to meet the challenges of multi-UAV trajectory planning, the present invention proposes a safety detection mechanism, expands the application scope of the Rapidly-exploring Random Tree star algorithm to the field of multi-UAV trajectory planning, and proposes node generation strategies, candidate node evaluation strategies, and path reconnection mechanisms to improve the RRT* algorithm. These measures are intended to reduce planning time and improve path quality, thereby optimizing the trajectory planning performance of the multi-UAV system.

[0222] The present invention adopts the Dubins algorithm to smooth the trajectory, ensuring that the generated path not only meets the dynamic characteristics of the UAV, but also can achieve smooth and continuous flight.

[0223] The specific implementation methods described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific implementation method of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A multi-UAV path planning method in a threat area based on the MUBCR-RRT algorithm, characterized in that: The method comprises the following steps: Divide the flight environment into grid units and construct a threat environment model; Calculating a threat value for each grid cell according to the threat environment model; According to the threat value of the threat unit, the environment space is divided into safe passable space, threatening passable space and impassable space; Generate multi-drone flight path points in safe navigable spaces and threatening navigable spaces; Dubins is used to optimize the trajectory of multiple UAV flight path points.

2. The method for multi-UAV path planning in threat area based on MUBCR-RRT algorithm according to claim 1 is characterized in that: The threat environment model is specifically: Among them, t(x,y,z) represents the threat level of the grid cell (x,y,z), x max Represents the limit boundary value of the grid cell space in the x-axis direction, y max Represents the limit boundary value of the grid cell space in the y-axis direction, z max Represents the limit boundary value of the grid unit space in the z-axis direction.

3. The method for multi-UAV path planning in threat area based on MUBCR-RRT algorithm according to claim 2 is characterized in that: In the threat environment model: in, represents the threat value of building i to grid unit (x, y, z), Nb represents the number of buildings, represents the threat value of firepower point i to (x, y, z), and Nf represents the number of firepower points; in, Where P is a grid point, P f Indicates the location of the firing point; v f Indicates the direction angle of the shooting; θ represents the angle between the grid unit and the firing point, that is, v f and The angle between , in degrees; dis is distance.

4. The method for multi-UAV path planning in threat area based on MUBCR-RRT algorithm according to claim 3 is characterized in that: The threat value of the grid cell is greater than or equal to 0 and less than or equal to 1.

5. The method for multi-UAV path planning in threat area based on MUBCR-RRT algorithm according to claim 1 is characterized in that: Generate path points for multiple drones based on the threat value of the grid cells, including: The MUBCR-RRT* algorithm is used to add the starting point of the drone to the corresponding expansion tree; Find the flight node in the expansion tree until all expansion trees reach the end; Connecting the starting point of the drone, the flight node, and the end point of the drone to obtain a flight path; The flight path is optimized using the Dubins curve.

6. A multi-UAV path planning device in a threat area based on the MUBCR-RRT algorithm, characterized in that: include: A construction unit is used to divide the flight environment into grid units and construct a threat environment model; A calculation unit, used for calculating the threat value of each grid unit according to the threat environment model; Divide the units, and divide the environment space into safe passable space, threatening passable space, and impassable space according to the threat value of the threat unit; A planning unit for generating multi-UAV flight path points in safe navigable spaces and navigable spaces with threats; The optimization unit is used to perform trajectory optimization of multiple UAV flight path points using Dubins.

7. The device for multi-UAV path planning in threat area based on MUBCR-RRT algorithm according to claim 6 is characterized in that: The threat environment model is specifically: Among them, t(x,y,z) represents the threat level of the grid cell (x,y,z), x max Represents the limit boundary value of the grid cell space in the x-axis direction, y max Represents the limit boundary value of the grid cell space in the y-axis direction, z max Represents the limit boundary value of the grid unit space in the z-axis direction.

8. The device for multi-UAV path planning in threat area based on MUBCR-RRT algorithm according to claim 7 is characterized in that: In the threat environment model: in, represents the threat value of building i to grid unit (x, y, z), Nb represents the number of buildings, represents the threat value of firepower point i to (x, y, z), and Nf represents the number of firepower points; in, Where P is a grid point, P f Indicates the location of the firing point; v f Indicates the direction angle of the shooting; θ represents the angle between the grid unit and the firing point, that is, v f and The angle between , in degrees; dis is distance.

9. The device for multi-UAV path planning in threat area based on MUBCR-RRT algorithm according to claim 8, characterized in that: The threat value of the grid cell is greater than or equal to 0 and less than or equal to 1.

10. The device for multi-UAV path planning in threat area based on MUBCR-RRT algorithm according to claim 6, characterized in that: The planning unit specifically includes: Used to add the starting point of the drone to the corresponding expansion tree using the MUBCR-RRT* algorithm; Used to find flight nodes in the expansion tree until all expansion trees reach the end point; Used to connect the starting point of the drone, the flight node and the end point of the drone to obtain a flight path; Used to optimize the flight path using the Dubins curve.

Images