Decentralized Hybrid Model PredictiveControl of a Formation of Unmanned

Aerial Vehicles

A. Bemporad ∗ C. Rocchi ∗

∗Department of Mechanical and Structural Engineering, University ofTrento, Italy (email: {bemporad,claudio.rocchi}@ing.unitn.it)

Abstract: This paper proposes a hierarchical MPC strategy for autonomous navigation of aformation of unmanned aerial vehicles (UAVs) of quadcopter type under obstacle and collisionavoidance constraints. Each vehicle is stabilized by a lower-level local linear MPC controlleraround a desired position, that is generated, at a slower sampling rate, by a hybrid MPCcontroller per vehicle. Such an upper control layer is based on a hybrid dynamical model of theUAV in closed-loop with its linear MPC controller and of its surrounding environment (i.e., theother UAVs and obstacles). The resulting decentralized scheme controls the formation basedon a leader-follower approach. The performance of the hierarchical control scheme is assessedthrough simulations and comparisons with other path planning strategies, showing the ability oflinear MPC to handle the strong couplings among the dynamical variables of each quadcopterunder motor voltage and angle/position constraints, and the flexibility of the decentralizedhybrid MPC scheme in planning the desired paths on-line.

Keywords: Model predictive control, hierarchical control, aerospace, decentralized control,hybrid systems, obstacle avoidance, unmanned aerial vehicles

1. INTRODUCTION

The last few years have been characterized by an in-creasing interest in stabilizing and maneuvering a for-mation of multiple vehicles. Research areas include bothmilitary and civilian applications (such as intelligence,reconnaissance, surveillance, exploration of dangerous en-vironments) where Unmanned Aerial Vehicles (UAVs) canreplace humans. There are different types of UAVs: planesare suitable for long-range applications because are energyefficient, but they need wide operating spaces; rotorcrafts,such as helicopters and quadcopters, have less autonomybut can operate in limited workspaces, they can take offand land vertically and easily hover above targets. Never-theless, VTOL (Vertical Take-Off and Landing) UAVs aremore complex to control [Altug et al., 2005], because ofhighly nonlinear and coupled dynamics, and to limitationson actuators and pitch/roll angles. In particular, quad-copters are a class of VTOL vehicles for whose stabilizationseveral approaches were proposed in literature: classicalPID [Hoffmann et al., 2008], nonlinear control [Castilloet al., 2004], LQR [Kivrak, 2006], visual feedback [Altuget al., 2005], H∞ control [Chen and Huzmezan, 2003, Raffoet al., 2010], and recently linear MPC (Model PredictiveControl) [Bemporad et al., 2009].

MPC is particularly suitable for control of multivariablesystems governed by constrained dynamics, as it allowsoperating closer to the boundaries imposed by hard con-straints; in the context of UAVs, MPC techniques have

? This work was partially supported by the European Commissionunder project “WIDE - Decentralized and Wireless Control of Large-Scale Systems”, contract number FP7-IST-224168.

been already applied for control of formation flight in Dun-bar [2001], Dunbar and Murray [2002], Borrelli et al.[2005b], Li and Cassandras [2006], Richards and How[2004], Manikonda et al. [1999].

In the context of path planning for obstacle avoidance,several other solutions have been proposed in the litera-ture, such as potential fields [Chuang, 1998, Paul et al.,2008], A∗ with visibility graphs [Hoffmann et al., 2008,Latombe, 1991], nonlinear trajectory generation (see e.g.the NTG software package developed at Caltech [Dunbarand Murray, 2002]), and mixed-integer linear program-ming (MILP) [Richards and How, 2002, Borrelli et al.,2005a]. In particular the latter has shown the great flexibil-ity of on-line mixed-integer optimization in real-time tra-jectory planning of aircrafts, as also reported in Pallottinoet al. [2002] where on-line MILP techniques were provedvery effective in handling multiaircraft conflict avoidanceproblems.

This paper extends the two-layer MPC approach to quad-copter stabilization and on-line trajectory generation forautonomous navigation with obstacle avoidance presentedearlier in Bemporad et al. [2009]. A linear constrainedMPC controller with integral action takes care of quad-copter stabilization and offset-free tracking of desired set-points. At a higher hierarchical level and lower samplingrate, a hybrid MPC controller generates on line the pathto follow to reach a given target position/orientation whileavoiding obstacles. The approach is extended in two ways:first, we let the underlying linear MPC algorithm control-ling directly motor voltages, rather than torques; second,the higher-level hybrid MPC controllers are organized in

linear MPC

linear MPC

linear MPC

hybrid MPC

hybrid MPC

Ts=71 ms

Thyb=1.5 s

continuoustime

measurements

voltages

measurements

desired positions

measure-ments

hybrid MPC

hybrid MPCmeasure-

ments

(centralized or decentralized)navigation

stabilization

dynamics

Fig. 1. Hierarchical control structure for UAV navigation

a decentralized control scheme, based on a leader-followerapproach. We assume that target and obstacle positionsmay be time-varying, and in this case that only their cur-rent coordinates are known (not the future ones), so thatoff-line (optimal) planning cannot be easily accomplished.

The paper is organized as follows. In Section 2 we describethe layers of the hierachical structure of each controlledquadcopter and its nonlinear dynamics, whose lineariza-tion provides the prediction model for linear MPC de-sign for stabilization under constraints and tracking ofgenerated trajectories. The higher-level hybrid MPC con-troller for path planning and obstacle avoidance is alsointroduced. Section 3 describes the potential fields methoddescribed in Paul et al. [2008] and results are comparedin simulation in Section 4. Finally, some conclusions aredrawn in Section 5.

2. HIERARCHICAL MPC OF EACH UAV

Consider the hierarchical control system depicted in Fig-ure 1. At the top layer a hybrid MPC controller generateson-line the desired positions (xd, yd, zd), in order to ac-complish the main mission, namely reach a given targetposition (xt, yt, zt) while avoiding collisions with obstaclesand other UAVs. Such references (xd, yd, zd) are tracked inreal-time by a linear MPC controller placed at a lower layerof the architecture. The bottom layer depicted in Figure 1is the physical layer representing the nonlinear dynamicsof the quadcopters. In the next subsections we describe indetails each layer of the architecture.

2.1 Nonlinear quadcopter dynamics

A quadcopter aerial vehicle is an underactuated mechan-ical system with six degrees of freedom and only fourcontrol inputs (see Figure 2). We denote by x, y, z theposition of the vehicle and by θ, φ, ψ its rotations aroundthe Cartesian axes, relative to the “world” frame I. Inparticular, x and y are the coordinates in the horizontalplane, z is the vertical position, ψ is the yaw angle (ro-tation around the z-axis), θ is the pitch angle (rotationaround the x-axis), and φ is the roll angle (rotation aroundthe y-axis). The dynamical model adopted in this paper ismainly based on the model proposed in Bresciani [2008],simplified to reduce the computational complexity and toease control design. As described in Figure 2, each of the

Fig. 2. Quadcopter model

four motors M1, M2, M3, M4 generate four thrust forcesf1, f2, f3, f4, and four torques τ1, τ2, τ3, τ4, respectively,which are adjusted by manipulating motor voltages. Weuse the linear equations obtained in Kivrak [2006]

fi = 9.81(22.5VMi − 9.7)/1000, i = 1, . . . , 4

approximated to consider all motors identical, to expressthe relationships between motors thrustes and their volt-ages. A total force f and three torques τθ, τφ, τψ aroundtheir corresponding axes are generated

f = f1 + f2 + f3 + f4 (1a)

τθ = (f2 − f4)l (1b)

τφ = (f3 − f1)l (1c)

τψ =

4∑i=1

τi (1d)

where l is the distance between each motor and the centerof gravity of the vehicle, that allow changing the positionand orientation coordinates of the quadcopter freely in thethree-dimensional space. The dynamics are described by

x= (−f sin θ − βx)1

m(2a)

y = (f cos θ sinφ− βy)1

m(2b)

z =−g + (f cos θ cosφ− βz) 1

m(2c)

θ=τθIxx

(2d)

φ=τφIyy

(2e)

ψ =l

Izz(−f1 + f2 − f3 + f4) (2f)

where the damping factor β takes into account realisticfriction effects, m denotes the mass of the vehicle, and Ixx,Iyy, Izz are the moments of inertia around the body frameaxes x, y, z, respectively. The nonlinear dynamical modelhas twelve states (six positions and six velocities) and fourinputs (the motors voltages VMi), largely coupled throughthe nonlinear relations in (2). The parameters used in thispaper are reported in Table 1.

Table 1. Parameters of quadcopter model

m [kg] l [m] β [Ns/m] Ixx [Nms2] Iyy [Nms2] Izz [Nms2]

0.408 0.136 0.2 0.0047 0.0047 0.0089

2.2 Linear MPC for stabilization

We design a linear MPC controller to stabilize the quad-copter vehicle around a desired position in space andattitude. In particular, the desired position (xd, yd, zd) willbe commanded by the upper-layer hybrid MPC controllerdescribed below in Section 2.3 for collision avoidance.

We first linearize the nonlinear dynamical model (2)around an equilibrium condition of hovering. The resultinglinear continuous-time state-space system is converted todiscrete-time with sampling time Ts{

ξL(k + 1) = AξL(k) +BuL(k)yL(k) = ξL(k)

(3)

where ξL(k) = [θ, φ, ψ, x, y, z, θ, φ, ψ, x, y, z, zI]′ ∈ R13 is

the state vector, uL(k) = [vm1, vm2, vm3, vm4]′ ∈ R4 isthe input vector, yL(k) ∈ R13 is the output vector (thatwe assume completely measured or estimated), and A, B,C, D are matrices of suitable dimensions obtained by thelinearization process. The additional state zI =

∫(z − zd)

is included to provide integral action on the altitude z,so that offset-free tracking of the desired setpoint zd isguaranteed in steady-state. The integral action is mainlydue to counteract effect of the gravity force acting againstthe force developed by the collective input f .

The linear MPC formulation of the Model PredictiveControl Toolbox for MATLAB [Bemporad et al., 2004b]based on quadratic programming is used to design thecontroller.

2.3 Hybrid MPC for collision avoidance

The proposed approach consists of constructing an ab-stract hybrid model of the controlled aerial vehicle andof the surrounding obstacles, and then use a hybrid MPCstrategy for on-line generation of the desired position(xd, yd, zd). The closed-loop dynamics composed by thequadcopter and the linear MPC controller can be veryroughly approximated by the discrete-time linear dynam-ical system{

x(k + 1) = α1xx(k) + β1x(xd(k − 1) + ∆xd(k))y(k + 1) = α1yy(k) + β1y(yd(k − 1) + ∆yd(k))z(k + 1) = α1zz(k) + β1z(zd(k − 1) + ∆zd(k))

(4)

where ∆ ∗d (k) = ∗d(k) − ∗d(k − 1) is the input ofthe system representing the increment of desired (·)-coordinate commanded at time kThyb, Thyb > Ts isthe sampling time of the hybrid MPC controller, and(x(k), y(k), z(k) is the output of the system, representingthe actual position of the quadcopter. The parameters α1∗and β1∗ in (4) are estimated by simulating the linear MPCcontroller designed in Section 2.2. The input increments∆xd(k), ∆yd(k), ∆zd(k) are upper and lower bounded bya quantity ∆,

−∆[

111

]≤[

∆xd(k)∆yd(k)∆zd(k)

]≤ ∆

[111

](5)

which is a tuning knob of the hybrid MPC controller, asit allows one to directly control the speed of maneuver of

the quadcopter by imposing constraints on the reference

derivatives

∥∥∥∥[ xd(t)yd(t)zd(t)

]∥∥∥∥∞≤ ∆ · Thyb.

Obstacles are modeled as polyhedral sets. To minimizethe complexity of the overall hybrid prediction modeldescribed in this section, the ith obstacle, i = 1, . . . ,M , ismodeled as the tetrahedron

Aobski

[x(k)−xi(k)y(k)−yi(k)z(k)−zi(k)

]≤ Bobs (6)

where Aobs

[xyz

]≤ Bobs is a fixed hyperplane repre-

sentation of a reference tetrahedron, ki is a fixed scal-

ing factor, and

[xi(k)yi(k)zi(k)

]is a reference point of the ob-

stacle. In this paper we choose Aobs, Bobs such thatthe corresponding polyhedron is the convex hull of vec-

tors[

000

],[

5/200

],[

05/20

],

[5/65/65/2

], which makes the reference

point[xiyizi

]its vertex with smallest coordinates.

Equation (6) can be rewritten as

Aobski

[x(k)y(k)z(k)

]≤ Cobs(k) (7)

where Cobs(k) = Bobs +Aobski

[xi(k)yi(k)zi(k)

], Cobs(k) ∈ R4, is a

quantity that may vary in real-time. Although we modelhere the predicted evolution of Cobs as

Cobs(k + h+ 1) = Cobs(k + h) (8)

non-constant dynamics might be used as well if estimatesof obstacle velocities and/or accelerations are available.

Finally, to represent obstacle avoidance constraints, definethe following binary variables δij ∈ {0, 1}, i = 1, . . . ,M ,j = 1, . . . , 4

[δij(k) = 1]↔ [Ajobski

[x(k)y(k)z(k)

]≤ Cjobs(k)] (9)

where j denotes the jth row (component) of a matrix(vector). The following logical constraints

4∨j=1

¬δij(k) = 1, ∀i = 1, . . . ,M (10)

impose that at least one linear inequality in (7) is violated,

therefore enforcing the quadcopter position

[x(k)y(k)z(k)

]to lie

outside each obstacle.

Differently from Bemporad et al. [2009], we want topenalize here that vehicles fly too close to obstacles. Tothis end, each obstacle seen by a UAV is surroundedby a “safety area”, represented by a larger tetrahedroncontaining the one defined by (7). For such safety areas,we also introduce binary variables δ`j(k), ` = 1, . . . , S, asin (9), where S is the number of safety areas (S ≤ Min case of one UAV flying in an area with M obstacles),but without imposing the logical constraints as in (10), sopermitting the UAV entering those areas. Such an eventis penalized by defining for each “safety area” a variableγ`(k), ` = 1, . . . , S

γ`(k) =

1 if

4∧j=1

δ`j(k) = 1 ∀` = 1, . . . , S

0 otherwise

(11)

that the controller will try to keep at zero. In this way,the vehicle will tend to avoid passing through the safetyareas that have been set around the obstacles. Note thatalthough γ` can only assume values 0 and 1, we treat itas a real variable to ease the hybrid MPC computationsthat will be defined in the next paragraphs. The samplingtime Thyb must be chosen large enough to neglect fasttransient dynamics, so that the lower and upper MPCdesigns can be conveniently decoupled. On the other hand,the obstacle avoidance constraint (10) is only imposed atmultiples of Thyb. Note that an excessively large Thyb maylead to trajectories that go through the obstacles duringintersampling intervals (kThyb, (k + 1)Thyb).

2.4 Hybrid model for UAV navigation in formation

The hierachical structure described above for one quad-copter is extended to coordinate a formation of V coop-erating UAVs, V > 1. We use a leader-follower approachwith decentralized scheme to manage the formation: oneof the vehicles is chosen as a leader to direct the formationfollowing a prescribed path, and all the other vehicles, thefollowers, are commanded to maintain a constant relativedistance reference from the leader. Each UAV is equippedwith its own hybrid MPC controller and takes decisionsautonomously, measuring its own state and the positionsof the neighboring vehicles and obstacles, planning its ownpath under obstacle avoidance constraints. The whole for-mation must be capable of reconfiguring, making decisions(for instance, changing relative distances to modify theformation shape), and achieving mission goals (e.g., targettracking). Moreover, each UAV must avoid collisions withthe other vehicles in the formation, that are treated asobstacles, so that M accounts now for both real obstaclesand other (neighboring) vehicles.

The overall hybrid dynamical model is obtained by collect-ing (4), (8), (9), (10), (11). These are modeled through themodeling language HYSDEL [Torrisi and Bemporad, 2004]and converted automatically by the Hybrid Toolbox forMATLAB [Bemporad, 2004] into mixed logical dynamical(MLD) form [Bemporad and Morari, 1999]

ξH(k + 1) = AξH(k) +B1∆uH(k) +B2δ(k) +B3γ(k)(12a)

yH(k) = CξH(k) +D1∆uH(k) +D2δ(k) +D3γ(k)(12b)

E2δ(k) + E3γ(k) ≤ E1∆uH(k) + E4ξH(k) + E5,(12c)

where ξH(k) = [x(k) y(k) z(k) C ′1(k) . . . C ′M (k) xd(k −1) yd(k − 1) zd(k − 1)]′ ∈ R6+4M is the state vector,∆uH(k) = [∆xd(k) ∆yd(k) ∆zd(k)]′ ∈ R3 is the inputvector, yH(k) = [x(k) y(k) z(k)] ∈ R3 is the outputvector, δ(k) ∈ {0, 1}4M+4S and γ(k) ∈ RS are respec-tively the vector of binary (defined in (9)) and continuousauxiliary variables. The inequalities (12c) include a big-Mrepresentation [Bemporad and Morari, 1999] of (9) and apolyhedral inequality representation of (10). Matrices A,B1, C, E1, E2, E4, E5 have suitable dimensions and are

generated by the HYSDEL compiler. In order to designa hybrid MPC controller, consider the finite-time optimalcontrol problem

min{∆u(k)}NH−1

k=0

NH−1∑j=0

(yH(k + j + 1)− yHt)′Qy(yH(k + j + 1)

− yHt) + ∆u′H(k + j)R∆uH(k + j)

+ γ′H(k + j)QγγH(k + j) (13a)

s.t. MLD dynamics (12) (13b)

constraints (5) (13c)

where NH is the prediction horizon, yHt = [xt yt zt]′ is the

desired position (e.g., the position of the target or of theleading vehicle), γH are the auxiliary continuous variableswith reference γHt, Qy ≥ 0, R > 0 ∈ R3×3 and Qγ ≥ 0∈ RS×S are weight matrices.

The MLD hybrid dynamics (12) have the advantage ofmaking the optimal control problem (13) solvable bymixed-integer quadratic programming (MIQP). At eachtime t = 0, Thyb, . . . , kThyb, . . ., given the current referencevalues yH(t) and the current state ξ(t), Problem (13)is solved to get the first optimal input sample ∆u∗H(t),which is commanded as the increment of desired set-point (xd, yd, zd) to the linear MPC controller at the lowerhierarchical level.

For comparison purposes, we also consider an alternativecentralized scheme, consisting of a single hybrid MPCcontroller based on an overall model of the dynamics andof the obstacles of the entire formation, and generating thereferences for all UAVs. Clearly, this centralized approachhas the drawback of requiring the solution of a singleMIQP optimization problem for the entire team, whichis typically more intensive from a computation viewpointthan solving V smaller hybrid MPC problems.

3. POTENTIAL FIELDS METHOD

For comparing the hybrid MPC approach with other exist-ing navigation schemes, we consider the 3D potential fieldsmethod proposed in Paul et al. [2008] for a formation ofhelicopters, adapted for the formation of quadcopters de-fined earlier. In this case, obstacles are treated as spherical,rather than tetrahedral. A potential field is generated foreach UAV depending on the formation pattern, desiredand actual position, and obstacle positions, for collisionand obstacle avoidance and target tracking. The total field

F tot = Ft + F totca + F totoa (14)

for each vehicle, used for generating the references forthe lower-level stabilizing linear MPC, is composed bythe three components Ft (target tracking), F totca (collisionavoidance), and F totoa (obstacle avoidance). The contribu-tion for tracking the position of the target is Ft = Kt(t−p),where Kt is the gain for target potential, t is the targetposition and p the vehicle position. For the leading vehicle,the target is a given fixed position pt, for the followers isa given distance pd from the leader. To avoid collisions,a safety space is defined around each vehicle or obstacle,defined as a sphere with positive radius rsav. The addi-tional field component for vehicle i, whose safety sphere isinvaded by vehicle j, is defined by

Fca(k) =

(Kcarsav||dji||

−Kca

)dji||dji||

if ||dji|| ≤ rsav

0 otherwise(15)

where Kca is the gain for collision avoidance and dji, i 6= j,is the distance between vehicles i and j. The total amountof the collision avoidance term is given by

F totca =

N∑j=1

F ijca for i 6= j (16)

Eqs. (15) and (16) change to

Foa(k) =

(Koa

||dki||− Koa

rsav

)dki||dki||

if ||dki|| ≤ rsav

0 otherwise(17)

F totoa =

M∑k=1

F ikoa for i 6= k (18)

for obstacle avoidance. Here, dki represents the distancebetween vehicle i and the center of obstacle k, and Koa isthe gain for obstacle avoidance. In both cases, to increaseperformance, rsav is chosen dynamically, depending on thevelocity p of the vehicle

rsav = rminsav +Ksav||p|| (19)

using Ksav as a gain and rminsav as the minimum distancefor safety. For collision avoidance rminsav = 2rq, for obstacleavoidance rminsav = robs(k) + rq, where rq and robs(k) arethe radius of quadcopter and of the obstacle, respectively.Finally, the reference trajectory pi,r for vehicle i is givenby

pi,r = pi + F toti (20)

where pi is the position of vehicle.

4. SIMULATION RESULTS

The overall system is tested by cascading the linear MPCcontroller with the hybrid MPC designed for navigation,according to the hierarchical scheme of Figure 1. Thesimulation consists of avoiding four obstacles of differentdimensions, placed along the path to the target point.

The linear MPC controller is tuned according to thefollowing setup. Regarding input variables, we set umin

Lj =

0 V, umaxLj = 11.1 V, w∆u

i,j = 0.1, ∀j = 1, . . . , 4, i =0, . . . , NL − 1. For output variables we set a lower boundzminL = 0 on altitude, and upper and lower boundsymaxL1−2 = −ymin

L1−2 = π6 on pitch θ and roll φ angles.

The output weights are wyj = 0, j ∈ {1, 2}, on θ and

φ, wyj = 1, j ∈ {7, 8}, on θ and φ, and wyj = 10 onthe remaining output variables. The chosen set of weightsensures a good trade-off between fast system response andactuation energy. The prediction horizon is NL = 20,the control horizon is NLu = 3, which, together withthe choice of weights, allow obtaining a good compromisebetween tracking performance, robustness, and limitedcomputational complexity. The sampling time of the linearMPC controllers is Ts = 1

14 s. The remaining parameters

V y,min, V y,max, ρε are defaulted by the Model PredictiveControl Toolbox [Bemporad et al., 2004a].

−100

1020

3040

−10

0

10

20

30

40−10

0

10

20

XY

Z

Leader

Follower 1

Follower 2

Targetpoints

Startingpoints

Fig. 3. Trajectories of formation with obstacle avoidance,hybrid approach

The following parameters are employed for hybrid MPC:α1x = α1y = α1z = 0.6, β1x = β1y = β1z = 0.4 forthe approximation of the lower level dynamics; NH=10(prediction horizon), Thyb=1.5 s, and ∆ = 1

5Thyb; k1 =

k4 = 511 , k2 = 10

27 , k3 = 516 are the scaling factors used to

model the obstacles as tetrahedra, and for each of them wehave the corresponding “safety area” with scaling factorks = ki

1+ki15/20 , i = 1, . . . , 4; finally, matrices Qy = 0.01 ·I3×3, Qγ = 10 ·I4×4 and R = 0.1 ·I3×3 are used as weights.

The initial positions of the UAVs are pL(0) , [xL(0)

yL(0) zL(0)]′ = [0 0 0]′ for the leader, pF1(0) ,[xF1(0) yF1(0) zF1(0)]′ = [−2 − 2 0]′, and pF2(0) ,[xF2(0) yF2(0) zF2(0)]′ = [−4 − 4 0]′ for the followers;the target point for the leader is located at [xt yt zt]L =[35 35 6]′; the followers take off with a delay of 2.5 and 5seconds respectively, and should follow the leader at givendistances [xt yt zt]

′F1 = pL − pd1, [xt yt zt]

′F2 = pd1 =

[6 1 0]′, and pL − pd2, pd2 = [1 6 0]′. The results wereobtained on a Core 2 Duo running MATLAB R2009b,the Model Predictive Control and the Hybrid Toolboxunder MS Windows, using the MIQP solver of CPLEX11.1 [ILOG, Inc., 2008].

4.1 Decentralized hierarchical hybrid + linear MPC

The trajectories obtained by using the decentralized hier-archical hybrid + linear MPC are shown in Figure 3. Theperformance is quite satisfactory: trajectories circumventobstacles without collisions, and finally the quadcopterssettle at the target point, while maintaining the desiredformation as much as the obstacles consent it.

4.2 Centralized hybrid MPC + decentralized linear MPC

Next, we compare the results with the trajectories ob-tained by a single centralized hybrid MPC planner cas-caded by the decentralized set of linear MPC controllersfor stabilization. The performance is very similar, and tra-jectories are also very similar to those reported in Figure 3.However, there is an increment of computational complex-ity: while with the decentralized scheme the average CPU

Table 2. Parameters for the potential fieldsmethod

Kt 0.15 Gain for target

rq 1.3 Quadcopter radius

Kca 10 Gain for collision avoidance

Koa 40 Gain for obstacle avoidance

Ksav 2 Gain for a save avoidance

robs(1) 2.5 Radius of obstacle 1

robs(2) 3 Radius of obstacle 2

robs(3) 3.5 Radius of obstacle 3

robs(4) 2.5 Radius of obstacle 4

time to compute the hybrid MPC action for set-pointgeneration is about 0.073 s per time step (Thyb=1.5 s),with the centralized scheme is about 0.466 s.

4.3 Comparison with the potential fields method

Consider now the potential fields method described inSection 3 to generate on-line the desired positions, whilemaintaining the lower-level linear MPC controllers for sta-bilization and reference tracking. In this case the obstaclesare modeled as spherical obstacles, rather than polyhedra.The parameters used for simulation are reported in Ta-ble 2.

The potential fields method has a lower computationalcomplexity, with a CPU time to calculate the desiredposition in the order of milliseconds. Even if the perfor-mance of obstacle avoidance (see Figure 4) is satisfactory,it takes a longer time to reach the target. Moreover, itis necessary to impose an upper bound zmax

L = 22.4 onaltitude in the linear MPC formulation, to avoid undesiredovershoots due to fast variations of the references. Finally,to make a quantitative comparison of the different controlstrategies, we show different performance indices in Table 3and compare them for the different navigation algorithms:decentralized hybrid MPC, centralized hybrid MPC, andpotential fields method. We consider the following threeindices defined on the simulation interval 25÷300 s (i.e.,350÷4200 samples)

Jtt=

4200∑k=350

‖pL(k)− pt‖22

Jfpt=

4200∑k=350

‖pL(k)− pF1(k)− pd1‖22 + ‖pL(k)− pF2(k)− pd2‖22

Ju=

4200∑k=350

‖u(k)− u(k − 1)‖1

where Jtt represents the target tracking Integral SquareError (ISE) index, Jfpt the formation pattern trackingISE index, and Ju the absolute derivative of input signals(IADU) index for checking the smoothness of controlsignals [Raffo et al., 2010]. The indices are normalized withrespect to the values obtained using centralized hybridMPC. It is apparent that the hybrid MPC approachoutperforms the potential fields method. Note also thatthe decentralized and the centralized hybrid MPC schemeshave almost equal performance, actually the decentralizedscheme is even slightly better. This maybe due to thereceding-horizon mechanism of MPC and to the fact

Fig. 4. Trajectories of formation with obstacle avoidance,potential fields method

Table 3. Comparison of different approaches

Jtt Jfpt Ju

centralized hybrid MPC 1 1 1

decentralized hybrid MPC -0.09% -0.51% -0.03%

potential fields +210.32% +294,30% +212.75%

that the MPC weights were tuned for the decentralizedapproach and then used for both schemes.

In the simulations we assumed that the positions of theobstacles are known at each sample step. In more realisticapplications with several obstacles it may be enough toonly know the locations of the obstacles which are closestto the vehicle, for a threefold reason. First, because ofthe finite-horizon formulation, remote obstacles will notaffect the optimal solution, and may be safely ignored tolimit the complexity of the optimization model. Second,because of the receding horizon mechanism, the optimalplan is continuously updated, which allows one to changethe maneuvers to avoid new obstacles. Third, in a practicalcontext obstacles may be moving in space, and since suchdynamics is not modeled here, taking into account remoteobstacles in their current position has a weak significance.The number M of obstacles to be taken into accountin the hybrid model clearly depends on the density ofthe obstacles and the speed of the vehicle. Note that,depending on the sensor system on board of the vehicle, itmay be even impossible to measure the position of remoteobstacles.

5. CONCLUSIONS

In this paper we have shown how model predictive controlstrategies can be employed for autonomous navigationof formation of unmanned aerial vehicles, such as quad-copters. A linear MPC controller takes care of vehiclesstabilization and reference tracking, and a hybrid MPCgenerates the path to follow in real-time to reach a giventarget while avoiding obstacles. The simulation resultshave shown the reduced computational load of the decen-tralized hybrid MPC scheme compared to the centralizedone, and the better performance of hybrid MPC in compar-ison to other on-line planning methods like potential fields.Compared to off-line planning methods, such a feature of

on-line generation of the 3D path to follow is particularlyappealing in realistic scenarios where the positions of thetarget and of the obstacles are not known in advance, butrather acquired (and possibly time-varying) during flightoperations.

REFERENCES

E. Altug, J.P. Ostrowski, and C.J. Taylor. Control of aquadrotor helicopter using dual camera visual feedback.The International Journal of Robotics Research, 24(5):329–341, 2005.

A. Bemporad. Hybrid Toolbox - User’s Guide, 2004. http://www.ing.unitn.it/~bemporad/hybrid/toolbox.

A. Bemporad and M. Morari. Control of systems integrat-ing logic, dynamics, and constraints. Automatica, 35(3):407–427, 1999.

A. Bemporad, M. Morari, and N.L. Ricker. Model Pre-dictive Control Toolbox for Matlab – User’s Guide. TheMathworks, Inc., 2004a. http://www.mathworks.com/access/helpdesk/help/toolbox/mpc/.

A. Bemporad, C.A. Pascucci, and C. Rocchi. Hierarchicaland hybrid model predictive control of quadcopter airvehicles. In 3rd IFAC Conference on Analysis andDesign of Hybrid Systems, Zaragoza, Spain, 2009.

A. Bemporad, N.L. Ricker, and J.G. Owen. Model Pre-dictive Control-New tools for design and evaluation. InAmerican Control Conference, volume 6, pages 5622–5627, Boston, MA, 2004b.

F. Borrelli, T. Keviczky, G.J. Balas, G. Stewart, K. Fre-gene, and D. Godbole. Hybrid decentralized control oflarge scale systems. Lecture Notes in Computer Science,pages 168–183. Springer-Verlag, 2005a.

F. Borrelli, T. Keviczky, K. Fregene, and G.J. Balas.Decentralized receding horizon control of cooperativevechicle formations. In Proc. 44th IEEE Conf. on De-cision and Control and European Control Conf., pages3955–3960, Sevilla, Spain, 2005b.

T. Bresciani. Modelling, identification and control of aquadrotor helicopter. Master’s thesis, Department ofAutomatic Control, Lund University, October 2008.

P. Castillo, A. Dzul, and R. Lozano. Real-time stabi-lization and tracking of a four-rotor mini rotorcraft.IEEE Transactions on Control Systems Technology, 12(4):510–516, 2004.

M. Chen and M. Huzmezan. A combined MBPC/2DOF H∞ controller for a quad rotor UAV. In AIAAAtmospheric Flight Mechanics Conference and Exhibit,2003.

J.H. Chuang. Potential-based modeling of three-dimensional workspace for obstacle avoidance. IEEETransactions on Robotics and Automation, 14(5):778–785, 1998.

W.B. Dunbar. Model predictive control: extension tocoordinated multi-vehicle formations and real-time im-plementation. Technical report, CDS Technical MemoCIT-CDS 01-016, California Institute of Technology,Pasadena, CA 91125, 2001.

W.B. Dunbar and R.M. Murray. Model predictive con-trol of coordinated multi-vehicle formations. In IEEEConference on Decision and Control, volume 4, pages4631–4636, 2002.

G.M. Hoffmann, S.L. Waslander, and C.J. Tomlin.Quadrotor helicopter trajectory tracking control. In

Proc. AIAA Guidance, Navigation, and Control Conf.,Honolulu, HI, 2008.

ILOG, Inc. CPLEX 11.2 User Manual. Gentilly Cedex,France, 2008.

A.O. Kivrak. Design of control systems for a quadrotorflight vehicle equipped with inertial sensors. Master’sthesis, Atilim University, Turkey, 2006.

J.C. Latombe. Visibility Graph Method, pages 155–161.Kluwer academic publishers, 1991.

W. Li and C.G. Cassandras. Centralized and distributedcooperative receding horizon control of autonomousvehicle missions. Mathematical and computer modelling,43(9-10):1208–1228, 2006.

V. Manikonda, PO Arambel, M. Gopinathan, RK Mehra,FY Hadaegh, S.S.C. Inc, and MA Woburn. A modelpredictive control-based approach for spacecraft forma-tion keeping and attitude control. In American ControlConference, 1999. Proceedings of the 1999, volume 6,1999.

L. Pallottino, E.M. Feron, and A. Bicchi. Conflict resolu-tion problems for air traffic management systems solvedwith mixed integer programming. IEEE Transactionson Intelligent Transportation Systems, 3(1):3–11, 2002.

T. Paul, T.R. Krogstad, and J.T. Gravdahl. Modelling ofUAV formation flight using 3D potential field. Simu-lation Modelling Practice and Theory, 16(9):1453–1462,2008.

G.V. Raffo, M.G. Ortega, and F.R. Rubio. An integral pre-dictive/nonlinear h∞ control structure for a quadrotorhelicopter. Automatica, 46:29–39, 2010.

A. Richards and J. How. Decentralized model predictivecontrol of cooperating UAVs. In Proc. 43rd IEEE Conf.on Decision and Control, pages 4286–4291, 2004.

A. Richards and JP How. Aircraft trajectory planningwith collision avoidance using mixed integer linear pro-gramming. In American Control Conference, 2002. Pro-ceedings of the 2002, volume 3, 2002.

F.D. Torrisi and A. Bemporad. HYSDEL-A tool forgenerating computational hybrid models for analysisand synthesis problems. IEEE Transactions on ControlSystems Technology, 12(2):235–249, 2004.