# High-Speed Motion Planning for Aerial Swarms in Unknown and Cluttered
Environments

Charbel Toumieh, Dario Floreano The authors are with the Laboratory of
Intelligent Systems, Ecole Polytechnique Federale de Lausanne (EPFL), CH1015
Lausanne, Switzerland (e-mail<EMAIL_ADDRESS>dario.floreano@epfl.ch).This work was supported by the Swiss National Science
Foundation (SNSF) with grant number 200020_212077. Video:
https://youtu.be/Af8mODuES4sCode: https://github.com/lis-epfl/multi_agent_pkgs

###### Abstract

Coordinated flight of multiple drones allows to achieve tasks faster such as
search and rescue and infrastructure inspection. Thus, pushing the state-of-
the-art of aerial swarms in navigation speed and robustness is of tremendous
benefit. In particular, being able to account for unexplored/unknown
environments when planning trajectories allows for safer flight. In this work,
we propose the first high-speed, decentralized, and synchronous motion
planning framework (HDSM) for an aerial swarm that explicitly takes into
account the unknown/undiscovered parts of the environment. The proposed
approach generates an optimized trajectory for each planning agent that avoids
obstacles and other planning agents while moving and exploring the
environment. The only global information that each agent has is the target
location. The generated trajectory is high-speed, safe from unexplored spaces,
and brings the agent closer to its goal. The proposed method outperforms four
recent state-of-the-art methods in success rate (100% success in reaching the
target location), flight speed (67% faster), and flight time (42% lower).
Finally, the method is validated on a set of Crazyflie nano-drones as a proof
of concept.

###### Index Terms:

aerial swarms, motion planning, obstacle avoidance, high-speed navigation

##  I Introduction

The ability of aerial swarms to rapidly fly through cluttered environments
while avoiding each other and any other obstacles could result in faster
mission accomplishment and increase area coverage in inspection tasks, aerial
logistics, or search for rescue missions. However, current approaches to
coordinated flight of multiple drones cannot leverage the maximum possible
speed of the drones and do not meet the speed conditions for the longest
flight distance [1]. Although recent work on drone racing achieved human-
competitive flight speed through gates [2][3], those trajectory planning
algorithms apply to a single drone, assume knowledge of the environment, and
do not generalize well to flight in environments with unknown obstacle
layouts.

Figure 1: Multiple agents (green spheres) moving towards each other in a
simulation environment where the obstacles are occupied voxels in orange. The
Safe Corridor of each agent is shown in red, the previous positions as a green
line, and the predicted future positions (MIQP/MPC solution) as the yellow
line.

While early methods of trajectory planning of aerial swarm in cluttered
environments were centralized [4], more recent work focused on decentralized
approaches where each drone can compute its trajectory based on local
information [5][6][7], and thus better scale up in computation time and
communication range. Consequently, here we discuss the state-of-the-art in
decentralized approaches that produce coordinated flight trajectories in
cluttered environments (Tab. I). These methods fall into two categories
depending on inter-drone communication mode: asynchronous or synchronous.

Asynchronous approaches do not require periodic communication between agents.
EGO-Swarm [7], an extension of a gradient-based planner for a single agent
[8], is a recent example of an asynchronous multi-drone planner. However, it
cannot handle communication delay between the agents and assumes perfect
knowledge of occluded obstacles. EGO-Swarm2 [9] uses the MINCO [10] trajectory
parametrization instead of B-Splines [11] of EGO-Swarm to produce smoother
trajectories and a lower optimization time. However, it cannot handle
communication delay and it flies slower in unknown environments [12]. EDG-Team
[13] improves over EGO-Swarm2 by dealing with deadlocks between multiple
agents that pass through narrow gaps. This is achieved by switching the method
to a centralized and synchronous planner in dense environments. The method was
proven to be more robust to communication delay but could not guarantee
collision-free navigation. MADER [6] and its delay-robust version RMADER [14]
are another family of asynchronous planners. They both use the MINVO basis
[15] to generate trajectories that can pass through narrow gaps. Both
approaches assume perfect knowledge of the obstacle positions and shapes
within a bounding box around each agent and do not account for the unknown
part of the environment, which could lead to collisions. DREAM [16] is another
asynchronous planning method for aerial swarms that minimizes collision
probabilities, but cannot guarantee safety in unknown environments. MRNAV [17]
was built on top of DREAM to provide collision-free and deadlock-free flight
using a centralized, long-horizon planning module. MRNAV retains communication
delay robustness of DREAM but still does not guarantee safety in unknown
environments.

TABLE I: Comparison of recent motion planning methods according to key
properties: asynchronous or synchronous (Async); decentralized or centralized
(Decentr.); handling of communication delay between agents (Delay); explicit
handling of unknown parts of the environment (Unkn.).

Method | Async | Decentr. | Delay | Unkn.
---|---|---|---|---
EGO-Swarm [7] | Yes | Yes | No | No
EGO-Swarm2 [9] | Yes | Yes | No | No
EDG-Team [13] | Yes$\mid$No1 | Yes$\mid$No1 | No | No
MADER [6] | Yes | Yes | No | No
RMADER [18] | Yes | Yes | Yes | No
DREAM [16] | Yes | Yes | Yes | No
MRNAV [17] | Yes$\mid$No2 | Yes$\mid$No2 | Yes | No
AMSwarmX [19] | No | Yes | No | No
DPDS [20] | No | Yes | No | No
LSC [21] | No | Yes | No | No
DLSC [22] | No | Yes | No | No
decMPC [5] | No | Yes | Yes | No
decMPC2 [23] | No | Yes | Yes | No
HDSM (proposed) | No | Yes | Yes | Yes

  * 1

In dense environments, EDG-Team switches to a centralized and synchronous
planner that executes joint optimization.

  * 2

The long horizon module runs on a centralized system that communicates with
the planning robots periodically.

In contrast to asynchronous methods, synchronous approaches require periodic
communication of the trajectories between all agents, which can result in
faster trajectories [5][23]. AMSwarmX [19] is a synchronous method that uses
Bernstein polynomials as trajectory parametrization. However, it assumes no
communication delays between the agents as well as prior knowledge of the
environment. The same assumptions are made by DPDS [20], which uses a
discretized trajectory and MPC optimization for obstacle avoidance where
obstacles are represented by mathematical functions (e.g. cylinders,
paraboloids). Some synchronous planning methods are based on linear safe
corridors, which use separating hyperplanes to guarantee collision avoidance
with static obstacles and the other planning agents, such as LSC [24] and its
extension to dynamic obstacles DLSC [22]. However, these methods do not
account for communication delay and assume prior knowledge of the environment.

Here we propose a high-speed decentralized and synchronous motion (HDSM)
planning method for aerial swarms that can operate in cluttered and unknown
environments with a guarantee of collision-free navigation (Fig. 1). The
method builds on our previous work for aerial swarm motion planning [5] and
its extension to arbitrary communication delays [23]. However, those methods
were unable to deal with unknown environments, unlike the proposed approach.
Furthermore, the method described here introduces novelties that reduce
trajectory lengths, allow drones to fly through narrow gaps, and adapt flight
speed to environment density. Taken together, all these improvements
substantially increase flight speed while producing collision-free
trajectories. When compared to four recent and open-source methods that
outperform all other state-of-the-art approaches (Sect. III-B), the method
described here results in 67% higher flight speed and 42% lower flight time
with 100% mission success rate. Finally, while all state-of-the-art approaches
(Tab. I) are implemented in ROS1 or Matlab, the method described here takes
advantage of ROS2 peer-to-peer communication and other real-time and security
features [25].

Agent in environmentPoint cloud from sensorVoxel grid generationVoxel
gridGlobal path Run at constant period $\displaystyle T_{\text{map}}$ Run at
constant period $\displaystyle T_{\text{path}}$ Run at constant period
$\displaystyle T_{\text{traj}}$ Free unknown voxelsInflate obstacles Create
potential field and find a path to the goal Occupy unknown voxelsInflate
obstacles Generate Time-Aware Safe Corridors around the path Sample the global
pathSolve for optimal trajectoryTime-Aware Safe CorridorRef. trajectoryOptimal
trajectoryAgent positionObstaclesPoint cloudFree voxelOccupied voxelUnknown
voxelOccupied voxel due to inflationPotential fieldGlobal pathGoal
positionConvex polyhedronReference trajectoryOptimal
trajectoryMappingPlanningGlobal path generationTrajectory generationTASC
generationReference generationMIQP/MPC solverTrajectories of other agentsGoal
Figure 2: The mapping and planning modules are shown in 2D for the sake of
clarity. The mapping framework takes a point cloud generated at any time $t$
by sensors and produces a voxel grid with free, occupied, and unknown voxels.
It is run at constant period $T_{\text{map}}$. The voxel grid is then fed to
the planning framework. The planning framework consists of 2 modules each
running at a different frequency and on a different CPU thread: the global
path generation module (running at period $T_{\text{path}}$) which takes the
voxel grid and generates a global path to the goal; the trajectory generation
module (running at period $T_{\text{traj}}$) which takes the voxel grid and
the global path to generate a collision-free and dynamically-feasible optimal
trajectory.

##  II The HDSM Method

###  II-A Overview

The HDSM method consists of two modules that run in parallel on each of the
$M$ drones that make the aerial swarm: the mapping module and the planning
module (Fig. 2). The mapping module takes in a depth point cloud from sensors
and generates a voxel grid representation of the environment. The voxel grid
is partitioned into free, occupied, and unknown voxels. The module is updated
at a period $T_{\text{map}}$. The voxel grid is given to the planning module
which consists of two sub-modules that run in parallel (Fig. 3). The first
sub-module takes the voxel grid as input and generates a global path between
the current position and the target position at a period $T_{\text{path}}$.
The second sub-module takes as inputs the voxel grid, the global path, and the
trajectories of the other drones, and generates a collision-free and
dynamically feasible trajectory at a period $T_{\text{traj}}$. Running the
path generation and trajectory generation in parallel allows each agent more
time to share its trajectory with other drones before the next planning
iteration begins (Fig. 3). The periods $T_{\text{map}}$ and $T_{\text{path}}$
are usually chosen equal to the sensor measurement period, whereas the period
$T_{\text{traj}}$ is chosen to be smaller than $T_{\text{path}}$.

MIQP/MPCRefTASCPathMIQP/MPCRefTASCPathPathCPUiteration
$\displaystyle\mathnormal{k}$iteration $\displaystyle\mathnormal{k}+1$
timemax. com. latencytimepath planning periodpath planning periodthread
1thread 0 Figure 3: In the proposed framework, the path planning is done on a
separate thread (thread 1), and is run at a potentially different frequency
than the trajectory planning thread (thread 0).

###  II-B The Mapping Module

The objective of the mapping module is to provide the most up-to-date
representation of the local environment in the form of a voxel grid given all
the previous sensory measurements. It relies on a depth point cloud from
360-degree sensors, which can be produced by several commercial drones (Skydio
[26], DJI [27], e.g.). Fig. 4 illustrates the mapping module steps. It takes
as input the most recent point cloud and transforms it into a voxel grid
$G_{\text{meas}}$, which can be generated by GPU accelerators for lower
latency [28][29], which is then merged with the latest computed voxel grid
$G_{\text{last}}$. Each grid is a cuboid of fixed dimensions and of fixed
voxel size $l_{\text{vox}}$ where each voxel is a cube. The user selects the
dimensions and voxel size to balance between the time it takes to compute the
voxel grid and the extent of the area mapped. The voxel grid’s origin
coordinates ($x,y,z$) are chosen according to the following rules: each
coordinate of the origin is a multiple of $l_{\text{vox}}$ and the voxel at
the center of the grid contains the current position of the agent. This makes
sure that as the grid moves with the agent, individual voxels can be compared
and merged in a one-to-one fashion between $G_{\text{meas}}$ and
$G_{\text{last}}$. Every time a new point cloud is produced by the sensors, a
grid full of unknown voxels $G_{\text{meas}}$ is generated (measurement grid).
First, all the voxels that contain at least a point of the point cloud are set
to occupied. Raycasting is then performed from the center of the grid to every
voxel on the borders of the grid, freeing all the voxels on the raycasted line
until an occupied voxel is encountered. Finally, the unknown voxels of
$G_{\text{meas}}$ are replaced with their value from $G_{\text{last}}$, and
$G_{\text{last}}$ is set to $G_{\text{meas}}$. This step can utilize a
probabilistic approach (log odds [30]) without requiring any modifications to
the planning module.

Point cloud from sensor$\displaystyle G_{\text{meas}}$: set voxels to
occupied$\displaystyle G_{\text{meas}}$: initialize all voxels to
unknown$\displaystyle G_{\text{meas}}$: raycast to free voxels$\displaystyle
G_{\text{last}}$: voxel grid result of last mergeSet unknown voxels of
$\displaystyle G_{\text{meas}}$ to their value from $\displaystyle
G_{\text{last}}$ then set $\displaystyle G_{\text{last}}$ = $\displaystyle
G_{\text{meas}}$ Figure 4: The mapping module steps.

###  II-C The global path generation module

The global path generation module (Fig. 2) takes as input the last voxel grid
$G_{\text{last}}$ and the goal position $\boldsymbol{p}_{\text{goal}}$ to
generate a path in the free space of the voxel grid. If the goal
$\boldsymbol{p}_{\text{goal}}$ is outside the voxel grid, an intermediate goal
$\boldsymbol{p}_{\text{goal,inter}}$ is set where the line connecting the
agent position and the goal position $\boldsymbol{p}_{\text{goal}}$ intersects
the voxel grid border (all border voxels are always left free for positioning
of $\boldsymbol{p}_{\text{goal,inter}}$).

(a) Traversability check.

JPSFinal PathDMPOccupiedPotential FieldSearch Corridor (b) Path planning
framework.

Figure 5: A 2D example of a traversability check (Fig. 5(a)): Green arrows
indicate the set of possible paths between adjacent voxels. A diagonal path,
indicated by the black arrow, is only considered feasible when one of the
(green) paths between adjacent voxels does not result in a collision. The
results of JPS, DMP, and the shortened final path in an example 2D environment
(Fig. 5(b)): the final path improves the optimality of DMP while keeping the
same margin distance from obstacles.

The module starts from $G_{\text{last}}$ and frees up all unknown voxels to
generate $G_{\text{last,free}}$ because if the unknown voxels are assumed to
be occupied, the goal may not be reachable when doing the path search. At this
point, the module inflates the dimensions of the voxels by taking into account
the agent radius (agent modeled as a sphere with radius $r_{\text{agent}}$)
and finds a path from the current agent position
$\boldsymbol{p}_{\text{curr}}$ to $\boldsymbol{p}_{\text{goal,inter}}$.

The objective is to find a short path that is also pushed away from obstacles
so that there is a safety margin between the drone and the obstacles (the
drone will try to follow that path in the trajectory generation module). For
this reason, a potential field is created around obstacles to increase the
cost of paths passing too close to these obstacles, discouraging their
selection during any subsequent path search (see Appendix VI-A1 for more
details). Doing an A* search with a potential field (denoted as Distance Map
Planner (DMP) [31]) over the whole voxel grid would be computationally
expensive. For this reason, the shortest path is first found while ignoring
the potential field. Then, that shortest path is used to create a small region
around it (search corridor). The DMP is then restricted to that region,
reducing the number of voxels the DMP search has to consider and thus reducing
the computational time. The path generated by DMP is finally shortened for
better optimality (Fig. 5(b) \- see Appendix VI-A2) to get the final global
path that the drone will try to follow in the trajectory generation module.

Jump Point Search (JPS) [32] is used to first find the shortest path while
ignoring the potential field because it offers path optimality guarantees with
reduced computation time compared to A*. The search corridor is the union of
all the voxels that are within a distance $d_{\text{search}}$ of the JPS path.

DMP or JPS can generate diagonal paths between 2 occupied voxels (Fig. 5(a)).
However, since a real drone would not be able to follow that path because
there is no empty space between the obstacles, we do not enable such paths.

###  II-D The trajectory generation module

This module takes as input the last generated global path, voxel grid, and
trajectories of other drones, and generates the trajectory executed by the
drone (Fig. 2). It runs synchronously on all agents, i.e. all the agents start
generating their own trajectory at the same time in a periodic fashion. This
requires the clocks of all agents to be synchronized. It is run at a constant
period $T_{\text{traj}}$. The objective of this module is to make the drone
follow the last generated global path as fast as possible while guaranteeing
no collisions with the static obstacles and other agents. This is achieved
through 3 sub-modules that run sequentially (Fig. 3): TASC (Time-Aware Safe
Corridor) generation, which generates constraints to avoid collisions with
obstacles and other drones; reference generation, which samples the global
path to generate a reference trajectory for the Model Predictive Controller
(MPC) to follow; and Mixed-Integer Quadratic Program/Model Predictive Control
(MIQP/MPC) solver, which generates a dynamically feasible trajectory that is
constrained in the TASC and that follows the reference trajectory while
optimizing for smoothness.

####  II-D1 TASC generation

In this module, a safe corridor is generated around the global path that
covers the free space and avoids static obstacles (blue polyhedra in Fig. 2).
Then, hyperplanes are added between the agents (in the middle) to split the
space into 2 volumes and constrain each agent to one volume to ensure no
collisions occur between the agents. The combination of the safe corridor and
the added hyperplanes is called the Time-Aware Safe Corridor (TASC). The time
awareness comes from the fact that the hyperplanes are generated from the
trajectories of the other agents at the previous iteration, and are used to
constrain future positions of agents at the current planning iteration. The
generation of the TASC is detailed in Appendix VI-B1.

####  II-D2 Reference trajectory generation

This module’s goal is to create a reference trajectory for the MPC/MIQP that
has $N$ discretization steps. It samples the most recent global path to
produce a reference trajectory for the MPC to follow. Each planning iteration
involves sampling $N$ points starting from a reference point
$\boldsymbol{p}_{0,\text{ref}}$, which is the agent’s location at the very
first iteration of the planning algorithm. The sampling progresses along the
global path at a speed $v_{\text{samp}}$ to end at
$\boldsymbol{p}_{N,\text{ref}}$ (Fig. 2). To ensure efficient navigation, the
module employs an algorithm to dynamically modulate $v_{\text{samp}}$,
allowing the agent to move faster in open areas and slow down in cluttered
spaces.

As the agent moves and progresses along the path, the reference trajectory is
updated to reflect this movement, ensuring that it is always pushing the drone
along the path and towards the final goal. This update mechanism involves
sampling the trajectory at each iteration and adjusting the starting point of
the sampling based on the agent’s proximity to the points sampled at the
previous planning iteration. The full description of the reference trajectory
generation process is in Appendix VI-B2.

####  II-D3 MIQP/MPC solver

This module takes the reference trajectory and the TASC and generates a
dynamically feasible and collision-free trajectory that brings the agent
closer to its goal position. The generated trajectory is the result of an
MPC/MIQP optimization that constrains the trajectory inside the TASC for
collision-free navigation. The cost function of this optimization makes the
generated trajectory follow closely the reference trajectory while ensuring a
level of smoothness through a jerk cost. The full optimization formulation is
derived in Appendix VI-B3.

####  II-D4 Communication between agents

During each planning iteration, every agent requires the previously generated
trajectories of other agents to generate the TASC (Sect. II-D1) and plan
safely. This necessitates broadcasting the current trajectory to all other
agents immediately after the last step in the trajectory planning module. The
trajectory must reach all other agents within $T_{\text{traj}}$ time from the
current iteration’s start. If the computation time for the current trajectory
is $t_{\text{comp}}$, this leaves
$t_{\text{lim}}=T_{\text{traj}}-t_{\text{comp}}$ time for the trajectory to
reach all other agents before the next planning iteration begins for all
agents. Minimizing computation time is essential to accommodate communication
latency, hence why the path planning module is done on a different thread
(Fig. 3). If the delay exceeds $t_{\text{lim}}$, we can deal with it actively
by adapting the planning period [23]. Furthermore, if an agent does not
receive the trajectory of another nearby agent due to packet loss, it
continues executing its previously generated trajectory, ensuring collision-
free navigation for all agents in the swarm (see Appendix VI-C for more
details).

##  III Simulation Results

TABLE II: Comparison between EGO-Swarm2 (ES2) [7], MADER [6], RMADER [18] and our method HDSM. The comparison is over 10 simulations (100 simulations for MADER and RMADER) of 10 agents in a circle of radius 10 m exchanging positions (Fig. 6(a)). The metrics displayed in the table are whether the method is synchronous or asynchronous (Async?), the percentage of generated trajectories that resulted in a collision, the average number of agent stops per simulation, the jerk cost ($\int{}||\boldsymbol{j}(t)||^{2}\mathrm{d}t$), mean flight time, velocity and distance, and the deadlock rate (agents blocking each other indefinitely). Method |  Async? |  Collision [%] |  Jerk cost (m2/s5) |  Mean flight time (s) |  Mean flight velocity (m/s) |  Mean flight distance (m) |  Deadlock rate [%]
---|---|---|---|---|---|---|---
AMSwarmX - 2 m/s [19] |  No |  0 |  25.2 |  12.38 |  1.69 |  20.89 |  0
AMSwarmX - 2.5 m/s [19] |  No |  100 |  - |  - |  - |  - |  -
ES2 - 4 m/s [9] |  Yes |  0 |  27 |  8.0 |  2.52 |  20.19 |  0
ES2 - 5 m/s [9] |  Yes |  0 |  78 |  7.7 |  2.63 |  20.26 |  0
ES2 - 6 m/s [9] |  Yes |  0 |  192 |  7.2 |  2.81 |  20.28 |  0
ES2 - 7 m/s [9] |  Yes |  0 |  250 |  8.0 |  2.55 |  20.32 |  0
MADER [6] |  Yes |  5 |  700 |  8.2 |  2.50 |  20.55 |  0
RMADER [18] |  Yes |  0 |  2099 |  10.7 |  1.91 |  20.46 |  1
HDSM (proposed) |  No |  0 |  2001 |  5.61 |  3.62 |  20.26 |  0

(a) Velocity profile.

0.10.20.3151015$0$$10$$20$$30$$40$$50$$60$$70$$80$$90$$100$$110$$21$$61$$74$$76$$85$$97$$100$communication
latency (ms)percentile (%) (b) Comunication latency percentile.

PathTASCOptTot$0$$10$$20$$30$$40$Computation time (ms) (c) Computation time.

Figure 6: The velocity profile (Fig. 6(a)) of 10 agents exchanging their
positions using the planner parameters detailed in Sect. III-B and Sect III-A.
The agents start on a circle of radius 10 m and each agent exchanges its
position with the agent that is diametrically opposing it. This simulation is
run 10 times and the agents’ performance statistics are shown in Tab. II. The
communication latency percentile (Fig. 6(b)) between the 10 agents over all
simulations (with and without obstacles). All communication latency is below
15 ms. The variation in communication latency is mainly due to the CPU being
heavily used. If we only use 5 agents, the communication latency between all
the agents would never exceed 10 ms in all the simulations. The computation
times (Fig. 6(c)) of the different steps of our planning algorithm over all
simulations (with and without obstacles). Path for the parallel global path
generation thread, TASC for the generation of the Time-Aware Safe Corridor,
Opt for the optimization time of the MIQP, and Tot for the total computation
time of one planning iteration.

The simulations are run on Intel i9-13900K CPU with a base frequency of 3.0GHz
and a turbo boost of 5.8GHz. ROS2 [25] is used for communication between the
mapping module and the planning module as well as for communication between
the different agents. All methods have access to ground truth states and
perfect control.

The planner is tested in 3 different environments. One free environment and 2
environments with obstacles. The comparison in the free environment is done
with AMSwarmX [19], EGO-Swarm2 (ES2) [9], MADER [6] and RMADER [18]. In the
environments with obstacles, the comparison is done only with EGO-Swarm2 since
it performs considerably better than the other methods. The comparison is done
with 2 versions of our planner. The first version is HDSM where the planner
has privileged information about the obstacles i.e. it knows the position and
shape of the obstacles within a bounded box around it. Privileged information
is required by all the planners used in the comparison. The other version is
HDSM* where the planner only sees the obstacles that are in the agent’s field
of view i.e. the planner is not aware of all the occluded obstacles. This is
where our mapping framework comes into play i.e. to determine the
unexplored/unseen areas that are unsafe to navigate. For comparison between
the different methods, the following performance metrics are used: collision
[%] (percentage of simulations that resulted in a collision); flight distance;
flight velocity; flight time; acceleration cost
($\int{}||\boldsymbol{a}(t)||^{2}\mathrm{d}t$); jerk cost
($\int{}||\boldsymbol{j}(t)||^{2}\mathrm{d}t$); success (number of simulation
runs where all agents successfully reached their goal position without any
collision); deadlock rate [%] (percentage of generated trajectories that
resulted in a deadlock).

###  III-A Planner parameters

The local voxel grid around each agent is of size $20\times 20\times 6$ m and
has a voxel size of $0.3$ m. The following parameters are chosen: $N=9$,
$h=100$ ms, $v_{\text{samp,min}}=4.5$ m/s, $v_{\text{samp,max}}=6$ m/s,
$s_{d}=0.001$, $s_{o}=0.01$, $d_{\text{pot,max}}=0.6$ m,
$d_{\text{search}}=1.5$ m. The vertical offset is set equal to the agent
radius $z_{\text{offset}}=r_{\text{agent}}$ (the radius will be different for
each environment). The period of the mapping module is $T_{\text{map}}=200$
ms, of the path generation submodule $T_{\text{path}}=200$ ms, and of the
trajectory generation submodule $T_{\text{traj}}=100$ ms. The starting index
for the path planning algorithm is $i_{\text{path,start}}=9$. The jerk and
acceleration limits are set differently for each experiment and will be
specified in the corresponding section.

TABLE III: Comparison between EGO-Swarm2 [9] and our planner HDSM/HDSM*. HDSM assumes the planner is aware of all obstacles in a bounding box around the agent. HDSM* is only aware of the obstacles in its field of view and not those that are occluded. HDSM* employs the mapping framework to determine the unknown, free, and occupied parts of the environment. The comparison is done over 2 environments where the agents have to exchange position in a circular configuration (circle - Fig. 8) or traverse a cluttered environment next to each other in a linear fashion (linear - Fig. 7(a)). The statistics are over 10 randomly generated maps of each environment. The mean $\mid$ standard deviation $\mid$ max of each metric is shown. The better performer is shown in bold. Exp. | Planner | Succ. | Distance (m) | Velocity (m/s) | Flight time (s) | Acc. cost (m2/s3) | Jerk cost (103m2/s5)
---|---|---|---|---|---|---|---
circle | ES2 - 4 m/s [9] | 10/10 | 46.1 $\mid$ 0.96 $\mid$ 49.2 | 2.23 $\mid$ 1.24 $\mid$ 4.03 | 20.7 $\mid$ 3.47 $\mid$ 30.1 |  21.5 $\mid$ 6.84 $\mid$ 49.8 |  0.16 $\mid$ 0.13 $\mid$ 0.76
ES2 - 5 m/s [9] | 10/10 | 46.6 $\mid$ 1.11 $\mid$ 52.1 | 2.32 $\mid$ 1.53 $\mid$ 5.04 | 20.1 $\mid$ 4.03 $\mid$ 30.5 | 32.9 $\mid$ 11.9 $\mid$ 79.7 | 1.59 $\mid$ 1.59 $\mid$ 15.6
ES2 - 6 m/s [9] | 5/10 | 47.0 $\mid$ 1.67 $\mid$ 53.6 | 2.59 $\mid$ 1.78 $\mid$ 7.84 | 18.2 $\mid$ 4.52 $\mid$ 32.2 | 67.4 $\mid$ 51.5 $\mid$ 240 | 3.65 $\mid$ 3.97 $\mid$ 20.6
HDSM (proposed) | 10/10 |  45.8 $\mid$ 0.74 $\mid$ 47.8 |  4.66 $\mid$ 1.62 $\mid$ 6.79 |  9.82 $\mid$ 0.45 $\mid$ 11.0 | 308 $\mid$ 58.8 $\mid$ 494 | 6.98 $\mid$ 1.88 $\mid$ 14.3
HDSM* (proposed) | 10/10 | 45.9 $\mid$ 0.78 $\mid$ 49.5 | 4.35 $\mid$ 1.52 $\mid$ 6.79 | 10.5 $\mid$ 0.65 $\mid$ 12.0 | 286 $\mid$ 57.3 $\mid$ 472 | 7.23 $\mid$ 2.20 $\mid$ 16.1
linear | ES2 - 4 m/s [9] | 10/10 | 101 $\mid$ 3.21 $\mid$ 121 | 2.46 $\mid$ 1.06 $\mid$ 4.01 | 41.1 $\mid$ 4.14 $\mid$ 53.2 |  35.0 $\mid$ 12.4 $\mid$ 73.0 |  0.38 $\mid$ 0.50 $\mid$ 4.29
ES2 - 5 m/s [9] | 3/10 | 101 $\mid$ 3.07 $\mid$ 109 | 2.68 $\mid$ 1.25 $\mid$ 4.98 | 37.7 $\mid$ 4.49 $\mid$ 45.5 | 41.8 $\mid$ 14.0 $\mid$ 94.1 | 2.80 $\mid$ 2.40 $\mid$ 17.7
HDSM (proposed) | 10/10 |  100 $\mid$ 1.83 $\mid$ 106 |  4.90 $\mid$ 1.27 $\mid$ 8.56 |  20.5 $\mid$ 0.98 $\mid$ 23.0 | 496 $\mid$ 132 $\mid$ 984 | 11.8 $\mid$ 4.34 $\mid$ 30.4
HDSM* (proposed) | 10/10 | 101 $\mid$ 2.22 $\mid$ 110 | 4.47 $\mid$ 1.32 $\mid$ 8.09 | 22.6 $\mid$ 1.58 $\mid$ 28.0 | 483 $\mid$ 117 $\mid$ 821 | 12.3 $\mid$ 3.78 $\mid$ 25.9

###  III-B Empty environment

The testing consists of 10 agents in a circular configuration exchanging
positions. The circle is of radius 10 m and the agents are positioned in an
equidistant way on the circle (Fig. 6(a)). Each agent exchanges its position
with the agent that is diametrically opposite. The comparison is done over 10
simulated runs.

The proposed method is compared with AMSwarmX [19], MADER [6], RMADER [18] and
EGO-Swarm2 (ES2) [9]. The maximum acceleration is set to $a_{\text{max}}=20\
\text{m/s\textsuperscript{2}}$ and the maximum jerk $j_{\text{max}}=30\
\text{m/s\textsuperscript{3}}$ for all the planners. For MADER and RMADER,
each agent is represented as a bounding box of size $0.25\times 0.25\times
0.25$ m. For EGO-Swarm2, AMSwarmX, and our planner, each agent is represented
as a sphere of diameter $0.25$. The MADER/RMADER values are taken from the
results reported by their authors [18] since they have been fine-tuned for
this experiment where the maximum speed is set to $v_{\text{max}}=10$ m/s (the
values were computed from 100 simulation runs [14]).

We fine-tuned to the best of our ability the parameters of AMSwarmX, mainly
relying on the parameters set by the authors. AMSwarmX results in collisions
as soon as the speed limit exceeds 2 m/s. For EGO-Swarm2, we fine-tuned its
parameters with a large sensing horizon (10 m) and planning horizon (15 m).
Different speed limits are tested: 4, 5, 6, and 7 m/s all shown in Tab. II.
Note that the average speed performance of the planner improves from 4 m/s
until 6 m/s is reached. After that, the average navigation speed decreases
while the jerk cost goes up (which is why we stopped at 7 m/s).

Our method outperforms the other methods in mean speed and flight time as
shown in Tab. II. It is 28.8% faster than the second-best performer (ES2 - 6
m/s). In terms of flight distance, the difference is negligible between all
the methods: the best performer (ES2 - 4 m/s) is only 3.3% better than the
worst performer (AMSwarmX).

###  III-C Environment with obstacles

(a) Velocity profile.

(b) Separating wall.

Figure 7: The velocity profile of 10 agents traversing an environment from
$x=0$ m to $x=96$ m (Fig. 7(a)). The obstacles are shown in red. The
environment consists of 2 areas of different obstacle densities (0.1 obs/m2
for $3<x<33$ and 0.2 obs/m2 for $63<x<93$). These areas are separated by a
wall with small openings at varying heights (orange wall in Fig. 7(b)). The
agents are the green spheres, their previous positions are the green lines and
their predicted trajectories (MIQP/MPC solutions) are the yellow lines. Figure
8: The velocity profile of 10 agents in a circular configuration exchanging
their positions. The circle is of radius 22 m and contains a forest of
cylinders of density of density 0.1 obst/m2.

Our planner is also tested in 2 environments with obstacles. The first one is
a circular exchange: the agents are set up in a circular configuration on a
circle of radius 22 m and each agent exchanges its position with the agent
that is diametrically opposite. A forest of 90 cylindrical obstacles is
generated inside a box of size $30\times 30\times 20$ m that is centered at
the circle’s center (density 0.1 obst/m2). The positions of the obstacles are
generated following a uniform distribution (Fig. 8).

The second environment is linear navigation: the agents start on a line next
to each other and then navigate through a cluttered environment to reach their
goal. The goal of each agent is its initial position translated 96 m in the
positive $x$ direction (Fig. 7(a)). The cluttered environment consists of 2
cylindrical forests of size $30\times 30\times 20$ m separated by a wall with
rectangular openings in it (Fig. 7(b)). The first forest is centered at
$(18,15,0)$ and contains 90 cylinders (density 0.1 obst/m2). The second forest
is centered at $(78,15,0)$ and contains 180 cylinders (density 0.2 obst/m2).
The separating wall is 0.6 m thick and contains 11 openings. Its width is 30 m
and its height is 15 m.

The cylinders are all of radius 0.15 m and length 20 m. Their center of
gravity is sampled uniformly inside the forest volume. additional environments
with obstacles. The radius of each agent is set to 0.3 m (in contrast to 0.125
m in the empty environment) for experimental diversity.

The dynamical limits are set to $a_{\text{max}}=40$ m/s2 and
$j_{\text{max}}=80$ m/s3 for EGO-Swarm2 and our planners HDSM/HSDM*. The
performance of the planner in the considered environments as well as in other
environments is shown in the supplementary video. The results of 10 simulation
runs where the forests of cylinders have been randomized are shown in Tab.
III.

HDSM and HDSM* are compared with 3 versions of EGO-Swarm2 where the maximum
velocity $v_{\text{max}}$ is set to 4, 5, and 6 m/s. Note that this limit is
on the $x$, $y$, and $z$ components of the velocity, and thus the norm of the
velocity can get up to $\sqrt{3}\cdot v_{\text{max}}$. In the circle
environment, EGO-Swarm2 loses robustness when the maximum speed is set to 6
m/s (5/10 runs where successful): the trajectories start going through the
static obstacles. This phenomenon happens when the speed is set to 5 m/s in
the linear environment and the collisions happen when agents are trying to
traverse the wall. This is because EGO-Swarm2 relies on discretization of the
trajectory which can result in a point being on one side of the wall and its
subsequent point being on the other side.

In both the circle and linear environments, HDSM and HDSM* largely outperform
EGO-Swarm2 in flight velocity and flight time. Note that even though EGO-
Swarm2 has privileged information about occluded obstacles in comparison with
HDSM*, HDSM* is still 67% faster in terms of mean flight velocity and has a
42% lower mean flight time than the best version of EGO-Swarm2 (ES2 - 6 m/s in
the circle experiment). In terms of smoothness (acceleration and jerk costs),
EGO-Swarm2 outperforms our planner due to its low-speed navigation.

(a) Real world experiments schematic.

(b) Real world experiments picture.

Figure 9: Hardware experiments: a schematic of the real world experiments with
7 Crazyflies [33] in the motion capture room (Fig. 9(a)). The drones have been
increased in size $2\times$ in the figure for better visibility. 7 Crazyflie
nano-drones positions during the navigation among obstacles (Fig. 9(b). The
previous positions of each drone are shown with increasing transparency.

###  III-D Computation time and communication latency

The computation time of each step of the planning algorithm over all
simulations (with and without obstacles) is shown in Fig. 6(c) using a boxplot
showing the median (line inside the box), 25th and 75th percentile (bottom and
upper limits of the box), and the min and max (bottom and upper whiskers). The
communication latency percentile is shown in Fig. 6(b). The computation time
for the local reference generation step is negligible compared to the other
steps, and thus not shown separately in Fig. 6(c). Since 100% of all
communication delay is less than 15 ms, and our total computation time
(without the path planning step which is run in parallel) never exceeds 40 ms,
safety is guaranteed because $40+15<T_{\text{traj}}=100$ ms. The mean
computation time of our planner is 9.3 ms and the mean for EGO-Swarm2 is 1.6
ms. The computation time of the mapping framework over all simulations that
require raycasting (HDSM*) has a mean of 10.6 ms, a standard deviation of 2.7
ms, and a max of 21.3 ms.

##  IV Hardware experiments

The dynamic feasibility of our planner is tested using 7 nano-drones: the
Crazyflies [33]. The drones start in the formation shown in Fig. 9(a), with a
0.9 m distance between them. They then proceed to traverse the environment to
the goal position which is their current position translated 6 m forward.
After reaching their goal, the drones traverse the environment again in the
opposite direction and go back to their initial positions. Due to the
computational limitations of the Crazyflie, the planner is run on a separate
PC and sends the commands to the drones to execute using CrazySwarm2 [34]. The
positions of the obstacles in the test environment (Fig. 9(a)) are assumed to
be known beforehand since the sensing capabilities of the Crazyflie are
limited. They are placed in the real world for symbolic purposes (Fig. 9(b)).

The dynamics are limited to the following to adhere to the drone’s dynamical
limitations and the built-in controller performance: $v_{\text{max}}=1.5$ m/s,
$a_{\text{max}}=3$ m/s2, $j_{\text{max}}=4$ m/s3, $v_{\text{samp,min}}=1.5$
m/s, $v_{\text{samp,max}}=1.5$ m/s. The other planner parameters are the same
as the ones presented in Sect. III-A. The Crazyflie has a radius of $7$ cm
radius, but the collision radius is set to $r_{\text{agent}}=0.3$ m, and the
vertical offset to avoid the downwash of other drones is set to
$z_{\text{offset}}=0.6$ m. The large safety radius is to avoid interactions
between the airflow/downwash of the drones, which can make the drones
unstable. The Mellinger controller [35] is used. However, one could reduce the
safety radius as well as the vertical offset by employing recent advances in
control for swarms such as Neural-Swarm2 [36].

The voxel size is set to $0.2$ m and the obstacles are inflated by one voxel.
The potential distance is set to $d_{\text{pot,max}}=0.4$ m, and
$d_{\text{search}}=0.6$ m. The drones were able to execute the trajectories
without any crashes (going to the goal area and back to the initial position 2
times). The mean tracking error was 6.8 cm, the standard deviation 2.5 cm, and
the maximum tracking error was 10.4 cm. Since the distance between the center
of gravity of the drone and the obstacles will always be bigger than 20 cm
(because the obstacles are inflated by one voxel), safety is guaranteed. This
is because the sum of the radius (7 cm) and the maximum tracking error (10.4
cm) is smaller than 20 cm. This demonstrates to a certain extent that the
trajectories generated by our planner are dynamically feasible enough for
real-world operations. The performance of the drones is shown in the video.

##  V Limitations

Dynamic obstacles are not considered in the framework. They can be added by
creating a larger potential field around them in the direction of their motion
or their reachable space. Limitations of the proposed framework also include
the lack of a mechanism to deal with deadlocks that happen when multiple
agents are passing through an extremely narrow gap (especially when the agents
are coming from opposing sides of the gap). Another notable limitation is
dealing with CPU clock drift, which can lead to unsynchronized trajectories
and thus, collisions. Modern CPU clocks have a drift of 100 parts per million
(ppm) [37]. This means if the clocks are synchronized at the start of a
mission, they will deviate by 180 ms after 30 minutes of operation (which is
much larger than the planning period of 100 ms). This would require periodic
synchronization of the clocks, or using clocks with lower drift (0.1 ppm)
which makes a single synchronization at the start of the mission sufficient
for safe operations.

##  VI Conclusion

In this work, a new framework for high-speed aerial swarm planning in unknown
and cluttered environments is presented. To the best of our knowledge, it is
the only framework that explicitly takes into account the unknown space of the
environment and can guarantee safety in unknown environments. The method is
compared to 4 state-of-the-art methods in simulation and is shown to
outperform them in multiple metrics such as speed (67% faster) and flight time
(42% lower). Finally, it is tested in the real world on hardware to show the
feasibility of the generated trajectories.

## Appendix

###  VI-A Path planning

####  VI-A1 Potential field generation

The potential field is created around the obstacles to push the path generated
by DMP away from them. In the voxel grid, occupied voxels are assigned a value
of 100, free voxels are assigned a 0 value, and voxels within a given distance
$d_{\text{pot,max}}$ from an obstacle are assigned an intermediate value
$o_{\text{voxel}}$:

$\displaystyle
o_{\text{voxel}}=100\cdot(1-\dfrac{d_{\text{voxel}}}{d_{\text{pot,max}}})^{4}$
(1)

where $d_{\text{voxel}}$ represents the distance from the voxel center to the
nearest occupied voxel.

####  VI-A2 Path shortening

The DMP path can be optimized in terms of length by shortening it. The
approach we take to shorten it is the following (Alg. 1): we walk through the
path until we find a point (index $i_{\text{start}}$) that is free and not in
a potential field (lines 1-3). Once we find a free voxel/point of the path, we
find the furthest subsequent point (index $i_{\text{end}}$) that is also free
and whose segment with the initial point does not intersect any obstacles or
potential field voxels (lines 4-13). We then remove the points between
$i_{\text{start}}$ and $i_{\text{end}}$ from the path to shorten it (line 14).
We continue walking along the path starting from the next point (line 15) in
this fashion until we reach the endpoint of the path. An example of the result
is shown in dashed green in Fig. 5(b).

1 $i=0$

2 while $i+1<$ size(path) do

3 if IsFree(path[$i$]) then

4 $i_{\text{start}}=i$

5 $i_{\text{end}}=i$

6 $j=i+1$

7 while $j<\text{size(path)}$ do

8 line_clear = IsLineClear(path[$i$], path[$j$])

9 if IsFree(path[$j$]) and line_clear then

10 $i_{\text{end}}=j$

11 $j=j+1$

12

13 else

14 break

15

16

17 RemovePointsFromPath(path, $i_{\text{start}}$, $i_{\text{end}}$)

18

19 $i=i+1$

20

Algorithm 1 Global path shortening

###  VI-B Trajectory generation

####  VI-B1 TASC generation

(a) $SC_{\text{raw}}$ avoids static obstacles

(b) Generation of $SC_{0}$

(c) Generation of $SC_{1}$

(d) Generation of $SC_{2}$

Figure 10: The generation of a Time-Aware Safe Corridor from $SC_{\text{raw}}$
which only avoids static obstacles [5]. The predicted positions of the
planning agent by the MPC are shown as yellow circles. The predicted positions
of the other agent that the planning agent has to avoid are shown as red
circles. The positions of each agent become more transparent as we move
forward in time. We start from the second position of the trajectory and
generate a separating hyperplane that is added as a constraint to each
polyhedron of $SC_{\text{raw}}$. This would result in $SC_{0}$. We do the same
procedure for the second and third positions of the trajectory to generate
$SC_{1}$ and $SC_{2}$. The collection of $SC_{0}$, $SC_{1}$, and $SC_{2}$ is
what we denote by Time-Aware Safe Corridor (TASC).

The TASC generation consists of first generating a safe corridor that avoids
static obstacles and then adding hyperplanes to ensure inta-agent collision
avoidance. To generate the safe corridor, we first take the last generated
voxel grid by the planning framework $G_{\text{last}}$ and set all the unknown
voxels to occupied to generate $G_{\text{last,occ}}$. We then generate a safe
corridor that covers only the free space as follows: we first start from the
current position of the agent and generate a polyhedron around it. Then, we
walk along the global path with a small step ($l_{\text{vox}}/10)$ until we
are outside the polyhedron. Once we arrive at the point outside the
polyhedron, we inflate a polyhedron around that point. We then continue
walking on the global path until we reach a point outside the current
polyhedron and we inflate a polyhedron around it. We continue with this
procedure until we reach the maximum number of polyhedra $P_{\text{hor}}$
(polyhedron horizon) which is fixed by the user. The generated safe corridor
is denoted by $SC_{\text{raw}}$.

Many methods exist to inflate a convex polyhedron around a seed point such as
[38][39][40]. When the seed point/voxel around which we want to inflate a
polyhedron is restricted in a given direction, we employ [40] for generating
the polyhedron due to its superior performance in cluttered environments (more
covered volume). We consider the voxel to be restricted and use [40] when
either the top and bottom voxels are both occupied or the forward and backward
are occupied or the left and right are occupied. Otherwise, we employ [39] to
generate the polyhedron.

We then add a hyperplane between every 2 agents where each agent is
constrained to one side of the hyperplane, thus ensuring intra-agent collision
avoidance. The hyperplanes are generated from the trajectory generated at the
last planning iteration $\boldsymbol{T}_{\text{last}}$ of the agent as well as
the trajectories of the other agents generated at the last planning iteration.
We denote by $\boldsymbol{p}_{i}$ the predicted $i^{\text{th}}$ position of
the agent by the optimal trajectory. This means that the agent should reach
this point after $i\cdot h$ time, where $h$ is the discretization step of the
dynamics in the MPC (Sect. II-D3). The hyperplane generation is done between
the planning agent and each other agent as follows: for every discrete point
$\boldsymbol{p}_{i,\text{plan}}$ in the planning agent trajectory, we take the
corresponding point $\boldsymbol{p}_{i,\text{other}}$ of the other agent we
are trying to avoid and generate a hyperplane between those points. We define
$\boldsymbol{v}_{\text{norm}}$ as the unit vector pointing from
$\boldsymbol{p}_{i,\text{other}}$ to $\boldsymbol{p}_{i,\text{plan}}$. The
generation of the hyperplane is done as follows: we take the middle point
between $\boldsymbol{p}_{i,\text{plan}}$ and $\boldsymbol{p}_{i,\text{other}}$
and then move towards $\boldsymbol{p}_{i,\text{plan}}$ in the direction of
$\boldsymbol{v}_{\text{norm}}$ by a distance $d_{\text{offset}}$ which is
computed according to an ellipsoid collision model that is used to avoid the
down-wash effects of the propellers (the sphere model is elongated in the $z$
direction by $z_{\text{offset}}$). After we move by the distance
$d_{\text{offset}}$, we arrive at the point $\boldsymbol{p}_{\text{hyp}}$. The
separating hyperplane is thus defined by the point through which it passes
$\boldsymbol{p}_{\text{hyp}}$ and its normal vector
$\boldsymbol{v}_{\text{norm}}$. The planning agent is then constrained to the
side of the hyperplane that it is already in.

This procedure is done between $\boldsymbol{p}_{i,\text{plan}}$ and every
position $\boldsymbol{p}_{i,\text{other}}$ of the other $M-1$ agents. This
would generate $M-1$ hyperplane constraints for the planning agent. These
constraints are added to each polyhedron of the safe corridor
$SC_{\text{raw}}$ (Fig. 10) to generate a new time-local safe corridor
$SC_{k}$ (where $k=i-1$). The generation of $SC_{k}$ is done for each position
$i$ of the last generated trajectory of the planning agent
$\boldsymbol{T}_{\text{last}}$ except for the first position $i=0$. This would
result in $N-1$ time-local safe corridors $SC_{k}$, the collection of which we
call Time-Aware Safe Corridor (TASC). The time awareness comes from the fact
that each future position predicted by the MPC at the current planning
iteration will be constrained to 2 time-local safe corridors. For example, the
segment formed by $\boldsymbol{p}_{0}$ and $\boldsymbol{p}_{1}$ will be
constrained inside $SC_{0}$, the one formed by $\boldsymbol{p}_{1}$ and
$\boldsymbol{p}_{2}$ will be constrained inside $SC_{1}$, and so on. And so,
$\boldsymbol{p}_{1}$ will be constrained inside both $SC_{0}$ and $SC_{1}$.

####  VI-B2 Reference trajectory generation

The objective of this module is to generate a reference trajectory for the
MPC/MIQP which has $N$ discretization steps and a time step $h$. The module
takes as input the last generated path from the global path planning module,
samples it, and outputs the reference trajectory that the MPC will try to
follow. At each iteration $l$, $N$ points are sampled using a starting point
$\boldsymbol{p}^{l}_{0,\text{ref}}$. At the start of a navigation mission,
this starting point is the agent’s position. Moving along the global path at a
sampling speed $v_{\text{samp}}$ for a time step $h$ results in arriving at
the second reference point $\boldsymbol{p}^{l}_{1,\text{ref}}$. Sampling
continues until $\boldsymbol{p}^{l}_{N,\text{ref}}$ is reached. The sampling
speed $v_{\text{samp}}$ is adapted according to the density of the environment
that the drone is navigating. The density can be inferred by whether the
voxels that the global path traverses are in a potential field.

The sampling speed modulation algorithm allows for the agent to navigate
faster in free environments and slower in cluttered environments. The
algorithm takes as user input a minimum sampling speed $v_{\text{samp,min}}$
that the agent will have in extremely cluttered environments and a maximum
sampling speed $v_{\text{samp,max}}$ that the agent will have in open/free
space. The choice of both of those speeds is fine-tuned according to the
dynamical limits of the agent.

1 $v_{\text{samp}}=v_{\text{samp,max}}$

2 for $i$ = 1:size(path) do

3 $o_{i}=$ GetValue(path[$i$])

4 $d_{i}=$ GetDistance(path[0], path[$i$])

5 $\alpha=1-e^{-s_{d}\cdot d_{i}}\cdot(1-e^{-s_{o}\cdot o_{i}})$

6
$v_{\text{tmp}}=v_{\text{samp,min}}+\alpha\cdot(v_{\text{samp,max}}-v_{\text{samp,min}})$

7 $v_{\text{samp}}=\min(v_{\text{samp}},v_{\text{tmp}})$

8

Algorithm 2 Speed adaptation using the global path

The algorithm for adapting the speed is described in Alg. 2. The sampling
speed is first set to the maximum sampling speed $v_{\text{samp,max}}$ (line
1). We then walk along the global path starting from the first position (line
2). At every point/voxel of the path, we get the voxel value $o_{i}$ (line 3),
which is set according to the equation (1) (we consider unknown voxels to be
occupied). We also get the distance $d_{i}$ of the point to the starting point
of the path, which is close to the current position of the agent (line 4). We
then compute the scaling factor $\alpha$ that depends on $o_{i}$, $d_{i}$, and
the user-chosen distance sensitivity $s_{d}$ and potential sensitivity $s_{o}$
(line 5). The sensitivities will determine how fast we slow down once we
realize that the path is entering a potential field, and thus becoming close
to obstacles. We compute the velocity limit set by the current point/voxel
$v_{\text{tmp}}$ (line 6) and then set the sampling speed to $v_{\text{tmp}}$
if it is smaller than its current value (line 7). Note that if a voxel has a
value $o_{i}=0$ (i.e. is free), then $\alpha$ will be 1, and $v_{\text{tmp}}$
will be $v_{\text{samp,max}}$. On the other hand, if $o_{i}>0$, then the
bigger that value, the closer we are to an obstacle (as per equation (1)) i.e.
the denser the environment. In that case, $\alpha$ will be smaller than 1 and
the velocity will be smaller than $v_{\text{samp,max}}$. The distance of the
point/voxel $d_{i}$ to the starting point of the path is also taken into
account: the smaller that distance, the closer we are to obstacles and the
smaller $\alpha$ is i.e. the smaller the sampling velocity is.

The reference path is sampled at each planning iteration. In subsequent
iterations, and as the agent moves forward and progresses along the path, the
reference trajectory needs to move along the global path as well. Path
progress is used as a measure of when to move along/increment the starting
point $\boldsymbol{p}^{l}_{0,\text{ref}}$ of the sampling on the path.

Path progress is computed in the following way (Fig. 11): first, the local
reference trajectory is sampled by a small value (typically 1 cm). Then the
point $\boldsymbol{p}_{\text{min}}$ (magenta circle in Fig. 11) that has the
minimum distance to the agent among the sampled points is found. The distance
between $\boldsymbol{p}_{\text{min}}$ and the starting point (yellow disk in
Fig. 11) of the local reference trajectory is the path progress. If at a given
iteration $l$ the path progress is 0 i.e. the closest point is the starting
point, then the same starting point for the sampling is kept
$\boldsymbol{p}^{l}_{0,\text{ref}}=\boldsymbol{p}^{l-1}_{0,\text{ref}}$. If
the path progress is bigger than 0, then we move along the starting point of
the sampling
$\boldsymbol{p}^{l}_{0,\text{ref}}=\boldsymbol{p}^{l-1}_{1,\text{ref}}$.

Figure 11: The process of computing the path progress: the local reference
trajectory (green path) is sampled (green circles) and we find the closest
point among these samples to the drone (magenta circle). The path progress is
the distance between the first point of the local reference trajectory (yellow
circle) and the magenta circle. The predicted trajectory of the drone is the
black dotted line and the final position is the black disk. The red disk is
the final point in the local reference trajectory.

The output of the path planning algorithm can vary in terms of direction
between one planning iteration and another, which results in reference
trajectories with a varying direction between iterations. This can result in
jerky behavior as the drone is trying to follow a reference trajectory that is
changing direction every iteration. To fix this issue, the path planning
module is given the last point of the reference trajectory as the initial
position to plan from, which results in smoother navigation. The degree of
smoothness can be optionally fine-tuned during the experiments by starting the
path planning from a reference point between the current position and the last
reference point (instead of starting from the last point). The index of this
point is denoted $i_{\text{path,start}}$.

####  VI-B3 MPC/MIQP solver

In this section, we will derive the MPC/MIQP formulation that is solved at
every planning iteration to generate the collision-free optimal trajectory
that is sent for the controller to execute. The initial state
$\boldsymbol{x}_{0}^{l}$ of the generated trajectory at iteration $l$ is set
to the second state of the trajectory generated at the previous iteration
$l-1$ i.e. $\boldsymbol{x}_{0}^{l}=\boldsymbol{x}_{1}^{l-1}$. The terminal
velocity $\boldsymbol{v}_{N}$ as well as the terminal acceleration
$\boldsymbol{a}_{N}$ are set to $\boldsymbol{0}$ to guarantee safety. If a
trajectory optimization fails or the computation time exceeds the trajectory
planning period $T_{\text{traj}}$, then the last successfully generated
trajectory continues to be executed. Since the terminal velocity and
acceleration of each agent is $\boldsymbol{0}$, if all trajectories fail
indefinitely to be generated, the agents will come to a stop and will not
collide.

The agent dynamics are represented by a point mass (center of gravity of the
agent) that is controlled by the jerk $\boldsymbol{j}$. This representation
can be used for omnidirectional vehicles in general, and multirotors in
particular [41]. We also introduce drag forces to ensure that the trajectory
is still feasible at high speeds. The drag forces are linear with respect to
the velocity to make sure that the dynamics are convex and the MPC solving
time is low. The dynamics are the following:

$\displaystyle\begin{aligned}
&\dot{\boldsymbol{p}}=\boldsymbol{v},\quad\dot{\boldsymbol{v}}=\boldsymbol{a}-\boldsymbol{D}_{\text{lin,max}}\cdot\boldsymbol{v},\quad\dot{\boldsymbol{a}}=\boldsymbol{j}\end{aligned}$
(2)

where $\boldsymbol{p}$ is the position vector, $\boldsymbol{v}$ the velocity
vector, $\boldsymbol{a}$ the acceleration vector, and
$\boldsymbol{D}_{\text{lin,max}}$ a diagonal matrix that represents the
maximum possible drag coefficient in every direction.

The continuous dynamics are discretized using Euler or Runge-Kutta 4th and
added as equality constraints to the optimization problem. The state of each
agent is formed by the position, velocity, and acceleration
$\boldsymbol{x}=[\boldsymbol{p}\ \boldsymbol{v}\ \boldsymbol{a}]^{T}$ and the
control input corresponds to the jerk $\boldsymbol{u}=\boldsymbol{j}$. The
continuous dynamics can be written under the form
$\dot{\boldsymbol{x}}=f(\boldsymbol{x}(t),\boldsymbol{u}(t))$. We choose Euler
discretization due to its better time efficiency and discretize the dynamics
that become under the form
$\boldsymbol{x}_{k+1}=f_{d}(\boldsymbol{x}_{k},\boldsymbol{u}_{k})$. Using a
discretization step of $h$, the discretized dynamics are:

$\displaystyle\boldsymbol{p}_{k+1}=\boldsymbol{p}_{k}+h\cdot\boldsymbol{v_{k}}$
(3)
$\displaystyle\boldsymbol{v}_{k+1}=\boldsymbol{v}_{k}+h\cdot(\boldsymbol{a}_{k}-\boldsymbol{D}_{\text{lin,max}}\boldsymbol{v_{k}})$
$\displaystyle\boldsymbol{a}_{k+1}={a}_{k}+h\cdot\boldsymbol{j_{k}}$
$\displaystyle\boldsymbol{x}_{k}=[\boldsymbol{p}_{k}\ \boldsymbol{v}_{k}\
\boldsymbol{a}_{k}]^{T},\quad\boldsymbol{u}_{k}=\boldsymbol{j}_{k}$

State bounds are inequality constraints that are added to the optimization to
ensure that the generated trajectory is dynamically feasible. The agent’s
physical dynamics and limitations determine the bounds that are set to its
acceleration and jerk. The velocity of each agent is inherently limited by the
drag forces in the dynamics. We limit the L1 norm of the acceleration in the
directions $x$ and $y$ to $a_{x,\text{max}}$ and $a_{y,\text{max}}$
respectively. We limit the acceleration in the $z$ direction to
$a_{z,\text{min}}$ and $a_{z,\text{max}}$ since it is harder to accelerate
upwards (fighting gravity) than downwards. The acceleration limitations are
deduced from the thrust-to-weight ratio of the multirotor. The L1 norm of the
control input (jerk) is limited in each direction to $j_{x,\text{max}}$,
$j_{y,\text{max}}$ and $j_{z,\text{max}}$. These limitations are determined by
the rotational dynamics of the multirotor.

The generated trajectory should avoid static obstacles as well as other
agents. For this end, it is contained inside the TASC generated in Sect.
II-D1. The constraints are generated by forcing 2 consecutive positions
$\boldsymbol{p}_{k}$ and $\boldsymbol{p}_{k+1}$ (and thus the segment formed
by them) to be inside one polyhedron of the time-local safe corridor $SC_{k}$.
Each $SC_{k}$ incorporates constraints based on the positions of all agents at
discrete time points $k$ in the future. This enables the planning agent to
plan within the anticipated free space at time $h\cdot k$. Each $SC_{k}$
contains $P_{\text{hor}}$ polyhedra each described by
$\\{(\boldsymbol{A}_{kp},\boldsymbol{c}_{kp})\\}$, $p=0:P_{\text{hor}}-1$.The
constraint ensuring that the discrete position, represented by
$\boldsymbol{p}_{k}$, resides within a polyhedron $p$ is expressed as
$\boldsymbol{A}_{kp}\cdot\boldsymbol{p}_{k}\leq\boldsymbol{c}_{kp}$. Binary
variables $b_{kp}$ are introduced (with $P_{\text{hor}}$ variables for each
$\boldsymbol{x}_{k}$, where $k=0:N-1$) to indicate whether
$\boldsymbol{p}_{k}$ and $\boldsymbol{p}_{k+1}$ are both within polyhedron
$p$. The requirement that all segments be within at least one polyhedron is
enforced by the constraint $\sum_{p=0}^{P-1}b_{kp}\geq 1$. This necessitates
the existence of an overlapping region across all polyhedra, with at least one
discrete point belonging to this overlap when transitioning from one
polyhedron to another. In our framework, we limit the number of polyhedra per
Safe Corridor $P_{\text{hor}}$ to 3 to avoid high solving times.

The cost function consists of the squared norm of the difference between the
predicted state $\boldsymbol{x}_{k}$ and its reference
$\boldsymbol{x}_{k,\text{ref}}$ ($k=1,2,...,N$). Each reference
$\boldsymbol{x}_{k,\text{ref}}$ is created from the reference trajectory
position:

$\displaystyle\boldsymbol{x}_{k,\text{ref}}=[\boldsymbol{p}_{k,\text{ref}}\
\boldsymbol{v}_{k,\text{ref}}\ \boldsymbol{0}_{1\times 3}]^{T}$ (4)

where $\boldsymbol{v}_{k,\text{ref}}$ is a vector of norm $v_{\text{samp}}$
and of direction
$\boldsymbol{v}_{\text{dir}}=\boldsymbol{p}_{k+1,\text{ref}}-\boldsymbol{p}_{k,\text{ref}}$
(except for $\boldsymbol{v}_{N,\text{ref}}$ which is equal to
$\boldsymbol{v}_{N-1,\text{ref}}$). The control input (jerk) is added to the
cost function to guarantee a level of smoothness.

We express our Model Predictive Control (MPC) using the Mixed-Integer
Quadratic Program (MIQP) formulation. To simplify, we omit the superscript $l$
denoting the iteration number from both the reference and state variables:

$\displaystyle\underset{\begin{subarray}{c}\boldsymbol{x}_{k},\boldsymbol{u}_{k}\end{subarray}}{\text{minimize}}$
$\displaystyle\sum_{k=0}^{N}(||\boldsymbol{x}_{k}-\boldsymbol{x}_{k,\text{ref}}||_{\boldsymbol{R}_{x}}^{2}+||\boldsymbol{u}_{k}||_{\boldsymbol{R}_{u}}^{2})$
$\displaystyle+||\boldsymbol{x}_{N}-\boldsymbol{x}_{N,\text{ref}}||_{\boldsymbol{R}_{N}}^{2}$
(5) subject to
$\displaystyle\boldsymbol{x}_{k+1}=f_{d}(\boldsymbol{x}_{k},\boldsymbol{u}_{k}),\quad
k=0:N-1$ (6)
$\displaystyle\boldsymbol{x}_{0}=\boldsymbol{X}_{0},\quad\boldsymbol{v}_{N}=\boldsymbol{0},\quad\boldsymbol{a}_{N}=\boldsymbol{0}$
(7) $\displaystyle|a_{x,k}|\leq a_{x,\text{max}}$ (8)
$\displaystyle|a_{y,k}|\leq a_{y,\text{max}},\quad a_{z,k}\leq
a_{z,\text{max}}$ (9) $\displaystyle a_{z,k}\geq
a_{z,\text{min}},\quad|j_{x,k}|\leq j_{x,\text{max}}$ (10)
$\displaystyle|j_{y,k}|\leq j_{y,\text{min}},\quad|j_{z,k}|\leq
j_{z,\text{max}}$ (11) $\displaystyle
b_{kp}=1\implies\begin{cases}\boldsymbol{A}_{kp}\cdot\boldsymbol{p}_{k}\leq\boldsymbol{c}_{kp}\\\
\boldsymbol{A}_{kp}\cdot\boldsymbol{p}_{k+1}\leq\boldsymbol{c}_{kp}\end{cases}$
(12) $\displaystyle\sum_{p=0}^{P_{\text{hor}}-1}b_{kp}\geq 1,\quad
b_{kp}\in\\{0,1\\}$ (13)

$\boldsymbol{R}_{x}$, $\boldsymbol{R}_{N}$, and $\boldsymbol{R}_{u}$ represent
the weight matrices for discrete state errors excluding the final state, the
final discrete state error (terminal state), and the input, respectively.

The optimization problem is solved in each planning iteration using the Gurobi
solver [42] to produce an optimal trajectory based on its cost function.

###  VI-C Packet loss

If an agent does not receive the trajectory of at least one nearby agent
before its planning for the next iteration begins, it will keep executing its
last generated trajectory. This guarantees that no collisions will occur
because of the following reasoning. Let’s consider the case of 2 agents A and
B. If agent A does not receive the trajectory of agent B at a given planning
iteration, it will continue executing its previous trajectory of fixed time
horizon. Since all generated trajectories have a terminal state with zero
velocity and acceleration, the agent will be stationary when it reaches the
end of the trajectory. We have 2 cases: either agent B also did not receive
the trajectory of agent A, or it did receive it.

  * •

If agent B did not receive the trajectory of agent A, it also continues
executing its previously generated trajectory, and safety is guaranteed
because both trajectories were optimized using the same separating hyperplanes
at the previous planning iteration. In the extreme case where both agents lose
complete communication, they will execute their last generated trajectory
until they reach the terminal state and remain stationary until communication
is re-established.

  * •

If agent B received the trajectory of agent A, new hyperplanes are generated
using the trajectory that agent A is currently executing and the last
generated trajectory of B. The key point is that we will always be able to
generate hyperplanes that guarantee collision safety because all previous
trajectories from which the hyperplanes are generated have safety guarantees
i.e. the positions of the agents are always more distant than the safety
distance due to the previous hyperplanes.

## References

  * [1] L. Bauersfeld and D. Scaramuzza, “Range, endurance, and optimal speed estimates for multicopters,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 2, pp. 2953–2960, 2022.
  * [2] E. Kaufmann, L. Bauersfeld, A. Loquercio, M. Müller, V. Koltun, and D. Scaramuzza, “Champion-level drone racing using deep reinforcement learning,” _Nature_ , vol. 620, no. 7976, pp. 982–987, 2023.
  * [3] Y. Song, A. Romero, M. Müller, V. Koltun, and D. Scaramuzza, “Reaching the limit in autonomous racing: Optimal control versus reinforcement learning,” _Science Robotics_ , vol. 8, no. 82, p. eadg1462, 2023.
  * [4] W. Hönig, J. A. Preiss, T. K. S. Kumar, G. S. Sukhatme, and N. Ayanian, “Trajectory planning for quadrotor swarms,” _IEEE Transactions on Robotics_ , vol. 34, no. 4, pp. 856–869, 2018.
  * [5] C. Toumieh and A. Lambert, “Decentralized multi-agent planning using model predictive control and time-aware safe corridors,” _IEEE Robotics and Automation Letters_ , pp. 1–8, 2022.
  * [6] J. Tordesillas and J. P. How, “Mader: Trajectory planner in multiagent and dynamic environments,” _IEEE Transactions on Robotics_ , pp. 1–14, 2021.
  * [7] X. Zhou, J. Zhu, H. Zhou, C. Xu, and F. Gao, “Ego-swarm: A fully autonomous and decentralized quadrotor swarm system in cluttered environments,” _2021 IEEE International Conference on Robotics and Automation (ICRA)_ , pp. 4101–4107, 2021.
  * [8] X. Zhou, Z. Wang, H. Ye, C. Xu, and F. Gao, “Ego-planner: An esdf-free gradient-based local planner for quadrotors,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 478–485, 2020.
  * [9] X. Zhou, Z. Wang, X. Wen, J. Zhu, C. Xu, and F. Gao, “Decentralized spatial-temporal trajectory planning for multicopter swarms,” _arXiv preprint arXiv:2106.12481_ , 2021.
  * [10] Z. Wang, X. Zhou, C. Xu, and F. Gao, “Geometrically constrained trajectory optimization for multicopters,” _IEEE Transactions on Robotics_ , vol. 38, no. 5, pp. 3259–3278, 2022.
  * [11] A. Kunoth, T. Lyche, G. Sangalli, S. Serra-Capizzano, T. Lyche, C. Manni, and H. Speleers, “Foundations of spline theory: B-splines, spline approximation, and hierarchical refinement,” _Splines and PDEs: From Approximation Theory to Numerical Linear Algebra: Cetraro, Italy 2017_ , pp. 1–76, 2018.
  * [12] X. Zhou, X. Wen, Z. Wang, Y. Gao, H. Li, Q. Wang, T. Yang, H. Lu, Y. Cao, C. Xu _et al._ , “Swarm of micro flying robots in the wild,” _Science Robotics_ , vol. 7, no. 66, p. eabm5954, 2022.
  * [13] J. Hou, X. Zhou, Z. Gan, and F. Gao, “Enhanced decentralized autonomous aerial robot teams with group planning,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 4, pp. 9240–9247, 2022.
  * [14] K. Kondo, J. Tordesillas, R. Figueroa, J. Rached, J. Merkel, P. C. Lusk, and J. P. How, “Robust mader: Decentralized and asynchronous multiagent trajectory planner robust to communication delay,” _arXiv preprint arXiv:2209.13667_ , 2022.
  * [15] J. Tordesillas and J. P. How, “Minvo basis: Finding simplexes with minimum volume enclosing polynomial curves,” _Computer-Aided Design_ , vol. 151, p. 103341, 2022.
  * [16] B. Şenbaşlar and G. S. Sukhatme, “Dream: Decentralized real-time asynchronous probabilistic trajectory planning for collision-free multi-robot navigation in cluttered environments,” _arXiv preprint arXiv:2307.15887_ , 2023.
  * [17] B. Şenbaşlar, P. Luiz, W. Hönig, and G. S. Sukhatme, “Mrnav: Multi-robot aware planning and control stack for collision and deadlock-free navigation in cluttered environments,” _arXiv preprint arXiv:2308.13499_ , 2023.
  * [18] K. Kondo, R. Figueroa, J. Rached, J. Tordesillas, P. C. Lusk, and J. P. How, “Robust mader: Decentralized multiagent trajectory planner robust to communication delay in dynamic environments,” _arXiv preprint arXiv:2303.06222_ , 2023.
  * [19] V. K. Adajania, S. Zhou, A. K. Singh, and A. P. Schoellig, “Amswarmx: Safe swarm coordination in complex environments via implicit non-convex decomposition of the obstacle-free space,” _arXiv preprint arXiv:2310.09195_ , 2023.
  * [20] E. Soria, F. Schiano, and D. Floreano, “Distributed predictive drone swarms in cluttered environments,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 1, pp. 73–80, 2021.
  * [21] J. Park, J. Kim, I. Jang, and H. J. Kim, “Efficient multi-agent trajectory planning with feasibility guarantee using relative bernstein polynomial,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2020, pp. 434–440.
  * [22] J. Park, Y. Lee, I. Jang, and H. J. Kim, “Dlsc: Distributed multi-agent trajectory planning in maze-like dynamic environments using linear safe corridor,” _IEEE Transactions on Robotics_ , 2023.
  * [23] C. Toumieh, “Decentralized multi-agent planning for multirotors: a fully online and communication latency robust approach,” _arXiv preprint arXiv:2304.09462_ , 2023.
  * [24] J. Park, I. Jang, and H. J. Kim, “Decentralized deadlock-free trajectory planning for quadrotor swarm in obstacle-rich environments - extended version,” _ArXiv_ , vol. abs/2209.09447, 2022.
  * [25] S. Macenski, T. Foote, B. Gerkey, C. Lalancette, and W. Woodall, “Robot operating system 2: Design, architecture, and uses in the wild,” _Science Robotics_ , vol. 7, no. 66, p. eabm6074, 2022. [Online]. Available: https://www.science.org/doi/abs/10.1126/scirobotics.abm6074
  * [26] A. Bachrach, “Skydio autonomy engine: Enabling the next generation of autonomous flight,” in _2021 IEEE Hot Chips 33 Symposium (HCS)_. IEEE Computer Society, 2021, pp. 1–43.
  * [27] DJI, “Dji mavic 3 pro,” https://www.dji.com/ch/mavic-3-pro, accessed 2023-11-06.
  * [28] C. Toumieh and A. Lambert, “Gpu accelerated voxel grid generation for fast mav exploration,” _arXiv preprint arXiv:2112.13169_ , 2021.
  * [29] ——, “High-speed planning in unknown environments for multirotors considering drag,” in _2021 IEEE International Conference on Robotics and Automation (ICRA)_ , 2021, pp. 7844–7850.
  * [30] A. Elfes, “Using occupancy grids for mobile robot perception and navigation,” _Computer_ , vol. 22, no. 6, pp. 46–57, 1989.
  * [31] K. Robotics, “Mrsl jump point search planning library v1.1,” https://github.com/KumarRobotics/jps3d, accessed 2023-11-06.
  * [32] D. D. Harabor and A. Grastien, “Online graph pruning for pathfinding on grid maps,” in _Twenty-Fifth AAAI Conference on Artificial Intelligence_ , 2011.
  * [33] W. Giernacki, M. Skwierczyński, W. Witwicki, P. Wroński, and P. Kozierski, “Crazyflie 2.0 quadrotor as a platform for research and education in robotics and control engineering,” in _2017 22nd International Conference on Methods and Models in Automation and Robotics (MMAR)_ , 2017, pp. 37–42.
  * [34] J. A. Preiss, W. Honig, G. S. Sukhatme, and N. Ayanian, “Crazyswarm: A large nano-quadcopter swarm,” in _2017 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2017, pp. 3299–3304.
  * [35] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in _2011 IEEE International Conference on Robotics and Automation_. IEEE, 2011, pp. 2520–2525.
  * [36] G. Shi, W. Hönig, X. Shi, Y. Yue, and S.-J. Chung, “Neural-swarm2: Planning and control of heterogeneous multirotor swarms using learned interactions,” _IEEE Transactions on Robotics_ , vol. 38, no. 2, pp. 1063–1079, 2021.
  * [37] Y. Li, G. Kumar, H. Hariharan, H. Wassel, P. Hochschild, D. Platt, S. Sabato, M. Yu, N. Dukkipati, P. Chandra _et al._ , “Sundial: Fault-tolerant clock synchronization for datacenters,” in _14th USENIX symposium on operating systems design and implementation (OSDI 20)_ , 2020, pp. 1171–1186.
  * [38] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor, and V. Kumar, “Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments,” _IEEE Robotics and Automation Letters_ , vol. 2, no. 3, pp. 1688–1695, 2017.
  * [39] C. Toumieh and A. Lambert, “Voxel-grid based convex decomposition of 3d space for safe corridor generation,” _Journal of Intelligent & Robotic Systems_, vol. 105, no. 4, pp. 1–13, 2022.
  * [40] ——, “Shape-aware safe corridors generation using voxel grids,” _arXiv e-prints_ , pp. arXiv–2208, 2022.
  * [41] ——, “Near time-optimal trajectory generation for multirotors using numerical optimization and safe corridors,” _Journal of Intelligent & Robotic Systems_, vol. 105, no. 1, pp. 1–10, 2022.
  * [42] L. Gurobi Optimization, “Gurobi optimizer reference manual,” 2020. [Online]. Available: http://www.gurobi.com

|  Charbel Toumieh is a postdoctoral fellow at the Ecole Polytechnique
Federale de Lausanne, Lausanne, Switzerland. He received his Ph.D. in robotics
from Paris-Saclay University, France in 2022. He received his M.Sc. in
Electrical and Computer Engineering from the Georgia Institute of Technology,
USA in 2019. His research interests include autonomous robotics, motion
planning, multi-agent systems, and aerial swarms.
---|---
|  Dario Floreano (Fellow, IEEE) received the M.A. degree in vision
psychophysics, the M.S. degree in neural computation, and the Ph.D. degree in
robotics. He is currently a Full Professor and Director of the Laboratory of
Intelligent Systems, Swiss Federal Institute of Technology Lausanne (EPFL),
Lausanne, Switzerland. Since 2010, he is also a Founding Director of the Swiss
National Center of Competence in Robotics. He held Visiting Fellowships with
Sony Computer Science Laboratory, Tokyo, Japan, with Caltech/JPL, Pasadena,
CA, USA, and with Harvard University in Boston, Boston, MA, USA. His research
interests focus on biologically inspired robotics and artificial intelligence.
He made pioneering contributions to the fields of evolutionary robotics,
aerial robotics, and soft robotics. He spun off two robotics companies:
senseFly (2009, acquired by the Parrot Group in 2016), which has become a
world leader in drones for agriculture and imaging, and Flyability (2015),
which is the world leader in inspection drones for confined spaces. In 2017,
The Economist dedicated a center page portrait to Prof. Floreano (Brain Scan).
Prof. Floreano served in the Advisory Board of the Future and Emerging
Technologies division of the European Commission, has been Vice-Chair of the
World Economic Forum Agenda Council on Smart Systems and Robotics, has been a
Cofounder of the International Society of Artificial Life, served as elected
member of the Board of Governors of the International Neural Network Society,
is on the Advisory Board of the Max-Planck Institute for Intelligent Systems,
and has joined the editorial board of ten scientific journals. He also co-
organized ten international conferences and several thematic workshops on
bioinspired drones and soft robotics, which are now considered foundational
events for those communities
---|---