# Model-Based Safe Reinforcement Learning with Time-Varying State and Control
Constraints: An Application to Intelligent Vehicles

Xinglong Zhang , Yaoqian Peng, Biao Luo , Wei Pan , Xin Xu , and Haibin Xie
This work has been submitted to the IEEE for possible publication. Copyright
may be transferred without notice, after which this version may no longer be
accessible. Xinglong Zhang, Yaoqian Peng, Xin Xu, and Haibin Xie are with the
College of Intelligence Science and Technology, National University of Defense
Technology, Changsha, China. Biao Lu is with the School of Automation, Central
South University, Changsha, China. Wei Pan is with the Department of Cognitive
Robotics, Delft University of Technology, the Netherlands. Corresponding to:
<EMAIL_ADDRESS><EMAIL_ADDRESS>
###### Abstract

In recent years, safe reinforcement learning (RL) with the actor-critic
structure has gained significant interest for continuous control tasks.
However, achieving near-optimal control policies with safety and convergence
guarantees remains challenging. Moreover, few works have focused on designing
RL algorithms that handle time-varying safety constraints. This paper proposes
a safe RL algorithm for optimal control of nonlinear systems with time-varying
state and control constraints. The algorithm’s novelty lies in two key
aspects. Firstly, the approach introduces a unique barrier force-based control
policy structure to ensure control safety during learning. Secondly, a multi-
step policy evaluation mechanism is employed, enabling the prediction of
policy safety risks under time-varying constraints and guiding safe updates.
Theoretical results on learning convergence, stability, and robustness are
proven. The proposed algorithm outperforms several state-of-the-art RL
algorithms in the simulated Safety Gym environment. It is also applied to the
real-world problem of integrated path following and collision avoidance for
two intelligent vehicles – a differential-drive vehicle and an Ackermann-drive
one. The experimental results demonstrate the impressive sim-to-real transfer
capability of our approach, while showcasing satisfactory online control
performance.

###### Index Terms:

Safe reinforcement learning, time-varying constraints, barrier force, multi-
step policy evaluation.

##  I Introduction

Reinforcement learning (RL) is promising for solving nonlinear optimal control
problems and has received significant attention in the past decades, see [1,
2] and the references therein. Until recently, significant progress has been
made on RL with the actor-critic structure for continuous control tasks [1, 3,
4, 5, 6, 7, 8, 9]. In actor-critic RL, the value function and control policy
are represented by the critic and actor networks, respectively, and learned
via extensive policy exploration and exploitation. However, the resulting
learning-based control system might not guarantee safety for systems with
state and stability constraints. It is known that safety constraint
satisfaction is crucial besides optimality in many real-world robot control
applications [10, 11]. For instance, autonomous driving has been viewed as a
promising technology that will bring fundamental changes to everyday life.
Still, one of the crucial issues concerns how to learn to drive safely under
dynamic and unknown environments with unexpected obstacles [12]. For these
practical reasons, many safe RL algorithms have been recently developed for
safety-critical systems, see e.g. [11, 13, 10, 14, 15, 16, 17, 18, 19, 20, 21,
22, 23, 24, 25] and the references therein. Note that there are fruitful works
in adaptive control with constraints, but the technique used differs from that
in RL, for related references in adaptive control might refer to [26, 27].

In general, current safe RL solutions can be categorized into three main
approaches. (i) The first family utilizes a unique mechanism in the learning
procedure for safe policy optimization using, e.g., control barrier functions
[28, 17, 21], formal verification [29, 30], shielding [31, 32, 33], and
external intervention [34, 35]. These methods are prone to safe-biased
learning by sacrificing greatly on performance. And some of them rely on
extra-human interference [34, 35]. (ii) The second family proposes safe RL
algorithms via primal-dual methods [36, 37, 16, 38]. In the resulting
optimization problem, the Lagrangian multiplier serves as an extra weight
whose update is noted sensitive to the control performance [16]. Moreover,
some optimization problems such as those with ellipsoidal constraints (covered
in this work) could not satisfy the strong duality condition [39]. (iii) The
third is reward/cost shaping-based RL approaches [40, 41, 42, 43] where the
cost functions are augmented with various safety-related parts, e.g., barrier
functions. As stated in [37], such a design only informs the goal of
guaranteeing safety by minimizing the reshaped cost function but fails to
guide how to achieve it well through an actor-critic structure design.
Specifically, the reshaped cost function values, which are usually evaluated
in discrete-time instants, might change rapidly when the system variables
approach the constraint boundary. Consequently, the weights of actor and
critic networks are prone to divergence in the training process. These issues
motivated our barrier force-based (simplified as barrier-based) actor-critic
structure. In this work, we incorporate this unique structure into a control-
theory-based RL framework, where model-based multi-step policy evaluation
mechanism is also utilized to ensure convergence and safety in online learning
scenarios. Moreover, few works have addressed the safe RL algorithm design
under time-varying safety constraints.

This work proposes a model-based safe RL algorithm with theoretical guarantees
for optimal control with time-varying state and control constraints. A new
barrier force-based control policy (BCP) structure is constructed in the
proposed safe RL approach, generating repulsive control forces as states and
controls move toward the constraint boundaries. Moreover, the time-varying
constraints are addressed by a multi-step policy evaluation (MPE). The
proposed safe RL approach is implemented by an online barrier force-based
actor-critic learning algorithm. The closed-loop theoretical property of our
approach under nominal and perturbed cases and the convergence condition of
the barrier-based actor-critic (BAC) learning algorithm is derived. The
effectiveness of our approach is tested on both simulations and real-world
intelligent vehicles. The effectiveness of our approach is tested on both
simulations and real-world intelligent vehicles. Our contributions are
summarized as follows.

(i) We proposed a safe RL for optimal control under time-varying state and
control constraints. Under certain conditions (see Sections IV-A and -B),
safety can be guaranteed in both online and offline learning scenarios. The
performance and advantages of the proposed approach are achieved by two novel
designs. The first is a barrier force-based control policy to ensure safety
with an actor-critic structure. The second is a multi-step evaluation
mechanism to predict the control policy’s future influence on the value
function under time-varying safety constraints and guide the policy to update
safely.

(ii) We proved that the proposed safe RL could guarantee stability and
robustness in the nominal scenario and under external disturbances,
respectively. Also, the convergence condition of the actor-critic
implementation was derived by the Lyapunov method.

(iii) The proposed approach was applied to solve an integrated path following
and collision avoidance problem of intelligent vehicles so that the control
performance can be optimized with theoretical guarantees even with external
disturbances. a) Extensive simulation results illustrate that our approach
outperforms other state-of-the-art safe RL methods in learning safety and
performance. b) We verified our approach’s offline sim-to-real transfer
capability and real-world online learning performance, as well as the
strengths to state-of-the-art model predictive control (MPC) algorithms.

The remainder of the paper is organized as follows. Section II introduces the
considered control problem and preliminary solutions. Section III presents the
proposed safe RL approach and the BAC learning algorithm, while Section IV
presents the main theoretical results. Section V shows the real-world
experimental results, while some conclusions are drawn in Section VI. Some
proofs of the theoretical results and additional experimental results are
given in the appendix.

Notation: We denote $\mathbb{N}$ and $\mathbb{N}_{a}^{b}$as the set of natural
numbers and integers $a,a+1,\cdots,b$. For a vector $x\in\mathbb{R}^{n}$, we
denote $\|x\|_{Q}^{2}$ as $x^{\top}Qx$ and $\|x\|$ as the Euclidean norm. For
a function $f(x)$ with an argument $x$, we denote $\triangledown f(x)$ as the
gradient to $x$. For a function $f(x,u)$ with arguments $x$ and $u$, we denote
$\triangledown_{z}f(x,u)$ as the partial gradient to $z$, $z=x$ or $u$. Given
a matrix $A\in\mathbb{R}^{n\times n}$, we use $\lambda_{\rm min}(A)$
($\lambda_{\rm max}(A)$) to denote the minimal (maximal) eigenvalues. We
denote $\rm Int(\mathcal{Z})$ as the interior of a general set $\mathcal{Z}$.
For variables $z_{i}\in{\mathbb{R}}^{q_{i}}$, $i\in\mathbb{N}_{1}^{M}$, we
define $(z_{1},z_{2},\cdots,z_{\rm\scriptscriptstyle M})=[\,z_{1}^{\top}\
z_{2}^{\top}\ \cdots\ z_{\rm\scriptscriptstyle
M}^{\top}\,]^{\top}\in{\mathbb{R}}^{q}$, where $q=\sum_{i=1}^{M}q_{i}$.

##  II Problem Formulation

In this section, we first describe the considered model and the associated
safety constraints. Then, the optimal control objective and preliminary RL
results are given. Finally, the safe RL problem formulation by cost
reconstruction with barrier functions is presented.

###  II-A System Model and Constraints

The considered system under control is a class of discrete-time nonlinear
systems described by

$\begin{array}[]{ll}x_{k+1}=f(x_{k},u_{k}),\end{array}$ (1)

where $x_{k}\in\mathcal{X}_{k}\subseteq{\mathbb{R}}^{n}$ and
$u_{k}\in\mathcal{U}_{k}\subseteq{\mathbb{R}}^{m}$ are the state and input
variables, $k$ is the discrete-time index,
$\mathcal{X}_{k}=\\{x\in\mathbb{R}^{n}|G^{i}_{x,k}(x)\leq 0,\,\forall
i\in\mathbb{N}_{1}^{p_{x}}\\}$ and
$\mathcal{U}_{k}=\\{u\in\mathbb{R}^{m}|G^{i}_{u,k}(u)\leq 0,\,\forall
i\in\mathbb{N}_{1}^{p_{u}}\\}$ are sets that represent time-varying
constraints, $\\{0\\}\subseteq\mathcal{U}_{k}$, $\forall\,k\in\mathbb{N}$;
functions $G^{i}_{z,k}(z)\in\mathbb{R}$ for $z=x,u$, are assumed to be
$C^{2}$; $f$ is a smooth state transition function and $f(0,0)=0$.

In principle, different types of state constraints can be formalized as
follows. For instance, (i) $\mathcal{X}_{k}$ with
$G^{i}_{x,k}(x)=E_{k}^{i}x-c^{i}_{k}$ is a linear convex set, where
$E_{k}^{i}\in\mathbb{R}^{1\times n}$ and $c^{i}_{k}\in\mathbb{R}$ are time-
varying parameters; (ii) $\mathcal{X}_{k}$ with
$G^{i}_{x,k}(x)=d_{k}^{i}-\|E_{k}^{i}x_{k}-c^{i}_{k}\|$ represents a dynamic
obstacle avoidance constraint of a robot in a 2-D map, where
$E_{k}^{i}\in\mathbb{R}^{2\times n_{i}}$, $c_{k}^{i}\in\mathbb{R}^{2}$ and
$d_{k}^{i}\in\mathbb{R}$ are the center and radius of the circular dynamic
obstacle respectively.

######  Definition 1 (Local stabilizability [39])

System (1) is stabilizable on $\mathcal{X}_{k}\times\mathcal{U}_{k}$ if, for
any $x_{0}\in\mathcal{X}_{k}$, there exists a $C^{1}$ state-feedback policy
$u(x_{k})\in\mathcal{U}_{k}$, $u(0)=0$, such that $x_{k}\in\mathcal{X}_{k}$
and $x_{k}\rightarrow 0$ as $k\rightarrow+\infty$.

######  Assumption 1 (Lipschitz continuous)

Model (1) is Lipschitz continuous in $\mathcal{X}_{k}\times\mathcal{U}_{k}$,
for all $k\in\mathbb{N}_{1}^{\infty}$, i.e., there exists a Lipschitz constant
$0<L_{f}<+\infty$ such that for all $x_{1},x_{2}\in\mathcal{X}_{k}$ and
$C^{1}$ control policies with $u(x_{1}),u(x_{2})\in\mathcal{U}_{k}$,

$\|f(x_{1},u(x_{1}))-f(x_{2},u(x_{2}))\|\leq L_{f}\|x_{1}-x_{2}\|.$ (2)

######  Assumption 2 (Model)

$\|\triangledown_{u}f(x,u)\|\leq g_{m}$ in the domain
$\mathcal{X}_{k}\times\mathcal{U}_{k}$, where $g_{m}$ is a positive scalar.

###  II-B Control Objective

Starting from any initial condition $x_{0}\in\mathcal{X}_{0}$, the control
objective is to find an optimal control policy
$u^{\ast}_{k}=u^{\ast}(x_{k})\in\mathcal{U}_{k}$ that minimizes a quadratic
regulation cost function of type

$J(x_{0},u_{0:+\infty})=\sum_{k=0}^{+\infty}\gamma^{k}r(x_{k},u_{k}),$ (3)

subject to model (1), $x_{k}\in\mathcal{X}_{k}$, and
$u_{k}\in\mathcal{U}_{k}$, $\forall k\in\mathbb{N}$; where

$r(x_{k},u_{k})=\|x_{k}\|_{Q}^{2}+\|u_{k}\|_{R}^{2},$ (4)

and $Q=Q^{\top}\in\mathbb{R}^{n\times n}$, $R=R^{\top}\in\mathbb{R}^{m\times
m}$, $Q,R\succ 0$, $\gamma$ is a discounting factor.

Without loss of generality, many waypoint tracking problems in the robot
control field can be naturally formed as the prescribed regulation one, with a
proper coordination transformation of the reference waypoints. More generally,
it is allowed that the time-varying state constraint might not contain the
origin for some $k\in\mathbb{N}$. Typical examples can be found in, for
instance, path following of mobile robots with collision avoidance, where the
potential obstacle to be avoided might occupy the reference waypoints, i.e.,
the origin after coordination transformation. In this scenario, it is still
reasonable to introduce the following assumption for convergence guarantee.

######  Assumption 3 (State constraint)

There exists a finite number $\bar{k}\in\mathbb{N}$ such that
$\\{0\\}\subseteq\mathcal{X}_{k}$ as $k\geq\bar{k}$.

######  Definition 2 (Multi-step safe control)

For a given state $x_{k}\in\mathcal{X}_{k}$ at time instant $k$, a control
policy $u(x_{k})\in\mathcal{U}_{k}$, is $L$-step safe for (1) if the resulting
future state evolutions of (1) under $u(x_{k})$ satisfy
$x_{k+i}\in\mathcal{X}_{k+i}^{u}$, $\forall i\in\mathbb{N}_{1}^{L}$, where
$\mathcal{X}_{k+i}^{u}$ is the resulting state constraint under $u(x_{k})$.

To simplify the notation, in the rest of the paper, the super index in
$\mathcal{X}_{k}^{u}$ is neglected, i.e., we use $\mathcal{X}_{k}$ to denote
$\mathcal{X}_{k}^{u}$.

###  II-C Preliminary Reinforcement Learning Solutions

In a special case where only control constraint is considered, i.e.,
$\mathcal{X}_{k}=\mathbb{R}^{n}$, the optimal value function can be defined by

$J^{\ast}(x_{k})=\mathrel{\mathop{\kern 0.0pt\rm
min}\limits_{u_{k}\in\mathcal{U}_{k},\forall
k\in\mathbb{N}}}\sum_{k=0}^{+\infty}\gamma^{k}r(x_{k},u(x_{k})),$

which satisfies the Hamilton-Jacobi-Bellman (HJB) equation

$J^{\ast}(x_{k})=\mathrel{\mathop{\kern 0.0pt\rm
min}\limits_{u_{k}\in\mathcal{U}_{k}}}r(x_{k},u(x_{k}))+\gamma
J^{\ast}(x_{k+1}),$ (5)

and the optimal control policy is

$\begin{array}[]{ll}u^{\ast}(x_{k})=\mathrel{\mathop{\kern 0.0pt\rm
argmin}\limits_{u_{k}\in\mathcal{U}_{k}}}r(x_{k},u(x_{k}))+\gamma
J^{\ast}(x_{k+1}).\end{array}$ (6)

Various RL solutions have been contributed to solving optimization problem (5)
with (6), resorting to an actor-critic approximating structure (cf. [5, 4,
44]). In control problems with state constraints, solving (5) with (6) with
safety guarantee is more challenging using the trial-and-error-based actor-
critic reinforcement learning framework. Recently, various safe RL solutions
have been emerged [11, 13, 10, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24,
25]. However, few works have addressed the safe RL algorithm design under
time-varying safety constraints.

One of the typical safe RL algorithms is to shape the cost (3) with barrier
functions (see [40, 41, 42, 43]), which could be insufficient in many cases to
guide how to ensure safety by actor-critic learning. Take the considered
discrete-time optimal control problem as an example. Note that the gradients
of barrier functions change rapidly when the state or control approach
constraints boundaries. On this ground, the reshaped cost function with
barrier functions might experience abrupt changes since it is evaluated at
discrete-time instants (in most cases the sampling interval is not chosen
small enough). As a result, the weights of the actor-critic networks are prone
to divergence due to the abrupt cost value changes in the training process.
For these reasons, we propose a safe RL approach using a BCP structure and an
MPE mechanism for optimal control under time-varying safety constraints.

###  II-D Definition on Barrier Functions

As policy improvement is usually performed by the gradient descent method in
actor-critic RL, we have to reconstruct the cost function in (3) by
incorporating continuous barrier functions of state and control constraints.
To this end, we first introduce a definition of barrier functions as follows.

######  Definition 3 (Barrier function [45])

For a general convex set
$\mathcal{Z}_{k}=\\{z\in\mathbb{R}^{l}|G^{i}_{z,k}(z)\leq 0,\,\forall
i\in\mathbb{N}_{1}^{p_{z}}\\}$, a barrier function is defined as

${\mathcal{B}}^{o}_{k}(z)=\left\\{\begin{array}[]{l}-\sum_{i=1}^{p_{z}}{\rm{log}}\big{(}-G^{i}_{z,k}(z)\big{)},\
\ z\in{\rm{Int}}(\mathcal{Z}_{k})\\\\[5.69046pt] +\infty\ \
\rm{otherwise}.\end{array}\right.\qquad$ (7)

To derive a satisfactory control performance, we define a recentered
transformation of ${\mathcal{B}}_{k}^{o}(z)$ centered at
$z_{c}\in\mathbb{R}^{l}$ is defined as
${\mathcal{B}}_{k}^{c}(z)={\mathcal{B}}^{o}_{k}(z)-{\mathcal{B}}_{k}^{o}(z_{c})-\triangledown_{z}{\mathcal{B}}^{o}_{k}(z_{c})^{\top}z$,
where $z_{c}=0$ if $\\{0\\}\subseteq\mathcal{Z}_{k}$ or $z_{c}$ is selected
such that $z_{c}\in\mathcal{Z}_{k}$ otherwise.  This definition leads to the
property that ${\mathcal{B}}^{c}_{k}(z)\geq 0$ and it reaches the minimum at
$z_{c}$, i.e.,

${\mathcal{B}}^{c}_{k}(z_{c})=0,\
\triangledown{\mathcal{B}}^{c}_{k}(z_{c})=0.$ (8)

For the case $\\{0\\}\nsubseteq\mathcal{Z}_{k}$, we suggest selecting $z_{c}$
far from the set boundary of $\mathcal{Z}_{k}$ and as the central point or its
neighbor of $\mathcal{Z}_{k}$ (if possible).

######  Lemma 1 (Relaxed barrier function [45])

Define a relaxed barrier function of ${\mathcal{B}}_{k}^{c}(z)$ as

${\mathcal{B}}_{k}(z)=\left\\{\begin{array}[]{ll}{\mathcal{B}}_{k}^{c}(z)&\bar{\sigma}_{k}\geq\kappa_{b}\\\
\gamma_{b}(z,\bar{\sigma}_{k})&\bar{\sigma}_{k}<\kappa_{b},\end{array}\right.$
(9)

where the relaxing factor $\kappa_{b}>0$ is a small positive number,
$\bar{\sigma}_{k}={\rm min}_{i\in\mathbb{N}_{1}^{p_{z}}}-G^{i}_{k}(z)$, the
function $\gamma_{b}(z,\bar{\sigma}_{k})$ is strictly monotone and
differentiable on $(-\infty,\kappa_{b})$, and
$\triangledown_{z}^{2}\gamma_{b}(z,\bar{\sigma}_{k})\leq\triangledown_{z}^{2}{\mathcal{B}}_{k}(z)|_{\bar{\sigma}_{k}=\kappa_{b}}$,
then there exists a matrix
$H_{z_{k}}\geq\triangledown_{z}^{2}{\mathcal{B}}_{k}(z)|_{\bar{\sigma}_{k}=\kappa_{b}},$
such that
$\|\triangledown_{z}{\mathcal{B}}_{k}(z)\|\leq{\mathcal{B}}_{z_{k},m},$
${\mathcal{B}}_{z_{k},m}=\max_{z\in\mathcal{Z}_{k}}\|2H_{z_{k}}(z-z_{c})\|$.

Proof: For details please see [45]. $\square$

##  III Safe RL with BCP and MPE

This section presents our safe RL approach and its implementation by an
efficient barrier-based actor-critic learning algorithm. Our safe RL approach
has two novel ingredients. The first is a barrier force-based control policy
structure, which has physics force interpretations to ensure safety. The
second is a multi-step policy evaluation mechanism, which provides the multi-
step safety risk prediction to guide the policy to update safely online under
time-varying constraints.

###  III-A Design of Safe RL with BCP and MPE

We reconstruct the performance index $J(x_{k})$ with state and control barrier
functions defined in (7). Letting $\mu>0$ be a tuning parameter, the resulting
value function, denoted as $\bar{J}(x_{k})$, is defined as

$\begin{array}[]{cl}\bar{J}(x_{k})=\sum_{k=0}^{+\infty}\gamma^{k}\bar{r}(x_{k},u_{k}),\end{array}$
(10)

where
$\bar{r}(x_{k},u_{k})=r(x_{k},u_{k})+\mu\mathcal{B}_{k}(u_{k})+\mu\mathcal{B}_{k}(x_{k})$.
Note that, in addition to the logarithmic barrier function (7), other general
types of differentiable barrier functions such as exponential, polynomial ones
can be naturally used instead to construct $\bar{J}(x_{k})$; which is,
however, beyond the scope of this work.

######  Proposition 1 (Unconstrained control problem equivalence)

Letting $s_{k}(x_{k})=\sqrt{\mathcal{B}_{k}(x_{k})}$, the control problem for
(1) with cost (10) is equivalent to an unconstrained optimal control problem
for the system

$\left\\{\begin{array}[]{ll}x_{k+1}=f(x_{k},u_{k})\\\
y_{k}=(x_{k},s_{k}(x_{k})),\end{array}\right.$ (11)

with the cost in (10) being rewritten as

$\bar{J}_{u}(x_{k})=\sum_{k=0}^{+\infty}\gamma^{k}(\|y_{k}\|_{Q_{y}}^{2}+\|u_{k}\|_{R}^{2}+\mu\mathcal{B}_{k}(u_{k})),$
(12)

where $Q_{y}={\rm{diag}}\\{Q,\mu\\}$.

Proof. First, note that $\bar{J}_{u}$ in (12) is equivalent to $\bar{J}$ in
(10) since $y_{k}=(x_{k},s_{k}(x_{k}))$ and
$s_{k}(x_{k})=\sqrt{\mathcal{B}_{k}(x_{k})}$. Hence, the control problem for
(11) with cost (12) is equivalent to that for (11) with cost (10), and
consequently to that for (1) with cost (10) by disregarding the definition
$y_{k}=(x_{k},s_{k}(x_{k}))$ in (11). $\square$

In (12), the overall objective function consists of the classical quadratic-
type regulation costs and the barrier functions on the state and control. The
tuning parameter $\mu$ determines the influence of the barrier function values
on the overall objective function. Given $\mu$, the barrier functions even
become dominant if the control and state are close to the boundaries of the
safety constraints. Indeed, the parameter $\mu$ (concerning $Q$ and $R$)
represents a trade-off between optimality and learning safety.

To solve the control problem with $\bar{J}(x_{k})$, we propose a novel barrier
force-based control policy inspired by the barrier method in interior-point
optimization [46], i.e.,

$u_{k}=v_{k}+\rho\triangledown_{v}\mathcal{B}_{k}(v_{k})+K\triangledown_{x}\mathcal{B}_{k}(x_{k}),$
(13)

where $v_{k}\in\mathbb{R}^{m}$ is a new virtual control input,
$\rho\in\mathbb{R}$ and $K\in\mathbb{R}^{m\times n}$ are decision variables to
be further optimized (see also Section IV);
$\triangledown_{v}\mathcal{B}_{k}(v_{k})$ is the gradient of
$\mathcal{B}_{k}(v_{k})$ for $v_{k}\in\mathcal{U}_{k}$,
$\triangledown_{x}\mathcal{B}_{k}(x_{k})$ is the gradient of
$\mathcal{B}_{k}(x_{k})$ for $x_{k}\in\mathcal{X}_{k}$.

###### Remark 1

In (13), the roles of the second and third terms are to generate the repulsive
forces, respectively, as the variables $x$ and $v$ move toward the
corresponding boundary of the constraints. As a result, (13) generates joint
forces to exactly balance the forces associated with $J(x_{k})$ and the
barrier functions in $\bar{J}(x_{k})$. Hence, our control policy has physics
force interpretations to ensure safety.

###### Remark 2

Different from cost shaping-based RL algorithms [40, 47, 41, 42, 11, 43] and
primal-dual approaches [36, 37, 16, 38], in the proposed approach, the
decision variables $v(x_{k}),\,\rho$, and $K$, instead of $u(x_{k})$, are
optimized (deferred in (15f)). Then the improved decision variables are used
to compute the three control terms in (13) to construct the real control
policy $u(x_{k})$.

Let at any time instant $k$, the $L$-step ahead control policy be
$u(x_{\tau})\,\forall\tau\in\mathbb{N}_{0}^{L-1}$ where $L\in\mathbb{N}$.
Hence, one can write the following difference equation for the multi-step
prediction of the stage cost under $u(x)$, i.e.,

$\begin{array}[]{ll}\bar{J}\big{(}x_{k}\big{)}&=\bar{r}(x_{k},u(x_{k}))+\gamma\bar{J}\big{(}x_{k+1}\big{)}\\\
&=\sum_{l=0}^{L-1}\gamma^{l}\bar{r}(x_{k+l},u(x_{k+l}))+\gamma^{L}\bar{J}\big{(}x_{k+L}\big{)}.\end{array}$
(14)

Under control (13), letting $\bar{J}^{*}(x_{k})$ be the optimal value function
at time instant $k$, a variant of the discrete-time HJB equation can be
written as

$\begin{array}[]{ll}\bar{J}^{*}(x_{k})&=\mathrel{\mathop{\kern 0.0pt\rm
min}\limits_{u_{k}\in\mathcal{U}_{k}}}\bar{r}(x_{k},u(x_{k}))+\gamma\bar{J}^{\ast}\big{(}x_{k+1}\big{)},\\\
&=\mathrel{\mathop{\kern 0.0pt\rm
min}\limits_{\begin{array}[]{c}\scriptscriptstyle
u_{k+l}\in\mathcal{U}_{k+l},\scriptscriptstyle
l\in\mathbb{N}_{0}^{L-1}\end{array}}}\sum_{l=0}^{L-1}\gamma^{l}\bar{r}(x_{k+l},u(x_{k+l}))+\gamma^{L}\bar{J}^{\ast}\big{(}x_{k+L}\big{)}\end{array}$

and the optimal solution is

$\begin{array}[]{ll}u^{\ast}(x_{k})&=\mathrel{\mathop{\kern 0.0pt\rm
argmin}\limits_{u_{k}\in\mathcal{U}_{k}}}\bar{r}(x_{k},u(x_{k}))+\gamma\bar{J}^{\ast}\big{(}x_{k+1}\big{)}\\\
&=\mathrel{\mathop{\kern 0.0pt\rm
argmin}\limits_{\begin{array}[]{c}\scriptscriptstyle
u_{k+l}\in\mathcal{U}_{k+l},\scriptscriptstyle
l\in\mathbb{N}_{0}^{L-1}\end{array}}}\sum_{l=0}^{L-1}\gamma^{l}\bar{r}(x_{k+l},u(x_{k+l}))+\gamma^{L}\bar{J}^{\ast}\big{(}x_{k+L}\big{)}.\end{array}$

We propose a safe RL algorithm with barrier-based control policy (BCP) and
multi-step policy evaluation (MPE) in Algorithm 1 to solve $u^{\ast}(x_{k})$
and $\bar{J}^{\ast}(x_{k})$.

Algorithm 1 Safe RL with BCP and MPE

0: $\bar{\epsilon}>0$, policy $u^{0}(x_{k})$, $i=0$.

for $k=1,2,\cdots$ do

while $\bar{J}^{i}_{k}-\bar{J}^{i-1}_{k}\geq\bar{\epsilon}$ do

1) Compute $x_{k+l}$ with $u^{i}(x_{k+l})$ based on model (1) for
$l\in\mathbb{N}_{1}^{L}$.

2) multi-step policy evaluation:
$\displaystyle\begin{array}[]{ll}\bar{J}^{i+1}\big{(}x_{k}\big{)}=\sum_{l=0}^{L-1}\gamma^{l}\bar{r}(x_{k+l},u^{i}(x_{k+l}))+\gamma^{L}\bar{J}^{i}\big{(}x_{k+L}\big{)}.\end{array}$
(15b)

3) Barrier-based control policy update:
$\displaystyle\begin{array}[]{ll}(v(x_{k}),\rho,K)^{i+1}=\mathrel{\mathop{\kern
0.0pt\rm
argmin}\limits_{v_{k},\rho,K}}\bar{r}(x_{k},u(x_{k}))+\gamma\bar{J}^{i+1}\big{(}x_{k+1}\big{)},\vspace{1mm}\\\
u^{i+1}(x_{k})=v^{i+1}(x_{k})+\rho^{i+1}\triangledown_{v}\mathcal{B}_{k}(v^{i+1}(x_{k}))\vspace{1mm}\\\
\hskip 156.49014pt+K^{i+1}\triangledown_{x}\mathcal{B}_{k}(x_{k}).\end{array}$
(15f)

4) $i\leftarrow i+1$.

end while

end for

###### Remark 3

Note that model-free RL algorithms have received considerable attention in
continuous control tasks [3]. However, model-free approaches still have the
data-inefficient issue, suitable for specific tasks with valid datasets [17,
48]. In our case, we focus on a model-based framework with safety and
convergence guarantees because it is more suitable for our concerned real-
world safety-critical vehicle control tasks. The extension of our approach to
the model-free case will be left for further investigation.

###  III-B Barrier-based Actor-Critic Implementation

In the following, Algorithm 1 is implemented with a barrier-based actor-critic
learning structure. We first construct a consistent type of critic network to
$\bar{J}$ in (10) with barrier functions:

$\hat{\bar{J}}(x_{k})=W_{c1}^{\top}\sigma_{c}(x_{k})+W_{c2}\mathcal{B}_{k}(x_{k}),$
(16)

where $W_{c1}\in\mathbb{R}^{N_{c}}$ and $W_{c2}\in\mathbb{R}$ are weighting
matrices, $\sigma_{c}\in\mathbb{R}^{N_{c}}$ is a vector composed of basis
functions. In a collective form, we write
$\hat{\bar{J}}(x_{k})=W_{c}^{\top}h_{c}(x_{k}),$ where
$W_{c}=(W_{c1},W_{c2})$,
$h_{c}(x_{k})=(\sigma_{c}(x_{k}),\mathcal{B}_{k}(x_{k}))$.

The ultimate goal of the critic network is to minimize the distance between
$\bar{J}^{\ast}$ and $\hat{\bar{J}}$ via updating $W_{c}$. However, as
$\bar{J}^{\ast}$ is not available, the following $\bar{J}^{d}(x_{k})$ (defined
according to (15b)) is used as the target to be steered by $\hat{\bar{J}}$,
i.e.,

$\begin{array}[]{l}{\bar{J}}^{d}(x_{k})=\sum_{l=0}^{L-1}\gamma^{l}\bar{r}(x_{k+l},u_{k+l})+\gamma^{L}\hat{\bar{J}}(x_{k+L}).\end{array}$
(17)

Let $\varepsilon_{c,k}={\bar{J}}^{d}(x_{k})-\hat{\bar{J}}(x_{k})$ be the
approximation residual, $\delta_{c,k}=\varepsilon_{c,k}^{2}$, and $\gamma_{c}$
be the learning rate, then the update rule of weight $W_{c}$ according to the
gradient descent is given as

$W_{c,k+1}={W_{c,k}}-{\gamma_{c}}\frac{\partial\delta_{c,k}}{\partial
W_{c,k}}.$ (18)

We next design the actor network for learning the control policy (13) with the
following form

$u(x_{k})=W_{a,\sigma}^{\top}\sigma_{a}(x_{k})+\hat{K}\triangledown_{x}\mathcal{B}_{k}(x_{k}))+\hat{\rho}\triangledown_{v}\mathcal{B}_{k}(v_{k}),$
(19)

where $W_{a,\sigma}\in\mathbb{R}^{N_{u}\times m}$,
$\hat{K}\in\mathbb{R}^{m\times n}$, and $\hat{\rho}\in\mathbb{R}$ are the
weighting matrices, $\sigma_{a}\in\mathbb{R}^{\scriptscriptstyle N_{u}}$ is a
vector of basis functions. Let $W_{a}^{\top}=[W_{a1}^{\top}\,\hat{\rho}I]$,
$W_{a1}^{\top}=[W_{a,\sigma}^{\top}\ \hat{K}]$ and
$h_{a}(x_{k})=(h_{a1}(x_{k}),\triangledown_{v}\mathcal{B}_{k}(v_{k}))$,
$h_{a1}(x_{k})=(\sigma_{a}(x_{k}),\triangledown_{x}\mathcal{B}_{k}(x_{k}))$,
then one can write (19) in a collective form as
$u(x_{k})=W_{a}^{\top}h_{a}(x_{k})$.

In view of (15f) and (19), letting
$\nu_{k}=2Ru_{k}+\mu\triangledown_{u}\mathcal{B}_{k}(u_{k})$, we define a
desired target of $\nu_{k}$, i.e., $\nu^{d}_{k}$ as
$\nu_{k}^{d}=-\triangledown_{u}f(x,u)^{\top}\partial\hat{\bar{J}}(x_{k+1})/\partial
x_{k+1}.$ Denote $\varepsilon_{a,k}=\nu^{d}_{k}-\nu_{k}$ as the approximation
residual, $\delta_{a,k}=\|\varepsilon_{a,k}\|^{2}$, and $\gamma_{a}$ be the
learning rate, then the update rule of $W_{a1}$ and $\hat{\rho}$ according to
the gradient descent is given as

$\displaystyle W_{a1,k+1}=$
$\displaystyle{W_{a1,k}}-{\gamma_{a}}\frac{\partial\delta_{a,k}}{\partial
W_{a1,k}},$ (20a) $\displaystyle\hat{\rho}_{k+1}=$
$\displaystyle{\hat{\rho}_{k}}-{\gamma_{a}}\frac{\partial\delta_{a,k}}{\partial\hat{\rho}_{k}}.$
(20b)

For a visual display of the barrier-based actor-critic learning algorithm,
please see Fig. 1.

Figure 1:  The schematic diagram of the barrier-based actor-critic learning
algorithm.

###### Remark 4

Although we introduce barrier-based terms in the actor-critic structure for
policy learning, the resulting algorithm implementation procedure is
comparable (only slightly more complex) to a standard actor-critic structure
since all the weights can be learned with standard gradient decent rules (see
(18) and (20)) and only a few more weighting matrices $W_{c,2}$, $\hat{K}$,
and $\hat{\rho}$ are to be updated alongside.

##  IV Theoretical results

The theoretical properties of the proposed safe RL in nominal and disturbance
scenarios are proven in Section IV-A and Section IV-B respectively. Then, the
convergence analysis of the barrier-based actor-critic learning algorithm
under time-invariant constraints is given in Section IV-C.

###  IV-A Safety and Stability Guarantees in Nominal Scenario

In the following, we prove the convergence of our proposed safe RL with BCP
and MPE, i.e., Algorithm 1 under time-varying constraints.

######  Assumption 4 (Stabilizability)

For any $x_{k}\in\mathcal{X}_{k}$, there exist $v(x_{k})$, $\rho$, $K$
constituting a control policy $u(x_{k})\in\mathcal{U}_{k}$ such that system
(11) is locally stabilizable under (13).

Specifically, by setting $\rho=0$ and $K=0$, Assumption 4 is equivalent to the
standard local stabilizability condition as in [39] when the state and control
constraints are time-invariant. From Assumption 4, one can promptly derive the
following $L$-step safe control condition: given $x_{k}\in\mathcal{X}_{k}$,
there exists an $L$-step safe control policy such that
$x_{k+l}\in\mathcal{X}_{k+l}$ $\forall l\in\mathbb{N}_{1}^{L}$. Note that this
condition is equivalent to the existence of a 1-step safe control policy since
the former can be derived from the latter using mathematical induction. To
verify the above safe control condition, the variation of the state
constraints can not be arbitrarily large. Let
$\tilde{\mathcal{X}}_{k+1}=\\{x_{k+1}|x_{k+1}=f(x_{k},u_{k}),\,\forall
x_{k}\in\mathcal{X}_{k},u_{k}\in\mathcal{U}_{k}\\}$ be the maximal reachable
set from $\mathcal{X}_{k}$ under $\mathcal{U}_{k}$. We require that the state
constraint at the any time $k+1$ satisfy
$\mathcal{X}_{k+1}\subseteq\tilde{\mathcal{X}}_{k+1}$.

######  Theorem 1 (Convergence)

If $u^{0}(x_{k})\in\mathcal{U}_{k}$ is such that the relaxed barrier function
$\mathcal{B}_{k+l}(x_{k+l})$ is finite111 This implies that we do not require
the initial control policy being $L$-step. This is due to the relaxed barrier
function design in Lemma 1, where a smoother quadratic term is introduced to
replace the barrier function when the variable approaches the set boundary or
is out of the set. $\forall l\in\mathbb{N}_{1}^{L}$, and the value function
$\bar{J}^{0}(x_{k})\geq\bar{r}(x_{k},u^{0}(x_{k}))+\gamma\bar{J}^{0}(x_{k+1})$;
then with (15), it holds that

  1. (i)

$\bar{J}^{i+1}(x_{k})\leq V^{i}(x_{k})\leq\bar{J}^{i}(x_{k})$, where
$V^{i}(x_{k})=\bar{r}(x_{k},u^{i}(x_{k}))+\gamma\bar{J}^{i}(x_{k+1})$;

  2. (ii)

$\bar{J}^{i}(x_{k})\rightarrow\bar{J}^{\ast}(x_{k})$ and
$u^{i}(x_{k})\rightarrow u^{\ast}(x_{k})$, as $i\rightarrow+\infty$.

Proof. Please refer to Appendix -A. $\square$

###### Remark 5

Let $J^{p}(x_{k})$ be a safe and optimal value of (14) with $\mu=0$ and (13)
under the control and state constraints. Then $\bar{J}^{\ast}(x_{k})$ is a
good approximation of $J^{p}(x_{k})$ given $\mu$ being small. Moreover, if the
optimization problem with (3) under (13) is dual feasible, then one can obtain
a quantifiable condition $\bar{J}^{\ast}(x_{k})-J^{p}(x_{k})\leq o(\mu)$,
where $o(\mu)$ is a function that decreases along with $\mu$ (see Page 566 in
[46]). This implies that the control policy $u^{\ast}(x_{k})$, associated with
$\bar{J}^{\ast}(x_{k})$, is $L$-step safe provided $\mu$ being chosen suitably
small.

###### Remark 6

Theorem 1 also implies that, at any time instant $k$, the initial control
policy is not necessarily $L$-step safe to guarantee safety and convergence.
Hence, the recursive feasibility under time-varying constraints is likely
guaranteed as long as an $L$-step safe control policy exists provided any
$x_{k}\in\mathcal{X}_{k}$, which can be certified by Assumption 4.

######  Proposition 2 (Stability)

Let $\gamma=1$, $x_{0}\in\mathcal{X}_{0}$, and $u^{\ast}$ be the control
policy with the optimal solution $v^{\ast}$, $\rho^{\ast}$, and $K^{\ast}$,
solved via minimizing (12) with (11). Under Assumptions 3 and 4, the state
$x_{k}$ of model (1) using $u^{\ast}$, converges to the origin as
$k\rightarrow+\infty$.

Proof. Please refer to Appendix -A. $\square$

###### Remark 7

Note that the discounting factor $0<\gamma\leq 1$ is a crucial ingredient in
reinforcement learning to ensure the convergence of the value function and
control policy to the optimal ones (see [49]). However, as illustrated by
Proposition 2, a choice of $\gamma$ close to 1 is suggested to guarantee
closed-loop stability.

###  IV-B Safety and Robustness Guarantees in Disturbed Scenario

We show that our approach can guarantee safety and robustness under
disturbances by properly shrinking the state constraints in the learning
process. To this end, let the real model dynamics be given as

$z_{k+1}=f(z_{k},u_{k})+w_{k},$ (21)

where $z_{k}$ is the real state, $w_{k}\in\mathcal{W}$ is an additive bounded
and unknown disturbance which can represent the modeled uncertainty or
measurement noise, $\mathcal{W}$ is a compact set containing origin in the
interior. In case the model dynamics are unavailable, the derivation of the
nominal model (1) may resort to data-driven modeling approaches. For a
specific data-driven modeling approach and the estimation of the associated
uncertainty set $\mathcal{W}$ please refer to [39].

Let at any time instant $k$, $x_{k+j|k}$ be the predicted state by applying
the control $u(x_{k}),\cdots,u(x_{k+L-1})$ using model (1). Assuming that the
uncertainty set $\mathcal{W}$ is norm-bounded, i.e.,
$\|w_{k}\|\leq\varepsilon_{w}$, then the following lemma is stated.

######  Lemma 2 ([50])

The difference between the real state under $u(z)$ and the nominal one under
$u(x)$ satisfies

$\|x_{k+j|k}-z_{k+j}\|\leq\frac{L_{f}^{j}-1}{L_{f}-1}\varepsilon_{w},$ (22)

where $x_{k|k}=z_{k}$.

Proof. The proof is similar to [50]. $\square$

Let the constraint on the nominal state be shrink, i.e.,
$x_{k+j|k}\in\bar{\mathcal{X}}_{k+j}$ where
$\bar{\mathcal{X}}_{k+j}=\mathcal{X}_{k+j}\ominus\mathcal{D}_{\varepsilon_{w}}^{j}$,
$\mathcal{D}_{\varepsilon_{w}}^{j}=\\{y\in\mathbb{R}^{n}|\|y\|\leq\frac{L_{f}^{j}-1}{L_{f}-1}\varepsilon_{w}\\}$.
The barrier function on the state in (10) is modified according to the
constraint $x_{k+j|k}\in\bar{\mathcal{X}}_{k+j}$. Assume that the computed
$\bar{\mathcal{X}}_{k+j}$ is non-empty and contains the origin in the interior
for all $k\geq\bar{k}$.

######  Theorem 2 (Robustness)

Under Assumptions 3-4, the state evolution of (21), by applying the learned
optimal policy $u^{\ast}$ with (1), converges to the set
$\mathcal{D}_{\varepsilon_{w}}^{\infty}$, i.e.,
$\lim_{k\rightarrow+\infty}x_{k}\rightarrow\mathcal{D}_{\varepsilon_{w}}^{\infty}$.

Proof. Please refer to Appendix -A. $\square$

As suggested in [50], to reduce the size of
$\mathcal{D}_{\varepsilon_{w}}^{j}$, i.e., the Lipschitz constant $L_{f}$, two
design choices are suggested: (i) a different suitable norm type can be used;
(ii) an additional feedback term $K(z_{k}-x_{k})$ can be added in the control
input to reduce the conservativeness of the multi-step prediction of (1),
where $K\in\mathbb{R}^{m\times n}$ is a stabilizing gain matrix of (1).

###  IV-C Convergence Analysis of BAC Learning Algorithm

Note that, as shown in the Proposition 1, the control problem for (1) with
$\bar{J}(x_{k})$ is equivalent to an unconstrained problem for a time-varying
model (11) with the (11). The convergence analysis for the BAC learning
algorithm in this scenario would be much involved by Lyapunov method since the
optimal weights of the actor and critic are time-dependent due to
$y_{k}=(x_{k},\sqrt{\mathcal{B}_{k}(x_{k})})$. For the sake of simplicity, we
recall that a time-varying constraint can be partitioned into several segments
of time-invariant ones. Hence, in the following, we prove the convergence of
the BAC learning algorithm under time-invariant state and control constraints,
i.e., $\mathcal{X}=\mathcal{X}_{k}$ and $\mathcal{U}=\mathcal{U}_{k}$. That
is, we prove that whenever the constraints are changed, our algorithm can
eventually converge after some time steps. To this end, one first write

${\bar{J}}^{\ast}(x_{k})={W_{c}^{\ast}}^{\top}h_{c}(x_{k})+\kappa_{c}(x_{k})$
$u^{\ast}(x_{k})={W_{a}^{\ast}}^{\top}h_{a}(x_{k})+\kappa_{a}(x_{k}),$

where $W_{c}^{\ast}$ and $W_{a}^{\ast}$ are constant weights, $\kappa_{c}$ and
$\kappa_{a}$ are reconstruction errors. In view of the universal capability of
neural networks with one hidden-layer, we introduce the following assumption
on the actor and critic network.

######  Assumption 5 (Weights and reconstruction errors of BAC)

  1. 1)

$\|W_{c}^{\ast}\|\leq W_{c,m}$, $\|\sigma_{c}(x)\|\leq\sigma_{c,m}$,
$\|\triangledown_{x}\sigma_{c}(x)\|\leq\bar{\sigma}_{c,m}$,
$\|\kappa_{c}(x)\|\leq\kappa_{c,m}$;

  2. 2)

$\|W_{a}^{\ast}\|\leq W_{a,m}$, $\|\sigma_{a}(x)\|\leq\sigma_{a,m}$,
$\|\kappa_{a}(x)\|\leq\kappa_{a,m}$. $\blacktriangleleft$

To state the following theorem in a compact form, we let
$\tilde{W}_{\star}=W_{\star}^{\ast}-W_{\star}$, $\star=a,c$ in turns, denote
$\Delta\bar{h}_{c,k}=\Delta h_{c,k}^{\top}\Delta h_{c,k}$, where $\Delta
h_{c,k}=\gamma^{L}h_{c,k+L}-h_{c,k}$, and use $q$ and $q^{+}$ to denote
$q_{k}$ and $q_{k+1}$ respectively unless otherwise specified. For simplicity,
we assume that $G^{i}(u)=E^{i}u$, $E^{i}\in\mathbb{R}^{1\times m}$.

######  Theorem 3 (Convergence of BAC learning)

Under Assumptions 2 and 5, if

$\displaystyle R-\mu H_{u}\succ 0\ {\rm and}\ I-3d_{m}(R+\mu H_{u})^{2}\succ
0,$ (23a) where
$d_{m}=4\gamma_{a}(\sigma_{a,m}^{2}+\mathcal{B}_{v,m}^{2}+\mathcal{B}_{x,m}^{2})$,
and $\displaystyle q_{1}\leq\Delta\bar{h}_{c,k}\leq q_{2},$ (23b) where
$q_{1},q_{2}>0$,

then it holds that
$\|(\xi_{a,k},\tilde{W}_{c,k})\|\leq\sqrt{\frac{\epsilon_{m}}{\lambda_{\rm
min}(S)}},\ {\rm as}\,k\rightarrow+\infty,$ where
$\xi_{a,k}=\tilde{W}_{a,k}^{\top}h_{a}(x_{k})$, $\epsilon_{m}$ is a bounded
error and $S$ is a positive-definite matrix, whose definitions are deferred in
Appendix -A. Also, $(\xi_{a,k},\tilde{W}_{c,k})\rightarrow 0,\ {\rm
as}\,k\rightarrow+\infty,$ if $\kappa_{\star}(x_{k})\rightarrow 0$,
$\star=a,c$ in turns.

Proof. Please refer to the Appendix. $\square$

##  V Simulation and experimental results

The developed theoretical results are first verified with two robot simulated
examples. Please see Appendix -C for the detailed implementation steps and
results. In this section, we focus on the applications of our approach to two
real-world intelligent vehicles. Specifically, an integrated path following
and collision avoidance problem is considered, which represents a crucial
capability for navigation of intelligent vehicles under unknown and dynamic
environments [51, 52].

TABLE I: Numerical comparisons in Safety Gym with randomly generated obstacle
positions.

Approach | Collision rate | Target reach | Average speed (m/s) | Training time (s) | Episode | Samples | Training scenario | Deployment scenario
---|---|---|---|---|---|---|---|---
CPO | 0.1 | 0.9 | 0.82 | 2.4e4 | 1000 | 3.3e5 | Safety Gym | Safety Gym
TRPO-L | 0.095 | 0.905 | 0.85 | 2.8e4 | 1000 | 3.3e5 | Safety Gym | Safety Gym
PPO-L | 0.095 | 0.905 | 0.84 | 2.7e4 | 1000 | 3.3e5 | Safety Gym | Safety Gym
DDPG-CS | – | – | – | 2.8e4 | 1000 | 3.3e5 | with data from (25) | –
SAC-CS | – | – | – | 2.8e4 | 1000 | 3.3e5 | with data from (25) | –
Ours without MPE | 0.025 | 0.975 | 0.76 | 1.7 | 100 | 5e3 | with model (25) | Safety Gym
0.03 | 0.97 | 0.8 | – | – | – | – | –
Ours | 0 | 0.945 | 0.76 | – | – | – | – | –

Figure 2:  (a) Simulation scenario in Safety Gym: the objective is to move the
vehicle (red) to the green region while avoiding two static obstacles (grey),
the moving soft object (purple) is not considered in the controller design;
(b) Experimental platform of the differential-drive vehicle and testing
scenario. TABLE II: Numerical comparisons in Safety Gym with generated
obstacles on the path between the target and vehicle.

Approach | Collision rate | Target reach | Average speed (m/s) | Training time (s) | Episode | Samples
---|---|---|---|---|---|---
CPO | 0.815 | 0.185 | 0.76 | 2.4e4 | 1000 | 3.3e5
TRPO-L | 0.835 | 0.165 | 0.8 | 2.8e4 | 1000 | 3.3e5
PPO-L | 0.8 | 0.2 | 0.78 | 2.7e4 | 1000 | 3.3e5
Ours without MPE | 0.23 | 0.77 | 0.76 | 1.7 | 100 | 5e3
Ours | 0 | 0.595 | 0.76 | – | – | –

###  V-A Application to a Differential-Drive Vehicle: Offline Learning
Scenario

Consider the integrated path following and collision avoidance problem of a
differential-drive vehicle. Its kinematics model is

$\dot{q}=(\dot{p}_{x},\dot{p}_{y},\dot{\theta})=(v_{o}\cos\theta,v_{o}\sin\theta,\omega),$
(24)

where $\text{(}p_{x},p_{y}\text{)}$ is the coordinate of the vehicle in
Cartesian frame, $\theta$ is the yaw angle,
$u=\left[v_{o},\omega\right]^{\top}$ is the control input, where $v_{o}$ and
$\omega$ are the linear velocity and yaw rate, respectively.

Let us define the path following error as $e=q_{r}-q$, where $q_{r}$ is the
reference state. One can write the error model as

$\left\\{\begin{array}[]{l}\dot{e}_{x}=\omega e_{y}-v_{o}+v_{o,r}\cos
e_{\theta}\\\ \dot{e}_{y}=-\omega e_{x}+v_{o,r}\sin e_{\theta}\\\
\dot{e}_{\theta}=\omega_{r}-\omega,\\\ \end{array}\right.$ (25)

where $(e_{x},e_{y},e_{\theta})=:e$, $v_{o,r}$ and $\omega_{r}$ are the
reference inputs.

In implementation, model (25) was discretized with a sampling interval
$\varDelta t=0.05s$ to derive the model like (1). The constraint for collision
avoidance was typically formulated as
$\mathcal{X}_{k}=\\{(p_{x},p_{y})|\|(p_{x},p_{y})-c_{k}\|\geq d\\}$, where $d$
and $c_{k}$ are the radius and center of the obstacle respectively. Also, the
size of $\mathcal{X}_{k}$ was properly shrunk by increasing $d$ to account for
uncertainties. In the training, the penalty matrices were selected as $Q=I$,
$R=0.1$, $\mu=0.001$. The discounting factor $\gamma$ was $\gamma=0.95$. The
relaxing factor $\kappa_{b}$ was $\kappa_{b}=0.05$. The basis functions
$\sigma_{c}(x)$ and $\sigma_{a}(x)$ were chosen as hyperbolic tangent
activation functions with $N_{c}=N_{u}=4$. The step $L$ was chosen as $L=10$.
Weights $W_{c}$ and $W_{a}$ were initialized with uniformly random numbers.

Simulation results using Safety Gym environment [53]. We tested our approach
in the Safety Gym environment with the MoJoCo simulator [54]. Our method was
compared with some of the state-of-the-art safe RL algorithms: constrained
policy optimization (CPO) [55], trust region policy optimization with
Lagrangian methods (TRPO-L) [53], proximal policy optimization with Lagrangian
methods (PPO-L) [53], deep deterministic policy gradient [56] with cost
shaping (DDPG-CS), and soft actor-critic (SAC) [3] with cost shaping (SAC-CS).
Note that the safety-aware RL [28] was not directly applicable in this case
since it is nontrivial to find an invertible barrier function of the obstacle
constraint. In the training stage, all the parameter settings of CPO, TRPO-L,
and PPO-L were consistent with that in [53]. In DDPG-CS and SAC-CS, we used
the same cost function as ours, i.e., (10). We directly deployed the offline
learned control policy using the kinematics model in implementation since we
did not know the vehicle’s dynamic model. All the comparative algorithms were
trained and deployed using the same environment in Safety Gym. The simulation
results in Table I show that our approach outperforms all the comparative
algorithms in data efficiency, collision avoidance, and performance (see the
video
details222https://youtube.com/playlist?list=PLPE5-2sIdTlhk5r0VQr-66PBEqpAXvdAx.
or https://pan.baidu.com/s/1NxJ-zgD4ZdVvqIXQgJkCqg, with extracting code:
9426.).

###### Remark 8

There are failures under our offline learned policy without MPE for the
following reasons. First, the obstacles’ locations were generated randomly
without considering the vehicle’s physical limit. Second, the estimated
uncertainty is inaccurate since the inputs in Safety Gym are saturated values
with unknown physical interpretations. These issues lead to Theorem 2 being
not verified. In our case, one can guarantee safety (possibly with
conservativeness) by activating our MPE mechanism even if the model is
inaccurate, see Table I and II.

As shown in Table II, when the obstacles overlapped with the reference path
between the target and vehicle, our approach offers a significant performance
improvement compared with other adopted approaches2. The DDPG-CS and SAC-CS
failed to obtain the converged policy after several training trials. Therefore
we are unable to show the results in the Tables. In summary, our approach
outperforms the comparative model-free safe RL approaches for the following
two reasons. Firstly, the proposed barrier force-inspired control policy
structure has a clear physical interpretation to guarantee safety online and
improve the generalization ability. Secondly, our approach is model-based,
facilitating multi-step policy evaluation online.

Real-world experimental results with comparisons to nonlinear MPC algorithms.
We also tested our proposed algorithm on a real-world differential-drive
vehicle platform. The control task is to follow a predefined reference path
(with $v_{o,r}=0.7$ m/s) while passing and avoiding collision with a moving
object (vehicle) that is traveling along the reference path. In such a
situation, the conflict between the goals of path following and collision
avoidance leads to a challenging multi-objective control problem.

In the experiment, the vehicle was equipped with a Laptop running Ubuntu in an
Intel i7-8550U<EMAIL_ADDRESS>GHz. The sampling interval was set as $\varDelta
t=0.1$s. We directly deployed the offline learned policy of our approach to
control the vehicle. At each sampling instant, the onboard laptop computed the
control input in real-time using the state information, which was periodically
measured by the onboard satellite inertial guidance integrated positioning
system (SIGIPS). To simplify the experimental setup, another wheeled vehicle
following the reference path with a lower speed profile ($v_{o,r}=0.3$ m/s)
was regarded as the obstacle to be avoided. Its position and velocity
information was measured in real-time by SIGIPS and transmitted to the ego
vehicle via a WIFI network.

TABLE III: Online average computational time in each time step (Unit=ms).

Method | Ours | NMPC-c | NMPC-e | NMPC-cbf
---|---|---|---|---
Online deploy | Online learning
MATLAB | 0.3 | 2.2 | 1360 | 1360 | 1450
C++ | 0.04 | – | 140 | 135 | 94

The following MPC algorithms were adopted for comparison.

  1. 1.

A nonlinear MPC algorithm with nonconvex circular constraints (NMPC-c), where
the constraint was designed according to [57]. The vehicle obstacle was
approximated by a circle, i.e., we enforce constraint $(\Delta
p_{x})^{2}+(\Delta p_{y})^{2}>d_{o}^{2}$ in NMPC-c, where $\Delta p_{x}$ and
$\Delta p_{y}$ were deviations from the robot to the obstacles in the
associated coordinate axes, $d_{o}=1$m.

  2. 2.

A nonlinear MPC algorithm with nonconvex ellipsoidal constraints according to
[58]. The vehicle obstacle was approximated by an ellipsoid, where the semi-
major axis of the ellipsoid was in the direction of the reference path. The
semi-major radius and semi-minor radius were computed as $1.517$m and $1.017$m
respectively according to [58].

  3. 3.

A nonlinear MPC algorithm with control barrier function [59] (NMPC-cbf). The
collision avoidance constraint is formulated by a control barrier function
constraint, i.e., $h(k+1)-h(k)\geq-\eta h(k)$, where $h=(\Delta
p_{x})^{2}+(\Delta p_{y})^{2}-d_{o}^{2}$ is a control barrier function, the
parameter $\eta$ is a positive scalar which is properly tuned for fair
comparisons (see Table IV).

The stage costs of all the comparative MPC algorithms were designed the same
as in (4), and the prediction horizon was set as $N_{p}=20$. According to
[58], the following potential function

$J_{\rm{p}}(k)=\sum_{j=0}^{N_{p}-1}\mu_{p}\frac{1}{(\Delta
p_{x}(k+j))^{2}+(\Delta p_{y}(k+j))^{2}+\epsilon_{p}},$

was additionally adopted to improve the collision avoidance performance in
NMPC-c and NMPC-e, $\epsilon_{p}$ was chosen as $0.0001$, and $\mu_{p}$ was
tuned for fair comparisons (see Table IV). All the MPC algorithms were solved
at each sampling interval based on the CasADi toolbox [60] with an Ipopt
solver [61]. All the algorithms were tested under different reference
profiles. Experimental results under dynamic collision avoidance were
illustrated in Table IV and Figs. 4-8 in Appendix -B (see the video details333
https://pan.baidu.com/s/1NxJ-zgD4ZdVvqIXQgJkCqg, with extracting code: 9426.).
The results show that the NMPC-c and NMPC-e failed in realizing overtaking and
followed behind the moving obstacle when the adopted reference points were
dense, while our approach can realize conflict resolution in all scenarios.
Also, our approach outperforms NMPC-c and NMPC-e in terms of the planning
performance and path following performance (see Table IV). In addition to the
unique policy design and learning mechanism of our approach, the performance
improvement to the MPC algorithms is also due to the significant computational
load reduction (see Table III, and Fig. 10 in Appendix -B). To further show
the effectiveness of our approach, we carried out extra tests by manually
manipulating the moving obstacle to block the path of the ego vehicle when the
latter reacted promptly to avoid collision successfully (see Fig. 9 in
Appendix -B).

Figure 3:  A: The HongQi EHS3 autonomous driving experimental platform. B: The
upper panel presents the road map with road boundary constraints, where “S”
stands for the starting point, “E” stands for the ending point, and the red
line is the reference path for path following control. The lower panel
presents the corresponding state errors Compared with offline learning case, a
significantly improved performance can be achieved by online policy learning.
C: The upper panel presents the road map with collision avoidance scenario,
while the lower panel gives the numerical state errors.

###  V-B Application of an Ackermann-Drive Vehicle: Online Learning Scenario

Consider the path following control of an Ackermann-drive vehicle with
collision avoidance. Its simplified lateral dynamics is described by a
“bicycle” model (cf. [62]), that is

$\displaystyle\dot{X}=v_{x}\cos\varphi-v_{y}\sin\varphi$
$\displaystyle\dot{Y}=v_{x}\sin\varphi+v_{y}\cos\varphi$
$\displaystyle\dot{v}_{y}=-v_{x}\dot{\varphi}+\frac{2}{m}\left[C_{f}\left(\delta-\frac{v_{y}+l_{f}\dot{\varphi}}{v_{x}}\right)+C_{r}\frac{l_{r}\dot{\varphi}-v_{y}}{v_{x}}\right]$
$\displaystyle\ddot{\varphi}=\frac{2}{I_{z}}\left[l_{f}C_{f}\left(\delta-\frac{v_{y}+l_{f}\dot{\varphi}}{v_{x}}\right)-l_{r}C_{r}\frac{l_{r}\dot{\varphi}-v_{y}}{v_{x}}\right],$
(26)

where $X$ and $Y$ are the coordinates of the vehicle center of mass in the
Cartesian frame $XoY$, $v_{x}$ and $v_{y}$ are the longitudinal and lateral
velocities respectively, $\varphi$ is the yaw angle, $I_{z}=4175$${\rm
kg}\cdot m^{2}$ is the yaw moment of inertia, $m=1723$$\rm kg$ is the mass of
the vehicle, $C_{f}=66900N$ and $C_{r}=62700N$ are the cornering stiffness of
the front and rear tires, respectively, $l_{f}=1.322m$, $l_{r}=1.468m$,
$\delta$ is the front wheel angle variable to be manipulated.

Given path reference points $(X^{r},Y^{r})$ and $v_{x}$, we aim to minimize
the lateral distance from the vehicle center of mass to the nearest reference
point while avoiding potential collisions with obstacles. To this end, let the
nearest point be $(X_{p}^{r},Y_{p}^{r})$, then one can compute the reference
yaw angle $\varphi^{r}_{p}$. Define $e_{y}=-(X-X_{p}^{r}){\rm
sin}(\varphi_{p}^{r})+(Y-Y_{p}^{r}){\rm cos}(\varphi_{p}^{r}),$
$e_{\varphi}=\varphi-\varphi_{p}^{r}$. Let
$x=(e_{y},\dot{e}_{y},e_{\varphi},\dot{e}_{\varphi})$, then one can obtain the
continuous-time lateral dynamical model:
$\dot{x}=F_{1}(x)+F_{2}(x)\delta+F_{3}(x)\varphi_{p}^{r},$ where $F_{1}(0)=0$,
$F_{3}(0)\neq 0$. Since $(x,\delta)=0$ might not be an equilibrium point if
$\varphi_{p}^{r}\neq 0$, we introduced a virtual control variable
$u=\delta+\delta_{f}$, where $\delta_{f}$ was selected such that
$F_{2}(x)\delta_{f}=F_{3}(x)\varphi_{p}^{r}$. Consequently, the lateral
dynamical model was discretized with a sampling interval $\Delta t=0.02s$,
i.e., $x_{k+1}=x_{k}+\Delta tF_{1}(x_{k})+\Delta tF_{2}(x_{k})u_{k}.$ In the
path following control task with collision avoidance, the cost function was
chosen as
$\bar{J}=\sum_{k=0}^{+\infty}\|x_{k}\|_{Q}^{2}+\|u_{k}\|_{R}^{2}+\mu\mathcal{B}_{k}(e_{y,k}),$
where $Q=I$, $R=1$, $\mu=0.02$. The basis functions $\sigma_{c}(x)$ and
$\sigma_{a}(x)$ were chosen as polynomial kernel functions with $N_{c}=10$ and
$N_{u}=14$.

Real-world experimental results2. We also tested our safe RL algorithm on the
real-world intelligent vehicle platform built with a HongQi EHS3 electric car
to realize the path following control (see Fig. 3). In the experiment, the
states of the vehicle were measured by a SIGIPS; then, the measured states
were transmitted to an industrial control computer, where the control policy
was computed using our approach with a sampling interval of $0.02$s. We first
applied our algorithm to follow a reference path with road boundaries (see
Fig. 3-B). Different from that in the simulation tests, the vehicle speed was
controlled by a PI controller to track a time-varying speed reference. This
caused a strong nonlinearity of the lateral dynamics, leading to extra
difficulties in the control task. The experimental results displayed in Fig. 3
show that the control policy of our approach can be learned offline and
deployed online safely, showing an impressive sim-to-real transfer capability.
Also, one can achieve better control performance by online learning the
control policy, which further demonstrates the adaptability of our approach to
dynamic environments.

To show the capability of dealing with time-varying state constraints, we
tested our approach to track a reference path that overlapped with obstacles
(see Fig. 3-C). Similarly, the location information of obstacles was assumed
to be pre-detected. In the experiment, the control policy was learned and
deployed synchronously online. The initial constraints were the road
boundaries. Then, the constraint on $e_{y}$ was changed accordingly once the
vehicle was near the obstacle. The vehicle using our approach can avoid
collision successfully and converge rapidly to the reference path after
completing the collision avoidance task (see again Fig. 3).

###  V-C Implementation Issues and Discussions

Implementation issues. First, the tuning parameter $\mu$ is suggested to be
chosen smaller than the entries of $Q$ and $R$ to obtain a satisfactory
control performance. A larger choice of $\mu$ might result in a safe but
conservative control policy. Second, the initial values of $W_{a,\sigma}$,
$\hat{K}$, and $\hat{\rho}$ in the actor must be properly selected such that
the initial control policy with (19) is $L$-step safe, which is a prior
condition in Theorem 1. Finally, the relaxing factor $\kappa_{b}$ in Lemma 1
must also be selected properly. A smaller choice is suggested if a less
conservative control policy is expected, while a larger choice can be made to
ensure absolute control safety.

Discussions. As a prominent feature, our approach can learn an explicit
control policy offline and deploy it to a different control scenario even if
the concerned constraints are nonlinear and nonconvex. However, in MPC, the
control action must be computed online by periodically solving an optimization
problem [39], which can be difficult for the on-the-fly implementation under
nonlinear and nonconvex constraints, see Section V-A. As shown in the
simulation and real-world experiments, our learned policy using an inaccurate
model shows an impressive sim-to-real transfer capability compared with state-
of-the-art model-free RL approaches. In the experiments of differential-drive
vehicles, our approach outperforms comparative MPC algorithms under
measurement noises and modeling uncertainties. Indeed, our approach is a step
forward in applying safe RL to the real-world intelligent vehicle control
problem.

##  VI Conclusions

This paper proposed a safe RL algorithm with a barrier-based control policy
structure and a multi-step policy evaluation mechanism for the optimal control
of discrete-time nonlinear systems with time-varying safety constraints. Under
certain conditions, safety can be guaranteed by our approach in both online
and offline learning cases. Our approach can solve continuous control tasks in
the environment with abrupt changes, both online and offline. The convergence
and robustness of our safe RL under nominal and disturbed scenarios were
proven, respectively. The convergence condition of the barrier-based actor-
critic learning algorithm was obtained.

Besides numerical simulations for theoretical verification, we tested our
approach in two real-world intelligent vehicle platforms. The simulation and
real-world experiment results illustrate that our method outperforms state-of-
the-art safe RL approaches in control safety, and shows an impressive sim-to-
real transfer capability and a satisfactory real-world online learning
performance. In general, the proposed safe RL is a step forward in applying
safe RL to the optimal control of real-world nonlinear physical systems with
time-varying safety constraints. Future works will consider the extension to
model-free safe RL with theoretical guarantees.

## References

  * [1] Ivo Grondman, Lucian Busoniu, Gabriel AD Lopes, and Robert Babuska.  A survey of actor-critic reinforcement learning: Standard and natural policy gradients.  IEEE Transactions on Systems, Man, and Cybernetics, Part C (Applications and Reviews), 42(6):1291–1307, 2012.
  * [2] Sangwoon Kim, David Donghyun Kim, and Brian Anthony.  Dynamic control of a fiber manufacturing process using deep reinforcement learning.  IEEE/ASME Transactions on Mechatronics, 2021.
  * [3] Tuomas Haarnoja, Aurick Zhou, Pieter Abbeel, and Sergey Levine.  Soft actor-critic: Off-policy maximum entropy deep reinforcement learning with a stochastic actor.  In International Conference on Machine Learning, pages 1861–1870. PMLR, 2018.
  * [4] Derong Liu and Qinglai Wei.  Policy iteration adaptive dynamic programming algorithm for discrete-time nonlinear systems.  IEEE Transactions on Neural Networks and Learning Systems, 25(3):621–634, 2013.
  * [5] Huaguang Zhang, Yanhong Luo, and Derong Liu.  Neural-network-based near-optimal control for a class of discrete-time affine nonlinear systems with control constraints.  IEEE Transactions on Neural Networks, 20(9):1490–1503, 2009.
  * [6] Yu Jiang and Zhong-Ping Jiang.  Computational adaptive optimal control for continuous-time linear systems with completely unknown dynamics.  Automatica, 48(10):2699–2704, 2012.
  * [7] Jaehyun Lim, Seungchul Ha, and Jongeun Choi.  Prediction of reward functions for deep reinforcement learning via gaussian process regression.  IEEE/ASME Transactions on Mechatronics, 25(4):1739–1746, 2020.
  * [8] Hui Wang, Jiawen Xu, Chuan Sun, Ruqiang Yan, and Xuefeng Chen.  Intelligent fault diagnosis for planetary gearbox using time-frequency representation and deep reinforcement learning.  IEEE/ASME Transactions on Mechatronics, 2021.
  * [9] Yongming Li, Tingting Yang, and Shaocheng Tong.  Adaptive neural networks finite-time optimal control for a class of nonlinear systems.  IEEE Transactions on Neural Networks and Learning Systems, 31(11):4451–4460, 2020.
  * [10] Javier Garcıa and Fernando Fernández.  A comprehensive survey on safe reinforcement learning.  Journal of Machine Learning Research, 16(1):1437–1480, 2015.
  * [11] Yinlam Chow, Ofir Nachum, Aleksandra Faust, Edgar Duenez-Guzman, and Mohammad Ghavamzadeh.  Lyapunov-based safe policy optimization for continuous control.  arXiv preprint arXiv:1901.10031, 2019.
  * [12] Tobias Kessler, Klemens Esterle, and Alois Knoll.  Mixed-integer motion planning on german roads within the apollo driving stack.  IEEE Transactions on Intelligent Vehicles, online access, 2022, 10.1109/TIV.2022.3162671.
  * [13] Felix Berkenkamp, Matteo Turchetta, Angela P Schoellig, and Andreas Krause.  Safe model-based reinforcement learning with stability guarantees.  Advances in Neural Information Processing Systems 30, 2:909–919, 2018.
  * [14] Krishnan Srinivasan, Benjamin Eysenbach, Sehoon Ha, Jie Tan, and Chelsea Finn.  Learning to be safe: Deep RL with a safety critic.  2020\.
  * [15] Ming Yu, Zhuoran Yang, Mladen Kolar, and Zhaoran Wang.  Convergent policy optimization for safe reinforcement learning.  arXiv preprint arXiv:1910.12156, 2019.
  * [16] Tengyu Xu, Yingbin Liang, and Guanghui Lan.  Crpo: A new approach for safe reinforcement learning with convergence guarantee.  In International Conference on Machine Learning, pages 11480–11491. PMLR, 2021.
  * [17] Haitong Ma, Jianyu Chen, Shengbo Eben Li, Ziyu Lin, Yang Guan, Yangang Ren, and Sifa Zheng.  Model-based constrained reinforcement learning using generalized control barrier function.  arXiv preprint arXiv:2103.01556, 2021.
  * [18] Tsung-Yen Yang, Justinian Rosca, Karthik Narasimhan, and Peter J Ramadge.  Accelerating safe reinforcement learning with constraint-mismatched baseline policies.  In International Conference on Machine Learning, pages 11795–11807. PMLR, 2021.
  * [19] Subin Huh and Insoon Yang.  Safe reinforcement learning for probabilistic reachability and safety specifications: A Lyapunov-based approach.  arXiv preprint arXiv:2002.10126, 2020.
  * [20] Liyuan Zheng, Yuanyuan Shi, Lillian J Ratliff, and Baosen Zhang.  Safe reinforcement learning of control-affine systems with vertex networks.  In Learning for Dynamics and Control, pages 336–347. PMLR, 2021\.
  * [21] Zahra Marvi and Bahare Kiumarsi.  Safe reinforcement learning: A control barrier function optimization approach.  International Journal of Robust and Nonlinear Control, 31(6):1923–1940, 2021.
  * [22] Baiming Chen, Zuxin Liu, Jiacheng Zhu, Mengdi Xu, Wenhao Ding, Liang Li, and Ding Zhao.  Context-aware safe reinforcement learning for non-stationary environments.  In 2021 IEEE International Conference on Robotics and Automation (ICRA), pages 10689–10695. IEEE, 2021.
  * [23] Yutong Li, Nan Li, H Eric Tseng, Anouck Girard, Dimitar Filev, and Ilya Kolmanovsky.  Safe reinforcement learning using robust action governor.  In Learning for Dynamics and Control, pages 1093–1104. PMLR, 2021\.
  * [24] Lukas Brunke, Melissa Greeff, Adam W Hall, Zhaocong Yuan, Siqi Zhou, Jacopo Panerati, and Angela P Schoellig.  Safe learning in robotics: From learning-based control to safe reinforcement learning.  arXiv preprint arXiv:2108.06266, 2021.
  * [25] Spencer M Richards, Felix Berkenkamp, and Andreas Krause.  The Lyapunov neural network: Adaptive stability certification for safe learning of dynamical systems.  In Conference on Robot Learning, pages 466–476. PMLR, 2018.
  * [26] Changyin Sun, Wei He, and Jie Hong.  Neural network control of a flexible robotic manipulator using the lumped spring-mass model.  IEEE Transactions on Systems, Man, and Cybernetics: Systems, 47(8):1863–1874, 2016.
  * [27] Linghuan Kong, Wei He, Chenguang Yang, Zhijun Li, and Changyin Sun.  Adaptive fuzzy control for coordinated multiple robots with constraint using impedance learning.  IEEE Transactions on Cybernetics, 49(8):3052–3063, 2019.
  * [28] Yongliang Yang, Yixin Yin, Wei He, Kyriakos G Vamvoudakis, Hamidreza Modares, and Donald C Wunsch.  Safety-aware reinforcement learning framework with an actor-critic-barrier structure.  In 2019 American Control Conference (ACC), pages 2352–2358. IEEE, 2019.
  * [29] Nathan Fulton and André Platzer.  Safe reinforcement learning via formal methods: Toward safe control through proof and learning.  In Proceedings of the AAAI Conference on Artificial Intelligence, volume 32, 2018.
  * [30] Matteo Turchetta, Andrey Kolobov, Shital Shah, Andreas Krause, and Alekh Agarwal.  Safe reinforcement learning via curriculum induction.  arXiv preprint arXiv:2006.12136, 2020.
  * [31] Mohammed Alshiekh, Roderick Bloem, Rüdiger Ehlers, Bettina Könighofer, Scott Niekum, and Ufuk Topcu.  Safe reinforcement learning via shielding.  In Proceedings of the AAAI Conference on Artificial Intelligence, volume 32, 2018.
  * [32] Mario Zanon and Sébastien Gros.  Safe reinforcement learning using robust MPC.  IEEE Transactions on Automatic Control, 2020.
  * [33] Brijen Thananjeyan, Ashwin Balakrishna, Suraj Nair, Michael Luo, Krishnan Srinivasan, Minho Hwang, Joseph E Gonzalez, Julian Ibarz, Chelsea Finn, and Ken Goldberg.  Recovery RL: Safe reinforcement learning with learned recovery zones.  IEEE Robotics and Automation Letters, 6(3):4915–4922, 2021.
  * [34] William Saunders, Girish Sastry, Andreas Stuhlmueller, and Owain Evans.  Trial without error: Towards safe reinforcement learning via human intervention, 2017.
  * [35] Nolan Wagener, Byron Boots, and Ching-An Cheng.  Safe reinforcement learning using advantage-based intervention.  arXiv preprint arXiv:2106.09110, 2021.
  * [36] Yichen Chen and Mengdi Wang.  Stochastic primal-dual methods and sample complexity of reinforcement learning.  arXiv preprint arXiv:1612.02516, 2016.
  * [37] Santiago Paternain, Miguel Calvo-Fullana, Luiz FO Chamon, and Alejandro Ribeiro.  Safe policies for reinforcement learning via primal-dual methods.  arXiv preprint arXiv:1911.09101, 2019.
  * [38] Dongsheng Ding, Xiaohan Wei, Zhuoran Yang, Zhaoran Wang, and Mihailo Jovanovic.  Provably efficient safe exploration via primal-dual policy optimization.  In International Conference on Artificial Intelligence and Statistics, pages 3304–3312. PMLR, 2021.
  * [39] Xinglong Zhang, Wei Pan, Riccardo Scattolini, Shuyou Yu, and Xin Xu.  Robust tube-based model predictive control with koopman operators.  Automatica, 137:110114, 2022.
  * [40] Peter Geibel and Fritz Wysotzki.  Risk-sensitive reinforcement learning applied to control under constraints.  Journal of Artificial Intelligence Research, 24:81–108, 2005.
  * [41] Anand Balakrishnan and Jyotirmoy V Deshmukh.  Structured reward shaping using signal temporal logic specifications.  In 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 3481–3486. IEEE, 2019.
  * [42] Yujing Hu, Weixun Wang, Hangtian Jia, Yixiang Wang, Yingfeng Chen, Jianye Hao, Feng Wu, and Changjie Fan.  Learning to utilize shaping rewards: A new approach of reward shaping.  arXiv preprint arXiv:2011.02669, 2020.
  * [43] Chen Tessler, Daniel J Mankowitz, and Shie Mannor.  Reward constrained policy optimization.  arXiv preprint arXiv:1805.11074, 2018.
  * [44] Biao Luo, Derong Liu, Tingwen Huang, and Jiangjiang Liu.  Output tracking control based on adaptive dynamic programming with multistep policy evaluation.  IEEE Transactions on Systems, Man, and Cybernetics: Systems, 49(10):2155–2165, 2017.
  * [45] Adrian G Wills and William P Heath.  Barrier function based model predictive control.  Automatica, 40(8):1415–1422, 2004.
  * [46] Stephen Boyd, Stephen P Boyd, and Lieven Vandenberghe.  Convex optimization.  Cambridge university press, 2004.
  * [47] Qisong Yang, Thiago D Simão, Simon H Tindemans, and Matthijs TJ Spaan.  Wcsac: Worst-case soft actor critic for safety-constrained reinforcement learning.  In Proceedings of the Thirty-Fifth AAAI Conference on Artificial Intelligence. AAAI Press, online, 2021.
  * [48] Alexander I Cowen-Rivers, Daniel Palenicek, Vincent Moens, Mohammed Abdullah, Aivar Sootla, Jun Wang, and Haitham Ammar.  Samba: Safe model-based & active reinforcement learning.  arXiv preprint arXiv:2006.09436, 2020.
  * [49] Syed Ali Asad Rizvi and Zongli Lin.  Output feedback Q-learning control for the discrete-time linear quadratic regulator problem.  IEEE Transactions on Neural Networks and Learning Systems, 30(5):1523–1536, 2018.
  * [50] D Limon Marruedo, T Alamo, and EF Camacho.  Input-to-state stable MPC for constrained discrete-time nonlinear systems with bounded additive uncertainties.  In Proceedings of the 41st IEEE Conference on Decision and Control, 2002., volume 4, pages 4619–4624. IEEE, 2002.
  * [51] Antonio Sgorbissa.  Integrated robot planning, path following, and obstacle avoidance in two and three dimensions: Wheeled robots, underwater vehicles, and multicopters.  The International Journal of Robotics Research, 38(7):853–876, 2019\.
  * [52] Lionel Lapierre, Rene Zapata, and Pascal Lepinay.  Combined path-following and obstacle avoidance control of a wheeled robot.  The International Journal of Robotics Research, 26(4):361–375, 2007\.
  * [53] Alex Ray, Joshua Achiam, and Dario Amodei.  Benchmarking safe exploration in deep reinforcement learning.  arXiv preprint arXiv:1910.01708, 7, 2019.
  * [54] Emanuel Todorov, Tom Erez, and Yuval Tassa.  Mujoco: A physics engine for model-based control.  In 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 5026–5033. IEEE, 2012.
  * [55] Joshua Achiam, David Held, Aviv Tamar, and Pieter Abbeel.  Constrained policy optimization.  In International Conference on Machine Learning, pages 22–31. PMLR, 2017.
  * [56] Timothy P Lillicrap, Jonathan J Hunt, Alexander Pritzel, Nicolas Heess, Tom Erez, Yuval Tassa, David Silver, and Daan Wierstra.  Continuous control with deep reinforcement learning.  arXiv preprint arXiv:1509.02971, 2015.
  * [57] Christopher Jewison, R. Scott Erwin, and Alvar Saenz-Otero.  Model predictive control with ellipsoid obstacle constraints for spacecraft rendezvous.  IFAC-PapersOnLine, 48(9):257–262, 2015.  1st IFAC Workshop on Advanced Control and Navigation for Autonomous Aerospace Vehicles ACNAAV’15.
  * [58] Bruno Brito, Boaz Floor, Laura Ferranti, and Javier Alonso-Mora.  Model predictive contouring control for collision avoidance in unstructured dynamic environments.  IEEE Robotics and Automation Letters, 4(4):4459–4466, 2019.
  * [59] Jun Zeng, Bike Zhang, and Koushil Sreenath.  Safety-critical model predictive control with discrete-time control barrier function.  In 2021 American Control Conference (ACC), pages 3882–3889, 2021\.
  * [60] Joel A E Andersson, Joris Gillis, Greg Horn, James B Rawlings, and Moritz Diehl.  CasADi – A software framework for nonlinear optimization and optimal control.  Mathematical Programming Computation, 11(1):1–36, 2019.
  * [61] Andreas Wächter and Lorenz T Biegler.  On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming.  Mathematical programming, 106(1):25–57, 2006.
  * [62] Rajesh Rajamani.  Vehicle dynamics and control.  Springer Science & Business Media, 2011.
  * [63] Huaguang Zhang, Qinglai Wei, and Yanhong Luo.  A novel infinite-time optimal tracking control scheme for a class of discrete-time nonlinear systems via the greedy HDP iteration algorithm.  IEEE Transactions on Systems, Man, and Cybernetics, Part B (Cybernetics), 38(4):937–942, 2008.

###  -A Proofs of the Main Results

1) Proof of Theorem 1

(i) First note that,

$\begin{array}[]{rl}\bar{J}^{1}(x_{k})&=\sum_{l=0}^{L-1}\gamma^{l}\bar{r}(x_{k+l},u^{0}(x_{k+l}))+\gamma^{L}\bar{J}^{0}(x_{k+L})\vspace{2mm}\\\
&=\sum_{l=0}^{L-2}\gamma^{l}\bar{r}(x_{k+l},u^{0}(x_{k+l}))+\vspace{1mm}\\\
&\quad\quad\gamma^{L-1}\bar{r}(x_{k+L-1},u^{0}(x_{k+L-1}))+\gamma^{L}\bar{J}^{0}(x_{k+L})\vspace{2mm}\\\
&=\sum_{l=0}^{L-2}\gamma^{l}\bar{r}(x_{k+l},u^{0}(x_{k+l}))+\quad\mathrel{\mathop{\kern
0.0pt\rm
min}\limits_{\\{v_{k+L-1},\rho,K\\}}}\gamma^{L-1}\left\\{\right.\vspace{1mm}\\\
&\left.\bar{r}(x_{k+L-1},u(x_{k+L-1}))+J^{0}(x_{k+L})\right\\}\vspace{1mm}\\\
&\leq\sum_{l=0}^{L-2}\gamma^{l}\bar{r}(x_{k+l},u^{0}(x_{k+l}))+\gamma^{L-1}\bar{J}^{0}(x_{k+L-1})\vspace{1mm}\\\
&\vdots\vspace{1mm}\\\
&\leq\bar{r}(x_{k},u^{0}(x_{k}))+\gamma\bar{J}^{0}(x_{k+1})=V^{0}(x_{k})\leq\bar{J}^{0}(x_{k}).\end{array}$

Hence, $\bar{J}^{1}(x_{k})\leq V^{0}(x_{k})\leq\bar{J}^{0}(x_{k})$. Then one
can obtain the result by induction.

(iii) Since $\bar{J}^{i+1}(x_{k})\leq\bar{J}^{i}(x_{k})$ and
$\bar{J}^{i}(x_{k})$ is a semi-definite positive function in view of the
property of the barrier function. Then one can conclude that
$\bar{J}^{i}(x_{k})$ converges to a value denoted as
$\bar{J}^{\infty}(x_{k})\geq 0$. From Claim 1), one has
$\bar{J}^{\infty}(x_{k})\leq V^{\infty}(x_{k})\leq\bar{J}^{\infty}(x_{k})$,
then

$\begin{array}[]{ll}\bar{J}^{\infty}(x_{k})=V^{\infty}(x_{k})\\\
=\mathrel{\mathop{\kern 0.0pt\rm
min}\limits_{\\{v_{k},\rho,K\\}}}\bar{r}(x_{k},u(x_{k}))+\gamma\bar{J}^{\infty}(x_{k+1})\\\
=\mathrel{\mathop{\kern 0.0pt\rm
min}\limits_{\\{v_{k+i},\rho,K\\},i\in\mathbb{N}_{0}^{1}}}\sum_{l=0}^{1}\gamma^{l}\bar{r}(x_{k+l},u(x_{k+l}))+\gamma^{2}\bar{J}^{\infty}(x_{k+2})\\\
=\mathrel{\mathop{\kern 0.0pt\rm
min}\limits_{\\{v_{k+i},\rho,K\\},i\in\mathbb{N}_{0}^{L-1}}}\sum_{l=0}^{L-1}\gamma^{l}\bar{r}(x_{k+l},u(x_{k+l}))+\gamma^{L}\bar{J}^{\infty}(x_{k+L})\\\
=\mathrel{\mathop{\kern 0.0pt\rm
min}\limits_{\\{v_{k+i},\rho,K\\},i\in\mathbb{N}}}\bar{J}^{\infty}(x_{k})=\bar{J}^{\ast}(x_{k})\end{array}$
(27)

One can promptly conclude that,
$\bar{J}^{\infty}(x_{k})=\bar{J}^{\ast}(x_{k})$. And $v^{\infty}$,
$\rho^{\infty}$, and $K^{\infty}$ equal to the optimal values
$v^{\ast}(x_{k})$, $\rho^{\ast}$, and $K^{\ast}$, respectively. Consequently,
$u^{\infty}(x_{k})=v^{\infty}(x_{k})+\rho^{\infty}\triangledown_{v}\mathcal{B}_{k}(v^{\infty}(x_{k}))+K^{\infty}\triangledown_{x}\mathcal{B}_{k}(x_{k})=u^{\ast}(x_{k})$.
$\square$

2) Proof of Proposition 2

In view of (8) and (3), one can observe that $u_{k}=0$ for $k\geq\bar{k}$
provided that $v_{k}=0$ and $x_{k}=0$. Also, $\mathcal{B}_{k}(u_{k})=0$ as
$u_{k}=0$ and $s_{k}=0$ as $x_{k}=0$ for $k\geq\bar{k}$. Hence,
$\bar{J}^{\ast}(x_{k})<+\infty$ under $u^{\ast}(x_{k})$ with
$v^{\ast}(x_{k})$, $\rho^{\ast}$, $K^{\ast}$. As a result,
$y_{k},u_{k}\rightarrow 0$ as $k\rightarrow+\infty$ under policy
$u^{\ast}(x_{k})$. Consequently, $x_{k}\rightarrow 0$ as
$k\rightarrow+\infty$. $\square$

3) Proof of Theorem 2

(i) Offline learning scenario. In view of the Lipschitz continuity condition
(2), the difference of the real state $z$ under $u^{\ast}(z)$ and the nominal
state $x$ under $u^{\ast}(x)$ can be computed as
$\|z_{1}-x_{1|0}\|=\|w_{0}\|\leq\epsilon_{w},$ since $x_{0|0}=z_{0}$. Then, by
induction, one has

$\|z_{j}-x_{j|0}\|\leq\|z_{j-1}-x_{j-1|0}\|+\epsilon_{w}\leq\frac{L_{f}^{j}-1}{L_{f}-1}\varepsilon_{w}.$

Hence, the real state $z_{k}$ converges to
$\mathcal{D}_{\varepsilon_{w}}^{\infty}$ as $k\rightarrow+\infty$.
(ii) Online learning scenario. Given an offline learned control policy, at any
time instant $k$, it is learned to obtain an improved control performance
under the constraint $x_{k+j|k}\in\bar{\mathcal{X}}_{k+j}$. In this case, the
control policy can not be updated if the learned one is evaluated (by MPE) to
be inferior to the current one. Given the proof in the offline learning case,
the real state of the online learned policy converges to the set
$\mathcal{D}_{\varepsilon_{w}}^{\infty}$. $\square$

4) Proof of Theorem 3

Define a Lyapunov function

$\begin{array}[]{ll}V_{k}=\underbrace{{\alpha_{c}/\gamma_{c}}\text{tr}(\tilde{W}_{c,k}^{\top}\tilde{W}_{c,k})}+\underbrace{{1/\gamma_{a}}\text{tr}(\tilde{W}_{a,k}^{\top}\tilde{W}_{a,k})}\\\
\hskip 51.21495pt\alpha_{c}V_{c,k}\hskip 65.44133ptV_{a,k}\end{array}$ (28)

where $\alpha_{c}>1$. In view of the update rule (18) and (20), one can write
the difference of $V_{\star,k}$ ($\star=a,c$ in turns), i.e., $\Delta
V_{\star,k}=V_{\star,k+1}-V_{\star,k}$ as

$\Delta
V_{\star,k}=\text{tr}\left(2\tilde{W}_{\star,k}^{\top}\frac{\partial\delta_{\star,k}}{\partial
W_{\star,k}}+\gamma_{\star}(\frac{\partial\delta_{\star,k}}{\partial
W_{\star,k}})^{\top}\frac{\partial\delta_{\star,k}}{\partial
W_{\star,k}}\right)$ (29)

To first compute $\Delta V_{c}$, note that

$\frac{\partial\delta_{c}}{\partial
W_{c}}=\frac{\partial<\varepsilon_{c},\varepsilon_{c}>}{\partial
W_{c}}=2\Delta h_{c}\varepsilon_{c}^{\top}.$ (30)

Moreover,

$\begin{array}[]{ll}\varepsilon_{c,k}=\sum_{l=0}^{L-1}\gamma^{l}\bar{r}(x_{k+l},u_{k+l})+\gamma^{L}\hat{\bar{J}}(x_{k+L})-\hat{\bar{J}}(x_{k})\\\
=-\gamma^{L}{\bar{J}}^{\ast}(x_{k+L})+{\bar{J}}^{\ast}(x_{k})+\gamma^{L}\hat{\bar{J}}(x_{k+L})-\hat{\bar{J}}(x_{k})\\\
=-\tilde{W}_{c,k}^{\top}\Delta h_{c,k}-\Delta\kappa_{c,k}\end{array}$ (31)

where $\Delta\kappa_{c,k}$=$\gamma^{L}\kappa_{c,k+L}-\kappa_{c,k}$, the second
equality in (31) is due to the Bellman equation (14).

Taking (30) with (31) into (29), leads to

$\begin{array}[]{lll}\Delta
V_{c}&=&-4c_{1}\|\tilde{W}_{c}\|^{2}+4\Delta\kappa_{c}c_{2}\tilde{W}_{c}+4\gamma_{c}\Delta\bar{h}_{c}\Delta\kappa_{c}^{2}\\\
&\leq&-4(c_{1}-\lambda_{\rm
max}(\bar{c}_{2})\beta_{1})\|\tilde{W}_{c}\|^{2}+\epsilon_{\kappa,1}\par\end{array}$
(32)

where $c_{1}=(1-\gamma_{c}\Delta\bar{h}_{c})\Delta\bar{h}_{c}$,
$c_{2}=-(1-2\gamma_{c}\Delta\bar{h}_{c})\Delta h_{c}^{\top}$,
$\bar{c}_{2}=c_{2}^{\top}c_{2}$,
$\epsilon_{\kappa,1}=4(\gamma_{c}\Delta\bar{h}_{c}+1/\beta_{1})\Delta\kappa_{c}^{2}$,
$\beta_{1}$ is a tuning constant.

To compute $\Delta V_{a}$, inline with (30), one has

$\displaystyle\frac{\partial\delta_{a}}{\partial\hat{\rho}}=-2\triangledown\mathcal{B}(v)\varepsilon_{a}^{\top}\bar{R}$
(33a) where $\bar{R}=2R+\mu\triangledown^{2}\mathcal{B}(u)$,
$\triangledown\mathcal{B}(z)$ stands for $\triangledown_{z}\mathcal{B}(z)$ for
a general variable $z$; and
$\displaystyle\begin{array}[]{ll}\frac{\partial\delta_{a}}{\partial
W_{a1}}=-2h_{a1}\varepsilon_{a}^{\top}\bar{R}.\end{array}$ (33c)

With (33), we write $\Delta V_{a}=\Delta V_{a1}+\Delta V_{a2}$ where $\Delta
V_{a1}$ and $\Delta V_{a2}$ are given as

$\displaystyle\Delta V_{a1}$
$\displaystyle=4\text{tr}(-\tilde{W}_{a1}^{\top}h_{a1}\varepsilon_{a}^{\top}\bar{R}+\gamma_{a}\bar{R}^{\top}\varepsilon_{a}\bar{h}_{a1}\varepsilon_{a}^{\top}\bar{R})$
(34a) $\displaystyle\Delta V_{a2}$
$\displaystyle=4\text{tr}(-\tilde{\rho}\triangledown\mathcal{B}(v)\varepsilon_{a}^{\top}\bar{R}+\gamma_{a}\bar{R}^{\top}\varepsilon_{a}\bar{\mathcal{B}}_{v}\varepsilon_{a}^{\top}\bar{R}),$
(34b)

where $\tilde{\rho}=\rho-\hat{\rho}$, $\bar{h}_{a1}=h_{a1}^{\top}h_{a1}$,
$\bar{\mathcal{B}}_{v}=\triangledown\mathcal{B}(v)^{\top}\triangledown\mathcal{B}(v)$.
In view of (34), letting $\bar{h}_{a}=\bar{h}_{a1}+\bar{\mathcal{B}}_{v}$, one
can write $\Delta V_{a}$ as

$\begin{array}[]{lll}\Delta V_{a}&=&\Delta V_{a1}+\Delta V_{a2}\\\
&=&-4\text{tr}(\xi_{a}\varepsilon_{a}^{\top}\bar{R})+4\gamma_{a}\text{tr}(\bar{R}^{\top}\varepsilon_{a}\bar{h}_{a}\varepsilon_{a}^{\top}\bar{R}).\end{array}$
(35)

Let $g_{c}=\triangledown_{u}f(x,u)^{\top}(\triangledown h_{c}^{+})^{\top}$,
$\bar{\kappa}_{1}=\triangledown_{u}f(x,u)^{\top}\triangledown\kappa_{c}^{+}+2R\kappa_{a}$,
then

$\begin{array}[]{lll}\varepsilon_{a}&=&\nu^{d}-\nu^{\ast}+\nu^{\ast}-\nu\\\
&=&\nu^{d}+g_{c}W_{c}^{\ast}+\bar{\kappa}_{1}+2R(W_{a}^{\ast})^{\top}h_{a}+\\\
&&\mu\triangledown\mathcal{B}(u^{\ast})-2RW_{a}^{\top}h_{a}-\mu\triangledown\mathcal{B}(u)\end{array}$

In view of the definition of $\mathcal{B}$ and $G^{i}(u)=E^{i}u$, it holds
that

$\bar{\mathcal{B}}=\triangledown\mathcal{B}(u^{\ast})-\triangledown\mathcal{B}(u)=-D(\xi_{a}+\kappa_{a})$
(36)

where $D=\sum_{i=1}^{p_{u}}(E^{i})^{\top}E^{i}/(G^{i}(u^{\ast})G^{i}(u))$ for
$\bar{\sigma}\geq\kappa_{b}$ and $D=2H_{u}$ otherwise. Hence, with (36), one
has

$\begin{array}[]{lll}\varepsilon_{a}=g_{c}\tilde{W}_{c}+R_{d}\xi_{a}+\bar{\kappa}.\end{array}$
(37)

where $R_{d}=2R-\mu D$ and $\bar{\kappa}=\bar{\kappa}_{1}-\mu D\kappa_{a}$.
Taking (37) into (35), one can compute:

$\begin{array}[]{lll}\Delta
V_{a}=4(-\|\xi_{a}\|_{R_{d}\bar{R}}^{2}-\xi_{a}^{\top}\bar{R}g_{c}\tilde{W}_{c}-\xi_{a}^{\top}\bar{R}\bar{\kappa})+d\|\varepsilon_{a}\|_{\bar{R}^{2}}^{2}\end{array}$
(38)

where $d=4\gamma_{a}\bar{h}_{a}$. Applying Young’s inequality to (37), leads
to $\|\varepsilon_{a}\|^{2}\leq
3\|\tilde{W}_{c}\|_{\bar{g}_{c}}^{2}+3\|\xi_{a}\|_{R_{d}^{2}}^{2}+3\|\bar{\kappa}\|^{2}$,
where $\bar{g}_{c}=g_{c}^{\top}g_{c}$. Hence, one can conclude from (38) that

$\begin{array}[]{lll}\Delta
V_{a}\leq-\|\xi_{a}\|_{P}^{2}+(4/\beta_{2}+3d)\lambda_{\rm
max}({\bar{g}_{c}})\|\tilde{W}_{c}\|^{2}+\epsilon_{\kappa,2},\end{array}$ (39)

where
$P=4R_{d}\bar{R}-(\beta_{2}+\beta_{3})\bar{R}^{2}-3dR_{d}^{2}\bar{R}^{2},$
$\beta_{2}$, $\beta_{3}$ are tuning constants,
$\epsilon_{\kappa,2}=(4/\beta_{3}+3d)\|\bar{\kappa}\|_{\bar{R}^{2}}^{2}$.
Therefore, combining (32) and (39), it holds that

$\begin{array}[]{lll}\Delta
V&\leq&-\|(\xi_{a},\tilde{W}_{c})\|_{S}^{2}+\epsilon_{t}\end{array}$ (40)

where $\epsilon_{t}=\alpha_{c}\epsilon_{\kappa,1}+\epsilon_{\kappa,2}$, matrix
$S$ is given as

$\begin{array}[]{ll}S=\text{diag}\\{P,4\alpha_{c}(c_{1}-\bar{c}_{2}\beta_{1})-(4/\beta_{2}+3d)\lambda_{\rm
max}({\bar{g}_{c}})\\}.\end{array}$ (41)

Note that, in view of Assumption 2 and Lemma 1, $\lambda_{\rm
max}({\bar{g}_{c}})$ is bounded by $\lambda_{\rm
max}({\bar{g}_{c}})\leq\|\triangledown_{u}f(x,u)^{\top}(\triangledown
h^{+}_{c})\|^{2}\leq g_{m}^{2}(\bar{\sigma}_{c,m}^{2}+\mathcal{B}_{x,m}^{2}),$
$R_{d},\,\bar{R}\preccurlyeq 2R+2\mu H_{u},$ and $d$ can be made small by
tuing $\gamma_{a}$, i.e., $d\leq 4\gamma_{a}\|h_{a}\|^{2}\leq
4\gamma_{a}(\sigma_{a,m}^{2}+\mathcal{B}_{v,m}^{2}+\mathcal{B}_{x,m}^{2}):=d_{m}.$Hence,
thanks to (23a), one can first tune $\beta_{2}$ and $\beta_{3}$ such that
$P\succ 0$. Then, in view of (23b), one can tune $\alpha_{c}$ and $\beta_{1}$
to make sure the second term of (41) is positive-definite.

Since $\epsilon_{t}$ is bounded, let $\epsilon_{m}$ be the lower bound of
$\epsilon_{t}$; then in view of Assumption 5, with $S\succ 0$, it follows that
$\Delta V\leq 0$ for all
$\|(\xi_{a,k},\tilde{W}_{c,k})\|\geq\sqrt{\frac{\epsilon_{m}}{\lambda_{\rm
min}(S)}}.$ Consequently, $\|(\xi_{a,k},\tilde{W}_{c,k})\|\rightarrow 0,\
\text{as}\,k\rightarrow+\infty,$ provided that $\epsilon_{t}\rightarrow 0$.
$\square$

TABLE IV: Numerical comparisons under dynamic obstacles. Cost
$J_{e}=1/M\sum_{j=1}^{M}\|e_{j}\|^{2}$. $d_{r}$ represents the distance
between adjacent reference points. “S” and “F” stand for “Succeed” and “Fail”,
respectively.

Methods | Scenarios | Coll. avoid./overtak. | $J_{e}$ (in coll. avoid.) | $J_{e}$ (in path foll.)
---|---|---|---|---
Parameters |  $d_{o}$(m) |  $d_{r}$(m)
Ours | – | – | $0.07$ | S/S | 0.237 | 0.003
$0.56$ | S/S | 0.286 | 0.031
$1.12$ | S/S | 0.365 | 0.107
NMPC-c | $\mu_{p}=5\cdot 10^{-4}$ | 1 | $0.07$ | S/F | – | –
$0.56$ | S/S | 0.579 | 0.03
$1.12$ | S/S | 0.96 | 0.199
$\mu_{p}=5\cdot 10^{-3}$ | 1 | $0.07$ | S/F | – | –
$0.56$ | S/S | 0.452 | 0.03
$1.12$ | S/S | 0.639 | 0.139
NMPC-e | $\mu_{p}=5\cdot 10^{-4}$ | 1 | $0.07$ | S/F | – | –
$0.56$ | S/S | 0.62 | 0.029
$1.12$ | S/S | 0.782 | 0.158
$\mu_{p}=5\cdot 10^{-3}$ | 1 | $0.07$ | S/F | – | –
$0.56$ | S/S | 0.885 | 0.033
$1.12$ | S/S | 0.798 | 0.164
NMPC-cbf | $\eta=0.4$ | 1 | $0.07$ | S/F | – | –
$0.56$ | S/S | 0.669 | 0.031
$1.12$ | S/S | 0.798 | 0.164
$\eta=0.5$ | 1 | $0.07$ | S/F | – | –
$0.56$ | S/S | 0.618 | 0.03
$1.12$ | S/S | 1.12 | 0.118
$\eta=0.6$ | 1 | $0.07$ | S/F | – | –
$0.56$ | S/S | 0.717 | 0.05
$1.12$ | S/S | 1.254 | 0.273
$\eta=0.8$ | 1 | $0.07$ | S/F | – | –
$0.56$ | S/S | 1.048 | 0.03
$1.12$ | S/S | 0.944 | 0.35
$\eta=1.0$ | 1 | $0.07$ | S/F | – | –
$0.56$ | S/S | 0.589 | 0.03
$1.12$ | S/S | 0.961 | 0.139
$\eta=2.5$ | 1 | $0.56$ | S/S | 0.428 | 0.06
$1.12$ | S/S | 0.626 | 0.3
$\eta=5.0$ | 1 | $0.56$ | S/S | 0.772 | 0.05
$1.12$ | S/S | 0.951 | 0.45
1.1 | $0.56$ | S/S | 1.274 | 0.04
$1.12$ | S/S | 1.277 | 0.45
$\eta=7.5$ | 1 | $0.56$ | S/S | 0.808 | 0.055
$1.12$ | S/S | 0.644 | 0.116
1.1 | $0.56$ | S/S | 0.947 | 0.04
$1.12$ | S/S | 1.377 | 0.84
$\eta=10$ | 1 | $0.56$ | S/S | 1.136 | 0.1
$1.12$ | S/S | 0.781 | 0.36
1.1 | $0.56$ | S/S | 1.359 | 0.07
$1.12$ | S/S | 1.352 | 0.789
$\eta=12.5$ | 1 | $0.56$ | S/S | 1.623 | 0.05
$1.12$ | S/S | 1.384 | 0.567
1.1 | $0.56$ | S/S | 0.428 | 0.06
$1.12$ | S/S | 0.626 | 0.3
$\eta=15$ | 1 | $0.56$ | S/S | 0.58 | 0.04
$1.12$ | S/S | 1.11 | 0.549
1.1 | $0.56$ | S/S | 0.577 | 0.04

Figure 4: The experimental results on path-following and collision avoidance
by NMPC-c ($\mu_{p}=5\cdot 10^{-3}$): the shorter line with a gray shade
represents the route of the moving vehicle, while the longer line represents
the trajectory of the ego vehicle. When using dense reference points
($d_{r}=0.07$m), the ego vehicle was unable to pass, but was successful when
using sparse reference points. In the latter scenario, the collision avoidance
process caused the ego vehicle to experience a short transient period of rapid
speed variation. Figure 5: The experimental results on path-following and
collision avoidance by NMPC-e ($\mu_{p}=5\cdot 10^{-4}$): the shorter line
with a gray shade represents the route of the moving vehicle, while the longer
line represents the trajectory of the ego vehicle. When using dense reference
points ($d_{r}=0.07$m), the ego vehicle was unable to pass, but was successful
when using sparse reference points. In the latter scenario, the collision
avoidance process caused the ego vehicle to experience a short transient
period of rapid speed variation. Figure 6: The experimental results on path-
following and collision avoidance by NMPC-cbf ($\eta=0.5$): the shorter line
with a gray shade represents the route of the moving vehicle, while the longer
line represents the trajectory of the ego vehicle. When using dense reference
points ($d_{r}=0.07$m), the ego vehicle was unable to pass, but was successful
when using sparse reference points. In the latter scenario, the collision
avoidance process caused the ego vehicle to experience a short transient
period of rapid speed variation. Figure 7: The experimental results on path-
following and collision avoidance by NMPC-cbf ($\eta=1$): the shorter line
with a gray shade represents the route of the moving vehicle, while the longer
line represents the trajectory of the ego vehicle. When using dense reference
points ($d_{r}=0.07$m), the ego vehicle was unable to pass, but was successful
when using sparse reference points. In the latter scenario, the collision
avoidance process caused the ego vehicle to experience a short transient
period of rapid speed variation. Figure 8: The experimental results on path-
following and collision avoidance by our approach: the shorter line with a
gray shade represents the route of the moving vehicle, while the longer line
represents the trajectory of the ego vehicle. The ego vehicle avoided and
overtook the moving vehicle in all scenarios. Figure 9: Noncooperative dynamic
collision avoidance by our approach. The moving vehicle (obstacle) was
manipulated by a human remote control handle to block the path, when the ego
vehicle actively avoided collision with the obstacle. Figure 10: The CPU
running time comparison in C++. In many time instants, the computational time
values of the NMPC-c, NMPC-e, NMPC-cbf are greater than the adopted sampling
interval, i.e., 0.1s, which could hamper the control performance (see Table
IV), while the computational time value of our approach is much smaller and
its influence on the control performance can be negligible.

###  -B Auxiliary Real-World Experimental Results on the Differential-Drive
Vehicle

Figs. 4-8 present the experimental results of NMPC-c, NMPC-e, NMPC-e, and our
approach to path following with dynamic collision avoidance. Fig. 9 presents
the experimental results of our approach in a noncooperative dynamic collision
avoidance scenario, and Fig. 10 shows the significant advantage of our
approach in computational load reduction in the C++ environment.

###  -C Simulation on Regulation of a Mass-Point Robot

Consider the regulation control of a mass-point robot. Its discrete-time model
is described by $x_{k+1}=Ax_{k}+Bu_{k}$ where

$A=\begin{bmatrix}0.995&0.0998\\\
-0.0998&0.995\end{bmatrix},B=\begin{bmatrix}-0.2\\\ -0.1\end{bmatrix},$

$x=(x_{1},x_{2})$ are the positions to the references.

Figure 11: Simulation results for control of mass-point robot: The state,
control, and cost comparisons between our approach and algorithms in [28],
[44], and [63]. Right-bottom panel: Statistical comparison of cost with 500
repetitive experiments; red lines stand for mean value of $J$, black boxes are
the area of standard deviations of $J$, while black horizontal lines are the
maximal and minimal deviations. TABLE V: Comparisons in terms of safety rate
in mass-point robot example.

Approach | Ours | Safety-aware RL [28]
---|---|---
Time-invariant constraint | 100% | 100%
Time-varying constraint | 100% |  $\leq$ 95%

In the simulation process, the penalty matrices were selected as $Q=I$,
$R=0.1$, $\mu=0.001$. The discounting factor $\gamma$ was $\gamma=0.95$. The
step $L$ was chosen as $L=10$. The entries of weighting matrices $W_{c}$ and
$W_{a}$ were initialized with uniformly random numbers. The initial state was
$x=(-0.5,-0.5)$. The state and control were initially limited by $-1\leq
x_{1},x_{2}\leq 0.5,\quad-1\leq u\leq 0.3.$ Then at $k=285$, $x$ was reset as
$x=(-0.65,-0.65)$ and the constraints were changed as $-0.5\leq
x_{1},x_{2}\leq 0.3,\,-0.5\leq u\leq 0.1$.

The control performances are compared in Fig. 11, which shows that all
approaches can converge to the origin. In contrast, our approach and safety-
aware RL [28] can ensure safety constraint satisfaction in the control
process. Moreover, comparisons in terms of safety for 500 repetitive
experimental tests are listed in Table V, which illustrates that our approach
can ensure safety for all the performed tests, while it did not hold so for
safety-aware RL in [28] under time-varying safety constraints. The reason
behind this is the adopted multi-step policy evaluation mechanism in our
approach. From the box-plot in the right-bottom panel of Fig. 11, one can see
that the mean values of $J$ in all approaches are comparable and its standard
cost deviation in our approach is smaller than that in [28], [44], and [63].
The results reveal that the performance of our approach is more stable under
state and control constraints due to the proposed barrier-based control policy
design.

###  -D Simulation on Regulation of Van Der Pol Oscillator

Consider the regulation control of Van der Pol oscillator [39]. Its discrete-
time model is given as

$\left\\{\begin{array}[]{ll}x_{1,k+1}=x_{1,k}+\Delta tx_{2,k}\\\
x_{2,k+1}=x_{2,k}+\Delta
t(x_{2,k}-x_{1,k}^{2}x_{2,k}-x_{1,k}+u_{k})\end{array}\right.$

where $x_{1}$ and $x_{2}$ are the states, and $u$ is the control variable,
$\Delta t=0.01$s. In the simulation process, the penalty matrices were
selected as $Q=I$, $R=0.1$, $\mu=0.01$. The discounting factor $\gamma$ was
$\gamma=0.95$. The step $L$ was chosen as $L=10$. The entries of weighting
matrices $W_{c}$ and $W_{a}$ were initialized using saturated uniformly random
numbers such that Theorem 1 was fulfilled. Starting with an initial condition
$x_{0}=(-0.5,-0.5)$, the training was performed under time-varying state
constraints, see Fig. 12.

Figure 12: Simulation results for control of Van der Pol oscillator: The state
variables (left panel) and the stage cost (right panel) with our approach and
adopted comparative algorithms. In each training of safety-aware RL [28],
DDPG-CS, and SAC-CS, the control (learning) safety was not fulfilled, and the
learning process was terminated when the size of state constraint was suddenly
reduced. However, our approach can adapt to the constraint variation due to
the adopted control policy structure and multi-step policy evaluation
mechanism.  TABLE VI: Comparisons in terms of safety rate in Van der Pol
oscillator.

Approach | Ours | Safety-aware RL [28]
---|---|---
Time-invariant constraint | 100% | 100%
Time-varying constraint | 100% |  $\leq$ 50%

We compared our approach with two classic control methods, i.e., heuristic
dynamic programming (HDP) [63], multi-step heuristic dynamic programming
(MsHDP) [44] and three safe RL approaches, e.g., safety-aware RL [28], DDPG-CS
and SAC-CS. The control parameters of the proposed safe RL and the comparative
approaches in [28], [44], and [63] were set similarly. In DDPG-CS and SAC-CS,
the cost function was reshaped with the same barrier functions adopted in the
paper, and all the training parameters are fine-tuned according to [56] and
[3] respectively. The simulation results in Fig. 12 and Table VI show that our
approach can cope with time-varying state constraints in the control
(learning) process while safety-aware RL in [28], DDPG-CS, and SAC-CS could
fail due to the sudden change of constraints. The reason behind this is that
the adopted safe RL approaches could not predict future changes of safety
constraints and inform how to achieve safety by the actor-critic structure,
hence prone to failing in abruptly changed environments. MsHDP in [44] and HDP
in [63] can not guarantee safety constraint satisfaction. Moreover, our
approach converged faster than that in [28], [44], and [63] (see Fig. 12).