# Avoiding Dense and Dynamic Obstacles in Enclosed Spaces: Application to
Moving in Crowds

Lukas Huber1 $\cdot$ Jean-Jacques Slotine 2 $\cdot$ Aude Billard 1 *This work
was funded in part by the EU ERC grant SAHR.1 LASA Laboratory, Swiss Federal
School of Technology in Lausanne - EPFL, Switzerland.
<EMAIL_ADDRESS>2 Nonlinear Systems Laboratory,
Massachusetts Institute of Technology, USA<EMAIL_ADDRESS>
###### Abstract

This paper presents a closed-form approach to constraining a flow within a
given volume and around objects. The flow is guaranteed to converge and to
stop at a single fixed point.
The obstacle avoidance problem is inverted to enforce that the flow remains
enclosed within a volume defined by a polygonal surface. We formally guarantee
that such a flow will never contact the boundaries of the enclosing volume or
obstacles. It asymptotically converges towards an attractor. We further create
smooth motion fields around obstacles with edges (e.g., tables). Both
obstacles and enclosures may be time-varying, i.e., moving, expanding, and
shrinking.
The technique enables a robot to navigate within enclosed corridors while
avoiding static and moving obstacles. It was applied on an autonomous robot
(QOLO) in a static complex indoor environment and tested in simulations with
dense crowds. The final proof of concept was performed in an outdoor
environment in Lausanne. The QOLO-robot successfully traversed a marketplace
in the center of town in the presence of a diverse crowd with a non-uniform
motion pattern.

##  I Introduction

Robots navigating in human-inhabited environments will encounter disturbances
constantly, for instance, when autonomous delivery robots drive around
pedestrians. The robot must have a flexible control scheme to avoid
collisions. As the number of obstacles increases and their motion becomes less
predictable, the robot needs to reevaluate its path within milliseconds to
prevent a crash while moving actively towards its goal.
Control using dynamical systems (DS) is ideal for addressing such situations.
In contrast to classical path planning, the control law is closed-form, hence
requires no replanning, and can ensure impenetrability of obstacles [1, 2]. DS
offer stability and convergence guarantees in addition to the desired on-the-
fly reactivity.

###  I-A Related Work

####  I-A1 Sampling Based Exploration

Sampling algorithms such as probabilistic road map (PRM) [3] or the rapidly
exploring random trees (RRT) [4] can find a path in cluttered environments.
They are computationally intensive, and early approaches were limited to
static environments. In [5], online (partial) replanning and elastic-band
methods deform the path locally. This allowed adapting to dynamic
environments. Reinforcement learning allowed the authors in [6] to learn from
previously explored PRM-paths and efficiently adapt to dynamic obstacles.
However, the switching often comes with the theoretical loss of global
convergence [7]. Recent work uses customized circuitry on a chip for onboard,
fast global sampling and evaluation [8]. This method allows fast replanning,
but a customized chip design is required for each robot configuration.

####  I-A2 Real-Time Optimization

With improvements in hardware and computational speed, optimization algorithms
such as model predictive control (MPC) have become feasible for onboard use in
dynamic path planning, and obstacle avoidance [9]. MPC has been used to
control non-holonomic robots in environments with multiple convex obstacles
[10]. Most optimization methods can not guarantee convergence to a feasible
solution at runtime.
Power diagrams were used to identify the robot’s collision-free, convex
neighborhood, and an associated, well-known convex optimization problem
generates a continuous flow [11]. This method is limited to convergence for
convex obstacles with almost spherical curvature.
Control barrier functions (CBFs) and Control Lyapunov Functions (CLFs) were
united through the use of quadratic programming (QP) to create collision-free
paths in [12]. The convergence constraint was softened to ensure the
feasibility of the optimization problem. As a result, full convergence cannot
be guaranteed anymore. In [13], local minima were overcome by introducing a
(virtual) orientation state. This orientation state introduces a dependence on
the history of the QP problem. Additionally, the authors do not address the
challenge of finding an appropriate Lyapunov-candidate.
In [14] a diffeomorphic transformation to a sphere-world is used, which is
based on [15]. The method introduces a dependence on its history through
virtual obstacle positions and radii. Furthermore, the construction of the
diffeomorphism requires full knowledge about the space.

####  I-A3 Learning Based Approaches

With the rising popularity of machine learning in the past years, these
algorithms have been applied to sensor data to infer data-driven control [16]
but this method cannot ensure impenetrability. Other approaches use neural
networks on a circular representation of crowds to create steering laws but
cannot guarantee convergence [17].

####  I-A4 Velocity Obstacles

Velocity obstacles extend the obstacle shape by its potential future positions
based on the current velocity [18]. Velocity obstacles allow safe navigation
in dynamic environments. They were successfully applied in multi-agent
scenarios [19] and extended to include acceleration, and non-holonomic
constraints of the agent [20]. The velocity obstacle approach often
conservatively limits the workspace.

####  I-A5 Artificial Potential Fields and Navigation Functions

Artificial potential fields were used to create collision-free trajectories
[21], but they are prone to local minima. In [22] artificial potential fields
for sphere-worlds were designed to have only a global minimum. A diffeomorphic
transformation was introduced to map star-worlds to sphere-worlds [15], and
extended to include trees-of-stars [23]. The tuning of critical parameters
needs the knowledge of the whole space. Hence, in practice, full convergence
is difficult to achieve.
More recent approaches introduce artificial potential fields with only the
global minimum for more general environments [24, 25]. Automated approximation
of the tuning parameter has been proposed [26], but it does not generalize
easily to dynamic environment. In [27], full convergence is ensured around
ellipse obstacles through quadratic potential functions. Learning methods were
used to tune the hyper-parameters of potential fields to obtain human-inspired
behavior for obstacle avoidance [28].
Danger-fields were used in [29] to ensure collision avoidance for robot arms
through repulsion from dynamic obstacles. The approach was extended by guiding
the motion through an artificial potential field in [30]. The design of the
artificial potential field remains a challenge for this method. Dynamic
reference points help to reduce the probability of converging to a local
minimum for potential fields [31].

####  I-A6 Harmonic Potential Fields

Harmonic potential functions are interesting as they guarantee that no
topologically critical points arise in free space. In [32], the harmonic
potential functions are evaluated numerically to overcome the challenge of
finding them analytically. Closed-form harmonic potential functions can be
generated by approximating the obstacles through linear panels [33]. This
linear approximation applies to concave obstacles but is limited to two-
dimensional environments [1]. In [34], known harmonic potential functions are
interpolated to navigate in more complex environments. A closed-form solution
to harmonic potential flow around simple obstacles was presented in [35]. The
work allows to avoid moving obstacles but is limited to convex. In [36], the
approach was extended to concave obstacles by using a discrete, sensor-based
representation. Closed-form approaches using harmonic potential fields are
often simplified to a circular world or require high (close to circular)
curvature.
In [37], a dynamic reference point was introduced to ensure convergence for
star-shaped environments. However, the approach was not able to handle
boundaries or deforming obstacles.

###  I-B Contributions

This paper addresses the need for a reactive (closed-form) obstacle avoidance
approach with formal guarantees of impenetrability to handle highly dynamic
environments and realistic obstacles, such as obstacles with sharp edges. To
this end, this paper extends our previous work [37], in which we presented a
closed-form obstacle avoidance approach guaranteed to not penetrate smooth
concave, albeit star-shaped, obstacles. We present three novel theoretical
contributions:
1) We invert the obstacle description to ensure that the robot moves within
the enclosed space defined by the boundary while preserving convergences
guarantees towards an attractor. This boundary may represent walls, furniture,
or even joint limits of manipulators (Sec. III).
2) We extend the approach to handle nonsmooth surfaces, i.e., obstacles with
sharp edges. The novelty comes from creating a smooth dynamical system around
an obstacle without approximation of the curvature (Sec. IV).
3) We show that the approach can be extended to tackle dynamic environments,
with obstacles that have deforming shapes (Sec. V).

####  I-B1 Implementation and Technical Contributions

We validate these contributions with a wheelchair robot moving in a simulated
crowd of pedestrians and an office environment with real furniture (Sec.
VIII).
For the real-world implementation, we present three technical contributions:
T.1) We extend the modulation parameters of the obstacle avoidance with
surface friction and repulsive value (Sec. II). They allow an agent to move
slower and further away from obstacles, respectively. The extensions result in
more cautious behavior.
T.2) We propose an approach on how to use the dynamical obstacle avoidance in
combination with a robot arm (Sec. VI). The safe joint velocity is evaluated
based on the proximity function combined with the obstacle avoidance
algorithm.
T.3) We introduce a general directional weighting for (ensured) non-trivial
summing of vector fields and corresponding gradient descent (Sec. -A).

###  I-C Notation

The state variable $\xi\in\mathbb{R}^{d}$ defines the state of a robotic
system. If not mentioned otherwise, $\xi$ will refer to the Cartesian position
of the agent.
The variable $\pi$ is used as the circle constant throughout the paper.
Superscripts are used to denote the name of variables and subscripts for
enumeration.
Bold-face Latin characters describe vectors and matrices. Vectors defined by
Greek characters are explicitly defined.
The brackets $\langle\cdot,\cdot\rangle$ denote the dot product of two
vectors.
The $\times$-operator indicates the cross product.
The square brackets indicate elements of a vector, e.g., $\xi_{[1]}$ denotes
the first element of $\xi$. Double dots within the brackets indicate a sub-
vector up to the specified number, e.g., $\xi_{[1:2]}$ is a vector of the
first two elements of $\xi$.

##  II Obstacle Avoidance Formulation

Dynamical system based obstacle avoidance has been proposed by the authors in
[37]. We restate previous definitions, and introduce two new extensions: a
friction parameter (Sec. II-E) and a repulsion parameter (Sec. II-F).

###  II-A Dynamical Systems

This work focuses on motion towards a goal of an autonomous dynamical system,
i.e. $\lim_{t-\rightarrow\infty}f(\xi_{a})=0$. The most direct dynamics
towards the attractor is a linear dynamical system of the form:

$\mathbf{f}(\xi)=-k(\xi-\xi^{a})$ (1)

where $k\in\mathbb{R}$ is a scaling parameter. The attractor $\xi^{a}$ is
visualized throughout this work as a star: $*$ and with set $k=1$.

###  II-B Obstacle Description

Each obstacle has a continuous distance function
$\Gamma(\xi):\mathbb{R}^{d}\mapsto\mathbb{R}_{\geq 0}$, which allows to
distinguish three regions:

Free points:
$\displaystyle\mathcal{X}^{f}=\\{\xi\in\mathbb{R}^{d}:\Gamma(\xi)>1\\}$
Boundary points:
$\displaystyle\mathcal{X}^{b}=\\{\xi\in\mathbb{R}^{d}:\Gamma(\xi)=1\\}$ (2)
Interior points:
$\displaystyle\mathcal{X}^{o}=\\{\xi\in\mathbb{R}^{d}\setminus(\mathcal{X}^{f}\cup\mathcal{X}^{b})\\}$

####  II-B1 Reference Point

For each obstacle $i$ a reference point is chosen such that it lies within the
kernel of the obstacle: $\xi^{r}_{i}\in\mathcal{X}^{o}_{i}$.111The kernel of a
star-shaped obstacle defines the region from which any surface point is
visible [38] The reference direction is defined as:

${\mathbf{r}_{i}}(\xi)={\left(\xi-\xi^{r}_{i}\right)}/{\|\xi-\xi^{r}_{i}\|}\quad\forall\xi\in\mathbb{R}^{d}\setminus\xi^{r}_{i}$
(3)

The reference point is visualized throughout this work as a cross $+$.

####  II-B2 Distance Function

By construction, the distance function $\Gamma(\cdot)$ increases monotonically
in radial direction and has a continuous first-order partial derivative
($C^{1}$ smoothness). Here, we define the general distance function as:

$\Gamma^{o}(\xi)=\left(\|\xi-\xi^{r}\|/R(\xi)\right)^{2p}\quad\forall\xi\in\mathbb{R}^{d}\setminus\xi^{r}$
(4)

with the power coefficient $p\in\mathbb{N}_{+}$. The local radius
$R(\xi)=\|\xi^{b}-\xi^{r}\|$ is a function of the local boundary point, which
is defined as:

$\xi^{b}=b\mathbf{r}(\xi)+\xi^{r}\quad\text{such that}\quad
b>0\,,\;\;\xi^{b}\in\mathcal{X}^{b}$ (5)

###  II-C Obstacle Avoidance through Modulation

Real-time obstacle avoidance is obtained by applying a dynamic modulation
matrix to a dynamical system $\mathbf{f}(\xi)$:

$\dot{\xi}=\mathbf{M}(\xi)\mathbf{f}(\xi)\qquad\text{with}\quad\mathbf{M}(\xi)=\mathbf{E}(\xi)\mathbf{D}(\xi)\mathbf{E}(\xi)^{-1}$
(6)

The modulation matrix is composed of the basis matrix:

$\mathbf{E}(\xi)=\left[{\mathbf{r}}(\xi)\;\;\mathbf{e}_{1}(\xi)\;\;..\;\;\mathbf{e}_{d-1}(\xi)\right]$
(7)

which has the orthonormal tangent vectors $\mathbf{e}_{i}(\xi)$ evaluated at
the boundary point $\xi^{b}$ given in (5).
The diagonal eigenvalue matrix is given as:

$\mathbf{D}(\xi)=\textbf{diag}\left(\lambda^{r}(\xi),\lambda^{e}(\xi),\,\ldots,\lambda^{e}(\xi)\right)$
(8)

We set the eigenvalues to

$\lambda^{r}(\xi)=1-{1}/{\Gamma(\xi)}^{1/\rho}\qquad\lambda^{e}(\xi)=1+{1}/{\Gamma(\xi)}^{1/\rho}$
(9)

with the reactivity factor $\rho\in\mathbb{R}_{>0}$ and the distance function
$\Gamma(\xi)$ from (4). In this work, we simply choose $\rho=1$.

###  II-D Multiple Obstacles

In the presence of multiple obstacles, the velocity is modulated for each
obstacle individually as described in (6). The final velocity is obtained by
taking the weighted directional mean of the individual velocities, see Sec.
-A.

###  II-E Surface Friction Imitation

The choice of eigenvalues in (9) is inspired by the harmonic potential flow,
i.e., the description of the potential flow of an incompressible fluid. The
incompressibility constraint forces the velocity to increase in regions where
the flow is pushed around the obstacle, i.e., the eigenvalues in the tangent
direction increase. This leads to acceleration close to the surface (see Fig.
1).
We propose to mimic surface friction, i.e., slowing down in tangent direction
close to an obstacle ($\lim_{\Gamma\rightarrow 1}\xi^{e}=0$). A friction
parameter $\lambda^{f}(\xi)$ ensures the slowing down close to the surface.
The friction dynamics $\dot{\xi}^{f}$ are obtained by applying the factor to
tangent and reference direction as follows:

$\dot{\xi}^{f}=\lambda^{f}(\xi)\frac{\|\mathbf{f}(\xi)\|}{\|\dot{\xi}\|}\dot{\xi}\qquad\text{with}\quad\lambda^{f}(\xi)=1-1/\Gamma(\xi)$
(10)

where the $\dot{\xi}$ is the modulated velocity from (6).

Figure 1: The velocity obtained with the isometric inspired eigenvalue (left)
results in accelerations close to the obstacle. The modulation inspired by
surface friction (right) reduces the velocity with decreasing distance to the
obstacle. The black star is the attractor $\xi^{a}$, and the black cross is
the (shared) reference point $\xi^{r}$.

###  II-F Repulsive Eigenvalue

The eigenvalues in reference direction given in (9) are always positive, as
follows from $\Gamma(\xi)\geq 1$. This results in eigenvalues in the range of
$\lambda^{r}(\xi)\in[0,1]$, hence reduces the velocity in radial direction.
Conversely, active repulsion can be achieved through negative eigenvalues,
i.e., $\lambda^{r}(\xi)<0$. Active repulsion increases the distance by which
the an agents avoids the obstacle (Fig. 2). Since the repulsive eigenvalues
are incorporated in the modulation matrix in (6), it is ensured that
attractors are preserved.
The eigenvalue in radial direction is defined for repulsive obstacles as:

$\lambda^{r}(\xi)=\begin{cases}1-\left({c^{\mathrm{rep}}}/{\Gamma(\xi)}\right)^{1/\rho}&\text{if}\;\langle\mathbf{f}(\xi),\mathbf{r}\rangle<0\\\
1&\text{otherwise}\end{cases}$ (11)

with the repulsive coefficient $c^{\mathrm{rep}}\geq 1$. A repulsive
coefficient $c^{\mathrm{rep}}=1$ corresponds to no repulsion. Note, that the
repulsive eigenvalues are coupled with no tail-effect, i.e.
$\lambda^{r}(\xi)=0$ in the wake of an obstacle (see [35]).

Figure 2: A repulsion coefficient of $c^{\mathrm{rep}}=1$ results in strictly
positive eigenvalues (top). An increased distance to the obstacle is obtained
with higher repulsion coefficients, such as $c^{\mathrm{rep}}=2$ (bottom).

##  III Inverted Obstacle Avoidance

An autonomous robot often encounters scenarios where it has boundaries that it
cannot pass. This might be a wall for a wheeled robot or the joint limits for
a robot arm. These constraints can be interpreted as staying within an
obstacle, where the obstacle represents the limits of the free space.

###  III-A Distance Inversion

The distance function $\Gamma(\xi)$ from (4) can be evaluated within the
obstacle $\xi\in\mathcal{X}^{o}$.222In the classic obstacle avoidance case,
this is of no use, since theoretically the obstacle does never reach the
boundary [37], and practically an emergency control has to be applied in this
case. For interior points, our boundary function is monotonically decreasing
along the radial direction, i.e. the Lie derivative with respect to the
reference direction is positive. Furthermore it is bounded. This can be
written as:

${L}_{\mathbf{r}}\Gamma=\langle\frac{\partial\Gamma(\xi)}{\partial\mathbf{r}(\xi)},\mathbf{r}(\xi)\rangle>0,\hskip
14.22636pt\;\;\Gamma(\xi)\in\left[0,1\right[\quad\forall\;\mathcal{X}^{o}\setminus\xi^{r}$

We consider the obstacle boundary $\mathcal{X}^{b}$ as the description of an
enclosing hull. It follows that the interior points of the classical obstacle
become points of free space of the enclosing hull and vice versa. Boundary
points stay boundary points. We define the distance function of wall-obstacles
as the inverse of the obstacle distance function:

$\Gamma^{w}(\xi)=1/\Gamma^{o}=\left(R(\xi)/\|\xi-\xi^{r}\|\right)^{2p}\qquad\forall\;\mathbb{R}^{d}\setminus\xi^{r}$
(12)

This new distance function fulfills the condition for the three regions as
given in (2). The distance function $\Gamma^{w}$ is now monotonically
decreasing along radial direction and reaches infinity at the reference point,
i.e., $\lim_{\xi\rightarrow\xi^{r}}\Gamma^{w}(\xi)\rightarrow\infty$ (see Fig.
3).

Figure 3: Forward and inverted $\Gamma$-function for the same shape. The brown
region marks the inside of the obstacle and wall, respectively.

###  III-B Modulation Matrix

The modulation matrix, defined in (6), consists of the eigenvalue
$\mathbf{D}(\xi)$ and basis matrix $\mathbf{E}(\xi)$. For inverted obstacles,
the diagonal eigenvalue matrix from (8) is a function of the inverted distance
function $\Gamma^{w}(\xi)$.
Conversely, the basis matrix is constant along the radial direction. It can be
evaluated everywhere (including the interior of a boundary) except at the
reference point ($\xi=\xi^{r}$). Since the reference direction from (3) is a
zero vector, no orthogonal basis is defined. However, the distance value
$\Gamma^{w}(\xi^{r})$ reaches infinity, hence from (8) we get that the
diagonal matrix is equal to the identity matrix:

$\Gamma^{w}(\xi^{r})\rightarrow\infty\quad\Rightarrow\quad\mathbf{D}(\xi^{r})=\mathbf{I}$
(13)

Using (6), it follows for the modulated dynamics at the reference point:

$\mathbf{M}(\xi^{r})=\mathbf{E}(\xi^{r})\mathbf{D}(\xi^{r})\mathbf{E}(\xi^{r})^{-1}=\mathbf{E}(\xi^{r})\mathbf{I}\mathbf{E}(\xi^{r})^{-1}=\mathbf{I}\\\
\Rightarrow\dot{\xi}^{r}=\mathbf{f}(\xi^{r})$ (14)

The influence of modulation approaches zero when reaching the reference point.
Hence the dynamical modulation is continuously defined.

Theorem 1 Consider a star-shaped enclosing wall in $\mathbb{R}^{d}$ with
respect to a reference point inside the obstacle $\xi^{r}$, and a boundary
$\Gamma^{w}(\xi)=1$ as in (12). Any trajectory $\\{\xi\\}_{t}$, that starts
within the free space of an enclosing wall, i.e., $\Gamma(\\{\xi\\}_{0})>1$
and evolves on a smooth path according to (6), will never reach the wall,
i.e., $\Gamma(\\{\xi\\}_{t})>1,\;t=0..\infty$ and converges towards an
attractor $\xi^{a}\in\mathcal{X}^{f}$, i.e.,
$\lim_{t\rightarrow\infty}\xi\rightarrow\xi^{a}$. Proof: see Appendix -B.

(a) Ellipsoid boundary

(b) Star-shaped boundary

Figure 4: A smooth flow with full convergence towards the attractor (black
star) can be observed within any star-shaped wall with reference point
$\xi^{r}$ (black plus).

###  III-C Guiding Reference Point to Pass Wall Gaps

In many practical scenarios, a hull entails gaps or holes through which the
agent enters or exits the space (e.g., door in a room). The modulation-based
avoidance slows the agent down when approaching the boundary and does not let
it pass through such an exit. We introduce a guiding reference point $\xi^{g}$
for boundary obstacles to counter this effect. We assume convex walls, and the
robot size being smaller than the gap width. It is assumed that gap is a
priori known from a map.
In (14), it was shown that at the center of an inverted obstacle, the
influence of the modulation vanishes.
Let us define the gap region $\mathcal{X}^{g}$ enclosed by the lines
connecting the gap edges and the reference point $\xi^{r}$ (see Fig. 5). The
guiding reference point is designed in the following manner: close to the gap,
the guiding reference point is equal to the position of the evaluation, hence
no influence of the modulation. Far away from the gap, the guiding reference
point is equal to the reference point; hence the wall has no influence. In
between the two regions, the guiding reference point is projected onto the gap
region $\mathcal{X}^{g}$. This can be written as:

$\xi^{g}=\begin{cases}\xi\quad&\text{if}\;\;\;\xi\in\mathcal{X}^{g}\\\
\text{argmin}_{\hat{\xi}\in\mathcal{X}^{g}}\|\xi-\hat{\xi}\|&\text{else
if}\;\frac{\|\xi^{c,g}-\xi\|}{\|\xi^{c,g}-\xi^{r}\|}>1\\\
\xi^{r}\quad&\text{otherwise}\end{cases}$

with $\xi^{c,g}$ the center point of the gap (see Fig. 5).

Figure 5: The dynamical system (left) is not modulated in front of the gap
since the $\Gamma$-function reaches infinity (right). The gap region
$\mathcal{X}^{g}$ is the blue region. The influence of the gap is limited by
the blue half-circle around the center of the gap $\xi^{c,g}$ (black square).

##  IV Nonsmooth Surfaces

Human-designed environments often contain obstacles and enclosing walls with
nonsmooth surfaces, e.g., a table with edges or a building with corners. An
approximation with a high gradient of these surfaces can lead to undesired
results. On the one hand, a smoothing of the edges increases the risk of
colliding with them. On the other hand, an increased, smooth hull
conservatively increases the boundary region, and certain parts of the space
are not reachable with such a controller.
Moreover, the obstacle avoidance algorithm applied to a surface with a high
gradient can lead to a fast change of the flow. Even though the trajectories
are smooth, the curvature of the flow can be high and induce fast
accelerations. This can result in dangerous behavior in the presence of humans
or simply exceed the robot’s torque limits. We propose an approach to avoid
obstacles with nonsmooth surfaces without smoothing the boundary.
A polygonal obstacle consists of $i=1...N^{s}$ individually smooth surface
planes which form a star shape in $d-1$ such that:

$\displaystyle\mathcal{X}^{s}_{i}=\\{$
$\displaystyle\xi,\hat{\xi}\in\mathcal{X}^{b},\;\exists\,\mathbf{n}_{i}\,:\,\langle\mathbf{n}_{i},\,(\xi-\hat{\xi})\rangle=0\\}$
(15)

Figure 6: The variables for the evaluation of the pseudo normal
$\hat{\mathbf{n}}(\xi)$ of a nonsmooth star-shaped obstacle are displayed for
three surface tiles. Only the surface reference point (cross) and the surface
normal (red arrow) are visualized for the other tiles. The angle $\varphi^{w}$
is evaluated for each surface at the edge-point closest to $\xi$.

###  IV-A Pseudo Normal Vector

The normal to the surface of the obstacle is not defined continuously. As a
result, the modulated flow would not be smooth.
We create a smoothly defined pseudo normal $\hat{\mathbf{n}}(\xi)$. It is
equal to the normal on the surface of the obstacle. While far away from the
obstacle, the pseudo normal approaches the reference direction, i.e.:

$\hat{\mathbf{n}}(\xi)=\mathbf{n}_{i}(\xi)\;\;\forall\xi\in\mathcal{X}^{s}_{i}\quad\text{and}\quad\lim_{\|\xi-\xi^{r}\|\rightarrow\infty}\hat{\mathbf{n}}(\xi)=\mathbf{r}(\xi)$
(16)

The pseudo normal is the weighted sum of the normals, with the weights being
evaluated as follows. At first the vector from the closest edge point
$\mathbf{p}_{i}^{s}$ (Fig. 6) to the agent’s state is created:

$\mathbf{v}_{i}^{\angle}(\xi)=\xi-\mathbf{p}_{i}^{s}\;\;\;\text{with}\;\;\;\mathbf{p}_{i}^{s}=\underset{\hat{\xi}\in\mathcal{X}^{e}_{i}}{\mathrm{argmin}}\|\xi-\hat{\xi}\|$
(17)

with $\mathcal{X}^{e}_{i}$ the set of all points at the edge of a surface tile
$i$. This vector is further projected onto the surface plane:

$\hat{\mathbf{e}}_{i}(\xi)=\left(\mathbf{v}^{\angle}_{i}(\xi)-\langle\mathbf{n}_{i}(\xi),\,\mathbf{v}_{i}^{\angle}(\xi)\rangle\,\mathbf{n}_{i}(\xi)\right)\mathrm{sign}\langle\mathbf{v}_{i}^{\angle}(\xi),\,\xi-\xi^{r,s}_{i}\rangle$

The angle to the plane (Fig. 6) in the range $[0,\pi]$ is evaluated as:

$\varphi^{w}_{i}(\xi)=\arccos\left(\frac{\langle\hat{\mathbf{e}}_{i}(\xi),\,\mathbf{v}_{i}^{\angle}(\xi)\rangle}{\|\hat{\mathbf{e}}_{i}(\xi)\|\,\|\mathbf{v}_{i}^{\angle}(\xi)\|}\right)\text{sign}\langle\mathbf{n}_{i}(\xi),\,\mathbf{v}_{i}^{\angle}(\xi)\rangle$
(18)

The edge-weight is evaluated as:

$\tilde{w}^{s}_{i}(\xi)=\begin{cases}\left(\frac{\pi}{\varphi^{w}_{i}(\xi)}\right)^{p}-1&\text{if}\;\;\varphi^{w}_{i}(\xi)\in\,]0,\,\pi]\\\
0&\text{otherwise}\end{cases}$ (19)

with the the weight power $p\in\mathbb{R}$, a free parameter. A lower weight
power $p$ results in an increased importance for the closest polygon face
compared to the other faces, we simply choose $p=3$. The final step is the
normalization of the weights:

$w^{s}_{i}(\xi)=\begin{cases}\tilde{w}^{s}_{i}(\xi)/\sum_{j}\tilde{w}^{s}_{j}(\xi)&\text{if}\;\;\xi\in\mathbb{R}^{d}\setminus\mathcal{X}^{s}_{i}\\\
1&\text{otherwise}\end{cases}$ (20)

The pseudo normal is evaluated as the directional weighted mean (see Sec. -A)
of normal vectors of the surface tiles $\mathbf{n}_{i}(\xi)$, the weights
$w^{s}_{i}$, and with respect to the reference direction $\mathbf{r}(\xi)$.
The basis matrix from (7) is redefined for nonsmooth surfaces as:

$\mathbf{E}(\xi)=\left[{\mathbf{r}}(\xi)\;\;\hat{\mathbf{e}}_{1}(\xi)\;\;..\;\;\hat{\mathbf{e}}_{d-1}(\xi)\right]$
(21)

with $\hat{\mathbf{e}}_{i}(\xi)$ the orthonormal basis to
$\hat{\mathbf{n}}(\xi)$. The resulting smooth vectorfield can be observed in
Fig. 7.

###  IV-B Inverted Obstacles

For an inverted obstacle (Sec. III) the pseudo normal is evaluated at the
mirrored position $\xi^{\mathrm{mir}}$. It is obtained by flipping the current
robot state $\xi$ along the reference direction $\mathbf{r}(\xi)=\xi-\xi^{r}$
onto the other side of the boundary (4):

$\xi^{\mathrm{mir}}=\Gamma(\xi)^{2}\left(\xi-\xi^{r}\right)+\xi^{r}$ (22)

The mirrored position allows the evaluation of the distance function as
described in Sec. IV-A. Further, the inverted obstacle is treated as described
in Sec. III. This allows avoiding nonsmooth obstacles and boundaries in Fig.
4.

Theorem 2 Consider a polygon composed of $N^{s}$ surfaces as given in
$(\ref{eq:non_smooth_planes})$ or alternatively an inverted polygon as
described Sec. IV-B. Any trajectory $\\{\xi\\}_{t}$, that starts in free
space, i.e. $\Gamma(\\{\xi\\}_{0})>1$ and evolves on a smooth path according
to (6), will never reach the surface, i.e.
$\Gamma(\\{\xi\\}_{t})>1,t=0..\infty$ and will converge towards the attractor
as long as it is placed outside all obstacles, i.e.
$\lim_{t\rightarrow\infty}\xi\rightarrow\xi^{a}\in\mathcal{X}^{f}$.  Proof:
see Appendix -C.

(a) Nonsmooth obstacles

(b) Nonsmooth boundaries

Figure 7: Nonsmooth inverted obstacles representing rooms or boundary
conditions.

###  IV-C Implementation

Pseudo-normals are useful for surroundings with sharp boundaries, for example,
pieces of furniture with edges or maps of buildings with sharp corners. These
cases consist of polygons with a small number of faces. They allow the
evaluation of the algorithm in real-time. It is designed for scenarios with
obstacles known from previously learned libraries or a known map retrieved at
runtime, such as the tracker of [39].
Conversely, if the input data is a point cloud (e.g., Lidar) or an obstacle-
mesh, the surface can be approximated using a standard regression technique.
The surface normal can be obtained directly by taking the derivative or by
learning the normal at regression time [ao2021spinnet].

##  V Dynamic Environments

In changing environments with moving or deforming obstacles the system is
modulated with respect to the relative velocity as:

$\dot{\mathbf{\xi}}={\mathbf{M}(\xi)}\left(\mathbf{f}(\xi)-\dot{\tilde{\xi}}^{\mathrm{tot}}\right)+\dot{\tilde{\xi}}^{\mathrm{tot}}$
(23)

The local, relative velocity is summed up over all obstacles

$\dot{\tilde{\xi}}^{\mathrm{tot}}=\sum_{o=1}^{N_{o}}w^{{l}}_{o}\dot{\tilde{\xi}}_{o}$
(24)

with the dynamic weight being a function of the distance $\Gamma(\xi)$:

${w}^{{l}}_{o}=\frac{w^{l}_{o}}{\sum_{o}{\tilde{w}^{{l}}_{o}}}\quad\text{with}\;\;\tilde{w}^{{l}}_{o}=\frac{1}{\Gamma_{o}(\xi)-1}\quad\forall\;\Gamma_{o}(\xi)>1$

The relative velocity consists of the obstacle’s velocity
$\dot{\tilde{\xi}}^{v}_{o}$ and deformation $\dot{\tilde{\xi}}^{d}_{o}$:

$\dot{\tilde{\xi}}_{o}=\dot{\tilde{\xi}}^{v}_{o}+\dot{\tilde{\xi}}^{d}_{o}$
(25)

Note that avoiding dynamic obstacles is not only a modulation of the DS, i.e.,
a matrix multiplication. This can result in the velocity at the attractor
being non-zero, even though the initial dynamical system has zero value there:
$\mathbf{f}(\xi^{a})=\mathbf{0}$.
For the rest of this section, we will assume the application to each obstacle
implicitly without using the subscript $(\cdot)_{o}$.

###  V-A Moving Obstacles

The relative velocity of a moving obstacle is obtained similarly to [40]:

$\dot{\tilde{\xi}}^{v}=\dot{\mathbf{\xi}}^{L,v}+\dot{\mathbf{\xi}}^{R,v}\times\tilde{\mathbf{\xi}}$
(26)

the linear velocity $\dot{\xi}^{L,v}$ and angular velocity $\dot{\xi}^{R,v}$
are with respect to the center point of the obstacle $\xi^{c}$. The relative
position is ${\tilde{\xi}}=\xi-\xi^{c}$.

###  V-B Deforming Obstacle

Obstacles and hulls can not only move, but they can also change their shape
with respect to time, e.g. breathing body for a surgery robot. Conversely, the
deformation of the perceived obstacle can be the result of uncertainties in
real-time obstacle detection and position estimation.
The deformation velocity of an obstacle is evaluated as:

$\dot{\tilde{\xi}}^{d}=\dot{\mathbf{\xi}}^{L,d}+\dot{\mathbf{\xi}}^{R,d}\times\tilde{\mathbf{\xi}}^{d}$
(27)

Where the linear velocity $\dot{\mathbf{\xi}}^{L,d}$ and angular velocity
$\dot{\mathbf{\xi}}^{R,d}$ are evaluated on the surface position in reference
direction, and $\tilde{\mathbf{\xi}}^{d}=\xi-\xi^{b}$ is the relative position
with respect to the boundary point (see Fig. 8).
The surface deformation should be explicitly given to the algorithm whenever
it is known. Alternatively, it can be estimated from sensor readings, such as
Lidar or camera.

####  V-B1 Repulsive Mode

In many scenarios, the consideration of the obstacle’s deformation is only of
importance when it reduces the robot’s workspace, and puts the robot at risk.
It is sufficient to consider the deformation only along positive normal
direction. For example for a circular object, we have:

$\dot{\tilde{\xi}}^{d}=\dot{\mathbf{\xi}}^{L,d}=\begin{cases}\dot{r}\;\mathbf{n}(\xi)\hskip
14.22636pt&\dot{r}>0\\\ 0&\text{otherwise}\end{cases}$ (28)

where $\dot{r}$ is the rate of change of the circle radius.

###  V-C Impenetrability with Respect to Maximum Velocity

Many agents have a maximum velocity they can move with, further referred to as
$v^{\mathrm{max}}$. This limits the motion of obstacles which an agent can
avoid to:

$v^{n}=\langle\dot{\tilde{\xi}},\mathbf{n}(\xi)\rangle<v^{\mathrm{max}}\qquad\text{as}\quad\Gamma(\xi)\rightarrow
1$ (29)

When close to an obstacle, the agent must prioritize moving away from the
obstacle over following the desired motion. Since the modulated velocity
$\dot{\xi}$, see (6), does not take into account the maximal velocity, smart
cropping needs to be applied. We propose the following method, which
prioritizes avoidance in critical situations but sticks to the initial DS as
when safe:

$\dot{\xi}^{s}=\begin{cases}v^{n}\mathbf{n}(\xi)+v^{e}\;\mathbf{e}(\xi)&\text{if}\;\;\langle\frac{\dot{\xi}}{\|\dot{\xi}\|},\mathbf{n}(\xi)\rangle<\frac{v^{n}}{v^{\mathrm{max}}}\\\
v^{\mathrm{max}}\,\dot{\xi}/\|\dot{\xi}\|\;&\text{else
if}\;\;\|\dot{\xi}\|>v^{\mathrm{max}}\\\
\dot{\xi}&\text{otherwise}\end{cases}$ (30)

with $v^{e}=\sqrt{\left(v^{\mathrm{max}}\right)^{2}-\left(v^{n}\right)^{2}}$,
see Fig. 8.

Theorem 3 Consider the dynamic environment with $N^{\mathrm{obs}}$ obstacles
which have a weighted local velocity $\dot{\tilde{\xi}}^{\mathrm{tot}}$,
resulting from the obstacles’ movements and surface deformations, defined in
(24). An agent is moving in this space and has a maximum velocity of
$v^{\mathrm{max}}$, further the obstacles’ surface velocities are limited by
(29). The agent which starts in free space, i.e.
$\Gamma_{o}(\\{\xi\\}_{0})>1,\;\forall o\in N^{\mathrm{obs}}$ and moves
according to (30), will stay in free space for infinite time, i.e.
$\Gamma_{o}(\\{\xi\\}_{t})>1,\;t=0..\infty,\;\forall o\in N^{\mathrm{obs}}$.
Proof: see Appendix -E.

Figure 8: In order to comply with a velocity limit of the robot while avoiding
a collision with an obstacle of velocity $\dot{\xi}$, the modulated velocity
$\dot{\xi}$ might be stretched only in normal direction to obtain the safe
velocity command $\dot{\xi}^{s}$.

###  V-D Reference Point Placement

####  V-D1 Dynamic Extension of Hull

Clusters of more than two convex obstacles do often not form a star-shape. In
such cases, we propose to extend the hull of each obstacle such that they all
include a common reference point. The new hull is designed to be convex for
each obstacle, and hence the cluster is star-shaped. The extended hull creates
a cone that is tangent to the obstacle’s surface and has the reference point
at its tip (Fig. 9). Since the clusters are star-shapes, there is a global
convergence of the vector field towards the attractor.
The extension of the surfaces can be done dynamically, as a collision-free
trajectory is ensured around deforming obstacles (Sec. V-B).

(a) Reference inside

(b) Reference close

(c) Reference far

(d) Reference inside

(e) Reference close

(f) Reference far

Figure 9: Dynamic extension of the hull for an ellipsoid object without margin
(a)-(c) and a nonsmooth polygon object with constant margin (d)-(e).

####  V-D2 Clustered Environments with Obstacles and Boundaries

For obstacles which intersect with the boundary, the reference point has to be
placed inside the wall, i.e., $\Gamma_{b}(\xi^{r}_{o})<1$ (see Fig. 10). This
enforces all trajectories to avoid the intersecting obstacles by moving away
from the wall (counterclockwise in this example). The boundary modulates them
in the same direction. Hence, there is full convergence of all trajectories
towards the attractor. This is true for a boundary-obstacle with a positive
(local) curvature:

$c^{b}(\xi)>0\qquad\forall\,\xi$ (31)

with the curvature given as:

$\displaystyle\quad
c^{(\cdot)}{(\xi)}=\lim_{\Delta\xi\rightarrow\mathbf{0}}\frac{R(\xi)-R(\xi+\Delta\xi)}{\Delta\xi}$
$\displaystyle\quad\forall\xi\in\mathcal{X}^{b},\;\;\langle\Delta\xi,\,\mathbf{n}(\xi)\rangle=0$
(32)

Figure 10: Full convergence towards the attractor (black star) in an
environment of three obstacles intersecting with the boundary.

##  VI Obstacle Avoidance with Robots

The algorithm has so far been described for a point mass. It is
straightforward to extend this to control robots, which can be approximated by
a circle (e.g., drones, wheel-based platforms) by creating a margin around all
obstacles wide enough to account for the shape of the robot. The method can
also be extended to higher dimensions and multiple degrees of freedom robot
arms by describing and evaluating the system (robot + obstacle) in joint-
space. This, however, requires representing the obstacle in configuration
space which is not always easy, especially when the obstacle moves.

Alternatively, in the rest of this section, we introduce a weighted evaluation
of the desired dynamics along a robot arm’s links to obtain a collision-free
trajectory towards the desired goal in Cartesian space.

###  VI-A Goal Command Towards Attractor

Consider a robot arm with end-effector’s position $\xi$. The desired velocity
towards the attractor is evaluated as described in Section II to Section V and
denoted as $\dot{\xi}$. The goal command in joint space is evaluated through
inverse-kinematics as:

$\dot{\mathbf{q}}^{g}=\mathbf{\hat{J}}^{{\dagger}}(\mathbf{q})\dot{\xi}$ (33)

where $\mathbf{\hat{J}}(\mathbf{q})$ is the Jacobian of the robot arm with
respect to position only333The inverse of the Jacobian
$\mathbf{J}^{{\dagger}}(\mathbf{q})$ is obtained through the Moore-Penrose
pseudo-inverse..

####  VI-A1 $\Gamma$-Danger Field

The closer an obstacle is to the robot, the more it is in danger to collide.
Based on the $\Gamma(\xi)$-function introduced in (2), we introduce a
$\Gamma$-danger field with respect to all $N^{\mathrm{obs}}$ obstacles as

$\Gamma^{d}(\xi)=\min_{o\in[1..N^{\mathrm{obs}}]}\Gamma_{o}(\xi)$ (34)

The field is displayed in Fig. 11. This field is used to evaluate the weights
which continuously from the goal command $\mathbf{\dot{q}}^{g}$ to the
avoidance command $\mathbf{\dot{q}}^{m}$.

Figure 11: $\Gamma$-field and desired direction in a multi-obstacle
environment.

###  VI-B Link Avoidance

The avoidance command $\mathbf{\dot{q}}^{m}$ ensures that each link is
avoiding the collision with the environment. To extend the obstacle avoidance
algorithm to a rigid body, we introduce $N^{S}$ section points $\xi^{S}_{l,s}$
for each link $l$ and $s\in[1..N^{s}]$. The dynamical system is evaluated at
each section point and coupled with a section weight $w^{S}_{s}$. The section
point weight $w^{S}_{s}$ increases the lower the $\Gamma$-danger value (see
Sec. -F3).

####  VI-B1 Rigid Body Dynamics

The linear and angular avoidance velocity for link $l$ are obtained as:

$\mathbf{v}^{L}=\sum_{s=1}^{N^{S}}w^{S}_{s}\dot{\xi}_{l,s}^{S}\;\;\text{and}\;\;\omega^{L}=\sum_{s=1}^{N^{S}}w^{S}_{s}(\xi_{l,0}^{S}-\xi_{l,s}^{S})\times\left(\mathbf{v}^{S}_{s}-\mathbf{v}^{L}\right)$
(35)

where $\xi_{l,0}^{S}$ is the position of the root of a link $l$, and
$\dot{\xi}_{l,s}^{S}$ is the dynamical system based avoidance evaluated as
described in in Sec. II \- V. A single link avoiding a circle can be seen in
Fig. 12.

Figure 12: The resulting linear velocity $\mathbf{v}^{L}$ and angular velocity
$\omega^{L}$ for a rigid body with to sections points.

####  VI-B2 Joint Modulation

The desired joint avoidance command is obtained through inverse kinematics as:

$\mathbf{\dot{q}}^{m}=\mathbf{J}^{{\dagger}}(\mathbf{q},\xi_{l,0}^{S})\begin{bmatrix}\mathbf{v}^{L}\\\
\omega^{L}\end{bmatrix}$ (36)

where $\mathbf{J}(\mathbf{q},\xi_{l,0}^{S})$ is the Jacobian up the root of
the link $l$, i.e., the positionof the section point $\xi_{l,0}^{S}$.

###  VI-C Evaluation Along the Robot Arm

The evaluation is performed by iterating over all $N^{L}$ links, starting from
to base of the robot arm. At each iteration, the joint control
$\dot{\mathbf{q}}^{c}$ of all links which are part of the kinematic chain are
updated, i.e., if the evaluation is performed for link $l$, the joint control
are updated for $\mathbf{\dot{q}}^{c}_{i}\;\forall i\leq l$.
The evaluation is weighted along with the link to ensure collision avoidance
of all links while trying to follow the goal command $\mathbf{\dot{q}}^{g}$.
The link weights $w_{l}^{L}$ are further described in Sec. -F2.

####  VI-C1 Initial Joint Control

The joint control is initialized based on the goal control from (33) as:

$\dot{\mathbf{q}}^{c}\leftarrow\left(1-\sum_{l=1}^{N^{L}}w_{l}^{L}\right)\mathbf{\dot{q}}^{g}$
(37)

The first element of the joint command is then updated based on the avoidance
velocity obtained in (36) as:

$\dot{\mathbf{q}}^{c}_{[1]}\leftarrow\dot{\mathbf{q}}^{c}_{[1]}+w_{1}^{L}\mathbf{\dot{q}}^{m}_{[1]}$
(38)

####  VI-C2 Joint Control Correction

As an effect of influence the obstacle avoidance of each link, the obtained
control command $\dot{\mathbf{q}}^{c}$ differs from the ideal goal command
$\dot{\mathbf{q}}^{g}$. This difference is obtained at link $l$ as:

$\mathbf{v}^{\Delta}=\hat{\mathbf{J}}_{l}(\mathbf{q})\left(\mathbf{\mathbf{\dot{q}}}^{g}_{[1:l]}-\mathbf{\dot{q}}^{c}_{[1:l]}\right)\quad\forall\;l>1$
(39)

where $\mathbf{\dot{q}}^{c}_{[:l]}$ is the current control command, evaluated
up to link $l-1$, and $\hat{\mathbf{J}}_{l}(\mathbf{q})$ is the arm Jacobian
with respect to position up to link $l$.
The joint speed of link $l$ which would best account for this difference can
be evaluated as:

$\dot{q}^{\Delta}=\langle{\mathbf{\omega}}^{\Delta},\mathbf{I}^{\omega}\rangle\quad\text{with}\;\;{\mathbf{\omega}}^{\Delta}=\mathbf{v}^{\Delta}\times\mathbf{I}^{L}$
(40)

where $\mathbf{I}^{\omega}$ is the direction of rotation of the joint
actuating link $l$444We assume single-degree of freedom joints. and
$\mathbf{I}^{L}$ is the direction along with the link, i.e., pointing from one
joint to the next. The variables can be observed in Fig. 13.

####  VI-C3 Joint Control Update

The velocity of each joint is evaluated one-by-one, starting at the joint
closest to the base of the robot towards the end-effector (see Algorithm 1).
The joint command $\dot{\mathbf{q}}^{c}$ is first updated by applying the
correction control from (40) to the current joint $l$ as:

$\dot{\mathbf{q}}^{c}_{[l]}\leftarrow\dot{\mathbf{q}}^{c}_{[l]}+\dot{q}^{\Delta}\sum_{i=1}^{l-1}w^{L}_{i}\qquad\forall\,l>1$
(41)

The modulation command $\dot{\mathbf{q}}^{m}$ from (36) is then applied to all
underlying joints:

$\dot{\mathbf{q}}^{c}_{[1:l]}\leftarrow\dot{\mathbf{q}}^{c}_{[1:l]}+w^{L}_{l}\mathbf{\dot{q}}^{m}_{[1:l]}\qquad\forall\,l>1$
(42)

This is executed iteratively for all joints $l>1$.

Algorithm 1 Joint Control Command for a Robot Arm

0: $N^{L}$, $N^{S}$, $f(\xi)$, obstacle-environment

0: $\mathbf{\dot{q}}^{c}$

1: $\xi^{r}_{o}\;\;\forall o\in{1..N^{\mathrm{obs}}}$ {update dynamic
obstacles as in Sec. V}

2:
$\dot{\mathbf{q}}^{g}\leftarrow\left(\mathbf{J}(\mathbf{q})\right)^{{\dagger}}\dot{\xi}$
{compute goal command as in (33)}

3: for $l=1\textbf{ to }N^{L}$ do

4: for $s=1\textbf{ to }N^{S}$ do

5: $\Gamma(\xi_{s,l})$ {compute danger-field as in (34)}

6: $w^{\Gamma}$ {compute danger-weight as in (55)}

7: end for

8: $w^{L}_{l}$ {compute link weight as in (57)}

9: end for

10:
$\dot{\mathbf{q}}^{c}\leftarrow\left(1-\sum_{l}w_{l}^{L}\right)\mathbf{\dot{q}}^{g}$
{initialize control command}

11:
$\dot{\mathbf{q}}^{c}_{[1]}\leftarrow\dot{\mathbf{q}}^{c}_{[1]}+w_{1}^{L}\mathbf{\dot{q}}^{m}_{[1]}$

12: for $l=2\textbf{ to }N^{L}$ do

13: if $w^{L}_{l}>0$ then

14: for $s=1\textbf{ to }N^{s}$ do

15: $w^{S}_{s}$ {compute section weight as in (58)}

16: end for

17: $\mathbf{v}^{L}_{l}$, $\omega^{L}_{l}$ {compute avoidance velocities as in
(35)}

18: $\mathbf{\dot{q}}^{m}_{l}$ {compute modulation command as in (36)}

19:
$\dot{\mathbf{q}}^{c}_{[1:l]}\leftarrow\dot{\mathbf{q}}^{c}_{[1:l]}+w^{L}_{l}\mathbf{\dot{q}}^{m}_{[1:l]}$

20: end if

21:
$\mathbf{v}^{\Delta}\leftarrow\mathbf{J}_{l}(\mathbf{q})\left(\mathbf{\mathbf{\dot{q}}}^{g}_{[1:l]}-\mathbf{\dot{q}}^{c}_{[1:l]}\right)$

22:
${\mathbf{\omega}}^{\Delta}\leftarrow\mathbf{v}^{\Delta}\times\mathbf{I}^{L}$

23:
$\dot{q}^{\Delta}\leftarrow\langle{\mathbf{\omega}}^{\Delta},\mathbf{I}^{\omega}\rangle$

24:
$\dot{\mathbf{q}}^{c}_{[l]}\leftarrow\dot{\mathbf{q}}^{c}_{[l]}+\dot{q}^{\Delta}\sum_{i}w^{L}_{i}$

25: end for

Figure 13: A robot arm with three links ($N^{L}=3$), which each containing
three sections points ($N^{S}=3$) is surrounded by two obstacles. The blue
arrows are the goal velocity obtained (only evaluated at the joints), and the
red arrows are the weighted avoidance velocities. The direction of joint
rotation $\mathbf{I}^{\omega}_{l}$ is pointing out of the plane.

###  VI-D Validation in Simulation

We applied the algorithm to two scenarios with planar robotic arms (Fig. 14).
The scenario in Fig. 14(a) was chosen similar to the one presented in [30].
Our approach can avoid obstacles in a similar setup without a navigation
function.
The scenario in Fig. 14(b) includes a more complex robot with three links.
Additionally, the skew placement of the obstacle and the position of the start
end goal point require the robot to actively go around the obstacle guided by
the reference point (see Fig. 11 for the full DS).

(a) 2 DoF arm

(b) 3 DoF arm

Figure 14: Time sequence of robot arms navigating in two different
environments.

##  VII Comparison Algorithms

###  VII-A Qualitative Comparison

We have selected multiple time-invariant, local obstacle avoidance algorithms
for qualitative comparison with the presented method Table I. The Navigation
Functions, Lyapunov QP, Sphere World QP and Danger Fields all rely on critical
tuning parameters. This is a Lyapunov function for the Lyapunov QP, and
parameters for the diffeomorphic transformation or navigation function for the
other three methods. While the parameters and functions can be obtained
through manual tuning, no solution exists to set them in real-time
automatically. Hence, these methods cannot be easily applied to dynamic
environments.
Navigation functions and the diffeomorphic transform are defined globally.
Their tuning parameter depends on the distribution of the obstacles all across
space. Two obstacles close together far from an agent will determine the
possible tuning parameters and influence the avoidance behavior. As a result,
the methods cannot be transferred easily to clustered dynamic environments.
While other methods have already allowed navigation inside walls, this has not
been done in combination with proven star-shape world convergence and dynamic
surroundings.

|  Dynamic |  Reference |  Orthog. |  Repulsion |  Navigation
Functions |  Lyapunov
QP |  Sphere
World QP |  Danger
Fields
---|---|---|---|---|---|---|---|---
| (presented) | [37] | [35] | [21] | [22, 15, 23] | [13] | [14] | [29, 30]
Time invariant | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | ✓
History invariant | ✓ | ✓ | ✓ | ✓ | ✓ |  |  | ✓
Convergence: convex | ✓ | ✓ | (✓) |  | ✓ | ✓ | ✓ | ✓
Convergence: star-shape | ✓ | ✓ |  |  | ✓ | (✓) | ✓ | ✓
No critical tuning parameter
(incl. Lyapunov function) | ✓ | ✓ | ✓ | ✓ |  |  |  |
Avoidance behavior independent
of global distribution | ✓ | ✓ | ✓ | ✓ |  | ✓ |  |
Closed from solution
(no optimization) | ✓ | ✓ | ✓ | ✓ | ✓ |  |  | ✓
Considering initial dynamics
(not goal position only) | (✓) | (✓) | (✓) | ✓ |  | ✓ | ✓ | (✓)
Navigation inside walls | ✓ |  | ✓ | ✓ | ✓ | ✓ | ✓ | ✓
Smooth motion around corners | ✓ |  |  | ✓ | ✓ | ✓ | ✓ | ✓
Clustered dynamic environment | ✓ | (✓) | (✓) | ✓ |  |  |  |
TABLE I: The proposed dynamic obstacle avoidance is compared to different
state-of-the-art methods. The last three items refer to the main contributions
of this work.

###  VII-B Quantitative Comparison

For a quantitative comparison, we chose algorithms that can function in
clustered, dynamic environments and can handle external hulls (see Table I).
The method for the modulation algorithm in dynamic environments is presented
in this paper (referred as Dynamic during this section). It is compared to
[35], which uses modulation matrix based on an orthogonal decomposition matrix
$\mathbf{E}(\xi)$ (referred as Orthogonal) and the potential field algorithm
[21] (referred to as Repulsion).
The comparison is made in a simulated environment (Fig. 15). Two ellipse-
shaped obstacles randomly change shape, and the movement is inspired by random
walk. The combined maximum expansion and obstacle’s velocity are lower than
the maximum speed of the agents of
$v^{\mathrm{max}}=1\,$\mathrm{m}\mathrm{/}\mathrm{s}$$. The three algorithms
are given the same attractor as a goal. They start at the same time and
encounter the same environment.
The Dynamic algorithm is observed to have the highest rate of convergence
(Tab. II) resulting from the increased environment information through the
reference point. The Repulsion has a preferable behavior on avoiding
collisions because of its conservative behavior around obstacles (moving far
away and only approaching them slowly). This influences the distance traveled
and the time needed to reach a goal (Tab. III). The mean of the velocity is
lower for the Dynamic algorithm. This is a result of no tail-effect behind the
obstacles (see Sec. II-F). The variance of the velocity is similar for the
three algorithms.555The source code of the implementation is available on
https://github.com/epfl-lasa/dynamic_obstacle_avoidance

| Converged | Collided | Minimum
---|---|---|---
Dynamic | 77% | 23% | 0%
Orthogonal | 20% | 23% | 57%
Repulsion | 39% | 1% | 60%
TABLE II: The outcome of the 300 trials runs were either full convergence, collision with an obstacle or getting stuck in a local minimum. |  d $[m]$ |  t $[s]$ |  $\bar{v}$ $[m/s]$ |  $\sigma_{v}$ $[m/s]$
---|---|---|---|---
Dynamic |  9.69 $\pm$ 1.19 |  1.03 $\pm$ 0.13 |  0.61 $\pm$ 0.05 |  0.34 $\pm$ 0.02
Orthogonal |  10.06 $\pm$ 1.53 |  1.17 $\pm$ 0.21 |  0.55 $\pm$ 0.05 |  0.34 $\pm$ 0.02
Repulsion |  9.8 $\pm$ 1.29 |  1.39 $\pm$ 0.2 |  0.46 $\pm$ 0.03 |  0.21 $\pm$ 0.02
TABLE III: The mean and the standard deviation (after the $\pm$) are compared
for the three algorithms from the 54 trials where all three agents converged.
The metrics of distance (d), duration of the run (t), the mean velocity
($\bar{v}$) and the standard deviation of the velocity ($\sigma_{v}$) are
listed. Figure 15: Three snapshots of one experimental run are placed from
left to right. The obstacles are randomly initialized and move according to a
random walk. One obstacle moved from the right to the left, while the other
one was stationary. The three algorithms start at the same randomly chosen
position and move towards the attractor.

###  VII-C Computational Complexity

The presented algorithm is closed-form, whereas the matrix inverse is the most
complex computation. Since it is applied to all obstacles, the complexity
follows as $\mathcal{O}(d^{2.4}N^{\mathrm{obs}})$, a function of the number of
dimensions $d$ and the number of obstacles $N^{\mathrm{obs}}$. It had an
average time of $3.48\text{\,}\mathrm{m}\mathrm{s}$ on a computer with 8 Intel
Core i7-6700 CPU @ 3.40GHz.
This is more complex than the Orthgonal, where the matrix multiplication is
the most complex operation. The complexity follows as
$\mathcal{O}(d^{2}N^{\mathrm{obs}})$. This reflects in the slightly faster
evaluation time of $2.84\text{\,}\mathrm{m}\mathrm{s}$.
The potential field is the simplest of the three algorithms, with the norm
being the most complex operation and a complexity
$\mathcal{O}(dN^{\mathrm{obs}})$. It has the lowest evaluation time of
$1.75\text{\,}\mathrm{m}\mathrm{s}$.
The search for the optimal reference point is the computationally most
extensive calculation because it requires the (iterative) closest-distance
evaluation between obstacles. The Python library
shapely666https://github.com/Toblerity/Shapely was used for the
implementation, and an evaluation time of $5.14\text{\,}\mathrm{m}\mathrm{s}$
was obtained.

##  VIII Empirical Validation

The empirical validation is performed with the mobile robot QOLO [44], see
Fig. 16; first in simulation and then on a real robot platform. QOLO, a semi-
autonomous wheelchair, is designed to navigate in pedestrian environments and
indoors. This platform is hence suited to test our algorithm’s ability to
avoid moving obstacles (pedestrians) and non-convex obstacles (walls, indoor
furniture) containing sharp edges (tables, shelves).

Figure 16: A picture of the semi-autonomous wheelchair real (left) and the
simulation rendering including an operator (right).

In all our experiments, we assume that QOLO has information about the goal,
i.e., the attractor $\xi^{a}$ of our nominal DS. A video of the experiments is
available at 777www.To Be Uploaded.com.

###  VIII-A Static Environment

We task QOLO to navigate in an office-like environment. The room is a square
($5\text{\,}\mathrm{m}$ x $5\text{\,}\mathrm{m}$), modeled as a boundary
obstacle. Further, two tables are located in the room, one at the side and one
at the center. The robot starts from the bottom left, and the attractor
$\xi^{a}$ is placed at the opposite side of the room (illustrated with a
star). All objects, including the wall, are static and known a priori, and the
localization is performed using the SLAM algorithm. The robot evaluates the
modulated avoidance in real-time. We run the following two scenarios:

  1. 1.

QOLO is in the room, and there are two possible paths to go around the center
table. The dynamical system is split by the obstacle at the center (Fig.
17(a)). The robot chooses its preferred trajectory at runtime.

  2. 2.

Additionally there is a (static) person in the room, which blocks the center
passage.
The reference point of the obstacles is automatically placed inside the wall.
The robot finds its path around the obstacles (Fig. 17(b)).

(a) Office with two passages

(b) Office with passage at side

Figure 17: Two static office environments with two tables in a rectangular
room. The center table divides the room into two passages in (a). In (b) one
of the passages is blocked by a static person. Convex expansion of the hull
around the reference point (black cross) ensures convergence towards the
attractor (black star). The path followed by the agent is visualized in red.

###  VIII-B Dense Crowd (Simulation)

The robot is navigating in a corridor within a dense, simulated crowd. The
motion of the crowd is created according to [45]. Two hundred people are
moving uniformly along the $6\text{\,}\mathrm{m}$ wide corridor, either in the
same or opposite direction as the robot.
QOLO is tasked to travel from one end of the corridor to the other. It is
guided by the attractor of the nominal DS. All pedestrians are modeled as
circular obstacles with radius of $0.6\text{\,}\mathrm{m}$ (Fig. 19(a)).
At each timestep, the problem is reduced to avoiding a subset of the
pedestrians. Due to the crowd’s density, the robot could realistically
perceive only a subset of the pedestrians in real-time. The number of
perceived people is set to $N^{c}=10$. The rest of the people is hidden behind
a virtual, circular wall. The center of the circular wall $\xi^{c,w}$ is
displaced from the position of the robot $\xi^{Q}$ based on the remaining
obstacles:

$\xi^{c,w}=\xi^{Q}+\sum_{i=N^{c}+1}^{N^{\mathrm{obs}}}\frac{\xi^{c}_{i}-\xi^{Q}}{\|\xi^{c}_{i}-\xi^{Q}\|}e^{-(\|\xi^{c}_{i}-\xi^{Q}\|-r^{p}-r^{Q})}$
(43)

where $i$ is iterating over the list of the obstacle which are ordered based
on their distance to the robot. The displacement factor is with respect to the
radius of each pedestrian, $r^{p}=$$0.6\text{\,}\mathrm{m}$, and the robot
radius, $r^{Q}=$$0.5\text{\,}\mathrm{m}$.888For a real-world implementation
sensory distance measurements in the horizontal plane can be used to create
the virtual circular wall and its displacement, since the detection of people
is still a time intensive task.
The radius of the hull is chosen such that the next closest obstacle $N^{c}+1$
is fully within the hull. The resulting environment has a dynamic hull with
changing center-position and radius (Fig. 19).
Reducing the environment to only sphere obstacles decreases the computational
time since there is a closed-form solution for the closest distance between
two spheres. The evaluation on ROS1999https://www.ros.org/ and Python 2.7 run
at around $200\text{\,}\mathrm{H}\mathrm{z}$ on a Up Board: Intel Celeron
N3350 with $2.4\text{\,}\mathrm{G}\mathrm{H}\mathrm{z}$ (CPU) and
$8\text{\,}\mathrm{G}\mathrm{B}$ of RAM. The number of nearby
obstacles(including the wall) was eleven, while the wall remained far from the
agent, it helped guide the robot around the local crowd (see Sec. V-D2). This
is done by placing a reference point inside the wall if a crowd-cluster is
touching the wall (see small cluster at the bottom in Fig. 19(b)). Further,
fast contraction of the boundary can happen when the local crowd density is
high. This forces the obstacle to stay away from surrounding obstacles.

Figure 18: QOLO moving in a crowd.

(a) All crowd agents

(b) Local Obstacle Hull

Figure 19: The environment with many agents (left) is reduced to a scenario
with 10 obstacles and an enclosing hull (right).

####  VIII-B1 Quantitative Analysis

We evaluate the effect of the crowd size on the time it takes for the robot to
travel through the corridor. The crowd moves along the (infinite) corridor at
an average velocity of $1\text{\,}\mathrm{m}\text{\,}{\mathrm{s}}^{-1}$. The
simulation runs with a steady-state crowd-flow. QOLO is tasked to move in the
same or opposing direction crowd’s flow with the desired velocity of
$1\text{\,}\mathrm{m}\text{\,}{\mathrm{s}}^{-1}$.
We assess the time, speed, and distance traveled by the robot when moving with
and opposite to the flow, see Fig. 20. When moving with the flow (parallel-
flow), the crowd has no significant effect on the distance traveled by the
agent or the velocity. When moving against the flow of the crowd, a decrease
of the robot’s velocity can be observed for crowds denser than 20 agents per
1000 square meters. (No effect on the crowd was observed since only a single
robot was moving against a large crowd.) The distance traveled increases
significantly for densities above 100 agents per 1000 square meters. As a
result, the average time needed to reach the goal more than doubles for a
crowd size of 100 people compared to 2 people.
In counterflow scenarios, the standard deviation of the flow increases. This
results from situations where the robot has to slow down or stop to avoid the
upcoming agents.

Figure 20: The QOLO agent is moving in parallel (red) and opposite direction
(blue) to the crowd. When the robot moves with the crowd, the density of the
crowd has a negligible effect. When the robot moves in the opposite direction,
the denser the crowd, the larger the cumulative distance (D) and mean velocity
(V), as well as time (T), needed to reach the end of the corridor. The
standard deviation of the velocity (V Variance) increases for dense crowds
with counterflow

###  VIII-C Proof of concept: Outdoor Environment

A qualitative proof of concept was performed in an outdoor environment. We
brought the QOLO robot to the center of Lausanne, Switzerland
city101010Approval was obtained from the EPFL Ethics board and the police of
Lausanne city. A driver was on board the robot during the experiment. He could
start and stop at all times. A second experimenter was watching the scene and
verified the output of the tracker. This was necessary in case the
detector/tracker dis-functioned.. The robot was tasked to travel back and
forth across a small marketplace (Fig. 21). The location is restricted to
pedestrians only, and a total of six streets meet at the crossing. This
results in a large diversity in both the pedestrians’ speed and direction of
movement. The robot’s controller is initialized with a linear DS to reach a
goal $20\text{\,}\mathrm{m}$ away from the onset position. Pedestrians are
detected with a camera and Lidar-based tracker developed by the authors of
[39]. The output of the tracker is displayed in Fig. 22. Recordings were taken
on Saturday morning when the market was ongoing, and the crowd had a high
density.
The non-holonomic constraints of QOLO are taken into account by evaluating the
dynamical system $0.53\text{\,}\mathrm{m}$ in front of the center of the
wheel-axes. The linear command of the robot is the velocity part in the moving
direction. The angular velocity follows the perpendicular part of the
velocity. These velocities are provided to the low-level controller of the
robot.
The geometry of QOLO is taken into account by placing a margin of
$0.5\text{\,}\mathrm{m}$ around each pedestrian.
A total of five runs were executed. The robot reached its goal autonomously
without intervention. The driver reported high angular acceleration during
parts of the trip.
Post-hoc analysis of the video recordings revealed that the crowd density
varied with a mean between 150 and 260 people per 1000 square meters (Fig.
23). The time to complete the runs ranged from
$115\text{\,}\mathrm{s}150\text{\,}\mathrm{s}$. No correlation was observed
between the density of the crowd and the time taken to reach the goal. We
expect this to result from external factors influencing the run, such as the
speed and direction of the crowd.
We see this as a successful proof-of-concept of the obstacle avoidance
algorithm in a real crowd scenario. The crowd motion was more complex than the
streamline simulation, as people would come from all directions and would not
group in a uniform flow. Moreover, the crowd included diverse pedestrians,
from families with small children to elderly people.

Figure 21: The desired path of the robot in the outdoor environment is the
direct line from the initial position of the robot (right) to the target
position on the left.

(a) Camera view

(b) Detector interpretation

Figure 22: The camera (a) and the LIDAR of the robot are interpreted by the
detector (b), which is used for the obstacle avoidance algorithm. Figure 23:
The crowd-density is highly varying during the five runs.

##  IX Discussion

In this work, we have introduced a Dynamical System-based obstacle avoidance
algorithm. The modulation-based approach has a theoretical proof of
convergence and can be applied to higher-dimensional space. The
implementations presented in this work focus on collision avoidance in two
dimensions, such as navigation of mobile robots and obstacle avoidance for
simplified robot arms.
The inverted obstacle has shown to be a great representation of workspace
boundaries of mobile robots, such as walls of a room or the local window in
dense crowd navigation. These boundaries could be interpreted as control
constraints similar to control barrier functions (CBFs). Recent works have
used such control barrier function in the context of safe learning by
demonstration [46, 47] or reinforcement learning [48]. Other than existing
methods, we propose a closed-form solution for star-shaped barriers.
Since many human-made environments contain nonsmooth surfaces (e.g., tables,
corners of rooms), the solution for providing a smooth flow without extending
the hull has shown suitable for practical implementations.
The concept of dividing dynamical systems into direction and magnitude and the
presented method summing vectors to avoid local minima has been used
throughout: (1) to create a smooth pseudo-normal for polygon obstacles and
walls and from that a smooth flow in their presence, (2) to sum the flow
created by several obstacles without creating local minima, and (3) to solve
the optimization problem to find the closest distance for pairs of obstacles.
Finally, the algorithm has been successfully applied to a non-holonomic robot
in a static indoor environment and dynamic outdoor crowds.

###  IX-A Scalability and Speed

The implementation used for the experiments on QOLO was running on Python 2.7.
Note that already the update to Python >3.6 gives around a 1.5-speed
improvement. Additionally, switching to a compiled language like C++ can
increase the speed by a factor of around 10 [49]. We estimated that our
algorithm could run at a frequency of
>$1\text{\,}\mathrm{k}\mathrm{H}\mathrm{z}$ onboard a mobile robot. This is
well above the human-reaction time of around
$250\text{\,}\mathrm{m}\mathrm{s}$.
The bottleneck of the current implementation is the image recognition/tracking
since it was only able to run at an average frequency of around
$5\text{\,}\mathrm{H}\mathrm{z}$. This is due to the computationally heavy
evaluation of the deep-neural networks used for obstacle recognition.
Nevertheless, we believe that the approach of separating perception and
motion-planning is favorable, supported by current trends in self-driving cars
and autonomous vehicles [50, 51].

###  IX-B Contribution

The proposed method adds value to the current state-of-the-art, not for global
path planning but for reactive obstacle avoidance with convergence proofs. The
modulation based avoidance algorithm fully converges in (local) star-shaped
environments towards the desired attractor $\xi^{a}$. The behavior is similar
to approaches using potential artificial fields. However, the presented
approach does not require finding and deriving an artificial potential
function, but the modulation directly outputs the desired velocity.
Compared to other closed-form and QP-based obstacle avoidance algorithms, our
method does not require any tuning of critical parameters for its convergence,
nor the finding of a Lyapunov candidate function. All parameters presented in
this paper can be chosen within the defined range, and theoretical convergence
is ensured.
In the presented paper (and initially introduced in [37]), we presented
several methods of how to place the reference point (i.e., tune its position).
Even though its position is critical for convergence, it is known for star-
worlds:

  * •

Convex Obstacles: The reference point can be placed anywhere within the
obstacle, i.e. $\xi^{r}\in\mathcal{X}^{i}$.

  * •

Star-shapes: The reference point has to be placed within the kernel of the
obstacle. Most of the time, this is just the geometrical center of the
obstacle. For polygons, an algorithm to find the kernel has been described in
[38].

  * •

Intersecting Convex Obstacles If several convex obstacles intersect and form a
star-shape, the reference point can be placed anywhere at the center of all
intersecting obstacles $o$, i.e., $\xi^{r}\in\mathcal{X}^{i}_{o}\;\;\forall
o$.
If the convex obstacles do not form a star-shape or are intersecting with the
boundary, the hull can be extended dynamically, as described in Sec. V-D.

The placement of the reference point only becomes challenging when obstacles
merge or separate dynamically. While we propose approximations for many cases
(see Sec. -A2 and Sec. -A3), we do not provide a solution for all scenarios.
To the best of the authors’ knowledge, there currently exists no closed-form
method to generate a flow around merging and dividing obstacles, which has
convergence guarantees. The placement of the reference point for such dynamic
scenarios is ongoing research.

###  IX-C Future Work

Future work can extend the proposed work in the following areas:

  * •

Low-level controller: The low-level controller used in crowd navigation
displaced the evaluation point away from the center of the robot. This
resulted in an increased (conservative) margin around the robot. The way the
shape of the robot is considered in the algorithm should be improved.

  * •

Environment recognition: The update rate of the (deep-learning-based) tracker
was approximately 5 Hz, while the present avoidance algorithm ran at a
frequency of 50 Hz to 100 Hz. The obstacle avoidance was often evaluated with
old environment information (but updated robot position). An intermediate
estimator could predict how the crowds move in-between.

  * •

High-level planning: The combination of the fast obstacle avoidance controller
with slower planning algorithms could allow to handle more complex
environments, i.e., including the avoidance of surrounding (non-star-shaped)
environments.

  * •

Evaluation in high dimensional space: The experimental implementation is
executed on an autonomous wheelchair (a two-dimensional scenario). However,
this work provides a theoretical solution, which can be applied to three and
higher dimensional spaces. The next step will be the implementation and
evaluation in a higher-dimensional space.

##  X Conclusion

A dynamical system-based algorithm for local navigation under convergence
constraint is presented. The work provides and tests the solution for local
crowd navigation. It ensures certain convergence constraints to not only
safely navigate but also reach the goal in local scenarios. The advantage of
the method comes from the low complexity and speed of the algorithm.
Furthermore, tuning free convergence is obtained in star-world scenarios. This
will allow to scale to higher dimensions and transfer to various scenarios.

##  XI ACKNOWLEDGEMENT

We would like to thank the support of Diego Paez-Granados and David Gonon for
the running of the experiments. Their effort and insight have contributed to
the implementation using QOLO and the qualitative analysis of the results.

###  -A Directional Weighted Mean

The weighted summation of vectors can result in a zero-sum (e.g., two vectors
opposing each other with equal weight) and lead to undesired local-minima of
dynamical systems.
We extend here the directional weighted summing introduced in [37] for more
general application. It ensures that the summed vector field is free of local
minima.

$\\{\mathbf{v}\in\mathbb{R}^{d}\,:\,\|\mathbf{v}\|=1\\}$ (44)

The transformation into direction space $\mathcal{K}$ is given by:

$\mathcal{K}=\\{\kappa\in\mathbb{R}^{d-1}\;:\;\|\kappa\|<\pi\\}$ (45)

The direction space is with respect to a base vector $\mathbf{b}$ which is the
first column of the orthonormal transformation matrix $\mathbf{B}$. This
allows the transformation into the new basis:

$\hat{\mathbf{v}}_{i}=\mathbf{B}^{T}\mathbf{v}_{i}$ (46)

The magnitude of the transformed vector in direction space is equal to the
angle between the original vector and the reference vector. The transformation
of the initial vector $\mathbf{v}_{i}$ in the direction-space is:

$\kappa_{i}(\mathbf{b})=\mathbf{k}(\mathbf{v}_{i},\mathbf{b})=\begin{cases}\arccos\left(\hat{\mathbf{v}}_{i[1]}\right)\frac{\hat{\mathbf{v}}_{i[2:]}}{\|\hat{\mathbf{v}}_{i[2:]}\|}&\text{if}\;\;\hat{\mathbf{v}}_{i[1]}\neq
1\\\ \mathbf{0}&\text{if}\;\;\hat{\mathbf{v}}_{i[1]}=1\end{cases}$ (47)

The mean is evaluated as a function of the weight $w_{i}$ of all $N^{v}$
vectors:

$\bar{\kappa}=\sum_{i=1}^{N^{v}}w_{i}\kappa_{i}$ (48)

The mapping into original space is evaluated as:

$\bar{\mathbf{v}}(\bar{\kappa})=\begin{cases}\mathbf{B}\begin{bmatrix}1&0&..&0\end{bmatrix}^{T}&\text{if}\;\|\bar{\kappa}\|=0\\\
\mathbf{B}\begin{bmatrix}\cos\left(\|\bar{\kappa}\|\right)&\sin\left(\|\bar{\kappa}\|\right)\frac{\bar{\kappa}}{\|\bar{\kappa}\|}\end{bmatrix}^{T}&\text{otherwise}\end{cases}$
(49)

####  -A1 Intuition

In the two-dimensional case, this hyper-sphere is a line that represents the
angle between the initial DS $\mathbf{f}(\xi)$ and the modulated DS
$\dot{\xi}$. It has a magnitude strictly smaller than $\pi$, the directional
space is a vector space, where the weighted mean is taken (see for the three
dimensional case in Fig. 24).

(a) Three initial vectors.

(b) Direction-space

Figure 24: Various directions (here three) are described with respect to a
basis direction $\mathbf{r}^{0}$ (a). The directions are transformed to the
direction-space $\mathcal{K}$ (b) where the weighted mean, $\bar{\kappa}$, is
obtained.

Theorem A Consider a unit vector $\mathbf{b}$ as the basis for the projection
given in (47) and the corresponding reconstruction function defined in (49).
The resulting transformation of unit vector
$\mathbf{k}(\mathbf{v},\mathbf{b})\;:\;\\{\mathbf{v}\in\mathbb{R}^{d}\setminus-\mathbf{b}\;\;:\;\;\|\mathbf{v}\|=1\\}\rightarrow\mathcal{K}$
defined in (45) is a bijection and the basis vector projects to the origin,
i.e., $\mathbf{b}\rightarrow\mathbf{0}$.

Proof:  The proof is divided into three parts: (I) showing that transformation
and reconstruction are the inverse functions of each other, (II) any unit
vector is transformed to the direction space $\mathcal{K}$, and (III) is
reconstructed to a unit vector.

(I) Inverse Functions: To be the inverse functions, applying one after the
other onto a vector, must results in the original vector. (Vectors
$\mathbf{b}$ will be treated separately below).
Let us apply the forward transformation based on (46) and (47) to a unit
vector $\mathbf{v}_{1}$. It can be summarized to:

$\displaystyle\kappa(\mathbf{b})=\arccos\left(\langle\mathbf{b},\mathbf{v}_{1}\rangle\right)\frac{{\hat{\mathbf{B}}}^{T}\mathbf{v}_{1}}{\|\hat{\mathbf{B}}{\mathbf{v}}_{1}\|}$
(50)

with $\hat{\mathbf{B}}$ the matrix $\mathbf{B}$ without the first row.
We apply it to a single vector, hence the corresponding weight is $w_{1}=1$.
From (48), we get $\bar{\kappa}=\kappa_{1}$. The reconstruction follows with
(49):

$\displaystyle\bar{\mathbf{v}}$
$\displaystyle=\mathbf{B}\begin{bmatrix}\cos\left({\|\bar{\kappa}\|}\right)\\\
\sin\left(\|\bar{\kappa}\|\right)\frac{\bar{\kappa}}{\|\bar{\kappa}\|}\end{bmatrix}=\mathbf{B}\begin{bmatrix}\cos\left(\arccos\left(\langle\mathbf{b},\mathbf{v}_{1}\rangle\right)\right)\\\
\frac{\sin(\arccos\left(\langle\mathbf{b},\mathbf{v}_{1}\rangle)\right)\left(\hat{\mathbf{B}}\right)^{T}{\mathbf{v}}_{1}}{\|\left(\hat{\mathbf{B}}\right)^{T}{\mathbf{v}}_{1}\|}\end{bmatrix}$
$\displaystyle=\mathbf{B}\begin{bmatrix}\langle\mathbf{b},\mathbf{v}_{1}\rangle\\\
{\left(\hat{\mathbf{B}}\right)^{T}{\mathbf{v}}_{1}}\end{bmatrix}=\mathbf{B}\left({\mathbf{B}}\right)^{T}{\mathbf{v}}_{1}=\mathbf{v}_{1}$

by using

$\sin(\arccos\left(\langle\mathbf{b},\mathbf{v}_{1}\rangle)\right)=\sqrt{1-\langle\mathbf{b},\mathbf{v}_{1}\rangle^{2}}\\\
=\sqrt{\|\mathbf{B}\mathbf{v}_{1}\|^{2}-\langle\mathbf{b},\mathbf{v}_{1}\rangle^{2}}={\|\left(\hat{\mathbf{B}}\right)^{T}{\mathbf{v}}_{1}\|}$

For the case that $\mathbf{v}_{1}=\mathbf{b}$, we get from (47) that
$\bar{\kappa}=\kappa_{1}=\mathbf{0}$. And with (49, we get
$\bar{\mathbf{v}}(\bar{\kappa})=\mathbf{b}$, i.e., the original vector.
Hence for all cases, the transformation is bijective.

(II) Transformation Domain From definition of the transform in (47), the
maximum magnitude is the $\arccos$, which is $\|\kappa_{i}\|<\pi$. Hence it
lies in the domain of (45). (The magnitude $\pi$ is only reached for a vector
$-\mathbf{b}$, which is excluded from the transform).

(III) Reconstruction Domain From the inverse transform (49), we have that norm
of any transformed vector $\|\bar{\mathbf{v}}(\bar{\kappa})\|=1$, this follows
from the fact that $\mathbf{B}$ is orthonormal. As a result they all lie in
the domain (44).  $\blacksquare$

####  -A2 Pairwise Closest Distance in Direction Space

The directional space can be used for gradient descent to find the closest
distance between two (convex) obstacles. This is done by moving along the
surface of the obstacle in direction space. Since the direction space is of
dimension $d-1$, finding the closest point of each obstacle has only $2(d-1)$
degrees of freedom (instead of $2d$ in the Cartesian space). The problem
converges to the global minimum when the points start on the line which
connects the two center points.
The optimization problem is given as:

$\min_{\Phi}\,{\mathbf{f}\,}^{b}(\Phi)\qquad\text{with}\;\;{\mathbf{f}\,}^{b}(\Phi)=\|\mathbf{\xi}^{b}_{1}(\phi_{1})-\mathbf{\xi}^{b}_{2}(\phi_{2})\|\,,\;\;\Phi=\begin{bmatrix}\phi_{1}\\\
\phi_{2}\end{bmatrix}$

where the $\xi^{b}_{i}(\phi_{i})$ denotes the boundary point in the direction
$\phi_{i}$ for the obstacle $i$ with respect to its reference point
$\xi^{r}_{i}$. The direction space of each obstacle is created such that the
null-direction points towards the other obstacles center. An example is
visualized in Fig. 25.

(a) Minimum Distance

(b) Value Function

Figure 25: The minimum distance problem for a rectangular object (with margin)
and an ellipsoid. The boundary-reference-point which corresponds to the closes
point is marked in red (a) and the corresponding gradient descent problem in
direction space (b).

####  -A3 Intersecting Obstacle Descent

Two intersecting obstacles need to share one reference point, which lies
within both obstacles. The simplification of the problem to surface points
only is not of use anymore. Hence, the optimization problem is evaluated in
Cartesian space to find a common point that lies inside of the two obstacles’
boundaries:

$\min_{\xi\in\mathcal{X}_{1}^{b}\cap\mathcal{X}_{2}^{b}}\,\mathbf{f}^{i}(\mathbf{\xi})\qquad\text{with}\;\;\mathbf{f}^{i}(\mathbf{\xi})=\frac{\Gamma^{b}}{\Gamma^{b}-\Gamma_{1}(\xi)}+\frac{\Gamma^{b}}{\Gamma^{b}-\Gamma_{2}(\xi)}$

with $\Gamma^{b}$ the base distance value, i.e. where the value function
reaches infinity. It is chosen slightly larger than the $\Gamma$-value on the
surface, we simply choose $\Gamma^{b}=1.1$. The step size can be optimized
based on the gradient. The optimization problem is convex, and points starting
within the intersection region will stay inside due to the infinite repulsion
at the boundary as $\Gamma\rightarrow 1$. The value function of two ellipses
can be found in Fig. 26.

Figure 26: The individual $\Gamma$-function from the obstacle in (a) and (b)
are summed as described in (-A3). The resulting value function (c) is used for
the angle based gradient-descent.

####  -A4 Closest Distance for Mixed Environments

The above method for gradient descent in directional space to find the closest
distance between two objects, can be applied to an object-boundary pair, if
obstacle’s curvature $c^{o}$ is larger than boundary’s curvature $c^{b}$ at
any position:

$c^{o}(\xi_{1})<c^{b}(\xi_{2})\qquad\forall\,\xi_{1},\,\xi_{2}$ (51)

with the local curvature being defined in (32).
Note that for non-circular obstacles, the condition might locally not hold
mainly if the space contains polygon obstacles with local flat regions
($c=0$). This can lead to a locally non-optimal solution when placing the
reference point based on the closest distance.

###  -B Proof of Theorem 1

####  -B1 Applicability of General Proofs

In [37] convergence has been proven for star-shaped obstacles. The proof was
developed based on the distance function $\Gamma(\xi)$. Due to inverting the
distance function for enclosing wall obstacles and a continuous definition of
the modulation, the proof of star-shaped obstacles applies to the case of
enclosing walls.

####  -B2 Continuity Across Reference Point

In Sec. III-A the Inverted distance function $\Gamma^{w}(\xi)$ was not defined
at the reference point, as it reaches an infinite value. The continuous
definition for the eigenvalue is a unit value, i.e.
$\lambda^{e}(\xi^{r})=\lambda^{r}(\xi^{r})=1$, it follows that the diagonal
matrix is equal to the identity matrix $D(\xi^{r})=I$. As shown in (1423), we
get:

$\xi=\xi^{r}\;\;\rightarrow\;\;\dot{\xi}=\mathbf{E}\,\mathbf{D}\,\mathbf{E}^{-1}\mathbf{f}(\xi^{r})=\mathbf{E}\,\mathbf{I}\,\mathbf{E}^{-1}\mathbf{f}(\xi^{r})=\mathbf{f}(\xi^{r})$
(52)

i.e. no modulation of the initial DS. In fact, this is equivalent to the case
far away for a classical obstacle with
$\lim_{\|\xi-\xi^{r}\|\rightarrow\infty}\Gamma^{o}(\xi)\rightarrow\infty$.
Even though the basis matrix $\mathbf{E}(\xi)$ is not defined at $\xi^{r}$,
the DS is continuously defined across this point since the modulation has no
effect.
The trajectory that traverses the reference point $\xi^{r}$ of the inverted
obstacle corresponds to the trajectory that gets stuck in a saddle point for a
common obstacle. As a result, there is full convergence for the inverted
obstacles.

###  -C Proof of Theorem 2

We show first that the modulation has full rank and hence that the dynamics
does not vanish outside the attractor and that it is smooth.

####  -C1 Full Rank

The basis matrix from (21) has full rank everywhere outside of the obstacle,
if the following condition holds:

$\arccos\left(\langle\mathbf{r}(\xi),\,\hat{\mathbf{n}}(\xi)\rangle\right)<\pi/2$
(53)

The angle between the normal to each surface i, $\mathbf{n}_{i}(\xi)$ and the
reference direction $\mathbf{r}(\xi)$ can be evaluated by defining an vector
$\tilde{\mathbf{n}}_{i}(\xi)=\mathbf{n}_{i}(\xi)+\sum_{j=1}^{d-1}k^{e}_{i}\mathbf{e}_{j}(\xi)$
with $\langle\mathbf{e}_{j}(\xi),\,\mathbf{r}(\xi)\rangle=0$,
$k^{e}_{i}\in\mathbb{R}$ such that
$\mathbf{p}_{i}^{s}(\xi)+\tilde{\mathbf{n}}_{i}(\xi)$ intersects with
$\xi^{r}+k^{r}\mathbf{r}(\xi)$ at $\mathbf{q}^{s}_{i}(\xi)$ with
$k^{r}\in\mathbb{R}$ (Fig. 27).
This allows to create a triangle spanned by the lines $\xi$,
$\mathbf{p}_{i}^{s}(\xi)$ and $\mathbf{q}^{s}_{i}(\xi)$, colored in blue in
Fig. 27. Using the associative law of the dot product, the geometry constraint
of the blue triangle and (19), the maximum angle results in:

$\langle\mathbf{n}_{i},\mathbf{r}\rangle=\langle\tilde{\mathbf{n}}_{i},\mathbf{r}\rangle\geq\langle\xi-\mathbf{p}_{i}^{s},\mathbf{r}\rangle\geq
0\quad\forall\,w_{i}(\xi)>0$ (54)

Figure 27: Visualization of variables used for the weighted directional mean.

Hence the directional transformation of (47) results in
$\|\kappa_{i}\|<\pi/2\|,\;\forall\,w_{i}(\xi)>0$. Using additionally the
triangle equality for vectors:
$\|\kappa_{1}+\kappa_{2}\|\leq\|\kappa_{1}\|+\|\kappa_{2}\|$ applied to all
surface directions, it follows with (48) that:

$\displaystyle\|\bar{\kappa}\|$
$\displaystyle=\|\sum_{i=1}^{N^{v}}w_{i}\kappa_{i}\|\leq\sum_{i=1}^{N^{v}}w_{i}\|\kappa_{i}\|\leq\left(\sum_{i=1}^{N^{v}}w_{i}\right)\|\max_{i\;\text{with}\;w_{i}>0}\kappa_{i}\|\leq\frac{\pi}{2}$

Since the basis vector of the directional mean is $\mathbf{r}(\xi)$, with (49)
condition (53) holds true.

####  -C2 Smooth Vector Field

The continuous extension across the reference point is defined in Appendix -B2
and applicable, too.
The reference direction $\mathbf{r}(\xi)$ and the distance function
$\Gamma(\xi)$ does not have any other discontinuity.
The pseudo normal $\hat{\mathbf{n}}(\xi)$ is smoothly defined across space.
Even in the case when the edge point with the minimum is switching (17), no
discontinuity occurs since the angle will stay the same due to the flat
surface.

####  -C3 Applicability of General Proofs

Since we have a smooth field of normal vectors $\mathbf{n}(\xi)$, we further
need to define any smooth distance function which decreases its value with
increasing distance. The two properties are sufficient to comply with the
proof of Sec. -B $\blacksquare$

###  -D Proof Theorem 3

As an agent approaches the surface of an obstacle, the importance weight from
this obstacle is approaching one, i.e.,
$\lim_{\Gamma_{\hat{o}}(\xi)\rightarrow 1}w_{\hat{o}}=1$, see (V). It follows
with (24) that the relative velocity is
$\dot{\tilde{\xi}}=\dot{\tilde{\xi}}^{\mathrm{tot}}=\dot{\tilde{\xi}}_{\hat{o}}$.
As a result it is sufficient to analyze impenetrability for each obstacle
individually. The next step is to ensure impenetrability of the three cases in
(30):

####  -D1 Evaluation in Moving Frame:

$\dot{\xi}=\dot{\xi}$
The simplest case comes with no stretching, but the evaluation in the local
frame of the moving boundary of the obstacle. It follows that the Neuman-
boundary condition for impenetrability holds (see also [37]).

####  -D2 Contraction within Margin

$\dot{\xi}=v^{\mathrm{max}}\|\dot{\xi}\|\dot{\xi}$
This contraction is only performed, if it results in a normal velocity which
is larger than the velocity of the obstacle $\dot{\tilde{\xi}}$, i.e. the
evaluation in the moving frame results
$\langle(\dot{\xi}-\dot{\tilde{\xi}},\mathbf{n}(\xi)\rangle\geq 0$, hence
ensuring impenetrability.

####  -D3 Contraction in Tangent Direction

$\dot{\xi}=v^{n}\mathbf{n}(\xi)+\sqrt{\left(v^{\mathrm{max}}\right)^{2}-\|\dot{\xi}^{n}\|^{2}}\;\mathbf{e}(\xi)$

This limited contraction along the normal direction ensures that the velocity
in normal direction remains equal to the obstacles’ velocity. The evaluation
of the Neuman boundary condition in the moving frame leads to:

$\langle(v^{n}\mathbf{n}(\xi)+\sqrt{\left(v^{\mathrm{max}}\right)^{2}-\|\dot{\xi}^{n}\|^{2}}\;\mathbf{e}(\xi))-\tilde{\xi},\mathbf{n}(\xi)\rangle=v^{n}-v^{n}=0$

using the definition of (29) and the fact that the normal $\mathbf{n}(\xi)$
and the tangent $\mathbf{e}(\xi)$ are orthogonal.

Hence, we have impenetrability for multiple obstacles.
$\blacksquare$

###  -E Proof Theorem 3

As an agent approaches the surface of an obstacle, the importance weight from
this obstacle is approaching one, i.e.,
$\lim_{\Gamma_{\hat{o}}(\xi)\rightarrow 1}w_{\hat{o}}=1$, see (V). It follows
with (24) that the relative velocity is
$\dot{\tilde{\xi}}=\dot{\tilde{\xi}}^{\mathrm{tot}}=\dot{\tilde{\xi}}_{\hat{o}}$.
As a result it is sufficient to analyze impenetrability for each obstacle
individually. The next step is to ensure impenetrability of the three cases in
(30):

####  -E1 Evaluation in Moving Frame:

$\dot{\xi}=\dot{\xi}$
The simplest case comes without stretching, but the evaluation in the local
frame of the moving boundary of the obstacle. It follows that the Neuman-
boundary condition for impenetrability holds (see also [37]).

####  -E2 Contraction within Margin

$\dot{\xi}=v^{\mathrm{max}}\|\dot{\xi}\|\dot{\xi}$
This contraction is only performed, if it results in a normal velocity which
is larger than the velocity of the obstacle $\dot{\tilde{\xi}}$, i.e. the
evaluation in the moving frame results
$\langle(\dot{\xi}-\dot{\tilde{\xi}},\mathbf{n}(\xi)\rangle\geq 0$, hence
ensuring impenetrability.

####  -E3 Contraction in Tangent Direction

$\dot{\xi}=v^{n}\mathbf{n}(\xi)+\sqrt{\left(v^{\mathrm{max}}\right)^{2}-\|\dot{\xi}^{n}\|^{2}}\;\mathbf{e}(\xi)$

This limited contraction along the normal direction ensures that the velocity
in normal direction remains equal to the obstacles’ velocity. The evaluation
of the Neuman boundary condition in the moving frame leads to:

$\langle(v^{n}\mathbf{n}(\xi)+\sqrt{\left(v^{\mathrm{max}}\right)^{2}-\|\dot{\xi}^{n}\|^{2}}\;\mathbf{e}(\xi))-\tilde{\xi},\mathbf{n}(\xi)\rangle=v^{n}-v^{n}=0$

using the definition of (29) and the fact that the normal $\mathbf{n}(\xi)$
and the tangent $\mathbf{e}(\xi)$ are orthogonal.

Hence, we have impenetrability for multiple obstacles.
$\blacksquare$

###  -F Evaluation Weights for a Robot Arm

We introduce link weights $w_{l}^{L}$ for each link $l\in[1..N^{L}]$, and
section weights $w_{l,s}^{S}$ for each section point $\xi_{l,s}^{S}$ with
$s\in[1,..N^{S}]$. The weights determine how much the obstacle avoidance
command influences the control, i.e., zero weights indicate following the goal
velocity only, whereas a weight of 1 means full avoidance at this specific
position.
These weights are designed, such that their product is smaller than one, i.e.:

$0\leq\sum_{l}\left(w^{L}_{l}\sum_{s}w^{S}_{l,s}\right)\leq 1$

Furthermore, if a section point approaches the surface of an obstacle, it
should dominate, i.e.:

$\Gamma^{d}(\xi_{l,s}^{S})\rightarrow 1\quad\Rightarrow\quad
w_{l}^{L}w_{l,s}^{S}\rightarrow 1$

And when the robot arm is far away from any surface, all weights should go to
zero:

$\min_{l,s}\left(\Gamma(\xi_{l,s}^{S})\right)\rightarrow\infty\quad\Rightarrow\quad\sum_{l}\left(w^{L}_{l}\sum_{s}w^{S}_{l,s}\right)\rightarrow
0$

####  -F1 Base Weight

For each link $l$ and section point $s$, we define a $\Gamma$-danger weight
$w^{\Gamma}(\Gamma^{d}):\left]1,\infty\right[\rightarrow\left]\infty,0\right[$
to represent the danger of colliding for a specific point: the higher the
weight, the greater the chance of collision. It is defined as:

$w^{\Gamma}(\Gamma^{d})=\begin{cases}\frac{\Gamma^{c}-\Gamma^{\mathrm{min}}}{\Gamma^{d}(\xi_{s,l}^{S})-\Gamma^{\mathrm{min}}}-1&\text{if}\;\;\quad\Gamma^{d}(\xi_{s,l}^{S})<\Gamma^{c}\\\
0&\text{otherwise}\end{cases}$ (55)

where $\Gamma^{\mathrm{min}}=1$ is the lower bound, $\Gamma^{c}>1$ the cutoff
value and $\Gamma^{d}(\xi)$ defined in 34.

####  -F2 Link Weights

The influence of each link $l$ is evaluated as:

$\hat{w}^{L}_{l}=c^{L}|\dot{\mathbf{q}}^{g}_{[l]}|\frac{l}{N^{L}}\max_{s\in[1..N^{S}]}w^{\Gamma}(\xi_{s,l}^{S})\quad\forall\,l\in[1..N^{L}]$
(56)

where $c^{L}\in\mathbb{R}_{>0}$ is a constant link-weight factor and
$\dot{\mathbf{q}}^{g}_{[l]}$ the $l$-th element of the goal vector defined in
(33). The $l$-factor in the equation gives an increasing importance for links
closer to the end-effector.
The link weights are obtained through normalization:

$w^{L}_{l}=\begin{cases}\hat{w}^{L}_{l}/\hat{w}^{\mathrm{sum}}\quad&\text{if}\;\hat{w}^{\mathrm{sum}}>1\\\
\hat{w}^{L}_{l}&\text{otherwise}\end{cases}\;\;\;\text{with}\;\;\hat{w}^{\mathrm{sum}}=\sum_{l=1}^{N^{L}}\hat{w}^{L}_{l}$
(57)

####  -F3 Section Weights

For all links $l$ with $w_{l}^{L}>0$, the influence weight of each section
point is evaluated as:

$w^{S}_{s}=\frac{\hat{w}^{S}_{s}}{\sum_{s=1}^{N^{S}}\hat{w}^{S}_{s}}\quad\text{with}\quad\hat{w}^{S}_{s}=\frac{s}{N_{S}}w^{\Gamma}(\xi_{l,s}^{S})\quad\forall\;s\in[1..N_{S}]$
(58)

## References

  * [1] Hans Jacob S Feder and J-JE Slotine “Real-time path planning using harmonic potentials in dynamic environments” In _Robotics and Automation, 1997. Proceedings., 1997 IEEE International Conference on_ 1, 1997, pp. 874–881 IEEE
  * [2] S Mohammad Khansari-Zadeh and Aude Billard “Learning stable nonlinear dynamical systems with gaussian mixture models” In _IEEE Transactions on Robotics_ 27.5 IEEE, 2011, pp. 943–957
  * [3] Lydia E Kavraki, Petr Svestka, J-C Latombe and Mark H Overmars “Probabilistic roadmaps for path planning in high-dimensional configuration spaces” In _IEEE transactions on Robotics and Automation_ 12.4 IEEE, 1996, pp. 566–580
  * [4] Steven M LaValle and James J Kuffner Jr “Rapidly-exploring random trees: Progress and prospects” Citeseer, 2000
  * [5] Oliver Brock and Oussama Khatib “Elastic strips: A framework for motion generation in human environments” In _The International Journal of Robotics Research_ 21.12 SAGE Publications Sage UK: London, England, 2002, pp. 1031–1052
  * [6] Yunfei Zhang, Navid Fattahi and Weilin Li “Probabilistic roadmap with self-learning for path planning of a mobile robot in a dynamic and unstructured environment” In _2013 IEEE International Conference on Mechatronics and Automation_ , 2013, pp. 1074–1079 IEEE
  * [7] John Vannoy and Jing Xiao “Real-time adaptive motion planning (RAMP) of mobile manipulators in dynamic environments with unforeseen changes” In _IEEE Transactions on Robotics_ 24.5 IEEE, 2008, pp. 1199–1212
  * [8] Sean Murray et al. “Robot Motion Planning on a Chip.” In _Robotics: Science and Systems_ , 2016
  * [9] Jie Ji, Amir Khajepour, Wael William Melek and Yanjun Huang “Path planning and tracking for vehicle collision avoidance based on model predictive control with multiconstraints” In _IEEE Transactions on Vehicular Technology_ 66.2 IEEE, 2017, pp. 952–964
  * [10] Xiaojing Zhang, Alexander Liniger and Francesco Borrelli “Optimization-based collision avoidance” In _IEEE Transactions on Control Systems Technology_ 29.3 IEEE, 2020, pp. 972–983
  * [11] Omur Arslan and Daniel E Koditschek “Exact robot navigation using power diagrams” In _Robotics and Automation (ICRA), 2016 IEEE International Conference on_ , 2016, pp. 1–8 IEEE
  * [12] Aaron D Ames, Jessy W Grizzle and Paulo Tabuada “Control barrier function based quadratic programs with application to adaptive cruise control” In _53rd IEEE Conference on Decision and Control_ , 2014, pp. 6271–6278 IEEE
  * [13] Matheus F Reis, A Pedro Aguiar and Paulo Tabuada “Control barrier function-based quadratic programs introduce undesirable asymptotically stable equilibria” In _IEEE Control Systems Letters_ 5.2 IEEE, 2020, pp. 731–736
  * [14] Gennaro Notomista and Matteo Saveriano “Safety of Dynamical Systems with Multiple Non-Convex Unsafe Sets Using Control Barrier Functions” In _IEEE Control Systems Letters_ IEEE, 2021
  * [15] Elon Rimon and Daniel E Koditschek “The construction of analytic diffeomorphisms for exact robot navigation on star worlds” In _Transactions of the American Mathematical Society_ 327.1, 1991, pp. 71–116
  * [16] Jeff Michels, Ashutosh Saxena and Andrew Y Ng “High speed obstacle avoidance using monocular vision and reinforcement learning” In _Proceedings of the 22nd international conference on Machine learning_ , 2005, pp. 593–600 ACM
  * [17] Pinxin Long, Wenxi Liu and Jia Pan “Deep-learned collision avoidance policy for distributed multiagent navigation” In _IEEE Robotics and Automation Letters_ 2.2 IEEE, 2017, pp. 656–663
  * [18] Paolo Fiorini and Zvi Shiller “Motion planning in dynamic environments using velocity obstacles” In _The International Journal of Robotics Research_ 17.7 Sage Publications Sage CA: Thousand Oaks, CA, 1998, pp. 760–772
  * [19] Jur Van den Berg, Ming Lin and Dinesh Manocha “Reciprocal velocity obstacles for real-time multi-agent navigation” In _2008 IEEE International Conference on Robotics and Automation_ , 2008, pp. 1928–1935 IEEE
  * [20] David Wilkie, Jur Van Den Berg and Dinesh Manocha “Generalized velocity obstacles” In _2009 IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2009, pp. 5573–5578 IEEE
  * [21] Oussama Khatib “Real-time obstacle avoidance for manipulators and mobile robots” In _The international journal of robotics research_ 5.1 Sage Publications Sage CA: Thousand Oaks, CA, 1986, pp. 90–98
  * [22] Daniel E Koditschek and Elon Rimon “Robot navigation functions on manifolds with boundary” In _Advances in applied mathematics_ 11.4 Elsevier, 1990, pp. 412–442
  * [23] Elon Rimon and Daniel E Koditschek “Exact robot navigation using artificial potential functions” In _IEEE Transactions on robotics and automation_ 8.5 IEEE, 1992, pp. 501–518
  * [24] Savvas G Loizou “Closed form navigation functions based on harmonic potentials” In _Decision and Control and European Control Conference (CDC-ECC), 2011 50th IEEE Conference on_ , 2011, pp. 6361–6366 IEEE
  * [25] Santiago Paternain, Daniel E Koditschek and Alejandro Ribeiro “Navigation functions for convex potentials in a space with convex obstacles” In _IEEE Transactions on Automatic Control_ IEEE, 2017
  * [26] Savvas G Loizou “The navigation transformation” In _IEEE Transactions on Robotics_ 33.6 IEEE, 2017, pp. 1516–1523
  * [27] Harshat Kumar, Santiago Paternain and Alejandro Ribeiro “Navigation of a quadratic potential with ellipsoidal obstacles” In _2019 IEEE 58th Conference on Decision and Control (CDC)_ , 2019, pp. 4777–4784 IEEE
  * [28] Anqing Duan et al. “Learning to Avoid Obstacles With Minimal Intervention Control” In _Frontiers in Robotics and AI_ 7 Frontiers Media, 2020
  * [29] Bakir Lacevic, Paolo Rocco and Andrea Maria Zanchettin “Safety assessment and control of robotic manipulators using danger field” In _IEEE Transactions on Robotics_ 29.5 IEEE, 2013, pp. 1257–1270
  * [30] Andrea Maria Zanchettin, Bakir Lacevic and Paolo Rocco “Passivity-based control of robotic manipulators for safe cooperation with humans” In _International Journal of Control_ 88.2 Taylor & Francis, 2015, pp. 429–439
  * [31] Liwei Zhang et al. “Distributed Cooperative Obstacle Avoidance for Mobile Robots Using Independent Virtual Center Points” In _Journal of Intelligent & Robotic Systems_ Springer, 2019, pp. 1–15
  * [32] Christopher I Connolly, John B Burns and R Weiss “Path planning using Laplace’s equation” In _Robotics and Automation, 1990. Proceedings., 1990 IEEE International Conference on_ , 1990, pp. 2102–2106 IEEE
  * [33] J-O Kim and Pradeep K Khosla “Real-time obstacle avoidance using harmonic potential functions” In _IEEE Transactions on Robotics and Automation_ 8.3 IEEE, 1992, pp. 338–349
  * [34] Jurgen Guldner and Vadim I Utkin “Sliding mode control for an obstacle avoidance strategy based on an harmonic potential field” In _Decision and Control, 1993., Proceedings of the 32nd IEEE Conference on_ , 1993, pp. 424–429 IEEE
  * [35] Seyed Mohammad Khansari-Zadeh and Aude Billard “A dynamical system approach to realtime obstacle avoidance” In _Autonomous Robots_ 32.4 Springer, 2012, pp. 433–454
  * [36] Matteo Saveriano and Dongheui Lee “Distance based dynamical system modulation for reactive avoidance of moving obstacles” In _Robotics and Automation (ICRA), 2014 IEEE International Conference on_ , 2014, pp. 5618–5623 IEEE
  * [37] Lukas Huber, Aude Billard and Jean-Jacques Slotine “Avoidance of convex and concave obstacles with convergence ensured through contraction” In _IEEE Robotics and Automation Letters_ 4.2 IEEE, 2019, pp. 1462–1469
  * [38] Der-Tsai Lee and Franco P Preparata “An optimal algorithm for finding the kernel of a polygon” In _Journal of the ACM (JACM)_ 26.3 ACM New York, NY, USA, 1979, pp. 415–421
  * [39] Dan Jia, Alexander Hermans and Bastian Leibe “DR-SPAAM: A Spatial-Attention and Auto-regressive Model for Person Detection in 2D Range Data” In _2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2020, pp. 10270–10277 DOI: 10.1109/IROS45743.2020.9341689
  * [40] S.M. Khansari-Zadeh “A Dynamical System-based Approach to Modeling Stable Robot Control Policies via Imitation Learning”, PhD Thesis, 2012
  * [41] William A Wolovich and H Elliott “A computational technique for inverse kinematics” In _The 23rd IEEE Conference on Decision and Control_ , 1984, pp. 1359–1363 IEEE
  * [42] Roger Penrose “A generalized inverse for matrices” In _Mathematical proceedings of the Cambridge philosophical society_ 51.3, 1955, pp. 406–413 Cambridge University Press
  * [43] Mahdi Khoramshahi and Aude Billard “A dynamical system approach to task-adaptation in physical human–robot interaction” In _Autonomous Robots_ Springer, 2018, pp. 1–20
  * [44] Diego Felipe Paez Granados, Hideki Kadone and Kenji Suzuki “Unpowered Lower-Body Exoskeleton with Torso Lifting Mechanism for Supporting Sit-to-Stand Transitions” In _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2018, pp. 2755–2761 IEEE
  * [45] Fabien Grzeskowiak, Marie Babel, Julien Bruneau and Julien Pettré “Toward virtual reality-based evaluation of robot navigation among people” In _2020 IEEE Conference on Virtual Reality and 3D User Interfaces (VR)_ , 2020, pp. 766–774 IEEE
  * [46] Aaron D Ames et al. “Control barrier functions: Theory and applications” In _2019 18th European Control Conference (ECC)_ , 2019, pp. 3420–3431 IEEE
  * [47] Andrew Taylor, Andrew Singletary, Yisong Yue and Aaron Ames “Learning for safety-critical control with control barrier functions” In _Learning for Dynamics and Control_ , 2020, pp. 708–717 PMLR
  * [48] Richard Cheng, Gábor Orosz, Richard M Murray and Joel W Burdick “End-to-end safe reinforcement learning through barrier functions for safety-critical continuous control tasks” In _Proceedings of the AAAI Conference on Artificial Intelligence_ 33, 2019, pp. 3387–3395
  * [49] Mathieu Fourment and Michael R Gillings “A comparison of common programming languages used in bioinformatics” In _BMC bioinformatics_ 9.1 Springer, 2008, pp. 1–9
  * [50] Claudine Badue et al. “Self-driving cars: A survey” In _Expert Systems with Applications_ 165 Elsevier, 2021, pp. 113816
  * [51] Wilko Schwarting, Javier Alonso-Mora and Daniela Rus “Planning and decision-making for autonomous vehicles” In _Annual Review of Control, Robotics, and Autonomous Systems_ 1 Annual Reviews, 2018, pp. 187–210