Safety-Critical Control for Robots

Miao Yu

9 Safety-Critical Control for Robots

Miao Yu

9. Safety-Critical Control and Applications in Advanced Manufacturing

9.1 Introduction

9.1.1 Motivation on Safety-Critical Control

9.1.2 Safety-Critical Control Techniques

9.1.3 Structure of the Chapter

9.2 Problem Formulation

9.3 Control Barrier Functions (CBF)

9.3.1 Safe Set and Forward Invariance

9.3.2 Forms of Barrier Functions

9.3.3 Control Lyapunov Functions

9.3.4 Control Barrier Functions

9.3.5 Applications

9.4 Hamilton-Jacobi Reachability Analysis (HJ-RA)

9.4.1 Backward Reachable Tube (BRT)

9.4.2 Backward Reach-Avoid Tube (BRAT)

9.4.3 Applications and Comparison to CBF

9.5 Summary

Practice Problems
Simulation & Animation
References

9.1 Introduction

9.1.1 Motivation on Safety-Critical Control

When designing controllers for dynamical systems, safety is always one of the most important con-siderations. Classical control methods are able to find solutions so that the system is asymptotically stable, or even better, converges to the equilibrium point arbitrarily fast. This can be considered as the controller guaranteeing the system safety since asymptotical stability guarantees the system to approach the equilibrium point and stay there for all future time. If the equilibrium point is safe, then the system will be safe in finite time.

So, why safety-critical control? The term of the “safety-critical” is used to distinguish the systems that safety is the major design consideration. With the rapid development of the robotics and autonomous systems, more and more applications are introduced into manufacturing and daily life, such as mobile robots, robot manipulators, autonomous cars, etc. This requires the system to be able to handle the situations with unknown environment such as sudden obstacles and moving objects. Furthermore, some applications require the interaction with human users. In these cases, safety is extremely important, and classical stability-based control methods may not be able to handle the complex environment. Thus, safety-critical control techniques are needed.

9.1.2 Safety-Critical Control Techniques

Various methods are used as safety-critical control. For example, control barrier functions, reacha-bility analysis, and contraction theory, etc.

Control barrier functions take advantage of safe set to build controllers that guarantee safety. A safe set is a set that contains the system states that are considered safe. Ddetails will be introduced in section 9.3.1. Barrier functions are Lyapunov-like functions that can be designed to stay non-negative in the interior of safe set and go to infinity when the states approach the boundary of the safe set. Therefore, by using a technique similar to the control lyapunov functions [1], we can design a controller to force the states to stay in the safe set.

(Hamiltanian-Jacobi) reachability analysis takes the system disturbance into account and treats the disturbance as a player in a “two-player game” that tries to steer the system to the unsafe set, while the system control input plays the role as another player that tries to steer the system away from the unsafe set. In this way, it is possible for the reachability analysis to find a controller in the worst-case scenario. Hamiltanian-Jacobi Isaac PDE will be used in reachability analysis to solve the dynamic programming and leads to an optimal controller.

Contraction theory, Lakshmanan et al., and Machester et al. construct a control contraction metric (CCM), which is analogous to control Lyapunov functions, and uses techniques such as optimization-based methods or sliding control to stabilize or track a trajectory of the system, thus guarantees the safety. This method can ensure the system to track arbitrary trajectories [2], however, deriving an analytical form of contraction metrics is challenging [3]. As the way it is used for the safety-critical control is similar to those in the former two methods, we will not discuss this method in detail in this chapter.

9.1.3 Structure of the Chapter

In this chapter, we will first outline the problem formulation in section 9.2. The concept and theories of control barrier functions will be introduced in section 9.3. Then we will introduce the Hamil-ton Jacobi reachability analysis in section 9.4, and comparison and applications will also be discussed.

9.2 Problem Formulation

The nonlinear dynamical system we will discuss in this chapter (for control barrier functions) will have the following form:

$\label{eq:nonlinear_form} \dot{\boldsymbol{x}}=f(\boldsymbol{x})+g(\boldsymbol{x})\boldsymbol{u},\quad\quad(9.1)\nonumber$

where $\boldsymbol{x}\in X\subset\mathbb{R}^n$ is the system state, which can usually be $(\boldsymbol{q}^T,\dot{\boldsymbol{q}}^T)^T$ in robotics, $f(\cdot):\mathbb{R}^n\rightarrow\mathbb{R}^n,\,g(\cdot):\mathbb{R}^n\rightarrow\mathbb{R}^m$ are nonlinear functions of system state, $\boldsymbol{u}\in U\subset\mathbb{R}^n$ is the system control.

Note that the general Euler-Lagrange equation for manipulators $M\ddot{\boldsymbol{q}}+C\dot{\boldsymbol{q}}+N=B\boldsymbol{u}$ can be written in the form of Equation (9.1) by defining $\boldsymbol{x}=(\boldsymbol{q}^T,\dot{\boldsymbol{q}}^T)^T$ and we have

$\dot{\boldsymbol{x}} &=\begin{bmatrix} \dot{\boldsymbol{q}} \\ M^{-1}(-C\dot{\boldsymbol{q}}-N) \\ \end{bmatrix}+\begin{bmatrix} 0\\ B \end{bmatrix}\boldsymbol{u}\\ &=f(\boldsymbol{x})+g(\boldsymbol{x})\boldsymbol{u}.\quad\quad\quad\quad(9.2)\nonumber$

While Equation (9.1) defines the form of deterministic control-affine system, in practical problems, disturbance may also present in the system and affect the system performance. Therefore, we can define the dynamical system subject to external disturbance as

(1) $\begin{equation*} \dot{\boldsymbol{x}}=f(\boldsymbol{x},\boldsymbol{u},\boldsymbol{d}),\quad\quad(9.3)\nonumber \end{equation*}$

where $\boldsymbol{x}$ and $\boldsymbol{u}$ are the same as in Equation (9.1), and $\boldsymbol{d}\in D\subset \mathbb{R}^n$ denotes the noise of model uncertainty. Besides, we use $\zeta_{\boldsymbol{\boldsymbol{x}_0,t_0}}^{\boldsymbol{u},\boldsymbol{d}}(t)$ to denote the solution, or the state trajectory of (9.3) starts from $\boldsymbol{x}_0$ at time $t_0$ , with control $\boldsymbol{u}$ and disturbance $\boldsymbol{d}$ . We can also use $\zeta(s;\boldsymbol{x},t,\boldsymbol{u},\boldsymbol{d})$ to denote the trajectory of Equation (9.3) starts from state $\boldsymbol{x}$ at time $t$ , where $s\in[t,0]$ . Equation (9.3) will be used as the expression of the dynamical systems in HJ reachability analysis.

9.3 Control Barrier Functions (CBF)

In this section, we will introduce control barrier functions (CBF), one of the most commonly used methods in safety-critical control design. One major reason that CBF is widely used is that, the recent development of CBF suggests that CBF can be used as an add-on constraint in many control design techniques based on Lyapunov and control Lyapunov functions (CLF) [1, 4].

9.3.1 Safe Set and Forward Invariance

Before designing the control scheme, it is crucial to have a mathematical expression of the safety. One way to express the safety of a system is the safe set. Given a dynamical system $\dot{\boldsymbol{x}}=f(\boldsymbol{x})$ with $\boldsymbol{x}\in\mathbb{R}^n$ , a safe set C can be written as

(2) $\begin{equation*} \mathcal{C}=\{\boldsymbol{x}\in\mathbb{R}^n:h(\boldsymbol{x})\geq0\},\quad\quad\quad(9.4)\nonumber \end{equation*}$

where $h:\mathbb{R}^n\rightarrow{}\mathbb{R}$ is a continuously differentiable function. Also, we care about the boundary and interior of the safe set:

(3) $\begin{align*} \partial\mathcal{C}=\{\boldsymbol{x}\in\mathbb{R}^n:h(\boldsymbol{x})=0\},\quad\quad\quad(9.5)\\ Int(\mathcal{C})=\{\boldsymbol{x}\in\mathbb{R}^n:h(\boldsymbol{x})>0\}.\quad\quad\quad(9.6)\nonumber \end{align*}$

Next we will have one simple example illustrating how the safe set can be defined and used to indicate whether the system is safe or not.

Example 9.1

A 2-link manipulator is shown in Figure 9.1 [change the format to be consistent with other chapters]. The orange semi-sphere in the figure is the obstacle. Define a safe set for the manipulator moving in the 2D space with the end effector not hitting the obstacle (to simplify the problem, we only care about the end effector).

Figure 9.1: A 2-link robot manipulator moving in the 2D space. The orange semi-sphere is the obstacle.

Created by Miao Yu.

First write the forward kinematic of the end effector:

(4) $\begin{align*} x=a_1cos(\theta_1)+a_2cos(\theta_2),\\ y=a_1sin(\theta_1)+a_2sin(\theta_2).\nonumber \end{align*}$

According to the geometric relation between the end effector and the obstacle, the end effector staying outside of the obstacle can be mathematically expressed as

(5) $\begin{equation*} (x-r)^2+y^2\geq r^2.\nonumber \end{equation*}$

Therefore, the safe set can be written as

(6) $\begin{equation*} \mathcal{C}=\{(x,y)^T\in\mathbb{R}^2:h(x,y)\geq0\}, \quad \text{with}\quad h(x,y)=(x-r)^2+y^2-r^2.\nonumber \end{equation*}$

Geometrically, $h(x)$ defines the constraints for the positions of the system state. It is worth noting that for certain applications, $h$ can also be a function of $x$ and $\dot{x}$ (for example, $h(\boldsymbol{q,\dot{q}})$ in [5]). Introducing x˙ in safe set gives velocity information of the system so that we can define the system to be unsafe not only based on the system’s current posture, but on the moving trend (velocity) as well. This will provide more space in designing controllers.

Now that we have the definition of safe sets, next step would be considering what kind of system can be considered safe. To do this, we first need the concept forward invariance.

Definition 9.1

A set $M$ is called positive invariant (forward invariant) with respect to $\dot{\boldsymbol{x}}=f(\boldsymbol{x})$ if

(7) $\begin{equation*} \boldsymbol{x}(0)\in M\Rightarrow \boldsymbol{x}(t)\in M,\quad \forall t\geq0.\nonumber \end{equation*}$

Roughly speaking, if a set is forward invariant, then if the state enters the set at any time instant, it will never leave the set for all further time. The idea of forward invariance can be used in Lyapunov theorem in proving the stability of a dynamical system [1]. Shortly, we will discuss how forward invariance can help in guaranteeing safety.

Example 9.2

Given a system $\dot{x}=-x$ , and a Lyapunov candidate $V(x)=\frac{1}{2}x^2$ , show that $M=\{V(x)\leq c\}$ is forward invariant for all positive $c$ .

First suppose that $x(0)\in M$ for some $c$ , i.e., $\frac{1}{2}x^2(0)\leq c$ . Next, observe that

(8) $\begin{equation*} \dot{V}=\frac{dV}{dt}=\frac{\partial V}{\partial x}\frac{dx}{dt}=x(-x)=-x^2\leq 0, \,\forall t\nonumber \end{equation*}$

Therefore, $V(x(t))\leq V(x(0))\leq c\Rightarrow x(t)\in M,\,\forall t\geq0$ . By the definition of forward invariance, set is forward invariant for any positive $c$ .

For a given safe set, when designing a controller, if we can guarantee that if the system starts in the safe set, the system state will stay in the safe set for all future time, then we can say that the system is safe with respect to the safe set. To formally define safety, we have the following definition [4].

Definition 9.2

The system Equation (9.1) is safe with respect to the set $\mathcal{C}$ if the set $\mathcal{C}$ is forward invariant.

One way to determine the controller that guarantees safety is using CBF. Starting from the next section, we will introduce the idea of CBF and its application in CLF-based control.

9.3.2 Forms of Barrier Functions

The history of study on safety of dynamical systems can date back to the 1940’s [6]. In [6], Nagumo provided necessary and sufficient condtions for set invariance based onh^˙ on the boundary of C:

(9) $\begin{equation*} \text{$\mathcal{C}$ is invariant}\iff \dot{h}(\boldsymbol{x})\geq0\quad\forall\boldsymbol{x}\in\partial\mathcal{C}.\nonumber \end{equation*}$

In the 2000’s, due to the need to verify hybrid systems, bBarrier certificates were introduced as a convenient tool to formally prove safety of nonlinear and hybrid systems [4, 7, 8]. The term “barrier” was chosen to indicate that it is used to be added to cost functions to avoid undesirable areas. In this section, we will introduce the concept and different forms of barrier functions. The usage of barrier functions in safety-critical control design will be discussed later.

In safety certificate, one first considers the unsafe set $\mathcal{C}_u$ and the initial condition $\mathcal{C}_0$ together with a function $B:\mathbb{R}^n\Rightarrow\mathbb{R}$ where $B(\boldsymbol{x})\leq0,\quad \forall \boldsymbol{x}\in\mathcal{C}_0$ and $B(\boldsymbol{x})>0,\quad \forall \boldsymbol{x}\in\mathcal{C}_u$ . Then $B$ is a barrier certificate if

(10) $\begin{equation*} \dot{B}(\boldsymbol{x})\leq0\quad\Rightarrow\quad \text{$\mathcal{C}$ is invariant}\nonumber \end{equation*}$

The safety set is chosen to be the complement of the unsafety set $\mathcal{C}_u$ . And by choosing $B(\boldsymbol{x})=-h(\boldsymbol{x})$ , the barrier certificate satisfies Nagumo’s theorem.

However, the above analysis is mostly based on the behavior on the boundary of the safe set. To extend the idea and make use of not only the boundary, but also the interior of the safe set, other formats of the barrier functions are needed. In this section, we will introduce two of the commonly used catagories of the barrier functions: reciprocal barrier functions and zeroing barrier functions.

Given a safe set $\mathcal{C}$ of the form Equation (9.4), we want to define a function $B:\mathbb{R}^n\Rightarrow\mathbb{R}$ such that $B(\boldsymbol{x})\geq0,\, \forall \boldsymbol{x}\in\mathcal{C}$ and $B(\boldsymbol{x})<0,\, \forall \boldsymbol{x}\notin\mathcal{C}$ . Consider the form

(11) $\begin{equation*} B(\boldsymbol{x})=-\log\left(\frac{h(\boldsymbol{x})}{1+h(\boldsymbol{x})}\right).\quad\quad\quad(9.7)\nonumber \end{equation*}$

Figure 9.2: One form of reciprocal barrier function Equation (9.7).

Created by Miao Yu

The shape of $B$ with respect to $h(\boldsymbol{x})$ can be found in fig. 9.2. It can be seen that Equation (9.7) satisfies the properties

(12) $\begin{equation*} \underset{\boldsymbol{x}\in\text{Int($\mathcal{C}$)}}{\text{inf}}B(\boldsymbol{x})\geq0,\quad \underset{\boldsymbol{x}\notin\text{Int($\mathcal{C}$)}}{\text{inf}}B(\boldsymbol{x})<0,\quad \underset{\boldsymbol{x}\rightarrow\partial\mathcal{C}}{lim}B(\boldsymbol{x})=\infty.\quad\quad\quad(9.8)\nonumber \end{equation*}$

The property that $B(\boldsymbol{x})$ goes to infinity as the state approaches the boundary of the safe set makes it clear why the term “barrier” is used. Given Equation (9.7), one common idea to ensure the forward invariance of $\mathcal{C}$ is to enforce $\dot{B}(\boldsymbol{x})\leq0,\, \forall \boldsymbol{x}\in X$ . However, this is overly constrained as when the state is far from the boundary $\partial\mathcal{C}$ . Tthere is no need to constrain the barrier function to be non-increasing when the states are far from the boundary. One way to relax the condition is as following

(13) $\begin{equation*} \dot{B}\leq\frac{\beta}{B},\quad\quad\quad\quad(9.9)\nonumber \end{equation*}$

with positive $\beta$ . In this way, when the state is far from the boundary of the safe set, Equation (9.9) allows the barrier function to grow. When the state approaches the boundary, i.e., when $B\rightarrow\infty$ , we will have $\frac{\beta}{B}\rightarrow0$ and thus force the changing rate to decrease to zero.

Another commonly used barrier function is

(14) $\begin{equation*} B(\boldsymbol{x})=\frac{1}{h(\boldsymbol{x})}.\quad\quad\quad(9.10)\nonumber \end{equation*}$

It can be easily seen that Equation (9.10) also satisfies the properties in Equation (9.8). Plug Equation (9.10) into Equation (9.9), we can have the inequality

(15) $\begin{equation*} h(\boldsymbol{x},t)\geq\frac{1}{\sqrt{2\beta t+\frac{1}{h^2(\boldsymbol{x}_0)}}},\quad\quad\quad(9.11)\nonumber \end{equation*}$

From Equation (9.11), if $\boldsymbol{x}_0\in\text{Int}(\mathcal{C})$ , i.e, $h(\boldsymbol{x}_0)>0$ , then $h(\boldsymbol{x},t)>0,\,\forall t$ , which means $\boldsymbol{x}\in\text{Int}(\mathcal{C})$ for all future time, thus guarantees the forward invariance of the safe set.

Equation (9.7) and (9.10) are two commonly used examples for reciprocal barrier functions. Next, we will provide a general definition of a reciprocal barrier function [9].

Definition 9.3

For the dynamical system $\dot{\boldsymbol{x}}=f(\boldsymbol{x})$ , a continuously differentiable function $B:\text{Int($\mathcal{C}$)}\rightarrow\mathbb{R}$ is a reciprocal barrier function (RBF) for the set $\mathcal{C}$ defined by Equation (9.4) and (9.5) for a continuously differentiable function $h:\mathbb{R}^n\rightarrow\mathbb{R}$ , if there exist class $\mathcal{K}$ functions $\alpha_1,\alpha_2,\alpha_3$ such that, for all $x\in\text{Int($\mathcal{C}$)}$ ,

(16) $\begin{equation*} \frac{1}{\alpha_1(h(\boldsymbol{x}))}\leq B(\boldsymbol{x})\leq \frac{1}{\alpha_2(h(\boldsymbol{x}))},\quad\quad\quad(9.12)\nonumber \end{equation*} \begin{equation*}\label{eq:rbf_def2} L_fB(\boldsymbol{x})\leq{\alpha_3(h(\boldsymbol{x}))}.\quad\quad\quad\quad(9.13)\nonumber \end{equation*}$

Where a class $\mathcal{K}$ function is defined as $\alpha: [0,a)\rightarrow[0,\infty)$ with $\alpha(0)=0$ and strictly increasing. Lie derivative is defined as $L_fV(\boldsymbol{x})=\frac{\partial V}{\partial\boldsymbol{x}}f(\boldsymbol{x})$ .

From definition 9.3, since $B(\boldsymbol{x})$ is bounded by two functions of the form $\frac{1}{\alpha(h(\boldsymbol{x}))}$ for some $\alpha$ , the barrier function $B(\boldsymbol{x})$ must behave like $\frac{1}{\alpha(h)}$ for some class $\mathcal{K}$ function $\alpha$ with the property

(17) $\begin{equation*} \underset{\boldsymbol{x}\in\text{Int($\mathcal{C}$)}}{\text{inf}}\frac{1}{\alpha(h(\boldsymbol{x}))}\geq0,\quad \underset{\boldsymbol{x}\rightarrow\partial\mathcal{C}}{\text{lim}}\frac{1}{\alpha(h(\boldsymbol{x}))}=\infty.\quad\quad\quad(9.14)\nonumber \end{equation*}$

Next, we will introduce a lemma that will be used to prove the theorem from [9], which describes the relation between RBF and the forward invariance of the safe set.

Lemma 9.1

Consider the dynamical system

(18) $\begin{equation*} \dot{y}=\alpha\left(\frac{1}{y}\right),\quad y(t_0)=y_0,\quad\quad\quad(9.15)\nonumber \end{equation*}$

with $\alpha$ a class $\mathcal{K}$ function. For every $y_0\in(0,\infty)$ , the system has a unique solution defined for all $t\geq t_0$ and given by

(19) $\begin{equation*} y(t)=\frac{1}{\sigma(\frac{1}{y_0}),t-t_0},\quad\quad\quad(9.16)\nonumber \end{equation*}$

where $\sigma$ is a class $\mathcal{KL}$ function.

In lemma 9.1, class $\mathcal{KL}$ functions are continuous functions $\beta:[0,a)\times[0,b)\rightarrow[0,\infty)$ and for fixed $s$ , $\beta(r,s)$ belongs to class $\mathcal{K}$ , for fixed $r$ , the function $\beta(r,s)$ is decreasing with respect to $s$ and $\beta(r,s)\rightarrow0$ as $s\rightarrow0$ .

Theorem 9.1

Given a set $\mathcal{C}\subset\mathbb{R}^n$ defined by Equation (9.4) and (9.5) for a continuously differentiable function $h$ , if there exists a RBF $B:\text{Int($\mathcal{C}$)}\rightarrow\mathbb{R}$ , then $\text{Int($\mathcal{C}$)}$ is forward invariant.

Proof

Using Equation (9.12) and (9.13), we have that

(20) $\begin{equation*} \dot{B}\leq\alpha_3\circ\alpha_2^{-1}(\frac{1}{B}):=\alpha(\frac{1}{B}).\quad\quad\quad(9.17)\nonumber \end{equation*}$

Since the inverse of a class $\mathcal{K}$ function is a class $\mathcal{K}$ function, and the composition of class $\mathcal{K}$ function, $\alpha=\alpha_3\circ\alpha_2^{-1}$ is a class $\mathcal{K}$ function, [1].

Let $\boldsymbol{x}(t)$ be a solution of $\dot{\boldsymbol{x}}=f(\boldsymbol{x})$ with $\boldsymbol{x}_0\in\text{Int($\mathcal{C}$)}$ , and let $B(t)=B(\boldsymbol{x}(t))$ . The next step is to apply the Comparison Lemma to Equation (9.17) so that $B(t)$ is upper bounded by the solution of Equation (9.15). To do so, it must be noted that the hypothesis “ $f(t,\boldsymbol{u})$ is locally Lipschitz in $\boldsymbol{u}$ ” used in the proof of Lemma 3.4 in [1], can be replaced by with the hypothesis “ $f(t,\boldsymbol{u})$ is continuous, non-increasing in $\boldsymbol{u}$ “. This is valid because the proof only uses the local Lipschitz assumption to obtain uniqueness of solutions to Equation (9.15), and this was taken care of with Peano’s Uniqueness Theorem in the proof of lemma 9.1 (not provided in this section, see [9]).\\

Hence, the Comparison Lemma in combination with lemma 9.1 yields

(21) $\begin{equation*} B(\boldsymbol{x}(t))\leq \frac{1}{\sigma(\frac{1}{B(\boldsymbol{x}_0)},t-t_0)},\quad\quad\quad(9.18)\nonumber \end{equation*}$

for all $t\in I(\boldsymbol{x_0})$ , where $\boldsymbol{x}_0=\boldsymbol{x}(t_0)$ . This, coupled with the left inequality in Equation (9.12), implies that

(22) $\begin{equation*} \alpha_1^{-1}(\sigma(\frac{1}{B(\boldsymbol{x}_0)},t-t_0))\leq h(\boldsymbol{x}(t)),\quad\quad\quad(9.19)\nonumber \end{equation*}$

for all $t\in I(\boldsymbol{x}_0)$ . By the properties of class $\mathcal{K}$ and $\mathcal{KL}$ functions, if $\boldsymbol{x}_0\in \text{Int($\mathcal{C}$)}$ and hence $B(\boldsymbol{x}_0)>0$ , it follows from Equation (9.19) that $h(\boldsymbol{x}(t))>0$ for all $t\in I(\boldsymbol{x}_0)$ . Therefore, $\boldsymbol{x}(t)\in \text{Int($\mathcal{C}$)}$ for all $t\in I(\boldsymbol{x}_0)$ , which implies that $\text{Int($\mathcal{C}$)}$ is forward invariant.

Another form of barrier functions is called zeroing barrier functions. Due to the page limit, we will not introduce it in detail. The definition of extended class $\mathcal{K}$ and zeroing barrier functions are as follows [1, 9].

Definition 9.4

A continous function $\alpha:(-b,a)\rightarrow(-\infty,\infty)$ is said to belong to $\textbf{extended class $\mathcal{K}$}$ for some $a, b > 0$ if it is strictly increasing and $\alpha(0)=0$ .

Definition 9.5

For the dynamical system $\dot{\boldsymbol{x}}=f(\boldsymbol{x})$ , a continuously differentiable function $h:\mathbb{R}^n\rightarrow\mathbb{R}$ is a zeroing barrier function (ZBF) for the set $\mathcal{C}$ defined by Equation (9.4) and (9.5), if there exist an extended class $\mathcal{K}$ function $\alpha$ and a set $\mathcal{D}$ with $\mathcal{C}\subseteq\mathcal{D}\subset\mathbb{R}^n$ such that, for all $\boldsymbol{x}\in\mathcal{D}$ ,

(23) $\begin{equation*} L_fh(\boldsymbol{x})\geq-\alpha(h(\boldsymbol{x})).\quad\quad\quad(9.20)\nonumber \end{equation*}$

9.3.3 Control Lyapunov Functions

Now that we have the barrier functions to verify the safety of a dynamical system, we can try to find a method that force the system to guarantee the safety. And this is a similar logic as in control Lyapunov functions (CLF). To motivate the safety-critical controller design, this section will first introduce one of the commonly used methods in state feedback sta-bilization design, control Lyapunov functions. Stabilization of equilibrium points is the core task in feedback control of nonlinear systems. Many techniques can be used to achieve this objective, e.g., partial feedback linearization, backstepping, passivity-based control, etc. [1]. Among these methods, CLF provides a systematic way to design the feedback controller, and the exis-tence of CLF is also a sufficient condition for the existence of a stabilizing state feedback control, which makes it a crucial technique. For detailed definitions and proofs shown in this section, see [1, 4].

Given a nonlinear system in the form of Equation (9.1), suppose the control objective is to stabilize the state $\boldsymbol{x}$ to an equilibrium point $\boldsymbol{x}^*=\boldsymbol{0}$ . Suppose there exists a locally Lipschitz stabilizing state feedback control $\boldsymbol{u}=k(\boldsymbol{x})$ such that the origin of

(24) $\begin{equation*} \dot{\boldsymbol{x}}=f(\boldsymbol{x})+g(\boldsymbol{x})k(\boldsymbol{x}),\quad\quad\quad(9.21)\nonumber \end{equation*}$

is asymptotically stable. Then, by the converse Lyapunov theorem [1], there is a smooth Lyapunov function $V(\boldsymbol{x})$ such that

(25) $\begin{equation*} \dot{V}(\boldsymbol{x},k(\boldsymbol{x}))=L_fV(\boldsymbol{x})+L_gV(\boldsymbol{x})k(\boldsymbol{x})\leq-\gamma(V(\boldsymbol{x})),\forall \boldsymbol{x}\neq 0\quad\quad\quad(9.22)\nonumber \end{equation*}$

where $L_fV(\boldsymbol{x}), L_gV(\boldsymbol{x})$ are Lie derivatives $\frac{\partial V}{\partial\boldsymbol{x}}f(\boldsymbol{x})$ , $\frac{\partial V}{\partial\boldsymbol{x}}g(\boldsymbol{x})$ , respectively, and $\gamma: \mathbb{R}_{\geq0}\rightarrow\mathbb{R}_{\geq0}$ is a class $\mathcal{K}$ function on the entire real line, i.e., $\gamma(0)=0$ , and $\gamma(\cdot)$ is strictly increasing. If such $V$ exists, it is called the control Lyapunov function. The corresponding $\boldsymbol{u}=k(\boldsymbol{x})$ can be applied to drive $\boldsymbol{x}$ converge to the origin. Concretely, $V$ is a control Lyapunov function if it is positive definite and satisfies [4]

(26) $\begin{equation*} \underset{\boldsymbol{u}\in U}{\text{inf}}[L_fV(\boldsymbol{x})+L_gV(\boldsymbol{x})k(\boldsymbol{x})]\leq-\gamma(V(\boldsymbol{x})). \quad\quad\quad(9.23)\nonumber \end{equation*}$

Therefore, we can write the set that contains all stabilizing controllers for every point $\boldsymbol{x}\in X$ as

(27) $\begin{equation*} K_\mathrm{clf}(\boldsymbol{x})=\{\boldsymbol{u}\in U:L_fh(\boldsymbol{x})+L_gh(\boldsymbol{x})\boldsymbol{u}\leq-\gamma(V(\boldsymbol{x}))\}.\quad\quad\quad(9.24)\nonumber \end{equation*}$

Note that if $V$ satisfies Equation (9.22), it must have the property

(28) $\begin{equation*} L_gV(\boldsymbol{x})=0\quad \text{and}\quad\boldsymbol{x}\neq0\quad\Rightarrow\quad L_fV(\boldsymbol{x})\leq -\gamma(V(\boldsymbol{x})). \quad\quad\quad(9.25)\nonumber \end{equation*}$

Therefore, given a CLF $V$ , a stabilizing state feedback control is given by $\boldsymbol{u}=k(\boldsymbol{x})$ , where

(29) $\begin{equation*} k(\boldsymbol{x}) = \begin{cases} -[L_fV+\sqrt{(L_fV)^2+(L_gV)^4}]/L_gV, &\text{if $L_gV\neq0$}\\ 0, &\text{if $L_gV=0$} \end{cases}\quad\quad\quad(9.26)\nonumber \end{equation*}$

To show that Equation (9.26) stabilizes the origin, we can choose V as the Lyapunov function candidate. Then, for $\boldsymbol{x}\neq\boldsymbol{0}$ , if $L_gV=0$ , from Equation (9.25), we have

(30) $\begin{equation*} \dot{V}=L_fV(\boldsymbol{x})\leq-\gamma(\boldsymbol{x})<0,\nonumber \end{equation*}$

and if $L_gV\neq0$ , we have

(31) $\begin{align*} \dot{V}&=\frac{\partial V}{\partial\boldsymbol{x}}(f(\boldsymbol{x})+g(\boldsymbol{x})k(\boldsymbol{x}))\nonumber\\ &=L_fV-[L_fV+\sqrt{(L_fV)^2+(L_gV)^4}]\nonumber\\ &=-\sqrt{(L_fV)^2+(L_gV)^4}\nonumber\\ &<0\nonumber \end{align*}$

Thus, for all $\boldsymbol{x}\neq0$ , in a neighborhood of the origin, $\dot{V}<0$ , which shows that the origin is asymptotically stable. The following example will demostrate how CLF will be used in control design.

Example 9.3

Design a controller for the scalar system

(32) $\begin{equation*} \dot{x}=2x-x^3+u,\nonumber \end{equation*}$

to stabilize the origin $x=0$ .

One can use feedback linearization to stabilize the origin by choosing the control to be $u=-2x+x^3-\alpha x$ , $\alpha>0$ . The asymptotical stability can be proved by $V(x)=\frac{1}{2}x^2$ .\\

If we choose $V=\frac{1}{2}x^2$ as the control Lyapunov function, then by Equation (9.26), the control can be written as

(33) $\begin{equation*} u=k(x)=-2x+x^3-x\sqrt{(2-x^2)^2+1}.\nonumber \end{equation*}$

Figure 9.3 shows the comparison of the control $u$ and the closed-loop $\dot{x}=f(x)$ between feedback linearization ( $\alpha=1$ ) and CLF. We can see that CLF results in a much smaller magnitude of the control for large $|x|$ , and meanwhile, much faster decaying rate for large $|x|$ . In this example, the control $k(x)$ in CLF takes advantages of the nonlinear term $-x^3$ , which is ignored by feedback linearization.

Figure 9.3: Comparison of the control $u$ and the closed-loop $\dot{x}=f(x)$ between feedback linearization (red dashed curve) and CLF (blue solid curve).

Created by Miao Yu

The role of CLF in safety-critical control with barrier functions will be introduced in section 9.3.4. Furthermore, inspired by the idea of CLF, we can use similar method to ensure the system safety, which will be discussed in section 9.3.4.

9.3.4 Control Barrier Functions

Inspired by the idea of control Lyapunov functions, along with the fact that Lyapunov theorem can be used to verify the forward invariance of a superlevel set consist of Lyapunov functions (or Lyapunov-like functions), it is natural to extend this idea to safety critical systems with barrier functions.

In section 9.3.2, we introduced the concept of barrier functions for autonomous systems $\dot{\boldsymbol{x}}=f(\boldsymbol{x})$ . Now we are ready to extend this idea into dynamical system in the form of Equation (9.1) to guide the safety-critical control design. Firstly, given that a reciprocal barrier function for an autonomous system needs to satisfy Equation (9.12) and (9.13) and it results in the property of Equation (9.14), what if $\dot{\boldsymbol{x}}=f(\boldsymbol{x})$ cannot have the set $\mathcal{C}$ to be forward invariant? How can we design a controller to ensure the forward invariance of $\text{Int}(\mathcal{C})$ ? This leads to the following definition [9].

Definition 9.6

Consider the control system Equation (9.1) and the set $\mathcal{C}\subset\mathbb{R}^n$ defined by Equation (9.4) and (9.5) for a continuously differentiable function $h$ . A continuously differentiable function $B:\text{Int($\mathcal{C}$)}\rightarrow\mathbb{R}$ is called a reciprocal control barrier function (RCBF) if there exist class $\mathcal{K}$ functions $\alpha_1,\alpha_2,\alpha_3$ such that, for all $\boldsymbol{x}\in\text{Int($\mathcal{C}$)}$ ,

(34) $\begin{equation*} \frac{1}{\alpha_1(h(\boldsymbol{x}))}\leq B(\boldsymbol{x})\leq \frac{1}{\alpha_2(h(\boldsymbol{x}))}, \quad\quad\quad(9.27)\nonumber \end{equation*}$

(35) $\begin{equation*} \underset{\boldsymbol{u}\in U}{\text{inf}}[L_fB(\boldsymbol{x})+L_gB(\boldsymbol{x})\boldsymbol{u}-{\alpha_3(h(\boldsymbol{x}))}]\leq0. \quad\quad\quad(9.28)\nonumber \end{equation*}$

\noindent The RCBF $B$ is said to be locally lipschitz continuous if $\alpha_3$ and $\frac{\partial B}{\partial \boldsymbol{x}}$ are both locally Lipschitz continusous.

Guaranteed Safety via RCBFs. Note that definition 9.6 is the control system version of definition 9.3 by changing $\dot{B}=L_fB(\boldsymbol{x})$ to $\dot{B}=L_fB(\boldsymbol{x})+L_gB(\boldsymbol{x})\boldsymbol{u}$ . Now we can define the set that contains all the possible controllers that satisfy definition 9.6. For a given RCBF $B$ , for all $\boldsymbol{x}\in\text{Int($\mathcal{C}$)}$ ,

(36) $\begin{equation*} K_\mathrm{rcbf}=\{\boldsymbol{u}\in U:L_fB(\boldsymbol{x})+L_gB(\boldsymbol{x})\boldsymbol{u}-{\alpha_3(h(\boldsymbol{x}))}\leq0\}. \quad\quad\quad(9.29)\nonumber \end{equation*}$

When determining the control, if we choose the controller from the set $K_\mathrm{rcbf}$ , it will allow us to guarantee the forward invariance of $\mathcal{C}$ . This can be formally stated in the following corollary, which is a direct application of theorem 9.1, [9].

Corollary 9.1

Consider a set $\mathcal{C}\subset\mathbb{R}^n$ be defined by Equation (9.4) adn (9.5) and let $B$ be an associated RCBF for the system of the form in Equation (9.1). Then any locally lipschitz continuous controller $\boldsymbol{u}: \text{Int($\mathcal{C}$)}\rightarrow U$ such that $\boldsymbol{u}(\boldsymbol{x})\in K_\mathrm{rcbf}$ will render asymptotical the set $\text{Int($\mathcal{C}$)}$ forward invariant.

Similarly, based on the idea of ZBF (definition 9.5), we can have the definition of corresponding CBF.

Definition 9.7

Definition 9.7. Given a set $\mathcal{C}\subset D\subset \mathbb{R}^n$ defined by Equation (9.4) and (9.5) for a continuously differentiable function $h:D\rightarrow\mathbb{R}$ , then $h$ is a zeroing control barrier function (ZCBF) if there exists an extended class $\mathcal{K}_\infty$ function α such that for the control system Equation (9.1):

(37) $\begin{equation*} \underset{\boldsymbol{u}\in U}{\text{sup}}[L_fh(\boldsymbol{x})+L_gh(\boldsymbol{x})\boldsymbol{u}]\geq -\alpha(h(\boldsymbol{x})), \quad\quad\quad(9.30)\nonumber \end{equation*}$

for all $\boldsymbol{x}\in X$ .

Note that one of the major differences between ZBF and RBF is that RBF focuses on the forward invariance of the interior of safe set $\text{Int\mathcal{C}}$ , while ZBF focuses on the forward invariance of the entire safe set, $\mathcal{C}$ . This is also true for ZCBF and RCBF.

Guaranteed Safety via ZCBFs. Therefore, similar as Equation (9.29), one can define the set that contains all the possible controllers that satisfy definition 9.7. For a given ZCBF $B$ ,

(38) $\begin{equation*} K_\mathrm{zcbf}(\boldsymbol{x})=\{\boldsymbol{u}\in U:L_fh(\boldsymbol{x})+L_gh(\boldsymbol{x})\boldsymbol{u}+\alpha(h(\boldsymbol{x}))\geq0\} \quad\quad\quad(9.31)\nonumber \end{equation*}$

Similar to corollary 9.1, the following result guarantees the forward invariance of $\mathcal{C}$ , [4].

Corollary 9.2

Let $\mathcal{C}\subset\mathbb{R}^n$ be a set defined as the superlevel set of a continuously differentialble function $h:D\subset \mathbb{R}^n\rightarrow\mathbb{R}$ . If $h$ is a control barrier function on $D$ and $\frac{\partial h}{\partial \boldsymbol{x}}(\boldsymbol{x})\neq0$ for all $\boldsymbol{x}\in\partial\mathcal{C}$ , then any Lipschitz continuous controller $\boldsymbol{u}(\boldsymbol{x})\in K_{zcbf}(\boldsymbol{x})$ for the system Equation (9.1) renders the set $\mathcal{C}$ safe. Additionally, the set $\mathcal{C}$ is asymptotically stable in $D$ .

Corollary 9.2 states that the safe set is not only forward invariant, but also asymptotically stable under the controller $\boldsymbol{u}(\boldsymbol{x})\in K_{zcbf}(\boldsymbol{x})$ . This means in the practical applications, system noise and uncertainties may force the system to enter the unsafe set. However, according to corollary 9.2, the controller is able to drive the system back to the safe set.

Necessity for Safety. Next, we will provide the lemma and theorem showing the necessity for barrier functions to safe set. The necessity for CBF will be a direct extension.

Lemma 9.2

Consider the dynamical system $\dot{\boldsymbol{x}}=f(\boldsymbol{x})$ and a nonempty, compact set $\mathcal{C}$ defined by Equation (9.4) and (9.5) for a continuously differentiable function $h$ . If $\dot{h}(\boldsymbol{x})>0$ for all $\boldsymbol{x}\in \partial\mathcal{C}$ , then for each integer $k\geq1$ , there exists a constant $\gamma>0$ such that

(39) $\begin{equation*} \dot{h}(\boldsymbol{x})\geq -\gamma h^k(\boldsymbol{x}), \forall\boldsymbol{x}\in\text{Int($\mathcal{C}$)}. \quad\quad\quad(9.32)\nonumber \end{equation*}$

Theorem 9.2

Under the assumptions of lemma 9.2, $B=\frac{1}{h}:\text{Int($\mathcal{C}$)}\rightarrow\mathbb{R}$ is a RBF and $h:\mathcal{C}\rightarrow \mathbb{R}$ is a ZBF for $\mathcal{C}$ .

Proof

Let k = 3 in lemma 9.2. Then there exists $\gamma_1>0$ such that for all $\boldsymbol{x}\in\text{Int($\mathcal{C}$)}$ , $\dot{h}\geq -\gamma_1h^3$ holds, which implies that $-\frac{\dot{h}}{h^2}\leq\gamma_1h$ holds, or equivalently, $\dot{B}\leq \frac{\gamma_1}{B}$ holds. By definition 9.3, $B=\frac{1}{h}$ is an RBF for $\mathcal{C}$ .

The next corollary extends theorem 9.2 to CBF.

Corollary 9.3

Let $\mathcal{C}$ be a compact set that is the superlevel set of a continuously differentiable function $h:D\rightarrow\mathbb{R}$ with the property that $\frac{\partial h}{\partial\boldsymbol{x}}(\boldsymbol{x})\neq0$ for all $\boldsymbol{x}\in\partial\mathcal{C}$ . If there exists a control law $\boldsymbol{u}=k(\boldsymbol{x})$ that renders $\mathcal{C}$ safe, i.e., $\mathcal{C}$ is forward invariant with respect to $\dot{\boldsymbol{x}}=f(\boldsymbol{x})+g(\boldsymbol{x})k(\boldsymbol{x})$ , then $h|_{\mathcal{C}}:\mathcal{C}\rightarrow\mathbb{R}$ is a control barrier function on $\mathcal{C}$ .

9.3.5 Applications

Now that we have introduced the concepts and theorems of CBFs, we can finally start to discuss how CBF can be applied in the robotics field. In this part, we will introduce two optimization-based techniques that are used in robotics. Furthermore, we will briefly discuss some practical applications in advanced manufacturing.

Quadratic Optimization-Based CBF Control. As discussed in section 9.3.4, if we choose the controller from sets like Equation (9.31) and (9.29), we can guarantee that the set $\mathcal{C}$ is forward invariant, i.e., safe. However, in most cases, there will be multiple (probably infinite many) feasible solutions for the controller that satisfy the CBF condition, in this case, we need to determine the “best” safe controller. Given the system Equation (9.1), suppose the desired controller without safety constraint is $\boldsymbol{u}=k(\boldsymbol{x})$ . To ensure safety, we need the controller to satisfy Equation (9.29) or Equation (9.31), and in the mean time, we want to make minimal changes to $k(\boldsymbol{x})$ . In this case, we define “best” as minimal 2-norm. This idea leads to the following quadratic programming (QP) problem (using ZCBF, RCBF is similar):

(40) $\begin{align*} \boldsymbol{u}(\boldsymbol{x})=& \underset{\boldsymbol{u}\in U\subset\mathbb{R}^{m}}{\text{argmin}} & & \frac{1}{2}||\boldsymbol{u}-k(\boldsymbol{x})||^2\quad\quad\quad (\text{ZCBF QP}) \nonumber\\ & \text{s.t.} & & L_fh(\boldsymbol{x})+L_gh(\boldsymbol{x})\boldsymbol{u}\geq-\alpha(h(\boldsymbol{x}))\nonumber \quad\quad\quad(9.33)\nonumber\\ \end{align*}$

CLF-CBF-Based Safety-Critical Control. As discussed in previous sections, CLF and CBF share very similar logic. Therefore, it is not a surprise that CBF can be added as an additional constraint in CLF-based control. In this way, one can achieve state feedback stablizatoin (or trajectory tracking) and meanwhile guarantee safety.

(41) $\begin{align*} \boldsymbol{u}(\boldsymbol{x})=& \underset{(\boldsymbol{u},\delta)\in\mathbb{R}^{m+1}}{\text{argmin}} & & \frac{1}{2}\boldsymbol{u}^TH(\boldsymbol{x})\boldsymbol{u}+p\delta^2\quad\quad\quad (\text{CLF-CBF QP})\\ & \text{s.t.} & & L_fV(\boldsymbol{x})+L_gV(\boldsymbol{x})\boldsymbol{u}\leq-\gamma(V(\boldsymbol{x}))+\delta\\ &&& L_fh(\boldsymbol{x})+L_gh(\boldsymbol{x})\boldsymbol{u}\geq-\alpha(h(\boldsymbol{x})) \quad\quad\quad(9.34)\nonumber\\ \end{align*}$

where $H(\boldsymbol{x})$ is the weight matrix, and $\delta$ is a relaxation variable ensures the feasibility of the QP as penalized by $p>0$ .

Applications in Advanced Manufacturing and Robotics. One of the applications using CBF is adaptive cruise control for vehicles and mobile robots. By combining CLF and CBF, the system can track the desired velocity and in the mean time, guarantee the safety. In this context, safety can have different meanings, depend on the design purpose. For example, safety can mean “keep a safe distance from the car in front of you”, or “detour to avoid unexpected obstacles in the way”.

Another very insteresting application is ensuring safe walking for bipedal robots with precise footstep placement [5]. Footed robots have shown potentials over wheeled robots in many areas, such as navigation or searching in uneven terrain. In discontinued, unevern terrain (see fig. 9.4), only using CLF-based control with fixed trajectories cannot handle the task. Therefore, CLF-CBF-QP is introduced to guarantee precise foot placement for each step. It is worth noting that to define the foot placement in the safe set, [5] uses the method shown in fig. 9.5. The safe area for the next foot placement is displayed as bold red line in the figure. For each step, two circles are generated, each intersects with the end of safe area by one point, as shown in the figure. Then the safe set is defined as the area among two circles and the floor (the blue area in the figure). Along with CLF, the bipedal robot can thus achieve safe walking over uneven terrain.

Figure 9.4: Bipedal robot walking on the discontinued, uneven terrain.

Created by Miao Yu based on information from [5].

Figure 9.5: Illustration of the safe set for foot placement.

Created by Miao Yu based on information from [5].

9.4 Hamilton-Jacobi Reachability Analysis (HJ-RA)

Hamilton-Jacobi reachability analysis is another important verification that is used to guarantee the performance and safety properties of dynamical systems [10, 11]. This section will briefly introduce the idea of backward reachable tube, backward reach-avoid tube, and how Hamilton-Jacobi equations can be used to determine the optimal controller under the worst-case disturbance.

9.4.1 Backward Reachable Tube (BRT)

First, we will introduce the concept of backward reachable set (BRS). This is the set of states such that the trajectories that start from this set can reach some given target set (see fig. 9.6). Here, if we define the target set to contain unsafe states, then the BRS is the set that the initial states should try to avoid.

Figure 9.6: Target set and backward reachable set. Trajectories starts from backward reachable set will eventually enter target set which contains the unsafe states.

Created by Miao Yu based on information from [10].

With the presence of the disturbance, we can treat the problem as a “Two Player Game” with Player 1 and Player 2 being the control input and system disturbance, respectively. Next, denote $\mathcal{G}_0$ as the target set wchich consists of the states that is known to be unsafe. Then, the role of Player 1 is to try to steer away from the target set with her input, while the role of Player 2 is to try to steer the system toward the target set with her input. Therefore, the BRS can be computed as

(42) $\begin{equation*} \mathcal{G}(t)=\{\boldsymbol{x}:\exists\boldsymbol{\gamma}\in\boldsymbol{\Gamma}(t),\forall \boldsymbol{u}\in U, \zeta(0;\boldsymbol{x},t,\boldsymbol{u}(\cdot),\boldsymbol{\gamma}[\boldsymbol{u}](\cdot))\in\mathcal{G}_0\},\,(9.35)\nonumber \end{equation*}$

where $\boldsymbol{\Gamma}[\cdot]$ denotes the feasible set of strategies for Player 2.

In reachability analysis, we assume Player 2 uses only non-anticipative strategies [10], i.e., Player 2 cannot have different responses to two Player one controls until they are different, which can be mathematically defined as

(43) $\begin{equation*} \boldsymbol{\gamma}\in\boldsymbol{\Gamma}(t):=\{\mathcal{N}:U(t)\rightarrow D(t):\boldsymbol{u}(r)=\hat{\boldsymbol{u}}(r)\quad a.e. \quad r\in[t,s]\Rightarrow\mathcal{N}[\boldsymbol{u}](r)=\mathcal{N}[\hat{\boldsymbol{u}}](r) \quad a.e. \quad r\in[t,s]\}.\,(9.36)\nonumber \end{equation*}$

Additionally, Player 2 has the advantage of factoring Player 1’s choice of input and responding accordingly (instantaneous informational advantage). Therefore, the setup allows us to consider the problem in a worst-case scenario as Player 2 will respond to Player 1’s choice and try her best to steer the system to the target set (unsafe).

Equation (9.35) only gives “game of kind” instead of “game of degree” results, i.e., Equation (9.35) can only be used to determine whether or not the system states reaches the target set. Fortunately, we can use level set method to transform these games into games of degree. But before we introduce level set approach, we first need to introduce Hamilton-Jacobian Isaacs (HJI) that can be used to determine the optimal control from a cost function.

In many differential game problems as well as optimal control applications, the objective is to optimize (minimize or maximize) a cost function over the system trajectories and the final state. If we define $J_t(\boldsymbol{x},\boldsymbol{u}(\cdot),\boldsymbol{d}(\cdot))$ as the cost function during $[t,0]$ , then we can write it as

(44) $\begin{equation*} J_t(\boldsymbol{x},\boldsymbol{u}(\cdot),\boldsymbol{d}(\cdot)) = \int_t^0 c(\boldsymbol{x}(s),\boldsymbol{u}(s),\boldsymbol{d}(s),s)\,ds+q(\boldsymbol{x}(0)),\quad\quad\quad(9.37)\nonumber \end{equation*}$

where $c(\boldsymbol{x}(s),\boldsymbol{u}(s),\boldsymbol{d}(s),s)$ denotes the cost at each time instant $s$ , and $q(\boldsymbol{x}(0))$ denotes the terminal cost. Then, in the “Two Player Game” setup, Player 1 will attemp to maximize the cost (steer away the target set) while Player 2 will attemp to minimize the cost (steer towards the target set). Therefore, under the assumptions to Player 1 and Player 2, the so-called “lower value” of the game can be written as

(45) $\begin{equation*} G(t,\boldsymbol{x})=\underset{\boldsymbol{\gamma}\in\Gamma(t)}{\inf} \underset{\boldsymbol{u}(\cdot)\in U}{\sup} J_t(\boldsymbol{x},\boldsymbol{u}(\cdot),\boldsymbol{d}(\cdot)),\quad\quad\quad(9.38)\nonumber \end{equation*}$

where $\Gamma(\cdot)$ is defined in Equation (9.36). In Equation (9.38), $\inf\sup$ is used because we assume that Player 2 has the information of Player 1’s input choice and can respond accordingly.

From [12], Equation (9.38) is the viscosity solution of the HJI PDE

(46) $\begin{equation*} D_tG(t,\boldsymbol{x})+H(t,\boldsymbol{x},\nabla G(t,\boldsymbol{x}))=0,\quad G(0,\boldsymbol{x})=q(\boldsymbol{x}),\quad\quad\quad(9.39)\nonumber \end{equation*}$

where $H(t,\boldsymbol{x},\nabla G(t,\boldsymbol{x}))$ is the Hamiltonian which is defined as

(47) $\begin{equation*} H(t,\boldsymbol{x},\boldsymbol{\lambda})=\underset{\boldsymbol{u}\in U}{\max}\,\underset{\boldsymbol{d}\in D}{\min}\,c(\boldsymbol{x},\boldsymbol{u},\boldsymbol{d},t)+\boldsymbol{\lambda}\cdot f(\boldsymbol{x},\boldsymbol{u},\boldsymbol{d}),\quad\quad\quad(9.40)\nonumber \end{equation*}$

where $\boldsymbol{\lambda}$ is $\nabla G(t,\boldsymbol{x})$ and is called costate. Therefore, the optimal control for Player 1 can be obtained as [10]

(48) $\begin{equation*} u^*(t,\boldsymbol{x})=\arg\,\underset{\boldsymbol{u}\in U}{\max}\,\underset{\boldsymbol{d}\in D}{\min}\,c(\boldsymbol{x},\boldsymbol{u},\boldsymbol{d},t)+\boldsymbol{\lambda}\cdot f(\boldsymbol{x},\boldsymbol{u},\boldsymbol{d}).\quad\quad\quad(9.41)\nonumber \end{equation*}$

For more details, please see [12].

Going back to BRS in Equation (9.35), to convert it from “game of kind” to “game of degree”, it is always possible to define a Lipschitz function $g(\boldsymbol{x})$ based on the level set method such that the target set $\mathcal{G}_0$ is the zero sublevel set of $g$ , i.e., we can have a Lipschitz function $g(\boldsymbol{x})$ such that $\boldsymbol{x}\in\mathcal{G}_0\iff g(\boldsymbol{x})\leq0$ . If we define the cost function as

(49) $\begin{equation*} J_t(\boldsymbol{x},\boldsymbol{u}(\cdot),\boldsymbol{d}(\cdot))=g(\boldsymbol{x}(0)),\quad\quad\quad(9.42)\nonumber \end{equation*}$

then the system reaching the target set under control $\boldsymbol{u}$ and disturbance $\boldsymbol{d}$ is equivalent to $J_t(\boldsymbol{x},\boldsymbol{u}(\cdot),\boldsymbol{d}(\cdot))\leq0$ . Because, as discussed ealier, Player 1 wants to maximize the cost function, while Player 2 wants to minimize the cost function, and Player 2 has the instantaneous informational advantage, we can compute BRS using HBJ. Based on the discussion above, BRS can be defined as

(50) $\begin{equation*} \mathcal{G}(t)=\{\boldsymbol{x}: G(t,\boldsymbol{x})\leq0\},\quad\quad\quad(9.43)\nonumber \end{equation*}$

where $G(t,\boldsymbol{x})$ comes from Equation (9.38) and satisfies

(51) $\begin{equation*} D_tG(t,\boldsymbol{x})+H(t,\boldsymbol{x},\nabla G(t,\boldsymbol{x}))=0,\quad G(0,\boldsymbol{x})=g(\boldsymbol{x}),\quad\quad\quad(9.44)\nonumber \end{equation*}$

where the Hamiltonian is

(52) $\begin{equation*} H(t,\boldsymbol{x},\boldsymbol{\lambda})=\underset{\boldsymbol{u}\in U}{\max}\,\underset{\boldsymbol{d}\in D}{\min}\,c(\boldsymbol{x},\boldsymbol{u},\boldsymbol{d},t)+\boldsymbol{\lambda}\cdot f(\boldsymbol{x},\boldsymbol{u},\boldsymbol{d}),\quad\quad\quad(9.45)\nonumber \end{equation*}$

and the optimal control for Player 1 is

(53) $\begin{equation*} u^*(t,\boldsymbol{x})=\arg\,\underset{\boldsymbol{u}\in U}{\max}\,\underset{\boldsymbol{d}\in D}{\min}\,c(\boldsymbol{x},\boldsymbol{u},\boldsymbol{d},t)+\boldsymbol{\lambda}\cdot f(\boldsymbol{x},\boldsymbol{u},\boldsymbol{d}).\quad\quad\quad(9.46)\nonumber \end{equation*}$

To summarize, if $\boldsymbol{x}(t)\in\mathcal{G}^C$ , then Player 1 can steer the system to avoid the target set, dispite Player 2’s input. Therefore, the reachability analysis provides a safety certificate as well as an optimal control solution with the presence of the system disturbance, which is one of the advantage of reachability analysis over control barrier functions.

Up till now, the BRS we discussed only cares about the states that starts exactly at time 0 and ends at time $t$ . Usually, a more practical problem is to check if a state can reach the target set within time duration $|t|$ . This brings us the backward reachable tube (BRT), which can be similarly defined as [10]

(54) $\begin{equation*} \mathcal{G}(t)=\{\boldsymbol{x}:\exists\boldsymbol{\gamma}\in\boldsymbol{\Gamma}(t),\forall \boldsymbol{u}\in U, \exists s\in [t,0], \,\zeta(s;\boldsymbol{x},t,\boldsymbol{u}(\cdot),\boldsymbol{\gamma}[\boldsymbol{u}](\cdot))\in\mathcal{G}_0\}.\quad\quad\quad(9.47)\nonumber \end{equation*}$

9.4.2 Backward Reach-Avoid Tube (BRAT)

Similar to BRT, we can define another set that contains the initials states which have a control sequence that can drive the system towards the target set $\mathcal{G}$ (safe), dispite the worst-case disturbance, and in the mean time, avoid entering the unsafe set $\mathcal{F}$ . This can be formally expressed as, [11]

(55) $\begin{equation*} \mathcal{R}_{t_0}^{BRAT}(\mathcal{G},\mathcal{F}) = \{\boldsymbol{x}_0\in\mathcal{X}:\, \exists\boldsymbol{u}(\cdot),\forall\boldsymbol{d}(\cdot),\exists s\in[t_0,T], \zeta_{\boldsymbol{x}_0,t_0}^{\boldsymbol{u,d}}(s)\in\mathcal{G},\,\forall t\in[t_0,s],\zeta_{\boldsymbol{x}_0,t_0}^{\boldsymbol{u,d}}(t)\notin\mathcal{F}\}\,(9.48)\nonumber \end{equation*}$

where similar as level set method in section 9.4.1, we can define the target set as $\mathcal{G}=\{\boldsymbol{x}:l(\boldsymbol{x})\leq0\}$ , and unsafe set as $\mathcal{F}=\{\boldsymbol{x}:g(\boldsymbol{x})\geq0\}$ . Therefore, using the HJI, the optimal control law is given by

(56) $\begin{equation*} \boldsymbol{u}^*_{BRAT}(\boldsymbol{x},t)=\underset{\boldsymbol{u}}{\text{argmin}}\,\underset{\boldsymbol{d}}{\text{sup}}\,D_x\tilde{\mathcal{G}}(\boldsymbol{x},t)\cdot \boldsymbol{f}(\boldsymbol{x,u,d}),\quad\quad\quad(9.49)\nonumber \end{equation*}$

where $\tilde{\mathcal{G}}(\boldsymbol{x},t)$ is the value function

(57) $\begin{equation*} \tilde{\mathcal{G}}(\boldsymbol{x},t) = \underset{\boldsymbol{u}}{\inf}\,\underset{\boldsymbol{d}}{\sup}\,\underset{t\in[t_0,T]}{\min}\max\{l(\zeta_{\boldsymbol{x},t_0}^{\boldsymbol{u,d}}(t)),\,\underset{s\in[t_0,t]}{\text{max}}\,g(\zeta_{\boldsymbol{x},t_0}^{\boldsymbol{u,d}}(s))\},\quad\quad\quad(9.50)\nonumber \end{equation*}$

where the Hamiltonian is

(58) $\begin{equation*} \tilde{H}(\boldsymbol{x},t)=\underset{\boldsymbol{u}}{\min}\,\underset{\boldsymbol{d}}{\text{sup}}\,D_x\tilde{\mathcal{G}}(\boldsymbol{x},t)\cdot \boldsymbol{f}(\boldsymbol{x,u,d}).\quad\quad\quad(9.51)\nonumber \end{equation*}$

Finally, the BRAT-based controller can be defined as

(59) $\begin{equation*} \pi_{BRAT}(t,\boldsymbol{x}) = \begin{cases} \boldsymbol{u}_{ref}, &\text{if $\tilde{\mathcal{G}}(\boldsymbol{x},t)\geq \epsilon_{BRAT}$,}\\ \boldsymbol{u}^*_{BRAT}(\boldsymbol{x},t), &\text{otherwise.} \end{cases}\quad\quad\quad(9.52)\nonumber \end{equation*}$

where $\boldsymbol{u}_{ref}$ denotes the original desired controller, and $\epsilon_{BRAT}$ is a small positive number provides the safety threshold.

9.4.3 Applications and Comparison to CBF

Now that we have introduced both CBF and reachability analysis, it is time to discuss their pros and cons, as well as the applications based on these properties. Note that this part will only discuss the advantages and limitations based on the classical problem setups. Given the rapid growth in safety-critical control area, researchers have been developing new techniques that aim to overcome the limitations of each method.

One of the major advantages of reachability analysis over CBF is that in the problem setup, HJ reachability analysis considers the presence of the disturbance or model uncertainty. Additionally, HJ reachability analysis treat the role of control input and disturbance as pursuer and evadar in a “two-player game”. Therefore, by finding the optimal control for HJ reachability analysis problem, one can find the optimal solution considering the worst-case scenario. The existence of the solution to the HJ reachability analysis indicates the existence of a control sequence that guarantees the system safety, dispite system disturbance. However, classical CBF does not consider the effect of the disturbance and could practically lead to unsafe configurations.

Another major difference between two methods is the computation complexity. Since HJ reachability analysis solves the PDE over grided state space using Dynamic programming, [11], its computation complexity increases expenontially with the number of system states. Therefore, classical HJ reachability analysis is usually stricted in applications with 4 or 5 states. Furthermore, due to the computation burden, HJ reachability analysis usually uses offline pre-computation. On the other hand, CBF converts the safety constraint into a simple linear constraint and thus can be easily solved by quadratic solver. Therefore, for applications that requires fast respond, e.g. walking robots, adaptive cruise control, etc, CBF is more applicable. Although HJ reachability analysis has the “curse of complexity”, researchers have developed various techniques, such as warm start and locally value function update technique, that significatly improves computation efficienty. These make HJ reachability analysis also applicable in some applications such as fast and safe tracking for motion planning, and unmanned aerial system traffic management, [10].

Last but not the least, developing a CBF might be challenging. There is no systematic way to construct a CBF for a system that is not feedback-linearizable [4]. Also, feasibility is another challenge in CBF based methods.

9.5 Summary

With the rapid development of the robotics, more and more applications are introduced in advanced manufacturing and daily life. This makes the safety bring more attention as autonomous are expected to operate in unkown environment, which makes it much harder to guarantee safety using conventional control methods. In this chapter, we discussed the concept of safety, and majorly introduced two commonly used safety-critical control methods, CBF and HJ reachability analysis. The chapter mainly focused on the problem derivation of two methods. Comparison between two methods and their applications were also briefly discusses. Finally, we will provide practice questions and simulation demostration using CBF techniques.

Practice Questions

Problem 1

What are the advantages and disadvantages of control barrier functions?

Advantages. Control barrier functions converts the safety constraint into a simple linear constraint and thus can be easily solved by quadratic solver. Therefore, CBF is capable of handling the applica-tions that require fast computation.

Disadvantages. There is no systematic way to construct a CBF for a system that is not feedback-linearizable. Besides, there might be no feasible solutions for a given CBF.

Problem 2

What are the advantages and disadvantages of reachability analysis?

Advantages. Reachability analysis considers the nonlinear problems with disturbance. Therefore, with proper definition of the disturbance, reachability analysis can provide the optimal controller that handles the worst-case scenario.

Disadvantages. The computation complexity increases exponentially with the number of states in reachability analysis. Therefore, the conventional reachability analysis can only be used in the nonlinear systems that has a maximum of four or five states.

Problem 3

Figure 9.7 is a 2D manipulator. Suppose that we want to steer the end-effector from the current position to the target (green dot in the figure). Since control is not the major topic in this chapter, we assume that the manipulator uses velocity control, i.e.,

(60) $\begin{align*} \dot{\theta}_1&=u_1,\\ \dot{\theta}_2&=u_2.\nonumber \end{align*}$

For now, we do not care about the obstacle.

Created by Miao Yu

First, we need to compute the inverse kinematic for the configuration that the end-effector reaches the target. By assigning $x=x_t,\,y=y_t$ , using the geometric method, we can write

(61) $\begin{align*} \theta_2^{des}&=\arccos{(-\frac{a_1^2+a_2^2-(x_t^2+y_t^2)}{2a_1a_2})},\\ \theta_1^{des}&= \arccos{(\frac{a_1^2+(x_t^2+y_t^2)-a_2^2}{2a_1\sqrt{x_t^2+y_t^2}})}+atan2(y_t,x_t)\nonumber \end{align*}$

Here, $des$ indicates the desired angle for each joint. Usually, there will be two solutions for this manipulator if the target is in the interior of the workspace. However, given the position of the target point, we need the configuration that stays above the floor. Thus, there is only one solution.

To steer the end-effector to the target, we can first define the error state $\boldsymbol{e}=[e_1,e_2]^T$ where

(62) $\begin{equation*} e_1=\theta_1^{des}-\theta_1,\,e_2=\theta_2^{des}-\theta_2.\nonumber \end{equation*}$

and according to the system defined in the problem statement, we have

(63) $\begin{align*} \dot{e}_1&=\dot{\theta}_1=u_1,\\ \dot{e}_2&=\dot{\theta}_2=u_2.\nonumber \end{align*}$

Therefore, steering the end-effector to the target is equivalent to find a controller $\boldsymbol{u}$ such that the error $\boldsymbol{e}$ will asymptotically approach to zero. One simple controller can be

(64) $\begin{align*} u_1&=-k_1e_1,\\ u_2&=-k_2e_2.\nonumber \end{align*}$

\noindent where $k_1,k_2$ are positive numbers that controls the speed of convergence.

Problem 4

For the setup in problem 3, now we take the obstacle into consideration. Please define a safe set. To simplify the problem, we only consider the end-effector, i.e., we will allow the links to cross the obstacle if necessary.

First write the forward kinematics of the manipulator,

(65) $\begin{align*} x &= a_1\cos{\theta_1}+a_2\cos{(\theta_1+\theta_2)},\\ y &= a_1\sin{\theta_1}+a_2\sin{(\theta_1+\theta_2)}.\nonumber \end{align*}$

\noindent Then to avoid the end-effector touching the obstacle, we need to ensure the distance between $(x_o,y_o)$ and $(x,y)$ to be larger than the radius $r$ , mathematically, we want

(66) $\begin{equation*} (x-x_o)^2+(y-y_o)^2\geq r^2.\nonumber \end{equation*}$

Therefore, the safe set can be written as

(67) $\begin{equation*} \mathcal{C}=\{(x,y)^T\in\mathbb{R}^2:h(x,y)\geq0\}, \quad with\quad h(x,y)=(x-x_o)^2+(y-y_o)^2-r^2.\nonumber \end{equation*}$

One can also write the safe set in terms of the system states $\theta_1,\theta_2$ by replacing $x,y$ using forward kinematics. Additionally, the safe set $\mathcal{C}$ only guarantees that the end-effector does not move across the obstacle. Usually, a “safer” way to define the safe set is to have the end-effector stays clear to the obstacle, i.e.,

(68) $\begin{equation*} \tilde{\mathcal{C}}=\{(x,y)^T\in\mathbb{R}^2:h(x,y)\geq0\}, \quad with\quad h(x,y)=(x-x_o)^2+(y-y_o)^2-(r+\delta)^2.\nonumber \end{equation*}$

\noindent with some positive $\delta$ .

Problem 5

Use the safe set defined in problem 4, write a barrier function.

Note that there are multiple ways to define a barrier function. In this problem, we will use the reciprocal barrier function. Readers can also use zeroing barrier function introduced in this chapter as an exercise.

According to definition 9.3, we can define the barrier function as

(69) $\begin{equation*} B(\theta_1,\theta_2) = \frac{1}{h(\theta_1,\theta_2)}=\frac{1}{(a_1\cos{\theta_1}+a_2\cos{(\theta_1+\theta_2)}-x_o)^2+(a_1\sin{\theta_1}+a_2\sin{(\theta_1+\theta_2)}-y_o)^2},\nonumber \end{equation*}$

The above definition of the barrier function satisfies definition 9.3 by chosing

(70) $\begin{equation*} \alpha_1(h) = \alpha_2(h) = h,\,\alpha_3(h)=0.\nonumber \end{equation*}$

Problem 6

Write a CBF-based safety-critical control using the barrier function defined in problem 5.

Denote the desired controller as $\boldsymbol{k}(\boldsymbol{\theta})$ . We want to find a controller that guarantees the safety (collision avoidance), and in the mean time, have minimal change to $\boldsymbol{k}(\boldsymbol{\theta})$ . Thus, we have

(71) $\begin{align*} \boldsymbol{u}(\boldsymbol{\theta})=& \underset{\boldsymbol{u}\in U\subset\mathbb{R}^{2}}{\text{argmin}} & & \frac{1}{2}||\boldsymbol{u}-\boldsymbol{k}(\boldsymbol{\theta})||^2\\ & \text{s.t.} & & L_fh(\boldsymbol{\theta})+L_gh(\boldsymbol{\theta})\boldsymbol{u}\geq-\alpha(h(\boldsymbol{\theta}))\nonumber \end{align*}$

where we can choose $\alpha(h(\boldsymbol{\theta}))$ to be $h(\boldsymbol{\theta})$ and

(72) $\begin{align*} L_fh(\boldsymbol{\theta})+L_gh(\boldsymbol{\theta})\boldsymbol{u}=&0+\frac{1}{D^2}[-2(a_1c_1+a_2c_{12}-x_o)(-a_1s_1-a_2s_{12})+2(a_1s_1+a_2s_{12}-y_o)(a_1c_1+a_2c_{12})]u_1\\ &+\frac{1}{D^2}[-2(a_1c_1+a_2c_{12}-x_o)(-a_2s_{12})+2(a_1s_1+a_2s_{12}-y_o)(a_2c_{12})]u_2\\ D = &(a_1c_1+a_2c_{12}-x_o)^2+(a_1s_1+a_2s_{12}-y_o)^2,\nonumber \end{align*}$

where $c,s$ denote $\cos,\sin$ , respectively, i.e., $c_1=\cos{\theta1},s_{12}=\sin{(\theta1+\theta_2)}$ , etc.

Simulation and Animation

In this part, we extend the manipulator problem into a 3-link case, see fig. 9.8. In the simulation, we set the green dot to be the target position, and the red ball to be the obstacle that the end-effector needs to avoid.

Figure 9.8: Simulation setup, the green dot is the target position, and the red ball is the obstacle that the end-effector needs to avoid.

Created by Miao Yu

There are two cases in the simulation. The first case is the conventional control, and the second case is the conventional control with CBF as the constraint.

The simulation shows that the CBF-based safety-critical control can successfully steer the end-effector to the target point and meanwhile guarantees the collision avoidance.

References

[1] Hassan K Khalil. Nonlinear systems; 3rd ed. Prentice-Hall, Upper Saddle River, NJ, 2002.

[2] Charles Dawson, Sicun Gao, and Chuchu Fan. Safe control with learned certificates: A survey of neural lyapunov, barrier, and contraction methods. arXiv preprint arXiv:2202.11762, 2022.

[3] Hiroyasu Tsukamoto, Soon-Jo Chung, and Jean-Jaques E Slotine. Contraction theory for nonlinear stability analysis and learning-based control: A tutorial overview. Annual Reviews in Control, 52:135–169, 2021.

[4] Aaron D Ames, Samuel Coogan, Magnus Egerstedt, Gennaro Notomista, Koushil Sreenath, and Paulo Tabuada. Control barrier functions: Theory and applications. In 2019 18th European control conference (ECC), pages 3420–3431. IEEE, 2019.

[5] Quan Nguyen and Koushil Sreenath. Safety-critical control for dynamical bipedal walking with precise footstep placement. IFAC-PapersOnLine, 48(27):147–154, 2015.

[6] Mitio Nagumo. Uber die lage der integralkurven gew¨ohnlicher differentialgleichungen. Proceed-ings of the Physico-Mathematical Society of Japan. 3rd Series, 24:551–559, 1942.

[7] Stephen Prajna and Ali Jadbabaie. Safety verification of hybrid systems using barrier certificates. In HSCC, volume 2993, pages 477–492. Springer, 2004.

[8] Stephen Prajna. Barrier certificates for nonlinear model validation. Automatica, 42(1):117–126, 2006.

[9] Aaron D Ames, Xiangru Xu, Jessy W Grizzle, and Paulo Tabuada. Control barrier function based quadratic programs for safety critical systems. IEEE Transactions on Automatic Control, 62(8):3861–3876, 2016.

[10] Somil Bansal, Mo Chen, Sylvia Herbert, and Claire J Tomlin. Hamilton-jacobi reachability: A brief overview and recent advances. In 2017 IEEE 56th Annual Conference on Decision and Control (CDC), pages 2242–2253. IEEE, 2017.

[11] Zhichao Li. Comparison between safety methods control barrier function vs. reachability analysis. arXiv preprint arXiv:2106.13176, 2021.

[12] Lawrence C Evans and Panagiotis E Souganidis. Differential games and representation formulas for solutions of hamilton-jacobi-isaacs equations. Indiana University mathematics journal, 33(5):773–797, 1984.

License

Icon for the Creative Commons Attribution-NonCommercial-NoDerivatives 4.0 International License

Advanced Robotics for Manufacturing Copyright © by Emma Brandberg; Payton Engelking; Yalun Jiang; Nithesh Kumar; Ryan Mbagna-Nanko; Rahul Narasimhan; Aayush Rai; Shahil Shaik; Venkata Ravindhra Reddy Varikuti; and Miao Yu is licensed under a Creative Commons Attribution-NonCommercial-NoDerivatives 4.0 International License, except where otherwise noted.

9.1 Introduction

9.1.1 Motivation on Safety-Critical Control

9.1.2 Safety-Critical Control Techniques

9.1.3 Structure of the Chapter

9.2 Problem Formulation

9.3 Control Barrier Functions (CBF)

9.3.1 Safe Set and Forward Invariance

9.3.2 Forms of Barrier Functions

9.3.3 Control Lyapunov Functions

9.3.4 Control Barrier Functions

9.3.5 Applications

9.4 Hamilton-Jacobi Reachability Analysis (HJ-RA)

9.4.1 Backward Reachable Tube (BRT)

9.4.2 Backward Reach-Avoid Tube (BRAT)

9.4.3 Applications and Comparison to CBF

9.5 Summary

Practice Problems

Simulation & Animation

References

9.1 Introduction

9.1.1 Motivation on Safety-Critical Control

9.1.2 Safety-Critical Control Techniques

9.1.3 Structure of the Chapter

9.2 Problem Formulation

9.3 Control Barrier Functions (CBF)

9.3.1 Safe Set and Forward Invariance

9.3.2 Forms of Barrier Functions

9.3.3 Control Lyapunov Functions

9.3.4 Control Barrier Functions

9.3.5 Applications

9.4 Hamilton-Jacobi Reachability Analysis (HJ-RA)

9.4.1 Backward Reachable Tube (BRT)

9.4.2 Backward Reach-Avoid Tube (BRAT)

9.4.3 Applications and Comparison to CBF

9.5 Summary

Practice Questions

Simulation and Animation

References

License

Share This Book