# Safe Navigation using Density Functions

Andrew Zheng∗ and Sriram S.K.S Narayanan∗ and Umesh Vaidya *These authors
contributed equallyFinancial support from of NSF CPS award 1932458 and NSF
2031573 is greatly acknowledged. Andrew Zheng, Sriram S.K.S Narayanan, and
Umesh Vaidya are with the Department of Mechanical Engineering, Clemson
University, Clemson, SC 29630, USA email<EMAIL_ADDRESS><EMAIL_ADDRESS><EMAIL_ADDRESS>
###### Abstract

This paper presents a novel approach for safe control synthesis using the dual
formulation of the navigation problem. The main contribution of this paper is
in the analytical construction of density functions for almost everywhere
navigation with safety constraints. In contrast to the existing approaches,
where density functions are used for the analysis of navigation problems, we
use density functions for the synthesis of safe controllers. We provide
convergence proof using the proposed density functions for navigation with
safety. Further, we use these density functions to design feedback controllers
capable of navigating in cluttered environments and high-dimensional
configuration spaces. The proposed analytical construction of density
functions overcomes the problem associated with navigation functions, which
are known to exist but challenging to construct, and potential functions,
which suffer from local minima. Application of the developed framework is
demonstrated on simple integrator dynamics and fully actuated robotic systems.
Our project page with implementation is available at
https://github.com/clemson-dira/density_feedback_control

##  I INTRODUCTION

Safe navigation of mission-critical systems is of utmost importance in many
modern autonomous applications. Autonomous vehicles and industrial robots are
all critical applications in which there exists a need for navigation that
adheres to safety constraints. Over the past decades, the general approach to
the navigation problem has consisted of formulating compositions of the system
that complies with the safety certification of the original system. This
traditionally implies a hierarchical architecture that decomposes the
navigation problem into planning and control [1].

The planning problem involves defining a collision-free trajectory in the
feasible configuration space given an initial and final configuration. These
are typically implemented through sample-based planners such as rapidly-
exploring random tree search (RRT) and probabilistic roadmaps (PRM) [2, 3].
These sample-based methods are observed to be probabilistically complete
through iterative samples of locally safe and feasible paths. Asymptotically
optimal variations of these planners have been developed in [4], where the
convergence rate for optimality is improved in [5, 6].

Designing controllers to track these trajectories from the plan while
satisfying dynamic and safety constraints is not so simple. Traditional
methods, such as inverse dynamics, rely on the exact cancellation of the
nonlinearities to track a resulting linear system through closed-loop control
[7]. However, they do not guarantee safety in the presence of unsafe regions.
More recently, control barrier functions (CBFs) have been introduced to
provide safety certificates for the controller [8]. However, CBFs only provide
safety, so augmentation of CBFs with control Lyapunov functions (CLFs) is
needed to guarantee convergence and safety [9].

This framework of hierarchical navigation has seen great success in many
robotic applications [10]; however, a natural issue of hierarchical navigation
is the evaluation of safety certificates from the planning to the control
level, which increases in complexity for large-scale systems [11].

Figure 1: Navigation framework using density where (a) defines the navigation
problem, (b) shows the density for navigation, and (c) shows occupancy
measure, which physically denotes the duration of system trajectories
occupying the set.

A natural proposal is to jointly solve the navigation problem without the
hierarchical structure. Artificial potential field based methods have
attempted to solve the joint problem by the sum of attractive and repulsive
potentials [12]. However, the existence of local minima is a well-known issue
[13, 14]. In [14, 15], a class of analytical potential functions, known as
navigation functions (NFs), are introduced, which guarantees almost everywhere
(a.e.) convergence while adhering to safety constraints. This method relies on
a range of problem-specific tuning parameters to guarantee a.e. convergence.
Moreso, complex safety constraints arising from arbitrarily shaped obstacles
are limited by the possible mapping to a model sphere world. Recent works have
proposed using altered NF or conformal mapping to navigate complex unsafe sets
[16, 17, 18]; however, these methods are nontrivial and physically
unintuitive.

The navigation problem can alternatively be formulated in the dual space of
density. In [19], a navigation measure was introduced to provide a convex
formulation for synthesizing safe controllers. In the continuous-time setting,
the density function was used as a safety certificate for the analysis and
synthesis using the sum of squares optimization method [20]. Similarly,
density-based approaches are also used for the convergence analysis of
existing navigation algorithms [21, 22]. More recently, convex data-driven
approaches based on the linear transfer Perron-Frobenius and Koopman operators
are used for solving the optimal navigation problem with safety constraints
[23, 24]. In contrast to using the convex dual formulation for navigation, we
provide an analytical construction of density functions for navigation. In
particular, the analytical construction of navigation density can be viewed as
the dual construction of the classical NFs from [14]. However, unlike [14],
the construction is not restricted to navigation in the sphere world
environment.

The main contribution of this paper is in providing analytical construction of
density functions used for solving the safe navigation problem. The density
function has a physical interpretation, where the measure associated with the
density is a measure of occupancy of the system trajectories in any set of the
state space as shown in Figure 1. We exploit this occupancy-based physical
interpretation of the density function in the construction of the navigation
density functions. Unlike NFs, the density formulation can represent arbitrary
shapes of the obstacle sets. We prove that the proposed density function can
navigate almost all initial conditions from the initial set to the target set
while avoiding the obstacle set. We show navigation results for simple
integrator dynamics in complex environments as well as high-dimensional
configuration spaces. Similarly, navigation results for obstacle avoidance
involving robotics systems such as the two-link planar robotic arm manipulator
are presented.

The rest of the paper is organized as follows. Section II discuss the
preliminaries and the problem formulation. Section III discusses the
construction of density functions, and Section IV discusses the properties of
density functions for the navigation problem. This is followed by application
to robotic systems in section V and conclusive remarks about the results in
section VI.

##  II Notations and Problem Statement

Notations: The following notations will be used in this paper.
$\mathbb{R}^{n}$ denotes the $n$ dimensional Euclidean space,
${\mathbf{x}}\in\mathbb{R}^{n}$ denotes a vector of system states,
${\mathbf{u}}\in\mathbb{R}^{n}$ is a vector of control inputs. Let
${\mathbf{X}}\subset{\mathbb{R}}^{n}$ be a bounded subset that denotes the
workspace for the robot.
${\mathbf{X}}_{0},{\mathbf{X}}_{T},{\mathbf{X}}_{u_{k}}\subset{\mathbf{X}}$,
for $k=1,\ldots,L$ denote the initial, target, and unsafe sets, respectively.
With no loss of generality, we will assume that the target set is a single
point set and located at the origin, i.e., ${\mathbf{X}}_{T}=\\{0\\}$.
${\mathbf{X}}_{u}=\cup_{k=1}^{L}{\mathbf{X}}_{u_{k}}$ defines the unsafe set
and ${\mathbf{X}}_{s}:={\mathbf{X}}\setminus{\mathbf{X}}_{u}$ defines the safe
set. We will denote by ${\mathbf{X}}_{1}:={\mathbf{X}}\setminus{\cal
B}_{\delta}$, where ${\cal B}_{\delta}$ is the $\delta$ neighborhood of the
origin for arbitrary small $\delta$. We use $\mathcal{C}^{k}({\mathbf{X}})$ to
denote the space of all $k$-times differentiable functions of ${\mathbf{x}}$.
We use ${\cal M}({\mathbf{X}})$ to denote the space of all measures on
${\mathbf{X}}$ and $m(\cdot)$ to denote the Lebesgue measure.
$\mathds{1}_{A}({\mathbf{x}})$ denotes the indicator function for set
$A\subset{\mathbf{X}}$.

The formal statement of the navigation problem that we solve in this paper is
stated as follows.

###### Problem 1

(Almost everywhere navigation problem) The objective of this problem is to
design a smooth feedback control input
${\mathbf{u}}={\mathbf{k}}({\mathbf{x}})$ to drive the trajectories of the
dynamical system

$\displaystyle\dot{\mathbf{x}}={\mathbf{u}},\;\;\;\;\;$ (1)

from almost every initial condition (w.r.t. Lebesgue measure) from the initial
set ${\mathbf{X}}_{0}$ to the target set ${\mathbf{X}}_{T}$ while avoiding the
unsafe set ${\mathbf{X}}_{u}$.

###### Assumption 1

We assume that there exists a feedback controller that solves the a.e.
navigation problem as stated above.

##  III Construction of Density Function

The a.e. navigation problem, as stated in Problem 1, is solved using the
navigation density function. The construction of the navigation density is
inspired by the work of [19, 25, 26]. The navigation measure, as introduced in
[19], has a physical interpretation of occupancy, where the measure of any set
is equal to the occupancy of the system trajectories in the set, as shown in
Figure 1. Hence, zero occupancy in a set implies system trajectories not
occupying that particular set. So by ensuring that the navigation measure is
zero on the obstacle set and maximum on the target set, it is possible to
induce dynamics whereby the system trajectories will reach the desired target
set while avoiding the obstacle set. We exploit this occupancy-based
interpretation in the construction of analytical density functions.

We start with the construction of the unsafe set, where the boundary of the
unsafe set is described in terms of the zero-level set of a function. Let
$h_{k}({\mathbf{x}})$ be a continuous scalar-valued function for
$k=1,\ldots,L$ such that the set
$\\{{\mathbf{x}}\in{\mathbf{X}}:h_{k}({\mathbf{x}})\leq 0\\}$, is connected
with only one component. Thus, the unsafe set ${\mathbf{X}}_{u_{k}}$ is
defined using the function $h_{k}({\mathbf{x}})$ as follows

$\displaystyle{\mathbf{X}}_{u_{k}}:=\\{{\mathbf{x}}\in{\mathbf{X}}:h_{k}({\mathbf{x}})\leq
0\\}.$ (2)

Next, we define a transition region ${\mathbf{X}}_{s_{k}}$, which encloses the
unsafe set ${\mathbf{X}}_{u_{k}}$. Let $s_{k}({\mathbf{x}})$ be a continuous
scalar-valued function for $k=1,\ldots,L$ such that the set
$\\{{\mathbf{x}}\in{\mathbf{X}}:s_{k}({\mathbf{x}})=0\\}$ defines the boundary
of this transition region. Then the transition region can be defined by the
following set

$\displaystyle{\mathbf{X}}_{s_{k}}:=\\{{\mathbf{x}}\in{\mathbf{X}}:s_{k}({\mathbf{x}})\leq
0\\}\setminus{\mathbf{X}}_{u_{k}}.$ (3)

The proposed navigation density function is assumed to be of the form

$\displaystyle\rho({\mathbf{x}})=\frac{\prod_{k=1}^{L}\Psi_{k}({\mathbf{x}})}{V({\mathbf{x}})^{\alpha}}.$
(4)

Here, the function $V({\mathbf{x}})$ is the distance function that measures
the distance from state ${\mathbf{x}}$ to the target set, (i.e., the origin),
and $\alpha$ is a positive scalar. In this paper, we assume $V({\mathbf{x}})$
to be of the form $V({\mathbf{x}})=\|{\mathbf{x}}\|^{2}$. Additionally,
$\Psi_{k}({\mathbf{x}})$ is a smooth $\mathcal{C}^{\infty}$ function that
captures the geometry of the unsafe set ${\mathbf{X}}_{u_{k}}$ and can be
constructed using the following sequence of functions. We first define an
elementary $\mathcal{C}^{\infty}$ function $f$ as follows

$\displaystyle f(\tau)=\begin{cases}\exp{(\frac{-1}{\tau})},&\tau>0\\\
0,&\tau\leq 0\end{cases},$ (5)

where $\tau\in\mathbb{R}$ [27]. Next, we construct a smooth version of a step
function $\bar{f}$ from $f$ as follows

$\bar{f}(\tau)=\frac{f(\tau)}{f(\tau)+f(1-\tau)}.$ (6)

Here, $\bar{f}$ serves as the elementary function for representing zero and
nonzero occupation through density. Furthermore, the form of the elementary
function, $\bar{f}$, is chosen to ensure that the gradient of the density
function is well-defined. To incorporate more general geometric information
about the environment, we define a change of variables such that
$\phi_{k}({\mathbf{x}})=\bar{f}\Bigl{(}\frac{h_{k}({\mathbf{x}})}{h_{k}({\mathbf{x}})-s_{k}({\mathbf{x}})}\Bigr{)}$.
The resulting function $\Phi_{k}({\mathbf{x}})$ take the following form,

$\displaystyle\Phi_{k}({\mathbf{x}})=\begin{cases}0,&{\mathbf{x}}\in{\mathbf{X}}_{u_{k}}\\\
\phi_{k}({\mathbf{x}}),&{\mathbf{x}}\in{\mathbf{X}}_{s_{k}}\\\
1,&\rm{otherwise}.\end{cases}$ (7)

Finally, the function $\Psi_{k}({\mathbf{x}})$ is defined as

$\displaystyle\Psi_{k}({\mathbf{x}})=\Phi_{k}({\mathbf{x}})+\theta,$ (8)

where $\theta>0$ is some positive parameter. The parameters $\theta$ and
$\alpha$ are introduced in the construction of the navigation density. The
physical significance of these parameters and the assumption made on these
parameters and functions are stated in the following remark.

###### Remark 1

  * •

The distance function $V({\mathbf{x}})$ can be modified to adapt to the
geometry of the underlying configuration space. For a Euclidean space with
${\mathbf{x}}\in\mathbb{R}^{n}$, we pick
$V({\mathbf{x}})=\|{\mathbf{x}}\|^{2}$.

  * •

The parameter $\alpha$ is used to control the sharpness of the distance
function and is used in the proof of the main convergence results.

  * •

The function $\Psi_{k}({\mathbf{x}})$ is a $\theta$ shifted version of inverse
bump function $\Phi_{k}({\mathbf{x}})$ and hence strictly positive i.e.,
$\Psi_{k}({\mathbf{x}})\geq\theta>0$ for $k=1,\ldots,L$.

  * •

$\Psi_{k}({\mathbf{x}})$ makes a smooth transition from $\theta$ to $1+\theta$
in the transition region ${\mathbf{X}}_{s_{k}}$.

  * •

The transition region, ${\mathbf{X}}_{s_{k}}$, acts as a sensing region for
system trajectories where they start to react to the unsafe set. We refer to
the transition region as the sensing region for the rest of this paper.

  * •

$h_{k}({\mathbf{x}})=0$ defines the boundary of the unsafe set and
$s_{k}({\mathbf{x}})=0$ defines the boundary of the sensing region. Refer to
Figure 2 for an illustrative example. In the simplest case, the function
$s_{k}({\mathbf{x}})$ can be chosen to be synonymous to $h_{k}({\mathbf{x}})$,
such that $h_{k}({\mathbf{x}})-s_{k}({\mathbf{x}})=\sigma$ (where $\sigma>0$
is a constant) uniformly scales the unsafe set to form a sensing region.

Figure 2: $\Psi(x)$ for (a) ${\mathbf{X}}_{u}$ as a circle
($h({\mathbf{x}})=||{\mathbf{x}}||^{2}-r_{1}^{2}\leq 0$) and transition region
boundary as an ellipse
($s({\mathbf{x}})=||\mathbf{a}{\mathbf{x}}||^{2}-r_{2}^{2}=0$ where
$r_{2}>r_{1}$ and $\mathbf{a}$ is a scaling vector), (b) ${\mathbf{X}}_{u}$ as
a rounded square ($h({\mathbf{x}})=||{\mathbf{x}}||^{4}-r_{1}^{4}\leq 0$) and
a transition region
$(s({\mathbf{x}})=a^{2}x_{1}^{2}+b^{2}x_{2}^{2}c^{x_{1}}-r_{2}^{2}$ where
$r_{2}>r_{1}$; $a,b,c$ are parameters) defined using equation (4), (c-d) 3D
view of (a) and (b) respectively.

We assume explicit bounds on the functions $\Psi_{k}$, $V$, and their
derivatives which follow from the construction of the density function in
equation (4). It is important to emphasize that it is not necessary to
estimate these bounds, but the existence of these bounds is used as part of
the proof of the main results of this paper.

###### Assumption 2

  1.   2. 1.

We assume that the distance between the initial set, the target set, and the
unsafe sets are all bounded away from zero by some positive constant, say
$\zeta$.

  3. 2.

For ${\mathbf{x}}\in{\mathbf{X}}_{u_{k}}$, let

$\displaystyle
V_{min}^{k}=\min_{{\mathbf{x}}\in{\mathbf{X}}_{u_{k}}}V({\mathbf{x}})>0.\;\;\;$
(9)

Since the distance between the unsafe set and the target set is bounded away
from zero, the above quantity is well-defined and greater than zero.

  4. 3.

Furthermore, $m({\mathbf{X}}_{u_{k}})$, i.e., the Lebesgue measure of the
unsafe set, is assumed to be finite, with $\theta$ satisfying the following
inequality for any given $\epsilon>0$,

$\displaystyle\theta\leq\frac{V_{min}^{k}}{m({\mathbf{X}}_{u_{k}})}\epsilon,\;\;\;\;k=1,\ldots,L.$
(10) $\underline{c}_{V}\leq
V({\mathbf{x}})\leq\bar{c}_{V},\;\;\;\underline{c}_{V_{x}}\leq\left|\frac{\partial
V}{\partial
x_{j}}\right|\leq\bar{c}_{V_{x}},\;\;\;{\color[rgb]{0,0,0}\left|\frac{\partial^{2}V}{\partial
x_{j}^{2}}\right|\leq\bar{c}_{V_{x^{2}}}}$ $\left|\frac{\partial\Psi}{\partial
x_{j}}\right|\leq\bar{c}_{\Psi_{x}},\;\;\;\;\left|\frac{\partial^{2}\Psi}{\partial
x_{j}^{2}}\right|\leq\bar{c}_{\Psi_{x}^{2}},\;\;\;\;{\color[rgb]{0,0,0}j=1,\ldots,n}.$

Further, by construction, both the first and second derivatives of $\Psi$
w.r.t. $x_{j}$ are zero outside the transition region

  5. 4.

Outside the transition region and in ${\mathbf{X}}_{1}$, we assume

$\frac{\partial^{2}V}{\partial
x_{j}^{2}}\leq\bar{d}_{V_{x}^{2}},\;\;V\leq\bar{d}_{V}\|{\mathbf{x}}\|^{2},\;\;\left|\frac{\partial
V}{\partial
x_{j}}\right|\geq\underline{d}_{V_{x}}\|{\mathbf{x}}\|\;j=1,\ldots,n.$

We have used lower bar, $\underline{c},\;\underline{d}$, and upper bar,
$\bar{c},\;\bar{d}$, notations to help define the lower and upper positive
bounds on functions. The subscripts for $c$ and $d$ signify the corresponding
functions.

##  IV Almost Everywhere Navigation Using Density Functions

Given the construction of $\rho(\mathbf{x})$ in (4), we design a controller
for navigation as the positive gradient of the density function
$\rho({\mathbf{x}})$, i.e.,

$\displaystyle\dot{\mathbf{x}}={\mathbf{k}}({\mathbf{x}})=\nabla\rho(\mathbf{x})$
$\displaystyle=\Biggl{(}-\frac{\alpha}{V^{\alpha+1}}\frac{\partial
V}{\partial{\mathbf{x}}}\prod_{k=1}^{L}\Psi_{k}({\mathbf{x}})+\frac{1}{V^{\alpha}}\frac{\partial}{\partial{\mathbf{x}}}\prod_{k=1}^{L}\Psi_{k}({\mathbf{x}})\Biggr{)}^{\top}.$
(11)

###### Remark 2

We make following modification to (11) to ensure that the vector field is
well-defined and the origin is locally asymptotically stable in ${\cal
B}_{\delta}$.
$\dot{\mathbf{x}}=\left[1-\bar{f}(\tau)\right]\nabla\rho({\mathbf{x}})-\bar{f}(\tau){\mathbf{x}}$
where, $\bar{f}$ is as defined in (6). With this modification, we will
continue to work with (11) with the assumption that the origin is locally
asymptotically stable in ${\cal B}_{\delta}$ for (11).

The main result of the paper is given in the following theorem.

###### Theorem 1

Under Assumptions 1 and 2, the dynamical system (11) will solve the a.e.
navigation problem as stated in Problem 1.

Proof of this main theorem is differed to the Appendix. The feedback
controller design for the a.e. navigation problem is illustrated in pseudo-
code in Algorithm 1.

Algorithm 1 Density-based Navigation Algorithm

$\mathbf{X_{0},X_{u},X_{T}}$

for $X_{u_{k}}$ in $\mathbf{X_{u}}$ do

$h_{k}(\mathbf{x})$ and $s_{k}(\mathbf{x})$ (see Remark 1 and 3)

end for

The rest of the section showcases the navigation results using the controller
designed from the analytical density function. We first show the
characteristics of the proposed controller, which validates the a.e.
navigation properties. Then, we extend our feedback controller to a more
complex environment. Lastly, a comparison of our algorithm to NFs is
presented.

###  IV-A Characteristics of Density Functions

In this example, we demonstrate the a.e. navigation properties of the proposed
controller. The navigation problem is defined with the target set at
${\mathbf{X}}_{T}=(4,-3)$ and the unsafe set ${\mathbf{X}}_{u}$, which is
constructed using a circular inverse bump function with
$h({\mathbf{x}})=||{\mathbf{x}}||^{2}-r_{1}^{2}$ and
$s({\mathbf{x}})=||{\mathbf{x}}||^{2}-r_{2}^{2}$ with $r_{1}=2$ and $r_{2}=3$.
Hence, ${\mathbf{X}}_{s_{k}}$ for the inverse bump function is defined on the
domain $2<\|{\mathbf{x}}\|<3$.

Figure 3a illustrates the a.e convergence of the proposed controller with
initial conditions set defined by a line at the top left of the environment
boundary. The blue contour lines represent the level sets of the density
function. For this example, all the initial conditions starting on the set
$\\{{\mathbf{X}}_{0}\subset{\mathbf{X}}:m({\mathbf{X}}_{0})=0\\}$, which is
polar opposite of the target set, cannot converge. This set of initial
conditions constitutes a measure zero set. Furthermore, these initial
conditions are attracted to a saddle point, implying the existence of local
maxima (shown in Figure 3b). Note that the existence of a saddle point will
imply the existence of local maxima. Any other trajectory starting from an
initial condition perturbed from the zero-measure set converges to the target
set ${\mathbf{X}}_{T}$ while avoiding the obstacle set ${\mathbf{X}}_{u}$.
Furthermore, we look at the characteristics of initial conditions starting
outside the sensing region, defined as a state ${\mathbf{x}}$ such that
$s({\mathbf{x}})\geq 0$ (trajectory A), and within the sensing region, defined
as a state ${\mathbf{x}}$ such that $0<h({\mathbf{x}})<s({\mathbf{x}})$
(trajectory B), shown in Figure 3c. The gradients of the density function
$\rho({\mathbf{x}})$ are such that trajectory A starts to react as it enters
the sensing region while trajectory B is repelled outward towards the boundary
of the sensing region before converging to the target set (see Figure 3d).

Figure 3: (a) Trajectories converge to the target set (green) while avoiding
the unsafe set (gray) with a.e. convergence, (b) Initial conditions along the
zero-measure set (black) converge to a saddle point (purple), (c) Trajectories
starting at A ($s({\mathbf{x}})>0$) and B (in ${\mathbf{X}}_{s_{k}}$) converge
to the target set, (d) Trajectories starting from A and B follow the same path
near the boundary of $s({\mathbf{x}})$.

###  IV-B Complex environment

One of the main features of our proposed navigation density is that it can
incorporate complex shapes of the obstacle set, which is captured in terms of
the unsafe set by some appropriate function $h_{k}({\mathbf{x}})$. The unsafe
set ${\mathbf{X}}_{u}\in\mathbb{R}^{2}$ in Figure 4a is constructed using an
implicit function that geometrically represents a circle, an ellipse, an oval,
and a bowtie. We show that the initial conditions starting along the boundary
converge to the goal at the center while safely avoiding obstacles. The
proposed controller can also satisfy a.e navigation in complex maze-like
environments. Figure 4b shows a trajectory finding a tight feasible region
between two obstacles while navigating to the target set. Furthermore, this
can be easily extended to navigation problems in higher dimensions. Figure 4c
shows all trajectories starting from a plane converging to the target set
while avoiding obstacles represented as 3D spheres. Figure 4d shows navigation
with unsafe sets composed of two tori, an unbounded cylinder, and a sphere. We
note that unlike [17, 16], the construction of the density function naturally
admits any complex shapes.

Figure 4: (a) Trajectories converge to the target set (green) while avoiding
arbitrary obstacles (gray), (b) Trajectory finding a narrow feasible region
around obstacles, (c) Navigation in a spherical grid, (d) Navigation through
two tori, unbounded cylinder, and sphere.

###  IV-C Comparison to Navigation Functions

In this section, we compare the a.e. convergence property of artificial
potential field NF to the proposed density functions in a complex environment
as shown in Figure 5. More specifically, we compare the tuning of
$s({\mathbf{x}})$ for a.e. convergence in the density function formulation
shown in equation (4) to the tuning of $\kappa\in\mathbb{R}$ for a.e.
convergence in NFs proposed in [14, Ch. 3, p. 36],

$\psi_{k}({\mathbf{x}})=\frac{||{\mathbf{x}}-{\mathbf{x}}_{g}||^{2}}{||{\mathbf{x}}-{\mathbf{x}}_{g}||^{2}+\beta({{\mathbf{x}}})^{1/\kappa}},$
(12)

where ${\mathbf{x}}_{g}$ is the desired goal location, $\beta({{\mathbf{x}}})$
is an obstacle function and $\kappa$ is a tuning parameter.

Although a domain is not necessary in the density formulation, NFs do require
a radially bounded sphere world. Hence, we define an appropriate bounded
sphere world of radius 25. The authors note that NFs do not make any claims
about tuning $\kappa$ for a.e. convergence other than the sphere world and its
extensions [14, 17], but for the sake of comparison, we look at an environment
with a C-shaped unsafe set. We then look at initial conditions that lie inside
the C-shaped unsafe set with the target set defined outside the cavity of the
unsafe set.

Figure 5: Comparison of density functions and NFs for random initial
conditions. The sensing region for the density function is defined by
$s({\mathbf{x}})=a^{2}x_{1}^{2}+b^{2}x_{2}^{2}c^{x_{1}}-r^{2}$ ($r$, $a,b,c$
are parameters). For (a) $r=2.5$, trajectories don’t converge, while setting
(c) $r=4.5$ leads to all trajectories converging. NFs with their corresponding
tuning parameter for convergence (b) $\kappa=1$ and (d) $\kappa=10$ lead to
trajectories not converging.

Figures 5a and 5b show that the trajectories do not converge to the goal for
all random initial conditions for small values in tuning parameter in either
the density function formulation or the artificial potential field NF
formulation. This is expected in NF as only large $\kappa$ in a sphere world
guarantees a.e. convergence. Likewise, the density formulation sees the same
results. However, tuning $s({\mathbf{x}})$ such that the density function
formulation has a.e. convergence property is intuitive, as stated below in
Remark 3. This is shown in Figure 5c, where tuning $s({\mathbf{x}})$ to be
larger than the C-shaped unsafe sets results in all system trajectories
converging to the target set. Note, no explicit mapping to a simplistic unsafe
set (e.g., circle) is required, where the same cannot be stated for NFs (even
with high $\kappa$), which does not give a.e. convergence results for complex
unsafe sets. This can be seen in Figure 5d, where some trajectories exit the
unsafe set and converge to the goal (by taking a large curvature path) while
others get trapped inside the cavity of the unsafe set.

###### Remark 3

The tuning parameter in the design of the navigation density functions are
$\alpha$, and $s_{k}({\mathbf{x}})$. The tuning of $\alpha$ depends on the
rate of convergence of the trajectories. Although a large value of $\alpha$ is
required for a.e. navigation (as shown in Appendix), in practice, even small
values of $\alpha$ (between 1 to 10) have shown to work. The tuning of
$s_{k}({\mathbf{x}})$ is physically intuitive, as it signifies the sensing
region. Hence, a sensing region that encompasses the unsafe set with a
sufficiently curved convex set has worked in the simulations.

##  V Application to Robotic Systems

We consider cases which are highly important in application, with specifics to
robotic systems. These cases, constrained control, stochastic settings, and
fully actuated multi-body systems are considered in the subsequent sections.

###  V-A Constrained Control w/ Density Function

The case of controlling the magnitude of the controller defined in (11) is
highly crucial in practical systems due to actuation limits. Although, the
magnitude of the controller can be implicitly controlled through the tuning of
$\alpha$, as change in $\alpha$ changes the sharpness of $V({\mathbf{x}})$,
hence change in gradient of density (i.e. change in magnitude of control), we
consider explicitly defining control constraints. In particular, we consider a
system with constraints in the following form

$\dot{\mathbf{x}}={\mathbf{u}}=\nabla\rho({\mathbf{x}}),\quad{\mathbf{u}}\in[-u_{max},u_{max}],$
(13)

where $u_{max}$ is the bound on control. Without formality, we constrain the
control when $||{\mathbf{u}}||_{\infty}>u_{max}$ by normalizing the control

$\bar{\mathbf{u}}=\frac{{\mathbf{u}}}{||{\mathbf{u}}||_{\infty}}u_{max},$ (14)

where $\bar{\mathbf{u}}$ is the constrained control.

Figure 6: Constrained control w/ navigation density

###  V-B Performance of Density Function w/ Noise

We consider the performance of our controller in a stochastic setting where
noise is entered through the control input

$\dot{\mathbf{x}}={\mathbf{u}}+{\mathbf{w}}\quad{\mathbf{u}}\in[-u_{max},u_{max}],$
(15)

where ${\mathbf{w}}\in\mathcal{N}(\mu,\Sigma)$ is the gaussian white noise
with mean $\mu=0$ and covariance $\Sigma$. Figure 7 showcases the navigation
problem with control noise for varying levels of covariance.

Figure 7: Density function w/ gaussian white noise of $\mu=0$ and (a)
$\Sigma=10^{-3}\times I_{2}$ (b) $\Sigma=5\times 10^{-3}\times I_{2}$

We see that the feedback controller is capable of invariance while converging
towards the goal with noise. Although the invariance of our control law is not
guaranteed, we see that up to a certain bound on the noise, the control
performance is robust.

###  V-C Fully Actuated Robotic System

We also extend the density function presented in Section III to a general
class of fully actuated robotic systems. For a robot with $n$ joints and $n$
rigid links, the system’s dynamics can be expressed using the Euler-Lagrange
equations. Consider an unconstrained system where $\bf{M}(\bf{q})$ is the
inertia matrix and $\bf{H}(\bf{q},\dot{\bf{q}})$ represents the Coriolis and
gravity effects on the system, $\bf{q}$
$\in{\mathbb{S}}^{1}\times{\mathbb{S}}^{1}$. Then the corresponding system is
represented as follows

$\bf{M}(\bf{q})\ddot{\bf{q}}+\bf{H}(\bf{q},\dot{\bf{q}})=\bf{u}.$ (16)

We then take a similar approach outlined in [15] in which there exists an
equivalent ”planning” system defined by $\dot{\bf{q}}=\nabla\rho(\mathbf{q})$
and a control law given by
${\mathbf{u}}=\nabla\rho(\mathbf{q})+d(\bf{q},\dot{\bf{q}})$
($d(\bf{q},\dot{\bf{q}})$ is a dissipative term and
$\dot{\bf{q}}^{\top}d(\bf{q},\dot{\bf{q}})<0$), where the system defined in
(16) tracks the planning system asymptotically [15]. For a general robotic
system such as the system defined in equation (16), $d(\bf{q},\dot{\bf{q}})$
can be selected such that it cancels out the nonlinearities of the system
similar to the inverse dynamics approach. Therefore, we define a density-based
inverse dynamics controller given by

$\bf{u}_{\rho}=\bf{M}(\bf{q})\ddot{\bf{q}}_{d}+\bf{H}(\bf{q},\dot{\bf{q}})+\bf{M}(\bf{q})\Biggl{(}\bf{K}_{p}\nabla\rho(\mathbf{e})-\bf{K}_{v}\bf{\dot{e}}\Biggr{)},$
(17)

where ${\mathbf{e}}:=\bf{q}-\bf{q}_{d}$,
$\dot{{\mathbf{e}}}:=\bf{\dot{q}}-\bf{\dot{q}}_{d}$, $\bf{q}_{d}$ is the
desired reference trajectory to follow, and $\bf{K}_{p}$ and $\bf{K}_{v}$ are
positive definite gain matrices.

Figure 8: (a) Robot (red) converges to the goal ($\pi,0$) starting from
equilibrium (0,0) while avoiding obstacles (gray). (b) state trajectories of
the robot and (c) control inputs for executing the swing-up maneuver.

Figure 8a shows a fully actuated two-link planar robotic arm executing a
swing-up maneuver with $\bf{K}_{p}$ = ${\rm diag}([1,1]),\;\;\;$ $\bf{K}_{v}$
= ${\rm diag}([10,10])$ and $V({\bf q})=(1-\cos(q_{1})(1-\cos(q_{2}))$. The
mass and length of each link are set to unity. The task space obstacles
(circular with a radius of 0.2) are mapped to joint space and approximated
using inverse bump functions. The reference trajectories are obtained in joint
space based on the planning system $\dot{\bf{q}}=\nabla\rho(\mathbf{q})$. The
corresponding state and control trajectories are shown in Figures 8b and 8c,
respectively. It is seen that the density-based inverse dynamics controller
drives the two-link manipulator to the upright position while avoiding the
obstacle set.

##  VI Conclusions

This work provides an analytical construction for the navigation density.
Moreso, we prove that the navigation density solves the almost everywhere
navigation problem. The proposed navigation density can be viewed as dual to
the popular navigation function and is derived based on the occupancy-based
interpretation of the density function. The navigation density has a few
advantages compared to navigation functions. Unlike navigation functions,
which are hard to construct, navigation density can be easily constructed.
Furthermore, the density function formulation can incorporate arbitrary shapes
of the unsafe set. We provide simulation results for navigation using density
function in complex and high dimensional environments as demonstrated. Lastly,
we also demonstrate the application of the density function for control on a
robotic system with safety constraints.

##  VII Appendix

The proof of Theorem 1 relies on the following Lemma.

###### Lemma 1

Consider the navigation density function as given in equation (4), then under
Assumption 2, we have

$\displaystyle\nabla\cdot({\mathbf{k}}({\mathbf{x}})\rho({\mathbf{x}}))\geq
0,\;\;\;a.e.\;\;{\mathbf{x}}\in{\mathbf{X}},$ (18)
$\displaystyle\nabla\cdot({\mathbf{k}}({\mathbf{x}})\rho({\mathbf{x}}))\geq\xi>0\;\;{\rm
for}\;\;{\mathbf{x}}\in{\mathbf{X}}_{0},$ (19)

where ${\mathbf{k}}({\mathbf{x}})=\nabla\rho({\mathbf{x}})$ is the feedback
control input as given in equation (11).

Proof:  We have

$\displaystyle\nabla\cdot({\mathbf{k}}({\mathbf{x}})\rho({\mathbf{x}}))=\rho({\mathbf{x}})\nabla\cdot{\mathbf{k}}({\mathbf{x}})+\frac{\partial\rho}{\partial{\mathbf{x}}}\frac{\partial\rho}{\partial{\mathbf{x}}}^{\top}.$
(20)

Since $\rho({\mathbf{x}})>0$ and
$\frac{\partial\rho}{\partial{\mathbf{x}}}\frac{\partial\rho}{\partial{\mathbf{x}}}^{\top}\geq
0$, the proof will follow if we can show that
$\nabla\cdot{\mathbf{k}}({\mathbf{x}})\geq 0$. We have

$\displaystyle\nabla\cdot{\mathbf{k}}({\mathbf{x}})=\sum_{j=1}^{n}\frac{\partial^{2}\rho}{\partial
x_{j}^{2}}.$ (21)

Letting $\Psi({\mathbf{x}})=\prod_{k=1}^{L}\Psi_{k}({\mathbf{x}})$, we obtain

$\displaystyle\frac{\partial^{2}\rho}{\partial x_{j}^{2}}$
$\displaystyle=\frac{\partial}{\partial
x_{j}}\left(-\frac{\alpha}{V^{\alpha+1}}\frac{\partial V}{\partial
x_{j}}\Psi({\mathbf{x}})+\frac{1}{V^{\alpha}}\frac{\partial\Psi}{\partial
x_{j}}\right)$
$\displaystyle=\frac{\alpha}{V^{\alpha}}\left(\frac{(\alpha+1)}{V^{2}}\left|\frac{\partial
V}{\partial
x_{j}}\right|^{2}{\color[rgb]{0,0,0}-\frac{1}{V}\frac{\partial^{2}V}{\partial
x_{j}^{2}}}\right)\Psi({\mathbf{x}})$
$\displaystyle+\frac{\alpha}{V^{\alpha}}\left(-\frac{2}{V}\frac{\partial
V}{\partial x_{j}}\frac{\partial\Psi}{\partial
x_{j}}{\color[rgb]{0,0,0}+}\frac{1}{\alpha}\frac{\partial\Psi^{2}}{\partial
x_{j}^{2}}\right).$ (22)

It is important to note that the last two terms in the above expression are
non-zero only in the transition region ${\mathbf{X}}_{s_{k}}$. Outside this
transition region $\frac{\partial\Psi}{\partial x_{j}}=0$ and
$\frac{\partial^{2}\Psi}{\partial x_{j}^{2}}=0$. To show that the above
quantity is positive outside the transition region in ${\mathbf{X}}_{1}$, we
use the bounds from Assumption 2. We have

$\frac{(\alpha+1)}{V}\left|\frac{\partial V}{\partial
x_{j}}\right|^{2}-\frac{\partial^{2}V}{\partial
x_{j}^{2}}\geq(\alpha+1)\bar{d}^{-1}_{V}\underline{d}^{2}_{V_{x}}-\bar{d}_{V_{x}^{2}}$

Thus, by choosing $\alpha$ sufficiently large, the above quantity can be made
positive.

We next show that equation (22) is non-negative in the transition region. For
this, we make use of the following facts. First,
$\Psi_{k}({\mathbf{x}})\geq\theta>0$ for $k=1,\ldots,L$ and hence
$\Psi({\mathbf{x}})$ is bounded away from zero. Second, from the construction
of $\Psi({\mathbf{x}})$ and $V({\mathbf{x}})$ functions there exists uniform
bounds on $\frac{\partial\Psi_{k}}{\partial
x_{j}},\frac{\partial^{2}\Psi_{k}}{\partial x_{j}^{2}}$, $\frac{\partial
V}{\partial x_{j}}$ and $\frac{\partial^{2}V}{\partial x^{2}_{j}}$. Third,
using Assumption 2, we know that the distance between the unsafe set and the
target set is bounded away from zero by a positive constant $\zeta$ and hence
$\left|\frac{\partial V}{\partial x_{j}}\right|^{2}$ is bounded away from
zero. Hence, the following bounds can be obtained for the
$\frac{\partial^{2}\rho}{\partial x_{j}^{2}}$ term

$\displaystyle\frac{(\alpha+1)}{V^{2}}\left|\frac{\partial V}{\partial
x_{j}}\right|^{2}\Psi({\mathbf{x}})\geq\left((\alpha+1)\bar{c}_{V}^{-2}\underline{c}_{V_{x}}^{2}\right)\theta,\;\;\;\frac{1}{\alpha}\frac{\partial^{2}\Psi}{\partial
x_{j}^{2}}\geq-\frac{\bar{c}_{\Psi_{x}^{2}}}{\alpha},$
$\displaystyle{\color[rgb]{0,0,0}-\frac{1}{V}\frac{\partial^{2}V}{\partial
x_{j}^{2}}}\Psi({\mathbf{x}})\geq{\color[rgb]{0,0,0}-\underline{c}_{V}^{-1}\bar{c}_{V_{x^{2}}}\theta},\;\;\;-\frac{2}{V}\frac{\partial
V}{\partial x_{j}}\frac{\partial\Psi}{\partial
x_{j}}\geq-2\underline{c}_{V}^{-1}\bar{c}_{V_{x}}\bar{c}_{\Psi_{x}}.$

Therefore, we have following lower bound for $\frac{\partial^{2}\rho}{\partial
x_{j}^{2}}$

$\displaystyle\frac{\partial^{2}\rho}{\partial x_{j}^{2}}\geq$
$\displaystyle\frac{\alpha}{V^{\alpha}}\left(\left((\alpha+1)\bar{c}_{V}^{-2}\underline{c}_{V_{x}}^{2}{\color[rgb]{0,0,0}-\underline{c}_{V}^{-1}\bar{c}_{V_{x^{2}}}}\right)\theta\right)$
$\displaystyle+\frac{\alpha}{V^{\alpha}}\left(-2\underline{c}_{V}^{-1}\bar{c}_{V_{x}}\bar{c}_{\Psi_{x}}-\frac{\bar{c}_{\Psi_{x}^{2}}}{\alpha}\right).$

Hence, by choosing $\alpha$ sufficiently large, of order $\frac{1}{\theta}$,
we can make the term inside the bracket positive.

To show that equation (19) is satisfied, we again make use of Assumption 2 and
the fact that $\Psi({\mathbf{x}})=1$,
$\frac{\partial\Psi}{\partial{\mathbf{x}}_{j}}=0$, and
$\frac{\partial^{2}\Psi}{\partial x_{j}^{2}}=0$ for
${\mathbf{x}}\in{\mathbf{X}}_{0}$ and $j=1,\ldots,n$. Further,
$\left|\frac{\partial V}{\partial x_{j}}\right|^{2}$ is bounded away from
zero. Hence for, ${\mathbf{x}}\in{\mathbf{X}}_{0}$, we obtain

$\displaystyle\nabla\cdot({\color[rgb]{0,0,0}{\mathbf{k}}({\mathbf{x}})}\rho({\mathbf{x}}))=$
$\displaystyle\frac{\partial\rho}{\partial{\mathbf{x}}}\frac{\partial\rho}{\partial{\mathbf{x}}}^{\top}+\frac{\alpha(\alpha+1)}{V^{\alpha+2}}\left|\frac{\partial
V}{\partial x_{j}}\right|^{2}$
$\displaystyle{\color[rgb]{1,0,0}-\frac{1}{V^{\alpha+1}}\frac{\partial^{2}V}{\partial
x_{j}^{2}}}\geq\xi>0.$

for some $\xi>0$.

Proof of Theorem 1:  Using the results of Lemma 1, we know that the density
$\rho$ satisfies

$\displaystyle\nabla\cdot({\mathbf{k}}({\mathbf{x}})\rho({\mathbf{x}}))=g({\mathbf{x}})$
(23)

for some $g({\mathbf{x}})\geq 0$ such that $g({\mathbf{x}})\geq\xi>0$ for
${\mathbf{x}}\in{\mathbf{X}}_{0}$.

Since $\rho({\mathbf{x}})$ satisfies the linear partial differential equation
(23), it follows using the method of characteristics that the solution
$\rho(x)$ can be written in terms of the solution
${\mathbf{s}}_{t}({\mathbf{x}})$, of the system
$\dot{\mathbf{x}}={\mathbf{k}}({\mathbf{x}})$ as follows [28]

$\displaystyle\rho({\mathbf{x}})=\frac{\Psi({\mathbf{x}})}{V^{\alpha}({\mathbf{x}})}=\int_{0}^{\infty}g({\mathbf{s}}_{-t}({\mathbf{x}}))\left|\frac{\partial{\mathbf{s}}_{-t}({\mathbf{x}})}{\partial{\mathbf{x}}}\right|dt,$
(24)

where $|\cdot|$ is the determinant. The proof follows by substituting the
integral formula for $\rho({\mathbf{x}})$ from (24) in (23) and using the fact
that

$\displaystyle\lim_{t\to\infty}g({\mathbf{s}}_{-t}({\mathbf{x}}))\left|\frac{\partial{\mathbf{s}}_{-t}({\mathbf{x}})}{\partial{\mathbf{x}}}\right|=0.$
(25)

The limit in (25) goes to zero as $\rho({\mathbf{x}})$ is bounded for all
${\mathbf{x}}\in{\mathbf{X}}_{1}$ and using Barbalat’s Lemma. The integrant in
(24) defines a semi-group of linear Perron-Frobenius (P-F) operator,
${\mathbb{P}}_{t}$, acting on function $g({\mathbf{x}})$ and hence can be
written compactly as

$\displaystyle[{\mathbb{P}}_{t}g]({\mathbf{x}})=g({\mathbf{s}}_{-t}({\mathbf{x}}))\left|\frac{\partial{\mathbf{s}}_{-t}({\mathbf{x}})}{\partial{\mathbf{x}}}\right|.$
(26)

Using (26), (24) can be written as

$\displaystyle\rho({\mathbf{x}})=\int_{0}^{\infty}[{\mathbb{P}}_{t}g]({\mathbf{x}})dt.$
(27)

Furthermore, (25) can be written as

$\lim_{t\to\infty}[{\mathbb{P}}_{t}g]({\mathbf{x}})=0\implies\lim_{t\to\infty}[{\mathbb{P}}_{t}\mathds{1}_{{\mathbf{X}}_{0}}]({\mathbf{x}})=0,$

where $\mathds{1}_{{\mathbf{X}}_{0}}$ is the indicator function for set
${\mathbf{X}}_{0}$. This implication follows because
$g({\mathbf{x}})\geq\xi>0$ for all ${\mathbf{x}}\in{\mathbf{X}}_{0}$ and from
dominated convergence theorem. For any set $A\subseteq{\mathbf{X}}_{1}$, we
have

$\displaystyle\int_{A}[{\mathbb{P}}_{t}\mathds{1}_{{\mathbf{X}}_{0}}]({\mathbf{x}})d{\mathbf{x}}=\int_{{\mathbf{X}}_{1}}[{\mathbb{P}}_{t}\mathds{1}_{{\mathbf{X}}_{0}}]({\mathbf{x}})\mathds{1}_{A}({\mathbf{x}})d{\mathbf{x}}$
$\displaystyle=\int_{{\mathbf{X}}_{1}}\mathds{1}_{{\mathbf{X}}_{0}}({\mathbf{x}})\mathds{1}_{A}({\mathbf{s}}_{t}({\mathbf{x}}))dx.$
(28)

The above follows by using the definition of ${\mathbb{P}}_{t}$ in (26) and
change of variables in the integration, i.e.,
${\mathbf{y}}={\mathbf{s}}_{-t}({\mathbf{x}})$ and
$d{\mathbf{y}}=|\frac{\partial{\mathbf{s}}_{-t}({\mathbf{x}})}{\partial{\mathbf{x}}}|d{\mathbf{x}}$
and after relabeling. Note that the right-hand side of (28) is nothing but

$\int_{A}[{\mathbb{P}}_{t}\mathds{1}_{{\mathbf{X}}_{0}}]({\mathbf{x}})d{\mathbf{x}}=m\\{{\mathbf{x}}\in{\mathbf{X}}_{0}:{\mathbf{s}}_{t}({\mathbf{x}})\in
A\\}.$

From Lebesgue dominated convergence theorem

$0=\int_{A}\lim_{t\to\infty}[{\mathbb{P}}_{t}\mathds{1}_{{\mathbf{X}}_{0}}]({\mathbf{x}})d{\mathbf{x}}$
$=\int_{{\mathbf{X}}_{1}}\mathds{1}_{{\mathbf{X}}_{0}}({\mathbf{x}})\lim_{t\to\infty}\mathds{1}_{A}({\mathbf{s}}_{t}({\mathbf{x}}))d{\mathbf{x}}=m\\{{\mathbf{x}}\in{\mathbf{X}}_{0}:{\mathbf{s}}_{t}({\mathbf{x}})\in
A\\}.$

Since the above is true for any measurable and positive Lebesgue measure set
$A\subseteq{\mathbf{X}}_{1}:={\mathbf{X}}\setminus{\cal B}_{\delta}$ for
arbitrary small $\delta$, we obtain

$\displaystyle
m\\{{\mathbf{x}}\in{\mathbf{X}}_{0}:\lim_{t\to\infty}{\mathbf{s}}_{t}({\mathbf{x}})\neq
0\\}=0.$ (29)

We next show that the unsafe set ${\mathbf{X}}_{u_{k}}$ will be avoided by
trajectories ${\mathbf{s}}_{t}({\mathbf{x}})$ starting from almost all w.r.t.
Lebesgue measure initial condition ${\mathbf{x}}\in{\mathbf{X}}_{0}$. We have
for ${\mathbf{x}}\in{\mathbf{X}}_{u_{k}}$

$\displaystyle\rho({\mathbf{x}})=\frac{\Psi_{k}({\mathbf{x}})}{V^{\alpha}}=\frac{\theta}{V^{\alpha}}.$
(30)

Following Assumption 2 (equation (9)), we have

$\displaystyle\rho({\mathbf{x}})=\frac{\theta}{V^{\alpha}}\leq\frac{\theta}{V_{min}^{k}}.$
(31)

Using the above bound on $\rho({\mathbf{x}})$, we obtain

${\mathbf{G}}:=\int_{{\mathbf{X}}_{u_{k}}}\int_{0}^{\infty}[{\mathbb{P}}_{t}\mathds{1}_{{\mathbf{X}}_{0}}]({\mathbf{x}})dtd{\mathbf{x}}=\int_{{\mathbf{X}}_{u_{k}}}\rho({\mathbf{x}})d{\mathbf{x}}\leq\frac{\theta}{V_{min}^{k}}m({\mathbf{X}}_{u_{k}}),$

where $m(\cdot)$ is the Lebesgue measure. Utilizing that
$d{\mathbf{y}}=|\frac{\partial{\mathbf{s}}_{-t}({\mathbf{x}})}{\partial{\mathbf{x}}}|d{\mathbf{x}}$,
which is described through the definition of ${\mathbb{P}}_{t}$ and performing
a change of variable ${\mathbf{y}}={\mathbf{s}}_{-t}({\mathbf{x}})$, we can
use the bounds on $\rho({\mathbf{x}})$ in (31) for
${\mathbf{x}}\in{\mathbf{X}}_{u_{k}}$ to obtain

${\mathbf{G}}=\int_{{\mathbf{X}}_{1}}\mathds{1}_{{\mathbf{X}}_{0}}({\mathbf{y}})\int_{0}^{\infty}\mathds{1}_{{\mathbf{X}}_{u_{k}}}({\mathbf{s}}_{t}({\mathbf{y}}))dtd{\mathbf{y}}\leq\frac{\theta}{V_{min}^{k}}m({\mathbf{X}}_{u_{k}}).$

The time integral on the left-hand side is the time spent by system
trajectories starting from the initial set ${\mathbf{X}}_{0}$ in the unsafe
set ${\mathbf{X}}_{u_{k}}$. Let this time be denoted by $T({\mathbf{y}})$.
Hence, we obtain

$\int_{{\mathbf{X}}_{1}}T({\mathbf{y}})\mathds{1}_{{\mathbf{X}}_{0}}({\mathbf{y}})d{\mathbf{y}}\leq\frac{\theta}{V_{min}^{k}}m({\mathbf{X}}_{u_{k}}).$

Following Assumption 2 (equation (10)), we have

$\theta\leq\epsilon\frac{V_{min}^{k}}{m({\mathbf{X}}_{u_{k}})}\implies\int_{{\mathbf{X}}_{0}}T({\mathbf{y}})d{\mathbf{y}}\leq\epsilon,$

for any given $\epsilon>0$.

Choose some $\eta<1$, then using Chebyshev’s inequality and the fact that
${\mathbf{X}}_{0}\subset{\mathbf{X}}_{1}$, we have

$m\\{{\mathbf{x}}\in{\mathbf{X}}_{0}:T({\mathbf{y}})\geq\epsilon^{\eta}\\}\leq\epsilon^{-\eta}\int_{{\mathbf{X}}_{0}}T({\mathbf{y}})d{\mathbf{y}}\leq\epsilon^{-\eta+1}.$

Since the above is true for arbitrary small $\epsilon>0$, we have

$\displaystyle
m\\{{\mathbf{x}}\in{\mathbf{X}}_{0}:T({\mathbf{y}})=\int_{0}^{\infty}\mathds{1}_{{\mathbf{X}}_{u_{k}}}({\mathbf{s}}_{t}({\mathbf{x}}))dt>0\\}=0.$
(32)

Now we make use of the continuity property of the flow
${\mathbf{s}}_{t}({\mathbf{x}})$ w.r.t. time to show that
$\mathds{1}_{{\mathbf{X}}_{u_{k}}}({\mathbf{s}}_{t}({\mathbf{x}}))=0$ for all
$t\geq 0$. Assume not, then there exists $\gamma$ and $\bar{t}$ such that
$\mathds{1}_{{\mathbf{X}}_{u_{k}}}({\mathbf{s}}_{\bar{t}}({\mathbf{x}}))\geq\gamma>0$.
Then from the continuity of solution ${\mathbf{s}}_{t}({\mathbf{x}})$ w.r.t.
time, we know that there exists $\Delta>0$ such that
$\mathds{1}_{{\mathbf{X}}_{u_{k}}}({\mathbf{s}}_{t}({\mathbf{x}}))>0$ for
$t\in[\bar{t},\bar{t}+\Delta]$. This violates (32).

## References

  * [1] Steven M LaValle.  Planning algorithms.  Cambridge university press, 2006.
  * [2] Steven M LaValle and James J Kuffner.  Rapidly-exploring random trees: Progress and prospects: Steven m. lavalle, iowa state university, a james j. kuffner, jr., university of tokyo, tokyo, japan.  Algorithmic and Computational Robotics, pages 303–307, 2001.
  * [3] Nancy M Amato and Yan Wu.  A randomized roadmap method for path and manipulation planning.  In Proceedings of IEEE international conference on robotics and automation, volume 1, pages 113–120. IEEE, 1996.
  * [4] Sertac Karaman and Emilio Frazzoli.  Sampling-based algorithms for optimal motion planning.  The international journal of robotics research, 30(7):846–894, 2011\.
  * [5] Lucas Janson, Edward Schmerling, Ashley Clark, and Marco Pavone.  Fast marching tree: A fast marching sampling-based method for optimal motion planning in many dimensions.  The International journal of robotics research, 34(7):883–921, 2015\.
  * [6] Jonathan D Gammell, Siddhartha S Srinivasa, and Timothy D Barfoot.  Batch informed trees (bit): Sampling-based optimal planning via the heuristically guided search of implicit random geometric graphs.  In 2015 IEEE international conference on robotics and automation (ICRA), pages 3067–3074. IEEE, 2015.
  * [7] Richard M Murray, Zexiang Li, and S Shankar Sastry.  A mathematical introduction to robotic manipulation.  CRC press, 2017.
  * [8] Aaron D Ames, Samuel Coogan, Magnus Egerstedt, Gennaro Notomista, Koushil Sreenath, and Paulo Tabuada.  Control barrier functions: Theory and applications.  In 2019 18th European control conference (ECC), pages 3420–3431. IEEE, 2019.
  * [9] Aaron D Ames, Xiangru Xu, Jessy W Grizzle, and Paulo Tabuada.  Control barrier function based quadratic programs for safety critical systems.  IEEE Transactions on Automatic Control, 62(8):3861–3876, 2016.
  * [10] Jared Di Carlo, Patrick M Wensing, Benjamin Katz, Gerardo Bledt, and Sangbae Kim.  Dynamic locomotion in the mit cheetah 3 through convex model-predictive control.  In 2018 IEEE/RSJ international conference on intelligent robots and systems (IROS), pages 1–9. IEEE, 2018.
  * [11] Andrew Singletary, Aiden Swann, Yuxiao Chen, and Aaron D Ames.  Onboard safety guarantees for racing drones: High-speed geofencing with control barrier functions.  IEEE Robotics and Automation Letters, 7(2):2897–2904, 2022.
  * [12] Oussama Khatib.  Real-time obstacle avoidance for manipulators and mobile robots.  In Autonomous robot vehicles, pages 396–404. Springer, 1986.
  * [13] Bruce Krogh.  A generalized potential field approach to obstacle avoidance control.  In Proc. SME Conf. on Robotics Research: The Next Five Years and Beyond, Bethlehem, PA, 1984, pages 11–22, 1984.
  * [14] Elon Rimon.  Exact robot navigation using artificial potential functions.  Yale University, 1990.
  * [15] D. E. Koditschek.  The Control of Natural Motion in Mechanical Systems.  Journal of Dynamic Systems, Measurement, and Control, 113(4):547–551, 12 1991.
  * [16] Savvas G Loizou.  Closed form navigation functions based on harmonic potentials.  In 2011 50th IEEE Conference on Decision and Control and European Control Conference, pages 6361–6366. IEEE, 2011.
  * [17] Ioannis F Filippidis and Kostas J Kyriakopoulos.  Navigation functions for everywhere partially sufficiently curved worlds.  In 2012 IEEE International Conference on Robotics and Automation, pages 2115–2120. IEEE, 2012.
  * [18] Li Fan, Jianchang Liu, Wenle Zhang, and Peng Xu.  Robot navigation in complex workspaces using conformal navigation transformations.  IEEE Robotics and Automation Letters, 8(1):192–199, 2022.
  * [19] Umesh Vaidya.  Optimal motion planning using navigation measure.  International Journal of Control, 91(5):989–998, 2018.
  * [20] Anders Rantzer and Stephen Prajna.  On analysis and synthesis of safe control laws.  In 42nd Allerton Conference on Communication, Control, and Computing, 2004.
  * [21] Savvas G Loizou and Ali Jadbabaie.  Density functions for navigation-function-based systems.  IEEE Transactions on Automatic Control, 53(2):612–617, 2008.
  * [22] Dimos V Dimarogonas and Kostas J Kyriakopoulos.  An application of rantzer’s dual lyapunov theorem to decentralized formation stabilization.  In 2007 European Control Conference (ECC), pages 882–888. IEEE, 2007.
  * [23] Hongzhe Yu, Joseph Moyalan, Umesh Vaidya, and Yongxin Chen.  Data-driven optimal control of nonlinear dynamics under safety constraints.  IEEE Control Systems Letters, 6:2240–2245, 2022.
  * [24] Joseph Moyalan, Yongxin Chen, and Umesh Vaidya.  Convex approach to data-driven off-road navigation via linear transfer operators.  IEEE Robotics and Automation Letters, pages 1–8, 2023.
  * [25] Anders Rantzer.  A dual to Lyapunov’s stability theorem.  Systems & Control Letters, 42(3):161–168, 2001.
  * [26] Umesh Vaidya and Prashant G Mehta.  Lyapunov measure for almost everywhere stability.  IEEE Transactions on Automatic Control, 53(1):307–323, 2008.
  * [27] Loring W Tu.  Manifolds.  In An Introduction to Manifolds, pages 47–83. Springer, 2011.
  * [28] R. Rajaram, U. Vaidya, M. Fardad, and B. Ganapathysubramanian.  Almost everywhere stability: Linear transfer operator approach.  Journal of Mathematical analysis and applications, 368:144–156, 2010.