text:
\section{Probabilistic Safe Control}\label{sec:prob_control}

This section presents our solution to Problem~\ref{prob: prob_clf_cbf}.
Inspired by the design~\eqref{eq:QP_origin} when the dynamics and the barrier function are known, we formulate the control synthesis problem via the following optimization problem:
\begin{align}\label{eq:CLF_CBF_QP_Gaussian}
& \min_{\underline{\bfu} \in \underline{{\cal U}},\delta \in \mathbb{R}}\,\, \|L(\mathbf{x})^\top(\underline{\bfu} - \underline{\tilde{\mathbf{k}}}(\mathbf{x}))\|^2 + \lambda \delta^2,\\
&\mathrm{s.t.} \, \,
\mathbb{P}(\textit{CLC}(\mathbf{x}, \underline{\bfu}) \leq \delta) \geq p, \quad \mathbb{P}(\textit{CBC}(\mathbf{x}, \underline{\bfu}) \geq 0) \geq p. \notag
\end{align}
The uncertainty in $F$ and $h$ affects the linearity in $\underline{\bfu}$ of the CLC and CBC conditions in the constraints of~\eqref{eq:CLF_CBF_QP_Gaussian}, making this optimization problem no longer a QP. Here, we justify that nevertheless the optimization can be solved efficiently. To show this, we start by analyzing the distributions of $\textit{CBC}(\mathbf{x},\underline{\bfu})$ and $\textit{CLC}(\mathbf{x},\underline{\bfu})$ in detail.

\begin{proposition}[\textbf{Mean and Variance for CBC}]\label{prop:cbc_mean_variance}
Assume $h$ is a CBF with a linear function $\alpha_h$, i.e., $\alpha_h(z) = a \cdot z$ for $a \in \mathbb{R}_{\ge 0}$. Given independent distributions $h(\mathbf{x}) \sim \mathcal{GP}(\tilde{h}(\mathbf{x}),K_h(\mathbf{x},\mathbf{x}'))$ and $\text{vec}(F(\mathbf{x})) \sim \mathcal{GP}(\text{vec}(\tilde{F}(\mathbf{x})), K_{F}(\mathbf{x},\mathbf{x}'))$, the mean and variance of $\textit{CBC}(\mathbf{x},\underline{\bfu})$ satisfy
\begin{subequations}\label{eq: cbc_mean_variance}
\begin{align}
  \label{eq:cbc_mean}
    &\mathbb{E}[\textit{CBC}(\mathbf{x},\underline{\bfu})] = \mathbb{E}[\mathbf{p}(\mathbf{x})]^\top\underline{\bfu}
    \\
    &\textit{Var}[\textit{CBC}(\mathbf{x},\underline{\bfu})] = \underline{\bfu}^\top \textit{Var}[\mathbf{p}(\mathbf{x})]\underline{\bfu},     \label{eq:cbc_var}
\end{align}
\end{subequations}
where $\mathbf{p}(\mathbf{x}) := F^\top(\mathbf{x})[\nabla_{\mathbf{x}} h(\mathbf{x})]+  \left[a h(\mathbf{x}) \quad \boldsymbol{0}_m^\top \right]^\top \in \mathbb{R}^{m+1}$
and $\mathbb{E}[\mathbf{p}(\mathbf{x})]$, $ \textit{Var}[\mathbf{p}(\mathbf{x})]$ are computed in \eqref{eq:covariance_Px}.
\end{proposition}
\begin{proof}
The control barrier condition can be written as:
\begin{align*}
    \textit{CBC}(\mathbf{x},\underline{\bfu}) &= [\nabla_{\mathbf{x}} h(\mathbf{x})]^\top f(\mathbf{x}) + [\nabla_{\mathbf{x}} h(\mathbf{x})]^\top g(\mathbf{x})\mathbf{u} + a h(\mathbf{x})
    \notag
    \\
     &= \big[ [\nabla_{\mathbf{x}} h(\mathbf{x})]^\top F(\mathbf{x}) \!+\! \left[a h(\mathbf{x}) \; \boldsymbol{0}_m^\top \right] \big ] \underline{\bfu} = \mathbf{p}(\mathbf{x})^\top \underline{\bfu} .

\end{align*}
Note that $\nabla_{\mathbf{x}} h(\mathbf{x})$ is a GP because the gradient of a GP with differentiable mean function and twice-differentiable covariance function is also a GP,
cf.~\cite[Lemma 6]{dhiman2020control},
\begin{equation*}
\begin{aligned}
    \nabla_{\mathbf{x}} h(\mathbf{x}) \sim
    \mathcal{GP}(\nabla_{\mathbf{x}} \tilde{h}(\mathbf{x}),\mathcal{H}_{\mathbf{x},\mathbf{x}'}K_h(\mathbf{x},\mathbf{x}')),
\end{aligned}
\end{equation*}
where $\mathcal{H}_{\mathbf{x},\mathbf{x}'}K_h(\mathbf{x},\mathbf{x}') = \left[ \frac{\partial^2 K_h(\mathbf{x},\mathbf{x}')}{\partial\mathbf{x}_i,\partial\mathbf{x}_j'} \right]_{i=1,j=1}^{n,n}$ is finite for all $(\mathbf{x},\mathbf{x}') \in \mathbb{R}^{2n}$. Since $\text{vec}(\mathbf{A}\mathbf{B}\mathbf{C}) = (\mathbf{C}^\top \otimes \mathbf{A})\text{vec}(\mathbf{B})$ for appropriately sized matrices $\mathbf{A}$, $\mathbf{B}$, $\mathbf{C}$, we can write
\begin{equation}
\label{eq:kronecker_trick}
\begin{aligned}
\textit{Var}(F(\mathbf{x}) \underline{\bfu})
&= \textit{Var}((\underline{\bfu}^\top \otimes \mathbf{I}_{n})\text{vec}(F(\mathbf{x})))\\
&= (\underline{\bfu}^\top \otimes \mathbf{I}_{n})K_F(\mathbf{x}, \mathbf{x}) (\underline{\bfu} \otimes \mathbf{I}_{n}) .
\end{aligned}
\end{equation}
For brevity, we let $K_F := K_F(\mathbf{x},\mathbf{x}')$ and $K_h := K_h(\mathbf{x}, \mathbf{x}')$ and $\mathbf{p}_1 = F^\top(\mathbf{x})[\nabla_\mathbf{x} h(\mathbf{x})]$. The term $[\nabla_{\mathbf{x}} h(\mathbf{x})]^\top F(\mathbf{x})\underline{\bfu}$ is an inner product of two independent GPs, $\nabla_{\mathbf{x}} h(\mathbf{x})$ and $F(\mathbf{x})\underline{\bfu}$.
Thus, using \cite[Lemma 5]{dhiman2020control}, \eqref{eq:kronecker_trick}, and that $\textit{Cov}(\nabla_\mathbf{x} h(\mathbf{x}), F(\mathbf{x})\underline{\bfu}) = 0$, $\mathbf{p}_1^\top\underline{\bfu}$ corresponds to a distribution with mean and variance:
\begin{equation}
\label{eq:part_1_mean_variance}
\begin{aligned}
&\mathbb{E}[\mathbf{p}_1^\top \underline{\bfu}] = [\nabla_{\mathbf{x}} \tilde{h}(\mathbf{x})]^\top\tilde{F}(\mathbf{x})\underline{\bfu} , \\
&\textit{Var}[\mathbf{p}_1^\top\underline{\bfu}] =
 [\nabla_{\mathbf{x}} \tilde{h}(\mathbf{x})]^\top(\underline{\bfu}^\top \otimes \mathbf{I}_n)K_{F}
  \\&\qquad
(\underline{\bfu} \otimes \mathbf{I}_n) \nabla_{\mathbf{x}} \tilde{h}(\mathbf{x}) +
 \underline{\bfu}^\top\tilde{F}^\top(\mathbf{x})\mathcal{H}_{\mathbf{x},\mathbf{x}'} K_h \tilde{F}(\mathbf{x}) \underline{\bfu}.
\end{aligned}
\end{equation}
To factorize $\underline{\bfu}$ from the variance expression, we apply
the property $(\mathbf{A} \otimes \mathbf{B})(\mathbf{C} \otimes \mathbf{D}) = \mathbf{A}\mathbf{C} \otimes \mathbf{B}\mathbf{D}$ two times
\begin{equation}
\label{eq:swapping-gradh-u}
\begin{aligned}
    &(\underline{\bfu} \otimes \mathbf{I}_n)[\nabla_\mathbf{x} \tilde{h}(\mathbf{x})] =
     (\underline{\bfu} \otimes \mathbf{I}_n)(1 \otimes [\nabla_\mathbf{x} \tilde{h}(\mathbf{x})])
     \\&\quad= \underline{\bfu} \otimes \nabla_\mathbf{x} \tilde{h}(\mathbf{x})

    = (\mathbf{I}_{m+1} \otimes \nabla_\mathbf{x} \tilde{h}(\mathbf{x})) \underline{\bfu}.
\end{aligned}
\end{equation}
By substituting \eqref{eq:swapping-gradh-u} in \eqref{eq:part_1_mean_variance}, we can factorize out $\underline{\bfu}$ to get,
\begin{align}
\label{eq:p1-var}
\textit{Var}[\mathbf{p}_1]
 &=
 (\mathbf{I}_{m+1} \otimes [\nabla_\mathbf{x} \tilde{h}(\mathbf{x})]^\top)K_{F}
 (\mathbf{I}_{m+1} \otimes \nabla_\mathbf{x} \tilde{h}(\mathbf{x}))
 \notag\\ &\quad
 +  \tilde{F}^\top(\mathbf{x})\mathcal{H}_{\mathbf{x},\mathbf{x}'} K_h \tilde{F}(\mathbf{x})  .
\end{align}
Next, we write $\textit{Cov}(h(\mathbf{x}), \mathbf{p}_1^\top\underline{\bfu})$ using \cite[Lemma 5]{dhiman2020control} and $\textit{Cov}(h(\mathbf{x}), F(\mathbf{x})\underline{\bfu}) = 0$,
\begin{align}
\label{eq:p1-h-cov}
\textit{Cov}(&h(\mathbf{x}), \mathbf{p}_1^\top\underline{\bfu}) =
\textit{Cov}(h(\mathbf{x}),  \nabla_\mathbf{x} h(\mathbf{x}))\tilde{F}(\mathbf{x})\underline{\bfu}
\notag \\ &\quad
= \big[[\nabla_\mathbf{x} K_h]^\top\tilde{f}(\mathbf{x}) \quad [\nabla_\mathbf{x} K_h]^\top\tilde{g}(\mathbf{x})\big]\underline{\bfu}.
\end{align}
Using \eqref{eq:part_1_mean_variance}, \eqref{eq:p1-var} and \eqref{eq:p1-h-cov}, we write the mean and variance,
\begin{align}
\label{eq:covariance_Px}
&\mathbb{E}[\mathbf{p}(\mathbf{x})] = [\nabla_\mathbf{x} \tilde{h}(\mathbf{x})]^\top \tilde{F}(\mathbf{x}) + a [\tilde{h}(\mathbf{x}) \quad \boldsymbol{0}_m^\top]^\top
 \notag\\
&\textit{Var}[\mathbf{p}(\mathbf{x})] =
\tilde{F}^\top(\mathbf{x})\mathcal{H}_{\mathbf{x},\mathbf{x}'} K_h \tilde{F}(\mathbf{x})
\notag\\ &\;
+
(\mathbf{I}_{m+1} \otimes \nabla_\mathbf{x} \tilde{h}(\mathbf{x})^\top)K_{F}
 (\mathbf{I}_{m+1} \otimes \nabla_\mathbf{x} \tilde{h}(\mathbf{x}))
\\ &\;
+
\begin{bmatrix}
a^2 K_h + 2a[\nabla_\mathbf{x} K_h]^\top \tilde{f}(\mathbf{x})& a[\nabla_\mathbf{x} K_h]^\top \tilde{g}(\mathbf{x})\\
a\tilde{g}(\mathbf{x})^\top [\nabla_\mathbf{x} K_h] &  \boldsymbol{0}_{m \times m}
\end{bmatrix} , \notag
\end{align}
from which the statement follows.
\end{proof}

Next, we describe the distribution of $\textit{CLC}(\mathbf{x},\underline{\bfu})$.

\begin{proposition}[\textbf{Gaussian distribution for CLC}]\label{prop:clc_mean_variance}
Given the distribution $\text{vec}(F(\mathbf{x})) \sim \mathcal{GP}(\text{vec}(\tilde{F}(\mathbf{x})), K_{F}(\mathbf{x},\mathbf{x}'))$, the $\textit{CLC}(\mathbf{x},\underline{\bfu})$ is Gaussian with mean and variance:
\begin{subequations}\label{eq: clc_mean_variance}
\begin{align}
\mathbb{E}[\textit{CLC}(\mathbf{x},\underline{\bfu})] &\!=\!
\mathbb{E}[\mathbf{q}(\mathbf{x})]^\top  \underline{\bfu}   \\
\textit{Var}[\textit{CLC}(\mathbf{x},\underline{\bfu})] & \!=\!  \underline{\bfu}^\top \textit{Var}[\mathbf{q}(\mathbf{x})] \underline{\bfu},
\end{align}
\end{subequations}
where $
    \mathbf{q}(\mathbf{x}) := F^\top(\mathbf{x}) [\nabla_{\mathbf{x}} V(\mathbf{x})] + [\alpha_V(V(\mathbf{x})) \quad \boldsymbol{0}_m^\top ]^\top\in \mathbb{R}^{m+1}$ and $\mathbb{E}[\mathbf{q}(\mathbf{x})]$, $ \textit{Var}[\mathbf{q}(\mathbf{x})]$ are computed in \eqref{eq:Qx_mean_variance}.
\end{proposition}
\begin{proof}
We can write the control Lyapunov condition as
\label{eq:gaussian_clf}
$    \textit{CLC}(\mathbf{x},\underline{\bfu})  =  [\nabla_{\mathbf{x}}  V(\mathbf{x})]^\top F(\mathbf{x})\underline{\bfu} + \alpha_{V}(V(\mathbf{x})) = \mathbf{q}^\top(\mathbf{x}) \underline{\bfu}$.
We use the Kronecker product property  $\text{vec}(\mathbf{A}\mathbf{B}\mathbf{C}) = (\mathbf{C}^\top \otimes \mathbf{A})\text{vec}(\mathbf{B})$ to rewrite first term in $\mathbf{q}(\mathbf{x})$ as:
\begin{equation*}
\begin{aligned}
[\nabla_{\mathbf{x}} V(\mathbf{x})]^\top F(\mathbf{x})
&= (\mathbf{I}_{m+1} \otimes [\nabla_{\mathbf{x}} V(\mathbf{x})]^\top)\text{vec}(F(\mathbf{x})).
\end{aligned}
\end{equation*}
Since $[\nabla_{\mathbf{x}} V(\mathbf{x})]$, $\alpha_{V}(V(\mathbf{x}))$ are known and deterministic and $\text{vec}(F(\mathbf{x})) \sim \mathcal{GP}(\text{vec}(\tilde{F}(\mathbf{x})), K_{F}(\mathbf{x},\mathbf{x}'))$, we can express the distribution of $\mathbf{q}(\mathbf{x})$ as follows:
\begin{align}
\label{eq:Qx_mean_variance}
\mathbb{E}[\mathbf{q}(\mathbf{x})] &\!=\!  \tilde{F}^\top(\mathbf{x}) [\nabla_{\mathbf{x}} V(\mathbf{x})] +[\alpha_V(V(\mathbf{x})) \quad \boldsymbol{0}_{m}^\top ]^\top \\
\textit{Var}[\mathbf{q}(\mathbf{x})]  &\!=\! (\mathbf{I}_{m+1} \otimes [\nabla_{\mathbf{x}} V(\mathbf{x})]^\top)K_{F}(\mathbf{I}_{m+1} \otimes [\nabla_{\mathbf{x}} V(\mathbf{x})]) \nonumber.
\end{align}
The result follows from plugging \eqref{eq:Qx_mean_variance} into $\textit{CLC}(\mathbf{x},\underline{\bfu})$.
\end{proof}

We use the mean and variance of $\textit{CBC}(\mathbf{x},\underline{\bfu})$ and $\textit{CLC}(\mathbf{x},\underline{\bfu})$ obtained above to approximate the probabilistic safety and stability constraints in \eqref{eq:CLF_CBF_QP_Gaussian}.

\begin{proposition}[\textbf{Probabilistic CLF-CBF SOCP}]\label{prop:gaussian_socp}
Given a user-specified risk tolerance $p \in [0,1)$, let $c(p) = \sqrt{\frac{p}{1-p}}$. The optimization problem \eqref{eq:CLF_CBF_QP_Gaussian} can be formulated as the following second-order cone program:
\begin{equation}
\label{eq:gaussian_socp_form}
\begin{aligned}
    &\min_{\underline{\bfu} \in \underline{{\cal U}}, \delta\in \mathbb{R},l\in \mathbb{R} } \, \, l& \qquad \qquad \\
    &\mathrm{s.t.} \, \,
    \delta - \mathbb{E}[\mathbf{q}(\mathbf{x})]^\top\underline{\bfu}  \geq c(p)\sqrt{\underline{\bfu}^\top\textit{Var}[\mathbf{q}(\mathbf{x})]\underline{\bfu}},
    \\
    &\qquad \quad \mathbb{E}[\mathbf{p}(\mathbf{x})]^\top\underline{\bfu} \geq c(p)\sqrt{\underline{\bfu}^\top\textit{Var}[\mathbf{p}(\mathbf{x})]\underline{\bfu}},
     \\

    &l+1 \geq
    \sqrt{ \|2L(\mathbf{x})^\top(\underline{\bfu}-\underline{\tilde{\mathbf{k}}}(\mathbf{x}))\|^2 + (2\sqrt{\lambda}\delta)^2 + (l-1)^2}

\end{aligned}
\end{equation}
where $\mathbf{p}$, $\mathbf{q}$ are defined in
Propositions~\ref{prop:cbc_mean_variance} and~\ref{prop:clc_mean_variance}, resp.
\end{proposition}
\begin{proof}
To deal with the probabilistic constraints in~\eqref{eq:CLF_CBF_QP_Gaussian}, we employ Cantelli's inequality \cite{CantelliSuiCD}. For any scalar $\gamma \geq 0$,
\begin{equation*}
\begin{aligned}
\label{eq:cantelli_1}
    \mathbb{P}(\textit{CBC}(\mathbf{x},\underline{\bfu}) \geq \mathbb{E}[\textit{CBC}(\mathbf{x},\underline{\bfu}))] -\gamma | \mathbf{x}, \underline{\bfu}) \geq \\
    1 - \frac{\textit{Var}[\textit{CBC}(\mathbf{x},\underline{\bfu})]}{\textit{Var}[\textit{CBC}(\mathbf{x},\underline{\bfu})]+\gamma^2}  .

\end{aligned}
\end{equation*}
Given this inequality, and since we want $\mathbb{P}(\textit{CBC}(\mathbf{x},\underline{\bfu}) \ge 0) \geq p$, we choose $\gamma =  \mathbb{E}[\textit{CBC}(\mathbf{x},\underline{\bfu})]$
and require the lower bound to be greater than or equal to $p$, i.e.,
$
1 - \frac{\textit{Var}[\textit{CBC}(\mathbf{x},\underline{\bfu})]}{\textit{Var}[\textit{CBC}(\mathbf{x},\underline{\bfu})]+\gamma^2} \geq p$.
The equation can be rearranged into
\begin{equation*}
\label{eq:cantelli_3}
    \mathbb{E}[\textit{CBC}(\mathbf{x},\underline{\bfu})] = \gamma \geq \sqrt{\frac{p}{1-p}\textit{Var}[\textit{CBC}(\mathbf{x},\underline{\bfu})]},
\end{equation*}
which corresponds to the safety constraint in \eqref{eq:gaussian_socp_form}.

Next, we show that this is a second-order cone (SOC) constraint. By \eqref{eq: cbc_mean_variance}, given that $\tilde{h}$, $\nabla\tilde{h}$ and $\tilde{F}$ are known and deterministic, the expectation $\mathbb{E}[\textit{CBC}(\mathbf{x},\underline{\bfu})] = \mathbb{E}[\mathbf{p}(\mathbf{x})]^\top\underline{\bfu} $ is affine in $\underline{\bfu}$. Since $\textit{Var}[\mathbf{p}(\mathbf{x})]$ is positive semi-definite,
\begin{equation}
\label{eq:cbc_var_norm}
    \sqrt{\textit{Var}[\textit{CBC}(\mathbf{x},\underline{\bfu})]} = \sqrt{\underline{\bfu}^\top\textit{Var}[\mathbf{p}(\mathbf{x})]\underline{\bfu}} = \|\mathbf{D}(\mathbf{x})\underline{\bfu}\|
\end{equation}
where $\mathbf{D}(\mathbf{x})^\top \mathbf{D}(\mathbf{x}) = \textit{Var}[\mathbf{p}(\mathbf{x})]$. Acccording to~\cite{alizadeh2003second}, the safety constraint in \eqref{eq:gaussian_socp_form} is a valid SOC constraint.

For stability, the CLC condition can be constructed using a similar approach with Cantelli's inequality, resulting in~\eqref{eq:gaussian_socp_form}. By \eqref{eq: clc_mean_variance}, we know that the expectation is affine in $\underline{\bfu}$ and the variance is quadratic in terms of $\underline{\bfu}$, similar to \eqref{eq:cbc_var_norm}. This shows that the CLC condition is also a valid SOC constraint.

Our last step is to reformulate the minimization of the objective function as a linear objective with an SOC constraint, resulting in the standard SOCP in~\eqref{eq:gaussian_socp_form}. We introduce a new variable $l$ so that the problem in \eqref{eq:CLF_CBF_QP_Gaussian} is equivalent to
\begin{align}
\label{eq:CLF_CBF_QP_Gaussian_Convert}
    & \min_{\underline{\bfu} \in \underline{{\cal U}}, \delta\in \mathbb{R},l\in \mathbb{R} } \, \, l \notag \\
    \text{s.t.} \, \,
    &\mathbb{P}(\textit{CLC}(\mathbf{x}, \underline{\bfu}) \leq \delta) \geq p, \quad \mathbb{P}(\textit{CBC}(\mathbf{x}, \underline{\bfu}) \geq 0) \geq p, \notag \\
    &\|L(\mathbf{x})^\top(\underline{\bfu} - \underline{\tilde{\mathbf{k}}}(\mathbf{x}))\|^2 + \lambda \delta^2 \leq l.
\end{align}
The last constraint in \eqref{eq:CLF_CBF_QP_Gaussian_Convert} corresponds to a rotated second-order cone, $\mathcal{Q}^n_{rot} \coloneqq \{(\mathbf{x}_{r},y_{r},z_{r}) \in \mathbb{R}^{n+2}\, | \,\|\mathbf{x}_{r}\|^2 \leq y_{r}z_{r}, y_{r} \geq 0, z_{r} \geq 0 \}$, which can be converted into a standard SOC constraint~\cite{alizadeh2003second},
$
\left\| \begin{bmatrix} 2 \mathbf{x}_{r} & y_r - z_r\end{bmatrix}^\top \right\| \leq y_r+z_r.
$
Let $y_r = l$, $z_r = 1$ and consider the constraint $\|L(\mathbf{x})^\top(\underline{\bfu}-\underline{\tilde{\mathbf{k}}}(\mathbf{x}))\|^2 + \lambda \delta^2 \leq l$. Multiplying both sides by $4$ and adding $(l-1)^2$, makes the constraint equivalent to
\[
4\|L(\mathbf{x})^\top(\underline{\bfu}-\underline{\tilde{\mathbf{k}}}(\mathbf{x}))\|^2 + 4\lambda\delta^2 + (l-1)^2 \leq (l+1)^2.
\]
Taking a square root on both sides, we end up with $\sqrt{ \|2L(\mathbf{x})^\top(\underline{\bfu}-\underline{\tilde{\mathbf{k}}}(\mathbf{x}))\|^2 + (2\sqrt{\lambda}\delta)^2 + (l-1)^2} \leq l+1$, which is equivalent to the third constraint in~\eqref{eq:gaussian_socp_form}.
\end{proof}

\begin{remark}[\textbf{Effects of risk-tolerance $p$ and variance}]
When $p=0$, the probabilistic CLF-CBF-SOCP \eqref{eq:gaussian_socp_form} reduces to the original CLF-CBF-QP \eqref{eq:QP_origin}. As $p$ and/or $\textit{Var}[\mathbf{p}(\mathbf{x})]$, $\textit{Var}[\mathbf{q}(\mathbf{x})]$ increase, the feasible region of \eqref{eq:gaussian_socp_form} gets smaller, and the optimal value worsens, cf. Fig.~\ref{fig:2b} for an illustration.
\end{remark}

\section{Robust Safe Control}
\label{sec:robust_control}

In this section, we develop a solution to Problem~\ref{prob: robust_clf_cbf}.
Let $\tilde{F}$ denote the estimated system dynamics, $\tilde{h}$, $\nabla \tilde{h}$ the estimated barrier function and its gradient, and let $e_F : \mathbb{R}^{n \times (m+1)} \mapsto \mathbb{R}_{\geq 0}$, $e_h : \mathbb{R} \mapsto \mathbb{R}_{\geq 0}$, and $e_{\nabla h}: \mathbb{R}^n \mapsto \mathbb{R}_{\geq 0}$ be associated error bounds. For convenience, for each $\mathbf{x} \in \mathcal{X}$, we denote $D_F(\mathbf{x}) := F(\mathbf{x}) - \tilde{F}(\mathbf{x})$, $d_h(\mathbf{x}) := h(\mathbf{x}) -\tilde{h}(\mathbf{x})$ and $\mathbf{d}_{\nabla h}(\mathbf{x}) := \nabla h(\mathbf{x}) - \nabla \tilde{h}(\mathbf{x})$.
By \eqref{error_dynamics} and~\eqref{error_barrier}, we have
\begin{equation}
\label{eq: error_bound}
    \|D_F (\mathbf{x})\|  \leq e_F(\mathbf{x}), \: |d_h (\mathbf{x})|  \leq e_h(\mathbf{x}), \:  \|\mathbf{d}_{\nabla h} (\mathbf{x}) \|   \leq e_{\nabla h}(\mathbf{x}).
\end{equation}
Using this notation, we can rewrite $\textit{CBC}(\mathbf{x},\underline{\bfu})$ as
\begin{equation*}
\begin{aligned}
&\textit{CBC}(\mathbf{x}, \underline{\bfu}) = [\nabla h(\mathbf{x})]^\top  F(\mathbf{x})\underline{\bfu}  +  \alpha_h( h(\mathbf{x})) \\
&= [\nabla \tilde{h}(\mathbf{x})]^\top \tilde{F}(\mathbf{x})\underline{\bfu} +  \mathbf{d}_{\nabla h}^\top(\mathbf{x})\tilde{F}(\mathbf{x})\underline{\bfu}+ [\nabla \tilde{h}(\mathbf{x})]^\top D_F(\mathbf{x})\underline{\bfu} \\
&\quad + \mathbf{d}_{\nabla h}^\top(\mathbf{x}) D_F(\mathbf{x})\underline{\bfu} +  \alpha_h( \tilde{h}(\mathbf{x})+d_h(\mathbf{x})).
\end{aligned}
\end{equation*}
Let $\tilde{\mathbf{p}}(\mathbf{x}) := \tilde{F}^\top(\mathbf{x})\nabla \tilde{h}(\mathbf{x}) $. We group the error term in the expression for $\textit{CBC}(\mathbf{x}, \underline{\bfu})$ in the variable $d_{\textit{CBC}}(\mathbf{x}, \underline{\bfu}) := \textit{CBC}(\mathbf{x}, \underline{\bfu})- \tilde{\mathbf{p}}(\mathbf{x})^\top \underline{\bfu}$.
Thus, $\textit{CBC}(\mathbf{x},\underline{\bfu}) \geq 0$ is satisfied if
\begin{align*}
 \min_{D_F,\mathbf{d}_{\nabla h}, d_h}\! \textit{CBC}(\mathbf{x}, \underline{\bfu}) = \tilde{\mathbf{p}}(\mathbf{x})^\top \underline{\bfu}  + \! \min_{D_F,\mathbf{d}_{\nabla h}, d_h} \! d_{\textit{CBC}}(\mathbf{x}, \underline{\bfu})  \ge 0.

\end{align*}
Similarly, let
$\tilde{\mathbf{q}}(\mathbf{x}) := \tilde{F}^\top(\mathbf{x})\nabla V(\mathbf{x})   + [\alpha_{V}(V(\mathbf{x}))  \quad \boldsymbol{0}_{m}^\top]^\top$ and $d_{\textit{CLC}}(\mathbf{x}, \underline{\bfu}) := [\nabla V(\mathbf{x})]^\top D_F(\mathbf{x})\underline{\bfu}$, a robust version of the stability constraint $\textit{CLC}(\mathbf{x},\underline{\bfu}) \leq \delta$ can be written as:
\begin{equation}
\label{eq:robust_clc}
\max_{D_F} \textit{CLC}(\mathbf{x}, \underline{\bfu}) = \tilde{\mathbf{q}}(\mathbf{x})^\top \underline{\bfu}  + \max_{D_F}  d_{\textit{CLC}}(\mathbf{x}, \underline{\bfu}) \leq \delta .
\end{equation}
This leads us to the following robust reformulation of the original control synthesis problem in~\eqref{eq:QP_origin},
\begin{equation}\label{eq:QP_robust}
\begin{aligned}
    &\min_{\underline{\bfu} \in \underline{{\cal U}}, \delta\in \mathbb{R},l\in \mathbb{R} } \, \, l \qquad \qquad \\
    \mathrm{s.t.} \, \,
    &\tilde{\mathbf{q}}(\mathbf{x})^\top \underline{\bfu} + \max_{D_F} \; d_{\textit{CLC}}(\mathbf{x}, \underline{\bfu}) \le \delta \\
    & \tilde{\mathbf{p}}(\mathbf{x})^\top \underline{\bfu} + \min_{D_F,  d_h, \mathbf{d}_{\nabla h}} d_{\textit{CBC}}(\mathbf{x}, \underline{\bfu}) \ge 0
    \\
    &l+1 \geq
    \sqrt{ \|2L(\mathbf{x})^\top(\underline{\bfu}-\underline{\tilde{\mathbf{k}}}(\mathbf{x}))\|^2 + (2\sqrt{\lambda}\delta)^2 + (l-1)^2}.
\end{aligned}
\end{equation}
Note that we used the same approach as in the proof of Proposition~\ref{prop:gaussian_socp} to reformulate the original quadratic objective with a linear objective plus a SOC constraint. The second constraint in \eqref{eq:QP_robust} requires solving $\min_{D_F,  d_h, \mathbf{d}_{\nabla h}} d_{\textit{CBC}}(\mathbf{x}, \underline{\bfu})$ subject to \eqref{eq: error_bound}. In general, this is a non-convex
constrained quadratic program which does not have a closed-form expression of the minimizer as a function of~$\underline{\bfu}$. Instead, we make the second constraint in \eqref{eq:QP_robust} more conservative using the Cauchy-Schwarz inequality, which leads to a convex SOCP, whose optimal solution is guaranteed to be feasible for \eqref{eq:QP_robust}.

\begin{proposition}[\textbf{Robust CLF-CBF SOCP}]\label{prop: worst_case_error}
Let $\tilde{F}$, $\tilde{h}$, $\nabla \tilde{h}$ denote estimates of the system dynamics and barrier function, with error bounds in \eqref{eq: error_bound}. Then, the feasible set of the following SOCP is included in the feasible set of \eqref{eq:QP_robust}:
\begin{align}
\label{eq:worst_case_SOCP_formulation}
    & \min_{\underline{\bfu} \in \underline{{\cal U}}, \delta\in \mathbb{R}, p\in \mathbb{R}, q \in \mathbb{R}, l\in \mathbb{R}} \, \, l \notag \\
    \mathrm{s.t.} \, \,
    &\delta - \tilde{\mathbf{q}}(\mathbf{x})^\top \underline{\bfu} \geq e_F(\mathbf{x})\|\nabla V(\mathbf{x})\| \|\underline{\bfu}\|,  \notag \\
    & p \geq e_{\nabla h}(\mathbf{x})\|\tilde{F}(\mathbf{x}) \underline{\bfu} \|  , \notag \\
    & q \geq \Big(e_F(\mathbf{x})\|\nabla \tilde{h}(\mathbf{x})\|  + e_{\nabla h}(\mathbf{x})e_F(\mathbf{x})\Big)\|\underline{\bfu} \| , \notag \\
    & [\nabla \tilde{h}(\mathbf{x})]^\top \tilde{F}(\mathbf{x})\underline{\bfu} + \alpha_h(\tilde{h}(\mathbf{x}) - e_h(\mathbf{x})) \geq p + q , \notag \\

    &l+1 \geq
    \sqrt{ \|2L(\mathbf{x})^\top(\underline{\bfu}-\underline{\tilde{\mathbf{k}}}(\mathbf{x}))\|^2 + (2\sqrt{\lambda}\delta)^2 + (l-1)^2}

\end{align}
\end{proposition}

\begin{proof}
The stability constraint in \eqref{eq:QP_robust} is reformulated using:
\begin{equation*}
    \max_{\|D_F (\mathbf{x})\|  \leq e_F(\mathbf{x})}d_{\textit{CLC}}(\mathbf{x}, \underline{\bfu}) = e_F(\mathbf{x})\|\nabla V(\mathbf{x})\|\|\underline{\bfu}\|.
\end{equation*}
For the safety constraint in \eqref{eq:QP_robust}, note that
\begin{align}
&\min_{D_F,  d_h, \mathbf{d}_{\nabla h}} d_{\textit{CBC}}(\mathbf{x}, \underline{\bfu}) \notag
\\
& = \min_{D_F,  \mathbf{d}_{\nabla h}} \Big(\mathbf{d}_{\nabla h}^\top(\mathbf{x})\tilde{F}(\mathbf{x})\underline{\bfu} + [\nabla \tilde{h}(\mathbf{x})]^\top D_F(\mathbf{x})\underline{\bfu} + \notag\\
&\qquad \mathbf{d}_{\nabla h}^\top(\mathbf{x}) D_F(\mathbf{x})\underline{\bfu} \Big) + \min_{d_h} \alpha_h(\tilde{h}(\mathbf{x}) + d_h(\mathbf{x})).
\end{align}
Since $e_h(\mathbf{x}) \geq 0$ and $\alpha_h$ is an extended class $\mathcal{K}_\infty$ function,
\begin{equation}
\label{eq:robust_cbc_rhs}
    \min_{|d_h(\mathbf{x})| \leq e_{h}(\mathbf{x})} \hspace*{-2pt} \alpha_h(\tilde{h}(\mathbf{x}) + d_h(\mathbf{x})) \!=\! \alpha_h(\tilde{h}(\mathbf{x}) - e_h(\mathbf{x})) .
\end{equation}
Applying the Cauchy-Schwarz inequality on each term,
\begin{align*}
& \min_{D_F,  d_h, \mathbf{d}_{\nabla h}} d_{\textit{CBC}}(\mathbf{x}, \underline{\bfu})
\ge -\|\mathbf{d}_{\nabla h}\|\|\tilde{F}(\mathbf{x})\underline{\bfu}\|
\notag\\  &\qquad
- \|\nabla \tilde{h}(\mathbf{x})\|\|D_F(\mathbf{x})\underline{\bfu}\| -  \|\mathbf{d}_{\nabla h}(\mathbf{x})\|\| D_F(\mathbf{x})\underline{\bfu}\|
\notag\\  &\qquad
+  \alpha_h(\tilde{h}(\mathbf{x}) - e_h(\mathbf{x}))
\notag\\
&\quad \ge - e_{\nabla h}(\mathbf{x}) \|\tilde{F}(\mathbf{x})\underline{\bfu}\| - e_F(\mathbf{x})\|\nabla \tilde{h}(\mathbf{x})\| \|\underline{\bfu}\| -
\notag\\
&\qquad e_{\nabla h}(\mathbf{x}) e_F(\mathbf{x})\|\underline{\bfu}\| + \alpha_h(\tilde{h}(\mathbf{x}) - e_h(\mathbf{x})).
\end{align*}
In the last step, we minimized each term independently, so the lower bound is not tight. We write the safety constraint~as
\begin{align}
\label{eq:robust_cbc_constraint}
    &e_{\nabla h}(\mathbf{x})\|\tilde{F}(\mathbf{x}) \underline{\bfu} \|
    + (e_F(\mathbf{x})\|\nabla \tilde{h}(\mathbf{x})\|  + e_{\nabla h}(\mathbf{x})e_F(\mathbf{x}))\|\underline{\bfu} \| \notag \\
    & \leq [\nabla \tilde{h}(\mathbf{x})]^\top \tilde{F}(\mathbf{x})\underline{\bfu} + \alpha_h(\tilde{h}(\mathbf{x}) - e_h(\mathbf{x})).
\end{align}
Constraints of the form $\|\mathbf{A}\mathbf{z} - \boldsymbol{a}\| + \|\mathbf{B}\mathbf{z} - \boldsymbol{b}\| \leq \boldsymbol{c}^\top \mathbf{z}$ can be replaced by the set of constraints $\|\mathbf{A}\mathbf{z} - \boldsymbol{a}\| \leq p$, $\|\mathbf{B}\mathbf{z} - \boldsymbol{b}\| \leq q$, $p+q \leq \boldsymbol{c}^\top \mathbf{z}$ combined. Thus,~\eqref{eq:robust_cbc_constraint} is equivalent to the second, third, and fourth constraints in \eqref{eq:worst_case_SOCP_formulation} together.
\end{proof}

\begin{remark}[\textbf{Effects of error bounds}]
If there are no errors in either the dynamics or the barrier function ($e_F \equiv e_h \equiv e_{\nabla h} \equiv 0$), then the robust CLF-CBF SOCP \eqref{eq:worst_case_SOCP_formulation} reduces to a CLF-CBF QP \eqref{eq:QP_origin}. If $e_F \equiv 0$ while $e_h(\mathbf{x}), e_{\nabla h}(\mathbf{x}) > 0$, the result in Proposition~\ref{prop: worst_case_error} recovers \cite[Proposition 2]{Long_learningcbf_ral21}. As the error bounds $e_F, e_h, e_{\nabla h}$ increase, the feasible region of~\eqref{eq:worst_case_SOCP_formulation} gets smaller and the optimal solution worsens. Also, note that the choice of kernel function, $K_F(\mathbf{x},\mathbf{x}) = \frac{e^2_F(\mathbf{x})}{c^2(p)}\mathbf{I}_{(m+1)n}$, reduces the inequality for stability in \eqref{eq:gaussian_socp_form} to that in \eqref{eq:worst_case_SOCP_formulation}.
\end{remark}

\section{Environment and System Dynamics Estimation}

In this section, we propose our approach to learn the system dynamics with uncertainty offline and estimate barrier functions for robots operating in some unknown environments online.

\subsection{Online Bayesian Signed Distance Function Learning}

\subsection{Bayesian System Dynamics Learning}
After addressing the problem of estimating SDFs online, we also need to deal with Problem~\ref{prob: estimate_dynamics}. Recently, the idea of incorporating neural networks with physics laws has becoming more and more popular.

\section{Safety with Time-Dependent Control Barrier Functions}
We use the following Lie-derivative notations for the given time-varying barrier function $h(\boldsymbol{x},t)$:
\begin{equation}
\begin{aligned}
\mathcal{L}_f h(\boldsymbol{x},t) = \frac{\partial h}{\partial \boldsymbol{x}}(\boldsymbol{x},t)f(\boldsymbol{x}), \quad \mathcal{L}_g h(\boldsymbol{x},t) = \frac{\partial h}{\partial \boldsymbol{x}}(\boldsymbol{x},t)g(\boldsymbol{x})
\end{aligned}
\end{equation}

\begin{align}
    \frac{\partial h(\mathbf{x}, \mathbf{y}(t))}
    {\partial t} = \frac{\partial h(\mathbf{x}, \mathbf{y}(t))}{\partial \mathbf{x}}
    (f(\mathbf{x}) + g(\mathbf{x})\mathbf{u}) + \frac{\partial h(\mathbf{x}, \mathbf{y}(t))}{\partial \mathbf{y}}\dot{\mathbf{y}}
\end{align}

consider the following QP problem for safe controller synthesis:
\begin{equation}
\label{eq:QP_moving}
\begin{aligned}
& \min_{\boldsymbol{u} \in {\cal U},\delta \in \mathbb{R}}\,\, \|L(\boldsymbol{x})^\top(\boldsymbol{u} - \tilde{\boldsymbol{u}}(\boldsymbol{x}))\|^2 + \lambda \delta^2\\
\mathrm{s.t.} \, \,  &\mathcal{L}_f V(\boldsymbol{x}) + \mathcal{L}_g V(\boldsymbol{x})  \boldsymbol{u} + \alpha_V(V(\boldsymbol{x})) \leq \delta \\
&\mathcal{L}_{f} h(\boldsymbol{x},t) + \mathcal{L}_{g} h(\boldsymbol{x,t})\boldsymbol{u} + \alpha_h(h(\boldsymbol{x},t)) \geq 0,
\end{aligned}
\end{equation}
\section{Probabilistic Safe Control with MVGP Assumption}
In this section, we assume the barrier functions follow the distribution as stated in Problem~\ref{prob: prob_clf_cbf} and system dynamics follows the matrix Variate Gaussian Process (MVGP) distribution as stated in \cite{dhiman2020control}. While the MVGP assumption is not useful for learning system dynamics using dropout neural network, it balances the efficiency and accuracy of learning the dynamics using GP based methods. From the MVGP assumption, $F(\boldsymbol{x})$ can be described by a three-parameter distribution $\mathcal{MN}(\tilde{F}, \mathbf{A}, \mathbf{B})$, where $\tilde{F} \in \mathbb{R}^{(1+m) \times n}$ is the mean and $\mathbf{A} \in \mathbb{R}^{(m+1) \times (m+1)}, \mathbf{B} \in \mathbb{R}^{n \times n}$ are the covariance matrices of the rows and columns of $F^\top(\boldsymbol{x})$.

\begin{lemma}[{\cite{Sun2017LearningSW}}]
\label{lemma: MVG_1}
Let $\mathbf{X}$ follow an MVG distribution
$\mathcal{MN}(\mathbf{M}, \mathbf{A}, \mathbf{B})$. Then, $\textit{vec}(\mathbf{X}) \sim \mathcal{N} (\textit{vec}(\mathbf{M}), \mathbf{B} \otimes \mathbf{A})$.
\end{lemma}

By Lemma~\ref{lemma: MVG_1}, we propose the following GP decomposition of the system dynamics,
\begin{equation}
    \textit{vec}(F(\boldsymbol{x})) \sim \mathcal{GP}(\textit{vec}(\tilde{F}(\boldsymbol{x})),\mathbf{B}(\boldsymbol{x},\boldsymbol{x}') \otimes \mathbf{A})
\end{equation}
where $\otimes$ denotes the Kronecker product.

In this section, we aim at synthesizing a safe controller via the following optimization problem:

\begin{equation}
\label{eq:mvgp_CLF_CBF_QP}
\begin{aligned}
& \min_{\boldsymbol{u} \in {\cal U},\delta \in \mathbb{R}}\,\, \|L(\boldsymbol{x})^\top(\boldsymbol{u} - \tilde{\boldsymbol{u}}(\boldsymbol{x}))\|^2 + \lambda \delta^2\\
\mathrm{s.t.} \, \,  &\mathbb{P}(\textit{CBC}(\boldsymbol{x}, \boldsymbol{u}) \geq \zeta) \geq p,
\end{aligned}
\end{equation}

\begin{proposition}
\label{prop:mvgp_cbc_mean_variance}
Assume that the extended class $\mathcal{K}$ function $\alpha_h$ is linear ($\alpha_h(h(\boldsymbol{x})) = a \cdot h(\boldsymbol{x}), a \in \mathbb{R}$), assume the barrier function $h$ and system dynamics $F$ are independent. Given $h(\boldsymbol{x}) \sim \mathcal{GP}(\tilde{h}(\boldsymbol{x}),K_h(\boldsymbol{x},\boldsymbol{x}'))$) and ($\textit{vec}(F(\boldsymbol{x})) \sim \mathcal{GP}(\textit{vec}(\tilde{F}(\boldsymbol{x})),\mathbf{B}(\boldsymbol{x},\boldsymbol{x}') \otimes \mathbf{A})$). We compute the mean and variance of $\textit{CBC}(\boldsymbol{x},\boldsymbol{u})$:
\begin{equation}
\label{eq: mvgp_cbc_mean_variance}
\begin{aligned}
&\mathbb{E}[\textit{CBC}(\boldsymbol{x},\boldsymbol{u})] = \mathbf{M}_1 (\boldsymbol{x}) \mathbf{M}_2 (\boldsymbol{x}) \\
& \textit{Var}[\textit{CBC}(\boldsymbol{x},\boldsymbol{u}] = \mathbf{M}_2 ^\top(\boldsymbol{x}) \mathbf{K}_1 (\boldsymbol{x},\boldsymbol{x}) \mathbf{M}_2(\boldsymbol{x}) + \\
&\mathbf{M}_1(\boldsymbol{x}) \mathbf{K}_2 (\boldsymbol{x},\boldsymbol{x}) \mathbf{M}_1^\top(\boldsymbol{x})
\end{aligned}
\end{equation}
where
\begin{equation*}
    \mathbf{M}_1(\boldsymbol{x}) = [a \quad \underline{\boldsymbol{u}}^\top \tilde{F}^\top(\boldsymbol{x})] \in \mathbb{R}^{1 \times (n+1)}
\end{equation*}
\begin{equation}
\label{eq:mvgp_mean_m2}
    \mathbf{M}_2(\boldsymbol{x}) = \begin{bmatrix}
        \tilde{h}(\boldsymbol{x}) \\
        \nabla_{\boldsymbol{x}} \tilde{h}(\boldsymbol{x})
    \end{bmatrix} \in \mathbb{R}^{(n+1) \times 1}
\end{equation}

\begin{equation}
\label{eq:mvgp_covariance_k1}
    \mathbf{K}_1(\boldsymbol{x},\boldsymbol{x}') = \begin{bNiceArray}{c|ccc}[margin]
0 & 0 & \cdots & 0 \\
\hline
0 & \Block{3-3}{\mathbf{B}^\top(\boldsymbol{x},\boldsymbol{x}') \otimes \underline{\boldsymbol{u}}^\top \mathbf{A}^\top \underline{\boldsymbol{u}}}  & &  \\
\vdots& & \hspace*{2cm} & \\
0 & & &
\end{bNiceArray} \in \mathbb{R}^{(n+1) \times (n+1)}
\end{equation}

\begin{equation}
\label{eq:mvgp_covariance_k2}
    \mathbf{K}_2(\boldsymbol{x},\boldsymbol{x}') =     \begin{bmatrix}
    \begin{aligned}
        K_h(\boldsymbol{x},\boldsymbol{x}')&, \nabla_{\boldsymbol{x}} K_h(\boldsymbol{x},\boldsymbol{x}')\\
        \nabla_{\boldsymbol{x}} K_h(\boldsymbol{x},\boldsymbol{x}')^\top&, \mathcal{H}_{\boldsymbol{x},\boldsymbol{x}}K_h(\boldsymbol{x},\boldsymbol{x}')
    \end{aligned}
    \end{bmatrix} \in \mathbb{R}^{(n+1) \times (n+1)}
\end{equation}
and ${\cal H}_{\boldsymbol{x},\boldsymbol{x}}$ denotes the Hessian matrix.
\end{proposition}

\begin{proof}
Since $\alpha_h(h(\boldsymbol{x})) = a \cdot h(\boldsymbol{x}), a \in \mathbb{R}$, the original control barrier constraint can be written as:
\begin{equation}
\begin{aligned}
\label{eq:mvgp_cbf}
    \textit{CBC}(\boldsymbol{x},\boldsymbol{u}) &= [\nabla_{\boldsymbol{x}} h(\boldsymbol{x})]^\top f(\boldsymbol{x}) + [\nabla_{\boldsymbol{x}} h(\boldsymbol{x})]^\top g(\boldsymbol{x})\boldsymbol{u} + a h(\boldsymbol{x})   \\
     &=     [a \quad f^\top(\boldsymbol{x})+(g(\boldsymbol{x})\boldsymbol{u})^\top] \cdot\begin{bmatrix}
    h(\boldsymbol{x}) \\
    \nabla_{\boldsymbol{x}} h(\boldsymbol{x})
    \end{bmatrix} \\
    &= [a \quad \underline{\boldsymbol{u}}^\top F^\top (\boldsymbol{x})] \cdot\begin{bmatrix}
    h(\boldsymbol{x}) \\
    \nabla_{\boldsymbol{x}} h(\boldsymbol{x})
    \end{bmatrix}
\end{aligned}
\end{equation}

\begin{lemma}[{\cite{Sun2017LearningSW}}]
\label{lemma: MVG_2}
    Let $\mathbf{X}$ follow an MVG distribution $\mathcal{MN}(\mathbf{M}, \mathbf{A}, \mathbf{B})$ and let $\mathbf{C} \in \mathbb{R}^{d \times n}$ and $\mathbf{D} \in \mathbb{R}^{m \times d}$. Then,

    \begin{equation*}
    \begin{aligned}
       &\mathbf{C} \mathbf{X} \sim \mathcal{MN}(\mathbf{C} \mathbf{M}, \mathbf{C} \mathbf{A} \mathbf{C}^\top, \mathbf{B}); \\
       &\mathbf{X} \mathbf{D} \sim \mathcal{MN}(\mathbf{D}\mathbf{C}, \mathbf{A} , \mathbf{D}^\top \mathbf{B} \mathbf{D})
    \end{aligned}
    \end{equation*}
\end{lemma}

By Lemma~\ref{lemma: MVG_2}, for any given control input $\boldsymbol{u}$, the MVGP of $\underline{\boldsymbol{u}}^\top F^\top(\boldsymbol{x})$ is:
\begin{equation}
    \underline{\boldsymbol{u}}^\top F^\top(\boldsymbol{x}) \sim \mathcal{MN}(\underline{\boldsymbol{u}}^\top \tilde{F}^\top(\boldsymbol{x}), \underline{\boldsymbol{u}}^\top\mathbf{A}^\top \underline{\boldsymbol{u}}, \mathbf{B}^\top)
\end{equation}
since $\underline{\boldsymbol{u}}^\top F^\top(\boldsymbol{x}) \in \mathbb{R}^{1 \times n}$ is a row vector, we have
\begin{equation}
    \underline{\boldsymbol{u}}^\top F^\top(\boldsymbol{x}) \sim \mathcal{GP}(\underline{\boldsymbol{u}}^\top \tilde{F}^\top(\boldsymbol{x}),\mathbf{B}^\top(\boldsymbol{x},\boldsymbol{x}') \otimes \underline{\boldsymbol{u}}^\top \mathbf{A}^\top \underline{\boldsymbol{u}})
\end{equation}
Now, we know $[a \quad \underline{\boldsymbol{u}}^\top F^\top (\boldsymbol{x})]$ in \eqref{eq:mvgp_cbf} is GP with mean $\mathbf{M}_1(\boldsymbol{x}) = [a \quad \underline{\boldsymbol{u}}^\top \tilde{F}^\top(\boldsymbol{x})]$ and kernel function $\mathbf{K}_1(\boldsymbol{x},\boldsymbol{x}')$ defined in \eqref{eq:mvgp_covariance_k1}.

In our previous work~\cite{dhiman2020control}, it is shown that the gradient of a GP is also GP. Specifically, the barrier function gradient $\nabla_{\boldsymbol{x}}   h(\boldsymbol{x})$ is GP given that $h(\boldsymbol{x}) \sim \mathcal{GP}(M_h(\boldsymbol{x}),K_h(\boldsymbol{x},\boldsymbol{x}'))$, and
\begin{equation}
\begin{aligned}
    \begin{bmatrix}
        h(\boldsymbol{x}) \\
         \nabla_{\boldsymbol{x}} h(\boldsymbol{x})
    \end{bmatrix}
    \sim
    \mathcal{GP}(\mathbf{M}_2(\boldsymbol{x}),\mathbf{K}_2(\boldsymbol{x},\boldsymbol{x}'))
\end{aligned}
\end{equation}
where $\mathbf{M}_2(\boldsymbol{x})$ and $\mathbf{K}_2(\boldsymbol{x},\boldsymbol{x}')$ are defined in \eqref{eq:mvgp_mean_m2} and \eqref{eq:mvgp_covariance_k2} respectively.

In~\eqref{eq:mvgp_cbf}, we need to compute $\textit{CBC}(\boldsymbol{x},\boldsymbol{u})$ as a product of two independent GPs. By Lemma~\ref{lemma: gp_product}, we show that the result is a real-valued scalar distribution with mean $\mathbb{E}[\textit{CBC}(\boldsymbol{x},\boldsymbol{u})]$ and variance $\textit{Var}(\textit{CBC}(\boldsymbol{x},\boldsymbol{u}))$,

\begin{equation}
\label{eq:mvgp_cbc_mean}
    \mathbb{E}[\textit{CBC}(\boldsymbol{x},\boldsymbol{u})] = \mathbf{M}_1 (\boldsymbol{x}) \mathbf{M}_2 (\boldsymbol{x})
\end{equation}

\begin{equation}
\begin{aligned}
\label{eq:mvgp_cbc_var}
    \textit{Var}[\textit{CBC}(\boldsymbol{x},\boldsymbol{u}] = &\mathbf{M}_2 ^\top(\boldsymbol{x}) \mathbf{K}_1 (\boldsymbol{x},\boldsymbol{x}) \mathbf{M}_2(\boldsymbol{x}) + \\
    &\mathbf{M}_1(\boldsymbol{x}) \mathbf{K}_2 (\boldsymbol{x},\boldsymbol{x}) \mathbf{M}_1^\top(\boldsymbol{x})
\end{aligned}
\end{equation}

\end{proof}

\begin{proposition}
\label{prop:mvgp_socp}
Assume the mean and variance of the control barrier constraint in Proposition~\ref{prop:cbc_mean_variance} as \eqref{eq: cbc_mean_variance}, we can write \eqref{eq:CLF_CBF_QP_Gaussian} as a second order cone program as follows:
\begin{equation}
\label{eq:mvgp_socp_form}
\begin{aligned}
    & \min_{\boldsymbol{u} \in {\cal U}, \delta\in \mathbb{R},l\in \mathbb{R} } \, \, l \\
    \mathrm{s.t.} \, \,
    &\delta - \mathbb{E}[\textit{CLC}(\boldsymbol{x},\boldsymbol{u})] \geq c(p)\|\textit{Var}[\textit{CLC}(\boldsymbol{x},\boldsymbol{u})\|_2
    \\
    &\zeta - \mathbb{E}[\textit{CBC}(\boldsymbol{x},\boldsymbol{u})] \leq -c(p)\|\textit{Var}[\textit{CBC}(\boldsymbol{x},\boldsymbol{u})\|_2
     \\
    &\Bigg\| \begin{bmatrix} 2L(\boldsymbol{x})^\top(\boldsymbol{u}-\tilde{\boldsymbol{u}}(\boldsymbol{x}))\\ 2\sqrt{\lambda} \delta \\ l-1 \end{bmatrix} \Bigg\| \leq l+1.

\end{aligned}
\end{equation}
where $c(p) = \sqrt{\frac{p}{1-p}}$.
\end{proposition}
\begin{proof}
Same as Proposition~\ref{prop:gaussian_socp}.

\end{proof}

\section{Convert QP to SOCP}
\label{qp_to_socp}
\begin{proposition}
\label{prop:qp_convert_socp}
For a general CLF-CBF-QP problem as follows,
\begin{equation}
\label{eq:QP_need_convert}
\begin{aligned}
& \min_{\boldsymbol{u} \in {\cal U},\delta \in \mathbb{R}}\,\, \|L(\boldsymbol{x})^\top(\boldsymbol{u} - \tilde{\boldsymbol{u}}(\boldsymbol{x}))\|^2 + \lambda \delta^2,\\
\mathrm{s.t.} \, \,  &\textit{CLC}(\boldsymbol{x},\boldsymbol{u}) \leq \delta,  \textit{CBC}(\boldsymbol{x},\boldsymbol{u}) \geq 0,
\end{aligned}
\end{equation}
one can formulate it as a convex second-order cone program (SOCP) by reformulating the objective function as a constraint, which results in,
\begin{equation}
\label{eq:converted_SOCP}
\begin{aligned}
    & \min_{\boldsymbol{u} \in {\cal U}, \delta\in \mathbb{R},l\in \mathbb{R} } \, \, l \\
    \mathrm{s.t.} \, \, &\textit{CLC}(\boldsymbol{x},\boldsymbol{u}) \leq \delta,  \textit{CBC}(\boldsymbol{x},\boldsymbol{u}) \geq 0, \\
    &\left\| \begin{bmatrix} 2L(\boldsymbol{x})^\top(\boldsymbol{u}-\tilde{\boldsymbol{u}}(\boldsymbol{x}))\\ 2\sqrt{\lambda} \delta \\ l-1 \end{bmatrix} \right\| \leq l+1.
\end{aligned}
\end{equation}
\end{proposition}

\begin{proof}
We first introduce a new variable $l$ so that the problem in \eqref{eq:QP_need_convert} is equivalent to
\begin{equation}
\label{eq:SOCP_error_bound}
\begin{aligned}
    & \min_{\boldsymbol{u} \in {\cal U}, \delta\in \mathbb{R},l\in \mathbb{R} } \, \, l \\
    \text{s.t.} \, \,
    &\textit{CLC}(\boldsymbol{x},\boldsymbol{u}) \leq \delta,  \textit{CBC}(\boldsymbol{x},\boldsymbol{u}) \geq 0, \\
    &\|L(\boldsymbol{x})^\top(\boldsymbol{u}-\tilde{\boldsymbol{u}}(\boldsymbol{x}))\|^2 + \lambda \delta^2 \leq l.
\end{aligned}
\end{equation}

The last constraint in \eqref{eq:SOCP_error_bound} corresponds to a rotated second-order cone, $\mathcal{Q}^n_{rot} \coloneqq \{(\boldsymbol{x}_{r},y_{r},z_{r}) \in \mathbb{R}^{n+2}\, | \,\|\boldsymbol{x}_{r}\|^2 \leq y_{r}z_{r}, y_{r} \geq 0, z_{r} \geq 0 \}$, which can be converted into a standard SOC constraint~\cite{alizadeh2003second},
\[
\left\| \begin{bmatrix} 2 \boldsymbol{x}_{r} \\ y_r - z_r\end{bmatrix} \right\| \leq y_r+z_r.
\]

Let $y_r = l$, $z_r = 1$ and consider the constraint $\|L(\boldsymbol{x})^\top(\boldsymbol{u}-\tilde{\boldsymbol{u}}(\boldsymbol{x}))\|^2 + \lambda \delta^2 \leq l$. Multiplying both sides by $4$ and adding $(l-1)^2$, makes the constraint equivalent to
\[
4\| L(\boldsymbol{x})^\top (\boldsymbol{u}-\tilde{\boldsymbol{u}}(\boldsymbol{x})) \|^2 + 4\lambda\delta^2 + (l-1)^2 \leq (l+1)^2.
\]
Taking a square root on both sides, we end up with $\sqrt{ \|2L(\boldsymbol{x})^\top (\boldsymbol{u}-\tilde{\boldsymbol{u}}(\boldsymbol{x}))\|^2 + (2\sqrt{\lambda}\delta)^2 + (l-1)^2} \leq l+1$, which is equivalent to the third constraint in \eqref{eq:converted_SOCP}.
\end{proof}
\section{Norm Sum as SOC Constraints}\label{sum_of_norms_soc}
\renewcommand{\thesection}{\Alph{section}}
\begin{lemma}
\label{lemma:sum_of_norms}
The convex problem:
\begin{equation*}
\label{eq:soc_need_convert}
\begin{aligned}
& \min_{\mathbf{z} \in \mathbb{R}^n}\,\, \boldsymbol{v}^\top \mathbf{z}\\
\mathrm{s.t.} \, \,  &\|\mathbf{A}\mathbf{z} - \boldsymbol{a}\| + \|\mathbf{B}\mathbf{z} - \boldsymbol{b}\| \leq \boldsymbol{c}^\top \mathbf{z}
\end{aligned}
\end{equation*}
is equivalent to the second-order cone program:
\begin{equation*}
\label{eq:converted_SOC_constraint}
\begin{aligned}
& \min_{\mathbf{z} \in \mathbb{R}^n, p\in \mathbb{R}, q\in \mathbb{R}}\,\, \boldsymbol{v}^\top \mathbf{z}\\
\mathrm{s.t.} \, \,  &\|\mathbf{A}\mathbf{z} - \boldsymbol{a}\| \leq p \\
& \|\mathbf{B}\mathbf{z} - \boldsymbol{b}\| \leq q \\
&  p+q \leq \boldsymbol{c}^\top \mathbf{z}.
\end{aligned}
\end{equation*}
\end{lemma}

\section{Conclusion}

We considered the problem of enforcing safety and stability of unknown robot systems operating in unknown environments. We showed that accounting for either Gaussian or worst-case error bounds in the system dynamics and safety constraints leads to a novel CLF-CBF-SOCP formulation for control synthesis. We validated our formulations in autonomous navigation tasks, simulating a ground robot in several unknown environments. Some drawbacks of our formulations include that large model error bounds may lead to infeasibility of the robust SOCP, and that the assumption that system dynamics and barrier functions are GPs may not be true in practice. Future work will implement the proposed formulations on a real robot, consider object category pre-training of the SDF neural network, and explore adaptive techniques for safe control synthesis given varying uncertainty levels and robot objectives.

\section{Evaluation}\label{evaluation}

In this section, we present an approach to estimate the unknown dynamics of a mobile robot, and construct CBF constraints online. Then, we evaluate our safe control synthesis using the estimated robot dynamics and CBFs in autonomous navigation tasks in $10$ simulated environments, containing obstacles a priori unknown to the robot.

\begin{figure}[t]
  \centering
  \subcaptionbox{Pybullet Simulator\label{fig:2a}}{\includegraphics[width=0.47\linewidth]{fig/pybullet_env.png}}%
  \hfill%
  \subcaptionbox{Probabilistic Trajectory\label{fig:2b}}{\includegraphics[width=0.48\linewidth]{fig/probabilistic_traj.png}}\\
  \caption{(a) is the Pybullet simulation environment where we conduct our experiments. (b) shows the results in a region of an environment, where the probabilistic ($p=0.2, 0.4, 0.8, 0.99$) controller and QP controller both succeed. The ground-truth obstacle surface is shown in black while the estimated obstacles is shown in orange.}
  \vspace*{-3ex}
\end{figure}

\subsection{System Dynamics Estimation}\label{sec:dynamics_estimate}

We consider a Turtlebot robot simulated in the PyBullet simulator~\cite{coumans2020} (see Fig.~\ref{fig:2a}). We first present a learning approach to model the unknown dynamics of the TurtleBot using training data collected from the PyBullet simulator. The robot state and input are $\mathbf{x} := [x, y,\mu]^\top \in \mathbb{R}^2 \times [-\pi,\pi)$ and $\underline{\bfu} := [1, v,\omega]^\top \in \{1\} \times \mathbb{R}^2$, respectively.
We collect a dataset $\mathcal{D} = \{t_{0:N}^{(i)}, \mathbf{x}_{0:N}^{(i)}, \underline{\bfu}_{0:N}^{(i)}\}_{i=1}^{D}$ of  $D=40000$ state sequences $\mathbf{x}_{0:N}^{(i)}$ obtained by applying random control inputs $\underline{\bfu}_{0:N}^{(i)}$ to the robot with initial condition $\mathbf{x}_{0}^{(i)}$ at time intervals of $\tau = 0.02$ seconds. For each trajectory $i$, a constant control input is applied for $N = 5$ time steps.

We employ a neural ODE network~\cite{chen2018neuralode} to approximate the unknown robot dynamics $F$ with a neural network $F_{\boldsymbol{\theta}}$ based on the dataset $\mathcal{D}$. A forward pass through the ODE network is obtained using an ODE solver:
\begin{equation*}
\{\tilde{\mathbf{x}}_{1}^{i}, \tilde{\mathbf{x}}_{2}^{i}, \cdots, \tilde{\mathbf{x}}_{N}^{i}\} = \text{ODESolve}(\mathbf{x}_0^{i},F_{\boldsymbol{\theta}}(\cdot)\underline{\bfu}^i, t_1^{i}, \cdots, t_N^{i}).
\end{equation*}
We use a loss function,
\begin{equation}
\begin{aligned}
    &\min_{\boldsymbol{\theta}} \sum_{i=1}^{D} \sum_{j=1}^{N} \ell(\mathbf{x}_{j}^{(i)}, \tilde{\mathbf{x}}_{j}^{(i)}), \\
    \text{s.t.} \, \, &\dot{\tilde{\mathbf{x}}}^{(i)}(t) = F_{\boldsymbol{\theta}}(\tilde{\mathbf{x}}^{(i)}(t)) \underline{\bfu}^{(i)}(t), \quad \tilde{\mathbf{x}}^{(i)}(j\tau) = \tilde{\mathbf{x}}^{(i)}_j,\\
    &\underline{\bfu}^{(i)}(t) \equiv \underline{\bfu}^{(i)}_j \;\;\text{for}\; t \in [j\tau,(j+1)\tau),
\end{aligned}
\end{equation}
where $\ell(\mathbf{x}, \tilde{\mathbf{x}}) = \|[x, y,\cos \mu,\sin \mu]^\top \!-\! [\tilde{x}, \tilde{y},\cos \tilde{\mu},\sin \tilde{\mu}]^\top\|^2$. To update the weights $\boldsymbol{\theta}$, the gradient of the loss function is back-propagated by solving another ODE with adjoint states backwards in time. Please refer to \cite{chen2018neuralode} for details.

Gal and Ghahramani~\cite{2015yarin} showed that introducing dropout layers in a neural network is approximately equivalent to performing deep Gaussian Process regression.
We use a $6$-layer fully-connected neural network with $\tanh$ activations and $800$ neurons in each layer to model $F_{\boldsymbol{\theta}}$, and apply dropout to each hidden layer with rate $0.05$.
Given a query state $\mathbf{x} \in {\cal X}$, Monte-Carlo estimates of the predictive mean $\tilde{F}_{\boldsymbol{\theta}}(\mathbf{x})$ and element-wise standard deviation $\tilde{\Sigma}(\mathbf{x})$ of the dynamics are obtained with $T = 100$ stochastic forward passes through the dropout neural network model. We use $\tilde{F}_{\boldsymbol{\theta}}(\mathbf{x})$ for the mean of system dynamics and  $K_F(\mathbf{x},\mathbf{x}) =  \diag(\text{vec}(\tilde{\Sigma}(\mathbf{x}))^2)$ for the variance of the dynamics. To obtain worst-case error bounds $e_F(\mathbf{x})$, we set  $e_F(\mathbf{x}) = \|3.89\tilde{\Sigma}(\mathbf{x})\|$ (99.99\% \textit{confidence}).

In our experiment, no external disturbances are added to the system dynamics model. Given $M = 5000$ random-sampled different state control sequences $\{\mathbf{x}_i,\underline{\bfu}_i\}_{i=1}^{M}$ as test data, we consider the following test-time loss function,
$L = \frac{1}{M} \sum_{i=1}^{M} \ell(F(\mathbf{x}_{i})\underline{\bfu}_i, \tilde{F}(\mathbf{x}_{i})\underline{\bfu}_i)$.
Our learned dynamics model is quite accurate, and the average test loss is $L = 0.0037$.

\subsection{Online CBF Estimation}
\label{sec:sdf_estimates}

\begin{table}[t]
\centering
\caption{Empirical SDF estimation error $\mathcal{E}$ and dropout-network SDF estimation error averaged across $8$ object instances under different LiDAR measurement noise standard deviation $\sigma$.}
\resizebox{\linewidth}{!}{
\begin{tabular}{ |c|c|c| }
\hline
LiDAR Noise $\sigma$ & SDF Empirical Error & SDF Dropout Error\\
\hline
$0.01$ & $0.0173$ & $0.0132$ \\
$0.02$ & $0.0288$ & $0.0184$ \\
$0.05$ & $0.0463$ & $0.0242$ \\
\hline
\end{tabular}}
\label{table: sdf_error}
\vspace*{-1ex}
\end{table}

\begin{figure}[t]
\centering
\subcaptionbox{Training data \label{fig:1a}}{\includegraphics[width=0.25\linewidth]{fig/training_heart.png}}%
\hfill%
\subcaptionbox{Mean SDF \label{fig:1b}}{\includegraphics[width=0.25\linewidth]{fig/mean_heart.png}}%
\hfill%
\subcaptionbox{Variance \label{fig:1c}}{\includegraphics[width=0.245\linewidth]{fig/variance_heart.png}}%
\hfill%
\subcaptionbox{$\mathbb{P}(\tilde{\varphi} \!\leq\! 0) \!=\! 0.95\!$ \label{fig:1d}}{\includegraphics[width=0.25\linewidth]{fig/probability_heart.png}}%
\caption{Shape estimation with dropout neural network. (a) shows the training data.
(b) shows the estimated mean SDF results. The black heart curve shows the ground-truth obstacle boundary, while colored regions are level-sets of the SDF estimate. The white region denotes the estimated obstacle boundary. The blue (resp. red) region denotes negative (resp. positive) signed distance. In (c), the variance of the SDF estimate is shown. In (d), we plot the estimated unsafe region with high probability, where $\mathbb{P}(\tilde{\varphi} \leq 0) = 0.95$.}
\label{fig:sdf_result}
\vspace*{-3ex}
\end{figure}

The robot is equipped with a LiDAR scanner with a $270^\circ$ field of view, $200$ rays per scan, $3$ meter range, and zero-mean Gaussian measurement noise with standard deviation $\sigma \in \{0.01, 0.02, 0.05 \}$. The LiDAR scans are used to estimate the unsafe regions ${\cal O}_i$ in the environment and construct a CBF constraint for each. We rely on the concept of signed distance function (SDF) (e.g. Fig.~\ref{fig:1b}) to describe each ${\cal O}_i$. The SDF function $\varphi_i: \mathbb{R}^2 \mapsto \mathbb{R}$ of set ${\cal O}_i \subseteq \mathbb{R}^2$ is
\begin{equation}
\varphi_i(\mathbf{y}) := \begin{cases}
 -d(\mathbf{y},\partial {\cal O}_i), & \mathbf{y} \in {\cal O}_i, \\
 \phantom{-} d(\mathbf{y},\partial {\cal O}_i), &  \mathbf{y} \notin {\cal O}_i,
 \end{cases}
\end{equation}
where $d$ denotes the Euclidean distance from a point $\mathbf{y} \in \mathbb{R}^2$ and the set boundary $\partial {\cal O}_i$.
We employ incremental training with replay memory (ITRM)~\cite[Sec. IV]{Long_learningcbf_ral21} to estimate an SDF $\varphi_i$ for each ${\cal O}_i$ from the LiDAR measurements. We use a $4$-layer fully-connected neural network with parameters $\boldsymbol{\theta}$ and dropout layers to yield $\tilde{\varphi}_i(\mathbf{y};\boldsymbol{\theta})$ with dropout rate $0.05$ applied to each $512$-neuron hidden layer.
Given $\mathbf{y} \in \mathbb{R}^2$, we obtain the predictive SDF mean $\hat{\varphi}_i(\mathbf{y})$ and standard deviation $\hat{\sigma}_i(\mathbf{y})$ by Monte-Carlo estimation with $T = 20$ stochastic forward passes through the dropout neural network model. When the TurtleBot moves along a circle of radius $2$ while the object is placed at the center, we measure the accuracy of the online SDF method using the empirical SDF error,
$\mathcal{E}_i = \frac{1}{m}\sum_{j=1}^{m} |\hat{\varphi}_i(\mathbf{y}_{j}) |$,
where $\{\mathbf{y}_j\}_{j=1}^{m}$ are $m = 500$ points uniformly sampled on the surface of the object.
In Fig.~\ref{fig:sdf_result}, we show the SDF estimation with measurement noise $\sigma = 0.01$.

Since we deal with system dynamics with relative degree one, one can verify~\cite{Chen2021BackupCB} that the SDF is a valid CBF. Let $\mathbf{z} = [x, y] \in {\cal Z} \subset \mathbb{R}^2$ be the position part of $\mathbf{x}$. To account for the fact that the robot body is not a point mass, we subtract the robot radius $\rho = 0.177$ from each SDF estimate when defining each mean CBF: $\tilde{h}_i(\mathbf{x}) = \tilde{\varphi}_i(\mathbf{z};\boldsymbol{\theta}) - \rho$. For variance $K_h(\mathbf{x},\mathbf{x})$ in Sec.~\ref{sec:prob_control}, we set $K_{h}^{i}(\mathbf{x},\mathbf{x}) = \hat{\sigma}_i^2(\mathbf{z})$. We also take $\nabla \tilde{h}_i(\mathbf{x}) = \nabla \tilde{\varphi}_i(\mathbf{z}; \boldsymbol{\theta})$ and compute ${\cal H}_{\mathbf{x}, \mathbf{x}'}K^i_h(\mathbf{x}, \mathbf{x}')$ by Monte-Carlo estimation using double back-propagation. We set the worst case error bounds $e_h(\mathbf{x})$, $e_{\nabla h}(\mathbf{x})$ in Sec.~\ref{sec:robust_control} as the $99.99\%$ confidence bounds of a Gaussian random variable with standard deviation $\hat{\sigma}_i(\mathbf{z})$. If the robot observes multiple obstacles in the environment, we compute multiple CBFs $\tilde{h}_i(\mathbf{x})$ and their corresponding uncertainty, and add multiple CBCs to \eqref{eq:QP_origin}, \eqref{eq:gaussian_socp_form}, \eqref{eq:worst_case_SOCP_formulation} for safe control synthesis.

\subsection{Safe Navigation}
\label{sec: safe_navigation}

\begin{table}[t]
\centering
\caption{Success rate of the navigation tasks in $100$ realizations ($10$ realizations for each of the $10$ different environments) using the Probabilistic CLF-CBF-SOCP, Robust CLF-CBF-SOCP, and the original CLF-CBF-QP frameworks for different LiDAR measurement noise levels $\sigma$.}
\resizebox{\linewidth}{!}{
\begin{tabular}{ |l|c|c|c|c|c| }
\hline
\multirow{2}{1cm}{LiDAR Noise $\sigma$} & \multirow{2}{1.4cm}{QP Success Rate} & \multicolumn{3}{c|}{Probabilistic Success Rate} & \multirow{2}{1.5cm}{Robust Success Rate}\\\cline{3-5}
&  & $p = 0.2$ & $p = 0.4$ & $p = 0.8$ &  \\
\hline
$0.01$ & $0.82$ & $0.98$ & $1.0$ & $1.0$ & $1.0$ \\
$0.02$  & $0.65$ & $0.92$ & $0.97$ & $1.0$ & $1.0$\\
$0.05$   &  $0.37$ & $0.72$ & $0.89$ & $0.96$ & $1.0$ \\\hline
\end{tabular}}
\label{table:noise_diff}
\caption{Fr\'echet distance between the reference path and the robot trajectories generated by the Probabilistic CLF-CBF-SOCP, Robust CLF-CBF-SOCP, and the CLF-CBF-QP controllers (smaller values indicate larger trajectory similarity, the value in the parentheses indicates the success rates while values without parentheses indicate the success rate is $1$}, and N/A indicates the robot collides with obstacles in all $10$ realizations).
\resizebox{\linewidth}{!}{
\begin{tabular}{ |l|c|c|c|c|c| }
\hline
\multirow{2}{*}{Env} & \multirow{2}{*}{QP} & \multicolumn{3}{c|}{Probabilistic} & \multirow{2}{*}{Robust} \\\cline{3-5}
&  & $p = 0.2$ & $p = 0.4$ & $p = 0.8$ &  \\
\hline
1 & $0.337$ & $0.338$ & $0.343$ & $0.363$ & $0.357$\\
2  & $0.378$& $0.408$ & $0.404$ & $0.432$ & $0.485$ \\
3   & $0.372$ & $0.398$ & $0.412$ & $0.457$ & $0.538$\\
4 & $0.416$&  $0.438$ & $0.427$ & $0.473$ & $0.515$ \\
5  & $0.395$ &  $0.418$ & $0.412$ & $0.483$ & $0.572$\\
6   & $0.385 \: (0.8)$ & $0.371$ & $0.378$ & $0.392$ & $0.424$\\
7 & $0.462 \: (0.5)$ & $0.502$ & $0.546$ & $0.593$ & $0.737$\\
8  &  $0.535 \: (0.2)$ & $0.588$ & $0.612$ & $0.673$ & $0.814$\\
9  &  N/A & $0.756 \: (0.8)$& $0.887 \: (0.9)$ & $0.926$ & $1.016$\\
10  &  N/A &$0.905 \: (0.4)$ & $0.937 \: (0.8)$ & $1.046$ & $1.224$\\
\hline
\end{tabular}}
\label{table:traj_diff}
\vspace*{-3ex}
\end{table}

\begin{figure}[t]
  \centering
  \subcaptionbox{Noise: $\sigma = 0.01$\label{fig:3a}}{\includegraphics[width=0.4\linewidth]{fig/result_noise001.png}}%

  \subcaptionbox{Noise: $\sigma = 0.02$\label{fig:3b}}{\includegraphics[width=0.4\linewidth]{fig/result_noise002_new1.png}} \\
  \centering
  \subcaptionbox{Safe Trajectory Tracking\label{fig:3c}}{\includegraphics[width=0.7\linewidth]{fig/env8_result.png}}
  \caption{Performance comparison among the three controllers. Ground-truth obstacle surfaces are shown as black curves. The mean of the estimated obstacles, obtained after the whole path is traversed by the probabilistic CLF-CBF-SOCP controller are shown in different colors (red, green, orange, blue). The trajectories generated by the probabilistic and robust CLF-CBF-SOCP controllers are in red and blue, respectively, while the CLF-CBF-QP trajectory is in green. The starting point is cyan and the goal region is a light-green disk. The robot (purple disk in (b) and (c)), controlled by the CLF-CBF-QP controller, collides with obstacles and does not reach the goal. In (a) and (b), we compare the controller performance under different LiDAR noise level for a same environment. In (a), the results are collected under LiDAR measurement noise $\sigma = 0.01$. In (b), the results are collected under LiDAR noise $\sigma = 0.02$. In (c), the trajectory tracking results of environment $8$ is shown and the reference path is shown in blue.}
  \label{fig:compare_result}
  \vspace*{-3ex}
\end{figure}

Our main experiments demonstrate safe navigation and safe trajectory tracking using the proposed probabilistic~\eqref{eq:gaussian_socp_form} and robust~\eqref{eq:worst_case_SOCP_formulation} CLF-CBF-SOCP formulations, utilizing the dynamics estimates from Sec. VI.A and the online CBF estimates from Sec. VI.B. To emphasize the importance of accounting for estimation errors, we also implement the original CLF-CBF-QP controller~\eqref{eq:QP_origin}, which assumes the estimated barrier functions and system dynamics are accurate (i.e., uses the mean values from the dropout-network estimation as the true values). In all three controllers, we set $L(\mathbf{x}) = \diag([0, 10, 3])$ and $\underline{\tilde{\mathbf{k}}}(\mathbf{x}) = [1, v_{\max}, 0]^\top$ where $v_{\max} = 0.65$ is the maximum linear velocity for the TurtleBot. The remaining parameters were $\lambda = 1000$, $\alpha_V(V(\mathbf{x})) = 2V(\mathbf{x})$, and $\alpha_h(h_i(\mathbf{x})) = h_i(\mathbf{x})$.

In the first set of experiments (Fig.~\ref{fig:3a} and Fig.~\ref{fig:3b}), we demonstrate safe navigation to a goal point with a CLF candidate $V(\mathbf{x}) = (x-2)^2 + (y-3)^2$. In Fig.~\ref{fig:3a}, when the LiDAR noise level is low, the robot controlled by all three controllers succeeds to reach the goal region and the SOCP formulations are slightly more conservative than the QP formulation. In Fig.~\ref{fig:3b}, the LiDAR noise level increases to $\sigma = 0.02$ and we can observe major differences among the paths generated by the three controllers. This is because the estimated variance and error bounds of the barrier function and its gradient increase with the increase of the LiDAR noise. The robot controlled by the CLF-CBF-QP controller collides with an obstacle,  while the robot controlled by  probabilistic or robust SOCP controller succeeds in avoiding obstacles. Importantly, the robot controlled by the robust SOCP controller switches to bypass the round obstacle from the right because controller cannot find a feasible path on the left with larger error bounds on estimated barrier functions.

In the following set of experiments, we consider the problem of safe trajectory tracking using the approach in \cite[Sec. VI]{Long_learningcbf_ral21} to construct a valid CLF $V(\mathbf{x})$ for path following.
In Table~\ref{table:noise_diff}, we report the success rate of the trajectory tracking task using the proposed formulations and the original QP framework under different measurement noises. As the noise increases, the success rate of the CLF-CBF-QP controller decreases rapidly, while the success rate of the probabilistic framework with high $p$ and the robust framework stays high.

In Fig.~\ref{fig:3c}, we show one realization in environment $8$ (with $\sigma = 0.01$), where the CLF-CBF-QP controller fails to avoid obstacles because it does not consider the errors in $\textit{CBC}(\mathbf{x},\underline{\bfu})$, while the proposed frameworks guarantee safety.
When there is an obstacle near or on the reference path, the robot controlled by the robust SOCP controller stays furthest away, while the probabilistic SOCP controller also guarantees the robot stays further away from the obstacles than the robot controlled by the CLF-CBF-QP controller.

In Table~\ref{table:traj_diff}, we show quantitative results using the Fr\'echet distance ~\cite[Sec. VI]{Long_learningcbf_ral21} as the metric to measure trajectory similarities. The distance value is computed by averaging the successful realizations in each environment, and the LiDAR noise is set to be $\sigma = 0.02$ in this set of experiments. We see that the robust CLF-CBF-SOCP controller is most conservative as it has the largest Fr\'echet distance values while the probabilistic CLF-CBF-SOCP controller is less conservative if we set the user-specified risk tolerance $p = 0.8$. By lowering the risk tolerance value ($p = 0.2/0.4$), the robot with the probabilistic controller follows the reference path better while facing a higher risk of collision. A qualitative result is shown in Fig.~\ref{fig:1b}, where larger $p$ values indicates higher probability of being safe for the robot. The trajectory generated by the CLF-CBF-QP controller has the smallest Fr\'echet distance values, but fails in several environments.

Finally, to demonstrate the efficiency of the proposed formulations, we compare the average time needed for solving the QP, probabilistic SOCP, and robust SOCP formulations per control synthesis along the trajectory tracking task. All optimization problems are solved using the Embedded Conic Solver in CVXPY~\cite{cvxpy} with an Intel i7 9700K CPU. The time needed for solving one QP instance is $0.00863s$ while the times needed for solving the proposed probabilistic and robust SOCPs are $0.0109s$ and $0.0122s$. As expected, our SOCP formulations require slightly more time than the original QP but are still suitable for online robot navigation.

\section{Introduction}
\label{sec: intro}

Autonomous robotic systems are increasingly employed in warehouse and home automation, transportation, and security applications. A crucial aspect of successfully deploying such systems is the satisfaction of safety and stability requirements, even in the presence of uncertainty in the system model or constraints. The notion of safety in the context of program correctness was first introduced in the $1970$'s \cite{lamport1977safety}. Around the same time, Artstein \cite{Artstein1983StabilizationWR} introduced control Lyapunov functions (CLFs) to enforce stability in the context of nonlinear system control. The seminal work of Sontag \cite{SONTAG1989117} established a universal formula for constructing feedback control laws that stabilize nonlinear systems. In the $2000$'s, barrier certificates were proposed to formally prove the safety of closed-loop nonlinear and hybrid systems \cite{barrier-certificate, barrier-hybrid}. Control barrier functions (CBFs) were developed to support task-independent safe control synthesis, serving as a barrier certificate for a closed-loop nonlinear system \cite{wieland2007}.

A key observation is that, for control-affine systems, the CLF and CBF conditions are linear in the control input, allowing a formulation of safe and stable control synthesis as a quadratic program (QP) \cite{ames2016control, nguyen2016acc, cbf}.
CLF-CBF-QP techniques have been successfully employed in a variety systems, including aerial robots \cite{Wang2017SafeCM}, walking robots \cite{nguyen2016cdc}, and automotive systems \cite{xu2017realizing}. Most existing work, however, assumes complete knowledge of the system dynamics and control barrier functions. In reality, the dynamics model and safety constraints are obtained using noisy sensor data and simplifying assumptions, leading to uncertainty and errors that should be captured when ensuring safety and stability.

Capturing system-model and barrier-function estimation errors impacts the formulation of CLF and CBF constraints, and no longer give rise to QPs. Our main contribution is to show that such uncertainty-aware stability and safety constraints can still be formulated as convex constraints under two different models of uncertainty: probabilistic and worst-case.
To capture probabilistic uncertainty, we specifically consider \emph{Gaussian Process} (GP) regression as an example approach for modeling a probability distribution over a function space.
When the estimated barrier function and system dynamics are described by a GP, we aim to ensure probabilistic safety and stability up to a user-specified risk tolerance. We compute the distribution of the CLF and CBF constraints, and use Cantelli's inequality \cite{CantelliSuiCD} to bound the computed means with a margin dependent on the variances and the desired risk-tolerance. The control input appears linearly in the mean and quadratically in the variance of the CLF and CBF constraints. This allows us to restate the probabilistic constraints as second-order cone constraints, leading to a second-order cone program (SOCP), which is convex and can be solved efficiently online.

When \emph{worst-case error bounds} on the system dynamics, barrier function and its gradient are given, we formulate a robust safe control
synthesis problem. Under worst-case disturbances, we show that the input appears both linearly and within a norm term in the CLF and CBF constraints. Like the probabilistic formulation, the original QP problem can be reformulated as a convex SOCP for safe control synthesis.

We demonstrate our safe control synthesis techniques in mobile robot navigation simulations. We consider a robot tasked to follow a desired path in an unknown environment, relying on online noisy obstacle sensing and offline dynamic model estimation to ensure safety and stability. We show that both the probabilistic and the robust CLF-CBF-SOCP formulation allows the robot to safely track the deisred path.

In summary, we make the following \textbf{contributions}. First, we formulate novel probabilistic safety and stability constraints by considering stochastic uncertainty in the barrier functions and system dynamics. Second, we formulate novel robust safety and stability constraints by considering worst-case error bounds in the barrier functions and system dynamics. Finally, we show that either the probabilistic or the worst-case formulations lead to a (convex) SOCP, enabling efficient synthesis of safe and stable control.

\subsection{Related work for reference}

\section{Preliminaries}
\label{sec:preliminary}
\subsection{Signed Distance Function}
Throughout the paper, we use the concept of signed distance function (SDF) to describe each unsafe region $\{v(\mathcal{O}_i)\}_{i=1}^K$. For each $i$, the SDF function $\varphi_i: \mathcal{Y} \mapsto \mathbb{R}$ is:
\begin{equation}
\varphi_{i}(\boldsymbol{y}) := \begin{cases}
 -d(\boldsymbol{y},\partial v({\cal O}_i)), & \boldsymbol{y} \in v({\cal O}_i), \\
 \phantom{-} d(\boldsymbol{y},\partial v({\cal O}_i)), &  \boldsymbol{y} \notin v({\cal O}_i),
 \end{cases}
\end{equation}
where $d$ denotes the Euclidean distance between a point and a set.

\subsection{Control Lyapunov Function}

A \emph{control Lyapunov function (CLF)} for the system dynamics in \eqref{eq: dynamic} is a continuously differentiable function $V: \mathbb{R}^n \mapsto {\mathbb{R}_{\geq 0}}$ for which there exist a class $\mathcal{K}$ function $\alpha_V$ such that, for all $\boldsymbol{x} \in \mathcal{X}$:
\begin{align*}

    \inf_{\boldsymbol{u}\in \mathcal{U}}[ \mathcal{L}_f V(\boldsymbol{x}) + \mathcal{L}_g V(\boldsymbol{x})\boldsymbol{u} + \alpha_V( V(\boldsymbol{x}))  ] \leq 0 .
\end{align*}

The function $V$ may be used to encode a variety of control objectives, including path following.

\subsection{Control Barrier Function}

\begin{definition}[{\cite{ames2016control}}]
A continuously differentiable function $h: \mathbb{R}^n \mapsto {\mathbb{R}}$, with $h(\boldsymbol{x}) >0$ if $\boldsymbol{x} \in \operatorname{Int}(\mathcal{S})$ and $h(\boldsymbol{x}) =0$ if $\boldsymbol{x} \in \partial \mathcal{S}$, is a \emph{zeroing control barrier function (ZCBF)} on $\mathcal{X} \subset \mathbb{R}^n$ if there exists an extended class $\mathcal{K}$ function $\alpha_h$ such that, for each $\boldsymbol{x} \in \mathcal{X}$, there exists $\boldsymbol{u} \in \mathcal{U}$ with
\begin{equation}\label{eq:cbf}
    \mathcal{L}_f h(\boldsymbol{x}) + \mathcal{L}_g h(\boldsymbol{x})\boldsymbol{u} + \alpha_h (h(\boldsymbol{x})) \geq 0,
\end{equation}
where $\mathcal{L}_f h(\boldsymbol{x})$ is the Lie derivative of $h(\boldsymbol{x})$ along $f(\boldsymbol{x})$.
\end{definition}

Any Lipschitz-continuous controller $\boldsymbol{u}: \mathcal{X} \mapsto \mathcal{U}$ such that $\boldsymbol{u}(\boldsymbol{x})$ satisfies \eqref{eq:cbf} renders the set $\mathcal{S}$ forward invariant for the control-affine system \eqref{eq: dynamic} \cite{cbf, ames2016control}.

When the barrier function is precisely known, one can combine CLF and CBF constraints to synthesize a safe controller via the following QP:
\begin{equation}
\label{eq:QP_origin}
\begin{aligned}
& \min_{\boldsymbol{u} \in {\cal U},\delta \in \mathbb{R}}\,\, \|L(\boldsymbol{x})^\top(\boldsymbol{u} - \tilde{\boldsymbol{u}}(\boldsymbol{x}))\|^2 + \lambda \delta^2\\
\mathrm{s.t.} \, \,  &\mathcal{L}_f V(\boldsymbol{x}) + \mathcal{L}_g V(\boldsymbol{x})  \boldsymbol{u} + \alpha_V(V(\boldsymbol{x})) \leq \delta \\
&\mathcal{L}_{f} h(\boldsymbol{x}) + \mathcal{L}_{g} h(\boldsymbol{x})\boldsymbol{u} + \alpha_h(h(\boldsymbol{x})) \geq 0,
\end{aligned}
\end{equation}

where $\tilde{\boldsymbol{u}}(\boldsymbol{x})$ is a baseline controller, $L(\mathbf{x})$ is a matrix penalizing control effort, and $\delta \geq 0$ (with the corresponding penalty $\lambda$) is a slack variable, introduced to relax the CLF constraints in order to ensure the feasibility of the QP. The baseline controller $\tilde{\boldsymbol{u}}(\boldsymbol{x})$ is used to specify additional control requirements such as desirable velocity or orientation but may be set to $\tilde{\boldsymbol{u}}(\boldsymbol{x}) \equiv 0$ if minimum control effort is the main objective. The QP formulation in \eqref{eq:QP_origin} modifies $\tilde{\boldsymbol{u}}(\boldsymbol{x})$ online to ensure safety and stability via the CBF and CLF constraints.

\section{Problem Formulation}\label{sec:problem}

Consider a robot with dynamics model:
\begin{equation}\label{eq: dynamic}
\begin{aligned}
    \dot{\mathbf{x}} = f(\mathbf{x}) + g(\mathbf{x}) \mathbf{u} &=     [f(\mathbf{x}) \; g(\mathbf{x})] \cdot\begin{bmatrix}
    1 \\
    \mathbf{u}
    \end{bmatrix} \mathrel{\ensurestackMath{\stackon[1pt]{=}{\scriptstyle\Delta}}} F(\mathbf{x})\underline{\bfu},
\end{aligned}
\end{equation}
where $\mathbf{x} \in {\cal X} \subseteq \mathbb{R}^{n}$ is the robot state and $\underline{\bfu} \in \underline{\mathcal{U}} = \{1\} \times \mathbb{R}^{m}$ is the control input.\footnotemark{} We assume $f : \mathbb{R}^{n} \mapsto \mathbb{R}^{n}$ and $g : \mathbb{R}^{n} \mapsto \mathbb{R}^{n \times m}$ are continuously differentiable.

\begin{definition}
A continuously differentiable function $V: \mathbb{R}^n \mapsto {\mathbb{R}_{\geq 0}}$ is a \emph{control Lyapunov function (CLF)} for the system \eqref{eq: dynamic} if there exists a class $\mathcal{K}$ function $\alpha_V$ such that:
\begin{equation}
    \inf_{\underline{\bfu} \in \underline{\mathcal{U}}} \textit{CLC}(\mathbf{x},\underline{\bfu}) \leq 0, \quad \forall \mathbf{x} \in {\cal X},
\end{equation}
where the \emph{control Lyapunov condition (CLC)} is:
\begin{equation}\label{eq:clc_define}
\begin{aligned}
    \textit{CLC}(\mathbf{x},\underline{\bfu}) &\mathrel{\ensurestackMath{\stackon[1pt]{=}{\scriptstyle\Delta}}} \mathcal{L}_f V(\mathbf{x}) + \mathcal{L}_g V(\mathbf{x})\mathbf{u} + \alpha_V( V(\mathbf{x})) \\

    & =  [\nabla_{\mathbf{x}} V(\mathbf{x})]^\top F(\mathbf{x})\underline{\bfu} + \alpha_{V}(V(\mathbf{x})).
\end{aligned}
\end{equation}
\end{definition}

A CLF $V$ may be used to encode a variety of control objectives, including path following \cite{Long_learningcbf_ral21}, adaptive cruise control \cite{xu2017realizing}, and bipedal robot walking \cite{nguyen2016cdc}.

\footnotetext{\textbf{Notation}: We denote by $\mathbf{I}_n \in \mathbb{R}^{n \times n}$ the identity matrix and $\partial {\cal A}$ the boundary of a set ${\cal A} \subset \mathbb{R}^n$. For a vector $\mathbf{x}$ and a matrix $\mathbf{X}$, we use $\|\mathbf{x}\|$ and $\| \mathbf{X}\|$ to denote the Euclidean norm and the spectral norm. We use $\text{vec}(\mathbf{X}) \in \mathbb{R}^{nm}$ to denote the vectorization of $\mathbf{X} \in \mathbb{R}^{n \times m}$, obtained by stacking its columns.
We denote by $\nabla$ the gradient and ${\cal L}_fV  = \nabla V \cdot f$ the Lie derivative of a differentiable function $V$ along a vector field $f$. We use $\otimes$ to denote the Kronecker product and $\mathcal{GP}(\mu(\mathbf{x}), K(\mathbf{x},\mathbf{x}'))$ to denote a Gaussian Process distribution with mean function $\mu(\mathbf{x})$ and covariance function $K(\mathbf{x},\mathbf{x}')$. A continuous function $\alpha: [0,a)\rightarrow [0,\infty )$ is of class ${\cal K}$ if it is strictly increasing and $\alpha(0) = 0$, and it is of class ${\cal K}_{\infty}$ and $\lim_{r \rightarrow \infty} \alpha(r) = \infty$.
}

To define safety requirements for the control objective, consider a continuously differentiable function $h: \mathbb{R}^n \mapsto {\mathbb{R}}$, which implicitly defines a (closed) safe set of system states $\mathcal{S}  \mathrel{\ensurestackMath{\stackon[1pt]{=}{\scriptstyle\Delta}}} \{\mathbf{x} \in \mathcal{X} \; | \; h(\mathbf{x}) \geq 0\}$. The following definition is a useful tool to ensure that $\mathcal{S}$ is forward invariant, i.e., the robot state remains in $\mathcal{S}$ throughout its evolution.

\begin{definition}
A continuously differentiable function $h: \mathbb{R}^n \mapsto {\mathbb{R}}$ is a \emph{control barrier function (CBF)} on $\mathcal{X} \subseteq \mathbb{R}^n$ for \eqref{eq: dynamic} if there exists an extended class $\mathcal{K}_{\infty}$ function $\alpha_h$ with:
\begin{equation}\label{eq:cbf}
    \sup_{\underline{\bfu}\in \underline{\mathcal{U}}} \textit{CBC}(\mathbf{x},\underline{\bfu}) \geq 0, \quad \forall \mathbf{x} \in {\cal X},
\end{equation}
where the \emph{control barrier condition (CBC)} is:
\begin{equation}
\label{eq:cbc_define}
\begin{aligned}
    \textit{CBC}(\mathbf{x},\underline{\bfu}) & \mathrel{\ensurestackMath{\stackon[1pt]{=}{\scriptstyle\Delta}}} \mathcal{L}_f h(\mathbf{x}) + \mathcal{L}_g h(\mathbf{x})\mathbf{u} + \alpha_h (h(\mathbf{x})) \\
    & = [\nabla_{\mathbf{x}} h(\mathbf{x})]^\top F(\mathbf{x})\underline{\bfu} + \alpha_{h}(h(\mathbf{x})).
\end{aligned}
\end{equation}
\end{definition}

According to~\cite{cbf,ames2016control}, any Lipschitz-continuous controller $\underline{\mathbf{k}}: \mathcal{X} \mapsto \underline{\mathcal{U}}$ that satisfies $\textit{CBC}(\mathbf{x},\underline{\mathbf{k}}(\mathbf{x})) \geq 0$ for all $\mathbf{x} \in \mathcal{X}$ renders the set $\mathcal{S}$ forward invariant for the system~\eqref{eq: dynamic}.

\subsection{Safety and Stability with Known System Dynamics and Barrier Function}

When the system dynamics $F(\mathbf{x})$ and barrier function $h(\mathbf{x})$ are known, a safe controller can be synthesized by combining CLF and CBF constraints in a quadratic program:
\begin{equation}\label{eq:QP_origin}
\begin{aligned}
& \min_{\underline{\bfu} \in \underline{{\cal U}},\delta \in \mathbb{R}}\,\, \|L(\mathbf{x})^\top(\underline{\bfu} - \underline{\tilde{\mathbf{k}}}(\mathbf{x}))\|^2 + \lambda \delta^2,\\
\mathrm{s.t.} \, \,  &\textit{CLC}(\mathbf{x},\underline{\bfu}) \leq \delta,\; \textit{CBC}(\mathbf{x},\underline{\bfu}) \geq 0.
\end{aligned}
\end{equation}
The term $\underline{\tilde{\mathbf{k}}}(\mathbf{x})$ is a baseline controller and may be used to specify additional control requirements, such as desirable velocity or orientation. This term may be set to $\underline{\tilde{\mathbf{k}}}(\mathbf{x}) \equiv \boldsymbol{e}_1$ if minimum control effort is the main objective. The term $L(\mathbf{x})$ is a weighting matrix penalizing deviation from the baseline controller. The term $\delta \geq 0$
is a slack variable that relaxes the CLF constraints to ensure the feasibility of the QP, controlled by the scaling factor $\lambda > 0$. The QP formulation in \eqref{eq:QP_origin} modifies the baseline controller $\underline{\tilde{\mathbf{k}}}(\mathbf{x})$ online to ensure safety and stability via the CBF and CLF constraints.

We focus on enforcing safety and stability for the control-affine system in \eqref{eq: dynamic} when the system dynamics $F(\mathbf{x})$ and the barrier function $h(\mathbf{x})$ are \emph{unknown} and need to be estimated from data.
We present an approach for estimating the system dynamics and barrier functions from data in Sec.\ref{sec:dynamics_estimate} and Sec.\ref{sec:sdf_estimates}, respectively. Our main goal is to develop techniques for safe and stable control synthesis with the estimated $F(\mathbf{x})$ and $h(\mathbf{x})$. We consider two scenarios, depending on whether probabilistic or worst-case error descriptions of the dynamics and barrier functions are available.

\subsection{Safety and Stability with Gaussian Process Distributed System Dynamics and Barrier Function}

When the system dynamics and barrier functions can be described as GPs, we consider the following probabilistic control synthesis problem.

\begin{problem}[\textbf{Safety and stability under Gaussian uncertainty}]\label{prob: prob_clf_cbf}
Given an estimated distribution on the unknown system dynamics $\text{vec}(F(\mathbf{x})) \sim \mathcal{GP}(\text{vec}(\tilde{F}(\mathbf{x})), K_{F}(\mathbf{x},\mathbf{x}'))$ and an estimated distribution on the barrier function $h(\mathbf{x}) \sim \mathcal{GP}(\tilde{h}(\mathbf{x}),K_h(\mathbf{x},\mathbf{x}'))$, design a feedback controller $\underline{\mathbf{k}}$ such that, for each $\mathbf{x} \in {\cal X}$:
\begin{equation*}
    \mathbb{P}(\textit{CLC}(\mathbf{x}, \underline{\mathbf{k}}(\mathbf{x})) \leq \delta) \geq p, \quad \mathbb{P}(\textit{CBC}(\mathbf{x}, \underline{\mathbf{k}}(\mathbf{x})) \geq 0) \geq p,
\end{equation*}
where $p \in (0,1)$ is a user-specified risk tolerance.
\end{problem}

\subsection{Safety and Stability with Worst-Case Uncertainty in System Dynamics and Barrier Function}

Many robotic systems require instead the guarantee that safety and stability hold under all possible error realizations,
which motivates us to also consider the following problem.

\begin{problem}[\textbf{Safety and stability under worst-case uncertainty}]\label{prob: robust_clf_cbf}
Given estimated system dynamics $\tilde{F}(\mathbf{x})$ with known error bound $e_F(\mathbf{x})$,
\begin{equation}
\label{error_dynamics}
    \|F(\mathbf{x}) - \tilde{F}(\mathbf{x})\| \leq e_F(\mathbf{x}) , \; \forall \mathbf{x} \in {\cal X} ,
\end{equation}
and estimated barrier function $\tilde{h}(\mathbf{x})$ and gradient $\nabla \tilde{h}(\mathbf{x})$ with known error bounds $e_h(\mathbf{x})$ and $e_{\nabla h}(\mathbf{x})$, i.e., for all $\mathbf{x} \in {\cal X}$,
\begin{equation}
\label{error_barrier}
     |h(\mathbf{x}) -\tilde{h}(\mathbf{x})| \leq e_h(\mathbf{x}) , \;
    \|\nabla h(\mathbf{x}) - \nabla \tilde{h}(\mathbf{x})\|  \leq e_{\nabla h}(\mathbf{x}),
\end{equation}
design a feedback controller $\underline{\mathbf{k}}$ such that, for each $\mathbf{x} \in {\cal X}$:
\begin{equation*}
    \textit{CLC}(\mathbf{x}, \underline{\mathbf{k}}(\mathbf{x})) \leq \delta, \quad \textit{CBC}(\mathbf{x}, \underline{\mathbf{k}}(\mathbf{x})) \geq 0.
\end{equation*}
\end{problem}

\section{Related Work}
\label{sec: related_work}

This section reviews recent works on safe control synthesis that address uncertainty due to unmodeled dynamics, input disturbances, and barrier function estimation.

Jankovic \cite{jankovic_robust_2018} considers worst-case disturbance bounds on the system dynamics and proposes robust CBF formulations. Eman \textit{et al.} \cite{yousef2019cdc} utilize convex hulls to model disturbances in a CBF-based safety framework.
Clark \cite{andrew2019acc} considers stochastic control systems with incomplete information and derives sufficient conditions for ensuring safety on average.
Nguyen and Sreenath \cite{Nguyen2021} formulate a robust CLF-CBF QP by introducing robust constraints to guarantee stability and safety under model uncertainty. Hewing \textit{et al.} \cite{lukas_2020_TCST} present a model predictive control (MPC) approach that integrates a nominal system with a residual part modeled as a GP. Compared to our formulation, this approach enables optimizing the control performance over a longer future horizon but requires time discretization and convexification of the safety constraints. In contrast, our formulations operate in continuous time and handle general safe set descriptions. Ahmadi \textit{et al.} \cite{Ahmadi2020RiskSensitivePP} introduce a conditional value-at-risk (CVaR) barrier function to ensure safety for systems with stochastic uncertainty. The approach guarantees safety with high probability even for worst-case scenarios but the computation cost is high and the formulation is restricted to linear systems. Our approach enables efficient control synthesis for general control-affine systems. Another line of research formulates safe control synthesis as trajectory optimization. Alcan and Kyrki \cite{Alcan_DDP_RAL} employ differential dynamics programming (DDP) to enforce safety under additive uncertainty. In \cite{Hassan_DBaS_RAL}, the DDP idea is combined with CBF to introduce a barrier state formulation for safety of discrete-time systems.

\emph{Input-to-state safety} (ISSf) was introduced in \cite{romdlony2016cdc} to handle input disturbances and was used in \cite{Kolathaya2019issf} to enlarge a safe set by modifying a CBF. Alan \textit{et al.} \cite{alan2021safe} introduce a tunable ISSf-CBF for safe control synthesis while reducing conservatism. Cosner \textit{et al.} in \cite{cosner2021measurement} introduce measurement-robust CBFs to account for uncertainty in state estimation and conduct experiments on a Segway.

Srinivasan \textit{et al.} \cite{srinivasan2020synthesis} estimate barrier functions online using a Support Vector Machine and solve a CLF-CBF QP to generate safe control inputs. Zhang \textit{et al.} \cite{zhang2021adversarially} construct robust output CBFs from safe expert demonstrations while considering worst-case error bounds in the measurement map and system dynamics.

This paper unifies and extends our prior work \cite{dhiman2020control, Long_learningcbf_ral21} by considering safe control synthesis with uncertainty in the system dynamics and the barrier function simultaneously and studying two separate cases of probabilistic and worst-case uncertainty. In contrast, \cite{dhiman2020control} only considered probabilistic uncertainty in the dynamics using Gaussian process regression, while \cite{Long_learningcbf_ral21} only considered worst-case error bounds in the barrier function. We show that in either case the safe control synthesis problem is a convex SOCP, which enables efficient safe and stable control synthesis online.

redpajama_set_name: RedPajamaArXiv


In 1984, 26 members founded the FZI Friends' Association, an association of affiliated companies, organisations and individuals, to foster the connection between the research and development and its application in practice.
Today, about 80 companies, organisations and individuals involve themselves in the FZI Friends' Association. They benefit from the association's network to exchange knowledge about methodical, technological and economic developments in the field of innovative information technology.
At the FZI Friends' Association, representatives from industry and research join in an active knowledge exchange.
The FZI Friends' Association organizes informative events, where members present and discuss their company's latest research trends and scientific developments.
Become a member at FZI Friends' Association and benefit from the advantageous knowledge exchange between industry and research!
Companies, organisations and individuals can become a member at FZI Friends' Association. A membership also enhances the opportunity to benefit from FZI's competence in research by initiating, planning and realizing new projects.


text:
Archiearis extrema är en fjärilsart som beskrevs av Hans Rebel 1910. Archiearis extrema ingår i släktet Archiearis och familjen mätare. Inga underarter finns listade i Catalogue of Life.

Källor

Mätare
extrema

redpajama_set_name: RedPajamaWikipedia