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)

AbstractThis paper proposes a hierarchical approach and a set of MPC strategies for au-tonomous navigation of a formation of unmanned aerial vehicles (UAVs) under obstacle andcollision avoidance constraints, including lower-level vehicle stabilization. Each vehicle, ofquadcopter type, is stabilized by a local linear MPC controller around commanded desired set-points. These are generated at a slower sampling rate by a hybrid MPC controller per vehicle atthe upper control layer, based on a hybrid dynamical model of the UAV and of its surroundingenvironment (i.e., the other UAVs and obstacles). The resulting decentralized scheme controlsthe formation based on a leader-follower approach. The performance of the hierarchical controlscheme is assessed through simulations and comparisons with other path planning strategies,showing the ability of linear MPC to handle the strong couplings among the dynamical variablesof each quadcopter under motor voltage and angle/position constraints, and the flexibility ofthe decentralized hybrid MPC scheme in planning the desired paths on-line.

Keywords: Model predictive control, hierarchical control, aerospace, decentralized control,hybrid systems, obstacle avoidance, unmanned air 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;Raffo et al., 2010), and recently linear MPC (Model Pre-dictive Control) (Bemporad et al., 2009).

MPC is particularly suitable for control of multivariablesystems governed by costrained dynamics, as it allowsto operate closer to the boundaries imposed by hardconstraints; in the context of UAVs, MPC techniques

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

have been already applied for control of formation flightin Dunbar (2001), Dunbar and Murray (2002), Borrelliet al. (2005b), Li and Cassandras (2006), Richards andHow (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 adopts 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. In this paper we extend the approachin two ways: first, we let the underlying linear MPC al-gorithm controlling directly motor voltages, rather thantorques; second, the higher-level hybrid MPC are orga-

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

Figure 1. Hierarchical control structure for UAV naviga-tion

nized in a decentralized control scheme, based on a leader-follower approach. We assume that target and obstaclepositions may be time-varying, and in this case that onlytheir current coordinates are known (not the future ones),so that off-line (optimal) planning cannot be easily accom-plished.

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 designin Section 2.2 for stabilization under constraints and track-ing of generated trajectories. Then, the higher-level hybridMPC controller for path planning and obstacle avoidanceis described in Section 2.3. In Section 4 we give the resultsof simulations, comparing our approach with the potentialfields method described in Paul et al. (2008). Finally, someconclusions are drawn 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. This references are tracked in real-timeby a linear MPC controller placed at middle layer of thearchitecture. At the bottom layer of the closed-loop systemlies the nonlinear dynamics of the quadcopter, commandedby linear MPC. In the next subsections we describe indetails each layer of the architecture.

2.1 Nonlinear quadcopter dynamics

A quadcopter air vehicle is an underactuated mechanicalsystem with six degrees of freedom and only four controlinputs (see Figure 2). We denote by x, y, z the position ofthe vehicle and by θ, φ, ψ its rotations around the Carte-sian axes, relative to the “world” frame I. In particular, xand y are the coordinates in the horizontal plane, z is thevertical position, ψ is the yaw angle (rotation around thez-axis), θ is the pitch angle (rotation around the x-axis),and φ is the roll angle (rotation around the y-axis). Thedynamical model adopted in this paper is mainly based onthe model proposed in Bresciani (October 2008), simplifiedto reduce the computational complexity and to make easier

Figure 2. Quadcopter model

the design of the controller. As described in Figure 2,each of the four motors M1, M2, M3, M4 generate fourthrust forces f1, f2, f3, f4, and four torques τ1, τ2, τ3,τ4, respectively, which are adjusted by manipulating thevoltages applied to the motors. We use the linear equationsobtained 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 inthe three-dimensional space. The dynamical system isdescribed by following equations

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, andIxx, Iyy, Izz are the moments of inertia around the bodyframe axes x, y, z. The nonlinear dynamical model hastwelve 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

In order to design a linear MPC controller to stabilize thequadcopter vehicle on given desired positions/angles, wefirst linearize the nonlinear dynamical model (2) aroundan equilibrium condition of hovering. The resulting lin-ear 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 positions. Theclosed-loop dynamics composed by the quadcopter and thelinear MPC controller can be very roughly approximatedas a 3-by-3 diagonal linear dynamical system, whose inputsare (xd, yd, zd) and whose outputs are (x, y, z). Accord-ingly, the dynamics is formulated in discrete-time as{

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

(4)

where ∆(·)d(k) is the increment of desired (·)-coordinatecommanded at time kThyb, and Thyb > Ts is the samplingtime of the hybrid MPC controller. Input increments∆xd(k), ∆yd(k), ∆zd(k) are upper and lower bounded bya quantity ∆

−∆[

111

]≤[

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

]≤ ∆

[111

](5)

Constraint (5) is a tuning knob of the hybrid MPC con-troller, as it allows one to directly control the speed ofmaneuver of the quadcopter by imposing constraints on

the reference derivatives

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

]∥∥∥∥∞≤ ∆ · Thyb.

Obstacles are modeled as polyhedral sets. For minimizingcomplexity, the ith obstacle, i = 1, . . . ,M , is modeled asthe 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 may be used as well if obstaclevelocities and/or accelerations were estimated.

Finally, to represent the obstacle avoidance constraint,define the 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)..Also for such safetyareas we define 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)to allow 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 it asa real variable to ease the hybrid MPC computations that

will be defined in the next paragraphs. The sampling timeThyb must be chosen large enough to neglect fast transientdynamics, so that the lower and upper MPC designs can beconveniently decoupled. On the other hand, the obstacleavoidance constraint (10) is only imposed at multiples ofThyb, and hence an excessively large sampling time maylead to trajectories that go through the obstacles duringintersampling intervals.

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; ac-cording to such an approach, one of the vehicles (Leader)is chosen to direct the formation, following a prescribedpath, and all the other aircraft (Followers) are expected tomaintain a constant relative distance reference from theLeader. Each UAV is equipped with its own hybrid MPCcontroller and takes decisions autonomously, measuring itsown state and the positions of the neighboring vehicles andobstacles, planning its own path with obstacle avoidance.The whole formation must be capable of reconfiguring,making decisions (for instance, changing relative distancesto modify the formation shape), and achieving missiongoals (e.g., target tracking). Moreover, each follower mustavoid collision with the other one. In this case the othervehicles in the formation are treated as obstacles, so thatM accounts now for both real obstacles and other (neigh-boring) vehicles. We take account the real dimensions ofthe vehicles and avoid that they move too close to eachother and to the obstacles to minimize risk of collision.

The overall hybrid dynamical model is obtained by col-lecting (4), (8), (9), (10), (11). These are modeled throughthe modeling language HYSDEL (Torrisi and Bemporad,2004) and converted automatically by the Hybrid Tool-box (Bemporad, 2004) into a mixed logical dynamical(MLD) system 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 aregenerated 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) has the advantage ofmaking the optimal control problem (13) solvable bymixed-integer quadratic programming (MIQP). At eachsample step k, given the current reference values yH(k) andthe current state ξ(t), Problem (13) is solved to get the firstoptimal input sample ∆u∗H(k), which is commanded asthe increment of desired set-point (xd, yd, zd) to the linearMPC controller at the lower hierarchical level.

As an alternative, to manage the formation in this ap-proach we use a centralized scheme consisting of a singlehybrid MPC controller based on a macromodel (includingthe dynamics and obstacles of the entire formation) gener-ating the references for all UAVs, that are passed to the in-dividual linear MPC controllers designed for stabilization.Clearly, this centralized approach has the drawback ofneeding the solution of a single MIQP optimization prob-lem for the entire team, which typically requires significantcomputation. We will compare the performance of thecentralized and decentralized hybrid navigation schemesin the next section.

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, tetrahedra are replaced by spher-ical obstacles. This approach generates a potential field foreach UAV depending on formation pattern, desired andactual position, and obstacle positions, for collision andobstacle 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 vehicle Leader,the target is a given fixed position pt, for the Followersis a given distance pd from the Leader. To avoid collisionbetween vehicles or with obstacles, a safety space is definedaround each vehicle, defined as a sphere with positiveradius rsav. The additional field component for vehicle iwhose safety sphere is invaded 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. The total amount of thecollision 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 thevehicle’s velocity p:

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

using Ksav as a gain and rminsav as the minimum distancefor a save avoidance. For collision avoidance rminsav = 2rq,for obstacle avoidance rminsav = robs(k) + rq, where rq androbs(k) are respectively the radius of quadcopter and ofthe obstacle.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 (tetrahedra)of different dimensions, placed along the path between thequadcopters and the target points.

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 = 10on the remaining output variables. The chosen set ofweights ensures a good trade-off between fast systemresponse and actuation energy. The prediction horizonis NL = 20, the control horizon is NLu = 3, which,together with the choice of weights, allow obtaining a goodcompromise between tracking performance, robustness,and limited computational complexity. The sampling timeof the controller 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

Figure 3. Trajectories of formation with obstacle avoid-ance, 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

for tetrahedra (obstacles), and for each of them we havethe corresponding “safety area” with scaling factor ks =

ki1+ki15/20 , i = 1, . . . , 4; Qy = 0.01 · I3×3, Qγ = 10 · I4×4

and R = 0.1 · I3×3 are the weight matrices.

The initial positions of the UAVs arepL(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 thefollowers; the target point for the Leader is located at[xt yt zt]L = [35 35 6]; the followers take off with adelay of 2.5 and 5 seconds respectively, and should followthe leader at given distances [xt yt zt]

′F1 = pL − pd1,

[xt yt zt]′F2 = pd1 = [6 1 0]′, and pL − pd2, pd2 = [1 6 0]′.

The results were obtained on a Core 2 Duo running MatlabR2009b, the Model Predictive Control and the HybridToolbox under MS Windows, using the MIQP solver ofCPLEX 11.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 allow 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, but withan increment of computational complexity: while with thedecentralized scheme the average CPU time to computethe hybrid MPC action for set-point generation is about

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

0.073 s per time step (Thyb=1.5 s), with the centralizedscheme is about 0.466 s.

4.3 Comparison with potential fields method

In order to assess further the performance of the proposedhybrid MPC approach to formation flying control, wecompare it with the potential fields method describedin Section 3 to generate on-line the desired positions,while maintaining the lower-level linear MPC controllersfor stabilization and reference tracking. In this case thetetrahedra are replaced by four spherical obstacles. Theparameters used for simulation are reported in Table 2.

The use of the potential field reduces the computationalcomplexity thanks to the absence of the overall hybridmodel; the CPU time to calculate the desired positionsis of the order of milliseconds. Even if the performanceof obstacle avoidance (see Figure 4) is satisfactory, ittakes a longer time to reach the target. Moreover, it isnecessary 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 in Table 3 different performance indicesand compare them on 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

Figure 4. Trajectories of formation with obstacle avoid-ance, 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 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 compared to the corresponding cen-tralized hybrid MPC scheme, and the better performanceof hybrid MPC in comparison to on-line planning methods

like potential fields. Compared to off-line planning meth-ods, such a feature of on-line generation of the 3D path tofollow is particularly appealing in realistic scenarios wherethe positions of the target and of the obstacles are notknown in advance, but rather acquired (and possibly time-varying) during flight operations.

REFERENCES

Altug, E., Ostrowski, J., and Taylor, C. (2005). Controlof a quadrotor helicopter using dual camera visual feed-back. The International Journal of Robotics Research,24(5), 329–341.

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

Bemporad, A. and Morari, M. (1999). Control of systemsintegrating logic, dynamics, and constraints. Automat-ica, 35(3), 407–427.

Bemporad, A., Morari, M., and Ricker, N. (2004a). ModelPredictive Control Toolbox for Matlab – User’s Guide.The Mathworks, Inc. http://www.mathworks.com/access/helpdesk/help/toolbox/mpc/.

Bemporad, A., Pascucci, C., and Rocchi, C. (2009). Hierar-chical and hybrid model predictive control of quadcopterair vehicles. In 3rd IFAC Conference on Analysis andDesign of Hybrid Systems, Zaragoza, Spain.

Bemporad, A., Ricker, N., and Owen, J. (2004b). ModelPredictive Control-New tools for design and evaluation.In American Control Conference, volume 6, 5622–5627.Boston, MA.

Borrelli, F., Keviczky, T., Balas, G., Stewart, G., Fregene,K., and Godbole, D. (2005a). Hybrid decentralized con-trol of large scale systems. Lecture Notes in ComputerScience, 168–183. Springer-Verlag.

Borrelli, F., Keviczky, T., Fregene, K., and Balas, G.(2005b). Decentralized receding horizon control ofcooperative vechicle formations. In Proc. 44th IEEEConf. on Decision and Control and European ControlConf., 3955–3960. Sevilla, Spain.

Bresciani, T. (October 2008). Modelling, Identificationand Control of a Quadrotor Helicopter. Master’s thesis,Department of Automatic Control, Lund University.

Castillo, P., Dzul, A., and Lozano, R. (2004). Real-time stabilization and tracking of a four-rotor minirotorcraft. IEEE Transactions on Control SystemsTechnology, 12(4), 510–516.

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

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

Dunbar, W. (2001). Model predictive control: extensionto coordinated multi-vehicle formations and real-timeimplementation. Technical report, CDS Technical MemoCIT-CDS 01-016, California Institute of Technology,Pasadena, CA 91125.

Dunbar, W. and Murray, R. (2002). Model predictivecontrol of coordinated multi-vehicle formations. InIEEE Conference on Decision and Control, volume 4,4631–4636.

Hoffmann, G., Waslander, S., and Tomlin, C. (2008).Quadrotor helicopter trajectory tracking control. InProc. AIAA Guidance, Navigation, and Control Conf.,Honolulu, HI.

ILOG, Inc. (2008). CPLEX 11.2 User Manual. GentillyCedex, France.

Kivrak, A. (2006). Design of Control Systems for aQuadrotor Flight Vehicle equipped with inertial Sensors.Master’s thesis, Atilim University, Turkey.

Latombe, J. (1991). Visibility Graph Method, 155–161.Kluwer academic publishers.

Li, W. and Cassandras, C. (2006). Centralized and dis-tributed cooperative receding horizon control of au-tonomous vehicle missions. Mathematical and computermodelling, 43(9-10), 1208–1228.

Manikonda, V., Arambel, P., Gopinathan, M., Mehra, R.,Hadaegh, F., Inc, S., and Woburn, M. (1999). A modelpredictive control-based approach for spacecraft forma-tion keeping and attitude control. In American ControlConference, 1999. Proceedings of the 1999, volume 6.

Pallottino, L., Feron, E., and Bicchi, A. (2002). Conflictresolution problems for air traffic management systemssolved with mixed integer programming. IEEE Transac-tions on Intelligent Transportation Systems, 3(1), 3–11.

Paul, T., Krogstad, T., and Gravdahl, J. (2008). Mod-elling of UAV formation flight using 3D potential field.Simulation Modelling Practice and Theory, 16(9), 1453–1462.

Raffo, G., Ortega, M., and Rubio, F. (2010). An integralpredictive/nonlinear h∞ control structure for a quadro-tor helicopter. Automatica, 46, 29–39.

Richards, A. and How, J. (2004). Decentralized modelpredictive control of cooperating UAVs. In Proc. 43rdIEEE Conf. on Decision and Control, 4286–4291.

Richards, A. and How, J. (2002). Aircraft trajectoryplanning with collision avoidance using mixed integerlinear programming. In American Control Conference,2002. Proceedings of the 2002, volume 3.

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