Distributed Formation Control of Quadrotors under LimitedSensor Field of View∗

Duarte Dias1,2, Pedro U. Lima2, Alcherio Martinoli11Distributed Intelligent Systems and Algorithms Laboratory, EPFL, Lausanne, Switzerland

2Institute for Systems and Robotics, IST, Universidade de Lisboa, Lisboa, Portugal1,2duarte.dias@epfl.ch

ABSTRACTThis work tackles the problem of quadrotor formation con-trol, using exclusively on-board resources. Local inter-robotlocalization systems are typically characterized by limitedsensing capabilities, either in range or in the field of view.Most of the existing literature on the subject uses inter-robotcommunication to obtain the unavailable information, butproblems such as communication delays and packet loss canseriously compromise the system stability, especially whenthe system shows fast dynamics such as that of quadrotors.This work focuses on the sensor field of view limitation: itproposes a formation control algorithm that extends the ex-isting methods to allow each quadrotor to control the occu-pied area of its sensor field of view, while moving to the rightplace in the formation. This decreases the situations whennecessary inter-robot information is unavailable through lo-cal sensing, thus reducing the communication requirements.The system is proven to be stable when this algorithm isapplied. Results, both using simulated and real quadrotors,show the correct behavior of the algorithm without the useof communications, even when each robot can only sense asubset of the robots in the group.

General TermsAlgorithms, Experimentation, Performance

KeywordsQuadrotors, Formation Control, Limited Field of View

1. INTRODUCTIONThe use of Unmanned Aerial Vehicles (UAVs) has been

substantially increasing because their design has been sim-plified and their control techniques made more robust, ulti-mately enabling for vehicles of smaller size. A Smaller ve-hicle size allows for a more convenient exploration of multi-UAV systems in confined spaces, and at close range. Quadro-tors are typically chosen due to their high maneuverability

∗This work has been partially supported by ISR/LARSySStrategic Funds from FCT [UID/EEA/5009/2013] and FCTSFRH/BD/51928/2012.

Appears in: Proceedings of the 15th International Conferenceon Autonomous Agents and Multiagent Systems (AAMAS 2016),J. Thangarajah, K. Tuyls, C. Jonker, S. Marsella (eds.),May 9–13, 2016, Singapore.Copyright c© 2016, International Foundation for Autonomous Agents andMultiagent Systems (www.ifaamas.org). All rights reserved.

in such confined spaces. Additionally, through vehicle coor-dination, the geometry of these systems can be explored tominimize the impact of each vehicle limitations. For exam-ple, UAVs can organize to pick and transport heavier objectswith a specific shape, or to direct their limited on-boardcameras so that their combined images provide full 3D en-vironment coverage. This coordination is achieved throughformation control algorithms.

Formation control is a widely studied topic, both in 2Dand 3D configurations, with an extensive literature. For-mation control laws typically require that each robot knowsthe relative positions between itself and its neighbors. Inmost common approaches, the robots obtain this informa-tion by sharing via a communication channel their absolutepositions with respect to an environment, computed usingGlobal Navigation Satellite Systems (GNSSs) [20] in outdoorenvironments, Motion Capture Systems (MCS) [19, 1, 9] inindoor environments, or even Montecarlo or SLAM methods[16, 15], if the required on-board positioning sensors and en-vironment conditions allow to do so.

Other approaches use the on-board sensors to directly ex-tract the relative positions between robots, without the needfor communication or external systems. However, these sen-sors have limited capabilities, either on accuracy or Field OfView (FOV). This is especially true for the 3D case, becauseof the challenging sensing design, either due to the fact thatthe robot body represents an obstacle for the sensor itself, orbecause there is a compromise between the sensing area thatneeds to be covered, and the resolution of the sensor. Forexample, in [18, 6], on-board cameras extract accurate rela-tive bearing information, which is then used in a formationcontrol algorithm. However, since the cameras are charac-terized by a limited FOV, the information required from theneighbors that are not directly observed has to be providedalso through communication. In [17], the proposed frame-work uses both range and bearing information collected byeach quadrotor to reduce the amount of required observedneighbors per quadrotor. However, if the required neighborsare not observed, communication is still needed to acquirethe missing information.

Since high control rates are necessary to stabilize the highlydynamical system formed by these vehicles, all previous ap-proaches become fragile to packet loss or latency in commu-nication links. This is particularly important for short rangeinter-robot interactions, requiring faster reaction times. Tokeep communication to a minimum, the sensor constraintsmust be directly addressed. This was done mostly for groundvehicles, and for sensor range constraints, using potential

1087

field algorithms that include specialized terms to guaranteethat neighbor behavior will not compromise this type of con-straint [8, 12]. FOV constraints have also been considered,but just for leader-follower formations, where each followercontrols its FOV in a way to maximize the observation ofits leader [11, 10, 21]. However, since only one neighboris considered inside the sensor FOV, the multi-robot sys-tem is bounded to a limited number of inter-robot connec-tions, compromising the number of geometries achievablefor formation control. Additionally, less connections meansreduced system reactivity.

This work tackles these last issues. Sensor range limi-tations are left aside, and for the experiments, it was madesure that all the necessary inter-robot positions were smallerthan the sensor range limits. Also, sensor FOV constraintsare considered to be on the horizontal dimension, as typicalquadrotor formations simply lie on horizontal planes. Withthe previous assumptions, the work proposes an algorithm,applicable to quadrotors or any robot with similar dynam-ics. In particular, we extend the algorithm presented in [5]for a second order holonomic robot dynamics, where eachquadrotor uses an additional term to control the critical an-gle formed between the two neighbors closest to sensor FOVlimits. This is performed by making the quadrotor movingcloser or further away from the neighbors, depending if theangle needs to increase or decrease respectively. Addition-ally, this angle is also centered in the FOV to optimize thesensor safety margins.

The algorithm provides two contributions. Firstly, quadro-tor trajectories become constrained to cope with the vehi-cle’s sensor limitations when observing multiple neighbors.Secondly, by also controlling angles, this algorithm is ableto tackle configurations not achievable by some of the pre-vious algorithms (as the one in [5]) in presence of sensorFOV constraints. This work shows that the terms added inthe algorithm do not compromise the system stability. Also,although no discussion is provided about the system initialconditions or disturbances for which the FOV constraintsare still maintained, results (simulated and real) show thatfor typical situations, these constraints are effectively con-trolled. Thus, the proposed algorithm allows for formationcontrol without the use of inter-robot communication.

This paper is organized as follows. Section 2 providesbasic definitions and assumptions about the quadrotor dy-namics and its sensing capabilities. Based on those defini-tions, Section 3 formally introduces the FOV constraints anddiscusses how to built valid geometric formation configura-tions. Section 4 presents the formation control algorithm,along with its stability properties. In Section 5, a set ofsimulation and real experiments are performed to show thecorrect behavior of the algorithm, and some modificationsare designed to cope with real world disturbances. The workconcludes with some remarks in Section 6.

2. BASIC DEFINITIONSThis work considers a group of N quadrotors, forming at

any time a geometric configuration, defined by a full net-work graph G := (V, E). In this graph, V is the set of Nnodes, each one representing a quadrotor, described in a 3Denvironment by a position xi ∈ R3, a velocity, vi ∈ R3,and an attitude, given by the roll φi, pitch θi, and yaw ψi,angles. Each quadrotor is considered to have an on-boardauto-pilot and inertial sensors, which allows for the control

verticalplane

horizontalplane

flying and sensorframes

inertialframe

Figure 1: Description of the quadrotor flying frame(xm, ym, zm), with respect to the inertial frame. Thequadrotor is placed horizontally for illustrative pur-poses, but it can have non-zero roll and pitch. Notethe sensor horizontal (θh) and vertical (θv) FOV.

of the vehicle thrust together with the estimation and con-trol of its attitude. This enables the quadrotor to move toany desired 3D position, so it can be considered has an al-most perfectly holonomic vehicle. Note that only roll andpitch control together with the thrust control are needed tomove the vehicle on the three position axes, leaving the yawcontrol free to be used for independent purposes (as it will bediscussed later). Therefore, we split the pose control of thequadrotor into 3D position control, with a double integratormodel, and yaw control with a single integrator dynamics:

xi = ui, ψi = uψi , (1)

where u = (uxi , uyi , u

zi ) and uψi are the desired control inputs

for the quadrotor 3D position and yaw angle respectively.The single integrator on the yaw angle is a feature commonlyprovided by the on-board auto-pilots of quadrotors.

The set of all N(N − 1)/2 edges of G is represented byE , where each edge represents relative kinematic informa-tion between two quadrotors, namely relative position, xij =xj − xi = (xij , yij , zij)

T and, as derived quantity, relativevelocity vij = vj − vi = (vxij , v

yij , v

zij)

T . To control G, thequadrotors need to collect information represented by a sub-set of the previous edges, using their on-board sensors. Theinformation that can be obtained at any time is describedby a sensing graph, defined by GS := (V, ES), with ES repre-senting the set of edges of G, for which the relative positionsbetween the respective quadrotors can be measured usingthe on-board sensors.

Each quadrotor i is considered to have an on-board sen-sor, measuring the position xij and the velocity vij , in areference frame described in Fig. 1. The frame origin is thesame as the quadrotors, and the z-axis is aligned with theinertial frame (vertical). In the remainder of this paper, wewill call this frame the flying frame; it has an attitude of(φ, θ, ψ) = (0, 0, ψi) with respect to the inertial frame. Thesensor has its FOV center in the horizontal xy-plane, consid-ered to be the sensor direction, rsi . The FOV is representedby its horizontal, θh, and vertical, θv, components, definedwith respect to the previous described horizontal plane, anda vertical plane formed by rsi and the z-axis of the flyingframe. This can be assumed as quadrotors usually moveusing low roll and pitch values, and the distances betweenthe center of the robot and the sensor are small. In case the

1088

Figure 2: Inter-edge aperture between the edgesconnecting nodes j and k, to node i, both within theFOV of node i. Apertures formed by different nodepairs, for example using node l, are contained insideit. Note the optimal sensor direction, rψi , equally di-viding the occupied area on both sides of the FOV.

quadrotor and the sensors are tilted, the measures extractedon the sensor frame can be transformed into the flying frame,using the φi and θi values acquired from the inertial sensors.

Since the z-axis of the flying frame is the same for allquadrotors, it is useful to decouple the formation controlinto horizontal and vertical components (as explained laterin Section 4). For this reason, the previous relative kine-matic information is divided into horizontal components,xhij = (xij , yij)

T and vhij = (vxij , v

yij)

T , and vertical compo-nents, zij and vzij . The horizontal position component can

be transformed into polar coordinates (ehij , rhij)T , where ehij

is the absolute distance between quadrotors i and j, and rhijis a unitary vector defining the direction between i and j.The next section formally introduces the FOV constraintsfor each quadrotor, and discusses the type of geometric con-figurations considered in this work.

3. FIELD OF VIEW CONSTRAINTSThe sensor FOV constraints can be mathematically de-

scribed through the concept of inter-edge aperture. Giventwo nodes j and k connected to node i, the inter-edge aper-ture of the respective connection edges, αjik, is defined bythe angle between the relative position vectors representedby those edges, as shown in Fig. 2. This concept is dividedinto horizontal and vertical components, by projecting theedges in the respective planes defined in Fig. 1. Therefore,the inter-edge aperture of every pair of edges belonging toGS for a given quadrotor i must be smaller than θh in thehorizontal case, or θv in the vertical case. Only the biggestaperture for each quadrotor needs to be considered, becauseif this value is smaller than the FOV limit, all the othersapertures will be also smaller, as illustrated in Fig. 2. Ouralgorithm operates on the FOV constraints by directly con-trolling these apertures, as explained in the next section.

Note that if the FOV constraints are verified, there isalways a quadrotor yaw ψi, describing the sensor direction,rsi , that allows all neighbors to be observed. In fact, it ispossible to define an optimal sensor direction rψi , illustratedin Fig. 2, defined so that it equally distributes the sensingarea around the center of the largest inter-edge aperture,optimizing the measurement safety margins.

A targeted geometric configuration is defined by a graphG whose edges correspond to desired inter-robot positionsxdij = (xdij , y

dij , z

dij), and zero velocity. From this informa-

tion, all the desired apertures between sets of three nodes,αdjik can be obtained. To control this configuration, a forma-

tion control graph GF := (V, EF ) is defined, where each edgein EF corresponds to a controlled inter-robot distance. Theconfiguration is controllable if the following two conditionsare verified: GF is rigid, so that it has a unique solution(the reader is referred to [14] for more details about graphrigidity); GS must contain enough information to enable thecontrol of all edges in GF . This work assumes GS and GFfixed and defined a priori. Note that, in order to achievethe desired GS , the robots need to start the algorithm inpositions that are already respecting the FOV constraints.

Note that the previous section described the sensor di-rection, rsi to be always directed and controlled horizon-tally. Therefore, this work assumes geometric configurationswhere all quadrotors have the same height, or smaller thanthe vertical FOV, avoiding therefore vertical FOV violations.Additionally, this work considers the edges in ES and EFto be bi-directional. For ES edges, this concretely meansthat the quadrotors forming any given edge can mutuallysense each other. For EF edges, this concretely means thatboth quadrotors forming the edge actively participate onthe control of the mutual distance. These bi-directionalityassumptions are made to reduce the distance instability be-tween quadrotors, caused by possible delays in the vehicleperception-to-action loop.

The previous assumptions can limit the formation config-urations that can be considered, as the number of robotsincreases. This work focuses on the simplest type of forma-tions, placing the robots on the convex hull of the target for-mation shape. Robots can also be placed inside the convexhull, at different heights to avoid sensor occlusion. Theirneighbors can be selected randomly as long as the largestinter-neighbor aperture respects the sensor FOV constraints,as explained above. However, should the previous edge bi-directionality assumptions be kept, since robots inside theconvex hull observe a reduced neighbor set, the number ofinter-robot connections achievable by the other robots is alsoreduced. Uni-directional edges can be used if the overall for-mation rigidity is kept, but this case is left for future work.The next section introduces the formation control algorithm,and how it provides sufficient controllability to establish GF .

4. CONTROL ALGORITHMThe algorithm leverages two distinct components: the

control of the inter-robot distance and the maintenance ofFOV visibility. To control the inter-robot distances, eachdistance is given a weight, describing how strongly the quadro-tor controls it. The weighting information can be elegantlyrepresented using a Laplacian matrix, L, where the elementLij defines the control weight for the distance between neigh-bors i and j. This matrix is positive definite, the sum of theelements of each line sum to zero, and Lii = −

∑j 6=i Lij.

If Lij = 0, the distance between quadrotors i and j is notdirectly controlled. In this work, L is constant, since GF isassumed constant. Each quadrotor controls distances usinginter-robot position information measured by its on-boardsensors, in the flying frame. Since the z-axis of this frame isthe same for all quadrotors, and equal to the z-axis of theinertial frame, this dimension is controlled with a simpleconsensus equation, similarly to [3, 13], as follows:

uzi = kp

N∑j=1

Lij

(zdij − zij

)+ kv

N∑j=1

Lijvzij , (2)

1089

where uzi is the desired vertical acceleration for quadrotori, kp and kv are gain parameters for the position and thevelocity components of the controller. To control the hori-zontal dimension, the range and bearing controller proposedin [5] is used, but extended for a double integrator case andsimplified to the case of holonomic vehicles:

uhi = kp

N∑j=1

rhijLij

(ehij − edij

)+ kv

N∑j=1

Lijvhij, (3)

where uhi = (uxi , u

yi )T is the desired horizontal input in

the flying frame for quadrotor i and edij is the desired hori-zontal distance between quadrotors i and j, obtained from||(xdij , ydij)||. In both the previous equations, the positioncomponent aims at achieving the formation requirements,while the velocity component stabilizes the second order dy-namic system. Distances controlled in this way allow theedges representing them to be added into GF . To controlthese distances, the respective relative position informationis needed, which means these control edges must also belongto ES .

The second component of the algorithm controls the FOVconstraints, which can happen as the formation rotates aroundits z axis, or when quadrotors move to their desired po-sitions. To solve this issue, two new terms are included,allowing each quadrotor to additionally control the largestobserved inter-edge aperture, and the sensor direction, rsi ,in the horizontal plane. A simple yaw controller is used toderive rsi to the optimal direction defined in the previoussection, rψi . This is done by computing the angle betweenthe two vectors, δψ, illustrated in Fig. 2. Note that rsi isalways known, and rψi is computed by averaging rij and rik.The obtained δψ is given as the control input of the yawcontroller and driven to zero:

uψi = −kψδψi, (4)

where uψi is the yaw control input defined in Eq. 1, and kψ isa control gain. The biggest inter-edge aperture observed byquadrotor i, αkij , is controlled by adding a term to Eq. 3,as follows:

uh∗i = uh

i + kα(αdkij − αkij)(Krci +K⊥rc⊥i ), (5)

where αdkij is the desired aperture between quadrotor i andneighbors j and k currently defining the biggest aperture, kαis a control gain, and K and K⊥ will be chosen according tothe stability analysis presented in Proposition 1. Vector rci ,illustrated in Figs. 3 and 4, is a unitary vector defining thedirection between quadrotor i and the averaged formationcenter, Ci. This center is defined for each quadrotor, andits displacement from the robot is computed using the firstterm of Eq. 3:

rci ei =

N∑j=1

rhijLijehij , (6)

where ei is the distance between quadrotor i and Ci. Thedistance between a neighbor j and Ci is defined as eji . Notethat rci is always in between the neighbors closer to the FOVedges. Therefore, if αkij is too large, robot i generates aforce pointing backwards, in a direction that will always de-crease αkij . The contrary occurs when αkij is too small.This corresponds to a direct control of the sensor FOV con-straint during the algorithm’s operation.

Figure 3: Possible GF for a square formation andan horizontal FOV of less than 90◦. Filled edgescorrespond to direct distance control. Dashed edgesare included if aperture control is activated. Notequadrotor i’s weighted formation center.

With the additional control terms, each quadrotor also in-directly controls the distances between the neighbors them-selves, without them knowing the respective relative positioninformation. Consider the example in Fig. 3, when the FOVis less than 90◦. The most complete GS possible with theseconstraints is the one shown in the figure (excluding thedashed lines). So, with just direct distance control, no rigidgraph can be defined. However, if quadrotor i additionalycontrols αjik, one can see that ejk is fully expressed in termsof the direct controlled quantities, eij , eik, and αjik. So, theedge between neighbors j and k is automatically includedin EF , which does not need to belong to GS . Each quadro-tor can control an additional edge in this way, allowing theestablishment of rigid, or even fully connected, formationgraphs that could not possibly be formed before.

The next proposition shows that the new proposed termsdo not compromise the system stability. The algorithm canbe easily extended to control more than one inter-edge aper-ture for each quadrotor, but the presented stability proper-ties are related to the largest. Additionally, it is not clearthat these properties hold if the neighbors forming the aper-ture changes in time. Since the previous case is rare, inthis paper we focus on characterizing the algorithm behav-ior when the inter-edge aperture remains the same duringoperation, and leave the previous issue for future work.

Proposition 1. As long the necessary FOV constraintsare not violated, the multi-robot system, with each quadrotori described by the dynamics in Eq. (1) and applying the con-troller presented in Eqs. (2), (3), (4), and (5), is stable, forany chosen set of weights described in L, and any kp, kv, Kand K⊥ greater than zero.

Proof. Note that [13] already proved convergence prop-erties for vertical controller components Eq. (2). Also,thecomponent in Eq. (4) is independent from the actual for-

mation control. Therefore, if the formation converges, rψiconverges as well, allowing the yaw controller to stabilizeδψi to zero, since it is a proportional control applied to asingle integrator system.

The proof for the horizontal controller components of uh∗i

follows the reasoning of [5], which performs the analysis sep-arately for each robot, and its neighbors are assumed to havefixed positions, and then combines the results at the end. Tosimplify the proof, the bias terms (αdkij and edij) are set tozero, but the stability still holds for non-zero terms (suchterms would only change the equilibrium point).

Let us assume the case for quadrotor i, described in Fig. 4,representative of the horizontal control components. Quadro-

1090

Figure 4: Horizontal formation control formulation.

tors j and k correspond to quadrotor i neighbors forming thelargest aperture in its FOV. The position of quadrotor i’sformation center, Ci, can be expressed in the inertial frameas xi + rci ei, where the second term comes from Eq. 6. Thisexpression does not depend on xi if

∑j 6=i Lij = 1, which

means Ci doesn’t change with quadrotor i’s movements. Ifthe neighbors are assumed to have fixed positions, their dis-tances to Ci, e

ji and eki , are constant. The axes of quadrotor

i’s reference frame are changed to rci and rc⊥i , representingrespectively the radial and orthogonal axis with respect toCi. The velocity of Ci in this new frame is decomposedon the radial, ei and orthogonal, e⊥i , axes. Note that thisvelocity corresponds to the second term of Eq. 3, and thatei is the velocity of ei, representing the distance betweenquadrotor i and Ci. From the previous definitions, a simpleLyapunov function is chosen to analyze the stability of thesystem for quadrotor i:

Vi(αkij , ei, ei, e⊥i ) =

1

2(kαα

2kij + kpe

2i + (ei)

2 + (e⊥i )2),

which is greater than zero except in Vi(0, 0, 0, 0). The fourcomponents were considered because they represent the statesthat are being controlled (aperture, distance, and radial andorthogonal velocity). The derivative of Vi with respect totime can be expressed as:

Vi = kααkijαkij + kpeiei + eiei + e⊥i e⊥i ,

where ei and e⊥i can be expressed from the uh∗i terms in

Eqs. 3 and 5, projected into the radial and orthogonal com-ponents respectively:

ei = Kkααkij − kpei − kv ei,e⊥i = K⊥kααkij − kv e⊥i .

Therefore, Vi can be simplified, by removing the equal termsin its expression, to:

Vi = kααkijαkij +kααkij(eiK+ e⊥i K⊥)−kv(ei)2−kv(e⊥i )2.

The last two terms are always negative, which leaves thestudy of the first component. From Fig. 4, the aperture αkijcan be divided into βj +βk, where β(∗) = arctan(w(∗)/v(∗)),and (∗) is either j or k. By differentiating β(∗), αkij can bedefined as:

αkij =vjwj − vjwj

e2ij+vkwk − vkwk

e2ik.

From the figure, on can define v(∗) = ei − e(∗)i cos(γ(∗)) and

w(∗) = e(∗)i sin(γ(∗)), where γ(∗) is the angle going from rc(∗)

to rci . Recalling that neighbors have fixed positions withrespect to Ci, γ(∗) only depends on e⊥i , according to the

linear-to-angular velocity equation, e⊥i = γ(∗)ei. Using the

previous result for γ(∗), and recalling that e(∗)i is constant,

the derivatives of the previous expressions for v(∗) and w(∗)are as follows:

v(∗) = ei + e⊥i sin(γ(∗))e(∗)i

ei, w(∗) = e⊥i cos(γ(∗))

e(∗)i

ei.

From the previous result, and noting that the alternativedefinitions w(∗) = ei(∗) sin(β(∗)) and v(∗) = e(∗) cos(β(∗)), theprevious expression for αkij can be re-arranged, to isolatethe terms in e⊥i and ei, as follows:

αkij = e⊥i

(ejei

cos(γj+βj)

eij+ ek

ei

cos(γk+βk)eieik

)−ei

(sin(βj)

eij+ sin(βk)

eik

).

One can now choose K and K⊥ of the aperture controllerto eliminate the previous components, ending with the fol-lowing result:

K =(

sin(βj)

eij+ sin(βk)

eik

)K⊥ = −

(ejei

cos(γj+βj)

eij+ ek

ei

cos(γk+βk)eik

)Vi = −kv(ei)

2 − kv(e⊥i )2.

Note that Vi ≤ 0, and therefore using the Lyapunov theo-rem, the system with the proposed controller is stable andconverges to a subset of the state-space defined by Vi = 0.This can be extended to all quadrotors, by setting V =V1 + ... + VN , where Vi is the previous Lyapunov functionbut for each quadrotor. V > 0 except in V (0) = 0, and

V <= 0, and therefore, the system as a whole is also stable.Finally, note that kp

∑j 6=i Lij can always be transformed

into k∗p∑j 6=i L

∗ij for each quadrotor, where

∑j 6=i L

∗ij = 1,

necessary to guarantee that Ci does not depend on robot i’smovement. This allows the use of any Laplacian matrix inthis system, regardless of kp.

The system is stable, but it converges to the set describedas V ≤ 0, which, from the previous proof, only guaranteesthat the quadrotor velocities are zero. Deadlocks can oc-cur, especially if the configuration is ill defined, i.e. the setof desired ranges and apertures correspond to an impossi-ble configuration. In this case, the system will converge toa situation where the aperture controller will counter-actthe distance controller, creating the deadlock. The study ofdeadlocks is considered to be future work. However, notethat those already existed in [5], referenced as local minima.

Finally, the gains K and K⊥ found for the aperture con-troller are analyzed. The value of K is related to the con-troller radial component. Its value is intuitive, saying thatit is always bigger than zero, as β(i) ≤ π, and it is biggeras the angle increases to π/2, corresponding to the pointof maximum influence of the controller in the angle. Also,as eij decreases, the gain increases since the influence onthe aperture also increases. The value of K⊥ is related tothe controller orthogonal component, and it is less intuitive.However, note that it uses cosine instead of sine functions,indicating that it is controlling an axis orthogonal to theone K controls. For example, if all quadrotors are found ina line, K = 0, since moving on the line does not control theaperture, but K⊥ 6= 0, since moving orthogonally to the lineincreases the aperture. For simplicity, and due to time con-straints, this work considers K⊥ = 0, but the experimentsshow that the system can converge without this component.

1091

(a)

(b)

Leader

FollowersLeader

Figure 5: Experimental setups. (a) Webots simu-lator using a square formation - note that only twoquadrotors are visible from the simulated camerasensor view (upper left corner). (b) Triangle forma-tion in the flying arena endowed with a MCS, man-ufactured by Motion Analysis Inc. and composedof 20 Osprey cameras able to track the quadrotorsusing a set of reflective markers in an useful volumeof 4x7x2.5 m. Note the static leader on the right.

Future work will include experiments done with a non-zerovalue of K⊥ in the controller.

5. EXPERIMENTSThe algorithm was tested through a set of experiments

conducted using the platforms described in Fig. 5. Both sim-ulation (Fig. 5 (a)) and real (Fig. 5 (b)) environments wereconsidered. In reality, Hummingbird quadrotors, manufac-tured by Ascending Technologies1, were used. Each quadro-tor was equipped with a set of active markers. An on-boardcamera with a 320x240 pixel frame and an horizontal FOV of90◦ was used to detect the active markers on the neighbors,and extract their relative positions using an algorithm simi-lar to the one presented in [4]. After calibration, the sensornoise had a standard deviation of 13 cm at a 3 meter range.The high-fidelity robotic simulator Webots2 was used as thesimulating environment, with a standard quadrotor model,not tuned to match the real quadrotors. The markers weresimulated as colored blobs, and an ideal camera sensor wassimulated with the same FOV. Perturbations were added onthe simulated images to achieve the same noise levels mea-sured in reality. Although in reality sensor noise decreasesat smaller distances, in simulation it was kept with the sameintensity at all distances. Finally, the actuation noise in real-ity was not measured, and in simulation it was set to be zero.A positioning ground truth is provided in reality through theMCS presented in Fig. 5. Additional details about the real

1http://www.asctec.de/2https://www.cyberbotics.com/

−1 −0.5 0 0.5 1 1.5 2 2.5−1.5

−1

−0.5

0

0.5

1

1.5

x(m)

y(m

)

Leader

n=1n=2

n=3n=4

(1)

(2)

Initial robotposition

Figure 6: Evolution of the quadrotor trajectoriesperformed in simulation using the formation con-trol algorithm and the defined leader goal followingstrategy. Each robot is represented by a black tri-angle, and each color illustrates a trajectory of aspecific quadrotor. The triangle orientation is thesame as rsi . (1) and (2) represent snapshots, whenthe formation is respectively stable and distorted.

setup and the system composed of the camera and activemarkers can be found in [2].

The formation task was performed using four and threequadrotors, in simulation and real environments, respec-tively. Although not needed for the formation control, aleader was selected with the objective of anchoring the for-mation in the environment, and for the simulation experi-ments, to move the formation. The simulation experimentsuse a square formation configuration, depicted in Fig. 5 (a),and also described in Fig. 3, to assess rigidity issues. Thesquare sides measured 1.2 m, and each quadrotor observedother two quadrotors, controlling an aperture of 45◦ betweenthem. The formation leader was chosen at random. The realexperiments use a triangle formation configuration, depictedin Fig. 5 (b), where the leader was static in the environment.The desired distances were 1.5 m between the leader and thefollowers, and 2.12 m between the followers. The desiredaperture for each follower was also 45◦. Both graphs, if theaperture control is included, are fully connected and rigid.The desired relative heights between the quadrotors in bothscenarios were set to zero.

To move the leader in simulation, an extra term is addedto the vertical and horizontal formation control laws in Eqs. (5)and (2), as follows:

uhli = kfuh∗i + kgkv(vh

i − vd),uzli = kfu

zi + kgkv(vzi − vzdi ),

where kf and kg regulate the importance of formation cohe-sion and goal following behaviors, both set to 0.5, indicatingequal importance. The values of kp, kv and kα were set to1.6, 2.2 and 1.0 respectively. Therefore, the leader performsboth goal following and formation cohesion, similar to thework presented in [7], and followers perform just the for-mation cohesion. The Laplacian weights between the leaderand the followers were set to 1, as opposed to the weights be-tween the followers, always chosen to be 1

Ni, where Ni is the

number of neighbors of robot i. This is because, in contrastto the leader, follower movements do not translate directly

1092

−50

0

50

angl

e7(d

egre

es)

(a1)

(b1)

(a2)

(b2)

45º

-45º

45º

22.5º

-22.5ºn=1

n=2n=4

n=3

Follower,7n=2,7observations Leader,7n=1,7observations

−50

0

50

angl

e7(d

egre

es)

0 10 20 30 40 50 60 70 8020

40

60

time7(s)

angl

e7(d

egre

es)

0 10 20 30 40 50 60 70 8020

40

60

time7(s)

angl

e7(d

egre

es)

Figure 7: Controlled bearings (a) and aperture (b) values, observed from quadrotors n=2, and n=1. Desiredbearing and apertures depicted using horizontal lines, with respected values shown on the right. The neighborto which each bearing line corresponds, is indicated in text on the left. FOV limits shown with dashed lines.

the behavior that is desired for the robot, but rather a re-sponse to other neighbor changes, that can either have lag,noise, or be wrong. Therefore, these behaviors should beaveraged. The next sections describe separately the resultsobtained in each environment.

5.1 Simulation ResultsThe formation stability, rigidity, and correct maintenance

of the sensor FOV constraint were tested by moving thesquare formation through leader commands with a certainvelocity in the x and y dimensions separately, as shown inFig. 6 (the z velocity is set to zero). The used velocity valuewas of 0.5 m/s. The initial quadrotor positions were set sothat all necessary FOV constraints were initially met. Theproposed algorithm was then activated, so that the quadro-tors could converge to the desired configuration. For oneexperimental run, Fig. 8 shows the progress of all the hori-zontal distance errors between two pairs of quadrotors, andFig. 7 shows the neighbor bearings and apertures observedby the leader n = 1 and the follower n = 2 (numbers de-scribed in Fig. 6). The bearings are defined with respect tothe camera FOV center, rsi , illustrated in Fig. 5 (a).

Fig. 8 shows that the formation always converges to theright configuration, even after the perturbation caused byleader movements, showing its rigidity and stability. Notethat, when the leader moves, the configuration is distorted(also observed in Fig. 6), and with higher intensity on they axis (observed by the higher distance errors). The dis-

0 20 40 60 80 100−0.5

0

0.5

1

1.5

time (s)

dist

ance

(m

)

1

2

Figure 8: Horizontal distance errors between twopairs of quadrotors, during the experiment run cor-responding to Fig. 6. The two snapshots describedon that figure are highlighted here using circles.

tortions happen because only the leader has the knowledgeof the desired velocity and the followers simply follow theleader. The distortion is bigger on the y axis, because thatis the direction where the leader is aligned with the followern = 4, that only relies on other followers for information.This will generate a bigger movement delays between theleader and that follower, creating a bigger distance and aper-ture errors.

Fig. 7 shows similar perturbations on the bearing andaperture values. However, Fig. 7 (a), shows the convergenceof neighbor bearings, in both leader and follower cases, tosymmetric values with respect to rsi , meaning that the yawcontroller described in Eq. (4) optimizes the sensor FOV.Additionally, Fig. 7 (b) confirms, for the leader and followercases, the convergence of the aperture to the desired values,which in combination with results presented in Fig. 8, showsthe correct behavior of the horizontal controller proposed inEqs. (2), (3), and (5), and the correct maintenance of theFOV constraints. The aperture distortions are also higherfor movements on the y axis, for the same reasons as before.

−2.5 −2 −1.5 −1 −0.5 0 0.5 1 1.5 2 2.5

−0.5

0

0.5

1

1.5

2

2.5

3

1.75m

2.19

m

2.16m

2.03m

1.55m

1.60m

2.05

m

1.76m

2.03m

1.53m

x (m)

y (m

)

Followers

Static leader

(1)

(2)

(3)

Figure 9: Real quadrotor trajectories tracked by theMCS, during a run of the algorithm. Note that theleader is static. Each color represents one quadro-tor. Vector rsi is defined by the orientation of thedescribed triangles. (1), (2), and (3) represent threesnapshots of the formation, where (1) correspondsto the initial follower positions.

1093

1

1.5

2

2.5

3

horiz

ont

alud

ista

nceu

gm)

Ver

tica

ludis

tanc

eugm

)−50

0

50

angl

eugd

egre

es)

0 10 20 30 40 5020

40

60

timeugs)

angl

eugd

egre

es)

45º

-45º

-22.5º

22.5º

45º

ga)

gb)

Sensorufailure

Follower

Leader

CameraFOVlimits

0 10 20 30 40 50 60−0.4

−0.2

0

0.2

0.4

timeugs)

Follower

LeaderSensoruFailure

MCS

Estimation

2.12um

1.50um

0.00um

gc)

gd)

Figure 10: Neighbor bearings (a), aperture (b), and distances observed in the local frame of one of thefollowers for a real experiment. The distance is divided into horizontal (c) and vertical (d) components.Desired bearings, apertures, and distances are depicted on the respective plot using horizontal lines, withtheir values written on the right. The neighbor to which each bearing or horizontal distance line correspondsis indicated in text on the left of the line. Vertical ranges of both neighbors indistinguishable since they areboth close to zero. The aperture is related to both neighbors. All values are tracked using the MCS, but onthe distance plots, the values estimated from the sensor data are also shown. Black dots on the lines signalmoments where the follower stopped receiving sensor data of the respective neighbor for more than 100 ms.

5.2 Real ExperimentsSimilar experiments were performed using the triangle for-

mation described above, using the real setup depicted inFig. 5 (b). The leader was placed in the environment, andit remains static through all the experiment. The follow-ers were teleoperated into their initial positions using theMCS position feedback, in order to guarantee that theiron-board cameras FOV initially cover their neighborhood.The algorithm was then activated, running exclusively on-board, using the camera for sensor feedback, until a stopand land command was broadcast. The resulting trajec-tories for each quadrotor, from the start of the algorithmto the end of one experiment, are shown in Fig. 9. Theneighbor bearing, aperture, and distance information wastracked throughout the experiment for one of the followers,and shown in Fig. 10. For the bearing and aperture, theresults show that, although the followers rotate around theleader, their values remain close to the desired, as in thesimulated results, showing the correct behavior of the con-troller. This allowed loss of sensor data to be kept to aminimum, as shown in the figure. Note that, because of fol-lower rotations, the leader had to be also rotated by hand,so that the followers would keep observing the leader.

Follower rotations happen due to biases present on actua-tion and on sensing. Fig. 10 (c) shows an example of a sensorbias on the horizontal plane, observed by the differences be-tween MCS and on-board estimations of the tracked relativepositions. These biases create unwanted forces competingwith the formation control algorithm. Since the algorithmforces are tangent to the edge between quadrotors, the algo-rithm becomes weak on the radial axis, allowing relativelysmall forces to still be able to generate rotation movements.The biases can be different for each quadrotor, it could hap-pen that they generate rotations in different directions, thatwould lead to a steady increase of the follower aperture ob-served by the leader. In this case, the leader would performaperture control, going backwards to maintain the desired

value. If these biases are constant, the system would movebackwards until the end of the experiment. However, thesebiases can be minimized using integrators, included on therange control on each edge. Note that more than one edge isneeded to compensate the bias, as range control on an edgeis tangential to that edge. An integrator was also includedin the height controller. This can generate low frequency os-cillations around the desired relative displacements, which ispossible to observe in Fig. 10, in both vertical, with a maxi-mum of 40 cm, and horizontal components, with a maximumof 30 cm. However, despite those oscillations, the algorithmwas still successfully maintaining the desired formation.

6. CONCLUSIONSThis work proposed a distributed formation control algo-

rithm for quadrotors, that actively controls the system tosatisfy the FOV constraints of the robot on-board sensors,for multiple observed neighbors. This allows for more com-plex and dynamic formation configurations with minimal orno communication requirements. Results show that the sys-tem is stable and that the FOV constraints are satisfied inboth simulation and reality.

As future work, the previously discussed deadlocks willbe investigated, together with the inclusion of goal follow-ing control for formation guidance. These experiments areto be performed with the orthogonal component of the aper-ture control activated. Furthermore, the sensing and controledge bi-directionality assumptions will be relaxed in order toconsider additional formation configurations, paying specialattention to how this might impact the formation stabil-ity. The results reported in this paper also outline that itis relevant to reproduce, in simulation, the real sensing andactuation mismatches, to further study the bias effects. Thisalgorithm can be strengthened if the stability properties arefound for the control of multiple apertures, and if the initialconditions that guarantee the FOV maintenance during thesystem operation are investigated.

1094

REFERENCES[1] F. Augugliaro, A. P. Schoellig, and R. D’Andrea.

Generation of collision-free trajectories for aquadrocopter fleet: A sequential convex programmingapproach. In International Conference on IntelligentRobots and Systems, pages 1917–1922, 2012.

[2] D. Dias, R. Ventura, P. U. Lima, and A. Martinoli.On-board vision-based 3d relative localization systemfor multiple quadrotors. In International Conferenceon Robotics and Automation, 2016 (to appear).

[3] M. Egerestedt. Graph-theoretic methods formulti-agent coordination. In ROBOMAT, 2007.

[4] M. Faessler, E. Mueggler, K. Schwabe, andD. Scaramuzza. A monocular pose estimation systembased on infrared leds. In IEEE InternationalConference on Robotics and Automation, 2014.

[5] R. Falconi, S. Gowal, and A. Martinoli. Graph-baseddistributed control of non-holonomic vehicles endowedwith local positioning information engaged inescorting missions. In International Conference onRobotics and Automation, pages 3207–3214, 2010.

[6] A. Franchi, C. Masone, V. Grabe, M. Ryll,H. Bulthoff, and P. Giordano. Modeling and control ofuav bearing formations with bilateral high-levelsteering. International Journal of Robotics Research,31(12):1504–1525, 2012.

[7] D. Goldin and J. Raisch. Controllability of secondorder leader-follower systems. In IFAC Workshop onDistributed Estimation and Control in NetworkedSystems, pages 223–238, 2010.

[8] M. Ji and M. Egerstedt. Connectedness preservingdistributed coordination control over dynamic graphs.In American Control Conference, pages 93–98, 2005.

[9] N. M. M. Turpin and V. Kumar. Decentralizedformation control with variable shapes for aerialrobots. In IEEE International Conference on Roboticsand Automation, pages 23–30, 2012.

[10] D. Panagou and V. Kumar. Maintaining visibility forleader-follower formations in obstacle environments. InIEEE International Conference on Robotics andAutomation, pages 1811–1816, 2012.

[11] D. Panagou and K. Kyriakopoulos. Cooperativeformation control of underactuated marine vehicles fortarget surveillance under sensing and communicationconstraints. In IEEE International Conference onRobotics and Automation, pages 1871–1876, 2013.

[12] G. Pereira, A. K. Das, V. Kumar, and M. Campos.Formation control with configuration spaceconstraints. In IEEE International Conference onIntelligent Robots and Systems, pages 2755 – 2760,2003.

[13] W. Ren and E. Atkins. Second-order consensusprotocols in multiple vehicle systems with localinteractions. In Guidance, Navigation, and ControlConference, 2005.

[14] R. O. Saber and R. M. Murray. Graph rigidity anddistributed formation stabilization of multi-vehiclesystems. In IEEE International Conference onDecision and Control, pages 2965–2971, 2002.

[15] D. Scaramuzza, M. Achtelik, L. Doitsidis, F. Friedrich,E. Kosmatopoulos, A. Martinelli, M. Achtelik, M. Chl,S. Chatzichristofis, L. Kneip, D. Gurdan, L. Heng,H. Gim, S. Lynen, M. Pollefeys, A. Renzaglia,R. Siegwart, J. Stumpf, P. Tanskanen, C. Troiani,S. Weiss, and L. Meier. Vision-controlled micro flyingrobots: from system design to autonomous navigationand mapping in gps-denied environments. IEEERobotics & Automation Magazine, 21(3):26–40, 2014.

[16] S. Shen, Y. Mulgaonkar, N. Michael, and V. Kumar.Vision-based state estimation and trajectory controltowards high-speed flight with a quadrotor. InRobotics: Science and Systems, 2013.

[17] G. Stacey, R. Mahony, and P. Corke. A bondgraphapproach to formation control using relative statemeasurements. In European Control Conference, pages1262–1267, 2013.

[18] R. Tron, J. Thomas, G. Loianno, J. Polin, V. Kumar,, and K. Daniilidis. Vision-based formation control ofaerial vehicles. In Robotics: Science and Systems,2014.

[19] M. Turpin, N. Michael, and V. Kumar. Trajectorydesign and control for aggressive formation flight withquadrotors. Autonomous Robots, 33(1-2):143–156,2012.

[20] G. Vasarhelyi, C. Viragh, G. Somorjai, N. Tarcai,T. Szorenyi, T. Nepusz, and T. Vicsek. Outdoorflocking and formation flight with autonomous aerialrobots. In International Conference on IntelligentRobots and Systems, pages 3866–3873, 2014.

[21] C. Verginis, C. Bechlioulis, D. Dimarogonas, andK. Kyriakopoulos. Decentralized 2-d control ofvehicular platoons under limited visual feedback. InIEEE International Conference on Intelligent Robotsand Systems, 2015.

1095