IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 21, NO.
5, SEPTEMBER 2013
Integrated Optimal Formation Control of Multiple
Unmanned Aerial Vehicles
Jianan Wang, Member, IEEE, and Ming Xin, Member, IEEE
Abstract— In this paper, we investigate the formation control of
multiple unmanned aerial vehicles (UAVs), specifically unmanned
aircraft, in an obstacle-laden environment. The main contribution
of this paper is to integrate the formation control, trajectory
tracking, and obstacle/collision avoidance into one unified optimal
control framework. A nonquadratic avoidance cost is innovatively
constructed via an inverse optimal control approach, which leads
to an analytical, distributed, and optimal formation control law.
The stability and optimality of the closed-loop system are proven.
In addition, the proposed optimal control law is dependent
only on the information from the local neighbors, rather than
all UAVs’ information. Simulation of multiple UAVs’ formation
flying demonstrates the effectiveness of the integrated optimal
control design with desired behaviors including formation flying,
trajectory tracking, and obstacle/collision avoidance.
Index Terms— Collision avoidance, formation control, multiple
unmanned aerial vehicles (UAVs), obstacle avoidance, optimal
control.
I. I NTRODUCTION
F ORMATION flying of multiple unmanned aerial vehicles
(UAVs) has been identified as the key technology for
many cooperative missions. It involves designing a centralized
or distributed controller that drives all UAVs to a desired
formation while maintaining precise relative positions and
velocities, or synchronizing attitude and angular velocity if
rotational motion is considered. Potential applications of mul-
tiple UAVs’ formation flying include monitoring of forest fires
and oil fields, border patrol, surveillance, reconnaissance, and
battle damage assessment, to name a few. Early studies [1]
also reported significant potential fuel savings that could be
gained by a close formation flight.
UAVs’ formation control problems have attracted intensive
research in the recent decade. Many formation control strate-
gies, such as leader-following [2], behavioral [3], and virtual
leader/virtual structure approaches [4], have been well studied.
A Lie group setting for the formation control problem was
presented in [5] as a natural outcome of the analysis of a
Manuscript received July 15, 2011; revised February 2, 2012; accepted
September 1, 2012. Manuscript received in final form September 11, 2012.
Date of publication November 12, 2012; date of current version August 12,
2013. This work was supported in part by the U.S. National Science
Foundation CAREER Award ECCS-0846877. Recommended by Associate
Editor M. Mattei.
J. Wang is with Department of Electrical Engineering and Computer
Science, University of Central Florida, Orlando, FL 32816 USA (e-mail:
wangjianan@ieee.org).
M. Xin is with the Department of Aerospace Engineering, Mississippi State
University, Starkville, MS 39759 USA (e-mail: xin@ae.msstate.edu).
Color versions of one or more of the figures in this paper are available
online at http://ieeexplore.ieee.org.
Digital Object Identifier 10.1109/TCST.2012.2218815
1063-6536 © 2012 IEEE
1731
planar two-vehicle formation control law. In [6], linear matrix
inequality was adopted to design a decentralized overlapping
feedback control law for the formation of UAVs. An inner–
outer loop formation control structure was employed and flight
tested in [7] using a group of YF-22 research UAVs. The outer
loop guidance law minimizes the position tracking error, using
the nonlinear dynamic inversion method, and generates desired
pitch and roll angles, whereas the inner-loop controller is
designed to track those desired angles using the classical linear
control method. A decentralized model predictive control
scheme for a number of cooperative spacecrafts or UAVs
with communication constraints was investigated in [8]. The
formation control of a group of vertical takeoff and landing
UAVs was studied in [9]. The methodology is based on the
separation of the translational and rotational dynamics by
virtue of the unit-quaternion extraction method for the desired
direction of the vehicle’s thrust and attitude to achieve the
position-tracking objective.
In more practical applications, obstacle avoidance must
be taken into consideration if an obstacle is detected right
on the trajectory of a UAV. Many formation controls with
obstacle avoidance capability use path planning approaches
[10], [13], [14], [16], [17], [19] or potential functions [11],
[12], [15], [18], [20], [35]–[37]. In particular, [15], [35], and
[37] employed the same potential function and they originated
from [36]. The work in [35] focused on the multiagent
collision avoidance problem in which cooperation among
agents was not considered, because it was assumed that each
agent knew its desired position to track. The work in [36]
investigated an evasion problem between an evader and a
pursuer, and did not address a cooperative control problem
either. In [21], a hierarchical architecture for the cooperative
control of multiple UAVs was proposed. It features an upper
level with global situation awareness and team mission plan-
ning, a middle level with local knowledge, formation control,
and obstacle avoidance, and a low level that interfaces with
onboard baseline controllers, sensors, communication systems,
and weapon systems. The collision avoidance was consid-
ered as a moving obstacle avoidance, which is implemented
by a potential value function design. In [22], a real-time
trajectory planning strategy with obstacle avoidance and a
formation control law were combined in a simulation platform
for cooperative control of a group of UAVs. A new dual-
mode control strategy was proposed in [23] for the UAVs’
formation flying in both obstacle-free and obstacle-laden envi-
ronments. A “safe mode” is defined as an operation in an
obstacle-free environment, and a “danger mode” is activated
when there is a chance of collision or obstacle in the path.
1732 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 21, NO. 5, SEPTEMBER 2013

The conventional linear optimal control and a modified Gross-

bery neural network were employed during safe mode and
danger mode, respectively. Decentralized receding horizon
control was employed in [24] for the high-level control to
achieve the formation of autonomous vehicle teams. Obstacle
avoidance is ensured by invoking emergency maneuvers that
are computed via the invariant set theory when feasibility
of the decentralized control is lost. In [25], nonlinear model
predictive control (NMPC) was applied to compute the cen-
tralized, sequential decentralized, as well as fully decentralized
formation flight control inputs. The control input saturation,
state constraints, and obstacle avoidance are incorporated as
inequality constraints using Karush–Kuhn–Tucker conditions
in the NMPC framework.
In this paper, we address the multi UAVs’ formation control Fig. 1. UAV model.
problem as well as the obstacle/collision avoidance in a unified
optimal control framework. The collision avoidance among
UAVs will be achieved by considering the colliding UAVs The Laplacian matrix L of graph G is defined as [28]
as moving obstacles. To this end, three objectives including L =  − Adj (1)
formation control, trajectory tracking, and obstacle/collision  
avoidance are formulated into one cost functional for opti- where = diag(Adj·1) with diagonal elements di = j adjij .
mization. A novel inverse optimal control strategy [26], [27] Let 1 and 0 denote the column vector of all ones and all zeros,
is adopted to incorporate a nonquadratic avoidance penalty respectively. In the case of an undirected information exchange
function into the cost function such that an analytical optimal graph, L has a simple zero eigenvalue with an associated
control law can be obtained. In addition, it can be shown that eigenvector 1, and all the other eigenvalues are positive if and
the resultant distributed optimal formation controller requires only if the graph is connected. In other words, L is positive
only local information from the neighbors. Furthermore, the semidefinite and has the property of
formation of multiple UAVs is achieved synchronously via an L1 = 0. (2)
optimal consensus formulation.
The remainder of this paper is organized as follows. III. P ROBLEM F ORMULATION
In Section II, some preliminary notions and definitions used in In this section, the kinematic and dynamic models of the
this paper are given. The problem formulation is described in UAV (aircraft) are first described. A feedback linearization
Section III. Section IV presents the main result of this paper. technique is then employed to obtain a reduced double-
Simulation results and analysis are shown in Section V. Some integrator model. The formation control problem with obsta-
concluding remarks are given in Section VI. cle/collision avoidance will be defined based on the reduced
model. The original control can be obtained via nonlinear
II. P RELIMINARIES
transformations.
It is convenient to use the graph theory to model the
information exchange among vehicles in order to study the A. Kinematic and Dynamic Models
multivehicle cooperative control problem. In this section, some
In this paper, point-mass aircraft model [29] is used to
necessary notions from the graph theory are briefly reviewed.
describe the motion of formation flying UAVs. The related
In general, the information exchange among vehicles can be
variables are defined with respect to the inertial coordinate
modeled by the directed or undirected graph. Let G = (N, E)
frame (x̂, ŷ, ĥ) and are shown in Fig. 1.
denote a directed graph, in which N is a finite nonempty set of
In what follows, the model assumes that the aircraft thrust
nodes and E is an edge set of ordered pairs of nodes. The pairs
is directed along the velocity vector and that the aircraft
of nodes in an undirected graph are unordered. A directed path
always performs coordinated maneuvers. It is also assumed
is a sequence of ordered edges of the form (i 1 , i 2 ), (i 2 , i 3 ), . . .,
that the Earth is flat and the fuel expenditure is negligible
where i j ∈ N. For example, (i 1 , i 2 ) ∈ E indicates that UAV
(i.e., the center of mass is time-invariant) [33]. Under these
i 2 obtains the information from UAV i 1 · (i 1 , i 2 ) ∈ E in
assumptions, the UAV equations of motion can be described by
an undirected graph indicates that UAV i 1 and UAV i 2 can
Kinematics
obtain the information from each other. An undirected graph
is connected if there is an undirected path between every pair ẋ i = Vgi cos γi cos χi
of distinct nodes. ẏi = Vgi cos γi sin χi
A nonnegative adjacency matrix Adj = [adji j ] specifies the
ḣ i = Vgi sin γi (3)
interconnection topology of a network of vehicles, which is
defined as adjii = 0, adji j = 1 if ( j, i ) ∈ E, and adji j = 0 where i = 1, . . . , n is the index of multiple UAVs. n is the
if ( j, i ) ∈
/ E, where i = j . For the undirected graph, the number of UAVs. For UAV i, x i is the down-range displace-
adjacency matrix is symmetric, that is, adji j = adj j i , ∀i = j . ment, yi is the cross-range displacement, h i is the altitude, Vgi
WANG AND XIN: INTEGRATED OPTIMAL FORMATION CONTROL OF MULTIPLE UAVs 1733

is the ground speed, γi is the flight-path angle, and χi is the

heading angle. The UAV dynamics are given by
Dynamics
Th − Dgi
V̇gi = i − ga sin γi
mi
ga  
γ̇i = n gi cos φbi − cos γi
Vgi
L fi sin φbi
χ̇i = (4)
m i Vgi cos γi
where Th i is the engine thrust, Dgi is the drag, m i is the Fig. 2. Formation flying scenario with obstacle/collision avoidance.
mass, ga is the acceleration due to gravity, L fi is the vehicle
lift, and φbi is the banking angle. The control variables in
the UAVs are the g-load n gi = L fi /(ga m i ) controlled by the where X ∈ R6n and U ∈ R3n are, respectively, the aggregate
elevator, the banking angle φbi controlled by the combination state and aggregate control input of all UAVs. ⊗ denotes the
of rudder and ailerons, and the engine thrust Th i controlled Kronecker product used to extend the dimension. In denotes
by the throttle. Throughout the formation control process, the the identity matrix with dimension n · 0n×n denotes the zero
control variables will be constrained to remain within their matrix.
respective limits. The formation control problem in this paper is to design a
The nonlinear UAV model can be transformed into a linear distributed optimal control law ai (t) based on the information
time-invariant double-integrator model by the feedback lin- exchange topology, and thereby the actual control laws φbi ,
earization [30]. Specifically, one can differentiate the kine- n gi , and Th i can be derived such that all the UAVs achieve
matic (3) once with respect to time, and then substitute the the desired formation, and the formation follows a prescribed
dynamic (4) to obtain reference trajectory. In the meantime, each UAV can avoid
obstacles and collision with other UAVs along its trajectory.
ẍ i = axi , ÿi = a yi , ḧ i = ah i (5) Fig. 2 is given to illustrate the scenario, where the dashed
where axi , a yi , and ah i are the new control variables in circle denotes the detection region of the UAV with radius R,
the double-integrator model. The relationships between these r denotes the radius of the UAV (assuming that all UAVs are
control variables and the actual control variables are given by identical with the same radius and detection region); r j denotes
(6) [30] the radius of the obstacle, and O j is the center of the
  obstacle, where j = 1, . . . , q with q the number of obstacles.
−1 a yi cos χi − a xi sin χi
φbi = tan pre f and ν re f represent the reference or desired formation
cos γi (ah i +ga )−sin γi (axi cos χi +a yi sin χi ) trajectory.
cos γi (ah i + ga ) − sin γi (axi cos χi + a yi sin χi ) For the convenience of formulation, the following regions
n gi =
ga cos φbi can be defined.
 
Th i = sin γi (ah i +ga )+cos γi (axi cos χi+a yi sin χi ) m i+Dgi . Collision region for the i th UAV
  
(6)  
i,k = x x ∈ R3 , x − ck  ≤ r + r̄ .
The heading angle χi and the flight-path angle γi are computed
by Reaction region for the i th UAV
ẏi ḣ i   
tan χi = , sin γi = . (7)  
ẋ i Vgi i,k = x  x ∈ R3 , r + r̄ < x − ck  ≤ R + r̄ .

B. Problem Statement Safety region for the i th UAV

  
The reduced UAV models can be also expressed as  
i,k = x x ∈ R3 , x − ck  > R + r̄ .
ṗi = ν i
, i = 1, . . . , n (8)
ν̇ i = ai where ck ∈ i , k = 1, . . . , n + q − 1; i denotes the set of
where pi = [ x i yi h i ]T , v i = [ẋ i ẏi ḣ i ]T , ai = [axi a yi ah i ]T obstacles and all the UAVs excluding UAV i
are the position, velocity, and acceleration, respectively.    
i  p1 , . . . , pl , . . . , p n , O 1 , . . . , O j , . . . , O q \ pi
Equation (8) can be recast in a matrix form 
r, if the colliding object is the UAV
Ẋ = AX + BU (9) r̄ =
rj, if the colliding object is the j th obstacle.
with
    In this paper, we make the following two assumptions.
0n×n In 0n×n
A= ⊗ I3 , B= ⊗ I3 1) All obstacles and UAVs are modeled as spherical
0n×n 0n×n In
objects.
X = [ p1T , . . . , pnT , ν 1T , . . . , ν nT ]T , U = [a 1T , . . . , anT ]T (10)


 2) The information exchange topology is undirected and
pT νT connected.
1734 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 21, NO. 5, SEPTEMBER 2013

IV. M AIN R ESULTS

In this section, we propose a unified optimal control
approach to address the formation control problem with tra-
jectory tracking and obstacle/collision avoidance capabilities.
In order to formulate the formation control problem, the
following definition is needed.
Definition 4.1 [31]: n vehicles in a formation can be defined
by a constant offset vector
 T T
σ p1 , . . . , σ Tpn 0T , . . . , 0T
σ =
 . (11)
σ Tp

The n vehicles are said to be in formation σ at time t if

there exist R3 valued vectors pcs and vcs such that pi − σ pi =
pcs and vi = vcs for i = 1, 2, . . . , n. pcs and vcs represent Fig. 3. UAVs in formation.
the consensus position and velocity, respectively. The vehicles
converge to the formation σ if there exist real-valued functions
pcs (t) and vcs (t) such that pi (t)−σ pi −pcs (t) → 0 and vi (t)− The final consensus state satisfies the dynamic equation
vcs (t) → 0, as t → ∞ for i = 1, 2, . . . , n. ˙ = A X̄ + B Ū = A X̄
Fig. 3 illustrates the offset vector in the above definition X̄ fs fs fs fs (17)
using four UAVs. The offset vector σ is a constant vector since Ū f s = 03n×1 when the UAVs reach consensus.
that defines the desired formation pattern with σ pi being the From (13) and (17), the error dynamics become
location of vehicle i in the formation. If pi − σ pi = pcs , it
˙ ˙ − X̄
˙ = A X̄ + BU − A X̄ = A X̂ + BU.
implies that pi − pk = σ pi − σ pk and thus the formation is X̂ = X̄ fs fs (18)
achieved. The final velocities of all UAVs should reach the The formation is achieved when (18) is asymptotically stable.
same consensus velocity ν cs . In this paper, the formation control problem is formulated
Note that in the formulation of [31], a desired formation in a unified optimal control framework including three cost
can be achieved with unpredictable trajectory and velocity, function components
whereas the formulation in this paper will be able to guarantee
the formation to follow a desired trajectory pre f with a desired min : J = J1 + J2 + J3
formation velocity ν re f , which will be shown in the following ˙ = A X̂ + BU
s.t. X̂ (19)
optimal control formulation.
Define a formation vector where J1 , J2 , and J3 represent the consensus formation cost,
 T   T the obstacle/collision avoidance and tracking cost, and the
X̄ = p̄T ν̄ T = X − σ = p − σ p T (ν − 0)T . control effort cost, respectively.
(12) J1 has the form of
Then  ∞
T
˙ = Ẋ − σ̇ = AX + BU = A( X̄ + σ ) + BU = A X̄ + BU J1 = X̂ R1 X̂dt
X̄
0 ∞  2 2   
(13) T wp L 0n×n
since Aσ = 0 and σ̇ = 0. Define a formation error vector = X̂ ⊗ I3 X̂ dt
0n×n wv2 L 2 − 2w p wc L
 
T T 
0
X̂ = p̂T ν̂ = X̄ − X̄ f s (14) (20)

where where L is the symmetric Laplacian matrix established by the

 T undirected and connected information exchange graph. w p ,
pT , . . . , p T ν T , . . . , ν T wv , and wc represent the weights on the position, velocity,
X̄ f s =
cs  cs
cs  cs
T
p̄cs ν̄ cs
T and control effort, respectively. The following lemmas show
     T that R1 is positive semidefinite.
= 11×n ⊗ αx α y αh 11×n ⊗ βx β y βh (15) Lemma 4.1 [32]: L 2 is positive semidefinite and L 2 1n×1 =
is the final consensus state. αx , α y , αh are the final consensus 0n×1 if the information exchange graph is undirected and
positions along x̂-, ŷ-, and ĥ-axes, respectively. βx , β y , βh connected.
are the final consensus velocity along x̂-, ŷ-, and ĥ-axes, Lemma 4.2 [32]: wv2 L − 2w p wc L is positive definite if the
respectively. Note that p̄cs and ν̄ cs are not known a priori. information exchange graph is undirected and connected, and
According to the property of the Laplacian matrix L in (2), wv2 ei2 − 2w p wc ei > 0 (21)
if the undirected communication graph is connected, when the
UAVs reach formation where ei is the eigenvalue of L.
Remark 4.1: Minimizing J1 results in an optimal consensus
(L ⊗ I3 ) p̄cs = 03n×1 algorithm to guarantee that all UAVs can reach the formation
(L ⊗ I3 )ν̄ cs = 03n×1 . (16) and the same velocity synchronously (at the same time) [32].
WANG AND XIN: INTEGRATED OPTIMAL FORMATION CONTROL OF MULTIPLE UAVs 1735

Remark 4.2: One can always find proper weights to satisfy In addition, if X̂ 0 ∈ D0 , then the feedback control (32)
the condition (21). For instance, a small enough wc for minimizes J ( X̂ 0 , U(·)) in the sense that
the given w p and wv are applicable because ei ≥ 0. It
is obvious that R1 in the formation cost (20) is positive J ( X̂ 0 , φ( X̂(·))) = min J ( X̂ 0 , U(·)) (34)
semidefinite because both the diagonal elements in R1 are U(·)∈S( X̂0 )
positive semidefinite. The term −2w p wc L in the formation
cost (20) is used to guarantee that the analytical solution of where S( X̂ 0 ) denotes the set of asymptotically stabilizing
the Riccati equation for the optimal control law is a linear controllers for each initial condition X̂ 0 ∈ D. Finally, if
function of the Laplacian matrix L, and is thus completely D = Rn , = Rm , and
dependent on the information exchange topology, which will  
 
be shown in the proof of Theorem 4.1. V ( X̂) → ∞ as  X̂  → ∞. (35)
J2 has the form of
 ∞
The solution X̂(t) ≡ 0 of the closed-loop system is globally
J2 = h( X̂)dt (22)
0 asymptotically stable.
Proof: Refer to [26].
where h( X̂) contains the tracking penalty function as well
Before presenting the main result, we first define the penalty
as the obstacle/collision avoidance penalty function. It will
function g( X̂)
be constructed from an inverse optimal control approach
in Theorem 4.1. g( X̂) = gtr ( X̂) + ga ( X̂). (36)
J3 has the regular quadratic form of
 ∞ The first component is the tracking penalty function
J3 = U T R2 Udt (23)
0 
n

where R2 = wc2 In
⊗ I3 is positive definite and wc is the gtr ( X̂)  gtri ( X̂) (37)
weighting parameter. i=1
⎧ " #T  
The following lemma is introduced to derive the main result ⎪ T
⎪
⎪ pi − pre f ν i − ν re f
of this paper and is used to prove both asymptotic stability and ⎪
⎪   
⎨ 2
optimality of the proposed formation control algorithm. wt p I 3 wt p wt v I 3 pi − pre f
gtri ( X̂)  ×
ν i − ν re f
Lemma 4.3 [26]: Consider the nonlinear controlled dynam- ⎪
⎪ wt p wt v I3 wt2v I3
⎪
⎪
ical system ⎪
⎩
if UAV i has access to the reference
0 if not
˙ = f ( X̂(t), U (t)), X̂(0) = X̂ , t ≥ 0
X̂(t) 0 (24) i = 1, . . . , n (38)
with f (0, 0) = 0 and a cost functional given by
 ∞ where wt p , wt v are tunable weights to control the tracking

J ( X̂ 0 , U(·)) = T ( X̂(t), U(t))dt (25) speed. pre f and ν re f are the position and velocity of the
0 reference, respectively.
where U(·) is an admissible control. Let D ⊂ Rn be an open Remark 4.3: The UAV that has access to the reference will
set and ⊆ Rm . Assume that there exists a continuously track the desired trajectory by means of minimizing the cost
differentiable function V : D → R and a control law φ : J2 that contains gtr ( X̂), which will be shown in Theorem 4.1.
D → such that Note that minimizing the cost function J1 guarantees that all
V (0) = 0 (26) the UAVs’ positions and velocities are coordinated to achieve
the desired formation and reach the consensus velocity ν cs
V ( X̂) > 0, X̂ ∈ D, X̂ = 0 (27) synchronously. But where the formation goes cannot be
φ(0) = 0 (28) specified as desired by merely minimizing J1 because the

V ( X̂) f ( X̂, φ( X̂)) < 0, X̂ ∈ D, X̂ = 0 (29) final UAVs’ velocity depends on each UAVs initial velocity
H ( X̂, φ( X̂)) = 0, X̂ ∈ D (30) and results from the negotiation among UAVs during the flight
via the optimal consensus formulation. Therefore, combining
H ( X̂, U) ≥ 0, X̂ ∈ D, U ∈ (31) the tracking penalty function gtr ( X̂) with the consensus

where H ( X̂, U) = T ( X̂, U) + V ( X̂) f ( X̂, U) is the Hamil- formation cost J1 , the formation is able to follow a desired
tonian function. The superscript denotes partial differentiation trajectory pre f with the desired formation velocity ν re f , that
with respect to X̂. is, ν cs = ν re f . It is also worth noting that only one UAV
Then, with the feedback control having access to the reference is sufficient to guarantee the
entire formation to track the desired trajectory.
U(·) = φ( X̂(·)) (32) The second component in g( X̂) is the obstacle/collision
the solution X̂(t) ≡ 0 of the closed-loop system is locally avoidance penalty function
asymptotically stable and there exists a neighborhood of the
origin D0 ⊆ D such that  
n n+q−1
ga ( X̂) = gi,k ( X̂) (39)
J ( X̂ 0 , φ( X̂(·))) = V ( X̂ 0 ), X̂ 0 ∈ D0 . (33) i=1 k=1
1736 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 21, NO. 5, SEPTEMBER 2013

⎧
⎪
⎪ 0, pi ∈ i,k
⎪
⎪" #
⎪
⎪ + r̄) − pi − ck 
2 2
⎨ (R
2
" #2
gi,k ( X̂)   pi −ck 2 −(r + r̄)2
⎪
⎪
⎪
⎪
x,y x,y
×(ν i − ν ak )T (ν i − ν ak ) pi ∈ i,k
⎪
⎪
⎩ not defined pi ∈ i,k
i = 1, . . . , n, k = 1, . . . , n + q − 1 (40)
x,y
where ck ∈ i ; ν i ∈ R2 is the projection of the UAV’s veloc-
ity vector ν i on the x̂ − ŷ plane; and ν ak ∈ R2 is the prescribed
obstacle/collision avoidance velocity. There are many ways to
avoid the obstacle or colliding UAVs from different directions.
Since UAVs move primarily along the down-range and cross-
range directions, we design the avoidance strategy in the (x̂, ŷ)
plane, which is shown in Fig. 4, where r̄ denotes the radius Fig. 4. Illustration of obstacle/collision avoidance.
of the colliding object (UAV or obstacle) and rsafe is defined
as a prespecified safe distance to the colliding object. rsafe
should be set large enough for safe avoidance. The coordinate of ck and OU in the coordinate frame x̂ − O − ŷ respectively.
system x̂ − O − ŷ is the horizontal plane of (x̂, ŷ, ĥ). The For the other layout, the rotation angle β can be calculated
coordinate system x̂ − OU − ŷ is coplanar to x̂ − O − ŷ likewise with the sign difference. For instance, if ck is located
with the origin at the center of the UAV and the x̂ -axis along below the dashed horizontal line x̂, the rotation angle will be
the tangential direction from the UAV to the dot-and-dashed calculated as
safety circle centered at the colliding object with the radius β = ∠ck OU x̂ + ∠ck OU x̂.
x,y
of r̄ + rsafe . The ν ak is chosen to be the projection of ν i on
Remark 4.4: The obstacle/collision avoidance penalty func-
the x̂ -axis. Apparently, the avoidance can be achieved if the
tion is designed to penalize the error between the current
UAV is driven to the direction of ν ak . x,y
velocity ν i and the avoidance velocity ν ak . The coefficient
ν ak can be calculated as follows. The rotation angle β
"  2 #2
between the two coordinate frames x̂ − O − ŷ and x̂ − OU − ŷ (R + r̄)2 −  pi − ck 
can be determined from the geometry when the current UAV’s
"  #2
position and the colliding object’s position are known. Then,  pi − ck 2 − (r + r̄)2
the velocity vector ν i expressed in the x̂ − OU − ŷ frame
x,y
x ,y
and denoted as ν i can be calculated by in g i,k ( X̂) is designed to guarantee the continuity of the
  penalty function on the upper boundary R + r̄ of the reaction
x ,y νix x,y region, which will be shown in the proof of Theorem 4.1.
νi = y = C (β) · ν i (41)
νi In addition, the coefficient
"  2 #2
where   (R + r̄)2 −  pi − ck 
cos β − sin β
C (β) = " #2
sin β cos β 
 pi − ck 2 − (r + r̄)2
is the rotation matrix.
The prescribed obstacle/collision avoidance velocity ν ak is of the avoidance penalty function (40) can be regarded as the
x,y x,y
obtained by  x  weight on the tracking error (ν i −ν ak )T (ν i −ν ak ) between
−1 ν the UAV velocity and the avoidance velocity ν ak . The closer
ν ak = C (β) i . (42)
0 the UAV is to the colliding object, the smaller the denominator
of the coefficient is and thus the larger the weight is. Thus, the
To calculate the rotation angle β between the two coordinate
priority of obstacle/collision avoidance increases as the UAV
frames x̂ −O− ŷ and x̂ −OU − ŷ , we can inspect the geometry
approaches the colliding object inside the reaction region. It
in Fig. 4. In the current layout of Fig. 4, the rotation angle
is also worth noting that the avoidance strategy as shown in
β = ∠ck OU x̂ − ∠ck OU x̂ Fig. 4 establishes a safety ball outside the colliding object
with a radius of rsafe + r̄ such that the UAV is driven to fly
where ∠ stands for the angle. ck denotes the center of the
along the direction tangential to this safety ball. This safety
colliding object; OU denotes the current position of the UAV.
ball serves as a buffer zone to ensure the avoidance if the UAV
It can be seen from the geometry that
does not exactly fulfill the avoidance velocity direction due to
rsafe + r̄
∠ck OU x̂ = arcsin the other requirements such as reference trajectory tracking
d and formation flying. rsafe can be prescribed large enough by
x̂ O − x̂U the designer in light of the size and maneuverability of the
∠ck OU x̂ = arccos
d aircraft.
where d denotes the distance from the UAV (OU ) to the center The main result of this paper is presented in the following
of the colliding object (ck ). x̂ O and x̂U are the x̂ components theorem.
WANG AND XIN: INTEGRATED OPTIMAL FORMATION CONTROL OF MULTIPLE UAVs 1737

Theorem 4.1: For a multi-UAV system (3) and (4) with the continuously differentiable with respect to X̂ in the open
$n+q−1
assumptions 1) and 2), one can always find proper weights domain ( k=1 i,k )c .
w p , wv , and wc such that the distributed feedback control law After showing that V ( X̂) is continuously differentiable, the
wp   Hamiltonian function for this optimal control problem can be
U = φ (X) = − (L ⊗ I3 ) p − σ p written as
wc " " ## " # " # " #
wν 1 H X̂, U, V T X̂ = T X̂, U + V T X̂ f X̂, U
− (L ⊗ I3 )v − g (X) (43)
wc 2wc2 v T
" #
= X̂ R1 X̂ + h X̂ + U T R2 U
is an optimal control for the formation control problem (19)  T " #  
with the cost function term h( X̂) in J2 being + 2 X̂ P + g T X̂ A X̂ + BU .
w p T wv T
h( X̂) = gv ( X̂)(L ⊗ I3 ) p̂ + g ( X̂)(L ⊗ I3 )ν̂ (49)
wc wc v

1   Setting (∂/∂U)H ( X̂, U, V T ( X̂)) = 0 yields the optimal
2
−g T
p ( X̂)ν̂ + 2 gv ( X̂) (44) control law
4wc
1
where gv (X), gv ( X̂),and g p ( X̂) in (43) and (44) denote the U ∗ = φ( X̂) = − R2−1 B T V ( X̂)
2
partial differentiation of g( X̂) with respect to the velocity error 1
= −R2−1 B T P X̂ − R2−1 B T g ( X̂). (50)
ν̂ and the position error p̂, respectively. In addition, the closed- 2
loop system is globally asymptotically stable. With (50) it follows that:
Proof: Pertaining to this optimal formation control problem, " " ## T
" #
we have the following equations corresponding to Lemma 4.3: V T ( X̂) f X̂, φ X̂ = X̂ A T P + P A − 2P S P X̂
T
T
T ( X̂, U) = X̂ R1 X̂ + h( X̂) + U T R2 U (45) − X̂ P Sg ( X̂) + g T ( X̂)
1
f ( X̂, U) = A X̂ + BU. (46) × (A− S P) X̂ − g T ( X̂)Sg ( X̂)
2
A candidate Lyapunov function V ( X̂) is chosen to be (51)

T
V ( X̂) = X̂ P X̂ + g( X̂) (47) where S = B R2−1 B T . Using (50) and (51) into (49) yields
" # T
" #
where P is the solution of a Riccati equation, which will be H X̂, φ( X̂), V T ( X̂) = X̂ A T P + P A + R1 − P S P X̂
shown afterward.
+g T ( X̂)(A − S P) X̂ + h( X̂)
In order for the function V ( X̂) in (47) to be a valid
1
Lyapunov function, it must be continuously differentiable − g T ( X̂)Sg ( X̂). (52)
with respect to X̂, or equivalently g( X̂) must be continu- 4
ously differentiable with respect to X̂ in the open domain In order to prove that the control law (50) is an optimal solu-
$n+q−1 tion for the formation control problem (19) using Lemma 4.3,
( k=1 i,k )c where “∪” and the superscript “c” denote
the union and complement of sets, respectively. From (38) the conditions (26)–(31) need to be verified.
and (40), it suffices to show that gi,k ( X̂) is continuously In order to satisfy the condition (30) in Lemma 4.3 or to
$n+q−1 let (52) be zero, we can let
differentiable in the open domain ( k=1 i,k )c . In fact,
this is true if gi,k ( X̂) and (dgi,k ( X̂)/d pi ) are continuous on A T P + P A + R1 − P S P = 0 (53)
the upper boundary R + r̄ of i,k . Since (40) implies that
and require that
lim pi −ck →(R+r̄)− gi,k ( X̂) = 0 = lim pi −ck →(R+r̄)+ gi,k ( X̂),
gi,k ( X̂) is continuous on the upper boundary R + r̄ of i,k by 1
g T ( X̂)(A − S P) X̂ + h( X̂) − g T ( X̂)Sg ( X̂) = 0. (54)
the definition of continuity. In addition 4
With (50), (53), and (54), it can be shown that
dgi,k ( X̂)
= H ( X̂, U, V T ( X̂))
dp
⎧ i T T
⎪ 0, pi ∈ i,k = U T R2 U +h( X̂)+ X̂ R1 X̂ +(2 X̂ P +g T ( X̂))(A X̂ + BU)
⎪
⎪
⎪
⎨ −4((R+r̄)  i k 
2 −(r+r̄)2 )((R+r̄)2 − p −c 2 ) T T
= U T R2 U +h( X̂)+ X̂ R1 X̂ +(2 X̂ P +g T ( X̂))(A X̂ + BU)
" #3
 pi −ck 2 −(r+r̄)2 T
⎪
⎪ − X̂ (A T P + P A + R1 − P S P) X̂
⎪ ×( pi − ck )(ν ix,y − ν ak )T (ν ix,y − ν ak ) pi ∈ i,k
⎪
⎩ = U T R2 U + h( X̂) + g T ( X̂)(A X̂ + BU)
not defined pi ∈ i,k
T T
i = 1, . . . , n, k = 1, . . . , n + q − 1. (48) +2 X̂ P BU + X̂ P S P X̂
1
It is easy to see that lim pi −ck →(R+r̄)− (dgi,k ( X̂)/d pi ) = = U T R2 U + g T ( X̂)Sg ( X̂) + g T ( X̂)S P X̂
4
03×1 = lim pi −ck →(R+r̄)+ (dgi,k ( X̂)/d pi ). Hence, (dgi,k T T
+ X̂ P S P X̂ + (2 X̂ P + g T ( X̂))BU
( X̂)/d pi ) is continuous on the upper boundary R + r̄ of 1 T T
i,k . Therefore, g( X̂) and the Lyapunov function V ( X̂) are = U T R2 U + (2 X̂ P + g T ( X̂))S(2 X̂ P + g T ( X̂))T
4
1738 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 21, NO. 5, SEPTEMBER 2013

T
+(2 X̂ P + g T ( X̂))BU P in (57), the Lyapunov function becomes
1
= U T R2 U + V T ( X̂)SV ( X̂) + U T B T V ( X̂) T
V ( X̂) = X̂ P X̂ + g( X̂) =
4 ⎧
= U T R2 U + φ( X̂)T R2 φ( X̂) − 2U T R2 φ( X̂) ⎪
⎪ w p wv p̄T (L 2 ⊗ I3 ) p̄ + wc wv v T (L ⊗ I3 )v
⎪
⎪
⎪
⎪ +2w p wc p̄T (L ⊗ I3 )v + gtr ( X̂) pi ∈ i,k
= [U − φ( X̂)]T R2 [U − φ( X̂)] ≥ 0. (55) ⎨
w p wv p̄T (L 2 ⊗ I3 ) p̄ + wc wv v T (L ⊗ I3 )v
⎪
⎪
Therefore, the condition (31) is satisfied. ⎪
⎪ +2w p wc p̄T (L ⊗ I3 )v + gtr ( X̂) + ga ( X̂) pi ∈ i,k
⎪
⎪
Next, substituting A, B, R1 , and R2 in (53) and assuming ⎩
not defined pi ∈ i,k
  i = 1, . . . , n, k = 1, . . . , n + q − 1. (60)
P1 P2
P= ⊗ I3
P2 P3 Note that the property (16) is used in (60) so that the first
three terms in V ( X̂) contains only p̄ and v. V ( X̂) > 0 when
yields X̂ = 0 can be guaranteed by selecting a large enough wv for
    the given w p such that the positive terms w p wv p̄T (L 2 ⊗ I3 ) p̄,
− 12 P22P1 − 12 P2 P3 w2p L 2 0n×n wc wv v T (L⊗I3 )v, and gtr ( X̂) are always greater than the sign-
wc wc
+ = 0.
P1 − 12 P3 P2 2P2 − 12 P32 0n×n wv2 L 2 − 2w p wc L indefinite terms and thus the condition (27) can be satisfied.
wc wc
(56) Next, the cost function term h( X̂) in J2 (see (22)) is
constructed from solving (54) and using (57)
Then, P can be solved in the analytical form w p T wv T
h( X̂) = g ( X̂)(L ⊗ I3 ) p̂ + g ( X̂)(L ⊗ I3 )ν̂
  wc v wc v
w p wv L 2 w p wc L 1  
2

P= ⊗ I3 . (57) −g T
p ( X̂)ν̂ + g v ( X̂)  (61)
w p wc L wc wv L 4wc 2

which turns out to be (44). Note that the cost function term
Note that the purpose of introducing the term −2w p wc L in
h( X̂) is obtained from the inverse optimal control strategy
R1 (see (20)) is to let it cancel 2P2 when solving P3 from
since it is not given a priori, but is constructed from the
(56) in order to make P3 a linear function of the Laplacian
optimality condition (54).
matrix L as mentioned in Remark 4.2.
Using (53) and (54), (51) becomes
Now, we will verify the condition (27). First note that the
matrix V T ( X̂) f ( X̂, φ( X̂))
   T
wt2p I3 wt p wt v I3 T 1
= − X̂ R1 X̂ + h( X̂) + ( X̂ P + g T ( X̂))
wt p wt v I3 wt2v I3 2
1 
×S(P X̂ + g ( X̂)) . (62)
in the tracking penalty function (38) is positive semidefinite. 2
To see this, we can inspect the eigenvalues of this matrix by It can be seen from (62) that the condition (29) can be met
T
   when h( X̂) ≥ 0 since X̂ R1 X̂ is positive semidefinite and
wt2p I3 wt p wt v I3 T
det λI − ( X̂ P +(1/2)g T ( X̂))S(P X̂ +(1/2)g ( X̂))is positive definite.
wt p wt v I3 wt2v I3
  By selecting proper values of the weights w p , wv , and wc in
(λ − wt2p )I3 −wt p wt v I3 (61), one can always ensure h( X̂) ≥ 0. Specifically, it can be
= det . (58)
−wt p wt v I3 (λ − wt2v )I3 achieved by choosing a small enough wc for the given w p
and wv such that the positive term 1/(4wc2 )gv ( X̂)2 in (61)
Since (λ − wt2p )I3 and −wt p wt v I3 commute [28], that is, is always greater than the other sign-indefinite terms.
[(λ − wt2p )I3 ][−wt p wt v I3 ] = [−wt p wt v I3 ][(λ − wt2p )I3 ], we Substituting P and g ( X̂) into (50) leads to the optimal
have control law
  wp wv 1
(λ − wt2p )I3 −wt p wt v I3 φ( X̂) = − (L ⊗ I3 ) p̂ − (L ⊗ I3 )ν̂ − 2 gv ( X̂). (63)
det wc wc 2wc
−wt p wt v I3 (λ − wt2v )I3
" # Substituting p̂ = p̄ − p̄cs and ν̂ = ν̄ − ν̄ cs into (63) and using
= det (λ − wt2p )(λ − wt2v )I3 − wt2p wt2v I3
" # (16), the optimal control law (63) becomes
= det λ2 I3 − (wt2p + wt2v )λI3 . (59) wp wv 1
φ (X) = − (L ⊗ I3 ) p̄ − (L ⊗ I3 )ν̄ − 2 gv ( X̂). (64)
wc wc 2wc
The eigenvalues are
Also note that
λ = 0, 0, 0, wt2p + wt2v , wt2p + wt2v , wt2p + wt2v ≥ 0 ∂gtr ( X̂) ∂ga ( X̂)

gv ( X̂) = gtr

( X̂) + ga v ( X̂) = + . (65)
v
∂ ν̂ ∂ ν̂
which implies that the weighting matrix in gtr ( X̂) is positive Since pre f and ν re f are known to UAV i if it has access to the
semidefinite and gtr ( X̂) ≥ 0. Moreover, from the solution of ( X̂) = g (X). Also, since ν is determined by
reference, gtr v trv ak
WANG AND XIN: INTEGRATED OPTIMAL FORMATION CONTROL OF MULTIPLE UAVs 1739

the current position and velocity of UAV i and the colliding

object’s position, that is, pi , ν i , and ck , respectively, if UAV
i detects the colliding object, we can see from (40) that
ga v ( X̂) = ga v (X), and thus gv ( X̂) = gv (X). Therefore, using
gv ( X̂) = gv (X) in (64) and noting ν̄ = ν, the optimal control
law turns out to be (43)
wp   wv 1
U =− (L ⊗ I3 ) p − σ p − (L ⊗ I3 )v − g (X).
wc wc 2wc2 v
It remains to verify the conditions (26) and (28). It can be
seen from (47) and (50) that conditions (26) and (28) are met
if g( X̂) = 0 and g ( X̂) = 0 when X̂ = 0. Equations (40) and
(64) imply that if the colliding object is inside the detection
region, the avoidance force is not zero and the UAV will be
driven away from the colliding object. When the colliding
object is outside the detection region and the UAV flies along Fig. 5. 3-D trajectories of UAVs’ formation flying without concern of
the reference trajectory, we have g( X̂) = 0 and g (X) = 0, obstacles.
which validates the conditions (26) and (28).
Now, all the conditions (26)–(31) in Lemma 4.3 can be
satisfied. According to Lemma 4.3, the control law (43) is an
optimal control for the problem (19) in the sense of (33) and
(34), and the closed-loop system is asymptotically stable. It
implies X − σ = X̄ f s and the desired formation is achieved.
In addition, it can be easily seen from (47) that V ( X̂) →
∞ as  X̂ → ∞. Therefore, the closed-loop system is glob-
ally asymptotically stable. Note that the globally asymptotic
$n+q−1
stability region excludes the undefined region k=1 i,k ,
which is physically meaningful because no UAV can start from
inside this undefined collision region.
As can be seen from the proof of Theorem 4.1, the
penalty function g( X̂) provides the reference tracking and
obstacle/collision avoidance capabilities. The inverse optimal
control approach facilitates the integration of this penalty
function into the cost function h( X̂) such that an analytical,
distributed, and optimal control law to achieve these capa- Fig. 6. Top view of UAVs’ formation flying with collision avoidance.
bilities can be obtained. The idea of inverse optimal control
is different from the conventional optimal control because the
cost function term h( X̂) is not given a priori and is constructed Remark 4.6: From (43), (38), and (40), it can be seen that
from the optimality condition (54). the optimal control law of each UAV requires only the local
Note that the first two terms in the proposed optimal control information from its neighbors since the first two terms of
law (43) are used to guarantee that the UAVs achieve the the optimal control law are a linear function of the Laplacian
desired formation. More importantly, the formation will be matrix L and the last term requires only the information about
achieved synchronously by an optimal consensus formula- the UAVs own position and velocity, the colliding object’s
tion, that is, the formulation of the Laplacian matrix L into position, and the reference if this UAV has access to it.
the formation cost J1 . The last term of the control law is This distributed control law offers a great implementation
−1/(2wc2 )gv (X) and it has two components as shown in (65). advantage.
The first component gtr (X) controls the final formation to Remark 4.7: The comparison between this paper and the
v
follow the desired trajectory, while the second term ga v (X) similar works in [15], and [35]–[37] can be summarized from
provides the obstacle/collision avoidance control. the following aspects.
Remark 4.5: To recapitulate the selection of the weight 1) Different Dynamics of the Vehicle: the vehicles in [15]
parameters, one can choose a large wv given w p for (60) are 2-D nonholonomic robots, whereas the vehicle in
to meet the condition (27). Then with the above chosen this paper is 3-D UAVs, which have more complex
wv and w p , one can choose a small enough wc for (61) dynamics. More importantly, the constraint on obsta-
to meet the condition (29). In addition, this rule of weight cle/collision avoidance for ground robots and aerial
selection also applies to the condition (21) for a meaning- vehicles like UAVs are quite different since UAVs cannot
ful formation cost function (20). Meanwhile, the tracking do the avoidance maneuver at a very low speed.
weights wt p and wt v can be tuned to obtain a proper tracking 2) Cooperative Control Strategy: the control law in [15] is
speed. derived from the stability perspective, whereas this paper
1740 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 21, NO. 5, SEPTEMBER 2013

Fig. 7. Time histories of the positions xi and velocities ẋi . Fig. 8. Time histories of the positions yi and velocities ẏi .

solves the formation control problem from the optimality

perspective. Moreover, many formation control methods
including the one in [15] design the control law via
the noncooperative leader–follower strategy, which may
not guarantee the simultaneous convergence of vehi-
cles to the desired formation. The proposed method in
this paper employs a consensus-based formation control
strategy that can synchronize all UAVs’ motion at any
time to guarantee a simultaneous convergence to the
desired formation. Furthermore, the cooperative control
law proposed in this paper is a linear function of
the Laplacian matrix and thus takes into consideration
the communication constraints among UAVs. Therefore,
only neighboring UAVs’ information is needed for coop-
eration, and it is not required for all UAVs to know a
global leader’s information. Fig. 9. Time histories of the positions h i and velocities ḣ i .
3) Avoidance Strategy: Equations (40) and (48) as well as
Remark 4.4 underline the rationale of obstacle/collision
avoidance. Compared to the penalty function adopted strategy in this paper is able to guarantee that the UAV
in [15], and [35]–[37], the avoidance penalty func- flies along the predefined direction ν ak and to avoid a
tion (40) is similar in part but different in the sense potential large maneuver as well as unfeasible low speed
of avoidance strategy. The penalty function in [15], of UAVs.
and [35]–[37]
" has only #the" relative-position-related
#
2 2 2 V. S IMULATION R ESULTS AND A NALYSIS
term (R+r̄)2 − pi −ck 2 /  pi −ck 2 − (r + r̄) ,
whereas the penalty function (40) in this paper has an The effectiveness of the proposed optimal formation control
x,y x,y law given by Theorem 4.1 is illustrated in this section. In this
additional velocity tracking term (ν i −ν ak )T (ν i −ν ak ).
Therefore, the principle of the avoidance control is paper, the drag in the UAV model (4) is calculated by [33]
different. Using the strategy in [15], and [35]–[37], the 0.5ρ(Vgi − Vwi )2 Area C D0 + 2kd kn2 n 2gi m 2i
avoidance direction may be along the opposite direction D gi = . (66)
[ρ(Vgi − Vwi )2 Area ]
of the line from the agent to the colliding object, which
may lead to a large maneuver or significant slowdown of It is assumed that the wing area is Area = 1.37 m2 . The UAV’s
the vehicle. This strategy may be feasible for robots. But radius and the radius of the detection region are assumed to
for flying vehicles, such as UAVs, they may not be able be r = 1.5 m and R = 100 m, respectively. Other parameters
to do that large maneuver or fly at a very low speed. For in the model are: zero-lift drag coefficient C D0 = 0.02, load-
that reason, we propose a new avoidance strategy in (40) factor effectiveness kn = 1, induced drag coefficient kd = 0.1,
to circumvent this problem by diverting the UAV along gravitational coefficient ga = 9.81 kg/m2, atmospheric density
the direction tangential to the safety ball as shown in ρ = 1.225 kg/m3 , and the weight of the UAV m i = 20 kg. The
Fig. 4. As discussed in Remark 4.4, the new avoidance gust model Vwi = Vwi ,normal + Vwi ,tan is scaled based on [34]
WANG AND XIN: INTEGRATED OPTIMAL FORMATION CONTROL OF MULTIPLE UAVs 1741

Fig. 10. Time histories of the actual states and actual controls. Fig. 12. Top view of UAVs’ formation flying with obstacle/collision
avoidance.

(0, 200, 95) m, respectively, and the initial velocities of

the four UAVs are assumed to be 40, 50, 60, and 70 m/s,
respectively. The initial flight-path angle and heading angle
are set to 0°. The UAVs are required to fly in a rhombic-
shaped formation as shown in Fig. 2. The desired rhombic
formation can be described by (11)
 T T
σ p1 , . . . , σ Tpn 0T , . . . , 0T
σ =
 (69)
σ Tp

where
σ p1 = [0; −100; 1] m; σ p2 = [100; 0; 1] m
σ p3 = [−100; 0; 1] m; σ p4 = [0; 100; 1] m.
The final velocity and altitude of the formation are desired
Fig. 11. 3-D trajectories of UAVs’ formation flying with two-obstacle
avoidance. to follow a straight-line reference trajectory with the initial
position of [ 0 −100 80 ]T m and the constant velocity of
[ 60 0 0 ]T m/s. Assume that only UAV 1 has access to the
and varies according to the altitude h i . In the simulated gust, reference. The associated weights in the control law are set to
the normal wind shear is given by w p = 0.08, wv = 2, wc = 2, wt p = 0.04, and wt v = 1. Two
simulation scenarios are considered as follows.
Vwi ,normal = 0.215Vm log10 (h i ) + 0.285Vm (67)

where Vm = 4.0 m/s is the mean wind speed at an altitude of
80 m, which is the simulated altitude. The turbulence part of
the wind gust Vwi ,tan has a Gaussian distribution with a zero
mean and a standard derivation of 0.09Vm . The constraints on
the control variables are Th i ≤ 125N, −1.5 ≤ n gi ≤ 2.0, and
−80° ≤ φbi ≤ 80°.
The undirected and connected communication topology of
the four UAVs is illustrated in Fig. 2 and can be described by
the Laplacian matrix L
⎡ ⎤
1 −1 0 0
⎢ −1 2 −1 0 ⎥
L=⎢ ⎣ 0 −1 2 −1 ⎦.
⎥ (68)
0 0 −1 1
The initial positions of the four UAVs are assumed to be
at (0, −200, 70) m, (0, −60, 80) m, (0, 60, 90) m, and
A. Formation Control With Collision Avoidance and Without
Obstacles on the Trajectories of UAVs
In this scenario, an obstacle is assumed to appear at (−1000,
0, 80) m, which is not on the trajectory of any UAV. The
radius of the obstacle is assumed to be r1 = 15 m. The
safe distance rsafe to the colliding object is set to 30 m.
The simulation results of the four UAVs’ motion under the
proposed optimal formation control are shown in Figs. 5–10.
Fig. 5 demonstrates the 3-D trajectories and Fig. 6 shows the
top view of the trajectory. As can be seen from Fig. 5, all the
UAVs are driven to a rhombic formation, and the formation
follows the desired reference trajectory. It can also be seen
from Fig. 6 that the obstacle avoidance does not take effect
since no obstacle enters the detection region of UAVs, whereas
the collision avoidance does take effect, as shown in the
rectanglular box in Fig. 6, where UAV 2 and UAV 3 enter each
1742 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 21, NO. 5, SEPTEMBER 2013
Fig. 13. Time histories of the relative distances between UAVs and obstacles.
Fig. 14. Time histories of the positions xi and velocities ẋi .
other’s detection region. Figs. 7–10 present the time histories
of the UAVs’ positions, velocities, flight path angles, heading
angles, and actual controls. It can be seen that UAV 1 tracks
the reference, and all UAVs fly in formation with the desired
altitude of 80 m and the desired velocity of [ 60 0 0 ]T m/s.
This is achieved by combining the optimization of the first cost
function component J1 with the optimization of the tracking
cost gtr ( X̂) as well as the collision avoidance cost component
of ga ( X̂) in J2 . In addition, it is worth noting that all the UAVs
achieve the formation simultaneously, which is guaranteed
by formulating the Laplacian matrix L in the cost function
J1 . Three bottom figures in Fig. 10 demonstrate the control
responses and they are all within the prescribed constraints.
Note that the actual controls are computed using (6).
B. Formation Control With Collision and Obstacle Avoidance
With Two Obstacles on the Trajectories of the UAVs
In this scenario, the first obstacle is assumed to appear
at (1546, 23, 84) m, which is on the trajectory of UAV 3.
Fig. 15. Time histories of the positions yi and velocities ẏi .
Fig. 16. Time histories of the positions h i and velocities ḣ i .
The radius of the obstacle is set to r1 = 15 m. The second
obstacle is assumed to appear at (5519, −105, 80) m, which
is on the trajectory of UAV 1. The radius of this obstacle is
set to r2 = 10 m. The safe distance rsafe to both obstacles is
set to 30 m. The simulation results are given in Figs. 11–17.
It can be seen from Fig. 11 that the proposed optimal
control law is able to prevent all the UAVs from colliding
with each other and with the obstacles. In addition, all four
UAVs finally reach the desired formation, and the formation
follows the reference trajectory. A top view of the trajectory
is shown in Fig. 12 to view the obstacle/collision avoidance
more clearly. The small black rectanglular boxes show where
the collision avoidance occurs, and the small black circle boxes
show where the obstacle avoidance occurs. The first collision
avoidance is the same as the one in Fig. 6. Then, the UAVs
keep flying forward until obstacle 1 enters the detection region
of UAV 3. With the proposed controller, UAV 3 moves toward
the positive ŷ direction to avoid obstacle 1, and other UAVs
follow UAV 3 to move toward the positive ŷ direction as
well due to the relative distance requirement of the desired
WANG AND XIN: INTEGRATED OPTIMAL FORMATION CONTROL OF MULTIPLE UAVs
Fig. 17. Time histories of the actual states and actual controls.
formation. When UAV 2 enters the detection region of UAV
3, the collision avoidance control prevents UAV 2 and UAV 3
from moving closer such that the second collision avoidance
can be observed. Soon after, obstacle 2 enters the detection
region of UAV 1. The proposed control succeeds driving
UAV 1 toward the positive ŷ direction to avoid obstacle 2.
Eventually, the UAVs achieve the desired formation and fly
along the reference trajectory. Fig. 13 gives the time histories
of the relative distances between UAVs and obstacles. It can
be easily seen from the blue line of the bottom zoomed figure
in Fig. 13 that the minimum relative distance between UAV
3 and obstacle 1 is 20 m, which is greater than the sum of
obstacle 1’s radius (15 m) and the UAV’s radius (1.5 m). This
implies the successful avoidance of obstacle 1. Also, from the
red line of the bottom zoomed figure in Fig. 13 one can see that
the minimum relative distance between UAV 1 and obstacle
2 is 15 m, which is greater than the sum of obstacle 2’s radius
(10 m) and the UAV’s radius (1.5 m) as well. Thus, it implies
the successful avoidance of obstacle 2.
The time histories of the UAV’s positions and velocities
in Figs. 14–16 show that UAV 1 tracks the reference, and
all UAVs fly into formation synchronously with the final
desired altitude of 80 m and the final desired velocity of
[ 60 0 0 ]T m/s, which is the same as the previous scenario.
The three top figures in Fig. 17 illustrate the time histories of
the ground velocities, flight path angles, and heading angles,
respectively, which are the actual states of UAVs and exhibit
variations in the presence of obstacle and collision avoidance.
The three bottom figures in Fig. 17 illustrate the actual control
responses and they are all within the prescribed constraints.
VI. C ONCLUSION
In this paper, the multiple UAV’s formation control with
obstacle/collision avoidance was designed in a unified optimal
control framework. A nonquadratic avoidance cost function
was constructed via a new inverse optimal control approach
such that the optimal control law can be obtained in an
analytical form. The unified formulation can guarantee not
1743
only the synchronous formation flying with obstacle/collision
avoidance but also formation flying along desired trajectory
with desired velocity. In addition, the optimal formation con-
trol law is a linear function of the Laplacian matrix and thus
only requires local information from the neighbors based on
the communication topology. Both asymptotic stability and
optimality of this algorithm have been proved. The simulation
results demonstrated that the proposed distributed optimal
control law is effective in controlling the formation flying in
an obstacle-laden environment.
R EFERENCES
[1] D. F. Chichka and J. L. Speyer, “Solar-powered, formation-enhanced
aerial vehicle systems for sustained endurance,” in Proc. Amer. Control
Conf., Philadelphia, PA, Jun. 1998, pp. 684–688.
[2] P. K. C. Wang, “Navigation strategies for multiple autonomous mobile
robots moving in formation,” J. Robot. Syst., vol. 8, no. 2, pp. 177–195,
Apr. 1991.
[3] T. Balch and R. C. Arkin, “Behavior-based formation control for
multirobot teams,” IEEE Trans. Robot. Autom., vol. 14, no. 6, pp. 926–
939, Dec. 1998.
[4] F. L. Lewis and K. H. Tan, “High precision formation control of mobile
robots using virtual structures,” Auton. Robots, vol. 4, no. 4, pp. 387–
403, 1997.
[5] E. W. Justh and P. S. Krishnaprasad, “A simple control law for UAV
formation flying,” ISR, Univ. Maryland, Baltimore, Tech. Rep. TR 2002-
38, 2002.
[6] D. Stipanovic, G. Inalhan, R. Teo, and C. J. Tomlin, “Decentralized
overlapping control of a formation of unmanned aerial vehicles,” Auto-
matica, vol. 40, no. 8, pp. 1285–1296, 2004.
[7] Y. Gu, B. Seanor, G. Campa, M. R. Napolitano, L. Rowe, S. Gururajan,
and S. Wan, “Design and flight testing evaluation of formation control
laws,” IEEE Trans. Control Syst. Technol., vol. 14, no. 6, pp. 1105–1112,
Nov. 2006.
[8] J. Lavaei, A. Momeni, and A. G. Aghdam, “A model predictive
decentralized control scheme with reduced communication requirement
for spacecraft formation,” IEEE Trans. Control Syst. Technol., vol. 16,
no. 2, pp. 268–278, Mar. 2008.
[9] A. Abdessameud and A. Tayebi, “Formation control of VTOL-UAVs,”
in Proc. Joint 48th IEEE Conf. Decision Control, Shanghai, China, Dec.
2009, pp. 3454–3459.
[10] A. K. Das, R. Fierro, V. Kumar, J. P. Ostrowski, J. Spletzer, and C.
J. Taylor, “A vision-based formation control framework,” IEEE Trans.
Robot. Autom., vol. 18, no. 5, pp. 813–825, Oct. 2002.
[11] H. G. Tanner and A. Kumar, “Formation stabilization of multiple agents
using decentralized navigation functions,” in Robotics: Science and
Systems I, S. Thrun, G. Sukhatme, S. Schaal, and O. Brock, Eds.
Cambridge, MA: MIT Press, 2005, pp. 49–56.
[12] Y. Liang and H. H. Lee, “Decentralized formation control and obstacle
avoidance for multiple robots with nonholonomic constraints,” in Proc.
IEEE Amer. Control Conf., Minneapolis, MN, Jun. 2006, pp. 5596–5601.
[13] J. Shao, G. Xie, and L. Wang, “Leader-following formation control of
multiple mobile vehicles,” IET Control Theory Appl., vol. 1, no. 2, pp.
545–552, Mar. 2007.
[14] C. D. L. Cruz and R. Carelli, “Dynamic model based formation control
and obstacle avoidance,” Robotica, vol. 26, no. 3, pp. 345–356, 2008.
[15] S. Mastellone, D. M. Stipanovic, C. Graunke, K. Intlekofer, and M.
W. Spong, “Formation control and collision avoidance for multiagent
nonholonomic systems: Theory and experiments,” Int. J. Robot. Res.,
vol. 27, no. 1, pp. 107–126, 2008.
[16] M. Defoort, T. Floquet, A. Kokosy, and W. Perruquetti, “Sliding-mode
formation control for cooperative autonomous mobile robots,” IEEE
Trans. Ind. Electron., vol. 55, no. 11, pp. 3944–3953, Nov. 2008.
[17] A. K. Ray, P. Benavidez, L. Behera, and M. M. Jamshidi, “Decentralized
motion coordination for a formation of rovers,” IEEE Syst. J., vol. 3,
no. 3, pp. 369–381, Sep. 2009.
[18] G. Antonelli, F. Arrichiello, and S. Chiaverini, “Experiments of for-
mation control with multirobot systems using the null-space-based
behavioral control,” IEEE Trans. Control Syst. Technol., vol. 17, no. 5,
pp. 1173–1182, Sep. 2009.
[19] T. Dierks and S. Jagannathan, “Neural network output feedback control
of robot formations,” IEEE Trans. Syst., Man, Cybern., B, Cybern.,
vol. 40, no. 2, pp. 383–399, Apr. 2010.
1744 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 21, NO. 5, SEPTEMBER 2013
[20] J. Wang, X. B. Wu, and Z. L. Xu, “Potential-based obstacle avoidance in
formation control,” J. Control Theory Appl., vol. 6, no. 3, pp. 311–316,
2008.
[21] G. Vachtsevanos, L. Tang, and J. Reimann, “An intelligent approach to
coordinated control of multiple unmanned aerial vehicles,” in Proc. 60th
Annu. Forum Amer. Helicopter Soc., Baltimore, MD, Jun. 2004, pp. 1–9.
[22] H. L. Yuan, V. Gottesman, M. Falash, Z. H. Qu, E. Pollak, and J.
M. Chunyu, “Cooperative formation flying in autonomous unmanned
air systems with application to training,” in Advances in Cooperative
Control and Optimization (Lecture Notes in Control and Information
Sciences), vol. 369. New York: Springer-Verlag, 2007.
[23] X. Wang, V. Yadav, and S. N. Balakrishnan, “Cooperative UAV forma-
tion flying with obstacle/collision avoidance,” IEEE Trans. Control Syst.
Technol., vol. 15, no. 4, pp. 672–679, Jul. 2007.
[24] T. Keviczky, B. Francesco, K. Fregene, D. Godbole, and G. J. Balas,
“Decentralized receding horizon control and coordination of autonomous
vehicle formations,” IEEE Trans. Control Syst. Technol., vol. 16, no. 1,
pp. 19–33, Jan. 2008.
[25] J. Shin and H. J. Kim, “Nonlinear model predictive formation flight,”
IEEE Trans. Syst., Man, Cybern., A, Syst. Humans, vol. 39, no. 5, pp.
1116–1125, Sep. 2009.
[26] D. S. Bernstein, “Nonquadratic cost and nonlinear feedback control,”
Int. J. Robust Nonlinear Control, vol. 3, no. 3, pp. 211–229, 1993.
[27] W. M. Haddad and V. Chellaboina, Nonlinear Dynamical Systems and
Control: A Lyapunov-Based Approach. Princeton, NJ: Princeton Univ.
Press, 2000.
[28] W. Ren and R. W. Beard, Distributed Consensus in Multivehicle Coop-
erative Control. London, U.K.: Springer-Verlag, 2008.
[29] P. K. Menon and G. D. Sweriduk, “Optimal strategies for free-flight
air traffic conflict resolution,” J. Guidance, Control, Dynamics, vol. 22,
no. 2, pp. 357–368, Mar. 1999.
[30] P. K. Menon, “Short-range nonlinear feedback strategies for aircraft
pursuit-evasion,” J. Guidance, Control, Dynamics, vol. 12, no. 1, pp.
202–211, 1989.
[31] G. Lafferriere, A. Williams, J. Caughman, and J. J. Veerman, “Decen-
tralized control of vehicle formations,” Syst. Control Lett., vol. 54, no. 9,
pp. 899–910, 2005.
[32] J. N. Wang and M. Xin, “Multiagent consensus algorithm with obstacle
avoidance via optimal control approach,” Int. J. Control, vol. 83, no. 12,
pp. 2606–2621, Dec. 2010.
[33] Y. J. Xu, “Nonlinear robust stochastic control for unmanned aerial
vehicles,” J. Guidance, Control, Dynamics, vol. 32, no. 4, pp. 1308–
1319, Jul.–Aug. 2009.
[34] “Advisory circular,” U.S. Department of Transportation and Federal
Aviation Administration, Washington, DC, Tech. Rept. AC 120-28D,
Jul. 1999.
[35] D. M. Stipanovic, P. F. Hokayem, M. W. Spong, and D. D. Siljak,
“Cooperative avoidance control for multiagent systems,” J. Dynamic
Syst., Meas. Control, vol. 129, no. 5, pp. 699–707, 2007.
[36] G. Leitmann and J. Skowronski, “Avoidance control,” J. Optim. Theory
Appl., vol. 23, pp. 581–591, Jul. 1977.
[37] E. J. Rodriguez-Seda, J. J. Troy, C. A. Erignac, P. Murray, D. M.
Stipanovic, and M. W. Spong, “Bilateral teleoperation of multiple mobile
agents: Coordinated motion and collision avoidance,” IEEE Trans.
Control Syst. Technol., vol. 18, no. 4, pp. 984–992, Jul. 2010.
Jianan Wang (S’09–M’12) received the B.S. degree
from Beijing Jiaotong University, Beijing, China,
and the M.S. degree from the Beijing Institute of
Technology, Beijing, in 2004 and 2007, respectively,
both in control science and engineering, and the
Ph.D. degree in aerospace engineering from Missis-
sippi State University, Starkville, in 2011.
He is currently a Post-Doctoral Research Fellow
with the Department of Electrical Engineering and
Computer Science, University of Central Florida,
Orlando. His current research interests include co-
operative control of multiagent systems, unmanned aerial vehicle formation
control, trustworthy networked systems, and sensor networks.
Dr. Wang is a Senior Member of the American Institute of Aeronautics and
Astronautics.
Ming Xin (M’02) received the B.S. and M.S.
degrees from the Nanjing University of Aeronautics
and Astronautics, Nanjing, China, in 1993 and 1996,
respectively, both in automatic control, and the Ph.D.
degree in aerospace engineering from the Missouri
University of Science and Technology (formerly
University of Missouri-Rolla), Rolla, in 2002.
He is currently an Associate Professor with the
Department of Aerospace Engineering, Mississippi
State University, Starkville. His current research
interests include nonlinear optimal control theory,
guidance and control of aerospace vehicles, control of multi-agent systems,
neural networks, and nonlinear estimation/filtering.
Dr. Xin was a recipient of the National Science Foundation CAREER Award
in 2009. He is an Associate Fellow of the American Institute of Aeronautics
and Astronautics (AIAA) and a member of the AIAA Atmospheric Flight
Mechanics Technical Committee.