J Intell Robot Syst DOI 10.1007/s10846-014-0077-y

Path Planning for Multi-UAV Formation YongBo Chen · JianQiao Yu · XiaoLong Su · GuanChen Luo

Received: 19 August 2013 / Accepted: 19 June 2014 © Springer Science+Business Media Dordrecht 2014

Abstract This paper presents an efficient and feasible algorithm for the path planning problem of the multiple unmanned aerial vehicles (multi-UAVs) formation in a known and realistic environment. The artificial potential field method updated by the additional control force is used for establishing two models for the single UAV, which are the particle dynamic model and the path planning optimization model. The additional control force can be calculated by using the optimal control method. Furthermore, the multi-UAV path planning model is established by introducing “virtual velocity rigid body” and “virtual target point”. Then, the motion states of the lead plane and wingmen are obtained from the path planning model. Finally, the path following process based on the quadrotor helicopter PID controllers is introduced to verify the

Y. Chen · J. Yu () · X. Su · G. Luo School of Aerospace Engineering, Beijing Institute of Technology, Beijing 100081, China e-mail: [email protected] Y. Chen e-mail: bit [email protected] X. Su e-mail: [email protected] G. Luo e-mail: gc [email protected] Y. Chen · J. Yu · X. Su · G. Luo Key Laboratory of Dynamics and Control of Flight Vehicle, Ministry of Education, Beijing 100081, China

rationality of the path planning results. The simulation results show that the artificial potential method with the additional control force improved by the optimal control method has a good path planning ability for the single UAV and the all UAVs formation. At the same time, the path planning results are available and the UAVs can basically track the UAV formation. Keywords Multi-UAV path planning · Virtual velocity rigid body · Virtual target point · Optimal control · The artificial potential field · Path following

1 Introduction The multi-UAV path planning is a process that the UAVs find their own paths from their starting points to their destinations cooperatively. It has been a research focus in recent years. Of course, it is based on the path planning of the single UAV. Comparing to the single UAV path planning, the multi-UAVs need to deal with their cooperative relationships. As the formation fly is an important cooperative way in many situations, the concept of the formation path planning is presented to solve the multi-UAV path planning problem. Specifically, the formation path planning means each UAV finds its own collision free path and simultaneously tries to keep their formation structure. Thus the key to solve the multi-UAV formation path planning is to solve: the single UAV path planning, the

J Intell Robot Syst

unique selection of the formation structure, the rule of the formation variation and refactoring process. The main problems in multi-UAV path planning are the single UAV path planning, formation constitution, formation configuration and formation retention. Some mature methods have been set up for the single UAV path planning in recent years, such as: A * method [1], artificial potential field method [2], receding horizon algorithm [3] and a large number of intelligent algorithms [4, 5]. Although these methods have solved the single UAV path planning successfully, most of them are difficult to be applied in the multiUAV formation path planning immediately. There are many researches being put forward to study the formation constitution, configuration, retention and control. Anderson B. D. O. and Yu Changbin did some research on fixed-formation system configuration [6]. Xiaohua Wang successfully solved the problem in UAV formation constitution combining optimal control and model predictive control [7]. At the aspect of the multi-UAV formation retention, Das A V, Fierro R. established the formation model based on the combination of bionics and vision [8]. Chang BoonLow solved the multi-UAV path planning based on dynamic virtual structure [9]. All the above methods provide a solid foundation to solve the multi-UAV formation path planning problems. But their researches focus on either the single UAV path planning or the formation method separately. The combination of them isn’t considered for the multi-UAV formation path planning. Some researchers also have provided some methods for the multi-UAV path planning. For example, C. R. McInnes built the single UAV and multi-UAV path planning model based on velocity potential field [10]. Alberto Bemporad and Claudio Rocchi applied the Model Predictive Control (MPC) method on the multiple rotor UAVs for path planning [11]. However, most of the above methods are applied in the twodimensional plane. And most of them only are based on the simple planning spaces. Moreover, the results of the multi-UAV path planning aren’t optimized by given indexes. Moreover, the concept of the formation fly is weakened during the path planning process. This paper classifies the multi-UAVs by the lead plane and wingmen. The virtual rigid body is established based on the position and attitude of the lead plane. After arranging the virtual target point on the virtual rigid body, the artificial potential field method

improved by optimal control method is used in the single UAV path planning and UAV formation configuration. The multi-UAVs in the formation can obtain their own collision free paths and keep their formation structure simultaneously by applying this method. In order to verify the rationality of the path planning result, we use the multiple quadrotor helicopters to follow the planning results. And the whole path following control system is designed by the PID control method. The structure of this paper is as follows: In the second section, the single UAV path planning optimization model using the artificial potential field method with additional control is built on the single UAV particle dynamic system. In the third section, the multi-UAV formation path planning model is built with the introduction of the virtual speed rigid body and the virtual target point. In the fourth section, the functional optimization model with constraints is solved by the optimal control method approximatively. Furthermore, the path planning results are obtained by substituting the control force into the UAV’s particle dynamics system. In the fifth section, the PID controller system for the path following process is designed on the quadrotor helicopter platform. In the sixth and seventh section, the path planning and path following results of the single UAV and multi-UAV formation are obtained and analyzed using the Matlab simulation in the simple and complex planning spaces. In the last section, some conclusions are drawn and further discussions are presented.

2 Single UAV Particle Dynamics and Optimization Model in the Path Planning Process 2.1 Single UAV Particle Dynamics in the Path Planning Process In order to simplify the algorithm, we can use the simplified particle dynamics to replace the real dynamic model in the path planning process. Then, the single ith UAV particle dynamics is based on the APF method with the additional control force. The main idea of the APF method is to use repulsive potential fields emanating from the obstacles to force the UAV away and an attractive potential field emanating from the target as to attract the UAV [12].

J Intell Robot Syst

The forms of the repulsive field functions and attractive field functions are various. Many entirely different field functions have been used in the references [13]. The repulsive field and attractive field are defined out of its simple mathematical form as follows: The repulsive field φ i (pi ) is caused by the obstacles and it has a safe space that the repulsive field can only exist at a safe distance. So the UAV needs to search the nearest points on the different obstacles to check whether it has entered into the safe space. The repulsion caused by the whole obstacles can be simplified into causing by the nearest points. 2 It is defined as φi (p i ) = kφ e−lφ pi  [14], where kφ and lφ mean the alterable coefficients of the repulsive field (kφ >0, lφ >0). The repulsion is the negative gradient of repulsive field and is defined as F c = 2 −∇φi (pi )= 2kφ lφ p i e−lφ pi  , where pi is the vector that the UAV points to the nearest point on the obstacles. The repulsion can help the UAV avoid the obstacles during the path planning process. According to the form of the repulsion Fc , the change of the function value is shown in Fig. 1 [12]. Thereinto, the value A and B are based on the alterable coefficients kφ and lφ . For the repulsion, it generally demands that the smaller ||pi || causes the bigger repulsion ||Fc || (Hatched Area). So the value A needs to be much less than the size of the safe distance. The target point generates the attractive field filled full planning space whose potential field is ϕ(p∗i ), where p∗i is the vector that the UAV points to the target point. The attraction is the negative gradient of the

attractive field and is defined as F y = −∇ϕ(pi∗ ) = p∗

−2ky pi∗  , where ky. means the alterable coefficient i of the attractive field. For the different planning space, kφ , lφ and ky need to vary with the size of the planning space. The resultant force joined by the potential field force and control force is used to control the UAV to avoid collision and reach target. The particle dynamics system of UAV under resultant force is [12, 15]:     t 2 I tI I 2 X(k + 1) = X(k) + a(k), (1) 0 I tI where X(k) is the motion state of UAV at time k which includes the speed v(k) and position p(k) of  p(k) UAV at time k and the expression is X(k) = . v(k) a(k) is the resultant force of potential field force q(k) (repulsion and attraction), the additional control force u(k) and damping force Fz (k) (Fz (k)=kz v(k), where kz means the drag coefficient). 2.2 Optimization Model for the Single UAV Particle Dynamics The optimization model is widely used in the single UAV path planning problem. Importantly, the planning path of the UAV can be optimized by given indexes. So the optimization model for the single UAV particle dynamic can be expressed as [16]: minu(k) J (X(k), u(k)) s.t. F ((X(k), u(k)) = 0 G((X(k), u(k)) > 0

(2)

2.2.1 Objective Function

Fig. 1 Sketch map of the repulsion

It is known that searching for the best path is often associated with searching for the shortest path. Obviously, the shorter path can save more fuel, save more time and give more security. At the same time, the smaller additional control force of the UAV is also important for the UAV path planning process. The control force is directly related to the energy. So the objective function of the optimization model for the single UAV particle dynamics is based on the weighted indexes of the shortest path Js and the least energy Je [17]. The indexes try to make the additional

J Intell Robot Syst

control energy consumption as small as possible and the path as short as possible in the premise of completing the path planning. Although the contradiction of the two indicators is clear, the weighted model can adjust the weight to achieve the purpose of adjusting optimization objective. The expression is:

Boundary conditions: according to the path planning process, the UAV flies from the starting point S to the target point T. So the boundary constraints of the optimization model are as follow, i.e. p(0)= S, p(n)= T.

minu(k) J = αJ s + βJ e

2.2.3 Single UAV Optimization Particle Dynamics Model

=

n−1 

(α p(k + 1) − p(k)2 + β u(k)2 )

k=0

(3) where α and β are the adjustment coefficients of the objective function (α >0, β >0).

In summary, the optimization model of the single UAV particle dynamics is:

min u(k)

2.2.2 Optimum Conditions The path planning results need to be applied in the real UAV automatic flight control system, so the reasonable optimization conditions are the premise of the accuracy and reasonableness of the optimization model. Considering the whole UAV flying process, the optimum conditions in the optimization model for the single UAV particle dynamic include performance constraints, spatial constraints, dynamic constraints and boundary conditions. Performance constraints: there are speed constraints and overload constraints on the UAV body. Only in the reasonable constraint range of the speed and overload, the path planning results are trackable for the real UAV control system. So the performance constraints which include the speed constrains and overload constraints are very necessary for the optimization model. For simplicity, the form of the overload constraints and speed constraints is similar to the form of speed constraints u ≤ um in the suitable artificial field. Spatial constraints: obviously, the UAV needs to avoid the obstacles in the planning space. In other words, the path planning points are not allowed to appear in the obstacles. The existence of the repulsion fields keeps the UAV away from the obstacles. So the spatial constraints can be satisfied by the repulsive field of the artificial potential field method [12]. Dynamic constraints: the optimization variable u(k) and state variable X(k) are not independent. Equation 1 shows the differential and integral relationship among the force, velocity and acceleration of UAV.

J = αJ s + βJ e 

X(k + 1) = s.t.

(4)

   t 2 I tI I 2 X(k) + a(k) 0 I tI . u ≤ um p(0) = S p(n) = T

3 Multiple UAVs Path Planning Model 3.1 Multiple UAVs Formation Configuration The formation structure of multi-UAVs is divided into the close formation and loose formation according to whether there is aerodynamic coupling [18]. Because of the complex aerodynamic coupling in close formation, the close formation configuration achieves the goal of saving funnel and expanding combat radius based on bionics principles. But the mechanism of the close formation is complex and changeful. So the loose formation is also used widely. The loose formation has variable formations considering the communication and combat requirements. In this paper, the whole formation structure is on the basis of the loose formation (lead plane- wingmen) without regard to the aerodynamic coupling. Specifically, the loose UAV formation applied in this paper is a herringbone formation and its size is shown in Fig. 2. The herringbone formation is similar to the flight mode of the geese flock and is widely used in close formation and loose formation. The concept of “virtual speed rigid body” is introduced to achieve the lead plane- wingmen topological structure of UAVs formation model. “Virtual speed rigid body” is the virtual rigid body model based on the velocity vector of

J Intell Robot Syst Fig. 2 Schematic diagram of the multi-UAV formation

wingman

wingman

Lead plane 120deg

which is perpendicular to x  in the vertical plane; z axis is obtained according to the right-hand rule. The conversion relationship between the coordinate in virtual rigid body coordinate system and coordinate in inertial coordinate system is shown as follows: ⎡ ⎤ ⎡ ⎤ x x ⎣ y  ⎦ = AB ⎣ y ⎦ , (5) z z ⎡

200m wingman

wingman

the lead plane. The rigid body of the whole multiUAV formation relies on the velocity vector of the lead plane. And the ideal position of each wingman in formation is S i . In other words, S i is the virtual target point of the i-th wingman. The virtual velocity rigid body is shown in Fig. 3. The lead plane locates in the “head” of the herringbone formation and the attitude angle of the lead plane is used to determine the attitude angle of the rigid body. Since this paper uses the UAV particle dynamics model, the attitude information of the rolling channel is already out of consideration. The velocity vector of the lead plane is defined as the x  axis of virtual rigid body coordinate system; y  axis is defined as the axis

⎤ cos θ sin θ 0 A = ⎣ − sin θ cos θ 0 ⎦ , 0 0 1 ⎤ cos ψ 0 − sin ψ B=⎣ 0 1 0 ⎦, sin ψ 0 cos ψ

⎡

(7)

where [x y z ]T is the coordinates in the virtual rigid body coordinate system for the UAVs; [x y z]T is the coordinates in inertial coordinate system for the UAVs; θ is the inclination angle between the velocity vector of the lead plane and horizontal plane; ψ is the yaw angle between the velocity vector of the leading plane and horizontal plane. 3.2 Motion Model for Multi-UAV Formation The motion model for the multi-UAV formation is based on the single UAV particle dynamic model and the multi-UAV formation configuration (Fig. 4). With the introduction of the virtual velocity rigid body, the path planning model for the lead plane is independent. Then, the wingmen need to follow the virtual position on the virtual velocity rigid body to maintain the UAV formation. So “virtual target point” is introduced to help the wingmen to keep the formation during their own path planning processes. In other words, the ideal positions of the wingmen are determined by the virtual target point and the virtual velocity rigid body. The virtual target points become the planning targets of the wingmen. Importantly, the wingmen use the same path planning method as the lead plane. So the functional relationship is as follows: X(k) = AP F (k − 1, T , O) T ∗i = X(k) + B T AT S i , X∗i (k) = AP F (k − 1, T ∗i , O)

Fig. 3 Schematic diagram of the virtual velocity rigid body

(6)

(8)

where AP F is the path planning function of the artificial potential field method with the additional control

J Intell Robot Syst Fig. 4 Information flow of the multi-UAV formation

Path planning

Path planning

APF Target point

Lead plane

u

force; i is the number of UAVs, T is the target point of the lead plane; T ∗i is the virtual target point of each wingman; O are the set of the obstacles; Si is the ideal coordinate of each wingman in the body-fixed coordinate system; X∗i is the actual path planning state of each wingman; X is the actual path planning state of the lead plane. 3.3 Collision-avoidance Model for Multi-UAVs The UAVs may collide with each other without a proper arrangement in the formation. The repulsion field of the artificial potential field method is introduced to handle this problem. When the distance between each two UAVs is less than the danger value, the repulsion field of UAV works only on the other UAVs and won’t work on itself. At the same time, the path planning of the lead plane won’t be affected by the repulsion fields of the wingmen so as to keep the dependence of the lead plane path planning. Therefore, the wingman is subjected to the repulsion field of other wingmen and the lead plane. The Fig. 5 Repulsive force field between the multi-UAVs

Virtual

rigid body

Virtual target point

APF Wingman

u

safe radius of the collision-avoidance potential field is 20m. It means the UAV will be subjected to the repulsion when it gets into the other UAVs’ safe radius (Fig. 5). Remark 1 Sometimes the virtual target points of the wingmen may locate in the obstacles, according to the formation structure of Section 3.1. The main reason is the virtual target points of the wingmen are based on the planning path of the lead-plane. Although the leadplane can avoid the obstacles easily under the help of the path planning method, the virtual speed rigid body and virtual target point may enter into the obstacles, as shown in Fig. 6 below. So in order to prevent the wingmen from flying into the obstacle, the repulsion force is much bigger than the attractive force. In other words, the collision avoidance is important than the formation fly. If the UAV flies into the repulsive force fields of the obstacles and the other UAVs, it will be manifest as giving up the formation. So the formation errors are big when they fly into the repulsive force fields. What’s more, it is not appropriate to judge the

J Intell Robot Syst

The cost function is rewritten as: J =α

n−1 

p(k + 1) − p(k)2 + β

k=0

=α

n−1  

  

t 2 2 G(k)

 tI

k=0

+β

n−1 

n−1 

a(k)2

k=0

p(k) v(k)



+

2  t 2 u(k)  2

u(k)2

k=0

(10) where G(k) is the simplified transitive matrix that varies with p(k) and r(k). It fits: Fig. 6 Special circumstances

r(k) = G(k)p(k).

formation-flight performance by the big errors during these situations.

In order to transform Eq. (10) into the quadratic form, partition the following quadratic form matrixes as: ⎡ 4 ⎤ t T (k)G(k) t 3 G(k) t 4 G(k) G 2 4 ⎢ 4 t 3 T ⎥ t 3 , (12) ⎣ t 2 I 2 G (k) 2 I ⎦ t 4 T 4 G (k)

4 Solution of Optimal Control for the Path Planning Optimization Model

t 3 2 I

t 4 4 I

⎡

The main difference between the lead plane and wingmen in the path planning process is the target point. The lead plane is in charge of the overall path while the wingman is responsible for maintaining the formation. But all UAVs can use the same method to solve their own single path planning problems. Then, the optimal control is applied in solving the optimization model. The general form of discrete optimal control system is:

⎤ 0 0 0 ⎣0 0 0⎦. 0 0 I Define:

k=k0

(9)



Q (k) =  Q1 =

−1  kf minu(k) J = φ x(kf ), kf + ϕ(x(k), u(k), k)

x(k + 1) = g(x(k), u(k), k) s.t. h(x(k), u(k), k) ≥ 0 x(k0 ) = x0

(11)

 S(k) =

.

The optimization model of the UAV dynamic model needs to be converted to the general form of the optimal control problem and it includes the conversion of the target functions and conversion of the constraint conditions. Because this paper focuses on the path planning for the multi-UAV formation, this solution procedure is sententious. The similar detailed derivation process is shown in reference [12].

R=

t 4 T t 3 4 G (k)G(k) 2 G(k) 3 t T t 2 I 2 G (k)

t 4 4 G(k) t 3 2 I



 ,

 0 0 , 0 0

t 4 I, 4

(k) =

(13)

(14)  ,

  0 S1 = , 0

(15)

R1 = I,

2

I + t2 G(k) tI tG(k) I

(16) 

 ,

=

t 2 2

t

 .(17)

With the definition of Eqs. 14–17, the cost function is formed into the quadratic form, and the dynamic

J Intell Robot Syst

constraint is formed as the state equation. By now, the optimization model is rewritten as: minu(k) J = 

1 2

n−1   k=0

X T (k) uT (k)

Then[19, 20]: u (k) =

T

umin (k) =

  2αQ(k) + 2βQ1 2αS(k) + 2βS1 X(k) 2αS T (k) + 2βS1T 2αR + 2βR1 u(k) ⎧ X(k + 1) = (k) X(k) + u(k) ⎪ ⎪ ⎪  T ⎨ X(0) = p Ts vT (0) s.t.  T T T ⎪ X(n) = p t v (n) ⎪ ⎪ ⎩ u(k) ≤ um

u• (k) =

 u (k) u (k) u (k) T x y z u

min x

(k) umin y (k) umin z (k) T

,

⎧ ⎨

um (umin • ≥ um ) umin •(k) (umin (k) ∈ U ) ⎩ −um (umin • ≤ −um ) (24)

(18)

where the subscript • represents x, y, z and umin (k) is the optimal control u (k) without the control constraint, which is:

Q2 (k) = 2αQ(k) + 2βQ1 ,

(19)

umin (k) = −R˜ −1 (k) T (P −1 (k + 1) +  R˜ −1 (k) T )−1  (k)X  (k),

S2 (k) = 2αS(k) + 2βS1 ,

(20)

R2 = 2αR + 2βR1 .

(21)

Define:

Go on with the variable substitutions: ˜ Q(k) = Q2 (k) − S2 (k)(R2−1 )T S2T (k) R˜ = R2 

(k) = (k) −  =



(25)

where P(k) can be calculated by the corresponding ˜ ˜ matrixes  (k),  , Q(k) andR(k), via the Riccati equation: ˜ P (k) =  (k)P (k + 1)  (k) + Q(k) −  (k)P (k + 1)    −1 T T R˜ +  P (k + 1)   P (k + 1)  (k). (26) T

T

(22)

(R2−1 )T S2T (k).

X (k) = X(k) u (k) = (R2−1 )T S2T (k)X(k) + u(k) All above, the general form of the discrete optimal control that satisfies Eq. (9) is obtained from the conversion of the constraint conditions and cost function of the optimization model Eq. (3). 

1 T  ˜ minu(k) J = n−1 k=0 2 X (k)Q(k)X (k)  ˜  (k) + 12 uT (k) Ru ⎧  X (k + 1) =  (k)X (k) +  u (k) , ⎪ (23) ⎪ ⎪  T ⎨ X (0) = p Ts vT (0) s.t.  T ⎪ X  (n) = p Tt vT (n) ⎪ ⎪ ⎩ u (k) ∈ U where U is the allowable interval of the control force. By the help of the optimal control method for the discrete optimal control system, we can get the approximate result u .

Then, the result of optimal control of discrete optimal control system with constraints: u (k) is obtained from the former equations Eq. 24 and u(k) is obtained according to Eq. 22. u(k) can be substituted into the UAV dynamics equations. Each UAV can get its own planning path by the help of its u(k).

5 Path Following Control It’s known that the UAV path planning model is built on the artificial potential field method. The rationality of the path planning result can’t be guaranteed considering some constraints. So after the multi-UAV path planning process, the path following control is an important link to verify the rationality of the path planning result. In the path following process, the real dynamic model of UAVs needs to be applied to simulate the real multi-UAV formation flying state. The quadrotor helicopters are used to follow the planning results which include the position information of the lead plane and wingmen. Obviously, the position information is time-varying [12].

J Intell Robot Syst

5.1 Dynamics Model of the UAVs in Path Following Process The quadrotor helicopter is an under actuated aircraft with fixed four pitch angle rotors as shown in Fig. 7 [12, 21]. It has several obvious characteristics such as: highly nonlinear, strongly coupled and underactuated system. In the structural diagram of the quadrotor helicopters, ψ  , θ  , φ  represent the Euler angles between the body-axis coordinate system B =(X b , Y b , Z b ) and the inertial coordinate system E=(X be , Y be , Z be ). So via the Newton’s laws and Euler equation, the simplified dynamic model of the UAV can be obtained as follows [22]: x¨b = y¨b = z¨ b = φ¨  = θ¨  = ψ¨  =

1 (cos φ  cos ψ  sin θ  + sin φ  sin ψ  )uc1 − m 1 (cos φ  sin ψ  sin θ  − sin φ  cos ψ  )uc1 − m 1 Kz (cos θ  cos φ  )uc1 − z˙ b − g m m   Iy − Iz l uc2 + θ˙  ψ˙  , Ix Ix   l Iz − Ix uc3 + φ˙  ψ˙  Iy Iy   − Iy I 1 x uc4 + φ˙  θ˙  Iz Iz

Kx x˙b (27) m Ky y˙b m

Y b , Z b ; uci (i=1,2,3,4) satisfy the following equations [23]: uc1 uc2 uc3 uc4

= b(ω12 + ω22 + ω32 + ω42 ) = b(ω22 − ω42 ) , = b(ω12 − ω32 ) = d(ω12 − ω22 + ω32 − ω42 )

where b is the scaling factor of the thrust generated by the rotor; d is the scaling factor of the propeller torque constant generated by the rotors. Above models are based on the following assumptions [24]: (1) The UAV structure is symmetrical and rigid; (2) The center of mass and Ob coincides; (3) Thrust and drag are proportional to the square of the propellers speed; (4) Ignore the rotational resistance of each UAV; (5) Ignore the effect of noise. 5.2 PID Control for Formation Path Following The PID controllers are mentioned in many references. The PID controllers are the standard tools in current industrial automation experience thank to their flexibility. The PID controller takes many structures but the most important one is in the following form [25]: t

where xb , yb , zb are the positions of the UAV;m is the mass of the UAV; g is the acceleration of gravity; l is the half length of the quadrotor helicopter;K x , K y , K z mean the drag coefficients; I x , I y , I z are the moments of the inertia with respect to the axes X b ,

ufollow (t) = KP e(t) + KI

e(τ )dτ + KD

de(t) . (29) dt

0

The algorithm of the PD controller is shown as follows: ufollow (t) = KP e(t) + KD

Fig. 7 Quadrotor helicopter structural diagram

(28)

de(t) , dt

(30)

where uf ollow (t) is the control variable, the tracking error e(t) is defined as e=W r -W where W r is the scheduled output and W is the real output. K P , K I and K D are controller gains associated with proportional (P), integral (I), and derivative (D) actions, respectively. In Section 4, we obtain the path planning results of the whole UAV formation which include the lead plane and wingmen. Those time-varying results are the input of the whole path following control system. Since the main research content of this paper is focused on the multi-UAV path planning, the simple traditional PID controller and PD

J Intell Robot Syst

controller are applied to solve the path following problem for the whole multi-UAV formation. The structure of the control system is shown as followed: In the structure chart, the horizontal position PD controller is as follows [23]: θri (t) = arcsin(Kpx (xpi (t) − xi (t)) + Kdx (x˙pi (t) − x˙i (t))) , φri (t) = − arcsin(Kpx (ypi (t) − yi (t)) + Kdx (y˙pi (t) − y˙i (t))) (31)

The altitude PD controller with the nonlinear compensation is as follows: Fig. 9 Following result of the simple example

1 uc1i (t) = (K z (zp −z)+Kdz (˙zp −˙z)+mg).  cos φi cos θi p (32) In order to verify the effectiveness of this control system, a simple example is provided. Concretely, in a small scale example, the UAV takes off from [0, 0, 0] to [4, 8, 0] using a predetermined orbit which is shown in Fig. 9. Moreover, the magnitude of the velocity remains 2m/s and the direction changes with the predetermined orbit. Finally, the result is shown in Fig. 9. The red line is the reference trajectory, and the blue line means the real path following result which uses the control system in Fig. 8. Figure 9 shows the simulation results of the path following of this reference trajectory. It can be seen that the following error is small and in

an acceptable range. The speed of the UAV is as follows: Figures 9 and 10 show the validity of the control system, and these results provide some help about the rationality for later simulation work.

6 Simple Simulation Examples 6.1 Planning Space and Constraint Conditions The formation simulation is carried out using 5 UAVs and it is a simple no-obstacle space. This simple simulation is provided in order to evaluate formationflight performance on simple scenario without the

Fig. 8 Control system chart

xbi (t ), ybi (t ), zbi (t )

ri

x pi (t )

y pi (t )

'i (t ), 'i (t ), 'i (t )

(t ) ri

(t )

ri

(t )

uc 2i (t )

uc 3i (t ) uc 4i (t )

z pi (t ) uc1i (t )

'i (t ), 'i (t ), zbi (t )

J Intell Robot Syst Fig. 10 Flying state of the simple example

effects of the obstacles. The size of the planning space is as follows: the range of x axis: [1500m, 6000m]; the range of y axis: [1500m, 7000m]; the range of z axis: [-50m, 1200m]. The target point of the UAV group is [6000m, 7000m, 1000m]. The specific circumstances of the starting points are as follows:

whose shape is always straight. The result is shown in Fig. 11. The simulation result is a straight space curve which is in line with expectations. But because the simulation scenario is too simple, it is not enough to verify the effectiveness of the single UAV path planning method.

6.2 Path Planning of Lead Plane

6.3 Path Planning of Multi-UAV Formation

In the no-obstacle space, the path planning result of the lead plane is a very simple space curve

The path planning of the multi-UAV formation uses the initial parameters which are listed in Table 1. In the no-obstacle space,the simulation result of the multiUAV formation is shown in Fig. 12. Figure 12 indicates that the UAVs set out from the different starting points and form the formation when they are flying. The UAVs fly in the expected formation and reach the target point at the same time. In order to describe the formation-flight performance exactly, the formation error results of four wingmen are shown in Fig. 13. The formation error Table 1 Table 1 Starting points coordinates of the UAVs

Fig. 11 Single UAV path planning result in a no-obstacle space

UAV

(x, y, z)/m

Lead plane (blue) Wingman 1 (black) Wingman 2 (red) Wingman 3 (yellow) Wingman 4 (green)

(1500,2000,500) (2000,1500,200) (1500,1500,400) (1500,2000,500) (1500,4000,500)

J Intell Robot Syst

From these results, the path planning of multi-UAV formation can be divided into two different stages: In the beginning of the path planning process, the UAVs set out from the different starting points. The virtual target points of the wingmen are faraway from the different starting points. In the other word, they do not form the formation yet. So the formation errors are big.The results are reasonable. After they formed the multi-UAV formation, the whole multi-UAV formation gets into the stableflying state. Figure 13 shows that the formation errors of the path planning results are all below 2m (Amplifying part in the last figure). In conclusion, the formation-flight performance is very good in the simple no-obstacle space. 6.4 Path Following of Multi-UAV Formation Fig. 12 Single UAV path planning result in a no-obstacle space

means that the distance between the path planning position of the i-th wingman and its corresponding virtual target point (wingman i corresponds to distance i ).

The rationality of the multi-UAV formation path planning results needs to be verified by the path following process. Naturally, the path planning results containing the whole formation position information become the input of the path following control system. Because of using the same control system, the effects of the path following are no differences between the lead plane and wingmen in the path following

Fig. 13 Formation error results of four wingmen in a no-obstacle space

J Intell Robot Syst Fig. 14 Path following error of multi-UAVs in a no-obstacle space

process. No matter the lead plane or wingmen, the all path following simulations are finished. The path following result is shown in Fig. 14. Where the red lines mean the real path following results, and the blue lines mean the multi-UAV path planning results. During the whole path following process, the path following error of each UAV is all below 2m, and the velocity and the acceleration are all below the given upper limits. So the control system can help the multi-UAV formation to achieve the multi-UAV formation path planning successfully in simple on-obstacle space.

7 Complex Simulation Examples

polation. The obstacles include: alpine terrain, radar zone, bad weather zone and so on. These obstacles are very common in the real battlefield environment. The specific circumstance is as follows (Fig. 15): All obstacles including the radar areas and terrain in the planning space are regarded as the no-fly zone where the trajectories of UAVs cant be allowed into. The horizontal velocity constraint of the UAV is vzh = 10 m/s, the vertical velocity constraint of the UAV is vzm = 5 m/s; the horizontal acceleration constraint of the UAV is azh = 10m/s2 , the vertical acceleration constraint of the UAV is azm = 10 m/s2 ; the flight height constraint of the UAV is z > z (surface), where z (surface) means the interpolation result of the planning space.

7.1 Planning Space and Constraint Conditions

7.2 Path Planning of Lead Plane

In the complex environment, the complicated and actual planning space is built by the MATLAB inter-

The flight status of the single UAV is the same as the lead plane in the formation, so the prior choice is

Fig. 15 Planning space in a complex simulation

J Intell Robot Syst Fig. 16 Single UAV path planning result in a complex simulation

verifying the path planning ability of the single UAV. The optimal control artificial potential field method is applied in the path planning from the starting point (lead plane) to the target point. The result is shown in Fig. 16. The simulation result shows that this method has a good path planning ability in the given planning space and it overcomes the problem of the dead zone in the path planning procedure. Combining with the simple simulation result, it is enough to verify the effectiveness of the single UAV path planning method. 7.3 Path Following of Multi-UAV Formation The path planning of the multi-UAV formation uses the initial parameters which are listed in table 1. The

Fig. 17 Multi-UAV formation path planning result in a complex simulation

simulation result of the multi-UAV formation is shown in Fig. 17. Figure 17 indicates that the UAVs set out from the different starting points and form the formation when they are flying. The UAVs fly in the expected formation and reach the target point at the same time. In order to describe the formation-flight performance exactly, the formation error results of four wingmen are shown in Fig. 18. The simulation results show that the formation errors are big in the influence area of the obstacles, and the formation errors are small in the noobstacle area (below 8m). Considering the remark 1 in Section 3.3, these results are reasonable. Therefore, the path planning of the multi-UAV formation is very successful in the complex simulation environment.

J Intell Robot Syst

Fig. 18 Formation error results of four wingmen in a complex space

7.4 Path Following of Multi-UAV Formation The rationality of the multi-UAV formation path planning results in the simple no-obstacle space are shown in Fig. 13. But in the complex environment the multiUAVs face the effects of the multifold obstacles. So the rationality needs to be further verified by the path following process in the complex environment. The path following result is shown in Fig. 19 where the red lines mean the real path following results, and Fig. 19 Path following error of multi-UAVs in a complex simulation

the blue lines mean the multi-UAV path planning results. Because the research focused in this paper isn’t the control system, we only show the simulation result of the lead plane which can be regarded as a representative of the whole formation. The 3-axis path following error of the lead plane is shown in Fig. 20. Then the flight states are shown in Fig. 21. According to the path following error, there are six different flying stages: In the first section (0- 300s), the

J Intell Robot Syst

Fig. 20 Following error result of the lead plane

multi-UAVs set out from the different starting points. Obviously, the flying stages of formation are fluctuating and unstable. In the section (1000- 2700s), they enter the first radar obstacle area. Then, the UAVs need to give up the formation. Under the effect of the repulsion, the path planning results are erratic. This phenomenon is called as the oscillation problem. In the presence of obstacles, the multi-UAVs enter into

Fig. 21 Flying state of the lead plane

oscillatory and unstable motion which is one of four inherent defects of the APF method. The four inherent defects are shown in [26]. The degree of the oscillation is based on the time step t and the repulsion gradient. The bigger time step t and repulsion gradient will cause the stronger oscillation. Then the multi-UAVs need to follow these erratic path planning results, so the paths following errors are big. In the sections

J Intell Robot Syst

(3300- 4000s), they enter the second radar obstacle area. Uniformly, they face the same problems during this process. In the other sections (300- 1000s, 28003300s, 4000- 5600s), the whole formation gets into the stable flying state. Their path planning processes are similar to ones in the simple simulation. The path planning results are straight and stable. So the tracking error is below 1m when they get into the stable flying state, comparing to the UAV formation scale which is 1 %. All above, we can see the velocity and the acceleration are all below the given upper limits which showed in the Section 6.1. Therefore, these results verify the rationality of the path planning algorithm. The control system can help the multi-UAV formation to achieve the multi-UAV formation path plannin g successfully.

5.

6.

7.

8.

9.

8 Conclusion 10.

Based on the APF method updated by the additional control force, the virtual velocity rigid body and the virtual target point have been introduced to solve the multi-UAV formation path planning problem. And the optimal control method is applied to solve the additional control force. This new method is favorable for the single UAV path planning in the given planning space. At the same time, it can also solve the multi-UAV formation path planning problem by applying the virtual structure. Then, the real UAV control system designed by the simple PID controller is used to verify the results of path planning. Simulation tests revealed: 1) the effectiveness of the path planning method; 2) the feasibility and availability of the real path following according to the path planning result.

11.

12.

13.

14.

15.

References 1. Garcia, M., Viguria, A., Ollero, A.: Dynamic Graph-Search Algorithm for Global Path Planning in Presence of Hazardous Weather (2012) 2. Khatib, O.: Real-time Obstacle Avoidance for Manipulators and Mobile Robots. Int. J. Robot. Res. 5(1), 90–98 (1986) 3. Dunbar, W.B., Caveney, D.S.: Distributed Receding Horizon Control of Vehicle Platoons: Stability and String Stability. IEEE Trans. Autom. Control 57(3), 620–633 (2012). doi:10.1109/TAC.2011.2159651 4. Pei, L., HaiBin, D.: Path Planning of Unmanned Aerial Vehicle Based on Improved Gravitational Search Algo-

16.

17.

18.

rithm. Sci. China Technol. Sci. 55(10), 2712–2719 (2012). doi:10.1007/s11431-012-4890-x Roberge, V., Tarbouchi, M., Labonte, G.: Comparison of Parallel Genetic Algorithm and Particle Swarm Optimization for Real-time UAV Path Planning. IEEE Trans. Ind. Inform. 9(1), 132-141 (2013). doi:10.1109/TII.2012.2198665 Anderson, B.D.O., Yu, C., Baris, F.: Information Architecture and Control Design for Rigid Formations. In: 26th Chinese Control Conference, 26-31 July 2007, Piscataway, NJ, USA 2007. Proceedings of the 26th Chinese Control Conference. IEEE (2007) Xiaohua, W., Yadav, V., Balakrishnan, S.N.: Cooperative UAV Formation Flying with Obstacle/Collision Avoidance. IEEE Trans. Control Syst. Technol. 15(4), 672–679 (2007). doi:10.1109/TCST.2007.899191 Das, A.K., Fierro, R., Kumar, V., Ostrowski, J.P., Spletzer, J., Taylor, C.J.: A Vision-based Formation Control Framework. IEEE Trans. Robot. Autom. 18(5), 813–825 (2002). doi:10.1109/TRA.2002.803463 Chang Boon, L.: A Dynamic Virtual Structure Formation Control for Fixed-wing UAVs. In: 2011 9th IEEE International Conference on Control and Automation (ICCA 2011),19-21 Dec. 2011, Piscataway, NJ, USA. IEEE (2011) McInnes, C.R.: Velocity Field Path-planning for Single and Multiple Unmanned Aerial Vehicles. Aeronaut. J. 107(1073), 419–426 (2003) Bemporad, A., Rocchi, C.: Decentralized Linear Timevarying Model Predictive Control of a Formation of Unmanned Aerial Vehicles. In: 2011 50th IEEE Conference on Decision and Control and European Control Conference (CDC-ECC 2011), 12-15 Dec. 2011, Piscataway, NJ, USA, 7488-7493 (2011). IEEE Chen Y.-b., Luo G.-c., Mei Y.-s., Yu J.-q., Su X.-l.: UAV path planning using artificial potential field method updated by optimal control theory. Int. J. Syst. Sci. (2014). doi:10.1080/00207721.2014.929191 Cetin, O., Zagli, I., Yilmaz, G.: Establishing Obstacle and Collision Free Communication Relay for UAVs with Artificial Potential Fields. J. Intell. Robot. Syst. 69(1-4), 361– 372 (2013) Charifa, S., Bikdash, M.: Comparison of geometrical, kinematic, and dynamic performance of several potential field methods. In: IEEE SoutheastCon 2009, 5-8 March 2009, Piscataway, NJ, USA. IEEE (2009) Luo, G.-c., Yu, J.-q., Zhang, S.-y., Zhang, W.: Artificial Potential Field based Receding Horizon Control for path planning. In: 2012 24th Chinese Control and Decision Conference (CCDC), 23-25 May 2012, Piscataway, NJ, USA. IEEE (2012) Zhang, X., Duan, H., Yu, Y.: Receding Horizon Control for Multi-UAVs Close Formation Control Based on Differential Evolution. Sci. China Ser. F (Inf. Sci.) 53(2), 223–235 (2010). doi:10.1007/s11432-010-0036-6 Yanyang, W., Tietao, W., Xiangju, Q.: Study of Multi-objective Fuzzy Optimization for Path Planning. Chin. J. Aeronaut. 25(1), 51–56 (2012). doi:10.1016/S1000-9361(11)60361-0 Hua, S., You, Y., Zhang, H., Song, H.: Receding Horizon Control of UAV Formations. Electron. Opt. Control. 19(3), 1–5 (2012)

J Intell Robot Syst 19. Wu, S.: Optimal Control Theory and Application. China Machine Press, Beijing (2008) 20. Xie, L.-j., Xie, G.-r., Chen, H.-w., Li, X.-l.: Solution to Reinforcement Learning Problems with Artificial Potential Field. J. Cent. South Univ. Technol. 15(4), 552–557 (2008). doi:10.1007/s11771-008-010421. Mohamed, H.A., Yang, S., Moghavvemi, M.: Sliding Mode Controller Design for a Flying Quadrotor with Simplified Action Planner. In: ICCAS-SICE, 2009. IEEE (2009) 22. Bouabdallah, S., Siegwart, R.: Full Control of a Quadrotor. In: 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems, 29 Oct.-2 Nov. 2007, Piscataway, NJ, USA 2007. IEEE (2007) 23. Bai, Y., Liu, H., Shi, Z., Zhong, Y.: Robust Flight Control of Quadrotor Unmanned Air Vehicles. Jiqiren/Robot

34(5), 519–524 (2012). doi:10.3724/SP.J.1218.2012. 00519 24. Salih, A.L., Moghavvemi, M., Mohamed, H.A., Gaeid, K.S.: Modelling and PID Controller Design for a Quadrotor Unmanned air Vehicle. In: Automation Quality and Testing Robotics (AQTR), 2010 IEEE International Conference on. IEEE (2010) ¨ Neural Network Assisted Computationally Sim25. Efe, M.O.: ple PID Control of a Quadrotor UAV. IEEE Trans. Ind. Informat. 7(2), 354–361 (2011) 26. Koren, Y., Borenstein, J.: Potential Field Methods and their Inherent Limitations for Mobile robot navigation. In: Robotics and Automation, 1991. Proceedings., 1991 IEEE International Conference on. IEEE (1991)