1 Introduction

Unmanned aerial platforms are used more and more often in all areas of human life. Such vehicles are lighter, cheaper, and easier to build and maintain than their manned counterparts. In addition, they can replace a human in the implementation of dangerous tasks. Rescue services have been using this type of machine at the sites of natural disasters for many years. In civil applications, unmanned platforms enable the monitoring of forests and have opened up new possibilities in terrain photography. Research is being conducted on the use of drones in medical transport and parcel delivery. Due to their potential, all aspects of unmanned platforms are a field of active research. Starting from engines, materials, and aerodynamic properties, to autopilots and on-board computers performing tasks such as processing video material or collecting data for photogrammetry. One of the most important problems is the problem of planning routes and trajectories for single drones as well as swarms of drones composed of homogeneous or heterogeneous platforms.

The paper presents an algorithm that determines the flight trajectories of a swarm of unmanned platforms, for which the take-off and landing sites have been defined in the form of a terrain area to which UAVs must fly. Unmanned platforms start their mission a short distance from each other, they have a common destination, they must maintain a certain minimum distance between them and they cannot move further away from each other than the permissible distance. In addition, a practical assumption was made that the determined trajectories must be consistent with the real trajectories for specific UAVs. For this purpose, the UAV motion model was extended with the air platform dynamics model described in the article [1].

The paper introduces a better way of constructing flight paths for drones in a swarm. The algorithm presented in the paper involves the use of a spacetime tunnel, which is essentially a set of vectors that represent the flight paths of the drones. These vectors take into account the times at which the drones enter specific locations. The article also uses some of the terminology found in RRT algorithms, but the authors have tried to minimize the use of such terminology to make the paper more accessible to readers.

The method of construction and modification of each of the flight trajectories was presented so that the imposed constraints on the method of swarm flight were maintained. The novelties presented in the work are:

  • swarm flight trajectory model allowing for the construction of the trajectories for a swarm composed of several drones using efficient versions of the time-dependent RRT* algorithm rarely described in the literature;

  • development of an algorithm allowing for finding optimal flight trajectories of a UAV group during a strictly defined number of iterations (described by the sampling parameter and the parameter limiting the size of the search area of the solution space);

  • modification of the model with elements of trajectory planning taking into account the aerodynamics of the real air platform;

  • description of a collision detection and avoidance algorithm used in aviation to determine platform movement direction;

  • checking the algorithm’s performance for complex environmental models used in other works.

The rest of the article is as follows. Section 2 discusses the general problem of planning a flight trajectory for a drone and drone swarms. The terminology used in the literature has been presented. Various classes of algorithms solving the problem of route planning have been described. Their advantages and disadvantages are presented. The focus was primarily on the class of RRT algorithms. The most important algorithms and their importance for solving the presented problem have been described based on selected literature items. Random sampling techniques and additional aspects that should be considered when using this class’s algorithms were approximated. In Section 3, the research problem was formulated. A formal description of the problem and mathematical models of the algorithms used to determine the trajectory are presented. The most important contributions are Algorithm 5 and Algorithm 3. Section 4 presents the results of the algorithm research. The algorithm was tested for a swarm composed of several drones moving in varied terrain with the given UAV flight parameters. The probability of finding routes has been determined. Measurement results are discussed. In addition, the software and hardware platform on which the algorithm was implemented was described. Section 6 describes further development directions of the presented algorithms and methods of increasing their efficiency.

2 Related works

In many practical applications, the use of a single platform is not sufficient, and multiple air platforms must be used simultaneously. A description of the types of missions that drone swarms can perform is beyond the scope of this article. The interested reader can see, for example, the work [2].

A possible example would be terrain search or reconnaissance tasks, where one needs to cover as much area as possible in the shortest possible time. Land cover generation issues are often discussed in the literature [3, 4].

Using multiple platforms simultaneously can perform tasks much faster or explore a larger area. However, coordinating multiple vehicles poses unique challenges, with collision avoidance being the most crucial one. A collision can lead to the failure or destruction of one or more vehicles and pose a threat to all objects in the area. RRT class algorithms can be adapted to multi-platform systems in various ways. The simplest approach is adjusting the speed of the vehicles, as presented in [5]. Vehicles are assigned a priority that determines the order of route planning. Higher priority vehicles plan their route first. Thanks to this solution, a route once planned does not have to be planned again. All vehicles treat the others as obstacles. In [6], vehicles plan their routes independently and then collision situations are detected. If such a situation occurs, the speed of both platforms is corrected. In a situation where vehicles are moving in a similar direction, a local correction of the routes takes place. A more formal approach used in aviation is presented in the work [7], which uses geometry-based algorithms to determine the course change of an air platform in the event of a potential in-flight collision with another platform.

UAV flight planning involves calculating the path between two points in space that are marked as \(x_{begin}\) and \(x_{end}\). This process takes into consideration the imposed constraints. Usually, these points represent larger areas which help in determining the flight trajectory of a group of UAVs. The presented model takes into account both terrain obstacles and the model of the platform’s aerodynamics, including its inertia, maximum speed, and acceleration. The algorithm determines the flight trajectory in the search space, which will be further denoted as \({\mathcal {X}}\). In this space, one can isolate a subspace contained in \({\mathcal {X}}\) in which there are located some obstacles. It will be marked as \({\mathcal {X}_{obstacle}}\). It was assumed that the trajectory of the UAV should not intersect with the boundaries of the area occupied by obstacles, regardless of their height. While other assumptions are possible, the one adopted in the article is commonly used in literature. The UAV’s flight trajectory in the \({\mathcal {X}}\) solution space, also known as the configuration space [8], consists of a series of points that the unmanned platform flies through. For each point, information about the platform’s arrival time and status are included.

In the presented RRT algorithm, the default maximum UAV flight time was adopted, which is calculated as the flight time of the slowest platform from the take-off point to the landing point multiplied by the scaling factor \(\sigma \). This time has a significant impact on the quality of the solutions obtained in the case of a swarm consisting of several UAVs. The status of the platform is usually understood as a set of values of the platform control variables that are set at the waypoint. This is usually information about roll, pitch, yaw, etc. depending on the accuracy of the UAV model.

2.1 RRT algorithms in swarm trajectory planning

The Rapidly-exploring Random Tree (RRT) algorithm was published in [9] in 1998 and described in more detail in [8]. Like the PRM algorithm, it is based on \({\mathcal {X}}\) random sampling, but unlike it, it does not create a graph, but a tree with the root at \(x_{begin}\). Therefore, the created data structure can answer a single route request. In [10] the algorithm is proven to be probabilistically complete but asymptotically suboptimal. The algorithm has a computational complexity of \(O(n\log n)\) of the tree phase, while queries are executed in O(n) time. Memory complexity is also O(n). In all cases, n is the number of vertices in the tree. Other examples of sampling methods are presented in [11]. Compared to other methods, RRT works well in solving many route-planning problems. Therefore, the RRT methods are often used to determine the trajectory of UAVs in \(R^2\) and \(R^3\) dimensions.

Bidirectional RRT is a variant of the algorithm proposed in [8]. It is similar to the original RRT, but unlike it, two trees are created - one with the root at the start point \(x_{begin}\) and the other at a selected point in the end region \(x_{end} \in {\mathcal {X}_{end}}\). The first part of the algorithm alternately extends one tree while the other tries to connect to the newly added point. If successful, the procedure exits and the route \(\{x_{begin}, ..., x_i,x_{i+1},..., x_{end}\}\) is returned. The disadvantage of the algorithm is the discontinuity that often appears where the trees connect. This can be reduced, for example, by route smoothing techniques or a local scheduling algorithm.

The RRT* algorithm described in [10] performs n iterations that add vertices to the tree. Compared to the original procedure, there is no aborting if a route is found. It is easy to see that this algorithm is more complex than the original RRT. It has been extended with tree rebuilding. Instead of appending \(x_{new}\) (new vertex of the RRT tree) to the tree with \(x_{nearest}\) as a predecessor (where \(x_{nearest}\) is the nearest vertex of RRT tree to connect with \(x_{new}\)), other points in the neighborhood are also considered. The set of these points is determined using the appropriate function on the graph G, from which the points are to be taken if they belong to a sphere with a predefined center and radius.

Spline-RRT* is designed to route fixed-wing platforms using Bézier curves to plot smooth routes. Compared to other unmanned platforms, fixed-wing aircraft cannot stop and turn in place, but to change the direction of movement, they rotate in all 3 axes. At the same time, rotations cannot be arbitrary, as they can lead the UAV to lose lift, which will lead to the destruction of the platform. The described algorithm modifies RRT* in such a way that subsequent vertices are added to the tree only when it is possible to lead a dynamically admissible route to them, i.e. taking into account the physical capabilities of the platform. Additionally, the edges of the tree are not straight slices, but Bézier curves. The resulting routes are smoother compared to RRT*.

InformedRRT* uses ellipsoids to significantly speed up the speed of finding the optimal solution [12]. Initially, the algorithm works in the same way as RRT*. It also creates a tree with n vertices. When the first route is found, the \({\mathcal {X}}\) sampling strategy changes. From then on, only an ellipsoid-shaped area is sampled with focal points at \(x_{begin}\) and \(x_{end}\) and axis lengths determined by the length of the best-so-far route. The InformedRRT* algorithm was used as a component algorithm of the presented method.

Liu et al. [13] have introduced the Adaptive Area Sampling RRT (NT-ARS-RRT) method to address efficiency issues in the RRT algorithm. They made modifications in several stages: implementing a map preprocessing method to reduce the exploration space of the random tree, developing a branch-leaf retraction method to optimize the selection of the next node, and refining the initial path selection.

Wang et. al. [14] solves the problem of path planning in complex environments with narrow channels, mazes, and multiple obstacles with the adaptive extension bidirectional RRT* (AEB-RRT*) algorithm. The AEB-RRT* algorithm extends from both the starting point and the ending point and adaptively adjusts the different expansion methods according to the collision detection failures. After finding a feasible path, the path is post-processed by interpolation and triangulation to obtain a shorter path. The Manhattan distance replaces the Euclidean one to calculate the distances between nodes. This lets to achieve higher computational efficiency.

In the paper [15], a time-based RRT named HPO-RRT* is proposed to solve the real-time path planning problem of UAVs operating in a dynamic environment. This algorithm defines a hierarchical framework including the perception system, path planner, and path optimizer. The algorithm can be used in the method for determining the flight of a drone swarm proposed in the article as a component algorithm. The advanced Skeleton Extraction RRT method described in the article [16] can be used as a component algorithm of the proposed method. The obtained results were compared to the results presented in this work.

An excellent overview of RRT algorithms is provided by Saadi et al. [17]. The interested reader is encouraged to read the algorithmic overview presented in this paper.

2.2 Other algorithms used in swarm trajectory planning

Wang et al. [18] presented a combination of two algorithms: PSO and a modified Sparrow Search algorithm (ESSA) named PESSA. Both algorithms run in parallel. PSO is an example of a swarm intelligence optimization method. Each particle models a feasible solution to the problem. Any particle is described by position and velocity vectors. Both vectors update patterns based on the particle and population’s experience. The authors changed the algorithm update strategy (i.e., the SSA step) to shorten the time to find feasible solutions. Objective functions have been defined for both algorithms and in each step. The smallest value of the objective function becomes the valid value. The algorithm is inspired by the group wisdom and anti-predation behavior of sparrows (see [19]). The obstacles were generated based on a DEM grid, but, the algorithm works for obstacles modeled in various ways. The objective function determines the route length of the UAV. The task constraints concern avoiding collisions with an obstacle and the turning angle allowed for the UAV, which corresponds to the constraints shown in our work. The PESSA was compared with several algorithms based on SSA or PSO. The presented results show that in an area containing several obstacles, the UAV flight trajectory can be found in not more than 300 iterations. The authors do not discuss the case of determining the swarm’s trajectory.

Wu et al. [20] propose a Global and Local Moth-flame Optimization (MFO) Algorithm for UAV Formation Path Planning Under Multi-constraints. The algorithm was tested for a swarm consisting of 5 UAVs. In this algorithm three improvement strategies were proposed and analyzed for MFO, namely chaos-based moth initialization, adaptive weighted position update strategy, and population diversity improvement strategy. The presented algorithm was faster than a version based on Genetic Algorithm presented by Elhoseny et al. [21]. This advantage of the simulation was a very simplified testing environment where the trajectories of UAVs could be easily calculated.

Peng et al. [22] presented a cooperative area search for multiple UAVs based on RRT and Decentralized Receding Horizon Optimization. This algorithm is a combination of other algorithms. In the first step the multiple UAVs cooperative search problem is posed as a receding horizon (RH) optimization decision problem and RH based UAV search decision process is proposed. Then, this optimization problem is partitioned into several UAV subsystem optimization problems and solved using Particle Swarm Optimization (PSO). Once the optimization decisions are made, the RRT algorithm is used [9] to search the solution space.

Li et. al. [23] present a modified Particle Swarm Optimization (PSO) algorithm that is used to find 3D trajectories for an Autonomous Underwater Vehicle (AUV). The obstacle model is based on the Digital Elevation Model (DEM) grid. The authors propose an improved PSO algorithm with the compression ratio to find a solution to the AUV route planning problem. A mathematical model is then created to plan the AUV route, considering the influence of ocean currents and seabed relief. The proposed method was compared with other algorithms such as GA, PSO, and ant colony optimization (ACO). Simulation results show that the proposed method finds a globally feasible solution in a shorter time. However, only one AUV is considered. What else, the number of obstacles on the seabed is also small. A comparison of the costs of determining the feasible trajectory for various planning algorithms running was presented.

The PRM (Probabilistic Roadmap) algorithm was introduced in 1996 in [24]. Its operation consists of two phases. In the training phase, the algorithm generates the graph G(VE), where V denotes a set of vertices, and E denotes a set of edges. The graph is created by randomly sampling \({\mathcal {X}}\) and then combining the generated samples. In the query phase, the trajectory is determined. Query handling consists of an attempt to connect the points \(x_{begin}\) and \(x_{end}\) with a graph, and then find a path inside the graph, e.g. using Dijkstra’s algorithm or A* (for larger graphs). Various versions of the algorithm can be found in the literature that omits some steps of the basic [10] algorithm. Collision detection is highly dependent on the number of degrees of freedom of the aerial platform, its environment, and the available computing power and memory. The algorithm presented in [24] selects configurations from an n-dimensional sphere around \(x_{sample}\) with radius r. Another option is to select k nearest neighbors. A modified version of the algorithm - PRM*, described in [10], proposes that k or r depend on the number of vertices in the graph and the measure and number of dimensions of \(\mathcal {X}\). In the same paper, it was formally proved that the simplified version of the PRM algorithm is probabilistically complete, i.e. if there is a trajectory from \(x_{begin}\) to \(x_{end}\), then the probability that the algorithm will find it tends to 1 with the number of nodes in a graph going to infinity. However, PRM* is additionally asymptotically optimal, i.e. the probability that the returned trajectory is optimal tends to 1 as the number of nodes in the graph tends to infinity.

The method of potential fields [25] determines the trajectory by imposing repulsive and attractive forces on the object. The sources of repulsive forces are the starting point and obstacles, while the attractive force is the endpoint. This method lets to find routes without the need for complex collision detection algorithms. The disadvantage of the algorithm is the susceptibility to the problem of local minima. In regions where there are many obstacles, the generated forces can balance out, and at the same time, such a point is not the endpoint. In such a situation, it may be necessary to use an additional algorithm that will allow the object to leave the local minima and continue its search. A variant of the algorithm proposed in [25] used a variable resolution of vector fields. At first, general planning was carried out using a small number of vectors, and then parts of the route were planned again using a more accurate representation.

An interesting and often-used method of increasing the efficiency of trajectory determination is the use of the Tabu Search technique [26] and [27]. It is precisely defined, depending on the type of problem, and metaheuristic is used to support finding solutions to optimization problems. The algorithm itself increases the efficiency of other optimization methods. Tabu Search does not guarantee optimal solutions, but methods used together with it may improve the quality of the solution. In such cases, solutions are found faster. The main purpose of Tabu Search is to avoid local minima by applying limiting solutions that lead to local minima.

Ant colony algorithms take their inspiration from the workings of an ant colony [28,29,30]. They are used to solve computational problems that can be reduced to finding a path in a graph, including route planning problems [31]. The algorithm works on the principle of agent simulation with discrete time. Each ant, i.e. the simulation agent, randomly explores the graph. After finding a solution, the ant leaves a certain amount on the visited edges. The probability of picking an edge depends on its cost and the intensity of pheromones on it. In each time step, a certain amount of them evaporates, i.e. intensity of pheromones on the edges decreases. Pheromones increase the probability of selecting an edge in subsequent iterations. In every time step the best solution found is saved and the cycle starts again. In subsequent iterations, a larger number of ants will move more often along previously visited edges. Sometimes all the ants chose the same route. In case of a block, the procedure is aborted and the best solution found so far is returned. If no block occurs, the function runs several iterations before returning the current best solution.

Aslan et al. [32] presented an immune plasma algorithm (IPA). The origin of heuristic comes from a model of the immune system. IPA was used to solve the problem of path planning. The results presented show the efficiency of the method but are worse than the results presented in the paper.

Pirozmand et al. [33] proposed a discrete metaheuristic method called D-PFA (Discrete Path Finding) used to find routes for the TSP problem. The work presents the PFA algorithm divided into four subalgorithms to increase their efficiency. The proposed algorithm is characterized by high flexibility, short response time, and strong search space exploration. The simulation results used 34 different test cases of different sizes, which were compared with known heuristic algorithms. The algorithm can be easily used in planning routes for a swarm, but it requires the preparation of a route’s model in the form of a graph. This can be done based on visibility graphs class algorithms.

Serap et al. [34] compare the results of determining paths in ad hoc wireless networks based on methods known from game theory and modifications of Dijkstra’s algorithm. The focus of the research revolved around routing in networks and the optimization problem of finding the shortest path between two distinct vertices in the network, taking into consideration the performance limitations of the batteries powering the wireless system components.

An overview of artificial intelligence methods used to plan routes in UAV swarms is presented in the work [35]. The work discusses, among others, the use of genetic algorithms (GA) to solve this type of problem.

2.3 Optimal control algorithms in swarm trajectory planning

Other methods of determining the UAV flight trajectory are based on optimal control using the so-called indirect or direct methods. Indirect methods of optimal control described in [36] are based on variational relationships between state variables and controls. The change of state variables can be approximated by solving linearized equations to non-linear equations of state. The linearized equations depend on the control variations and can be used to determine the change of the functionals defining the optimal control task from the change of controls. An efficient method of determining the change of functionals to the change of controls is based on solving coupled equations to linearized equations. By introducing coupled equations, a method of calculating reduced gradients is obtained. One can determine state variables by solving state equations, etc. In the indirect method, the problem of optimal control is solved in such a way that the problem of optimal control is reduced to a problem in which the decision variables are only controls. When the equations of state and coupled equations are solved, then the solution satisfies the necessary conditions of optimality in the form of the weak maximum principle for the optimal control problem. However, the method is very sensitive, and often, it does not find the trajectory.

In the direct methods described by Betts [37], the optimal control problem is discretized and solved with given initial conditions. This task is still a non-linear problem. In each subsequent iteration, a feasible solution is obtained, usually local. Then, by changing the discretization, the solver tries to find a global optimal solution. The direct methods are simpler, but the size of the problem increases significantly, and obtaining the optimal solution takes a long time. The work [38] shows a simplification related to the discretization of the task of determining the flight trajectory on a given section, where all functions were linear functions. This type of approach is convenient to implement and does not require advanced knowledge in the field of optimal control.

3 Models and methods

This section will describe the optimization problem. In the \(R^2\) search space, the trajectories for several UAVs from the initial area \({\chi _{begin}}\) to the landing area \({\chi _{end}}\) are planned. The assumptions of the methods are as follows:

  • UAVs start at a distance of at most d from each other;

  • UAVs have a common landing area \({\chi _{end}}\), where the landing area can be a circle of radius r;

  • UAVs must maintain a minimum distance \(\delta \) during flight;

  • UAVs must not collide with each other at any time;

  • UAVs must maintain a maximum distance of D during flight.

Let \({H} = \{1,2,...,h,...,H\}\) describe the set of the indices of UAVs. Let \(\mathcal {X} \subseteq \mathbb {R}^2 \times \mathbb {T} \times \mathbb {R}^n\) denote the state space, i.e. vectors \(x_i=<w_i,t_i^h,p_i^h>\) where \(w_i\) describes the position in \(R^2\) in the area where trajectories are searched, \(t_i^h\) is time when h-th platform enters the \(w_i\). Element \(p_i^h\) is a vector of n elements describing the current values of the variables controlling the h-th UAV flight that flew to the \(w_i\) point (i.e. platform speed, pitch, yaw, roll, etc.). This vector is primarily used in algorithms that take into account the aerodynamics of the platform when determining subsequent waypoints. In other cases, only the platform speed at the waypoint is used, denoted as \(s_i^h\), so \(x_i=<w_i,t_i^h,s_i^h>\) is a simplified version of the flight model.

The time is discrete, with a constant step. The minimum distance ensures the safe flight of the platforms. The maximum distance ensures the possibility of establishing radio communication between the UAVs, i.e. at any moment, for any UAV h1, there exists another UAV h2, which flies at a distance not greater than D. It was also assumed that each UAV can simultaneously communicate with several others, so it is always possible to indicate a sequence of UAVs with the use of which the pair (h1, h2) can establish communication.

Recall that the dimension \(\mathbb {T}\) is dependent on \(\sigma \). \(\sigma \) gives an upper bound on UAV flight time and indirectly search time, which is a common assumption in RRT algorithms.

In the further part of the work, the simplification of the notation of vertices and edges of the graph G will be used. The vertex of the graph will be marked as \(x_i\) and not \(w_i \in x_i\) and the edge as \((x_i,x_j)\) instead of \((w_i,w_j) : w_i \in x_i, w_j \in x_j\). The proposed simplification is intended to increase the readability of the models.

3.1 Determining UAV trajectory with RRT*

The RRT* algorithm is shown in the pseudocode as Algorithm 1. The algorithm is a component of the main algorithm presented in the article and is therefore discussed here. The InformedRRT* algorithm, which is described in detail in [12], is presented in Algorithm 2. Both algorithms are used as component algorithms in the presented trajectory determination method.

In line 4, a point of the solution space is randomly selected, to which the algorithm will determine the trajectory. Function \(sample({\chi _{free}})\) is used to determine the point in the search space that determines the direction of UAV movement in the next step. The UAV will move in this direction, but it doesn’t necessarily have to reach \(x_{sample}\). It depends primarily on whether there are any obstacles in the way.

In line 5, the vertex of the tree G containing admissible flight trajectories is determined, which is closest to the randomly selected point in space. The function \(nearest(G,x_{sample})\) is used to indicate in graph G the vertex that is closest to the selected vertex described by \(x_{sample}\). In the case of RRT algorithms, the G graph is a tree, whose root is the initial vertex \(x_{begin}\) and the leaves are points to which the UAV can reach in a given time with given controls.

Line 6 specifies the possibility of flying on a straight section between \(x_{nearest}\) and \(x_{sample}\). Point \(x_{new}\) is a point that can be reached in a straight line from \(x_{nearest}\). If there are obstacles in the area, \(x_{new}\) is closer than \(x_{sample}\). Function \(trajectory(x_{nearest},x_{sample})\) checks the possibility of determining the trajectory of a single UAV between the points \(x_{nearest}\) and \(x_{sample}\). In the case of determining a straight flight trajectory without taking into account the aerodynamics of the aerial platform, only the absence of obstacles between the points is checked. If there are obstacles in the path, the point \(x_{new}\) is set on the segment between \(x_{nearest}\) and \(x_{sample}\) immediately before the obstacle.

The algorithm has been extended with a tree reconstruction mechanism in which trajectories are determined. Instead of appending \(x_{new}\) to the graph with \(x_{nearest}\) as its predecessor, other nearby points are also considered (lines 7-25). The set of these points is determined in step 8 using the closeTo(Gxr) function, where G is the tree from which the points are to be taken if they belong to a sphere at the center of x and radius r. Parameter r is determined as a function:

$$\begin{aligned} r(n)= & \min \{\gamma _{\text {RRT}^*}\cdot (\frac{\log (n)}{n})^{\frac{1}{d}},\eta \} \end{aligned}$$
(1)
$$\begin{aligned} \gamma _{\text {RRT}^*}= & 2(1+ \frac{1}{d})^{\frac{1}{d}}(\frac{\mu ({\chi _{ free}})}{\zeta _d})^{\frac{1}{d}} \end{aligned}$$
(2)

where d is the number of dimensions of \(\pmb {\chi }\), \(\mu \) is the measure (volume) of the set, and \(\zeta \) is the volume of the d-dimensional unit sphere. |V| means the cardinality of the set of vertices.

Algorithm 1
figure a

RRT\(^*\) algorithm

In the case of determining a straight flight trajectory, taking into account the aerodynamics of the air platform, the possibility of flight between points is checked by the given aerodynamic model. In the case of the presented work, the model described in the article [1] was taken into account, in which the values of the X vector variables were determined, the elements of which were linear and angular speeds in the UAV’s body frame \(O_{xyz}\), etc. In practice, therefore, a trajectory will be defined between the points in the form of a path defined in graph theory as a set of vertices starting from the initial vertex \(x_{nearest }\) to the final vertex \(x_{sample}\) or \(x_{new}\) (in case there is an obstacle in the path of the UAV). We define a trajectory as a set of \(\{ x_{nearest},...,x_{i},x_{i+1},...,x_{new}\}\).

Function \(collisionFree(x_{nearest}, x_{new})\) verifies whether the UAV collided with an obstacle or there was a situation in which two different UAVs were in the same place at the same time. For this situation not to occur, the condition for two states \(x_i\) and \(x_j\) must be met that \(w_i^{h1}=w_j^{h2} \implies t_i^{h1} \ne t_j^{h2}\) where \(h1, h2 \in {H}\).

It can be seen that the search radius for nearby points decreases proportionally to the logarithm of the number of vertices in the tree. After determining the set, it is checked whether the path leading from \(x_{begin}\) to \(x_{new}\) through one of these points is shorter than if it led through \(x_{nearest}\). If so, this point becomes the predecessor of \(x_{new}\). Another additional step of the algorithm is the revision of existing connections. Points in \({\mathcal {X}_{close}}\) are checked to see if the route from \(x_{begin}\) to them would be shorter if it went via \(x_{new}\) (lines 12-24). If so, \(x_{new}\) becomes the new predecessor of those points. The algorithm does not terminate the procedure after finding a route but always creates a tree with n vertices. Additional vertices are used in tree restructuring to optimize the route.

Another variant of the RRT* algorithm can also be considered, in which k of the nearest neighbors is selected, instead of a sphere. Then the formula in step 8 should be replaced with the following:

$$\begin{aligned} k(n)= & k_{\text {RRT}^*}\log (n) \end{aligned}$$
(3)
$$\begin{aligned} k_{\text {RRT}^*}> & e \cdot (1+ \frac{1}{d}) \end{aligned}$$
(4)

where \(e \approx 2.71\) is the Euler’s number.

This variant preserves the asymptotic completeness and optimality of RRT*. Both variants retain the computational and memory complexity of the original RRT algorithm. Due to the possibility of finding optimal solutions, most of the RRT class algorithms are based on RRT* and solve problems specific to the domain in which the goal is to accelerate the rate of convergence of the solution to the optimal solution.

3.2 Determining UAV trajectory with Informed RRT*

This algorithm uses ellipsoids to significantly speed up the speed of finding the optimal solution [12]. Initially, the algorithm works identically to RRT*. It also creates a tree with n vertices. When the first route is found, the \({\mathcal {X}}\) sampling strategy changes. From then on, only the ellipsoid-shaped area is sampled with focal points at \(x_{begin}\) and \(x_{end}\) and axis lengths determined by the length of the best-so-far route. Such an ellipsoid is shown in the Fig. 1.

Fig. 1
figure 1

The ellipsoid shape while the Informed RRT* algorithm is running

This allows for a significant reduction of the search area and, consequently, faster finding of the optimal route. Sampling an ellipsoid can be done by sampling a sphere and then transforming those samples into an ellipsoid. The algorithm is presented in the pseudocode Algorithm 2. The algorithm uses \(c_{best}\) to track the length of the shortest route found. Based on this, the position and orientation of the ellipsoid to be sampled are calculated.

Algorithm 2
figure b

Algorithm Informed RRT*

There are many methods by which the problem of sphere sampling can be solved, differing in simplicity, efficiency, and application. The rejection method consists of generating some random numbers (the coordinates of the selected point in the coordinate system), and then checking whether the point belongs to the sphere. If not, the point is rejected. Otherwise, the point is accepted, what was described in [39]. Spherical coordinate mapping consists of generating some random numbers and then converting them to a spherical coordinate system. The disadvantage of this method is the greater density of generated points. The concentric mapping presented in [40] is similar to the mapping of spherical coordinates. This method maps concentric squares to concentric circles. The advantage of the method is a more even distribution of points - smaller squares are mapped to smaller rings.

3.3 Determining swarm trajectory using Multiplatform Spacetime RRT*

At the first steps of the Multiplatform Spacetime RRT* algorithm (see Algorithm 3), in lines 1-5, the existence of a centroid is checked. The algorithm calculates the centroid position (\(w_m\)) as the arithmetic average of the initial positions (\(w_h\)) of all UAVs. The number of n samples is selected prior. In lines 2-5, it is checked if the centroid is in search space, and in lines 6-9 it is checked if the centroid is reachable from the starting positions of all UAVs. It should be emphasized that the UAV flight trajectory is determined between two selected points. Each of them can be the center of gravity of the UAV’s starting and target position. High level description of the algorithm is presented in Fig. 2.

Algorithm 3
figure c

Multiplatform Spacetime RRT \(^*\)

Fig. 2
figure 2

High level overview of Multiplatform Spacetime RRT*. First, a midpoint between all vehicles is found. Then a path from this point to the goal is calculated. Then, a spacetime tunnel is determined around the path. Finally, the paths to the destination area are calculated inside the tunnel for all drones

Then, in line 11 RRT* or Informed RRT* algorithm is used to plan route from centroid to \({\mathcal {X}_{end}}\). Notice that this procedure works in 2-dimensional space. The selected route is used to reduce the search space (see Algorithm 4). Informed RRT* is a probabilistic method. It is possible that a route is not found even if the route exists. Our algorithm checks whether the tree obtained as a result of the InformedRRT* algorithm contains a vertex in the end region \({\mathcal {X}_{end}}\) (lines 12-14). If not, the algorithm aborts planning.

2-dimensional space is transformed into 3-dimensional Spacetime in lines 15-18. The platform with the lowest flight speed is selected in line 15. In line 16, the flight path is extracted from G. Then, in line 17 the slowest platform flight time is calculated. This time is denoted by T. In line 18 two-dimensional search space is transformed into three-dimensional spacetime, i.e. set of points from search space additionally described with a time component. To determine the size of the temporal dimension, time T is multiplied by the scaling factor \(\sigma \). Next, in line 20, the trajectories are planned for each UAV in newly determined spacetime. When all trajectories are calculated, the final condition (line 21) is checked regarding whether the platform has reached the target area. If so, additional obstacles are added to the solution space (see line 22), i.e. a place where the UAV will be in time \(t_i^h\) is marked as an occupied place.

Then, for each UAV, the SpacetimeRRT* algorithm is called to find the route in \({\mathcal {X}}\) (see Algorithm 4). SpacetimeRRT* uses similar procedures as RRT*, but additional constraints were added. The \(trajectory(x_{nearest},x_{sample})\) procedure selects a vertex, which can be reached by the UAV in a few steps (see Algorithm 6) because the radius of the sphere in which the search for a new vertex \(x_{new}\) is limited by the number of steps and the speed of the UAV.

3.4 Space search with spacetime RRT*

The Spacetime RRT* algorithm (see Algorithm 4) determines the flight trajectory for the other platforms. The main difference is that this algorithm runs in spacetime, and not search space (i.e., the algorithm considers the time of arrival at the location). This allows to check for collision with dynamic objects i.e. other platforms. Collision checking with dynamic objects can be simplified to a series of collision checks with static objects at different points in time.

Algorithm 4
figure d

Spacetime RRT \(^*\)

The use of a spacetime tunnel is in line 4 of Algorithm 4. Usage of spacetime tunnel allows for efficient pathfinding for a fleet of vehicles. The algorithm presented in the article takes into account the aerodynamics of the platform and the need to avoid collisions with other platforms. The designed algorithm takes into account models used in other projects [1, 7].

An area called a spacetime tunnel is designated to speed up route finding. This tunnel is determined by the route planned earlier in line 11 of Algorithm 3. A spacetime tunnel is a subset of search space that contains all points within radius D from the route found in line 11 of Algorithm 3 (i.e. maximum allowable distance) in spatial dimensions and without constraints in the temporal dimension. This procedure is shown in Algorithm 5. First, a random point from route G is selected (line 2). \(\mathcal {U}(0,1)\) denotes a random number from a uniform distribution between 0 and 1. In this case, 0 means the starting point of the route and 1 is the final point. Then in line 3, a random point from a sphere with a radius D and a center in that point is sampled using the method plausible for the UAV used. Then a moment in time is sampled (line 4). For this purpose, a random number with uniform distribution is used. Finally point from radius and moment in time are combined to create a new sample for SpacetimeRRT*. This method lets us immediately reject most of the search space \(\chi \) and focus the search on the region where the path exists. The sampleSphere() procedure becomes much more complicated when a real fixed-wing UAV dynamics model is used. In this case, the transformation matrices between individual frames are calculated as presented in [1]. The matrix \(L_{qg}\) (see [1]) transforms the coordinates between the Earth’s and the UAV’s reference frames. Determining the new UAV position itself requires additional calculations of linear velocities in the reference frame of the flying platform. In practice, this is useful for determining the UAV roll angles, which directly affect the change in the platform’s position. Several options for randomly selecting the UAV roll angle can be defined. One option that works well in practice is to determine the angle in such a way that the trajectories of individual UAVs are similar to each other. This lets the drone swarm to move close to each other and fly through relatively tight corridors. In the experiments, a more advanced method of calculating the new UAV position presented by Chodnicki et al. [1] was used.

Another important difference is the tree rebuilding method in Algorithm 4. A set \(x_{near}\) of points is further narrowed down to points with a time component smaller than \(x_{new}\) (line 12). Next, a set \(x_{near}\) of points is further narrowed down to points with a time component greater than \(x_{new}\) (line 19). This ensures that all consecutive points in the trajectory have a time component higher than the previous. Intuitively one can interpret it as preventing time travelling of UAV.

Algorithm 5
figure e

Sampling of spacetime tunnel

The procedure for determining the UAV flight trajectory in the case of a real platform is more complicated and requires taking into account the platform’s maneuverability (see [1]). In addition, the need to avoid a moving obstacle or another unmanned platform should be considered. For this purpose, algorithms for determining the flight trajectory between the points \(x_{nearest}\) and \(x_{new}\) were used, based on a geometric approach using Dubin [38] curves. This is the widely used approach that is easy to implement. An example of determining the flight trajectory to the next point is shown in Fig. 3.

Fig. 3
figure 3

An example of determining the UAV flight trajectory between points \(x_{nearest}\) depicted as \(w_{i+1}\) and \(x_{new}\) depicted as \(w_{i+4}\) using Dubin curves [38]. Vertex \(w_{i}\) is a predecessor of \(x_{nearest}\)

Fig. 4
figure 4

Graphical representation of variables of the collision avoidance algorithm in a 3-dimensional cartesian coordinate system [7]

A separate issue is determining the UAV collision avoidance direction if the platform flying to the \(x_{new}\) point is on a collision course with another platform. In this case, collision prediction methods and collision avoidance course determination based on geometric methods can be used. Such an algorithm was used in one of the projects and described in [7]. The algorithm assumes knowledge of the position and velocity vectors of the platforms on the collision course. Then vectors of new courses of both platforms are determined for the assumed safe distance between platforms. The distances may be changed depending on whether the other platform on the collision course is assumed to also have trajectory modification systems. An example of determining the flight trajectory to the next point in the case of a UAV flight on a collision course with another platform is shown in Fig. 4. \(\overrightarrow{V_A}\) and \(\overrightarrow{V_B}\) are UAVs speed vectors. \(\overrightarrow{U_A}\) and \(\overrightarrow{U_B}\) are new UAVs speed vectors.

Algorithm 6
figure f

trajectory (Spacetime RRT \(^*\) version)

The algorithm for determining the flight trajectory between points \(x_{nearest}\) and \(x_{new}\) is shown as Algorithm 6. To determine the route between the points, the algorithm generates a sequence of points located on tangents to the circles defining the real turning capabilities of the UAV A in given weather conditions (lines 1-2). When the procedure is finished there are no static obstacles between \(x_{nearest}\) and \(x_{new}\) (line 3). Only other platforms may appear on the route of UAV A. Therefore, it may be necessary to modify the course according to the collision avoidance algorithm (lines 5-6). In this case, a new \(x_{new}\) is generated.

It is worth noting the practical importance of the UAV flight trajectories between individual points. In practice, UAV flight control is based on PID (Proportional Integral Derivative) controllers, as shown in [1]. The determined trajectories are of auxiliary importance and allow planners to check the possibility of flight on a given route. The PID controller guides the unmanned platform in such a way that the UAV flies along the route without significant deviations. However, deviations can occur and sometimes it is not worth minimizing them, because it is associated with too much energy loss.

4 Results

The algorithm performance tests were executed in three simulation environments, which differed in the number of obstacles (see Fig. 5). Additionally, tests of the designed algorithm were performed to compare the results with those presented in [16]. In [16], the authors used environments with a large number of obstacles that make it difficult for the RRT algorithm to find feasible solutions. The simulation environments are shown in Fig. 7.

The algorithm for determining the flight trajectory of a swarm of drones composed of two, three, and four drones was tested. The RRT* algorithm was used alternately with InformedRRT* to find routes for the Spacetime tunnel. If it is not stated differently, the following values of simulation were used:

  • Maximum velocity of drone = 30km/h;

  • Minimal separation between drones = 10m;

  • Maximum separation between drones = 150m;

  • Spatial dimension of search Space = 1km x 1km;

  • Goal area position = (0.70km, 0.15km) and size = 100m x 100m.

It is worth noting that the use of fewer drones does not significantly simplify the drone swarm flight planning solution. In practical applications, it is assumed that in a swarm composed of several UAVs, the principle of flying behind the leader of the formation applies. In this case, a flight trajectory is planned only for individual drones. Other drones must maintain appropriate distances to the leader and between themselves during the flight.

Table 1 presents the results of the Multiplatform Spacetime RRT* algorithm for various input parameters. The time in which the algorithm determines acceptable solutions for a given number of samples and the percentage of cases in which a trajectory for a given a-priori number of samples could not be determined was shown. The tests were carried out on the Ubuntu 18 LTS operating system. To ensure optimal performance, all unnecessary services occupying the computer processors were removed. The tests were run on an i7-870 processor with 32 GB of available operating memory. The g++ compiler was set to code optimization mode.

Fig. 5
figure 5

Flight trajectories of a swarm composed of two to four drones are determined for three environments with a different number of static obstacles. On the left side, the flight trajectories in the environment with the least number of obstacles are shown, marked as Environment I. In the middle, the flight paths in Environment II are determined, and on the right side, in Environment III. The blue line represents the spacetime tunnel, which is the trajectory for the drone that is not constrained by drones flying nearby. Each drone flies towards the rectangular area marked in red from the launch site. The drones do not take off from the same place, but they start a short distance from each other

Table 1 Average times of determining the trajectory of a drone swarm in an environment with static obstacles

To test the execution time of the algorithm, it was run with 1000, 2000, 3000, and  4000 samples, enough times for the algorithm to return success in most tested cases. In practice, the algorithm was able to find a route in almost all trials with this number of samples. There were almost no failures for N greater than 2000 samples. The execution time of the algorithm was measured, as well as the execution times for individual platforms. The results are presented in the graph in the Fig. 6.

The failure rate parameter determines the average number of cases in which the algorithm failed to determine the swarm trajectory for a given environment and a given number of samples. A larger number of samples reduces the failure rate but increases the time of determining the flight trajectory for the swarm.

It can be seen that the addition of another platform extended the calculation time due to the need to perform the UAV collision test in the air. The first platform made a minimal number of collisions, only with static obstacles, the second and third already checked for collisions with other platforms. The standard deviation error also increased with the number of samples and for each subsequent platform.

It should be emphasized that the aspects of determining safe routes where the risk of collision is minimal are usually omitted in the literature. It is then assumed that the UAVs are small enough that an accident is unlikely. However, such an approach is unacceptable when designing algorithms used in flying systems in a real environment.

Figure 6 shows the dependence of the flight time on the number of samples used to find the flight path of the platforms. It can be seen that if the number of samples is increased above 2000, the algorithm finds faster flight paths than those found in the case of a small number of samples. An important conclusion that can be drawn from the analysis of the results is that increasing the number of samples above 3000 (or 4000 samples for the complicated environments) does not significantly improve the quality of the determined solutions. This allows the conclusion that the algorithm can find good quality solutions for a relatively small number of samples, which guarantees obtaining acceptable results.

Empirical experiments demonstrate the ability of the proposed algorithm to solve the given problem. The algorithm can find a solution even with a small number of samples, which significantly reduces the time of searching for a solution. With a sufficiently large number of steps (over 2000), the algorithm determines acceptable solutions to the problem and it is practically impossible for the algorithm not to find an acceptable trajectory. Additional experiments conducted in environments with more obstacles allowed for a better determination of the properties of the algorithm. Of course, in an environment with a large number of obstacles in a small area to the maneuverability of the UAV, the algorithm has difficulty finding a solution. However, it was assumed (applied in practice) that in such environments the UAV is not used due to the possibility of a potential collision with a static obstacle.

An important role in the process of determining the UAV flight trajectory is played by the D parameter (see Fig. 6), which can mean, for example, the maximum radio range between UAVs. The a-priori value of this parameter was adopted in the work to verify the efficiency of the algorithm for a swarm of drones with a different number of platforms. It is worth emphasizing, however, that the impact of D on the speed of the algorithm can be significant depending on the number of obstacles in the area of operation. In practice, the lower the D value, the more often individual UAVs lose communication with each other. In this case, flight trajectories can be very chaotic in an environment with many obstacles. This situation usually occurs in military systems, where the transmitting power of the UAV on-board radios is minimized, usually due to the need to hide the flight of the platforms. In this case, collision avoidance algorithms are heavily used.

Finally, it is worth discussing the influence of the scaling factor \(\sigma \) on the UAV flight time. The \(\sigma \) parameter is a scaling factor that can be used to increase the maximum trajectory search time in the solution space. When \(\sigma =1\), the maximum flight time of the swarm cannot exceed the flight time of the slowest drone from the centroid point to the landing point. In that case, the probability of success of the algorithm is very low and depends on the platform’s initial positions relative to each other. A situation is possible in which one of the platforms starts too far from the goal and reaching the goal within time T would require exceding maximum velocity. The \(\sigma \) scaling factor should not be increased unreasonably above \(\sigma =3\). As one can see in Fig. 6, allowing the search for a solution for a time equal to multiples of the flight of the slowest platform can result in longer flight paths. This is because higher values of \(\sigma \) result in a higher volume of spacetime that needs to be searched. Such a situation occurs when there are a lot of obstacles in the swarm’s flight area and the trajectories are subject to restrictions related to avoiding collisions and respecting the dynamic parameters of the platforms, which was described in the article. In such a situation, the algorithm tries to separate the platforms flying in the swarm, which results in longer flight paths.

Fig. 6
figure 6

The results of the algorithm for the case of a swarm of four drones that moves in an environment with the largest number of obstacles tested during the experiments. The figures on the left show the dependence of the flight time on the value of the D parameter (max distance). The figures in the middle show the flight time of each UAV depending on the number of samples that were set when determining the flight trajectory. The figures on the right show the dependence of the flight time on the value of the \(\sigma \) parameter

Fig. 7
figure 7

Flight trajectories of a swarm composed of three drones determined for two environments with a different number of static obstacles - environments presented in [16]. The selected environments were the test bed for comparing the results of the presented algorithms

To compare the results obtained in this study with other works, the results presented in the article [16] were used. The article presents an extension of the RRT algorithm with greedy search methods. To speed up the development of the RRT algorithm, a skeleton extraction method was introduced to calculate the skeleton (net modeling a preferred route of UAV) of the search space, and RRT only expands the nodes on the skeleton. In [16], a trajectory for one UAV is determined, but a quadrocopter dynamics model is introduced, which significantly hinders the process of finding a solution. To compare the achieved results, two terrain models described in the article [16] were used. The flight trajectories of a swarm composed of three and four UAVs were determined analogously to the trajectories from the reference work. Figure 7 presents the determined trajectories that correspond to the results from the reference work. The average times for determining the trajectory for one UAV in the case of the [16] article are almost the same (comparing results for 4000 samples). This is primarily due to the use of a complex UAV dynamics model in the calculations in [16] (complicated 6 Degrees Of Freedom model). Upon comparing the results of both methods, it is evident that the Spacetime tunnel-based planning approach for swarm flight is highly efficient, even when compared to the latest versions of RRT. In an environment with a large number of obstacles, introducing an additional constraint related to the unmanned platform model is always crucial. The increase in the processing time of the reference algorithm also results from smoothing the determined flight trajectory. But in fact, the use of an additional trajectory smoothing algorithm is not applicable because the trajectory is smoothed by the UAV autopilot’s PID controllers, which are described by the platform dynamics model. Finally, it should be emphasized that the method of detecting collisions with terrain obstacles has a very large impact on the effectiveness of the algorithm. In most works in the area of flight trajectory planning using the RRT algorithm, information on this subject is not available.

5 Discussion

To compare the proposed method of planning the flight of a swarm of drones in an environment with a large number of obstacles, it is necessary to consider several design issues and describe what are the main limitations of existing methods.

There are several methods used to determine the flight trajectories of unmanned platforms. The most commonly used methods are heuristic search algorithms based on the Rapidly-exploring Random Tree (RRT) and Particle Swarm Optimization (PSO). However, Genetic Algorithms (GA) and Optimal Control Algorithms based on direct or indirect methods are used less frequently.

Flight trajectory planning methods have certain limitations. One of the primary limitations is that the algorithms used do not take into account the time it takes to reach individual points in the solution space. This is an essential consideration when planning a swarm flight. Another limitation is related to the rules for determining the subsequent positions of UAVs. For example, in the case of PSO algorithms, the position of multiple particles must be determined, which influences the average position of the drone. When it is necessary to determine the position of several drones located in the same area while maintaining collision avoidance rules, the algorithm may perform redundant calculations. This issue has been observed in one of the works described in this section. As a result, the RRT algorithm is a better choice for planning the swarm’s flight, as it does not have the drawbacks of the PSO algorithm.

It’s difficult to compare the effectiveness of heuristic trajectory planning algorithms based solely on the quality of solutions they provide. In the case of route determination algorithms, designated routes can be compared based on parameters such as the route length or the number of turns, which affect the smoothness of the route. However, relying solely on these factors to assess the algorithms is not reliable.

Comparing heuristic algorithms for drone swarm planning missions is crucial. One important factor to consider is whether the planning can be extended into the 3D search space. The quality of an algorithm can be determined by its ability to generate trajectories in space while accounting for static and dynamic obstacles. For example, one can compare various algorithms based on specific criteria, as shown in Table 2.

However, these algorithms are usually used to determine routes for individual platforms. It makes comprehensive comparison difficult. But it can be assumed that each of these algorithms can be easily used instead of InformedRRT* used in the algorithm presented in this work. The presented Multiplatform Spacetime RRT* algorithm allows the use of one of many available modifications of the RRT algorithm as a component of the entire presented method.

Table 2 Comparison of selected algorithms for determining the trajectory of unmanned platforms

Table 2 refers to several heuristic algorithms used for trajectory planning. It is worth discussing a few of them here. The article [41] presents an algorithm from the PSO group for planning the flight trajectory of a swarm of unmanned platforms. The proposed approach is based on a particle swarm optimization algorithm applied to the field search problem with improvements such as collision avoidance. The algorithm assumes an initial division of the set of unmanned platforms into separate swarms. In each swarm, the leader determines the direction in which the platforms should be moved (following the leader model), which significantly simplifies the problem being solved. The algorithm does not take into account the occurrence of terrain obstacles. The proposed collision avoidance mechanism is based only on the distance dependencies of the platforms. The algorithm has several simplifications that limit its practical application. The use of a swarm consisting of several leaders followed by other UAVs can be simplified to a task solved in this article. It is the leader of each UAV group that decides the direction of flight, while other UAVs follow the leader. The specificity of PSO operation causes the UAV flight trajectories to be chaotic. Especially in the first phase of the flight. On the other hand, attempts to solve the problem of swarm flight to a region with a small area (as in the problem presented in the article) will result in groups of PSO particles generating a large number of potential conflicts due to the limitations regarding the minimum distance between the particles. In practice, the time to solve the task will significantly exceed the times shown in the work [41].

The works [19] and [23] present PSO algorithms enabling the flight of one UAV taking into account static obstacles. The [19] algorithm can be used as a modification to the [41] work, but it should be borne in mind that the efficiency of calculations will decrease and the swarm of drones will still have to fly according to the rule of following the leader.

Interesting results of UAV flight trajectory planning were obtained in the RRT algorithm shown in [13]. The simulation results demonstrate the efficiency of this method. The algorithm presented by [13] lets to find a route with a quality corresponding to the RRT algorithm shown in this paper.

Guo et al. [15] presented a time-based RRT named HPO-RRT* is proposed to solve the real-time path planning problem of UAVs operating in a dynamic environment. A time-based RRT* algorithm improves the sampling process, tree expansion process, and collision checking. This method can be easily integrated with the proposed Multiplatform Spacetime RRT* due to the use of the time-based RRT* algorithm, which takes into account the time of arrival of the UAV to a location in the search space.

6 Conclusion

RRT class algorithms are interesting versions of the methods of planning the flight path of unmanned platforms. However, since these algorithms should also be implemented on the air platform itself, additional challenges arise for system designers. The designer should primarily design an algorithm that will be executed in the shortest possible time on a computer installed on the platform. For this purpose, an architecture should be developed that uses appropriate computing units, and the algorithms themselves should be designed to generate flight trajectories in parallel.

As time passes, newer and more efficient computing units are being introduced in the market. However, the acceleration rate of successive generations of processors has slowed down. Nowadays, the highest gains in computing power are achieved through the use of multi-processor and multi-core architectures. Graphics cards are also becoming increasingly popular for calculations. Although their instruction set is limited, they offer much higher computing power due to the use of thousands of cores. This trend is also visible in the microcontroller market. The available NVidia Jetson [43], Arduino Portenta [44], and Raspberry Pi 4 [45] modules have multi-core processors while being small and light enough to be placed on board an unmanned platform. For this reason, algorithms that can be run in parallel are of increasing importance. In the case of the RRT and RRT* algorithm, individual steps can be executed in parallel. The measurements made in [46] show that although the collision checking procedure in RRT has a complexity of O(1) for static environments, and searching for nearest neighbors and adding new vertices \(O(\log n)\), this the first takes 99% of the time. It’s worth noting that collision checking in RRT* grows logarithmically, which means that parallelizing the process can significantly improve the algorithm’s performance. Moreover, parallelizing the search for neighbors and restructuring the tree can further enhance the algorithm’s efficiency. For instance, every loop iteration in Algorithm 4 at lines 12 (loop section) can be performed independently, which is the simplest way to introduce parallelism.

An important element that should be taken into account is the fact that the UAV operates in conditions of dynamic changes in the situation, so the algorithms implemented on the platform should operate in real-time (in the case of modification of the flight trajectory, real-time is not available since small platforms are equipped with computers with low computing power). When planning a route, the dynamic environment in which the planning takes place must be taken into account when solving certain problems. First of all, obstacles can change. They may appear, disappear, or change shape and position. In practice, the maximum number of possible moving objects in the UAV flight area should be assumed. From the point of view of aviation regulations, the minimum distances must be maintained.

The target area may also change, and as the platform moves, its current location may be selected as the new starting state. In such a situation, the algorithms must not only be able to plan the route but also react quickly to any changes in the environment. In the case of RRT*, this is done by making appropriate use of an already existing tree restructuring procedure. This strategy was used in [47]. During iterations, the algorithm alternately expands and rebuilds the tree and uses it to plan the route of the platform. For restructuring purposes, two vertex queues are maintained - one contains newly added random vertices from the tree and the other vertices are located near the platform. As the object moves, the root of the tree changes, and the cost of moving between nodes is updated. Appearing, vanishing, and moving obstacles cause covered vertices to have a cost of \(\infty \), which means that the vertex is unreachable. Any cost changes are propagated to successors.

The direction of further work is primarily related to tuning the optimization algorithms implemented on the unmanned platform.