Assistive Collision Avoidance for Quadrotor Swarm Teleoperation

Dingjiang Zhou1 and Mac Schwager2

Abstract— This paper presents a method for controllingquadrotor swarms in an environment with obstacles, with anintuitive human-swarm interface operated by a single humanuser. Our method allows for the quadrotor swarm to maintaina desired formation, while also keeping the quadrotors a safedistance from obstacles and from one another. We use a VirtualRigid Body abstraction to provide a bridge between the singlehuman user and the quadrotor swarm, so that a human user canfly an arbitrarily large quadrotor swarm from a single joystick.By applying multiple vector fields, collisions are automaticallyavoided within the swarm of quadrotors, and between thequadrotors and obstacles, while the Virtual Rigid Body iscontrolled by the human user. Our method is demonstrated inhardware experiments with groups of quadrotor micro aerialvehicles teleoperated by a single human operator in a motioncapture system.

I. INTRODUCTION

In this paper, we propose a teleoperation architecture fora single human user to control an arbitrarily large swarm ofquadrotors through a cluttered environment with a standardgaming joystick. The user controls the whole swarm asa single body, while local formation control and collisionavoidance is taken care of autonomously by each robot.Our approach can be useful in security, surveillance, andsearch and rescue applications, in which a human operatormust maneuver a swarm through, e.g., a cluttered building,a forest, or a disaster site. By allowing the quadrotors toautonomously avoid collisions, the human user can focus onthe overall maneuver of the swam, instead of taking careof each quadrotor to avoid the obstacles in the environment.Based on the abstraction of a Virtual Rigid Body proposed inour previous work [1], [2], we implement an intuitive human-swarm interface with a standard gaming joystick to fly aquadrotor swarm from a single human user. Our assistivecollision avoidance algorithm supplements the quadrotorswarm control, so that the maneuver of the swarm is notinterrupted by the obstacles. Our strategy is integrated withdifferential flatness-based control techniques, which havebeen used in the past primarily to design controllers forsingle quadrotors to execute agile trajectories [3], [4].

Our first idea is to control the group of quadrotors asa single body with a standard gaming joystick, in theframework of Virtual Rigid Body (VRB). The VRB allowsus to decouple the trajectory of the whole swarm from thetrajectories of the individual quadrotors within the swarm,

This work was supported in part by NSF grants IIS-1350904 and CNS-1330008, and ONR grant N00014-12-1-1000. We are grateful for thissupport.

1D. Zhou is with the Department of Mechanical Engineering, BostonUniversity, Boston, MA 02215, USA zdj@bu.edu

2M. Schwager is with the Department of Aeronautics andAstronautics, Stanford University, Stanford, CA 94305, USAschwager@stanford.edu

Fig. 1. Two snapshots from our experiment video. A swarm offive KMel Nano Plus quadrotors teleoperated by a gaming joystickin an environment with two obstacles. The “Trapezoid” formation ofthe swarm of five quadrotors distorts itself using vector fields toavoid collisions with the obstacles. The desired “Trapezoid” forma-tion is shown in the upper-right conner. Our video is available athttp://sites.bu.edu/msl/vrb-obstacles/.

and allows us to generate the trajectory of the whole swarmfrom a joystick in real time. As long as the smoothnessrequirement of the trajectory is satisfied in SE(3), eachquadrotor computes its own dynamically feasible SE(3)trajectory based on the differential flatness property of itsdynamics. Our second idea is to let the collision avoidancebe achieved automatically during the flight such that thehuman user will not be overwhelmed by controlling eachsingle quadrotor to avoid colliding to the obstacle, and oneanother. Inspired by our previous work [4], in which we useddifferential flatness based control together with a vector fieldmethod for a single quadrotor to avoid a static obstacle, herewe apply multiple vector fields for the obstacles, and alsofor the quadrotors, in order to keep a desired formation, aswell as to avoid collision with the obstacles.

We demonstrate the effectiveness of our strategy in twosets of hardware experiments with KMel Nano Plus quadro-tors. One experiment is to control a swarm of five quadrotorsin a “Trapezoid” formation in an environment with twoobstacles, and the another is to control the swarm in a “Line”formation in the same environment. In both experiments, thequadrotors are controlled by a Logitech Extreme 3D Projoystick operated by the first author of this paper. Figure1 shows two snapshots of our experiment.

There exists an extensive literature in formation control.For example, [5], [6] proposed a formation control strategy inwhich a leader quadrotor flies an agile trajectory while otherfollower quadrotors control themselves to keep a desiredformation with respect to the leader. In [7] the authorsachieved formation control for a team of quadrotors usingonly onboard vision, without an external positioning system.In [8], control and trajectory generation tools for controllingswarms of micro quadrotors between different formationswere described. Also in [9], the authors displayed 3D featureswith multiple aerial vehicles and shown animation with

a sequence of 3D features. The teleoperation of mobilerobot swarm is considered in [10], [11], where multiplenonholonomic vehicles were controlled in a leader-followerformation and the leader robot was remotely driven to adesired velocity. In [12], the authors used a haptic interface tocontrol the formation of a group of quadrotors. Gesture basedhuman-swarm interaction is seen in [13], in which authorsdeciphered human gestures to drive multiple ground mobilerobots and let the robots show different faces. Obstacle avoid-ance for single quadrotor and multiple quadrotors is alsoheavily researched in the literature. For a single quadrotor,control through a hatpic interface control is demonstrated in[14] where the pilot received force feedback when obstaclesare close, and in [15] the authors proposed an approach toconstruct a novel dynamic kinesthetic boundary (DKB) toaid a pilot to navigate an aerial robotic vehicle through acluttered environment. In [16], the authors provided a methodto assist the operator to fly a quadrotor to perform collisionavoidance by predicting its future trajectory. The paper [17]presents a method to assist the teleoperation of a quadrotorby constructing the nearby environment using sonar sensors.For multi-quadrotor scenarios, the authors of [18] formulateda mixed-integer linear program (MILP) approach to generateoptimal trajectories for the aircrafts to avoid collisions, and in[19], mixed-integer quadratic programming (MIQP) methodwas proposed to generate optimal trajectories for teams ofheterogeneous quadrotors in 3D environment with obstacles.

Our work is different from the above works in two aspects.Firstly, we apply our Virtual Rigid Body abstraction suchthat the human-swarm interface is naturally integrated in ourcontrol. Secondly, our assistive collision avoidance algorithmis applied to the swarm instead of a single quadrotor, andit is implemented experimentally in real time. This makesit directly applicable to a real-world setting (assuming thequadrotors have an adequate perception of their surround-ings).

The remainder of this paper is organized as follows. Thequadrotor dynamics and our Virtual Rigid Body abstraction,are presented in Section II. In Section III, we introduce ourintuitive human-swarm interface which is used to control aquadrotor swarm by a single joystick. Our vector field basedassistive collision avoidance algorithm is then introducedin Section IV. In Section V, we present our experimentswith KMel Nano Plus quadrotors in an environment withtwo obstacles, controlled from a standard gaming joystickby a human user. Finally, we offer our concluding remarksin Section VI.

II. PRELIMINARY

In this section, we give a necessary background on quadro-tor dynamics, and our Virtual Rigid Body abstraction.

A. Quadrotor Dynamics

A quadrotor is well-modeled as a rigid body with forcesand torques applied from the four rotors and gravity [3]. Therelevant forces, moments, and coordinate frames are shownin Figure 2.

Fig. 2. Coordinate frames of a quadrotor. The North-East-Down (NED)global reference frame is denoted by Fw , and the quadrotor body-fixedframe is denoted by Fb.

We define a global reference frame Fw

with a North-East-Down system, and have a body-fixed frame F

b

attachedto the quadrotor, with its origin at the center of mass. Theorientation of the quadrotor is defined by the rotation matrixR, parameterized with ZY X Euler angles, i.e., roll �, pitch✓ and yaw [1], which are also known as TaitBryan angles.The quadrotor dynamics are given by the nonlinear systemof equations

8>>>>><

>>>>>:

˙

v = g +

1

mRf

˙

R = R⌦

˙!b

= J

�1⌧ � J

�1⌦J!

b

˙

p = v,

(1)

(2)(3)(4)

where v = [vx

, vy

, vz

]

T is the velocity in the global frameF

w

, g = [0, 0, g]T is the acceleration due to gravity,m is the mass, and f is the total thrust force generatedfrom the rotors, with f = [0, 0, f

z

]

T , where fz

is thesummation of the four rotors’ thrust, hence f is alignedwith the negative vertical body-fixed direction �z

b

of thebody frame F

b

. The rotation matrix R is defined with Eulerangles �, ✓ and . The angular velocity of the quadrotor,!

b

= [!x

,!y

,!z

]

T , is expressed in the body frame Fb

, and⌦ = !^

b

= [0,�!z

,!y

;!z

, 0,�!x

;�!y

,!x

, 0] is the tensorform of !

b

. The torque generated from the rotors on thequadrotor is given by ⌧ = [⌧

x

, ⌧y

, ⌧z

]

T , expressed in thebody frame F

b

. The inertia matrix of the quadrotor is J,and p = [x, y, z]T is the position of the quadrotor in theglobal frame F

w

. The quadrotor’s inputs are the total thrustand the three torques, µ = [f

z

, ⌧x

, ⌧y

, ⌧z

]

T , which is in fourdimensions, and the system has a 12 dimensional state, i.e.,⇠ = [x, y, z, v

x

, vy

, vz

, , ✓,�,!x

,!y

,!z

]

T .Planning a trajectory to be executed by a quadrotor is

not trivial, as one must find a trajectory that satisfies thedynamics (1)–(4), and find the control input that producesthat trajectory. We call a trajectory that satisfies the dynamicsof a quadrotor for some input signal dynamically feasible,which is defined formally as follows.

Definition 1: (Dynamically Feasible Trajectory) Aquadrotor trajectory ⇠(t) is called dynamically feasible overa time interval [t1, t2] if there exists a control input µ(t)such that ⇠(t) and µ(t) satisfy the equations (1)–(4) for allt1 t t2.

Differential flatness [20] is a property of some nonlin-ear control systems that greatly simplifies the problem offinding dynamically feasible trajectories. If a control systemis differentially flat, its states and inputs can be writtenin terms of a smaller number of, so called, flat outputs,and some number of time derivatives of those outputs [21].Quadrotor dynamics are known to be differentially flat [1],[3]. This is useful in trajectory planning because one canplan an arbitrary trajectory for the flat outputs (as long as itis sufficiently smooth), and analytically find a dynamicallyfeasible quadrotor trajectory, and the control inputs requiredto execute that trajectory. The key tool from differentialflatness is called the endogenous transformation [20]. Theendogenous transformation for quadrotor dynamics can befound in previous work [1], [3], which maps the flat outputs� = [x, y, z, ]T of a quadrotor to its 12 dimensional states⇠ and 4 dimensional control inputs µ.

B. Virtual Rigid BodyIn our previous work [1], [2], we defined the concept of

Virtual Rigid Body, which is a key idea in our approach tocontrol quadrotor swarms.

Consider a swarm of N robots labeled by {1, 2, . . . , N}.Let F

i

denote the local reference frame of robot i, and wedenote by p

i

(t) 2 R3 the position, and R

i

(t) 2 SO(3) theorientation of robot i with respect to the global frame F

w

,at time t.

Definition 2 (Virtual Rigid Body): A Virtual Rigid Body(VRB) is a group of N robots and a local referenceframe F

v

, in which the local positions of the robotsare specified by a set of potentially time-varying vectors{r1(t), r2(t), · · · , rN (t)}.

An example of a Virtual Rigid Body of three robots isshown in Figure 3, in which the formation and transforma-tion are defined as follows.

Definition 3 (Formation): A formation ⇧ is a VirtualRigid Body with constant local positions {r1, r2, · · · , rN}in F

v

for a group of N robots associated with a time periodT⇧ > 0.

Definition 4 (Transformation): A transformation � is aVirtual Rigid Body with time varying local positions{r1(t), r2(t), · · · , rN (t)} in F

v

for a group of N robotsassociated with a time period T� > 0.

Let pv

(t) 2 R3 and R

v

(t) 2 SO(3) denote the positionand orientation of the origin of F

v

in the global referenceframe F

w

at time t, respectively. Since robots’ positions aredefined locally in the Virtual Rigid Body as in Definition 2,the relationship between p

i

(t) and r

i

(t) for robot i isdescribed as

p

i

= p

v

+R

v

r

i

, i = 1, 2, · · · , N, (5)

where “(t)” is dropped for simplicity.We define the trajectory of a Virtual Rigid Body to be the

position and orientation of its local frame Fv

as a functionof time,

�v

(t) = (p

v

(t),Rv

(t)) 2 C4: R�0 7! R3 ⇥ SO(3). (6)

An example of the Virtual Rigid Body trajectory �v

(t) isshown again in Figure 3, in which it is depicted as a black

Fig. 3. The concept of a Virtual Rigid Body with a group of three robots.The VRB is in a formation ⇧m when tm < t tmm+1, and ⇧m+1

when tm+1 < t tm+1m+2, and it is in a transformation �m

m+1 whentmm+1 t tm+1.

dashed line. In correspondence, the trajectory of robot i inthe global frame is denoted by �

i

(t) = (p

i

(t),Ri

(t)) 2 C4.In Figure 3, the trajectories for the three robots are depictedin red, blue and green dashed lines, respectively.

III. INTUITIVE HUMAN INTERFACE

Here we describe the human-swarm interface componentof our control architecture. With our human-swarm interface,the human operator is able to control an arbitrarily largequadrotor swarm as if the swarm were a single aircraft,without be overwhelmed by the excessive number of degreesof freedom of the quadrotor swarm. Our Virtual Rigid Body,which abstracts the quadrotor swarm as a single body,decouples the trajectory generation for a group of robotsinto two subproblems, (i) generating the trajectory of theVirtual Rigid Body in the global reference frame, and (ii)generating the trajectory for each individual quadrotor withinthe VRB local reference frame. Our human-swarm interfacehence solves the first subproblem, by means of generatinga trajectory for the Virtual Rigid Body from a standardjoystick. The second subproblem, however, is solved witha formation library, which stores a set of formations, as wellas transformations between any pair of formations, for theVirtual Rigid Body. To fully appreciate the implementationof a formation library, please refer to our previous work [2].

The joystick gives commands to our base computer, whichinterprets the commands as a state trajectory for the VirtualRigid Body, then the commanded VRB state is broadcast tothe quadrotors, as shown in Figure 4. This control method isintuitive, since the joystick is widely used in flight simulatorsand computer games. The axes of the joystick are interpretedas commanded Euler angles and spatial velocity for the VRB,while the buttons are used to select different formationsfrom the formation library. The goal of this section is todescribe the map between the joystick signal, namely, Eulerangles and thrust (�

J

, ✓J

, J

, fJ

) and their first derivatives(

˙�J

, ˙✓J

, ˙ J

, ˙fJ

), to the Virtual Rigid Body trajectory �v

(t)in real time.

The raw data from the joystick is noisy due to unavoidablyjerky motion from the human hand. To smooth this signal,we apply a first order filter. Notice that the roll �

J

, pitch ✓J

and yaw rate ˙ J

are from the joystick directly, so that the

Fig. 4. The concept of controlling a quadrotor swarm from a standardjoystick. The 4 axes on the joystick are assigned to the three Euler anglesand the thrust, and a few among the 12 buttons are assigned to differentformations.

roll rate ˙�J

and the pitch rate ˙✓J

are obtained by numericaldifferentiation from �

J

and ✓J

, while the yaw J

is obtainedby numerical integration from ˙

J

. Also, ¨ J

is obtainedby numerical differentiation from ˙

J

. Given these filteredjoystick commands, we must find a VRB trajectory that theuser intends.

We impose virtual dynamics on the Virtual Rigid Body tomap the joystick commands to a VRB trajectory. The VRBdynamics can be as simple as possible, since there is noinherent physical dynamic constraints on the VRB. In thispaper, we let the VRB have single integrator dynamics,

˙

x

v

= �, (7)

where the state is x

v

= [p

T

v

, v

]

T . Note that roll �v

andpitch ✓

v

of the VRB are not modeled since we set them tozero. With this model, the VRB trajectory �

v

(t) defined in(6) is simplified as the position trajectory p

v

(t) with the yawtrajectory

v

(t).We apply again a first order filter to the input � in (7),

˙� = �a� + u1, (8)

where a is a positive parameter, and the input u1 is from thejoystick by

u1 =

2

664

e1✓Je1�Je2fJe3 ˙

J

3

775 , (9)

in real time, where e1, e2 and e3 are positive constant scalingfactors, such that e1 and e2 scale the spatial speed, and e3scales the yaw rate of the Virtual Rigid Body. The threescaling factors, along with the positive parameter a in (8),can be used as four variables for tuning the aggressivenessof the maneuver. These parameters should be tuned so as toavoid actuator saturation.

From (7) to (9), we obtain the VRB position trajectoryp

v

(t), yaw angle trajectory v

(t), as well as its velocityv

v

(t). However, we require that the Virtual Rigid Body to befour times continuously differentiable (in C4) in order to usedifferential flatness based control tools. Therefore, the VRBacceleration ˙

v

v

(t) ⌘ ¨

p

v

(t), jerk ¨

v

v

(t) ⌘...p

v

(t), and snap...v

v

(t) ⌘....p

v

(t) must be found from the joystick commands.

Similar to (8), we apply a first order filter to the accelerationof the VRB,

˙

a

v

= �aav

+ u2, (10)

where the input u2 are from the joystick by

u2 =

2

4e1 ˙✓Je1 ˙�Je2 ˙f

J

3

5 .

Finally, we obtain jerk and snap directly by(j

v

=

˙

a

v

s

v

=

˙

j

v

. (11)

In conclusion, (7)–(11) fully define the trajectory �v

(t) forthe Virtual Rigid Body. With the VRB trajectory �

v

(t), andthe formation or transformation of the VRB from a formationlibrary, one can apply the methods in our previous work [1]to find the trajectory for each quadrotor.

IV. ASSISTIVE COLLISION AVOIDANCE

In this section, we introduce the collision avoidancecomponent of our control architecture, which supplementsthe previous human-swarm interface to assist the humanuser by autonomously avoiding collisions with obstacles.Using multiple interacting vector fields, which is similar toa potential field, the algorithm allows for the formation todeform enough to avoid collisions with obstacles, and relaxback into the formation when far away from obstacles. Thisbehavior can be seen in our simulation in Section IV-D andour experiments in Section V. One possible draw back ofpotential field-based methods is that the deadlocks may occur[10], [11]. However, since the swarm is teleoperated by ahuman user in our case, this can be less of a problem. Theuser can see when a deadlock is occurring, and reactivelymaneuver the swarm to dislodge the quadrotors that are stuckin a local minimum.

Our algorithm for collision avoidance is applied to en-vironments like the scenario shown in Figure 5, in whicha swarm of five quadrotors is under control from a humanuser in an environment with two obstacles. The obstacles aremodeled as cylinders with different radii. Since we focus onformation control and collision avoidance, we assume thatthe robots sense their local environment precisely.

In our strategy, the obstacles affect the maneuver of thequadrotor swarm in two different ways. First, the obsta-cles influence the commanded velocity v

v

, as well as thecommanded acceleration a

v

of the VRB through a vectorfield, such that the VRB will slow down or change itsheading direction when there is an obstacle in its path.Second, the obstacles influence the individual quadrotorsthat are close to it, also through vector fields, that eachindividual quadrotor takes its own action to deviate from itsoriginal heading direction in order to avoid collision to theobstacles. Additionally, to avoid inter-vehicle collision, eachquadrotor considers all the other quadrotors as dynamicalobstacles with repelling vector fields. Meanwhile, the desiredformation is recovered after the swarm passes through theobstacle area by formation control algorithm.

���

�

0

0.5

1

1.5

2

� ���3 3.5 4 4.5 5

�

Obs. 1

��

�

VRB

�

��

��

��

��

�

Obs. 2

��

���

Fig. 5. Top view of a scenario with two obstacles in the environment.The obstacles are plotted as solid black disks. The magenta lines show theconnectivity of the quadrotor swarm. The orange arrowhead shape in themiddle denotes the VRB of our quadrotor swarm, and the vv depicted by athick blue arrow is the commanded velocity of the VRB from the joystick.The velocity of all quadrotors are denoted as v1 to v5, respectively. Thevelocities are different from the VRB velocity vv , since the VRB is inrotation about the z axis of Fv .

Our control algorithm for collision avoidance runs in realtime and is distributed among the quadrotor swarm, withthe exception of the component that acts on the VRB, asdescribed below. For the scenario as shown in Figure 5, weapply the vector field method from our previous work [4],in which it was applied for a single quadrotor to avoid astatic “virtual” obstacle in the environment. With runningthis control algorithm, the dynamical feasible trajectory ofeach quadrotor described in Definition 1, and the smoothnessrequirement of the states and inputs, can be satisfied. Asystem diagram of our control architecture is shown in Figure6.

Fig. 6. System diagram of controlling a quadrotor swarm with collisionavoidance. The human user controls the maneuver of the Virtual RigidBody with a joystick, and selects a desired formation from the formationlibrary. The obstacle avoidance algorithm affects the VRB, and as well aseach individual quadrotor in the VRB. The interaction between each pairof neighboring quadrotors avoids inter-vehicle collision, and the formationalgorithm keeps the desired formation for the quadrotor swarm.

A. Formation Vector Fields

In our Virtual Rigid Body framework, we apply a vectorfield for each quadrotor in the VRB local frame F

v

, suchthat when the quadrotor deviates from its desired position,

a forcing vector acts on the quadrotor to pull it back to thedesired position in the formation. Here we take advantageof our VRB abstraction, so that instead of considering themaneuver of the quadrotors in the global frame, we onlyconsider their maneuver in the VRB local frame.

Let the desired position of quadrotor i in the VRB localframe F

v

be denoted as rdi

, then the vector in the vector fieldat position r for quadrotor i is defined as

V0,i(r) = A(r

d

i

� r), i = 1, 2, · · · , N, (12)

where A is a positive constant parameter. As long as thequadrotor can follow its vector field, it will eventuallyconverge to its desired position in the VRB local frame, whenthe environment is obstacle free. On the other hand, whenthe desired formation is feasible, this vector field will alsokeep that desired formation.

An example of a 2D version of this vector field for eachquadrotor is shown in Figure 7, in which different colordenotes the vector field for different quadrotor. Note thatall five vector fields actually act on the quadrotor at once.Another example of this vector field for quadrotor 1 is shownin Figure 9, plotted in gray arrows.

Fig. 7. Formation vector fields for quadrotor 1, 4 and 5 in the VRB localframe Fv .

B. Vector Fields from ObstaclesAs illustrated in Figure 6, our collision avoidance algo-

rithm works at two levels: The first level is that we generatea vector field to force the Virtual Rigid Body to changeits commanded velocity v

v

to decrease the aggressivenessof the VRB, and the second level is that we generateanother vector field for each obstacle to guarantee collisionavoidance for each quadrotor, while the VRB is still trackingits trajectory. The two levels can be unequally responsible inachieving collision avoidance. For example, one can increasethe influence of the first level and hence decrease that of thesecond level, such that the entire swarm can be far away fromthe obstacles while keeping a desired formation. However,we choose to let the second level to be more responsiblefor collision avoidance. By doing so, collision avoidance isguaranteed for the quadrotor swarm, while maintaining thedesired formation is a secondary priority.

In this paper, for any obstacle, we consider a boundingcylinder that encompasses the obstacle in order to generatea repelling vector field. Figure 8 shows a plot of our repellingvector field for the obstacles in the scenario of Figure 5.

(a) Vector field for the VRB. (b) Vector field for the quadrotors.

Fig. 8. The two vector fields for the VRB and quadrotors, respectively.Each vector field is generated from (13), or (17), with different parameters.The darker color means larger magnitude and the lighter color means smallermagnitude. Intuitively, the VRB, or each single quadrotor, will be pushedtoward the light region and avoid the dark region.

1) Vector Field for the Virtual Rigid Body: We assumethat the quadrotor swarm maneuvers in the horizontal planeonly, so that climbing to a higher altitude to avoid theobstacles is not an option in our setting. For the VirtualRigid Body, and for a single obstacle, we define the repellingvector from the obstacle at horizontal coordinate ¯

p to be avector pointing in the direction from the obstacle to ¯

p, withits magnitude to be a Gaussian-like function related to theposition and radius of this obstacle, as well as the VRBheading direction. Assuming that there are m obstacles inthe environment, and we denote the horizontal position ofobstacle k in the global frame F

w

as ¯

o

w,k

, then the overallrepelling vector in the vector fields generated from the mobstacles at horizontal coordinate ¯

p is

V1(¯p) =

mX

k=1

fk

(

¯

o

w,k

, rk

, ˆv)¯

p� ¯

o

w,k

k¯p� ¯

o

w,k

k , (13)

where the magnitude fk

(

¯

o

w,k

, rk

, ˆv) is a function of theobstacle position ¯

o

w,k

, radius rk

and the VRB headingdirection ˆ

v = v

v

/kvv

k, described as

fk

(

¯

o

w,k

, rk

, ˆv) =

Bk

exp

⇣� (

¯

p� ¯

o

w,k

)

T

⌃

�1(

¯

p� ¯

o

w,k

)

⌘, (14)

where Bk

is a positive scalar parameter related to the radiusrk

of obstacle k. The value of Bk

can be chosen so that thecommanded velocity v

v

for the VRB can still overcome thelargest repelling vector, since the VRB is virtual. The 2⇥ 2

matrix ⌃ is positive definite and defines the major and minoraxes of the dynamic Gaussian-like function by

⌃

�1=

⇥ˆ

v, ˆv?⇤"

12�2

10

0

12�2

2

#⇥ˆ

v, ˆv?⇤T , (15)

where ˆ

v

? is the perpendicular unit vector of ˆ

v, and �1 and�2 are the covariance variables. From (15) and from Figure8(a), one can see that the VRB is repelled more from theobstacle if the VRB is heading towards the obstacle directly,than if it is passing by the obstacles from the side.

The vector field V1 is shown in Figure 8(a).2) Vector Field for Quadrotors: In contrast to the vector

field for the VRB, we need to design a stronger vector fieldfor the individual quadrotors, such that when a quadrotorgets too close to an obstacle, the repelling vector is powerfulenough to “push” the quadrotor away from the obstacle,

V2(¯p) =

mX

k=1

✓C

k

exp

⇣� 1

2�23

(

¯

p� ¯

o

w,k

)

T

(

¯

p� ¯

o

w,k

)

⌘·

¯

p� ¯

o

w,k

k¯p� ¯

o

w,k

k

◆(16)

=

mX

k=1

✓C

k

exp

⇣� 1

2�23

k¯p� ¯

o

w,k

k2⌘◆

¯

p� ¯

o

w,k

k¯p� ¯

o

w,k

k ,

(17)

where Ck

is again a positive scalar parameter related to theradius of obstacle k. The value of C

k

must be chosen largeenough to guarantee collision avoidance.

The vector field V2 is shown in Figure 8(b). Equation(16) and (17) are expressed in the global frame, while inour simulation of collision avoidance in Section IV-D, weexpress them in the VRB local frame by

V2(¯r) =

mX

k=1

✓C

k

exp

⇣� 1

2�23

k¯r� ¯

o

v,k

k2⌘◆

¯

r� ¯

o

v,k

k¯r� ¯

o

v,k

k , (18)

where ¯

r denotes the horizontal coordinate in Fv

, and ¯

o

v,k

isthe horizontal position of obstacle k in F

v

. Equation (18) isobvious since k¯p� ¯

o

w,k

k = k¯r� ¯

o

v,k

k.The magnitude of the vector V2 in the vector field of

obstacle 1 in the VRB local frame can be seen in Figure 9.

C. Vector Fields from QuadrotorsHere we also consider inter-vehicle collision avoidance.

With the two vector fields described in Section IV-B, weguarantee that the obstacle avoidance is achieved, as seenfrom our experiment in Section V. To ensure inter-vehiclecollision avoidance, we again apply the vector field methodto each quadrotor in the VRB local frame F

v

. Similar to(18), we have

V3,i(r) =

X

j2Ni

✓D exp

⇣� 1

2�24

kr� r

j

k2⌘◆

r� r

j

kr� r

j

k , (19)

where i = 1, 2, · · · , N , and Ni

denotes the neighbors ofquadrotor i. The scale parameter D and covariance �4 canbe tuned so that the vector field from each quadrotor will noteffect the other quadrotors when the swarm is in a desiredformation. The magnitude of the vectors of a 2D version ofthis vector field for quadrotor 3 can be seen in Figure 9.

D. Collision Avoidance Simulation in VRB Local FrameIn previous sections, we defined vector fields to prevent

collisions with the obstacles, and with other quadrotors, ineither the global frame F

w

, or the VRB local frame Fv

. Inthis section, we show the effectiveness of our vector fields ina simulation of a quadrotor swarm with collision avoidance.

By taking the advantage of our Virtual Rigid Body abstrac-tion, instead of considering the static or dynamic obstacles inthe global reference frame F

w

in which the quadrotors aremaneuvering, we consider dynamic obstacles in the VRBlocal frame. The global maneuver of each quadrotor is fusedfrom the local maneuver of each quadrotor in F

v

and theglobal maneuver of the Virtual Rigid Body in F

w

[1]. Whencollision avoidance is achieved in the VRB local frame, it isautomatically achieved in the global frame for the quadrotorswarm.

Figure 9 shows a sequence of snapshots in our simulationstarting from the scenario shown in Figure 5, in the VRBlocal frame F

v

. Because the scenario is shown in the VRBlocal frame, only the obstacles and quadrotors appear tomove, while the VRB center appears to be fixed. For thissimulation, we pre-planned the trajectories for the obstacles,as plotted by gray lines. These trajectories are designed tobe smooth, although they can be more complicated in thereal-world.

(a) Time = 0 s. (b) Time = 4.4 s.

(c) Time = 9.0 s. (d) Time = 11.8 s.

Fig. 9. A sequence of snapshots in a simulation of collision avoidance fora quadrotor swarm, shown in the VRB local frame Fv . The velocities ofthe obstacles are plotted with thick blue arrows, and the velocities of thequadrotors are plotted with thin blue and thick red arrows (the red arrowis a summation of the thin blue arrows). The vector field generated fromobstacle 1 is indicated by a gray shadow, and the vector field from quadrotor3 is indicated by a red shadow. Both vector fields move in Fv with obstacle1 and quadrotor 3, respectively. The formation vector field for quadrotor 1 isplotted with gray arrows, and it is static in Fv , since its purpose is to “pull”quadrotor 1 to a desired position. The figures are available from our simu-lation video at http://sites.bu.edu/msl/vrb-obstacles/.

V. EXPERIMENTS

Our assistive collision avoidance algorithm for the teleop-erated quadrotor swarm is here validated experimentally with

a group of KMel Nano Plus quadrotors in an environmentwith two obstacles, as shown in Figure 1. The radii of theobstacles are r1 = 0.15m and r2 = 0.11m, respectively. Theparameters for the vector fields described in Section IV arelisted in Table I. These parameters are obtained from tuningin the simulation and experiments.

TABLE IVECTOR FIELDS PARAMETERS FOR EXPERIMENTS

Value Description.A 0.70 Magnitude of formation vector field to each quadrotor.B1 0.018 Magnitude of vector field of obstacle 1 to the VRB.B2 0.0132 Magnitude of vector field of obstacle 2 to the VRB.�1 0.50 Covariance of obstacle vector fields to the VRB.�2 0.25 Covariance of obstacle vector fields to the VRB.C1 6.00 Magnitude of vector field of obstacle 1 to quadrotors.C2 4.40 Magnitude of vector field of obstacle 2 to quadrotors.�3 0.25 Covariance of obstacle vector fields to quadrotors.D 1.20 Magnitude of vector fields from quadrotors.�4 0.14 Covariance of vector fields from quadrotors.

In the experiment, position and orientation for the quadro-tors are obtained with an OptiTrack motion capture systemrunning at 120Hz. Our experiments share the same code basewith our previous work [1], [2], [4] to fly the quadrotors.Once again, a video of our experiments is available athttp://sites.bu.edu/msl/vrb-obstacles/.

A. Experiment I: Trapezoid FormationOur first experiment is to control a swarm of five quadro-

tors flying a “Trapezoid” formation in the experimental arenawith the two cylinder obstacles. The “Trapezoid” formationis designed in our formation library. The human user canchoose the formation by pressing a button on the joystick.A six-frame sequence of this experiment is shown in Figure10, but of course, the motion of the quadrotors is difficultto appreciate from the still frames, please refer to ourexperiment video.

B. Experiment II: Line FormationThe second experiment is the same with experiment I,

except that the VRB has a transformation from a “Trapezoid”formation to a “Line” formation, and we control the quadro-tor swarm in the “Line” formation through the obstacle area.Again, a few snapshots of the experiment video are shownin Figure 11.

VI. CONCLUSION REMARKS

In this paper, we proposed a method for a human user toteleoperate a swarm of quadrotors from a standard joystick,while the quadrotors autonomously avoid collisions withobstacles and with each other. The intuitive human-swarminterface allows a single human user to operate an arbitrarilylarge quadrotor swarm. Using the framework of a VirtualRigid Body, we successfully apply multiple vector fieldsamong the quadrotor swarm, in both the global referenceframe and the VRB local reference frame to avoid collisionsby compliantly deforming the formation. Our algorithmassists the human user in flying quadrotor swarms throughcluttered environments. The assistive collision avoidancemethod is validated in our experiments with a group of KMel

Fig. 10. A sequence of snapshots of experiment I. (a), The quadrotorswarm is initialized as a “Trapezoid” formation; (b), The quadrotor swarmsignificantly deforms while passing through obstacle 2 from two sides; (c),The corner of the “Trapezoid” formation is squeezed by obstacle 2; (d),The top quadrotor deviates from its desired position considerably; (e), Onequadrotor comes close to obstacle 1, but with no collision; (f), The quadrotorswarm recovers its “Trapezoid” formation.

Fig. 11. A sequence of snapshots of experiment II. (a), The quadrotorswarm is initialized as a “Trapezoid” formation; (b), The VRB is in atransformation from the “Trapezoid” formation to a “Line” formation; (c),The “Line” formation is deformed by obstacle 1; (d), The quadrotor swamhas successfully passed through the two obstacles; (e), The quadrotors are ina “Line” formation again; (f), the quadrotor swarm is significantly deformedby obstacle 2.

Nano Plus quadrotors in a motion capture system. In thefuture, we plan to adapt this method to work with on-boardperception of the obstacles.

REFERENCES

[1] D. Zhou and M. Schwager, “Virtual rigid bodies for coordinated agilemaneuvering of teams of micro aerial vehicles,” in Robotics andAutomation (ICRA), 2015 IEEE International Conference on. IEEE,2015, pp. 1737–1742.

[2] ——, “Virtual rigid bodies for agile coordination of quadrotor swarmsand human-swarm teleoperation,” in IEEE Transactions on Robotics(Under Review), 2015.

[3] D. Mellinger and V. Kumar, “Minimum snap trajectory generationand control for quadrotors,” in Robotics and Automation (ICRA), 2011IEEE International Conference on. IEEE, 2011, pp. 2520–2525.

[4] D. Zhou and M. Schwager, “Vector field following for quadrotorsusing differential flatness,” in Robotics and Automation (ICRA), 2014IEEE International Conference on. IEEE, 2014, pp. 6567–6572.

[5] M. Turpin, N. Michael, and V. Kumar, “Capt: Concurrent assignmentand planning of trajectories for multiple robots,” The InternationalJournal of Robotics Research, vol. 33, no. 1, pp. 98–112, 2014.

[6] ——, “Trajectory design and control for aggressive formation flightwith quadrotors,” Autonomous Robots, vol. 33, no. 1-2, pp. 143–156,2012.

[7] E. Montijano, E. Cristofalo, D. Zhou, M. Schwager, and C. Sagues,“Vision-based distributed formation control without an external po-sitioning system,” IEEE Transactions on Robotics (Under Review),2015.

[8] A. Kushleyev, D. Mellinger, C. Powers, and V. Kumar, “Towards aswarm of agile micro quadrotors,” Autonomous Robots, vol. 35, no. 4,pp. 287–300, 2013.

[9] J. Alonso-Mora, M. Schoch, A. Breitenmoser, R. Siegwart, andP. Beardsley, “Object and animation display with multiple aerialvehicles,” in Intelligent Robots and Systems (IROS), 2012 IEEE/RSJInternational Conference on. IEEE, 2012, pp. 1078–1083.

[10] S. Mastellone, D. M. Stipanovic, and M. W. Spong, “Remote formationcontrol and collision avoidance for multi-agent nonholonomic sys-tems,” in Robotics and Automation (ICRA), 2007 IEEE InternationalConference on. IEEE, 2007, pp. 1063–1067.

[11] S. Mastellone, D. M. Stipanovic, C. R. Graunke, K. A. Intlekofer,and M. W. Spong, “Formation control and collision avoidance formulti-agent non-holonomic systems: Theory and experiments,” TheInternational Journal of Robotics Research, vol. 27, no. 1, pp. 107–126, 2008.

[12] A. Franchi, C. Masone, V. Grabe, M. Ryll, H. H. Bulthoff, andP. R. Giordano, “Modeling and control of uav bearing-formations withbilateral high-level steering,” The International Journal of RoboticsResearch, vol. 31, no. 12, pp. 1504–1525, 2012.

[13] J. Alonso-Mora, S. Haegeli Lohaus, P. Leemann, R. Siegwart, andP. Beardsley, “Gesture based human-multi-robot swarm interaction andits application to an interactive display,” in Robotics and Automation(ICRA), 2015 IEEE International Conference on. IEEE, 2015, pp.5948–5953.

[14] A. M. Brandt and M. B. Colton, “Haptic collision avoidance for aremotely operated quadrotor uav in indoor environments,” in SystemsMan and Cybernetics (SMC), 2010 IEEE International Conference on.IEEE, 2010, pp. 2724–2731.

[15] X. Hou and R. Mahony, “Dynamic kinesthetic boundary for hapticteleoperation of aerial robotic vehicles,” in Intelligent Robots andSystems (IROS), 2013 IEEE/RSJ International Conference on. IEEE,2013, pp. 4945–4950.

[16] J. Israelsen, M. Beall, D. Bareiss, D. Stuart, E. Keeney, and J. van denBerg, “Automatic collision avoidance for manually tele-operated un-manned aerial vehicles,” in Robotics and Automation (ICRA), 2014IEEE International Conference on. IEEE, 2014, pp. 6638–6643.

[17] J. Mendes and R. Ventura, “Assisted teleoperation of quadcoptersusing obstacle avoidance,” Journal of Automation Mobile Roboticsand Intelligent Systems, vol. 7, no. 1, pp. 54–58, 2013.

[18] A. Richards and J. P. How, “Aircraft trajectory planning with collisionavoidance using mixed integer linear programming,” in AmericanControl Conference, 2002. Proceedings of the 2002, vol. 3. IEEE,pp. 1936–1941.

[19] D. Mellinger, A. Kushleyev, and V. Kumar, “Mixed-integer quadraticprogram trajectory generation for heterogeneous quadrotor teams,” inRobotics and Automation (ICRA), 2012 IEEE International Conferenceon. IEEE, 2012, pp. 477–483.

[20] P. Martin, P. Rouchon, R. Murray et al., “Flat systems, equivalenceand trajectory generation. 3eme cycle,” Castro Urdiales (Espagne),p. 81, 2006.

[21] R. M. Murray, “Optimization-based control,” California Institute ofTechnology, CA, 2009.