2021 IEEE International Conference on Robotics and Automation (ICRA 2021)

May 31 - June 4, 2021, Xi'an, China

Learning Barrier Functions with Memory for Robust Safe Navigation

Kehan Long∗ Cheng Qian∗ Jorge Cortés Nikolay Atanasov

Abstract— Control barrier functions are widely used to

enforce safety properties in robot motion planning and con-
trol. However, the problem of constructing barrier functions
online and synthesizing safe controllers that can deal with the
associated uncertainty has received little attention. This paper
investigates safe navigation in unknown environments, using
on-board range sensing to construct control barrier functions
online. To represent different objects in the environment, we use
the distance measurements to train neural network approxima-
tions of the signed distance functions incrementally with replay
memory. This allows us to formulate a novel robust control
barrier safety constraint which takes into account the error in
the estimated distance fields and its gradient. Our formulation
leads to a second-order cone program, enabling safe and stable
control synthesis in a prior unknown environments.

I. I NTRODUCTION Fig. 1: A ground robot equipped with a LiDAR scanner is navigat-

ing safely along a desired path (blue curve) in an unknown room.
Modern applications of ground, aerial, and underwater
mobile robots necessitate safe operation in unexplored and measurements to ensure safety. We focus on constructing
unstructured environments. Planning and control techniques CBFs, approximating the shape of the objects in the envi-
that ensure safety, relying on limited field-of-view obser- ronment, and introduce corresponding safety constraints in
vations, are critical for autonomous navigation. The semi- the synthesis of the robot’s control policy to enforce safety.
nal work of Khatib [1] introduced artificial potential fields The signed distance function (SDF) of a set Ω in a metric
to enable collision avoidance during not only the motion space determines the distance of a given point x to the
planning stage but also the real-time control of a mobile boundary of Ω, with sign indicating whether x is in Ω or
robot. Potential fields inspired subsequent work on joint path- not. SDFs are an implicit surface representation, employed
planning and control. Rimon and Koditschek [2] developed in computer vision and graphics for surface reconstruction
navigation functions, special artificial potential functions and rendering [7], [8]. In contrast with other object geometry
designed to simultaneously guarantee collision avoidance and representations, an SDF provides distance and gradient infor-
stabilization to a desired goal configuration. In the 2000’s, mation to object surfaces directly, which is necessary infor-
barrier certificates were proposed as a general construction mation for collision avoidance in autonomous navigation [9].
to verify safety of closed-loop nonlinear and hybrid systems In this work, we approximate the SDFs of observed objects
[3], [4]. Barrier certificates were extended by [5] to consider and define corresponding CBF safety constraints for control
control inputs explicitly and enable safe control design. Con- synthesis. Using the LiDAR measurements, we incrementally
trol barrier functions (CBFs) have become a key technique train a multilayer perceptron to model each obstacle’s SDF
for encoding safety constraints, and have been successfully shape online. To balance the accuracy and efficiency of SDF
employed, along with control Lyapunov functions (CLFs), reconstruction, we employ replay memory in training [10].
to encode stability requirements in quadratic program (QP) The obstacle SDF approximations provide distance and
control synthesis for control-affine nonlinear systems [6]. gradient information for the barrier functions. We take into
A common assumption in many CLF-CBF-QP works account the estimation errors for the controller synthesis,
is that the robot already has complete knowledge of the resulting in safety constraints where the input appears both
unsafe regions, encoded in an a priori known CBF. How- linearly and within an l2 -norm term. The presence of such
ever, navigation in unknown environments requires online constraints means that the control synthesis problem can no
estimation of unsafe regions using onboard sensing and, longer be stated as a QP but we show that the problem can be
hence, a CBF should also be constructed online. This paper formulated as a second-order cone program (SOCP), which is
considers a mobile robot equipped with a LiDAR scanner and still convex and can be solved efficiently. The proposed CLF-
tasked to follow a motion plan, relying on streaming range CBF-SOCP framework integrates the SDF estimation of the
∗ indicates equal contribution. CBFs to synthesize safe controls at each time instant, allow-
We gratefully acknowledge support from NSF RI IIS-2007141. ing the robot to safely navigate the unknown environment.
The authors are with the Contextual Robotics Institute, In summary, this paper makes the following contributions.
University of California San Diego, La Jolla, CA 92093, USA.
{k3long,chqian,cortes,natanasov}@ucsd.edu • An online incremental training approach, utilizing re-

978-1-7281-9077-8/21/$31.00 ©2021 IEEE 1124

 play memory, is proposed for deep neural network ap- are used to meet safety requirements. However, the barrier
proximation of the signed distance functions of objects functions in these approaches are assumed to be known.
observed with streaming distance measurements. When the environment is unknown, a robot may only rely
• Our analysis shows that incorporating safety constraints on the estimation of barrier functions using its sensors. A
that account for the worst-case estimation error of the closely related work by Srinivasan et al. [17] presents a
SDF approximations into a control synthesis optimiza- Support Vector Machine (SVM) approach for the online
tion problem leads to a convex SOCP formulation. synthesis of barrier functions from range data. By approxi-
mating the boundary between occupied and free space as the
II. R ELATED W ORK SVM decision boundary, the authors extract CBF constraints
This section reviews related work on scene representations and solve a CBF-QP to generate safe control inputs. Our
and CLF-CBF techniques for safe control. approach constructs CBFs directly from an approximation
Occupancy mapping techniques aim to partition an envi- of the object SDFs and explicitly considers the potential
ronment into occupied and free subsets using sensor obser- errors in the CBF constraints when formulating the control
vations. Octomap [11] is a probabilistic occupancy mapping synthesis optimization problem.
algorithm that uses a tree-based structure to store and update
the probability of occupied and free space based on streaming III. P ROBLEM F ORMULATION
range observations. Occupancy information alone may not be Consider a robot whose dynamics are governed by a non-
enough for safe planning and control, since many algorithms linear control-affine system:
require distance (or even gradient) information to the obstacle
surfaces. Voxblox [12] is an incremental truncated SDF ẋ = f (x) + g(x)u
(1)
mapping algorithm that approximates the distance to the y = v(x)
nearest surface at a set of weighted support points. The
where x ∈ X ⊆ Rn is the robot state, u ∈ U ⊆ Rm
approach also provides an efficient extension from truncated
is the control input, and y ∈ Y ⊆ Rw is the system
to complete (Euclidean) SDF using an online wavefront
output. Taking the robot in Sec. VI as an example, x is the
propagation algorithm. Fiesta [9] further improves the ef-
robot position and orientation, u is the linear and angular
ficiency and accuracy of building online SDF maps by using
velocity input, while y is the robot position. Assume that
two independent queues for inserting and deleting obstacles
f : Rn 7→ Rn , g : Rn 7→ Rn×m , and v : Rn 7→ Rw are
separately and doubly linked lists for map maintenance.
continuously differentiable functions. Define the admissible
These SDF representations, however, require discretizing the
control input space as U := {u ∈ Rm | Au ≤ b}, where
environment into voxels, resulting in potentially large mem-
A ∈ Rk×m and b ∈ Rk . The output space Y is a Euclidean
ory use. Recently, impressive results have been achieved in
robot workspace (e.g., robot positions), used for collision
object shape modeling using deep neural networks. DeepSDF
checking. The state space X is partitioned into a closed safe
[7] and Occupancy Nets [13] implicitly represent 3D shapes
set S and an open obstacle set O such that S ∩ O = ∅
by supervised learning via fully connected neural networks.
and X = S ∪ O. Correspondingly, the workspace Y is
Gropp et al. [14] further extended the DeepSDF model by
partitioned into a closed set v(S) and an open set v(O),
introducing the Eikonal constraint that any SDF function
obtained by applying the output function v to each element
must satisfy in training the neural network. We extend this
in S and O. The obstacle set O is further partitioned into K
offline training method in two ways: enabling online training
obstacles, denoted O1 , O2 , . . . OK , such that O = ∪Ki=1 Oi
by introducing replay memory, which helps the network to
and Oj ∩ Ok = ∅ for any 0 ≤ j, k ≤ K. Each of these
remember the reconstructed shape from past observations,
sets is defined through a continuously differentiable function
and adding truncated signed distance points to the training
ϕi : Rw 7→ R, with gradient ∇ϕi : Rw 7→ Rw . Formally,
set to ensure the correct sign of the approximated SDF.
Oi = {x ∈ X | ϕi (v(x)) < 0}. Note that ϕi (v(x)) < 0 if
Compared with state-of-the-art SDF methods [9], [12] for
the output y is in the open set v(Oi ) and ϕi (v(x)) = 0 if
navigation and obstacle avoidance, which rely on discretiza-
y is on its boundary ∂v(Oi ).
tion, our approach enables continuous and differentiable SDF
The robot is equipped with a range sensor, such as a
representations, as required by the CBF framework.
LiDAR scanner, and aims to follow a desired path, relying on
Quadratic programming with CBF constraints offers an
the noisy distance measurements to avoid collisions. A path
elegant and efficient framework for safe control synthesis
is a piece-wise continuous function r : [0, 1] 7→ Int(v(S)).
in robot navigation tasks [15], [16]. This approach employs
Let F(x) ⊂ Y be the field of view of the range sensor when
CLF constraints to stabilize the system and achieve the
the robot is in state x. At discrete times tk , for k ∈ N, the
control objectives whenever possible, whereas the CBF con-
range sensor provides a set of points Pk,i = {pk,i,j }j ⊂
straints are used to ensure safety of the resulting controller
F(x(tk )) ∩ ∂v(Oi ) on the boundary of each obstacle v(Oi )
at all times. CBFs are used for safe multi-robot navigation in
which are within its field-of-view.
[15]. CLF and CBF constraints are combined to solve a si-
multaneous lane-keeping (LK) and adaptive speed regulation Problem. Consider the system in (1) operating in an un-
K
(ASR) problem in [16]. CLFs are used to ensure convergence known environment, i.e., the obstacles sets {v(Oi )}i=1 are
to the control objectives for LK and ASR, whereas CBFs unknown a priori. Given a desired path r and noisy range

1125
sensor measurements Pk,i for k ∈ N and i = 1, . . . , K, The training set D0 for the Eikonal term can be generated
design a feedback controller u(x) for (1) that ensures that arbitrarily in Y since ϕ̃i needs to satisfy the Eikonal con-
the robot can move safely along the path r, i.e., y(t) ∈ v(S) straint almost everywhere in Y. In practice, we generate D0
for all t and y(t) → r(1) as t → ∞. by sampling points from a mixture of a uniform distribution
IV. O BSTACLE E STIMATION VIA O NLINE S IGNED and Gaussians centered at the points in the distance training
D ISTANCE F UNCTION A PPROXIMATION set D with standard deviation equal to the distance to the k-
th nearest neighbor (k = |D|/2). For the distance training set
Throughout the paper, we rely on the concept of signed D, ideally, we should use as much data as possible to obtain
distance function to describe each unsafe region {v(Oi )}K
i=1 . an accurate approximation of the SDF ϕi . For example,
For each i, the SDF function ϕi : Y 7→ R is: at time tk , all observed data may be used as the distance
(
−d(y, ∂v(Oi )), y ∈ v(Oi ), training set D = ∪kl=0 Dl,i . However, the online training
ϕi (y) := (2) time becomes longer and longer as the robot receives more
d(y, ∂v(Oi )), y ∈ / v(Oi ),
and more LiDAR measurements, which makes it impractical
where d denotes the Euclidean distance between a point and for real-time mapping and navigation tasks. We introduce
a set. In this section, we describe an approach to construct an “Incremental Training with Replay Memory” approach
approximations to the obstacle SDFs ϕi using the point cloud in Sec. IV-C, which obtains accurate SDF estimates with
observations {Pk,i }k at time step tk . training times suitable for online learning.
A. Data Pre-processing
C. Incremental Learning
Given the point cloud Pk,i ⊂ Rw on the surface of the ith
obstacle at time tk , we can regard Pk,i as the points on the When an obstacle i is first observed at time t0 , we use the
zero level-set of a distance function. To normalize the scale data set D0,i to train the SDF network ϕ̃i (y; θ 0 ). When new
of the training data, we define the point coordinates with re- observations Dk,i of obstacle i are obtained at k = 1, 2, . . . ,
spect to the obstacle centroid. Since the centroid is unknown, we need to update the network parameters θ k . We first
we approximate consider an approach that updates ϕ̃i (y; θ k−1 ) based on the
1
Pm it as the sample mean of the training points new data set Dk,i and uses the previous parameters θ k−1 as
p̄k,i := m j=1 pk,i,j . The points pk,i,j − p̄k,i are centered
around p̄k,i and have a measured distance of 0 to the obstacle initialization. We call this approach “Incremental Training”
surface v(∂Oi ). Let c = kv(xk ) − qk,i,j k be the Euclidean (IT). Alternatively, we can update ϕ̃i (y; θ k−1 ) using all prior
distance between the robot state xk := x(tk ) and pk,i,j . Let data Di = ∪kl=0 Dl,i , observed up to time tk , and θ k−1 as
δ > 0 be a small positive constant. Define a point qk,i,j := initialization. We call this approach “Batch Training” (BT).
δ δ The IT approach is efficient because, at each time tk , it
c v(xk )+(1− c )pk,i,j along the LiDAR ray from the output
v(xk ) to pk,i,j that is approximately a distance δ from the uses data sets Dk,i of approximately constant cardinality,
obstacle surface ∂v(Oi ). We call the set {qk,i,j − p̄k,i }j a leading to approximately constant update times during online
truncated SDF point set. The training set for each obstacle training. However, discarding the old data ∪k−1 l=0 Dl,i and
i is constructed as a union of the points on the boundary using stochastic gradient descent to re-train the network
and the truncated SDF points. The training set at time tk is parameters θ k−1 on the new data Dk,i causes degradation
Dk,i := {(pk,i,j − p̄k,i , 0)} ∪ {(qk,i,j − p̄k,i , δ)}. in the neural network’s ability to represent the old data.
In other words, the SDF approximation ϕ̃i (y; θ k ) at time
B. Loss Function tk is good at approximating the latest observed obstacle
Inspired by the recent impressive results on multilayer surface but the approximation quality degrades at previously
perceptron approximation of SDF [7], [14], we introduce a observed surfaces, as shown in Fig. 2. The BT approach
fully connected neural network ϕ̃i (y; θ k ) with parameters θ k does not have this limitation since it uses all data ∪kl=0 Dl,i
to approximate the SDF of each observed obstacle i at time for training at time tk but, as a result, its training time
step tk . Our approach relies on the fact that the norm of the increases over time. Hence, our motivation for introducing
SDF gradient satisfies the Eikonal equation k∇ϕi (y)k = 1 in an “Incremental Training with Replay Memory” (ITRM)
its domain. We use a loss function that encourages ϕ̃i (y; θ k ) approach is to balance the trade-off between SDF estimation
to approximate the measured distances in a training set D error and online training time, making it suitable for real-
(distance loss `D i ) and to satisfy the Eikonal constraint for set time robotic tasks.
of points D0 (Eikonal loss `E i ). For example, ϕ̃i equals 0 for Experience replay is an effective and commonly used
points on the obstacle surface and equals δ for the truncated technique for online reinforcement learning, which enables
SDF points along the LiDAR rays. The loss function is convergence of stochastic gradient descent to a critical point
defined as `i (θ k ; D, D0 ) := `D E 0
i (θ k ; D) + λ`i (θ k ; D ), with in policy and value function approximation [10], [18]. This
a parameter λ > 0 and: idea has not been explored for online supervised learning
1 X of geometric surfaces. The first contribution of this paper
`Di (θ k ; D) := |ϕ̃i (p; θ k ) − d|,
|D| is to use replay memory for online incremental learning of
(p,d)∈D
(3) SDF. At each time step tk−1 , We construct an experience
0 1 X
`Ei (θ k ; D ) := (k∇ ϕ̃ i (p; θ k )k − 1) . replay memory Qk−1,i at each time step by utilizing the
|D0 | 0 SDF approximations ϕ̃i (y; θ k−1 ).
p∈D

1126
 ϕ̃i (y; θ k ) are trained via the loss function in (3) but each
method uses a different training set D:
IT: D = Dk,i BT: D = ∪kl=0 Dl,i ITRM: D = Dk,i ∪Q̄k,i .
We use the continuously differentiable Softplus function
ln(1+ex ) as the non-linear activation to ensure that ϕ̃i (y; θ k )
is continuously differentiable. The gradient ∇ϕ̃i (y; θ k ) is an
(a) Training data at k = 0 (b) IT data at k = 70 (c) ITRM data at k = 70
input in the loss function in (3) and can be calculated via
backpropagation [14].
V. S AFE NAVIGATION WITH E STIMATED O BSTACLES
We rely on the estimated SDFs constructed in Sec. IV to
formalize the synthesis of a controller that guarantees safety
with respect to the exact obstacles, despite errors in the SDF
approximation. Our analysis assumes error bounds are avail-
(d) Estimation at k = 0 (e) IT estimation at (f) ITRM estimation at
k = 70 k = 70 able, and we leave their actual computation for future work.
In this regard, several recent works study the approximation
Fig. 2: Shape estimation with and without replay memory. The
top row shows the training data used at time step k = 0 and power and error bounds of neural networks [20], [21]. In
k = 70 by the IT and ITRM approaches. The purple points are the our evaluation in Sec. VI, we obtain SDF error bounds by
observed LiDAR end points, while the blue points are the truncated comparing the estimated and ground-truth object SDFs.
SDF points along the LiDAR rays. In (c), the green and red points
are boundary and truncated SDF points obtained from the replay A. Safe Control with Estimated Barrier Functions
memory. The bottom row shows the SDF estimation of the obstacle A useful tool to ensure that the robot state remains in the
surface at time step k = 0 and k = 70 for the IT and ITRM
approaches. The black rectangle shows the ground-truth obstacle safe set S throughout its evolution is the notion of control
boundary, while colored regions are level-sets of the SDF estimate. barrier function (CBF).
The white region denotes the estimated obstacle boundary. The blue
(resp. red) region denotes negative (resp. positive) signed distance. Definition 2 ([22]). A continuously differentiable function
IT and ITRM use the same data and lead to the same estimate at k = h : Rn 7→ R, with h(x) > 0 if x ∈ Int(S) and h(x) = 0
0 because the replay memory set is empty. In (e), the SDF estimate if x ∈ ∂S, is a zeroing control barrier function (ZCBF) on
of the top obstacle region at k = 70, without memory replay, X ⊂ Rn if there exists an extended class K function αh such
degrades compared to (d). In (f), training with replay memory helps that, for each x ∈ X , there exists u ∈ U with
the neural network remember the overall obstacle shape.
Lf h(x) + Lg h(x)u + αh (h(x)) ≥ 0, (4)
Definition 1. The replay memory Q associated with a signed
distance function ϕ and truncation parameter τ ≥ 0 is the where Lf h(x) is the Lie derivative of h(x) along f (x).
set of points that are at most a distance τ from the zero-level Any Lipschitz-continuous controller u : X 7→ U such
set of ϕ: Q := {(q, ϕ(q)) ∈ (Rw , R) | |ϕ(q)| ≤ τ }. that u(x) satisfies (4) renders the set S forward invariant
To construct the replay memory set Qk−1,i , we need for the control-affine system (1) [6], [22]. If the exact SDFs
to generate samples from the level sets of the SDF ap- ϕi describing the obstacles Oi were known, we could define
proximation ϕ̃i (y; θ k−1 ). In robotics applications, where the ZCBFs hi (x) := ϕi (v(x)) that ensure the forward invariance
environments are commonly 2-D or 3-D, samples from the of S. However, when the environment is observable through
level sets of ϕ̃i (y; θ k−1 ) can be obtained using the Marching online range measurements as described in Sec. IV, we only
Cubes algorithm [19]. In our experiments in Sec. VI, we used have the estimated SDFs ϕ̃i (v(x); θ k ) at our disposal to
Marching Cubes to extract samples q0 and qδ from the zero define h̃i (x) := ϕ̃i (v(x); θ k ). Our next result describes
and δ level-sets of ϕ̃i (y; θ k−1 ), respectively, and construct how to use this information to ensure the safety of S. The
the replay memory as Qk−1,i := {(q0 , 0)} ∪ {(qδ , δ)}. statement is for a generic h̃ (e.g., a single obstacle). We later
Given the replay memory Qk−1,i , our ITRM approach particularize our discussion to the case of multiple obstacles.
constructs a training set at time tk by combining the latest Proposition 1. Let e1 (x) := h(x) − h̃(x) ∈ R be the error
observation Dk,i with a randomly sampled subset Q̄k−1,i at x in the approximation of h, and likewise, let e2 (x) :=
from Qk−1,i . To make the algorithm efficient without losing ∇h(x)−∇h̃(x) ∈ Rn be the error at x in the approximation
significant information about prior data, we let |Q̄k−1,i | = of its gradient. Assume there are available known functions
|Dk,i |, i.e., we pick as many points from the replay memory eh (x) : R 7→ R≥0 and e∇h (x) : Rn 7→ R≥0 such that
as there are in the latest observation Dk,i .
The three training techniques, IT, BT, and ITRM, dis- |e1 (x)| ≤ eh (x), ke2 (x)k ≤ e∇h (x) (5)
cussed in this section, are formally defined as follows. In with eh (x) → 0 and e∇h (x) → 0 as x → ∂S. Let
all approaches, for obstacle i, the parameters θ k−1 obtained
at the previous time step are used as an initial guess for Kh̃ (x) := {u ∈ U | Lf h̃(x) + Lg h̃(x)u− (6)
the new parameters θ k at time tk . The SDF approximations kf (x) + g(x)uke∇h (x) + αh (h̃(x) − eh (x)) ≥ 0}.

1127
Then, any locally Lipschitz continuous controller u : X 7→ U where ũ(x) is a baseline controller, L(x) is a matrix
such that u(x) ∈ Kh̃ (x) guarantees that the safe set S is penalizing control effort, and δ ≥ 0 (with the corresponding
forward invariant. penalty λ) is a slack variable, introduced to relax the CLF
constraints in order to ensure the feasibility of the QP. The
Proof. We start by substituting h(x) = h̃(x) + e1 (x) in (4):
baseline controller ũ(x) is used to specify additional control
Lf h̃(x) + Lg h̃(x)u + e2 (x)> f (x) + e2 (x)> g(x)u requirements such as desirable velocity or orientation (see
Sec. VI) but may be set to ũ(x) ≡ 0 if minimum control
≥ −αh (h̃(x) + e1 (x)).
effort is the main objective. The QP formulation in (8)
For any fixed x and any errors e1 (x) and e2 (x) satisfying modifies ũ(x) online to ensure safety and stability via the
(5), we need the minimum value of the left-hand side greater CBF and CLF constraints.
than the maximum value of the right-hand side to ensure that Without exact knowledge of the barrier function h, we
(4) still holds, namely: need to replace the CLF constraint in (8) by (6):
n
min Lf h̃(x) + Lg h̃(x)u + e2 (x)> f (x)+ min kL(x)> (u − ũ(x))k2 + λδ 2
ke2 (x)k≤e∇h (x) u∈U ,δ∈R
o n o (7)
e2 (x)> g(x)u ≥ max − αh (h̃(x) + e1 (x)) . s.t. Lf V (x) + Lg V (x)u + αV (V (x)) ≤ δ
|e1 (x)|≤eh (x)
(9)
Lf h̃(x) + Lg h̃(x)u + αh (h̃(x) − eh (x))
Note that since eh (x) ≥ 0 and αh is an extended class K
≥ kf (x) + g(x)uk e∇h (x).
function, the maximum value in (7) is obtained by:
This makes the optimization problem in (9) no longer a
max −αh (h̃(x) + e1 ((x))) = −αh (h̃(x) − eh (x)).
|e1 (x)|≤eh (x) quadratic program. However, the following result shows
The minimum value is attained when e2 (x) is in the opposite that (9) is a (convex) second-order cone program (SOCP).
direction to the gradient of f (x) + g(x)u, namely Proposition 2. The optimization problem in (9) is equivalent
n o to the following second-order cone program:
min e2 (x)> f (x)+e2 (x)> g(x)u =
ke2 (x)k≤e∇h (x) min l
u∈U ,δ∈R,l∈R
− kf (x) + g(x)uke∇h (x).
s.t. Lf V (x) + Lg V (x)u + αV (V (x)) ≤ δ
Therefore, the inequality condition (7) can be rewritten
as Lf h̃(x) + Lg h̃(x)u − kf (x) + g(x)uke∇h (x) ≥ kf (x) + g(x)uk e∇h (x) ≤ Lf h̃(x) + Lg h̃(x)u
(10)
−αh (h̃(x) − eh (x)), which is equivalent to the condition + αh (h̃(x) − eh (x))
in (6). 
2L(x)> (u

√ − ũ(x))
Proposition 1 allows us to synthesize safe controllers even  2 λδ  ≤ l + 1.
though the obstacles are not exactly known, provided that l−1
error bounds on the approximation of the barrier function Proof. We first introduce a new variable l so that the problem
and its gradient are available and get better as the robot state in (9) is equivalent to
gets closer to the boundary of the safe set S.
min l
u∈U ,δ∈R,l∈R
B. Control Synthesis via Second-Order Cone Programming
s.t. Lf V (x) + Lg V (x)u + αV (V (x)) ≤ δ
We encode the control objective using the notion of a
Lyapunov function. Formally, we assume the existence of kf (x) + g(x)uk e∇h (x) ≤ Lf h̃(x) + Lg h̃(x)u (11)
a control Lyapunov function, as defined next. + αh (h̃(x) − eh (x))
Definition 3 ([6]). A control Lyapunov function (CLF) for kL(x)> (u − ũ(x))k2 + λδ 2 ≤ l.
the system dynamics in (1) is a continuously differentiable The last constraint in (11) corresponds to a rotated second-
function V : Rn 7→ R≥0 for which there exist a class K order cone, Qnrot := {(xr , yr , zr ) ∈ Rn+2 | kxr k2 ≤
function αV such that, for all x ∈ X : yr zr , yr ≥ 0, zr ≥ 0}, which can be converted into a
inf [Lf V (x) + Lg V (x)u + αV (V (x))] ≤ 0. standard SOC constraint [23]:
u∈U  
The function V may be used to encode a variety of control 2xr
≤ yr + zr .
objectives, including for instance path following. We present yr − zr
a specific Lyapunov function for this purpose in Sec.VI. Let yr = l, zr = 1 and consider the constraint kL(x)> (u −
When the barrier function is precisely known, one can ũ(x))k2 + λδ 2 ≤ l. Multiplying both sides by 4 and adding
combine CLF and CBF constraints to synthesize a safe l2 + 1, makes the constraint equivalent to
controller via the following QP:
4kL(x)> (u − ũ(x))k2 + 4λδ 2 + (l − 1)2 ≤ (l + 1)2 .
> 2 2
min kL(x) (u − ũ(x))k + λδ
u∈U ,δ∈R Taking a square root on both sides, we end up with
(8)
q √
s.t. Lf V (x) + Lg V (x)u + αV (V (x)) ≤ δ k2L(x)> (u − ũ(x))k2 + (2 λδ)2 + (l − 1)2 ≤ l + 1,
Lf h(x) + Lg h(x)u + αh (h(x)) ≥ 0, which is equivalent to the third constraint in (10).

1128
TABLE I: SDF estimation error (14) of the IT, ITRM, and BT
training methods for LiDAR measurements with zero-mean Gaus-
sian range noise with standard deviation σ = 0.01.
Case E in IT E in ITRM E in BT
1 (Ball) 0.0287 0.0148 0.0140
2 (Sofa) 0.0653 0.0212 0.0215 (a) Ball (b) Sofa (c) Table (d) Toy
3 (Table) 0.0564 0.0230 0.0227
4 (Toy) 0.1064 0.0242 0.0198
5 (Dog) 0.0328 0.0211 0.0123
6 (Duck) 0.0751 0.0102 0.0111
7 (Rabbit) 0.0603 0.0134 0.0109
8 (Cat) 0.0292 0.0149 0.0123
Average 0.0568 0.0179 0.0147
(e) Dog (f) Duck (g) Rabbit (h) Cat

Fig. 3: Object instances used to evaluate the estimation performance

For multiple obstacles in the environment, one can add of online signed distance function approximation.
multiple CBF constraints to (10). We leave for future work
the characterization of the Lipschitz continuity properties of
the controller resulting from (10).
VI. E VALUATION
We use the PyBullet simulator [24] to evaluate our ap-
proach for online shape estimation and safe navigation. We
use a TurtleBot, equipped with a LiDAR scanner with a
270 degree field of view, 150 rays per scan, 3 meter range,
and zero-mean Gaussian measurement noise with standard Fig. 4: Robot motion (left) and training time (right) for the SDF
deviation σ = 0.01. The simulation environments contain estimation experiment in Sec. VI-B. The robot starts at (2, 0) and
obstacles with various shapes, a priori unknown to the robot. follows a circular reference path (green) to a goal position at
(−2, 0) (red triangle). The actual trajectory followed by the robot
A. Modeling is shown in blue. The black dots are points on the surface of a
ground-truth Dog object and vary for different object instances.
We model the robot motion using unicycle kinematics and The average training times for the BT, IT, and ITRM methods for
take a small distance a 6= 0 off the wheel axis as in [25] to SDF approximation of the 8 objects in Fig. 3 are compared.
obtain a relative-degree-one model:
   
ẋ ψ cos(θ) − aω sin(θ)
ẏ  = ψ sin(θ) + aω cos(θ) , Gropp et al. [14] and Park et al. [7] for ITRM training. The
(12)
details of the neural network architecture are specified in
θ̇ ω
Sec. VI-C. We measure SDF approximation error as:
where ψ, ω represent the robot linear and angular veloc- m
1 X
ity, respectively. The state, input, and output are x := E= |ϕ̃(y i ; θ k )|, (14)
[x, y, θ]> ∈ R2 × [−π, π), u := [ψ, ω]> ∈ R2 , and m i=1
y = v(x) := [x, y] ∈ R2 . We use a baseline controller where {y i }m
i=1 are m = 500 points uniformly sampled on
ũ(x) ≡ [ψmax , 0]> to encourage the robot to drive at the surface of the ground-truth object. The SDF error is
max velocity ψmax in a straight line whenever possible. shown in Table I for the eight object instances in Fig. 3. The
The desired robot path is specified by a 2-D curve r(γ), error of our ITRM approach is comparable with the error
γ ∈ [0, 1]. To capture the path-following task via a CLF of the BT approach and is much smaller than the error of
constraint, we define η(x) := v(x) − r(γ(x)). The closest the IT approach. The training update time is the time needed
point on the reference path r(γ) to the robot state x is for updating the network parameters from θ k−1 to θ k . The
obtained by γ(x) := arctan(y/x)/(2π) ∈ [0, 1]. According average training update time across the 8 object instances for
to [26], the following is a valid CLF for path following: the three methods is shown in Fig. 4. We see that as the robot
V (x) = [η > (x), η̇ > (x)]P [η > (x), η̇ > (x)]> , (13) moves around the environment, the average training update
time of the ITRM approach remains at about 0.7s while the
where P is a positive-definite matrix calculated by solving BT approach requires more and more time for training.
the Lyapunov equation of the input-output linearization.
C. Network Architecture and Training Implementation
B. SDF Estimation Results To accelerate the training time of the ITRM approach
We compare the training time and prediction accuracy of further to support online navigation, we explore the trade-
the proposed ITRM approach for SDF estimation versus the off between training time and accuracy for different neural
IT and BT approaches described in Sec. IV-C for various 2- network configurations. The baseline neural network used in
D obstacle shapes. Figs. 3 and 4 show our experiment setup. Sec. VI-B has 8 fully connected layers with 512 neurons each
We used the multilayer perceptron architecture proposed by and a single skip connection from the input to the middle

1129
TABLE II: Training time and validation error of different neural
network configurations for SDF approximation. The number of
network layers, the number of training epochs, and whether a GPU
is used are varied. The results are obtained using the settings in
Table I for the dog object shown in Fig. 3. The SDF training time
includes both pre-processing of the LiDAR data and neural network
training. We report the average time per LiDAR scan along the robot
path. The SDF validation error is computed via (14).
Training Setting Average Training Time SDF Validation Error (a) Environment 1 (b) Environment 2
(number of layer,
GPU CPU GPU CPU
training epochs)
4, 5 0.0744 0.197 0.0212 0.0266
4, 10 0.143 0.388 0.0215 0.0207
4, 16 0.224 0.716 0.0207 0.0218
6, 5 0.0978 0.407 0.0132 0.0215
6, 10 0.193 0.731 0.0136 0.0193
6, 16 0.312 1.258 0.0149 0.0158
8, 5 0.178 0.544 0.0181 0.0208
8, 10 0.325 1.127 0.0201 0.0184
8, 16 0.497 1.841 0.0189 0.0156 (c) Environment 6 (d) Environment 7

Fig. 5: Simulation results for the CLF-CBF-SOCP and CLF-CBF-

QP controllers. The reference path is shown in blue. The ground-
layer. All internal layers use Softplus nonlinear activations. truth obstacle surfaces are shown in black. The estimated obstacles,
We set the parameter λ = 0.1 in (3). At each time step tk , obtained after the whole path is traversed by the CLF-CBF-SOCP
controller are shown in different colors (red, green, blue, orange).
we trained the network for 16 epochs with the ADAM The trajectory generated by CLF-CBF-SOCP is shown in green,
optimizer [27] with constant learning rate of 0.001. We also while the trajectory generated by CLF-CBF-QP is shown in pink.
experimented with neural network configurations with fewer The starting point is cyan and the goal region is a light green circle.
layers (4 or 6) and different number of training epochs (5, 10,
or 16), while keeping fixed the rest of the architecture. As for estimation errors, we compare to a CLF-CBF-QP that
real-time navigation necessitates obstacle shape estimation assumes the estimated CBFs h̃i (x) are accurate and ignores
to be as quick as possible, we aim to obtain the smallest the estimation errors.
network architecture, trained with as few epochs as possible, To avoid low velocities, we set√diagonal of L(x) in Sec. V-
that still maintains good SDF prediction accuracy. We report B as l1 = 10, l2 = 1, l3 = 10 10, where l1 ,l2 ,l3 are the
training time and accuracy results using a GPU (one Nvidia penalty parameters for linear velocity, angular velocity and
Geforce 2080 Super) or a CPU (Intel i7 9700K) in Table II. path following, respectively. If there
The best trade-off between training time and SDF error for a √ is no solution found at
some time step, l1 is divided by 2 until a feasible solution
2-D shape is obtained using a GPU to train a 6 layer network found. We use the Fréchet distance between the reference
for 5 epochs. This configuration enables training times of less path r and the paths produced by the CLF-CBF-SOCP and
than 0.1s, suitable for real-time navigation. CLF-CBF-QP controllers to evaluate their similarities. For
D. Safe Navigation Results paths A, B, the Fréchet distance is computed by:

The second set of experiments demonstrates safe trajectory F (A, B) = inf max {d(A(α(t)), B(β(t)))} , (15)
α,β t∈[0,1]
tracking using online SDF obstacle estimates to define con-
straints in the CLF-CBF-SOCP control synthesis optimiza- where α, β : [0, 1] 7→ [0, 1] are continuous, non-decreasing,
tion in (10). The robot moves along a reference path while surjections and d is the Euclidean distance in our case.
avoiding unknown obstacles in 8 different environments, In Fig. 5, the robot can follow the prescribed reference
similar to the one shown in Fig. 1. To account for the path if no obstacles are nearby. The green trajectory gener-
fact that the robot body is not a point mass, we subtract ated by CLF-CBF-SOCP is always more conservative than
the robot radius τ = 0.177 from the SDF estimate when the pink trajectory since it takes the errors in the CBF
defining the CBF: h̃i (x) = ϕ̃i (y; θ) − τ . The error bounds estimation into account. When there is an obstacle near or
of CBF eh (x) and its gradient e∇h (x) are approximated on the reference path, the robot controlled by the CLF-CBF-
based on Table I. We compare our CLF-CBF-SOCP approach SOCP controller stays further away from the obstacles than
to a CLF-CBF-QP. To account for estimation errors, it is the robot controlled by the CLF-CBF-QP controller. This
possible to inflate the CBFs in the CLF-CBF-QP formulation agrees with the Fréchet distance results presented in Table III.
by both the robot radius and the SDF approximation error, In Fig. 6, we see that the CLF-CBF-QP controller sometimes
h̃i (x) = ϕ̃i (y; θ)−τ −eh (x). This preserves linearity of the fails to avoid obstacles because it does not consider the errors
CBF constraints and leads to a more conservative controller. in the CBF estimation. In contrast, the CLF-CBF-SOCP
Comparing with (10), we can see that the CLF-CBF-SOCP controller is guaranteed by Prop. 2 to remain safe if the CBF
formulation accounts for both direct and gradient errors in estimation is captured correctly in the SOC constraints.
the CBF approximations, and naturally reduces to a QP if In summary, Table III indicates that the trajectory mis-
e∇h (x) is zero. To emphasize the importance of accounting match with respect to the reference path is larger under

1130
 [3] S. Prajna, “Barrier certificates for nonlinear model validation,” in
Conference on Decision and Control (CDC), pp. 2884–2889, 2003.
[4] S. Prajna and A. Jadbabaie, “Safety verification of hybrid systems us-
ing barrier certificates,” in Hybrid Systems: Computation and Control,
pp. 477–492, Springer Berlin Heidelberg, 2004.
[5] P. Wieland and F. Allgöwer, “Constructive safety using control barrier
functions,” in IFAC Proceedings Volumes, pp. 462–467, 2007.
[6] A. Ames, S. Coogan, M. Egerstedt, G. Notomista, K. Sreenath, and
P. Tabuada, “Control barrier functions: Theory and applications,” in
(a) Environment 3 (b) Zoom out of (a) European Control Conference (ECC), pp. 3420–3431, 2019.
[7] J. J. Park, P. Florence, J. Straub, R. Newcombe, and S. Lovegrove,
“DeepSDF: Learning Continuous Signed Distance Functions for Shape
Representation,” in IEEE Conference on Computer Vision and Pattern
Recognition (CVPR), 2019.
[8] C.-H. Lin, C. Wang, and S. Lucey, “SDF-SRN: Learning Signed
Distance 3D Object Reconstruction from Static Images,” ArXiv,
vol. abs/2010.10505, 2020.
[9] L. Han, F. Gao, B. Zhou, and S. Shen, “Fiesta: Fast incremental
euclidean distance fields for online motion planning of aerial robots,”
in IEEE/RSJ International Conference on Intelligent Robots and
(c) Environment 8 (d) Zoom out of (c) Systems (IROS), 2019.
Fig. 6: Simulation results in environments where the CLF-CBF- [10] T. Schaul, J. Quan, I. Antonoglou, and D. Silver, “Prioritized experi-
ence replay,” in Int. Conf. on Learning Representations (ICLR), 2016.
SOCP controller succeeds but CLF-CBF-QP one fails. The robot is
[11] A. Hornung, K. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard,
shown as a purple circle when crashing into an obstacle using the “Octomap: An efficient probabilistic 3D mapping framework based on
CLF-CBF-QP controller. Fig. 6b and Fig. 6d show enlargements of octrees,” Autonomous Robots, vol. 34, no. 3, pp. 189–206, 2013.
the crash regions in Fig. 6a and Fig. 6c, respectively. [12] H. Oleynikova, Z. Taylor, M. Fehr, R. Siegwart, and J. Nieto,
TABLE III: Fréchet distance between the reference path and the “Voxblox: Incremental 3d euclidean signed distance fields for on-board
robot trajectories generated by the CLF-CBF-SOCP and the CLF- mav planning,” in IEEE/RSJ International Conference on Intelligent
CBF-QP controllers (smaller values indicate larger trajectory simi- Robots and Systems (IROS), pp. 1366–1373, 2017.
[13] L. M. Mescheder, M. Oechsle, M. Niemeyer, S. Nowozin, and
larity, N/A indicates that the robot failed to reach the goal region).
A. Geiger, “Occupancy networks: Learning 3d reconstruction in func-
Environment CLF-CBF-SOCP CLF-CBF-QP tion space,” IEEE/CVF Conference on Computer Vision and Pattern
1 0.3428 0.3442 Recognition (CVPR), pp. 4455–4465, 2019.
2 0.5339 0.3971 [14] A. Gropp, L. Yariv, N. Haim, M. Atzmon, and Y. Lipman, “Implicit
3 0.8356 N/A geometric regularization for learning shapes,” in International Confer-
4 0.3968 0.3492 ence on Machine Learning, 2020.
5 1.0535 N/A [15] P. Glotfelter, J. Cortés, and M. Egerstedt, “Nonsmooth barrier func-
6 0.3611 0.3444 tions with applications to multi-robot systems,” IEEE Control Systems
7 0.4643 0.3493 Letters, vol. 1, no. 2, pp. 310–315, 2017.
8 0.8674 N/A [16] X. Xu, T. Waters, D. Pickem, P. Glotfelter, M. Egerstedt, P. Tabuada,
J. W. Grizzle, and A. D. Ames, “Realizing simultaneous lane keeping
and adaptive speed regulation on accessible mobile robot testbeds,”
in IEEE Conference on Control Technology and Applications (CCTA),
our CLF-CBF-SOCP controller than under the CLF-CBF-QP pp. 1769–1775, 2017.
controller if they both succeed. However, our approach guar- [17] M. Srinivasan, A. Dabholkar, S. Coogan, and P. Vela, “Synthesis
antees safe navigation, while the CLF-CBF-QP controller of control barrier functions using a supervised machine learning
approach,” arXiv preprint:2003.04950, 2020.
sometimes causes collisions due to errors in CBF estimation. [18] D. Rolnick, A. Ahuja, J. Schwarz, T. Lillicrap, and G. Wayne,
“Experience replay for continual learning,” in AAAI Conference on
VII. C ONCLUSION Artificial Intelligence, 2018.
We introduced an incremental training approach with [19] W. E. Lorensen and H. E. Cline, “Marching cubes: A high resolution
3d surface construction algorithm,” COMPUTER GRAPHICS, vol. 21,
replay memory to enable online estimation of signed dis- no. 4, pp. 163–169, 1987.
tance functions and construction of corresponding safety [20] B. Bailey, Z. Ji, M. Telgarsky, and R. Xian, “Approximation power of
constraints in the form of control barrier functions. The use random neural networks,” ArXiv, vol. abs/1906.07709, 2019.
[21] D. Yarotsky, “Error bounds for approximations with deep relu net-
of replay memory balances training time with estimation works,” Neural Networks, vol. 94, pp. 103–114, 2017.
error, making our approach suitable for real-time estimation. [22] A. Ames, X. Xu, J. Grizzle, and P. Tabuada, “Control barrier function
We showed that accounting for the direct and gradient errors based quadratic programs for safety critical systems,” IEEE Transac-
tions on Automatic Control, vol. 62, no. 8, pp. 3861–3876, 2016.
in the CBF approximations leads to a new CLF-CBF-SOCP [23] F. Alizadeh and D. Goldfarb, “Second-order cone programming,”
formulation for safe control synthesis. Future work will Mathematical programming, vol. 95, no. 1, pp. 3–51, 2003.
consider capturing localization and robot dynamics errors in [24] E. Coumans and Y. Bai, “PyBullet, a Python module for physics
simulation for games, robotics and machine learning.” http://
the safe control formulation and will apply our techniques pybullet.org, 2016.
to real autonomous navigation experiments. [25] J. Cortés and M. Egerstedt, “Coordinated control of multi-robot
systems: A survey,” SICE Journal of Control, Measurement, and
R EFERENCES System Integration, vol. 10, no. 6, pp. 495–503, 2017.
[26] A. D. Ames, K. Galloway, K. Sreenath, and J. W. Grizzle, “Rapidly
[1] O. Khatib, “Real-Time Obstacle Avoidance for Manipulators and exponentially stabilizing control Lyapunov functions and hybrid zero
Mobile Robots,” The International Journal of Robotics Research, dynamics,” IEEE Transactions on Automatic Control, vol. 59, no. 4,
vol. 5, no. 1, pp. 90–98, 1986. pp. 876–891, 2014.
[2] E. Rimon and D. E. Koditschek, “Exact robot navigation using [27] D. P. Kingma and J. Ba, “Adam: A method for stochastic optimiza-
artificial potential functions,” IEEE Transactions on Robotics and tion,” arXiv preprint: 1412.6980, 2015.
Automation, vol. 8, no. 5, pp. 501–518, 1992.

1131