J Intell Robot Syst (2015) 77:229–246
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 / Published online: 20 July 2014
© Springer Science+Business Media Dordrecht 2014
Abstract This paper presents an efficient and feasible
algorithm for the path planning problem of the multi-
ple unmanned aerial vehicles (multi-UAVs) formation
in a known and realistic environment. The artificial
potential field method updated by the additional con-
trol 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 opti-
mal control method. Furthermore, the multi-UAV path
planning model is established by introducing “vir-
tual 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: jianqiao@bit.edu.cn
Y. Chen
e-mail: bit chenyongbo@163.com
X. Su
e-mail: suxiaolong@bit.edu.cn
G. Luo
e-mail: gc luo@eyou.com
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 simula-
tion 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 for-
mation. 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 sin-
gle UAV path planning, the multi-UAVs need to deal
with their cooperative relationships. As the formation
fly is an important cooperative way in many situa-
tions, the concept of the formation path planning is
presented to solve the multi-UAV path planning prob-
lem. Specifically, the formation path planning means
each UAV finds its own collision free path and simul-
taneously tries to keep their formation structure. Thus
the key to solve the multi-UAV formation path plan-
ning is to solve: the single UAV path planning, the
230
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 constitu-
tion, formation configuration and formation retention.
Some mature methods have been set up for the sin-
gle UAV path planning in recent years, such as: A *
method [1], artificial potential field method [2], reced-
ing horizon algorithm [3] and a large number of intel-
ligent algorithms [4, 5]. Although these methods have
solved the single UAV path planning successfully,
most of them are difficult to be applied in the multi-
UAV formation path planning immediately. There are
many researches being put forward to study the for-
mation constitution, configuration, retention and con-
trol. 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 con-
trol 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 com-
bination 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 for-
mation method separately. The combination of them
isn’t considered for the multi-UAV formation path
planning.
Some researchers also have provided some meth-
ods 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 two-
dimensional 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 estab-
lished 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
J Intell Robot Syst (2015) 77:229–246
improved by optimal control method is used in the
single UAV path planning and UAV formation config-
uration. The multi-UAVs in the formation can obtain
their own collision free paths and keep their forma-
tion 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 fol-
lowing 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 opti-
mization 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 approxi-
matively. 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 plat-
form. In the sixth and seventh section, the path
planning and path following results of the single
UAV and multi-UAV formation are obtained and ana-
lyzed 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 sim-
plified particle dynamics to replace the real dynamic
model in the path planning process. Then, the single i-
th 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 ema-
nating 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 (2015) 77:229–246
The forms of the repulsive field functions and
attractive field functions are various. Many entirely
different field functions have been used in the ref-
erences [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 obsta-
cles 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 obsta-
cles 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.
It is defined as φi (p i ) = kφ e−lφ pi  [14], where kφ
2
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 =
−∇φi (pi )= 2kφ lφ p i e−lφ pi  , where pi is the vec-
2
tor 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. Accord-
ing 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 tar-
get point. The attraction is the negative gradient of the
Fig. 1 Sketch map of the repulsion
231
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 resul-
tant force is [12, 15]:
 
  t 2
I tI I
X(k + 1) = X(k) + 2 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 plan-
ning 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
It is known that searching for the best path is often
associated with searching for the shortest path. Obvi-
ously, 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
232
control energy consumption as small as possible and
the path as short as possible in the premise of com-
pleting 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:
minu(k) J = αJ s + βJ e

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).
2.2.2 Optimum Conditions
The path planning results need to be applied in the
real UAV automatic flight control system, so the rea-
sonable 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 con-
straints 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 opti-
mization model. For simplicity, the form of the over-
load 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 repul-
sion 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 rela-
tionship among the force, velocity and acceleration of
UAV.
J Intell Robot Syst (2015) 77:229–246
Boundary conditions: according to the path plan-
ning 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.
2.2.3 Single UAV Optimization Particle Dynamics
Model
In summary, the optimization model of the single UAV
particle dynamics is:
min J = αJ s + βJ e (4)
u(k)
   
I tI t 2
I
X(k + 1) = X(k) + 2 a(k)
0 I tI
s.t. 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 forma-
tion, 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 for-
mation has variable formations considering the com-
munication 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 “vir-
tual speed rigid body” is introduced to achieve the
lead plane- wingmen topological structure of UAVs
formation model. “Virtual speed rigid body” is the vir-
tual rigid body model based on the velocity vector of
J Intell Robot Syst (2015) 77:229–246
Fig. 2 Schematic diagram
of the multi-UAV formation wingman
wingman
Lead plane
120deg
200m
wingman
wingman
the lead plane. The rigid body of the whole multi-
UAV formation relies on the velocity vector of the lead
plane. And the ideal position of each wingman in for-
mation 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 herring-
bone 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
Fig. 3 Schematic diagram of the virtual velocity rigid body
233
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 vir-
tual rigid body coordinate system and coordinate in
inertial coordinate system is shown as follows:
⎡ ⎤ ⎡ ⎤
x x
⎣ y  ⎦ = AB ⎣ y ⎦ , (5)
z z
⎡ ⎤
cos θ sin θ 0
A = ⎣ − sin θ cos θ 0 ⎦ , (6)
0 0 1
⎡ ⎤
cos ψ 0 − sin ψ
B=⎣ 0 1 0 ⎦, (7)
sin ψ 0 cos ψ
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 for-
mation 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 , (8)
X∗i (k) = AP F (k − 1, T ∗i , O)
where AP F is the path planning function of the artifi-
cial potential field method with the additional control
234
Fig. 4 Information flow of
the multi-UAV formation
Path planning
APF
Target
point
u 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 coor-
dinate 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 intro-
duced 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 repul-
sion field of other wingmen and the lead plane. The
Fig. 5 Repulsive force field
between the multi-UAVs
J Intell Robot Syst (2015) 77:229–246
Path planning
Virtual APF
Virtual
Lead Wing-
target
plane rigid body man
point
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 lead-
plane 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 (2015) 77:229–246 235

The cost function is rewritten as:


n−1 
n−1
J =α p(k + 1) − p(k)2 + β a(k)2
k=0 k=0
n−1 
   2
 p(k) t 2 
=α  t 2 + u(k)
 2 G(k) tI
v(k) 2 
k=0

n−1
+β 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). (11)

In order to transform Eq. (10) into the quadratic form,

formation-flight performance by the big errors during
partition the following quadratic form matrixes as:
these situations.
⎡ 4 ⎤
t
G T (k)G(k) t 3 G(k) t 4 G(k)
⎢ 4 t 3 T 2 4
t 3 ⎥
⎣ 2 G (k) t 2 I 2 I ⎦
, (12)
t 4 T t 3 t 4
4 Solution of Optimal Control for the Path 4 G (k) 2 I 4 I
Planning Optimization Model
⎡ ⎤
0 0 0
The main difference between the lead plane and wing- ⎣0 0 0⎦. (13)
men in the path planning process is the target point.
0 0 I
The lead plane is in charge of the overall path while
the wingman is responsible for maintaining the forma- Define:
tion. But all UAVs can use the same method to solve  
their own single path planning problems. Then, the t 4 T t 3
Q (k) = 4 G (k)G(k) 2 G(k) ,
3
optimal control is applied in solving the optimization t T t 2 I
2 G (k)
model.  
0 0
The general form of discrete optimal control system Q1 = , (14)
0 0
is:
 −1
 kf
minu(k) J = φ x(kf ), kf + ϕ(x(k), u(k), k)    
t 4
4 G(k) 0
k=k0
S(k) = t 3
, S1 = , (15)
x(k + 1) = g(x(k), u(k), k) (9)
2 I
0
s.t. h(x(k), u(k), k) ≥ 0 .
x(k0 ) = x0
t 4
The optimization model of the UAV dynamic model R= I, R1 = I, (16)
4
needs to be converted to the general form of the opti-
mal control problem and it includes the conversion    
2 t 2
of the target functions and conversion of the con- I + t2 G(k) tI
straint conditions. Because this paper focuses on the
(k) = , = 2 .(17)
tG(k) I t
path planning for the multi-UAV formation, this solu-
tion procedure is sententious. The similar detailed With the definition of Eqs. 14–17, the cost function
derivation process is shown in reference [12]. is formed into the quadratic form, and the dynamic
236 J Intell Robot Syst (2015) 77:229–246

constraint is formed as the state equation. By now, the Then[19, 20]:

optimization model is rewritten as:  u (k) u (k) u (k) T
n−1  T u (k) = x y z
1  X T (k)
minu(k) J = u
2 uT (k) min x (k) umin y (k) umin z (k) T

k=0
  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) ⎨ um (umin • ≥ um )
⎧ u• (k) = umin •(k) (umin (k) ∈ U )
⎪
⎪ X(k + 1) =
(k) X(k) + u(k) ⎩
⎪
⎨  T −um (umin • ≤ −um )
X(0) = p Ts vT (0)
s.t.  T T T (24)
⎪
⎪ X(n) = p t v (n)
⎪
⎩
u(k) ≤ um where the subscript • represents x, y, z and umin (k)
(18) is the optimal control u (k) without the control con-
straint, which is:
Define:
umin (k) = −R̃ −1 (k) T (P −1 (k + 1) (25)
Q2 (k) = 2αQ(k) + 2βQ1 , (19)
 −1 T −1  
+ R̃ (k) )
(k)X (k),

where P(k) can be calculated by the corresponding

S2 (k) = 2αS(k) + 2βS1 , (20)
matrixes
 (k),  , Q̃(k) andR̃(k), via the Riccati
equation:
R2 = 2αR + 2βR1 . (21) P (k) =
 (k)P (k + 1)
 (k) + Q̃(k) −
 (k)P (k + 1) 
T T

 −1 T
R̃ +  P (k + 1)   P (k + 1)
 (k). (26)
T
Go on with the variable substitutions:

Q̃(k) = Q2 (k) − S2 (k)(R2−1 )T S2T (k) (22)

R̃ = R2 Then, the result of optimal control of discrete opti-


(k) =
(k) − 
(R2−1 )T S2T (k). mal 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
X (k) = X(k) UAV dynamics equations. Each UAV can get its own
u (k) = (R2−1 )T S2T (k)X(k) + u(k) planning path by the help of its u(k).

All above, the general form of the discrete optimal

control that satisfies Eq. (9) is obtained from the con-
5 Path Following Control
version of the constraint conditions and cost function
of the optimization model Eq. (3).
It’s known that the UAV path planning model is built

1 T  on the artificial potential field method. The rational-
minu(k) J = n−1 k=0 2 X (k)Q̃(k)X (k)
 ity of the path planning result can’t be guaranteed
+ 12 uT (k) R̃u (k) considering some constraints. So after the multi-UAV
⎧ 
⎪
⎪ X (k + 1) =
 (k)X (k) +  u (k) , (23) path planning process, the path following control is
⎪
⎨  T
X (0) = p Ts vT (0) an important link to verify the rationality of the path
s.t.  T planning result. In the path following process, the
⎪
⎪ X  (n) = p Tt vT (n)
⎪
⎩ real dynamic model of UAVs needs to be applied to
u (k) ∈ U
simulate the real multi-UAV formation flying state.
where U is the allowable interval of the control force. The quadrotor helicopters are used to follow the plan-
By the help of the optimal control method for ning results which include the position information of
the discrete optimal control system, we can get the the lead plane and wingmen. Obviously, the position
approximate result u . information is time-varying [12].
J Intell Robot Syst (2015) 77:229–246 237

5.1 Dynamics Model of the UAVs in Path Following Y b , Z b ; uci (i=1,2,3,4) satisfy the following equations
Process [23]:
uc1 = b(ω12 + ω22 + ω32 + ω42 )
The quadrotor helicopter is an under actuated aircraft
uc2 = b(ω22 − ω42 )
with fixed four pitch angle rotors as shown in Fig. 7 , (28)
uc3 = b(ω12 − ω32 )
[12, 21]. It has several obvious characteristics such as:
uc4 = d(ω12 − ω22 + ω32 − ω42 )
highly nonlinear, strongly coupled and underactuated
system. where b is the scaling factor of the thrust generated by
In the structural diagram of the quadrotor heli- the rotor; d is the scaling factor of the propeller torque
copters, ψ  , θ  , φ  represent the Euler angles between constant generated by the rotors.
the body-axis coordinate system B =(X b , Y b , Z b ) and Above models are based on the following
the inertial coordinate system E=(X be , Y be , Z be ). So assumptions [24]:
via the Newton’s laws and Euler equation, the simpli-
(1) The UAV structure is symmetrical and rigid;
fied dynamic model of the UAV can be obtained as
(2) The center of mass and Ob coincides;
follows [22]:
(3) Thrust and drag are proportional to the square of
the propellers speed;
1 Kx
ẍb = (cos φ  cos ψ  sin θ  + sin φ  sin ψ  )uc1 − ẋb (27) (4) Ignore the rotational resistance of each UAV;
m m
1 Ky (5) Ignore the effect of noise.
ÿb = (cos φ  sin ψ  sin θ  − sin φ  cos ψ  )uc1 − ẏb
m m
1 Kz 5.2 PID Control for Formation Path Following
z̈b = (cos θ  cos φ  )uc1 − żb − g
m m
 
l Iy − Iz The PID controllers are mentioned in many refer-
φ̈  = uc2 + θ̇  ψ̇  ,
Ix Ix ences. The PID controllers are the standard tools in
 
l Iz − Ix
θ̈  = uc3 + φ̇  ψ̇  current industrial automation experience thank to their
Iy Iy
  flexibility.
1 I − Iy
ψ̈  = uc4 + φ̇  θ̇ 
x The PID controller takes many structures but the
Iz Iz
most important one is in the following form [25]:
t
de(t)
where xb , yb , zb are the positions of the UAV;m is ufollow (t) = KP e(t) + KI e(τ )dτ + KD . (29)
dt
the mass of the UAV; g is the acceleration of grav- 0
ity; l is the half length of the quadrotor helicopter;K x , The algorithm of the PD controller is shown as
K y , K z mean the drag coefficients; I x , I y , I z are the follows:
moments of the inertia with respect to the axes X b ,
de(t)
ufollow (t) = KP e(t) + KD , (30)
dt
where uf ollow (t) is the control variable, the track-
ing 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 pro-
portional (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 con-
trol system. Since the main research content of
this paper is focused on the multi-UAV path plan-
Fig. 7 Quadrotor helicopter structural diagram ning, the simple traditional PID controller and PD
238 J Intell Robot Syst (2015) 77:229–246

controller are applied to solve the path follow-

ing 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 (ẋpi (t) − ẋi (t)))
,
φri (t) = − arcsin(Kpx (ypi (t) − yi (t)) + Kdx (ẏpi (t) − ẏi (t)))
(31)

The altitude PD controller with the nonlinear compen-

sation is as follows:
Fig. 9 Following result of the simple example
1
uc1i (t) = (K z (zp −z)+Kdz (żp −ż)+mg).
cos φi cos θi p

an acceptable range. The speed of the UAV is as
(32)
follows:
In order to verify the effectiveness of this control sys- Figures 9 and 10 show the validity of the control
tem, a simple example is provided. Concretely, in a system, and these results provide some help about the
small scale example, the UAV takes off from [0, 0, 0] rationality for later simulation work.
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 pre- 6 Simple Simulation Examples
determined orbit. Finally, the result is shown in Fig. 9.
The red line is the reference trajectory, and the blue 6.1 Planning Space and Constraint Conditions
line means the real path following result which uses
the control system in Fig. 8. The formation simulation is carried out using 5 UAVs
Figure 9 shows the simulation results of the and it is a simple no-obstacle space. This simple
path following of this reference trajectory. It can simulation is provided in order to evaluate formation-
be seen that the following error is small and in flight performance on simple scenario without the

Fig. 8 Control system chart

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

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

ri (t )
x pi (t ) uc 2i (t )
ri (t )
y pi (t ) uc 3i (t )
ri (t )
uc 4i (t )

z pi (t )

uc1i (t )
'i (t ), 'i (t ), zbi (t )
J Intell Robot Syst (2015) 77:229–246
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:
6.2 Path Planning of Lead Plane
In the no-obstacle space, the path planning result
of the lead plane is a very simple space curve
Fig. 11 Single UAV path planning result in a no-obstacle space
239
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.3 Path Planning of Multi-UAV Formation
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 multi-
UAV 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 per-
formance 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
UAV (x, y, z)/m
Lead plane (blue) (1500,2000,500)
Wingman 1 (black) (2000,1500,200)
Wingman 2 (red) (1500,1500,400)
Wingman 3 (yellow) (1500,2000,500)
Wingman 4 (green) (1500,4000,500)
240
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 correspond-
ing virtual target point (wingman i corresponds to
distance i ).
Fig. 13 Formation error results of four wingmen in a no-obstacle space
J Intell Robot Syst (2015) 77:229–246
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 for-
mation gets into the stableflying state. Figure 13
shows that the formation errors of the path plan-
ning 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
The rationality of the multi-UAV formation path plan-
ning results needs to be verified by the path fol-
lowing 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
J Intell Robot Syst (2015) 77:229–246
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 follow-
ing results, and the blue lines mean the multi-UAV
path planning results. During the whole path follow-
ing 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
7.1 Planning Space and Constraint Conditions
In the complex environment, the complicated and
actual planning space is built by the MATLAB inter-
Fig. 15 Planning space in a
complex simulation
241
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 accelera-
tion 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.2 Path Planning of Lead Plane
The flight status of the single UAV is the same as
the lead plane in the formation, so the prior choice is
242
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
J Intell Robot Syst (2015) 77:229–246
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 for-
mation 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 obsta-
cles, and the formation errors are small in the no-
obstacle area (below 8m). Considering the remark 1
in Section 3.3, these results are reasonable. There-
fore, the path planning of the multi-UAV forma-
tion is very successful in the complex simulation
environment.
J Intell Robot Syst (2015) 77:229–246
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 plan-
ning results in the simple no-obstacle space are shown
in Fig. 13. But in the complex environment the multi-
UAVs 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
243
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 representa-
tive 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. Accord-
ing to the path following error, there are six differ-
ent flying stages: In the first section (0- 300s), the
244
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 fluctu-
ating 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
J Intell Robot Syst (2015) 77:229–246
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 (2015) 77:229–246
(3300- 4000s), they enter the second radar obstacle
area. Uniformly, they face the same problems during
this process. In the other sections (300- 1000s, 2800-
3300s, 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 forma-
tion to achieve the multi-UAV formation path plannin
g successfully.
8 Conclusion
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 favor-
able 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 con-
trol system designed by the simple PID controller is
used to verify the results of path planning. Simula-
tion 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.
References
1. Garcia, M., Viguria, A., Ollero, A.: Dynamic Graph-Search
Algorithm for Global Path Planning in Presence of Haz-
ardous 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 Hori-
zon Control of Vehicle Platoons: Stability and String Sta-
bility. 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-
245
rithm. Sci. China Technol. Sci. 55(10), 2712–2719 (2012).
doi:10.1007/s11431-012-4890-x
5. Roberge, V., Tarbouchi, M., Labonte, G.: Com-
parison of Parallel Genetic Algorithm and Particle
Swarm Optimization for Real-time UAV Path Plan-
ning. IEEE Trans. Ind. Inform. 9(1), 132-141 (2013).
doi:10.1109/TII.2012.2198665
6. Anderson, B.D.O., Yu, C., Baris, F.: Information Archi-
tecture 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)
7. 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
8. Das, A.K., Fierro, R., Kumar, V., Ostrowski, J.P., Spletzer,
J., Taylor, C.J.: A Vision-based Formation Control Frame-
work. IEEE Trans. Robot. Autom. 18(5), 813–825 (2002).
doi:10.1109/TRA.2002.803463
9. Chang Boon, L.: A Dynamic Virtual Structure Forma-
tion 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)
10. McInnes, C.R.: Velocity Field Path-planning for Single
and Multiple Unmanned Aerial Vehicles. Aeronaut. J.
107(1073), 419–426 (2003)
11. Bemporad, A., Rocchi, C.: Decentralized Linear Time-
varying 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
12. 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
13. Cetin, O., Zagli, I., Yilmaz, G.: Establishing Obstacle and
Collision Free Communication Relay for UAVs with Arti-
ficial Potential Fields. J. Intell. Robot. Syst. 69(1-4), 361–
372 (2013)
14. Charifa, S., Bikdash, M.: Comparison of geometrical, kine-
matic, and dynamic performance of several potential field
methods. In: IEEE SoutheastCon 2009, 5-8 March 2009,
Piscataway, NJ, USA. IEEE (2009)
15. 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 Con-
ference (CCDC), 23-25 May 2012, Piscataway, NJ, USA.
IEEE (2012)
16. Zhang, X., Duan, H., Yu, Y.: Receding Horizon Control for
Multi-UAVs Close Formation Control Based on Differen-
tial Evolution. Sci. China Ser. F (Inf. Sci.) 53(2), 223–235
(2010). doi:10.1007/s11432-010-0036-6
17. Yanyang, W., Tietao, W., Xiangju, Q.: Study of
Multi-objective Fuzzy Optimization for Path Plan-
ning. Chin. J. Aeronaut. 25(1), 51–56 (2012).
doi:10.1016/S1000-9361(11)60361-0
18. Hua, S., You, Y., Zhang, H., Song, H.: Receding Horizon
Control of UAV Formations. Electron. Opt. Control. 19(3),
1–5 (2012)
246
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-0104-
21. 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 Con-
trol of Quadrotor Unmanned Air Vehicles. Jiqiren/Robot
J Intell Robot Syst (2015) 77:229–246
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)
25. Efe, M.Ö.: Neural Network Assisted Computationally Sim-
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 naviga-
tion. In: Robotics and Automation, 1991. Proceed-
ings., 1991 IEEE International Conference on. IEEE
(1991)