Safe Certificate-Based Maneuvers for Teams of Quadrotors Using Differential Flatness *  Li Wang, Aaron D. Ames, and Magnus Egerstedt †  Abstract —   Safety Barrier Certificates   that ensure collision-free maneuvers for teams of differential flatness-based quadrotors are presented in this paper. Synthesized with control barrier functions,   the   certificates   are   used   to   modify   the   nominal trajectory   in   a   minimally   invasive   way   to   avoid   collisions. The proposed collision avoidance strategy complements existing flight control and planning algorithms by providing trajectory modifications with provable safety guarantees. The effectiveness of this strategy is supported both by the theoretical results and experimental validation on a team of five quadrotors.  I. INTRODUCTION Due to recent advances in the design, control, and sensing technology, teams of quadrotors have become widely used in aerial robotic platforms, e.g., [7], [12]. Their ability to hover and fly agilely in three dimensional space makes quadrotors effective tools for surveillance, delivery, precision agriculture, search and rescue tasks, see e.g., [18], [24]. When teams of quadrotors are deployed to collaboratively fulfil these higher level tasks, it is crucial to make sure that they do not collide with each other. The focus of this paper is to rectify the nominal flight trajectory, which is generated with exisiting control and planning algorithms for teams of quadrotors, in a minimally invasive way to avoid collisions. Due to the under-actuated and intrinsically unstable nature of quadrotors, it is often challenging to generate safe trajec- tories for arbitrary tasks. An artificial potential field approach was used in [10] to avoid inter-quadrotor collisions, which can only be qualified to work for the linearized quadrotor model, i.e., near the hovering state. Additionally, real-time trajectory generation approach, utilizing the nonlinear dy- namics together with time-optimal planning algorithm, was proposed in [7]. However, it is computationally expensive to accommodate collision avoidance constraints when solving optimal control problems in real time. One remedy to this problem is to exploit the differential flatness property of quadrotors,   as   introduced   in   [12],   [25],   to   simplify   the trajectory planning process, while still leveraging the non- linear dynamics of the quadrotors. This property has been successfully used for flight trajectory planning in cluttered environments [11], as well as avoiding static and moving obstacles [13].  *The work by the first and third authors was sponsored by Grant No. 1544332 from the U.S. National Science Foundation, and the work of the second author was sponsored by Grant No. 1239055 from the U.S. National Science Foundation.  † Li Wang and Magnus Egerstedt are with the School of Electrical and Computer Engineering, Aaron D. Ames is with the School of Mechanical Engineering and the School of Electrical and Computer Engineering, Geor- gia Institute of Technology, Atlanta, GA 30332, USA. Email:   { liwang, magnus,   ames } @gatech.edu Nominal Trajectory Safe Trajectory  Fig. 1: Safe certificate-based flight maneuvers. The nominal trajectories generated with existing planning algorithms are modified by the safety barrier certificates to avoid collisions. The safe regions of quadrotors are modelled as rectangles to avoid both collisions and air flow disturbances. In contrast to the aforementioned methods, the goal of this paper is to modify the trajectories in a provably safe manner that is compatible with existing control and planning techniques, while exploiting the nonlinear dynamics (allow- ing significant deviation from hovering state and large Euler angles) of teams of quadrotors. To achieve this objective, all collision-free states of the quadrotors are encoded in a safe set. Then,   Safety Barrier Certificates   are synthesized based on the differential flatness property, and a class of non-conservative control barrier functions [2], [23], [15] are used to ensure the forward invariance of the safe set. Control barrier functions were used in [21], [22] to avoid static/moving obstacles for a single planar or 3D quadrotor. And   Safety Barrier Certificates   have been applied to teams of ground mobile robots as well for collision avoidance [19], [20]. As such, in this paper, the certificates are extended to more complicated multi-quadrotor systems. The main contributions of this paper are threefold: 1)  Safety Barrier Certificates   are developed that provably en- sure the safety of differential flatness based teams of quadro- tors;   2)   a   strategy   is   developed   to   modify   the   nominal trajectory in a minimally invasive way to avoid collisions, which is compatible with existing flight control and planning algorithms; 3) the feasibility of the proposed method is demonstrated through experimental implementation of the  arXiv:1702.01075v1 [cs.RO] 3 Feb 2017
Safety Barrier Certificates   on a team of five palm-sized quadrotors. The rest of the paper is organized as follows: the differen- tial flatness of quadrotor dynamics and the exponential con- trol barrier functions are briefly revisited in Sections II and III. The methods to synthesize the   Safety Barrier Certificates  and minimally-invasively modify nominal trajectories are presented in Section IV. In Section V, the proof of feasibility along with a virtual vehicle parameterization method to deal with actuator limits are provided. The experimental work is the topic of Section VI, and the conclusions are given in Section VII. II. D IFFERENTIAL   F LATNESS OF   Q UADROTOR   D YNAMICS  The quadrotor is a well-modelled dynamical system with forces and torques generated by four propellers and gravity.  Z   − Y   − X   Euler angles conventions are used to define the roll ( φ   ), pitch ( θ   ), and yaw ( ψ ) angles between the quadrotor body frame and the world coordinate frame. The relevant coordinate frames and Euler angles are illustrated in Fig. 2.  Fig. 2: Quadrotor coordinate frames. The subscripts   w   de- notes the world frame   F w ,   b   for the quadrotor body frame   F b , and   c   for an intermediate frame   F c   after yaw angle rotation.  ω 1   to   ω 4   are the angular velocities of the four propellers. The palm-sized quadrotor illustrated is a Crazyflie 2.0 [1] used in the experiment section. Assuming   that   the   damping   and   drag-like   effects   are negligible [7], the dynamics of a quadrotor is governed by the Newton-Euler equation,  m  ̈ r   =   mgz w   +   f z z b ,  J    ̇ ω b   =   τ   −   ω b   ×   J ω b ,  where   r   = [ x ,   y ,   z ] T   is the position of the center of mass in the world frame   F w ,   ω b   = [ p ,   q ,   r ] T   is the angular velocity in the body frame   F b , and   m   and   J   are the mass and inertia matrix of the quadrotor respectively.   f z   is the total thrust and  τ   = [ τ x ,   τ y ,   τ z ]   is the torque generated by the motors.   z w   and  z b   are the unit vectors in the   Z -direction in the world and body frames, respectively. The quadrotor dynamics has been shown to be differ- entially   flat   in   [12],   [25],   i.e.,   the   states   and   inputs   of the system can be written in terms of algebraic functions of appropriately chosen flat outputs and their derivatives. By using the differential flatness property, trajectories can be generated by leveraging the nonlinear dynamics of the quadrotors, rather than simply viewing the system dynamics as constraints. As   shown   in   [25],   the   flat   output   for   quadrotor can   be   chosen   as   σ   = [ x ,   y ,   z ,   ψ ] T   .   The   full   state   ξ   = [ x ,   y ,   z ,   v x ,   v y ,   v z ,   ψ ,   θ   ,   φ   ,   p ,   q ,   r ] T   and input   μ   = [   f z ,   τ x ,   τ y ,   τ z ] T  of the system can be represented algebraically using the following functions  ξ   =   β   ( σ   ,    ̇ σ   ,    ̈ σ   ,   ...  σ   ) ,  μ   =   γ ( σ   ,    ̇ σ   ,    ̈ σ   ,   ...  σ   ,   ....  σ   ) ,  where we refer to [25] for a detailed derivation and formula of the so-called endogenous transformation   ( β   ,   γ ) . The flight trajectory can be planned in a greatly simplified flat output space with the differential flatness property. Any sufficiently smooth trajectory   σ   ( t )   ∈   C 4   in the flat output space can then be converted analytically back into a feasible state trajectory   ξ   ( t )   and corresponding control input   μ ( t )  using the endogenous transformation. Switching from [25] to this paper, in order to generate four times differentiable flight trajectory, a virtual control input  v   ∈   R 3   is created for the integrator dynamics 1 . For simplicity of planning, the yaw angle is always set to zero ( ψ ( t ) =   0), ....  r   =   v ,   (1) where   r   =   σ 1:3   = [ x ,   y ,   z ] T   ∈   R 3 . The integrator system can be equivalently written as state space form  ̇ q   =  F ∈ R 4 × 4  ︷   ︸︸   ︷       0   1   0   0 0   0   1   0 0   0   0   1 0   0   0   0         ⊗ I 3 × 3   ·   q  ︸   ︷︷   ︸  f   ( q )  +  G ∈ R 4  ︷︸︸︷       0 0 0 1         ⊗ I 3 × 3  ︸   ︷︷   ︸  g ( q )  · v ,   (2) where   q   = [ r T   ,    ̇ r T   ,    ̈ r T   ,   ...  r   T   ] T   ∈   R 12 ,   ⊗   is Kronecker product. Note that since collision avoidance requires simultaneous response of three degrees of freedom, the trajectory planning problem here can not be simplified by decoupling three independent degrees of freedom, as was done in [12], [7]. III. E XPONENTIAL   C ONTROL   B ARRIER   F UNCTIONS  With   the   simplified   forth-order   integrator   model   for quadrotors introduced in Section II, Control Barrier Func- tions   (CBF)   can   be   used   to   ensure   collision-free   flight maneuvers. CBFs are Lyapunov-like functions, which can be used to provably guarantee the forward invariance of a desired set. When collision-free maneuvers are encoded as a safe set, CBFs can then be used to ensure that quadrotors  1 Note that the trajectory generated with the integrator dynamics (1) is not necessarily four times differentiable. In addition, the virtual control input  v   ∈   R 3   needs to be Lipschitz continuous for the forth derivative to exist [9], which will be shown in Section IV.
never escape from the safe set, i.e., they never collide. A class of non-conservative CBFs [2], [23], which allow the state to grow inside the safe set as opposed to strictly non- increasing as shown in Fig. 3, is adopted to synthesize the  Safety Barrier Certificates . Consequently, quadrotor flight controllers are provided with more freedom to excise desire maneuvers while remaining safe. Let the safe set of quadrotor states be defined as  C 0   =   { q   ∈   R 12   |   h ( q )   ≥   0 } ,   (3) where   h   :   R 12   →   R   is a smooth function. Safe Region Unsafe Region  Fig. 3: Examples of non-conservative CBFs for set invari- ance. The diamonds and curves are example initial states and allowed state trajectories, respectively. The state is allowed to grow or even approach the boundary from inside the safe region. Outside the safe region, the state will converge asymptotically to the safe region, due to CBF constraints. Position based safety constraints, i.e., constraints defined over   r , are of particular interest for the quadrotor system (2), so that the quadrotor does not collide with static or moving obstacles. With a slight abuse of notation, we denote   y 0 ( r ) =  h ( q )   as the output of the system, where   h ( q )   only contains the position variable   r . Because the virtual control input   v  is the forth derivative of the position variable   r , the relative degree of   y 0 ( r )   is 4, which means that  y ( 4 )  0   ( r ) =   L 4  f   h ( q ) +   L g L 3  f   h ( q ) v ,   (4) where the Lie derivative formulation stands for  ̇ h ( q ) =   ∂   h ( q )  ∂   q   (   f   ( q ) +   g ( q ) v ) =   L   f   h ( q ) +   L g h ( q ) v .  Note that due to the high relative degree of   y ( r ) , CBFs in [2], [23] can not be directly applied here. A variation called Exponential Control Barrier Function (ECBF) [15] can however be leveraged to ensure the forward invariance of   C 0 .  Definition III.1 : Given the dynamical system (2) and a set   C 0   defined in (3), the smooth function   h   :   C 0   →   R  with relative degree of 4 is an Exponential Control Barrier Function (ECBF) if there exists a vector   K   ∈   R 1 × 4   such that  ∀ x   ∈   C 0 ,  sup  u ∈ U  [ L 4  f   h ( q ) +   L g L 3  f   h ( q ) v   +   K η ]   ≥   0 ,   (5) and   h ( q ( t ))   ≥   C e F − GK   η ( q 0 )   ≥   0 when   h ( q 0 )   ≥   0, where   η   = [ h ( q ) ,   L   f   h ( q ) ,   L 2  f   h ( q ) ,   L 3  f   h ( q )] T   ,   C   = [ 1 ,   0 ,   0 ,   0 ] . The eligible vector   K   can be obtained by placing the poles of the closed-loop matrix   ( F   −   GK )   at   p   =   − [ p 1 ,   p 2 , ...,   p 4 ] T   , where   p i   >   0 for   i   =   1 ,   2 ,   3 ,   4. With these pole locations, a family of outputs   y i ,   i   =   1 ,   2 ,   3 ,   4 can be defined as  y i   = (   d d t   +   p 1 )   ◦   (   d d t   +   p 2 )   ◦   ...   ◦   (   d d t   +   p i )   ◦   h ( q ) ,  with   y 0   =   h ( q ) , and the associated family of super level sets  C i   =   { q   ∈   R 12   |   y i ( q )   ≥   0 } .   (6)  Theorem 3.1: Given a safe set   C 0   in (3) and associated ECBF h ( q )   :   C 0   →   R , with initially q 0   ∈   C i ,   i   =   0 ,   1 ,   2 ,   3   for system (2), any Lipschitz continuous controller v ( q )   ∈   K v ( q )  renders   C 0   forward invariant, where K v ( q ) =   { v   ∈   V   |   L 4  f   h ( q ) +   L g L 3  f   h ( q ) v   +   K v η   ≥   0 } ,  and   η   = [ h ( q ) ,   L   f   h ( q ) ,   L 2  f   h ( q ) ,   L 3  f   h ( q )] T   . We refer to [15] for the detailed proof of general cases of this theorem. The basic idea is to design a stabilizing controller for the system using pole placement, then use Comparison Lemma [9] to show recursively that   C i ,   i   =  0 ,   1 ,   2 ,   3, is forward invariant. IV. S AFETY   B ARRIER   C ERTIFICATES FOR   T EAMS OF  Q UADROTORS  Two of the main tools, i.e., differential flatness property of quadrotor and ECBF for a single quadrotor, for constructing  Safety Barrier Certificates   have been revisited in sections II and III. This section focuses on assembling   Safety Barrier Certificates   for teams of quadrotors utilizing these tools.  A. Safety Region Modelled With Super-ellipsoids  Consider   a   team   of   quadrotors   indexed   by   M   =  { 1 ,   2 ,   3 , . . . ,   m } , the dynamics of quadrotors are modelled as forth-order integrators with virtual inputs   v i   ∈   R 3 , ....  r   i   =   v i ,   i   ∈   M   (7) where   r i   = [ x i ,   y i ,   z i ] T   is the position of the center of mass of quadrotor   i . The full state of quadrotor   i   is represented by  q i   = [ r T i   ,    ̇ r T i   ,    ̈ r T i   ,   ...  r   T i   ] T   ∈   R 12 . Let   r   = [ r T  1   ,   r T  2   , ...,   r T m ] T   ∈   R 3 m  and   v   = [ v T  1   ,   v T  2   , ...,   v T m ] T   ∈   R 3 m   denote the aggregate position and virtual control of the team of quadrotors. In order to ensure the safety of the team of quadrotors,   all  pairwise collisions between quadrotors need to be avoided. In addition, quadrotors can not fly directly over each other due   to   the   air   flow   disturbance   generated   by   propellers as illustrated in Fig. 1. During actual flights, the bottom quadrotor generally goes unstable or even crash due to the strong wind blowing from above. To accommodate these safety requirements, each quadro- tor is encapsulated with a ‘rectangle shape’ super-ellipsoid 2 .  2 A super-ellipsoid is a solid geometry generally defined with the implicit function   [(   x a   ) r   + (   y b   ) r   ]   n r   + (   z c   ) n   ≤   1 with   r ,   n   ∈   R + [3].   r   =   n   =   4 is selected to approximate a ‘rectangle shape’ here.
Considering any pair of quadrotors   ( i ,   j ) , the pairwise safe set is defined as  C i j   =   { ( q i ,   q   j )   |   h i j ( q i ,   q   j )   ≥   0 } ,   (8)  h i j ( q i ,   q   j )   =   ( x i   −   x   j ) 4   + ( y i   −   y   j ) 4   + (   z i   −   z   j  c   ) 4   −   D 4  s   ,  where   D s   is the safety distance,   c   is the scaling factor along the Z axis caused by air flow disturbance. In practice,   c  is obtained by flying two quadrotors over each other and identify the critical separation distance at which the bottom quadrotor goes unstable. Two quadrotors are considered safe when two ‘rectangle shape’ super-ellipsoids do not intersect with each other as shown in Fig. 4. -2   2   4  -4 -2 2 4   x 2   + (   y  3   ) 2   = 1  x 4   + (   y  3   ) 4   = 1  | x | ∞   +   |   y  3   | ∞   = 1  (a)   XY   cross   section   of   a super-ellipsoid   (b)   Two   quadrotors   mod- elled with super-ellipsoids  Fig. 4: Safe regions of quadrotors modelled with super- ellipsoids. Note that a ‘rectangle shape’ super-ellipsoid is chosen to appropriately approximate the safe region, since the yaw angle of quadrotors is trivially set to   ψ   =   0 and quadrotors are flying with ‘X’ configuration 3 . Alternatively, a ‘cylinder shape’ super-ellipsoid can be picked for arbitrary yaw angles,  ̄ h i j ( q i ,   q   j ) = [( x i   −   x   j ) 2   + ( y i   −   y   j ) 2 ]   n  2   + (   z i   −   z   j  c   ) n   −   D n s   .  Since the pairwise safe set   C i j   is defined in terms of position variables   r i ,   r   j , the ECBF candidate   h i j ( q i ,   q   j )   has a relative degree of 4.   ( q i ,   q   j )   is omitted hereafter for notation convenience. To ensure the forward invariance of   C i j , virtual controls of quadrotor   i   and   j   need to satisfy ....  h   i j   +   K   ·   [ h i j ,    ̇ h i j ,    ̈ h i j ,   ...  h   i j ] T   ≥   0 ,   (9) where ....  h   i j   is affine in   v i ,   v   j . Thus, the safety barrier con- straint (9) can be rearranged into a linear constraint on the virtual control when   q i ,   q   j   are given,  A i j ( q i ,   q   j )   ·   v   ≤   b i j ( q i ,   q   j ) ,   (10) where   A i j ( q i ,   q   j ) =   − [ 0 , ...,   1 ︸︷︷︸  i th  , ...,   − 1  ︸︷︷︸  j th  , ...,   0 ]   ⊗   [ 4 ( x i   −  x   j ) 3 ,   4 ( y i   −   y   j ) 3 ,   4   ( z i − z   j   ) 3  c 4   ]   ∈   R 1 × 3 m ,  b i j ( q i ,   q   j ) =   K   ·   [ h i j ,    ̇ h i j ,    ̈ h i j ,   ...  h   i j ] T   + ( 24  ̇ δ   4   +   144 δ   ◦    ̇ δ   2   ◦    ̈ δ   +  36 ̄ r 2   ◦    ̈ δ   2   +   48 ̄ r 2   ◦    ̇ δ   ◦   ...  δ   )   ·   1 3 ,   δ   = [ x i   −   x   j ,   y i   −   y   j ,   z i − z   j  c   ] , and   ◦   stands for elementwise vector product.  3 ‘X’ configuration is when the quadrotor is configured with two propellers facing forward as shown in Fig. 2. ‘X’ configuration is often favored due to improved flight agility and onboard camera view clearance.  The   Safety Barrier Certificates   are formed by assembling all the pairwise safety barrier constraints  K safe   =   (11)  { v   ∈   R 3 m   |   A i j ( q i ,   q   j )   ·   v   ≤   b i j ( q i ,   q   j ) ,   ∀ i   <   j ,   i ,   j   ∈   M   } .  As long as the virtual control   v   satisfies the   Safety Barrier Certificates K sa f e   and corresponding initial conditions, the team of quadrotors is guaranteed to be safe by   Theorem 3.1 .  B. Modifying the Nomimal Trajectory with Safety Barrier Certificates  It is often difficult to generate provably collision-free trajectories when planning the nominal trajectory for teams of quadrotors. Instead, we can first plan the flight trajectory without considering collisions, and then modify it using  Safety Barrier Certificates   in a minimally invasive way to avoid collisions. Here we consider the case when a nominal trajectory ˆ r ( t ) = [ ˆ r T  1   ( t ) ,   ˆ r T  2   ( t ) , ...,   ˆ r T m ( t )]   ∈   C 4   is provided. For generality, the preplanned nominal trajectory can be gen- erated by any methods, e.g., optimal control approach [7], vector field approach[25], or parametrized curves [12], as long as it is sufficiently smooth, i.e., four times differentiable. This smooth reference trajectory ˆ r i ( t )   is then tracked by a simulated integrator model using a pole placement controller with a simulated time step of 0 . 02 s   (simulates the 50Hz flight controller), ˆ v i   =   ....  r   i   −   K   ·   [ ˆ r i    ̇ ˆ r i    ̈ ˆ r i  ... ˆ r   i ] T   ,   (12) where   K   is picked to be the same as used for ECBFs in (9) to trade-off tracking performance and safety enforcement. To respect the nominal control   ˆ v i   as much as possible, a quadratic program is used to minimize the difference between the actual and nominal control, which is what is meant by minimally invasive trajectory modifications,  v ∗   =   argmin  v  J ( v ) =  N  ∑  i = 1  ‖ v i   −   ˆ v i ‖ 2  s.t.   A i j ( q i ,   q   j ) v   ≤   b i j ( q i ,   q   j ) ,   ∀ i   <   j ,  ‖ v i ‖ ∞   ≤   α i ,   ∀ i   ∈   M   ,  (13) where   α i   is a bound on the virtual snap control. It can be observed that the actual controller   v i   will be the same as ˆ v i , if it is safe. The controller will only be rectified if it violates the   Safety Barrier Certificates , i.e., if it leads to collisions. The   dynamics   of   the   simulated   forth-order   integrator system is integrated forward using forward Euler method. Since (12) is a feedback tracking controller for a fully controllable integrator system, the numerical stability of the simple forward Euler method is already very reliable. In addition, since the nominal trajectory   ˆ r ( t )   is four times differentiable,   the   nominal   controller   ˆ v i   in   (12)   will   be Lipschitz continuous. According to [14], the controller   v i  generated by the QP (13) will be Lipschitz continuous as well. Thus, the rectified collision-free trajectory   r ( t )   will still be four times differentiable. Differential flatness property of quadrotors can still be used to execute the rectified collision- free trajectory   r ( t ) .
The advantages of using   Safety Barrier Certificates   to enforce collision-free maneuvers are as follows: 1) they can be used in conjunction with conventional tra- jectory generation methods to provide provable safety guarantees; 2) modifications made to   ˆ r ( t )   is minimal possible in the least-squares sense, and the rectified collision-free trajectory   r ( t )   is still four times differentiable. V. F EASIBILITY AND   P ARAMETERIZATION  Section IV provides a systematic approach to modify pre- planned trajectory in a minimally-invasive and smooth way using   Safety Barrier Certificates . However, it is not clear whether the quadratic program in (13) is always feasible or not. In addition, the generated trajectory might require excessive amount of control effort to execute. This section provides theoretical guarantees and parameterization method to address those issues.  A. Proof of Existence of Solution  The following theorem guarantees that a feasible safe solution to the QP problem (13) always exists.  Theorem 5.1: Given a team of quadrotors indexed by  M   with dynamics given in (7), the aggregate admissible safe control space K safe   in (11) allowed by Safety Barrier Certificates is guaranteed to be non-empty. Proof:   If any pair of quadrotors does not collide with each other, we have   r i   6   =   r   j   and   A i j ( q i ,   q   j )   6   =   0,   ∀ i   <   j . Thus,  h i j ( q i ,   q   j )   are individually valid control barrier functions. However, this does not imply that a common controller exists such that all constraints are satisfied. In order to prove that a common solution exists, let   H   ∈  R 1 × 3 m   be a convex combination of   − A i j ( q i ,   q   j )   ∈   R 1 × 3 m ,  H   =   −   Σ  i <   j α i j A i j ( q i ,   q   j ) ,   (14) where   [ α i j ]   ∈   D ,   D   =   { [ α i j ]   |   Σ  i <   j α i j   =   1 ,   α i j   ≥   0 } . It can be observed that   H   is the gradient of a convex function   F ( r )   :   R 3 m   →   R , where   r   = [ r T  1   ,   r T  2   , ...,   r T m ] T   denotes the aggregate position of the quadrotors,  F ( r ) =   Σ  i <   j α i j [( x i   −   x   j ) 4   + ( y i   −   y   j ) 4   + (   z i   −   z   j  c   ) 4 ] .   (15) Notice that   F ( r )   is non-negative and has a global minimum of 0, when   α i j ( r i   −   r   j ) =   0 ,   ∀ i   <   j . Since all local minimums of the convex function   F ( r )   are global minimums [4], it can be deduced that its gradient   ∇ F ( r ) =   H   =   0 if and only if  α i j ( r i   −   r   j ) =   0 ,   ∀ i   <   j . Since   r i   6   =   r   j   (quadrotors do not collide), it can be further inferred that   H   =   0 if and only if   α i j   =   0 ,   ∀ i   <   j , which violates the fact that   Σ  i <   j α i j   =   1. Thus we have shown by contradiction that   H   6   =   0. Using this fact, it is guaranteed that   Hv   +   Σ  i <   j α i j b i j ( q i ,   q   j )   >   0 always has a solution for any convex combination of   − A i j ( q i ,   q   j ) , i.e., min  [ α i j   ] ∈ D   sup  v ∈ R 3 m  {   Σ  i <   j α i j [ − A i j ( q i ,   q   j ) v   +   b i j ( q i ,   q   j )] }   >   0 .  Notice that   D   is closed and bounded, and   R 3 m   is closed. In this case, we can exchange min and sup by using the minmax theorem [17], i.e., min  [ α i j   ] ∈ D   sup  v ∈ R 3 m  {   Σ  i <   j α i j [ − A i j ( q i ,   q   j ) v   +   b i j ( q i ,   q   j )] }  =   sup  v ∈ R 3 m  min  [ α i j   ] ∈ D {   Σ  i <   j α i j [ − A i j ( q i ,   q   j ) v   +   b i j ( q i ,   q   j )] }  =   sup  v ∈ R 3 m  min  i <   j   {− A i j ( q i ,   q   j ) v   +   b i j ( q i ,   q   j ) }   >   0 ,  which is equivalent to say that a common controller   v   that satisfies all the pairwise barrier constraints (10) always exists, i.e.,   K safe   is non-empty. The idea of control sharing barrier function used in the proof is similar to control-sharing and merging control Lyapunov functions introduced in [6].  B. Virtual Vehicle Parameterization  Collision avoidance maneuvers of quadrotors might some- times lead to significant deviations from reference trajecto- ries. After the collision hazard disappears, excessive control effort might be required for the quadrotors to return to the reference point along the nominal trajectory. To address this issue, a virtual vehicle parameterization method proposed in [5] is adopted. The basic idea of virtual vehicle paramterization is to slow down or speed up the virtual vehicle (reference point ˆ r ( t )  on the nominal trajectory) as the tracking error   e r   =   ‖ r   −  ˆ r ‖   increases or decreases. In this particular application, we use the following virtual time variable to parameterize the reference point on the nominal trajectory  ̇ s   =   e − k s ‖ e r   ‖ 2  ,   (16) where   k s   is the virtual parameterization gain. Instead of ˆ r ( t ) , ˆ r ( s ( t ))   is fed into the   Safety Barrier Certificates   rectifier shown in Fig. 6. Intuitively, the virtual vehicle will slow down (  ̇ s   <   1) when the tracking error is large; it will travel exactly at the desire speed (  ̇ s   =   1) when the tracking error is zero. This parameterization mechanism is intended to reduce the amount of control effort when the quadrotor has to deviate away from the virtual vehicle to avoid collisions. To demonstrate the effectiveness of virtual vehicle param- eterization, a simulation of two quadrotors flying pass each other is presented. The trajectories of two quadrotors are illustrated in Fig. 5. In this example, the collision avoidance manuever requires a maximum pitch angle of 70 ◦   and a maximum thrust of 2 . 8 times hovering thrust without parameterization ( k s   =   0) as shown in Fig. 7. In contrast, a maximum pitch angle of 25 ◦   and a maximum thrust of 1 . 2 times hovering thrust are needed with parameterization ( k s   =   100). In both cases, the desired task is accomplished within 6s. As proved in section V-A, the QP in (13) will always generate a feasible solution. However, the required control effort to avoid collisions might be excessive. In this circum- stance, virtual vehicle parameterization method can be used to reduces instantaneous control efforts. By increasing the
Reference Trajectory Generator  Safety Barrier Certi   cates Recti   er  Di   eretial Flatness Tranformation  Reference trajectory  Safe trajectory  Feasibility Checker  Full State   Control  Virtual Vehicle Parameterization  Quadrotor Plant  Yes No Fig. 6: Flowchart of safe trajectory generation strategy. − 1 . 5   − 1 . 0   − 0 . 5   0 . 0   0 . 5   1 . 0   1 . 5  X ( m )  − 1 . 0  − 0 . 5 0 . 0 0 . 5 1 . 0  Y   ( m )  Fig. 5: Trajectories of two quadrotors flying pass each other plotted in X-Y plane. Control efforts for performing this task are illustrated in Fig. 7. virtual parameterization gain   k s   significantly, the quadrotors will be granted considerably more time to perform collision avoidance manuevers. Thus, the parameterization mechanism will generate a feasible trajectory that satisfies given actuator constraints.  C. Overview of Safe Trajectory Generation Strategy  An overview of the safe trajectory generation strategy is summarized in Fig. 6. A smooth reference trajectory ˆ r ( t )   ∈  C 4   is first fed into the safety barrier certificates rectifier, where the QP controller (13) is used to enforce collision-free flight maneuvers. The rectified smooth safe trajectory   r ( t )   ∈  C 4   is then transformed into quadrotor states and controls using the differential flatness property. The full states and controls are checked to ensure that actuator limits are not violated. Otherwise, the reference trajectory is parameterized ˆ r ( s ( t ))   ∈   C 4   and fed into the safety barrier certificates rectifier again. This process can be repeated until the virtual vehicle parameterization strategy yields appropriate flight trajectory that respects both safety and actuator constraints. In the end, the generated feasible safe trajectory is sent to execute on 0   1   2   3   4   5   6  − 80  − 40 0 40 80  Pitch( ◦ )   k s   = 0  k s   = 100  0   1   2   3   4   5   6 0 . 0 0 . 3 0 . 6  Error( m )   k s   = 0  k s   = 100  0   1   2   3   4   5   6  t ( s ) 0 . 2 0 . 4 0 . 6 0 . 8 1 . 0  Thrust( N )   ks   = 0  ks   = 100  Fig. 7: Comparisons of control efforts for the quadrotor using ( k s   =   100) or without using ( k s   =   0) virtual vehicle parameterization. the team of quadrotors. VI. E XPERIMENT  The   Safety Barrier Certificates   are implemented on a team of five palm-sized quadrotors (Crazyflie 2.0). All com- munication channels between different devices and control programs are coordinated by a ROS server. The real-time positions and Euler angles of quadrotors are tracked by the Optitrack motion capture system with an update rate of 50Hz. The 50Hz quadrotor motion controller is developed based on the ROS driver for Crazyflie 2.0 built by ACTLab at USC [8]. To ensure stable trajectory tracking behavior, Euler angles and Euler angle rates generated with the differential flatness property are sent to quadrotors as control commands. The overall quadrotor control diagram is shown in Fig. 8.  A. Showcase: Flying Through a Static Formation  In the first experiment, one of the quadrotor ( Q 5) is commanded to fly through a static formation consisting of four quadrotors ( Q 1   −   Q 4) as shown in Fig. 9.
ROS Server  Optitrack System  Controller + Barrier Certi   cates  Positions Euler Angles Controls Quad States  Controls States  Controls States  Controls States  Controls States  Controls States Fig. 8: Quadrotor control system diagram  Fig. 9: Snapshot from a experiment of quad   Q 5 flying through a static formation consisting of four quads   Q 1   −   Q 4. The video of this experiment is available online [16]. Quadrotors ( Q 1   −   Q 4) are designed to hover at four places with reference trajectories given as ˆ r 1 ( t ) =      0 . 25 0  − 0 . 8      ,   ˆ r 2 ( t ) =      0 0 . 25  − 0 . 8      ,  ˆ r 3 ( t ) =    − 0 . 25 0  − 0 . 8      ,   ˆ r 4 ( t ) =      0  − 0 . 25  − 0 . 8      .  Another quadrotor ( Q 5) is designed to go from   p 0   = [ 0 . 6 ,   − 0 . 6 ,   − 0 . 8 ] T   to   p 1   = [ − 0 . 6 ,   0 . 6 ,   − 0 . 8 ] T   . The nominal trajectory can be generated as ˆ r 5 ( t ) =   BezierInterp ( p 0 ,   p 1 ) ,  where the BezierInterp function stands for the Bezier curve interpolation algorithm between two waypoints. The safety distance between quadrotors is specified as  D s   =   25 cm   to account for the tracking frames and controller tracking errors. Intuitively, quadrotors will collide with each other if nominal trajectories are directly executed. During the experiment, the   Safety Barrier Certificates   are applied to modify the nominal trajectories in a minimally invasive way to avoid collisions. As demonstrated in Fig. 10,   Q 5 successfully navigated through the static formation of four quadrotors within 3.4s. − 0 . 8   − 0 . 4   0 . 0   0 . 4   0 . 8  − 0 . 8  − 0 . 4 0 . 0 0 . 4 0 . 8  Q1 Q2 Q3 Q4 Q5  (a) Agents at 8.8s − 0 . 8   − 0 . 4   0 . 0   0 . 4   0 . 8  − 0 . 8  − 0 . 4 0 . 0 0 . 4 0 . 8  Q1 Q2 Q3 Q4 Q5   (b) Agents at 9.6s − 0 . 8   − 0 . 4   0 . 0   0 . 4   0 . 8  − 0 . 8  − 0 . 4 0 . 0 0 . 4 0 . 8  Q1 Q2 Q3 Q4 Q5  (c) Agents at 11.0s − 0 . 8   − 0 . 4   0 . 0   0 . 4   0 . 8  − 0 . 8  − 0 . 4 0 . 0 0 . 4 0 . 8  Q1 Q2 Q3 Q4 Q5   (d) Agents at 12.2s  Fig. 10: Experimental data of the team of quadrotors plotted in the X-Y plane. The tail of each quadrotor illustrates its trajectory in the past 0.8s.  B. Showcase: Flying Through a Spinning Formation  Similar to the previous experiment, the quadrotor   Q 5 is commanded to fly through a spinning formation as illustrated in Fig. 11.  Fig. 11: Snapshot from a experiment of quad   Q 5 flying through a spinning formation consisting of four quads   Q 1   −  Q 4. The video of this experiment is available online [16]. The reference trajectories of quadrotors are designed as ˆ r 1 ( t ) =    0 . 45 sin (   π  2   t   −   π  2   )  0 . 45 cos (   π  2   t   −   π  2   )  − 0 . 8      ,   ˆ r 2 ( t ) =    0 . 45 cos (   π  2   t )  0 . 45 sin (   π  2   t )  − 0 . 8      ,  ˆ r 3 ( t ) =    0 . 45 cos (   π  2   t   +   π  2   )  0 . 45 sin (   π  2   t   +   π  2   )  − 0 . 8      ,   ˆ r 4 ( t ) =    0 . 45 cos (   π  2   t   +   π )  0 . 45 sin (   π  2   t   +   π )  − 0 . 8      ,  ˆ r 5 ( t ) =   BezierInterp (    − 0 . 9  − 0 . 9  − 0 . 8      ,      0 . 9 0 . 9  − 0 . 8    ) .
As shown in Fig. 12,   Q 5 successfully navigated through the spinning formation with minimal impact on the other four quadrotors by applying the   Safety Barrier Certificates . − 0 . 8   − 0 . 4   0 . 0   0 . 4   0 . 8  − 0 . 8  − 0 . 4 0 . 0 0 . 4 0 . 8  Q1 Q2 Q3 Q4 Q5  (a) Agents at 24.3s − 0 . 8   − 0 . 4   0 . 0   0 . 4   0 . 8  − 0 . 8  − 0 . 4 0 . 0 0 . 4 0 . 8  Q1 Q2 Q3 Q4 Q5   (b) Agents at 25.3s  s − 0 . 8   − 0 . 4   0 . 0   0 . 4   0 . 8  − 0 . 8  − 0 . 4 0 . 0 0 . 4 0 . 8  Q1 Q2 Q3 Q4 Q5  (c) Agents at 26.7s − 0 . 8   − 0 . 4   0 . 0   0 . 4   0 . 8  − 0 . 8  − 0 . 4 0 . 0 0 . 4 0 . 8  Q1 Q2 Q3 Q4 Q5   (d) Agents at 27.5s  Fig. 12: Experimental data of the team of quadrotors plotted in the X-Y plane. The tail of each quadrotor illustrates its trajectory in the past 0.6s. Experimental results provided in this section demonstrate that the   Safety Barrier Certificates   can save flight plan- ners the hassle of considering collision avoidance when designing the higher level multi-robot coordination algo- rithm. This strategy can be easily used in conjunction with other complicated motion planning strategies, e.g., optimal control algorithms, temporal/spatial assignment algorithms, to provide desired safety guarantees. The minimally invasive enforcement of the   Safety Barrier Certificates   ensures that desired controller will not be rectified unless truly necessary. VII. C ONCLUSIONS  A flight trajectory modification strategy is presented in this paper to ensure collision-free manuevers for teams of differ- ential flatness based quadrotors. To do this, nominal flight trajectories, which are generated with existing control and planning algorithms, are modified in a minimally invasive way using the   Safety Barrier Certificates   to avoid collisions. The proof of existence of feasible controller and virtual vehicle parameterization method to accommodate actuator limits are presented. In the end, experimental implementation of the   Safety Barrier Certificates   on a team of five quadrotors validates the effectiveness of the proposed strategy. To for- mally prove that the virtural vehicle parameterization method results in feasible flight trajectories subject to actuator limits will be an interesting future direction. R EFERENCES [1]   Bitcraze AB.   https://www.bitcraze.io/ . [2]   A. D. Ames, J. W. Grizzle, and P. Tabuada.   Control Barrier Func- tion Based Quadratic Programs with Application to Adaptive Cruise Control.   In   Decision and Control (CDC), 2014 IEEE 53rd Annual Conference on , pages 6271–6278, Dec 2014. [3]   A. H. Barr. Superquadrics and angle-preserving transformations.   IEEE Computer graphics and Applications , 1(1):11–23, 1981. [4]   S. Boyd and L. Vandenberghe.   Convex Optimization .   Cambridge University Press, 2004. [5]   M. Egerstedt, X. Hu, and A. Stotsky.   Control of mobile platforms using a virtual vehicle approach.   IEEE Transactions on Automatic Control , 46(11):1777–1782, 2001. [6]   S. Grammatico, F. Blanchini, and A. Caiti.   Control-sharing and merging control lyapunov functions.   IEEE Transactions on Automatic Control , 59(1):107–119, 2014. [7]   M.   Hehn   and   R.   DAndrea.   Real-time   trajectory   generation   for quadrocopters.   IEEE Transactions on Robotics , 31(4):877–892, 2015. [8]   W. Hoenig, C. Milanes, L. Scaria, T. Phan, M. Bolas, and N. Ayanian. Mixed reality for robotics. In   IEEE/RSJ Intl Conf. Intelligent Robots and Systems , pages 5382 – 5387, Hamburg, Germany, Sept 2015. [9]   H. K. Khalil.   Nonlinear systems . Prentice hall, third edition, 2002. [10]   Y. Kuriki and T. Namerikawa. Consensus-based cooperative formation control with collision avoidance for a multi-uav system.   In   2014 American Control Conference , pages 2077–2082. IEEE, 2014. [11]   B. Landry. Planning and control for quadrotor flight through cluttered environments. Master’s thesis, Massachusetts Institute of Technology, 2015. [12]   D. Mellinger and V. Kumar. Minimum snap trajectory generation and control for quadrotors.   In   Robotics and Automation (ICRA), 2011 IEEE International Conference on , pages 2520–2525. IEEE, 2011. [13]   D. Mellinger, A. Kushleyev, and V. Kumar. Mixed-integer quadratic program trajectory generation for heterogeneous quadrotor teams. In  Robotics and Automation (ICRA), 2012 IEEE International Conference on , pages 477–483. IEEE, 2012. [14]   B. Morris, M. J. Powell, and A. D. Ames. Sufficient conditions for the lipschitz continuity of qp-based multi-objective control of humanoid robots.   In   52nd IEEE Conference on Decision and Control , pages 2920–2926. IEEE, 2013. [15]   Q. Nguyen and K. Sreenath.   Exponential control barrier functions for enforcing high relative-degree safety-critical constraints. In   2016 American Control Conference (ACC) , pages 322–328. IEEE, 2016. [16]   Online.   Barrier certificates for safe quad swarm.   https://www. youtube.com/watch?v=rK9oyqccMJw , 2016. [17]   R. T. Rockafellar.   Convex analysis . Princeton university press, 2015. [18]   M. Valenti, D. Dale, J. How, and J. Vian. Mission health management for 24/7 persistent surveillance operations. In   AIAA Guidance, Control and Navigation Conference, Myrtle Beach, SC , 2007. [19]   L. Wang, A. D. Ames, and M. Egerstedt. Safety Barrier Certificates for Heterogeneous Multi-robot Systems.   In   2016 American Control Conference (ACC) , pages 5213–5218, July 2016. [20]   L. Wang, A. D. Ames, and M. Egerstedt. Multi-objective compositions for collision-free connectivity maintenance in teams of mobile robots. In   Decisions and Control Conference , 2016, to appear. [21]   G. Wu and K. Sreenath. Safety-critical control of a planar quadrotor. In  2016 American Control Conference (ACC) , pages 2252–2258. IEEE, 2016. [22]   G. Wu and K. Sreenath.   Safety-critical control of a 3d quadrotor with range-limited sensing. In   ASME Dynamics Systems and Control Conference (DSCC) , to appear, 2016. [23]   X. Xu, P. Tabuada, J. W. Grizzle, and A. D. Ames.   Robustness of control barrier functions for safety critical control.   In   Analysis and Design of Hybrid Systems, 2015 IFAC Conference on . IEEE, Oct 2015. [24]   C. Zhang and J. M. Kovacs. The application of small unmanned aerial systems for precision agriculture: a review.   Precision agriculture , 13(6):693–712, 2012. [25]   D. Zhou and M. Schwager.   Vector field following for quadrotors using differential flatness. In   2014 IEEE International Conference on Robotics and Automation (ICRA) , pages 6567–6572. IEEE, 2014.