Skip to main content

METHODS article

Front. Robot. AI, 22 June 2021
Sec. Robotic Control Systems

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

  • 1Robotics and Multibody Mechanics (R&MM), Department of Mechanical Engineering, Vrije Universiteit Brussel, Brussels, Belgium
  • 2Imec, Leuven, Belgium
  • 3Flanders Make, Leuven, Belgium
  • 4Robotics, Optimization, and Constrained Control (ROCC), Department of Electrical, Computer, and Energy Engineering, University of Colorado Boulder, Boulder, CO, United States

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.

1 Introduction

Swarms of aerial robots or Unmanned Aerial Vehicles (UAVs) are emerging as a disruptive technology that enables highly re-configurable, on-demand, distributed intelligent autonomous systems with high impact on many areas of science, technology, and society (Chung et al., 2018).

These swarms can be employed to solve real-world tasks where the environment is to be explored (Marconi et al., 2012; Bayram et al., 2017), and to be traversed or exploited (Vásárhelyi et al., 2018) with a prescribed goal state or a desired formation. To operate effectively in uncertain real-world environments, each agent in the swarm must be capable of safely navigating to its target along a-priori unknown paths. Not only does each robot need to respect its operational constraints (e.g. actuator saturation, speed limits, allowed flight zones), it must also avoid collisions with environmental hazards and other agents (Franchi et al., 2012; Alonso-Mora et al., 2015; Franchi et al., 2016; Zhou et al., 2018) in the presence of imperfect dynamic models, measurement noise, and communication delays. Most importantly, to ensure a high level of safety and robustness, the robots should use their on-board computational resources rather than relying on off-board resources (e.g. a ground control station). The latter provide a central point of failure, and are susceptible to time delays, communication overhead, and information loss. This calls for reactive and distributed control algorithms that can be implemented in real-time on-board UAVs and only rely on local information to solve the global navigation task safely.

Achieving goal satisfaction and safety certificates for a swarm of autonomous Micro Aerial Vehicles (MAVs) presenting limited resources for on-board computation, power, communication, 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 mission-dependent 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.

2 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.

2.1 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 (Luis and Schoellig, 2019), a scalable distributed model predictive control algorithm with on-demand collision avoidance is proposed to perform point-to-point 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 (Vukosavljev et al., 2019). Although the run-time 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 multi-agent swarms using sequential priority ordering (Bajcsy et al., 2019) or the selection of air highways (Chen et al., 2017). 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 nano-quadrotors. 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 nano-quadrotor 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).

2.2 On-Board Navigation Strategies for Nano-Quadrotors

Only few works such as (Preiss et al., 2017b; Desaraju and Michael, 2018; 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 (Desaraju et al., 2018) and uses an experience driven Explicit MPC (EMPC). This method was implemented in (Desaraju and Michael, 2018) 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.

2.3 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 (Nicotra and Garone, 2018). 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 (Nicotra and Garone, 2018) 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.

3 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. 0m×n and 1m×n represent m×n matrices of zeros and ones, respectively. In represents an identity matrix of dimension n×n. The concatenation of vectors vi to vk is denoted by the vector vi:k=[viT,,vkT]T. Given a vector in 3, xy denotes the following norm vxy=v12+v22. The hat operator :3SO(3) denotes the skew-symmetric matrix transformation

v=[0v3v2v30v1v2v10],(1)

whereas the vee operator :SO(3)3 denotes the vector extraction of the skew-symmetric terms

R=12[R32R23R13R31R21R12].(2)

4 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.

4.1 Dynamic Model

As depicted in Figure 1, each agent of the robotic swarm is modeled as a quadrotor with mass m>0 and moment of inertia J>03×3, J=JT defined with respect to the body reference frame . Let p=[x,y,z]T3 and p˙=[x˙,y˙,z˙]T3 denote the position and the velocity of the body reference frame 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 Θ=[ϕ,θ,ψ]T3 that realign the axes of with the axes of W. Finally, ω=ωxx^+ωyy^+ωzz^3 denotes the angular velocity of the vehicle expressed in the frame .

FIGURE 1
www.frontiersin.org

FIGURE 1. Schematic representation of a quadrotor agent.

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 T0 in the z^ direction, and a torque vector τ3 about the axes of is

{Positiondynamics:mp¨=Tz^+mg,Attitudedynamics:Jω˙=ωJω+τ,R˙=ωR,(3)

where z^=Re^3, g=ge^3, and g9.81m/s2 is the gravitational acceleration. System (3) possesses fourth-order dynamics and can be entirely described by the state vector

x=[pT,ΘT,p˙T,ωT]T12(4)

subject to the control input vector

u=[T,τT]T4.(5)

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=[U1,,U4]T4, leading to

u=[KTKTKTKTKTd2KTd2KTd2KTd2KTd2KTd2KTd2KTd2KτKτKτKτ][U12U22U32U42](6)

where d is the nominal distance between the motor axis and the center of mass of the aircraft, and KT,Kτ>0 denote the actuator’s thrust and torque constant respectively.

4.2 State and Input Constraints

To ensure safety of a swarm of Na agents, every agent i{1,,Na} is subject to the following constraints.

4.2.1 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 Uj is required to stay within its lower and upper saturation limits,

UminUjUmax,j{1,2,3,4},(7)

with Umin<Uh=mg/(4KT)<Umax>0 and Uh defines the motor voltages required for static hovering in place.

4.2.2 Walls (Static Polytopic State Constraints)

All agents have collision radius Ra>0 and are required to operate in a confined environment defined by a convex polytope of Nw oriented faces (i.e. planar walls). To enforce this requirement, each agent i must satisfy the following convex constraint

c^wjTpidwjRa,j{1,,Nw},(8)

with c^wj3 denoting the normal vector on the wall pointing in the inadmissible direction and dwj describing the shortest distance between the origin of W and the wall.

4.2.3 Obstacles (Static Cylindrical/Spherical State Constraints)

In addition to planar walls, all agents must also avoid collision with No cylindrical obstacles. To enforce this requirement, each agent i must satisfy the following non-convex constraints

piojxyRoj+Ra,j{1,,No},(9)

with cylinder radius Roj>0 and center oj3. Note that the cylindrical obstacles can be replaced with spheres by replacing xy with the Eucledian norm.

4.2.4 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

pipkxy2Ra,k{1,,Na}:ki.(10)

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.

4.3 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 ri(t)=[pir(t)T,ψir(t)]T4, where pir and ψir are the target position and yaw of agent i. The aggregate reference for the swarm, denoted by r1:Na(t), is steady-state admissible at time t if p1:Nar(t) satisfies constraints (8)(10).

The purpose of this paper is to design a feedback control law in the form U1:Na(r1:Na(t),x1:Na(t)) such that the following objectives are achieved for a suitably large set of initial conditions x1:Na(0):

• Safety: For any piecewise continuous reference r1: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(x1:Na(t),U1:Na(t))0,t0;

• Asymptotic Stability: If the reference r1:Na is constant and steady-state admissible, the closed-loop system satisfies limt([p1:Na(t)T,ψ1:Na(t)]T)=r1:Na;

• 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, inter-agent communication is limited to a given radius.

5 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 (Nicotra and Garone, 2018) 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 reference vi(t)=[piv(t)T,ψiv(t)]T4. 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 v1:Na(t) so that the constraints are always satisfied. This layer is also responsible for coordinating the overall swarm and reaching the target configuration r1: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.

FIGURE 2
www.frontiersin.org

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 ui using only xi 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 vNi available in its local one-hop spherical neighborhood Ni to enforce state and input constraints and achieve asymptotic convergence to ri. In this article vNi represents the set of applied references vk in the distributed policy or the set of states xk in the decentralized policy (such that a worst-case approximation of vk 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.

6 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.

FIGURE 3
www.frontiersin.org

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.

6.1 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 RdSO(3) and assume that RRd. The position dynamics in the dynamic model (3) then become

mp¨=TRde^3mge^3,(11)

where TRde^3 is the desired thrust vector expressed in W. Using a PD control law with gravity compensation, the outer loop control inputs T and Rd are chosen so that

Td=TRde^3=m(KP(pvp)KDp˙+ge^3),(12)

where KP, KD>0 are diagonal gain matrices. The total thrust can thus be obtained as

u1=T=mKP(pvp)KDp˙+ge^3.(13)

The target attitude is Rd=RψvRαd, where Rψv is a standard rotation of ψv around the third axis, whereas Rαdis the minimum rotation αd that aligns z^W with the desired z^d=Td/T and one can obtain it using the Rodrigues formula with αd=arctan((Txd)2+(Tyd)2/Tzd).

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 Rd. As detailed in Lee (2011), a possible strategy to compute the torque vector is to define the attitude error as

eR=12(RdTRRTRd),(14)

and compute the control torques as follows,

τ=KReRKωω,(15)

where KR, Kω>0 are diagonal gain matrices.

6.2 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 KP,KD>0, and the inner loop controller (15), with KR,Kω>0. Assume that the inner loop dynamics are sufficiently fast with respect to the outer loop dynamics. Given a constant applied position reference pv and a constant applied yaw reference ψv, then

V(p,p˙,pv)=[ppvp˙]TP[ppvp˙],(16)

with

P=12[KP+εKD2εKDεKDI3],(17)

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 Re^3=RRdTRde^3, the closed loop position dynamics, obtained by combining (3) and (12), without assuming RdR, have the form

p¨=R˜KP(pvp)R˜KDp˙+(R˜I3)ge^3,(18)

where R˜=RRdT represents the attitude error. Equation (18) is a Linear Parameter Varying (LPV) system that can be written in state-space form

[p˙p¨]=f(p,p˙,pv)=A(R˜)[pp˙]+B(R˜)[pv03×1]+d(R˜),(19)

with

A(R˜)=[03×3I3R˜KPR˜KD],B(R˜)=[03×303×3R˜KP03×3].

Noting that A(I3)TP+PA(I3)<0 as detailed in (Khalil, 2001, Example 4.5, pp. 121–122), it follows that A(R˜)TP+PA(R˜)0 for R˜ sufficiently close to I3 (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

7.1 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]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;

limtv(t)=Φ(1).

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

v˙=ρ(v,r)Δ(x,v),(20)

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.

FIGURE 4
www.frontiersin.org

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 steady-state 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.

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(xi,xk)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 ki. Hence, the original ERG framework, presented in (Nicotra and Garone, 2018, Theorem 1), would require a single, centralized ERG scheme to enforce the full set of constraints c(x1:Na,v1:Na)0 on the aggregated states and references. Computing a single, non-conservative 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

v˙i=ρ(vNi,ri)Δ(xi,vi),(21)

with vNi defined in Figure 2. The proposed solution computes a DSM for each agent and is based on decomposing the multi-agent coordination constraints g(xi,xk)0 into an auxiliary constraint on the references, i.e. γ1(x¯vi,x¯vk)δ, and an auxiliary constraint on the dynamics of the individual agents, i.e. γ2(xi,vi)0, which can be accounted for in the NF and the DSM, respectively. In what follows h(xi,vi)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 ρ(vNi,ri) and the DSM Δ(xi,vi) used in (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.

FIGURE 5
www.frontiersin.org

FIGURE 5. Distributed Explicit Reference Governor (D-ERG) Scheme for Agent i. To ensure that the constraints are satisfied for any desired reference configuration ri, the ERG manipulates the rate of change of the applied reference v˙i by computing a suitable Navigation Field (NF) ρi and a Dynamic Safety Margin (DSM) Δi. The NF determines the current direction of v˙i and the DSM regulates the modulus of v˙i such that dynamic transients do not cause constraint violations. Agent i relies on the signal vNi, as defined in Figure 2, that is available in its local one-hop neighborhood and communicates its own signals vi or xi (but not both) with its neighboring agents, making the ERG distributed.

DEFINITION 1 (Navigation Field). Let the NF ρ(vNi,ri) be such that, for any possibly time-varying piecewise continuous reference r1:N, the initial value problem

{ν˙i(τ)=ρ(νNi(τ),ri),νi(0)=vi,(22)

satisfies the following.

1. ||ρ(νNi,ri)|| is finite for all possible (νNi,ri);

2. h(x¯vi,vi)δh(x¯vi(τ),νi(τ))δ,τ0;

3. γ1(x¯vi,x¯vk)δγ1(x¯vi(τ),x¯vk(τ))δ,τ0;

4. For any constant reference r1:N, there exists a non-empty set of initial conditions V such that v1:NV, then

h(x¯r1:N,r1:N)δγ1(x¯r1:N)δlimτν1:N(τ)=r1:N.

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 v1:N to the target references r1: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 Δ(xi,vi) be such that the solution of the initial value problem

{ξ˙i(τ)=f(ξi(τ),vi),ξi(0)=xi,(23)

satisfies the following.

1. Δ(xi,vi)>0h(ξi(τ),vi)>0,τ0;

2. Δ(xi,vi)>0γ2(ξi(τ),vi)>0,τ0;

3. Δ(xi,vi)0h(ξi(τ),vi)0,τ0;

4. Δ(xi,vi)0γ2(ξi(τ),vi)0,τ0;

5. Δ(xi,vi)=0Δ(ξi(τ),vi)0,τ0;

6. δ>0, ε>0 such that

h(x¯vi,vi)δγ1(x¯vi,x¯vki)δΔ(x¯vi,vi)ε.

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(xi,vi) such that, if the applied reference vi remains constant, the closed-loop equilibrium point x¯vi is asymptotically stable. Let each agent be subject to a set of agent-independent constraints h(xi,vi)0 and a set of multi-agent coordination constraints g(xi,xk)0 with ik. Moreover, let the auxiliary constraints γ1(x¯vi,x¯vk)δ and γ2(xi,vi)0 be defined so that

γ1(x¯vi,x¯vk)δγ2(xi,vi)0γ1(x¯vk,x¯vi)δγ2(xk,vk)0}g(xi,xk)0.(24)

Given the navigation field ρ(vNi,ri) and the dynamic safety margin Δ(xi,vi), let the initial conditions x1:N(0), v1:N(0) be such that Δ(x1:N(0),v1:N(0))0. Then, the D-ERG formulation (21) ensures constraint satisfaction, i.e.

h(x1:N(t),v1:N(t))0,t0;

g(xi(t),xk(t))0,t0,i{1:N},ki,

for any piecewise continuous reference r1:N(t).

Moreover, given a constant aggregate reference r1:N satisfying h(x¯ri,ri)δ and g(x¯ri,x¯rk)δ, with i{1:N},ki, the D-ERG formulation (21) also ensures convergence, i.e.

limtx1:N(t)=x¯r1:N,

as long as v1:N(t)V,t0.

Proof: As detailed in the proof of (Nicotra and Garone, 2018), Theorem 1, it can be shown that (21) ensures Δ(xi(t),vi(t))0,t0. As a result, it follows by definition of the DSM that h(x1:N(t),v1:N(t))0 and γ2(x1:N(t),v1:N(t))0, t0. Moreover, it follows by definition of the NF that, for any piecewise continuous and non-negative signal Δ(t), the solution to v˙i=Δ(t)ρ(vNi,ri) satisfies γ1(x¯vi(t),x¯vk(t))δ, t0,i{1:N}, ki. As a result it follows from (24) that g(xi(t),xk(t))0, t0, i{1:N}, ki. Finally, the convergence result limtx1:N(t)=x¯r1:N follows from the property Δ(x¯v1:N,v1:N)ε, as detailed in the proof of (Nicotra and Garone, 2018, Theorem 1).▪

It is worth noting that, if V is equal to the entire set of steady-state admissible constraints, Theorem 1 implies convergence v1: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 multi-agent 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.

FIGURE 6
www.frontiersin.org

FIGURE 6. Geometric 2D representation of distributed collision avoidance between two pre-stabilized agents i(left) and k(right) with safety radii Ra (dark gray disks), drawn from the perspective of agent i. The current position of each agent is pi(t), pk(t), whereas their current reference is piv(t),pkv(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 piwc, pkwc. 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 pkv(t). If agent i only knows the position of agent k (case B), it must use pk(t) to compute the worst-case current reference pkv,wc(t) and must then compute the worst-case future position based on pkv,wc(t).

TABLE 1
www.frontiersin.org

TABLE 1. Computational Requirements of the D-ERG Algorithm Type and amount of operations to be executed on-board an agent having in its one-hop local neighborhood Nw walls, No static obstacles, and Na1 dynamic agents. The required computations are basic arithmetic scalar, vector and matrix operations, scalar and vector min/max operations that scale approximately linear with the number of state constraints. Note that there is no iterative solver or matrix inversion required.

7.2 Navigation Field

As detailed in (Nicotra and Garone, 2018), the NF of agent i can be designed using a traditional attraction and repulsion field1

ρ(vNi,ri)=ρiatt+ρirep,(25)
ALGORITHM 1
www.frontiersin.org

Pseudocode of the Distributed Explicit Reference Governor (D-ERG) Algorithm for Agent i.

where the attraction field is

ρiatt=[l(pirpiv,η)T,l(ψirψiv,ηψ)]T,(26)

η,ηψ>0 are small smoothing radii chosen to avoid numerical problems when rivi0, and

l(x,η)=xmax(x,η).(27)

The repulsion field is the sum of linear repulsion fields pushing away from walls (w), obstacles (o), and nearby agents (a), i.e.

ρirep=ρiw+ρio+ρia.(28)

The repulsion field of all wall constraints is

ρiw=j=1Nwmax(ζw(dwjRacwjTpiv)ζwδw,0)[c^wj0],(29)

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

ρio,co=j=1Nomax(ζojCj(piv)ζojδoj,0)[(ojpiv)xy0]^,(30)

with an influence margin ζoj>0, a static safety margin δoj(0,ζoj) and Cj(piv)=pivojxy(Roj+Ra). For spherical constraints, one can just use the full Euclidean norm and not project (ojpiv) 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

ρio=ρio,co+ρio,nco,(31)

where

ρio, nco={αojj=1No[oj(2)piv(2)oj(1)+piv(1)00]^ifζojCj(piv),04×1ifζoj<Cj(piv)(32)

with circulation gain αoj>0. For the case of a sphere, the term within brackets can be replaced by

[oj(2)+piv(2)+oj(3)piv(3)oj(1)piv(1)oj(3)+piv(3)oj(1)+piv(1)+oj(2)piv(2)0]^.(33)

In a similar way, one can define the repulsion field that acts on agent i caused by the other agents k as

ρia=ρia,co+ρia,nco,(34)

where

ρia,co=k=1kiNamax(ζaCik(pikv)ζaδa,0)[p^ikv0],(35)

with Cik(pikv)=pikvxy2Ra2Sa, and

ρia,nco={αak=1kiNa[pikv(2)pikv(1)00]^ifζaCik(pikv),04×1ifζa<Cik(pikv),(36)

with ζa>0, δa(0,ζa), Cik(pikv)=pikvxy2Ra2Sa, and αa>0. This is sufficient to ensure the auxiliary constraint

γ1(pikv):pikvxy2Sa2Raδa.(37)

Following from Theorem 1, (24), agent collision can now be avoided by introducing the auxiliary constraint

γ2(pi,piv):Sapivpi0.(38)

As shown in Figure 6, the combination of (37) and (38) satisfies (10).

Remark 1. Equations (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 ||pikv||xyζa+2Ra+2Sa. As a result, it is assumed that agents only share their reference with other agents within an inter-agent distance of ζa+2Ra+4Sa. 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 worst-case references of the neighbors that would still ensure that (37) and (38) imply (10). This leads to two possible options

pikv={pkvpiviknowspkvpkv,WCpiv=pkSapkpivpkpivpiviknowspk,

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.

7.3 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) constraints2,

Δi=max(min(Δis,Δiw,Δio,Δia),0)0.(39)

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 x˙=f(x,v) with state vector x, applied reference v, equilibrium point x¯v, let

V(x,v)=(xx¯v)TP(xx¯v),withP>0,(40)

be a Lyapunov function and let

cTxd(v)(41)

be a linear constraint. Then, the Lyapunov treshold value

Γ(v)=(cTx¯v+d(v))2cTP1c,(42)

is such that V(x,v)Γ(v)(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.

7.3.1 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.

FIGURE 7
www.frontiersin.org

FIGURE 7. Phase plane representation of the proposed input constraint enforcement strategies, illustrated for a second-order dynamical system mp¨=Tmg subject to the pre-stabilizing control law T=m(kP(pvp)kDp˙+g) with pv=0 and the input constraint TTmax. 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, is already very well aligned under the traditional strategy. Hence, performance benefits from optimal alignment are marginal.

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,

TminT=mKP(pvp)KDp˙+ge^3Tmax.(43)

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 mge^3 and the dynamic feedback m(KP(pvp)KDp˙). For the upper limit of the thrust constraint, this can be done by using the triangular inequality, and we obtain TmKP(pvp)KDp˙+mg. Hence,

ifmKP(pvp)KDp˙+mgTmaxTTmax,(44)

it is therefore sufficient to ensure that, e3:

[KPe^TKDe^T][pvpp˙]Tmaxmgm.(45)

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 form (41) with c=[caT,cbT]T by choosing ca=KPe^, cb=KDe^, and d(pv)=Tmaxmg/mKPe^Tpv. Assuming unidirectional gains KP=kPI3 and KD=kDI3, the associated threshold value (42) is,

ΓTmax=12(Tmaxmg)2m2kP+ε(1ε)kD2kP2+kD2(kP+εkD22εkP).(46)

Similarly, ΓTmin can be computed by replacing Tmax in (46) with Tmin. The DSM that prevents the total thrust to saturate is

Δs=κs(min(ΓTmax,ΓTmin)V(p,p˙,pv)),(47)

with κs>0.

Optimally Aligned Lyapunov Level Set Strategy (Opt Lyap): This section is an extension of the theory in (Garone et al., 2018) 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

VT(p,p˙,pv)=[ppvp˙]TPT[ppvp˙],(48)

with PT>0 that satisfies the Lyapunov equation A(R˜)TPT+PTA(R˜)0 and A(R˜) defined in (6.2). By taking advantage of the rotational symmetry of the system and defining

PT=[P^T,11I3P^T,12I3P^T,21I3P^T,22I3],(49)

the optimal Lyapunov function can be obtained by solving the following linear matrix inequality

{minlogdet(P^T)subject to:A(0)TP^T+P^TA(0)0A(Δα)TP^T+P^TA(Δα)0P^TcTcTT,(50)

where α˜ and Δα are the current and the maximum allowed rotational error between z^ and z^d, cT=m[kP,kD]T and

A(α˜)=[01kPcos(α˜)kDcos(α˜)].(51)

Given the quadratic Lyapunov function (48), we obtain the threshold values

ΓTmax=(Tmaxmg)2cTTP^T1cT,ΓTmin=(Tminmg)2cTTP^T1cT.(52)

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

Δs=κs(min(ΓTmax,ΓTmin)VT(p,p˙,pv)).(53)

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

{ΓTmax/min=minp,p˙,pvVT(p,p˙,pv)subjectto:Td(p,p˙,pv)=T(p,p˙,pv)=Tmax/minf(p,p˙,pv)TT(p,p˙,pv)0/0,(54)

with the closed position loop dynamics f(p,p˙,pv) and the total thrust gradient T(p,p˙,pv). 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 TTmax or TTmin. The invariant set based DSM can be computed as,

Δs=κsmin(min(ΓTmax,ΓTmin)VT(p,p˙,pv)(ΓTmax+ΓTmin)/2,minj{1,2,3,4}(UmaxUj(UmaxUmin)/2,UjUmin(UmaxUmin)/2)).(55)

Remark 2. To avoid motor saturation when tracking a non-zero 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,ψminj{1,2,3,4}(UmaxUj(UmaxUmin)/2,UjUmin(UmaxUmin)/2),(56)

with κs,ψ>0.

7.3.2 Wall Constraints

The convex inequality constraints (8) are equivalent to (41) with c=[cwjT,03×1T]T, and d(pv)=dwjRa. As a result, the threshold value associated to the j-th wall constraint is

Γwj=12(kP+ε(1ε)kD2)(c^wTjpvdwj+Ra)2.(57)

The dynamic safety margin corresponding to the wall constraint closest to violation then becomes,

Δw=κw(minj{1,,Nw}(Γwj)V(p,p˙,pv)),(58)

with κw>0.

7.3.3 Obstacle Constraints

Constraint (9) defines a non-convex admissible region. Given a fixed reference pv, it can be shown using triangular inequalities that

pojppvpvojRoj+Ra.(59)

As a result, (9) can be enforced by simply ensuring

(pvoj)^T(pvp)Roj+Ra+pvoj0.(60)

The inequality constraints define a reference-dependent virtual wall and are equivalent to (41) with c=[(pvoj)^T,03×1T]T, and d(pv)=(pvoj)^TpvRojRapvoj. The DSM related to this constraint then becomes,

Δo=κo(minj{1,,No}(Γoj)V(p,p˙,pv)).(61)

with κo>0.

7.3.4 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

[e^T03×1T][pvpp˙]Sa,e^3:e^=1,(62)

thus leading to

Γa=12(kP+ε(1ε)kD2)Sa2.(63)

The DSM related to this constraint then becomes,

Δa=κa(ΓaV(p,p˙,pv)),(64)

with κa>0.

8 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 our methodology. A summary of these results can be found in Section 9.

8.1 Experimental Setup

The experiments are performed using Crazyflie 2.1 nano-quadrotors 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 500Hz 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 100Hz. 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 Ra=0.08m and a mass of approximately 34.6g. Its inertia matrix J=diag(17.31,17.94,33.75)106kgm2 is calculated from a CAD model and is only used to estimate the actuator torque constant. The estimated actuator thrust and torque constants amount KT=0.012N/V2 and Kτ=6.84106Nm/V2, respectively. The nominal distance between the motor axis and the center of mass of the aircraft amounts d=4.65cm.

8.2 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.

First tune the inner loop gains KR, Kω > 0 and then the outer loop gains KP, KD > 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 pre-stabilized 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.

Eliminate numerical noise in the attraction field by selecting a strictly positive, but small value for the smoothing radius η.

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.

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.

For cooperative agent collision avoidance, choose the maximum position error radius Sa. 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.

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.

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 KP = 13.0 I3, KD = 5.0 I3, KR = 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.

8.3 Single Aerial Robot Experiments

8.3.1 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 pv 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 pr 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 Umin=0.0V, Umax=3.5V or Tmin=0.0N, Tmax=0.59N by using either:

Trad Lyap: traditional Lyapunov-based DSM (47), with κs=2.5 and ε=0.5;

Opt Lyap: optimally aligned Lyapunov-based DSM (53), with κs=9.45, Δα=0.349, P^T,11=0.8810, P^T,12=P^T,21=0.3202, P^T,22=0.1511, ΓT=min(ΓTmax,ΓTmin)=0.266;

Opt Inv: invariance-based DSM (55), with κs=1.80, ΓT=3.00.

As is depicted in Figure 8, in each of these experiments, the UAV starts from the initial hovering position p(t0.5s)=[4.0,1.0,0.25]Tm, i.e. p˙(t0.5s)=0m/s. At t=0.5s and at t=12.5s it is asked to transition between the points pr(0.5st<12.5s)=[0.50,1.0,2.50]Tm and pr(t12.5s)=[4.0,1.0,1.25]Tm.

FIGURE 8
www.frontiersin.org

FIGURE 8. Point-to-Point Transitions Without Violation of Input Constraints The three strategies for computing DSMs give provably stable and safe performance. The traditional Lyapunov strategy is the most conservative one, whereas the invariance based strategy outperforms the other two in terms of settling time and peak velocity.

The desired position set-point is always reached in a stable and safe (i.e. DSM0) 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.4m/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(t1.0s)=[4.0,1.0,0.25]Tm while hovering. At t=1.0s and at t=6.0s 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 ψ).

FIGURE 9
www.frontiersin.org

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.

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.0s but becomes unstable for the commands given at t=6.0s. 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.

8.3.2 Point-to-Point Transitions Wall Avoidance

The results depicted in Figure 10 show the aerial vehicle avoiding two virtual walls with cw1=[1,0,0]Tm, dw1=4.8m, and cw2=[0,1,0]Tm, dw2=2.0m, when using an ERG with an invariance based DSM for the input constraints and a Lyapunov based DSM for the convex wall constraints with κw=1.5, ζw=1.0m, and δw=0.01 m. The UAV is initially hovering at [4.0,1.0,0.25]Tm and is commanded consecutively to the positions [1.5,2.5,1.5]Tm, [5.5,2.5,1.5]Tm, and [4.0,1.0,1.0]Tm. From the logged data one can see that the quadrotor initially speeds up to a maximum speed of 2.0m/s, and slows down such that overshoots do not cause collisions with the virtual walls. 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.

FIGURE 10
www.frontiersin.org

FIGURE 10. Point-to-Point Transitions with Wall Avoidance The quadrotor achieves top speeds of 2m/s and slows down as to avoid wall collisions, even when the position references are steady-state inadmissible.

8.4 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.

8.4.1 Provably Safe Human-Swarm Teleoperation

In this experiment we show that the D-ERG ensures a swarm of Na=4 quadrotors can be teleoperated by a human in a provably safe way within a confined environment composed of wall constraints with cw1=[1,0,0]Tm, dw1=3m, cw2=[1,0,0]Tm, dw2=4.8m, cw3=[0,1,0]Tm, dw3=2.0m, and cw4=[0,1,0]Tm, dw4=1.5m. We use the same ERG parameters as in Section 8.3.2, and for the collision avoidance between agents, we exchange pv between the agents and use Sa=0.80m, αa=0.0, ζa=1.50m, δa=0.01m, 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 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.0s, there are short periods where the actuator inputs come very close to their upper and lower limits and Δis is very close to zero, such that the applied reference is kept almost constant.

FIGURE 11
www.frontiersin.org

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.0s), 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.0s vary in a range of 2.1V to 3.0V. 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.

8.4.2 Point-to-Point Transitions Agent Collision Avoidance

In Figure 12 the results of two experiments with a swarm of Na=5 agents are depicted. Every agent is commanded to transition between specific goal positions at t=1.0s and at t=26.0s, 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 cw1=[1,0,0]Tm, dw1=4.8m, cw2=[1,0,0]Tm, dw2=4.8m, cw3=[0,1,0]Tm, dw3=2.0m, and cw4=[0,1,0]Tm, dw4=1.5m. The navigation layer consists of a D-ERG using the parameters as in Section 8.4.1, but with Sa=0.55m, δa=0.1m, αa=0.1. The same navigation task is performed twice, first by sharing p and then by sharing pv locally between the agents, as detailed in Remark 1. The results clearly show the D-ERG ensures every agent asymptotically reaches its desired position while avoiding collisions with other agents and the small circulation gain ensures the agents to not get stuck in local saddle points. Comparing the two cases one can see that sharing pv reduces the worst-case settling time over all agents for transitioning between formations by a factor of two. This is because the swarm remains more dense and the agents have to travel less distance. A potential drawback of the latter is that this explicitly requires communication between the agents, whereas sharing p could be communication-free (i.e. decentralized) if the agents would be equipped with sensors to measure inter-agent position vectors.

FIGURE 12
www.frontiersin.org

FIGURE 12. 2D Point-to-Point Transitions with Agent Collision and Deadlock Avoidance Exchanging the applied reference position piv(t) over the actual position pi(t) leads to a denser swarm, less distance traveled, and smaller settling times.

Similar to the 2D line formation experiments, Figure 13 depicts the results of formation transitioning experiments in 3D with a swarm of Na=9 agents. The actual applied reference positions piv between the agents are exchanged with Sa=0.25m, δa=0.01m, and αa=0.2. Every agent is commanded to some set-point goal positions at t=3.0s, at t=28.0s, at t=53.0s, at t=71.0s, and at t=89.0s that must be reached without causing any undesirable agent interactions such as collisions or deadlocks. Moreover the agents stay inside a confined environment bounded by the same four wall constraints. Note that since in this experiment Sa is smaller than in Figure 12, this leads here to smaller peak velocities, but a more dense swarm (agents coming as close as 15cm), since the agents have to travel less distance to avoid each other.

FIGURE 13
www.frontiersin.org

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).

8.5 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-to-point 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 lengths of 16m which is symmetrically centered in the origin. For each simulation we randomly place No static spherical obstacles with Ro=0.8m, ζo=1m, κo=20, and the initial and goal positions of Na quadrotors with ζa=1m, Sa=1.2m, κa=20, κs=6, that exchange pv 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 No obstacles and Na 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.

TABLE 2
www.frontiersin.org

TABLE 2. Simulation Statistics on Safety and Goal Satisfaction A cubic environment is randomly and increasingly densely filled with No spherical obstacles and Na spherical agents doing 3D point to point transitions. The number of simulations that contain at least one instability, collision, or deadlock are denoted by #I, #C, and #D, respectively. Almost global asymptotic stability, with no collisions and no instabilities confirm the strong safety certificates of the D-ERG. This compared with another explicit approach solely based on Navigation Fields (NF).

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.

9 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 (500Hz) 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 low-level control layer is proven to be robust to small attitude errors. Moreover, the D-ERG leverages the robustness of low-level 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).

Algorithm 1In 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 (Wang et al., 2019; Yang et al., 2021a), and unavailable velocity measurements as in (Yang et al., 2021b) due to noisy low-cost sensors.

10 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.

Funding

This work was supported by the Research Foundation Flanders (FWO) under grant numbers 37472, 60523, and 62062, by the Flemish Government under the program “Onderzoeksprogramma Artificiële Intelligentie (AI) Vlaanderen”, and by the US government’s National Science Foundation award CMMI 1904441.

Conflict of Interest

The authors declare that the research was conducted in the absence of any commercial or financial relationships that could be construed as a potential conflict of interest.

Acknowledgments

The authors would like to thank the Research and Engineering Center for Unmanned Vehicles (RECUV) of the University of Colorado Boulder for providing continued access to the motion capture system used for the experimental validation, even in the midst of moving to a new campus.

Footnotes

1dependency of ρ on (vNi,ri) is omitted for simplicity of notation.

2dependency of Δ on (xi,vi) is omitted for simplicity of notation.

References

Alonso-Mora, J., Naegeli, T., Siegwart, R., and Beardsley, P. (2015). Collision Avoidance for Aerial Vehicles in Multi-Agent Scenarios. Autonomous Robots 39, 101–121. doi:10.1007/s10514-015-9429-0

CrossRef Full Text | Google Scholar

Bajcsy, A., Herbert, S. L., Fridovich-Keil, D., Fisac, J. F., Deglurkar, S., Dragan, A. D., et al. (2019). A Scalable Framework for Real-Time Multi-Robot, Multi-Human Collision Avoidance. In International Conference on Robotics and Automation (IEEE), 936–943. doi:10.1109/ICRA.2019.8794457

Google Scholar

Bayram, H., Stefas, N., Engin, K. S., and Isler, V. (2017). Tracking Wildlife with Multiple UAVs: System Design, Safety and Field Experiments. in Proceedings of the IEEE International Symposium on Multi-Robot and Multi-Agent Systems. Los Angeles, CA, December 4–5, 2017, 97–103. doi:10.1109/MRS.2017.8250937

CrossRef Full Text | Google Scholar

Brambilla, M., Ferrante, E., Birattari, M., and Dorigo, M. (2013). Swarm Robotics: a Review from the Swarm Engineering Perspective. Swarm Intelligence 7, 1–41. doi:10.1007/s11721-012-0075-2

CrossRef Full Text | Google Scholar

Brockers, R., Hummenberger, M., Weiss, S., and Matthies, L. (2014). Towards Autonomous Navigation of Miniature UAV. in Proceedings of the IEEE Computer Society Conference on Computer Vision and Pattern Recognition Workshops. Washington, DC: IEEE Computer Society, 645–651. doi:10.1109/CVPRW.2014.98

CrossRef Full Text | Google Scholar

Campos-Macías, L., Gómez-Gutiérrez, D., Aldana-López, R., De La Guardia, R., and Parra-Vilchis, J. I. (2017). A Hybrid Method for Online Trajectory Planning of Mobile Robots in Cluttered Environments. IEEE Robotics Automation Lett. 2, 935–942. doi:10.1109/LRA.2017.2655145

CrossRef Full Text | Google Scholar

Cappo, E. A., Desai, A., Collins, M., and Michael, N. (2018a). Online Planning for Human-Multi-Robot Interactive Theatrical Performance. Autonomous Robots 42, 1771–1786. doi:10.1007/s10514-018-9755-0

CrossRef Full Text | Google Scholar

Cappo, E. A., Desai, A., and Michael, N. (2018b). Robust Coordinated Aerial Deployments for Theatrical Applications Given Online User Interaction via Behavior Composition. in Distributed Autonomous Robotic Systems. 6 edn. Cham, Switzerland: Springer, 665–678. doi:10.1007/978-3-319-73008-0_46

CrossRef Full Text | Google Scholar

Carrio, A., Tordesillas, J., Vemprala, S., Saripalli, S., Campoy, P., and How, J. P. (2020). Onboard Detection and Localization of Drones Using Depth Maps. IEEE Access 8, 30480–30490. doi:10.1109/access.2020.2971938

CrossRef Full Text | Google Scholar

Chamanbaz, M., Mateo, D., Zoss, B. M., Tokić, G., Wilhelm, E., Bouffanais, R., et al. (2017). Swarm-Enabling Technology for Multi-Robot Systems. Front. Robotics AI 4, 12. doi:10.3389/frobt.2017.00012

CrossRef Full Text | Google Scholar

Chen, M., Hu, Q., Fisac, J. F., Akametalu, K., Mackin, C., and Tomlin, C. J. (2017). Reachability-based Safety and Goal Satisfaction of Unmanned Aerial Platoons on Air Highways. J. Guidance, Control Dyn. 40, 1360–1373. doi:10.2514/1.G000774

CrossRef Full Text | Google Scholar

Chung, S.-J., Paranjape, A., Dames, P., Shen, S., and Kumar, V. (2018). A Survey on Aerial Swarm Robotics. IEEE Trans. Robotics 34, 837–855. doi:10.1109/TRO.2018.2857475

CrossRef Full Text | Google Scholar

Coppola, M., McGuire, K. N., De Wagter, C., and de Croon, G. C. (2020). A Survey on Swarming With Micro Air Vehicles: Fundamental Challenges and Constraints. Front. Robotics AI 7, 18. doi:10.3389/frobt.2020.00018

CrossRef Full Text | Google Scholar

Desaraju, V. R., and Michael, N. (2018). Efficient Prioritization in Explicit Adaptive NMPC through Reachable-Space Search. in AIAA Guidance, Navigation, and Control Conference, Kissimmee, FL, January 8–12, 2018, 1–19. doi:10.2514/6.2018-1847

CrossRef Full Text | Google Scholar

Desaraju, V. R., Spitzer, A. E., O’Meadhra, C., Lieu, L., and Michael, N. (2018). Leveraging Experience for Robust, Adaptive Nonlinear MPC on Computationally Constrained Systems with Time-Varying State Uncertainty. Int. J. Robotics Res. 37, 1690–1712. doi:10.1177/0278364918793717

CrossRef Full Text | Google Scholar

Ding, W., Gao, W., Wang, K., and Shen, S. (2019). An Efficient B-Spline-Based Kinodynamic Replanning Framework for Quadrotors. IEEE Trans. Robotics 35, 1287–1306. doi:10.1109/TRO.2019.2926390

CrossRef Full Text | Google Scholar

Du, X., Luis, C. E., Vukosavljev, M., and Schoellig, A. P. (2019). Fast and in Sync: Periodic Swarm Patterns for Quadrotors. in Proceedings of the 2019 International Conference on Robotics and Automation (ICRA), Montreal, QC, Canada, May 20–24, 2019, 9143–9149. doi:10.1109/ICRA.2019.8794017

CrossRef Full Text | Google Scholar

Fathian, K., Safaoui, S., Summers, T. H., and Gans, N. R. (2019). Robust 3D Distributed Formation Control with Collision Avoidance and Application to Multirotor Aerial Vehicles. in Proceedings of the 2019 International Conference on Robotics and Automation (ICRA), Montreal, QC, Canada, May 20–24, 2019, 9209–9215. doi:10.1109/ICRA.2019.8794349

CrossRef Full Text | Google Scholar

Fathian, K., Summers, T. H., and Gans, N. R. (2018). Robust Distributed Formation Control of Agents with Higher-Order Dynamics. IEEE Control. Syst. Lett. 2, 495–500. doi:10.1109/LCSYS.2018.2841941

CrossRef Full Text | Google Scholar

Franchi, A., Secchi, C., Son, H. I., Bülthoff, H. H., and Giordano, P. R. (2012). Bilateral Teleoperation of Groups of Mobile Robots with Time-Varying Topology. IEEE Trans. Robotics 28, 1019–1033. doi:10.1109/TRO.2012.2196304

CrossRef Full Text | Google Scholar

Franchi, A., Stegagno, P., and Oriolo, G. (2016). Decentralized Multi-Robot Encirclement of a 3D Target with Guaranteed Collision Avoidance. Autonomous Robots 40, 245–265. doi:10.1007/s10514-015-9450-3

CrossRef Full Text | Google Scholar

Fridovich-Keil, D., Herbert, S. L., Fisac, J. F., Deglurkar, S., and Tomlin, C. J. (2018). Planning, Fast and Slow: A Framework for Adaptive Real-Time Safe Trajectory Planning. IEEE Int. Conf. Robotics Automation, 387–394. doi:10.1109/ICRA.2018.8460863

CrossRef Full Text | Google Scholar

Garone, E., and Nicotra, M. M. (2016). Explicit Reference Governor for Constrained Nonlinear Systems. IEEE Trans. Automatic Control. 61, 1379–1384. doi:10.1109/TAC.2015.2476195

CrossRef Full Text | Google Scholar

Garone, E., Nicotra, M., and Ntogramatzidis, L. (2018). Explicit Reference Governor for Linear Systems. Int. J. Control. 91, 1415–1430. doi:10.1080/00207179.2017.1317832

CrossRef Full Text | Google Scholar

Herbert, S. L., Chen, M., Han, S., Bansal, S., Fisac, J. F., and Tomlin, C. J. (2017). FaSTrack: a Modular Framework for Fast and Guaranteed Safe Motion Planning. in Proceedings of the IEEE Conference on Decision and Control, Melbourne, VIC, Australia, December 12–15, 2017. 1517. doi:10.1109/CDC.2017.8263867

Google Scholar

Honig, W., Preiss, J. A., Kumar, T. K., Sukhatme, G. S., and Ayanian, N. (2018). Trajectory Planning for Quadrotor Swarms. IEEE Trans. Robotics 34, 856–869. doi:10.1109/TRO.2018.2853613

CrossRef Full Text | Google Scholar

Hua, M. D., Hamel, T., Morin, P., and Samson, C. (2013). Introduction to Feedback Control of Underactuated VTOL Vehicles. IEEE Control. Syst. Mag. 33, 61–75. doi:10.1109/MCS.2012.2225931

CrossRef Full Text | Google Scholar

Jung, S., Hwang, S., Shin, H., and Shim, D. H. (2018). Perception, Guidance, and Navigation for Indoor Autonomous Drone Racing Using Deep Learning. IEEE Robotics Automation Lett. 3, 2539–2544. doi:10.1109/LRA.2018.2808368

CrossRef Full Text | Google Scholar

Khalil, H. K. (2001). Nonlinear Systems. 3rd edn. Hobooken, NJ: Prentice-Hall.

CrossRef Full Text

Koditschek, D. E., and Rimon, E. (1990). Robot Navigation Functions on Manifolds with Boundary. Adv. Appl. Math. 11, 412–442. doi:10.1016/0196-8858(90)90017-S

CrossRef Full Text | Google Scholar

Kolaric, P., Chen, C., Dalal, A., and Lewis, F. L. (2018). Consensus Controller for Multi-UAV Navigation. Control. Theor. Technol. 16, 110–121. doi:10.1007/s11768-018-8013-5

CrossRef Full Text | Google Scholar

Lee, T. (2011). Geometric Tracking Control of the Attitude Dynamics of a Rigid Body on SO(3). IEEE Am. Control. Conf. 3, 1200–1205. doi:10.1109/ACC.2011.5990858

Google Scholar

Liu, H., Ma, T., Lewis, F. L., and Wan, Y. (2019). Robust Formation Trajectory Tracking Control for Multiple Quadrotors With Communication Delays. IEEE Trans. Control. Syst. Technol. 26 (6), 1–8. doi:10.1109/tcst.2019.2942277

CrossRef Full Text | Google Scholar

Liu, Y., Montenbruck, J. M., Zelazo, D., Odelga, M., Rajappa, S., Bulthoff, H. H., et al. (2018). A Distributed Control Approach to Formation Balancing and Maneuvering of Multiple Multirotor UAVs. IEEE Trans. Robotics 34, 870–882. doi:10.1109/TRO.2018.2853606

CrossRef Full Text | Google Scholar

Luis, C. E., and Schoellig, A. P. (2019). Trajectory Generation for Multiagent Point-To-Point Transitions via Distributed Model Predictive Control. IEEE Robotics Automation Lett. 4, 375–382. doi:10.1109/lra.2018.2890572

CrossRef Full Text | Google Scholar

Marconi, L., Melchiorri, C., Beetz, M., Pangercic, D., Siegwart, R., Leutenegger, S., et al. (2012). The SHERPA Project: Smart Collaboration between Humans and Ground-Aerial Robots for Improving Rescuing Activities in Alpine Environments (SSRR). in IEEE International Symposium on Safety, Security, and Rescue Robotics. College Station, TX, November 5–8, 2012, doi:10.1109/SSRR.2012.6523905

CrossRef Full Text | Google Scholar

McGuire, K. N., De Wagter, C., Tuyls, K., Kappen, H. J., and de Croon, G. C. H. E. (2019). Minimal Navigation Solution for a Swarm of Tiny Flying Robots to Explore an Unknown Environment. Sci. Robotics 4, 1–14. doi:10.1126/scirobotics.aaw9710

CrossRef Full Text | Google Scholar

Mellinger, D., and Kumar, V. (2011). Minimum Snap Trajectory Generation and Control for Quadrotors. IEEE Int. Conf. Robotics Automation, 2520–2525. doi:10.1109/ICRA.2011.5980409

CrossRef Full Text | Google Scholar

Murray, R. M. (2007). Recent Research in Cooperative Control of Multivehicle Systems. J. Dynamic Syst. Meas. Control Trans. ASME 129, 571–583. doi:10.1115/1.2766721

CrossRef Full Text | Google Scholar

Nicotra, M. M., Bartulovic, M., Garone, E., and Sinopoli, B. (2015). A Distributed Explicit Reference Governor for Constrained Control of Multiple UAVs. IFAC-PapersOnLine 48, 156–161. doi:10.1016/j.ifacol.2015.10.323

CrossRef Full Text | Google Scholar

Nicotra, M. M., and Garone, E. (2018). The Explicit Reference Governor: A General Framework for the Closed-form Control of Constrained Nonlinear Systems. IEEE Control. Syst. Mag. 38, 89–107. doi:10.1109/MCS.2018.2830081

CrossRef Full Text | Google Scholar

Parker, L. E., Rus, D., and Sukhatme, G. S. (2016). “Multiple Mobile Robot Systems,” in Springer Handbook of Robotics. Editors B. Siciliano, and O. Khatib (Cham: Springer), 1335–1379. doi:10.1007/978-3-319-32552-1_53

CrossRef Full Text | Google Scholar

Preiss, J. A., Honig, W., Ayanian, N., and Sukhatme, G. S. (2017a). Downwash-aware Trajectory Planning for Large Quadrotor Teams. IEEE Int. Conf. Intell. Robots Syst., 250–257. doi:10.1109/IROS.2017.8202165

CrossRef Full Text | Google Scholar

Preiss, J. A., Hönig, W., Sukhatme, G. S., and Ayanian, N. (2017b). Crazyswarm: A Large Nano-Quadcopter Swarm. IEEE Int. Conf. Robotics Automation, 3299–3304. doi:10.1109/ICRA.2017.7989376

CrossRef Full Text | Google Scholar

Purohit, A., Zhang, P., Sadler, B. M., and Carpin, S. (2014). Deployment of Swarms of Micro-Aerial Vehicles: From Theory to Practice. IEEE Int. Conf. Robotics Automation, 5408–5413. doi:10.1109/ICRA.2014.6907654

CrossRef Full Text | Google Scholar

Rubies-Royo, V., Fridovich-Keil, D., Herbert, S., and Tomlin, C. J. (2019). “A Classification-Based Approach for Approximate Reachability,” in International Conference on Robotics and Automation. 7697–7704. doi:10.1109/ICRA.2019.8793919

Google Scholar

Sanket, N. J., Singh, C. D., Ganguly, K., Fermuller, C., and Aloimonos, Y. (2018). GapFlyt: Active Vision Based Minimalist Structure-Less Gap Detection for Quadrotor Flight. IEEE Robotics Automation Lett. 3, 2799–2806. doi:10.1109/LRA.2018.2843445

CrossRef Full Text | Google Scholar

Vásárhelyi, G., Virágh, C., Somorjai, G., Nepusz, T., Eiben, A. E., and Vicsek, T. (2018). Optimized Flocking of Autonomous Drones in Confined Environments. Sci. Robotics 3, 1–13. doi:10.1126/scirobotics.aat3536

CrossRef Full Text | Google Scholar

Vukosavljev, M., Kroeze, Z., Schoellig, A. P., and Broucke, M. E. (2019). A Modular Framework for Motion Planning Using Safe-By-Design Motion Primitives. IEEE Trans. Robotics 35, 1233–1252. doi:10.1109/TRO.2019.2923335

CrossRef Full Text | Google Scholar

Wang, L., Ames, A. D., and Egerstedt, M. (2017). Safe Certificate-Based Maneuvers for Teams of Quadrotors Using Differential Flatness. IEEE Int. Conf. Robotics Automation, 3293–3298. doi:10.1109/ICRA.2017.7989375

CrossRef Full Text | Google Scholar

Wang, N., Su, S. F., Han, M., and Chen, W. H. (2019). Backpropagating Constraints-Based Trajectory Tracking Control of a Quadrotor with Constrained Actuator Dynamics and Complex Unknowns. IEEE Trans. Syst. Man Cybernetics: Syst 49, 1322–1337. doi:10.1109/TSMC.2018.2834515

CrossRef Full Text | Google Scholar

Xu, B., and Sreenath, K. (2018). Safe Teleoperation of Dynamic UAVs through Control Barrier Functions. IEEE Int. Conf. Robotics Automation, 7848–7855. doi:10.1109/ICRA.2018.8463194

CrossRef Full Text | Google Scholar

Yang, T., Sun, N., and Fang, Y. (2021a). Adaptive Fuzzy Control for a Class of MIMO Underactuated Systems With Plant Uncertainties and Actuator Deadzones: Design and Experiments. IEEE Trans. Cybern. [Epub ahead of print]. doi:10.1109/TCYB.2021.3050475

CrossRef Full Text | Google Scholar

Yang, T., Sun, N., Fang, Y., Xin, X., and Chen, H. (2021b). New Adaptive Control Methods for n-Link Robot Manipulators With Online Gravity Compensation: Design and Experiments. IEEE Trans. Ind. Electron.. doi:10.1109/TIE.2021.3050371

CrossRef Full Text | Google Scholar

Zhou, D., Wang, Z., and Schwager, M. (2018). Agile Coordination and Assistive Collision Avoidance for Quadrotor Swarms Using Virtual Structures. IEEE Trans. Robotics 34, 916–923. doi:10.1109/TRO.2018.2857477

CrossRef Full Text | Google Scholar

Keywords: aerial robotics control, multi-robot systems, actuator saturation, distributed collision avoidance, guaranteed safety, human-swarm interaction, invariant set control, nano-quadrotor swarm

Citation: Convens B, Merckaert K, Vanderborght B and Nicotra MM (2021) Invariant Set Distributed Explicit Reference Governors for Provably Safe On-Board Control of Nano-Quadrotor Swarms. Front. Robot. AI 8:663809. doi: 10.3389/frobt.2021.663809

Received: 03 February 2021; Accepted: 21 April 2021;
Published: 22 June 2021.

Edited by:

Holger Voos, University of Luxembourg, Luxembourg

Reviewed by:

Ning Sun, Nankai University, China
Navid Razmjooy, Independent researcher, Ghent, Belgium

Copyright © 2021 Convens, Merckaert, Vanderborght and Nicotra. This is an open-access article distributed under the terms of the Creative Commons Attribution License (CC BY). The use, distribution or reproduction in other forums is permitted, provided the original author(s) and the copyright owner(s) are credited and that the original publication in this journal is cited, in accordance with accepted academic practice. No use, distribution or reproduction is permitted which does not comply with these terms.

*Correspondence: Bryan Convens, bryan.convens@vub.be

Disclaimer: All claims expressed in this article are solely those of the authors and do not necessarily represent those of their affiliated organizations, or those of the publisher, the editors and the reviewers. Any product that may be evaluated in this article or claim that may be made by its manufacturer is not guaranteed or endorsed by the publisher.