Invariant Set Distributed Explicit Reference Governors for Provably Safe On-Board Control of Nano-Quadrotor Swarms

This article provides a theory for provably safe and computationally efficient distributed constrained control, and describes an application to a swarm of nano-quadrotors with limited on-board hardware and subject to multiple state and input constraints. We provide a formal extension of the explicit reference governor framework to address the case of distributed systems. The efficacy, robustness, and scalability of the proposed theory is demonstrated by an extensive experimental validation campaign and a comparative simulation study on single and multiple nano-quadrotors. The control strategy is implemented in real-time on-board palm-sized unmanned erial vehicles, and achieves safe swarm coordination without relying on any offline trajectory computations.


INTRODUCTION
sensing, and actuation is considerably challenging (Chung et al., 2018). Moreover, even for large platforms with more advanced capabilities, the computational power available to implement control algorithms is typically limited in favor of running missiondependent algorithms related to localization and sensing systems (Brockers et al., 2014). Hence, computationally efficient and provably safe on-board algorithms for multi-robot systems are of paramount importance for achieving safety-critical tasks in complex environments.
In this work, we develop a provably safe and robust constrained control methodology that is fully distributed and can be implemented on the individual agents of a swarm of Vertical Take-Off and Landing (VTOL) vehicles. The algorithm is validated using the smallest open-source available nano-quadrotor platform, i.e. Bitcraze's Crazyflie 2.1. An accompanying video can be found at https://youtu.be/le6WSeyTXNU.

RELATED WORK
As discussed in (Murray, 2007;Brambilla et al., 2013;Parker et al., 2016;Chamanbaz et al., 2017;Chung et al., 2018;Coppola et al., 2020), swarm robotics has become an active area of research covering a broad spectrum of topics within the robotics and control communities. The problem of safely controlling the motion of aerial robot swarms can be classified based on approaches for which the main portion of the algorithm, and especially the part that ensures safety and goal satisfaction, is running either off-board or on-board the UAVs. This classification is motivated because most existing works provide algorithmic contributions which belong to the off-board category (see Section 2.1), but as explained in Section 1, on-board navigation algorithms (see Section 2.2) are preferred from a safety and autonomy perspective.
Unfortunately, there does not exist one safe navigation strategy that suits all UAV applications. For each strategy there is an inherent trade-off between computational efficiency, performance, safety guarantees, simplicity, generality, and scalability to swarms. To provide a fair point of comparison, it is worth noting that VTOLs can vary significantly in terms of the available on-board computational power. For instance, a 35 g Crazyflie quadrotor carries an STM32F4 microprocessor with a clock speed of 168MHz and 192kB RAM. For comparison, larger platforms with a mass above ±700 g can use processors like the Odroid-XU4 (Liu et al., 2018) or the NVIDIA TX2 (Jung et al., 2018;Sanket et al., 2018;Ding et al., 2019;Carrio et al., 2020). The latter has a six-core CPU, each with a clock speed of 2GHz, a 256-core NVIDIA GPU, and 8 GB RAM. Since very limited battery power for computation, memory, and communication available to tiny MAVs intrinsically calls for different kinds of navigation and control strategies (Purohit et al., 2014), the literature review is mainly limited to off-board and on-board navigation strategies applied to nano-quadrotors.

Off-Board Navigation Strategies for Nano-Quadrotors
Most approaches, such as (Campos-Macías et al., 2017;Chen et al., 2017;Herbert et al., 2017;Preiss et al., 2017a;Wang et al., 2017;Fridovich-Keil et al., 2018;Honig et al., 2018;Kolaric et al., 2018;Cappo et al., 2018a;Cappo et al., 2018b;Xu and Sreenath, 2018;Bajcsy et al., 2019;Du et al., 2019;Fathian et al., 2019;Liu et al., 2019;Luis and Schoellig, 2019;Rubies-Royo et al., 2019;Vukosavljev et al., 2019), try to ensure a particular level of safety and robustness, by running the core search-based or optimization-based algorithms off-board the UAVs, and thus outsource the high computational cost to ground control stations that send the trajectories to the UAV's on-board position or attitude controller. Frameworks such as (Preiss et al., 2017a;Honig et al., 2018) combine graph-based planning and continuous trajectory optimization to compute safe and smooth trajectories, but take several minutes for a swarm of hundreds of quadrotors in obstacle-rich environments. In , a scalable distributed model predictive control algorithm with ondemand collision avoidance is proposed to perform point-topoint transitions with labeled agents. This strategy reduces the computation time to the order of seconds. (Campos-Macías et al., 2017) introduces a hybrid approach to trajectory planning, fusing sampling-based planning techniques and model-based optimization via quadratic programming (QP). For a single nano-quadrotor in obstacle-dense environments, a provably safe trajectory can be computed online every 0.1-1s, depending on the scenario. Frameworks such as (Du et al., 2019;Vukosavljev et al., 2019) are based on designing off-board libraries of safe motion primitives for a swarm of tiny MAVs, but typically require too much memory for on-board implementation. (Du et al., 2019) relies on combinatorial and nonlinear optimization techniques that are executed on a central computer, requires iterative procedures to resolve collisions between agents in a sequential manner, and does not guarantee to find a feasible solution. A modular, robust, and hierarchical framework for safe planning of robot teams is proposed in . Although the runtime components, executed off-board, require only a small computing time, this approach is centralized, requires a-priori known environments and is conservative due to the restriction to a discretization, i.e. a gridded workspace partitioned into rectangular boxes. Works based on the online FaSTrack motion planner (Herbert et al., 2017) provide strong safety guarantees under the assumption of a single near-hover quadrotor with a decoupled structure (Fridovich-Keil et al., 2018) or obtain weaker safety guarantees using neural network classifiers to consider control-affine dynamics (Rubies-Royo et al., 2019). Hamilton-Jacobi reachability analysis was applied to multiagent swarms using sequential priority ordering (Bajcsy et al., 2019) or the selection of air highways . A centralized multi-robot system planner for enabling theatrical performance is designed in (Cappo et al., 2018a;Cappo et al., 2018b) using time-aware trajectory formulation for validation, verification, and trajectory refinement. The human intent is translated online into non-colliding and dynamically feasible trajectories for multiple nanoquadrotors. Safety barrier certificates based on exponential control barrier functions are used in (Wang et al., 2017) to ensure in a minimally invasive way collision-free maneuvers for teams of small quadrotors flying through formations and in (Xu and Sreenath, 2018) for the safe teleoperation of nanoquadrotor swarms via a remote joystick in a set of static constraints. In (Wang et al., 2017) this requires a centralized QP to be solved at 50 Hz on a ground PC to minimize the difference between the actual and nominal control. Distributed formation control approaches that have been demonstrated on small quadrotors, but are computed off-board have shown robustness to bounded measurement noise (Kolaric et al., 2018), to communication delays, nonlinearities, parametric perturbations, and external disturbances (Liu et al., 2019). Input feasibility and collision avoidance is guaranteed in (Fathian et al., 2019) for single-integrator dynamics, and is claimed to be extendable to agents with higher-order dynamics in (Fathian et al., 2018).

On-Board Navigation Strategies for Nano-Quadrotors
Only few works such as (Preiss et al., 2017b;McGuire et al., 2019) achieved to run computationally efficient navigation algorithms on-board the small embedded flight controllers of nano-quadrotors, but mostly with limited safety guarantees. These strategies typically can only handle first order dynamics, can only deal with a small set of constraints and a small number of agents, or require too much on-board memory. In (McGuire et al., 2019), a swarm gradient bug algorithm reacts to static obstacles on the fly, but collisions still occur. In (Preiss et al., 2017b), single piece polynomial planners can follow predefined paths uploaded offline for a single quadrotor, but are not suitable for dynamically changing environments. They use artificial potential fields on a swarm of these UAVs hovering in formation and show avoidance of an obstacle with a known position in a distributed fashion, but without providing theoretical safety certificates on collision avoidance or actuator saturation. A promising approach to the computationally efficient robust constrained control of nonlinear systems is proposed in  and uses an experience driven Explicit MPC (EMPC). This method was implemented in  and reliably ran at 100 Hz on board the tiny MAV's firmware in the presence of control input and velocity constraints. Due to the nature of EMPC, however, the introduction of collision avoidance constraints between multiple robots would make the EMPC database grow exponentially in size, thus becoming prohibitive for fast online queries.

Contributions
To the best of our knowledge, the literature does not provide any provably safe control techniques that achieve on-board real-time control of large nano-quadrotor swarms with higher-order dynamics in the presence of actuator, obstacle, and agent collision avoidance constraints.
This work is based on the Explicit Reference Governor (ERG), which is a novel framework for the closed-form feedback control of nonlinear systems subject to constraints on the state and input variables . This approach does not rely on online optimization and is particularly promising for control applications with fast dynamics, limited on-board computational capabilities, or strict regulations on code reliability. This article extends the centralized ERG framework ) and a distributed ERG (D-ERG) (Nicotra et al., 2015) formulation, and encapsulates these two core contributions: 1. The ERG theory is extended to distributed multi-agent systems with fourth-order dynamics and subject to constraints on states and actuator inputs. This work supplies all theoretical details of a general and scalable D-ERG framework along with a formal proof on correctness, the formulation of different offline design strategies for computing safe threshold values of Lyapunov and invariance-based level sets. Moreover we formulated two swarm collision avoidance control policies, a decentralized and a distributed version, that require a different information exchange. 2. The effectiveness, robustness, and computational efficiency of our control and navigation layers, running on-board the Crazyflie nano-quadrotor at 500 Hz, is validated extensively in several scenarios with single or multiple quadrotors subject to state and input constraints. All proposed formulations are validated and quantitatively compared. These are the first published experimental results on the use of ERG and D-ERG on quadrotors, and (to the best of our knowledge) is the only work in the literature that achieves provably safe constrained control at such high frequencies on-board nano-quadrotors for such a broad set of state and input constraints. The D-ERG's goal satisfaction and safety certificates are put in sharp contrast with those of a Navigation Field method that suffers from instabilities and collisions when the agents posses higher-order dynamics.
The rest of this article is organized as follows. Section 3 introduces the used notation. The problem is formulated in Section 4. The proposed strategy is outlined in Section 5, and constitutes the control layer and the navigation layer which are described in Section 6 and in Section 7, respectively. The results of extensive hardware validations and a comparative simulation study with single and swarms of nano-quadrotors are presented in Section 8, and discussed in Section 9. Finally, some concluding remarks are given in Section 10.

NOTATION
In this work, all vectors are column vectors. Unit vectors are denoted using the hat symbol a. Unit vectors aligned with the axes of a right-handed Cartesian reference frame are denoted as e 1 , e 2 , e 3 . 0 m×n and 1 m×n represent m × n matrices of zeros and ones, respectively. I n represents an identity matrix of dimension n × n. following norm v xy v 2 1 + v 2 2 . The hat operator ∧ :

The concatenation of vectors
whereas the vee operator ∨ : SO(3)1R 3 denotes the vector extraction of the skew-symmetric terms

PROBLEM FORMULATION
The system and parts of the problem are stated first. Section 4.1 presents the dynamic model of a generic quadrotor. Nevertheless, the proposed method can be readily extended to any VTOL vehicle. The state and input constraints, which each agent should always satisfy, are defined in Section 4.2 and illustrated in this video https://youtu.be/le6WSeyTXNU.

Dynamic Model
As depicted in Figure 1, each agent of the robotic swarm is modeled as a quadrotor with mass m ∈ R > 0 and moment of inertia J ∈ R 3×3 > 0 , J J T defined with respect to the body reference frame B. Let p [x, y, z] T ∈ R 3 and _ p [ _ x, _ y, _ z] T ∈ R 3 denote the position and the velocity of the body reference frame B with respect to the inertial reference frame W. The attitude of each agent is represented by either the rotation matrix R or by the roll, pitch, and yaw angles Θ [ϕ, θ, ψ] T ∈ R 3 that realign the axes of B with the axes of W. Finally, ω ω x x B + ω y y B + ω z z B ∈ R 3 denotes the angular velocity of the vehicle expressed in the frame B.
As detailed in (Hua et al., 2013), the dynamic model of a generic VTOL subject to a gravitational force in the − z W direction, a unidrectional thrust force T ∈ R ≥ 0 in the z B direction, and a torque vector τ ∈ R 3 about the axes of B is where z B R e 3 , g −g e 3 , and g ≈ 9.81 m/s 2 is the gravitational acceleration. System (3) possesses fourth-order dynamics and can be entirely described by the state vector subject to the control input vector For the specific case of a quadrotor, it is possible to rewrite the control input (5) as a function of the motor voltage commands U [U 1 , . . . , U 4 ] T ∈ R 4 , leading to where d is the nominal distance between the motor axis and the center of mass of the aircraft, and K T , K τ ∈ R > 0 denote the actuator's thrust and torque constant respectively.

State and Input Constraints
To ensure safety of a swarm of N a agents, every agent i ∈ {1, . . . , N a } is subject to the following constraints.

Saturation (Static Box Input Constraints)
Actuator saturation has been observed as the primary cause of instability for quadrotors in free flight. Indeed, whenever one of the motors is subject to saturation, the control law is unable to generate an arbitrary torque vector. This can lead to undesired attitude oscillations that quickly devolve into catastrophic failures. To prevent this scenario, each motor voltage U j is required to stay within its lower and upper saturation limits, with U min < U h mg/(4K T ) < U max ∈ R > 0 and U h defines the motor voltages required for static hovering in place.

Walls (Static Polytopic State Constraints)
All agents have collision radius R a ∈ R > 0 and are required to operate in a confined environment defined by a convex polytope of N w oriented faces (i.e. planar walls). To enforce this requirement, each agent i must satisfy the following convex constraint with c wj ∈ R 3 denoting the normal vector on the wall pointing in the inadmissible direction and d wj ∈ R describing the shortest distance between the origin of W and the wall.

Obstacles (Static Cylindrical/Spherical State Constraints)
In addition to planar walls, all agents must also avoid collision with N o cylindrical obstacles. To enforce this requirement, each agent i must satisfy the following non-convex constraints with cylinder radius R oj ∈ R > 0 and center o j ∈ R 3 . Note that the cylindrical obstacles can be replaced with spheres by replacing xy with the Eucledian norm.

Agent Collisions (Collaborative Cylindrical/ Spherical State Constraints)
To prevent undesirable interactions between agents (e.g. collision, propeller downwash, sonar jamming), each pair of agents is tasked with satisfying the following dynamic cylindrical exclusion constraints As per the previous case, it is trivial to replace the cylindrical constraint with a spherical constraint if vertical agent interactions are not deemed problematic.

Control Objectives
The aim of this paper is to develop a guaranteed safe distributed constrained control strategy for an homogeneous swarm of quadrotors with very limited on-board resources for computation, memory, and communication. It is assumed that all agents are collaborative and that the locations of all nearby obstacles are known within the MAV's limited sensing range. Let each agent be subject to an a priori unknown and arbitrary reference r i (t) [p r i (t) T , ψ r i (t)] T ∈ R 4 , where p r i and ψ r i are the target position and yaw of agent i. The aggregate reference for the swarm, denoted by r 1:N a (t), is steady-state admissible at time t if p r 1:N a (t) satisfies constraints (8)-(10). The purpose of this paper is to design a feedback control law in the form U 1:N a (r 1:N a (t), x 1:N a (t)) such that the following objectives are achieved for a suitably large set of initial conditions x 1:N a (0): • Safety: For any piecewise continuous reference r 1:Na (t), the control law is able to guarantee constraint satisfaction, i.e. the set of constraints (7)-(10) on the state and input variables of all agents c(x 1:Na (t), U 1:Na (t)) ≥ 0, ∀t ≥ 0; • Asymptotic Stability: If the reference r 1:Na is constant and steady-state admissible, the closed-loop system satisfies lim t → ∞ ([p 1:N a (t) T , ψ 1:Na (t)] T ) r 1:N a ; • Robustness: The control law must ensure safety and stability in the presence of model uncertainty, sensor noise, and external disturbances; • Reactiveness: The control law must run in real-time on-board the nano-quadrotor's hardware, without relying on off-board pre-generated trajectories; • Scalability: Each agent must be capable of generating its own control input based on local information. To this end, interagent communication is limited to a given radius.

PROPOSED STRATEGY
The main challenge that arises from the control problem stated in Section 4.3 is that it combines the nonlinear dynamics of the individual agent with the nonconvex constraints of the aggregated swarm. The higher-order nonlinear agent dynamics (3) would be significantly easier to stabilize in the absence of constraints, whereas the position constraints (8)-(10) would be easier to enforce if the agent dynamics were a first-order linear system _ p i ρ i as in (Fathian et al., 2019). We propose a multi-layer control architecture that relies on the ERG framework  and decouples the control problem into more tractable sub-tasks to facilitate on-board implementation. The first task, which is handled by the Control Layer, consists in pre-stabilizing the dynamics of each agent to a locally defined T ∈ R 4 . This will be done using a classical inner-outer loop controller that does not account for system constraints and does not require any form of inter-agent coordination. The second task, which is handled by the Navigation Layer, consists in manipulating the aggregate auxiliary references v 1:Na (t) so that the constraints are always satisfied. This layer is also responsible for coordinating the overall swarm and reaching the target configuration r 1:Na (t).
The proposed control architecture is illustrated in Figure 2. The detailed design of the control and navigation layers will be addressed in Sections 6 and 7, respectively.

CONTROL LAYER
The goal of the control layer is to pre-stabilize the individual quadrotors using a classical nonlinear inner-outer loop control law (Mellinger and Kumar, 2011;Hua et al., 2013). This is done without accounting for the state or input constraints, which will instead be handled by the navigation layer. The proposed architecture of the control layer is illustrated in Figure 3.

Inner-Outer Loop Control Law
The objective of the outer loop is to control the position of the quadrotor under the assumption that the attitude dynamics are instantaneous. To this end, we define the auxiliary control input where TR d e 3 is the desired thrust vector expressed in W. Using a PD control law with gravity compensation, the outer loop control inputs T and R d are chosen so that where K P , K D > 0 are diagonal gain matrices. The total thrust can thus be obtained as The target attitude is R d R ψ v R α d , where R ψ v is a standard rotation of ψ v around the third axis, whereas R α d is the minimum rotation α d that aligns z W with the desired z d B T d /T and one can obtain it using the Rodrigues formula with α d arctan( The objective of the inner loop is to control the attitude dynamics of the UAV such that the rotation matrix R asymptotically tends to a constant R d . As detailed in Lee (2011), a possible strategy to compute the torque vector is to define the attitude error as and compute the control torques as follows, FIGURE 2 | Distributed Constrained Control Architecture − The higher-order dynamics of each agent in the multi-robot system are stabilized by a Pre-Stabilizing Control (PSC) unit that computes the control inputs u i using only x i for state feedback and without accounting for constraints. An Explicit Reference Governor (ERG) block is placed in a distributed fashion before each pre-stabilized agent and only relies on information v N i available in its local one-hop spherical neighborhood N i to enforce state and input constraints and achieve asymptotic convergence to r i . In this article v N i represents the set of applied references v k in the distributed policy or the set of states x k in the decentralized policy (such that a worst-case approximation of v k can be locally computed) for all agents k in the one-hop local neighborhood of agent i. We assume each agent can communicate in parallel with its neighbors.
FIGURE 3 | Pre-Stabilizing Control Scheme − In the traditional inner-outer loop control paradigm, it is assumed that the inner loop control law stabilizes the attitude dynamics an order of magnitude faster than the outer loop control law stabilizes the position dynamics.

Robust Closed Loop Dynamics
The following Lemma states the robustness of the outer loop dynamics to attitude errors. LEMMA 1. Let system (3) be subject to the outer loop controller (12), with K P , K D > 0, and the inner loop controller (15), with K R , K ω > 0. Assume that the inner loop dynamics are sufficiently fast with respect to the outer loop dynamics. Given a constant applied position reference p v and a constant applied yaw reference ψ v , then with P 1 2 is a Lyapunov function of the outer loop dynamics ∀ε ∈ (0, 1). Moreover, the outer loop is Input-to-State Stable (ISS) with restrictions on the attitude error. Proof: Given ∀ε ∈ (0, 1), (16) is an ISS-Lyapunov candidate function for the outer loop dynamics. Noting that for a non-ideal inner loop R e 3 RR d T R d e 3 , the closed loop position dynamics, obtained by combining (3) and (12), without assuming R d ≈ R, have the form whereR RR d T represents the attitude error. Equation (18) is a Linear Parameter Varying (LPV) system that can be written in state-space form with Noting that A(I 3 ) T P + PA(I 3 ) < 0 as detailed in (Khalil, 2001, Example 4.5, pp. 121-122), it follows that A(R) T P + PA(R) ≤ 0 forR sufficiently close to I 3 (i.e. for a sufficiently small attitude error). This shows that (18) is Input to State Stable (ISS) with respect to sufficiently small attitude errors. ▪ 7 NAVIGATION LAYER

Distributed Explicit Reference Governor
The ERG is a general framework for the constrained control of nonlinear systems introduced in (Garone and Nicotra, 2016;Nicotra and Garone, 2018). Consider a pre-stabilized system _ x f (x, v) such that, if the applied reference v remains constant, the closed-loop equilibrium point x v is asymptotically stable. Given a continuous steady-state admissible path Φ : [0, 1] → R 3 between an initial reference Φ(0) v(0) and a target reference Φ(1) r, the principle behind the ERG is to generate a reference v(t) ∈ {Φ(s)|s ∈ [0, 1]} such that • the transient dynamics of the closed-loop system cannot cause a constraint violation; However, rather than pre-computing a suitable trajectory v(t), the ERG achieves these objectives by continuously manipulating the derivative of the applied reference as follows where ρ(v, r) is the Navigation Field (NF), i.e. a vector field that generates the desired steady-state admissible path Φ(s), and Δ(x, v) is the Dynamic Safety Margin (DSM), i.e. a scalar that quantifies the "distance" between the transient dynamics of the pre-stabilized system and the constraint boundaries if the current v(t) were to remain constant. The principle behind the ERG framework is illustrated in Figure 4. This section extends the ERG framework to handle the case of multi-agent systems. The main challenge is given by the fact that the Distributed ERG (D-ERG) solution must ensure the satisfaction of multi-agent coordination constraints g(x i , x k ) ≥ 0, such as the collision avoidance constraints (10). These constraints are not only dependent on agent's i own dynamics, but also on the dynamics of agents k with k ≠ i. Hence, the original ERG framework, presented in (Nicotra and FIGURE 4 | Basic Idea of the Invariant Level Set Explicit Reference Governor − The spherical obstacle is avoided by moving the applied reference v(t) over the a priori unknown (i.e. non pre-computed) path Φ(s) of steadystate admissible equilibria. The green ellipsoid represents the invariant level set value V(x, v) which embeds the future trajectory of x(t) if the current v(t) were to remain constant. The orange ellipsoid represents the threshold value Γ(v) of the invariant level set that touches the obstacle constraint. The Dynamic Safety Margin (DSM) Δ(x, v) is proportional to the difference between these level-set values and represents how safe it is to change v(t) in the direction of the Navigation Field (NF) ρ(v, r), with attraction toward the desired reference r and repulsion away from obstacles.
Frontiers in Robotics and AI | www.frontiersin.org June 2021 | Volume 8 | Article 663809 Garone, 2018, Theorem 1), would require a single, centralized ERG scheme to enforce the full set of constraints c(x 1:N a , v 1:N a ) ≥ 0 on the aggregated states and references. Computing a single, nonconservative DSM would be challenging. Moreover, this scheme would inherently limit the velocity of the aggregate reference _ v 1:Na based on the agent that is closest to constraint violation, resulting in poor performance.
Here, the objective is to show that it is possible to ensure convergence and constraint satisfaction for the overall swarm by manipulating the reference of each agent in a distributed fashion as follows with v N i defined in Figure 2. The proposed solution computes a DSM for each agent and is based on decomposing the multiagent coordination constraints g(x i , x k ) ≥ 0 into an auxiliary constraint on the references, i.e. γ 1 (x vi , x v k ) ≥ δ, and an auxiliary constraint on the dynamics of the individual agents, i.e. γ 2 (x i , v i ) ≥ 0, which can be accounted for in the NF and the DSM, respectively. In what follows h(x i , v i ) ≥ 0 denotes the set of agent independent constraints, such as constraints (7)- (9). The rest of this section provides the updated definitions of the NF (21) by identifying sufficient conditions for the correct behavior of the D-ERG, as proven in Theorem 1. The schematic representation of the D-ERG is illustrated in Figure 5. DEFINITION 1 (Navigation Field). Let the NF ρ(v N i , r i ) be such that, for any possibly time-varying piecewise continuous reference r 1:N , the initial value problem satisfies the following. The key takeaway from Definition 1 is that it only considers the first-order dynamics (22). Thus, the NF is only responsible for generating a steady-state admissible path that connects the current references v 1:N to the target references r 1:N . Since the NF does not account for the system dynamics, we refer to δ > 0 as the "static safety margin".
DEFINITION 2 (Dynamic Safety Margin). Let the DSM Δ(x i , v i ) be such that the solution of the initial value problem satisfies the following.
The intuition behind the DSM is that it quantifies the distance between the constraints and the transient dynamics of the individual closed-loop system. THEOREM 1. Consider N identical pre-stabilized systems _ x i f (x i , v i ) such that, if the applied reference v i remains constant, the closed-loop equilibrium point x v i is asymptotically stable. Let each agent be subject to a set of agent-independent constraints h(x i , v i ) ≥ 0 and a set of multi-agent coordination constraints g(x i , x k ) ≥ 0 with i ≠ k. Moreover,  Figure 2, that is available in its local one-hop neighborhood and communicates its own signals v i or x i (but not both) with its neighboring agents, making the ERG distributed.

▪
It is worth noting that, if V is equal to the entire set of steady-state admissible constraints, Theorem 1 implies convergence ∀v 1:N (0) ∈ V. However, if the NF admits deadlock configurations, the D-ERG will inherit the same limitations. The following subsections specialize the proposed D-ERG theory to the constrained control of a swarm of quadrotors. The choice of the auxiliary constraints that ensure multiagent collision avoidance, as stated in (24), is illustrated in Figure 6. The pseudocode of the D-ERG is given in Algorithm 1, and the accompanying Table 1, which lists the type and amount of instructions to be executed, shows that the proposed D-ERG approach is computationally efficient and scalable.

Navigation Field
As detailed in , the NF of agent i can be designed using a traditional attraction and repulsion field 1 FIGURE 6 | Geometric 2D representation of distributed collision avoidance between two pre-stabilized agents i (left) and k (right) with safety radii R a (dark gray disks), drawn from the perspective of agent i. The current position of each agent is p i (t), p k (t), whereas their current reference is p v i (t), p v k (t). Due to the auxiliary constraint (38) (in light gray), accounted for in the DSM, the smallest possible distance between the two agents is equal to the distance between their worst-case future positions p wc i , p wc k . Together with the auxiliary constraint (37), which is enforced by the NF, this ensures the collision avoidance constraint (10). If the agents share their references (case A), agent i can compute the worst-case future position of agent k based on its current reference p v k (t). If agent i only knows the position of agent k (case B), it must use p k (t) to compute the worst-case current reference p v,wc k (t) and must then compute the worst-case future position based on p v,wc k (t).
1 dependency of ρ on (v N i , r i ) is omitted for simplicity of notation. where the attraction field is η, η ψ > 0 are small smoothing radii chosen to avoid numerical problems when r i − v i → 0, and The repulsion field is the sum of linear repulsion fields pushing away from walls (w), obstacles (o), and nearby agents (a), i.e.
The repulsion field of all wall constraints is where ζ w > 0 is the influence margin outside of which the repulsion field has no effect and δ w ∈ (0, ζ w ) is the static safety margin which guarantees that the reference is strictly steady-state admissible. The repulsion field of all static cylindrical obstacles includes the conservative (co) term with an influence margin ζ o j > 0, a static safety margin δ oj ∈ (0, ζ o j ) and C j (p v i ) p v i − o j xy − (R oj + R a ). For spherical constraints, one can just use the full Euclidean norm and not project (o j − p v i ) on the xy-plane. As detailed in (Koditschek and Rimon, 1990), however, conservative vector fields cannot achieve global stability in the presence of obstacle constraints. Therefore, the repulsion field also includes a non-conservative (n-co) term that destabilizes local saddle points where with circulation gain α o j > 0. For the case of a sphere, the term within brackets can be replaced by In a similar way, one can define the repulsion field that acts on agent i caused by the other agents k as where ρ a,co with C ik (p v ik ) p v ik xy − 2R a − 2S a , and ρ a,n−co with ζ a > 0, δ a ∈ (0, ζ a ), C ik (p v ik ) p v ik xy − 2R a − 2S a , and α a > 0. This is sufficient to ensure the auxiliary constraint Following from Theorem 1, (24), agent collision can now be avoided by introducing the auxiliary constraint As shown in Figure 6, the combination of (37) and (38) satisfies (10). (35) and (36) assume that agent i knows the difference between its own reference and the reference of agent k. However, the contribution of agent k becomes zero if p v ik xy ≥ ζ a + 2R a + 2S a . As a result, it is assumed that agents only share their reference with other agents within an interagent distance of ζ a + 2R a + 4S a . A possible option to eliminate communication entirely (i.e. a decentralized approach) is to have each agent measure the position of its neighbors (instead of communicating the applied references) and compute the worstcase references of the neighbors that would still ensure that (37) and (38) imply (10). This leads to two possible options

REMARK 1. Equations
where the latter has the advantage of not requiring inter-agent communication but also leads to a more conservative coordination strategy, as illustrated in Figure 6.

Dynamic Safety Margin
For each agent i its DSM, used in (21), can be obtained by taking the worst case DSM (i.e. the smallest one) of all active saturation (s), wall (w), obstacle (o), and agent collision (a) constraints 2 , For the offline design of the DSM we do not rely on explicit trajectory predictions, but use Lyapunov theory and optimization to design the DSM. As such, the following lemma is an important result used throughout this work to compute offline safe threshold values of Lyapunov level sets. As was visualized in Figure 4, it guarantees constraint satisfaction if the system dynamics never make its Lyapunov level set value V(x(t), v(t)) exceed that threshold value Γ(v(t)).
LEMMA 2. Given a nonlinear pre-stabilized system _ be a Lyapunov function and let be a linear constraint. Then, the Lyapunov treshold value is such that V(x, v) ≤ Γ(v)0 (41). Proof: See (Nicotra et al., 2019).

▪
Since the DSM is computed on a per-agent basis, the agent index i will be omitted for the sake of notational simplicity.
The following paragraphs address each constraint separately.

Saturation Constraints
In this section we show three strategies to compute a safe threshold value that ensure constraints on at least a subset of the inputs (5) are satisfied. The quantitative effects of these three strategies for an input constrained double integrator system are depicted in Figure 7.
Traditional Lyapunov Level Set Strategy (Trad Lyap): One practical approach is to consider the outer loop control law and ensure the box constraints on the total thrust are satisfied, Since the inequality constraint (43) is nonlinear in the outer loop state variables, it is necessary to find a linear constraint that implies (43), in order to apply Lemma 2. A possible approach to provide a linear constraint is to make a distinction between the steady-state thrust mg e 3 and the dynamic feedback For the upper limit of the thrust constraint, this can be done by using the triangular inequality, and we obtain it is therefore sufficient to ensure that, ∀e ∈ R 3 : This is equivalent to limiting the maximum acceleration of the UAV in any direction. The main interest with (45) is that it defines a rotationally invariant constraint that is linear for any given unitary vector e, which can be expressed in the linear and d(p v ) T max − mg/m − K P e T p v . Assuming unidirectional gains K P k P I 3 and K D k D I 3 , the associated threshold value (42) is, Similarly, Γ Tmin can be computed by replacing T max in (46) with T min . The DSM that prevents the total thrust to saturate is with κ s ∈ R > 0 .
Optimally Aligned Lyapunov Level Set Strategy (Opt Lyap): This section is an extension of the theory in  and applies it to higher-order quadrotor dynamics. Since linear systems are characterized by an infinite choice of quadratic Lyapunov functions, a way to improve the performance of the outer loop dynamics is to select the optimal Lyapunov based threshold value that is perfectly aligned with the total thrust constraints, instead of using (46), which is not aligned. Hence, one can find a common Lyapunov function in the quadratic form with P T > 0 that satisfies the Lyapunov equation A(R) T P T + P T A(R) ≤ 0 and A(R) defined in (6.2). By taking advantage of the rotational symmetry of the system and defining P T P T,11 I 3 P T,12 I 3 P T,21 I 3 P T,22 I 3 , the optimal Lyapunov function can be obtained by solving the following linear matrix inequality whereα and Δα are the current and the maximum allowed rotational error between z B and z d Given the quadratic Lyapunov function (48), we obtain the threshold values The DSM that prevents the total thrust to saturate and is based on the Lyapunov function that is optimally aligned with this constraint, then becomes Optimally Aligned Invariant Level Set Strategy (Opt Inv): A more generic safe set can be obtained by considering the outer loop dynamics (19) with input (12) and computing offline the threshold value associated to the largest possible optimally aligned Lyapunov level set that satisfies the constraints of the following minimization problem with the closed position loop dynamics f (p, _ p, p v ) and the total thrust gradient ∇T(p, _ p, p v ). Doing so, one can obtain a safe invariant set by taking the optimally aligned Lyapunov level set and subtracting the inadmissible region, i.e. the region where the constraints are violated T ≥ T max or T ≤ T min . The invariant set based DSM can be computed as, REMARK 2. To avoid motor saturation when tracking a nonzero yaw reference, it is also necessary to add an ERG on the yaw axis. This can be done using the NF in (26) and the DSM Δ s,ψ κ s,ψ min j∈{1,2,3,4} with κ s,ψ ∈ R > 0 .

Wall Constraints
The convex inequality constraints (8) are equivalent to (41) with c [c T w j , 0 T 3×1 ] T , and d(p v ) d wj − R a . As a result, the threshold value associated to the j-th wall constraint is The dynamic safety margin corresponding to the wall constraint closest to violation then becomes, with κ w ∈ R > 0 .

Obstacle Constraints
Constraint (9) defines a non-convex admissible region. Given a fixed reference p v , it can be shown using triangular inequalities that As a result, (9) can be enforced by simply ensuring The inequality constraints define a reference-dependent virtual wall and are equivalent to (41) The DSM related to this constraint then becomes, with κ o ∈ R > 0 .

Agent Collision Avoidance
As explained in Section 7.2, collision avoidance can be satisfied by also enforcing the auxiliary constraint (38). Since constraint (38) applies equally in every direction in 3D space, it can be enforced using the Lyapunov threshold value associated to the linear constraint thus leading to The DSM related to this constraint then becomes, with κ a ∈ R > 0 .

RESULTS
We present the first results of an extensive experimental validation of the ERG and the D-ERG frameworks by means of single and multi-robot hardware experiments (a video of the experiments can be found at https://youtu.be/le6WSeyTXNU) using the experimental setup described hereafter. In a comparative simulation campaign we have analyzed statistically the goal and constraint satisfaction properties of FIGURE 7 | Phase plane representation of the proposed input constraint enforcement strategies, illustrated for a second-order dynamical system m € p T − mg subject to the pre-stabilizing control law T m(k P (p v − p) − k D _ p + g) with p v 0 and the input constraint T ≤ T max . The traditional Lyapunov based level-set (dark-grey) yields the most conservative DSM (47). Aligning the level-set with the constraints (medium gray) by solving the offline optimization problem (50) drastically increases the certified safe region (53). Further improvements can be obtained by solving (54) and using the invariant set which is the set obtained after subtracting the intersection between the light-grey Lyapunov level set and the region violating the input constraint from the light-grey Lyapunov level set. All three sets are certifiably safe since the flow vectors of the closed-loop system (in blue) all point inward. Note that, due to the high values in the first block diagonal of (17), any constraint that only depends on the position error variables, e.g. of the form p a with a ∈ R, is already very well aligned under the traditional strategy. Hence, performance benefits from optimal alignment are marginal.
Frontiers in Robotics and AI | www.frontiersin.org June 2021 | Volume 8 | Article 663809 our methodology. A summary of these results can be found in Section 9.

Experimental Setup
The experiments are performed using Crazyflie 2.1 nanoquadrotors in a Vicon motion capture system for indoor localization based on the Crazyswarm system architecture of (Preiss et al., 2017b). The computationally efficient control and navigation layers of Sections 6 and 7 are implemented in C and run at 500 Hz on-board the Crazyflie's STM32F4 microprocessor's firmware. The only programs running on the ground station are the special purpose motion capture tracker (Preiss et al., 2017b), a code for sending goal configurations to each quadrotor, and a code that mimics local communications between agents. Each UAV sends and receives new goal and feedback signals (i.e. the agent's own state and neighbor information) via Crazyradios PA at 100 Hz. An on-board Kalman filter updates the agent's own states at a higher rate than the motion capture system, but for the neighbor information such a Kalman filter update is not present. The experiment data is logged on-board the quadrotors on micro SD cards. Each UAV is modeled with a static safety radius of R a 0.08 m and a mass of approximately 34.6 g. Its inertia matrix J diag(17.31, 17.94, 33.75) · 10 −6 kgm 2 is calculated from a CAD model and is only used to estimate the actuator torque constant. The estimated actuator thrust and torque constants amount K T 0.012 N/V 2 and K τ 6.84 · 10 −6 Nm/V 2 , respectively. The nominal distance between the motor axis and the center of mass of the aircraft amounts d 4.65 cm.

Tuning Guidelines
Here, we list guidelines for the tuning of the main parameters of the control and navigation layer and how this relates to the obtained performance and robustness. We advise users of this approach to tune the parameters in the order as they are listed below and to start with the input saturation constraints, followed by static and dynamic obstacle constraints.
(1) First tune the inner loop gains K R , K ω > 0 and then the outer loop gains K P , K D > 0 for stable regulation control performance. The outer loop's settling time should be an order of magnitude slower than the one of the inner loop. This step is accomplished without worrying about the effect on any of the input or state constraints. The stiffer the prestabilized closed-loop system is tuned, the more the agents can be stacked in a smaller volume, at the cost of a more precise and higher rate odometry.
(2) Eliminate numerical noise in the attraction field by selecting a strictly positive, but small value for the smoothing radius η.
(3) Increase the DSM gains κ until no further performance increase is obtained. These gains are chosen such that the DSMs of the active constraints have the same order of magnitude. (4) Choose medium influence margins ζ defining from how far the obstacles are considered in the repulsion field. Too large values will require too large sensing ranges for static obstacles or communication ranges for dynamic obstacles, whereas too low values do not give enough reaction time. (5) For cooperative agent collision avoidance, choose the maximum position error radius S a . The larger this value, the higher the maximum attainable robot's speed, but the larger the distance traveled by each agent to reach its goal. (6) Select small circulation gains α around obstacles and agents to avoid robots getting stuck in local saddle points. Too large values tend to increase the settling time. (7) Choose strictly positive static safety margins δ to increase robustness. This also ensures the NF's repulsion term achieves its maximum amplitude while the DSM stays strictly positive. Hence this allows moving (and not blocking) the reference in directions pointing outward the obstacle constraint.
In all the experiments, the control gains of the inner-outer loop control law detailed in Section 6 are K P 13.0 I 3 , K D 5.0 I 3 , K R diag(0.005, 0.005, 0.0003), and K Ω diag(0.001, 0.001, 0.00005), which give moderately aggressive performance. The attraction field of the navigation layer is chosen with η η ψ 0.005. Other parameters defined in Section 7 are specified in the following sections.

Point-to-Point Transitions − Input Constraints
In the accompanying video we show that point-to-point transitions can easily destabilize a pre-stabilized quadrotor due to actuator saturation when the changes in p v become too abrupt.
The goal of the experiments is to validate the theory of Section 7.3.1 by showing that the navigation layer ensures safety for whatever p r and to quantify the difference in performance of the three strategies used to compute the DSM. To do so, we sequentially performed the following three experiments with a quadrotor where the navigation layer ensures input constraints satisfaction with U min 0.0 V, U max 3.5 V or T min 0.0 N, T max 0.59 N by using either: The desired position set-point is always reached in a stable and safe (i.e. DSM ≥ 0) manner. As expected from the theory in Section 7.3.1, a large reduction in settling time and an increase in the peak velocity is obtained when passing from a traditional Lyapunov based strategy, to the optimally aligned Lyapunov based strategy, and finally to the optimally aligned invariance based strategy. The latter gives the most aggressive performance and allows the aerial vehicle to obtain peak velocities of 2.4 m/s, which is about 2.76 times larger than what is obtained with the traditional Lyapunov based strategy. Note that the values of κ for these three cases where chosen such that the value of the DSMs are equal during hovering, i.e. when t ∈ [0.0, 0.5] s, or t ∈ [8.7, 12.5] s, or t ∈ [21.8, ∞) s.
To show the effect of time-varying yaw angle references, we sequentially performed the following two experiments with the quadrotor using the invariance based ERG on the total thrust constraints and using either: • no ERG on the yaw axis ψ; • an ERG on yaw axis ψ as in (56) with κ s,ψ 1.80.
In each of these experiments, depicted in Figure 9, the UAV starts from the initial position p(t ≤ 1.0 s) [4.0, 1.0, 0.25] T m while hovering. At t 1.0 s and at t 6.0 s it receives the same position step references as in the previous experiment, but simultaneously it also receives yaw step references between 0 + and 120 + (No ERG on ψ), and between −90 + and 270 + (ERG on ψ).
In the absence of an ERG on the yaw axis, the system remains stable under severe actuator saturation for the simultaneous position and yaw commands given at t 1.0 s but becomes unstable for the commands given at t 6.0 s. On the other hand, the system displays a stable, safe, and aggressive behavior during the whole experiment when the ERG is also applied to the yaw axis.

Point-to-Point Transitions − Wall Avoidance
The results depicted in Figure 10 show the aerial vehicle avoiding two virtual walls with c w 1 [1, 0, 0] T m, d w 1 4.8 m, and One can also see that the NF is designed such that it handles steady-state inadmissible references, which are depicted by black dots outside of the convex region in Figure 10.

Multiple Aerial Robots Experiments
In these experiments the UAVs are modeled as cylinders as detailed in Section 4.2.4, preventing them to fly over each other. Similarly to (Preiss et al., 2017a;Honig et al., 2018;Vukosavljev et al., 2019), this choice prevents a MAV's propeller downwash effect to destabilize other MAVs which are flying closely underneath.

Provably Safe Human-Swarm Teleoperation
In this experiment we show that the D-ERG ensures a swarm of N a 4 quadrotors can be teleoperated by a human in a provably safe way within a confined environment composed of wall constraints with c w 1 [−1, 0, 0] T m, d w 1 3 m, c w 2 [1, 0, 0] T m, d w 2 4.8 m, c w 3 [0, −1, 0] T m, d w 3 2.0 m, and c w 4 [0, 1, 0] T m, d w 4 1.5 m. We use the same ERG parameters as in Section 8.3.2, and for the collision avoidance between agents, we exchange p v between the agents and use S a 0.80 m, α a 0.0, ζ a 1.50 m, δ a 0.01 m, and κ a 50.0. The human operator accelerates and decelerates the motion capture calibration wand fast in 3D space such as to exploit the quadrotor dynamics. Each agent is tasked to yaw in the direction of the wand and follows its relative position displacement. The logged data is depicted in Figure 11. The requested aggregated reference that wants to keep the swarm in FIGURE 9 | Simultaneous Point-to-Point Transitions and Discontinuous Yaw References With and Without Violation of Input Constraints − Besides an ERG on the position variables that limits the total thrust, an invariance based ERG on the yaw axis is required to ensure safety for non-stationary yaw references.
Frontiers in Robotics and AI | www.frontiersin.org June 2021 | Volume 8 | Article 663809 a rigid square formation is deformed by the navigation layer by decreasing the rate of change of the reference applied to each pre-stabilized agent when it comes closer to violations of input, wall, or agent collision constraints. One can see that around t 20.0 s, there are short periods where the actuator inputs come very close to their upper and lower limits and Δ s i is very close to zero, such that the applied reference is kept almost constant.

Point-to-Point Transitions − Agent Collision Avoidance
In Figure 12 the results of two experiments with a swarm of N a 5 agents are depicted. Every agent is commanded to transition between specific goal positions at t 1.0 s and at t 26.0 s, such that if the agents are coordinated effectively, this globally leads to a line formation for the swarm. Moreover, they have to stay inside a confined environment bounded by four walls with FIGURE 10 | Point-to-Point Transitions with Wall Avoidance − The quadrotor achieves top speeds of 2 m/s and slows down as to avoid wall collisions, even when the position references are steady-state inadmissible.
FIGURE 11 | Results of the Human-Swarm Teleoperation Experiment in a Confined Environment − The D-ERG ensures the safe coordination of the quadrotor team formation. During the short periods where the actuator inputs come very close to their upper and lower limits (around t 20.0 s), the DSM decreases rapidly such that the applied reference is kept almost constant. Note that the steady-state motor voltages during hovering after t > 37.0 s vary in a range of 2.1 V to 3.0 V. This is caused by variability in model parameters (e.g. battery displacements from the MAV's center of mass, different motor-propeller constants) and shows the robustness of the overall approach to model uncertainty.

Analysis of Safety and Goal Satisfaction Certificates
In this simulation study we show some relevant statistics on the occurrence of constraint violations or deadlocks and compare the D-ERG with another optimization-free (i.e., closed form or explicit) approach solely based on attractive and repulsive Navigation Fields. The latter method is implemented by using the NF of Section 7.2 and by setting the DSM, which is a dynamic state-dependent and reference-dependent gain, to a user-tuned constant value. The latter can be interpreted as a fixed reference filter gain, which can only be selected before executing an experiment. The results on safety and goal satisfaction for 3D point-topoint transitions of quadrotors in an increasingly densely filled environment with static obstacles and dynamic agents are depicted in Table 2. We use a cubic environment with side FIGURE 13 | 3D Point-to-Point Transitions with Agent Collision Avoidance − Asymptotically stable, collision free consecutive formations of the initials of the University of Colorado Boulder (UCB) are made. Nine consecutive shots (a-i) show the swarm members safely navigating from an initial configuration (in blue, shot a), to the U configuration (in yellow, shot c), to the C configuration (in green, shot e), to the B configuration (in purple, shot g), and finally back to the initial configuration (in blue, shot i ).
Frontiers in Robotics and AI | www.frontiersin.org lengths of 16 m which is symmetrically centered in the origin. For each simulation we randomly place N o static spherical obstacles with R o 0.8 m, ζ o 1 m, κ o 20, and the initial and goal positions of N a quadrotors with ζ a 1 m, S a 1.2 m, κ a 20, κ s 6, that exchange p v with their neighbors. This random placement is done under the condition that none of the influence margins are overlapping in steady-state. Hence, the swarm's initial and desired position is at least steady-state admissible and convergence to the desired position of each agent can be detected as a static final error at the end of the simulation. For each defined combination of N o obstacles and N a agents, 500 random simulations are performed for each of the settings 1a), 1b), 2a), and 2b) depicted in Table 2. When there is at least one instability, one collision, or one deadlock detected in a simulation, the respective counters are incremented by one.
The strong safety certificates obtained when employing the D-ERG method are clear from the simulation data summarized in Table 2. The occurrence of instabilities and collisions is zero for the certified safe D-ERG, whereas for the Navigation Field (NF) method the occurrence is considerably large. When the constant reference gain in the NF approach is increased from Δ 2.8 to Δ 3.2, this leads to a larger number of collisions and instabilities due to severe control input saturation. For fair comparison, these DSM values were chosen around Δ 2.9, which is the steady-state value of the DSM in the D-ERG when a UAV hovers far away from obstacles.
For what concerns the goal satisfaction certificates, we observe almost global asymptotic stability. The statistical occurrence of deadlocks is almost negligible and only becomes measurable for very densely filled environments cluttered with agents and obstacles. Although a non-zero circulation gain ensures that pairs of agents cannot get stuck in local-saddle points, one can see that there is little benefit in using a circulation gain with a large number of agents. For some simulations it helps to avoid a deadlock, whereas in other simulations it can cause agents to get stuck in a local minimum. However, it is worth noting that this limitation is a consequence of the proposed NF and is not inherent to the D-ERG framework.

DISCUSSION
In Section 8, we presented an extensive set of experimental and simulation studies of the proposed ERG and Distributed ERG framework, with the first real-world experiments to be found in the literature. These studies demonstrate the following key results (R) when applied to a homogeneous swarm of cooperative Crazyflie 2.1 quadrotors: • R1: Computational efficiency allows high-rate real-time (500 Hz) computation of control commands on-board small UAVs with severely constrained CPU and RAM; • R2: Almost globally asymptotically stable control performance for arbitrary position and yaw references (e.g. point-to point transitions or human-swarm teleoperation scenarios) for swarms in constrained environments. The measured statistical deadlock occurrence is negligible; • R3: Provable safety under actuator inputs and state constraints, including collision avoidance between dynamical agents, and between agents and static obstacles; • R4: Robustness in the presence of real-world uncertainties (e.g. non-modeled inner loop dynamics, variability of thrust and torque constants or battery voltages, battery displacement from center of mass, sensor noise, communication delays). The lowlevel control layer is proven to be robust to small attitude errors. Moreover, the D-ERG leverages the robustness of lowlevel controllers and maintains this property. Since the D-ERG's DSMs itself relies on level-sets (i.e. Lyapunov or invariant set-based) and not on explicit state and input trajectory predictions to obtain safety guarantees, the overall approach is less model dependent and hence more robust; • R5: Planner or reference agnostic safety certification with the ability to handle steady-state inadmissible references; • R6: Offline ERG design strategies for the selection of safe threshold values to Lyapunov level-sets can lead to significant improvements in the control performance over traditional methods. Especially when the level sets are aligned with the constraints or when the more generic invariant safe sets are used with negligible increase of the on-board computational requirements. • R7: The local nature of the D-ERG makes the algorithm scale very well with the number of agents. The distributed formulation that relies on local inter-agent distance and direction in applied reference positions (i.e. requiring agent communication) can lead to significantly smaller settling times and a denser swarm when compared to the decentralized formulation relying on inter-agent distance and direction in positions (i.e. requiring communication or exteroceptive sensing). In future work, the proposed model-based add-on scheme can be further extended and combined with other control approaches, such as the adaptive control laws to deal with e.g. unmodeled dynamics, actuator deadzones as in Yang et al., 2021a), and unavailable velocity measurements as in (Yang et al., 2021b) due to noisy low-cost sensors.

CONCLUSION
In this article we formulated the theory of a provably safe distributed constrained control framework, i.e., the Distributed Explicit Reference Governor (D-ERG), and demonstrated its efficacy on a homogeneous swarm of collaborative nano-quadrotors (i.e., a swarm of palm-sized Crazyflies 2.1) through multiple hardware and simulation experiments.
This approach has the following merits. Safety is guaranteed for agents with higher-order dynamics and with a large set of hard constraints such as the four actuator input limits and static and dynamic collision avoidance constraints. In contrast to optimization-based control schemes, this algorithm has a low cost of computation and memory and runs in real-time at a 500 Hz rate on-board the limited available robot hardware. Thereby, its local and reactive nature provides a good scalability to a large number of robots and obstacles. Since this add-on scheme only requires a pre-stabilized plant, it can be of great practical use when the controller is not accessible or not allowed to be changed, which is very often the case for commercial UAV flight control units. Its simple yet effective design makes it an interesting method for industrial robotic applications requiring safe real-time control systems.
However, some limitations still exist and can be addressed in future work. Since the Dynamic Safety Margin uses a single scalar to change the amplitude of the applied reference signal in the direction of the Navigation Field, the performance would reduce when applying this technique to systems with an increased state space dimension. Also, this robust level-set based D-ERG approach comes at the cost of an increased level of conservatism compared to approaches where the future trajectory is explicitly predicted or optimized for. Although the statistical occurrence of deadlocks is very low, the employed Navigation Field does not formally guarantee the absence of deadlocks.

DATA AVAILABILITY STATEMENT
The raw data supporting the conclusion of this article will be made available by the authors, without undue reservation.

AUTHOR CONTRIBUTIONS
BC and MN contributed to the conception and formulation of the theory behind the D-ERG. BC was involved in designing and performing all the numerical simulations and hardware validations on the aerial robot swarm. KM assisted with the hardware validations and debugging the numerical simulations. BC wrote the first draft of the manuscript, all authors contributed to manuscript revision, read, and approved the submitted version.