A New Method of Solving UAV Trajectory Planning Under Obstacles and Multi-constraint

Multi-constraint trajectory planning for unmanned aerial vehicles (UAVs) has been widely used in military and civil fields. The existing path planning methods, such as swarm intelligence algorithm and graph-based algorithm, cannot incorporate the flying time and UAV kinematic model into evolution. To overcome such disadvantage, a method of solving trajectory planning under obstacles and multi-constraint is investigated in this paper. Firstly, the flying time is discretized as a certain number of Chebyshev points which are the optimized moments of control variable, and they can reduce the computational burden. The process of solution is divided into multi-phase, i.e., two points as a phase to generate the trajectory. Then, angular velocity is taken as control variable, and function of angular velocity is solved by cubic spline interpolation. Besides, functions of angle and position are obtained by integration. The results are substituted into the model consisted by particle swarm optimization (PSO) and the UAV kinematic model to optimize. On this basis, the angular velocity, angle and position are calculated according to the allocated moments. Finally, Monte-Carlo simulation and comparison with existed method are carried out in obstacle environment. All the results illustrate that the multi-phase method can calculate the kinematic parameters of UAV accurately and plan smooth trajectories. Meanwhile, the proposed method is easier to meet the complicated constraints than the single-phase method. In addition, the dimensionality of solution is also enriched effectively.


I. INTRODUCTION
Unmanned system is becoming more common as it can complete complicated missions without human intervention. Unmanned aerial vehicle (UAV) is an aircraft without human pilot onboard. With the development of technology, UAV has been widely applied to both military and civilian tasks, such as surveillance, reconnaissance, rescue and commercial performance [1]- [3]. As one of the important directions of application, UAV trajectory planning has attracted much attention. Besides, UAV with trajectory planning capability can guarantee its own safety and reduce the requirements and dependence on professional operators. When UAV carries out mission, trajectory planning system can provide route guidance for flight system, which is important for UAV to realize autonomous flight and to enhance the autonomy level.
In general, path or trajectory planning of UAVs can be regarded as an optimization problem which aims to find an optimal or near-optimal solution from the starting position to the destination position under certain criteria and mission constraints. The main challenge is to avoid collisions with environmental obstacles and other members and to provide a capable guidance for the tracking control system. In [4], a newest review on path planning of unmanned robot system was presented and some categories about existing results have been made. For the path planning issue, the most used methods and algorithms include Voronoi diagram algorithm [5], A* algorithm [6], probabilistic roadmaps algorithm [7], rapidly-exploring random trees based algorithm [8], and so on. But the UAV kinematic model and dynamic model are seldom considered, thus they are usually not appropriate for practical situations. The field-based method [9] and fluid based method [10] are another two candidate methods for path planning. But they are easy to trap into local minimum and sometimes no feasible path could be generated when the target and obstacles are near. In the past two years, the machine learning technique was also utilized for UAV trajectory planning [11], [12], but the kinematic model of UAV was also not considered in the calculation process. In addition, all the aforementioned methods can only provide path information other than trajectory information, which means the flying time is usually not involved and the results cannot be tracked directly by control system.
For UAV trajectory planning problem, it is also usually modeled as a nonlinear optimal control (NOC) problem with multiple state and control constraints. Many gradient based numerical methods have been adopted to solve the problem [13], [14]. Generally, these methods are mainly classified into two branches: indirect method and direct method [15]. Both methods attempt to minimize objective function and constraint violations by using discrete approximations. But, there are still some limitations. When the dimension of the problem and the complexity are large, the numerical method will have problems such as long running-time, slow convergence rate and even inability to obtain the feasible solution. In [16], a Chebyshev pseudospectral method (GPM) was introduced, and reentry trajectory optimization for hypersonic glide vehicle with flexible initial conditions was addressed. GPM is able to maintain relative faster convergence and accurate solutions during the optimization process. However pseudo-spectral method also inherent the drawbacks of sensitive to initial values and lack of a fast global searching capability.
Besides numerical methods, the swarm intelligence algorithms, such as ant colony optimization (ACO), particle swarm optimization (PSO) and plant growth algorithm, have also been investigated for path planning by researchers [17]- [19]. Among the existing swarm intelligence algorithms, PSO maintains the advantages of simple implementation and being able to access the optimal or sub-optimal solution, thus it is often utilized for path planning and other optimization tasks. In [20]- [21], a comprehensive analysis on the factors that can affect the performance of PSO was carried out and it was pointed out that parameter selection, topology structure and combination with other methods were the main aspects to improve the efficiency and feasibility of PSO. In [22], PSO was adopted to solve optimal scheduling for a swarm robotic system. In [23], the dynamic hybrid PSO combined with the trim-maneuver library technology was proposed for UAV motion planning. In [24], the authors considered the drawback of basic PSO that some high-quality waypoints in previous candidate paths might not be exploited for further evolution, and proposed an idea of separately evaluating and evolving waypoints for two dimensional UAV path planning. Similar idea was extended by [25], where the authors introduced a strategy about optimal path competition strategy between the single waypoint selection and the integrated waypoints selection, and completed a three dimensional UAV path planning demonstration. In order to overcome the defects of local minimum and premature of PSO, the authors in [26] proposed an improved PSO combining with an adaptive decision operator for three dimensional UAV path planning, and results illustrated the advantages of the proposed strategy. Apart from the utilization in UAV path planning, some researchers have also focused on PSO theoretical analysis and improvements.
Taking both advantages of swarm intelligence algorithm and numerical methods, it may provide a meaningful and effective solution for UAV trajectory planning by involving UAV kinematic model with swarm intelligence algorithm. The newest results on robot trajectory planning also enriched the relevant research. In [27], graph-based multi-agent path planner was involved with nonlinear optimization to generate smooth trajectories for nonholonomic mobile robots. The resulting trajectories indicated the superiority of hybrid optimization. In [28], in order to minimize energy consumption and running time while ensuring the smoothness of collaborative welding robot with multiple manipulators, a methodology for optimal trajectory planning was proposed. The trajectory was interpolated in each joint space by means of cubic Bspline and motion time nodes were optimized based on particle swarm optimization (PSO) algorithm. In [29], the authors designed a time-optimal trajectory planning method for the manipulator by searching the optimal path simultaneously, and they proposed a cubic uniform Bspline interpolation algorithm to derive the motion curve expression of the manipulator joint with unknown path points. In [30], an adaptive cuckoo search (ACS) algorithm with high efficiency and excellent stability was proposed to minimize the total motion time under strict dynamic constraints. Depending on the key nodes of the joint angles calculated by a hybrid inverse kinematics algorithm, the continuous quantic B-spline curve algorithm was utilized for planning smooth trajectories of the feeding motion from peer to peer. But, these literatures all focus on trajectory planning for robot system. Besides, the curve fitted by B-spline does not pass through the data points, which will affect the accuracy of data.
Cubic polynomial interpolation trajectory planning is widely used due to its simplicity in calculation, but trajectory planning based on polynomial interpolation is difficult to optimize under traditional methodology due to its high order and lack of convex hull. Using intelligent algorithms to achieve time optimal trajectory planning is a worthwhile research objective to this effect. In [31], the authors introduced a new idea that polynomial interpolation function was combined with improved cuckoo search algorithm to generate time-optimal trajectory for UR robot. In [32]- [34] a hybrid approach of control parametrization and time discretization (CPTD) and PSO was proposed to solve the optimization problem of trajectory planning. However, the three literatures focused on that the time was divided equally, the control input was close to the piecewise constant value, and the kinematic model was two-dimensional. In [35], a coordinated path planning model for a UAAV and an AUV was introduced, and an effective particle swarm optimization (PSO)-based algorithm was also developed to solve the established model. Although the strong coupling relationship between the UAV kinematic model and PSO was considered, only a general framework, and some verifications and analysis of simulation were proposed for trajectory planning problem. Besides, the strategy of generating trajectory was not clarified. Based on the analysis, the contributions of the paper are as follows Firstly, a new method of solving UAV trajectory planning is proposed in this paper, which mainly includes that UAV kinematic model is combined with PSO to generate the trajectory, and the final values of control variable and state variable are obtained by cubic spline interpolation and integration. Secondly, Chebyshev polynomials are introduced to combine with the model consisted by PSO and the UAV kinematic model. The control variables optimized in the model are determined by Chebyshev collocation points. Those collocation points are distributed according to the flying time of UAV, which can reduce the computational burden and match well with the established model. Thirdly, the trajectory planning model is established. In this model, the maneuverability of UAV in different scenes is taken into account, and the terminal constraints, flying length and threat constraints are all included. Besides, comparing with the single-phase method (in this method the task is treated as a single process), we proposed a multi-phase method (two trajectory points as a phase to generate the trajectory) which makes it easier to obtain satisfactory solution. Also, comparing with the CPTD in [32]- [34], the trajectory generated by the proposed multi-phase method is much smoother in theory.
The rest of the paper is mainly described as: the upcoming section introduces the kinematics model of UAV and the constraint index during flight. The next section will introduce the PSO and design of fitness function. Then, the strategy of trajectory planning is introduced in detail, the design of PSO combined with kinematic model is discussed, and the strategy is solved by cubic spline interpolation and integration. Various simulations and comparisons are carried out to verify in a later section. Finally, conclusion is given in the last section.

II. PROBLEM DESCRIPTION AND KINEMATIC MODEL
UAV has been widely applied to both military and civilian tasks, and a feasible trajectory planning system can provide route guidance for flight system effectively. In this section, we introduce the trajectory planning problem for UAV in a target strike mission firstly. Besides, the kinematic model for UAV is described.

A. PROBLEM FORMULATION
The target strike mission specified in this paper is defined as: UAV flies with multi-constraint to search for the target point quickly, and it can avoid the obstacles in the process of flying. Once the distance between UAV and target point is smaller than the threshold, the target will be hit, then multi-phase method is adopted to compute the trajectory, head for the target. According to the above description, the trajectory planning problem can be shown in Fig. 1.
According to the computing analysis, the position and angle information can provide instructions for control system, which can generate feasible trajectory for actual flight.

B. KINEMATIC MODEL OF UAV
In general, the UAV is regarded as a particle in trajectory planning problem, and a particle motion model is used. In some literature, only the kinematic model is considered [36]. This is because that compared to control module, trajectory planning module is an outer loop to provide the important command signal, which requires a fast computation speed. For a rigid body, the motion model which includes a set of nonlinear differential equations with variable coefficients is used. It will cost much time to solve such complicated equations. Thus, in order to further save the computing time, a simplified UAV kinematic model is introduced as follows [35] cos cos cos sin where (1) denotes the UAV kinematic model. , , , x y z V are position and speed of UAV. , qr are trajectory inclination velocity and heading angular velocity respectively,  ,  are trajectory inclination and heading angle. Therefore, the control variable can be defined as [ ] T u q r = , and the state variable is designed as The motion performance parameters of UAV are calculated according to (1) after control variable is determined.

III. PSO AND DESIGN OF FITNESS FUNCTION
In the following, the formula of PSO is firstly introduced in detail. Then, the design of fitness function is discussed and analyzed.

A. DESCRIPTION OF FORMULA
PSO is a population-based optimization algorithm. It starts from random initializations with candidate solutions and could find an optimal or near-optimal solution with particle position and velocity updating [20]. For each particle, the current velocity represents the searching direction and is dynamically updated by its previous value, the personal best position and the global best position. For trajectory planning problem, each particle corresponds to a possible trajectory and is usually consisted by a series of collocation points. In this paper, the number of collocation points is set as n . The number of total particles, namely the swarm size, is defined as N . Then the position and velocity vector for the th i particle can be written as , is the current iteration with T denoting the total iterations.  is the inertia weight for balancing global and local searching, and 12 , cc are acceleration coefficients reflecting the ability of learning from global best particle and personal best particles. b p is personal best value, g p is global best value, and 12 , rr are two random values which is distributed in (0,1) . In order to accelerate the convergence of algorithm, some researchers have adopted linear-varying inertia weight, which is set as [36]

B. CHAOS-BASED PARTICLE INITIALIZATION
As is well known, PSO is initialized with a population of random distributed solutions, and the distribution quality of particle initialization plays a critical role in the searching for optimal solution. Thus, in order to enrich the diversity of particle initialization in the process of UAV trajectory planning, the Logistic map, which is one of the simplest and the most widely used maps, is introduced clearly as follows [20] 1 (1 ) where  is defined as the bifurcation coefficient, p x is set as the th p chaotic variable to calculation. When [3.57, 4]   , the whole system gradually evolves into the chaotic state. When 0 4, (0,1) x  = , the proposed system will produce chaotic signal (0,1) p x  uniformly, which will be utilized for particle initialization.

C. ADAPTIVE PARAMETER ADJUSTMENT
Apart from inertia weight, acceleration coefficients 1 c , 2 c are another two critical parameters. Usually, they indicate the weight of stochastic acceleration terms that pull each particle to local best positions and global best positions, and play important roles in adjusting the convergence speed and searching direction. Because of the randomness which is reflected in the searching process, it is difficult to carry out precise quantitative analysis between coefficient values and iterative times. However, we can analyze and discuss the mapping relationship between them in a qualitative way. For PSO evolution, searching and convergence are the two main objectives during the process. Searching means each particle should always enrich the diversity of the swarm and convergence means the algorithm should finally achieve a same value that is the global optimal solution. Theoretically, it is reasonable to conclude that searching process should predominate at the evolution at the first half stage while convergence process should predominate at the last half stage. Therefore, the proposed acceleration coefficients are adaptively designed as [36] max where T is the total iterative times and t is the .This principle guarantees that the average 1 c and 2 c for the proposed PSO are the same as those for conventional PSO, and makes the simulation comparisons much more fair and well-founded.
It is obvious that 1 c and 2 c are respectively linearly decreasing and linearly increasing, and the sum satisfies c c c c + = + meaning that the ability for particle searching and convergence is constant. For Eqs. (6) ~ (7), it is satisfied that 12 cc  at the first half stage, thus the searching diversity is strengthened, which would be benefit for fast obtaining global optimal solution and avoiding local minimum. 12 cc  is satisfied at the last half stage and has the benefit of fast convergence to the same global optimal solution. In a word, the adaptive coefficients would improve the optimality and rapidity of PSO.

D. CONSTRAINTS
To achieve of goal of cooperative trajectory planning, several constraints should be taken into consideration. They mainly include performance constraint, minimal flying length of trajectory, terminal constraint, mountain terrain constraint, and radar threat constraint. The details are given in the following. In this paper, the collision avoidance is reflected by the mountain constraint and radar constraint. When the trajectory points pass through mountain or radar, the fitness function value will be given a large penalty value. Therefore, the points passing through the mountain or radar should be eliminated.

1) PERFORMANCE CONSTRAINT
Maneuverability is an important index to reflect the ability of position and motion direction for a vehicle. The constraints on the maneuverability of vehicle must be met, or the obtained trajectory is unfeasible and useless. The goal for UAV trajectory planning is to find a feasible and optimal trajectory to the target under complex environment and state constraints. Performance constraints mean that the performance of trajectory planning should meet engineering requirements. Generally, the performance constraints mainly include the position constraint, angle constraint and angular velocity constraint, which are formulated as min max The corresponding fitness function can be expressed as

2) MINIMAL FLYING LENGTH OF TRAGECTORY
For flying missions, shorter trajectory is always preferred than longer trajectory length, because shorter trajectories usually result in less fuel consumption and can reduce the probability of encountering an unexpected threat, which is given as follows where n represents the number of total collocation points in this paper, mainly including the starting waypoint , target waypoint and the designed waypoints ( , , )

3) TERMINAL CONSTRAINT
As is well known, the purpose of UAV trajectory planning is to plan a trajectory, reach the destination, and avoid the obstacles successfully in the process of forward. On this basis, the trajectory of calculation can appropriately provide guidance and instruction for actual situation. Thus, in order to achieve the desired effect of flight, and get the feasible trajectory, UAV should hit the target at the end of the moment. When the distance between UAV and target is less than a small value, the target is considered to have been attacked. It is expressed as follows 0 For UAV safe flying, the planned trajectory must avoid all the mountains in 3-D environment. In common applications, accurate and updatable terrain maps and terrain constraints can be known in advance. In this paper, we suppose mountains as the main terrain feature and UAV cannot fly through mountains. The simplified model of mountains is descripted as [25]

5) RADAR THREAT COST
For a flying UAV under penetration condition, it may face high risk threat. In this paper, the radar is regard as the risk threat. The mathematical model of the threat is set as a geometric sphere. The simplified mathematical model for a radar threat is introduced as The corresponding fitness function can be expressed as: x y z is set as the position coordinate of the th i collocation point. ( is set as the cost value of the th k radar corresponding to th i collocation point, and the specific value is determined by the simulation. ik B is the cost value after comparison and judgement. R ik d is the distance between the collocation point and the radar center, and R f is the total cost of radar constraint.

E. OBJECTIVE
To establish clearly an optimization problem, the objective for UAV trajectory planning should also be determined and designed. In this paper, length of trajectory, performance parameter and collision avoidance are considered in fitness function. The specific objective function about trajectory planning can be written as follows  f is the radar detection cost of UAV in the process of flight. In this paper, we aim to find an optimal or near optimal trajectory under performance constraint, terminal constraint, mountain constraint and radar constraint. In order to meet these four constraints, the penalty terms are added in the objective, which are respectively denoted by When the constraint is satisfied, the corresponding penalty term is set as 0. When any constraint is not satisfied, the corresponding term will be set a penalty value. Too large penalty value will make the length term L f meaningless. Too small penalty value will make the four constraints meaningless. Thus in this paper, the penalty values are set as constants with the same magnitude but smaller than the trajectory length. In the simulation, the penalty value for are set as 30.

IV. SOLVING STRATEGY OF TRAJECTORY PLANNING
In order to obtain efficient trajectory of UAV, a trajectory planning strategy which considers the characteristic and performance of the established model must be developed.
In this section, we introduce the distribution of collocation points firstly. Then we describe the model consisted by PSO and UAV kinematic model. Besides, cubic spline interpolation and the method based on integral and cubic spline interpolation are given in Sections IV-C and IV-D respectively. Finally, we propose the detailed process of the proposed method.

A. DISTRIBUTION OF COLLOCATION POINTS
In order to reduce the burden of calculation, the flying time between the initial moment of UAV (denoted as 0 t ) and the moment of end is discretized as a certain number of Chebyshev points, and they are the moments where the control variable is optimized. The moments of Chebyshev points are calculated by the following formula where the number of collocation points is set as n and the two endpoints, i.e., 0 t and end t are also included, m z is the moment of ( 1)th m + collocation point, and the Chebyshev points are dense at both ends and sparse in the middle. This feature is just suitable for meeting the terminal constraints easily that there are more control variables to be operated at the end of phase [35]. As [ 1,1]

FIGURE 2. Distribution of Chebyshev points
In the implementation, the selection of the number of trajectory point n is very important. While increasing n will exponentially increase the computational burden, decreasing n will cause the loss of accuracy. After analysis and comparison, we set 22 n = , which can guarantee trajectory feasibility and algorithm efficiency.

B. KINEMATIC MODEL COMBINED WITH PSO
In many previous studies, each particle in PSO algorithm is a collection of discrete points, which presents a possible trajectory of UAV. In this paper, as the kinematic model is considered, each particle of the algorithm represents a set of control variables, and the corresponding trajectory can be obtained by solving the kinematic equations. The scale of particle swarm is set as N , and the number of collocation points is defined as n in 3-D environment. The formulas of updating solutions are presented as 11 22 ( where ()  ( 1) i ut + goes out of range, it will be set to the closest threshold, which is used to ensure the normal operation of algorithm.

C. CUBIC SPLINE INTERPOLATION
The concept of spline is originated from conscripting technique of using a flexible strip known as spline to draw the smooth curve through a set of points. To achieve more smoothness and less rate of error in the curve, the cubic spline interpolation method is adopted which is the most common and useful version in the field of engineering and science [37].
, as the cubic spline function, it should meet the following conditions.
In order to determine all undetermined coefficients in () sx , we need to find other two conditions to calculate.
The most common constraint which is used widely is the so-called natural boundary conditions, that is On this basis, combining (23), (24), (25), with (26), we can reconstruct () sx that can be determined by solving linear equations uniquely. The important thing should be noted is that the cubic spline interpolation has been shown to have good convergence and fine smoothness, and the algorithm commonly used to calculate approximately the function in the neighborhood of its maximum. In order to make the effect more clear, the original cross-correlation sampling data are shown in Fig.3 and Fig.4, we can see clearly the distribution about the data of sample points and the result by using cubic spline interpolation.

D. SOLUTION OF CUBIC SPLINE COMBINED WITH INTEGRATION
In this paper, the model consisted by PSO and kinematic model for UAV is used to optimize the trajectory planning problem. In the progress of optimizing the problem, cubic spline interpolation and algebraic integration are used to solve and calculate. The detailed steps are as follows where , kk qi ri FF ( 1,..., 1) kn =− are defined as the function of trajectory inclination velocity and heading angle velocity with time in the th k phase respectively. Coefficients of the function are obtained by cubic spline interpolation, on this basis, the method of constructor function is adopted to establish the specific function, such as the function of trajectory inclination velocity is introduced as follows This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/ACCESS.2021.3132650, IEEE Access VOLUME XX, 2017 9 STEP 3: According to the principle and theory of physics, the angular velocity is integrated to get the angle. After the function of angular velocity is obtained, the function of angle with time are calculated by integration, and they are expressed as follows 1, 2,...,  Because the function of positional derivative is complex to calculate, In order to overcome the problem, the value of positional derivative in each collocation point is calculated firstly. Based on these discrete points, the function of positional derivative is obtained by cubic spline interpolation, and taking the function of x as an example, it is introduced as where 0 1 2 3 , , , b b b b are the polynomial coefficients. STEP 5: Using the algebraic integration again to get the expression of the positional function as follows Taking the function expression of x as an example, and it is as follows 4 3 2 0 1 2 3 0 where 0 c is determined by the initial numerical value of the position in the th k phase.  , and swarm initialization by (5).
STEP 3: Angular velocity is set as the control variable, the function of angular velocity is solved by cubic spline interpolation, and the functions of position and angle are obtained by integration. Construct the fitness function for UAV, and carry out the algorithm for one time.
STEP 4: Calculate the fitness function, figure out the best position and swarm best position. STEP 5: Update parameters by given strategies, including acceleration coefficients swarm initialization, and inertia weight. points. Finish the evolution and plot the optimal trajectory for trajectory planning.

V. SIMULATION ANALYSIS
In this section, a variety of numerical simulations are carried out to demonstrate the feasibility and effectiveness of the proposed method in detail. The algorithm is implemented in MATLAB environment and is run on a PC with 2.1GHz CPU, 8.00GB of RAM and 64-bit operating system.

A. PARAMETER SETTING
In the simulation scenario, UAV flies from the starting point to the target point under the given battlefield environment. The environment parameters of radars and mountains are shown in Table 1 Table 2.   Fig .5 shows the fitness value evaluations and 3-D trajectory generated by the proposed method. The fitness value is measured by (18) and a penalty term is added when UAV conflicts with obstacles or other parameters surpass the ranges of threshold. Fig. 5(a) shows the evolution process for fitness value, which is vital to evaluate the proposed method performance, and the fitness value achieves convergence after about 50 times of iterations. Figs. 5(b) and (c) are the planned trajectories, it's easy to see that the strategy can guarantee feasible trajectory. After calculation, the flight distance of UAV is about 264.18m. Besides, the distance between the end point of the trajectory and the target point is 0.14m, it is smaller than f d , thus the results meet the preset requirements and verify the feasibility of the algorithm.   Fig. 6 introduces the information of angular velocity. In this paper, angular velocity is regarded as the control variable, which is optimized by the proposed method. Figs. 6(a) and (b) are the trajectory inclination velocity and heading angular velocity respectively. The black asterisks indicate the collocation points, the cubic spline interpolation represent the reference values, and the two straight lines are the preset boundaries of angular velocity. One can see that the curve of cubic spline interpolation is smooth and can provide an effective reference for the flight of UAV. The numerical values of angular velocity are also within the preset ranges, meaning that they can meet the given constraints. Thus, the proposed method can guarantee that optimized angular velocity can be used as the reference of control variable.  Beside angular velocity, the states of angle are also provided and shown in Fig. 7. The information of angle can reflect the flight directions of UAV in 3-D environment. Fig. 7(a) is the curve of trajectory inclination, and Fig. 7(b) is the curve of heading angle. It can be seen that each angle satisfies the boundary conditions which are expressed by two straight lines. Meanwhile, we can also see that the curve of cubic spline interpolation is consistent with the curve of discrete This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/ACCESS.2021.3132650, IEEE Access VOLUME XX, 2017 9 points, which indicates the discrete collocation points can offer good guidance.
The above results demonstrate that the proposed multi-phase method of solving trajectory planning is effective in generating optimal trajectory for UAV, and the solution can meet all of the constraints proposed in the trajectory planning model.  To further verify the feasibility of the proposed method, different case is selected for simulation; the coordinates of starting point and target point are changed. On this basis, UAV can strike different target from different starting point.
The coordinates of starting point and target point are shown in Table 3. Herein, we give the related results and show them in Fig. 8. Fig. 8(a) is the fitness value convergence curve, it can be seen that the fitness value decreases from 730 to 223.25 and becomes convergent after about 40 times of iterations. Figs. 8(b) and (c) are the planned trajectory in 3-D environment. It can be found that the distance from starting point to target point is about 223.25m, besides, UAV can plan effective trajectory and the results can satisfy all the constraints in the mathematical model. Thus, the feasibility of the proposed multi-phase method is verified.

C. MONTE-CARLO SIMULATION IN 3-D ENVIRONMENT
Apart from the feasibility, effectiveness and success rate of trajectory are also important factors in evaluating the performance. Due to the randomness of PSO, it is not enough to evaluate the performance by running only one time. In order to better show the advantages of the proposed method, we also carry out Monte-Carlo simulation. In the following, the Monte-Carlo simulation times are set as 20 and the related results are shown from Figs. 9-11. As a comprehensive indicator for solution optimality, the fitness values for UAV are firstly calculated in Fig.  9(a). From the result, one can see that curves of fitness function converge to a same value after 150 times of iterations, which means that almost the same final value is obtained. Figs. 9(b) and (c) indicate the trajectories in 3-D environment. The starting point, target point, flight speed and other parameters are the same as Fig. 5(b). It is obvious that the planned trajectories are effective and smooth, which illustrates that UAV can reach the target point accurately and avoid obstacles successfully in the 20 times of simulations. Fig. 10 shows the results for trajectory inclination velocity and heading angular velocity. Fig. 10 (a) is the curve of trajectory inclination velocity after Monte-Carlo simulation, and Fig. 10(b) is the curve of heading angular velocity. In Figs. 10(a) and (b), the discrete points represent control variable at the collocation points, the cubic spline interpolations represent the reference command for the actual flight of UAV. One can see that the discrete points are in agreement with the curves of cubic spline interpolation, and all of the values are within the ranges of constraints. Thus the proposed strategy can provide effective reference for the UAV system, and all the optimized solutions are feasible. To make a better understanding, the trajectory inclination and heading angle are presented in Fig. 11. Firstly, it can be seen that the values of discrete points are consistent with the values of cubic spline interpolation, which illustrates that the proposed multi-phase method can also provide effective guidance for the angles. Besides, the value of each angle is within the two straight lines, showing that the results meet the constraint range.
To better indicate the stability and performance of the proposed method, the runtime and objective function value are shown in Fig. 12. After calculation the mean of running time is 88.1575s, and the mean of objective value is 254.2834m. Besides, the values of standard deviation are 5.68518 and 0.141842, respectively. By analyzing these data, we can get a conclusion that the proposed multiphase method is feasible and stable. It is to be noted that symbolic variables are called in our Matlab program, which is time-consuming. When the symbolic variables are removed, the running time would be greatly reduced. The above results of Monte-Carlo demonstrate that UAV can reach the target successfully, and the planned trajectories can meet all of the constraints proposed in the mathematical model, which ensures the effectiveness and feasibility of the proposed method.

D. COMPARISON WITH CPTD
To evaluate our proposed method, we adopt two different methods to make the simulation. One is the method we proposed which has been introduced, and the other is the method in literatures [32]- [34] (CPTD). The related results of comparison are shown in Figs. 13-14. Figs. 13(a)-13(b) are the trajectory planning results by CPTD and proposed multi-phase method respectively, where the first figure is the evolution process for fitness value. It is straightforward that the fitness value of proposed method converges faster than that of CPTD, thus the proposed method maintains faster convergence than CPTD. Fig. 13(b) shows the planned trajectory in two dimensional views. It is clear that flying trajectory of the proposed method is smoother than that of CPTD. To investigate the reason, the detailed information of angular velocity and angle by two methods are presented in Fig. 14. It can be seen from Fig. 14(a) that the time is divided equally, the control input of CPTD is close to the piecewise constant value. However, according to the method we proposed, the flying time is discretized as a certain number of Chebyshev points, and the control input is a curve. Comparing with the constant input, the control input in our research is closer to the real scene. Besides, in Fig. 14(b) the connecting line of the angle generated by CPTD is a broken line, which results in the trajectory not smooth. But, we can observe that the angle value in our research is close to a curve. Therefore, a much smoother trajectory can be generated by using our method.

E. RESULTS AND COMPARISONS IN 2-D ENVIRONMENT
In order to highlight the effect of the proposed method in complex environment, the environment of trajectory planning is changed to 2-D, namely UAV flies with a constant height, and the circles are adopted as the obstacles. The center coordinates and radius of the obstacles are shown in Table 4, the given parameters for control variable and state variable are the same with those in 3-D environment. In this section, multi-phase method is used to obtain the results of 2-D trajectory planning and the results are shown in Figs. [15][16] Fig. 15(a) is the convergence curve of fitness function, as can be seen from the figure, the fitness value is about 225.62 after about 70 times of iterations. Fig. 15(b) shows the planned trajectory. In the figure, the starting point is set as (0,100)m, and the target point is set as (0,200)m. It is obvious that the trajectory is smooth and effective. UAV reaches the target point and avoids the obstacles successfully in the process of flying. After calculation, the distance between the target point and the end position is 0.067m which is smaller than 1m. In Fig. 16, heading angular velocity and the heading angle are introduced in detail. One can see all the values are within the preset range and the curves are smooth. Because UAV is assumed to fly with a constant height in 2-D environment, so the trajectory inclination and the velocity is not considered.    Fig. 19(a), one can see that all of the fitness values can reach the convergent values after about 20 times of simulations, and the final fitness values are between 235m-250m, the difference of values is due to the randomness of PSO. After calculation, the fitness values are stable in the end, which demonstrates that the multiphase method can obtain the feasible solution in each simulation. Fig. 19(b)  process of flying successfully. To further investigate the effectiveness of the multi-phase method, the heading angular velocity and heading angle are also presented in Fig. 20. Clearly, the constraints of angle and angular velocity are satisfied, and discrete points are in agreement with the curves of cubic spline interpolation. Thus, the results of Monte-Carlo simulation demonstrate that UAV can reach the target successfully, and the planned trajectory can meet all of the constraints. The effectiveness of the proposed multi-phase method in 2-D environment is verified.

F. COMPARISON WITH SINGLE-PHASE METHOD
In order to verify the advantage of the proposed multiphase method, the single-phase method (in this method the process of solving is treated as a single process) is proposed for comparison and verification. The difference between the multi-phase method and single-phase method is that the single-phase method uses curve fitting and integral to solve the problem of trajectory planning, and all of the collocation points are regarded as a whole to calculate. The related results of the single-phase method are shown in Figs. 21-22. In order to achieve fair comparison, the flight and environmental parameters of the single-phase method are the same as the multi-phase method. In Fig. 21(a), one can see that the fitness value does not converge after about 150 times of iterations, which indicates the solution may not be feasible. Fig. 21(b) shows the planned trajectory, we can observe that UAV does not hit the target and deviates from the target greatly under the single-phase method.
Compared to Fig. 15(b), it's obvious that the multiphase can guarantee that UAV obtains feasible trajectory, but the single-phase method is failed. Besides, compared to Fig. 15 (a) it is clear that the fitness value reaches the feasible value after about 70 times of iterations in multiphase method. However, the fitness value of single-phase method does not reach the feasible solution in Fig. 21(a).
This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/ACCESS.2021.3132650, IEEE Access VOLUME XX, 2017 9 The results of simulation mean that UAV may not finish the task under the single-phase method. To further investigate the reasons for the failure of the single-phase method, the detailed information of angular velocity and angle is presented in Fig. 22. Fig. 22(a) introduces the information of heading angular velocity, the discrete points are the calculated control variables at collocation points, and the curve fitting represent reference values of the actual flight for UAV. In Fig. 22(a), it's easy to see that the fitted curve is not consistent with the curve of discrete points, which shows that there are some errors between the theoretical calculated values and the reference values. The reason is that fitted curve only reflects entire change trend of all the collocation points. Compared to Fig.  16(a), we can also conclude that the results of the multiphase method are better than the single-phase method.
Besides, the results of Monte-Carlo simulation for the single-phase method are shown in Fig. 23. It is obvious that the fitness values are different in the end, and most of the values are inefficient or fall into local optimum, and UAV cannot hit the target point as expected. Thus the planned trajectories are ineffective, and no one is successful. On the contrary, Fig. 16 shows the feasible results with the multi-phase method. Therefore, compared to the single-phase method, the proposed multi-phase method decomposes the complicated constraints into many subparts, i.e., the process of solving the trajectory planning problem is divided into multi-phase, which makes it easier to obtain satisfactory solution. In a word, the above results demonstrate and verify the superiority of the proposed multi-phase method.

VI. CONCULSION
In this paper, we proposed a new method of solving UAV trajectory under obstacles and multi-constraint. The PSO, UAV kinematic model, Chebyshev points, cubic spline interpolation, and multi-segment strategy are respectively introduced and a smooth and feasible can be obtained. The detailed contributions are as follows (1) To solve the established trajectory planning model, a PSO-based algorithm is developed to generate the feasible trajectories. Collocation points are set to reduce the computation load of search and improve the solution quality, the collocation points are distributed by moment.
This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/ACCESS.2021.3132650, IEEE Access VOLUME XX, 2017 9 Cubic spline interpolation and integration are adopted to acquire the functions of control variable and state variable. By proposing the multi-phase method extending the traditional path planning based on PSO with only position information to four-dimensional trajectory planning with flying time and three-dimensional (3-D) position.
(2) In the simulation studies, the superiority of the proposed multi-phase method is verified by comparisons. Comparing with single-phase, the proposed multi-phase method can better address the complicated constraints and result is very close to the theoretical optimal solution. Comparing with CPTD the multi-phase method can obtain more accurate and closer to the actual trajectory.
In the future, we will continue to research on the issue, the dynamic model of quadrotor UAV will be established, cubic spline interpolation and integration is also adopted to generate the trajectory. Besides, other applications of the UAV system need to be discovered to expand its military and civil use.