Collision free 4D path planning for multiple UAVs based on spatial ref ined voting mechanism and PSO approach

Yang LIU , Xujun ZHANG , Yu ZHANG , Xiangmin GUAN

a School of Information Science & Electric Engineering, Shandong Jiaotong University, Jinan 250357, China

b School of Electronic & Information Engineering, Beihang University, Beijing 100083, China

c College of Air Traff ic Control, Civil Aviation University of China, Tianjin 300350, China

d College of Transportation Engineering, Tongji University, Shanghai 200092, China

e School of General Aviation, Civil Aviation Management Institute of China, Beijing 100102, China

KEYWORDS 4D path planning;Collision free;Multiple UAVs;Obstacle avoidance;Particle swarm optimization;Spatial ref ined voting mechanism

Abstract In this paper, a four-dimensional coordinated path planning algorithm for multiple UAVs is proposed, in which time variable is taken into account for each UAV as well as collision free and obstacle avoidance.A Spatial Ref ined Voting Mechanism(SRVM)is designed for standard Particle Swarm Optimization(PSO)to overcome the defects of local optimal and slow convergence.For each generation candidate particle positions are recorded and an adaptive cube is formed with own adaptive side length to indicate occupied regions.Then space voting begins and is sorted based on voting results, whose centers with bigger voting counts are seen as sub-optimal positions. The average of all particles of corresponding dimensions are calculated as the ref ined solutions. A time coordination method is developed by generating specif ied candidate paths for every UAV, making them arrive the same destination with the same time consumption. A spatial-temporal collision avoidance technique is introduced to make collision free. Distance to destination is constructed to improve the searching accuracy and velocity of particles. In addition, the objective function is redesigned by considering the obstacle and threat avoidance,Estimated Time of Arrival(ETA),separation maintenance and UAV self-constraints. Experimental results prove the effectiveness and eff iciency of the algorithm.

1. Introduction

Unmanned Aerial Vehicles (UAVs) are aircrafts without onboard pilots that can be remotely controlled or can f ly autonomously based on preprogrammed f light plans.1-3Multiple UAVs are nowadays deployed for various missions,including aerial photography, search and rescue tasks, geophysical survey, surveillance and security enforcement, and high-risk target penetration. As multiple UAVs are deployed in more complicated missions, they may f ly through areas f illed with obstacles and high collision risk between them. Also, when a mission becomes more complicated, it is desirable to have a shared arrival time by them.4In such scenarios, UAVs should be able to f ly through the desired waypoints without crashing into obstacles(Obstacle-avoidance)or other agents(Collisionfree), as well as arrive at the same appointed destination with the same and least f lying time consumption. Therefore, path planning is an essential element of mission planning and execution for multiple UAVs. However, it is especially diff icult to coordinate the operations of all UAVs, especially for timecritical missions that require precise timing and sequencing of operations, and Collision Avoidance (CA).5

Multi-UAV path planning is a challenging problem due to its high dimensionality, equality and inequality constraints involved, and the requirements of spatial-temporal cooperation of multiple UAVs, which has recently received extensive attentions.6These years various path planning algorithms have been developed to solve the problem, including several collision avoidance techniques and time adjustment strategies.7-10In most of them, Mixed Integer Linear Programming (MILP)is the mostly used method because it can handle dynamic systems with discrete decision variables and there are many eff icient solvers available4,11,12for MILP. General Tau theory is a kind of bio-inspired path planning method,which was established based on the study of the natural motion patterns of animals when they approach a f ixed or moving object for perching or capturing prey.13Kuwata and How14presented a cooperative distributed robust trajectory optimization approach,using RHMILP with independent dynamics but coupled objectives and hard constraints. Bellingham used MILP for multivehicle path planning to handle waypoint visiting problems.15Also,some variations of MILP have been applied to this problem.16,17However, the existing literature only handled small size problem that may not applicable if the problem is in big size. Furthermore, these MILP modeling assumed a centralized system with an objective function for the entire system,it may not work well when the central control station or communication links are broken.

Besides MILP,another group of optimization techniques,a.k.a. population-based evolutionary algorithms, are also used for solving multi-UAV path planning problems,including Particle Swarm Optimization (PSO),18,19Genetic Algorithm(GA),20Firef ly Algorithm (FA),21Ant Colony Optimization(ACO),22differential evolution (DE),23memetic computing method,24Artif icial Bee Colony (ABC),25and their related improved methods. All the algorithms can be easily implemented and calculation cost is low with better results, which are popular in optimization f ields.These methods are often used to give perfect solutions to path planning problem for two or three-dimensional single UAV or multiple UAVs. However,these methods more focus on collision avoidance, f lying time is not considered at all. So how to apply these methods in 4D coordinated path planning for multiple UAVs to achieve collision and obstacle avoidance, coordinated time consumptions,and least cost is the biggest challenge. To the best knowledge of authors,such integrated problem has not been solved yet.

In this paper,a four-dimensional coordinated collision free path planning algorithm for multiple UAVs is proposed by taking time variable into account. In our model an improved mechanism is designed for standard PSO, which is called Spatial Ref ined Voting Mechanism (SRVM). By using this mechanism, the defects of local optimal and slow convergence are overcome. Time coordination mechanism and collision avoidance technique are developed for the multiple UAVs. Meanwhile distance to the destination and solution boundary of searching space are constructed to improve the searching accuracy and velocity of all particles.The objective function is redesigned by taking the obstacle and threat avoidance,Estimated Time of Arrival(ETA),separation maintenance and UAV selfconstraints into account. Experimental results demonstrate that the proposed method is capable of generating collisionfree paths eff iciently for multiple UAVs with the same time consumption.

The remainder of this paper is organized as follows. Collision free coordinated path planning problem for multiple UAVs is described in Section 2. Concise standard PSO algorithm and explicit realization of proposed method are given in Section 3. Section 4 presents the applications of the proposed algorithm and experimental results of global path planning. Finally, the paper is concluded in Section 5.

2. Problem formation

Coordinated path planning problem for multiple UAVs in four-dimensional space is to generate feasible f lying trajectories for each UAV from different starting points to the same destination at the same time,which are def ined by a set of optimization criteria and constraints with global minimum cost based on their coordination. It includes the minimization of the risk of destruction of the UAVs as well as the restrictions imposed by the internal UAVs' constraints and the external environment, and threat dynamics with global optimal solutions.

As shown in Fig. 1, three main contents should be taken into account, which are four-Dimensional Time (4DT) CA,self-constraints of each UAV and external dynamics. Suppose there is a formation of multiple UAVs to be required to f ly through a specif ied complicated territory with their starting points different and are required to reach the same destination with the same time consumption. There are a number of terrain obstacles and threats in the f light environments, all of which are known in prior accurately.The designed paths have to ensure the obstacle and threat avoidance.Also given UAVs'specif ications, the generated path should be feasible and safe for UAVs to follow,i.e.relative f lying height,limited fuel supply, the maximum climbing and gliding slope, and maximum turning angles for each UAV, should be considered in modeling. In order to reach the destination safely, a mechanism of CA between multiple UAVs is designed to maintain the separations between multiple UAVs, meanwhile f lying speed of each UAVs are set together with the planned path to make sure all UAVs arrive at the same ETA.

2.1. Path representation

Fig. 1 Block diagram of UAV path planning problem.

An example of global path planning in four-dimensional space for multiple UAVs is shown in Fig. 2. There are three UAVs and six threat areas in the f lying space,as well as terrain obstacles. We assume that the f light environment is f ixed and all obstacles and threat areas are known a priori.The mission is to establish three feasible f lying paths suitable for the three aircrafts to arrive at the preset destination at the same time avoiding all the threats and terrain obstacles at least f lying cost. So how to represent the three f lying paths accurately and the relations of different UAV paths during the f light become a crucial problem.

The model is simplif ied into a two-dimensional view given in Fig. 3. In Fig. 3 Si(i=1, 2, 3) denotes the starting point of UAV-i and D is the preset shared destination. Four threat areas Ti(i=1,2,3,4)locate at different positions in the task region with different covered space whose coordinates are known in advance. UAVs are safe out of the threat areas but may encounter threats if they f ly into threat areas and the possibility of being affected by the threats(or called UAVs'vulnerability) increases when UAVs get closer to the center of the threat areas. Path planning is to generate three optimal or suboptimal f lying paths for the UAVs in the region between point Siand D, with the same arriving time and least cost,as well as collision free and threat avoidance.

In the f irst step,the starting point with the longest distance to the shared destination D is identif ied and denoted as S1. S1and D are connected together with a straight line.The line S1D is then divided into (M+1) equal segments by M points,which lead to the formation of M vertical lines of S1D passing M points, which can be denoted as {L1, L2, ..., LM}.

Fig. 2 An example of path planning for 4D multiple UAVs.

Fig. 3 Path representation in two-dimensional view of multiple UAVs.

Then,for UAV-1, on each line Li(i=1,2,...,M)a point W1iis selected, which is called waypoint, forming a waypoint sequence WP1={W11, W12, ..., W1M}. The f light path of UAV-1 can be formed by connecting W1i. For UAV-2, based on the geometric relation in Fig. 3, there will be N waypoints on the N vertical lines Li(i=1, 2,...,N). n this way the waypoint sequence WP2={W21, W22, ..., W2N} for UAV-2 can also be formulated. Similarly, for UAV-3, its corresponding waypoint sequence WP3={W31,W32,...,W3P}can be determined. If there are more UAVs in the space, the same procedure will be taken to f ind out their own waypoint sequences.After all of these steps are accomplished, path planning for multiple UAVs in 4D space is turned into an optimization problem of how to optimize the waypoint series to achieve minimal f lying time and cost so that multiple UAVs can arrive the destination with the same ETA,and obstacle and collision avoidance.

In order to further simplify the calculation process and accelerate the searching speed,coordinate system is necessarily transformed as following.26The point where S1locates will be set as a new zero point and S1D will become the new x axis(X′),which is illustrated in Fig.3.The angle between two axes,namely S1D and OX, is θ, which means OX will be rotated in an anti-clockwise direction with θ in the new coordinate system(X′,Y′) The geometrical relationship between original and transformed coordinate systems of the same waypoint can be computed using following equation.

In Eq.(1)[x1,y1,z1]Tis the coordinate of new zero point S1in the original coordinate system.[x0,y0,z0]Tand[xT,yT,zT]Tare original and transformed coordinate of the same waypoint in two coordinate systems respectively. Thus, for any UAV-i,the horizontal coordinate xi,jof the j th waypoint in the sequence WPiin new coordinate system can be easily obtained with following equation.

where Niis the size of waypoint sequence WPi. So any waypoint sequence WPican be written in a simplif ied form with Eq. (2), which makes the computational cost further reduced greatly.

2.2. Objective function design

Multi-UAV path planning has multiple objectives. Designing the objective function to ref lect all objectives is an essential part of modeling and solving the path-planning problem.The objective function that evaluates a candidate path should take into account of the cost of the path,the performance and mission constraints.The different factors in the objective function are given in the following subsections, respectively.

(1) Obstacles in the terrain

A feasible path cannot go through the obstacles and has to avoid collisions with them, such as mountains and other terrains.An accurate and updated terrain map exists in most real situations.In this way it is reasonable to assume the characteristic of all obstacles are known in advance. To ensure this obstacle avoidance behavior, the algorithm would penalize the solutions that have at least one point of the spline trajectory inside the obstacles. Suppose f(xi,j, yi,j) to be the function that returns the altitude of the terrain at any(xi,j,yi,j)point on XOY plain of the j th waypoint for UAV-i. Total number of waypoints falling into the obstacles could be calculated by following equation to realize penalty.27

UAVs need to f ly in a specif ied region under the control of ground station. Flying to anywhere beyond the region will be penalized. The following equation is given to record the number of waypoints outside of the restricted f lying space in the candidate panned path for all UAVs.

In Eq.(4),SAFdenotes the allowed f lying space and usually designed as cube with certain length,width and height.[xi,j,yi,j,zi,j]Tis the coordinate of the j th waypoint on the UAV-i's path.

Combining Eqs. (3) and (4), the penalties of UAV-i not achieving obstacle avoidance is represented as following equation.

(2) Threat source modeling

In this study, we assume that the threats are deterministic and known in advance. It is desirable to keep the UAV away from these threats. If UAV f lies into the coverage of the threats, they are possible to be detected and taken down.

Threat area is def ined as a cylinder with coordinate vector PT=[xT, yT,hT, rT, LT]T, where [xT, yT]Tis the center on the XOY plain, hTis the covered height and rTis the detect range. And LTis the destruction gain. Larger the LT, more dangerous the threat is.

Any UAV-i's waypoint falls into the threat area will be recorded and penalized. Suppose UAV-i's j th waypoint has the coordinate[xi,j,yi,j,zi,j]T,then the UAV exposure function can be def ined with the following equation.

Secondly,based on different threats the penalties on candidate UAV paths are different, which will be discussed below.As described in the part before, after the coordinate system is transformed each path of UAV is consisted of many waypoint sequence WPiand correspondingly path segments are formed. Fig. 4 shows two path segments on two UAV f lying routes, WikWi(k+1)and WjdWj(d+1)between two vertical lines LSand LS+1for UAV-i and UAV-j, respectively. As show in Fig. 4, one of the two UAVs in the transformed coordinate system falls into the two threat areas, which will be punished obviously.A sample method is used to better evaluate the cost of candidate UAV paths.28Path segment WikWi(k+1)is then further divided into(L+1)parts by L sample points,namely Pl/Land l=1, 2, ..., L. The rules of these sample points can be built using Eq. (7).

Fig. 4 Cost calculation in threat areas.

Once L sample points are determined the cost of path segment WikWi(k+1)in the threat areas will be calculated as following.

In the equation |WikWi(k+1)| is the length of path segment WikWi(k+1),L is the number of sample points on the segment,and LT,jis the destruction gain of Threat j. And dl/L,jdenotes the distance of the l th sample point that on segment to the center of Threat j and l=1, 2, ..., L. In the end the number of threats that could affect this path segment WikWi(k+1)is set as NT. The total effects of threat areas on planned f lying path for UAV-i can be calculated by adding all the costs of path segments by using Eqs. (7) and (8).

After Eqs.(6)and(9)are obtained the threat cost of UAV-i can f inally be calculated by Eq. (10).

(3) Relative UAV f lying height

UAVs f lying at a low altitude can benef it from the terrainmask effect that help them avoid to be detected by unknown radars.29However, f lying at low altitude consumes more fuel.Thus in our model,minimum and maximum f lying heights relative to the terrain, are set as the constraints.

To achieve the goal, the accumulated differences between UAV's altitude and the altitude of terrain at current point is calculated, which is then divided by the total waypoints in the planned trajectory. Therefore, the proposed algorithm minimizes the elevation of the UAV over the terrain instead of the elevation of the UAV over the sea level. The following expression could be used to calculate this objective value,which integrates the f light altitude along the route to make the UAV tend to search the low height routes.

[xik,yik,zik]Tis the k th waypoint of UAV-i,f(xik,yik)is the terrain height at point[xik,yik]Ton the XOY plain,and Hminis the desired minimum f lying height of UAV. M is the total number of waypoints for UAV-i.

(4) Limited fuel supply

UAVs usually have a limited quantity of fuel supply and they have to reach their destination before consuming all of it.In real situations the fuel consumption is highly related with UAV descendingascending slopes and turning angles, which will be discussed in the following part.So here two parameters are used to stand for fuel consumptions, which are the total path length and distance to destination of current waypoint.

For any UAV in the sky,the shorter the path is,the less fuel consumption and less time under the same conditions. By reducing the exposure in the air, the chance of being detected by an unknown threat in a complicated environment will also decrease. The Eq. (12) is used to calculate this value.

Here

Coordinates [xi,0, yi,0, zi,0] and (xi,M+1, yi,M+1, zi,M+1) denote start and destination points of UAV-i respectively. Any waypoint coordinate of UAV-i is denoted as (xi,k, yi,k, zi,k) with k=1, 2, ..., M.

During the calculation process a new parameter,distance to destination, for each waypoint is added in our algorithm,which is used to evaluate the performance of its current position.As shown in Fig.5,for UAV-i,there are many candidate waypoints on each vertical line, such as LSand Ls+1. However, in the optimization process the distance to destination is so important it can determine the searching directions in the path planning algorithm to save a lot of computation time and also could make fuel consumption the least. In this way only those waypoints with smallest distance to destination will be chosen. The distance to destination for UAV-i can be denoted as dik(j), which means the distance between the j th candidate waypoint on the k th vertical line for UAV-i to destination.The number of all waypoints are M.The average distance of all the candidate waypoints in the path is computed in Eq. (13). Waypoint with least distance to destination will be selected as the f inal one.

Fig. 5 Distance selection to destination for UAV-i.

So for any UVA-i the cost of fuel consumption in objective function can be formulated by Eq. (14).

(5) UAV slope

The UAV maneuverability is also constrained by its maximum climbing slope αikand its minimum gliding slope βikof UAV-i on its k th waypoint, which in our case depend on altitude zik.27These desired slope are calculated with Eqs.(15)and(16).

The desired slope Sikof UAV-i at the k th waypoint can be obtained using Eq. (17).

So for all the waypoints on UAV-i's candidate path,whose slope is out of the desired range,will be punished using following equation.

(6) Turning angle

For UAV-i, the maneuverability is also constrained by its maximum turning angle.30The path planned for UAVs with big turning angles cannot be followed for the limited mobility of UAVs themselves. Theoretically the smaller the turning angle is the smoother the path will be. In our model the ideal turning angle will be the least one.

For the k th waypoint on UAV-i's candidate path its corresponding desired turning angle φikcould be calculated with following equation.

In this way for the whole path planned for UAV-i,the total turning angles will be calculated by Eq. (20). It will be the index to evaluate the effects of turning angles on the UAV f lying path.

Similarly, M is still the number of waypoints on the generated path. [xi0, yi0]Tand [xi(M+1), yi(M+1)]Trespectively stand for the coordinates of starting and destination points of UAV-i on XOY plain.

(7) Time coordination method

As in our model, the path generated for multiple UAVs should ensure they will arrive at the same destination simultaneously, which means they should have the same ETA. They are supposed to leave different starting points at the same departure time. Also the total f lying time of UAVs should be as little as possible to minimize their exposure to threats.Here an arriving time coordinated mechanism is introduced31which is given in Fig.6.In order to assign the global optimal ETA for the multiple UAVs the length of generated path and UAVs'f lying velocity should be both taken into account.

As shown in Fig. 6, there are three UAVs in the airspace and each of them has their own velocity constraint[vi,min, vi,max]. For the j th path of UAV-i with path length Li,j,the scope of its ETA ti,jcan be calculated as following.7,32

For UAV-i, Nicandidate f lying routes will be produced,which its ETA could be given by following equation.

In Fig.6 correspondingly all the three UAVs have the same Ni=3. coordinated time of arrival for M UAVs will be certainly contained in the time intersection T of Tiwith i=1,2, ..., M, which will be calculated using Eq. (23).

In some situations, T may be empty. In this way the UAV that has no time intersections with all others will be neglected.Then a new T will be formulated within the left UAVs. If T is not empty there will exist a time tETA∈T and each UAV would have at least one planned f lying route satisfying the ETA tETA, which will make every UAV reach the destination with the same time consumption. It is not hard to f ind that the optimal time is the minimum value between the two red lines shown in Fig. 6. And consequently, the Path-1, Path-2 and Path-3 of UAV-1, UAV-2 and UAV-3 can be selected,respectively.The cost of f lying time is equal to its value,which is denoted in Eq. (24).

Fig. 6 Mechanism of coordinated estimated time of arrival.

(8) Separation maintenance

When generating paths for several UAVs,it is important to check whether two UAVs are getting too close to each other while following their respective paths. Any possible coincidences between any UAV path in space and time should be avoided.

To this purpose, given the planned path of UAV-i and other UAVs' paths,every spline curve point of the f irst trajectory is compared with every point of the others.If the distance di,j(m, n) between the m th waypoint of UAV-i and n th waypoint of UAV-j is smaller than the permitted safe distance dsafethen the arrival times to those specif ied waypoints will be checked to f ind out whether they are too close to constitute a risk.If the time difference ti,j(m,n)is smaller than the permitted safe time tsafe, collisions will take place. Eqs. (25) and (26)give how to compute the values of di,j(m, n) and ti,j(m, n).

The two parameters dsafeand tsafe, which are determined manually, specify the requested safety level. Once the two are violated it will be punished, which introduces more cost of the planned path.Combining Eqs.(25)and(26)the cost calculation can be realized by Eq. (27).

(9) Complete objective function design Regarding the evaluation of one candidate f light path and UAV self-performance above, the complete objective function can be given as following.

Given this objective function, the next challenge is to compute the optimal or sub-optimal solutions, which is the core mission in the next part.

3. Realization of global optimal path planning

To obtain the global optimal path, we applied the standard PSO algorithm to solve the path planning problem described in Section 2.

3.1. Standard PSO

The standard PSO was f irst used to formulate social and cognitive behavior by Kennedy and Eberhart in 1995,33,34which now has wide applications in engineering f ields. PSO is a population based stochastic computational technique that simulates social behaviors of a swarm of birds, f locking bees, and f ish schooling, taking them as many particles f lying through a multi-dimensional searching space and each particle represented as a point. The PSO algorithm could lead to a global optimum,beginning from a randomly initialization with candidate solutions and then an iterative procedure based on the location and velocity renewing in an evolutionary system.

In PSO process the position xiof any particle i will be taken as one of candidate solutions, the number of which is set as S a.k.a. swarm size. In the process, D stands for the dimensions of the problem that needs to be solved.For particle i,there are two crucial parameters,namely particle position xiand particle velocity vi,whose dimensions are also the same as D.They can be written together in the following equation.

As analyzed above the number of articles is called the swarm size denoted as S. In this way a particle swarm can be written as following with its size S.

A cost value is calculated for all the particles in the swarm using specif ied objective function in each iteration. Two kinds of cost values are selected, local best Pi,bestof each particle i and the global best Gbestof all particles, denoted as following.

Obviously there are S local best cost values in the whole particle swarm, but only one global best. They will have the same dimension D.Then the position xiand velocity viof each particle in each dimension is updated by keeping track of the two best positions using the equations in the following.

As shown in Eq. (32) w is inertia weight, which controls the impact of the previous velocity of the particle on its current velocity in this iteration. c1and c2are positive constants,named self-cognition and social knowledge, which stand for the abilities of learning from particle itself and the effects from the whole particle swarm respectively. ζ and η are random numbers between 0 and 1, which ref lects the randomness of PSO for global optimization. And r is a constant factor used to constrain the position updating rate. N is the number of total iterations that the algorithm needs to run. The standard PSO will not stop until the termination criteria are satisf ied. Pseudo code of standard PSO is given in the following.

Fig. 7 Spatial ref ined voting mechanism.

The biggest advantage of standard PSO is its algorithmic simplicity so that it can be implemented by controlling just a few parameters. In addition, the effects of global best particle can be shared with others during the iterations, which makes the convergence of optimal or sub-optimal solutions at a fast speed. However, the disadvantages also exist. First, standard PSO usually fails to adjust its velocity step size for f ine tuning in the search space, which often leads to premature convergence. Second, the self-searching ability in latter iterations degrades sharply, which leads to low accuracy of convergence and in some extreme circumstances solutions cannot be obtained. To overcome the disadvantages of standard PSO,an improved PSO is designed to solve 4D-coordinated path planning for multiple UAVs.

3.2. Proposed method

In this section the Spatial Ref ined Voting Mechanism(SRVM)is described f irst and then the improved PSO is proposed.

(1) Spatial ref ined voting mechanism

In order to overcome the defects of standard PSO in solving multiple UAVs path planning problem,the SRVM is designed for improving PSO. By taking this mechanism into account premature and local optimum could be avoided.

Suppose there are S particles with D dimensions, as shown in Fig. 7. Pjis particle j in the swarm of UAV-a and Pjiis the solution space of the i th dimension.Each dimension stands for a possible waypoint. The key task is to f ind the optimal positions in this solution space. The proposed spatial ref ined voting mechanism includes following steps.

Step 1. Generation. There will be one possible location in every solution space Pjifor every particle j, which is produced by the standard PSO process. For the k th generation the candidate position in Pjiis denoted asAn adaptive cube will be formulated withas its center andas it side length,which is calculated using Eq. (33). Theis adjusted adaptively as current generation changes. Gen is the total number of generations that the algorithm needs to run. The space occupied by the cube is voted with value 1 and will be recorded. To realize coordinated estimated arrival time, for UAV-acandidate paths should be generated for being selected.

Step 2. Voting. After all the generations are f inished, there will be Gen cubes with different centers and side lengths in each solution space Pji. Correspondingly many adaptive subcubes based on the different vote values are formed, depicted asandwith different colors in Fig. 7. All these sub-cubes are sorted with descending sequence based on the different vote values, and the set below is generated.

Step 3. Selection. Because of the need of coordinated estimated time of arrivalcandidate path should be generated.From all the sub-cubes voted in Step 2 the f irstones will be selected and the corresponding centers will be calculated. As the situation for UAV-a in Fig. 7 four centers of sub-cubes are selected asandThey will be seen as temporary positions in the solution space Pjiof particle j for UAV-a.

Step 4. Ref inement. After the selection for UAV-a with Napcandidate positions in each particle dimension Pji, the f inal optimal position La,miof the i th dimension for the UAV-a on the m th candidate path,namely optimal UAV waypoint,could be obtained using following Eq. (35).

Once the four steps are realized, the m th optimal waypoint sequence WPmafor UAV-a can be written as Eq. (36), and the path planning problem of UAV-a is f inally solved.

(2) Implementation of the proposed algorithm

By applying the aforementioned SRVM with standard PSO technique, we can obtain an improved SRVM and solve the multi-UAV path planning problem eff iciently. The f lowchart is shown in Fig. 8 and explicit step are given in the following.

Step 1. Construction. In this step the scene construction is accomplished, including terrain (obstacle) and threat modeling. The f lying space is constrained with some side length in three-dimensional space and the topographic in the airspace are determined. Then the accurate positions and destruction gain of these threat areas are formulated. Mission assignment of all UAVs will be realized based on the constructed scene.

Step 2. Initialization. Suppose NUAVUAVs are considered in path-planning problem and NUAVparticle swarms are generated,each having S particles with dimension D.Each particle is given initial position and velocity randomly in the model at the beginning. The initial position of the farthest UAV to the shared destination is selected as zero point in new coordinate system, which is used to make the coordinate transformation with Eq. (1). All the coordinates in the model should be transformed using Eq. (1) in the construction process, too.Cost values are computed for every particle in every swarm using Eq. (28).

Step 3.Start iterations.Positions and velocities of each particle in current generation will be updated based on Eq. (32).By these updated values new cost will be calculated using Eq. (28). Local best positions for each particle itself and one global best position are found out and stored.

Fig. 8 Flowchart of path planning for multiple UAVs.

Step 4.Call for the f irst time.Combing Eq.(33)the SRVM is called for the f irst time.An adaptive cube is formulated with Lkjias its center and dkjias its side length.The iterations keep on running until the termination condition is satisf ied.

Step 5. Call for the second time. After all iterations are terminated SRVM is called for the second time using Eqs. (34)and (35). After voting, selection and ref inement procedures in the SRVM are all f inished, waypoint sequences for UAVs,denoted as Eq. (36), are obtained.

Step 6. Output results. Finally, the coordinated ETA can be obtained using Eq. (23) and (24). The velocities of UAV can be calculated according to the path lengths and f light time.

The steps described above are summarized in Fig. 9.

4. Experiment evaluation and comparison

In this section experiments are made to prove the effectiveness of the proposed algorithm. The modeling and algorithm is coded in MATLAB and ran on a server with a 2.8 GHz CPU and 16.0 GB of RAM.

4.1. Scenario 1: general scenario

Two experimental scenarios are set,general scenario(Scenario 1)and complicated scenario(Scenario 2),with more comparable threat areas with different destruction gains in the latter one.For these two scenarios,we also test the cases with either two or four UAVs, both of which are offered three candidate f lying routes to realize a coordinated time of arrival.

In Scenario 1,the f lying space has f ive threat areas with different locations, radiuses and destruction gains. The detailed parameter settings could be found in Table 1.

The parameters of the path-planning problem are listed in Table 2. The f lying space is a cube with side length 100 km.The swarm size is set as 100 and particle size is set as 10.The maximal number of iterations is set as 400. NSand NPare numbers of sample points and candidate paths for each UAV, respectively. In the algorithm constrained searching scope are added. The values of up and down scopes are given as △dUand △dDin the following table.

(1) Two UAVs in the space

Table 3 shows the parameters of the two UAVs in the f lying space, including their starting points, the shared destination,and limits of their f lying velocities and heights.In order to realize CA minimum separations, or safe distance (dsafe), between UAVs is set as 0.02 km.

Fig. 10(a) shows the planned path for two UAVs in Scenario 1. The two UAVs successfully reach the shared destination at the same time from different starting points and avoid all the threats and obstacles in the terrain. The curves generated are smooth enough for UAVs to follow. This is because the maximum turning angles and climbinggliding slopes are constrained at acceptable levels.

As analyzed above the two UAVs will share the same arrival time. In Scenario 1 with S=100 and D=10 the arrival time is tETA=9.46 h and the corresponding velocities are VUAV-1=18.00 km/h and VUAV-2=14.04 km/h,respectively,which both satisfy the requirements of their speed limits in Table 3. Distance between the two UAVs is also shown in in Fig. 10(b), whose y-axis is in the form of logarithmic coordinate. Because they arrive at the destination at the same time,their distance at the last time unit is zero, which is not shown in Fig. 10(b). From the curve we can see that the distance between the two UAVs is always above the red line, i.e. minimal distance for safe operations of two UAVs dsafe,and CA is successfully maintained.

Fig. 9 Flowchart of proposed path planning algorithm.

Table 1 Parameters of threats in Scenario 1.No. Location (km) Radius (km) Gain Threat 1 (15,25) 10 1.3 Threat 2 (45,25) 15 1.0 Threat 3 (55,58) 10 1.2 Threat 4 (70,82) 8 1.5 Threat 5 (81,58) 12 1.6

The cost and length of the generated paths in consecutive iterations are two indexes to evaluate the proposed algorithm.In Scenario 1 with two UAVs,the cost of them increases in the f irst few iterations,which is mainly caused by the characteristic of PSO. But after the 100th iteration, the searching process becomes stable and soon converges to optimal solutions. Path length curves have similar trends as the path costs but they are much smoother.It proves the eff iciency of the algorithm in this paper.

Table 2 Parameters of path-planning problem in Scenario 1.

(2) Four UAVs in the space

To verify the effectiveness of our proposed algorithm for more UAVs in the space,two more UAVs are added at different starting points with the other parameters of threats and algorithm itself unchanged.The four UAVs have following settings. They have different start positions, velocity and f lying height constraints. Details are listed in Table 4.

Fig. 11 shows the results of the four UAVs in the f lying space. Fig. 11(a) depicts the paths of the four UAVs in Scenario 1. They can all avoid threats without entering any of them, based on the covered areas and destruction gains. Our algorithm also ensures f light paths avoid any terrain obstacles.The paths determined are smooth and easy to follow, without any sharp turnings or slopes.

Table 3 Parameters of two UAVs in Scenario 1.Parameter UAV-1 UAV-2 Starting point (km) (3,3,1) (5,75,2)Destination (km) (95,95,1) (95,95,1)Speed limits (km/h) (12,18) (10,15)Height limits (km) (10,17) (10,15)Min separation (km) 0.02 0.02

All UAVs, are able to reach the specif ied destination point with the same time consumption tETA=9.23 h,see Fig.11(b).The velocities of the four UAVs are the follows,all falling into the speed constraints in Table 4.

Fig.11(b)also shows the distance between two UAVs.The distances of each UAV pair are all above the set safe distance dsafe, which is set as dsafe=0.02 km in Table 4.

When there are four UAVs exist in Scenario 1, the length and cost of each UAV's path presents the similar trend,which increase at the beginning and then go down to a stable level after the 150th and 250th generation, respectively. But for UAV-4, its cost increased to a level and then stays stable. It is because of the path property of UAV-4,there are more turnings with big angles and most part of the path is close to the edge of threat areas.

(3) Comparisons with other algorithms in Scenario 1

Table 4 Parameters of four UAVs in Scenario 1.Parameter UAV-1 UAV-2 UAV-3 UAV-4 Starting point (km) (3,3,1) (5,75,2) (10,50,1) (25,5,5)Destination (km) (95,95,1) (95,95,1) (95,95,1) (95,95,1)Speed limits (km/h) (12,18) (10,15) (13,16) (14,19)Height limits (km) (10,17) (10,15) (12,17) (9,14)Min separation (km) 0.02 0.02 0.02 0.02

The bio-inspired Tau theory is a kind of typical 4D path planning method, which was established based on the study of the natural motion patterns of animals (including humans)when they approach a f ixed or moving object for perching or capturing prey.In order to investigate the feasibility and effectiveness of the proposed method in this work, series of experiments are conducted and then the further comparative experimental results with general Tau theory are given in Fig. 12. There are two UAVs in Fig. 12(a). The paths generated by the proposed algorithm could avoid all the obstacles as well as the threats in the airspace. However, some parts of the trajectories given by Tau theory pass through one of threats, which is not desirable and not safe. When there are four UAVs in Scenario 1 as shown in Fig. 12(b), the same situation happens. Although Tau theory could make UAVs f ly away from obstacles in the terrain, they are not able to avoid all the threats in the environment. For the proposed method,no matter how many UAVs there are in the airspace all the paths could avoid all the threats and obstacles,which is better than Tau theory.

Combining with all the curves in Fig.12,Tau theory fails to realize coordination between multiple UAVs, as well as collision free when they are f lying. The proposed algorithm could satisfy all the requirements and generate the optimum path based on all the effecting factors.

A*and AO*are another two algorithms that are widely used in path planning problem.In these two algorithms,f lying distance is the only objective.In this way the shorter the path is the better it is.Path length is an important index for path planning, which is compared and the corresponding results are listed in Tables 5 and 6 in Scenario 1 with different numbers of UAVs.

In Table 5, comparing with Tau theory, A*and AO*algorithms, the proposed method could provide the shortest path for UAV-1. For UAV-2, A*generates the shortest path and Tau theory gives the longest.This is because there is a coordination mechanism and collision avoidance is taken into account in the proposed algorithm, which are oppositely neglected by the other three. In this way, it makes the generated path get longer.

Fig. 10 Planned path with S=100 and D=10 in Scenario 1 with 2 UAVs.

Fig. 11 Planned path with S=100 and D=10 in Scenario 1 with 4 UAVs.

Fig. 12 Path comparisons with Tau theory in Scenario 1.

Table 5 Path length of two UAVs in Scenario 1.No. Path lenth (km)Proposed Tau A* AO*UAV-1 170.2 173.3 184.8 179.5 UAV-2 132.8 145.3 111.2 113.8

Table 6 Path length of four UAVs in Scenario 1.No. Path length (km)Proposed Tau A* AO*UAV-1 166.2 173.3 184.8 179.5 UAV-2 132.6 145.3 111.2 113.8 UAV-3 127.5 132.2 130.8 128.9 UAV-4 147.8 151.4 169.8 158.6

The same situation happens in Table 6. In order to obtain the overall optimum routes for UAV-2,it has to f ly the longest way to arrive the destination in a specif ied time. For other three UAVs, the path lengths of the proposed algorithm are the shortest compared with those of Tau theory, A*and AO*algorithms.

4.2. Scenario 2: complicated scenario

To further prove the proposed algorithm experiments are performed in a more complicated scenario,Scenario 2,with more threat areas and higher destruction gains.Also Scenario 2 covers more space with a bigger radius. The settings of threats,including locations, radii, and destruction gains, are listed in Table 7.

Table 7 Parameters of threats in Scenario 2.No. Location (km) Radius (km) Gain Threat 1 (15,25) 10 1.2 Threat 2 (45,25) 15 1 Threat 3 (50,58) 10 1.1 Threat 4 (65,82) 8 1.3 Threat 5 (81,50) 12 1.4 Threat 6 (25,50) 9 1.5 Threat 7 (40,75) 7 1.9

Table 8 Parameters of algorithm for Scenario 2.Items Values Items Values Swarm size 200 △d U (km) 0.004 Particle size 15 △d D (km) 0.004 Iterations 400 N S 5 Terrain size (km) 100 N P 3

Table 9 Parameters of two UAVs in Scenario 2.Parameters UAV-1 UAV-2 Starting point (km) (3,3,1) (5,75,2)Destination (km) (95,80,1) (95,80,1)Speed limits (km/h) (12,18) (10,15)Height limits (km) (10,17) (12,16)Min separation (km) 0.02 0.02

The swarm size and particle size in Scenario 2 are changed to 200 and 15,respectively,while the other parameters are kept the same as those in Scenario 1, as shown in Table 8.

(1) Two UAVs in the space

First, experiment results with two UAVs in Scenario 2 are presented. The parameters of the two UAVs are listed in Table 9.

Fig.13(a)depicts the planned f lying routes for two different UAVs in Scenario 2. It is obvious to see that even in a more complicated scenario with seven threats, our algorithm can f ind perfect paths for both UAVs without entering threat areas or hitting the obstacles in the space.Also the planned paths are both smooth enough for UAVs to follow without any big turnings or slopes. Especially in Fig. 13(a) most part of the f lying route for UAV-2 is a nearly straight line at the beginning.

In Fig. 13(b) the distances between the two UAVs at any time is bigger that the set safe distance dsafein Table 9, represented by the red line. The estimated arrival time for the two UAVs is tETA=8.53 h.The f lying velocities of the two UAVs are VUAV-1=17.99 km/h and VUAV-2=15.00 km/h.

Normalized path cost and path length are still important in this simulation.The path costs of all UAVs converge after 270 iterations. The differences of the cost values of UAV-1 and UAV-2 are big, one is half of the other. This is because the two UAVs are in different positions to the threats and have different path lengths.The path lengths shows the same trends and converge after 270 iterations as well.

(2) Four UAVs in the space

We also tested Scenario 2 with four UAVs.Table 10 lists all parameters needed for the UAVs, including start positions,destination, speed limits and relative f lying height limits. Minimum separation between the four UAVs are the same as 0.02 km.

Table 10 Parameters of four UAVs in Scenario 2.Parameters UAV-1 UAV-2 UAV-3 UAV-4 Starting point (km) (3,3,1) (5,75,2) (10,50,1) (25,5,5)Destination (km) (95,80,1) (95,80,1) (95,80,1) (95,80,1)Speed limits (km/h) (12,18) (10,15) (13,16) (14,19)Height limits (km) (10,17) (12,16) (12,17) (9,14)Min separation (km) 0.02 0.02 0.02 0.02

Fig. 13 Planned path with S=200 and D=15 in Scenario 2 with 2 UAVs.

Fig. 14 Planned path with S=200 and D=15 in Scenario 2 with 4 UAVs.

The planned f lying path for the four UAVs are shown in Fig. 14(a). All the paths avoid every threat and obstacle successfully and they are smooth enough for UAVs to follow without big turnings and climbing/gliding. Fig. 14(b) shows that there are no collisions between any UAV pairs. The distance between any two UAVs is bigger than the safe value.The four UAVs arrive at the specif ied destination position with the shared time consumption tETA=8.04 h and their corresponding f lying speeds are the follows.

From all the f igures,we could conclude that the UAV path planning problem under 4D conditions can be solved using our proposed algorithm and global optimal solutions can be obtained for each UAV in the space.

The f inal path cost and path length comparisons of the four generated f lying routes proves that the path costs and path lengths converge after about 220 iterations. Comparing the needed iterations for Scenario 1 and 2, it shows that the more complicated Scenario 2 does not need additional iterations to f ind optimal solution. It proves the effectiveness of our proposed algorithm.

(3) Comparisons with other algorithms in Scenario 2

In Scenario 2, the number of threats in the airspace increases to seven. Obviously all the paths generated by the two algorithms as given in Fig. 15 could avoid all the terrain obstacles. But the path of UAV-2 given by Tau theory falls into some areas covered by some threats, which means the probability of discovered by enemies will be high.Also the trajectory of UAV-1 obtained by Tau theory is so close to the edge of threats as in Fig. 15(a) which means the probability of affected by the threats will be higher and it is not desired and not safe. The further the path is from the threats,the better it is.Similarly,in Fig.15(b),most of the four paths given by Tau theory are overlapped,which is caused by the lack of collision avoidance in it. This situation does not exist in our proposed method. All the possible factors are taken into account to make the generated paths perfect.

Fig. 15 Path comparisons with Tau theory in Scenario 2.

Table 11 Path length of two UAVs in Scenario 2.No. Path length (km)Proposed Tau A* AO*UAV-1 153.6 164.4 199.7 168.1 UAV-2 128.0 143.4 123.0 124.7

Table 11 gives the comparisons of path lengths from different algorithms for two UAVs. UAV-1 f lies the shortest distance by following the route given by the proposed algorithm in this paper.Tau theory is better than A*but worse than AO*.For UAV-2,A*is the best,that is because the f lying distance is the only objective of A*and no collision avoidance and time coordination is considered. Although the shortest path is obtained, collision occurs between the two UAVs and could not satisfy the requirement that all the UAVs should arrive the same destination using the same time cost. The differences between A*and AO*is not so obvious and because AO*is derived from A*. The path given by Tau theory is the longest which is determined by the property of itself.

If there are four UAVs as given in Table 12, there is no doubt that the proposed method generates the shortest path for all the four UAVs. From the tables and f igures above,we can conclude that when the environment gets more complicated and the number of UAVs gets larger, the prosed algorithm generates perfect paths. There are no collisions between UAVs and the shortest f lying distance can be obtained,as well as satisf ies the requirement that all the UAVs have to arrive at the same destination at the same time.

There is an important problem should be addressed here.Accurate trajectory tracking in practical f light in presence of disturbances and control delays is very important.In the algorithm proposed in this paper, by accepting as inputs a path of waypoints and desired velocities, the control input can be updated frequently to accurately track the desired path, while the path planning occurs as a separate process before the f light.In this way,once disturbances or control delays occur,by iterative learning and updating the next waypoint,UAVs are able to resume the predetermined position in the next time sequence. But in some extreme conditions, the disturbances are so strong or there are a lot of control delays, the UAVs would prefer to hover in current position or return to the starting point. In the future we will focus on this f ield.

5. Conclusions

This paper presents a four-dimensional coordinated path planning algorithm for multiple UAVs,sharing a same arrival time for each as well as collision free and obstacle avoidance.A spatial ref ined voting mechanism is designed to overcome the defects of standard PSO in local optimal and slow convergence. Specif ied candidate paths for each UAV are generated for time coordination, which makes all the UAVs arrive at the same destination with the same time consumption.Searching accuracy and velocity of all particles are improved by constructing the distance to the destination. In addition, solution boundary of searching space is constructed based on properties of all threats. The objective function is redesigned by taking the obstacle and threat avoidance, estimated time of arrival(ETA),separation maintenance and UAV self-constraints into account. Experimental results demonstrate that the proposed method is capable of generating higher quality paths eff iciently for multiple UAVs in 4D space.

Acknowledgements

The authors thank the anonymous reviewers for their critical and constructive review of the manuscript. This study was co-supported by China Scholarship Council (No.201604000003), the National Natural Science Foundation of China (Nos. U1433203, U1533119 and L142200032), and the Foundation for Innovative Research Groups of the National Natural Science Foundation of China (No. 61221061).