#  Avoidance of Concave Obstacles
through Rotation of Nonlinear Dynamics

Lukas Huber1, Jean-Jacques Slotine2, Aude Billard1 This work was supported by
EU ERC grant SAHR.1 LASA Laboratory, Swiss Federal School of Technology in
Lausanne - EPFL, Switzerland<EMAIL_ADDRESS>3
Nonlinear Systems Laboratory, Massachusetts Institute of Technology, USA.
<EMAIL_ADDRESS>
###### Abstract

Controlling complex tasks in robotic systems, such as circular motion for
cleaning or following curvy lines, can be dealt with using nonlinear vector
fields. In this paper, we introduce a novel approach called rotational
obstacle avoidance method (ROAM) for adapting the initial dynamics when the
workspace is partially occluded by obstacles. ROAM presents a closed-form
solution that effectively avoids star-shaped obstacles in spaces of arbitrary
dimensions by rotating the initial dynamics towards the tangent space. The
algorithm enables navigation within obstacle hulls and can be customized to
actively move away from surfaces, while guaranteeing the presence of only a
single saddle point on the boundary of each obstacle. We introduce a sequence
of mappings to extend the approach for general nonlinear dynamics. Moreover,
ROAM extends its capabilities to handle multi-obstacle environments and
provides the ability to constrain dynamics within a safe tube. By utilizing
weighted vector-tree summation, we successfully navigate around general
concave obstacles represented as a tree-of-stars. Through experimental
evaluation, ROAM demonstrates superior performance in terms of minimizing
occurrences of local minima and maintaining similarity to the initial
dynamics, outperforming existing approaches in multi-obstacle simulations. The
proposed method is highly reactive, owing to its simplicity, and can be
applied effectively in dynamic environments. This was demonstrated during the
collision-free navigation of a 7 degree-of-freedom robot arm around dynamic
obstacles.

##  I Introduction

Reactive motion plays a crucial role in numerous real-world robotics
applications. When operating outside the controlled environments of factory
floors, robots are exposed to unpredictable and dynamic surroundings, making
precise estimation challenging. As a result, real-time adaptive controllers
are essential to enable robots to adapt and reevaluate their actions in
response to changing conditions.

A primary constraint when navigating in dynamic and cluttered environments is
to ensure the safety of individuals moving around the robot. This requires the
robot to constantly and rapidly replan its path to avoid collisions while
making every effort to maintain its intended task and adhere to the originally
intended movement dynamics. Furthermore, to protect physical hardware from
potential damage caused by high accelerations, it is crucial to design a
smooth system. Additionally, collision avoidance needs to seamlessly integrate
with reactive control techniques (Figure 1(a)).

(a) QOLO-robot navigating within small labyrinth

(b) Initial dynamics

(c) Rotated dynamics

Figure 1: An autonomous wheelchair is guided by obstacle avoidance to navigate
in an environment of complex obstacles (c). The intensity of the shading in
(a) and (b) indicates the magnitudes of the velocity.

Control methods based on vector fields [1] and dynamical systems [2] have
proven to be well-suited for addressing the challenges posed by such
scenarios. Rather than precomputing a trajectory for the robot, these methods
generate a (nonlinear) control field that is evaluated in real-time at the
robot’s position. This allows the robot to react instantaneously to
disturbances and perceive changes in its environment. In this work, we
leverage the framework of dynamical systems and vector fields to develop an
adaptive obstacle avoidance approach capable of modifying nonlinear motion in
dynamic and complex environments (Fig. 1(a)).

Dynamical systems represent the evolution of a system’s state without
considering its history, effectively capturing the system’s dynamics as a
vector field. In robotics, dynamical systems have been employed for learning
complex dynamics [3] and enforcing stability guarantees through techniques
such as Lyapunov Stability [4] or Contraction Theory [2]. Force control in
robotics often utilizes second-order dynamical systems [5], while obstacle
avoidance typically relies on first-order systems that output desired
velocities based on the current position [6]. Consequently, additional
controllers are employed to ensure force- and torque-controlled robots follow
the desired motion of the dynamical system [7].

Analogously, desired dynamics based on position can be represented as a vector
field [1]. These nonlinear vector fields can be designed to ensure convergence
and stability properties. Nonlinear vector fields designed for path following,
known as ”vector-field-guided path following,” enable smooth convergence
towards a desired path, reducing path-following errors [8].

In this paper, we develop an adaptive obstacle avoidance framework capable of
modifying nonlinear motion in response to dynamic and complex environments.
The proposed approach aims to navigate the control system safely while
maintaining the integrity of the intended motion dynamics. The effectiveness
of our approach is demonstrated through experiments.

###  I-A Related Work

Obstacle avoidance methods are often categorized into local and global
strategies [9]. Global methods focus on convergence to the goal, while local
methods prioritize reactive collision avoidance in dynamic scenarios.

Global sampling-based methods, such as Rapidly-Exploring Random Trees (RRT),
have been widely utilized to populate the search space by branching out a
space-filling tree in order to find a feasible path [10]. Building upon RRT,
the RRT* algorithm was introduced to automatically scale the sampling length
during the search, resulting in improved performance and convergence rate
[11]. Similarly, Probabilistic Roadmaps (PRM) employ space sampling to
determine the collision-free status of points. By connecting the collision-
free samples, local connectivity graphs are formed, enabling graph-search
algorithms to find an optimal path from the initial position to the goal [12].
Although PRM and RRT are probabilistic-complete, meaning they can find a
feasible path if one exists [13], the sampling-based approach incurs a
significant computational cost. This renders it unsuitable for real-time
recomputation, which is essential for dynamic environments. Sampling
algorithms like RRT and PRM exhibit a complexity that is linear to the number
of nodes $n$ in the sampled tree, represented as
$\mathcal{O}(n)=\mathcal{O}(exp(N))$ [14]. Even in two-dimensional
environments, the trees need to cover the entire workspace, leading to a large
number of nodes. To address the computational challenges and facilitate fast
computation in dynamic environments, initial paths are often reshaped. One
approach involves interpreting trajectories as elastic strips, enabling
efficient adaptation and obstacle avoidance [15]. However, this comes at the
loss of convergence guarantees.

Motion Optimization (MO) has been employed to dynamically adapt the originally
sampled trajectories, analogous to stretching an elastic band around an object
[16]. However, MO tends to perform poorly in highly non-convex problems and
often converges to suboptimal local minima. To alleviate this issue, Markov
chains have been utilized for joint-space control [17], while other approaches
incorporate Riemann geometry and geometric constraints to improve convergence
heuristics [18, 19]. Nonetheless, MO remains local in nature and often fails
to converge when dealing with non-convex motions.

Model Predictive Control (MPC) reduces the optimization problem to a finite
time horizon, thereby reducing the convergence time [20]. The increased
computational power in recent years has facilitated the utilization of MPC for
real-time collision avoidance [21]. Sampling-based MPC has been employed for
dynamic configuration-space collision avoidance in scenarios where an analytic
cost function is absent [22, 23]. However, the limited time horizon of MPC
often limits the problem to local optimality, and global convergence cannot
always be guaranteed.

In recent years, the application of Machine Learning (ML) in collision
avoidance for robotics has gained significant traction. End-to-end learning
approaches have enabled collision-free multi-robot navigation [24], while
fuzzy artificial potential fields augmented with neural networks have been
utilized for motion planning of mobile robots [25]. Conversely, RRT has served
as a foundation for training reinforcement learning algorithms [26]. While
learning-based methods demonstrate impressive performance, they often lack
collision avoidance guarantees and cannot ensure convergence to a reliable
solution. Deep learning methods, such as multi-layer neural networks, incur a
computational cost that scales with the number of parameters $p$, denoted as
$\mathcal{O}(p)$ [27]. Given that modern neural networks frequently comprise
millions of parameters, the inference process becomes the most time-consuming
component of the control loop. In numerous robotic implementations, the
vision-based neural network constitutes the most time-intensive aspect of the
control loop [28]. However, employing neural networks to output control
commands introduces substantial computation time to the control loop [23, 26].

While most global methods lack the ability to adapt to dynamic environments,
this limitation can be overcome by employing local methods. Many local methods
can be interpreted as nonlinear feedback controllers.

One of the early collision avoidance methods in robotics is Artificial
Potential Fields (APF). APF represents each obstacle as a repulsive field
through which the agent navigates [29]. The reactive nature of APF makes it
suitable for dynamic environments, such as tracking moving targets [30]. APF
has also been integrated with closed-feedback loops for real-time collision
avoidance of robotic manipulators [31]. While APF is efficient and easy to
implement, it is prone to local minima in multi-obstacle environments [32].

Navigation functions (NFs) were developed to address the issue of multiple
local minima by constructing a potential field with a single minimum [33].
While initially designed for sphere worlds, NFs utilize diffeomorphic mappings
between star-worlds and sphere-worlds to extend their applicability to more
general scenarios [34]. These functions can also be employed to navigate
around inverted obstacles that represent space boundaries, and their scope has
been further expanded to include navigation around trees-of-stars [35].
However, achieving optimal performance with NFs requires comprehensive
knowledge of the obstacle distribution, making the tuning of critical
parameters a challenging task. Efforts have been made to enhance NFs by
eliminating critical parameters in sphere worlds [36], and alternative
approaches have been proposed to automate the tuning process [37]. NFs offer
the advantage of quick re-evaluation, making them robust against disturbances.
Nonetheless, due to the inherent difficulty in tuning them for general
obstacle configurations, NFs pose challenges when applied in dynamic
environments. Furthermore, as NFs rely on the gradient of the potential
function to guide the system dynamics, they are unable to effectively track
arbitrary nonlinear dynamics.

Vector fields (VF) employ highly nonlinear vector fields to navigate paths
while ensuring collision avoidance. They have been successfully applied to
evade multiple circular obstacles in two-dimensional space using local
repulsive fields [38]. VF techniques have also been utilized for controlling
fixed-wing aircraft [39] and extended to accommodate initially nonlinear
dynamics, such as following a limit cycle [40]. Nonetheless, it involves
switching between different dynamics, which can result in high accelerations.
However, VF methods typically handle one obstacle at a time, imposing a
conservative constraint where the influence regions of obstacles cannot
overlap.

Harmonic potential functions (HPF) are intriguing because they guarantee the
absence of topologically critical points in free space. While analytical HPFs
are often elusive, numerical approximations have been employed [41]. Linear
panel representations for obstacles enable the generation of closed-form HPFs
[42]. Although this linear approximation can be extended to concave obstacles,
its application is limited to two-dimensional environments [43].

Dynamical system modulation (DSM) has emerged as an effective approach for
collision avoidance in reactive environments by redirecting initial dynamics
away from obstacles [6]. To overcome challenges posed by intersecting, convex
obstacles in three dimensions, DSM was extended to incorporate switching to
surface following [44]. In our previous research, we successfully imitated the
behavior of harmonic potential functions to ensure the convergence of DSM
around concave (star-shaped) obstacles [2]. Furthermore, we expanded the scope
of DSM to encompass indoor environments, facilitating reactive avoidance based
on sensor data [45, 28]. While DSM traditionally assumes a zero-dimensional
point, we achieved collision-free rigid-body dynamics by representing mobile
agents through multiple control points [46]. However, it is important to note
that DSM utilizes straight dynamics towards an attractor as an input, and the
convergence guarantees are presently limited to star-shaped obstacles.

###  I-B Contributions

This work significantly expands upon the reviewed DSM approach, enabling
obstacle avoidance with nonlinear dynamical systems and for general concave
obstacles. The paper’s tehcnical contributions are outlined as follows:

  * •

We introduce the Rotational Obstacle Avoidance Method (ROAM), which ensures
local minima-free obstacle avoidance for nonlinear dynamics (Section III).

  * •

We present a diffeomorphic mapping that guarantees convergence while
maintaining proximity to the general nonlinear dynamics, while trying to
preserve the initial flow (Section IV).

  * •

We extend the ROAM formulation to handle obstacle avoidance of multiple
dynamic obstacles and enclosed spaces, utilizing the concept of inverted
obstacles as introduced in [33, 45] (Section V).

  * •

Finally, we demonstrate the application of ROAM for avoiding general obstacles
represented by trees-of-stars in the presence of nonlinear dynamics (Section
VI).

  * •

Additionally, we develop a method for vector rotation in general dimensions
and the weighted summing of trees of vector rotations (Appendix -G).

To validate the properties of ROAM, we conducted following evaluations:

  * •

We performed a quantitative comparison of ROAM against two recent analytical
obstacle avoidance methods through simulations, assessing convergence rate,
similarity to unperturbed motion, and acceleration along the trajectories
(Section VII).

  * •

Furthermore, we conducted a qualitative evaluation of ROAM’s application in
controlling a 7DoF robot arm to avoid trees-of-stars in three dimensions
(Section VII-D2).

The proposed rotation obstacle avoidance method (ROAM) ensures collision
avoidance in a local minima-free vector field in the presence of initial
nonlinear dynamics. Moreover, the influence regions of the obstacle can
overlap, and the method has been extended to inverted obstacles, too, as well
as multiple obstacles with overlapping regions of influence, as can be seen in
the comparison with similar algorithms in Table I.

| RF | NF | MuMo | VF-CAPF | ROAM
---|---|---|---|---|---
| [29] | [33] | [45] | [40] |
Local minima free |  | ✓ | ✓ | (✓) | ✓
Switching free | ✓ | ✓ | ✓ |  | ✓
Overlapping regions | ✓ | ✓ | ✓ |  | ✓
Nonlinear dynamics | ✓ |  | (✓) | ✓ | ✓
Dynamic environments | ✓ |  | ✓ | ✓ | ✓
(Optional) repulsion | ✓ |  |  |  | ✓
Inverted obstacle | ✓ | ✓ | ✓ |  | ✓
Trees of Obstacles | ✓ | ✓ |  |  | ✓

TABLE I:  Comparison of five different time-invariant algorithms across
multiple criteria. All algorithms avoid solving an optimization problem.

##  II Preliminairies

###  II-A Notations

In this section, we establish the notations used throughout this paper. We
consider a space of general dimension $N$ as the underlying framework for our
developments. Vectors are denoted using bold symbols, and the state variable
is represented by $\bm{\xi}\in\mathbb{R}^{N}$. The time derivative of a vector
$\bm{\xi}$ is denoted as $\dot{\bm{\xi}}$, assuming $\bm{\xi}$ is a function
that is differentiable with respect to time. Unless explicitly stated
otherwise, the paper employs the Euclidean norm, denoted as $|\cdot|$. The
circular constant is denoted by $\pi$. The matrix $\bm{I}$ represents the
identity matrix of appropriate dimensions. The symbol $\circ$ signifies the
iterative evaluation of a function. For example, when considering two
functions $a(\cdot)$ and $b(\cdot)$, the expression $a\circ b(\bm{\xi})$
denotes the composition $a(b(\bm{\xi}))$. In the context of this paper, the
term vector field (VF) refers to the velocity field $\dot{\bm{\xi}}$, while
dynamical system (DS) specifically denotes the continuous-time feedback system
described in Section II-B.

###  II-B Dynamical Systems

Consider a vector field that governs the evolution of the position
$\bm{\xi}\in\mathbb{R}^{N}$ of a continuous-time system, described by the
state derivative $\dot{\bm{\xi}}\in\mathbb{R}^{N}$:

$\dot{\bm{\xi}}=\bm{f}(\bm{\xi})$ (1)

Let us define straight dynamics, that do maintain the direction along a
trajectory:

######  Definition II.1 (Straight Dynamics).

A dynamical system of the form (1) is referred to as straight dynamics if, for
all initial conditions $\bm{\xi}_{0}$ and $\dot{\bm{\xi}}_{0}$, the flow
remains collinear with the initial velocity, satisfying
$\dot{\bm{\xi}}_{0}^{T}\dot{\bm{\xi}}_{t}=\|\dot{\bm{\xi}}_{0}\|\|\dot{\bm{\xi}}_{t}\|$,
for all $t\geq 0$. The flow is said to be locally straight if it is straight
within a subdomain $\mathcal{X}^{s}$. In the special case where the attractor
is positioned infinitely far away, hence
$\exists\bm{v}^{a}\in\mathbb{R}^{N}:\lim_{\|\bm{\xi}-\bm{\xi}^{a}\|\rightarrow\infty}\dot{\bm{\xi}}_{t}^{T}\bm{v}^{a}=\|\dot{\bm{\xi}}_{t}^{T}\|$,
hence the dynamics are called (locally) collinear.

Multiple straight dynamics are visualized in Figure 2. In the subsequent
sections, we adopt the following parameterization for globally straight
dynamics:

$\bm{f}^{s}(\bm{\xi})=q(\bm{\xi})\left(\bm{\xi}^{a}-\bm{\xi}\right)$ (2)

where $q:\mathbb{R}^{N}\rightarrow\mathbb{R}\setminus 0$ represents a
continuous state-dependent scaling function that modulates the speed of the
linear system $(\bm{\xi}^{a}-\bm{\xi})$ towards a fixed point $\bm{\xi}^{a}$.
This scaling allows for the adjustment of the speed of the linear dynamics as
the system approaches or moves away from the attractor
$\bm{\xi}^{a}\in\mathbb{R}^{N}$, which serves as the unique stable fixed point
or attractor of the system (Fig. 2(a)). Importantly, the condition
$q(\bm{\xi})\neq 0$, combined with the continuity of $q$, preserves the
directionality of the flow. Consequently, the vector field consistently points
towards or away from the attractor across the state space.

(a) Globally straight

(b) Globally collinear

(c) Locally straight

Figure 2:  Dynamical systems can exhibit different straightness
characteristics. Such as globally straight with a single, stable attractor
$\bm{\xi}^{a}$ visualized as a star (a). They can be globally collinear when
the attractor is infinitely far away (b). Conversely, dynamics might be
globally defined but only locally straight in a subdomain, visualized as the
dark gray region (c).

###  II-C Obstacle Description

In accordance with [2], each obstacle is characterized by a continuous
distance function $\Gamma(\bm{\xi}):\mathbb{R}^{N}\mapsto\mathbb{R}_{\geq 0}$.
This function enables the differentiation of regions outside, on the boundary,
or inside the obstacle based on the isolines of $\Gamma$.

Free space:
$\displaystyle\mathcal{X}^{f}=\\{\bm{\xi}\in\mathbb{R}^{N}:\Gamma(\bm{\xi})>1\\}$
Boundary:
$\displaystyle\mathcal{X}^{b}=\\{\bm{\xi}\in\mathbb{R}^{N}:\Gamma(\bm{\xi})=1\\}$
(3) Interior set::
$\displaystyle\mathcal{X}^{i}=\\{\bm{\xi}\in\mathbb{R}^{N}\setminus(\mathcal{X}^{f}\cup\mathcal{X}^{b})\\}$

####  II-C1 Star-Shaped Obstacles

For representing star-shaped obstacles, we adopt the notation introduced in
[47, 48]. An obstacle is considered star-shaped if there exists a point
$\bm{\xi}^{r}$ with $\Gamma(\bm{\xi}^{r})<1$ such that every point on the
boundary or in the interior set of the obstacle, i.e.,
$\bm{\xi}\in\mathcal{X}^{i}$ and
$\mathcal{X}^{i}:{\bm{\xi}\in\mathbb{R}^{N},\Gamma(\bm{\xi})\leq 1}$, is
connected to $\bm{\xi}^{r}$ by a line segment $l(\bm{\xi}^{r},\bm{\xi})$
contained within $\mathcal{X}^{i}$.

The set of all such points $\bm{\xi}^{r}$ is referred to as the kernel of
$\mathcal{X}^{i}$ and denoted as:

$\text{ker}(\mathcal{X}^{i})=\left\\{\bm{\xi}^{r}\in\mathcal{X}^{i}:l(\bm{\xi}^{r},\bm{\xi})\subset\mathcal{X}^{i},\;\forall\,\bm{\xi}\in\mathcal{X}^{i}\right\\}$
(4)

Throughout this paper, a specific choice of $\bm{\xi}^{r}$ is referred to as
the reference point, as commonly used in [2, 45], and represented by a cross
symbol $+$. 111The reference point is in literature sometimes denoted kernel
point.

####  II-C2 Distance Function

The distance function $\Gamma(\bm{\xi})$ increases monotonically in the radial
direction, i.e., along the vector $\bm{\xi}-\bm{\xi}^{r}$, and has a
continuous first-order partial derivative ($C^{1}$ smoothness). In this paper,
we compute a distance function relative to one boundary point
$\bm{\xi}^{b}\in\mathcal{X}^{b}$:

$\Gamma(\bm{\xi})=\|\bm{\xi}-\bm{\xi}^{b}\|/d_{0}+1\quad\forall\bm{\xi}\in\mathcal{X}^{e}$
(5)

where $d_{0}\in\mathbb{R}_{>0}$ is a scaling factor, which modulates the
obstacle influence. The boundary point $\bm{\xi}^{b}$ lies at the intersection
between a ball of radius $R(\bm{\xi})$ around the reference point
$\bm{\xi}^{r}$:

$R(\bm{\xi})=\|\bm{\xi}^{b}-\bm{\xi}^{r}\|\quad\bm{\xi}^{b}=b\bm{r}(\bm{\xi})+\bm{\xi}^{r}\;,\;b>0\,,\;\;\bm{\xi}^{b}\in\mathcal{X}^{b}$
(6)

and the reference direction vector
$\bm{r}:\mathbb{R}^{N}\setminus\bm{\xi}^{r}\rightarrow\\{\bm{r}\in\mathbb{R}^{N}:\|\bm{r}\|=1\\}$:

${\bm{r}}(\bm{\xi})={\left(\bm{\xi}-\bm{\xi}^{r}\right)}/{\|\bm{\xi}-\bm{\xi}^{r}\|}.$
(7)

The boundary point is hence a function of the state, $\bm{\xi}^{b}(\bm{\xi})$.

Further, the normal direction can be defined as the derivative of the distance
across space:

$\bm{n}(\bm{\xi})=d\Gamma(\bm{\xi})/d\bm{\xi}$ (8)

###  II-D Rotated Vector Field

We present a construct that is fundamental to the obstacle avoidance approach
proposed here, inspired by the concept of stereographic projection [45]. 222A
stereographic projection maps points located on a sphere onto a plane
perpendicular to the sphere surface. Consider a unit vector with respect to a
basis vector $\bm{b}$, denoted as
$\bm{k}(\bm{b},\bm{\xi}):\mathbb{R}^{N}_{I}\times\mathbb{R}^{N}_{I}\rightarrow\mathbb{R}^{N-1}$,
under the condition that
$\bm{\xi}:\left\langle{-\bm{b}},\,{\bm{\xi}}\right\rangle\neq-1$. This vector
extraction captures the rotation of $\bm{\xi}$ with respect to $\bm{b}$ and
has proven to be useful for minima-free vector-summing. The relative rotation
satisfies the following properties:

$\cos\left(\|\bm{k}(\bm{b},\bm{\xi})\|\right)=\left\langle{\bm{b}},\,{\bm{\xi}}\right\rangle\quad\text{and}\quad\|\bm{k}(\bm{b},\bm{\xi})\|<\pi$
(9)

The basis vector $\bm{b}$ is utilized as the first column to construct the
orthonormal transformation matrix $\bm{B}$, enabling the transformation into a
new basis: $\hat{\bm{\xi}}=\bm{B}^{T}\bm{\xi}$. In the direction space, the
magnitude corresponds to the angle between the original vector and the
reference vector. The transformation of the initial vector $\bm{\xi}$ in the
direction space is given by:

$\bm{k}(\bm{b},\bm{\xi})=\begin{cases}\arccos\left(\hat{\bm{\xi}}_{[1]}\right)\hat{\bm{\xi}}_{[2:]}/\|\hat{\bm{\xi}}_{[2:]}\|&\text{if}\;\;\hat{\bm{\xi}}_{[1]}\neq
1\\\ \bm{0}&\text{otherwise}\end{cases}$ (10)

Note that a vector $\bm{\xi}$, which is anti-collinear to $\bm{b}$, does not
have a unique transformation and is hence excluded; see [45] for further
discussion. Correspondingly, we use
$\bar{\bm{k}}(\bm{b},\bar{\bm{\xi}}):\mathbb{R}^{N}_{I}\times\mathbb{R}^{N-1}\rightarrow\mathbb{R}^{N}_{I}$
to describe the inverse mapping, such that:

$\bm{\xi}=\bar{\bm{k}}(\bm{b},\bar{\bm{\xi}})\quad\text{with}\quad\bar{\bm{\xi}}=\bm{k}(\bm{b},\bm{\xi})$
(11)

The mapping to the original space is evaluated as follows:

$\bar{\bm{k}}(\bm{b},\bar{\bm{\xi}})=\begin{cases}\bm{B}\begin{bmatrix}1&0&..&0\end{bmatrix}^{T}&\text{if}\;\|\bar{\bm{\xi}}\|=0\\\
\bm{B}\begin{bmatrix}\cos(\|\bar{\bm{\xi}}\|)&\sin(\|\bar{\bm{\xi}}\|)\bar{\bm{\xi}}/\|\bar{\bm{\xi}}\|\end{bmatrix}^{T}&\text{otherwise}\end{cases}$
(12)

It is important to note that the reverse mapping $\bar{\bm{k}}(\cdot)$ is
defined for angles larger than $\pi$, but the function is not bijective for
these values.

###  II-E Problem Statement

We establish the following requirements for our obstacle avoidance controller:

  * •

Collision free: The flow must remain outside the obstacles at all times, i.e.,
$\\{\bm{\xi}\\}_{t}\in\mathcal{X}^{e}\;\forall t$ if
$\\{\bm{\xi}\\}_{0}\in\mathcal{X}^{e}$.

  * •

State dependent: The dynamics are history-invariant and depend solely on the
current state, i.e.
$\dot{\bm{\xi}}_{t}=\bm{f}(\bm{\xi}_{t},\bm{\xi}_{t-1},..,\bm{\xi}_{0})=\bm{f}(\bm{\xi}_{t})$.

  * •

Local minima free: As demonstrated in [40], each $C^{1}$-smooth vector field
obstacle introduces at least one stationary point on the surface. However, it
must be ensured that (1) this point is a saddle point and not a minimum and
(2) there are no additional stationary points in space.

  * •

Limited and smooth dynamics; The magnitude of the vector field is upper
bounded, i..e.,
$\exists\bm{v}^{\mathrm{max}}=\text{const.}:\|\dot{\bm{\xi}}\|<\bm{v}^{\mathrm{max}}$.
Additionally, the vector field is smooth, meaning that small displacements
result in proportionally small velocities:
$\lim_{\bm{\xi}_{1}\rightarrow\bm{\xi}_{2}}\|\dot{\bm{\xi}}_{2}-\dot{\bm{\xi}}_{1}\|=0$

  * •

General dimensions: The obstacle avoidance algorithm is applicable in a space
of dimensions $N\geq 2$.

Furthermore, we assume that the environment and initial dynamics
$\bm{f}(\bm{\xi})$, as described in (1), possess the following properties:

  * •

Star-shaped: All obstacles are star-shaped as defined in Sec. II-C1, or are
composed of obstacles (trees-of-stars) with a nonzero intersection regions
among the components of a tree, as discussed in Section II-C1.

  * •

Limited and smooth dynamics: Analogously to the output, the magnitude of the
initial dynamics $\bm{f}(\bm{\xi})$ is required to be $C^{1}$-smooth and
limited, i.e.,
$\|\bm{f}(\bm{\xi})\|<v^{\mathrm{max}},\;\;\forall\bm{\xi}\in\mathbb{R}^{N},v^{\mathrm{max}}\in\mathbb{R}_{>0}$.

  * •

Dynamics as a vector rotation: The initial dynamics $\bm{f}(\bm{\xi})$ can be
evaluated as a local rotation (Section II-D) or a sequence of rotations
(Appendix -G) of globally straight dynamics with respect to a fixed point
$\bm{\xi}^{a}$ (Definition II.1).

##  III Obstacle Avoidance through Rotation

A smooth vector field that effectively avoids an obstacle should have the
velocity $\dot{\bm{\xi}}$ directed away from or perpendicular to the normal
$\bm{n}(\bm{\xi})\in\mathbb{R}^{N}$ as defined in (8). This principle is
commonly expressed as follows [6, 43]:

$\left\langle{\bm{n}(\bm{\xi})},\,{\dot{\bm{\xi}}}\right\rangle\geq
0\quad\forall\,\bm{\xi}\in\mathcal{X}^{b}$ (13)

###  III-A Vector Rotation for Collision Avoidance

We propose a method called Rotational Obstacle Avoidance Method (ROAM) to
achieve collision-free motion. ROAM smoothly adjusts the initial velocity
$\bm{f}(\bm{\xi})$ as given in Eq. (1), rotating it towards a feasible half-
space as the position approaches the obstacle (Fig. 3). The steps involved in
ROAM for avoiding a single obstacle are as follows:

  1. 1.

Evaluation of pseudo-tangent direction $\bm{e}(\bm{\xi})$ using the
convergence direction $\bm{c}(\bm{\xi})$ (Sec. III-A1)

  2. 2.

Rotation of the initial dynamics $\bm{f}(\bm{\xi})$ towards the pseudo-tangent
$\bm{e}(\bm{\xi})$ to obtain the collision-free direction $\dot{\bm{\xi}}$
(Sec. III-A2).

  3. 3.

Evaluation of the velocity magnitude $h(\bm{\xi})$ to ensure a smooth vector
field (Sec. III-A3)

(a) Initial dynamics

(b) Rotated system

Figure 3: A nonlinear dynamical system
$\bm{f}(\bm{\xi})=\text{diag}(1\;\;\Delta\bm{\xi}_{[2]})\|\Delta\bm{\xi}\|$
with $\Delta{\bm{\xi}}=\bm{\xi}^{a}-\bm{\xi}$ (a) is rotated using ROAM to
ensure convergence towards the attractor $\bm{\xi}^{a}$ with a single saddle
point on the surface (b).

####  III-A1 Evaluation of Preferred Tangent Direction

At each position on the surface of an obstacle, there can exist multiple
tangent directions. In the case of $d=2$, there are exactly two tangent
directions, while for $d\geq 3$, there are infinitely many. To construct a
smoothly defined pseudo-tangent $\bm{e}(\xi)$, we introduce the concept of
convergence dynamics, which are used to obtain the pseudo-tangent:

######  Definition III.1 (Convergence Dynamics).

The $C^{1}$-smooth convergence-dynamics
$\bm{c}:\mathbb{R}^{N}\rightarrow\mathbb{R}^{N}$ guides the initial dynamics
$\bm{f}(\bm{\xi})$ around obstacles. The convergence dynamics are locally
straight according to Definition II.1, on the surface of the obstacle
$\mathcal{X}^{b}$ as defined in (3) Furthermore, the convergence dynamics
$\bm{c}(\bm{\xi})$ is set to never be anti-collinear to the initial dynamics,
i.e.,
$\left\langle{\bm{f}(\bm{\xi})},\,{\bm{c}(\bm{\xi})}\right\rangle\neq-\|\bm{f}(\bm{\xi})\|\,\|\bm{c}(\bm{\xi})\|\;\forall\bm{\xi}\in\mathbb{R}^{N}$.

In this section, we utilize convergence dynamics of the form
$\bm{c}(\bm{\xi})=\bm{\xi}-\bm{\xi}^{a}$. For more general convergence
dynamics, please refer to Section IV.

The desired pseudo-tangent $\bm{e}(\bm{\xi})$ is obtained by rotating the
convergence dynamics $\bm{c}(\bm{\xi})$ away from the reference direction
$\bm{r}(\bm{\xi})$ until it lies in the tangent plane (Fig. 3). Since, in the
angular space $\bm{k}(\bm{n},\cdot)$, given in (9), any tangent vector lies at
a distance $R^{e}=\pi/2$ to the normal direction, the tangent hyper-plane
$\mathcal{T}$ forms a hyper-sphere in the direction space, as visualized in
Figure 4. Hence, the rotation of the convergence dynamics $\bm{c}(\bm{\xi})$
away from reference direction $\bm{r}(\bm{\xi})$ can be evaluated by
intersecting the line connecting $\bm{r}(\bm{\xi})$ and $\bm{c}(\bm{\xi})$
with a circle of radius $R^{e}\in[\pi/2,\pi]$, see Fig. 4. Thus,
$\bm{e}(\bm{\xi})$ can be obtained through the following constraints333For
conciseness dependency on $\bm{\xi}$ is omitted.:

$\displaystyle\text{if}\quad\|\bm{k}\left(-\bm{n},\bm{c}\right)\|\geq{R^{e}}\quad\text{then}\quad{\bm{e}}={\bm{c}}\quad\text{otherwise}$
$\displaystyle\bm{k}\left(-\bm{n},{\bm{e}}\right)=(1-b)\bm{k}\left(-\bm{n},{\bm{r}}\right)+b\bm{k}\left(-\bm{n},\bm{c}\right)$
(14) $\displaystyle\text{such
that}\quad\|\bm{k}\left(-\bm{n},\bm{e}\right)\|=R^{e},\quad
b\in\mathbb{R}_{>0}\;\;\forall\bm{\xi}:\bm{c}\neq\bm{r}$

The solution to the above equality constraints is obtained analytically by
solving a quadratic equation with respect to the scalar $b$.

As a result, the pseudo-tangent $\bm{e}(\xi)$ can either lie in the tangent
plane $\mathcal{T}$ or point away from the obstacle, with a distance to the
normal direction contained within the green region depicted in Figure 4. In
the special case where $\bm{c}(\bm{\xi})=\bm{r}(\bm{\xi})$, the intersection
of the hyper-circle in Equation 14 does yield a solution. However, it is
ensured that the final vector field $\dot{\bm{\xi}}$ is continuously defined,
as discussed in Section III-A2.

######  Lemma III.1.

Let us assume the convergence dynamics $\bm{c}(\bm{\xi})$, as given in
Definition III.1. The pseudo tangent $\bm{e}(\bm{\xi})\in\mathbb{R}^{N}$
obtained through (14) is smooth and satisfies the boundary inequality
$\left\langle{\bm{n}(\bm{\xi})},\,{\bm{e}(\bm{\xi})}\right\rangle\geq 0$,
stated in (13), at any position on the surface of the obstacle but the saddle
point, i.e.,
$\bm{\xi}\in\mathcal{X}^{b}:\left\langle{\bm{c}(\bm{\xi})},\,{\bm{n}(\bm{\xi})}\right\rangle\neq-1\\}$

###### Proof.

For a starshaped obstacle, we have by definition of the starshaped kernel
space in (4), that
$\left\langle{\bm{n}(\bm{\xi})},\,{\bm{r}(\bm{\xi})}\right\rangle<0$. Hence,
$\|\bm{k}(-\bm{n},\bm{r})\|<\pi/2$, i.e., $\bm{k}(-\bm{n},\bm{r})$ is strictly
inside the hyper-sphere $\mathcal{T}$. Furthermore, as long as
$\bm{c}(\bm{\xi})\neq\bm{r}(\bm{\xi})$, the equality of finding the
intersection of a line and a hyper-sphere from (14) has exactly one solution
with $b>0$.

Concerning the boundary condition; looking at the case of
$\|\bm{k}\left(-\bm{n},\bm{c}\right)\|>{R^{e}}$. From the definition of the
direction-space in (9), it follows that:

$\left\langle{\bm{c}(\bm{\xi})},\,{\bm{n}(\bm{\xi})}\right\rangle>0\underset{\bm{c}(\bm{\xi})=\bm{e}(\bm{\xi})}{\Rightarrow}\left\langle{\bm{e}(\bm{\xi})},\,{\bm{n}(\bm{\xi})}\right\rangle>0$
(15)

For the case that
$\left\langle{\bm{c}(\bm{\xi})},\,{\bm{n}(\bm{\xi})}\right\rangle<0$, from (9)
and (14) we can conclude:

$\begin{split}\left\langle{\bm{n}},\,{\bm{e}}\right\rangle&=\cos\left(\|\bm{k}(\bm{n},\bm{e})\|\right)=-\cos\left(\|\bm{k}(-\bm{n},\bm{e})\|\right)\\\
&\geq-\cos\left(R^{e}\right)\geq-\cos\left(\pi/2\right)=0\end{split}$ (16)

Hence, we have a uniquely defined pseudo tangent $\bm{e}(\bm{\xi})$, which
satisfies the boundary condition given in (13). ∎

(a) Euclidean space

(b) Directional space $\bm{k}(-\bm{n},\cdot)$

Figure 4: When approaching the surface of the obstacle, the collision-free
vectors are located on one side of the tangent plane $\mathcal{T}$ formed by
the linearly independent tangent vectors $\bm{e}_{(\cdot)}$ (a). These unit
vectors can be projected onto a circular hypersphere of dimension $N-1$, where
the collision-free pseudo tangent $\bm{e}$ is calculated (b). Any vector
$\bm{\xi}$ pointing towards the obstacle, i.e.,
$\left\langle{\bm{n}},\,{\bm{\xi}}\right\rangle<0$ (a), is projected to lie
inside the circle of radius $\pi/2$ (b), while a vector pointing away from the
obstacle is projected to the outer ring.

####  III-A2 Rotation Towards Tangent Direction

In a second step, the initial dynamics $\bm{f}(\bm{\xi})$ is rotated towards
the pseudo tangent $\bm{e}(\bm{\xi})$. This rotation operation is performed in
the direction space, as described in Section II-D, but this time with respect
to the convergence dynamics $\bm{c}(\bm{\xi})$:

$\displaystyle\dot{\bm{\xi}}=\bar{\bm{k}}\Bigl{(}\bm{c},\,\bigl{(}1-\lambda(\bm{\xi})\bigr{)}\bm{k}\left(\bm{c},\,\bm{f}\right)+\lambda(\bm{\xi})\,\bm{k}\left(\bm{c},\,\bm{e}\right)\Bigr{)}\quad\text{with}$
$\displaystyle\lambda(\bm{\xi})=\left(\frac{1}{\Gamma(\bm{\xi})}\right)^{q},\;\;R^{r}=\min\left(R^{e}-\|\bm{k}\left(-\bm{n},\,\bm{r}\right)\|,\frac{\pi}{2}\right)$
(17) $\displaystyle
q=\max\left(1,\;\frac{R^{r}}{\Delta\bm{k}(\bm{c})}\right)^{s},\quad\Delta\bm{k}(\bm{c})=\|\bm{k}\left(-\bm{n},\,\bm{r}\right)-\bm{k}\left(-\bm{n},\,\bm{c}\right)\|$

The rotation weight $\lambda(\bm{\xi})\in[0,1]$ is determined based on the
inverse of the distance $\Gamma$ to ensure that the rotation has a decreasing
influence as the distance increases. This allows for a smooth transition in
the avoidance behavior. Additionally, the smoothing factor $q$ is introduced,
which gradually decreases to zero when the convergence dynamics point towards
the robot. This effectively cancels the rotation avoidance effect in regions
where the tangent $\bm{e}(\bm{\xi})$ is not defined, resulting in a smooth
avoidance behavior across those positions. The impact of the smoothness
constant $s\in\mathbb{R}_{>0}$ can be observed in Figure 5, and unless
otherwise specified, we use $s=0.3$.

######  Lemma III.2.

The vector field $\dot{\bm{\xi}}$ obtained through rotation as given in (17)
satisfies the boundary condition as defined in (13) if the pseudo tangent
$\bm{e}(\bm{\xi})$ is tangent or pointing away from the obstacle, i.e.,
$\left\langle{\bm{e}},\,{\bm{n}}\right\rangle\geq 0$,
$\forall\bm{\xi}:\Delta\bm{k}(\bm{c})\neq 0$.

###### Proof.

When approaching the obstacle, the rotated velocity $\dot{\bm{\xi}}$ is
evaluated as:

$\lim_{\Gamma(\bm{\xi})\rightarrow
1,\;}\lambda(\bm{\xi})=1\;\;\underset{\text{with}\;\eqref{eq:inverse_angular_mapping}}{\Rightarrow}\;\;\lim_{\lambda\rightarrow
1}\left\langle{\bm{n}},\,{\dot{\bm{\xi}}}\right\rangle=\left\langle{\bm{n}},\,{\bm{e}}\right\rangle\geq
0$ (18)

using Lemma III.1 and that $\Delta\bm{k}(\bm{c})\neq 0\Rightarrow q>0$ with
the smoothness factor $q$ given in (17).

Following the same logic, we can also analyze the behavior far away from
obstacles:

$\lim_{\Gamma(\bm{\xi})\rightarrow\infty}\lambda(\bm{\xi})=0\;\;\Rightarrow\;\;\lim_{\lambda\rightarrow
0}\dot{\bm{\xi}}=\bm{f}(\bm{\xi})$ (19)

∎

(a) $s=$1e-6

(b) $s=0.3$

(c) $s=1.0$

Figure 5: A smaller smoothness-constant $s$ increases the reactivity when
approaching an obstacle (a), while a larger value ensures a smoother
transition across the (red) saddle point trajectory (b, c).

####  III-A3 Evaluation of Speed

The directional space mapping $\bm{k}(\cdot)$ and its inverse mapping
$\bar{\bm{k}}(\cdot)$ operate on unit vectors in the original space. As a
result, the algorithm in (17) modifies the direction of the initial dynamics,
rather than its magnitude. To incorporate magnitude control in a decoupled
manner, we introduce an additional component using
$h(\bm{\xi}):\mathbb{R}^{N}\rightarrow[0,1]$:

$\|\dot{\bm{\xi}}\|=h(\bm{\xi})\|\bm{f}(\bm{\xi})\|$ (20)

The stretching factor $h(\bm{\xi})$ is designed to slow down when pointing
towards an obstacle, and the effect decreases with increasing distance from
the obstacle:

$h(\bm{\xi})=\min\left(1,\,\left({\frac{\|\Delta\bm{k}\left(\bm{c}\right)\|}{R^{r}}}\right)^{2}+\left(1-\frac{1}{\Gamma(\bm{\xi})}\right)^{2}\right)$
(21)

where the reference radius $R^{r}$ and the rotation space distance
$\Delta\bm{k}(\bm{c})$ are given in (17). In Figure 6, the velocity scaling
can be observed to decrease the magnitude of the vectors pointing toward the
obstacle.

######  Theorem III.3.

Consider a vector field $\dot{\bm{\xi}}\in\mathbb{R}^{N}$ obtained after a
local rotation as defined in (17) of an initial dynamics $\bm{f}(\bm{\xi})$
and fixed point at $\bm{\xi}^{a}$, with pseudo tangent $\bm{e}(\bm{\xi})$
defined in (14) with respect to initial dynamics $\bm{f}(\bm{\xi})$, is
locally straight on the surface of the obstacle $\mathcal{X}^{b}$ according to
Definition II.1, and motion scaling $h(\bm{\xi})$ according to (21). Any
motion starting in free space $\\{\bm{\xi}\\}_{0}\in\mathcal{X}^{e}$ which
evolves according to $\dot{\bm{\xi}}$ will stay in free space for finite time
$\\{\bm{\xi}\\}_{t}\in\mathcal{X}^{e}$ with $t\in\mathbb{N}_{>0}$ and
maintains the stationary point, i.e., $\dot{\bm{\xi}}=\bm{0}$ if
$\bm{f}(\bm{\xi})=\bm{0}$

###### Proof.

From Lemma III.1, we know
$\left\langle{\bm{n}(\bm{\xi})},\,{\bm{e}(\bm{\xi})}\right\rangle\geq 0$ and
Lemma III.2 states that $\left\langle{\bm{e}},\,{\bm{n}}\right\rangle\geq
0\;\;\forall\bm{\xi}:\Delta\bm{k}(\bm{c})\neq 0$. Additionally, in the latter
case, the magnitude scaling from (21) evaluates as

$\lim_{\|\Delta\bm{k}(\bm{c})\|\rightarrow 0,\Gamma(\bm{\xi})\rightarrow
1}h(\bm{\xi})=0$ (22)

Hence, the speed reaches smoothly zero as we approach the saddle point, and
its direction does not matter to fulfill smoothness and the boundary condition
given in (13).

Furthermore, far away from the obstacle, we have:

$\lim_{\Gamma(\bm{\xi})\rightarrow\infty}h(\bm{\xi})=1$ (23)

Hence, the magnitude is equal to the original magnitude and only vanishes
around existing stationary points, i.e., $\bm{f}(\bm{\xi})=0$. ∎

The spurious stationary point on the surface is given by
$\\{\bm{\xi}:\bm{\xi}\in\mathcal{X}^{b},\,\|\bm{k}(\bm{c})\|=0\\}$.
Furthermore, by construction, the pseudo tangent $\bm{e}(\bm{\xi})$ defined in
(14) points away from the reference direction and hence points away from the
stationary point on the surface of the obstacle. Hence, it is a saddle point
with a single trajectory converging to it $\mathcal{X}^{s}$.

###  III-B Surface Repulsion

n the vicinity of critical obstacles, it may be desirable to incorporate
active repulsion from them without significantly altering their shape. This
can be achieved by introducing a behavior similar to artificial potential
fields [35], but without introducing spurious attractors in free space.
Building upon the developments presented in this section, we can further
refine the tangential radius. By increasing the tangential radius such that
$R^{e}\in]\pi/2,\pi]$, values larger than $\pi/2$, leads to a repulsive
behavior while maintaining the avoidance properties (Fig. 6).

Figure 6: While a surface repulsion radius of $R^{e}=\pi/2$ ensures collision
avoidance, an increased radius, i.e., $R^{e}\in]\pi/2,\pi]$ leads to increased
repulsion around the obstacle. The initial dynamics are given by
$\bm{f}(\bm{\xi})=(\bm{\xi}-\bm{\xi}^{a})$.

###  III-C Inverted Obstacles

Initial dynamics might be contained within a boundary, such as an agent moving
in a room or a robot staying within its joint limits. These constraints can be
incorporated by inverting an obstacle and ensuring that the dynamics remain
inside a boundary hull (Fig. 7). Analogously to [45], this is achieved by
inverting the distance function $\Gamma(\bm{\xi})$, the reference direction
$\bm{r}(\bm{\xi})$ and normal vector $\bm{n}(\bm{\xi})$, defined in Sec. II.
The distance function divides the space into free, boundary, and interior
points, as described in (3). Consequently, the distance for the boundary
obstacle can be defined as follows:

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

with power weight $p\in\mathbb{R}_{>0}$, we choose $p=1$.

As the normal direction points away from the surface, it naturally points
towards the interior of an inverted obstacle, in contrast to the normal
direction of a regular obstacle. Consequently, in order to utilize the star-
shaped constraint from (17) of
$\left\langle{-\bm{r}(\bm{\xi})},\,{\bm{n}(\bm{\xi})}\right\rangle>0$ for
rotational obstacle avoidance, we need to flip the reference direction:

$\bm{r}(\bm{\xi})={\left(\bm{\xi}^{r}-\bm{\xi}\right)}/{\|\bm{\xi}^{r}-\bm{\xi}\|}\qquad\forall\bm{\xi}\in\mathbb{R}^{N}\setminus\bm{\xi}^{r}$
(25)

Using the inverted values, the ROAM can be applied on as described throughout
this section (Fig. 7).

(a) Wavy DS in starshape

(b) Linear DS in ellipse

Figure 7: The inverted obstacle description is used to keep nonlinear dynamics
$\bm{f}(\bm{\xi})$ within star-shaped boundaries, in (a) with
$\bm{f}(\bm{\xi})=\bm{R}(\sin(\|\bm{\xi}^{a}-\bm{\xi}\|)(\bm{\xi}^{a}-\bm{\xi})$
where $\bm{R}(\cdot)$ is a two dimensional rotation matrix and $\bm{\xi}^{a}$
the attractor. Further, it can be used to create active repulsion from walls,
as in (b) for globall straight dynamics with attractor $\bm{\xi}^{a}$.

######  Lemma III.4.

The rotated dynamics $\dot{\bm{\xi}}$ evaluated according to (17) are
$C^{1}$-smooth and collision-free as shown in Theorem III.3 when navigating
within a boundary described as an inverted obstacle with distance function
$\Gamma(\bm{\xi})$ as defined in (24), and reference vector $\bm{r}(\bm{\xi})$
as given in (25).

###### Proof.

Since the distance function $\Gamma(\bm{\xi})$, average vector
$\bm{n}(\bm{\xi})$, and reference direction $\bm{r}(\bm{\xi})$ possess all the
necessary properties outlined in Theorem III.3, the collision avoidance
properties directly carry over.

The distance function $\Gamma(\bm{\xi})$ from (24) and normal direction (25)
are not defined at the reference point $\bm{\xi}^{r}$. However, at this point
the rotation weight $\lambda(\bm{\xi})$ reaches zeros:

$\lim_{\xi\rightarrow\xi^{r}}\Gamma(\bm{\xi})\rightarrow\infty\;\;\underset{\text{using}\;\eqref{eq:pulling_weight}}{\Rightarrow}\;\;\lim_{\xi\rightarrow\xi^{r}}\lambda(\bm{\xi})=0\;\;\Rightarrow\;\;\lim_{\xi\rightarrow\xi^{r}}\dot{\bm{\xi}}=\bm{f}(\bm{\xi})$
(26)

Thus, the rotation has no effect, and the system is smoothly defined. ∎

A more detailed analysis of inverted obstacles can be found in [45]. Further
development applies to both standard and inverted obstacles if not stated
otherwise.

##  IV General Nonlinear Motion

For systems characterized by small nonlinearities and a single attractor, the
convergence dynamics $\bm{c}(\bm{\xi})$ exhibit global straightness, as
defined in Definition II.1, resulting in desirable behavior. However, in
systems with high nonlinearities, this can lead to avoidance patterns that do
not accurately reflect the global dynamics, as illustrated by the example
shown in Figure 8(a). To address this issue, this section introduces modified
convergence dynamics that aim to maintain similarity with the original
dynamics while being locally straight, as depicted in Figure 8(b).

(a) Globally straight $\bm{c}(\bm{\xi})$

(b) Locally straight $\bm{c}(\bm{\xi})$

Figure 8:  For motion with initial dynamics of
$\bm{f}(\bm{\xi})=\begin{bmatrix}-1&2\\\ -2&-1\end{bmatrix}\bm{\xi}$ (gray)
and a single stationary point (black star), employing a globally straight
convergence dynamics $\bm{c}(\bm{\xi})$ results in a trajectory (orange) that
deviates significantly from the original motion (a). In contrast, by utilizing
locally straight convergence dynamics, the trajectory (b) maintains similarity
to the original motion throughout its course.

###  IV-A Nonlinear Motion without Stationary Point

Let us first focus on initial dynamics $\bm{f}(\bm{\xi})$ which do not have
any directional singularity point, i.e.,
$\nexists\bm{\xi}:\|\bm{f}(\bm{\xi})\|=0,\nabla\bm{f}(\bm{\xi})\neq 0$. The
convergence dynamics $\bm{c}(\bm{\xi})$ of such initial dynamics
$\bm{f}(\bm{\xi})$ is constructed by evaluating as a weighted sum of the
initial velocity at the reference point of the obstacle $\bm{\xi}^{r}$ and at
position $\bm{\xi}$:

$\bm{c}(\bm{\xi})=w^{c}(\bm{\xi})\bm{f}(\bm{\xi}^{r})\hat{+}(1-w^{c}(\bm{\xi}))\bm{f}(\bm{\xi})$
(27)

The convergence weight is chosen such that the convergence dynamics
$\bm{c}(\bm{\xi})$ is straight on the surface of the obstacle:

$w^{c}(\bm{\xi})=\begin{cases}1&\text{if}\;\;\Gamma(\bm{\xi})<1\\\
1/\Gamma(\bm{\xi})&\text{otherwise}\end{cases}$ (28)

where $\hat{+}$ describes the rotational summing described in Appendix -G.
Note that the rotation summing from (71) is not defined for two anti-collinear
vectors. Hence, we cannot apply this summing at a directional singularity
point, as will be further discussed in the next subsection.

######  Lemma IV.1.

The convergence dynamics
$\bm{c}(\bm{\xi}):\mathbb{R}^{N}\rightarrow\mathbb{R}^{N}$ as proposed in (27)
for initial dynamics without directional singularity points, i.e.,
$\left\\{\bm{\xi}:\|\bm{f}(\bm{\xi})\|=0,\nabla\bm{f}(\bm{\xi})\neq
0\right\\}=\emptyset$, are straight according to Definition II.1 on the
surface and inside the obstacle, i.e,
$\bm{c}(\bm{\xi})\in\mathcal{F}^{s},\;\bm{\xi}\in\mathcal{X}^{b}\cup\mathcal{X}^{i}$.

###### Proof.

Inside and on the surface of the obstacle, we have:

$\bm{\xi}\in\mathcal{X}^{b}\cup\mathcal{X}^{i}\quad\Rightarrow\quad\Gamma(\bm{\xi})\leq
1\quad\Rightarrow\quad w^{c}(\bm{\xi})=1$ (29)

Hence the dynamics in this region are given as:

$\bm{c}(\bm{\xi})=1\bm{f}(\bm{\xi}^{r})\hat{+}0\bm{f}(\bm{\xi})=\bm{f}(\bm{\xi}^{r})\qquad\bm{\xi}\in\mathcal{X}^{b}\cup\mathcal{X}^{i}$
(30)

Thus, we have locally collinear dynamics. ∎

As the convergence dynamics $\bm{c}(\bm{\xi})$ are straight on the surface of
the obstacle, they can be used in the rotational obstacle avoidance method
defined in Section III, see Figure 9.

(a) Nonlinear dynamics to follow the line at $y=0$ in red

(b) Convergence dynamics $\bm{c}(\bm{\xi})$ are locally straight in the gray
subset

(c) Rotated avoidance without local minima

Figure 9: The line following dynamics
$\bm{f}(\bm{\xi})=[1\;\;-\bm{\xi}_{2}]^{T}$ shown in (a) uses locally straight
convergence dynamics (b) to ensure the absence of local minima when avoiding
the obstacle (c).

###  IV-B Nonlinear Motion in the Presence of a Stationary Point

Many initial dynamics can be characterized by a motion with a single
directional singularity point, i.e.,
$\left\\{\bm{\xi}:\|\bm{f}(\bm{\xi})\|=0,\nabla\bm{f}(\bm{\xi})\neq
0\right\\}=\\{\bm{\xi}^{a}\\}$. This singularity point may correspond to a
desired attractor point or arise from a limit cycle where an unstable
stationary point resides at its center. In the presence of such a singularity
point, the directional summing approach employed in (27) cannot be directly
applied. This is because we cannot smoothly define local rotations around
$\bm{\xi}^{a}$. However, we can overcome this limitation by unfolding the
space using a sequence of mappings (Fig. 10).

####  IV-B1 Shrinking and Inverse Mapping

The first step is to shrink the obstacles to a single point, i.e., all
boundary points of the obstacle $\mathcal{X}^{b}$ are mapped to the reference
point $\bm{\xi}^{r}$.

######  Definition IV.1 (Shrinking Mapping).

The shrinking mapping
$\bm{m}^{s}({\bm{\xi}}):\mathcal{X}^{e}\rightarrow\mathbb{R}^{N}\setminus\bm{\xi}^{r}$
defined as

$\bm{m}^{s}({\bm{\xi}})={\bm{\xi}}^{r}+\bm{r}({\bm{\xi}})\left(\|{\bm{\xi}}-{\bm{\xi}}^{r}\|-\|\bm{\xi}^{b}-\bm{\xi}^{r}\|\right)\;\;\forall\bm{\xi}\in\mathcal{X}^{e}$
(31)

is a bijection, and it maps the point on the obstacle’s surface to the
obstacle’s reference direction, i.e., $\lim_{\Gamma(\bm{\xi})\rightarrow
1}\bm{m}^{s}({\bm{\xi}})\rightarrow{\bm{\xi}}^{r}$.

Analogously, we define the inverse of this mapping:

######  Definition IV.2 (Inflating Mapping).

The inflating mapping
$\bm{m}^{i}({\bm{\xi}}):\mathbb{R}^{N}\setminus{\bm{\xi}}^{r}\rightarrow\mathcal{X}^{e}$
defined as:

$\bm{m}^{i}({\bm{\xi}})={\bm{\xi}}^{r}+\bm{r}({\bm{\xi}})\left(\|{\bm{\xi}}-{\bm{\xi}}^{r}\|+\|\bm{\xi}^{b}-\bm{\xi}^{r}\|\right)\;\;\forall{\bm{\xi}}\neq{\bm{\xi}}^{r}$
(32)

is a bijection and the inverse function of the shrinking mapping defined in
(31), i.e.,
$\bm{m}^{i}\circ\bm{m}^{s}(\bm{\xi})=\bm{\xi}\;\;\forall\bm{\xi}\in\mathcal{X}^{e}$

The effect of shrinking and inflation mapping can be observed in Figure 10.

####  IV-B2 Folding Mapping

Let us introduce a folding mapping $\bm{m}^{f}(\bm{\xi})$, which moves the
stationary point infinitely far away. Hence, the dynamics in the mapped space
are without directional singularity point, which can be treated with the
method introduced in Section IV-A. Conversely, the surface of the obstacle
should not be affected by the mapping. The desired properties of this folding
mapping are given as follows:

  1. 1.

the stationary point gets mapped infinitely far away

$\lim_{\bm{\xi}\rightarrow\bm{\xi}^{a}}\Gamma\left(\bm{m}^{f}({\bm{\xi}})\right)\rightarrow\infty$
(33)

  2. 2.

at the reference point (which represents the surface of the obstacle after the
shrinking mapping), the effect of the mapping vanishes

$\bm{\xi}^{r}=\bm{m}^{f}({\bm{\xi}^{r}})$ (34)

Let us first define the unit directions in the coordinate system, which has
its center at the stationary point and the first axis points towards the
reference direction of the obstacle:

$\hat{\bm{\xi}}=\bm{B}^{T}(\bm{\xi}-\bm{\xi}^{a})/\|\bm{\xi}-\bm{\xi}^{a}\|$
(35)

where $\bm{B}$ is the orthonormal matrix of which the first row aligns with
${\bm{\xi}}^{r}-{\bm{\xi}}^{a}$

The desired constraints of the mapping from (33) and (34) can be achieved by
uniformly stretching along dimensions $i\in[2..N]$ of $\hat{\bm{\xi}}$ as:

$\begin{split}\bm{s}_{i}=\left(2/(1+p)-1\right)^{g}\frac{\hat{\bm{\xi}}_{i}}{\|\hat{\bm{\xi}}_{[2:N]}\|}\quad
p\in]-1,\,1],\;\bm{\xi}\neq\bm{\xi}^{a}\\\ \text{with}\qquad
p=\frac{\left\langle\bm{\xi}-\bm{\xi}^{a},\,\bm{\xi}^{r}-\bm{\xi}^{a}\right\rangle}{\|\bm{\xi}-\bm{\xi}^{a}\|\,\|\bm{\xi}^{r}-\bm{\xi}^{a}\|}\end{split}$
(36)

where $g\in\mathbb{R}_{>0}$ is the power factor, we choose $g=2$. The above
equation pushes points opposite the attractor relative to the obstacle
infinitely far away, i.e.,
$\lim_{\bm{\bm{\xi}}_{1}\rightarrow-1}\bm{s}_{i}\rightarrow\infty$, see the
green line in Figure 10.

Finally, the stretching of the folding mapping along dimension $i=1$ is
constructed as follows:

$\bm{s}_{1}=\|{\bm{\xi}}^{r}-{\bm{\xi}}^{a}\|\left(1+\ln\left(\frac{\|{\bm{\xi}}-{\bm{\xi}}^{a}\|}{\|{\bm{\xi}}^{r}-{\bm{\xi}}^{a}\|}\right)\right)$
(37)

Using this, the folding mapping
$\bm{m}^{f}({\bm{\xi}}):\\{{\bm{\xi}}\in\mathbb{R}^{N}:\hat{\bm{\xi}}_{1}\neq-1,\bm{\xi}\neq\bm{\xi}^{a}\\}\rightarrow\mathbb{R}^{N}$
is defined as:

$\bm{m}^{f}({\bm{\xi}})=\bm{B}\,\text{diag}(\bm{s})\bm{B}^{T}(\bm{\xi}-\bm{\xi}^{a})+\bm{\xi}^{a}\quad\forall{\bm{\xi}}\in\mathbb{R}\setminus{\bm{\xi}}^{a}$
(38)

Figure 10:  The initial dynamics $\bm{f}(\bm{\xi})$ undergo a series of three
consecutive mappings, depicted in a clockwise manner starting from the top
left. The convergence weight $w^{c}(\bm{\xi})$ is evaluated in the transformed
space, where its value is represented by the gray shading. Notably, the weight
diminishes to zero at the attractor point $\bm{\xi}^{a}$. The directional
summing operation from (27) is performed to obtain the convergence dynamics
$\bm{c}(\bm{\xi})$. This step is carried out in the mapped space. To obtain
the reverse mapping, the vector rotation $\bm{v}_{r}(\cdot)$ is simply
evaluated in the original space. The folding mapping $\bm{m}^{f}(\bm{\xi})$
maps the attractor infinitely far away in the $-\hat{\bm{\xi}}_{1}$ direction,
whereas the line with $\hat{\bm{\xi}}_{1}=-1$ is folded out to be infinitely
far in the directions of $\pm\hat{\bm{\xi}}_{[2..d]}$.

######  Lemma IV.2.

The mapping
$\bm{m}^{f}(\bm{\xi}):\bm{m}^{f}({\bm{\xi}}):\\{{\bm{\xi}}\in\mathbb{R}^{N}:\hat{\bm{\xi}}_{1}\neq-1,\bm{\xi}\neq\bm{\xi}^{a}\\}\rightarrow\mathbb{R}^{N}$
as given in (38) is a bijection, and smoothly defined, i.e.,
$\lim_{{\bm{\xi}}\rightarrow{\bm{\xi}}_{j}}\bm{m}^{f}({\bm{\xi}}_{i})=\bm{m}^{f}({\bm{\xi}}_{j})$.
Furthermore, the mapping has no effect at the reference point, i.e.,
$\bm{m}^{f}({\bm{\xi}}^{r})={\bm{\xi}}^{r}$, and the attractor is mapped
infinitely far away, i.e.,
$\lim_{\bm{\xi}\rightarrow\bm{\xi}^{a}}\Gamma\left(\bm{m}^{f}({\bm{\xi}})\right)\rightarrow\infty$.

###### Proof.

The system is made up of the two transformations, one along the
${\bm{\xi}}^{r}-{\bm{\xi}}^{a}$ as given in (37), and one perpendicular to,
given in in (36). Since all the underlying functions involved are smooth and
bijective, the resulting transformation is also smooth and bijective. However,
it is important to note that there are discontinuities at
${\bm{\xi}}={\bm{\xi}}^{a}$ and $\hat{\bm{\xi}}_{1}=-1$. This points are
excluded from the input set (in Eq. 42 we will additionally ensure that the
corresponding weight goes to zero, for a smooth effect).

Furthermore for an input value of ${\bm{\xi}}^{r}$ in (37), we get that
$\bm{s}_{1}=\|{\bm{\xi}}^{r}-{\bm{\xi}}^{a}\|$, and using (36) we get
$\bm{s}_{i}=0$ with $i\in[2..d]$. Hence, the transformation in (38) yields,

$\begin{split}\bm{m}^{f}({\bm{\xi}}^{r})&=\bm{B}\;\text{diag}(\bm{s})\bm{B}^{T}(\bm{\xi}^{r}-\bm{\xi}^{a})+\bm{\xi}^{a}\\\
&=\bm{B}\;\text{diag}(\bm{s})\begin{bmatrix}1&0&..&0\end{bmatrix}^{T}+\bm{\xi}^{a}\\\
&=\bm{B}\;\begin{bmatrix}\|\bm{\xi}^{r}-\bm{\xi}^{a}\|&0&..&0\end{bmatrix}^{T}+\bm{\xi}^{a}\\\
&=\bm{\xi}^{r}-\bm{\xi}^{a}+\bm{\xi}^{a}=\bm{\xi}^{r}\end{split}$ (39)

For the region $\hat{\bm{\xi}}_{1}=-1$, we get for the stretching factors
$s_{i}\rightarrow\infty$ for $d\in[2..d]$ from (36). ∎

####  IV-B3 Evaluation of the Relative Rotation

The total mapping can be written as follows:

$\bm{m}(\bm{\xi})=\bm{m}^{i}\circ\bm{m}^{f}\circ\bm{m}^{s}(\bm{\xi})$ (40)

Since this mapping transforms the attractor infinitely far away, it produces a
vector field without directional singularity, as Section IV-A requires.
Thereof, we can evaluate the convergence dynamics in the mapped space:

$\bar{\bm{c}}(\bm{\xi})=w^{m}\circ\bm{m}(\bm{\xi})\bm{f}(\bm{\xi}^{r})\hat{+}\bigl{(}1-w^{m}\circ\bm{m}(\bm{\xi})\bigr{)}\bm{f}({\bm{\xi}})$
(41)

the symbol $\hat{+}$ implies the directional summing as defined in Appendix
-G.

The mapping weight is defined as:

$w^{m}(\bm{\xi})=1/\sqrt{\bigl{(}\Gamma(\bm{\xi})-1\bigr{)}\bigl{(}\Gamma(m(\bm{\xi}))-1\bigr{)}+1}$
(42)

This weight function is designed to ensure a decreasing influence as the
distance from the obstacle increases, as well as a decreasing influence when
the position is opposite to the singularity point relative to the obstacle
(green line in Figure 10). Additionally, it ensures that the weight approaches
one when the position lies on the surface of the obstacle.

Finally, the mapping into the original space can be made through a vector
rotation

$\bm{c}(\bm{\xi})=\bm{v}_{r}\left(\beta,\bar{\bm{c}}\right)$ (43)

where the vector rotation $\bm{v}^{r}$ and angle $beta$ are obtained according
to Eq. (71), using input vector $\bm{v}_{i}=\bm{m}(\bm{\xi})-\bm{\xi}^{a}$ and
output vector $\bm{v}_{o}=\bm{\xi}-\bm{\xi}^{a}$.

######  Theorem IV.3.

The smoothly defined convergence dynamics $\bm{c}(\bm{\xi})$ as given in (43)
for obstacles in a vector field with a directional singularity point
$\bm{\xi}^{a}$, i.e.,
$\left\\{\bm{\xi}:\|\bm{f}(\bm{\xi})\|=0,\nabla\bm{f}(\bm{\xi})\neq
0\right\\}=\\{\bm{\xi}^{a}\\}$ is ensured to be straight according to
Definition II.1 on the surface and inside the obstacle, i.e.,
$\bm{c}(\bm{\xi})\in\mathcal{F}^{s},\;\bm{\xi}\in\mathcal{X}^{b}\cup\mathcal{X}^{i}$.

###### Proof.

Lemma IV.2 states that the unfolding has no effect at $\bm{\xi}^{r}$ in the
shrunk space, which is the surface of the obstacle in the original space as
stated in Definition IV.1. Further, the inflating mapping is the inverse of
the shrinking as stated in Definition IV.2. Thus, when approaching the
surface, i.e., $\Gamma(\bm{\xi})\rightarrow 1$, Lemma IV.1 transfers to
Theorem IV.3. ∎

####  IV-B4 Dynamical Systems with Multiple Attractors

A dynamical system can exhibit multiple attractors [49]. However, if the
system is continuous across space, there must exist a region known as the
saddle boundary. Points starting on the saddle boundary do not converge to any
specific attractor, and this boundary divides the space into regions that
contain only a single attractor each. By interpreting the saddle boundary as
an inverted obstacle, as described in Section III-C, the method presented in
this section can be applied to the local regions. This allows for effective
obstacle avoidance in the presence of multiple attractors, ensuring that the
system remains within specific regions corresponding to individual attractors.

####  IV-B5 Obstacles Across the Attractor

When an obstacle spans across the attractor $\bm{\xi}^{a}$, i.e.,
$\Gamma({\bm{\xi}}^{a})\leq 1$, the unfolding mapping is not defined. Hence,
the convergence dynamics $\bm{c}(\bm{\xi})$ must approach globally straight
dynamics according to Definition II.1. To account for this, the initial
dynamics are updated as follows:

$\bm{f}(\bm{\xi}^{r})\leftarrow
w^{\Gamma}\text{sign}\left(\nabla\bm{f}(\bm{\xi})|_{\bm{\xi}=\bm{\xi}^{a}}\right)(\bm{\xi}^{r}-\bm{\xi}^{a})+(1-w^{\Gamma})\bm{f}(\bm{\xi}^{r})$
(44)

The weight is designed to reach one when the obstacle reaches the attractor,
e.g., $w^{\Gamma}=1/\Gamma(\bm{\xi}^{a})$.

##  V Multi-Obstacle Environments

In the presence of multiple obstacles, the rotational obstacle avoidance
method for a single obstacle can be extended using a weighted rotational
summing. First, the weighted convergence dynamics $\bm{c}(\bm{\xi})$ as
introduced in (27) is averaged as follows:

$\bm{c}(\bm{\xi})=\bm{f}(\bm{\xi})\hat{+}{\sum_{o=1}^{N^{obs}}}w_{o}\bm{c}_{o}(\bm{\xi})=\bm{f}(\bm{\xi})\hat{+}{\sum_{o=1}^{N^{obs}}}w_{o}w^{c}_{o}\bm{f}(\bm{\xi}^{r}_{o})$
(45)

where $\hat{+}$ denotes the rotational summing as defined in Appendix -G.

The obstacle weights have been proposed in [45] as:

$w_{o}(\bm{\xi})=\frac{\tilde{w}_{o}(\bm{\xi})}{\sum_{i=1}^{N^{\mathrm{obs}}}\tilde{w}_{i}(\bm{\xi})}\;\;\text{with}\;\;w_{o}(\bm{\xi})=\frac{1}{\Gamma_{o}(\bm{\xi})}\quad\forall\,\bm{\xi}\in\mathcal{X}^{e}$
(46)

The weights associated with each obstacle ensure that their sum is at most one
and converge to zero as the distance from the respective obstacle increases.
Furthermore, on the surface of an obstacle $o$, the corresponding weight
equals 1. Moreover, the weighting allows for overlapping of the influence
regions of the obstacles.

These convergence dynamics are used to evaluate the preferred pseudo tangent
for each obstacle $\bm{e}_{o}(\xi)$ as defined in (14). Finally, the rotation
of the initial dynamics from (17) can be restated for multi-obstacle scenarios
as:

$\dot{\bm{\xi}}=\bm{f}(\bm{\xi})\hat{+}{\sum_{o=1}^{N^{obs}}}w_{o}\dot{\bm{\xi}}_{o}=\bm{f}(\bm{\xi})\hat{+}{\sum_{o=1}^{N^{obs}}}\lambda_{o}w_{o}m(\bm{\xi})\bm{e}_{o}(\bm{\xi})$
(47)

The method is summarized in Algorithm 1 and handles star-shaped obstacles and
boundaries, as shown in Figure 11.

Figure 11: The rotational obstacle avoidance with respect to wavy dynamics as
used in Figure 7(a) moves towards the attractor (black star) while avoiding
collisions inside a rectangular hull with two obstacles.

######  Lemma V.1.

The weighted rotation of the convergence dynamics $\bm{c}(\bm{\xi})$ from (45)
and the final dynamics $\dot{\bm{\xi}}$ from (47) conserves impenetrability
and saddle-point properties introduced in Theorem III.3.

###### Proof.

When approaching an obstacle $o$, the weight defined in (46) results in
$\lim_{\Gamma_{o}(\bm{\xi})\rightarrow 1}w_{o}(\bm{\xi})=1$. Hence the
evaluation of the multi-obstacle avoidance converges to the single obstacle
scenario of obstacle $o$. It follows that the properties of the single
obstacle case are conserved locally. ∎

Algorithm 1 Rotational Obstacle Avoidance Method (ROAM)

0: $\bm{f}({\bm{\xi}})$, $N^{\mathrm{obs}}$ obstacles

0: $\dot{\bm{\xi}}$

1: for $o=1$ to $N^{\mathrm{obs}}$ do

2: $\tilde{w}_{o}(\bm{\xi})=1/\Gamma_{o}(\bm{\xi})$ {Evaluate weights (46)}

3: end for

4:
$w_{o}(\bm{\xi})=\tilde{w}_{b}(\bm{\xi})/\sum_{i=1}^{N^{\mathrm{obs}}}\tilde{w}_{i}(\bm{\xi})$
{Normalize weights (46)}

5: for $o=1$ to $N^{\mathrm{obs}}$ if $w_{o}(\bm{\xi})>0$ do

6: $\bm{f}(\bm{\xi}^{r}_{o})$ {Compute DS at reference point}

7: end for

8:
$\bm{c}(\bm{\xi})=\bm{f}(\bm{\xi})\hat{+}{\sum_{o=1}^{N^{obs}}}w_{o}w^{c}_{o}\bm{f}(\bm{\xi}^{r}_{o})$
{Rotational average 45}

9: for $o=1$ to $N^{\mathrm{obs}}$ if $w_{o}(\bm{\xi})>0$ do

10: ${\bm{e}_{o}}(\bm{\xi})$ {Compute pseudo tangent (14)}

11: $h(\bm{\xi})$ {Compute magnitude (21)}

12: end for

13:
$\dot{\bm{\xi}}=\bm{f}(\bm{\xi})\hat{+}{\sum_{o=1}^{N^{obs}}}\lambda_{o}w_{o}m(\bm{\xi})\bm{e}_{o}(\bm{\xi})$
{Rot. average (47)}

###  V-A Dynamic Environments

v The closed-form description of the algorithm enables short computation time
without the need for offline trajectory planning. Dynamic obstacles can be
considered by transposing the avoided direction into the moving reference
frame:

$\dot{\bm{\xi}}=\bm{M}(\bm{\xi})\bm{f}(\bm{\xi})+\dot{\tilde{\bm{\xi}}}\quad\text{with}\quad\dot{\tilde{\bm{\xi}}}=\sum_{o=1}^{N^{\mathrm{obs}}}w_{o}\dot{\tilde{\bm{\xi}}}_{o}$
(48)

The method’s application to dynamics obstacles is experimentally evaluated in
Section VII-D2.

###  V-B Motion within Multiple Enclosing Hulls

The outer boundary might not be star-shaped, for example, when a robot moves
through a curvy corridor. However, there such a general space can be divided
into multiple stars-shapes [50], which can be used by ROAM to guide a motion
to stay within the boundary. This requires convergence dynamics, which
transition between the obstacles as:

$\bm{c}(\bm{\xi})=\bm{f}(\bm{\xi})\hat{+}{\sum_{b=1}^{N^{bnd}}}w_{b}(\bm{\xi})\bm{c}_{b}(\bm{\xi})$
(49)

where $N^{bnd}\in\mathbb{N}_{>0}$ is the number of boundaries. Furthermore,
the weights of the multi-boundary environment are set to:

$w_{b}(\bm{\xi})=\frac{\max\left(\Gamma_{b}(\bm{\xi}),1)\right)-1}{\sum_{i=1}^{N^{bnd}}\max\left(\Gamma_{i}(\bm{\xi}),1\right)-1}$
(50)

Where the local dynamics $\bm{c}_{b}(\bm{\xi})$ are locally straight according
to Definition II.1 in the subdomain of the surface of each boundary. The
attractor of each hull $\bm{\xi}^{a}_{b}$ is placed such that it lies outside
of the corresponding boundary $b$ for all boundaries that do not contain the
global attractor $\bm{\xi}^{a}$, i.e.,

$\begin{cases}\bm{\xi}^{a}_{b}=\bm{\xi}^{a}&\text{if}\;\;\Gamma_{b}(\bm{\xi}^{a})>1\\\
\bm{\xi}^{a}_{b}\in\mathcal{X}^{i}_{b}\cup\mathcal{X}^{b}_{b}&\text{otherwise}\end{cases}$
(51)

The boundary encapsulation can be seen in Figure 12.

(a) The data (position and velocity) are fit using Gaussian Mixture Model
(GMM) with four components [51], which is used to predict the velocity.

(b) The GMM components are used as a multi-hull environment to bind the
dynamics and enforce leaving the boundary in the direction of the local
attractors $\bm{\xi}^{a}_{b}$ (colored stars).

Figure 12:  Approaches for learning nonlinear motion from demonstration can
guarantee stability by ensuring that the system asymptotically converges
towards the attractor $\bm{\xi}^{a}$ [52]. However, they do not prohibit the
dynamics of taking an undesired shortcut or moving away from the data, which
is the known region (a). Convex hulls can be obtained through trajectory
learning methods, by interpreting the Gaussian Mixture Model applied to the
data as ellipse-shaped obstacles [3]. ROAM can enforce the final dynamics to
stay close to the data (b).

######  Lemma V.2.

A motion starting within multiple boundaries
$\\{\bm{\xi}\\}_{0}\in\bigcup_{b\in N^{bnd}}\mathcal{X}^{e}_{b}$ and evolving
according to $\dot{\bm{\xi}}$ as described in (47) and with convergence
dynamics $\bm{c}(\bm{\xi})$ defined in (49) will stay within the boundaries,
i.e, $\\{\bm{\xi}\\}_{t}\in\bigcup_{b\in
N^{bnd}}\mathcal{X}^{e}_{b}\;\;\forall\;t\in\mathbb{N}_{>0}$.

###### Proof.

If $\bm{\xi}$ is within multiple boundaries, and it approaches boundary $b$,
the weight given in in (50) evaluates to:

$\lim_{\Gamma_{b}(\xi)\rightarrow 1}w_{b}(\bm{\xi})=\frac{0}{0+\sum_{i\neq
b}^{N^{bnd}}\max\left(\Gamma_{b}(\bm{\xi}),1)\right)-1}=0$ (52)

Hence, boundary $b$ has no effect, and the motion can traverse any boundary
$b=1..N^{\mathrm{bnd}}$. Until, $\bm{\xi}$ is within only one boundary $b$. In
this case, the corresponding boundary weights simplify to:

$w_{b}(\bm{\xi})=\frac{\max\left(\Gamma_{b}(\bm{\xi}),1)\right)-1}{\max\left(\Gamma_{b}(\bm{\xi}),1)\right)-1+\sum_{i\neq
b}^{N^{bnd}}0}=1$ (53)

given that $\Gamma_{b}(\bm{\xi})>1$. Thus, the algorithm evolves according to
the single-boundary case, i.e., collision avoidance with the boundary $b$ is
ensured. ∎

##  VI General Concave Obstacles

A general obstacle can be described as a union of multiple star-shaped
obstacles [34], also referred to as trees of stars. Extending the algorithm to
such shapes enables navigating in many more environments. Let us for this
introduce a general obstacle using nomenclature from graph theory [53]:

######  Definition VI.1 (Tree of Obstacles).

A tree of star-shaped obstacles represents a shape without holes. Each
obstacle (node) in the tree can have multiple children (successors), but have
exactly one single parent (predecessor), except for the root obstacle which
does not have a parent. All obstacles have a non-zero intersection with their
parent and children. Obstacles are assigned a level $l$ in the tree, starting
from $l=0$ at the root.

###  VI-A Velocity Propagation through Obstacle Tree

The avoidance velocity $\dot{\bm{\xi}}$ in the presence of trees-of-obstacles
is obtained through the summed average of a rotation-tree as described in
Appendix -G. The tree is constructed as described in Algorithm 2, and the
individual steps are detailed below.

Algorithm 2 Avoidance of Tree-of-Obstacles

0: $\bm{f}(\cdot)$, $N^{\mathrm{com}}$ obstacle-components

0: $\dot{\bm{\xi}}$

1: $\bm{t}^{r}(\cdot)$ {Create rotation tree, see Algo. 4}

2: for $o=1\;\textbf{to}\;N^{\mathrm{com}}$ do

3: $\bm{s}_{0}=\bm{r}(\bm{\xi})$ {Set initial surface point}

4: $w^{h}_{o}\leftarrow\Gamma_{o}(\bm{s}_{0})$ {Compute hiding-weight (58)}

5: $w_{o}\leftarrow\Gamma_{o}(\bm{\xi})$ {Compute obstacle weights}

6: $c=o$ {Initialize $c$ to obstacle $o$}

7: for $l=l(n)\;\textbf{to}\;0$ do {From node to root}

8: $\bm{s}_{p(c)}=b(\bm{\xi}^{r}_{c}-\bm{s}_{c})+\bm{s}_{c}$ {Propagate
reference (54)}

9: $c=p(c)$ {Set iterator $c$ to parent $p$}

10: end for

11: $\bm{f}_{0}=\bm{f}(\bm{\xi}^{r})$ {Set initial tangent}

12: for $c=1\;\textbf{to}\;l(n)$ do {From root to node}

13:
$\bm{v}^{r}(\cdot),\beta\leftarrow(\bm{s}_{c}-\bm{\xi}^{r}_{c}),(\bm{s}_{p(c)}-\bm{\xi}^{r}_{p(c)})$
{Get rot. (71)}

14: $\bm{f}_{c}=\bm{v}^{r}(\bm{f}_{p(c)},\beta)$ {Propagate rotated velocity
(72)}

15: $\bm{f}_{c}\Rightarrow\bm{t}^{r}(\cdot)$ {Append to tree}

16: end for

17: $\dot{\bm{\xi}}_{o}\leftarrow\bm{f}_{o}$ {Avoidance with propagated
velocity (57)}

18: end for

19: $w_{f}=1-\sum_{o}w^{h}_{o}w_{o}$ {Weight of initial velocity
$\bm{f}(\bm{\xi})$}

20: $\dot{\bm{\xi}}\leftarrow\bm{t}^{r}(w_{f},w^{h}_{o}(\bm{\xi}))$ {Evaluate
rotation tree, Algo. 4}

####  VI-A1 Surface Point Propagation

The rotational avoidance of each obstacle $o=1..N^{\mathrm{com}}$ of the tree
is obtained at the corresponding surface point $\bm{s}_{o}$. The surface
points are obtained by propagating the position $\bm{\xi}$ through the
obstacle tree, starting from the an obstacle $o$ down to the root $r$:

$\bm{s}_{p}=b(\bm{\xi}^{r}_{c}-\bm{s}_{c})+\bm{s}_{c}\quad\text{such
that}\quad\Gamma_{p}(\bm{s}_{p})=1$ (54)

where $p$ is the parent of the component $c$. The factor $b\in\mathbb{R}_{>0}$
is evaluated such that $\bm{s}_{p}$ lies on the parent’s surface. Note, that
the obstacles are intersecting, hence we have $\Gamma(\bm{s}_{p})<1$. See the
surface points of a three-component tree in Figure 13.

####  VI-A2 Velocity Propagation

We can now propagate the velocity $\bm{f}(\bm{\xi}^{r})$ iteratively from
parent $p(c)$ to the component $c$ until we reach the respective obstacle $o$.
The iteration starts at the root $r$ and is done for each obstacle, except the
root. The propagated velocity $\bm{f}_{c}$ is obtained as follows:

$\bm{f}_{c}=\bm{v}^{r}(\bm{f}_{p(c)},\phi_{c})\qquad
o=1..N^{\mathrm{obs}}\setminus r$ (55)

where $\bm{v}^{r}(\cdot)$ is the vector rotation as described in (72). The
vector rotation is obtained with respect to the vectors
$\bm{v}_{i}=(\bm{s}_{c}-\bm{\xi}^{r}_{c})$ and
$\bm{v}_{o}=(\bm{s}_{p(c)}-\bm{\xi}^{r}_{p(c)})$ as described in (71). When
constructing the obstacle tree, the parent of an obstacle needs to be chosen
such that $\bm{v}_{i}$ and $\bm{v}_{o}$ are not anti-collinear. This is
ensured if the direction from the component to the parent is never opposing
the direction from the parent to the grand-parent (parent of the parent):

$\frac{\left\langle\bm{\xi}^{r}_{p(c)}-\bm{\xi}^{r}_{c},\,\bm{\xi}^{r}_{p(p(c))}-\bm{\xi}^{r}_{p(c)}\right\rangle}{\|\bm{\xi}^{r}_{p(c)}-\bm{\xi}^{r}_{c}\|\,\|\bm{\xi}^{r}_{p(p(c))}-\bm{\xi}^{r}_{p(c)}\|}\neq-1$
(56)

Figure 13: The surface points $\bm{s}_{(\cdot)}$ (purple) are evaluated for
each obstacle up to the root, to then propagate the desired tangent velocity
$\bm{e}$ (green) from the root to each obstacle.

####  VI-A3 Tangent Evaluation

Finally, after propagating the velocity to the obstacle $o$, the pseudo
tangent is evaluated. For trees-of-stars, we want to enforce the pseudo
tangent to be parallel to the surface at all times, hence we adopt (17) as
follows:

$\begin{split}&\text{if}\quad\frac{\left\langle-\bm{n},\,\bm{f}\right\rangle}{\|-\bm{n}\|\,\|\bm{f}\|}>\cos(R^{e}):\\\
&\qquad\bm{k}\left(-\bm{n},{\bm{e}_{o}}\right)=\bm{k}\left(-\bm{n},{\bm{r}}\right)+b\left(\bm{k}\left(-\bm{n},\bm{e}_{p}\right)-\bm{k}\left(-\bm{n},\bm{r}\right)\right)\\\
&\qquad\text{such
that}\quad\|\bm{k}\left(-\bm{n},\bm{e}_{p}\right)\|=R^{e},\quad
b\in\mathbb{R}_{>0}\\\ &\text{otherwise}\\\
&\qquad\bm{k}\left(\bm{n},{\bm{e}}_{o}\right)=\bm{k}\left(\bm{n},-{\bm{r}}\right)+b\left(\bm{k}\left(\bm{n},\bm{e}_{p}\right)-\bm{k}\left(\bm{n},-\bm{r}\right)\right)\\\
&\qquad\text{such
that}\quad\|\bm{k}\left(\bm{n},\bm{e}_{p}\right)\|=\pi-R^{e},\quad
b\in\mathbb{R}_{>0}\end{split}$ (57)

####  VI-A4 Hiding Weights

An obstacle $o$ which is occluded by its parent $p(o)$ should not influence
the avoidance velocity. For this reason, we introduce the hiding weight which
reaches zero at full occlusion:

$\begin{split}w^{h}_{o}=\begin{cases}1&\text{if}\;\;\Gamma(\bm{s}_{o})>1\\\
\Gamma(\bm{s}_{o})^{\frac{1}{1-b}}&\text{if}\;\;b<1\\\
0&\text{otherwise}\end{cases}b=\frac{\left\langle\bm{\xi}-\bm{\xi}^{r}_{o},\,\bm{\xi}^{r}_{p(o)}-\bm{\xi}^{r}_{o}\right\rangle}{\|\bm{\xi}-\bm{\xi}^{r}_{o}\|\,\|\bm{\xi}^{r}_{p(o)}-\bm{\xi}^{r}_{o}\|}\end{split}$
(58)

######  Lemma VI.1.

Let us assume a tree with obstacle components $o=1..N^{\mathrm{com}}$ and the
corresponding reference points $\bm{\xi}^{r}_{o}$, which all lie within
obstacle $o$ and the corresponding parent $p$, i.e.,
$\bm{\xi}^{r}_{o}\in\mathcal{X}^{i}_{o}\cap\mathcal{X}^{i}_{p(o)}$.
Conversely, the reference of each parent lies outside of the obstacle, i.e.,
$\bm{\xi}^{r}_{p(o)}\in\mathcal{X}^{i}_{p(o)}\setminus\mathcal{X}^{i}_{o}$.
Moreover, the direction from obstacle to parent is never opposing the
direction from the parent to the grandparent as described in (56). Let us
assume the dynamics $\bm{f}(\bm{\xi})$ are locally straight in the surrounding
of the obstacle according to Definition II.1. The vector field
$\dot{\bm{\xi}}$ obtained through the propagation of the velocity
$\bm{f}(\bm{\xi})$ as described in (55), with the rotation of the final
velocity given by (57), and vector tree summing using the weights $w^{h}_{o}$
given in (58) ensures collision avoidance according to the boundary condition
(13) with the absence of local minima in free-space.

###### Proof.

As shown in Lemma III.1, the tangent is defined everywhere as long as the
reference direction $\bm{r}_{o}(\bm{\xi})$ is not parallel to the propagated
velocity $\bm{f}_{o}(\bm{\xi})$. Moreover, smoothness is ensured due to the
adaptable rotation weight $\lambda(\bm{\xi})$ as shown in Theorem III.3.

Furthermore, the rotation defined in (55) is smoothly defined for obstacle-
parent-pair, due to the fact that the reference point of the parent is
required to lay outside of the child obstacles, and the parent-opposing
inequality from (56).

This is also ensured for the last surface point $s_{o}$, i.e., the projection
of the position on the obstacle’s surface since the hiding weight $w^{h}_{o}$
goes to zero if the two vectors are opposing:

$\frac{\left\langle\bm{s}_{c}-\bm{\xi}^{r}_{c},\,\bm{s}_{p(c)}-\bm{\xi}^{r}_{p(c)}\right\rangle}{\|\bm{s}_{c}-\bm{\xi}^{r}_{c}\|\,\|\bm{s}_{p(c)}-\bm{\xi}^{r}_{p(c)}\|}\;\;\Rightarrow\;\;b=1\;\;\Rightarrow\;\;w^{h}=0$
(59)

Hence, the corresponding tangent and the vector can be omitted.

Since all weights and corresponding vectors are smoothly defined, according to
Lemma .1 the vector tree evaluation leads to continuous and minima-free
dynamics.

Furthermore, from Lemma V.1 we know that the impenetrability of the obstacles
is preserved as a single component dominates when approaching the
corresponding surface. ∎

Figure 14: Obstacle avoidance of a two-dimensional human with a total of seven
sub-obstacles (corresponding reference points as black crosses) of a circular
motion with a single stationary point (black star).

###  VI-B Convergence Sequence

The obstacle avoidance algorithm is dependent on locally straight dynamics on
the surface of the tree-of-obstacles. Since the general shapes described can
be encapsulating directional singularity points, such as attractors, the
design of convergence direction requires special care. The computation has to
ensure smoothness when getting closer to the directional singularity point
$\bm{\xi}^{a}$. It is detailed in Algorithm 3.

Algorithm 3 Convergence Direction for Tree-of-Obstacles. The abbreviation
pred. refers to the predecessor of the direction tree.

0: $\bm{f}({\bm{\xi}})$, $N^{\mathrm{com}}$ obstacle-components

0: $\bm{c}(\bm{\xi})$ Locally straight dynamics

1: $w^{m}_{1}(\bm{\xi})$ {Mapping weight for root component (42)}

2:
$\bm{f}(\bm{\xi}),(\bm{\xi}-\bm{\xi}^{a})\Rightarrow\bm{v}^{r,i}(\cdot),\beta_{i}$
{Initial rotation (71)}

3:
$\bm{v}^{r,c}(\cdot),\beta_{c}\leftarrow\bm{f}(\bm{\xi}^{r}),(\bm{\xi}^{r}-\bm{\xi}^{a})$
{Convergence rot. (71)}

4:
$\bm{v}^{r,a}(\cdot),\beta_{a}\leftarrow(\bm{\xi}-\bm{\xi}^{a}),(\bm{\xi}^{r}-\bm{\xi}^{a})$
{Get rotation (71)}

5:
$\bm{t}^{r}_{1}(\cdot)\leftarrow\bm{v}^{r,i}(\cdot),\bm{v}^{r,a}(\cdot),\bm{v}^{r,c}(\cdot)$
{Create rotation tree}

6: {Tree reduction with Algo. 1 using vector-weight pairs:}
$\bm{t}_{1}^{r}\left([\bm{f}(\bm{\xi}^{r}),w^{m}_{1}],[\bm{f}(\bm{\xi}),(1-w^{m}_{1})]\right)\Rightarrow\bm{t}^{r}(\cdot)$

7: for $n=1\;\mathbf{to}\;N^{\mathrm{com}},i\neq r$ do {Iteration over
components}

8: $c\leftarrow n$ {Set initial node}

9: $w^{m}_{i}(\bm{\xi})$ {Mapping weight (42)}

10: for $l=l(n)\;\mathbf{to}\;0$ do {From node to root}

11: $(\bm{\xi}^{r}_{p(c)}-\bm{\xi}^{r}_{c})\Rightarrow\bm{t}^{r}(\cdot)$
{Append to tree with pred. $c$}

12: $c\leftarrow p(c)$ {Set current node iterator to its parent}

13: end for

14: $\bm{v}^{r,c}(\cdot)\Rightarrow\bm{t}^{r}(\cdot)$ {Append to tree with
pred. $c=r$}

15: end for

16: $w^{n}_{o}\leftarrow w^{m}_{o}$ {Weight normalization (60)}

17: $\bm{c}(\bm{\xi})\leftarrow\bm{t}^{r}(w^{n}_{o})$ {Tree reduction, Algo.
4}

###  VI-C Mapping Weight Normalization

A mapping weight $w^{m}_{o}$ as introduced in (60) approaching one indicates
that the $\bm{\xi}$ is on the surface of the corresponding obstacle $o$. To
ensure that this weight remains high while taking into account other
obstacles, the normalized weights are defined as follows:

$w^{n}_{o}=\begin{cases}\hat{w}^{n}_{o}/\sum_{i}^{N^{\mathrm{com}}}\hat{w}^{n}_{i}&\text{if}\;\;\sum_{i}^{N^{\mathrm{com}}}\hat{w}^{n}_{i}>1\\\
\hat{w}^{n}_{o}&\text{otherwise}\end{cases}\;,\;\;\hat{w}^{n}_{o}=1/(1-w_{o})$
(60)

for all obstacle $o=1...N^{\mathrm{obs}}$.

###  VI-D Practical Considerations

As for each obstacle, the algorithm needs to propagate the whole obstacle
tree. The presence of multiple obstacles and long trees can lead to many
computations. Therefore, it is advised to adapt the distance function
$\Gamma_{o}$ to approach infinity after a certain distance for outer leaves,
as proposed in (70). However, the distance function of the root $\Gamma_{r}$
should decrease slower, i.e.,

$\left\|\frac{d}{d\bm{\xi}}\Gamma_{o}(\bm{\xi})\right\|\geq\left\|\frac{d}{d\bm{\xi}}\Gamma_{r}(\bm{\xi})\right\|$
(61)

This has the effect that the algorithm only considers the obstacle core far
away, while when approaching the obstacle, the more detailed leaf structure is
considered. This leads to a sparse evaluation tree and enables real-time
applicability.

##  VII Evaluation

###  VII-A Computational Cost

The most computationally intensive part of the ROAM algorithm is the matrix-
vector multiplication in $N$ dimensions for the stereographic projections and
unfolding mappings. Therefore, the algorithm’s complexity, given $O$
obstacles, $K$ components, and an obstacle tree of level $L$, can be expressed
as $\mathcal{O}(N^{2}OKL)$.

###  VII-B Obstacle Avoidance While Following a Stable Limit Cycle

####  VII-B1 Setup

We compare the three algorithms MuMo [45], VF-CAPF [40], and ROAM (proposed
approach). The chosen scenario uses initial dynamics and the obstacle
distribution as proposed in [40]. The initial dynamics represent a circular
limit cycle of the form:

$\bm{f}(\bm{\xi})=\left(\begin{bmatrix}0&1\\\
-1&0\end{bmatrix}+2(R_{0}-\|\bm{\xi}\|){\bm{I}}\right)\bm{\xi}$ (62)

where the circle radius is $R_{0}=2$, and $\bm{I}$ is the two-dimensional
identity matrix. The environment contains six convex obstacles, and the agent
is aware of their location at all times (see Fig. 15). A grid of 10x10 evenly
distributed starting points was constructed, of which 93 were in free space.
The trajectory is evaluated for these starting points through Euler
integration with a time step $dt=0.01s$ and a maximum of 500
iterations.444Source code on
https://github.com/hubernikus/nonlinear_obstacle_avoidance.git, 2022/02/31

Figure 15: MuMo [45], VF-CAPF [40] and ROAM are used to guide a nonlinear,
limit-cycle-following vector field around six concave obstacles. The resulting
limit cycles are visualized as a solid line in the respective color.
Additionally, the trajectories from specific starting points (black circles)
are visualized with a dashed line.

####  VII-B2 Metrics

The trajectories are compared by observing the local minima, the distance to
the desired limit cycle, and the similarity between the velocity after
obstacle avoidance and the initial velocity. Furthermore, we look at the
evolution of the velocity over time, i.e., the discrete acceleration.

The root mean square error (RMSE), i.e.,
$\text{RMSE}=\sum^{M}_{m=1}\|\bm{v}_{i}-\bm{u}_{i}\|$ is used, as well as the
normalized inverted cosine similarity (NICS)

$\mathrm{NICS}=\frac{1}{2}\left(1-\frac{1}{M}\sum_{m=1}^{M}\frac{\left\langle\bm{v}_{i},\,\bm{u}_{i}\right\rangle}{\|\bm{v}_{i}\|\,\|\bm{u}_{i}\|}\right)$
(63)

with $\text{NICS}\in[0,1]$. Note that the smaller NICS, the higher the
similarity of the two vectors.

####  VII-B3 Results

In Table II, it can be observed that MuMo is the only method among the three
that results in trajectories ending up in local minima on the surface of an
obstacle in almost half of the cases. These local minima lie on the limit
cycle, suggesting that by increasing the step number, all MuMo trajectories
would eventually converge to these local minima. Due to this behavior, MuMo is
deemed inappropriate for the proposed scenario. Its limited applicability to
nonlinear initial dynamics as pointed out in Table I.

The focus of the comparison is thus between ROAM and VF-CAPF. ROAM
demonstrates better path following, with trajectories maintaining a shorter
distance to the reference circle throughout the motion. Moreover, ROAM
exhibits lower acceleration along the path, indicating smoother motion with
fewer abrupt changes. Additionally, ROAM shows higher similarity to the
initial dynamics compared to VF-CAPF.

These findings can be qualitatively observed in Figure 15, where VF-CAPF
closely follows the initial path (gray) outside the obstacle region and
deviates only when in close proximity to the obstacle. The metrics indicate
that this behavior leads to higher accelerations and more significant
deviations from the initial trajectory overall. On the other hand, MuMO moves
more directly towards the limit cycle compared to ROAM, but at the expense of
creating local minima on the surface of the bottom obstacle.

| ROAM | MuMo | VF-CAPF | Original
---|---|---|---|---
| (proposed) |  |  | dynamics
$N^{m}$ | 0% | 48% | 0% | 0%
$\text{RMSE}(\bm{\xi},R_{0})$ | 1.46 $\pm$ 1.11 | 1.24 $\pm$ 1.15 | 1.48 $\pm$ 0.87 | 0.93 $\pm$ 0.96
$\text{RMSE}(\dot{\bm{\xi}},\bm{f}_{0})$ | 0.15 $\pm$ 0.07 | 0.04 $\pm$ 0.09 | 0.47 $\pm$ 0.43 | 0.00 $\pm$ 0.00
$\text{NICS}(\dot{\bm{\xi}},\bm{f}_{0})$ | 0.04 $\pm$ 0.02 | 0.01 $\pm$ 0.02 | 0.12 $\pm$ 0.11 | 0.00 $\pm$ 0.00
$\text{RMSE}(\dot{\bm{\xi}}_{t},\dot{\bm{\xi}}_{t+1})$ | 2.51 $\pm$ 2.66 | 2.04 $\pm$ 5.98 | 3.47 $\pm$ 2.14 | 0.27 $\pm$ 0.17
$\text{NICS}(\dot{\bm{\xi}}_{t},\dot{\bm{\xi}}_{t+1})[10^{-4}]$ | 0.63 $\pm$ 0.66 | 0.51 $\pm$ 1.49 | 1.08 $\pm$ 0.82 | 0.07 $\pm$ 0.04

TABLE II: The different trajectories are compared in (1) the ratio of
trajectories which end up in a local minimum on the obstacle $N^{m}$, (2) the
distance to the desired trajectory, i.e., the difference to radius $R_{0}$.
Furthermore, (3) RMSE and NICS between all approaches to the original DS are
evaluated, as well as (4) the change of the dynamics over time (corresponds to
acceleration). The mean and standard deviation are evaluated over the 93
trajectories.

###  VII-C Nonlinear Path Following with Autonomous Wheelchair

####  VII-C1 Experimental Setup

(a) QOLO-robot navigating along a wavy road using global PF

(b) Local straight - initial

(c) Local straight - avoiding

(d) Local PF - initial

(e) Local PF - avoiding

(f) Global PF - initial

(g) Global PF - avoiding

Figure 16:  The robot is navigating between static tables on a wavy road
(gray) through a grass field (green). The initial dynamics and the reference
trajectory (orange) are on the left, with the corresponding obstacle dynamics
on the right. The local attractors (colored starts) and switching regions
(colored lines) are used to create global dynamics (a, c). The global path
following (f) uses a single attractor only.

In the second evaluation, the autonomous wheelchair QOLO was employed, and
various initial vector fields combined with avoidance methods were tested. The
control point of the wheelchair was positioned in front of the wheel axis to
ensure effective maneuverability. To account for the shape and size of the
QOLO wheelchair, a margin of 0.7 meters was added to the obstacles during the
evaluation [45].

The path followed by the wheelchair consisted of three road segments, denoted
as $s=1$, the segment closest to the goal, up to $s=3$. Although there was no
strict constraint to remain within the road boundaries, the initial dynamics
were specifically designed to guide the wheelchair towards the center of the
road, promoting adherence to the desired path (see Fig. 16(a)).

Seven tables were randomly placed along the path, with their centers
positioned on the path itself. The tables were positioned away from the
starting point and the attractor. Additionally, no more than two tables
intersected, including the margin, allowing all table shapes to be represented
as star-shapes, as required by MuMo [45].

####  VII-C2 Navigation Algorithms

Three approaches are used to navigate in this environment.

Local straight dynamics (2) are combined with MuMo [45]. The initial dynamics
consist of three separate vector fields with corresponding local attractors
(star), see Fig. 16(b). The attractor switches when transitioning from one
region to the next (crossing the line).

Local PF (path following) is combined with ROAM. The local PF dynamics are
given as:

$\bm{f}_{s}(\bm{\xi})=\bm{u}_{s}+\left\langle{\Delta\bm{\xi}},\,{\bm{u}_{s}}\right\rangle\bm{u}_{s}-\Delta\bm{\xi}\;\;\;\text{with}\;\;\;\Delta\bm{\xi}=(\bm{\xi}-\bm{\xi}^{a}_{s})$
(64)

for all segments $s=1..3$, where $\bm{u}_{s}\in\mathbb{R}^{2}$ is the (local)
nominal direction pointing along the road segment, and
$\bm{\xi}^{a}_{s}\in\mathbb{R}^{2}$ is the local attractor.

Global PF dynamics is evaluated by using directional-tree averaging as
described in Appendix -G. The root of the direction-tree is given as
$\bm{v}_{0,1}=\bm{\xi}-\bm{\xi}^{a}$. The direction tree is populated
iteratively:

  * •

$\bm{v}_{s,1}(\bm{\xi})=\bm{u}_{s}$ with respective parent direction
$\bm{v}_{s-1,1}$

  * •

$\bm{v}_{s,2}(\bm{\xi})=\bm{f}_{s}(\bm{\xi})$ with respective parent direction
$\bm{v}_{s,1}$

for all segments $s=1..3$. The final dynamics are obtained through the
weighted evaluation described Algorithm 4, using the following weights

$w_{s,1}(\bm{\xi})=0,\;\;w_{s,2}(\bm{\xi})=\frac{1}{d_{s}}(1+\min(\left\langle{\bm{v}_{s}},\,{\bm{\xi}^{a}_{s}-\bm{\xi}}\right\rangle,0))$
(65)

where $d_{s}\in\mathbb{R}_{\geq 0}$ the distance to the line segment $s$. The
segment weights $w_{s}(\bm{\xi})\in[0,1]$ are normalized if their sum exceeds
one.

####  VII-C3 Results

The combination of the global PF with ROAM ensures the convergence of all
trajectories, as shown in Table III. The other two methods achieve a
convergence rate of approximately 92 %. This can be attributed to the use of a
high-level planner, specifically switching between the local dynamics. Since
this conflicts with the guarantees of absences of local minima. While more
sophisticated switching or transitioning methods may exist, to the best of our
knowledge, there is no global path sequencer that can guarantee these
convergence properties within a finite time. Furthermore, when using the local
potential field (PF) with ROAM, the robot spends less time on the desired path
and has a greater average distance to the path boundaries compared to the
local straight algorithm combined with MuMo. However, the average distance
traveled remains approximately the same across all methods.

| Local straight [45] | Local PF | Global PF
---|---|---|---
Converged [] | 92% | 92% | 100%
Off-track [%] | 1.49 $\pm$ 0.00 | 0.79 $\pm$ 0.00 | 1.89 $\pm$ 0.00
$\Delta d$ [m] | 1.54 $\pm$ 0.04 | 1.73 $\pm$ 0.02 | 1.65 $\pm$ 0.04
Distance [m] | 20.2 $\pm$ 3.4 | 21.0 $\pm$ 3.8 | 20.8 $\pm$ 2.8
TABLE III:  The three approaches for following the local path are compared
based on the following metrics: the convergence ratio to the attractor, the
ratio of trajectories deviating from the path, the distance to the road border
$\Delta d$, and the total length of the trajectory. The reported values
represent the mean and standard deviation calculated from 100 runs with
randomly distributed furniture, while keeping the start and endpoints
consistent.

###  VII-D Obstacle Avoidance in Three Dimensions

####  VII-D1 Spiraling Motion Around Human in Simulation

Inspired by (62), we propose spiraling dynamics as:

$\begin{split}\bm{f}(\bm{\xi})=\bm{B}^{T}\left(\begin{bmatrix}0&1\\\
-1&0\end{bmatrix}+2\left(R_{0}-\|\bm{B}\tilde{\bm{\xi}}\|\right){\bm{I}}\right)\bm{B}\tilde{\bm{\xi}}+\bm{p}({\bm{\xi}})\\\
\text{with}\;\;\bm{B}=\begin{bmatrix}1&0&0\\\
0&0&1\end{bmatrix}\quad\text{and}\quad\bm{p}(\bm{\xi})=\begin{bmatrix}0&1&0\end{bmatrix}^{T}\end{split}$
(66)

with the spiraling radius of $R_{0}=$ $0.1\text{\,}\mathrm{m}$ and
$\tilde{\bm{\xi}}=\bm{\xi}-\bm{\xi}^{a}$ the relative position with respect to
the center $\bm{\xi}^{a}$.

The obstacle tree representing the human in ROAM consists of components
corresponding to the limbs and main body, with the core serving as the root of
the tree. When applying ROAM for collision avoidance, see Figure 17, it can be
observed that all trajectories successfully avoid the human without becoming
trapped in local minima. Furthermore, the dynamics of the system return to the
initial state, far away from the obstacle, both at the beginning and after
successfully avoiding the collision. This behavior highlights the
effectiveness of ROAM in generating smooth and convergent trajectories while
maintaining the desired dynamics of the system.

(a) Initial spiraling motion $\bm{f}(\bm{\xi})$

(b) Rotated motion around human $\bm{\xi}$

Figure 17: ROAM is used to guide trajectories from 16 different initial
positions and ensures that all trajectories successfully avoid the static
human in simulation.

####  VII-D2 Qualitative Evaluation on Robot Arm

Experiments with the real robot were performed using the 7 DoF Panda robot by
Franka Emika on a fixed base (see Fig. 18). The scenario chosen is the
automated disinfecting of a running conveyor belt, which transports various
parcels. The initial dynamics $\bm{f}(\bm{\xi})$ are similar to the spiral
motion in (66), but the basis $\bm{B}$ and perpendicular dynamics
$\bm{p}(\bm{\xi})$ given by:

$\bm{B}=\begin{bmatrix}1&0&0\\\
0&1&0\end{bmatrix}\quad\text{and}\quad\bm{p}(\bm{\xi})=\begin{bmatrix}0&0&\left(\bm{\xi}^{a}(t)-\bm{\xi}\right)\end{bmatrix}^{T}$
(67)

Moreover, the attractor is dynamic and moves back and forth the conveyer belt:

$\bm{\xi}^{a}(t)=\begin{bmatrix}0.5&0.6+0.1\sin\left(\frac{\pi}{10}t\right)&0.3\end{bmatrix}^{T}$
(68)

As a result, the initial dynamics $\bm{f}(\bm{\xi},t)$ are time-varying, too.

It is assumed that the robot has complete knowledge of the relative position
and shape of the conveyor belt. The position and velocity of the parcel are
determined using reflective markers (Optitrack). The analytical shape of the
objects is known analytically. Additionally, an operator is handling parcels
on the conveyer belt, but the robot is not directly aware of its presence.
However, since an impedance controller is employed [7], the robot is capable
of adapting to physical interactions.

Figure 18: The robot is aware of the two obstacles (brown shapes) as well as
the conveyor belt (green block) to obtain the avoidance dynamics (blue arrow).
The center of the initial dynamics (blue dot) is moving across the conveyor
belt.

The robot successfully avoids both the parcels and the conveyor belt while
maintaining adherence to the initial dynamics whenever feasible. By utilizing
trees-of-stars to represent the concave obstacle and positioning the reference
point of the root-component on the conveyor belt, the robot effectively avoids
the parcel from above (see Fig. 19). Notably, comparable methods such as MuMo
[45] or VF-CAPF [40] do theoretically not permit the placement of a reference
point in trees of obstacles that facilitates collision avoidance in such
environments.

Figure 19: The 7DoF robot arm adeptly avoids the star-shaped parcels that are
being transported on the conveyor belt.

##  VIII Discussion

The proposed rotational obstacle avoidance method (ROAM) has successfully
addressed the challenge of avoiding collisions with initially nonlinear
dynamics around general concave obstacles without holes. It stands out as the
first state-dependent solution for trees-of-stars obstacles that is free from
local minima without the need for hyperparameter tuning. Furthermore, ROAM
enables obstacle avoidance while attempting to maintain nonlinear dynamics,
which is a significant advantage. The algorithm has demonstrated improved
convergence and motion similarity compared to the baseline methods in
experimental evaluations. Moreover, its low computational cost has allowed its
application to dynamic obstacle avoidance scenarios involving a robotic arm.

###  VIII-A Stationary Points

ROAM introduces a new stationary point for each obstacle (or obstacle tree).
However, due to the topological properties of smooth vector fields, at least
one fixed point is created for each obstacle (a hole in space). These fixed
points are observed to be saddle points and have no practical significance as
they are unstable and the probability of reaching them is effectively zero.
Additionally, any noise or perturbation in the system pushes the motion away
from these unstable points. It is worth noting that while ROAM is used for
dynamic scenarios, the trajectories of these saddle points should be preserved
as they reflect the smoothness of the velocity. Smoothness is crucial as it
ensures that even with uncertainties in perception or unexpected disturbance,
there will be no discontinuity in the desired velocity command
$\dot{\vec{\xi}}$. Nevertheless, the removal of saddle points could be
achieved by setting the smoothness factor $q$ to 1 in Equation (17) and
selecting any tangent direction for $\vec{e}(\vec{\xi})$ when
$\vec{c}(\vec{\xi})$ and $\vec{\xi}^{r}$ are collinear.

###  VIII-B Trees of Stars

ROAM relies on a star-shaped (or trees-of-stars) obstacle environment. While
in certain real-world scenarios, such division can be achieved based on the
rigid-body features of the surroundings (e.g., dividing a human into limbs and
core or a table into plate and legs), it is often challenging to determine
such divisions for more complex obstacles or in higher-dimensional spaces,
such as joint space collision avoidance. Some propositions for extracting
star-shaped environments exist [54]; however, further research is needed to
extend these approaches for real-time applications.

However, there exist algorithms to simplify general shapes by approximating
them as a union of overlapping spheres [55], which could serve as the basis
for constructing the obstacle tree in future work. Additionally, the circular
shape of these components reduces the computational complexity of tasks such
as checking for component intersections and evaluating the normal direction
and distance to the obstacles.

###  VIII-C Tangent Following on the Surface

Each tree of stars has exactly two saddle points, which lie on the convergence
direction line passing through the root of the tree. At any other surface
position, the velocity is tangent to the surface. This can lead to extensive
wall-following behaviors for obstacles with multiple levels. To address this,
future work should focus on optimizing the selection of saddle points and
allowing the rotated velocity to deviate from the obstacle’s tangent plane.
However, this would require a combination with high-level planning.

###  VIII-D Higher Dimensional Applications: Joint Limits

During the experiments with the robotic arm, there were instances where
obstacle-free motions led the arm to reach its joint limits or encounter
singularities. Future research will investigate a unified framework that
combines task-space collision avoidance with joint-space constraint avoidance.
ROAM is well-suited for evaluating the desired motion in both representations
due to its low computational cost and applicability in higher dimensions.

###  VIII-E Region of Influence

Navigation functions for obstacle avoidance [33, 36] and vector field
avoidance algorithms [38, 40] often require parameter tuning based on the
obstacle distribution to ensure the absence of local minima. In contrast, the
introduced method, ROAM, can handle intersecting obstacle regions, and its
absence of local minima is guaranteed as long as each distance function
$\Gamma_{o}(\vec{\xi})$ for each obstacle $o$ individually satisfies the
limitations outlined in Section II-C2. The distance function proposed in
Equation (5), which was used throughout this work, is globally active.
However, when dealing with dense environments, the linear scaling of ROAM with
the number of obstacles can result in high computational costs. To address
this, the influence region can be reduced by introducing a distance threshold
$d^{\text{max}}\in\mathbb{R}$, such that:

$\lim_{\|\bm{\xi}-{\bm{\xi}}^{b}\|\rightarrow
d^{\mathrm{max}}}\Gamma(\bm{\bm{\xi}})\rightarrow\infty\quad\text{and}\quad\lim_{\|{\bm{\xi}}-{\bm{\xi}}^{b}\|\rightarrow
0}\Gamma(\bm{\bm{\xi}})=1$ (69)

For example, the distance function can be set as follows:

$\Gamma({\bm{\xi}})=\begin{cases}d^{\mathrm{max}}/(d^{\mathrm{max}}-\|{\bm{\xi}}-{\bm{\xi}}^{b}\|)&\text{if}\;\;\|{\bm{\xi}}-{\bm{\xi}}^{b}\|<d^{\mathrm{max}}\\\
\infty&\text{otherwise}\end{cases}$ (70)

###  VIII-F Application to Robotic Systems

The proposed algorithm assumes that the robot is a point mass. However, it can
be extended to real robots by introducing a margin around the obstacles or
using multiple control points, as proposed in [45]. It is important to note
that the latter method does not guarantee full convergence in general
scenarios. Alternative methods employ a full-body representation of the agent
[19, 23], which involves sampling or optimization. However, they cannot
guarantee finding a feasible path in a finite time. Therefore, future work
should focus on extending ROAM to handle analytic avoidance scenarios where
trees of stars represent both the agent and the obstacle.

###  -G Vector Rotations

The rotational obstacle avoidance method (ROAM) requires weighted summing
between vectors as well as partial (sequential) rotations applied to vector
fields. Herefore, we introduce vector math concepts to simplify these
operations.

####  -G1 Perpendicular Rotation Base

Let us consider two initial directions
$\bm{v}_{i},\bm{v}_{o}\in\mathbb{R}^{N}\setminus\bm{0}$. They are used to
construct the base vectors
$\bm{b}_{i},\bm{b}_{o}\in\\{\bm{b}\in\mathbb{R}^{N}:\|\bm{b}\|=1\\}$:

$\displaystyle{\bm{b}}_{i}=\frac{{\bm{v}}_{i}}{\|{\bm{v}}_{i}\|}\,,\;{\bm{b}}_{o}=\frac{{\hat{\bm{b}}}_{o}}{\|{\hat{\bm{b}}}_{o}\|}\,,\;\quad\beta=\arccos\left(\frac{\left\langle\bm{v}_{i},\,\bm{v}_{o}\right\rangle}{\|\bm{v}_{i}\|\,\|\bm{v}_{o}\|}\right)$
(71)
$\displaystyle\;\;\text{with}\quad{\hat{\bm{b}}}_{o}=\bm{v}_{o}-\bm{v}_{i}\left\langle{\bm{v}_{i}},\,{\bm{v}_{o}}\right\rangle\;\;,\quad\forall\,\bm{v}_{i},\bm{v}_{o}:\frac{\left\langle\bm{v}_{i},\,\bm{v}_{o}\right\rangle}{\|\bm{v}_{i}\|\,\|\bm{v}_{o}\|}\neq-1$

The angle $\beta$ and two vectors $\bm{b}_{i}$, $\bm{b}_{o}$ represent the
rotation of a vector in $N$ dimensions, this is equivalent to $2N+1$
parameters.555 This compares to $N(N-1)/2$ parameters required to describe
rigid body rotation in $N$ dimensions. Hence, already at $N=4$ the rigid body
orientation uses more parameters to describe vector rotation, and stays more
efficient for higher dimensions.

####  -G2 Rotating a Vector

Rotating a vector $\bm{v}$ entails rotating the component which lies in the
plane spanned by $\bm{b}_{i}$ and $\bm{b}_{o}$, while conserving the part
orthogonal to the plane.

$\displaystyle{\bm{v}}_{r}\left(\bm{v},\beta,\bm{b}_{\\{i,o\\}}\right)=p_{0}\bm{b}_{i}\cos(\phi)+\bm{b}_{o}\sin(\phi)+\bm{v}-\sum_{j\in\\{i,o\\}}\bm{b}_{j}\left\langle{\bm{b}_{j}},\,{\bm{v}}\right\rangle$
$\displaystyle\quad\text{with}\quad\phi=\phi_{0}+\beta\;,\;\;\tan(\phi_{0})=\frac{\left\langle{\bm{b}_{i}},\,{\bm{v}}\right\rangle}{\left\langle{\bm{b}_{o}},\,{\bm{v}}\right\rangle}\;,$
(72)
$\displaystyle\;\;p_{0}=\sqrt{\left\langle{\bm{b}_{i}},\,{\bm{v}}\right\rangle^{2}+\left\langle{\bm{b}_{o}},\,{\bm{v}}\right\rangle^{2}}$

Note that by changing the rotation angle $\beta$, a partial rotation or over-
rotation can be applied to a vector, too. In this work, the basis is only
explicitly stated if not clear from the context.

Moreover, as each rotation basis is defined by the two vectors $\bm{b}_{i}$
and $\bm{b}_{o}$, a rotation can be rotated, too. For example, a rotation
expressed in a relative reference frame can be rotated to the global frame.
This allows for the evaluation of weighted rotation sequences, as will be
discussed later in this section.

####  -G3 Weighted Rotation Summing

Let us assume that we have $N^{\mathrm{vec}}\in\mathbb{N}_{>0}$ vector
rotations which are defined with respect to a shared basis vector
$\bm{b}_{i}$. The rotations have a respective second basis vector
$\bm{b}_{o,v}$ and rotation angle $\beta_{v}$ with $v\in 1..N^{\mathrm{vec}}$.
The basis and angle of the weighted average rotation is defined as:

$\hat{\bm{b}}_{o}=\bm{B}(\bm{b}_{i})\left(\sum_{v=1}^{N^{\mathrm{vec}}}\bm{B}^{T}(\bm{b}_{i})w_{v}\beta_{v}\bm{b}_{o,v}\right)\quad\text{and}\quad\hat{\beta}=\|\hat{\bm{b}}_{o}\|$
(73)

where $w_{v}\in[0,1]$ are the vector weights, and $\bm{B}(\bm{b}_{i})$ is an
orthonormal basis with respect to the vector $\bm{b}_{i}$.

We further define the rotational sum using the symbol $\hat{+}$ as:

$\hat{\bm{v}}=\bm{v}_{0}\hat{+}\sum_{v=1}^{N^{\mathrm{vec}}}w_{v}\bm{v}_{v}$
(74)

where the summation is performed according to (73), with normalization of the
vector pairs $[\bm{v}_{0},\bm{v}_{v}]$ to obtain the rotations as described in
(71).

####  -G4 Weighted Rotation Sequence

Let us assume that we have $N^{\mathrm{rot}}$ sequential rotations such that
the output vector $\bm{b}_{o,n}$ of the element $n$ is equal to the input
vector of the next in the sequence $n+1$, i.e., $\bm{b}_{o,n}=\bm{b}_{i,n+1}$
with $n=1..N^{\mathrm{rot}}-1$. To evaluate the weighted sequence each basis
is adapted with respect to all the parent rotations as:

$\bm{b}_{\\{i,o\\},c}\leftarrow\bm{v}_{r}((w_{n}-1)\beta_{n},\,\bm{b}_{\\{i,o\\},c})\quad
n=1..N^{\mathrm{rot}}-1,\;c>n$ (75)

The full weighted rotation of the sequence is obtained using the vector
rotation as defined in (71) with input vector $\bm{b}_{i,1}$ and output
$\bm{v}_{o,n}=\bm{v}_{r}((1-w_{n})\beta_{n},\,\bm{b}_{o,n})$ with
$n=N^{\mathrm{rot}}$.

####  -G5 Direction Tree Evaluation

Let us define a direction tree with nodes $n=1..N^{\mathrm{node}}$. There
exists a single (unique) root node $r$, which has no parent. All other nodes
$n$ have single parent $p(n)$, but a node can have multile children, given by
the set $\mathcal{C}_{n}$. The set of all offspring, i.e., including the
children of children etc., is referred to as $\mathcal{C}_{n}^{\mathrm{tot}}$.
Each node has a level $l$, which indicates the discrete distance to the root.
The value of each node corresponds to a unit vector $\bm{v}_{i}$, and hence
the vector rotation between the parent and the node can be evaluated using
(71). For given weights $w_{n}$, the weighted direction is evaluated as
described in Algorithm 4. The rotation tree is interpreted as a combination of
weighted summations and sequences, as described earlier in this section.

Algorithm 4 Rotation Tree Summing

0: $w_{n}$, $\bm{v}_{n}$ for $n=1..N^{\mathrm{node}}$

0: Averaged vector-rotation $\bm{v}^{*}$

1: for $l=L^{\mathrm{max}}\,..\,1$ do {Reverse iteration over levels}

2: for $n:l_{n}=l$ do {For all nodes of the same level}

3: $w_{p(n)}\leftarrow w_{p(n)}+w_{n}$ {Update cumulative weight}

4: end for

5: end for

6: $\bm{v}^{*}_{1}=\bm{v}_{r}$ {Initialize base of root $r$}

7: for $l=2\,..\,L^{\mathrm{max}}$ do {Iteration over levels}

8: $\bm{v}^{*}_{l}=\bm{v}^{*}_{l-1}\hat{+}\sum_{c}^{l_{c}==l}\bm{v}_{c}$
{Weighted average from (74)}

9: $\bm{v}_{r}(\cdot),\beta\leftarrow\bm{v}_{l-1}^{*},\bm{v}_{l}^{*}$
{Rotation from vectors (71)}

10: for $c\in\mathcal{C}_{l}^{\mathrm{tot}}$ do {For all offsprings of $l$}

11: $\bm{v}_{r,c}(\cdot),\beta_{c}\leftarrow\bm{v}^{*}_{l},\bm{v}_{c}$
{Rotation from vectors (71)}

12: $\bm{b}_{j,c}\leftarrow\bm{v}_{r,c}(-\beta_{c},\,\bm{b}_{j,c})$ {Sequence
inversion (75)}

13: $\bm{b}_{j,c}\leftarrow\bm{v}_{r}(\beta,\bm{b}_{j,c})$ {Apply rotation to
$\bm{v}^{*}_{l}$ as (72)}

14: end for

15: end for

16: $\bm{v}^{*}=\bm{v}^{*}_{L^{\mathrm{max}}}$

######  Lemma .1.

The vector rotation as described in Algorithm 4 ensures a smooth vector
summing for weights $w_{n}\in[0,1]$ such that
$\sum_{n=1}^{N^{\mathrm{node}}}w_{n}=1$, such that we reach converge to a
vector when its corresponding weight is approaching one, i.e.,
$\lim_{w_{i}\rightarrow 1}\bm{v}^{*}=\bm{v}_{i}$.

###### Proof.

When a specific direction-node $n$ has weight $w_{n}=1$, it follows from
Algorithm 4 all the child-nodes weight one and zero otherwise:

$w_{c}=\begin{cases}1&\text{if}\;\;c\in\mathcal{C}_{n}\\\
0&\text{otherwise}\end{cases}$ (76)

Hence, we have a rotation sequence of the form (75). Due to the sum of the
weights of all sequence elements being equal to one, the summed weight equals
$\bm{b}_{o,e}$ where $e$ is the index of the ending element. Note successive
vectors cannot be anti-parallel, as denoted in (71). ∎

Note, that the weighted evaluation of the direction tree can be reduced to a
single vector $\bm{v}^{*}$, or the whole sequence can be kept by storing all
$\bm{v}^{*}_{(\cdot)}$. If all node-parent pair had an angle $\beta<\pi$, then
this will be the case for the resulting sequence, too.

## References

  * [1] Vinicius M Goncalves et al.  “Vector fields for robot navigation along time-varying curves in $n$-dimensions”  In _IEEE Transactions on Robotics_ 26.4 IEEE, 2010, pp. 647–659
  * [2] 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
  * [3] Nadia Figueroa and Aude Billard  “A Physically-Consistent Bayesian Non-Parametric Mixture Model for Dynamical System Learning”  In _2nd Annual Conference on Robot Learning, CoRL 2018, Zürich, Switzerland, 29-31 October 2018, Proceedings_ 87, 2018
  * [4] Jean-Jacques E Slotine and Weiping Li  “Applied nonlinear control”  Prentice hall Englewood Cliffs, NJ, 1991
  * [5] Seyed Sina Mirrazavi Salehian and Aude Billard  “A dynamical-system-based approach for controlling robotic manipulators during noncontact/contact transitions”  In _IEEE Robotics and Automation Letters_ 3.4 IEEE, 2018, pp. 2738–2745
  * [6] 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
  * [7] Klas Kronander and Aude Billard  “Passive interaction control with dynamical systems”  In _IEEE Robotics and Automation Letters_ 1.1 IEEE, 2015, pp. 106–113
  * [8] Yuri A Kapitanyuk, Anton V Proskurnikov and Ming Cao  “A guiding vector-field algorithm for path-following control of nonholonomic mobile robots”  In _IEEE Transactions on Control Systems Technology_ 26.4 IEEE, 2017, pp. 1372–1385
  * [9] Kevin M Lynch and Frank C Park  “Modern robotics”  Cambridge University Press, 2017
  * [10] Steven M LaValle  “Rapidly-exploring random trees: A new tool for path planning”  Ames, IA, USA, 1998
  * [11] Sertac Karaman and Emilio Frazzoli  “Sampling-based algorithms for optimal motion planning”  In _The international journal of robotics research_ 30.7 Sage Publications Sage UK: London, England, 2011, pp. 846–894
  * [12] 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
  * [13] Steven M LaValle, Michael S Branicky and Stephen R Lindemann  “On the relationship between classical grid search and probabilistic roadmaps”  In _The International Journal of Robotics Research_ 23.7-8 SAGE Publications, 2004, pp. 673–692
  * [14] Iram Noreen, Amna Khan and Zulfiqar Habib  “Optimal path planning using RRT* based approaches: a survey and future directions”  In _International Journal of Advanced Computer Science and Applications_ 7.11 ScienceInformation (SAI) Organization Limited, 2016
  * [15] 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
  * [16] Sean Quinlan and Oussama Khatib  “Elastic bands: Connecting path planning and control”  In _Robotics and Automation, 1993. Proceedings., 1993 IEEE International Conference on_ , 1993, pp. 802–807  IEEE
  * [17] Matt Zucker et al.  “Chomp: Covariant hamiltonian optimization for motion planning”  In _The International Journal of Robotics Research_ 32.9-10 SAGE Publications Sage UK: London, England, 2013, pp. 1164–1193
  * [18] Nathan Ratliff, Marc Toussaint and Stefan Schaal  “Understanding the geometry of workspace obstacles in motion optimization”  In _2015 IEEE International Conference on Robotics and Automation (ICRA)_ , 2015, pp. 4202–4209  IEEE
  * [19] Jim Mainprice, Nathan Ratliff, Marc Toussaint and Stefan Schaal  “An interior point method solving motion planning problems with narrow passages”  In _2020 29th IEEE International Conference on Robot and Human Interactive Communication (RO-MAN)_ , 2020, pp. 547–552  IEEE
  * [20] Ignacio Sánchez et al.  “Nonlinear model predictive path following controller with obstacle avoidance”  In _Journal of Intelligent & Robotic Systems_ 102 Springer, 2021, pp. 1–18
  * [21] Grady Williams, Andrew Aldrich and Evangelos A Theodorou  “Model predictive path integral control: From theory to parallel computation”  In _Journal of Guidance, Control, and Dynamics_ 40.2 American Institute of AeronauticsAstronautics, 2017, pp. 344–357
  * [22] Mohak Bhardwaj et al.  “Storm: An integrated framework for fast joint-space model-predictive control for reactive manipulation”  In _Conference on Robot Learning_ , 2022, pp. 750–759  PMLR
  * [23] Mikhail Koptev, Nadia Figueroa and Aude Billard  “Neural Joint Space Implicit Signed Distance Functions for Reactive Robot Manipulator Control”  In _IEEE Robotics and Automation Letters_ 8.2 IEEE, 2022, pp. 480–487
  * [24] Benjamin Riviere, Wolfgang Hönig, Yisong Yue and Soon-Jo Chung  “Glas: Global-to-local safe autonomy synthesis for multi-robot motion planning with end-to-end learning”  In _IEEE robotics and automation letters_ 5.3 IEEE, 2020, pp. 4249–4256
  * [25] Dongshu Wang, Shuli Chen, Yadong Zhang and Lei Liu  “Path planning of mobile robot in dynamic environment: fuzzy artificial potential field and extensible neural network”  In _Artificial Life and Robotics_ 26 Springer, 2021, pp. 129–139
  * [26] Mingyu Cai, Erfan Aasi, Calin Belta and Cristian-Ioan Vasile  “Overcoming Exploration: Deep Reinforcement Learning for Continuous Control in Cluttered Environments from Temporal Logic Specifications”  In _IEEE Robotics and Automation Letters_ IEEE, 2023
  * [27] Yann LeCun, Yoshua Bengio and Geoffrey Hinton  “Deep learning”  In _nature_ 521.7553 Nature Publishing Group UK London, 2015, pp. 436–444
  * [28] Lukas Huber, Jean-Jacques Slotine and Aude Billard  “Fast obstacle avoidance based on real-time sensing”  In _IEEE Robotics and Automation Letters_ IEEE, 2022
  * [29] 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
  * [30] Loulin Huang  “Velocity planning for a mobile robot to track a moving target—a potential field approach”  In _Robotics and Autonomous Systems_ 57.1 Elsevier, 2009, pp. 55–63
  * [31] Andreea Tulbure and Oussama Khatib  “Closing the loop: Real-time perception and control for robust collision avoidance with occluded obstacles”  In _2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2020, pp. 5700–5707  IEEE
  * [32] Y Koren and J Borenstein  “Potential field methods and their inherent limitations for mobile robot navigation”  In _Proceedings. 1991 IEEE International Conference on Robotics and Automation_ , 1991, pp. 1398–1399  IEEE Computer Society
  * [33] 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
  * [34] 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
  * [35] E Rimon and DE Koditschek  “Exact robot navigation using artificial potential functions”  In _IEEE Transactions on Robotics and Automation_ 8.5 IEEE, 1992, pp. 501–518
  * [36] 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_ 63.9 IEEE, 2017, pp. 2944–2959
  * [37] Savvas G Loizou  “The navigation transformation”  In _IEEE Transactions on Robotics_ 33.6 IEEE, 2017, pp. 1516–1523
  * [38] Dimitra Panagou  “Motion planning and collision avoidance using navigation vector fields”  In _2014 IEEE International Conference on Robotics and Automation (ICRA)_ , 2014, pp. 2513–2518  IEEE
  * [39] Jay P Wilhelm and Garrett Clem  “Vector field UAV guidance for path following and obstacle avoidance with minimal deviation”  In _Journal of Guidance, Control, and Dynamics_ 42.8 American Institute of AeronauticsAstronautics, 2019, pp. 1848–1856
  * [40] Weijia Yao, Bohuan Lin, Brian DO Anderson and Ming Cao  “Guiding vector fields for following occluded paths”  In _IEEE Transactions on Automatic Control_ IEEE, 2022
  * [41] 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
  * [42] Jin-Oh Kim and Pradeep K Khosla  “Real-time obstacle avoidance using harmonic potential functions”  In _IEEE Transactions on Robotics and Automation_ 8.3 Ju, 1992
  * [43] Hans Jacob S Feder and J-JE Slotine  “Real-time path planning using harmonic potentials in dynamic environments”  In _Proceedings of International Conference on Robotics and Automation_ 1, 1997, pp. 874–881  IEEE
  * [44] Dake Zheng, Xinyu Wu, Yizhang Liu and Jianxin Pang  “A Dynamical System Approach to Real-time Three-Dimensional Concave Obstacle Avoidance”  In _2020 IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM)_ , 2020, pp. 1082–1087  IEEE
  * [45] Lukas Huber, Jean-Jacques Slotine and Aude Billard  “Avoiding Dense and Dynamic Obstacles in Enclosed Spaces: Application to Moving in Crowds”  In _IEEE Transactions on Robotics_ IEEE, 2022
  * [46] Federico M Conzelmann et al.  “A Dynamical System Approach to Decentralized Collision-free Autonomous Coordination of a Mobile Assistive Furniture Swarm”  In _2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2022, pp. 7259–7265  IEEE
  * [47] Hermann Brunn  “Über Kerneigebiete”  In _Mathematische Annalen_ 73.3 Springer, 1913, pp. 436–440
  * [48] Guillermo Hansen, Irmina Herburt, Horst Martini and Maria Moszyńska  “Starshaped sets”  In _Aequationes mathematicae_ 94 Springer, 2020, pp. 1001–1092
  * [49] Xiaodong Luo, Michael Small, Marius-F Danca and Guanrong Chen  “On a dynamical system with multiple chaotic attractors”  In _International Journal of Bifurcation and Chaos_ 17.09 World Scientific, 2007, pp. 3235–3251
  * [50] Stephen R Lindemann and Steven M LaValle  “Simple and efficient algorithms for computing smooth, collision-free feedback laws over given cell decompositions”  In _The International Journal of Robotics Research_ 28.5 SAGE Publications Sage UK: London, England, 2009, pp. 600–621
  * [51] F. Pedregosa et al.  “Scikit-learn: Machine Learning in Python”  In _Journal of Machine Learning Research_ 12, 2011, pp. 2825–2830
  * [52] Nicolas Perrin and Philipp Schlehuber-Caissier  “Fast diffeomorphic matching to learn globally asymptotically stable nonlinear dynamical systems”  In _Systems & Control Letters_ 96 Elsevier, 2016, pp. 51–59
  * [53] Donald Ervin Knuth  “The art of computer programming”  Pearson Education, 1997
  * [54] Albin Dahlin and Yiannis Karayiannidis  “Creating Star Worlds–Modelling Concave Obstacles for Reactive Motion Planning”  In _arXiv preprint arXiv:2205.09336_ , 2022
  * [55] Philip M Hubbard  “Approximating polyhedra with spheres for time-critical collision detection”  In _ACM Transactions on Graphics (TOG)_ 15.3 ACM New York, NY, USA, 1996, pp. 179–210