Trajectory optimization of multiple quad-rotor UAVs in collaborative assembling task

Chen Yongbo,Yu Jianqiao,Mei Yuesong,Zhang Siyu,Ai Xiaolin,Jia Zhenyue

School of Aerospace Engineering,Beijing Institute of Technology,Beijing 100081,China

1.Introduction

Unmanned aerial vehicles(UAVs)have become increasingly attractive for missions in which the human presence is dangerous or dif ficult.Among these UAVs,the unmanned quadrotor helicopters have become increasingly popular platforms for the study of the UAVs from many viewpoints,such as the reconnaissance,the communications relay,the individual combat and so on.1The UAVs trajectory optimization that deals with the time evolution of the flight path is a very important part of the UAVs autonomous control system.Many researches about the quad-rotor helicopters focus on the controller design field,but the trajectory optimization process speci fically aiming at the quad-rotor helicopters,is scarce.

In many references,the path planning and trajectory planning are similar.They can be collectively called the trajectory optimization.But in the strict sense,the UAVs trajectory planning process is different from the UAVs path planning process.The path planning is a process in which the UAV finds a threedimensional(3D)space path from the starting point to the destination.The 3D space path is a2static geometry path.It does not include the concept of time.However,the results of the trajectory planning process are the time-varying flying paths.The results include the flying state of the vehicles.Generally speaking,the model and solution algorithm of the trajectory planning problem are more complicated than the ones of the path planning problem.However,many ideas from the path planning algorithms can help to solve the UAVs trajectory planning problem.For many simple scenarios,the simpler path planning algorithms can offer some sketchy and flyable flying results for the UAVs automatic control system quickly and ef ficiently.Both of them are very signi ficant to the UAVs autonomous control system.

Because of the simpler model,the UAVs path planning problem has been solved by many methods,such as A*algorithm,3arti ficial potential field(APF)method,4,5rapidlyexploring random tree(RRT)algorithm,6,7a large number of intelligent optimization algorithms8,9and so on.Recently,the intelligent optimization algorithms have drawn a lot of attention.For example,Duan et al.introduced many novel optimization algorithms to solve this problem without assuming kinematic and dynamic constraints,including the max–min adaptive ant colony optimization approach,10the chaotic predator–prey biogeography-based optimization(CPPBBO)algorithm,11the improved gravitational search algorithm8and the chaotic arti ficial bee colony(ABC)approach.12In order to use these algorithms,the original path planning problem which is an in finite dimensional problem is simpli fied to the finite dimensional optimization problem by the partition of the two-dimensional(2D)planning space.Although these methods can ensure the near-optimality of the path under some given objective functions,the dynamic and kinematic model of the UAVs is entirely ignored.These planning results are always unacceptable for the general UAVs,especially the fixed-wing aircraft.Even for the quad-rotor helicopters,these results will strongly limit the speed scheme of the UAVs.

Because through the trajectory planning algorithm the time-varying state variables can be obtained,the models of the trajectory planning problem need to be closer to the real aircraft model.Obviously,the high- fidelity model will help the control system to get the appropriate control commands that will maintain the actual flying vehicle on the obtained trajectories accurately.But the complicated model signi fies the huge amount of calculation and the large dif ficulty of the planning process.So the suitable model of the planning object is very important for the computing processes and planning results.

Essentially,the trajectory planning problem is a multiconstraints optimal control problem.The most intuitive approach is to use the optimal control theory.Yao and Zhao13put forward the model predictive control(MPC)algorithm to solve the UAV trajectory planning problem in an uncertain environment.In Ref.14,a novel finite horizon suboptimal controller is applied to solve the trajectory planning problems for the approach and landing(A&L)phase of the reusable launch vehicle(RLV).In this work,the model of the vehicle is a threedegree of freedom(3-DOF)longitudinal particle model.In addition to the optimal control theory,there are some other algorithms which can solve this problem.In Ref.15,Zhang et al.applied a planning algorithm based on the inverse dynamics optimization to solve the ground attack trajectory planning problem of the unmanned combat aerial vehicle(UCAV)which is mathematically formulated as a receding horizon optimal control problem(RHC-OCP).In this work,the model of the UAV is described by the kinematic and dmyondaeml.i cI n m R o de f e.l16a,cKc oa rmd iyna gr ta o nd a T f a uhl leb rilo uwsne d 3 t-hDe O dF iff p era e rnt itci a l el evolution-sequential quadratic programming (DE-SQP)method and the particle swarm optimization-sequential quadratic programming(PSO-SQP)method to solve the trajectory planning problem based on the high- fidelity,6-DOF dynamic model with the integration of accurate aerodynamic and propulsion models.In the existing literature,most of these papers focused on the fixed-wing UAV.There are only a small number of scholars researching the trajectory planning problem of the quad-rotor helicopters whose model is closer to the real UAV kinematic and dynamic model.Based on the ce o t m al.m17o u nsleyd e tmh ep B l oey´zeide r qp uo al y dnro omt o iar l U fuA ncV tio mn o adnedl, thC eh daimf fse er d end tiina el flatness method to solve the trajectory planning problem of the quad-rotor UAV.

The above references only show the single UAV path and trajectory planning method.Some multiple UAVs cooperative trajectory planning algorithms are proposed in some references.In the path planning area,Eva et al.18presented a path planner for multiple UAVs(multi-UAVs)based on the multiple coordinated agents co-evolution evolutionary algorithms(MCACEA)for the realistic scenarios.In Ref.19,a path planning method based on the RRT was developed to generate paths for multi-UAVs in real time.In the trajectory planning area,Gu et al.20put forward a virtual motion camou flage(VMC)to solve the cooperative trajectory planning problem of the multi-UAVs,combining with the differential flatness theory,the Gauss pseudospectral method(GPM)and the nonlinear programming.In this work,the model of each UAV is described by the kinematic and dynamic model according to a full-blown 3-DOF particle model.The main dif ficulties of the multi-UAVs trajectory planning problem are the amount of calculation and the cooperative way.Of course,the amount of calculation of the problem increases with the size of the cluster and the complexity of the model.At the same time,the cooperative way is also very important.

The motivation of this paper is to solve the cooperative trajectory optimization problem of multiple quad-rotor unmanned aerial vehicles.Our concern is to find more ef ficient,more realistic and suboptimal trajectories for multiple quadrotor UAVs without making too many simplifying assumptions on the trajectories.The existing references focusing on the similar problems have some shortcomings.The first problem is the lack of the reasonable systematic research about the overall solution frameworks.The other problem is that in order to simplify the problem,there are too many simplifying assumptions on the aircraft models.Faced with these problems,the hierarchic optimization strategy based on the of fline path planning process and online trajectory planning process is put forward to solve this trajectory optimization problem.This hierarchic optimization strategy can improve the solution ef ficiency,enhance the factuality of the result and ensure the realtime performance of the results.

The main content,the obtained results and the contributions of this paper are presented in this paragraph.The hierarchic optimization strategy includes the path planning process and trajectory planning process.In order to make the trajectory planning process more ef ficient and more real-time,the off-line path planning process is introduced to complete the discrete preliminary trajectory points planning work.The dynamic and kinematic constraints of the UAV in this process just include the velocity constraint and normal overload constraint.A novel parallel intelligent optimization algorithm,the central force optimization-genetic algorithm(CFO-GA),which combines the CFO algorithm with the GA is put forward creatively to solve the multiple quad-rotor UAVs path optimization which is a multi-objective problem.The simulation results show that through the CFO-GA algorithm,the better objective function and faster convergence rate can be obtained compared with other algorithms.At the same time,because of the immaturity of the CFO algorithm,the convergence analysis of the CFO algorithm is completed by the stability theory of discrete time linear time-variant system.This proof process is original.Then,by the help of the result points,the trajectory planning process is finished based on the appropriate simpli fied 6-DOF rigid-body dynamic model of the quad-rotor helicopters whose inner-loop attitude is controlled by the full-blown PID controller.This controlled optimization object is unique and original.What’s more,the cubic B-spline parameterization algorithm and the concept of the security time are introduced to compute the optimal control inputs and the optimal time of this optimal control problem.The full-blown SQP algorithms are introduced to compute the final suboptimal trajectories.The simulation results show that the hierarchic optimization strategy algorithm is effective for the multi-UAVs trajectory optimization problem.At last,the discussion and analysis on the real-time performance of the hierarchic optimization strategy are presented around the group number of the waypoints and the equal interval time.The obtained results show that the real-time demand of this system can be guaranteed by some parameters.

2.Problem description,analysis and solution frame

The multi-UAVs trajectory optimization problem widely appears in many conditions which include the surveillance,the search,the rescue missions,the geographic studies,the military and the security applications.First of all,it is very necessary to con firm the details of the textual problem.

2.1.Problem description and analysis

At first,the research objects of this paper are the quad-rotor UAVs.Then,this paper focuses on the multi-UAVs collaborative assembling problem which means that some UAVs fly from their own starting points to the same target point and reach the target point at the same time.The schematic diagram of this task is shown in Fig.1.In the flying process,the multi-UAVs need to avoid the complex terrain and optimize the flight performance of the whole UAV fleet.In this problem,there are several key demands as follows:(1)the planning results are effective to the real dynamic and kinematic constraints of the quad-rotor UAVs;(2)the assembling time intervals among these UAVs need to be minimized;(3)the flight performance of the whole UAV fleet needs to be optimized.

Fig.1 Sketch diagram of multi-UAVs collaborative assembling problem.

2.2.Solution frame

At first,the multiple quad-rotor UAVs collaborative assembling problem is a multi-objects planning problem.Obviously,the computational ef ficiency of the solving algorithm is very important for this problem.To solve the ef ficiency problem,the parallel concept is introduced to compute different individuals.But the collaborations among the UAV fleet are the taboo of the parallel algorithms.The parallel algorithms are more suitable for the UAV fleet whose individuals are independent without connection.So the collaboration information among these quad-rotor UAVs needs to be disposed into relatively independent parts.This work will be finished in the path planning process.

Then,in order to improve the real-time ability of the algorithm,the whole solution is divided into two processes.The first process is the early of fline path planning which is computed by the intelligent optimization algorithms.In this process,the problem is degraded into a path planning problem whose purpose is to obtain a series of optimized trajectory points for the following trajectory planning problem.The offline results can help to separate the whole trajectory into several small trajectory segments.By this way,the huge planning problem can be solved by each small planning problem whose calculation amount and planning dif ficulty are signi ficantly smaller than those of the original problem.At the same time,because of the hierarchic optimization strategy,the adjustment of the distance between each two waypoints helps to control the length of the computing time.In other words,it can help to realize the request of the online planning.In addition,because the small scene for the small trajectory planning problem can improve the proportion of the feasible solution in the de finitional domain of the independent variables,the real-time ability of the small trajectory planning problem is further improved.

Subsequently,the second process is the online trajectory planning.The trajectory planning problem which is an optimal control problem can be solved typically by two approaches,namely,indirect approach and direct approach.The direct methods are to convert the continuous optimal control problem into a nonlinear parameter optimization problem.Generally speaking,the direct methods are easy to solve,due to the avoidance of the two point boundary value problem(TPBVP).So the direct method is applied to this paper.In this paper,the control inputs of the whole system are parameterized by the cubic B-spline whose control points are the independent variable of the nonlinear parameter optimization problem.At the same time,the 6-DOF rigid-body dynamic model of the quad-rotor helicopters with attitude inner-loop controller is used in the optimization process to improve the practicability of the trajectory planning results.

The speci fic design flowchart is shown in Fig.2.

3.Path planning process

The multiple quad-rotor UAVs path planning process is the first step of the whole algorithm.This process needs to obtain the discrete waypoints of the paths for the trajectory planning process.In this section,the whole path planning process,including the environmental modeling,the optimal model creation,the solution algorithm and the simulation result,is finished.

3.1.Environmental modeling

Modeling of the planning space is a key part of the UAV path planning.The way used to describe the obstacles has an effect on the independent variable design,the representation of the path and the search algorithm.The reasonable modeling way can simplify the problem and improve the solution ef ficiency.

The commonest environment modeling way is the digital map.The digital map means the parameterization of the terrain in the flying range of the UAVs.It mainly includes the modeling of the mountains,the hills,the cities and so on.There are many ways to create the digital map.In this paper,the feature point interpolation way is used,9and the main process is as follows.

Firstly,the space rectangular coordinate system is established in the planning space.The coordinates of any point on the ground can be obtained.Secondly,the terrain in the planning space is divided intoNxandNyparts along thexaxis andy-axis.By this way,we can obtainNx×Nyfeature points whose coordinates are （xi1，yi2，z（xi1，yi2））(i1=1,2,...,Nx;i2=1,2,...,Ny),wherez（xi1，yi2）means the altitude coordinates.Then,these feature points are interpolated by the 4-point spline interpolation method,and we can get the digital topographic information bank.At last,by the visualization technology,the three-dimensional digital map is built.Besides the terrain,the threat and bad weather zone also need to be modeled.Some papers such as Ref.21regard them as the‘soft obstacle”.The UAVs can fly into these soft obstacles.But in this paper all of them are regarded as no- fly zone where the trajectories of UAV cannot be allowed.The terrain,the threat and the bad weather zone are collectively referred to as the obstacle zero.All of the scenarios are bounded within a horizontal-plane box of 6500 m×5500 m×1200 m.The whole planning space is shown in Fig.3.

3.2.Optimal model creation

As for the optimization problem,the independent variables design,the objective function and the constraints are the basic elements.The multiple quad-rotor UAVs path planning problem itself is a nonlinear complex constrained optimization problem.The general form of the optimization problem22is:

where x is the independent variable;fis the objective function;Dmeans the feasible region of the independent variable;hk1andgk2are the equality constraints and inequality constraints,respectively;l1andl2mean the number of the constraints;nis the dimension of the independent variable.

The detailed descriptions of the problem will be shown as follows.

3.2.1.Independent variables

The design of the independent variables is the indispensable element of the optimization problem.It directly in fluences the complexity of the model and the solver,so it is very important to de fine the constructed variables properly.The independent variables of the multiple quad-rotor UAVs path planning problem are the coordinate representing way of the waypoints.In the path planning process,the model of the UAVs is reduced to the free particle model.Then,the simple polyline paths generated by the optimization algorithm are composed of the line segments and the characteristic waypoints.At last,the smooth paths and discrete waypoints of the UAVs are obtained by the 3D circular curve.

Fig.3 Whole planning space.

Because this problem is the collaborative assembling problem whose targets are the same,the cylindrical coordinate system centered on the target point T=(Tx,Ty,Tz)is introduced to parameterize the independent variables.The speci fic parametric processes are as follows: firstly,make a straight linelTwhich passes through the target point and parallel to thezaxis;then,set up the cylinders according tolT.The number of the cylinders isN,andNis the trunc oflm/r,ris the minimum radius of the coaxial cylinders(preestablished),andlmis the furthest horizontal distance between the start points Sj(j=1,2,...,M)and target point.So the radiuses arei×r(i=1,2,...,N),whereiis the serial number of the cylinders.The characteristic waypoints Xi,jof the UAVs locate on the surface of the corresponding cylinder.De fine the independent variable x based on the cylindrical coordinate system as

where θi,jmeans the scalar from the target point to the characteristic waypoint of thejth UAV on theith cylinder surface;hi,jis the ordinate value of the characteristic waypoint of thejth UAV on theith cylinder surface;Mis the total number of the UAVs(in this paper,M=3).

Based on the de finition of the independent variable x,the characteristic waypoint of thejth UAV on theith cylinder surface is

Then,the smooth paths of the UAVs are obtained by the 3D inscribed circular curve.The turning radiusrlof these circular curves can be limited by the rated running conditions of the quad-rotor UAV.At last,M×Nwdiscrete waypoints ot,j(t=1,2,...,Nw)are chosen uniformly in these 3D curve paths.And they meet:

3.2.2.Objective function

The objective of this part is to plan the paths for the UAVs from the starting points to the target.Meanwhile,the time difference of the whole fleet is minimized.Obviously,the total time difference among these UAVs is one of the objective functions.To address this problem,there are some references considering the flying time coordination problem,especially in the mission planning problem.The main time adjustment ways include the speed adjustment,the maneuver adjustment,the path length adjustment and so on.Chandler et al.23adjusted the time by changing the velocities of the UAVs.Alighanbari et al.24ensured the time coordination by the circulating flight way.Beard et al.25changed the trajectory length to realize that the UAVs reach the target at the same time.Although they can solve the time coordination problem partly,these ways cannot ensure the optimality of each UAV or the whole UAV fleet.The UAVs waste the flight time and energy in solving this problem.Actually,the coordination of the takeoff time is the best way to solve this problem.We only need to change the takeoff times of the UAVs based on the elapsed flight time from the starting points to the target without considering changing the path.In this way,the minimized flight time and energy are the most important objectives.Because the model of the UAVs in the path planning problem is simpli fied into the 3-DOF free particle model,we restrict the speed scheme of the UAVs to the constant speed.Therefore,the flight time and energy optimization problem is further simpli fied into the shortest route distance problem.What is more,the total length of the UAVs paths is the commonest objective function in the references.The approximate expression of the objective function is shown as

3.2.3.Constraint condition

The constraint conditions of the optimization problem for the common UAV path planning process include the performance constraint and collision avoidance constraint.

Although the model of the path planning process has been greatly simpli fied,this part is still the foundation of the hierarchic optimization strategy.The path planning process cannot totally ignore the dynamic characteristic of the UAVs.The better flight performance can help the following trajectory planning process to obtain a better result.In fact,the performance constraint of the quad-rotor UAVs is much simpler than the one of the fixed-wing UAVs.They can complete the hovering and low speed flight maneuver which is very dif ficult for the fixed-wing UAVs.Originally,the usual turning radius constraint is not suitable for the quad-rotor UAVs.But we think that the straighter path can reduce the burden of the control system and ensure the high ef ficiency of the path.So the steering angles between each two adjacent line segments are introduced to limit the straightness of the path.The feasible scope of the following line segment of a path is shown in Fig.4.The equation of the performance constraint is as

where δi,jis the angle between each two adjacent line segments,and δmaxthe constraint value of the steering angle.

Fig.4 Feasible scope of following line segment of path.

In addition,the collision avoidance constraint is essential for this problem.The planning results of these UAVs cannot pass through the terrain,the threat and the bad weather zone in the planning space.So the collision avoidance constraint of this problem is shown in the following expression:

whereot,j(3)is the ordinate component ofot,j,andzrthe corresponding ordinate of the obstacle zone.

3.2.4.Optimization model

In conclusion,according to the typical optimal model Eq.(1),the nonlinear optimization model of the multiple quad-rotor UAVs path planning problem is

3.3.Optimization algorithms

Although the path planning process is finished of fline,the higher ef ficiency can save the computing resource and total response time.In the real combat environment,the whole simulation time directly affects the operational effectiveness of the system.Considering the above demand,the multiple quadrotor UAVs path planning problem is solved with the parallel CFO-sequential quadratic programming(CFO-SQP)and the parallel CFO-GA algorithms separately.

The idea of the parallel computing is introduced to improve the computational ef ficiency of the algorithm for the multiagent problem.The parallel implementation of the optimization algorithm is developed,as depicted in Fig.5.The independent variable x is split intoNparts,and each part represents the path of one UAV in the whole UAV fleet.Then each part of x can be computed in different computing core.By this way,this problem is solved ef ficiently.But for the parallel optimization problem,the cooperative relation among these UAVs is an inevitable problem.The main relation exists in the computing of the objective function.In Section 3.2.2,the objective function has already been simpli fied due to considering the coordination problem by changing the takeofftimes.

Fig.5 Parallel optimization algorithm.

Therefore,the objective function is divided intoNindependent objective functions.The new objective functions are shown as

wherelj(xj)is the objective function of thejth UAV and xjthe independent variable of thejth UAV path planning problem.

The CFO algorithm is a new intelligent multi-particle optimization method,in which the search algorithm is inspired by the law of gravity.The particles update their positions in the search space according to a rule that each particle attracts every other particle with the virtual gravity force.The virtual mass of each particle is dependent on its objective function.The particles move according to the Newton’s law of motion:

whereGis the gravitation constant;U(ϖ)is the judgement function,and it meetsα and β are the tuning parameters,and they satisfy α＞0 and β＞0;the objective function of themth particle in thekth iteration.

The CFO is an effective and ef ficient deterministic search and optimization algorithm.Its effect has been tested by many test functions in the Ref.26.The convergence rate of the CFO which is based on the parametersG,α and β is both the strength and weakness.The appropriate rate is very dif ficult to grasp.Hence,the CFO algorithm is augmented with an inner-loop local optimizer.The inner-loop local optimizer is to optimize each particlein its small field.The inner-loop local optimizer can improve the regional search ability which reduces the bad in fluence of the convergence rate.The local optimizer is more suitable to be solved by the full-blown optimization algorithm,so SQP and GA are applied to finish this optimizer comparatively.The inner-loop local optimizer acts in the 5%range of the independent variable x.The whole optimization algorithm structure is shown in Fig.6.

Fig.6 Whole optimization algorithm structure.

3.4.Convergence analysis

The CFO algorithm is a novel optimization algorithm which needs more tests.There are only a few studies focusing on the convergence analysis of the CFO.27–30In this section,the new convergence analysis of the CFO is developed.At the same time,some convergence conditions are put forward herein.All particles using the CFO method converge to the optimum solutions or the special points.However,nothing will be said about whether these solutions represent local or global optimums.Without loss of generality,the CFO method is limited in one-dimensional situation and the selection of the particlemis arbitrary,so all related vectorsdegenerate into the scalars

De finition 1(Linear difference equations).LetJ+be a set of the nonnegative integer,the square nonsingular matrix A(·):the linear differenceequation (Eq.(13))and corresponding homogeneous linear equation(Eq.(14))with variable coef ficients are as follows:

Theorem 1.The movement equation of the particle pkmis a firstorder linear difference equation with variable coef ficients.

Proof.According to the expression ofU(·),de fineMm={n|The whole update of the CFO method is rewritten as

whereAm（k）∈R,fm（k）is bounded.So Theorem 1 is proved.

After obtaining the differential form of the intelligent algorithm(like Eq.(19)),there are many references applying the linear system theory which is based on the characteristic equations of the differential equations to solve the convergence analysis problems.27–29Based on the linear system theory,the restriction condition to guarantee the stability of the system is that the spectral radius of the system matrixAm（k）stays in the unit circle.This condition is the necessary and suf ficient condition of the asymptotic stability for the discrete linear time-invariant system.But this conclusion is not suitable for the linear time-variant discrete system.31Xiao and Du32put forward two counter-examples respectively which show two situations.The first one shows that the movement is also unstable when the spectral radius of the system matrixAm（k）stays in the unit circle for the linear time-variant discrete system.Then the second counter-example shows that the movement is still stable when the spectral radius of the system matrixAm（k）stays outside the unit circle for the linear timevariant discrete system.Obviously,the differential forms of the intelligent algorithms are all time-variant.So the conclusions of above Refs.27–29are wrong and circumscribed by the assumption of the fixity coef ficient.

Theorem 2. ∀ f＊(ρ)∈[J+,Rn],the stability degree of the lineardifference equations is equivalent to the stability degree of thegeneral solution of the corresponding homogeneous linear

33

Theorem 3.If there is a kind of matrix norm that satis fies:

where||·||xmeans a certain norm,∀ ρ0 ∈R.Then the trivial solu

tion of the Eq.(19)has the uniform asymptotic stability.33It isonly a suf ficient condition for the uniform asymptotic stability of the solution.

According to Theorems 2 and 3,a suf ficient condition which can ensure that the movement equation of the particlesbased on the CFO method is stable(in other words,each particlewill converge to its own stablePm)is shown as follows:

where λ is the eigenvalue ofAm（k）,∀k0∈R.

The condition is further simpli fied into:

Based on the above suf ficient condition,the particles

converge to the stable valuesIn the Ref.27,one of its conclusions is that all particles using the CFO method converge to a stableP.But we find that this conclusion is wrong because some special circumstances were not taken into consideration.Then,we need to prove our own conclusion.

Theorem 4.Under condition of Eq.(22),{Pm,m=1,2,...,Np}will converge to the optimum solutions(local/global)or the special points.

Proof.Based on Eq.(15)andwe have

Further,substitute Eq.(15)into Eq.(22),and we have

(1)Situation 1

For∀mif it exists:Pn=Pm,you can get that all particles using the CFO method converge to a stable optimum solutionP:

(2)Situation 2

(3)Situation 3

If somemexists:

·（Pn-Pm）=0,you can get that these particles using the CFO method convergeto severalspecialpointswhich are not the local/global optimums.But their accelerations are zero.This situation only exists in rare circumstances.They are de fined as the special points.

So Theorem 4 is proved.

As an example,these three situations are introduced further by Fig.7(3 particles).

The special points are only a smaller proportion of the de finitional domain for most optimization problems.In the overwhelming majority of cases,the{Pm,m=1，2，...，Np}will converge to the optimum solutions(local/global)under condition of Eq.(22).Even if the Situation 3 exists,there are at least two other stablePmwhose objective functions are better than the objective functions of the special points.The final results will rule out these special points in the sorting process.

In conclusion,the convergence of the CFO method can be guaranteed as long assatis fies the Eq.(22).

3.5.Simulation results

In this part,the simulation results of the path planning problem are shown and analyzed.The CFO-GA algorithm and CFO-SQP algorithm are contrasted in the same simulation settings.The simulation settings are shown as follows.

The number of UAVs isM=3;the starting points of the UAVs are:S1=(1500,2000,500)m,S2=(2500,1500,500)m and S3=(2000,1500,500)m;the common target point T=[6000,7000,0]m;the number of the particles in CFONp=20;the maximum number of iterations in CFO is 300;the gravitation constantG=0.01;the tuning parameters α=0.4 andβ =0.5;the accommodation coef ficienth=0.5;the number of the discrete waypointsNw=1300;the minimum radius of the coaxial cylinders which is very important to the time-consumption of the following trajectory planning process will be discussed deeply in Section 5 and is provisionally de fined asr=1000 m,so the total number of the cylindersN=6;the constraint value of the steering angle which is related to the turning radius of the UAVsrland the minimum radius of the coaxial cylindersris provisionally de fined as δmax= π/2 rad;the maximum number of iterations of the inner-loop local optimizer changes from 5 to 30;the other settings of the local optimizer are the defaults of the MATLAB R2011b.

Fig.7 Three possible convergency results of CFO algorithms.

In order to further verify the effectiveness of the proposed algorithms,the other state-of-the-art methods(GA and PSO)are introduced.The same simulation settings are shown as follows:The simulation parameters of the planning scene are all same as the parameters of the CFO-GA algorithm and CFOSQP algorithm;the maximum number of iterations is 300;the population size in each generation is 20;the number of the discrete waypointsNw=1300;the minimum radius of the coaxial cylindersr=1000 m,so the total number of the cylindersN=6;the constraint value of the steering angle is provisionally de fined as δmax= π/2 rad.The different simulation settings are shown as follows:Because the GA is programmed based on the library function of the MATLAB R2011b,the settings of the GA are the defaults of the MATLAB R2011b.The learning factor,the acceleration coef ficient and the inertia weight of the PSO algorithm respectively are 0.5,0.5 and 0.8.34

The simulation results of the parallel CFO-GA method,the parallel CFO-SQP method,the parallel GA method,the parallel PSO method and the parallel CFO method depend on the same parameter settings.The effects of the maximum number of iterations and the maximum number of iterations of the inner-loop local optimizer will be presented in the latter part.So we only present the best situation of the path effect picture whose maximum number of iterations is 300 and the maximum number of iterations of the inner-loop local optimizer is 30(See Fig.8).

Fig.8 shows the planning results of five algorithms.The dotted lines represent the planning results obtained by the parallel PSO algorithm.The star full lines represent the planning results obtained by the parallel GA algorithm.The dot dash lines represent the planning results obtained by the parallel CFO-GA algorithm.The imaginary lines represent the planning results obtained by the parallel CFO algorithm and the full lines without stars represent the planning results obtained by the parallel CFOSQP algorithm.And the black lines,the purple lines and red lines represent the planning results whose starting points are S1,S2and S3respectively.They all keep away from the obstacle zones and reach the same target.It is observed from Fig.8 that the parallel CFO-GA has selected a shorter and smoother path compared with the path found by the parallel GA,PSO,CFO-SQP and CFO.The path obtained by the parallel CFO-GA fly above the slit which is formed by the upper surface of the radar areas.Because this planning space is complex,the optimization algorithm is easy to fall into the local minimum value.The results of the other algorithms are limited in the other slit which presents a distinct local minimum.Therefore,we can see that the parallel CFO-GA algorithm has the stronger global convergence ability than the other algorithms.The objective functions changing with the iterations(1–300)are shown in Fig.9.

Fig.8 Planning results of five algorithms.

From Fig.9,we can see that the traditional GA and PSO algorithms have the poorer performance compared with three CFO-based algorithms.So in the following discussion,we only consider the parallel CFO-GA algorithm,the parallel CFOSQP algorithm and the parallel CFO algorithm.

From the view of the fluctuation of the objective function values,the parallel CFO-GA algorithm is more suitable for the complicated path planning than the parallel CFO-SQP algorithm and the parallel CFO algorithm.At the initial segment(about 1–100 iterations),because of the unsatis fied constraints and penalty function method,some fluctuations appear in the objective function values.They respectively meet the constraints at the 107th(parallel CFO),the 90th(parallel CFO-SQP)and the 15th(parallel CFO-GA)iteration.Therefore,although the optimization range of the inner-loop local minimum problem is small,because of the complexity of the planning space,the local minimum problem also needs some algorithm like the GA whose global convergence is better than the traditional algorithm like the SQP.

From the view of the overall trend of the objective function values,the objective function values converge to their convergence values with the number of iterations increasing at last.At the initial segment(about 1–25 iterations),the convergence speeds of these three algorithms are all rapid.The main reason of this phenomenon is the strong convergence property of the outer loop optimization algorithm(the parallel CFO method).But because the convergence speed of the parallel CFO algorithm largely depends upon the differences between the particles,when the differences between the particles decrease(i.e.|f（pkn）-f（pkm）|αconverges to zero),the convergence speed will greatly slow down.Just then the power of the inner-loop local optimizer is shown.Especially the local GA algorithm still propels the optimization after the initial segment.

In brief,the analyses of the convergence property of these three algorithms further provide that the parallel CFO-GA algorithm has a better performance for this problem than the parallel GA,PSO,CFO and CFO-SQP algorithm.After obtaining the above conclusion,we will further discuss the effect of the maximum number of the iterations of the innerloop local optimizer in the convergence situation of the whole algorithm.The convergence values changing with the maximum number of the iterations(5–30)are shown in Fig.10.

Fig.9 Convergence property of five algorithms.

Fig.10 Convergence values changing with the maximum number of iteration.

From Fig.10,we can see that the convergence values decrease with the maximum number of the iterations of the inner-loop local optimizer.In the beginning,the decreasing velocity is slow.Then,the convergence values converge rapidly to the good solution of this problem after 25 iterations.This shows that the effective inner-loop local optimizer iterations for this problem need to be 25 at least.What is more,the bigger maximum number of the iterations of the inner-loop local optimizer will not improve the results of this problem and will increase the computing time of this problem after 25 iterations.

4.Trajectory planning process

With the help of the hierarchic optimization strategy,the large-scale optimization problem is decomposed and simpli fied into many small trajectory planning problems.Then,the small trajectory planning problems,namely the optimal control problems,are converted into several nonlinear parameter optimization problems by the parameterization process.At last,they can be solved by the mature optimization algorithm quickly enough in the real situation during processing.In this section,the whole trajectory planning process including the flight simulator modeling,planning inputs de finition and parameterization(independent variable de finition),the optimization model creation,the solution algorithm and the simulation result is finished.

4.1.Flight simulator model

In this work,the nonlinear 6-DOF dynamic equations of the quad-rotor UAVs are used to model the UAV motion.The quad-rotor helicopter UAV is an under-actuated,highly nonlinear and strongly coupled system with four fixed pitch angle rotors as shown in Fig.11.35

Fig.11 Quad-rotor helicopter structural diagram.

The realistic and complicated model can better simulate the real dynamical system of the quad-rotor UAVs,but the complicated model increases the amount of calculation of the trajectory planning problem,which is not conducive to realizing the demand of real-time ability.So the appropriately simpli fied model of the UAV is as follows:36

where ψ,θ and φ are the Euler angles between the body-axis coordinate systemand the inertial coordinate systemare the positions of the UAV;muis the mass of the UAV;gis the acceleration of gravity;lis the half length of the quad-rotor helicopter;Kx,KyandKzare the drag coef ficients;Ix,IyandIzare the moments of inertia with respect to the axesXb,YbandZb,respectively;the control inputsuci(i=1,2,3,4)satisfy the following equations:37

wherebis the scaling factor of the thrust generated by the rotor;cis the scaling factor of the propeller torque constant generated by the rotors; ωi（i=1，2，3，4）is the rotational speed of theith propeller.

Above models Eq.(26)are based on the following assumptions:38

(1)The UAV structure is symmetrical and rigid.

(2)The center of mass andObcoincide.

(3)Thrust and drag are proportional to the square of the propellers speed.

(4)Ignore the rotational resistance of each UAV.

(5)Ignore the non-determinacy of the UAV system.

4.2.Planning inputs de finition and parameterization

According to the direct method of the optimal control problem,the planning inputs need to be de fined and parameterized.In Ref.16,the inputs of the trajectory planning process are directly the control inputs of the F-16 control system.Drawn from above,the control inputsuci(i=1,2,3,4)may be chosen for the planning inputs of the quad-rotor UAV trajectory planning.If so,theuci(i=1,2,3,4)can make up the independent variable of the new optimization problem.

But this assumption proved to be wrong in the simulation process.The quad-rotor helicopter is a strong unstable system no matter from the viewpoint of the attitude inner-loop or from the viewpoint of the position outer-loop.It cannot fly stably without the effect of the control system.Obviously,the stability of the attitude inner-loop is the basis of the stability of the whole system.As a double-integral system,a few unreasonable inputs on the system will lead to the divergence and unreasonable results(large attitude angle).If the trajectory planning is finished without the attitude inner-loop control,most values of the independent variables in the de finitional domain are infeasible.The percentage of success is too low to realize the real-time demand.Most of the planning time is wasted in searching the feasible solutions rather than searching the optimal solution.At the same time,the optimization effect will be very poor.The schematic diagram of two situations is shown in Fig.12.

Therefore,the planning inputs need to be re-selected.By the help of the above analysis,the planning inputs need to ensure that the attitude parameters are in the proper range at least.The classical control system of the quad-rotor helicopter UAV is shown in Fig.13,wherexp,ypandzpare the coordinates of the reference trajectory; ψr，θrand φrare the reference attitude angles.In order to realize the stabilization of the attitude angles,the attitude inner-loop PD control is introduced in the planning process.So the planning inputs change into the control inputs ψr（-30°≤ ψr≤ 30°）,θr（-30°≤ θr≤ 30°）, φr（-30°≤ φr≤ 30°） anduc1(0.9mug≤uc1≤1.1mug)of the attitude inner-loop controller.At the same time,the planning object also turns into the nonlinear 6-DOF dynamic equations of the quad-rotor UAVs with the attitude inner-loop PD control.The algorithm of the PD controller is shown as

Fig.12 Schematic diagram of two situations.

whereufollow（t）is the control variable;the tracking errore（t）is de fined ase（t）=Wr（t）-W（t）,withWr（t）the reference output andW（t）the real output;KPandKDare controller gains associated with proportional(P)and derivative(D)actions,respectively.

The control inputs are discretized at a number of equally spaced nodes(control points)between the initial timet0and the inde finite final timetfof the flying process.The values of the control points are limited in[LB,UB],where LB and UB are the upper and lower bound of the control inputs.Then,these control points are parameterized by the cubic B-spline.But as a TPBV optimal control problem,the optimal final timetfis very dif ficult to obtain.In order to obtain it,the security timeTfwhich is bigger than the inde finite final timetfis presented.The discretized and parameterization processes are finished between the initial timet0and the security timeTf.The security timeTfwill obtain an integrated trajectory.The end waypoint whose corresponding time istfis de fined as the closest waypoint to the target in the whole trajectory.So although the control inputs betweent0andTfare used in the following optimization process,only the control inputs betweent0andtfact on the objective function of the trajectory planning problem.In other words,the optimization only occurs betweent0andtf.By this way,the optimal final timetfand control inputs betweent0andtfare obtained and used in the real flying state.The parameterization of control inputs is shown in Fig.14.

Fig.13 Classical control structure for quad-rotor helicopter UAV.

Fig.14 Parameterization of control inputs.

From the above,the optimal control problem has been converted into the parameter optimization problem in this section.It’s worth noting that the planning inputs have been changed into the control inputs ψr，θr, φranduc1of the attitude inner-loop controller.Because ψr，θrand φrare the reference attitudes of the UAV,they need to ensure its continuity.Therefore,the first control point of each small optimal control problem needs to be limited by the final value of the control input of the previous problem.Above all,the total number of the independent variables of the trajectory planning problem except the first one is 4(Tf-t0)/tx,withtxthe equal interval time.Moreover,the total number of the first small trajectory planning problem is 4+4(Tf-t0)/tx.Because this process is solved on-line,the optimization ef ficiency which is greatly in fluenced by the scale of the optimization problem is very important.The value oftxneeds to be relatively big to reduce the number of the independent variables which can improve the optimization ef ficiency.At the same time,the value oftxneeds to be small enough to ensure the availability and optimality of the results.In conclusion,the new structure of the whole optimization problem is shown in Fig.15.

4.3.Optimization model creation and solution

The objective function and constraint conditions which are the basic elements of the optimization problem will be presented in this section.Then,the whole trajectory planning problem of each UAV in the fleet is established.In addition,the solving method is put forward.

Because of the hierarchic optimization strategy,the objective function of the single UAV trajectory planning problem is based on the waypoints ot,j(t=1,2,...,Nw)of the of fline path planning results.The number of the waypoints ot,j(t=1,2,...,Nw)is too large,so we cannot build the small trajectory planning problem in every two waypoints.Therefore,the waypoints are divided intoNGgroups.At the same time,the largescale optimization problem is decomposed and simpli fied intoNGsmall trajectory planning problems.The last waypoint of the prior group is the first point of the later one.The first waypoint and the last waypoint of each group respectively are the starting point and the target point of the small trajectory planning problem.The equation of the grouping is

wheresis the serial number of the group and the serial number of the small trajectory planning problem;round(·)is the round down function;Gs,jis thesth group of the waypoints of thejth UAV.

The objective of this paper is to finish the multiple quadrotor UAVs collaborative assembling problem.So the objective function of the trajectory planning problem also focuses on the minimized length and time difference.With the consideration of the path planning process,the length of the trajectory is guaranteed by the staged target points.So the lengthoptimal problem can be reduced into the problem whose goal is to minimize the distance between the end point and the staged target point.For the distance/time difference-optimal trajectory,based on the weighting method,the objective function of thejth UAV in thesth trajectory planning problem is de fined as

where ωu1and ωu2are the adjustment coef ficients,and ωu1＞ 0, ωu2＞ 0;is the end waypoint of thejth UAV in thesth small problem;mean the initial time and final time of this problem,respectively;is the predicted flight time of thejth UAV in thesth small problem whose purpose is to allocate the flight time among the different small trajectory planning problems and minimize the time difference.It is de fined as

where Φ（Gs，j）is the sum length of the path which is composed of line segments according to the initial and final waypoints ofGs，j.

Fig.15 New structure of whole optimization problem.

The constraint conditions of the optimization problem include the dynamic constraints,the obstacle constraints and the initial/ final conditions.The dynamic constraints have been studied in the above section.It is manifested in the 6-DOF nonlinear differential dynamic equation of the quad-rotor UAV with the attitude inner-loop PD controller and the value range constraints of the planning inputs.Obviously,the UAV cannot fly through the obstacle,so the flight height meetsandzrrespectively mean the UAV flight height and the obstacle height of thejth UAV in thesth trajectory planning problem.The initial/ final conditions of thejth UAV in thesth trajectory planning problem are de fined as follows:

In conclusion,thejth UAV in the sth optimization problem is de fined as

This trajectory problem needs to be optimized on-line in the airborne computer of the UAV,so the mature,simple and ef ficient method must be used in this process.The SQP method which is the most widely-used algorithm is used to solve this problem.In this part,the whole trajectory planning problem is finished by the Microsoft Visual C++V 6.0.The maximum number of allowed iterations is 15,and all other optimization settings imitate the default parameters of the MATLAB R2011b.At the same time,the initial values of the control inputs of θr, φranducirespectively are

4.4.Simulation results

In this section,the simulation results of the same trajectory planning problem are shown and analyzed.The simulation parameters are shown as follows:the mass of the UAVs ismu=0.504 kg;the moments of inertiaIx,IyandIzrespectively are 1.9757 × 10-4kg·m2,1.9757 × 10-4kg·m2and 3.9514 × 10-4kg·m2;the half length of the quad-rotor helicopter UAV isl=0.028 m;the group number isNG=10;the interval between the initialt0and the security timeTfis 90 s;the value of the equal interval time istx=10 s;the rated speed of the rotor-UAVs isv=9 m/s.The simulation results are shown as follows.

Fig.16 shows the trajectory planning results of the collaborative assembling task.The multi-UAVs take off from different starting points(black line:UAV1(S1),purple line:UAV2(S2)and red line:UAV3(S3)),avoid the obstacles and finish the collaborative assembling task successfully.The distances between the terminal positions and the target respectively are 13.9 m,7.4 m and 8.6 m.

Fig.16 Trajectory planning results of collaborative assembling task.

It is observed that the trajectories are not as straight as the paths in the path planning process.The main reason of this phenomenon is that the small trajectory planning problems are short of the terminal attitude constraints and the terminal fight state of the previous small problem will impact the optimization process of the next small problem.Certainly,this is the defect of this algorithm,and it forms the objectives of the future research and works.Then the flight data which include the optimal inputs,the flight velocities in three directions and the attitude angles versus time are shown in Figs.17 and 18.

The above results(see Figs.17 and 18)show that the variation ranges of the optimal inputs,the flight velocities and the attitude angles all satisfy the constraint conditions.At the same time,because the initial values of the control inputs are chosen as the zero vectors or the row vector whose values are allmug,the values of the optimal control inputs keep in the small values.This phenomenon means that the UAVs are in the stable flight state.The total flight time of these three UAVs respectively is 819.76 s(UAV1),761.52 s(UAV2)and 832.69 s(UAV3).Because the rated speed of the rotor-UAVs is 9 m/s and the sum lengths of the whole paths respectively are 7232 m,6835 m and 7228 m,the predicted flight time respectively is 803.6 s(UAV1),759.4 s(UAV2)and 803.1 s(UAV3).It follows that the algorithm ensures the rated flight time to some extent.So in order to ensure that the multirotor-UAVs can reach the target at the same time,the takeoff time of UAV1and UAV2needs to be postponed for 12.93 s and 71.17 s,respectively.

In conclusion,these results prove that the hierarchic optimization strategy algorithm is effective for the multi-UAVs trajectory optimization problem without the consideration of the real-time ability.In this section,we also do not discuss the real-time requirement of the trajectory planning process and we will discuss it in the following section.

5.Discussion and analysis of real-time performance of hierarchic optimization strategy

Fig.17 Variations of optimal inputs.

Fig.18 Variations of flight velocities and attitude angles.

The of fline path planning process is computed in ground station before the takeoff of the UAVs.Then,the results will be transferred and loaded into the airborne memory device which can be called the airborne computers.After the storage of the of fline data,the UAVs are ready to hop off.The optimal time-varying flying states which are the inputs of the real UAV control system are obtained by the online trajectory planning process in the airborne computers.In the real flying process,the UAVs need to fly steadily and plan quickly enough.So the airborne computers need to simultaneously calculate the guidance law and control command.The graphical illustration of the online trajectory planning process is shown in Fig.19.

As you can see,the calculation and implementation of the UAV system are finished in several time units.In every unit,the airborne computer needs to complete the operations of the data loading,the trajectory planning resolving and the data post-processing in due order.Accordingly,the fixed length of the time unitTstepincludes the constant loading time Δtloading,the planning time Δtplanning,and the constant post-processing

time Δtpost-processing (Tstep=Δtloading+Δtplanning+Δtpost-processing, Δtloading+Δtpost-processing=5 s).Of course,for a UAV in one task,the time unitsTstepof the different small trajectory planning problems are different,so we de fine that the value of the time unitTstep(s,j)means the time unit of thejth UAV in thesth trajectory planning problem.Before takeoff,the UAVs wait for at least a time unitTstep(1,j).During this period of time,the airborne computer needs to accomplish the first small trajectory planning process which is from the starting point Sjto the later point(os×round（Nw/NG）+1，j).Then,the UAVs receive the planning results and begin to follow the trajectory until they reach the later points(oNG×round（Nw/NG）+1，j)at time intervalsTflying(1,j),withTflying(1,j)the optimal final timetfof thejth UAV in the first trajectory planning problem.By this way,the planning results need to be obtained before the implementation for the same flight process.So the feasible condition which can ensure the real-time ability is

Therefore,in order to meet the above condition,the coordinated relations between the planning time Δtplanningand the implementation timeTflyingare studied in the following part.The in fluence factors of the planning time Δtplanningmainly include the equal interval time of the control inputstxand the group number of the waypointsNG.However,the main in fluence factor of the implementation timeTflyingis the group number of the waypointsNG.So in the following part,we will discuss the impact degree of these factors by some simulations.

Because the planning time Δtplanningnot only depends upon the complexity level of the optimization problem,but also depends upon the performance of the simulation computer,all these trajectory planning problems are solved by the following computer systems shown in Table 1.

The first in fluence factor is the group number of the waypointsNGwhich directly affects the distance between each two starting waypoints in the adjacent groups,then further affects the implementation timeTflyingand the planning time Δtplanning.In order to separately study the impact ofNG,we keep the value of the equal interval timetx(tx=(Tf-t0)/9)fixed.Accordingly,the discretized and parameterization time between the initial timet0and the security timeTfis de fined as 900/NG.For a collaborative assembling task,the distances between the multi-UAVs terminal positions and the target are the evaluation criteria to judge whether the task is achieved.In this part,the success sign of the simulation is de fined as that the distances between the multi-UAVs terminal positions and the target are less than 100 m.WhenNGis small,the small maximum number of iterations cannot ensure that the distances are small enough.So in order to achieve the collaborative assembling task,the maximum number of iterations is unlimited whenNGchanges from 1 to 9.Even so,because of the complexity of this problem,the completion status of the simulations is also very bad for some situations whoseNGis too small.Then,the real-time performance of the simulation is shown in Table 2 and Fig.20.Where average Δtplanningmeans the average value of the planning time in all small trajectory planning problems;averageTflyingmeans the average value of the flying time in all small trajectory planning results;completion status means whether the simulations reach the success sign or not;∞means there is no successful case;real-time ability means all simulation results satisfy Eq.(22).

In Fig.20,we can see that the average time unitTstepand average flying timeTflyingare both getting smaller with the increase ofNG.WhenNGis too small,the SQP algorithm falls into local minima and cannot obtain a successful simulation result.At the same time,we can realize that the gradients of them are different and these two curves intersect.The intersection of these two curves is very important to judge the realtime performance of the hierarchic optimization strategy.WhenNGis bigger than 6,the real-time performance of the algorithm basically meets the demand of the online system.Or else it cannot meet the online demand de finitely.In Table 2,a special phenomenon needs to be paid attention to.WhenNGis 6,the real-time demand still is not satis fied although the average time unit is smaller than the average flying time.The reason of this phenomenon is that,for different UAVs and different small trajectory planning problem,the time unitTstep(s,j)and the flying timeTflying(s,j)are different and around the average value.

Fig.19 Graphical illustration of online trajectory planning process.

Table 1 Computer system used for experimental test.

The second in fluence factor is the equal interval timetxwhich directly affects the independent variables of the optimization problem,thus affects the planning time Δtplanning.In order to study the in fluence oftxseparately,the group number of the waypointsNGand the maximum number of allowed iterations are de fined as 10 and 15 respectively.Then,the value of the control inputstxchanges from(Tf-t0)/9 to(Tf-t0)/2.The real-time performance of the simulation is shown in Table 3.

After the corrections of the constant loading time Δtloadingand the constant post-processing time Δtpost-processing,the curves of the planning time Δtplanningand the time unitTstepwith the changes oftxare shown in Fig.21.

With the decrease oftx,the dimensions of the independent variables become larger and larger.Of course,the planning time and time unit also become longer and longer.For this problem,the relationship between the planning time andtxis in the linearity range(from(Tf-t0)/2 to(Tf-t0)/9).Clearly,all of these results can meet the real-time requirement.But thelarger dimensions of the independent variables can expand the resolution space,which is favorable to solve the more complex planning space.Sotxis set to 10 s in Section 4.

Table 2 Real-time performance changes with NG.

Fig.20 Variation curves of average planning time,average flying time and average time unit.

Table 3 Real-time performance changes with tx.

Fig.21 Changing curves of time unit and planning time.

In short,the real-time demand of this system can be guaranteed for this hardware system when the group number of the waypointsNGis bigger than 6 and the value of the control inputstxis bigger than(Tf-t0)/9.For different computer hardware con figurations,the real-time conditions need to be adjusted by the adjustment of the group number of the waypointsNGand the value of the control inputstx.

6.Conclusions

(1)A hierarchic optimization strategy which is composed of

the of fline path planning process and online trajectory planning process is used to solve the multiple quadrotor UAVs collaborative assembling task.By the help of the hierarchic optimization strategy,the trajectory optimization problem is divided into a path planning problem and several small trajectory planning problems.

(2)In the path planning process,a novel parallel intelligent optimization algorithm which combines the CFO algorithm with the GA algorithm is presented.Moreover,the suf ficient conditions of the CFO algorithm are obtained by the stability theory of linear time-variant discrete system,under which it is guaranteed to converge to the global/local optimal value or the special points under a prede fined initial distribution.

(3)In thetrajectory planning process,the trajectory

planning optimization problem is established based on a high- fidelity and six-degrees-of-freedom nonlinear dynamic model of the quad-rotor UAVs and the planning results of the front path planning process.In order to limit the range of the attitude angle and guarantee the lf ight stability of the planning result,the optimized object is changed from the ordinary quad-rotor UAVs dynamic model to the 6-DOF rigid-body dynamic model of the quad-rotor helicopters with an inner-loop attitude controller.What’s more,the cubic B-spline parameterization algorithm and the concept of the security time are introduced to compute the optimal control inputs and the optimal time of this optimal control problem.All the three UAVs fly over the regions and reach the target at the same time.

(4)Finally,the discussion and analysis on the real-time performance of the hierarchic optimization strategy are presented around the group number of the waypoints and the equal interval time.

1.Peng H,Su F,Bu YL,Zhang GZ,Shen LC.Cooperative area search for multiple UAVs based on RRT and decentralized receding horizon optimization.Proceedings of 2009 the 7th Asian control conference;2009 Aug 27–29;HongKong,China.Piscataway(NJ):IEEE Press;2009.p.298–303.

2.Shen LC,Xu X,Zhu HY.Autonomous mobile robot control theory and technology.Beijing:Science Press;2011.p.151–68[Chinese].

3.Garcia M,Viguria A,Ollero A.Dynamic graph-search algorithm for global path planning in presence of hazardous weather.J Intell Rob Syst2013;69(1–4):285–95.

4.Khatib O.Real-time obstacle avoidance for manipulators and mobile robots.Int J Rob Res1986;5(1):90–8.

5.Cetin O,Zagli I,Yilmaz G.Establishing obstacle and collision free communication relay for UAVs with arti ficial potential fields.J Intell Rob Syst2013;69(1–4):361–72.

6.Dunbar WB,Caveney DS.Distributed receding horizon control of vehicle platoons:stability and string stability.IEEE Trans Autom Control2012;57(3):620–33.

7.Ichnowski J,Alterovitz R.Scalable multicore motion planning using lock-free concurrency.IEEE Trans Rob2014;30(5):1123–36.

8.Li P,Duan HB.Path planning of unmanned aerial vehicle based on improved gravitational search algorithm.Sci China Technol Sci2012;55(10):2712–9.

9.Roberge V,Tarbouchi M,Labonte´G.Comparison of parallel genetic algorithm and particle swarm optimization for real-time UAV path planning.IEEE Trans Ind Inf2013;9(1):132–41.

10.Duan HB,Zhang XY,Wu J,Ma GJ.Max–min adaptive ant colony optimization approach to multi-UAVs coordinated trajectory replanning in dynamic and uncertain environments.J Bionic Eng2009;6(2):161–73.

11.Zhu WR,Duan HB.Chaotic predator–prey biogeography-based optimization approach for UCAV path planning.Aerosp Sci Technol2014;32(1):153–61.

12.Xu CF,Duan HB,Liu F.Chaotic arti ficial bee colony approach to uninhabited combat air vehicle(UCAV)path planning.Aerosp Sci Technol2010;14(8):535–41.

13.Yao M,Zhao M.Unmanned aerial vehicle dynamic path planning in an uncertain environment.Robotica2015;33(3):611–21.

14.Heydari A,Balakrishnan S.Path planning using a novel finite horizon suboptimal controller.J Guid Control Dyn2013;36(4):1210–4.

15.Zhang Y,Chen J,Shen LC.Real-time trajectory planning for UCAV air-to-surface attack using inverse dynamics optimization method and receding horizon control.Chin J Aeronaut2013;26(4):1038–56.

16.Kamyar R,Taheri E.Aircraft optimal terrain/threat-based trajectory planning and control.J Guid Control Dyn2014;37(2):466–83.

17.Chamseddine A,Zhang Y,Rabbath CA.Trajectory planning and re-planning for fault tolerant formation flight control of quadrotor unmanned aerial vehicles.Proceedings of the 2012 American control conference(ACC);2012 Jun 27–29;Montreal(QC).Piscataway(NJ):IEEE Press;2012.p.3291–6.

18.Eva BP,Lius T,Cruz JM,Bonifacio AT.Evolutionary trajectory planner for multiple UAVs in realistic scenarios.IEEE Trans Rob2010;26(4):619–34.

19.Yang K,Keng GS,Sukkarieh S.A Gaussian process-based RRT planner for the exploration of an unknown and cluttered environment with a UAV.Adv Rob2013;27(6):431–43.

20.Gu XQ,Shen LC,Chen J,Zhang Y,Zhang WP.A virtual motion camou flage approach for cooperative trajectory planning of multiple UCAVs.Math Prob Eng2014;2014:1–15.

21.Luo GC,Yu JQ,Mei YS,Zhang SY.UAV path planning in mixed-obstacle environment via arti ficial potential field method improved by additional control force.Asian J Control2014;17(4):1–11.

22.Zhang XY,Duan HB,Yu YX.Receding horizon control for multi-UAVs close formation control based on differential evolution.Sci China Inf Sci2010;53(2):223–35.

23.Chandler P,Rasmussen S,Pachter M.UAV cooperative path planning.Proceedings of AIAA guidance,navigation,and control conference and exhibit;2000 Aug 14–17;Denver(CO).Reston:AIAA;2000.p.1255–65.

24.Alighanbari M,Kuwata Y,How JP.Coordination and control of multiple UAVs with timing constraints and loitering.Proceedings of the 2003 American control conference;2003 Jun 4–6.Piscataway(NJ):IEEE Press;2003.p.5311–6.

25.Beard RW,McLain TW,Goodrich MA,Anderson EP.Coordinated target assignment and intercept for unmanned air vehicles.IEEE Trans Rob Autom2002;18(6):911–22.

26.Formato RA.Central force optimization with variable initial probes and adaptive decision space.Appl Math Comput2011;217(21):8866–72.

27.Ding DS,Qi DL,Luo XP,Chen JF,Wang XJ,Du PY.Convergence analysis and performance of an extended central force optimization algorithm.Appl MathComput2012;219(4):2246–59.

28.van den BF,Engelbrecht AP.A study of particle swarm optimization particle trajectories.Inf Sci2006;176(8):937–71.

29.Emara HM,Fattah HAA.Continuous swarm optimization technique with stability analysis.Proceedings of the 2004 American control conference;2004 Jun 30–Jul 2;Boston(MA).Piscataway(NJ):IEEE Press;2004.p.2811–7.

30.Chen YB,Yu JQ,Mei YS,Wang YF,Su XL.Modi fied central force optimization(MCFO)algorithm for 3D UAV path planning.Neurocomputing2016;171(1):878–88.

31.Jin XL,Ma LH,Wu TJ,Qian JX.Convergence analysis of the particle swarm optimization based on stochastic processes.Acta Autom Sin2007;33(12):1263–8[Chinese].

32.Xiao Y,Du XY.Theorem and algorithm of asymptotic stability test for time variant discrete system.J Northern Jiaotong Univ1998;22(6):1–7[Chinese].

33.Liao WD,Liu ZX,Liao XX.Stability conditions based on Cauchy-matrix for some classes of time-varying linear difference equations.J Huazhong Normal Univ(Nat Sci)2001;35(4):386–9[Chinese].

34.Wang Q,Zhang A,Qi LH.Three-dimensional path planning for UAV based on improved PSO algorithm.Proceedings of the 26th Chinese control and decision conference(CCDC);2014 May 31–Jun 2;Changsha,China.Piscataway(NJ):IEEE Press;2014.p.3981–5[Chinese].

35.Mohamed HA,Yang S,Moghavvemi M.Sliding mode controller design for a flying quadrotor with simpli fied action planner.Proceedings of 2009 ICCAS-SICE;2009 Aug 18–21;Fukuoka,Japan.Piscataway(NJ):IEEE Press;2009.p.1279–83.

36.Bouabdallah S,Siegwart R.Full control of a quadrotor.Proceedings of IEEE/RSJ international conference on intelligent robots and systems;2007 Oct 29–Nov 2;San Diego(CA).Piscataway(NJ):IEEE Press;2007.p.153–8.

37.Bai YQ,Liu H,Shi ZY,Zhong YS.Robust flight control of quadrotorunmanned airvehicles.Robot2012;34(5):519–24[Chinese].

38.Salih AL,Moghavvemi M,Mohamed HA,Gaeid KS.Modelling and PID controller design for a quadrotor unmanned air vehicle.Proceedings of 2010 IEEE international conference on automation quality and testing robotics(AQTR);2010 May 28–30;Cluj-Napoca.Piscataway(NJ):IEEE Press;2010.p.1–5.