title: On Compatibility and Region of Attraction for Safe, Stabilizing Control Laws
authors: Wenceslao Shaw Cortez Dimos V. Dimarogonas
text: 
  
  
  
  Complete classification of cosmological teleparallel geometries
      Manuel Hohmann
      Received XXX 2019 / Accepted XXX 2020
  ===============================================================
  
  empty
  empty
  
  
  
  
  A novel method is proposed to ensure stability and constraint satisfaction, i.e. “compatibility", for nonlinear affine systems. We require an asymptotically stabilizing control law and a zeroing control barrier function (ZCBF), and define a region of attraction for which the proposed control safely stabilizes the system. Our methodology requires checking conditions of the system dynamics over the state space, which may be computationally expensive. To facilitate the search for compatibility, we extend the results to a class of nonlinear systems including mechanical systems for which a novel controller is designed to guarantee passivity, safety, and stability. The proposed technique is demonstrated using numerical examples.
  
  
  
  
  
  
  § INTRODUCTION
  
  
  
  Zeroing control barrier functions (ZCBFs) have gained attention for constraint satisfaction of nonlinear systems <cit.>. ZCBFs are robust to perturbations <cit.>, can respect input constraints <cit.>, and are less restrictive than those of Lyapunov-based methods as the derivative of the ZCBFs need not be positive semi-definite <cit.>. 
  
  Other constraint satisfying methods include Nonlinear Model Predictive Control (NMPC) and reference governors (RG). ZCBFs are typically implemented in a one-step lookahead fashion that reduces the computational effort compared to NMPC where large prediction horizons may be required <cit.>. RGs can be conservative as they require the Lyapunov function level sets to be completely contained in the constraint set <cit.>. ZCBFs on the other hand allow the system to “touch" the constraint boundary for less conservative behaviour <cit.>. 
  
  Despite the advantages of existing ZCBF methods, there is a lack of guarantees therein regarding simultaneous stability and constraint satisfaction (i.e safety), which we refer to as “compatibility". In many cases it is desirable to know regions in which both the ZCBF and stabilizing conditions (i.e. decrescent Lyapunov function) hold simultaneously. For example, passivity is often coupled with a Lyapunov function (e.g., mechanical systems). Thus ensuring that the Lyapunov function remains decrescent preserves passivity, which is favourable for interacting with the environment <cit.> and for interconnected systems. Earlier work considered “safety-critical" ZCBF controllers <cit.>, where constraint satisfaction was prioritized over stability. There have been extensions addressing event-triggering <cit.>, sampled-data systems <cit.>, high-relative degree <cit.>, ZCBFs with NMPC <cit.>, and input constraints <cit.>, and learning <cit.>. Although safety is a priority, it is desirable to simultaneously ensure both constraint satisfaction and stability to safely accomplish a desired task. 
  
  ZCBFs have been combined with a control Lyapunov function (CLF) in a CLF-ZCBF formulation to help stabilize the system <cit.>. Local stability guarantees have been provided in <cit.>, while <cit.> eliminates undesirable equilibrium points, however there is no explicit region of attraction for which asymptotic stability holds. Furthermore, the CLF-ZCBF method uses a slack variable with respect to the stabilizing condition. Thus the given CLF may increase in the safe set. Finally, those methods depend on a previously designed CLF, which for nonlinear systems is non-trivial to construct. The problem addressed here is to design a control law with an associated region of attraction that ensures compatibility between a ZCBF and a nominal stabilizing control law that is not dependent on a CLF.
  
  An important application for deriving such a region of attraction and an associated safe, stabilizing controller is planning and control of mechanical systems. In this application, the concept of safety is closely related to passivity to prevent an unknown environment (e.g. a human) from de-stabilizing the system <cit.>. Recent work relies on linearizing the system dynamics <cit.> to conservatively address this problem. Another method employs a CLF-ZCBF controller, while being restricted to ellipsoidal constraints and assuming Lipschitz continuity of the control law <cit.>. Furthermore, neither of those existing methods address passivity to properly address safety of the overall system.
  
  In this paper, we address compatibility. The contribution is two-fold. First, we construct a region of attraction wherein safety and stability hold simultaneously. We design a novel control law that ensures asymptotic stability and safety over the region of attraction, which is not dependent on a CLF and allows Lyapunov functions that rely on LaSalle's invariance principle. Second, we extend our results to design a novel control law for mechanical systems that guarantees safety, stability, and passivity of the closed-loop system. Numerical simulations are used to validate the proposed method. 
   
  Notation: The Lie derivatives of a function h(x) for the system x = f(x) + g(x) u are denoted by L_f h and L_g h, respectively. The term ∇ h(x) for a function h: ℝ^n →ℝ is the gradient of h and ∇^2 h(x) denotes its Hessian. The interior, boundary, and closure of a set A are denoted Int(A), ∂A, and clos(A) respectively. The ball of radius δ about a point y is defined by B_δ(y) := {x∈ℝ^n: ||x - y|| ≤δ}, and denoted by B_δ if y = 0. The norm ||y ||^2_A for a matrix A is y^T A y. A set S is forward invariant if for every x(0) ∈S, x(t) ∈S ∀ t ∈I, where I is the maximal interval of existence for the solution x(t). A continuous function α:ℝ→ℝ is an extended class-𝒦 function if it is strictly increasing and α(0) = 0.
  
  
  
  § BACKGROUND
  
  
  
  
  
   §.§ Control Barrier Functions
  
  
  Here we introduce existing work regarding control barrier functions for nonlinear affine systems: 
  
      x = f(x) + g(x) u
  
   where f: ℝ^n →ℝ^n and g: ℝ^n →ℝ^n× m, are locally Lipschitz functions, x(t, x_0) ∈ℝ^n is the state, u(x(t)) ∈U⊆ℝ^m is the control input. Here we consider U = ℝ^m. Without loss of generality, we consider the origin x = 0 as the desired equilibrium point of the system (<ref>).
  
  Let h(x): ℝ^n →ℝ be a continuously differentiable function, and let the associated constraint set be defined by:
  
      C = {x∈ℝ^n: h(x) ≥ 0},
   
  
  
  Safety is ensured by showing that the system states are always directed into the constraint set (Theorem 3.1 of <cit.>). This condition is written as: ḣ(x) ≥ -α(h(x)) for an extended class-𝒦 function α, and formally defined as:
  <cit.>:
  Given the set C defined by (<ref>) for a continuously differentiable function h: E→ℝ, the function h is called a ZCBF defined on an open set E with C⊂E⊂ℝ^n if there exists a locally Lipschitz, extended class-𝒦 function α such that the following holds:
  
      u∈Usup [L_f h (x) + L_g h(x) u + α(h(x))] ≥ 0, ∀x∈E
  
  
  
  If h is a ZCBF, then (<ref>) can be enforced, which is linear with respect to u. Many methods implement this condition in a quadratic program to define the constraint satisfying control u <cit.>. One example of such a controller is defined as follows:
  
      u(x)  =    u∈ℝ^margmin || u - k(x) ||_2^2   
         s.t. L_f h(x) + L_g h(x) u≥ -α(h(x))
  
  where k(x) ∈ℝ^m is a locally Lipschitz control law. Many related methods construct barrier-function related control laws similar to (<ref>), but provide no guarantees of stability. 
  
  
  
  
   §.§ Problem Formulation
  
  
  The objective is to develop a control law and specify a region of attraction that ensures  asymptotically stability and constraint satisfaction simultaneously. 
  
  There exists a continuously differentiable ZCBF h:E→ℝ for the system (<ref>) where C⊂E⊂ℝ^n, C is defined by (<ref>),  0 ∈C, and E is an open set.
  
  
  
  There exists a locally Lipschitz control law k: D→ℝ^m with D⊆ℝ^n, 0 ∈D for which f(0) + g(0) k(0) = 0, and a continuously differentiable Lyapunov function V:D→ℝ, such that (<ref>) under u =k(x) is asymptotically stable[ see e.g., <cit.> for standard definitions] with respect to the origin. Furthermore if the given V is non-increasing for the system (<ref>) under u = k(x), then it is assumed that the conditions of LaSalle's principle (Corollary 4.1 of <cit.>) hold for (<ref>) under u = k(x).
  
  
  
  
  Consider the system (<ref>) and suppose Assumptions <ref> and <ref> hold. Design a control law u and define a region of attraction Γ⊂ℝ^n, Γ∩C≠∅, such that for all x(0) ∈Γ∩C, x(t) ∈C for all t≥ 0 and the system (<ref>) in closed-loop with u is asymptotically stable with respect to the origin.
  
  
  
  
  
  § PROPOSED SOLUTION
  
  
  Here we define a CBF-stabilizable level set to define the region of attraction given a ZCBF and a control law that asymptotically stabilizes (<ref>) with respect to the origin. We then define the proposed safe, stable control law u^*. Finally, we address a class of nonlinear systems for which the proposed method facilitates the design of the region of attraction. We further define a novel passive, safe, stabilizing controller for mechanical systems.
  
   
  Then we proceed to design a modified CLF-ZCBF control law to ensure stability and safety of (<ref>) by exploiting the proposed controller. 
  
  
  
  
   §.§ Safe, Stabilizing Control Design
  
  
  We define a CBF-stabilizable set as follows:
  
  Consider the system (<ref>) and suppose Assumptions <ref> and <ref> hold. Let Γ_ν:= {x∈D: V(x) ≤ν} for ν∈ℝ_>0. For a given locally Lipschitz continuous, positive-definite matrix G(x) ∈ℝ^m× m, Γ_ν is CBF-stabilizable if 
  
      ∇ V^T(x)g(x) G^-1(x) g(x)^T ∇ h(x) < 0
  
  holds on Γ_ν∩C except on a subset A⊂Γ_ν∩C for which the following holds:
  
      z(x) := L_fh(x) + L_gh(x) k(x)  + α(h(x)) ≥  0 , ∀x∈A.
  
  
  
  The intuition behind A is that in this set, the control barrier function condition (ḣ =∇ h(x)^T x≥ - α(h)) may act to de-stabilize the system. For stability, V̇ must be negative definite such that ∇ V(x)^T x≤ 0. Thus the inner product of ∇ V(x) and ∇ h(x) dictates if the conditions for safety and stability may counteract one another. Note that by construction, this inner product is weighted by g(x) G^-1(x) g(x)^T, for which the reason will become clear in the later analysis. It is important to emphasize the influence of G(x), which is a design parameter for the proposed control law (to be defined) and can be used to design A for compatibility. 
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  In the following Lemma, we ensure that Definition <ref> is well-defined in that there always exists a CBF-stabilizable set:
  
  Consider the system (<ref>) and suppose Assumptions <ref> and <ref> hold. For any locally Lipschitz continuous, positive-definite matrix G(x) ∈ℝ^m× m, there exists a ν̅∈ℝ_>0 such that Γ_ν̅ is CBF-stabilizable.
  
  
  Let G be a compact neighborhood of the origin for which 0 ∈G. Due to the fact that 0 ∈Int(C), where C is given in (<ref>), we restrict G⊂C such that h(x) > 0 for all x∈G. Let y(x) = f(x) + g(x) k(x). By Lipschitz continuity of y(x) and the fact that the origin is an equilibrium point, it follows that ||y(x) - 0 || ≤ L || x - 0 || ≤ L x, where L ∈ℝ_>0 is the Lipschitz constant of y(x) for all x∈G.
  
  Due to continuity of h and ∇ h and the fact that h(x) > 0 in G, it follows that there exists a ε∈ℝ_>0 such that α(h(x))/L ||∇ h(x)||≥ε for all x∈G. Let δ∈ (0, ε] be sufficiently small such that B_δ⊆G⊂C. Then it follows that || x || ≤δ≤α(h(x))/L ||∇ h (x)|| for all x∈B_δ⊂C. Thus by construction f(x) + g(x) k(x) ≤ L δ≤α(h(x))/||∇ h (x)||, which yields: -∇ hf(x) + g(x) k(x) + α(h(x))≥ 0. By using the Cauchy-Schwartz inequality it follows that z(x) ≥ -∇ hf(x) + g(x) k(x) + α(h(x))≥ 0 for all x∈B_δ. Now we choose ν̅ sufficiently small such that Γ_ν̅⊂B_δ. Note that we can always find a sufficiently small ν̅ because B_δ is non-empty and V(x) is a continuous, positive definite function with V(0) = 0. Since Γ_ν̅⊂B_δ⊂C, z(x) ≥ 0 holds for all x∈Γ_ν̅ and so (<ref>) holds for any A⊂Γ_ν̅ = Γ_ν̅∩C. Consequently Γ_ν̅ is CBF-stabilizable.
  
  
  
  We note that Lemma <ref> ensures a CBF-stabilizable set always exists by shrinking Γ_ν until Γ_ν⊂C. However this is a conservative approach to ensuring safety and we aim to avoid this by addressing A in Definition <ref>. We must emphasize that the set A is a subset of Γ_ν∩C over which (<ref>) must hold. We do not require that Γ_ν⊂C as in Lemma <ref>. On the contrary, we allow a designer to enlarge Γ_ν so long as the subset, A, satisfies (<ref>). This allows for larger regions of attraction and lets the system “touch" the constraint boundary.
  
  
  
  Next we introduce the proposed safe, stabilizing control law:
   
  
      u(x)^*  =    u∈ℝ^margmin1/2u - k(x)^2_G(x)
         s.t. L_f h(x) + L_g h(x) u≥ -α(h(x))
  
  
  where G(x) ∈ℝ^m× m is the locally Lipschitz continuous, positive-definite matrix from Definition <ref>. The controller (<ref>) seeks a u that is “minimally close" to k(x) to retain stability properties, while always respecting the ZCBF condition (<ref>) to enforce forward invariance of C. We note that (<ref>) is a generalized version of (<ref>) (set G = I_n× n)  and so the results presented here also apply to (<ref>).
  
  We will make use of the following assumption to ensure local Lipschitz continuity of (<ref>):
  
  Given a differentiable function h: E→ℝ with the closed set C⊂E⊆ℝ^n and a set S⊆ℝ^n, suppose either of the following conditions hold for (<ref>):
  
  
    * L_g h(x)≠ 0 for all  x∈S∩E; or
  
    * L_g h(x) = 0 only if x∈S∩C, and if L_g h(x) = 0 then ∇ h(x) = 0 for all x∈S∩E.
  
  
  
  
  Next we ensure the proposed control (<ref>) is well-defined:
  
  Consider the system (<ref>) and suppose Assumptions <ref> and <ref> hold. Given any locally Lipschitz continuous, positive-definite matrix G(x) ∈ℝ^m × m and a CBF-stabilizable set Γ_ν, then ∀x∈D∩E, the proposed control (<ref>) exists, is unique, and can be written in closed-form as:
  
      u^*(x) = u(x),  if  z(x)< 0
      k(x),  if  z(x) ≥ 0
  
  where
  
      u(x):= k(x) -z(x)/|| L_g h(x)^T ||_G^-1^2 G^-1(x) L_g h(x)^T,
  
  for z(x) from (<ref>). Furthermore, if Assumption <ref> holds for the ZCBF h and S = Γ_ν, then u^*(x) is locally Lipschitz continuous for all x∈Γ_ν∩E.
  
  
  Positive-definiteness of G(x) ensures that (<ref>) is well-posed in that the cost function is positive definite and (<ref>) is affine w.r.t u. This, along with Definition <ref>, ensures there always exists a unique solution u^* for all x∈E <cit.>.
  
  To derive u^* in closed-form, we define the control action when the constraint in (<ref>) is active (i.e holds with equality) by using the Schur-complement method from <cit.> when L_g h(x)^T _G^-1^2 ≠ 0. We denote this action as u defined in (<ref>). Thus u^* = u if (<ref>) holds with equality, otherwise u^* = k(x), since k(x) is the optimal solution for the unconstrained version of (<ref>). We similarly note if (<ref>) holds with a strict inequality, then λ^*(x) = 0, where λ^*(x) denotes the Lagrange multiplier of (<ref>).
  
  
  To derive (<ref>), we claim that u^* = k(x) only if z(x) ≥ 0. First, consider when z(x) > 0 such that L_f h(x) + L_g h(x) k(x) > -α(h(x)). Then the minimizing solution of (<ref>) is u^* = k(x) as (<ref>) holds with strict inequality. Now consider the case when z(x) = 0 such that (<ref>) holds with equality. Substitution of z(x) = 0 into u^*(x) = u(x) yields u^*(x) = k(x). Finally, consider when z(x) < 0 which yields: L_f h(x) + L_gh(x) k(x) < -α(h(x)). Clearly (<ref>) is not satisfied with u^* = k(x) and so u^* ≠k(x). Thus u^* = k(x) only if z(x) ≥ 0. This implies that u^* ≠k(x) if z(x) < 0, and the only other solution for u^* is u^* = u, which yields (<ref>). Note that by Definition <ref> when L_g h(x) = 0 the ZCBF condition  (<ref>) must hold independently of u (i.e. any choice of u will suffice). Thus when L_g h(x) = 0, since any u satisfies (<ref>), the quadratic program (<ref>) will output the solution that minimizes the cost (<ref>), namely u^* = k(x). As stated, u^* = k(x) only when z(x) ≥ 0 and so the control law (<ref>) is well-defined when z(x) = 0 and L_g h(x) = 0. 
  
  To ensure local Lipschitz continuity we show that (<ref>) satisfies the conditions of Theorem 3.1 of <cit.>. The premise of this proof is to check that the control law is “well-behaved" (i.e u^* = k(x)) in a neighborhood around L_g h(x) = 0, which by assumption is restricted to C∩Γ_ν (for x∈Γ_ν∩E) and implies that ∇ h(x) = 0. Then for any states outside of this neighborhood, L_gh ≠ 0 and does not get arbitrarily close to 0 such that the control is well-defined. To do so, we must first show that for every point x∈Γ_ν∩E for which L_g h(x) = 0, there exists a compact set B_Δ(x) such that u^* = k(x) for all x∈B_Δ(x). As mentioned in the previous paragraph, if L_g h(x)= 0 then u^* = k(x). We choose a Δ' ∈ℝ_>0 such that B_Δ'(x) ⊂Int(C). By continuity of h, α, f, g, and k, and (<ref>), there exists a ε_1, ε_2 ∈ℝ_> 0 such that for all x∈B_Δ'(x), α(h(x)) ≥ε_1 and ||y(x)|| ≤ε_2 for y(x) = f(x) + g(x) k(x). Now we design Δ≤Δ', where by continuity of ∇ h, there exists a μ∈ℝ_>0 such that ∇ h(x)≤μ for all x∈B_Δ(x). Let Δ be sufficiently small such that μ < ε_1/ε_2 for all x∈B_Δ(x). Substitution of μ < ε_1/ε_2  with the previous bounds yields: || ∇ h^T y(x) || ≤ ||∇ h|| ||y(x)|| ≤ ||∇ h|| ε_2 < ε_1. It follows that || L_f h(x) + L_g h(x) k(x)|| < ε_1 ≤α(h(x)) for all x∈B_Δ(x) and so the choice of u^* = k(x) satisfies (<ref>) for x∈B_Δ(x). Since u^* = k(x) is the optimal solution and renders the constraint inactive, then u^* = k(x) in a neighborhood of x for which L_g h(x) = 0. Note that when x∈B_Δ(x), λ^*(x) = 0 follows since (<ref>) is inactive.
  
  We next show that u^* is locally Lipschitz continuous by addressing the conditions of Theorem 3.1 of <cit.>. For a given x, we construct a convex, compact set H⊂Γ_ν∩E such that x∈H. Note that although Γ_ν∩E may not be convex, there always exists a convex, compact neighborhood of x for any x∈Γ_ν∩E. We construct H as follows. If x∈Γ_ν∩E is such that L_g h(x)= 0, we define H = B_Δ(x) for which u^*(x) = k(x) in H. For any x such that L_g h(x) ≠ 0, we let H be any compact, convex subset of Γ_ν∩E for which L_g h(x) ≠ 0 in H. As discussed previously, there always exists a solution to (<ref>) for all x∈ (Γ_ν∩E) ⊃H such that condition A.1 of Theorem 3.1 <cit.> is satisfied. 
  
  By assumption, G(x) is a positive-definite matrix and a continuous function of x such that G is bounded in H. Continuity of ∇ h ensures it is bounded in H. Thus there exist γ_1, γ_2 ∈ℝ_>0, γ_1, γ_2 < ∞ such that ||G|| ≤γ_1, ||L_g h|| ≤γ_2 for all x∈H, which satisfies A.2 of <cit.>. 
  
  Positive-definiteness and continuity of G for x∈H ensures there exists a ν∈ℝ_>0 such that u^T G u≥ν ||u||^2 for all x∈H. Next, we show there exists a β∈ℝ_>0 such that ||(L_g h(x))^T λ^*|| ≥β ||λ^*|| for all λ^*, for all x∈H. We note that when λ^* = 0, the condition follows trivially. Now consider the case when H = B_Δ(x). By construction, it follows that for all x∈B_Δ(x), λ^* = 0, and the condition follows trivially. For any H where L_g h(x) ≠ 0, since L_g h(x) is continuous on the compact set H, then there exists a β such that L_g h(x)^T ≥β.  Furthermore since λ^* is a scalar it follows that ||L_g h^T λ^*|| = |λ^*| ||L_g h^T|| ≥β |λ^*|.  Thus the condition ||L_g h^T λ^*|| ≥β |λ^*| holds for all λ^*, and for all x∈H and condition A.3 of <cit.> is satisfied. Thus from Theorem 3.1 of <cit.>, u^* is  Lipschitz continuous on every H, and so is locally Lipschitz continuous on Γ_ν∩E.
  
  
  
  The following theorem states the main result of this paper and ensures that the control law u^* along with a CBF-stabilizable level set ensures compatibility:
  
  Consider the system (<ref>) and suppose all the conditions of Theorem <ref> hold. Then the system (<ref>) under the proposed control (<ref>) satisfies: 
  
  
    * x(t) ∈Γ_ν∩C for all t ≥ 0.
  
    * The closed-loop system is asymptotically stable with respect to the origin.
  
  
  
  Theorem <ref> ensures u^* exists, is unique, and is locally Lipschitz continuous on Γ_ν∩E. Furthermore u^* can be written in closed form as defined in (<ref>), (<ref>).
  
  1) We first prove forward invariance of Γ_ν∩C. Substitution of x = 0 in (<ref>) and the fact that f(0) + g(0) k(0) = 0 by Assumption <ref> ensures that the origin is an equilibrium point of the closed-loop system. Furthermore, since 0 ∈C and V(0) = 0, then Γ_ν∩C≠∅.
  
  Next, we prove that the Lyapunov function, V, for the nominal system ((<ref>) with k(x)) is a valid Lyapunov function for the closed-loop system ((<ref>) with (<ref>)) on Γ_ν∩C. It follows trivially that when u^* = k(x), then V̇ = ∇ V(x)^T y(x)  ≤ 0 for y(x) = f(x) + g(x) k(x). Thus we need to check V̇ for when u^* = u. Computation of V̇ along with substitution of u^* = u yields: V̇  =  ∇ V(x)^T(f(x) + g(x) u ) =  ∇ V(x)^T ( y(x) -z(x)/|| L_g h(x)^T ||_G^-1^2g(x) G(x)^-1g(x)^T  ∇ h(x) ), and ultimately:
  
      V̇≤  
       -z(x)/|| L_g h(x)^T ||_G^-1^2∇ V(x)^T g(x) G(x)^-1g(x)^T ∇ h(x)
  
  Since z(x) ≥ 0 implies the trivial case when u^* = k(x), we need only consider when z(x) < 0. By (<ref>), V̇ < 0 for any x∈ (Γ_ν∩C) ∖A. Now consider x∈A. By assumption, (<ref>) is satisfied such that u^* = k(x) and V̇≤ 0 ∀x∈A. Thus V̇≤ 0 ∀x∈Γ_ν∩C.
   
  The previous analysis shows that for x(t) ∈Γ_ν∩C, V̇≤ 0 under the control (<ref>). Since V̇≤ 0 for every sub-level set of V in Γ_ν∩C, we know that x can not escape Γ_ν∩C via the boundary ∂Γ_ν (see proof of Theorem 4.1 of <cit.>). Furthermore, since h is a ZCBF and ḣ(x) ≥ - α(h(x)), Brezis Theorem (Theorem 4 of <cit.>) ensures that x(t) ∈C for all t ∈ [0, T), where [0, T) is the maximal interval of existence of x(t) for some T ∈ℝ_>0. We want to show that T = ∞, and we prove this by repeated application of Theorem 3.1 of <cit.> and Brezis Theorem as follows. 
  
  First, since x(0) ∈Γ_ν∩C and the closed-loop system is locally Lipschitz on Γ_ν∩E (which contains Γ_ν∩C) via Theorem <ref>, Theorem 3.1 of <cit.> ensures there exists a t_1 ∈ℝ_>0 such that x(t) is unique and exists on [0, t_1]. Now since all the conditions of Brezis Theorem hold on [0, t_1], we know that x(t) ∈Γ_ν∩C for t ∈ [0, t_1]. Since x(t_1) ∈Γ_ν∩C, we repeat this analysis for all i ≥ 2 for which t_i ∈ℝ_>0, t_i > t_i-1, such that x(t) is uniquely defined on each [0, t_i] and x(t) ∈Γ_ν∩C for t ∈ [0, t_i]. 
  
  Next we show that as i →∞ we have t_i →∞. Suppose instead that as i →∞, we have t_i →τ, τ < ∞, then x(t) would need to leave every compact subset of Γ_ν∩E (see proof of Theorem 3.3 of <cit.>). However, any solution that leaves Γ_ν∩C must traverse (Γ_ν∩E) ∖ (Γ_ν∩C) for which the closed-loop system is still locally Lipschitz and Brezi's theorem still holds. So those solutions could never have left Γ_ν∩C, leading to a contradiction. Thus x(t) is defined on [0, ∞) and  x(t) ∈Γ_ν∩C for all t ≥ 0. We emphasize that since Γ_ν∩C is forward invariant we need only check Lipschitz continuity of the closed-loop system on Γ_ν∩E, and hence we allow ∇ h(x) = 0 on C for states outside of Γ_ν.
  
  2) If (<ref>) holds with a strict inequality on (Γ_ν∩C)∖{0}, then asymptotic stability of the closed loop system under (<ref>) follows via Theorem 4.1 of <cit.>. For a non-increasing V, we must show that the only solution that can stay identically in {x∈ℝ^n: V̇(x) = 0} is x(t) ≡ 0 to apply LaSalle's invariance principle (see Corollary 4.1 of <cit.>). We do this by showing that V̇ = 0 only occurs when u^* = k(x). We need to check when u^* ≠k(x) (i.e. z(x) < 0). First, when ∇ V(x)^T  g(x) G(x)^-1g(x)^T ∇ h(x) < 0, then V̇ is in fact negative definite and V̇≠ 0. When ∇ V(x)^T  g(x) G(x)^-1g(x)^T ∇ h(x) ≥ 0 (i.e. x∈A), then it follows that u^* = k(x). Thus V̇ = 0 only when u^* = k, and furthermore no new equilibrium points can arise. Now since the original system satisfies Corollary 4.1 of <cit.> by assumption, the same results follow, i.e., asymptotic stability of the closed loop system under (<ref>).
  
  
  
  In previous work, ∇ h ≠ 0 on C was required <cit.>. However, the conditions of Theorems <ref> and <ref> are more relaxed in that ∇ h = 0 may occur on sections of C. The reason for this is that since u^*(x) keeps Γ_ν invariant, the states for which ∇ h(x) = 0 on C will never be reached. 
  
  
  
  The ZCBF is used to improve the stability of the original nominal control law by increasing the rate at which the Lyapunov function decreases. In this respect, if a designer can construct a CBF-stabilizable set, then they need not find a new Lyapunov function to guarantee safety and stability. For passivity-based nominal controllers, the proposed methodology preserves passivity by ensuring a decrescent V, i.e., no additional energy is added to the system via the ZCBF controller. This however means that the results presented herein are merely sufficient conditions for ensuring safety and stability. We further note that due to the use of a ZCBF and the results on asymptotic stability to the origin, the system (<ref>) in closed-loop with the proposed control is robust to bounded perturbations <cit.> <cit.>.
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  Many existing methods require a CLF to define safe, stabilizing controllers <cit.>. The assumption that a CLF is known is strong, but can be used in our controller (<ref>) by constructing a stabilizing control k_CLF  <cit.> to substitute for k in (<ref>).
  
  
  
  
  
   §.§ A Special Class of Nonlinear Systems
  
  
  The proposed methodology is dependent on computing the set A, however this may be cumbersome as it depends on checking g(x) over all Γ_ν∩C. Here we exploit G(x) to facilitate the design process. We consider a class of nonlinear systems satisfying the following assumption:
  
  The system (<ref>) can be written with g(x) = E g(x), where E = [0_(n-m)× m^T, I_m × m]^T, m ∈{1,...,n}, and g∈ℝ^m × m is invertible[If g = E g where E = [0_(n-l)× m^T, I_l × l]^T and g∈ℝ^l× m with l ≤ m is full row rank, then we can satisfy this assumption by substituting u = g^T μ for which g = gg^T and treat μ as the control input.] in a neighborhood of the origin.
  
  Assumption <ref> applies to mechanical systems, for which constraint satisfaction and stability are highly desirable. For such systems, we propose the choice of G(x) = g(x)^T g(x) = g(x)^T g(x). Substitution into (<ref>) yields: ∇ V^T(x) E E^T ∇ h(x) < 0, which facilitates the design of CBF-stabilizable sets by completely removing the dynamic terms to check in (<ref>). Now A is defined geometrically and can be checked without requiring the computation of g(x) nor G(x). Next, we apply the proposed methodology to mechanical systems.
  
  Consider a mechanical system with generalized coordinates q∈M⊂ℝ^n, generalized velocities v∈ℝ^n, and with the following dynamics:
  
      q   = v
      v   = M(q)^-1(-C(q, v) v  - τ_g(q) +  u  )
  
  where M: M→ℝ^n × n is the positive-definite inertia matrix, C: M×ℝ^n →ℝ^n× n is the Coriolis and centrifugal matrix, and τ_g: M→ℝ^n is the generalized gravity, and  u∈ℝ^n. Let (q(t, q_0), v(t,v_0)) ∈M×ℝ^n be the solution of (<ref>) starting at t = 0, which for ease of notation is denoted by x = (q, v). The system (<ref>) satisfies Assumption <ref> with E = [0_n× n, I_n× n]^T.
  
  
  
    §.§.§ Mechanical Systems with Relative-degree One ZCBF
  
  Here, we apply the results to mechanical systems with relative-degree one ZCBFs:
  
  Consider the system (<ref>) with the nominal, computed torque control law u = k(x) = C v + g + M(-K_p q - K_d v) and Lyapunov function V = 1/2(q^T K_p q + v^T v). Suppose Assumption <ref> holds with h = b - 1/2 (q^TP_q q +  v^TP_v v), where b ∈ℝ_>0,  P_q ∈ℝ^n× n is a symmetric matrix, and P_v ∈ℝ^n× n is a positive-definite, symmetric matrix. Then for G = M^-T M^-1, any Γ_ν⊂M×ℛ^n is CBF-stabilizable. Furthermore if Assumption <ref> holds for the ZCBF h and S = Γ_ν, then the results of Theorem <ref> hold and (<ref>) in closed-loop with (<ref>) is passive with respect to the input term μ = -K_d v and output v. 
  
  
  Assumption <ref> holds for the well-known computed torque control and associated V with D = M×ℝ^n <cit.>. Computation of the left-hand-side of (<ref>) with the chosen G (which is positive-definite and smooth <cit.>) yields: -v^TP_v v, which is non-negative only when v = 0. Substitution of v = 0 into (<ref>) yields z = α(h) ≥ 0 in A, such that any Γ_ν⊂D is CBF-stabilizable according to Definition <ref>. If Assumption <ref> holds, the results of Theorem <ref> follow directly. For passivity, let y = v and V be the storage function such that substitution of μ into V̇ for u = k(x) clearly yields V̇≤y^T μ. Substitution of u = u(x) (for which z < 0) into V̇ yields: V̇ = y^Tμ + z/L_g h^T_G^-1^2v^TP_v v < y^Tμ. Thus V̇≤y^Tμ holds and the proof is complete.
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
    §.§.§ Mechanical Systems with Relative-degree Two ZCBF
  
  
  We extend the results to the non-trivial case when the ZCBF is of higher order and to the well-known control law k(x) := τ_g(q) -K_p q - K_d v, for positive-definite, symmetric matrices K_p, K_d ∈ℝ^n× n, which asymptotically stabilizes (<ref>) to the origin with the following Lyapunov function:
  
      V(x) = 1/2v^T M(q) v + 1/2q^T K_p q
  
  We note that (<ref>) is no longer quadratic as in the previous section, which complicates the design of a CBF-stabilizable set. We will refer to the potential function as: P(q) = 1/2q^T K_p q with a level set P_ν := {q∈M: P(q) ≤ν}.
  
  To construct a ZCBF, let c: M→ℝ be a twice-continuously differentiable function with a locally Lipschitz Hessian. We define the safe set as:
  
      Q := {q∈M: c(q) ≥ 0}.
  
  
  Since c(q) is of relative-degree two with respect to (<ref>), we construct our candidate ZCBF using a high-order barrier function approach <cit.>:
  
      h(x) = ċ(q) + ϕ(c(q)) = ∇ c(q)^T v + ϕ(c(q))
  
  where ϕ is a continuously differentiable, extended class-𝒦 function. We proceed by differentiating h which yields L_f h(x) and L_g h(x) as follows:
  
      L_fh(x)    =  ∇ c(q)^T M(q)^-1( -C(q, v) v  - τ_g(q) ) 
         + ∂ϕ/∂ c∇ c(q)^T v  
      +v^T ∇^2 c(q) v
      
      L_g h(x)    = ∇ c(q)^T M(q)^-1
  
  
  
  
  
  
  
  
  
  
  In the following Lemma, we ensure that h is a zeroing control barrier function:
  
  Consider the system (<ref>) with the sets C = {x∈M×ℝ^n: h(x) ≥ 0} and C:= (Q×ℝ^n) ∩C for h defined in (<ref>) and Q defined in (<ref>). If Assumption <ref> holds for h with a given open set E⊃C, E⊆M×ℝ^n, and S = M×ℝ^n, then there exists a controller u such that (<ref>) holds. Furthermore, if any u satisfying (<ref>) is locally Lipschitz continuous and implemented in closed-loop in (<ref>), then C and Q are forward invariant with respect to (<ref>).
  
  
  Let α(h) be any extended class-𝒦 function. Consider the control: u_c = M( C v + τ_g + ∇ c/||∇ c||_2^2( L_fh - α(h)) ) when ∇ c ≠ 0. Since M is positive-definite, L_g h = 0 if and only if ∇ c = 0, and by assumption ∇ c = 0 implies that ∇ h = 0. When ∇ c≠ 0, then u_c is well-defined and substitution of u_c for u in ḣ yields: L_fh + L_gh u_c = - α(h). When ∇ c = ∇ h = 0, ḣ = 0, we choose any u∈ℝ^n. Since ∇ h = 0 only occurs in C (see Assumption <ref>) and α is an extended class-𝒦 function, it follows that ḣ = 0 > - α(h). Thus there exists a u satisfying (<ref>).
  
  The function h is in fact a high-order control barrier function as per Definition 5 of <cit.> and so implementation of a locally Lipschitz u satisfying (<ref>) ensures C is forward invariant via Theorem 1 of <cit.>. By construction, for any x∈C, q∈Q. Thus since C is forward invariant, so is Q. 
  
  
  
  
  
  We proceed by constructing a u^* similar to (<ref>) to safely stabilize the system. Substitution of (<ref>) into V̇ for when z  = L_f h + L_g h  k + α (h)< 0 yields: V̇ = 1/2v^T Ṁv + v^T ( - C v  + τ_g + u^* )
   + q^T K_p v  = v^T ( - K_d v - zG^-1 L_g h^T/ L_g h^T _G^-1^2 )  = - v^T  K_d v - z v^T G^-1 M^-1∇ c/( ∇ c^T M^-1 G^-1 M^-1∇ c ). We note that the skew-symmetric property of mechanical systems (i.e., v^T (1/2Ṁ - C) v = 0) <cit.> was used in this derivation. Now we choose G = M^-1, which yields:
  
      V̇   = - v^T  K_d v - z ċ/∇ c_M^-1^2
  
  and A = {x∈Γ_ν∩C: ċ = ∇ c^T v≥ 0 }. From Theorem <ref>, if we can find a Γ_ν for which ċ < 0 whenever z < 0, then asymptotic stability follows. However it may be difficult to guarantee such a condition holds for general constraint sets. Here we deviate from the analysis of Section <ref> and propose a novel, augmented form of u^* to guarantee asymptotic stability (we drop dependencies for notational convenience):
  
      u^*(x) = k,  if  z ≥ 0, 
      u,  if  z < 0, ċ≤ 0
      u + ρ^2 z ċ/|| ∇ c ||_M^-1^2 v ,  if  z < 0, ċ > 0
  
  where u  = k -z/|| ∇ c ||_M^-1^2 ∇ c and ρ∈ℝ_>0 is a gain parameter to be designed. The additional term,  ξ:= ρ^2 z ċ/|| ∇ c ||_M^-1^2 v, in (<ref>) cancels out the effects of ċ from (<ref>) for v_2 > 1/ρ, and prevents the ZCBF condition from injecting additional energy into the system. With the new control law (<ref>), we can compute A = {x∈Γ_ν∩C: ċ≥ 0, v_2 ≤1/ρ}. To address the case when the state enters A, we present an alternative condition to (<ref>) for mechanical systems that is more practical to check a priori: 
  
      ψ(q):= - ∇ c^TM(q)^-1 K_p q + α( ϕ(c(q))) > 0, ∀q∈P_ν∩Q
  
  
  
  Finally, if (<ref>) holds, we compute ρ as follows:
  
      ρ = max_q∈P_ν∩Qη_1(q) + √(η_1(q)^2 + 4ψ(q) η_2(q))/2ψ(q)
  
  where η_1(q) := k_c ∇ c^T M(q)^-1_2 + ∇^2 c(q) _2, η_2(q) := ∇ c^T M(q)^-1 K_d _2, and k_c ∈ℝ_>0 is the bound on C (i.e. C_2 ≤ k_c v_2)[This is a well-known property of mechanical systems <cit.>]. This tuning method only requires searching through the configuration space instead of the entire state space, which alleviates the computational complexity for implementation. Note that since ψ > 0 holds from (<ref>), η_1, η_2, ψ are continuous functions, and P_ν∩Q is compact, ρ is well-defined and bounded.
  
  
  
  Consider the system (<ref>) and suppose the conditions of Lemma <ref> hold for h defined by (<ref>), C as defined in Lemma <ref>, and Q defined by (<ref>). If (<ref>) holds for a given P_ν, ρ satisfies (<ref>), and x(0) ∈Γ_ν∩C, then (<ref>) under u^* from (<ref>) ensures the following conditions hold:
  
  
    * q(t) ∈P_ν∩Q and x(t) ∈Γ_ν∩C, for all t ≥ 0, 
  
    * The closed-loop system (<ref>) under (<ref>) is asymptotically stable with respect to the origin,
  
    * The closed-loop system is passive[See <cit.> for standard definitions of passivity.] with respect to the input term μ = - K_d v in (<ref>) and output v.
  
  
  
  1) First, when z ≥ 0 and z < 0, ċ≤ 0 the control law is well-defined and locally Lipschitz from Theorem <ref>. Since (<ref>) holds, (<ref>) is well-defined and ρ > 0. Thus by using the same analysis as in Theorem <ref>, it follows that the additional term ξ is also locally Lipschitz continuous on E, where E is defined from Lemma <ref>. Thus (<ref>) is well-defined in E.
  
  When z ≥ 0 and z<0, ċ≤ 0, we know that u^* is equivalent to (<ref>) for the system (<ref>). In this case it follows that ḣ≥ - α(h) which ensures (q, v) ∈C as per Lemma <ref>. When z<0 and ċ > 0, then u^* = u + ξ for u from (<ref>). Implementation of u^* = u ensures:  ḣ = L_f h + L_g h u̅≥ - α(h). Thus implementation of u^* = u + ξ yields: ḣ  = L_f h + L_g h (u +  ξ )≥ - α(h)  + L_g h ξ, where L_g h ξ is a perturbation. Here we show that this perturbation is benign in that forward invariance of C still holds. Let I:= [t_0, t_1) ⊆ℝ_≥ 0 denote the time interval over which ξ is active. Since ḣ≥ -α(h) holds whenever ξ is not implemented and by assumption x(0) ∈C, it follows that c(q(t_0)) ≥ 0 and h(x(t_0)) ≥ 0. Since ξ is only implemented when ċ > 0, it follows that c ≥ 0 for all t ∈I, i.e., q∈Q on this interval. Furthermore, since both ċ > 0 and c ≥ 0 on I, then h ≥ 0 (see (<ref>)). Thus x(t) ∈C on I and x(t) ∈C for all t ∉I holds from Lemma <ref>. Thus C is forward invariant. We note again that by construction of C, it follows that Q is also forward invariant.
  
  
  From (<ref>), it follows that (<ref>) under (<ref>) ensures V̇≤ 0 on Γ_ν∩C when (a) z ≥ 0, (b) z < 0, ċ≤ 0, and (c) z < 0, ċ > 0, v_2 > 1/ρ. Here we claim that the final possible case (d) (where  z < 0, ċ > 0, v_2 ≤1/ρ) can never occur such that V̇≤ 0 holds. We prove this claim by showing that for a slightly more general case than (d) where ċ≥ 0, v_2 ≤1/ρ, z must be non-negative. First, we use (<ref>), (<ref>), and the definition of z to separate z as z = χ  + θ where χ = -∇ c^T M^-1 C v  + v^T ∇^2 c  v- ∇ c^T M^-1 K_dv and θ = -∇ c^TM^-1K_p q + ∂ϕ/∂ cċ + α(ċ + ϕ(c)). We can state that θ≥ψ since ċ≥ 0 and ϕ, α are extended class-𝒦 functions. Now we can lower bound χ for v_2 ≤1/ρ via |χ| ≥ - 1/ρη_1(q) - 1/ρ^2η_2(q) such that z ≥ -  1/ρη_1(q) - 1/ρ^2η_2(q) + ψ. Now ρ as defined by (<ref>) is the solution to ρ^2 ψ - ρη_1 - η_2 = 0, and since ρ > 0 it follows that z ≥ψ -  1/ρη_1(q) - 1/ρ^2η_2(q) = 0 for all q∈P_ν∩Q. Now from (<ref>) and the fact that M is positive definite, it follows that for all x∈Γ_ν∩C, q∈P_ν∩Q and so z ≥ 0 holds in Γ_ν∩C for v_2 ≤1/ρ and ċ≥ 0. Thus the claim holds such that V̇≤ 0 in Γ_ν∩C, and so Γ_ν∩C is forward invariant. Since Γ_ν∩C is compact, it follows that x(t) ∈Γ_ν∩C for all t ≥ 0 using the same argument as in the proof of Theorem <ref>. Since x∈Γ_ν∩C implies that q∈P_ν∩Q, then q(t) ∈P_ν∩Q for all t≥ 0 follows. Furthermore, since the above analysis holds when ċ = 0, it follows that V̇ = 0 if and only if v = 0, and u^* = k(x) when v = 0.
  
  2.) Let Ω = {x∈Γ_ν∩C: v = 0}. Let v(t) be a solution that belongs identically to  Ω i.e. v(t) ≡ 0 for all t ≥ 0, for which v(t) ≡v(t) ≡ 0. Since u^* = k(x) when v = 0, the only solution that can stay identically in Ω is the origin, and the proof follows from Corollary 4.1 of <cit.>. 
  
  
  3) Passivity follows by treating V as the storage function and substituting μ = -K_d v and y = v in V̇. When u^* = k(x), it is clear that V̇= y^T u. When u^* = u(x), then V̇= y^T u - z ċ/∇ c_M^-1^2, and we know that z< 0, ċ≤ 0 (see (<ref>)) such that V̇≤y^T u. Finally, when u^* = u(x) + ρ^2 z ċ/∇ c_M^-1^2, then V̇ =y^T u - z ċ/∇ c_M^-1^2 (1- ρ^2 v_2^2). Since in this case z < 0, ċ > 0 (see (<ref>)) then V̇ < y^T u for v_2 > 1/ρ. Furthermore, from the previous analysis we need not check when v_2 ≤1/ρ since this control  u^* = u(x) + ρ^2 z ċ/∇ c_M^-1^2 will never be active in this case. Thus V̇≤y^T μ, which completes the proof.
  
  
  Theorem <ref> does not require linearized approximations of the dynamics <cit.> nor assumptions on Lipschitz continuity of the control, and it is not restricted to ellipsoidal sets <cit.>. The condition (<ref>) can be checked offline and is only dependent on the coordinates q, yet this approach may still be subject to the curse of dimensionality as n grows.
  
  
  
  
  § NUMERICAL EXAMPLES
  
  
  Here we demonstrate the results on a 2-DOF manipulator consisting of two identical links with a length of 1 m and mass of 1 kg. We refer to <cit.> for the system dynamics. First consider the relative-degree one candidate ZCBF with P_q = 0_n× n, P_v = I_n× n: h(x) = b - 1/2v_2^2, which is used to restrict the squared-norm of the velocity of the closed-loop system below b. Now h is a ZCBF because for any extended-class 𝒦 function α, the choice of u = g + Cv + α(h) M v/v_2^2, if v_2 >0, and u = 0, if v = 0, ensures that (<ref>) holds. Furthermore ∇ h = -v≠ 0 on C. From Proposition <ref>, the choice of G = M^-T M^-1 ensures that any Γ_ν is CBF-stabilizable. Figure <ref> shows the proposed control (<ref>) implemented for α(h) = h, K_p = I_2× 2, K_d = 0.5 I_2× 2, and b = 0.01 for various initial conditions x(0) ∈Γ_ν∩C. We see that all trajectories converge to the origin, while ensuring h ≥ 0, such that velocity constraints are always respected. 
  
  
  
  Now consider the constraint set Q defined by the relative-degree two constraint c̅(q) = a - (q - q_r)^T P (q -  q_r), for a = 0.8, q_r_1 = [0.9, 0]^T, P_1 = diag([1.0,2.0]). The parameters of the stabilizing controller are K_p = diag([1.0, 1.0]), K_d = diag([0.5, 0.5]), with the ZCBF parameters as ϕ(h) = h, α(h) = h. We note that ∇c̅ = 0 at q  = q_r, which is a singularity where Assumption <ref> would not hold. We remove the singularity using the technique from <cit.> and define c(q) = χ_δ(c̅(q)), where χ_δ = 1 if c̅/δ > 1 and χ_δ = (c̅/δ - 1)^3 + 1 if c̅/δ≤ 1. Since c(q) = 0 when c̅(q)= 0, the boundaries of their respective Q sets are the same. A value of δ = 0.7 was used for these simulations. Furthermore, by construction when ∇ c = 0, we have ∇^2 c = 0 such that the condition of Lemma <ref> is satisfied and h defined by (<ref>) is a high-order ZCBF. For this choice of P and c, (<ref>) holds for any P_ν. We note that (<ref>) was checked by performing a grid search.
  
  
  Figure <ref> shows the resulting trajectories for initial conditions in C for the proposed control (<ref>) applied to the 2-DOF manipulator. Figure <ref> shows the trajectories remaining inside Q as they converge to the origin and Figure <ref> shows the resulting Lyapunov function decreasing to zero. Note that the Lyapunov function never increases, but does plateau in regions as expected due to the application of LaSalle's principle. To demonstrate robustness, we apply the time-varying perturbation d = [d_0(t), d_1(t)]^T for d_i = A_i sin(ω_i t) for i ∈{0,1} and A_0 = A_1 = 0.1, ω_0 = ω_1 = 1.0. The results of the closed-loop system with the proposed control (<ref>) and matched disturbance d(t) is shown in Figure <ref>, wherein the system remains bounded close to the origin and remains in Q. 
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  § CONCLUSION
  
  
  We proposed a methodology to ensure simultaneous stability and constraint satisfaction of nonlinear systems. The proposed approach defines a region of attraction, coined a CBF-stabilizable set, for which compatibility of an existing nominally stabilizing control law and ZCBF is ensured. A control law was proposed to ensure stability and safety in the CBF-stabilizable set. Furthermore, the proposed control was extended into a novel passive, safe, stabilizing controller for mechanical systems. Future work will incorporate input constraints in the proposed approach and investigate the relation between CBF-stabilizable sets and viability-kernels. 
  
  
  IEEEtran
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  