Swarm Assignment and Trajectory
Optimization Using Variable-Swarm,
Distributed Auction Assignment and
Sequential Convex Programming
The International Journal of Robotics
Research
35(10):1–32
c
The Author(s) 2016
Reprints and permission:
sagepub.co.uk/journalsPermissions.nav
DOI: 10.1177/ToBeAssigned
www.sagepub.com/
Daniel Morgan
1
, Giri P. Subramanian
1
, Soon-Jo Chung
1
, and Fred Y. Hadaegh
1
Abstract
This paper presents a distributed, guidance and control algorithm for reconfiguring swarms composed of hundreds
to thousands of agents with limited communication and computation capabilities. This algorithm solves both the
optimal assignment and collision-free trajectory generation for robotic swarms, in an integrated manner, when given
the desired shape of the swarm (without pre-assigned terminal positions). The optimal assignment problem is solved
using a distributed auction assignment that can vary the number of target positions in the assignment, and the
collision-free trajectories are generated using sequential convex programming. Finally, model predictive control is used
to solve the assignment and trajectory generation in real time using a receding horizon. The model predictive control
formulation uses current state measurements to resolve for the optimal assignment and trajectory. The implementation
of the distributed auction algorithm and sequential convex programming using model predictive control produces the
Swarm Assignment and Trajectory Optimization (SATO) algorithm that transfers a swarm of robots or vehicles to a
desired shape in a distributed fashion. Once the desired shape is uploaded to the swarm, the algorithm determines
where each robot goes and how it should get there in a fuel-efficient, collision-free manner. Results of flight experiments
using multiple quadcopters show the effectiveness of the proposed SATO algorithm.
1 Introduction
Motion planning, often called guidance in the aerospace community, and feedback control of multi-agent systems
have been a major area of research over the past decades. The majority of this work focused on swarm
robotics Jadbabaie et al. (2003); Earl and D’Andrea (2005); Kingston and Egerstedt (2010); Milutinovi´c and
Lima (2006); Cheah et al. (2009); Zhao et al. (2011); Hsieh et al. (2008); Alonso-Mora et al. (2012); Yu et al.
(2015). However, in recent years the idea of multi-agent systems has been extended to spacecraft Scharf et al.
(2003, 2004); Alfriend et al. (2009); Breger and How (2007); Vaddi et al. (2005); Campbell (2003, 2005); Zanon
and Campbell (2006); Hadaegh et al. (2016). The most recent idea is to fly a swarm containing a large number
(hundreds to thousands) of femtosatellites (100-gram-class spacecraft) Hadaegh et al. (2016).
For a swarm to have a cost benefit compared to a monolithic agent or a smaller formation, the individual
agents need to be smaller and cheaper. Due to their small size and low cost, the agents in a swarm have limited
actuation, communication, and computation capabilities, which require the guidance and control algorithms
1
University of Illinois at Urbana-Champaign, USA
2
Jet Propulsion Laboratory, California Institute of Technology, USA
Corresponding author:
Soon-Jo Chung, Department of Aerospace Engineering & Coordinated Science Laboratory, University of Illinois at Urbana-Champaign,
Urbana, IL, 61801, USA
Email: sjchung@alum.mit.edu
Prepared using sagej.cls [Version: 2015/06/09 v1.01]
2 The International Journal of Robotics Research 35(10)
of the swarm to be both fuel and computationally efficient. Swarms of agents create interesting challenges
in guidance and control due to the large number of agents, the small size of each individual agent, and the
complicated dynamics. Specifically, the large number of vehicles moving in 3-D makes collision avoidance a
major challenge Morgan et al. (2012, 2014). Also, the limited computation and communication capabilities of
each agent require the swarm reconfiguration algorithm to be very simple so that it can be run on board each
small robot or vehicle in real time.
The swarm reconfiguration problem consists of two parts: assignment and trajectory generation. The
assignment problem consists of finding the optimal mapping from a set of agents to a set of targets or tasks in
order to minimize the total cost of interest. This problem has been well researched and many methods exist for
finding the optimal assignment, including the Hungarian algorithm Kuhn (1955) and iterative methods Bertsekas
(1981); Hung (1983). The drawback to many of these algorithms is that they are centralized with respect to
communication and computation. On the other hand, the auction algorithm Bertsekas and Castanon (1991);
Bertsekas (1992) possesses a distributed nature suitable for distributed systems. More recent research on the
auction algorithm Zavlanos et al. (2008); Choi et al. (2009) has shown that it can be implemented in a distributed
manner. In contrast with the prior work, one important aspect of the assignment algorithm in this paper is its
versatility to adapt to information, specifically collision avoidance and disconnected communication networks.
The second part of the swarm reconfiguration is optimal trajectory generation and collision avoidance.
This part requires an algorithm that can solve the nonlinear optimization that minimizes the cost of the
trajectory while satisfying collision avoidance and dynamic constraints. The trajectory optimization has
been solved using a variety of methods, including mixed integer linear programming Richards et al. (2003),
pseudospectral methods Ross and Fahroo (2003), convex programming Schulman et al. (2013), and sequential
convex programming (SCP) Morgan et al. (2014).
In this paper, we simultaneously solve both the target assignment and trajectory optimization problems
using a variable-swarm, distributed auction assignment (VSDAA), which allows the swarm size to change over
time, to solve the assignment problem and our prior work on SCP Morgan et al. (2014) to solve the trajectory
optimization. Additionally, we integrate these algorithms using model predictive control (MPC) Bemporad and
Morari (1999); Mayne et al. (2000) in order to run the algorithms in real time and on swarms with distributed
communication networks. The integration with the trajectory optimizer is what allows the auction algorithm
the ability to adjust its assignment based on collisions that were previously undetected.
The contributions of this paper, compared with the existing literature including our own work Morgan
et al. (2014), are in the development of VSDAA, its integration with trajectory optimization in the MPC-
SCP formulation, and the convergence proof for SCP as well as in the experimental validation using multiple
quadrotors. The variable-swarm characteristic means that VSDAA can adapt the number of targets in the
assignment to match the number of agents. This is incredibly useful when the number of agents in the swarm
changes. In the case of a significant loss of agents due to an external object or fuel/battery depletion, VSDAA
would adjust the number of targets to match the number of remaining agents and the agents would fill in the
gap left by the external object. This allows the swarm to handle the loss of a significant number of agents and
still maintain the desired shape. On the other hand, VSDAA will also increase the number of targets if there are
more agents than targets. This is a situation that will break a typical auction algorithm Bertsekas and Castanon
(1991); Bertsekas (1992); Zavlanos et al. (2008); Choi et al. (2009) since the agents cannot all be assigned to a
target so they bid indefinitely. Also, it should be noted that VSDAA can handle an arbitrary distribution shape
of targets, as opposed to the use of uniform target distributions Yu et al. (2015).
Another contribution is that the SCP method is shown to converge to a Karush-Kuhn-Tucker (KKT)
point Boyd and Vandenberghe (2004); Ruszczynski (2006) of the nonconvex program. This proof shows that
once a feasible solution is found, the sequence of optimal solutions resulting from the convex programs will
converge. Additionally, the trajectory to which they converge satisfies the KKT conditions, which are necessary
conditions for an optimal trajectory.
Additionally, the implementation of VSDAA with SCP using MPC allows an assignment to be achieved even
in a disconnected communication network. Since the assignment is updated throughout the reconfiguration,
the distance-based, swarm communication network will be different every time an assignment is computed.
Therefore, the agents do not need to be fully connected to every other agent at all times. In fact, if two agents
from separate, disconnected networks are assigned to the same target, they will eventually move close enough
to become connected and will be assigned to different targets.
Prepared using sagej.cls
3
(a) Visualization of a swarm reconfiguration
1
4
2
3
5
7 8
6
Disconnected Agents
Connected Agents Neighboring Agents
(b) Visualization of connected and disconnected
networks. Dashed lines represent communication links.
Figure 1. Visualization of problem statement and communication networks
The result of the MPC implementation of VSDAA and SCP is the swarm assignment and trajectory
optimization (SATO) algorithm. SATO, the main algorithm of this paper, is distributed in both communication
and computation, and provides near-optimal, collision-free trajectories for swarm reconfiguration. In contrast
to other methods that simultaneously solve the assignment and trajectory optimization Turpin et al. (2014),
SATO uses SCP as underlying trajectory optimization, which allows it to handle complex dynamic environments.
Additionally, this algorithm works with disconnected communication networks and is robust to the loss or gain
of a significant number of agents. Finally, the model predictive control formulation allows SATO to run on
board each agent and provides robustness to unmodeled disturbances. A preliminary version of this paper was
previously presented at a conference Morgan et al. (2015), but this paper adds significant results of experimental
validation along with detailed description of dynamic modeling and nonlinear control design.
The paper is organized as follows. In Sec. 2, the swarm reconfiguration problem is formulated as a
constrained, nonlinear optimal control problem and converted to a nonlinear optimization. In Sec. 3, the swarm
reconfiguration problem is broken into an assignment problem and a trajectory optimization problem. Then,
the assignment is solved using VSDAA. In Sec. 4, the trajectory optimization problem is converted to a convex
optimization and the SCP algorithm is described. Additionally, the SCP algorithm is shown to converge to a
trajectory that satisfies the KKT conditions of the nonconvex problem. In Sec. 5, MPC is used to integrate
VSDAA and SCP, and to implement a finite horizon so that the resulting algorithm, SATO, can be run on
board each agent in real time with a disconnected communication network. In Sec. 6, SATO is run for both a
2-D, double integrator dynamics scenario and a 3-D, relative orbit dynamics scenario. The results of the two
scenarios are analyzed and discussed. In Sec. 7, we elaborate on our experimental setup, the feedback control
law design to track the desired trajectories obtained by SATO, and the experimental results.
2 Problem Statement
In this section, the optimal swarm reconfiguration is presented as a continuous, finite horizon optimal control
problem. The swarm reconfiguration involves the transfer of hundreds to thousands of agents from their current
shape to a desired shape while satisfying various constraints, such as collision avoidance, and minimizing the
total fuel used during the transfer. A visualization of a swarm reconfiguration and communication networks is
shown in Fig. 1.
Figure 1a shows a visualization of the swarm reconfiguration problem. The agents begin at their current
positions (green) and move towards the desired formation (red). The agents are interchangeable so any agent
can go to any target. In Figure 1b, the connectedness of various agents is shown. The dashed arrows represent
communication links between the agents and the different colors represent different communication networks. In
other words, the blue agents are connected to the other blue agents (agents 1 and 4) but not to the red agents
(agents 2 and 5). Two agents that have a communication link between them are called neighboring agents (agents
7 and 8).
Prepared using sagej.cls
4 The International Journal of Robotics Research 35(10)
2.1 Nonlinear Optimal Control Problem
The objective of the optimal swarm reconfiguration is to minimize the L
1
-norm of the control input. Therefore,
we can define the swarm reconfiguration as follows
Problem 1 (Constrained, Nonlinear Optimal Control).
min
u
j
(t),j=1,...,N
N
X
j=1
Z
t
f
0
ku
j
(t)k
q
dt subject to (1)
˙x
j
(t) = f(x
j
(t)) + Bu
j
(t)) ∀t ∈ [0, t
f
], j = 1, . . . , N (2)
ku
j
(t)k
r
≤ U
max
∀t ∈ [0, t
f
], j = 1, . . . , N (3)
kG[x
j
(t) − x
i
(t)]k
2
≥ R
col
∀t ∈ [0, t
f
], i < j, j = 1, . . . , N − 1 (4)
x
j
(0) = x
j,0
, j = 1, . . . , N (5)
x
j
(t
f
) ∈ X
f
, j = 1, . . . , N (6)
where B = [0
3×3
I
3×3
]
T
, G = [I
3×3
0
3×3
], x
j
= (`
T
j
,
˙
`
T
j
)
T
, `
j
∈ R
n
is the position vector of agent j, x
j,0
is
the initial state of agent j, u
j
is the control vector of agent j, and N is the number of agents in the swarm. (2)-
(5) represent the dynamics constraint, maximum control constraint, collision avoidance constraint, and initial
state constraint, respectively, with U
max
being the maximum control magnitude and R
col
being the minimum
allowable distance between two agents. (6) represents the terminal state constraint with X
f
being a set of M
discrete points in R
n
. This constraint is what introduces the need for solving for the optimal assignment and
differentiates this paper from our prior work Morgan et al. (2014) where individual terminal state assignments
were given.
Remark 1 (Norms). The norms used in (1) and (3), k · k
q
and k · k
r
, respectively, are dependent on the hardware
used on board the agents. In this paper, we use q = 1 and r = ∞. However, the convex optimizations are valid
for q, r ∈ {1, 2, ∞}.
Remark 2 (Fixed Terminal Time). The optimizations used in this paper all have a fixed terminal time. This
is due to the fact that the swarm is trying to reconfigure to a specific shape so the agents need to arrive at
their terminal positions at the same time. If some agents arrive earlier than others, they will either drift off of
their target position or require extra cost to maintain their position in the presence of dynamics. Additionally,
the trajectories are generally cheaper for longer reconfiguration times so having some agents arrive earlier than
others usually increases the cost for those agents arriving early.
2.2 Convexification of Differential Equations
In this section, the dynamics constraints in (2) are converted to affine equality constraints. This is done by
linearizing (2) and discretizing Problem 1. This results in a finite number of linear equality constraints, which
are acceptable in a convex programming problem.
In order to rewrite the dynamics in (2) as a constraint that can be used in a convex programming problem,
these equations must first be linearized about the nominal trajectory x
0
j
. Linearizing (2) yields
˙
x
j
= A(x
0
j
)x
j
+ Bu
j
+ z(x
0
j
) (7)
where A(x
0
j
) =
∂f
∂x
j
x
0
j
and z(x
0
j
) = f(x
0
j
) −
∂f
∂x
j
x
0
j
x
0
j
.
The next step in the process of converting (2) into a constraint that can be used in convex programming is to
convert the ordinary differential equation in (7) to a finite number of algebraic constraints. In order to do this,
the problem is discretized using a zero-order-hold approach such that
u
j
(t) = u
j
[k], t ∈ [t
k
, t
k+1
), k = k
0
, . . . , T −1 (8)
Prepared using sagej.cls
5
where t
f
= T ∆t, T is the number of discrete time steps, t
0
= 0, t
T
= t
f
, and ∆t = t
k+1
− t
k
for k = k
0
, . . . , T −1.
This method of discretization reduces (7) to
x
j
[k + 1] = A
j
[k]x
j
[k] + B
j
[k]u
j
[k] + z
j
[k], k = k
0
, . . . , T −1, j = 1, . . . , N (9)
where x
j
[k] = x
j
(t
k
), u
j
[k] = u
j
(t
k
), and
A
j
[k] =e
A(x
0
j
(t
k
))∆t
, B
j
[k] =
Z
∆t
0
e
A(x
0
j
(t
k
))τ
B dτ, z
j
[k] =
Z
∆t
0
e
A(x
0
j
(t
k
))τ
z(x
0
j
(t
k
))dτ (10)
Now that the nonlinear, continuous-time equations of motion from (2) have been rewritten as linear, finite
dimensional constraints in (9), they can be used in a convex programming problem. The constraints from (3)-(6)
can be written in discretized form as
ku
j
[k]k
∞
≤ U
max
k = k
0
, . . . , T −1, j = 1, . . . , N (11)
kG(x
j
[k] − x
i
[k])k
2
≥ R
col
k = k
0
, . . . , T, i < j, j = 1, . . . , N − 1 (12)
x
j
[0] = x
j,0
, j = 1, . . . , N (13)
x
j
[T ] ∈ X
f
, j = 1, . . . , N (14)
Note that the only constraints that do not satisfy the requirements of convex programming are (12) and (14).
These constraints will be modified in the following sections so that the problem can be efficiently solved using
convex programming.
3 Distributed Optimal Target Assignment
In this section, the nonlinear optimal control problem (Problem 1) is broken into two parts: an optimal
assignment problem and an optimal trajectory-planning problem. This separation allows us to rewrite the
terminal constraints in (14), which are nonconvex and require the problem to include integer variables. By
solving an assignment problem to determine the terminal states of each agent, the remaining trajectory-planning
problem can be approximated by a convex program and efficiently solved.
Claim 1 (Assignment). If the terminal set (X
f
) is a set of points with every pair of points separated by a safe
distance (R
col
), then the constraints x
j
[T ] ∈ X
f
(14) and kG(x
j
[T ] − x
i
[T ])k
2
≥ R
col
((12) at k = T ) can be
equivalently written as
x
j
[T ] ∈ X
f
, x
j
[T ] 6= x
i
[T ], ∀j 6= i (15)
Now, the assignment problem can be written as shown below.
Problem 2 (Assignment Problem).
min
x
j,f
, j=1...N
N
X
j=1
C(x
j,0
, x
j,f
) subject to (16)
x
j,f
∈ X
f
, x
j,f
6= x
i,f
, ∀j = 1 . . . N, ∀i 6= j
where C(x
0
, x
f
) is the cost required for an agent to go from x
0
to x
f
.
The solution to the Assignment Problem (Problem 2) will yield the desired terminal points for each agent
(x
j,f
), which are then used to formulate the following terminal constraint for the trajectory optimization problem.
x
j
[T ] = x
j,f
, j = 1, . . . , N (17)
The resulting trajectory optimization can be written as follows:
Problem 3 (Trajectory Optimization).
Prepared using sagej.cls
