# Robust Decentralized Formation Flight Control

Authors: Zhao Weihua; Tiauw Hiong Go
Journal: International Journal of Aerospace Engineering
 (2011)

---

## Abstract

Motivated by the idea of multiplexed model predictive control (MMPC), this paper introduces a new framework for unmanned aerial vehicles (UAVs) formation flight and coordination. Formulated using MMPC approach, the whole centralized formation flight system is considered as a linear periodic system with control inputs of each UAV subsystem as its periodic inputs. Divided into decentralized subsystems, the whole formation flight system is guaranteed stable if proper terminal cost and terminal constraints are added to each decentralized MPC formulation of the UAV subsystem. The decentralized robust MPC formulation for each UAV subsystem with bounded input disturbances and model uncertainties is also presented. Furthermore, an obstacle avoidance control scheme for any shape and size of obstacles, including the nonapriorily known ones, is integrated under the unified MPC framework. The results from simulations demonstrate that the proposed framework can successfully achieve robust collision-free formation flights.


---

## Body

## 1. Introduction

Formation flight control of multiple Unmanned Aerial Vehicles (UAVs) has been a hot research topic for the last few years. Formation flight of UAVs can potentially be useful for increased surveillance coverage, better target acquisition, and increased security measures [1–3].Different approaches have been developed for formation flight control. Linear formation flight controller has been discussed [2, 4, 5]. The advantage of the linear control is that it is intuitive and easy to synthesize, but it cannot handle the constraints directly and may not be valid for large operation range since it is designed around a fixed operating point. Some researchers addressed the nonlinear formation flight control problem by using feedback linearization [6, 7] and adaptive control [3, 8, 9]. Although these nonlinear control methods can deal with the unmodelled dynamics, they cannot handle the constraints directly and the implementation of such controller may result in ill-defined control inputs.Model Predictive Control (MPC) is one of the frequently applied advanced optimal control methods in industry. Recently, with the advent of powerful microcontroller and small-size sensors, it has become possible to extend the MPC method to UAV systems which exhibit relatively fast dynamics. Many works have addressed the “decentralized MPC” in UAV cooperative control [10–14]. Although the control optimization for the UAVs are all performed sequentially, these published works can be sorted in two groups based on their input update scheme.Scheme 1.
Updates all the UAVs’ control inputs simultaneously. For example, in the schemes proposed in [14–16], the MPC solution is solved iteratively for each vehicle input, but it is assumed that all the UAVs’ inputs are updated simultaneously when the iterations have been completed.Scheme 2.
Similar to Scheme1 except that it updates each UAV’s input as soon as its computation is finished [10–12, 17].In this paper, motivated by the MMPC proposed in [18], the centralized formation flight problem is put into a periodic form in which only one UAV’s inputs are updated at each time step. This substantially reduces the computation time and load. Furthermore, by neglecting the dynamical couplings between the individual vehicles, the centralized periodic formation flight system can be treated as a decentralized system. Suppose that a given UAVs formation flight control problem can be solved in not less than T seconds by applying the Scheme 1. In the MMPC scheme, we propose to update only one UAV’s inputs at interval smaller than T. If the dynamics and optimization of each UAV is the same, then typically the update interval is T/m, where m is the number of UAVs in the formation group. The main motivation for this formation flight scheme is the belief that updating only one UAV’s input at a faster time step will be beneficial for rapid response to commands from Ground Control Station (GCS) and to disturbances as compared to Scheme 1. Compared to Scheme 2, since we know the exact update timing of each UAV in the current proposed scheme, it is easier for the human operator to schedule the formation tasks or to trouble-shoot when errors exist in the formation flight group. This scheme functions like an “update manager” system which systematically handle and schedule the update of each UAV in the formation system. Additionally, this proposed scheme is useful in the heterogeneous formation flight scenario, where the leader UAV or the more capable UAV (equipped with more sensors) can be updated much more often than the other UAVs without affecting the stability of the overall formation flight system (since the formation stability does not depend on the update sequence).Another contribution of this paper is the extension of the obstacle avoidance algorithm in the formation flight MPC framework. Most of the existing collision avoidance scheme under the MPC formation flight treats the shape of the obstacles to be rectangle and does not take into account the small or nonapriorily known obstacles. The framework proposed will try to eliminate these limitations.In summary, the contributions of this paper are in the development of a robust collision-free formation flight control system using the MMPC idea. Specifically, the paper addresses the following.(1)
In the overall formation flight control scheme, extend the MMPC algorithm for formation flight application and provide the conditions (terminal cost and stabilizing feedback gain) for the whole formation group to be stable by applying the periodic linear system theory.(2)
In the obstacle avoidance scheme, extend the current collision avoidance algorithm to include various shapes of obstacles and prevent the collision with small or nonapriorily known obstacles by using the spatial detection horizon idea, which is then translated into an additional convex position constraint.The paper is organized as follows. Section2 presents the overview of the formation flight control strategy. The controlled UAV dynamics, the multiplexed formation flight system, and stability requirements for each UAV subsystem are given in Section 3. The stable robust decentralized formation flight control is detailed in Section 4, including its problem formulation, the MPC controller design, and stability analysis. In Section 5, the obstacle avoidance control scheme is addressed. Some simulation results are presented in Section 6. Finally, discussions and conclusions are given in Section 7.


## 2. Formation Flight Control Strategy Overview

Assumption 1.
Each UAV is equipped with autopilot system capable of waypoint tracking.
With this assumption, the dynamics of each UAV subsystem in the formation flight group can be represented by their controlled dynamics (with autopilot), which can be modeled as linear system or piecewise linear system.Assumption 2.
The couplings between the UAVs, if any, come only from the cost function and constraint formulations in the MPC optimization problem. The dynamical couplings, like induced vortices, are neglected.
With this assumption, the whole formation flight system can be formulated as a linear periodic system under the MMPC as detailed in Section3.The overall formation flight control scheme is represented in Figure1. Each UAV in the formation is considered as a subsystem in the framework of the whole MMPC formation flight system. In the following section, we show that under the MMPC framework, the whole formation flight system is stable if proper terminal cost and stabilizing feedback gain are added to the optimization problem formulation of each UAV subsystem.Figure 1
The overall formation flight control system under MMPC.The key idea of the proposed framework is to break the big centralized MPC formation flight problem into decentralized problems of smaller size while the stability of the whole formation flight system is guaranteed. In order to achieve this, the framework utilizes two steps: first, to apply the MMPC update scheme by formulating the whole formation flight as a linear periodic system; and second, to break the centralized periodic system into decentralized systems consisting of individual UAV, which is possible under Assumption2 above.The use of robust decentralized model predictive control (RDMPC) scheme to update only one UAV subsystem at a time is proposed. Assume that there arem UAVs in the formation, so that at each time step T/m, we update one UAV’s control inputs. After m updates, a fresh cycle of updates begins, so that each whole cycle of updates repeats with a cycle time T. Figure 1 shows the scenario for m=3.Each RDMPC controller is associated with a different vehicle and computes the local control inputs based only on its states and that of it neighbors (shown in Figure2). On each UAV, the current state and the model of its neighbors are used to predict their possible trajectories and move accordingly. The information exchange topology and intervehicle constraints are described by a graph structure in the problem formulation and depends on the task descriptions. The cost function depends on the formation’s mission and includes terms that minimize relative distances and/or velocities between neighboring vehicles. The coupling constraints arise from interagent collision avoidance and communication range. The interaction graph is based on a “closest spatial neighbors” model.Figure 2
Decentralized UAV formation flight control.


## 3. UAV Subsystem Dynamics and Overall Formation Flight System under MMPC

### 3.1. Dynamics of UAV Subsystem

In this paper, the inputs, outputs, and states constraints are all regarded as polytopes. Some pertinent definitions are given in the AppendixA. For more details, readers may refer to [19].Under Assumption1, consider a set of m linear decoupled UAV dynamical systems, where the ith vehicle dynamics is described by the discrete-time time-invariant state equation
(1)xk+1i=fi(xki,uki)=(Ai+Âi)xki+Biuki+Dwki,yki=hi(xki,uki)=Cixki,
where the state update function fi:R6×R3→R6 and output map hi:R6→R6 are linear or piecewise linear functions of its inputs. The states and inputs of the vehicle at time k are denoted by xki∈R6 and uki∈R3, respectively. In particular,(2)uki=[NcmdEcmdhcmd],yki=[NEhṄĖḣ]=[yk,posiyk,veli],xki=[xk,posixk,veli],
and xk,posi∈R3 is the vector of N,E,h (north-east-down) coordinates and xk,veli∈R3 denotes a vector of states corresponding to N-,E-, and h-axis velocity at time k. The subscript cmd means the current commanded position.In general, (1) subjects to the disturbance input(3)wki∈W⊂Rq.Additionally, as indicated in (1), there may also be model uncertainty in the system, indicated by the uncertainty set (4)Ai={Âi∣‖Âi‖‖Ai‖=ε},
where the constant matrix Ai gives the average value of the state matrix, and Âi describes its statistical variation. For example, if we assume that the uncertainty in the state matrix is 10% of its nominal value, then ε=∥Âi∥/∥Ai∥=0.1.LetUki∈Rmi, Xi∈Rni, and Yki∈Rpi denote the set of feasible inputs, states, and outputs of the ith vehicle
(5)uki∈Ui,xi∈Xi,yki∈Yi,k≥0,
where Yki and Uki are given polytopes. In the case of UAV, (5) often indicates the constraints on the speed and acceleration of the vehicle as follows:
(6)yveli∈Yv={z∈R3∣y̲veli≤zi≤y̅veli},ẏveli∈Ya={z∈R3∣ẏ̲veli≤zi≤ẏ̅veli}.
The set of m constrained systems are referred to as the multivehicle system. Let xk∈Rñ with ñ=∑ini and uk∈Rm̃ with m̃=∑imi be the vectors which collect the states and inputs of the multivehicle system at time k, that is, xk=[xk1;…;xkm]T, uk=[uk1;…;ukm]T, with  xk+1=f(xk,uk). Given the particular command (target, formation shape, etc.), the equilibrium pair of the ith vehicle is denoted by (xei,uei), that is, the ith vehicle is in the specified formation position with respect to the neighboring vehicle, while (xe,ue) denotes the corresponding equilibrium for the multivehicle system, which means the required formation states.


### 3.2. Formation Flight System under MMPC

Under Assumption2, the whole formation flight system can be expressed as (7)xk+1=[xk+11⋮xk+1m]=(A100⋮⋱⋮0⋯Am)[xk1⋮xkm]+(B1…0⋮⋱⋮0⋯Bm)[uk1⋮ukm]=Axk+Buk=Axk+∑j=1mBjukj,
where Bj is the jth column of B.As we will be referring to the expression(kmodm)+1 often in this section, it is convenient to introduce the indexing function
(8)σ(k)=(kmod⁡m)+1.According to our UAV input update scheme, at each fixed time step only one of UAV’s inputs,ukσ(k), will be updated. Under such scheme, the centralized system can be represented as a periodic linear system with one UAV’s input:
(9)xk+1=Axk+Bσ(k)ukσ(k).According to [18], in order to make the whole linear periodic system (9) closed-loop stable by using the MMPC scheme, the following two additional terms have to be added to the optimization problem formulation.(i)
Terminal cost term:(10)F(xk+N)=min⁡ukσ(k){∑i=N∞(‖xk+i+1-xk+i+1d‖Q2+‖uk+iσ(k)‖R2)∣xk+1=Axk+Bσ(k)ukσ(k)∑i=N∞}=xk+NTP̅σ(k+N)xk+N+2q̅σ(k+N)Txk+N+r̅σ(k+N),
where P̅σ(k+N) can be calculated according to (B.2) in Appendix B.(ii)
Terminal states constraints:(11)xk+N+1∈XI(Kσ(k)),
where XI(Kσ(k)) denotes the set in which none of the constraints is active. This is the maximum positive invariant set for the linear periodic system (9), when a linear periodic feedback controller (B.3) is applied. Kσ(k) can be calculated according to (B.4) in Appendix B.Remark 1.
The terminal cost and the linear terminal stabilization controller can be derived from the unconstrained infinite-time linear quadratic (LQ) control theory of periodic system (See AppendixB).Remark 2.
The stability requirements for the MMPC formation flight framework from (10) and (11) actually need the state information of the multivehicle system, that is, xk, to be included in the terminal cost calculation, but according to Assumption 2, since the dynamical effects between UAV systems are ignored, the terminal cost and terminal stabilization controller of each vehicle subsystem can be calculated independently, that is, using only the ith vehicle and its neighbors’ states information.


## 3.1. Dynamics of UAV Subsystem

In this paper, the inputs, outputs, and states constraints are all regarded as polytopes. Some pertinent definitions are given in the AppendixA. For more details, readers may refer to [19].Under Assumption1, consider a set of m linear decoupled UAV dynamical systems, where the ith vehicle dynamics is described by the discrete-time time-invariant state equation
(1)xk+1i=fi(xki,uki)=(Ai+Âi)xki+Biuki+Dwki,yki=hi(xki,uki)=Cixki,
where the state update function fi:R6×R3→R6 and output map hi:R6→R6 are linear or piecewise linear functions of its inputs. The states and inputs of the vehicle at time k are denoted by xki∈R6 and uki∈R3, respectively. In particular,(2)uki=[NcmdEcmdhcmd],yki=[NEhṄĖḣ]=[yk,posiyk,veli],xki=[xk,posixk,veli],
and xk,posi∈R3 is the vector of N,E,h (north-east-down) coordinates and xk,veli∈R3 denotes a vector of states corresponding to N-,E-, and h-axis velocity at time k. The subscript cmd means the current commanded position.In general, (1) subjects to the disturbance input(3)wki∈W⊂Rq.Additionally, as indicated in (1), there may also be model uncertainty in the system, indicated by the uncertainty set (4)Ai={Âi∣‖Âi‖‖Ai‖=ε},
where the constant matrix Ai gives the average value of the state matrix, and Âi describes its statistical variation. For example, if we assume that the uncertainty in the state matrix is 10% of its nominal value, then ε=∥Âi∥/∥Ai∥=0.1.LetUki∈Rmi, Xi∈Rni, and Yki∈Rpi denote the set of feasible inputs, states, and outputs of the ith vehicle
(5)uki∈Ui,xi∈Xi,yki∈Yi,k≥0,
where Yki and Uki are given polytopes. In the case of UAV, (5) often indicates the constraints on the speed and acceleration of the vehicle as follows:
(6)yveli∈Yv={z∈R3∣y̲veli≤zi≤y̅veli},ẏveli∈Ya={z∈R3∣ẏ̲veli≤zi≤ẏ̅veli}.
The set of m constrained systems are referred to as the multivehicle system. Let xk∈Rñ with ñ=∑ini and uk∈Rm̃ with m̃=∑imi be the vectors which collect the states and inputs of the multivehicle system at time k, that is, xk=[xk1;…;xkm]T, uk=[uk1;…;ukm]T, with  xk+1=f(xk,uk). Given the particular command (target, formation shape, etc.), the equilibrium pair of the ith vehicle is denoted by (xei,uei), that is, the ith vehicle is in the specified formation position with respect to the neighboring vehicle, while (xe,ue) denotes the corresponding equilibrium for the multivehicle system, which means the required formation states.


## 3.2. Formation Flight System under MMPC

Under Assumption2, the whole formation flight system can be expressed as (7)xk+1=[xk+11⋮xk+1m]=(A100⋮⋱⋮0⋯Am)[xk1⋮xkm]+(B1…0⋮⋱⋮0⋯Bm)[uk1⋮ukm]=Axk+Buk=Axk+∑j=1mBjukj,
where Bj is the jth column of B.As we will be referring to the expression(kmodm)+1 often in this section, it is convenient to introduce the indexing function
(8)σ(k)=(kmod⁡m)+1.According to our UAV input update scheme, at each fixed time step only one of UAV’s inputs,ukσ(k), will be updated. Under such scheme, the centralized system can be represented as a periodic linear system with one UAV’s input:
(9)xk+1=Axk+Bσ(k)ukσ(k).According to [18], in order to make the whole linear periodic system (9) closed-loop stable by using the MMPC scheme, the following two additional terms have to be added to the optimization problem formulation.(i)
Terminal cost term:(10)F(xk+N)=min⁡ukσ(k){∑i=N∞(‖xk+i+1-xk+i+1d‖Q2+‖uk+iσ(k)‖R2)∣xk+1=Axk+Bσ(k)ukσ(k)∑i=N∞}=xk+NTP̅σ(k+N)xk+N+2q̅σ(k+N)Txk+N+r̅σ(k+N),
where P̅σ(k+N) can be calculated according to (B.2) in Appendix B.(ii)
Terminal states constraints:(11)xk+N+1∈XI(Kσ(k)),
where XI(Kσ(k)) denotes the set in which none of the constraints is active. This is the maximum positive invariant set for the linear periodic system (9), when a linear periodic feedback controller (B.3) is applied. Kσ(k) can be calculated according to (B.4) in Appendix B.Remark 1.
The terminal cost and the linear terminal stabilization controller can be derived from the unconstrained infinite-time linear quadratic (LQ) control theory of periodic system (See AppendixB).Remark 2.
The stability requirements for the MMPC formation flight framework from (10) and (11) actually need the state information of the multivehicle system, that is, xk, to be included in the terminal cost calculation, but according to Assumption 2, since the dynamical effects between UAV systems are ignored, the terminal cost and terminal stabilization controller of each vehicle subsystem can be calculated independently, that is, using only the ith vehicle and its neighbors’ states information.


## 4. Robust Decentralized Formation Flight

A graph topology [20] is used in this paper to present the connections among the UAVs in the formation group. A directed graph G consists of a set of vertices, denoted V, and a set of edges A⊂V2, where a=(α,β)∈A and α,β∈V.The first element of a is denoted tail(a), and the second is denoted head(a). We assume that tail(a)≠head(a) for all, a meaning that the graph has no self-loops. We also assume that each element of A is unique. A graph with the property that for any (α,β)∈A, the arc (β,α)∈A is said to be undirected. If every possible arc exists, the graph is said to be complete.In this research, a graph topology is used to represent the couplings and we will assume that for every wingman UAV, there is at least one path connecting it with the leader UAV, this assumption guarantees the controllability of the formation flight group [21]. The ith vehicle system is associated with the ith node of the graph. If an edge (i,j)∈A connecting the ith and jth node is present, then the cost and the constraints of the optimal control problem will have a component which is a function of both xi and xj. Before defining the optimal control problem, we define a graph (which can be time-varying) G(t)=(V,A(t)) associated with the vehicle formation group.Using the graph structure defined previously, the optimization problem can be formulated by lettingqi,j(xi,xj) be the interconnection function between the ith and jth UAV for (i,j)∈A and
(12)qi,j(xi,xj)≤0
define the interconnection constraints between the ith and the jth vehicle, with qi,j:Rni×Rnj→R.Remark 3.
Depending on the group tasks, the interconnection functionqi,j(xi,xj) can have several variant forms.(i)
For interagent collision avoidance, this constraints define nonconvex requirements in the following way:(13)qi,j(xki,uki,xkj,ukj)=dsafe-‖yk,posi-yk,posj‖p≤0,
where the parameter dsafe in (13) represents a lower bound on the norm of relative position between neighboring vehicles. The subscript p denotes the type of norm which will be used in the optimization problem. p=2 leads to a circular protection zones around vehicle and the optimization problem becomes quadratic. If p=1 or ∞, then the protection zones around vehicle are square.(ii)
Since the group of vehicles is equipped with a communication network, and that the connectivity of the network depends upon the relative distance between neighboring vehicles, maintaining network connectivity constrains the maximum allowable distance between vehicles. These constraints define convex requirements in the following way:(14)qi,j(xki,uki,xkj,ukj)=‖yk,posi-yk,posj‖p-rcom≤0,
where rcom represents the radius of the detection cylinder, that is, the maximum allowable distance between vehicles.In general, guaranteed collision avoidance and network connectivity between any vehicle pair in a formation flight problem would necessitate the use of a complete graph for describing inter-vehicle constraints. However, for practical decentralization purposes, it is usually sufficient for each vehicle to consider only a neighboring subset of all vehicles to accomplish the formation flight. Furthermore, the allowed number of vehicles in these neighboring subsets might be limited. For example, the maximum number of similar agents whose detection cylinder is in contact with the detection cylinder of the computing agent is 6 (see Figure3).Figure 3
Agent in the center of the cluster exchange information with most 6 neighbors.Consider the following cost function for theith UAV subsystem at time step k:
(15)Jki={∑t=0N-1(‖Qf(yk+t∣ki-yfi)‖+‖R(uk+t∣ki-ufi)‖+F(xk+N)‖Qf(yk+t∣ki-yfi)‖),i=1,∑t=0N-1(‖Ql(yk+t∣ki-yk+t∣kj-L)‖+‖R(uk+t∣ki-ufi)‖+F(xk+N)‖Ql(yk+t∣ki-yk+t∣kj-L)‖),i≠1,(i,j)∈A(k).The leader in the formation fight group is denoted as the first UAV, that is,i=1. ∥Qf(yk+t∣ki-yfi)∥ represents the cost term driving the leader to the target position, the terminal cost term F(xk+N) comes from Section 2, which makes the decentralized MPC controller similar to the infinite horizon formulation. For the wingmen’s cost function, that is,  i>1, the formation shape is maintained by the cost term ∥Ql(yk+t∣ki-yk+t∣kj-L)∥ and L is the separation vector between neighboring UAVs.This cost term can also be used in formation reconfiguration purposes.Given a certain graph interconnection structureG(t), for each UAV, the associated robust decentralized MPC controller (RDMPC) can be presented as
(16)uki=argmin⁡Jki
subject to several constraints below:(1)
system dynamics (without disturbance and model uncertainty).(17)xk+t+1∣ki=Aixk+t∣ki+Biuk+t∣ki,t≥0,yk+t∣ki=Cixk+t∣ki,xk∣ki=xki.(2)
State and control constraints(18)xk+t∣ki∈Xti,ûk+t∣ki∈Uti,t=0,1,…,N.(3)
Stabilizing controller after the prediction horizon(19)uk+t∣ki=K̅ixk+t∣ki+gi,t≥N.(4)
Interconnection constraints forith UAV and its neighboring UAV, that is, (i,j)∈A(k)(20)qi,j(xk+t+1∣ki,uk+t+1∣ki,xk+t+1∣kj,uk+t+1∣kj)≤0,t≥0,(i,j)∈A(k),
where the control sequence is given by Uki=[uk∣ki;uk+1∣ki;…;uk+N-1∣ki]T, and the double indices (k+j∣k) denote the prediction at time k of a value at time k+j.Remark 4.
(i) For the stabilizing controller around the equilibrium point, the close-loop system equation can be expressed as(21)xk+1i=(Ai+Âi)xki+B(K̅ixki+gi)+Dwki=(Ai+Âi+BK̅i)xki+Dwki+Bgi.
The set of periodic stabilizing control gains (K̅i, i=1,…,m) for the whole formation flight system should not only satisfy the stability requirement of the linear periodic system, that is, all the eigenvalues of Ψ1 in (B.5) must lie inside unit circle, but also for each stabilizing gain K̅i, the eigenvalues of matrix (Ai+Âi+BK̅i) should lie inside unit circle. This stringent requirement is because of the model uncertainty Âi.
(ii) The constraint 2 comes from the constraint tightening technique [22] to provide the margin for robustness
(22)Xti=Xi~Rti,Uti=Ui~(K̅ixk+t∣ki+gi)Rti,
where Rti denotes the set of states of the ith UAV subsystem under the stabilizing control (B.3), which is reachable in t steps from the equilibrium point. Symbol ~ denotes the sets substraction operation.
(iii) It can be shown [23] that R∞i is the smallest invariant set and if the initial optimization problem is feasible, then the feasibility here will also be guaranteed.Combining this scheme with the stability results in Section3, the whole formation flight system is guaranteed to be stable.


### 4.1. Stability of the Formation Flight under MMPC

Proposition 1.
The multivehicle system with the Multiplexed Robust Decentralized MPC scheme is closed-loop stable.Proof.
According to [24], if the following four conditions hold:(1)
state constraints satisfy terminal constraint set;(2)
control constraints satisfy terminal constraint set;(3)
the terminal constraint set is positively invariant under a local controller;(4)
the terminal cost is a local Lyapunov function.
Then the closed-loop stability is obtained.In our setup, conditions (1)–(3) are satisfied by problem formulation. So we only have to check that condition (4) is satisfied, namely, that F(xk+N+1)-F(xk+N)≤0.According to [25], F(xk+N) as defined in (10) is the value function of the unconstrained infinite horizon periodic optimal control problem. As a consequence,(23)F(xk+N)=F(xk+N+1)+‖xk+N-xk+Nd‖Q2+‖uk+Nσ(k)‖R2.Hence(24)F(xk+N+1)-F(xk+N)=-‖xk+N-xk+Nd‖Q2-‖uk+Nσ(k)‖R2≤0
with equality satisfied if and only if  xk+N-xk+Nd=uk+Nσ(k)=0. This shows that F(xk+N) is a Lyapunov function in some neighborhood of the desired formation shape xd. It can be seen that this result, as well as its proof, depends only on using an appropriate terminal cost F(xk+N), and not at all on the details of the constrained optimization over the horizon of length N. There exist a number of techniques to calculate a suitable Lyapunov function to be used as a terminal cost function, for example, the LMI-based method. Consequently, the result and its proof hold for any other multiplexed scheme which involves constrained optimization over a finite horizon, providing that the terminal cost is properly defined. For example, for a 3-UAV fleet formation flight system, it may be beneficial to update the UAV subsystem in the sequence (1, 2, 1, 3, 1, 2, 1, 3,…), thus updating UAV1, that is, the leader twice as often as the other two UAVs. This update scheme will be very useful when the leader UAV has more capabilities (equipped with more sensors), and the leader can response faster to the GCS.The proof above depends on the feasibility of the constrained optimization at each step, and the feasibility is guaranteed by the constraints tightening (18), so if the initial states are feasible, then the following decentralized MPC for each UAV will be feasible and the whole formation group is thus stable.


## 4.1. Stability of the Formation Flight under MMPC

Proposition 1.
The multivehicle system with the Multiplexed Robust Decentralized MPC scheme is closed-loop stable.Proof.
According to [24], if the following four conditions hold:(1)
state constraints satisfy terminal constraint set;(2)
control constraints satisfy terminal constraint set;(3)
the terminal constraint set is positively invariant under a local controller;(4)
the terminal cost is a local Lyapunov function.
Then the closed-loop stability is obtained.In our setup, conditions (1)–(3) are satisfied by problem formulation. So we only have to check that condition (4) is satisfied, namely, that F(xk+N+1)-F(xk+N)≤0.According to [25], F(xk+N) as defined in (10) is the value function of the unconstrained infinite horizon periodic optimal control problem. As a consequence,(23)F(xk+N)=F(xk+N+1)+‖xk+N-xk+Nd‖Q2+‖uk+Nσ(k)‖R2.Hence(24)F(xk+N+1)-F(xk+N)=-‖xk+N-xk+Nd‖Q2-‖uk+Nσ(k)‖R2≤0
with equality satisfied if and only if  xk+N-xk+Nd=uk+Nσ(k)=0. This shows that F(xk+N) is a Lyapunov function in some neighborhood of the desired formation shape xd. It can be seen that this result, as well as its proof, depends only on using an appropriate terminal cost F(xk+N), and not at all on the details of the constrained optimization over the horizon of length N. There exist a number of techniques to calculate a suitable Lyapunov function to be used as a terminal cost function, for example, the LMI-based method. Consequently, the result and its proof hold for any other multiplexed scheme which involves constrained optimization over a finite horizon, providing that the terminal cost is properly defined. For example, for a 3-UAV fleet formation flight system, it may be beneficial to update the UAV subsystem in the sequence (1, 2, 1, 3, 1, 2, 1, 3,…), thus updating UAV1, that is, the leader twice as often as the other two UAVs. This update scheme will be very useful when the leader UAV has more capabilities (equipped with more sensors), and the leader can response faster to the GCS.The proof above depends on the feasibility of the constrained optimization at each step, and the feasibility is guaranteed by the constraints tightening (18), so if the initial states are feasible, then the following decentralized MPC for each UAV will be feasible and the whole formation group is thus stable.


## 5. Collision Avoidance Integration

Mixed Integer Linear Programming (MILP) combined with MPC [11, 13, 15, 17] has been applied in the collision avoidance of multiple agents’ coordination. But there are some drawbacks in this collision avoidance method. Firstly, the obstacle’s shape is normally modeled as rectangular in the problem formulation, while in the real-life environment, the shape of the obstacles can be of any form and formulating it as a box may waste the space that maybe useful for the vehicle to perform the necessary maneuver. Secondly, since the collision avoidance constraint is formulated based on the predicted future positions, in the situation where the obstacle’s width is smaller than the vehicle’s one step predictive travel distance (this can happen if the time step is long or the vehicle velocity is high), such algorithm may fail to prevent the collisions. Figure 4 illustrates such scenario, where the obstacle’s (red) width is the same as the vehicle’s longest travel distance in one predictive step, and since the MPC does not “see” the obstacle in its predicted positions, it leads the vehicles to the trajectories colliding the obstacle. To deal with this problem and the variety of the obstacle shapes, as well as to handle the nonapriorily known obstacles, the following scheme is proposed.Figure 4
UAVs penetrating the small size obstacle.Receding horizons can be divided into two families, that is, temporal and spatial ones. The temporal horizons manage the time frames, while the spatial horizons manage the physical areas. A temporal horizon, as called in most of the MPC literatures [26], is defined as the prediction horizon and unique for each vehicle (hi,i=1,2,…,n). It sets the temporal windows in the future over which the optimizer of the cost functions and the collision avoidance constraints is applied, starting from the present time. The temporal horizon can be effectively used to avoid the obstacles whose size is larger than one-step predictive distance.A number of spatial horizons can also be defined. One example can be the sensor detection horizon [27] that allows for detection of nearby small-size or pop-up obstacles.


### 5.1. Collision Avoidance of Large Apriorily Known Obstacles

In this work, a polytope is used to present various shapes of the obstacles. In order to ensure that every trajectory point of a UAV does not lie inside the boundaries of an obstacle, binary variables are introduced in the constraint formulation. For the hexagon shape obstacle for example, each edge can be regarded as a hyperplane which divides the fly zone into two half-spaces, one is safe while the other is not. In the collision avoidance constraint formulation, the predicted future point on the trajectory of each UAV is attached with an array of binary variables. In the case of a hexagon-shaped obstacle, an array of 6 binary variables is attached to each predicted point. The mathematic formulation is(25)bi=1iffx∈{x∣aiTx<bi},∑i=16bi≥1,
where b  is the binary variable array attached to the predicted position x. For each edge, a hyperplane {x∣aiTx=bi} can be defined, and {x∣aiTx<bi} denotes the safe half-space for the UAV. So the formulation above means that the predicted position point should lie in the safe half-space.The collision avoidance formulation here is an extension of that in [15], which takes care of the rectangular obstacles. Note that more edges correspond to more binary variables in the formulation, and hence more time will be needed in the optimization computation. If this is a concern, a reduced shape-representation can be used. For example, hexagon can be used to represent a round obstacle.The whole formation flight control optimization problem becomes nonconvex after adding the collision avoidance constraints. In order to solve the nonconvex optimization problem more efficiently, bounds on the variables are added explicitly(26)x̲≤x≤x̅,
where x̅ and x̲ denote the upper and lower bounds for the aircraft predicted position.


### 5.2. Collision Avoidance of Small or Nonapriorily Known Obstacles

In this part, the small obstacle’s shape is specified as a circle or cylinder (note that if the shape of the obstacle is not a circle or cylinder, the collision avoidance algorithm can still be applied after some modifications, which will not be detailed in this paper).In the proposed approach, position constraints are computed using the intersection points between the vehicle spatial horizonHi* and the obstacle. Suppose the obstacles detection sensor’s range Hi is larger than the distance that a vehicle can travel in one predictive step, so any unknown obstacle appearing in range Hi will be successfully detected as shown in Figure 5. A temporary and smaller vehicle spatial horizon Hi* can be set, based on the distance between the vehicle and the obstacle center and the intersection points with the obstacle. A position constraint can be added into the MPC formulation to deal with such scenario:
(27)norm⁡(x̂k+1-xk)≤Hi*,
where x̂k+1 and xk denote the next predicted position and current position, respectively.Figure 5
Collision avoidance with a small obstacle.Note that this position constraint actually prevents the next vehicle predictive position to be located outside ofHi*, which ensure that the UAV will not penetrate the small obstacle, and this constraint adds no difficulty to the optimization problem since it is a convex inequality constraints on the predicted states. Illustration of the whole collision avoidance control scheme is given in Figure 6.Figure 6
Collision avoidance flow chart.


## 5.1. Collision Avoidance of Large Apriorily Known Obstacles

In this work, a polytope is used to present various shapes of the obstacles. In order to ensure that every trajectory point of a UAV does not lie inside the boundaries of an obstacle, binary variables are introduced in the constraint formulation. For the hexagon shape obstacle for example, each edge can be regarded as a hyperplane which divides the fly zone into two half-spaces, one is safe while the other is not. In the collision avoidance constraint formulation, the predicted future point on the trajectory of each UAV is attached with an array of binary variables. In the case of a hexagon-shaped obstacle, an array of 6 binary variables is attached to each predicted point. The mathematic formulation is(25)bi=1iffx∈{x∣aiTx<bi},∑i=16bi≥1,
where b  is the binary variable array attached to the predicted position x. For each edge, a hyperplane {x∣aiTx=bi} can be defined, and {x∣aiTx<bi} denotes the safe half-space for the UAV. So the formulation above means that the predicted position point should lie in the safe half-space.The collision avoidance formulation here is an extension of that in [15], which takes care of the rectangular obstacles. Note that more edges correspond to more binary variables in the formulation, and hence more time will be needed in the optimization computation. If this is a concern, a reduced shape-representation can be used. For example, hexagon can be used to represent a round obstacle.The whole formation flight control optimization problem becomes nonconvex after adding the collision avoidance constraints. In order to solve the nonconvex optimization problem more efficiently, bounds on the variables are added explicitly(26)x̲≤x≤x̅,
where x̅ and x̲ denote the upper and lower bounds for the aircraft predicted position.


## 5.2. Collision Avoidance of Small or Nonapriorily Known Obstacles

In this part, the small obstacle’s shape is specified as a circle or cylinder (note that if the shape of the obstacle is not a circle or cylinder, the collision avoidance algorithm can still be applied after some modifications, which will not be detailed in this paper).In the proposed approach, position constraints are computed using the intersection points between the vehicle spatial horizonHi* and the obstacle. Suppose the obstacles detection sensor’s range Hi is larger than the distance that a vehicle can travel in one predictive step, so any unknown obstacle appearing in range Hi will be successfully detected as shown in Figure 5. A temporary and smaller vehicle spatial horizon Hi* can be set, based on the distance between the vehicle and the obstacle center and the intersection points with the obstacle. A position constraint can be added into the MPC formulation to deal with such scenario:
(27)norm⁡(x̂k+1-xk)≤Hi*,
where x̂k+1 and xk denote the next predicted position and current position, respectively.Figure 5
Collision avoidance with a small obstacle.Note that this position constraint actually prevents the next vehicle predictive position to be located outside ofHi*, which ensure that the UAV will not penetrate the small obstacle, and this constraint adds no difficulty to the optimization problem since it is a convex inequality constraints on the predicted states. Illustration of the whole collision avoidance control scheme is given in Figure 6.Figure 6
Collision avoidance flow chart.


## 6. Simulation Results

This section presents some simulation results of the multiplexed collision-free decentralized control scheme. The vehicle model used in the simulations is a small-size quadcopter with autopilot installed. Most of the simulations are done in a 2D space; however, it can be extended to a 3D space in a straightforward manner. The 2D representation is selected since it facilitates the visualization of the vehicle trajectories in a clearer manner. Throughout this section, the interconnection is described using a directed graph, which is determined by the two closest visible neighbors to each vehicle.It is important to note that the method proposed in this paper can easily accommodate any other particular UAV dynamics described by higher fidelity, heterogeneous, higher order, more complex linear, or piecewise-linear models. Within the multiplexed robust decentralized MPC framework, each vehicle is modeled as a low-order linear system that represents the controlled dynamics of the vehicle. The details of the simulation setup are elaborated below.(1)
The predictive horizonNu as Nu=3. Assume there is no delay in the simulations and the sample time is set as 0.3s. In the multiplexed robust decentralized scheme, the update interval between UAV subsystems is 0.1s.(2)
The following simulations are performed using multiplexed robust decentralized scheme, and the update cycle is sequential, that is, (1, 2, 3, 1, 2, 3,…) except that in Case 5, the update cycle is (1, 2, 1, 3, 1, 2, 1, 3,…).(3)
Terminal cost for (16) can be determined by associated LQ problem.(4)
Given the system dynamics and a “two-closest-neighbors” interconnection policy, each vehicle solves the decentralized optimization problem with the cost function defined in (16).(5)
The vehicles in the formation have identical dynamics.(6)
Linear constraints on single vehicle’ velocity is|v|≤[vxmaxvymaxvzmax]T=[555]Tm(7)
The nonconvex interconnection constraints (collision avoidance) are represented by(28)qi,j(xki,uki,xkj,ukj)=dsafe-‖yk,posi-yk,posj‖inf⁡≤0,dsafe=0.5m.
For pop-up constraint, the position constraint norm([x̂i,k+1ŷi,k+1]-[xi,kyi,k])≤Hi* is added.(8)
Network connectivity constraints:(29)qi,j(xki,uki,xkj,ukj)=‖yk,posi-yk,posj‖inf⁡-dcom≤0,dcom=10m.(9)
The weights in the cost function are chosen to beQf=I for all vehicles, where I denotes the identity matrix. Other weights are chosen as Ql=I and R=0.1ICase 1 (Formation Flight with Triangular-Shape Obstacle).
In Figure7, the leader (black), wingman1 (blue), and wingman2 (green) are initially aligned along the y-axis when the leader is commanded to fly to the target position (25,0) m with the two wingman maintaining the formation, that is, the commanded separation between each other is 1 m in the y-direction. Figure 7 shows that the formation is maintained and the fleet successfully avoids the triangular obstacle shown in red.Figure 7
Triangle obstacle collision avoidance.Case 2 (Formation Splitting).
The scenario in Figure8 is more complicated than the previous case. Initially, the leader (black), wingman1 (blue), wingman2 (green), wingman3 (cyan), and wingman4 (magenta) are aligned on the y axis at x=0. Firstly, the leader is commanded to fly to the target position at (15,0) m, and the formation is commanded to maintain the V shape as indicated by the red dotted line in the figure. Secondly, the 5-vehicle formation is commanded split into two formation groups, that is, group 1 (leader, wingman1, and wingman2) and group 2 (wingman3 and wingman4). The leader and wingman3 are commanded to fly to the targets (50,0) m and (50,-15) m, respectively. Figure 8 indicates that the vehicles can execute the commands while successfully maintaining safety. Notice that there is overshoot on the trajectories; this is mainly due to the short predictive horizons. If the computational power is enough, better performance can be attained by setting the predictive horizon longer.Figure 8
Formation split and obstacle avoidance.Case 3 (Comparison of Robust Decentralized Approach [with Constraint Tightening] with Decentralized Approach without Robustness Guarantee).
In this simulation, the leader (blue) is commanded to visit two target points (green circles), and the wingman is trying to maintain the formation (3 m from the leader iny direction). Both UAVs are subjected to disturbances ∥wki∥∞<1m which are randomly injected to the x and y position. The simulation results for formation flight with and without robustness considerations are shown in Figures 9(a) and 9(b)), respectively. As expected, without robustness consideration, the decentralized flight controller loses its feasibility, so it stops when it hits the first obstacle. On the contrary, by using the constraint tightening and stabilizing feedback controller, the robust decentralized formation flight controller can successfully fulfill the mission.Decentralized formation flight with robustness and without robustness. (a) Formation flight with robustness. (b) Formation flight without robustness.
(a)(b)Case 4 (Small-Obstacle Avoidance).
In this simulation, we show the benefit of the small-obstacle avoidance algorithm. In Figure10(a)), the predicted future point in one step lies 1.5 m ahead, which is larger than the diagonal length of the obstacle (1.13 m), so without imposing the position constraint presented in Section 5, the Leader “ignores” the obstacle and heading to the target position (green circle). While in Figure 10(b)), during the simulation, the position constraint has been added to the optimization, so as expected, the formation can successfully avoid the small obstacle and reach the target position with no collision.Small-obstacle-collision free formation flight. (a) Formation without position constraint. (b) Formation with position constraint.
(a)(b)Case 5 (Multiplexed Versus Nominal Decentralized Formation Flight Control).
In this simulation, the performances of the multiplexed decentralized formation flight control and the nominal decentralized formation flight control are compared and presented in Figure11. Initially the Leader is at position (0,0), the Wingman1 and Wingman2 are at (0,-3) m and (0,-6) m, respectively. At first, the Leader is commanded to visit target1 (the green circle) at (12,0) m, but at time 1.3 s, the Leader is commanded to fly to the target2 (the black circle). In the multiplexed decentralized scheme, the UAV subsystems are updated in the sequence (1, 2, 1, 3, 1, 2, 1, 3,…) which means that the Leader will response faster than the nominal decentralized scheme that updates the UAV subsystem when the whole input updates are available. The total simulation time in both cases are 3 seconds, as expected, and the formation response in Figure 11(b)) is faster than that in Figure 11(a)). This multiplexed decentralized scheme will be very beneficial when the agents in the formation are heterogeneous. Using a specified updating sequence, the human operator or the GCS can fully making use of the advantages of each UAV subsystem.Multiplexed decentralized versus nominal decentralized scheme. (a) Nominal decentralized formation. (b) Multiplexed decentralized formation.
(a)(b)Case 6 (Formation Flight in 3D).
This simulation is used to demonstrate that the scheme can be easily extended to 3D case as can be seen from Figure12. In the 3D collision avoidance constraint formulation, if the UAV has hovering ability, several options can be chosen, for example, the vehicle can avoid the obstacles either in the X-Y, X-Z, or Y-Z plane depending on the current situation and the group mission requirements.Figure 12
3D Formation Flight.


## 7. Concluding Remarks

In this paper, the MMPC approach is employed to systematically handle the multiple UAV formation flight control. In its implementation, careful consideration should be given to the following.(i)
If the number of UAV in the formation is large, updating one UAV’s inputs at each time step will be not acceptable since the update interval will be too long. In this case, update can be applied to a small group of UAVs at each time step to reduce the total update interval.(ii)
Since in the MMPC scheme, updating time of each UAV subsystem is fixed, an additional synchronization clock is required in the formation system, which may add complexities to the formation flight system.The 3D multiplexed decentralized vehicle formation flight control problem has been formulated using decomposition in a hierarchical fashion. Under the multiplexed decentralized scheme, the leader UAV can response faster to the GCS compared to the nominal decentralized scheme, that is, when the whole group update their inputs simultaneously. Moreover, the closed-loop stability of the whole formation flight system is always guaranteed even if a different updating sequence is used which makes the scheme flexible and the capability of each UAV can be fully exploited. The stable collision-free control and the robustness have been proposed under the unified MPC framework. By integrating the spacial horizon, the collision avoidance of the small pop-up obstacles is transformed into a convex position constraint in the MPC problem formulation. Simulation results demonstrate that the proposed framework is able to maintain the desired formation geometry within the constraints posted and guarantees collision-free flight.



---

*Source: 157590-2011-09-08.xml*