Article

The International Journal of

Robotics Research
Multi-robot formation control and object 2017, Vol. 36(9) 1000–1021
© The Author(s) 2017
transport in dynamic environments via
constrained optimization Reprints and permissions:
sagepub.co.uk/journalsPermissions.nav
DOI: 10.1177/0278364917719333
journals.sagepub.com/home/ijr

Javier Alonso-Mora1,2 , Stuart Baker1 and Daniela Rus1

Abstract
We present a constrained optimization method for multi-robot formation control in dynamic environments, where the
robots adjust the parameters of the formation, such as size and three-dimensional orientation, to avoid collisions with
static and moving obstacles, and to make progress towards their goal. We describe two variants of the algorithm, one for
local motion planning and one for global path planning. The local planner first computes a large obstacle-free convex
region in a neighborhood of the robots, embedded in position-time space. Then, the parameters of the formation are
optimized therein by solving a constrained optimization, via sequential convex programming. The robots navigate towards
the optimized formation with individual controllers that account for their dynamics. The idea is extended to global path
planning by sampling convex regions in free position space and connecting them if a transition in formation is possible
- computed via the constrained optimization. The path of lowest cost to the goal is then found via graph search. The
method applies to ground and aerial vehicles navigating in two- and three-dimensional environments among static and
dynamic obstacles, allows for reconfiguration, and is efficient and scalable with the number of robots. In particular, we
consider two applications, a team of aerial vehicles navigating in formation, and a small team of mobile manipulators
that collaboratively carry an object. The approach is verified in experiments with a team of three mobile manipulators and
in simulations with a team of up to sixteen Micro Air Vehicles (quadrotors).

Keywords
Multi-robot systems, motion planning, formation control, constrained optimization, sequential convex programming, team
of aerial vehicles, micro air vehicles, collaborative mobile manipulators, collaborative object transport

1. Introduction among static and dynamic obstacles. These techniques pro-

vide good computational efficiency, local guarantees, and
Multi-robot teams can be employed for various tasks, such
generality. Leveraging these tools, we introduce two cen-
as surveillance, inspection, and automated factories. In
tralized algorithms - a local motion planner and a global
these scenarios, robots may be required to navigate in for-
path planner - that enable a team of robots to navigate
mation, for example, to maintain a communication network,
in formation in two-dimensional and three-dimensional
to collaboratively manipulate an object, or to survey an area.
environments with static and dynamic obstacles.
In this work we consider two motivating applications: for-
Given a set of target formation shapes, which serve as
mation flight for teams of unmanned aerial vehicles (UAVs)
abstractions, our method optimizes the parameters (such as
in tight spaces with static and moving obstacles, and collab-
position, orientation, and size) of the multi-robot formation
orative transport of large objects by multiple mobile manip-
to avoid moving obstacles and make progress towards the
ulators in automated factories and working side by side with
goal. For local motion planning, an obstacle-free region,
humans and other robots.
Within the field of multi-robot navigation, formation 1 Computer Science and Artificial Intelligence Laboratory, Massachusetts
control and reconfiguration in three-dimensional dynamic Institute of Technology, USA
environments with moving obstacles remains challeng- 2 Delft Center for Systems and Control, Delft University of Technology,

ing. In this work, we leverage efficient optimization tech- Netherlands

niques, namely quadratic programming, semi-definite pro-
Corresponding author:
gramming, and (nonlinear) sequential quadratic program-
Javier Alonso–Mora, Delft University of Technology, Mekelweg 2, 2628
ming to address this issue. Each one is employed at dif- CD Delft, Netherlands.
ferent stages of the proposed method for formation control Email: [email protected]
Alonso-Mora et al. 1001

Fig. 1. Two mobile manipulators collaboratively carry a rigid Fig. 2. A triangular formation with sixteen aerial robots can be
object. A projection of the obstacle-free convex region is super- abstracted by a triangle defined by three vertices. The formation
imposed in green. can also be defined in three-dimensional space.

embedded in position-time space, is first grown in a neigh- formation to avoid collisions. Furthermore, the formation
borhood of the robots, and then the parameters of the forma- control method scales well with the number of robots, since
tion are optimized, via a constrained optimization, to remain its complexity is independent from the number of robots in
within this region. The formation optimization method the team (see Figure 2 for an example of the abstraction of
guarantees that the team of robots remains collision-free the formation by its outer vertices). Finally, we validate the
and makes progress towards the goal. To make global approach in simulations with teams of aerial vehicles and
progress towards a goal configuration, we also present a in extensive experimental demonstrations with three mobile
global path planner which builds a graph of feasible forma- manipulators carrying a rigid object.
tions in the environment. The graph is created by a random In an earlier conference version of this work (Alonso–
sampling of convex regions in free space, which are kept Mora et al., 2015a), the local motion planner was intro-
if a valid formation exists within. A human may also pro- duced. In this paper, we describe the approach in detail, we
vide the global path for the robots, or a desired velocity for extend it for global path planning, and we present additional
the formation, and the robots will adapt their configuration experiments with a team of three mobile manipulators
automatically. An example of the method for mobile manip- collaboratively carrying an object.
ulators is shown in Figure 1. A video illustrating the results The geometrical and optimization ideas of the central-
of this paper is available at https://youtu.be/sDNqdEPA7pE. ized method of this paper can be combined with consensus
for distributed formation control. Recently, we presented an
1.1. Contribution extension (Alonso–Mora et al., 2016) to the case where the
robots have a reduced communication and visibility range
The main contribution of this paper is a scalable and effi- and share information with their neighbors.
cient method for navigation of a team of robots while recon-
figuring their formation to avoid collisions with static and
dynamic obstacles. The method applies to robots navigating 1.2. Related works
in 2D and 3D workspaces and contributes the following. In the following we provide an overview of the related
literature. In particular, we distinguish between methods
1. Locally optimal formation control. The parameters of for global path planning, which are typically off-line, and
the group formation are optimized online within the online methods for local motion planning and control.
neighborhood of the robots via a centralized sequen-
tial convex optimization with avoidance constraints in
dynamic environments. 1.2.1. Global path planning. Deadlock-free navigation in
2. A global path planner for navigating in formation. A complex, yet static, environments can be achieved by com-
sampling based graph-search algorithm where convex puting a global path from the initial configuration to the
regions in free space are sampled and connected if goal configuration and a set of intermediate collision-
the intersections are traversable in formation. Sampling free configurations for the team of robots. For example,
and nonlinear optimization are combined to find a safe Kushleyev et al. (2012) coined the problem as a mixedin-
global path. teger quadratic optimization and Saha et al. (2014) relied
on discretized linear temporal logic. Both methods pro-
This work provides a working solution to a difficult prob- vide global guarantees, but scale poorly with the number of
lem that has not been treated at this scale before. The robots and do not consider arbitrary formation definitions,
main strength is its ability to handle dynamic obstacles in instead they rely on squared formations.
three-dimensional environments via constrained optimiza- An alternative is to randomly sample configurations in
tion, which automatically computes the parameters of the state-space to compute a set of safe configurations defining
1002 The International Journal of Robotics Research 36(9)

the path for the team of robots. Barfoot and Clark (2004) review on this topic we refer the reader to Chen and Wang
computed a global path for the formation via Probabilis- (2005). In our work we do not intend to maintain a specific
tic Roadmaps (PRM) by considering a circle enclosing the formation, but instead to adjust its parameters to achieve
formation and a leader. Krontiris et al. (2012) later com- collision-free navigation in dynamic environments.
puted a PRM directly for the formation, considering its Many methods have been proposed for formation con-
real shape and a set of templates. Our global path plan- trol in obstacle-free environments. Balch and Arkin (1998)
ner is also sampling-based, yet it differs from PRM, or pure employed a set of reactive behaviors. Other reactive
sampling-based strategies, in that we compute both feasible approaches include potential fields (Olfati-Saber and Mur-
formations and traversable areas in free space, which we ray, 2002; Sabattini et al., 2011), and flocking (Dimarogo-
then use to focus the sampling in unexplored regions of the nas and Johansson, 2008; Tanner et al., 2007). Two alterna-
workspace. tives to reactive approaches are to use navigation functions
The idea of computing convex regions in free space (Michael et al., 2008) and to synthesize controllers (Hsieh
presents similarities with early work on cell decomposition et al., 2008). Other approaches exist, Desai et al. (2001)
(Latombe, 1991; LaValle, 2006). One way to decompose combined decentralized feedback laws with graph theory,
the workspace into cells is to triangulate the free space. Zhou and Schwager (2015) considered rigid formations,
Conner et al. (2003) and Kallem et al. (2011) used such and Cheah et al. (2009) defined a formation via coverage.
a triangulation to synthesize controllers for single robot Shape stabilization in obstaclefree environments has also
navigation in planar environments, Ayanian et al. (2011) been analyzed by Fredslund and Mataric (2002), Fax and
combined a triangulation of the environment with naviga- Murray (2004), and Cortés (2009).
tion functions to achieve multi-robot control, and Derenick Several of these approaches were also extended to pla-
and Spletzer (2007) combined a triangulation of the pla- nar environments with static obstacles. For example, social
nar environment with second order cone programming to potentials were used by Balch and Hybinette (2000), con-
compute a feasible path for a circular formation. Yet, these trol of rigid body formations by Egerstedt and Hu (2001),
methods are limited to planar environments. abstractions to enclosing shape by Belta and Kumar (2004)
We do not compute a typical cell decomposition of the and by Michael and Kumar (2008), local planning in for-
environment, but instead rely on intersections of large con- mation space by Kloder and Hutchinson (2006), and con-
vex regions to guarantee collision-free navigation in forma- troller synthesis by Ayanian et al. (2009). Our method is
tion and reconfiguration for the team of robots. Our method conceptually similar to Belta and Kumar (2004) in that
builds on the work by Deits and Tedrake (2015), where con- we also employ an abstraction of the formation, whose
vex polytopes were used to compute trajectories for single dimension is independent of the number of robots. Yet,
quadrotors. Our approach for global path planning com- we do not synthesize controllers, but instead formulate a
bines sampling-based and constrained optimization tech- constrained optimization to compute the parameters of a
niques to explore the large configuration space. In partic- general formation of arbitrary shape. In contrast to these
ular, we sample overlapping convex regions in free position frameworks, which were limited to planar workspaces, our
space and rely on a nonlinear constrained optimization to method achieves collision-free motion and reconfigura-
compute the configuration of the robots that can occupy tion in planar and three-dimensional dynamic environments
those spaces. with moving obstacles, and therefore it applies to teams of
To handle moving obstacles online, we require a local aerial vehicles.
motion planner which utilizes the global path for guidance We formulate the problem as a constrained optimization,
and incorporates local modifications. In our local motion which can be solved online via tools for Sequential Con-
planner, like the global planner, we employ large convex vex Programming (SCP). Constrained optimization, and
regions, embedded in position-time space and computed in in particular semidefinite programming, was employed by
the neighborhood of the robots. The local planner computes Derenick et al. (2010) for navigating a team of robots in
safe motions for the team of robots in three-dimensional environments with circular obstacles, yet limited to robots
dynamic environments within this convex region, and is the moving on the plane. Sequential Convex Programming has
main contribution of this work. been recently employed by Augugliaro et al. (2012) and
Chen et al. (2015) to compute collision-free trajectories for
1.2.2. Local motion planning. A large part of formation multiple UAVs, although they did not consider formation
control literature is devoted to the problem of maintaining control. Morgan et al. (2016) combined goal assignment
a formation while respecting the kinematic and dynamic with sequential convex programming to optimize the tra-
constraints of the robots. Examples of typical approaches jectory for a team of robots to reach a target formation, but
for formation control include Lyapunov functions (Ogren was limited to obstacle-free environments.
et al., 2001), model predictive control (Dunbar and Murray, For efficiency, we abstract the robot dynamics when com-
2002), flocking (Dimarogonas and Kyriakopoulos, 2005) puting the parameters of the formation, but include them
and leader-follower control (Ren and Sorensen, 2008), each in the individual robot controllers. This abstraction is like
one with its own advantages and disadvantages. For a short that of our work on pattern formation for animation display
Alonso-Mora et al. 1003

in obstacle-free environments (Alonso-Mora et al., 2012), In the local motion planner, we rely on the notion of
where experiments were performed with 50 robots. Since position-time space, where the time dimension is added
the individual controllers do account for the robot kine- to the workspace to account for moving obstacles. This is
matic and dynamic models, our method does apply to non- similar to the concept of configuration-time space intro-
holonomic robots, as we show in simulations with teams of duced by Erdmann and Lozano-Perez (1987), but differs
aerial vehicles. in that it is embedded in R4 instead of in the potentially
large high-dimensional space - as would be the case for sys-
1.2.3. Cooperative manipulation. Our approach for for- tems with many degrees of freedom. We make this natural
mation control applies to teams of ground and aerial robots, choice explicit with the idea of planning traversable regions
and it also extends to teams of cooperative manipulators in position-time space and letting a non-convex optimizer
collaboratively carrying an object. compute the remaining degrees of freedom of the system to
One of the first approaches for collaborative object trans- safely navigate within those traversable regions.
port in obstacle-free environments was the use of vir-
tual linkages by Khatib et al. (1996). The idea was later 1.3.1. Local motion planning. Given a set of target for-
extended to decentralized control laws by Sugar and Kumar mation shapes, our method, see Section 3, optimizes the
(2002) and by Tang et al. (2004), which enable a team of parameters (such as position, orientation, and size) of the
robots to accurately maintain a stable grasp in an obstacle- multi-robot formation. First, a convex obstacle-free region
free environment. Static obstacles can be avoided by intro- in position-time space is grown in a neighborhood of the
ducing potential functions that repel the robots from them, robots. Second, the parameters of the formation are opti-
as shown by Tanner et al. (2003), but little control is then mized within the convex region by solving a constrained
retained over the resulting configuration. Static and moving optimization. The method guarantees that the team of
obstacles can also be avoided via constrained optimization, robots remains collision-free and makes progress towards
as shown by Alonso-Mora et al. (2015b) for the case of the goal. To make global progress towards a goal configura-
deformable objects. In these approaches, it is common to tion, only waypoints for the formation center are required.
rely on force sensing to coordinate the robots. A human may also provide the global path for the robots,
In this work, we build on these ideas and present a gen- or a desired velocity for the formation, and the robots will
eral, although centralized, non-convex method to compute adapt their configuration automatically.
the parameters of the formation automatically and online When individual robots navigate in formation, each robot
via sequential convex programming, which includes both independently progresses towards its assigned position in
global path planning and local motion planning to avoid the optimal formation via a low-level planner. We employ
static and dynamics obstacles. The method applies to gen- the distributed convex optimization in velocity space by
eral scenarios, specific formation types, and an arbitrary Alonso-Mora et al. (2015c), which avoids collisions and
number of robots. Yet, it enforces that the convex hull of respects the dynamic constraints of the robot.
the formation remains collision-free and is therefore best In Figure 3(a) we provide an overview of the method. In
suited for robots carrying convex, or near-convex, objects. Figure 1 and in Figure 3(b) we show two examples of the
method for mobile manipulators collaboratively carrying an
object.
1.3. Method overview
Motion planning for a formation of robots is an instance 1.3.2. Global path planning. We also describe a method
of planning for a high-dimensional system, which can be for global path planning from an initial configuration to a
solved with sampling-based methods. We expand on this final configuration. The method, see Section 4, computes
method with a two-step approach. for the team of robots a path, and a set of safe inter-
1. Computes convex obstacle-free regions in position mediate formations. The robots avoid static obstacles and
space (global planner) or in position-time space (local reconfigure their formation as required.
planner), embedded in R3 or R4 . Our method presents similarities with sampling-based
2. Executes an optimizer to compute the degrees of free- methods such as the Rapidly-Exploring Random Trees RRT
dom of the formation, such as its position, size, and approach by LaValle and Kuffner (2001). There, sampling
orientation, so that the robots remain within the convex was performed in configuration space and samples (i.e.
regions in free space. configurations) and transitions were collision-checked with
respect to obstacles. Here we describe an alternative, where
This method combines some of the benefits of sampling- sampling is performed in the low dimensional workspace,
based methods - namely exploring a non-convex workspace transitions between formations are guaranteed via convex
and improving the quality of the solution over time - with polytopes, and safe configurations of the formation are
those of local optimization methods - namely efficiently obtained via a constrained optimization. The method intro-
finding a local optimum in continuous space. We describe duced in this work explores the non-convex workspace and
two algorithms and one extension following this idea. improves the quality of the solution over time thanks to
1004 The International Journal of Robotics Research 36(9)

Fig. 3. (a) Schematic overview of the method. Given a goal location for the team of robots, we first compute a global path from the
start to the goal location, see Section 4. Then, the robots navigate along this path with continuous replanning via a local motion planner,
which is described in Section 3. (b) Example with three mobile manipulators collaboratively carrying an object, see Section 5 for the
extension of the method to cooperative object transport. In this case, the robots can rotate around their grasping point and a projection
of the obstacle-free convex region is shown in blue. (c) Global path to navigate from the formation on the bottom left to the formation
on the top right, see Section 4. Obstacles are shown in gray and the robots’ formation in green. Obstacle-free convex regions (blue)
connect the start with the goal configuration. Two regions are grown from the start and goal positions and the two intermediate regions
are grown from random samples in the workspace (black dots). An optimized formation, in green, was computed for each of the two
intersections between adjacent regions. The resulting path (solid black line) connects the start and the goal configurations and traverses
through the convex regions.

sampling, while remaining efficient due to the constrained configuration) such that the robots are fully contained in the
optimization and the use of convex regions, which provide convex region. The only requirements to adapt the method
a dimensionality reduction. are (a) a function V(z) that converts configurations z to the
We create a graph of feasible formations, which con- outer vertices v of the formation, and (b) a way to com-
nects the initial with the goal configuration. Each node in pute derivatives of the position of those outer vertices with
the graph is a valid configuration, which corresponds to respect to the configuration z (unless they are computed
a feasible formation embedded in free-space. Each edge numerically).
between two configurations is associated with an obstacle- In this paper, we describe two applications.
free convex region embedded in the workspace F̄. The
graph is created by random sampling of positions in the 1. Formation control: The configuration of the robot team
workspace F̄ from which obstacle-free convex regions are is given by the 3D position, size, and 3D orientation of
grown. The parameters of valid formations within intersec- the formation, i.e. z ∈ R3 × R+ × SO(3). Given a tem-
tions of polytopes are computed via an efficient constrained plate formation, such as a square, the outer vertices are
optimization. computed via an isomorphic transformation. This is our
In Figure 3(c) we show an example of the method where running example.
three mobile manipulators carry an object from a start 2. Collaborative transportation with mobile manipulators:
configuration (bottom left) to a goal configuration (top The configuration of the robot team is given by the 2D
right). We display the first feasible path found by the algo- position and orientation of the object that the robots
rithm, together with the sampled convex regions and the carry, the orientation of the n robots around their grasp-
intermediate formations within the intersections. ing points and the length of their arms, i.e. z ∈ R2 ×
SO(2)n+1 ×Rn . The outer vertices of the robots and
1.3.3. Generality. We will first describe, in Sections 3 and object can be computed with their shapes and a set of
4, the method for a team of mobile robots navigating in a rigid body transformations defined by the configura-
formation that can change shape via isomorphic transfor- tion. This is described as an extension of the method
mations. We will then, in Section 5, describe an alterna- in Section 5.
tive formation definition for mobile manipulators collabo-
ratively carrying objects. The method is general and can be An advantage of the method is that planning is decoupled
adapted to other high-dimensional problems or formation into: (a) finding convex regions in the lower-dimensional
definitions. The core idea is to generate convex, obstacle- free position-time space (R4 ) and (b) efficiently optimizing
free regions and then optimize the parameters of the for- the configuration of the team of robots within those con-
mation (i.e. the degrees of freedom of the high-dimensional vex regions. This comes at the expense of completeness,
Alonso-Mora et al. 1005

since in our approach we require that the robot team main- 2.3. Obstacle-free workspace
tains a formation that does not intersect with obstacles, i.e.
The obstacle-free workspace, accounting only for static
the robots can not maintain a formation while letting an
obstacles, is denoted by
obstacle pass through. In the event of dynamic obstacles,
the team may break the formation to let a moving obstacle F̄ = R3 \ Ō ⊂ R3 (3)
pass through, and come back to the original formation as
soon as there is enough free room. For current time to , and time horizon τ of the motion plan-
ner, denote the union of static and dynamic obstacles seen
by the robots by
1.3.4. Organization. In Section 2 we introduce the nota-
tion and we describe the formation definition and the 
method to compute convex regions. The algorithm for local Ōτ (to ) = Ō × [0, τ ] ∪ D̄j (to + t) × t ⊂ R4 (4)
t∈[0,τ ]
motion planning is detailed in Section 3, followed by the j∈ID
global path planner in Section 4. In Section 5 we introduce
an extension of the method for transportation of an object where × denotes the Cartesian product of two sets, and with
by multiple manipulators. Section 6 presents experimen- a slight abuse of notation we denote by variable t ∈ R the
tal results with mobile manipulators and simulations with set {t} containing a single point.
aerial vehicles. Finally, Section 7 provides a discussion of The position-time obstacle-free workspace is then
the method and Section 8 concludes this paper.
F̄ τ (to ) = R3 × [0, τ ] \ Ōτ (to ) ⊂ R4 (5)

2. Preliminaries
2.4. Directed obstacle-free convex region
In Table 1 we provide a list of the main symbols and
variables employed in this paper. The first building block of the proposed algorithm is to,
given an obstacle map and a starting point, compute an
obstacle-free convex polytope. We employ a fast iterative
2.1. Robots method, by Deits and Tedrake (2014), to compute large con-
vex polytopes in free position space, i.e. P(F̄) ⊂ F̄, or in
Consider a team of robots navigating in formation. For each free position-time space, i.e. P(F̄ τ (to )) ⊂ F̄ τ (to ). With an
robot i ∈ I = {1, . . . , n} ⊂ N, its position at time t is abuse of notation, we may refer to this polytope by P, and
denoted by pi (t) ∈ R3 . In the next two sections, we consider recall that for local motion planning (Section 3) we embed
all robots to have the same dynamic model and cylindrical it in position-time space and for global path planning (Sec-
non-rotating shape of radius r and height 2h in the verti- tion 4) we embed it in position space. The method consists
cal dimension. Denote the volume occupied by a robot at of two recurrent steps: (a) it computes the separating hyper-
position p by A(p) ⊂ R3 . planes between an ellipsoid E and the obstacles Ō via a
For an alternative description of the robots, where cylin- quadratic optimization; and (b) it computes, via a semi-
drical shape is not required, refer to the extension for definite program, the largest ellipsoid E contained within
rectangular mobile manipulators in Section 5. the convex polytope P. This polytope P can be described
by the union of hyperplanes, see Figure 4 for an example.
We extend the method by means of the following.
2.2. Obstacles
Consider a set of static obstacles O ⊂ R3 defining the 1. Considering a set of points In, potentially {p1 , . . . , pn },
global map. Denote by Ō the set O dilated by half of the to be contained within P. The iterative algorithm breaks
robot’s volume, formally at convergence or when In  P.
2. Growing the region P towards a desired point gdir . This
is achieved by initializing E to be the minimal ellipsoid
Ō = {p ∈ R3 | A(p) ∩ O = ∅} (1)
such that {In, gdir } ⊂ E. The point gdir is typically set to
the goal position for the robot team g(tf ), and must also
Moving obstacles within the field of view of the robots can be contained within P. If no solution exists, we evaluate
be accounted for. Denote by ID = {1, . . . , nd } ⊂ N the list alternative points via a linear search between gdir and
of moving obstacles. For moving obstacle j ∈ ID and time the centroid of the points in In.
t, we denote by Dj (t) ⊂ R3 the volume that it occupies, and
If the polytope is embedded in position space, we denote
D̄j (t) = {p ∈ R3 | A(p) ∩ Dj (t) = ∅} (2) g
by PIndir (F̄) the resulting convex polytope, which contains
the points in In and does not intersect any of the obstacles,
g
its dilation by half of robot’s volume. For predicted future i.e. satisfies In ⊂ PIndir (F̄) ⊂ F̄ and which is grown in the
positions we employ the constant velocity assumption. direction of gdir as described in the previous paragraph.
1006 The International Journal of Robotics Research 36(9)

Table 1. List of symbols employed in the method.

Symbol Definition First appears

n/m Number of robots / template formations Section 2

p Position or point in the workspace (typically in R3 ) Section 2
A(p) Volume occupied by a robot Section 2
τ Time horizon of the local motion planner Section 2
O / Ō Static obstacles / Dilated Section 2
Dj / D̄j Dynamic obstacle / Dilated Section 2
Oτ (to ) / Ōτ (to ) Union of static and dynamic obstacles for time [to , to + τ ] / Dilated Equation (4)
F / F̄ Obstacle-free static workspace (workspace minus static obstacles / Dilated) Equation (3)
F τ (to ) / F̄ τ (to ) Obstacle-free workspace in position-time / Dilated Equation (5)
P Obstacle-free convex polytope (several variants) Section 2.4, 2.3
f f
rj / r0,j Position of robot j in the optimized/template formation f Section 2.5
f f
vj / wj Outer vertex of the optimized/template formation f Section 2.5
t/s/q Position (translation), size and orientation (quaternion) of a formation Section 2.5
z Configuration, i.e. optimization parameters of a formation Section 2.5
V(z, f ) Set of outer vertices of formation f with configuration z Equation (8)
g Goal position for the centroid of the robot team Section 3
s̄ / q̄ Desired size/orientation for the formation Section 3
G = {V , E} Graph containing convex regions and target formations Section 4

g
Fig. 4. Example of a convex directed polytope PIndir (F̄) (in red)
and its associated ellipsoid (blue) in an environment with two
Fig. 5. (a) Example of a template square formation with sixteen
static obstacles (gray). The polytope contains both the In points
MAVs. The four outer vertexes define the convex hull. (b) The
and the target point gdir .
formation can be transformed with a translation t, a 3D rotation q
and a size s isomorphic transformation.

Definition 1 . (Directed polytope). We refer to a polytope

g
PIndir (F̄) ⊂ R3 , embedded in F̄ as a directed polytope,
g
towards gdir . Analogously for a polytope PIndir (F̄ τ (to )) ⊂ Further denote by df the minimum distance between any
R4 , embedded in F̄ τ (to ). given pair of robots in the template formation f . Template
formations can be defined by a human designer or automat-
ically computed for optimal representation of a target shape
2.5. Definition of the formation as shown by Alonso–Mora et al. (2012).
A formation is then defined by an isomorphic transfor-
For an alternative description of the formation, refer to the mation, which includes the size s ∈ R+ , a translation
extension for mobile manipulators in Section 5. t ∈ R3 , and a rotation R(q) described by a unit quaternion
We consider a predefined set of m ∈ N template forma- q ∈ SO(3), its conjugate denoted by q̄. With this formation
tions, such as square, line, or T. Each template formation definition, the configuration for the team of robots is fully
f ∈ If = {1, . . . , m} is given by a set of robot positions defined by z = [t, s, q] ∈ R3 × R+ × SO(3).
f f f f
{r0,1 , . . . , r0,n } and a set of outer vertices {w1 , . . . , wnf } rel- Given the configuration z, and template formation ID
ative to the center of rotation (typically the centroid) of f , the robot positions and outer vertices of the resulting
the formation, where nf denotes the number of outer ver- formation are computed by
tices defining formation f . The set of vertices represents the
convex hull of the robot’s positions in the formation, thus
f f
reducing the complexity for formations with many robots. ri = t + s R(q) r0,i , ∀i ∈ [1, n]
f f (6)
See Figure 5(a) for an example. vj = t + s R(q) wj , ∀j ∈ [1, nf ]
Alonso-Mora et al. 1007

where the rotation in SO(3) is given by the quaternion Algorithm 1 Local motion planning.
operation Given: Union of static and dynamic obstacles in
 T  T position-time space Ōτ (to ) ⊂ R4 at the initial time. The
f
0, R(q) wj = q × 0, wj × q̄
f
(7) goal position g(t1 ) ∈ R3 for the centroid of the for-
mation at time t1 and desired size s̄ and orientation
For template formation f and configuration z we denote the q̄.
set of outer vertices by Compute: Target configuration z∗ and formation f ∗ .
Collision-free motion for the team of robots for up to
f
V(z, f ) = [v1 , . . . , vfnf ] (8) the time horizon τ .
—————————————————————–
In the exposition of the method we rely on this definition for ———————- Main process ————————–
the formation, but the method is general and can be applied 1: while not converged do
to alternative definitions, as shown in Section 5 for the case 2: Compute large convex polytope P ⊂ F̄ τ (to ) in a
of several manipulators transporting a rigid object. neighborhood of the robots.
3: Compute optimal configuration z∗ and formation
3. Local motion planning f ∗ , such that the outer vertices V(z∗ , f ∗ ) are contained
within P.
The local motion planner computes the optimal parameters, 4: end while
i.e. the configuration, of the formation, in a neighborhood of ——- Second parallel process, at high frequency ——–
the robots, via a constrained nonlinear optimization. For a 5: while not converged do
given template formation f ≤ m, the vector of optimization 6: Assign robots to target positions in the formation.
variables, i.e. the configuration, is denoted by z = [t, s, q] ∈ 7: Navigate towards the target formation.
R3 × R+ × SO(3). 8: end while
Denote by g(t) ∈ R3 the goal position for the centroid
of the formation at time t. This goal position, and a target
orientation q̄ and size s̄, can be given by a human oper- respect the robot’s dynamics. In particular, we build on
ator or a global planner, as described in the forthcoming a distributed convex optimization described by Alonso–
Section 4. Mora et al. (2015c), extended to account for static
For an alternative formulation of the optimization, see obstacles in a seamless way.
the extension for mobile manipulators in Section 5. 4. If no feasible formation exists in a neighborhood of the
robots, we search for the parameters of a target forma-
tion near the goal position. In this case, the robot team
3.1. Algorithm overview splits and each robot navigates independently towards
its assigned position in the target formation.
To make progress towards the goal position while avoiding
obstacles, the local planner computes a target formation and
the required motion of the robots for a given time horizon
τ > 0, which must be longer than the required time to stop. 3.2. Obstacle-free convex region
Denote the current time by to and t1 = to + τ . First, the obstacle-free space in position-time R3 × [0, τ ] ⊂
Our method consists of the following steps. R4 is obtained, accounting for static and dynamic obstacles.
For the given time horizon τ consider the union of
1. Compute a large convex polytope P contained in free static and dynamic obstacles Ōτ (to ) and the associated
position-time space, such that the robots are inside it, position-time obstacle-free workspace F̄ τ (to ), as described
i.e. pi (to ) ∈ P ⊂ F̄ τ (to ), ∀i ∈ I, and that is directed in equation (5).
towards the goal g(t1 ). This is described in Section 3.2. Following Section 2.4, we compute two convex
2. Compute the optimal formation f ∗ and configuration polytopes.
z∗ such that the outer vertices V(z∗ , f ∗ ) are contained
within P and the distance between the formation’s cen- 1. Pfo →g in free position-time space, which contains all
troid and the goal g(t1 ) is minimized. The parameters the robots at their current positions and initial time,
of the formation are optimized subject to a set of con- i.e. [p1 (to ) , . . . , pn (to ) ] × 0, and which is directed
straints via a centralized sequential convex optimization towards the formation’s goal at the time horizon, i.e.
described in Section 3.3. In this computation, the robot’s [g(t1 ) ×τ ] ∈ R4 . Formally
dynamics are abstracted.
Pfo →g := P[p1 (t1o ),...,pn (to )]×0 (F̄ τ (to ))
[g(t )×τ ]
3. In a faster loop, described in Section 3.5, the robots (9)
are optimally assigned to target positions in the for-
mation and move towards them employing a low level 2. Po→g in free position-time space, which contains the
local planner that generates collision-free inputs that centroid of the robots’ current positions and initial time,
1008 The International Journal of Robotics Research 36(9)

3.3.2. Constraints. Constraints are introduced to guaran-

tee that all the robots in the formation are within the convex
polytope (C1 ) and to satisfy a minimum inter-robot distance
(C2 ) to avoid collisions between the robots in the team.
Recalling Section 2.5 the constraints are then given by
C1 ≡ V(z, f ) × t1 ⊂ P ≡
nf
 f
Fig. 6. Example of convex regions computed with the method {A[[t + s R(q) wj ] × t1 ]T ≤ b} (14)
described in Section 2.4 for a scenario with six yellow disk robots j=1
 
and two (gray) squared obstacles. The convex regions are: Pfo →g max( r, h)
in blue solid border, Po→g in red dotted border and Pg (defined
C2 ≡ s ≥ 2
df
in Section 3.4) in green dashed border.
f
where C1 contains a constraint for each vertex wj of the
 convex hull of the template formation f and implies that
i.e. [ i∈I pi (to ) /n] × 0 ∈ R4 , and which is directed the robots do not collide with any obstacle. The constraint
towards the goal. Formally C2 guarantees inter-robot collision avoidance since df is the
Po→g := P[1
[g(t )×τ ]
(F̄ τ (to )) (10) minimum inter-robot distance for the template formation,
i∈I pi (to )/n]×0 recall Section 2.5, and the transformation applied to the
This polytope may not contain all the robots at their formation is isomorphic.1
current positions. For planar formations, the additional constraints q ·
[0, 1, 0, 0]T = 0 and q · [0, 0, 1, 0]T = 0 may also be imposed
A representative example of these regions (projected in to ensure rotation only occurs around the vertical axis.
R2 ) is shown in Figure 6. In general, we consider the convex
polytope
3.3.3. Nonlinear program. For a template formation f ∈
P = Pfo →g ∩ Po→g (11)
If the optimal configuration z∗f is found by solving the
which: nonlinear optimization
• guarantees that the transition to the new formation will z∗f = arg min Jf (z)
z
be collision-free, since P ⊂ Pfo →g and all the robots V(z, f ) ×t1 ⊂ P
s.t.  ( C1 ) (15)
are within the convex region Pfo →g ;
s ≥ 2 max(r,h) ( C2 )
• is likely to make progress in future iterations, since P ⊂ d f

Po→g , which is directed towards the goal. We employ the nonlinear solver SNOPT by Gill et al.
Once the robots are within this intersection they can (2002), which internally executes a sparse Sequential
make progress towards the goal within Po→g in a collision- Quadratic Program and converges to a feasible local min-
free manner. If P = ∅, an alternative convex region is imum of the cost function.
selected as described in the forthcoming Section 3.4. The derivatives of the cost function (equation (13)) and
We rely on a representation of the collision-free convex constraints (equation (14)) are given by
polytope P given by its equivalent set of linear constraints ∂Jf (z) /∂z ≡ 2[wt (t − g(t1 )) , ws ( s − s̄) , wq (q − q̄) ]. (16)
nl ×4
P = {x ∈ R | Ax ≤ b, for A ∈ R
4
,b∈R } nl
(12) ∂C1 /∂z ≡
nf f f
A R(q) wj , s A ∂R(q) wj /∂q) ]
j=1 [A, (17)
where nl denotes the number of faces of P. ∂C2 /∂s ≡ df
f
where ∂R(q) wj /∂q is the Jacobian of equation (7).2
3.3. Nonlinear optimization We set the initial point for the optimizer to
We formulate a constrained nonlinear optimization to com- zini = [g(t1 ) , 2 max( r, h) /df , qini ] (18)
pute a locally optimal formation f ∗ and the configuration z∗
for the team of robots. where the initial quaternion is chosen to be equal to the
quaternion addition of the desired orientation and a small
random quaternion, i.e. qini = q̄ + qrand . The additional
3.3.1. Optimization cost. We minimize a weighted sum of
term is included to avoid singularities of the optimizer when
the deviation with respect to the formation’s goal g(t1 ), a
some components of q̄ are zero.
desired size s̄ and a desired rotation q̄. The cost term is then
If the constrained optimization of equation (15) is solved
Jf (z) = wt ||t−g(t1 ) ||2 +ws ||s−s̄||2 +wq ||q− q̄||2 +cf (13) for each template formation, the index f ∗ of the locally
optimal formation is then given by
where wt , ws , wq are design weights and cf is the predefined
cost for formation type f ∈ If . f ∗ = arg min Jf (z∗f ) (19)
f
Alonso-Mora et al. 1009

This formation definition and its associated nonlinear opti- Following Alonso–Mora et al. (2012), this assignment can
mization are given as an example, but the framework is be centrally computed with the optimal Hungarian algo-
general and can be applied to other problem instances. In rithm by Kuhn (1955), used in this work, or a suboptimal
Section 5 we describe how to apply this method for the auction algorithm by Bertsekas (1988), which scales well
manipulation of rigid objects. with the number of robots.

3.4. Iterations if problem is infeasible 3.5.2. Collision avoidance. To control the individual
robots in the team and to avoid collisions between them,
The method, as described in the previous subsections, we employ the collision avoidance algorithm introduced
results in a formation and its configuration for the robots’ by Alonso–Mora et al. (2015c). We employ the same con-
team. It may occur though, that the robots make no progress straints to avoid moving obstacles and to respect the kine-
towards the goal (deadlock) or that the optimization is infea- matic model of the robots. To better handle environments
sible, for example if the region P defined in equation (11) is with static obstacles, we include additional linear con-
too small and no feasible formation fits inside. In that case, straints defined by a convex polytope in free space, com-
one may search for a feasible formation using an alternative puted in a neighborhood of the robot. For details refer to the
region. For a representative example see Figure 6. Appendix. This method, a convex optimization in velocity
First, we would repeat the optimization using the convex space, is well suited for our application. The formation con-
region Pfo →g . If a valid target formation is found in this trol algorithms described in this paper are agnostic to the
step, or in the original optimization with polytope P, the low-level controller and a different one could be employed.
transition is guaranteed to be collision-free, thanks to the
convexity of the polytope Pfo →g which contains the current
position of all the robots. 4. Global path planning
If this additional step is also unfeasible, then no forma- Given an initial and a target configuration for the robot
tion may exist such that the robots can continue navigating team, the global path planner computes a feasible path and
in formation towards the goal. In that case the optimiza- intermediate formations to connect them. This is achieved
tion can be repeated using the polytope Po→g or directly by combining a sampling-based approach with constrained
the polytope Pg := P[g(t1 ),τ ] (F̄) that contains the forma- nonlinear optimization, the idea being to sample in a low
tion’s goal. Note though that these two polytopes do not dimensional space (workspace) and letting the optimizer
contain the current robot positions. If a formation is found, compute the remaining degrees of freedom.
the robots move individually, i.e. separately, towards their In particular, we create a graph where each node is a fea-
respective positions in the target formation. In this case, sible formation and which contains the initial and the goal
the formation is likely broken during the transition, but, configuration. An edge between two nodes, or formations,
this gives further flexibility to the method and the robots to is a convex region in free space, which contains both forma-
navigate in formation whenever possible, via splitting and tions. An edge provides the means to transition between two
merging. nodes in the graph. An example was shown in Figure 3(c).
The approach can be applied to a single user-defined
3.5. Individual planning towards target formation (i.e. square) or when multiple formations are
given. In the latter, reconfiguration between formation
formation
shape would be allowed. In an abuse of notation, throughout
The result of the computation of Section 3.3 is a target this section we drop the subindex f ∈ If , consider a single
formation f ∗ and configuration z∗ . The associated set of tar- formation f and refer to a polytope Pp (F̄) embedded in the
get robot positions rj , for all j ∈ I can be computed with free position space, i.e. P(F̄) ⊂ F̄, by P. This is in con-
equation (6). trast to the local planning approach, where we embedded
In this section, we describe the local planner that links the convex polytope in the free position-time space. There-
the centralized formation optimization with the physical fore, we do not consider moving obstacles in the global path
robots. At each step of the execution, at higher frequency planning.
than that of computing a new formation, the following steps
are executed.
4.1. Algorithm
3.5.1. Goal assignment. Robots are optimally assigned to Consider the obstacle-free workspace F̄ defined by equa-
the target positions rj with the objective of minimizing the tion (3), the start position s of the formation’s centroid and
sum of squared traveled distances. The optimal assignment its goal position g ∈ R3 . In Algorithm 2 we describe the
σ : I → I is proposed anytime method to compute a path for the robot
team to navigate in formation from s to g.
min ||pi − rσ (i) ||2 (20) A graph G = {V , E} is incrementally created. Each ver-
σ tex in the vertices list V is given by the configuration z
i∈I
1010 The International Journal of Robotics Research 36(9)

of a feasible formation. Each edge in the edge list E con- Algorithm 2 Global path planning.
nects two nodes, i.e. valid configurations z1 , z2 for the team 1: Given: obstacle field O, start configuration zs with cen-
of robots, if a convex region P exists such that the robots troid s and destination g for the formation’s centroid.
in both configurations are fully contained in the convex 2: Returns: a path T of feasible configurations (forma-
polytope. tions) from s to g.
We keep a list of existing polytopes P. And, for each We describe a bidirectional graph search. The method
polytope P ∈ P we keep a list LP of configurations for can be adapted to a tree search.
which the team of robots is fully contained within the poly- —————————————————————
tope. The node list is initialized with the initial zs and 3: Initialize empty graph G = {V , E}: V = ∅; E = ∅
final zg configurations with centroids s and g. Analogously, 4: Initialize empty polytope list P = ∅
the polytopes list is initialized with the convex regions Ps 5: Add the initial configuration to the node list V ← zs
and Pg , which contain the initial and final configurations 6: Generate Ps , Pg ⊂ F̄ from s and g
respectively. # Add them to the polytope list
The method proceeds by drawing random samples in the 7: P ← Ps , P ← Pg
workspace (R3 for aerial vehicles). Each random sample # Compute a valid configuration in the goal region
p ∈ R3 is rejected if it is inside an obstacle or one of the 8: zg = formation(Pg )
polytopes in the list P. If p ∈ F̄ \ ∪ P then the following 9: Add the goal configuration to the node list V ← zg
P∈P
steps are executed. # Create lists of valid configurations for both polytopes
10: LPs = {zs }, LPg = {zg }
1. A large convex polytope Pp (F̄) ⊂ F̄ is grown from p # Check if the start and goal can be connected
following the method of Section 2.4. 11: if ∃z = formation(Ps ∩ Pg ) then
2. For each polytope P ∈ P that intersects Pp , we compute 12: V ← {z}
a configuration z and formation f for the team of robots 13: E ← {zs , z, Ps }
such that the formation’s vertices are fully contained 14: E ← {zg , z, Pg }
within the intersection of both polytopes V( z, f ) ⊂ Pp ∩ 15: end if
P and that minimizes the squared distance from its cen- # The following search loop can be executed until the
troid to g. The configuration z and formation f are com- first feasible path is found, until the whole space F̄ is
puted via a nonlinear optimization analogous to that of explored or up to a maximum time bound.
Section 3.3. For polytope P we denote this function by 16: while not end do
formation(P). If a valid configuration exists, it is added 17: Generate random sample p ∈ F̄\( ∪i∈P Pv )
to the node list. 18: Generate polytope Pp ⊂ F̄ grown from p
3. If a valid configuration z is added to the node list, then # Try to create new node
(a) an edge {z, zi , Pp } is added for all configurations 19: if ∃z = formation(Pp ) then
zi ∈ LPp and (b) an edge {z, zi , P} is also added for 20: LPp = ∅
all configurations zi ∈ LP . Recalling the previous sec- # Try to create new nodes and edges
tion, it is guaranteed that the team of robots can navigate 21: for P ∈ P do
between both formations through the associated convex 22: if ∃z1 = formation(P ∩ Pp ) then
polytope. 23: for zi ∈ LP do
24: E ← {z1 , zi , P}
A feasible solution is found as soon as a path (or 25: end for
sequence of connected vertexes in the graph G) is found 26: for zi ∈ LPp do
which connects the initial position with the goal position of 27: E ← {z1 , zi , Pp }
the formation’s centroid. If we let the algorithm run longer, 28: end for
for example until most of the free space is covered by con- 29: V ← z1
vex regions, the best path so far is found via graph search. 30: LPp ← {z1 }; LP ← {z1 }
This is, by computing the path of lowest cost in the graph 31: end if
G. For each edge E between two configurations z1 and z2 32: end for
we define its cost by the distance between the centroids of 33: P ← Pp
z1 and z2 . 34: end if
35: return T =shortestPath(G)
36: end while
4.2. Execution in composition with the local
motion planner
To navigate the team of robots from the initial to the goal path planning algorithm of the previous section. Each con-
configuration a global path consisting of a sequence T = figuration zw ∈ T in the sequence, provides an intermediate
{zs , . . . , zg } of configurations is first obtained via the global setpoint for the team of robots. Denote its centroid by w.
Alonso-Mora et al. 1011

Algorithm 3 Function: z = formation(P) Algorithm 4 Function: T =shortestPath(G)

Input: Convex polytope P ⊂ F̄. Input: Graph G.
Output: A valid configuration z such that V( z, f ) ⊂ P, Output: A sequence of valid configurations T =
or ∅. {zs , . . . , zg } and convex polytopes {Ps , . . . , Pg } such
1: if P = ∅ then that the robot team can navigate through them from the
2: return ∅ start to the goal, or ∅.
3: else 1: if zs and zg are not connected in G then
4: return result of a nonlinear optimization analogous 2: return ∅
to that of Section 3.3 with convex polytope P and min- 3: else
imizing J (z) the deviation to a target configuration at g, 4: return result of graph search on G where the cost of
i.e. traversing an edge E = {z1 , z2 , P} is given by d( z1 , z2 ),
z∗ = arg min J (z) the Euclidean distance between the centroids of the two
z
s.t. V(z, formations.
 f)⊂ P (21)
5: end if
s ≥ 2 max(r,h)
d f

5: end if

The position t ∈ R2 and orientation θo ∈ SO(2) of the

To reach the final goal, the team of robots sequentially object can vary. Each manipulator i = {1, . . . , n} can rotate
follows the intermediate setpoints in the path and the local around the grasping point gi by an angle θi ∈ [θmin , θmax ] ⊂
planner minimizes the deviation towards the associated con- [−π/2, π/2] ⊂ SO(2) relative to the direction pointing
figuration zw at every instance. To make progress towards towards the center of the object. Each manipulator may
the intermediate setpoints, and for improved performance, change the arm length, denote ai ∈ [amin , amax ] ⊂ R
we slightly modify Algorithm 1 by selecting the convex the distance from the center of the robot to the grasping
region P ⊂ F̂ τ (to ) as follows. position.
We do not directly use the convex regions stored in the The vertices of the object relative to its center and
global path, since the robots need to account for dynamic expressed in the object coordinate frame are denoted by
obstacles in real-time. {w01 , . . . , w0n0 }. The vertices of manipulator i relative to its
First, a convex region Pfo →w containing all the robots in center and expressed in the robot coordinate frame are
the team at their current positions, and directed towards w denoted by {wi1 , . . . , wini }. Denote the grasping positions on
is computed. If the setpoint w is also inside the polytope, the object, relative to its center, by gi0 . Without loss of gen-
i.e. z0 , w ∈ Pfo →w , then we use this polytope P := Pfo →w erality, in this derivation we assume that the angle between
for navigation. the robot base and arm is constant (additional degrees of
Otherwise, we compute its intersection with a polytope freedom could added to the set of optimization variables)
generated with only the centroid of z0 and directed towards and denote by ai0 the vector from the center of the robot
the waypoint w, i.e. P := Pfo →w ∩ Po→w . In this case to the grasping point, of length ai , expressed in the robot
the robots may reconfigure to make progress towards the coordinate frame. An example with three robots grasping a
intermediate setpoint. See Section 2.4 for details on the triangular object is shown in Figure 7.
computation of these polytopes. Given the configuration z = [t, θo , a1 , . . . , an ,
θ1 , . . . , θn ] ∈ R3+2n of the formation, the vertices of
the manipulator and object, expressed in the world
5. Extension for mobile manipulators coordinate frame, are

In this section, we describe an extension where a team of

mobile manipulators collaboratively carry an object in a v0j = [t, 0]T + Rθ0 w0j , ∀j ∈ [1, n0 ]
dynamic environment. To achieve this, the robot shape, for- vij = [t, 0] + Rθ0
T
gi0 + Rθi (wij −
, ∀j ∈ [1, ni ]
ai0 )
mation definition, and optimization equations are modified,
and the derivations follow the same line of thought of the (22)
previous sections. where Rθ is the rotation matrix [cos( θ ) , − sin( θ ) , 0;
sin( θ ) , cos( θ ) , 0; 0, 0, 1]. We denote by

5.1. Robot and formation definition

V(z, i) = [vi1 , . . . , vini ] (23)
The formation is defined by n mobile manipulators, each
equipped with a robotic arm and grasping a rigid object
at given points, see Figure 5(b) for an example with two the set of ni vertices for the object (i = 0) and each robot
mobile manipulators. (i > 0) at configuration z.
1012 The International Journal of Robotics Research 36(9)

The derivatives of the constraints with respect to the

optimization variable z are computed analogously to
equation (17).

6. Results
In this section, we present experiments with a team of
three Kuka Youbot mobile manipulators collaboratively car-
rying an object and simulations with teams of quadrotor
Fig. 7. Right: optimization variables for three mobile manipu- UAVs navigating in 3D environments. A video illustrating
lators grasping a triangular object. Left: vertices of a mobile the results accompanies this paper and is also available at
manipulator and the grasped object. For this triangular object, the https://youtu.be/sDNqdEPA7pE.
vertices are equal to the grasping points. The mobile manipulators are holonomic platforms. For
the UAVs, we employ the nonlinear dynamical model and
LQR controller used by Alonso–Mora et al. (2015c) with
5.2. Obstacle-free convex region real quadrotors.
We use SNOPT by Gill et al. (2002) to solve the non-
Recalling Section 2.3, the position-time obstacle-free
linear program via Sequential Quadratic Programming, a
workspace is given by
goal-directed version of IRIS by Deits and Tedrake (2014)
Oτ (to ) = O × [0, τ ] ∪ Dj (to + t) ×t ⊂ R4 to compute the large convex regions and the Drake toolbox 3
t∈[0,τ ]
j∈ID (24) from MIT to handle quaternions.
F τ (to ) = R3 × [0, τ ] \ Oτ (to ) ⊂ R4

where, now, the static and dynamic obstacles are not dilated. 6.1. Multiple aerial vehicles in formation
The collision-free convex polytope containing the robots To evaluate our approach in 3D environments with aerial
at their current state and directed towards the goal is vehicles we present experiments in three simulated scenar-
ios. The first scenario consists of four controlled quadrotors
( F τ (to ))
[g(t1 )×τ ]
Pfo →g := P∪ i i (25) and four dynamic obstacles. The second scenario consists
i=0:n [v1 (to ),...,vni (to )]×0
of four controlled quadrotors flying in formation and avoid-
additional polytopes in free space are computed ing several static obstacles and one dynamic obstacle. The
analogously. last scenario involves sixteen quadrotors flying in formation
through a narrow corridor. In our visualizations, we employ
a cylinder since that is the shape we use for collision avoid-
5.3. Nonlinear optimization
ance. Internally the quadrotors have an attitude controller
Consider zini = [tini , θoini , aini
1 , . . . , an , θ1 , . . . , θn ] the ini-
ini ini ini
and position controller and change their 3D pose within the
tial configuration of the robots and object at the current enclosing cylinder, which is always kept vertical.
time. In all cases a new formation is computed every 2 s. The
Since the robots are rigidly attached to the object, we individual collision avoidance planners run at 5 Hz. The
must explicitly impose that the transition between the cur- quadrotors move at speeds between 0.5 m/s and 1.5 m/s. In
rent and the target configuration remains within the con- our simulations with four quadrotors a time horizon τ = 4 s
vex polytope. Consider K > 0 interpolation steps, and is considered. This is longer than the required time to reach
denote by zλ the linearly interpolated configurations such a full stop. For the experiments with sixteen quadrotors a
that zλ=0 = zini and zλ=K = z. Angles are interpolated time horizon of τ = 10 s is chosen, due to the large size of
in the direction of minimum change and each interpolated the formation and the scenario.
configuration zλ is expressed as a function of zini and z, e.g. Consider the first scenario. Figure 8 shows the trajecto-
tλ = λ(t − tini ) /K + tini . ries of four quadrotors (in green and blue) passing through
Recalling equation (22) and the representation of the two lanes of dynamic obstacles (in yellow). The dynamic
collision-free polytope P by a set of linear constraints, as obstacles in the left lane move downwards at 0.4 m/s and
in equation (12), the optimization is the ones in the right move upwards with the same speed.
Two default formations are considered, square (which is
z∗ = arg min ||t − g(t1 ) ||2 preferred) and diamond. The goal for the formation follows
z
s.t. V(zλ , i) × t1 ⊂ Pfo →g a constant velocity trajectory along the middle horizontal
θmin ≤ θi ≤ θmax (26)
line and the team successfully adapts the parameters of
amin ≤ ai ≤ amax the formation to remain collision-free and pass in-between
∀j ∈ {1, . . . , ni }, ∀i ∈ {0, . . . , n} the obstacles. In this case, we imposed that the formation
∀λ ∈ {1, . . . , K} remains on the horizontal plane for illustrative purposes.
Alonso-Mora et al. 1013

Fig. 8. Top view. Four robots (green-blue) navigate a 8 x 15 m2

environment with two lanes of dynamic obstacles (orange). The
four robots locally reconfigure the formation and make progress
towards the right side.

In order to evaluate the robustness of the method, we

performed additional experiments for this first scenario for
varying speeds of the dynamic obstacles and the quadro- Fig. 9. Results for 3D navigation in the scenario of Figure 8 with
tors flying in formation. The results are presented in four quadrotors and varying speeds of the dynamic obstacles and
Figure 9. We observe that most of the time the target the formation. Fixed time horizon of τ = 5 s is used in all
formation for the robots is within Pfo −>g , thus the for- experiments. The figure shows the percentage of time in which
mation is kept. But, at high speeds, in order to quickly the target formation is within Pfo −>g . This is, the percentage of
progress towards the goal, the robots temporally break it and time in which the formation is guaranteed to be maintained. If
select a target one within Po−>g or Pg . Good results, espe- the optimization becomes unfeasible, due to the higher speed of
cially at lower dynamic obstacle speeds are observed. We the dynamic obstacles and the limited time horizon, the forma-
believe that the results could be improved with an adaptive tion might break (in this case the target formation is found within
time horizon depending on the speed of both the moving Po−>g or Pg , but not in Pfo −>g ). Scenarios for which a collision
obstacles and the formation. In scenarios with only static happened are marked with a red dot. As will be discussed in Sec-
obstacles, the formation is maintained at all times. tion 7, collisions may arise mostly due to unforeseen changes of
In this scenario, very few collisions arise when the tar- speed by moving obstacles or the limited time horizon.
get speed of the formation is higher or similar to that of the
dynamic obstacles and our framework successfully drives
the robots towards the goal while avoiding collisions. Some
collisions arise when the speed of the dynamic obstacles
is much higher than that of the formation. This is due Results of the third scenario, where sixteen quadrotors
to the local planning horizon and the robots being unable move along a corridor of three different widths are shown in
to escape on time due to their lower speed. Again, these Figure 11. Three default formations are considered: 4x4x1
results may be improved with an adaptive time horizon of defined by four vertices (preferred), 4x2x2 defined by eight
the framework. vertices and 8x2x1 defined by four vertices. At each time
Next, we present experiments for the second scenario. step the method computes the optimal parameters for each
Figure 10 shows snapshots and trajectories of four quadro- of the three and selects the one of lowest cost. Between
tors tracking a circular trajectory while locally avoiding times 75 s and 110 s the method successfully rotates the
three static obstacles and a dynamic obstacle. Three default formation by 90o for it to be collision free (the default for-
formations are considered here: square (1st preference), mations were horizontal, which is also preferred in the cost
diamond (2nd preference), and line. The optimal parame- function).
ters are computed with the nonlinear optimization allowing Thanks to the abstraction of a formation by the vertices
rotation in 3D (flat horizontal orientation preferred) and of its convex hull, see Section 2.5, the computation time of
reconfiguration. the nonlinear optimization is independent of the number of
The four quadrotors start from the horizontal square and robots - as long as the same convex shape is maintained -
slightly tilt it (11 s) to avoid the incoming dynamic obstacle. and can be executed in real-time. It is worth noting that in
To fully clear it while avoiding the obstacle in the lower this algorithm the dimension of the space where the robots
corner, they shortly switch to a vertical line, and then back move has little influence in the computational cost, which
to the preferred square formation (20 s). To pass through the depends mostly on the number of variables defining the for-
next narrow opening they switch back to the line formation mation. In Table 2 we provide computational times for our
(30 s) and then to the preferred square, tilted to avoid the implementation using a 2.6 GHz i7 laptop. The approach
dynamic obstacle (37 s). Once the obstacles are cleared they shows close to real time performance, typically below 300
return to the preferred horizontal square formation (45 s). milliseconds
1014 The International Journal of Robotics Research 36(9)

Fig. 10. Four quadrotors (green-blue cylinders) navigate in a 12 x 12 x 6 m3 scenario with three static obstacles (grey) and a dynamic
obstacle (yellow). The four quadrotors track a circular motion (black dots in top view) and locally reconfigure the formation to avoid
collisions and make progress.

Fig. 11. 16 quadrotors navigate along a 70 x 10 x 10 m corridor, with obstacles shown in gray. The quadrotors locally adapt the
formation to remain collision free. The following formations are observed: 4x4x1 - 4x2x2 - 4x4x1 (vertical) - 8x2x1 (vertical), finally
transitioning towards horizontal 8x2x1.

Table 2. Computational time [ms] for our implementation. change their orientation and distance with respect to the
Compute Min Mean Max Std deviation object (θi={1,2} = 0, ai={1,2} = constant). We optimize for
the position and orientation of the object only.
Convex region 31.8 82.8 221.4 72.1 Four snapshots are shown in Figure 12 of an experiment
NL optimization 93.7 226.4 522.7 64.1 where the two mobile manipulators successfully carry the
rigid object to the goal position behind the orange boxes
6.2. Collaborative transport with two mobile while locally avoiding collisions with the human. In all our
experiments, executed with external tracking, the robots
manipulators successfully adapted their formation to avoid collisions.
We performed initial experiments with two mobile manip- This assumes that the human cooperates, otherwise, colli-
ulators carrying a rigid object, as described in Section 5. sions may still occur if the human moves faster than the
In this first experiment the two robots are not allowed to robots or traps them against an obstacle.
Alonso-Mora et al. 1015

Fig. 12. Four consecutive snapshots of an avoidance maneuver where two mobile manipulators collaboratively carry a rigid object and
navigate it to the goal while adjusting their formation to avoid collisions with the orange boxes and the human. Robots and human are
tracked by overhead cameras. This maneuver is performed in 1 minute.

A visualization with convex regions of another experi- Navigation in all these scenarios was successfully achieved
ment is shown In Figure 13. For each snapshot the current by the three mobile manipulators.
(blue) and target (green) formation given by the optimiza- A representative experiment where the three robots nav-
tion are displayed. The two robots successfully adapt their igate through the boxes and avoid a human is shown in
formation, rotating as required, to avoid both the dynamic Figure 15. For reference, in Figure 16 we show twelve dif-
obstacle (red) and static (grey) obstacles in this 8 m x 6 ferent scenarios and the configuration of the three robots
m scenario. Slices at to and ff = to + τ of the convex when navigating through the environment.
region computed in position-time space are also shown for In these experiments, we employed a triangular object
illustrative purposes. A time horizon τ = 2 s was employed. with foam exterior. The foam provides a small degree of
deformability to compensate for the lack of compliance in
the robot arms and low level controller of the mobile manip-
ulators. Note that successful manipulation of a perfectly
6.3. Collaborative transport, three mobile rigid body was shown in the previous experiments with two
manipulators mobile manipulators, albeit at lower speeds.
We performed additional experiments with three mobile
manipulators carrying an object, as described in Section 5. 6.4. Global planning in large scenarios
All three robots can change their orientation respect to the
object, but their distance remains constant. We optimize for We also tested the approach in several larger scenarios. In
the position and orientation of the object and the orientation Figure 17 we show two examples of the global planner in
of all three manipulators. Given a goal for the formation, simulated 2D environments. In these two cases the global
we first compute a global path with the algorithm of Sec- planner can run for several iterations, up to a fixed amount
tion 4 considering only the two static obstacles. The local of time. We display all the computed convex regions and
motion planner then runs at a frequency of approximately 5 formations. After finding the first feasible path connect-
Hz, accounts for the dynamic obstacle (person), and updates ing the start with the goal position, we store (and display)
the parameters of the formation. A low-level controller is the subsequent shorter paths found by the algorithm. An
employed which, via high-frequency interpolation, drives advantage of the method is that large areas in free space are
the robots towards the desired formation. explored by each convex polytope, which reduces the need
We tested different configurations of the two boxes in for additional samples within.
our experimental space, covering all possible scenarios we
thought of (some examples are shown in Figure 14). In all
our experiments the robots could avoid collisions and reach
7. Discussion
the goal - as long as the human moved at a reasonable speed The method described in this paper showed good real-
below that of the robots and did not aggressively push them time performance and could successfully compute the opti-
against a wall. mal parameters for the multi-robot formation, while allow-
Several configurations of the two boxes, with the com- ing for reconfiguration. The method provided collision-free
puted global path, are shown in Figure 14. All of them were navigation among static and dynamic obstacles in simula-
computed in the order of below ten seconds. The initial tions with aerial vehicles and in experiments with mobile
configuration is in the lower part of the images and the manipulators.
goal configuration in the upper part. In each figure, we dis- At least in part, the computational efficiency and the
play the samples (black dots), the convex regions (blue), good scalability with respect to the number of robots in
the optimal formations within each intersection (green) and the formation is achieved by (a) not including the agent
the path. We stop the construction of the graph as soon dynamics in the formation optimization but handling them
as the first solution is found. We observe that in general, in the individual local planners (this works well for robots
very few iterations were required to find a feasible solu- with fast dynamics); and (b) considering the convex hull
tion, which is also of good quality thanks to the optimizer. of the formation. In fact, the number of variables and
1016 The International Journal of Robotics Research 36(9)

Fig. 13. Four consecutive snapshots of a 10 s avoidance maneuver where two mobile manipulators collaboratively carry a rigid object
and navigate to two goals (crosses) while avoiding collisions with static (gray) and dynamic (red hexagon) obstacles. The current state
of the two manipulators and the rigid object is displayed in blue and the target one (given by the optimization) in green. Two slices of
the convex polytope are shown, purple for the current time to = t (shown in the figure titles) and light green for time t1 = to + τ (the
intersection is the larger blueish region). The dynamic obstacle is shown at time to and it is moving at constant speed downwards. As
displayed, the red dynamic obstacle may intersect with the light green slice of the convex polytope (at t1 ), but not with the purple one
(at to ). The manipulators successfully navigate the rigid object through the two set points avoiding collisions. The initial, intermediate
and final setpoints are shown with dots, the currently active one in red, the others in black.

Fig. 14. Five examples of the global path planner for different scenarios with two static obstacles. The three mobile manipulators carry
an object and can rotate and translate while grasping. The initial and final formations are displayed in dark green. Light green formations
are additional nodes of the graph. The first feasible path is displayed with a solid black line. All samples (black dots), polytopes (blue)
and optimized formations (green) within the intersections of polytopes are shown. Typically, a feasible path is found with very few
samples.

Fig. 15. Three mobile manipulators collaboratively carry an object and navigate to a goal position in the other side of the room. The
global path planner guides them through the two static obstacles and they locally avoid the walking human. The robots successfully
adapt their formation to pass through the narrow opening and avoid the human.
Alonso-Mora et al. 1017

Fig. 16. Twelve different experiments where the three mobile manipulators collaboratively carry an object and navigate to a goal
position in the other side of the room. For each one of them, a snapshot while they traverse through the two obstacles is shown. This
experiment shows robustness to the location of the obstacles and that the robot formations vary across different execution runs. If the
obstacles leave enough free space, as is the case in the lower right corner experiment, the team of robots maintain the preferred formation
and do not need to rotate around their grasping points. Otherwise, they successfully adapt the configuration.

Fig. 17. Two examples of the global path planner connecting a start (lower left corner) with a goal (upper right corner) for two large
scenarios with many static obstacles. The three mobile manipulators carry an object and can rotate and translate while grasping. The
algorithm runs for a fixed amount of time. The first feasible path, and the feasible paths found in subsequent iterations that decrease
the cost, are displayed with a solid black line. All samples (black dots), polytopes (blue), and optimized formations (green) within the
intersections of polytopes are shown.

constraints of the formation. In fact, the number of vari- has four vertices, independently of the number of robots
ables and constraints of the formation control method is therein. For collaborative manipulators, the number of vari-
independent from the number of robots. The optimization ables (3 + 2n), and constraints (2n + m · nl · ( no + ni · n)),
problem in Equation (15) has eight variables for 3D motion scale linearly with the number of robots. In this case, no and
and ni · nl + 2 constraints, where ni is the number of ni are the number of vertices of the object and each robot,
vertices of the convex hull of robot positions in formation respectively.
i and nl is the number of sides in the convex polytope. We In our experiments, the computation of a large obstacle-
recall that the number of vertices of the convex hull depends free convex polytope following Section 2.4 showed very
on the shape of the formation, e.g. a square formation good results, but no guarantees exist that the best volume
1018 The International Journal of Robotics Research 36(9)

will be obtained. In fact, the method will converge to a the configuration of the team of robots within those convex
local optimum of the cost function, which is guaranteed regions. This comes at the expense of completeness, since
to be fully contained in free space. Searching over several in our approach we require that the robot team maintains
regions might prove advantageous. One may also consider a formation that does not intersect with obstacles, i.e. the
employing a faster, albeit suboptimal algorithm to quickly robots cannot maintain a formation while letting an obsta-
compute a convex region. cle pass through. In the event of dynamic obstacles, the
To compute the parameters of the multi-robot formation team may break the formation to let a moving obstacle pass
our method solves a nonlinear optimization via Sequential through, and come back to the original formation as soon
Convex Programming. This method converges to a local as there is enough free room.
optimum of the non-convex problem. Global optimality can Lastly, the method is general and can be adapted to other
only be guaranteed if the original optimization problem is high-dimensional problems or formation definitions. The
convex, which is typically not the case. For the non-convex core idea of the algorithms is to generate convex obstacle-
case, the number of iterations required to find a locally opti- free regions and then optimize the parameters of the for-
mal, or even feasible, solution is not defined. In practice, mation (i.e. the degrees of freedom of the high-dimensional
the method performed very well, quickly returning good configuration) such that the robots are fully contained in the
parameters for the formation in all cases where a valid convex region. The only requirements to adapt the method
formation could be fitted within the convex polytope. are (a) a function that converts configurations z to the
These observations also apply to the case of the global outer vertices of the formation V(z, f ) or high-dimensional
planner and no strong guarantees can be given for the gen- system, and (b) a way to compute derivatives with
eral non-convex optimization case. Thanks to the sampling respect to the configuration z (unless they are computed
of convex regions, the method will successfully explore numerically).
the whole workspace. For speed-up, as described in Algo-
rithm 2, we limit the sampling of regions to points outside
of the union of current convex regions in the graph. In most
8. Conclusion
scenarios this heuristic works well, but it can potentially In this paper, we showed that navigation of teams of
miss narrow openings, since, although the whole space is robots in formation among arbitrary static and dynamic
covered by convex regions, the intersection might not be obstacles can be achieved via a constrained nonlinear opti-
traversable. Two advantages of this method are (a) that mization. By first computing a large obstacle-free con-
sampling is performed in a low dimension space - the vex polytope and then optimizing the formation parame-
workspace - instead of in the high-dimensional configura- ters, low computational cost is achieved together with good
tion space and (b) that large areas of the free space are navigation results. In several simulations with aerial vehi-
explored/covered at once when contained within a convex cles navigating in 3D environments we showed success-
polytope. This has the potential to speed-up global path ful navigation in formation where robots may reconfigure
planning for formations of robots. the formation as required to avoid collisions and make
If the optimizations are feasible and a solution is found, progress.
the motion is guaranteed to be collision-free up to the time Our method can be applied both for real-time local nav-
horizon of the local planner, under the assumption that the igation in a dynamic environment and to compute global
moving obstacles maintain a constant speed. This is true paths in static environments. The global planner success-
because: (a) the convex region is fully contained in free fully combines a sampling-based method in the workspace
position-time space; (b) the robots at their initial position with nonlinear optimization for the remaining degrees of
and at the positions in the target formation are fully con- freedom of the formation, thus reducing the dimensionality
tained in the convex region and (c) the motion in between of the sampling problem.
the two formations as well, if the robots move in a straight For formation control, the approach scales to teams of
line (the linear combination of two points lies within the robots of arbitrary size, since only the convex hull of the for-
convex polytope). For mobile manipulators collaboratively mation is employed in the constrained optimization. Sim-
carrying an object, this is satisfied up to the interpolation. ulations with sixteen quadrotors -although more could be
Nonetheless, collisions with moving obstacles can still used - demonstrate this. The approach is general and can
arise if the assumptions are not met. For instance, if the also be adapted to other formation definitions and appli-
moving obstacles change the direction of motion quicker cations, as showed in our experiments with three mobile
than the robots can react, if the moving obstacles move too manipulators collaboratively carrying an object,
fast, if the planning horizon is not long enough, or if the In this work, we did allow for splitting and merging of
team of robots are trapped in a corner from where they robots, from/to a joint formation to/from individual nav-
cannot feasibly escape. igation. An interesting avenue for future work is that of
An advantage of the method is that planning is decoupled splitting and merging of the group formation into smaller
into: (a) finding convex regions in the lower dimensional sub-formations, or to maintain the formation while letting
free position-time space (R4 ) and (b) efficiently optimizing dynamic obstacles through, which is currently not possible.
Alonso-Mora et al. 1019

Additional avenues of future research include incorporat- Ayanian N, Kallem V and Kumar V (2011) Synthesis of feed-
ing the dynamic constraints of the robots in the nonlinear back controllers for multiple aerial robots with geometric con-
optimization problem and accounting for uncertainties in straints. In: IEEE/RSJ international conference on intelligent
the prediction of the movement of the dynamic obstacles. In robots and systems (IROS), San Francisco, CA, 2011, pp.
this work, the nonlinear dynamics of the robots were decou- 3126–3131. DOI: 10.1109/IROS.2011.6094943.
Ayanian N, Kumar V and Koditschek DE (2009) Synthesis of con-
pled from the formation control and accounted for by the
trollers to create, maintain, and reconfigure robot formations
individual controllers locally.
with communication constraints. In: International symposium
on robotics research (ISRR) 2009, Zurich, pp.625–642.
Acknowledgements Balch T and Arkin RC (1998) Behavior-based formation con-
trol for multirobot teams. IEEE Transactions on Robotics and
The authors are grateful to Robin Deits and Hongkai Dai from Automation 14(6): 926–939.
MIT for their help with IRIS, Drake and SNOPT. Balch T and Hybinette M (2000) Social potentials for scal-
able multi-robot formations. In: IEEE international confer-
ence on robotics and automation (ICRA) Symposia pro-
Funding ceedings, San Francisco, CA, 2000, pp. 73–80 vol. 1.DOI:
10.1109/ROBOT.2000.844042.
This work was supported in part by the MIT Lincoln Laboratory,
Barfoot TD and Clark CM (2004) Motion planning for forma-
SMARTS N00014-09-1051, pDOT ONR N00014-12-1-1000 and
tions of mobile robots. Robotics and Autonomous Systems 46:
the Boeing Company. 65–78.
Belta C and Kumar V (2004) Abstraction and control for groups
of robots. IEEE Transactions on Robotics 20(5): 865–875.
Notes
Bento J, Derbinsky N, Alonso–Mora J, et al. (2013) A message-
1. In our implementation we represent quaternions by vectors in passing algorithm for multi-agent trajectory planning. In:
R4 of unit norm and consider the additional constraint C3 = Advances in neural information processing systems NIPS. pp.
{||q||2 = 1}. 521–529.
2. We employ the implementation within the Drake toolbox, Bertsekas DP (1988) The auction algorithm: A distributed relax-
available in RobotLocomotion/drake/. ation method for the assignment problem. Annals of Operations
3. http://drake.mit.edu Research 14(1): 105–123.
Cheah CC, Hou SP and Slotine J (2009) Region-based shape
control for a swarm of robots. Automatica 45: 2406–2411.
Chen Y, Cutler M and How JP (2015) Decoupled multiagent
References
path planning via incremental sequential convex program-
Alonso–Mora J, Baker S and Rus D (2015a) Multi-robot nav- ming. In: IEEE international conference on robotics and
igation in formation via sequential convex programming. automation (ICRA), Seattle, WA, 2015, pp. 5954–5961. DOI:
In: IEEE/RSJ international conference on intelligent robots 10.1109/ICRA.2015.7140034.
and systems (IROS), Hamburg, 2015, pp. 4634–4641. DOI: Chen YQ and Wang Z (2005) Formation control: A review and
10.1109/IROS.2015.7354037. a new consideration. In: IEEE/RSJ international conference
Alonso–Mora J, Knepper RA, Siegwart R, et al. (2015b) Local on intelligent robots and systems, 2005, pp. 3181–3186. DOI:
Motion planning for collaborative multi-robot manipulation of 10.1109/IROS.2005.1545539.
deformable objects. In: IEEE international conference robotics Conner DC, Rizzi AA and Choset H (2003) Composition of local
and automation, Seattle, WA, 2015, pp. 5495–5502. DOI: potential functions for global robot control and navigation. In:
10.1109/ICRA.2015.7139967. IEEE/RSJ international conference on intelligent robots and
Alonso–Mora J, Montijano E, Schwager M, et al. (2016) Dis- systems (IROS) (Cat. No. 03CH37453), 2003, pp. 3546–3551
tributed multi-robot navigation in formation among obsta- vol. 3. DOI: 10.1109/IROS.2003.1249705.
cles: A geometric and optimization approach with con- Cortés J (2009) Global and robust formation-shape stabi-
sensus. In: IEEE international conference on robotics and lization of relative sensing networks. Automatica 45(12):
automation (ICRA), Stockholm, 2016, pp. 5356–5363. DOI: 2754–2762.
10.1109/ICRA.2016.7487747 Deits R and Tedrake R (2014) Computing large convex regions
Alonso–Mora J, Naegeli T, Siegwart R, et al. (2015c) Colli- of obstacle-free space through semidefinite programming.
sion avoidance for aerial vehicles in multi-agent scenarios. In: workshop on the algorithmic fundamentals of robotics,
Autonomous Robots 39(1): 101–121. Cham: Springer International Publishing, pp. 109–124, DOI:
Alonso–Mora J, Schoch M, Breitenmoser A, et al. (2012) 10.1007/978-3-319-16595-0_7.
Object and animation display with multiple aerial vehi- Deits R and Tedrake R (2015) Efficient mixed-integer planning for
cles. In: IEEE/RSJ international conference on intelligent UAVs in cluttered environments. In: IEEE international confer-
robots and systems, Vilamoura, 2012, pp. 1078–1083. DOI: ence on robotics and automation (ICRA), Seattle, WA, 2015,
10.1109/IROS.2012.6385551 pp. 42–49. DOI: 10.1109/ICRA.2015.7138978.
Augugliaro F, Schoellig AP and D’Andrea R (2012) Generation Derenick J, Spletzer J and Kumar V (2010) A semidefinite pro-
of collision-free trajectories for a quadrocopter fleet: A sequen- gramming framework for controlling multi-robot systems in
tial convex programming approach. In: IEEE/RSJ international dynamic environments. In: 49th IEEE conference on decision
conference on intelligent robots and systems, Vilamoura, 2012, and control (CDC), Atlanta, GA, 2010, pp. 7172–7177. DOI:
pp. 1917–1922. DOI: 10.1109/IROS.2012.6385823. 10.1109/CDC.2010.5717711.
1020 The International Journal of Robotics Research 36(9)

Derenick JC and Spletzer JR (2007) Convex optimization strate- LaValle SM (2006) Planning Algorithms. Cambridge: Cambridge
gies for coordinating large-scale robot formations. 47th IEEE University Press.
Transactions on Robotics 23: 1252–1259. LaValle SM and Kuffner JJ (2001) Randomized kinodynamic
Desai JP, Ostrowski JP and Kumar V (2001) Modeling and planning. The International Journal of Robotics Research
control of formations of nonholonomic mobile robots. IEEE 20(5): 378–400.
Transactions on Robotics and Automation 17(6): 905–908. Michael N and Kumar V (2008) Controlling shapes of ensem-
Dimarogonas DV and Johansson KH (2008) On the stability of bles of robots of finite size with nonholonomic constraints.
distance-based formation control. In: IEEE conference on deci- Proceedings of Robotics: Science and Systems IV, June 2008.
sion and control (CDC), Cancun, 2008, pp. 1200–1205. DOI: Michael N, Zavlanos MM, Kumar V, et al. (2008) Distributed
10.1109/CDC.2008.4739215. multi-robot task assignment and formation control. In: Pro-
Dimarogonas DV and Kyriakopoulos KJ (2005) Formation con- ceedings of the IEEE international conference on robotics
trol and collision avoidance for multi-agent systems and a and automation, Pasadena, CA, 2008, pp. 128–133. DOI:
connection between formation infeasibility and flocking behav- 10.1109/ROBOT.2008.4543197.
ior. In: Proceedings of the 44th IEEE conference on deci- Morgan D, Subramanian GP, Chung SJ, et al. (2016) Swarm
sion and control (CDC), 2005, Sevilla, pp. 84–89. DOI: assignment and trajectory optimization using variable-swarm,
10.1109/CDC.2005.1582135. distributed auction assignment and sequential convex program-
Dunbar WB and Murray RM (2002) Model predictive control of ming. The International Journal of Robotics Research 35(10):
coordinated multi-vehicle formations. In: Proceedings of the 1261–1285.
41st IEEE conference on decision and control2002, vol. 4, pp. Ogren P, Egerstedt M and Hu X (2001) A control Lyapunov func-
4631–4636. DOI: 10.1109/CDC.2002.1185108. tion approach to multi-agent coordination. In: Proceedings of
Egerstedt M and Hu X (2001) Formation constrained multi-agent the 40th IEEE conference on decision and control, Orlando,
control. IEEE Transactions on Robotics and Automation 17(6): FL, 2001, pp. 1150–1155, vol.2. DOI: 10.1109/.2001.981040.
947–951. Olfati–Saber R and Murray RM (2002) Distributed cooperative
Erdmann M and Lozano–Perez T (1987) On multiple moving control of multiple vehicle formations using structural poten-
objects. Algorithmica 2: 477–521. tial functions. In: IFAC world congress, vol. 35.1, pp. 495–500,
Fax JA and Murray RM (2004) Information flow and cooperative Barcelona.
control of vehicle formations. IEEE Transactions on Automatic Ren W and Sorensen N (2008) Distributed coordination archi-
Control 49(9): 1465–1476. tecture for multi-robot formation control. Robotics and
Fredslund J and Mataric MJ (2002) A general algorithm for Autonomous Systems 56(4): 324–333.
robot formations using local sensing and minimal communi- Sabattini L, Secchi C and Fantuzzi C (2011) Arbitrarily
cation. IEEE Transactions on Robotics and Automation 18(5): shaped formations of mobile robots: Artificial potential
837–846. fields and coordinate transformation. Autonomous Robots 30:
Gill PE, Murray W and Saunders MA (2002) SNOPT: An SQP 385–397.
algorithm for large-scale constrained optimization. SIAM jour- Saha I, Ramaithitima R, Kumar V, et al. (2014) Automated com-
nal on optimization 12(4): 979–1006. position of motion primitives for multi-robot systems from
Hsieh A, Kumar V and Chaimowicz L (2008) Decentralized con- safe LTL specifications. In: IEEE/RSJ international confer-
trollers for shape generation with robotic swarms. Robotica 26: ence on intelligent robots and systems, Chicago, IL, 2014, pp.
691–701. 1525–1532. DOI: 10.1109/IROS.2014.6942758.
Kallem V, Komoroski AT and Kumar V (2011) Sequential com- Sugar TG and Kumar V (2002) Control of cooperating mobile
position for navigating a nonholonomic cart in the presence of manipulators. IEEE Transactions on Robotics and Automation
obstacles. IEEE Transactions on Robotics 27(6): 1152–1159. 18(1): 94–103.
Khatib O, Yokoi K, Chang K, et al. (1996) Vehicle/arm coordi- Tang CP, Bhatt R and Krovi VN (2004) Decentralized kinematic
nation and multiple mobile manipulator decentralized coop- control of payload transport by a system of mobile manip-
eration. In: Proceedings of the 1996 IEEE/RSJ international ulators. In: IEEE international conference on robotics and
conference on intelligent robots and systems Osaka, 1996, vol. automation 2004, pp. 2462–2467, vol. 3, New Orleans. DOI:
2, pp. 546–553. DOI: 10.1109/IROS.1996.570849. 10.1109/ROBOT.2004.1307430.
Kloder S and Hutchinson S (2006) Path planning for permutation- Tanner HG, Jadbabaie A and Pappas GJ (2007) Flocking in
invariant multirobot formations. IEEE Transactions on fixed and switching networks. IEEE Transactions on Automatic
Robotics 22(4): 650–665. Control 52(5): 863–868.
Krontiris A, Louis S and Bekris KE (2012) Multi-level forma- Tanner HG, Loizou SG and Kyriakopoulos KJ (2003) Nonholo-
tion roadmaps for collision-free dynamic shape changes with nomic navigation and control of cooperating mobile manipu-
non-holonomic teams. In: IEEE international conference on lators. IEEE Transactions on Robotics and Automation 19(1):
robotics and automation (ICRA) Saint Paul, MN, 2012, pp. 53–64.
1570–1575. DOI: 10.1109/ICRA.2012.6225372. Yu J and LaValle SM (2013) Fast, near-optimal computation for
Kuhn HW (1955) The Hungarian method for the assignment prob- multi-robot path planning on graphs. In: AAAI conference on
lem. In: Naval Research Logistics. pp. 83–97 Wiley Subscrip- artificial intelligence, late breaking papers, July, Washington.
tion Services, Inc. DOI: 10.1002/nav.3800020109. Zhou D and Schwager M (2015) Virtual rigid bodies for coor-
Kushleyev A, Mellinger D and Kumar V (2012) Towards a dinated agile maneuvering of teams of micro aerial vehi-
swarm of agile micro quadrotors. Autonomous Robots 35(4): cles. In: IEEE international conference on robotics and
287–300. automation (ICRA), Seattle, WA, 2015, pp. 1737–1742. DOI:
Latombe JC (1991) Robot Motion Planning. Boston: Kluver. 10.1109/ICRA.2015.7139422.
Alonso-Mora et al. 1021

Appendix: Collision avoidance velocity space. Therefore, the convex region needs to be
converted to an equivalent region in velocity space. Given
In this appendix, we provide a description of the method for
the time horizon τ of the planner, this is formally
collision avoidance employed with the aerial vehicles. We
implement the convex optimization introduced by Alonso– r
P u ( p, ε) := (Ppiσ (i) ( R3 \ Ō ) −pi ) /τ (27)
Mora et al. (2015c) with identical motion constraints and
constraints for avoidance of other agents. This approach r
where each linear constraint defining Ppiσ (i) ( R3 \ Ō ) is
adapts to changes in the environment, avoids moving obsta-
expressed relative to the current position of the robot and
cles, and respects the dynamics of the robot via a set of
is divided by the time horizon. In particular, if the robot
motion primitives. Each motion primitive was defined to
selects a reference velocity that satisfies this constraint,
track a constant reference velocity with a robot-specific
i.e. u ∈ P u ( p, ε), then all future positions up to the time
controller. r
We extend the method towards environments with com- horizon τ are within Ppiσ (i) ( R3 \ Ō ). This polytope is then
plex static obstacles. In particular, using the convex poly- included in the distributed convex optimization of Alonso–
tope computation described in Section 2.4, we add a new Mora et al. (2015c).
constraint to guarantee that the motion of the robot is If the target position rσ (i) of robot i is within its line of
within the obstacle-free workspace F. Following the nota- sight (this is the case if z ⊂ Pfo →g ), then the collision
tion of Alonso–Mora et al. (2015c), the additional con- avoidance algorithm successfully drives the robot towards
straint for avoidance of static obstacles is computed as it. Otherwise, a global planner, such as the ones proposed
follows. by Bento et al. (2013) or Yu and LaValle (2013), can be
Denote by Ō = {p ∈ R3 | A (p) ∩ O = ∅} the used for guidance.
set of static obstacles dilated by the robot volume plus a
r
small value ε > 0. A convex polytope Ppiσ (i) ( R3 \ Ō ) ⊂ Index to multimedia extension
R3 is computed following Section 2.4. This polytope is in
Archives of IJRR multimedia extensions published prior
obstacle-free space, contains the initial position pi of the
to 2014 can be found at http://www.ijrr.org, after 2014
robot, and is directed towards the robot’s goal position rσ (i)
all videos are available on the IJRR YouTube channel at
in the new formation.
http://www.youtube.com/user/ijrrmultimedia
In Alonso–Mora et al. (2015c) the collision avoidance
algorithm was formulated as a constrained optimization in