Trajectory optimization of multiple quad-rotor UAVs in collaborative assembling task Academic research paper on "Mechanical engineering"

Chinese Journal of Aeronautics, (2016), 29(1): 184-201

JOURNAL OF

AERONAUTICS

Chinese Society of Aeronautics and Astronautics & Beihang University

Chinese Journal of Aeronautics

cja@buaa.edu.cn www.sciencedirect.com

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

Received 3 June 2015; revised 18 August 2015; accepted 22 October 2015 Available online 22 December 2015

KEYWORDS

Hierarchic optimization strategy;

Parallel CFO-GA algorithm; Path planning; Six-degree-of-freedom rigid-body dynamic model; Trajectory optimization; Trajectory planning

Abstract A hierarchic optimization strategy based on the offline path planning process and online trajectory planning process is presented to solve the trajectory optimization problem of multiple quad-rotor unmanned aerial vehicles in the collaborative assembling task. Firstly, the path planning process is solved by a novel parallel intelligent optimization algorithm, the central force optimization-genetic algorithm (CFO-GA), which combines the central force optimization (CFO) algorithm with the genetic algorithm (GA). Because of the immaturity of the CFO, the convergence analysis of the CFO is completed by the stability theory of the linear time-variant discrete-time systems. The results show that the parallel CFO-GA algorithm converges faster than the parallel CFO and the central force optimization-sequential quadratic programming (CFO-SQP) algorithm. Then, the trajectory planning problem is established based on the path planning results. In order to limit the range of the attitude angle and guarantee the flight stability, the optimized object is changed from the ordinary six-degree-of-freedom rigid-body dynamic model to the dynamic model with an inner-loop attitude controller. The results show that the trajectory planning process can be solved by the mature SQP algorithm easily. Finally, the discussion and analysis of the real-time performance of the hierarchic optimization strategy are presented around the group number of the waypoints and the equal interval time.

© 2016 Production and hosting by Elsevier Ltd. on behalf of CSAA & BUAA. This is an open access article under the CC BY-NC-ND license (http://creativecommons.org/licenses/by-nc-nd/4.0/).

1. Introduction

* Corresponding author. Tel.: +86 10 68912407. E-mail addresses: bit_chenyongbo@163.com (Y. Chen), jianqiao@ bit.edu.cn (J. Yu), mys001@bit.edu.cn (Y. Mei), ylww@bit.edu.cn (S. Zhang), bitaixiaolin@163.com (X. Ai), jiazhenyue@163.com (Z. Jia).

Unmanned aerial vehicles (UAVs) have become increasingly attractive for missions in which the human presence is dangerous or difficult. 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.1 The 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

Peer review under responsibility of Editorial Committee of CJA.

http://dx.doi.Org/10.1016/j.cja.2015.12.008

1000-9361 © 2016 Production and hosting by Elsevier Ltd. on behalf of CSAA & BUAA.

This is an open access article under the CC BY-NC-ND license (http://creativecommons.org/licenses/by-nc-nd/4.0/).

controller design field, but the trajectory optimization process specifically 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 three-dimensional (3D) space path from the starting point to the destination. The 3D space path is a static geometry path. It does not include the concept of time.2 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 efficiently. Both of them are very significant 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,3 artificial potential field (APF) method,4'5 rapidly-exploring random tree (RRT) algorithm,6,7 a large number of intelligent optimization algorithms8,9 and 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,10 the chaotic predator-prey biogeography-based optimization (CPPBBO) algorithm,11 the improved gravitational search algorithm8 and the chaotic artificial bee colony (ABC) approach.12 In order to use these algorithms, the original path planning problem which is an infinite dimensional problem is simplified 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 signifies the huge amount of calculation and the large difficulty 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 multi-constraints optimal control problem. The most intuitive approach is to use the optimal control theory. Yao and Zhao13 put 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 three-degree 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 dynamic model according to a full-blown 3-DOF particle model. In Ref.16, Kamyar and Taheri used the differential 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 commonly employed quad-rotor UAV model, Chamseddine et al.17 used the Bezier polynomial function and the differential 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.18 presented 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.20 put forward a virtual motion camouflage (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 difficulties 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 efficient, 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 offline 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 efficiency, 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 efficient 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 simplified 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 confirm 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

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

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.

2.2. Solution frame

At first, the multiple quad-rotor UAVs collaborative assembling problem is a multi-objects planning problem. Obviously, the computational efficiency of the solving algorithm is very important for this problem. To solve the efficiency 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 offline 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 difficulty are significantly 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 definitional 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 specific 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.

axis and y-axis. By this way, we can obtain Nx x Ny feature points whose coordinates are (xh, yt , z(xh, yt )) (i1 = 1, 2, • ••, Nx; i2 = 1,2, ..., Ny), where z(xix, yt ) 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.21 regard 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 x 5500 m x 1200 m. The whole planning space is shown in Fig. 3.

3.2. Optimal model creation

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 efficiency.

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,9 and 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 into Nx and Ny parts along the x-

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 problem22 is:

min f(x)

hk1 (x) =0 h = 1; 2; • ••; ^ s.t. gk2 (x) 6 0 h2 = 1; 2; •••; l2 x e D # Rn

where x is the independent variable; f is the objective function; D means the feasible region of the independent variable; hkl and gkl are the equality constraints and inequality constraints, respectively; l1 and l2 mean the number of the constraints; n is 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 influences the complexity of the model and the solver, so it is very important to define 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

Fig. 2 Flowchart of hierarchic optimization strategy.

Fig. 3 Whole planning space.

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.

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 specific parametric processes are as follows: firstly, make a straight line lT which passes through the target point and parallel to the z-axis; then, set up the cylinders according to lT. The number of the cylinders is N, and N is the trunc of lm/r, r is the minimum radius of the coaxial cylinders (preestablished), and lm is the furthest horizontal distance between the start points Sj (j =1,2, ..., M) and target point. So the radiuses are i x r (i = 1,2, ..., N), where i is the serial number of the cylinders. The characteristic waypoints Xtj of the UAVs locate on the surface of the corresponding cylinder. Define the independent variable x based on the cylindrical coordinate system as

x — [x1; x2; ... ; xj; ... ; xm]

— [hh 1 , 1 ; , 1 ; . . . ; hN, 1;h 1, 1 ; h2, 1 ; . . • ; hN}; i^z^UAV,;

UAV1 (2)

h1,M5 @2,M-' ... ' hN,M, hl,M, h2,M, ... ; hN,M]

where 0tj means the scalar from the target point to the characteristic waypoint of the jth UAV on the ith cylinder surface; hij is the ordinate value of the characteristic waypoint of the jth UAV on the ith cylinder surface; M is the total number of the UAVs (in this paper, M = 3).

Based on the definition of the independent variable x, the characteristic waypoint of the jth UAV on the ith cylinder surface is

Xij — (Tx - ir cos 6ij, Ty - ir sin 6ij, hij) (3)

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

i01j — S, (4)

I 0Nwj — T

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.23 adjusted the time by changing the velocities of the UAVs. Alighanbari et al.24 ensured the time coordination by the circulating flight way. Beard et al.25 changed 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 simplified 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 simplified 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

M Nw-1

l(x)^El|ot+1j - otj\\ (5)

j—1 t—1

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 simplified, 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 difficult 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 efficiency 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

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:

°t.j(3) > zr (7)

where otj- (3) is the ordinate component of otj, and zr the 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

min l(x)

dMl 6 dm Otj(3) > zr

3.3. Optimization algorithms

Although the path planning process is finished offline, the higher efficiency 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 efficiency 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 into N parts, 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 efficiently. 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 simplified due to considering the coordination problem by changing the takeoff times.

Therefore, the objective function is divided into N independent objective functions. The new objective functions are shown as

lj (xj) = X!l|0t+1 j - Otjj|

where lj (xj) is the objective function of the jth UAV and xj the independent variable of the jth 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:

pk:x = pm+vmDt+1 «mAt2 m=1,2 ,... , n

where prm 2 R x N is the position of the mth particle in the kth iteration; vkm is the velocity vector of the mth particle in the kth iteration. In the original CFO method, vkm is ignored (vkm — 0ix2N); At is the time constant; Np means the particle number; akm is the acceleration of the mth particle in the kth iteration, and it is defined as follow:

«m = u(f(Pn ) -fpm )) ■(f(pk ) -fpk ))a

p - pm

Pk - Pm iib

_ m = 1 ,2 ,... ,Np

tion, and it meets U(-) =

where G is the gravitation constant; U(-) is the judgement func-

1 K - e R; a and b are the

tuning parameters, and they satisfy a > 0 and b > °; f{pkm ) is the objective function of the mth particle in the kth iteration.

If pk (d) is bounded, the supplementary information needs to be added, as follow:

' pmin(d)+%m-1 (d) -pmin(d)) if piw <puT(d)

pm (d)=pu

(d)-h(p) (d)

(max m

(d)-pm-1 (d)) Eiseifpm(d)>pmax(d)

Fig. 5 Parallel optimization algorithm.

where pkm (d) is the dth components of pm; h > 0 is the accommodation coefficient; pmin (d) and pmax (d) are the minimum value and maximum value of pkm (d), respectively.

The CFO is an effective and efficient deterministic search and optimization algorithm. Its effect has been tested by many test functions in the Ref. . The convergence rate of the CFO which is based on the parameters G, a and b is both the strength and weakness. The appropriate rate is very difficult to grasp. Hence, the CFO algorithm is augmented with an inner-loop local optimizer. The inner-loop local optimizer is to optimize each particle pkm in its small field. The inner-loop local optimizer can improve the regional search ability which reduces the bad influence 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.

pm+1 = pi+ai

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-30 In 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 particle m is arbitrary, so all related vectors pi, akm and p degenerate into the scalars pi, ~k and pk.

Definition 1 (Linear difference equations). Let J+ be a set of

the nonnegative integer, the square nonsingular matrix A(-): j+ ? Rnxn, f*( ): j+ ? Rn and x*(p)eRn, for each peJ+, the linear difference equation (Eq. (13)) and corresponding homogeneous linear equation (Eq. (14)) with variable coefficients are as follows:

x*(p + 1)—A(p)x'(p) +f(p) x*(p + 1) — A(p)x*(p)

am—1 g x

Define:

p - pi I

(A - pi)Dt2

,k (f(pk) -f(jt)T pk

/m — G^ ^ pk

I pnk - p

(k = G V ((f(pk) -f(p'Cm))a

m G / y

ipk - pj Eq. (15) is rewritten as

pm+1 — pm+2 At2/m - 2 At2empm

— (1 - - At2^)/ + - At2/'

v 2 m)"m 1 2 •

Define:

Am(k) — 1 - 2 At2em

Jm(k)—2 At2/m

Go on with a variable substitution, and Eq. (17) is formed into:

pm+1 — Am(k)pm + fm (k)

where Am(k) eR,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-29 Based on the linear system theory, the restriction condition to guarantee the stability of the system is that the spectral radius of the system matrix Am(k) stays in the unit circle. This condition is the necessary and sufficient 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.31 Xiao and Du32 put 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 matrix Am (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 matrix Am(k) stays outside the unit circle for the linear time-variant discrete system. Obviously, the differential forms of the intelligent algorithms are all time-variant. So the conclusions of above Refs.27-29 are wrong and circumscribed by the assumption of the fixity coefficient.

Theorem 2. 8 f (p)e[J+, Rn], the stability degree of the linear difference equations is equivalent to the stability degree of the general solution of the corresponding homogeneous linear

equations.

Theorem 1. The movement equation of the particle pm is a firstorder linear difference equation with variable coefficients.

Proof. According to the expression of U(-), define Mm = {n| f (pk„) - f pj >0, n= 1, 2,..., Np}. The whole update of the CFO method is rewritten as

Theorem 3. If there is a kind of matrix norm that satisfies: P + 1

l|A(p)|

x 6 p + 2

P — Po, Po + 1;...

where ||-||x means a certain norm, 8 p0 eR. Then the trivial solution of the Eq. (19) has the uniform asymptotic stability.33 It is

only a sufficient condition for the uniform asymptotic stability of the solution.

According to Theorems 2 and 3, a sufficient condition which can ensure that the movement equation of the particles pkm based on the CFO method is stable (in other words, each particle pm will converge to its own stable Pm) is shown as follows:

1 -1 At2 em,

k +1 k + 2

k = ko ; ko + 1; .

where k is the eigenvalue of Am (k), 8 k0eR. The condition is further simplified into:

4k + 6

(k + 2)At2 ' (k + 2)At2

Based on the above sufficient condition, the particles {pm} converge to the stable values {Pm}, limk!1pm — Pm. In the Ref.27, one of its conclusions is that all particles using the CFO method converge to a stable P. 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 (localjglobal) or the special points.

Proof. Based on Eq. (15) and lim pkm = Pm, we have

klim ~k = lim (pk+' - pk,) = Pm - Pm = 0

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

For 8 m if it exists: Pn = Pm, you can get that all particles using the CFO method converge to a stable optimum solution P:

P1 = P2 = ... = PMm = P

(24) (2) Situation 2

If it exists: f(Pn) — f(Pm) \ Pn—Pm, you can get that all particles using the CFO method converge to several stable values {Pm0, m — 1,2,..., Np; Np 6 Np} which have the equal corresponding objective function values (the local/global optimums).

{Pm ; m = 1,2;..., Np g ! {Pm ; m' = 1,2,..., Np ; Np 6 Np g

(3) Situation 3

If some m exists: f(Pn )-f(Pm) n Pn-Pm ^ „tMm ^ )—ff"

m \Fn—Pm \

•(Pn — Pm) = 0, you can get that these particles using the CFO method converge to several special points |Pm", m" = 1; 2; •• •; Np'; Np 6 Np} which are not the local/global optimums. But their accelerations are zero. This situation only exists in rare circumstances. They are defined 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 definitional 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 stable Pm whose 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 as dkm satisfies 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 is M = 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 CFO Np = 20; the maximum number of iterations in CFO is 300; the gravitation constant G = 0.01; the tuning parameters a = 0.4 and b = 0.5; the accommodation coefficient h = 0.5; the number of the discrete waypoints Nw = 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 defined as r = 1000 m, so the total number of the cylinders N = 6; the constraint value of the steering angle which is related to the turning radius of the UAVs r and the minimum radius of the coaxial cylinders r is provisionally defined as dmax = p/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.

Px P2 P3 Particles

(b) Situation 2

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 CFO-SQP algorithm; the maximum number of iterations is 300; the population size in each generation is 20; the number of the discrete waypoints Nw = 1300; the minimum radius of the coaxial cylinders r = 1000 m, so the total number of the cylinders N = 6; the constraint value of the steering angle is provisionally defined as dmax = я/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 coefficient 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 CFO-SQP algorithm. And the black lines, the purple lines and red lines represent the planning results whose starting points are S1, S2 and S3 respectively. 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 CFO-SQP 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 unsatisfied 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. IflPn) _ fpk)T 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.

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

Number of iterations

Fig. 9 Convergence property of five algorithms.

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

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 simplified 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 definition and parameterization (independent variable definition), the optimization model creation, the solution algorithm and the simulation result is finished.

4.1. Flight simulator model

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 simplified model of the UAV is as follows:36

yb = — (cos u cos W sin h + sin u sin W)uci — —xb

yb = — (cos u sin W sin h — sin u cos W)uc1---y b

zb = — cos h cos uuc1--zb — g

.. l __ fly — I,\ (26)

u = yuc2 + hW 1

1 = jUc3 + u W--

Iz — Ix

'Ix — Iy

W = jUc4 + (p0- -

where W, h and u are the Euler angles between the body-axis coordinate system B: ObXbYbZb and the inertial coordinate system E: ObeXbeYbeZbe; xb, yb and zb are the positions of the UAV; mu is the mass of the UAV; g is the acceleration of gravity; l is the half length of the quad-rotor helicopter; Kx, Ky and Kz are the drag coefficients; Ix, Iy and Iz are the moments of inertia with respect to the axes Xb, Yb and Zb, respectively; the control inputs uci (i = 1, 2, 3, 4) satisfy the following equations:37

' Uci = + x2 + x2 + x2)

uc2 = b(x2 — x2 ) uc3 = b(x\ — x2 ) ^ uc4 = c(x2 — x2 + x2 — x2 )

where b is the scaling factor of the thrust generated by the rotor; c is the scaling factor of the propeller torque constant generated by the rotors; x (i = 1,2,3,4) is the rotational speed of the ith propeller.

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

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.

(1) The UAV structure is symmetrical and rigid.

(2) The center of mass and Ob coincide.

(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 definition and paraketerization

According to the direct method of the optimal control problem, the planning inputs need to be defined 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 inputs uci (i = 1, 2, 3, 4) may be chosen for the planning inputs of the quad-rotor UAV trajectory planning. If so, the uci (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 definitional 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, where xp, yp and zp are the coordinates of the reference trajectory; hr and ur are 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 Wr (—30° 6 Wr 6 30°), hr (-30° 6 h 6 30°), ur (-30° 6ur 6 30°) and uc1

(0.9mug 6 uc1 6 1.1 mug) 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

"follow (t) = Kpe(t) + Kd

de(t) dt

Definitional domain Fig. 12 Schematic diagram of two situations.

where ufollow(t) is the control variable; the tracking error e(t) is defined as e(t) = Wr(t) — W(t), with Wr(t) the reference output and W(t) the real output; KP and KD are 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 time t0 and the indefinite final time tf of 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 time tf is very difficult to obtain. In order to obtain it, the security time Tf which is bigger than the indefinite final time tf is presented. The discretized and parameterization processes are finished between the initial time t0 and the security time Tf. The security time Tf will obtain an integrated trajectory. The end waypoint whose corresponding time is tf is defined as the closest waypoint to the target in the whole trajectory. So although the control inputs between t0 and Tf are used in the following optimization process, only the control inputs between t0 and tf act on the objective function of the trajectory planning problem. In other words, the optimization only occurs between t0 and tf. By this way, the optimal final time tf and control inputs between t0 and tf are obtained and used in the real flying state. The parameterization of control inputs is shown in Fig. 14.

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 hr, and uc1 of the attitude inner-loop controller. Because hr and ur are 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

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

Fig. 14 Parameterization of control inputs.

of the independent variables of the trajectory planning problem except the first one is 4 (Tf — t0)/tx, with tx the 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 efficiency which is greatly influenced by the scale of the optimization problem is very important. The value of tx needs to be relatively big to reduce the number of the independent variables which can improve the optimization efficiency. At the same time, the value of tx needs 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 otj- (t = 1,2, ..., Nw) of the offline path planning results. The number of the waypoints otj (t = 1, 2, ..., Nw) is too large, so we cannot build the small trajectory planning problem in every two waypoints. Therefore, the way-points are divided into NG groups. At the same time, the large-scale optimization problem is decomposed and simplified into Ng small 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

Gs j — {°(s— 1)xround(Nw/NG) + 1j; °(s—1)xround(Nw/NG)+2j; ••• ; °sxround (Nw/Ng ) + 1j'}

where s is the serial number of the group and the serial number of the small trajectory planning problem; round(-) is the round down function; Gsj is the sth group of the waypoints of the jth 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 length-optimal 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 the jth UAV in the sth trajectory planning problem is defined as

Jsj = Xul ll(xf, f, 4J) +®u2 f - - Tj

- °sx round (Nw/Ng )+1j H

where xu1 and xu2 are the adjustment coefficients, and xu1 > 0, xu2 > 0; (f, yf ; zf ) is the end waypoint of the jth UAV in the sth small problem; t^ and ts/ mean the initial time and final time of this problem, respectively; Tj is the predicted flight time of the jth UAV in the sth small problem whose purpose is to allocate the flight time among the different small trajectory planning problems and minimize the time difference. It is defined as

= r U(Gsj)/v

Us-1 j - t0-1 j - TT1 j) + U(Gsj)/v s > 1

where U(Gsj) is the sum length of the path which is composed of line segments according to the initial and final waypoints of Gs j.

The constraint conditions of the optimization problem include the dynamic constraints, the obstacle constraints and the initial/final conditions. The dynamic constraints have been

1 —T ! * Attitude controller

1 J 1 l

1 1 —± 1 1 l !

I 1 MC1 1-

L 1 1 Optimization object

Parameterization

Optimization algorithm Objective function evaluation

Fig. 15 New structure of whole optimization problem.

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 meets zb > zr, where zb and zr respectively mean the UAV flight height and the obstacle height of the jth UAV in the sth trajectory planning problem. The initial/final conditions of the jth UAV in the sth trajectory planning problem are defined as follows:

ÎK, 0ix

s = 1 s > 1

f, fj ) 2 Or=loo(oNw, )

where X0 and Xs/ are the initial and terminal state vectors of the 6-DOF differential dynamic equation; the state vectors can then b e formed as [xb, yb, zb, Xb, yb, ¿bJ, Usj, 0sj, Wsj, Usj, hy, Wsj]T; they represent the flight state of the jth UAV in the sth trajectory planning problem which includes the position, the speed, the Euler angle and the Euler angular velocity; Or=ioo(c) is a sphere whose center and radius respectively are 1 and 100 m.

In conclusion, the jth UAV in the sth optimization problem is defined as

wr. hr, Ur. uc1

{Differential Eqs.(26)-(28)

z'b > zr

Initial/final conditions in Eq.(32)

This trajectory problem needs to be optimized on-line in the airborne computer of the UAV, so the mature, simple and efficient 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 hr, ur and uci respectively are [0 ,0 ,... , 0]1x(Tf-to)/ti,

[0,0,... ,0]1X( T—to)/tx and [mug, mug, ... , mug]1x(Tf-t0)/tx.

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 is mu = 0.504 kg; the moments of inertia Ix, Iy and Iz respectively are 1.9757 x 10-4kg-m2, 1.9757 x 10-4 kg-m2 and 3.9514 x 10-4 kg-m2; the half length of the quad-rotor helicopter UAV is l = 0.028 m; the group number is NG = 10; the interval between the initial t0 and the security time Tf is 90 s; the value of the equal interval time is tx = 10 s; the rated speed of the rotor-UAVs is v = 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

Fig. 16 Trajectory planning results of collaborative assembling task.

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.

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 all m^g, 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 multi-rotor-UAVs can reach the target at the same time, the takeoff time of UAV1 and UAV2 needs 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

The offline 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

UAV, — UAV2 — UAV, Fig. 17 Variations of optimal inputs.

Fig. 18 Variations of flight velocities and attitude angles.

which can be called the airborne computers. After the storage of the offline 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 unit rstep includes the constant loading time Atloading, the planning time Atplanning, and the constant post-processing

post-processing

post-processing,

loading

(Tstep Atloading + At

¿planning

post-processing

for a UAV in one task, the time units T

5 s). Of course, of the different

small trajectory planning problems are different, so we define that the value of the time unit Tstep (s, j) means the time unit of the jth UAV in the sth trajectory planning problem. Before takeoff, the UAVs wait for at least a time unit Tstep (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 Sj to the later point (0Sxround(Nw/NG)+ij). Then, the UAVs receive the planning results and begin to follow the trajectory until they reach the later points (oNGxround(Nw/NG)+ij) at time intervals Tflying (1, j), with Tflying (1, j) the optimal final time f of the jth 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

Tstep (s + 1, j) 6 fing (s, j) S = 1, 2, ... , Ng - 1

Therefore, in order to meet the above condition, the coordinated relations between the planning time Atplanning and the implementation time Tflying are studied in the following part. The influence factors of the planning time Atplanning mainly include the equal interval time of the control inputs and the group number of the waypoints NG. However, the main influence factor of the implementation time Tflying is the group number of the waypoints NG. So in the following part, we will discuss the impact degree of these factors by some simulations.

Because the planning time Atplanning not 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 influence factor is the group number of the way-points NG which directly affects the distance between each two starting waypoints in the adjacent groups, then further affects the implementation time Tflying and the planning time Atplanning. In order to separately study the impact of NG, we keep the value of the equal interval time (tx = (TTf — t0)/9) fixed. Accordingly, the discretized and parameterization time between the initial time t0 and the security time Tf is defined 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 defined as that the distances between the multi-UAVs terminal positions and the target are less than 100 m. When NG is 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 when NG changes 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 whose NG is too small. Then, the real-time performance of the simulation is shown in Table 2 and Fig. 20. Where average Atplanning means the average value of the planning time in all small trajectory planning problems; average Tflying means 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; 1 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 unit Tstep and average flying time Tflying are both getting smaller with the increase of NG. When NG is 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. When Ng is 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 definitely. In Table 2, a special phenomenon needs to be paid attention to. When NG is 6, the real-time demand still is not satisfied 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 unit Tstep (s, j) and the flying time Tflying (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.

Computer CPU RAM OS IDE

ThinkStation Intel(R) 4.00 GB Windows 7 Microsoft

S20 Xeon(R) DDR3 professional Visual

E5620 1600 MHz C+ +

4105-D22 CPU @ 64 bits V6.0

2.40 GHz

The second influence factor is the equal interval time tx which directly affects the independent variables of the optimization problem, thus affects the planning time Atplanning. In order to study the influence of tx separately, the group number of the waypoints NG and the maximum number of allowed iterations are defined as 10 and 15 respectively. Then, the value of the control inputs tx changes 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 Atloading and the constant post-processing time Atpost-processing, the curves of the planning time Atplanning and the time unit Tstep with the changes of tx are shown in Fig. 21.

With the decrease of tx, 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 and tx is in the linearity range (from (Tf — t0)/2 to (Tf — t0)/9). Clearly, all of these results can meet the real-time requirement. But the

Table 3 Real-time performance changes with tx.

tx Average Atoning (s)

(Tf - %)/2 22.1

(Tf - t0)/3 28.2

(Tf - t0)/4 34.7

(Tf - %)/5 40.3

(Tf - %)/6 45.7

(Tf - %)/7 50.9

(Tf - t0)/8 56.2

(Tf - %)/9 62.4

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

Table 2 Real-time performance changes with NG.

Ng Average Dtplanning (s) Average Tflying (s) Completion status Real-time ability

1 1 1 Failure No

2 777.3 349.2 Success No

3 452.1 220.5 Success No

4 210.1 189.4 Success No

5 175.2 151.1 Success No

6 121.3 127 Success No

7 84.9 105.2 Success Yes

8 69.4 94.6 Success Yes

9 67.7 83.6 Success Yes

10 62.4 80.5 Success Yes

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

larger dimensions of the independent variables can expand the resolution space, which is favorable to solve the more complex planning space. So tx is set to 10 s in Section 4.

In short, the real-time demand of this system can be guaranteed for this hardware system when the group number of the waypoints NG is bigger than 6 and the value of the control inputs tx is bigger than (Tf — t0)/9. For different computer hardware configurations, the real-time conditions need to be adjusted by the adjustment of the group number of the way-points Ng and the value of the control inputs tx.

6. Conclusions

(1) A hierarchic optimization strategy which is composed of the offline 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 sufficient 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 predefined initial distribution.

(3) In the trajectory 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 flight 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.

References

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. Piscat-away (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 Syst 2013;69(1-4):285-95.

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

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

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

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

8. Li P, Duan HB. Path planning of unmanned aerial vehicle based on improved gravitational search algorithm. Sci China Technol Sci 2012;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 Inf 2013;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 Eng 2009;6(2):161-73.

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

12. Xu CF, Duan HB, Liu F. Chaotic artificial bee colony approach to uninhabited combat air vehicle (UCAV) path planning. Aerosp Sci Technol 2010;14(8):535-41.

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

14. Heydari A, Balakrishnan S. Path planning using a novel finite horizon suboptimal controller. J Guid Control Dyn 2013;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 Aeronaut 2013;26 (4):1038-56.

16. Kamyar R, Taheri E. Aircraft optimal terrain/threat-based trajectory planning and control. J Guid Control Dyn 2014;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 Rob 2010;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 Rob 2013;27(6):431-43.

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

21. Luo GC, Yu JQ, Mei YS, Zhang SY. UAV path planning in mixed-obstacle environment via artificial potential field method improved by additional control force. Asian J Control 2014;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 Sci 2010;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 Autom 2002;18(6):911-22.

26. Formato RA. Central force optimization with variable initial probes and adaptive decision space. Appl Math Comput 2011;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 Math Comput 2012;219 (4):2246-59.

28. van den BF, Engelbrecht AP. A study of particle swarm optimization particle trajectories. Inf Sci 2006;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. Modified central force optimization (MCFO) algorithm for 3D UAV path planning. Neurocomputing 2016;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 Sin 2007;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 Univ 1998;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 simplified 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/PS1/ 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 quadrotor unmanned air vehicles. Robot 2012;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.

Chen Yongbo received his B.S. degree in Beijing Institute of Technology in 2012. He is currently working toward a doctoral degree at School of Aerospace Engineering, Beijing Institute of Technology. His research interest is UAV path planning.

Yu Jianqiao received B.S., M.S. and Ph.D. degrees from Beijing Institute of Technology in 1994, 1997 and 2007, respectively, and now is a professor there. His main research interests include flight dynamics and control, cooperative control, flight vehicle system design and robust control.

Mei Yuesong received his Ph.D. degree in flight vehicle design from Beijing Institute of Technology in 2007. He is now a lecturer in School of Aerospace Engineering, Beijing Institute of Technology. His research interests include flight vehicle system design, flight dynamics and control.

Zhang Siyu received his B.S. degree in Mechanical Design Manufacturing and Automation from Beijing University of Technology in 2010. He is currently working toward a Ph.D. degree at School of Aerospace Engineering, Beijing Institute of Technology. His research interests include UAV path planning and area search.

Ai Xiaolin received his B.S. degree in Beijing Institute of Technology in 2013. He is currently working toward a doctoral degree at School of Aerospace Engineering, Beijing Institute of Technology. His research interests are flight dynamics and control, cooperative control and flight vehicle system design.

Jia Zhenyue received his B.S. degree in Beijing Institute of Technology in 2014. He is currently working toward a doctoral degree at School of Aerospace Engineering, Beijing Institute of Technology. His research interests are flight dynamics and control, cooperative control and UAV mission planning.