Robust Decentralized Formation Flight Control

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.

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 Scheme 1 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 𝑇 seconds by applying the Scheme 1. In the MMPC scheme, we propose to update only one UAV’s inputs at interval smaller than 𝑇. If the dynamics and optimization of each UAV is the same, then typically the update interval is 𝑇/𝑚, 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. Section 2 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 Section 3.

The overall formation flight control scheme is represented in Figure 1. 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 Assumption 2 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 are 𝑚 UAVs in the formation, so that at each time step 𝑇/𝑚, we update one UAV’s control inputs. After 𝑚 updates, a fresh cycle of updates begins, so that each whole cycle of updates repeats with a cycle time 𝑇. Figure 1 shows the scenario for 𝑚=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 Figure 2). 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 Appendix A. For more details, readers may refer to [19].

Under Assumption 1, consider a set of 𝑚 linear decoupled UAV dynamical systems, where the 𝑖th vehicle dynamics is described by the discrete-time time-invariant state equation 𝐱^𝑖_𝑘+1=𝐟^𝑖𝐱^𝑖_𝑘,𝐮^𝑖_𝑘=𝐴^𝑖+𝐴^𝑖𝐱^𝑖_𝑘+𝐵^𝑖𝐮^𝑖_𝑘+𝐷𝐰^𝑖_𝑘,𝐲^𝑖_𝑘=𝐡^𝑖𝐱^𝑖_𝑘,𝐮^𝑖_𝑘=𝐶^𝑖𝐱^𝑖_𝑘, (1) where the state update function 𝐟^𝑖∶𝑅⁶×𝑅³→𝑅⁶ and output map 𝐡^𝑖∶𝑅⁶→𝑅⁶ are linear or piecewise linear functions of its inputs. The states and inputs of the vehicle at time k are denoted by 𝐱^𝑖_𝑘∈𝑅⁶ and 𝐮^𝑖_𝑘∈𝑅³, respectively. In particular,𝐮^𝑖_𝑘=𝑁_cmd𝐸_cmdℎ_cmd,𝐲^𝑖_𝑘=𝑁𝐸ℎ𝑁̇𝐸̇ℎ̇=𝐲^𝑖_𝑘,pos𝐲^𝑖_𝑘,vel,𝐱^𝑖_𝑘=𝐱^𝑖_𝑘,pos𝐱^𝑖_𝑘,vel, (2) and 𝐱^𝑖_𝑘,pos∈𝑅³ is the vector of 𝑁,𝐸,ℎ (north-east-down) coordinates and 𝐱^𝑖_𝑘,vel∈𝑅³ denotes a vector of states corresponding to 𝑁-,𝐸-, and ℎ-axis velocity at time 𝑘. The subscript cmd means the current commanded position.

In general, (1) subjects to the disturbance input𝐰^𝑖_𝑘∈𝑊⊂𝑅^𝑞. (3)

Additionally, as indicated in (1), there may also be model uncertainty in the system, indicated by the uncertainty set 𝐴^𝑖=𝐴^𝑖∣𝐴^𝑖𝐴^𝑖=𝜀, (4) where the constant matrix 𝐴^𝑖 gives the average value of the state matrix, and 𝐴^𝑖 describes its statistical variation. For example, if we assume that the uncertainty in the state matrix is 10% of its nominal value, then 𝜀=‖𝐴^𝑖‖/‖𝐴^𝑖‖=0.1.

Let 𝐔^𝑖_𝑘∈𝑅^𝑚𝑖, 𝐗^𝑖∈𝑅^𝑛𝑖, and 𝐘^𝑖_𝑘∈𝑅^𝑝𝑖 denote the set of feasible inputs, states, and outputs of the 𝑖th vehicle 𝐮^𝑖_𝑘∈𝐔^𝑖,𝐱^𝑖∈𝐗^𝑖,𝐲^𝑖_𝑘∈𝐘^𝑖,𝑘≥0, (5) where 𝐘^𝑖_𝑘 and 𝐔^𝑖_𝑘 are given polytopes. In the case of UAV, (5) often indicates the constraints on the speed and acceleration of the vehicle as follows: 𝐲^𝑖_vel∈𝑌_𝑣=𝐳∈𝑅³∣𝐲^𝑖_vel≤𝑧_𝑖≤𝐲^𝑖_vel,𝐲̇^𝑖_vel∈𝑌_𝑎=𝐳∈𝑅³∣𝐲̇^𝑖_vel≤𝑧_𝑖≤𝐲̇^𝑖_vel. (6) The set of 𝑚 constrained systems are referred to as the multivehicle system. Let 𝐱_𝑘∈𝑅𝑛 with 𝑛=∑_𝑖𝑛^𝑖 and 𝐮_𝑘∈𝑅𝑚 with 𝑚=∑_𝑖𝑚^𝑖 be the vectors which collect the states and inputs of the multivehicle system at time 𝑘, that is, 𝐱_𝑘=[𝐱¹_𝑘;…;𝐱^𝑚_𝑘]^𝑇, 𝐮_𝑘=[𝐮¹_𝑘;…;𝐮^𝑚_𝑘]^𝑇, with 𝐱_𝑘+1=𝐟(𝐱_𝑘,𝐮_𝑘). Given the particular command (target, formation shape, etc.), the equilibrium pair of the 𝑖th vehicle is denoted by (𝐱^𝑖_𝑒,𝐮^𝑖_𝑒), that is, the 𝑖th vehicle is in the specified formation position with respect to the neighboring vehicle, while (𝐱_𝑒,𝐮_𝑒) denotes the corresponding equilibrium for the multivehicle system, which means the required formation states.

3.2. Formation Flight System under MMPC

Under Assumption 2, the whole formation flight system can be expressed as 𝐱_𝑘+1=𝐱¹_𝑘+1⋮𝐱^𝑚_𝑘+1=𝐴¹00⋮⋱⋮0⋯𝐴^𝑚𝐱¹_𝑘⋮𝐱^𝑚_𝑘+𝐵¹…0⋮⋱⋮0⋯𝐵^𝑚𝐮¹_𝑘⋮𝐮^𝑚_𝑘=𝐴𝐱_𝑘+𝐵𝐮_𝑘=𝐴𝐱_𝑘+^𝑚_𝑗=1𝐵_𝑗𝐮^𝑗_𝑘, (7) where 𝐵_𝑗 is the 𝑗th column of 𝐵.

As we will be referring to the expression (𝑘mod𝑚)+1 often in this section, it is convenient to introduce the indexing function 𝜎(𝑘)=(𝑘mod𝑚)+1. (8)

According to our UAV input update scheme, at each fixed time step only one of UAV’s inputs, 𝐮_𝑘^𝜎(𝑘), will be updated. Under such scheme, the centralized system can be represented as a periodic linear system with one UAV’s input: 𝐱_𝑘+1=𝐴𝐱_𝑘+𝐵_𝜎(𝑘)𝐮_𝑘^𝜎(𝑘). (9)

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: 𝐹𝐱_𝑘+𝑁=min_𝐮𝑘^{𝜎(𝑘)∞}_𝑖=𝑁𝐱_𝑘+𝑖+1−𝐱^𝑑_𝑘+𝑖+1²_𝑄+𝐮^𝜎(𝑘)_𝑘+𝑖²_𝑅∣𝐱_𝑘+1=𝐴𝐱_𝑘+𝐵_𝜎(𝑘)𝐮_𝑘^𝜎(𝑘)=𝐱^𝑇_𝑘+𝑁𝑃_{𝜎(𝑘+𝑁)}𝐱_𝑘+𝑁+2𝑞^𝑇_{𝜎()𝑘+𝑁}𝐱_𝑘+𝑁+𝑟_{𝜎(𝑘+𝑁)}, (10) where 𝑃_{𝜎(𝑘+𝑁)} can be calculated according to (B.2) in Appendix B.(ii)Terminal states constraints: 𝐱_𝑘+𝑁+1∈𝑋_𝐼𝐾_𝜎(𝑘), (11) where 𝑋_𝐼(𝐾_𝜎(𝑘)) 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. 𝐾_𝜎(𝑘) 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 Appendix B).

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, 𝐱_𝑘, 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 𝑖th 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 𝐺 consists of a set of vertices, denoted 𝑉, and a set of edges 𝐴⊂𝑉², where 𝑎=(𝛼,𝛽)∈𝐴 and 𝛼,𝛽∈𝑉. The first element of 𝑎 is denoted tail(𝑎), and the second is denoted head(𝑎). We assume that tail(𝑎)≠head(𝑎) for all, a meaning that the graph has no self-loops. We also assume that each element of 𝐴 is unique. A graph with the property that for any (𝛼,𝛽)∈𝐴, the arc (𝛽,𝛼)∈𝐴 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 𝑖th vehicle system is associated with the 𝑖th node of the graph. If an edge (𝑖,𝑗)∈𝐴 connecting the 𝑖th and 𝑗th node is present, then the cost and the constraints of the optimal control problem will have a component which is a function of both 𝐱^𝑖 and 𝐱^𝑗. Before defining the optimal control problem, we define a graph (which can be time-varying) 𝐺(𝑡)=(𝑉,𝐴(𝑡)) associated with the vehicle formation group.

Using the graph structure defined previously, the optimization problem can be formulated by letting 𝑞^𝑖,𝑗(𝐱^𝑖,𝐱^𝑗) be the interconnection function between the 𝑖th and 𝑗th UAV for (𝑖,𝑗)∈𝐴 and 𝑞^𝑖,𝑗𝐱^𝑖,𝐱^𝑗≤0 (12) define the interconnection constraints between the 𝑖th and the 𝑗th vehicle, with 𝑞^𝑖,𝑗∶𝑅^𝑛𝑖×𝑅^𝑛𝑗→𝑅.

Remark 3. Depending on the group tasks, the interconnection function 𝑞^𝑖,𝑗(𝐱^𝑖,𝐱^𝑗) can have several variant forms.(i)For interagent collision avoidance, this constraints define nonconvex requirements in the following way: 𝑞^𝑖,𝑗𝐱^𝑖_𝑘,𝐮^𝑖_𝑘,𝐱^𝑗_𝑘,𝐮^𝑗_𝑘=𝑑_safe−𝐲^𝑖_𝑘,pos−𝐲^𝑗_𝑘,pos𝑝≤0, (13) where the parameter 𝑑_safe in (13) represents a lower bound on the norm of relative position between neighboring vehicles. The subscript 𝑝 denotes the type of norm which will be used in the optimization problem. 𝑝=2 leads to a circular protection zones around vehicle and the optimization problem becomes quadratic. If 𝑝=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: 𝑞^𝑖,𝑗𝐱^𝑖_𝑘,𝐮^𝑖_𝑘,𝐱^𝑗_𝑘,𝐮^𝑗_𝑘=𝐲^𝑖_𝑘,pos−𝐲^𝑗_𝑘,pos𝑝−𝑟_com≤0, (14) where 𝑟_com 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 Figure 3).

Figure 3 

Agent in the center of the cluster exchange information with most 6 neighbors.

Consider the following cost function for the 𝑖th UAV subsystem at time step 𝑘: 𝐽^𝑖_𝑘=^{𝑁−1𝑡=0}𝑄_𝑓𝐲^𝑖_{𝑘+𝑡∣𝑘}−𝐲^𝑖_𝑓+𝑅𝐮^𝑖_{𝑘+𝑡∣𝑘}−𝐮^𝑖_𝑓+𝐹𝐱_𝑘+𝑁,𝑖=1,_{𝑁−1𝑡=0}𝑄_𝑙𝐲^𝑖_{𝑘+𝑡∣𝑘}−𝐲^𝑗_{𝑘+𝑡∣𝑘}−𝐋+𝑅𝐮^𝑖_{𝑘+𝑡∣𝑘}−𝐮^𝑖_𝑓+𝐹𝐱_𝑘+𝑁,𝑖≠1,𝑖,𝑗∈𝐴(𝑘). (15)

The leader in the formation fight group is denoted as the first UAV, that is, 𝑖=1. ‖𝑄_𝑓(𝐲^𝑖_{𝑘+𝑡∣𝑘}−𝐲^𝑖_𝑓)‖ represents the cost term driving the leader to the target position, the terminal cost term 𝐹(𝐱_𝑘+𝑁) comes from Section 2, which makes the decentralized MPC controller similar to the infinite horizon formulation. For the wingmen’s cost function, that is, 𝑖>1, the formation shape is maintained by the cost term ‖𝑄_𝑙(𝐲^𝑖_{𝑘+𝑡∣𝑘}−𝐲^𝑗_{𝑘+𝑡∣𝑘}−𝐋)‖ and 𝐋 is the separation vector between neighboring UAVs.

This cost term can also be used in formation reconfiguration purposes.

Given a certain graph interconnection structure 𝐺(𝑡), for each UAV, the associated robust decentralized MPC controller (RDMPC) can be presented as 𝐮^𝑖_𝑘=argmin𝐽^𝑖_𝑘 (16) subject to several constraints below:(1)system dynamics (without disturbance and model uncertainty).𝐱^𝑖_{𝑘+𝑡+1∣𝑘}=𝐴^𝑖𝐱^𝑖_{𝑘+𝑡∣𝑘}+𝐵^𝑖𝐮^𝑖_{𝑘+𝑡∣𝑘}𝐲,𝑡≥0,_{𝑖𝑘+𝑡∣𝑘}=𝐶^𝑖𝐱^𝑖_{𝑘+𝑡∣𝑘},𝐱^𝑖_𝑘∣𝑘=𝐱^𝑖_𝑘. (17)(2)State and control constraints𝐱^𝑖_{𝑘+𝑡∣𝑘}∈𝐗^𝑖_𝑡,𝐮^𝑖_{𝑘+𝑡∣𝑘}∈𝐔^𝑖_𝑡,𝑡=0,1,…,𝑁. (18)(3)Stabilizing controller after the prediction horizon𝐮^𝑖_{𝑘+𝑡∣𝑘}=𝐾_𝑖𝐱^𝑖_{𝑘+𝑡∣𝑘}+𝑔_𝑖,𝑡≥𝑁. (19)(4)Interconnection constraints for 𝑖th UAV and its neighboring UAV, that is, (𝑖,𝑗)∈𝐴(𝑘)𝑞^𝑖,𝑗𝐱^𝑖_{𝑘+𝑡+1∣𝑘},𝐮^𝑖_{𝑘+𝑡+1∣𝑘},𝐱^𝑗_{𝑘+𝑡+1∣𝑘},𝐮^𝑗_{𝑘+𝑡+1∣𝑘}(𝑘),≤0,𝑡≥0,𝑖,𝑗∈𝐴 (20) where the control sequence is given by 𝐔^𝑖_𝑘=[𝐮^𝑖_𝑘∣𝑘;𝐮^𝑖_{𝑘+1∣𝑘};…;𝐮^𝑖_{𝑘+𝑁−1∣𝑘}]^𝑇, and the double indices (𝑘+𝑗∣𝑘) denote the prediction at time 𝑘 of a value at time 𝑘+𝑗.

Remark 4. (i) For the stabilizing controller around the equilibrium point, the close-loop system equation can be expressed as 𝐱^𝑖_𝑘+1=𝐴^𝑖+𝐴^𝑖𝐱^𝑖_𝑘+𝐵𝐾_𝑖𝐱^𝑖_𝑘+𝑔_𝑖+𝐷𝐰^𝑖_𝑘=𝐴^𝑖+𝐴^𝑖+𝐵𝐾_𝑖𝐱^𝑖_𝑘+𝐷𝐰^𝑖_𝑘+𝐵𝑔_𝑖. (21)
The set of periodic stabilizing control gains (𝐾_𝑖, 𝑖=1,…,𝑚) for the whole formation flight system should not only satisfy the stability requirement of the linear periodic system, that is, all the eigenvalues of Ψ₁ in (B.5) must lie inside unit circle, but also for each stabilizing gain 𝐾_𝑖, the eigenvalues of matrix (𝐴^𝑖+𝐴^𝑖+𝐵𝐾_𝑖) should lie inside unit circle. This stringent requirement is because of the model uncertainty 𝐴^𝑖.
(ii) The constraint 2 comes from the constraint tightening technique [22] to provide the margin for robustness 𝐗^𝑖_𝑡=𝐗^𝑖∼𝐑^𝑖_𝑡,𝐔^𝑖_𝑡=𝐔^𝑖∼𝐾_𝑖𝐱^𝑖_{𝑘+𝑡∣𝑘}+𝑔_𝑖𝐑^𝑖_𝑡, (22) where 𝐑^𝑖_𝑡 denotes the set of states of the 𝑖th UAV subsystem under the stabilizing control (B.3), which is reachable in 𝑡 steps from the equilibrium point. Symbol ~ denotes the sets substraction operation.
(iii) It can be shown [23] that 𝐑^𝑖_∞ 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 Section 3, 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 𝐹(𝐱_𝑘+𝑁+1)−𝐹(𝐱_𝑘+𝑁)≤0.

According to [25], 𝐹(𝐱_𝑘+𝑁) as defined in (10) is the value function of the unconstrained infinite horizon periodic optimal control problem. As a consequence,𝐹𝐱_𝑘+𝑁=𝐹𝐱_𝑘+𝑁+1+𝐱_𝑘+𝑁−𝐱^𝑑_𝑘+𝑁²_𝑄+𝐮^𝜎(𝑘)_𝑘+𝑁²_𝑅. (23)

Hence𝐹𝐱_𝑘+𝑁+1−𝐹𝐱_𝑘+𝑁=−𝐱_𝑘+𝑁−𝐱^𝑑_𝑘+𝑁²_𝑄−𝐮^𝜎(𝑘)_𝑘+𝑁²_𝑅≤0 (24) with equality satisfied if and only if 𝐱_𝑘+𝑁−𝐱^𝑑_𝑘+𝑁=𝐮^𝜎(𝑘)_𝑘+𝑁=0. This shows that 𝐹(𝐱_𝑘+𝑁) is a Lyapunov function in some neighborhood of the desired formation shape 𝐱^𝑑. It can be seen that this result, as well as its proof, depends only on using an appropriate terminal cost 𝐹(𝐱_𝑘+𝑁), and not at all on the details of the constrained optimization over the horizon of length 𝑁. 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 (ℎ_𝑖,𝑖=1,2,…,𝑛). 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𝑏_𝑖=1iff𝑥∈𝑥∣𝑎^𝑇_𝑖𝑥<𝑏_𝑖,_6𝑖=1𝑏_𝑖≥1, (25) where 𝑏 is the binary variable array attached to the predicted position 𝑥. For each edge, a hyperplane {𝑥∣𝑎^𝑇_𝑖𝑥=𝑏_𝑖} can be defined, and {𝑥∣𝑎^𝑇_𝑖𝑥<𝑏_𝑖} 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) where 𝐱 and 𝐱 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 horizon 𝐻_𝑖^∗ and the obstacle. Suppose the obstacles detection sensor’s range 𝐻_𝑖 is larger than the distance that a vehicle can travel in one predictive step, so any unknown obstacle appearing in range 𝐻_𝑖 will be successfully detected as shown in Figure 5. A temporary and smaller vehicle spatial horizon 𝐻_𝑖^∗ 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: norm𝑥_𝑘+1−𝑥_𝑘≤𝐻_𝑖^∗, (27) where 𝑥_𝑘+1 and 𝑥_𝑘 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 of 𝐻_𝑖^∗, 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 horizon 𝑁_𝑢 as 𝑁_𝑢=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 |𝑣|≤[𝐯_𝑥max𝐯_𝑦max𝐯_𝑧max]^𝑇=[555]^𝑇m(7)The nonconvex interconnection constraints (collision avoidance) are represented by 𝑞^𝑖,𝑗𝐱^𝑖_𝑘,𝐮^𝑖_𝑘,𝐱^𝑗_𝑘,𝐮^𝑗_𝑘=𝑑_safe−𝐲^𝑖_𝑘,pos−𝐲^𝑗_𝑘,posinf𝑑≤0,_safe=0.5m. (28) For pop-up constraint, the position constraint 𝑥norm(𝑦^𝑖,𝑘+1_𝑖,𝑘+1−^{𝑥𝑖,𝑘}_{𝑦𝑖,𝑘})≤𝐻_𝑖^∗ is added.(8)Network connectivity constraints: 𝑞^𝑖,𝑗𝐱^𝑖_𝑘,𝐮^𝑖_𝑘,𝐱^𝑗_𝑘,𝐮^𝑗_𝑘=𝐲^𝑖_𝑘,pos−𝐲^𝑗_𝑘,posinf−𝑑_com𝑑≤0,_com=10m. (29)(9)The weights in the cost function are chosen to be 𝑄_𝑓=𝐼 for all vehicles, where 𝐼 denotes the identity matrix. Other weights are chosen as 𝑄_𝑙=𝐼 and 𝑅=0.1𝐼

Case 1 (Formation Flight with Triangular-Shape Obstacle). In Figure 7, the leader (black), wingman1 (blue), and wingman2 (green) are initially aligned along the 𝑦-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 𝑦-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 Figure 8 is more complicated than the previous case. Initially, the leader (black), wingman1 (blue), wingman2 (green), wingman3 (cyan), and wingman4 (magenta) are aligned on the 𝑦 axis at 𝑥=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 in 𝑦 direction). Both UAVs are subjected to disturbances ‖𝐰^𝑖_𝑘‖_∞<1m which are randomly injected to the 𝑥 and 𝑦 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.

(a)

(b)

Figure 9 

Decentralized formation flight with robustness and without robustness. (a) Formation flight with robustness. (b) Formation flight without robustness.

Case 4 (Small-Obstacle Avoidance). In this simulation, we show the benefit of the small-obstacle avoidance algorithm. In Figure 10(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.

(a)

(b)

Figure 10 

Small-obstacle-collision free formation flight. (a) Formation without position constraint. (b) Formation with position constraint.

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 Figure 11. 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.

(a)

(b)

Figure 11 

Multiplexed decentralized versus nominal decentralized scheme. (a) Nominal decentralized formation. (b) Multiplexed decentralized formation.

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 Figure 12. 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 𝑋-𝑌, 𝑋-𝑍, or 𝑌-𝑍 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.

Appendices

A. Definitions for Polyhedron and Polytope

Definition 1 (polyhedron). A convex set 𝑄∈𝑅^𝑛 given as an intersection of a finite number of closed half-spaces 𝑄=𝐱∈𝑅^𝑛∣𝑄^𝑥𝐱≤𝑄^𝑐,𝑄^𝑥∈𝑅^𝑛×𝑛,𝑄^𝑐∈𝑅^𝑛 (A.1) is called polyhedron.

Definition 2 (polytope). A bounded polyhedron 𝑃∈𝑅^𝑛, 𝑃=𝐱∈𝑅^𝑛∣𝑃^𝑥𝐱≤𝑃^𝑐,𝑃^𝑥∈𝑅^𝑛×𝑛,𝑃^𝑐∈𝑅^𝑛, (A.2) is called polytope.
One of the fundamental properties of polytope is that it can also be described by its vertices 𝑃=𝐱∈𝑅^𝑛∣𝐱=^𝑣𝑝_𝑖=1𝛼_𝑖𝑉_𝑝^(𝑖),0≤𝛼_𝑖≤1,^𝑣𝑝_𝑖=1𝛼_𝑖=1, (A.3) where 𝑉_𝑝^(𝑖) denotes the 𝑖th vertex of 𝑃 and 𝑣𝑝 is the total number of vertices of 𝑃. Therefore, in the constraints or obstacles representations, we can either use the half-space form or vertex form to denote the polytopes.

B. Linear Quadratic Control Theory of Periodic System

Consider the system (9) and the quadratic cost function𝐽_𝑘=^∞_𝑖=0𝐱_𝑘+𝑖+1−𝐱^𝑑_𝑘+𝑖+1²_𝑄+𝐮^𝜎(𝑘)_𝑘+𝑖²_𝑅, (B.1) where 𝐱^𝑑_𝑘+𝑖+1 is the desired trajectory which can include information of formation shape or the target position for the leader UAV. 𝑄=𝑄^T≥0,𝑅=𝑅^𝑇>0 are given state cost and input cost matrices.

Using dynamic programming technique [28], the cost function is minimized by the cost-to-go function 𝑉_𝑘(𝑧) (which gives the minimum LQ cost-to-go starting from state z at time k): 𝑉_𝑘(𝑧)=𝑧^𝑇𝑃_𝑘𝑧+2𝑞^𝑇_𝑘𝑧+𝑟_𝑘, (B.2) where 𝑃_𝑖, 𝑞_𝑖, and 𝑟_𝑖 (𝑖=1,…,𝑚) are the solutions of the following discrete-time periodic Riccati-like equations: 𝑃_𝑘=𝑄+𝐴^𝑇𝑃_𝑘+1𝐴−𝐴^𝑇𝑃_𝑘+1𝐵_𝜎(𝑘)𝑅+𝐵^𝑇_𝜎(𝑘)𝑃_𝑘+1𝐵_𝜎(𝑘)⁻¹𝐵^𝑇_𝜎(𝑘)𝑃_𝑘+1𝑞𝐴,_𝑘=𝐴+𝐵_𝜎(𝑘)𝐾_𝜎(𝑘)𝑞_𝑘+1−𝑄𝐱^𝑑_𝑘,𝑟_𝑘=𝑟_𝑘+1+𝐱^𝑑_𝑘𝑄𝐱^𝑑_𝑘+𝑞^𝑇_𝑘+1𝐵_𝜎(𝑘)𝑔_𝜎(𝑘), (B.3) by setting𝐮^{𝜎(𝑘)∗}_𝑘=𝐾_𝜎(𝑘)𝐱_𝑘+𝑔_𝜎(𝑘) (B.4) with𝐾_𝜎(𝑘)=−𝑅+𝐵^𝑇_𝜎(𝑘)𝑃_𝜎(𝑘+1)𝐵_𝜎(𝑘)⁻¹𝐵^𝑇_𝜎(𝑘)𝑃_𝜎(𝑘+1)𝑔𝐴,_𝜎(𝑘)=−𝑅+𝐵^𝑇_𝜎(𝑘)𝑃_𝜎(𝑘+1)𝐵_𝜎(𝑘)⁻¹𝐵^𝑇_𝜎(𝑘)𝑞_𝑘+1. (B.5)

Remark 5. The set of periodic control gains {𝐾_𝑗∶𝑗=1,…,𝑚} is stabilizing, that is, the monodromy matrix Ψ₁=Φ_𝑚Φ_𝑚−1⋯Φ₂Φ₁ (B.6) has all its eigenvalues inside the unit circle, where Φ_𝑗=𝐴−𝐵_𝑗𝐾_𝑗. (B.7)

It is a standard fact that this is a necessary and sufficient condition for closed-loop stability of a linear periodic system, and that the eigenvalues of the monodromy matrix are invariant under cyclic permutations of the Φ_𝑗 matrices.