#  Mathematical Properties of Generalized Shape Expansion-Based Motion
Planning Algorithms

Adhvaith Ramkumar1, Vrushabh Zinage1 and Satadal Ghosh1 1Adhvaith Ramkumar,
Vrushabh Zinage and Satadal Ghosh are with the Department of Aerospace
Engineering, Indian Institute of Technology Madras, India
<EMAIL_ADDRESS>
###### Abstract

Motion planning is an essential aspect of autonomous systems and robotics and
is an active area of research. A recently-proposed sampling-based motion
planning algorithm, termed ’Generalized Shape Expansion’ (GSE), has been shown
to possess significant improvement in computational time over several existing
well-established algorithms. The GSE has also been shown to be
probabilistically complete. However, asymptotic optimality of the GSE is yet
to be studied. To this end, in this paper we show that the GSE algorithm is
not asymptotically optimal by studying its behaviour for the promenade
problem. In order to obtain a probabilistically complete and asymptotically
optimal generalized shape-based algorithm, a modified version of the GSE,
namely ’$\text{GSE}^{\star}$’ algorithm, is subsequently presented. The
forementioned desired mathematical properties of the $\text{GSE}^{\star}$
algorithm are justified by its detailed analysis. Numerical simulations are
found to be in line with the theoretical results on the $\text{GSE}^{\star}$
algorithm.

###### Index Terms:

motion planning, sampling-based algorithms.

##  I Introduction

Unmanned vehicle Systems (UxS) have seen a steady increase in attention over
the past decade due to rapid advancements in technology and reduced human risk
associated to it. Applications of UxS range from civilian ones like pollutant
monitoring to defense-related ones like surveillance and reconnaissance.
Motion planning being a key aspect of any UxS is an active area of research.

An early seminal paper in the field of motion planning utilized artificial
potential fields [1], however it was shown to suffer from the problem of local
minima [2]. Numerous online motion planning strategies have been formulated
using several methods like velocity obstacles [3] and its extensions such as
optimal reciprocal collision avoidance [4], collision cone approach [5, 6]
gradient vector fields [7], pseudospectral methods [8] and terminal angle-
constrained guidance theory [9], [10]. In the presence of prior knowledge
about the environment, offline motion planners such as discrete search-based
methods [11, 12] and sampling-based methods [13, 14] are employed.
Optimization-based techniques such as sequential convex programming [15, 16]
and quadratic programming methods [17] are also used.

Among offline planners, sampling-based algorithms are a highly favoured
approach due to computational advantages in higher-dimensional spaces. A
sampling-based algorithm generates a connected graph, containing the initial
and goal points, entirely inside the free-space. This is accomplished by
obtaining random samples from the free-space and successively adding them to
the graph, eventually connecting the initial and goal points. The optimal path
is then found among the paths between the initial and goal points. Several
sampling-based algorithms have been developed in literature such as
Probabilistic Road Maps (PRM) [13], Rapidly-exploring Random Trees (RRT)[18]
and Fast Marching Tree (FMT⋆) [19]. A spherical expansion-based motion
planning algorithm, termed ’SE-SCP’ [20] was shown to be computationally more
efficient than PRM⋆ and RRT⋆ via numerical studies. In order to better
leverage obstacle-space information, an algorithm termed ’Generalized Shape
Expansion’ (GSE) algorithm, relying on a more generic shape expansion has
recently been presented in [21] for motion planning in 2-D obstacle-cluttered
environments. Its extension to 3-D environment was also presented in [22, 23].
The GSE algorithm leveraged a novel generalized shape for the expansion of the
generated graph, which helped in exploring the free-space in a fast yet more
extensive manner. Through extensive numerical simulation studies, the
performance of the GSE in terms of computation time was found to be
significantly better than several other well-established sampling-based
methods. An even faster planner has been presented in [24] by embedding
directional sampling scheme to the GSE for 2-D environments. While sampling-
based motion planning algorithms cannot provide deterministic guarantees, two
probabilistic criteria are used to evaluate the utility of these algorithms.
These properties are probabilistic completeness and asymptotic optimality.
Probabilistic completeness of an algorithm guarantees that the probability of
failure of an algorithm decays to zero, as the number of samples goes to
infinity, while asymptotic optimality of an algorithm guarantees that the
algorithm is highly likely to find the optimal path as the number of samples
goes to infinity. Algorithms like PRM and RRT were found to be
probabilistically complete [25, 18], but they are not asymptotically optimal
[14]. In order to ensure asymptotic optimality, their modified versions such
as RRT⋆ and PRM⋆ were developed and analyzed in [14].

This paper is evolved from [26], where the probabilistic completeness property
of the GSE algorithm was established. However asymptotic optimality of the GSE
algorithm has not yet been studied so far. To this end, in this paper, we
consider the promenade problem, for which following the methodology of [27] we
find that the GSE has a non-zero probability of returning low-quality
solutions for the promenade problem. This establishes the lack of asymptotic
optimality of the GSE algorithm. Subsequently, we present a modification of
the GSE algorithm, termed as the GSE⋆ algorithm, in order to ensure asymptotic
optimality as well as probabilistic completeness. Besides the GSE expansion,
the $\text{GSE}^{\star}$ algorithm adds additional vertices and edges based on
a decreasing connection radii criterion to the graph generated by the GSE,
thus limiting the path cost while further exploring the environment. The basic
GSE graph, a subset of the $\text{GSE}^{\star}$ graph, also helps in retaining
computational advantage. A detailed analysis is carried out of the GSE⋆
algorithm for theoretical verification of both probabilistic completeness and
asymptotic optimality.

The paper is organized as follows. The problem description along with a brief
description of the probabilistic completeness and asymptotic optimality of a
path planning problem is given in Section II. We provide the $d$-dimensional
GSE algorithm in Section III-A. This is followed by the study of mathematical
properties of the GSE algorithm in Section III-B and III-C. Next, we present
the $\text{GSE}^{\star}$ algorithm in Section IV-A followed by an analysis of
the same in Section IV-B and IV-C. Simulation studies are presented to compare
GSE and $\text{GSE}^{\star}$ in Section V, followed by the conclusion in
Section VI.

(a) $\boldsymbol{r}_{1,X}$ and ${\theta_{1,X}}$

(b) $\boldsymbol{r}_{2,X}$ and ${\theta_{2,X}}$

(c) $\boldsymbol{r}_{3,X}$ and ${\theta_{3,X}}$

(d) $\boldsymbol{r}_{4,X}$ and ${\theta_{4,X}}$

Figure 1: Depiction of $\theta_{i,X}$ and $\boldsymbol{r}_{i,X}$ for all
$i\in\\{1,2,3,4\\}$ for a 2-D environment

##  II Problem Description

In this section, we introduce the general path planning problem along with the
probabilistic completeness and asymptotic optimality of the path planning
problem. Finally the problem statement is introduced which we address in this
paper.

###  II-A Path Planning Problem

Consider a compact configuration space $\mathbb{X}=[0,1]^{d}$, where $d$ is
the dimension of the workspace. Let the obstacles in the configuration space
be denoted as $\mathbb{X}_{\text{obs}}$. The free-space in $\mathbb{X}$ is
then given by
$\mathbb{X}_{\text{free}}=\mathbb{X}\setminus\mathbb{X}_{\text{obs}}$. We
assume that the free-space is open and the obstacle space is closed. The Start
point $\boldsymbol{X}_{\text{init }}$ is an element of $\mathbb{X}_{\text{free
}}$, and the Goal point $\boldsymbol{X}_{\text{goal
}}\in\mathbb{X}_{\text{free}}$. Given a function
$\sigma:[0,1]\rightarrow\mathbb{X}$, the total variation of $\sigma$ is
defined as follows,

$\displaystyle\operatorname{TV}(\sigma)=\sup_{\left\\{n\in\mathbb{N},0=b_{0}<\cdots<b_{n}=1\right\\}}\sum_{i=1}^{n}\left|\sigma\left(b_{i}\right)-\sigma\left(b_{i-1}\right)\right|$
(1)

Where, the supremum is taken over all partitions of the set $[0,1]$. The
symbol $\|.\|$ denotes the Euclidean norm. A function $\sigma$ with
$\mathrm{TV}(\sigma)<\infty$ is said to have bounded variation.

######  Definition 1.

(Path): A function $\sigma:[0,1]\rightarrow\mathbb{X}$ of bounded variation is
called a

  * •

Path, if it is continuous (Bounded variation of $\sigma$ is equivalent to a
path of finite length)

  * •

Collision-free path, if it is a path, and $\sigma(b)\in\mathbb{X}_{\text{free
}},$ for all $b\in[0,1]$

  * •

Feasible path, if it is a collision-free path,
$\sigma(0)=\boldsymbol{X}_{\text{init }},$ and
$\sigma(1)=\boldsymbol{X}_{\text{goal}}$

Let $\mathfrak{S}$ denote the set of all collision-free paths in the given
configuration space. We define the norm
$\|{\sigma}\|_{\text{BV}}=\mathrm{TV}(\sigma)+\int_{0}^{1}|\sigma(t)|dt$. The
topology on the space $\mathfrak{S}$ is derived from the metric induced by
this norm. Define a cost function $c:\mathfrak{S}\rightarrow\mathbb{R}_{\geq
0}$. We assume that the cost function increases with the length of path, that
is $c(\sigma_{1})>c(\sigma_{2})$ if
$\mathrm{TV}(\sigma_{1})>\mathrm{TV}(\sigma_{2})$. Further note that a path
$\sigma:[0,1]\rightarrow\mathbb{X}$ is said to have

  * •

strong $\delta$-clearance if for each $s\in[0,1]$,
$\mathcal{B}_{\delta}(\sigma(s))\subseteq\mathbb{X}_{\text{free}}$ where
$\mathcal{B}_{r}(P)=\\{x\mid\|x-P\|<r\\}$.

  * •

weak $\delta$-clearance if there exists a homotopy, i.e. a continuous function
$\psi:[0,1]\rightarrow\mathfrak{S}$ with $\psi(0)=\sigma$ and
$\psi(1)=\sigma_{1}$ where $\sigma_{1}$ has strong $\delta$-clearance. Further
if $\alpha\in[0,1]$ then the path $\psi(\alpha)$ has strong
$\delta_{\alpha}$-clearance for some $\delta_{\alpha}>0$.

We also assume the following:

  * •

There exists a feasible path $\sigma$ between the start and goal points with
strong $\epsilon$-clearance for some $\epsilon>0$.

  * •

There exists a path $\sigma^{\star}$ with weak $\delta$-clearance for some
$\delta>0$ and optimal cost $c^{\star}$, such that if there exists a sequence
$(\sigma_{n})_{n=1}^{\infty}$ with
$\underset{n\to\infty}{\lim}\sigma_{n}=\sigma^{\star}$, then
$\underset{n\to\infty}{\lim}c(\sigma_{n})=c^{\star}$.

A path planning problem entails finding a feasible path in an environment
characterized by the triplet
$\left(\mathbb{X}_{\text{free}},\boldsymbol{X}_{\text{init}},\boldsymbol{X}_{\text{goal
}}\right)$.

###  II-B Probabilistic Completeness

Sampling-based motion planning algorithms rely on uniform random sampling from
the free-space in each step. Consequently, it is difficult to provide
deterministic guarantees on the success of the algorithm. We can however study
the claim that success in finding a feasible path from
$\boldsymbol{X}_{\text{init}}$ to $\boldsymbol{X}_{\text{goal}}$ is guaranteed
as the number of iterations of the algorithm increases to infinity. This
constitutes the fundamental idea of probabilistic completeness of a algorithm.

We define a few preliminary quantities

  * •

We denote the graph generated by an algorithm after $n$ steps
${G}_{n}=(\mathbb{V}_{n},\mathbb{E}_{n})$, where $\mathbb{V}_{n}$ and
$\mathbb{E}_{n}$ are the vertex and edge sets, respectively.

  * •

Let $\mathcal{P}_{n}$ be an indicator random variable that takes value one
indicating the event that there is a connected path between
$\boldsymbol{X}_{\text{init}}$ and $\boldsymbol{X}_{\text{goal}}$ in
${G}_{n}$.

The algorithm can be said to possess the probabilistic completeness property
if

$\displaystyle\underset{n\to\infty}{\limsup}\;\mathbb{P}(\mathcal{P}_{n}=0)=0$
(2)

where, $\mathbb{P}$ denotes probability of an event.

###  II-C Asymptotic Optimality

Let $Y_{n}$ denote the cost of the least cost solution returned by a sampling-
based motion planning algorithm in $n$ iterations. We define
$c^{\star}=\text{inf}\\{c(\sigma):\text{$\sigma$ is a feasible path}\\}$. An
algorithm is said to be asymptotically optimal if

$\mathbb{P}(\\{\underset{n\to\infty}{\limsup}\;{Y_{n}}=c^{\star}\\})=1$ (3)

###  II-D Problem Statement

In this paper, first the GSE algorithm, which was proposed for 2-D and 3-D
environments in [21, 22] respectively, is extended to a general $d$
dimensional configuration space. Then, probabilistic completeness of the GSE
algorithm is to be studied first followed by an analysis on cost of paths
returned by the GSE algorithm.

If the GSE algorithm is not found to be asymptotically optimal, then an
amended version of the GSE that would satisfy both the probabilistic
completeness and asymptotic optimality is to be presented and analyzed.

##  III Generalized Shape Expansion (GSE) Algorithm

Let there be $m$ obstacles in configuration space $\mathbb{X}$. We assume that
the points on the obstacle boundary in $\mathbb{X}$ are known. Let the set of
points on the $i^{\text{th}}$ obstacle be denoted by $\mathbb{Q}_{i}$, where
$i\in\\{1,\dots m\\}$. Consider a point
$\boldsymbol{X}\in\mathbb{X}\subset\mathbb{R}^{d}$. The minimum distance
vector of $\boldsymbol{X}$ from the $i^{\text{th}}$ obstacle is denoted by
$\boldsymbol{r}_{i,X}$, and its magnitude is given by
$\|\boldsymbol{r}_{i,X}\|$. Without loss of generality, obstacles are numbered
such that $\|\boldsymbol{r}_{i,X}\|\leq\|\boldsymbol{r}_{j,X}\|$ for $i<j$.
Let $\theta_{i,X}$ denotes the maximum angle made by the vector
$\boldsymbol{r}_{i,X}$ with $(\boldsymbol{X}-\boldsymbol{x})$, where
$\boldsymbol{x}\in\mathbb{Q}_{i}$. In other words,

$\displaystyle\theta_{i,X}=\underset{\boldsymbol{x}\in\mathbb{Q}_{{i}}}{\operatorname{argmax}}\;\text{cos}^{-1}\left(\frac{(\boldsymbol{X}-\boldsymbol{x}).\boldsymbol{r}_{i,X}}{\|\boldsymbol{X}-\boldsymbol{x}\|\;\|\boldsymbol{r}_{i,X}\|}\right)\;i\in\\{1,\dots,m\\}$
(4)

where $\boldsymbol{a}.\boldsymbol{b}$ denotes the dot product of vectors
$\boldsymbol{a}$ and $\boldsymbol{b}$. Note that the formulation of the
maximum angular spectrum covering $i^{\text{th}}$ obstacle as seen from a
point $\boldsymbol{X}$ given above in Eq. (4) is a generalization of the
formulation presented in [22]. A sample depiction of $\theta_{i,X}$ and
$\boldsymbol{r}_{i,X}$ for four obstacles in two-dimensional configuration
space is shown in Fig. 1.

###  III-A Description of the GSE Algorithm

Algorithm 1 GSE Algorithm

1:${\mathbb{V}\leftarrow\\{\boldsymbol{X_{\text{init}}},\boldsymbol{X_{\text{goal}}}\\}},\;\;\mathbb{E}\leftarrow\emptyset$

2:${\mathcal{S}_{\text{init}}(\boldsymbol{P})\leftarrow\texttt{Shape}(\boldsymbol{P},\boldsymbol{X_{\text{init}}},\mathbb{X}_{\text{obs}})}$

3:${\mathcal{S}_{\text{goal}}(\boldsymbol{P})\leftarrow\texttt{Shape}(\boldsymbol{P},\boldsymbol{X_{\text{goal}}},\mathbb{X}_{\text{obs}})}$

4:for $j=1\dots n$ do

5: $\boldsymbol{X_{\text{rand}}}\leftarrow\texttt{GenerateSample}$

6:
$\mathcal{S}_{\text{rand}}(\boldsymbol{P})\leftarrow\texttt{Shape}(\boldsymbol{P},\boldsymbol{X_{\text{rand}}},\mathbb{X}_{\text{obs}})$

7:
$\boldsymbol{X_{\text{nearest}}}\leftarrow\texttt{Nearest}(\mathbb{G}=(\mathbb{V},\mathbb{E}),\boldsymbol{X_{\text{rand}}})$

8:
$\mathcal{S}_{\text{nearest}}(\boldsymbol{P})\leftarrow\texttt{Shape}(\boldsymbol{P},\boldsymbol{X_{\text{nearest}}},\mathbb{X}_{\text{obs}})$

9: $\boldsymbol{X_{\text{new}}}\leftarrow\texttt{Steer-
GSE}(\boldsymbol{X_{\text{nearest}}},\boldsymbol{X_{\text{rand}}})$

10: $\mathcal{S}_{\text{new}}(\boldsymbol{P})\leftarrow\texttt{GSE-
Shape}(\boldsymbol{P},\boldsymbol{X_{\text{new}}},\mathbb{X}_{\text{obs}})$

11:
$\mathbb{X_{\text{intersect}}}\leftarrow\texttt{IntersectShape}(G,\boldsymbol{X_{\text{new}}})$

12:
$\mathbb{V}\leftarrow\mathbb{V}\cup\\{\boldsymbol{X_{\text{new}}},\boldsymbol{X_{\text{rand}}}\\}$

13: for $\boldsymbol{X_{\text{n}}}\in\mathbb{X}_{\text{intersect}}$ do

14:
$\mathbb{E}\leftarrow\mathbb{E}\cup\\{(\boldsymbol{X_{\text{n}}},\boldsymbol{X_{\text{new}}})\\}$

15: end for

16: $\mathbb{G}=(\mathbb{V},\mathbb{E})$

17:
$Path_{\text{init,goal,shortest}}\leftarrow\texttt{MinPath}(G,\boldsymbol{X_{\text{init}}},\boldsymbol{X}_{\text{goal}})$

18:end for

####  III-A1 Initialization

During initialization, the start and goal positions
$\boldsymbol{X}_{\text{init}}$ and $\boldsymbol{X}_{\text{goal}}$,
respectively, are added to the vertex set $\mathbb{V}$ with their
corresponding parameters
$\\{\boldsymbol{r}_{{i,X}},\theta_{{i,X}}\\}^{m}_{i=1}$. Values of these
parameters are then used to compute the corresponding shape functions
$\mathcal{S}_{\text{init}}(\boldsymbol{P})$ and
$\mathcal{S}_{\text{goal}}(\boldsymbol{P})$, respectively, using the function
$\texttt{Shape}(\boldsymbol{P},\boldsymbol{X},\mathbb{X}_{\text{obs}})$ for
$\boldsymbol{X}=\boldsymbol{X_{\text{init}}}$ and
$\boldsymbol{X_{\text{goal}}}$, respectively (Lines 2, 3 of Algorithm 1). The
edge set $\mathbb{E}$ is set empty. This is shown in Lines 1-3 of Algorithm 1.

####  III-A2 Sample point

A random point $\boldsymbol{X}_{\text{rand}}\in\mathbb{X}_{\text{free}}$, is
drawn from a uniform distribution such that the sequence of sampled points is
independent and identically distributed as shown in Line 5 of Algorithm 1.

####  III-A3 Nearest point

For a given $\boldsymbol{X}_{\text{rand}}$, the next step is to find a point
$\boldsymbol{X}_{\text{nearest}}\in\mathbb{V}$, which is nearest to
$\boldsymbol{X}_{\text{rand}}$ (see Line 7 of Algorithm 1).

$\texttt{Nearest}(\mathbb{G}=(\mathbb{V},\mathbb{E}),\boldsymbol{X}_{\text{rand}})=\underset{\boldsymbol{X}\in\mathbb{V}}{\operatorname{argmin}}\|\boldsymbol{X}-\boldsymbol{X}_{\text{rand}}\|$
(5)

Algorithm 2 $\text{GSE Shape
Function}\;(\texttt{Shape}(\boldsymbol{P},\boldsymbol{X},\mathbb{X}_{\text{obs}}))$

1:${\ell_{0}=0}$

2:for $i=1,2,\dots,m$ do

3:
$\ell_{i}\leftarrow\texttt{sat}{(\text{cos}^{-1}(\frac{\boldsymbol{r_{i,X}}.{(\boldsymbol{P}-\boldsymbol{X})}}{\|\boldsymbol{r_{i,X}}\|\;\|{\boldsymbol{P}-\boldsymbol{X}}\|})-\theta_{i,X})}$

4:
$f_{i}\leftarrow\texttt{sat}(\theta_{i,X}-\texttt{cos}^{-1}(\frac{\boldsymbol{r_{i,X}}.{(\boldsymbol{P}-\boldsymbol{X})}}{\|\boldsymbol{r_{i,X}}\|\;\|{\boldsymbol{P}-\boldsymbol{X}}\|}))+\texttt{sat}(r_{i,X}-\|\boldsymbol{P}-\boldsymbol{X}\|)$

5: $g_{{i}}\leftarrow f_{{i}}+{\sum_{j=1}^{i-1}\ell_{j}}-(i+1)$

6:end for

7:if $\texttt{all}(\ell_{{i}})=1\;$ then

8: $\texttt{Shape}(\boldsymbol{P},\boldsymbol{X},\mathbb{X}_{\text{obs}})=0$

9:else

10:
$\mathcal{S}_{\boldsymbol{X}}(\boldsymbol{P})=\texttt{Shape}(\boldsymbol{P},\boldsymbol{X},\mathbb{X}_{\text{obs}})\leftarrow
g_{1}*g_{2}*\dots g_{m}$

11:end if

(a) ${\mathcal{R}_{1,\boldsymbol{X}}}$ and ${\mathcal{R}_{2,\boldsymbol{X}}}$

(b) $\widehat{\mathcal{R}_{1,\boldsymbol{X}}}$,
$\widehat{\mathcal{R}_{2,\boldsymbol{X}}}$ and $\mathcal{I}$

(c) $\widetilde{\mathcal{R}_{12,\boldsymbol{X}}}$

(d)
$\mathcal{S}_{\boldsymbol{X}}={\mathcal{R}_{1,\boldsymbol{X}}}\cup({\mathcal{R}_{1,\boldsymbol{X}}}-\widetilde{\mathcal{R}_{12,\boldsymbol{X}}})\cup\mathcal{I}$

Figure 2: An illustration of generalized shape about $\boldsymbol{X}$ in 2-D
environment

####  III-A4 Generalized shape generation

The generalized shape $\mathcal{S}_{\boldsymbol{X}}$ generated about any point
$\boldsymbol{X}$ is given by the Shape function, which is detailed in
Algorithm 2. The function
Shape$(\boldsymbol{P},\boldsymbol{X},\mathbb{X}_{\text{obs}})$ in Algorithm 2
returns a value zero if a test point $\boldsymbol{P}$ is within the
generalized shape about the sampled point $\boldsymbol{X}$, and non-zero
otherwise, that is,

$\displaystyle\mathcal{S}_{\boldsymbol{X}}({\boldsymbol{P}})\left\\{\begin{array}[]{l}=0;\quad\quad\text{if}\>{\boldsymbol{P}}\in\mathcal{S}_{\boldsymbol{X}}\\\
\neq 0,\quad\quad\text{otherwise}\end{array}\right.$ (8)

Note that the $\texttt{sat}(.)$ function in Algorithm 2 is given by
$\texttt{sat}({x})=1$ if $x\geq 0$ else $0$.

####  III-A5 Steering $\boldsymbol{X}_{\text{rand}}$ to generate
$\boldsymbol{X}_{\text{new}}$

($\texttt{Steer-
GSE}(\boldsymbol{X_{\text{nearest}}},\boldsymbol{X_{\text{rand}}})$):

The randomly selected point $\boldsymbol{X}_{\text{rand}}$ is steered to a
suitable point $\boldsymbol{X}_{\text{new}}$ as follows:

  * •

If
$\boldsymbol{X}_{\text{rand}}\in\mathcal{S}_{\boldsymbol{X}_{\text{nearest}}}$,
then $\boldsymbol{X}_{\text{new}}$ is equal to $\boldsymbol{X}_{\text{rand}}$.

  * •

Otherwise, $\boldsymbol{X}_{\text{new}}$ is the point of intersection of the
boundary of $\mathcal{S}_{\boldsymbol{X}_{\text{nearest}}}$ and the line
joining $\boldsymbol{X}_{\text{nearest}}$ and $\boldsymbol{X}_{\text{rand}}$.

The point $\boldsymbol{X}_{\text{new}}$ thus computed is then added to the set
$\mathbb{V}$.

####  III-A6 Generation of new edges

In this step, first a set $\mathbb{X}_{\text{intersect}}$ is defined as
$\mathbb{X}_{\text{intersect}}\triangleq\\{\boldsymbol{X}\in\mathbb{V}|\mathcal{S}_{\boldsymbol{X}}\cap\mathcal{S}_{\boldsymbol{X}_{\text{new}}}\cap{\boldsymbol{X}\boldsymbol{X}_{\text{new}}}\neq\emptyset\\}$.
Next, $\boldsymbol{X}_{\text{new}}$ is connected with the vertices
$\boldsymbol{X}_{\text{n}}\in\mathbb{X}_{\text{near}}$ via newly generated
edges $\boldsymbol{X}_{\text{n}}\boldsymbol{X}_{\text{new}}$, which are then
added to the set $\mathbb{E}$.

####  III-A7 Generating shortest path from $\boldsymbol{X}_{\text{init}}$ to
$\boldsymbol{X}_{\text{goal}}$

Once a directed graph $\mathbb{G}=(\mathbb{V},\mathbb{E})$ connecting
$\boldsymbol{X}_{\text{init}}$ and $\boldsymbol{X}_{\text{goal}}$ is found,
using Dijkstra’s algorithm, the minimum cost connected path is found from
$\boldsymbol{X}_{\text{init}}$ to $\boldsymbol{X}_{\text{goal}}$.

###  III-B Probabilistic Completeness of the GSE Algorithm

We denote the vertex set and edge set of GSE after $n$ iterations by
$\mathbb{V}_{n}^{\text{GSE}}$ and $\mathbb{E}_{n}^{\text{GSE}}$, respectively.
We indicate a summary of the proof of probabilistic completeness of the GSE
algorithm. Further details are provided in [26]. We show this fact by
establishing a few properties of the GSE visibility function
$f:2^{\mathbb{X}_{\text{free}}}\rightarrow 2^{\mathbb{X}_{\text{free}}}$
defined as

$\displaystyle f(S)\triangleq\\{\boldsymbol{X}\in\mathbb{X}_{\text{free
}}\mid\mathcal{S}_{\boldsymbol{X}}\cap S\neq\emptyset\\}$ (9)

For the analysis of GSE, we study the iterates of this function
$f^{(2)}(S)\triangleq f(f(S))$, $f^{(3)}(S)=f^{(2)}(f(S))$ etc. More
precisely, we establish that

  * •

For any path planning problem (conforming to the conditions laid down in our
problem statement), there exists a finite constant $q>0$ such that
$f^{(q)}(\boldsymbol{X}_{\text{goal}})=\mathbb{X}_{\text{free}}$.

  * •

Let $\widehat{\mathbb{V}}_{n}^{\text{GSE}}$ denote the component of the GSE
graph that contains the initial point $\boldsymbol{X}_{\text{init}}$. Then,
the algorithm succeeds in finding a path from $\boldsymbol{X}_{\text{init}}$
to $\boldsymbol{X}_{\text{goal}}$ in $n$ iterations if
$\widehat{\mathbb{V}}_{n}^{\text{GSE}}\cap\mathbb{X}_{\text{free}}\neq\emptyset$.

  * •

The probability that the vertex set of GSE fails to progress from the set
$f^{(q)}(\boldsymbol{X}_{\text{goal}})$ to the set
$f(\boldsymbol{X}_{\text{goal}})$ is bounded above by the tail probability of
a Bernoulli random variable.

  * •

Using Chernoff bounds, we show that the probability of failure of the GSE
algorithm decays exponentially with the number of iterations of the algorithm.

Following this argument, we conclude that the GSE algorithm is
probabilistically complete.

###  III-C Analysis of Cost of Path Returned by the GSE Algorithm

In this section, we analyse the cost of the paths returned by the GSE
algorithm for investigating whether the GSE has asymptotic optimality
property. In particular, we study performance of the algorithm in the
promenade problem, and conduct the analysis by defining a finite state machine
called the Automaton of Sampling Diagrams (ASD) similar to the one defined in
[28]. However, in this section, the automaton is modified to better suit the
analysis of the GSE algorithm.

####  III-C1 The promenade problem and the automaton of sampling diagrams for
the GSE algorithm

The promenade problem described below offers some perspective on how the GSE
algorithm performs for certain problem types. The automaton we define provides
a way to study the critical events in the progress of the GSE algorithm. This
helps determine the probability of achieving solutions of different qualities.
For ease of understanding, here, the analysis is given for 2-D environments.
However, it can easily be extended to more general settings of higher
dimensional environments.

####  III-C2 Promenade problem

The setting of this problem is as follows: Define
$\mathbb{X}={[0,\alpha+2]}^{2}$ and
$\mathbb{X}_{\text{obs}}=[1,\alpha+1]^{2}$. As usual we denote an arbitrary
point by coordinates $(x,y)$. The initial and goal points are set to be on the
left and right of the central obstacle. While the exact location need not be
precise, we set $\boldsymbol{X}_{\text{init}}=(1-\epsilon,1+2\epsilon)$ and
$\boldsymbol{X}_{\text{goal}}=(\alpha+1+\epsilon,1+2\epsilon)$ where
$\epsilon>0$ is much smaller than min$(\alpha,1)$. The aim is to generate a
path between these two points. In this environment we define the following
regions

  * •

$L_{1}=\\{(x,y)\in\mathbb{X}|x+y\leq 2\\}$

  * •

$L_{2}$ is the reflection of $L_{1}$ about the line $x=\alpha/2+1$

  * •

$B_{1}=[0,1]\times[\alpha+1,\alpha+2]$

  * •

$B_{2}$ is the reflection of $B_{1}$ about the line $x=\alpha/2+1$

A representative figure is given in Fig. 3 We further classify all solutions
to the promenade problem into two types -

  * •

Type-B solutions where the path returned by the algorithm passes through
$B_{1}$ and $B_{2}$, that is, passes through the region above the square
obstacle.

  * •

Type-L solutions where the path returned passes through $L_{1}$ and $L_{2}$,
that is, passes through the region below the square obstacle.

It is immediately evident that Type-B solutions are of higher cost under the
given conditions for the stated locations of the initial and goal points.

####  III-C3 Automaton of sampling diagrams

Figure 3: A schematic representation of the promenade problem

In the context of the problem above, we develop an automaton to model the
progress of the GSE algorithm. Type-L and Type-B solutions are the two major
types of solutions we wish to develop. Therefore, we consider two states
$s_{\text{accepting}}$ and $s_{\text{rejecting}}$, associated with Type-L and
Type-B solutions respectively. These states are by necessity ’sink’ states.
Notice that the automaton begins with the state $s_{\text{init}}$. We are
concerned primarily with the progress of the component of the GSE graph
containing the initial point. Consequently, in order to better study the GSE
graph, we define two interim states $s_{1}$ and $s_{2}$. Progress from
$s_{\text{init}}$ to $s_{\text{accepting}}$ through the two interim states
gives us a high cost solution, and we ’reject’ all paths that might lead to
low cost solutions via the rejecting state. Developing this heuristic, we have
the following definition

######  Definition 2.

An automaton of sampling diagrams is a finite state machine with five
states.The states are
$s_{\text{init}},s_{1},s_{2},s_{\text{accepting}},s_{\text{rejecting}}$. The
machine takes as input a point $\boldsymbol{X}$ sampled from the free-space.
Based on the location of the point, decisions are made as to the progress of
the automaton. Two sets $F_{i}$ and $R_{i}$ are affiliated with each state
$s_{i}$, where $i\in\\{1,2,\text{init},\text{rejecting},\text{accepting}\\}$.
The states are ordered $s_{\text{init}}\rightarrow s_{1}\rightarrow
s_{2}\rightarrow s_{\text{accepting}}$. The state $s_{\text{rejecting}}$ is
outside of this structure. The operations of the automaton are then described
as below,

  * •

If the automaton reaches a rejecting or an accepting state, then it does not
progress further.

  * •

Let the automaton be in a regular (neither accepting nor rejecting) state
$s_{i}$. Suppose a point $\xi$ is sampled. If the next vertex added to the
graph generated by the GSE algorithm using the point $\xi$ lies in $R_{i}$
then the automaton moves to the rejecting state.

  * •

If the next vertex added to the graph lies in the region $F_{i}$, then the
automaton progresses to the next state.

  * •

If the vertex added to the graph of GSE is in neither $F_{i}$ nor in $R_{i}$,
then the automaton stays in the same state.

By suitably defining the regions $R_{i}$ and $F_{i}$, we can classify the
solutions returned by the GSE algorithm. The accepting state is associated
with solutions that are of high cost. This motivates us to define these
regions as:

  * •

For all states $R_{i}=L_{1}$.

  * •

$F_{\text{init}}$ is a $\gamma\times\gamma$ square, with $\gamma$ a very small
postive number. This square shares its upper right corner with $B_{1}$ and is
homothetic to it.

  * •

$F_{1}\subset[1,\alpha+1]\times[\alpha+1,\alpha+2]$ is a sufficiently small
region that is parallel to $F_{\text{init}}$. We require that $F_{2}$ lies
entirely within the shape of every point of $F_{1}$.

  * •

$F_{2}$ is the mirror image of $F_{\text{init}}$ about the line $x=\alpha/2+1$

A sample illustration of the regions $F_{i}$ (the forward regions) and $R_{i}$
is provided in in Fig. 4.

Figure 4:  An illustration of the regions $F_{i}$ and $R_{i}$

####  III-C4 Low-quality solutions in the GSE algorithm

We analyse the solutions that the GSE algorithm returns for arbitrarily high
number of iterations. This then allows us to conclude whether the GSE
algorithm is asymptotically optimal or not. The reasoning for this is provided
below. Note that $Y_{n}$ is defined as in Section II-C

The proof below proceeds in two parts. Firstly, we prove that the automaton
provides a schematic for the GSE algorithm in some sense. More precisely, it
reaches certain states only when the GSE algorithm returns certain solutions.
Paths returned by the GSE, passing through the regions $B_{i}$ are not
optimal, we associate these paths with the accepting state of the automaton,
thereby if we show that the probability that the automaton reaches the
accepting state is bounded below, then we have the desired result. Before we
prove this, we define a few quantities.

######  Definition 3.

Associated with each (non-rejecting) state $s$ of the automaton, we define two
regions ${H}^{+}(s)$ and ${H}^{-}(s)$ as follows.

  * •

${H}^{-}(s)=L_{1}$ for all non-rejecting states $s$

  * •

$H^{+}(s_{\text{init}})=\emptyset$

  * •

$H^{+}(s_{1})$ = $F_{\text{init}}$

  * •

$H^{+}(s_{2})=F_{1}$

  * •

$H^{+}(s_{\text{accepting}})=F_{2}$

We denote by $\mathcal{E}_{i}$ the swath of GSE, that is, the union of line
segments that constitute its edges after $i$ iterations.

######  Lemma 1.

For all $\alpha\geq 2$, for any $\boldsymbol{X}_{\text{init}}$ in between
$L_{1}$ and $B_{1}$ in the free space, and $\boldsymbol{X}_{\text{goal}}$
between $L_{2}$ and $B_{2}$, The automaton described in Section III-C3 moves
to

  * •

A rejecting state on all inputs(samples) if GSE returns a Type-L solution

  * •

An accepting state only if GSE returns a Type-B solution

###### Proof.

Consider any two arbitrary points $X_{1},X_{2}\in L_{1}^{c}$. Then, the line
segment joining them does not pass through $L_{1}$ (with $L_{1}$ as in Fig. 3)
Consequently, in order to have a Type-L path, we must have a vertex of GSE in
$L_{1}$. But this moves the automaton to a rejecting state, by definition. The
first part of the lemma is proved. Now, for the second part of the lemma,
consider the following proposition.

######  Proposition 1.

Let a sequence of vertices $\xi_{1}\xi_{2}\dots\xi_{n}$ and swath
$\mathcal{E}_{n}$ be generated by the GSE algorithm and read by the automaton.
For each non-rejecting state $s$, if $H^{+}(s)\neq\emptyset$, then
$\mathcal{E}_{n}\cap H^{+}(s)\neq\emptyset$.

###### Proof.

We prove this claim by induction on $n$.

For $n=1$, if $\xi_{1}\in F_{\text{init}}$ then the automaton progresses to
state $s_{1}$. Since $H^{+}(s_{1})=F_{\text{init}}$, the claim is true.

Next, we assume that the claim holds for some $n\in\mathbb{N}$ and the
automaton is now in state $s_{i}$. Therefore,
$H^{+}(s_{i})\cap\mathcal{E}_{n}\neq\emptyset$. By statement of the
proposition, $s_{i}$ is non-rejecting. The automaton reads the vertex
$\xi_{n+1}$. In the event that the automaton does not progress, $H^{+}(s)$
remains fixed, referring to the induction hypothesis, the claim holds
trivially. If the next added vertex lies in $F_{i}$, the automaton moves
forward to the next state $s_{j}$, by Def. 2. Since $s_{j}\neq
s_{\text{init}}$, $H^{+}(s)$ is the same as $F_{i}$ (referring to Def. 3). we
have $H^{+}(s_{j})\cap\mathcal{E}_{n+1}\neq\emptyset$. This proves the claim
in Proposition 1. ∎

Lastly, we conclude that if the automaton reaches the accepting state on an
input $\xi_{1}\xi_{2}\dots\xi_{n}$, then we can conclude that a vertex of the
GSE graph lies in $F_{2}$. By suitably defining the region $F_{2}$, we ensure
that $\boldsymbol{X}_{\text{goal}}$ (the goal point) is in the shape of points
in $F_{2}$, and vice versa. Then the GSE algorithm will end on the
$({n+1})^{\text{th}}$ iteration. Further, the swath of GSE will intersect
$B_{1}$, $B_{2}$, and cannot intersect $L_{1}$. Consequently, the type of
solution will be Type-B, a high cost sub-optimal solution. This proves the
second part of lemma. ∎

The next lemma shows that the Automaton of Sampling Diagrams achieves an
accepting state with a probability that is bounded below, for an arbitrarily
high number of iterations.

######  Lemma 2.

If the sets $F_{i}$ in the definition of the automaton have a positive volume,
then there exists a constant $\pi_{0}>0$, and a natural number $N$ such that
for all $n\geq N$, $\mathbb{P}(\text{Accept})\geq\pi_{0}$, where, ’Accept’ is
the event that the automaton enters the accepting state after reading $n$
samples.

###### Proof.

We begin by assuming the automaton is in a state $s_{j}$, where
$j\notin\\{\text{accepting},\text{rejecting}\\}$. We define

  * •

an event $\mathfrak{F}(s_{j})$ as the event that the next sample is in a
region which moves the automaton forward.

  * •

an event $\mathfrak{R}(s_{j})$ as the event that the next sample is in a
region which moves the automaton to a rejecting state.

We call these events as ’critical events’. The probability that the automaton
moves forward (denoted $\pi(s_{j})$), given the occurrence of a critical event
can be given by

$\pi(s_{j})\triangleq\quad\mathbb{P}(\mathfrak{F}(s_{j})|\mathfrak{F}(s_{j})\cup\mathfrak{R}(s_{j}))=\frac{\mu(F_{j})}{\mu(F_{j})+\mu(R_{j})}$
(10)

Here, $\mu$ is the Lebesgue measure. The above equation allows us to draw two
conclusions. Firstly, given a sufficiently large input size, the automaton is
guaranteed to enter either an accepting or a rejecting state. The set of
states forms a finite state Markov Chain with $s_{\text{accepting}}$ and
$s_{\text{rejecting}}$ as ’trap’ states, and hence for arbitrarily large input
sizes, the automaton is guaranteed to visit one of these. Consequently we can
conclude that there exists a minimal $N$ such that for all $n>N$ after reading
$\xi_{1},\xi_{2},\dots\xi_{n}$.

$\mathbb{P}(\text{reached accepting or rejecting state})\geq 1-\delta$ (11)

Secondly, the probability of progressing from one state to the next is non-
zero. Let the probability of progressing from state $s_{j}$ to the next be
bounded below by some
$p_{j}>\frac{\mu(+F_{j})}{\mu(\mathbb{X}_{\text{free}})}$, by virtue of the
geometry of the problem. Denote $\pi_{1}=\prod_{j}p_{j}$. We then have that

$\quad\mathbb{P}(\text{Accept})\geq(1-\delta)(\pi_{1})=\pi_{0}$ (12)

Thus, the lemma follows. ∎

Thus, we have shown that the Automaton of Sampling Diagrams moves to an
accepting state, with a probability that is bounded below, for an arbitrarily
high number of iterations. Further by Lemma 1, we know that the automaton
reaches an accepting state only if a Type-B solution is returned by the GSE
algorithm. Hence the probability of returning a Type-B solution as defined in
Section III-C2 is bounded below by a positive constant. This leads us to the
following result that shows the GSE algorithm is not asymptotically optimal.

######  Theorem 1.

With $Y_{n},c^{\star}$ defined as in Section II-C,

$\quad\mathbb{P}(\\{\underset{n\to\infty}{\limsup}\;{Y_{n}}=c^{\star}\\})=0$
(13)

###### Proof.

Since $Y_{n}>c^{\star}$ for all $n>N$ with positive probability, then
$\underset{n\to\infty}{\limsup}\;{Y_{n}}>c^{\star}$ with positive probability,
which then means

$\mathbb{P}(\\{\underset{n\to\infty}{\limsup}\;{Y_{n}}=c^{\star}\\})<1$ (14)

Probabilistic completeness property of the GSE algorithm guarantees that the
algorithm returns a feasible path of finite cost. Therefore, invoking Lemma 25
of [14], we have the following.

######  Lemma 3.

[14] The event $\\{{\limsup}\;{Y_{n}}=c^{\star}\\}$ is a tail event, and hence
by Kolmogorov’s zero-one law, it has probability either 0 or 1.

Lemma 3 along with Eq. (14) verify that Eq. (13) holds. This proves the
theorem. ∎

From the above, we conclude that while the GSE is probabilistically complete,
it does not possess the desired asymptotic optimality property.

##  IV $\text{GSE}^{\star}$ Algorithm

###  IV-A Revised Algorithm

In this section, a modified version of the GSE algorithm is presented such
that the resulting algorithm possesses asymptotic optimality property, while
it also retains the advantages of the GSE algorithm. To this end, the
$\text{GSE}^{\star}$ algorithm is formulated, which along with its detailed
analysis (in Sections IV-B and IV-C) forms one of the key contributions of
this paper. The $\text{GSE}^{\star}$ in essence contains all the major steps
used in the iterations of the GSE algorithm. In addition, a few more routines
are added given in Line 10,13 and in Lines 17-23 in Algorithm 3.

Before describing the revised algorithm, we introduce a few
functions/notations used in the algorithm. First, the steer function
($\texttt{Steer}(\boldsymbol{X},\boldsymbol{Y},\eta)$) in Line 10 of Algorithm
3 returns a point
$\boldsymbol{z}\in\mathbb{X}_{\text{free}}\cap\mathcal{B}_{\eta}(\boldsymbol{X})$,
which is closest to the point $\boldsymbol{Y}$, that is, it returns a point
lying in $\mathcal{B}_{\eta}(\boldsymbol{X})$ and on the line joining
$\boldsymbol{X}$ and $\boldsymbol{Y}$. More precisely,
$\texttt{Steer}(\boldsymbol{X},\boldsymbol{Y},\eta)$ is given as,

$\displaystyle\texttt{Steer}(\boldsymbol{X},\boldsymbol{Y},\eta)=\underset{z\in\mathcal{B}_{\eta}(\boldsymbol{X})\cap\mathbb{X}_{\text{free}}}{\text{argmin}}\|\boldsymbol{z}-\boldsymbol{Y}\|$
(15)

Here, the parameter $\eta$ is arbitrary, and can be suited to the problem at
hand.

Let $|\mathbb{V}|$ denote the number of vertices of the graph generated by the
GSE algorithm. Following Lines 9 and 10 of Algorithm 3 at each iteration of
the algorithm two vertices are added. Line 9 adds a vertex following the GSE
algorithm, while Line 10 introduces another vertex following the steering
procedure in Eq. (15).

We then define
$r_{n}\triangleq\text{min}(\gamma_{\text{GSE}^{\star}}(\frac{\text{log}(|\mathbb{V}|)}{|\mathbb{V}|}))^{\frac{1}{d}},\eta)$.
This choice of connection radius $r_{n}$ is motivated to ensure asymptotic
optimality. In the proof of optimality, relevant bounds will be established
for the constant $\gamma_{\text{GSE}^{\star}}$. The function
($\texttt{Near}\left(\mathbb{V},\boldsymbol{X},r_{n}\right)$) returns a subset
of the set of vertices of the graph generated by the GSE⋆ algorithm. These
consist of those vertices which lie within the ball
$\mathcal{B}_{r_{n}}{(\boldsymbol{X})}$. That is,

$\displaystyle\texttt{Near}\left(\mathbb{V},\boldsymbol{X},r_{n}\right)=\mathbb{V}\cap\mathcal{B}_{r_{n}}(\boldsymbol{X})$
(16)

Thus, the additional routines in the GSE⋆ algorithm over the earlier GSE
algorithm are stated below. In Line 10 of Algorithm 3, a new vertex is added
using the steering function Eq. (15). Subsequently, we use the Near function,
with connection radius as defined above, to establish a set of candidate
vertices for connection in Line 18. Lines 19-22 use the Shape function to
check for viability of collision-free connection with each vertex and add
edges to the edge set. Note that using the Shape function is a stronger
criterion than collision-checking, and is a key feature of the
$\text{GSE}^{\star}$ algorithm similar to the GSE algorithm.

###  IV-B Probabilistic Completeness of the $\text{GSE}^{\star}$ Algorithm

We denote the vertex set and edge set of $\text{GSE}^{\star}$ after $n$
iterations by $\mathbb{V}_{n}^{\text{GSE}^{\star}}$ and
$\mathbb{E}_{n}^{\text{GSE}^{\star}}$, respectively. The graph of the GSE⋆
algorithm is denoted $\mathbb{G}^{\text{GSE}^{\star}}_{n}$. As discussed in
Section IV-A, the $\text{GSE}^{\star}$ algorithm adds vertices and edges to
the graph generated by the GSE algorithm. So, at any iteration,
$\mathbb{V}_{n}^{\text{GSE}}\subset\mathbb{V}_{n}^{\text{GSE}^{\star}}$ and
$\mathbb{E}_{n}^{\text{GSE}}\subset\mathbb{E}_{n}^{\text{GSE}^{\star}}$. We
note that the GSE algorithm is probabilistically complete, as described in
Section III-B (details available in [26]). Since the graph generated by the
GSE algorithm is a subgraph of the graph generated by the $\text{GSE}^{\star}$
algorithm, the $\text{GSE}^{\star}$ algorithm is probabilistically complete.

Algorithm 3 $\text{GSE}^{\star}$ Algorithm

1:${\mathbb{V}\leftarrow\\{\boldsymbol{X_{\text{init}}},\boldsymbol{X_{\text{goal}}}\\}},\;\;\mathbb{E}\leftarrow\emptyset$

2:${\mathcal{S}_{\text{init}}(\boldsymbol{P})\leftarrow\texttt{Shape}(\boldsymbol{P},\boldsymbol{X_{\text{init}}},\mathbb{X}_{\text{obs}})}$

3:${\mathcal{S}_{\text{goal}}(\boldsymbol{P})\leftarrow\texttt{Shape}(\boldsymbol{P},\boldsymbol{X_{\text{goal}}},\mathbb{X}_{\text{obs}})}$

4:for $j=1\dots n$ do

5: $\boldsymbol{X_{\text{rand}}}\leftarrow\texttt{GenerateSample}$

6:
$\mathcal{S}_{\text{rand}}(\boldsymbol{P})\leftarrow\texttt{Shape}(\boldsymbol{P},\boldsymbol{X_{\text{rand}}},\mathbb{X}_{\text{obs}})$

7:
$\boldsymbol{X_{\text{nearest}}}\leftarrow\texttt{Nearest}(\mathbb{G}=(\mathbb{V},\mathbb{E}),\boldsymbol{X_{\text{rand}}})$

8:
$\mathcal{S}_{\text{nearest}}(\boldsymbol{P})\leftarrow\texttt{Shape}(\boldsymbol{P},\boldsymbol{X_{\text{nearest}}},\mathbb{X}_{\text{obs}})$

9: $\boldsymbol{X_{\text{new,g}}}\leftarrow\texttt{Steer-
GSE}(\boldsymbol{X_{\text{nearest}}},\boldsymbol{X_{\text{rand}}})$

10:
$\boldsymbol{X_{\text{new}}}\leftarrow\texttt{Steer}(\boldsymbol{X_{\text{nearest}}},\boldsymbol{X_{\text{rand}}},\eta)$

11:
$\mathcal{S}_{\text{new,g}}(\boldsymbol{P})\leftarrow\texttt{Shape}(\boldsymbol{P},\boldsymbol{X_{\text{new,g}}},\mathbb{X}_{\text{obs}})$

12:
$\mathbb{X_{\text{intersect}}}\leftarrow\texttt{IntersectShape}(G,\boldsymbol{X_{\text{new,g}}})$

13:
$\mathbb{V}\leftarrow\mathbb{V}\cup\\{\boldsymbol{X_{\text{new,g}}},\boldsymbol{X_{\text{new}}}\\}$

14: for $\boldsymbol{X_{\text{n}}}\in\mathbb{X}_{\text{intersect}}$ do

15:
$\mathbb{E}\leftarrow\mathbb{E}\cup\\{(\boldsymbol{X_{\text{n}}},\boldsymbol{X_{\text{new,g}}})\\}$

16: end for

17:
$b\leftarrow\gamma_{\text{GSE}^{\star}}(\log(|\mathbb{V}|)/|\mathbb{V}|)^{1/d}$

18:
$\mathbb{U}\leftarrow\texttt{Near}\left(\mathbb{V},\boldsymbol{X}_{\text{new}},\text{min}(b,\eta)\right)$

19: for $\boldsymbol{u}\in\mathbb{U}$ do

20: if
$\texttt{Shape}(\boldsymbol{u},\boldsymbol{X}_{\text{new}},\mathbb{X}_{\text{obs}})=0$
then

21:
$\mathbb{E}\leftarrow\mathbb{E}\cup\\{(\boldsymbol{u},\boldsymbol{X}_{\text{new}})\\}$

22: end if

23: end for

24: $\mathbb{G}=(\mathbb{V},\mathbb{E})$

25:
$Path_{\text{init,goal,shortest}}\leftarrow\texttt{MinPath}(G,\boldsymbol{X_{\text{init}}},\boldsymbol{X}_{\text{goal}})$

26:end for

###  IV-C Asymptotic Optimality of the $\text{GSE}^{\star}$ Algorithm

We now turn to proving that the GSE⋆ algorithm is asymptotically optimal.
Since the GSE⋆ algorithm is quite similar in spirit to the RRG algorithm, we
can reasonably expect this to be the case. In the following proof, we
carefully follow the argument provided in [14] for the RRG algorithm. However,
the selection of the parameter $\gamma_{\text{GSE}^{\star}}$ has been altered
from $\gamma_{\mathrm{GSE}}$ to better suit for the analysis of the GSE⋆
algorithm.

Let the optimal path between the initial and goal points be $\sigma^{\star}$.
This path is assumed to have weak $\delta$-clearance. We assume that
$\mu(\sigma^{\star})=0$, where $\mu$ is the Lebesgue measure on
$\mathbb{X}_{\text{free}}$. The following lemma, while substantially similar
to Lemma 50 of [14], is included for completeness. Further, it serves to
introduce several key parameters.

######  Lemma 4.

For a path $\sigma^{\star}$ with optimal cost and weak $\delta$-clearance
($\delta>0$), there exists a sequence of paths $(\sigma_{n})_{n=1}^{\infty}$
such that the following holds

  * •

Each path $\sigma_{n}$ has strong $\delta_{n}$-clearance, with
$0<\delta_{n}\leq\delta$ and
$\underset{n\rightarrow\infty}{\lim}\;\delta_{n}=0$

  * •

$\underset{n\rightarrow\infty}{\lim}\;\sigma_{n}=\sigma^{\star}$

###### Proof.

Let us define
$\delta_{n}\triangleq\text{min}(\delta,\frac{(1+\phi)r_{n}}{2+\phi})$, for
some bounded positive constant $\phi$. Clearly, the sequence $\delta_{n}$
(with $0\leq\delta_{n}\leq\delta$) is non-increasing and
$\underset{n\to\infty}{\text{lim}}\delta_{n}=0$ (from the definitions of
$\delta_{n}$ and $r_{n}$). Consider the $n^{\text{th}}$ element of the
sequence $(\mathcal{Y}_{n})_{n=1}^{\infty}$ defined as
$\mathcal{Y}_{n}=\\{x\in\mathbb{X}_{\text{free}}\mid\mathcal{B}_{\delta_{n}}(x)\subseteq\mathbb{Y}_{\text{free}}\\}$.

Now, we consider a homotopy $\psi$ where $\psi(0)=\sigma_{0}$ is a path with
strong $\delta$-clearance. Further, $\psi(1)=\sigma^{\star}$. We can then
define $\sigma_{n}=\psi(\alpha_{n})$, where
$\alpha_{n}=\underset{[0,1]}{\text{max}}\\{t\mid\psi(t)\subseteq\mathcal{Y}_{n}\\}$,
that is, $\sigma_{n}$ is the path with strong $\delta_{n}$-clearance that is
’closest’ to $\sigma^{\star}$. Since $\delta_{n}$ is a non-increasing
sequence, by construction,
$\mathcal{Y}_{1}\subseteq\mathcal{Y}_{2}\subseteq\dots\subseteq\mathbb{X}_{\text{free}}$.
And, $\bigcup_{n=1}^{\infty}\mathcal{Y}_{n}=\mathbb{X}_{\text{free}}$. This
establishes that the limit
$\underset{n\rightarrow\infty}{\text{lim}}\alpha_{n}$ exists. Let this limit
be $a$. Then,
$\underset{n\rightarrow\infty}{\text{lim}}\psi(\alpha_{n})=\psi(a)$. Further,
the clearance of $\psi(a)$ is given by
$\underset{n\rightarrow\infty}{\text{lim}}\delta_{n}$. By the construction of
the homotopy $\psi$, the clearance of $\psi(\alpha)$ is zero, only if
$\alpha=1$. Since $\underset{n\rightarrow\infty}{\text{lim}}\delta_{n}=0$,
$a=1$. Thus, $\underset{n\rightarrow\infty}{\text{lim}}\alpha_{n}=1$.
Consequently,
$\underset{n\rightarrow\infty}{\text{lim}}\sigma_{n}=\sigma^{\star}$ ∎

We have proved that there exists a sequence of paths
$(\sigma_{n})_{n=1}^{n=\infty}$ such that each path $\sigma_{n}$ has strong
$\delta_{n}$-clearance, and $\underset{n\to\infty}{\text{lim}}\delta_{n}=0$,
and $\underset{n\to\infty}{\text{lim}}\sigma_{n}=\sigma^{\star}$. Next, we
show that the probability that there exists a path approximating $\sigma_{n}$
in $\mathbb{G}^{\text{GSE}^{\star}}_{n}$ tends to 1 as $n\rightarrow\infty$.
This would ensure that the algorithm is asymptotically optimal. The following
definition provides us with a framework toward generating the paths in GSE⋆
that approximate $\sigma_{n}$.

######  Definition 4.

We define the sets

$\mathfrak{B}(\sigma_{n},q_{n},l_{n})\triangleq\\{B_{n,1},B_{n,2},B_{n,3},\dots
B_{n,M_{n}}\\}$ (17)

Where each element $B_{n,m}$ is a ball of radius
$q_{n}=\frac{\delta_{n}}{2+\phi}$, $l_{n}=\phi q_{n}$ and

  * •

The center of $B_{n,1}$ is $\sigma_{n}(0)=\sigma(0)$.

  * •

The center of $B_{n,k}$ is $\sigma_{n}(b_{k})$ , where
$b_{k}\triangleq\min\\{b\in[b_{k-1},1]\mid|\sigma(b)-\sigma(b_{k-1})|\geq\phi
l_{n}\\}$ for $k\in\\{2,3,\dots M_{n}\\}$

  * •

The centre of the last ball must be the point $\sigma_{n}(1)$, that is,
$b_{M_{n}}=1$.

Figure 5: Ilustration of the paths $\sigma_{n}$ and $\sigma_{n}^{\prime}$
along with the set of covering balls $\mathfrak{B}(\sigma_{n},q_{n},l_{n})$

A representation of the set $\mathfrak{B}(\sigma_{n},q_{n},l_{n})$ is shown in
Fig. 5.

We wish to show that each ball $B_{n,m}$ contains a vertex of GSE⋆ with higher
probability as $n$ increases. This then allows us to consider paths in the
graph of GSE⋆ that can approximate paths in the sequence
$(\sigma_{n})_{n=1}^{\infty}$. To prove this result, we first show that any
point in the free-space is ’close’ to a vertex of $\text{GSE}^{\star}$ after
$n$ iterations with a probability that increases with $n$.

######  Lemma 5.

Let $C_{n}$ at any iteration $n$ denote the event that for any
$x\in\mathbb{X}_{\text{free}}$, there exists a vertex
$v\in\mathbb{V}_{n}^{\text{GSE}^{\star}}$, such that $\|x-v\|\leq\eta$. Then
$\mathbb{P}(C_{n}^{c})\leq ae^{-bn}$ for some positive $\eta,a,b$.

###### Proof.

Define the diameter of a set $\mathbb{S}$ by $d(\mathbb{S})$ for a set
$\mathbb{S}\subset\mathbb{R}^{2}$ as follows:

$\displaystyle d(\mathbb{S})=\underset{x,y\in\mathbb{S}}{\sup}\|x-y\|$ (18)

We partition the set $\mathbb{X}_{\text{free}}$ into a collection of sets
$\\{\mathcal{X}_{i}\\}$ for $i\in\\{1,2,\dots,M\\}$, with $M$ finite, such
that $d(\mathcal{X}_{i})<\eta$ for all $i\in\\{1,2,\dots,M\\}$. An
illustration is provided in Fig. 6. Define the indicator variable

$C_{n,i}\triangleq\begin{cases}0&\text{if
$\mathbb{V}_{n}^{\text{GSE}^{\star}}\cap{\mathcal{X}_{i}}=\emptyset$}\\\
1&\text{otherwise}\end{cases}$ (19)

We can conclude that $C_{n}^{c}\subseteq\bigcup_{i=1}^{M}C_{n,i}^{c}$.
Consequently

$\mathbb{P}(C_{n}^{c})\leq\sum_{i=1}^{M}\mathbb{P}(C_{n,i}^{c})$ (20)

We now wish to show that $\mathbb{P}(C_{n,i}^{c})$ decays exponentially with
increasing $n$.

Figure 6: An illustration of $\mathbb{X}_{\text{free}}$ and the corresponding
exhaustive partitions $\mathcal{X}_{1},\mathcal{X}_{2},\dots,\mathcal{X}_{M}$.
The black regions represent the obstacles.

To this end, define the function $g:2^{\mathbb{X}_{\text{free}}}\rightarrow
2^{\mathbb{X}_{\text{free}}}$ as

$g(S)=\\{y\in\mathbb{X}_{\text{free}}\mid B_{\eta}(x)\cap S\neq\emptyset\\}$
(21)

The iterates of $g$ are denoted $g^{(2)}(S)=g(g(S))$,
$g^{(3)}(S)=g^{(2)}(g(S))$ etc. For convenience, we denote $g(\\{x\\})$ (where
$x\in\mathbb{X}_{\text{free}}$) by $g(x)$. Notice that $\mu(g^{(t)}(x))$ is an
increasing function of $t$. We conclude that there exists an iterate of $g$
such that $\mathbb{X}_{\text{free}}\subseteq g^{(t)}(x)$, for any given point
$x\in\mathbb{X}_{\text{free}}$. Consider a point
$x\in\mathbb{X}_{\text{free}}$. We wish to show that

$\quad\mathbb{P}(g(x)\cap\mathbb{V}_{n}^{\text{GSE}^{\star}}=\emptyset)\leq
a_{x}e^{-b_{x}n}$ (22)

Let $i_{k}\triangleq\underset{i\in\mathbb{N}}{\text{min}}(\mathbb{V}_{k}\cap
g^{(i)}(x)\neq\emptyset)$. Further

$\displaystyle D_{k}\triangleq\begin{cases}0&\text{if $i_{k+1}\geq i_{k}$}\\\
1&\text{otherwise}\end{cases}$ (23)

And let $D=\sum_{i=1}^{n}D_{i}$. Since passing to successive iterates of the
function $g$ at least $t$ times, ensures that there will be a vertex of
$\text{GSE}^{\star}$ in the set $g(x)$,

$\quad\mathbb{P}(\text{there is no vertex in
$B_{\eta}(x)$})\leq\quad\mathbb{P}(D<t)$.

We claim that for $k^{\text{th}}$ iteration of GSE, $\mathbb{P}(D_{k}=1)$ is
bounded below by a positive fraction $p<1$ for all $k$ where $i_{k}>1$.

At the $k^{\text{th}}$ iteration of GSE⋆, let there be $w$ $(\leq k)$ vertices
of the GSE⋆ generated graph belonging to ${g}^{(i_{k})}(x)$. We need to
establish that for any location of these $w$ vertices in ${g}^{(i_{k})}(x)$,
there is a minimum probability of extending the graph to a point belonging to
${f}^{(i_{k}-1)}(x)$.

It is evident that the probability of $D_{k}=1$ increases as $w$ increases
(assuming other $k-w$ vertices are kept fixed), because the number of
candidate vertices for extension of the GSE⋆ generated graph increases. Hence,
it suffices to consider the case, wherein $w=1$. Let this vertex be denoted by
$\boldsymbol{Z}$. Let $\text{Vor}(\boldsymbol{Z})$ denote the Voronoi cell of
the vertex $\boldsymbol{Z}$ in the Voronoi diagram consisting of the $k$
vertices of the GSE-generated graph so far. Because of the finite number $(k)$
of vertices in the GSE-generated graph, for the only vertex
$\boldsymbol{Z}\in{g}^{(i_{k})}(x)$, it follows that
$\mu(\text{Vor}(\boldsymbol{Z}))$ is bounded below by some number, say
$e_{i_{k}}$. Now, for any arbitrary location of $\boldsymbol{Z}$ in the set
${g}^{(i_{k})}(x)$, there is a subset of $\text{Vor}(\boldsymbol{Z})$, in
which, when the new vertex is sampled, leads to extension of the GSE-generated
graph to a point in ${g}^{(i_{k}-1)}(x)$. Let the minimum of this fraction
over the set ${g}^{(i_{k})}(x)$ be denoted as $g_{i_{k}}$. We can then
conclude that for $p=\underset{i_{k}\leq
m}{\text{min}}{\frac{{e_{i_{k}}g_{i_{k}}}}{{\mu(\mathbb{X}_{\text{free}})}}}$,
the statement holds true.

Let us next consider a random variable $E=\sum_{i=1}^{t}E_{i}$, where the
$E_{i}$ are i.i.d Bernoulli random variables ($\mathbb{P}(D_{i}=1)=p$). We
know that $\mathbb{P}(D_{i}=1)>\mathbb{P}(D_{i}=1)=p$. Consequently, the
probability distribution of the random variable $C$ provides a worst-case
(lower) bound on $\mathbb{P}(D>t)$, that is,
$\mathbb{P}(D>t)>\mathbb{P}(E>t)$. Taking complement on both sides we have
$\mathbb{P}(D<t)<\mathbb{P}(E<t)$. We can then use Chernoff bounds on the
random variable $E$, so for some $k,l>0$:

$\displaystyle\mathbb{P}(D<t)$ $\displaystyle\leq\mathbb{P}(C<t)\leq
e^{-2t}e^{-ln+k/n}$ (24)

For sufficiently large $n$, $\mathbb{P}(D<t)\leq a_{x}e^{-b_{x}n}$. Since
$\mathbb{P}(\mathbb{V}_{n}^{\text{GSE}^{\star}}\cap
g(x)=\emptyset)\leq\mathbb{P}(D<t)$, we have $\mathbb{P}(\mathbb{V}_{n}\cap
g(x)=\emptyset)\leq a_{x}e^{-b_{x}n}$.

Since we can select a suitable point in any given set $\mathcal{X}_{i}$ and
apply the argument above, we can conclude that,therefore (from Eq. (19))
$\mathbb{P}(C_{n,i}^{c})\leq a_{i}e^{-b_{i}n}$ for some positive $a_{i},b_{i}$
independent of $n$. Therefore, we can conclude that since the sum of finitely
many exponentially decreasing functions is bounded above by an exponentially
decreasing function, from Eq. (20) one can conclude that
$\mathbb{P}(C_{n}^{c})\leq ae^{-bn}$. ∎

Let $\rho\in(0,1)$ be a variable independent of $n$. The event
$(\bigcap_{i=\lfloor\rho n\rfloor}^{n}C_{i})$ indicates that between the
$\lfloor\rho n\rfloor^{\text{th}}$ iteration and the $n^{\text{th}}$ iteration
of the GSE⋆ algorithm, it is not necessary to steer any new sampled vertices
using the Steer function (Line 10 of Algorithm 3), they are directly added to
the graph. Since bounds on the probability of this event are very relevant to
our proof, we have the following lemma.

######  Lemma 6.

$\sum_{n=1}^{\infty}\mathbb{P}\left((\bigcap_{i=\lfloor\rho
n\rfloor}^{n}C_{i})^{c}\right)<\infty$

###### Proof.

Since

$\sum_{n=1}^{\infty}\mathbb{P}\left((\bigcap_{i=\lfloor\rho
n\rfloor}^{n}C_{i})^{c}\right)=\sum_{n=1}^{\infty}\mathbb{P}(\bigcup_{i=\lfloor\rho
n\rfloor}^{n}C_{i}^{c})\leq\sum_{n=1}^{\infty}\sum_{i=\lfloor\rho
n\rfloor}^{n}ae^{-bn}$ (25)

We conclude that $\sum_{n=1}^{\infty}\mathbb{P}((\bigcap_{i=\lfloor\rho
n\rfloor}^{n}C_{i})^{c})<\infty$ for all $a,b\in\mathbb{R}$. ∎

Lemmas 5 and 6 above lead us to the proof of Lemma 7 below.

######  Lemma 7.

Define the event $A_{n,m}$ as the event that the ball $B_{n,m}$ contains a
vertex of $\text{GSE}^{\star}$ after $n$ iterations of the
$\text{GSE}^{\star}$ algorithm. Define
$A_{n}\triangleq\bigcap_{m=1}^{M_{n}}A_{n,m}$, where $M_{n}$,$B_{n,m}$ etc.
are as defined in Def. 4. Then,
$\mathbb{P}(\underset{n\rightarrow\infty}{\liminf}A_{n})=1$.

###### Proof.

We proceed by showing that $\sum_{n=1}^{\infty}\mathbb{P}({A_{n}^{c}})$ is
bounded. Then by the Borel-Cantelli lemma, we have
$\mathbb{P}(\underset{n\rightarrow\infty}{\limsup}A_{n}^{c})=0$. Thus, we can
directly conclude that
$\mathbb{P}(\underset{n\rightarrow\infty}{\liminf}A_{n})=1$.

Let $s_{n}$ be the length of the path $\sigma_{n}$. We denote the volume of
the unit ball in $\mathbb{R}^{d}$ by $\zeta_{d}$. Let
$n_{0}=\underset{n\in\mathbb{N}}{\text{argmin}}(\delta_{n}<\delta)$. In what
follows, we are concerned only with $n$ for which $n>n_{0}$. Since the
distance between the centres of two successive balls (along the curve) in
$\mathfrak{B}_{\sigma_{n},q_{n},l_{n}}$ is less than $l_{n}$, we can conclude
that

$\displaystyle M_{n}<\frac{(2+\phi)^{2}s_{n}}{\phi(1+\phi)\delta_{n}}\;,\hskip
28.45274pt\mu(B_{n,m})=\zeta_{d}q_{n}^{d}$ (26)

Let $\lfloor x\rfloor$ denote the largest integer less than a given real
number $x$. Observe that the event $\bigcap_{i=\lfloor\rho
n\rfloor}^{n}C_{i}$, denotes the event that every vertex sampled between
iterations $\lfloor\rho n\rfloor$ and $n$ is within $\eta$ distance of an
existing vertex of GSE⋆. This ensures that every new vertex of the
$\text{GSE}^{\star}$ graph does not need to be steered. Thus, we can treat the
vertices in the set
$\mathbb{V}^{\text{GSE}^{\star}}_{n}\setminus\mathbb{V}^{\text{GSE}^{\star}}_{\lfloor\rho
n\rfloor}$ as being uniform random samples that are added directly to the
graph generated by GSE⋆. Let $h\triangleq\frac{1+\phi}{(2+\phi)^{2}}$. The
conditional probability of the event $A_{n,m}^{c}$ conditioned on the event
$\bigcap_{i=\lfloor\rho n\rfloor}^{n}C_{i}$) can therefore be given by

$\displaystyle\mathbb{P}\left(A_{n,m}^{c}\mid\bigcap_{i=\lfloor\rho
n\rfloor}^{n}C_{i}\right)$
$\displaystyle\leq\left(1-\frac{\mu(B_{n,m})}{\mu(\mathbb{X}_{\text{free}})}\right)^{n-\lfloor\rho
n\rfloor}$
$\displaystyle\leq\left(1-\frac{\zeta_{d}r_{n}^{d}}{h^{d}\mu(\mathbb{X}_{\text{free}})}\right)^{(1-\rho)n}$
(27)

We observe that $(1-x)^{a}\leq e^{-ax}$ for real $a$ and $x$. Thus the right
hand side of Eq. (27) is bounded above as follows:

$\displaystyle\left(1-({\zeta_{d}r_{n}^{d}}/{h^{d}\mu(\mathbb{X}_{\text{free}}))}\right)^{(1-\rho)n}$
$\displaystyle\leq
e^{\frac{(\rho-1)n\zeta_{d}r_{n}^{d}}{h^{d}\mu(\mathbb{X}_{\text{free}})}}$
(28)

Since $|\mathbb{V}^{GSE^{\star}}_{n}|\geq n$, we know that for sufficiently
large $n$,
$\frac{{\text{log}(\text{card}(\mathbb{V}))}}{\text{card}(\mathbb{V})}\leq\frac{\text{log}(n)}{{n}}$.
Thus, we have the following inequality

$\displaystyle
e^{\left[{\frac{(\rho-1)n\zeta_{d}r_{n}^{d}}{h^{d}\mu(\mathbb{X}_{\text{free}})}}\right]}$
$\displaystyle\leq
n^{\left[{\frac{-(1-\rho)(\gamma_{\text{GSE}^{\star}}^{d}\zeta_{d})}{\mu(\mathbb{X}_{\text{free}})h^{d}}}\right]}$
(29)

From Eqs. (27) and (28) along with the definition of $A_{n}$,

$\displaystyle\mathbb{P}\left(A_{n}^{c}\mid\bigcap_{i=\lfloor\rho
n\rfloor}^{n}C_{i}\right)$ $\displaystyle\leq
M_{n}n^{\frac{-(1-\rho)(\gamma_{\text{GSE}^{\star}}^{d}\zeta_{d})}{\mu(\mathbb{X}_{\text{free}})h^{d}}}$
$\displaystyle\leq\frac{(2+\phi)^{2}s_{n}n^{\frac{1}{d}-\frac{(1-\rho)(\gamma_{\text{GSE}^{\star}}^{d}\zeta_{d})}{\mu(\mathbb{X}_{\text{free}})h^{d}}}}{(1+\phi)\phi\gamma_{\text{GSE}^{\star}}{\text{log}(n)}^{\frac{1}{d}}}$
(30)

In order to ensure that the left hand side of Eq. (IV-C) decreases with
increasing $n$ and to have the sequence on the right hand side of Eq. (IV-C)
summable, we require that the exponent of $n$ be less than -1, that is,

$\displaystyle\left[({(1-\rho)(\gamma_{\text{GSE}^{\star}}^{d}\zeta_{d})}/{\mu(\mathbb{X}_{\text{free}})h^{d}})-({1}/{d})\right]>1$
(31)

If the above criterion is satisfied, the sequence
$\sum_{n=1}^{\infty}\mathbb{P}(A_{n}^{c}\mid\bigcap_{i=\lfloor\rho
n\rfloor}^{n}C_{i})$ is summable. Therefore, the condition on
$\gamma_{\text{GSE}^{\star}}$ reduces to the following:

$\gamma_{\text{GSE}^{\star}}>h\left[\left(1+\frac{1}{d}\right)\left(\frac{\mu(\mathbb{X}_{\text{free}})}{1-\rho}\right)\right]^{\frac{1}{d}}$
(32)

Lastly, from the definition of conditional probability, we have,

$\displaystyle\mathbb{P}(A_{n}^{c}\mid\bigcap_{i=\lfloor\rho
n\rfloor}^{n}C_{i})$
$\displaystyle=\frac{\mathbb{P}\left(A_{n}^{c}\cap(\bigcap_{i=\lfloor\rho
n\rfloor}^{n}C_{i})\right)}{\mathbb{P}(\bigcap_{i=\lfloor\rho
n\rfloor}^{n}C_{i})}$
$\displaystyle\geq\mathbb{P}\left(A_{n}^{c}\cap\left(\bigcap_{i=\lfloor\rho
n\rfloor}^{n}C_{i}\right)\right)$
$\displaystyle\geq\mathbb{P}(A_{n}^{c})-\mathbb{P}\left((\bigcap_{i=\lfloor\rho
n\rfloor}^{n}C_{i})^{c}\right)$ (33)

Thus, we have:

$\sum_{n=1}^{\infty}\mathbb{P}(A_{n}^{c})\leq\sum_{n=1}^{\infty}\mathbb{P}\left(A_{n}^{c}\mid\bigcap_{i=\lfloor\rho
n\rfloor}^{n}C_{i}\right)+\sum_{n=1}^{\infty}\mathbb{P}\left((\bigcap_{i=\lfloor\rho
n\rfloor}^{n}C_{i})^{c}\right)$ (34)

Following Lemma 6 and Eq. (IV-C), the right hand side of the inequality above
is summable. Therefore, $\sum_{n=1}^{\infty}\mathbb{P}(A_{n}^{c})$ is bounded,
and consequently by the Borel-Cantelli lemma, we have
$\mathbb{P}(\underset{n\rightarrow\infty}{\limsup}A_{n}^{c})=0$. Taking
complement on both sides of
$\mathbb{P}(\underset{n\rightarrow\infty}{\limsup}A_{n}^{c})=0$ gives
$\mathbb{P}(\underset{n\rightarrow\infty}{\liminf}A_{n})=1$. ∎

######  Observation 1.

Observe that the distance between two points $x\in B_{n,i}$ and $y\in
B_{n,i+1}$ can be bounded as

$\displaystyle\|x-y\|\leq 2q_{n}+l_{n}\leq\delta_{n}<r_{n}$ (35)

Further, the distance between any point $x$ in a ball $B_{n,i}$ to the
obstacle space is denoted $o_{x}$. Then
$o_{x}\leq\delta_{n}-q_{n}\leq\delta_{n}$. Thus, for any point $x\in B_{n,i}$,
every point in the adjacent balls lies in the shape of the point $x$. We have
shown in Lemma 7 that the event $A_{n}$ occurs infinitely often with
probability one as $n$ increases to infinity. Since the distance between any
two points in two consecutive balls is less than $r_{n}$, two vertices in
consecutive balls are connected. Consequently, we can claim that the
$\text{GSE}^{\star}$ algorithm creates a path between the initial and goal
points.

######  Definition 5.

(Homogeneous Poisson Point Process ([29]))

Let $\text{Poisson}(a)$ be the Poisson random variable of intensity $a$. A
homogeneous Poisson point process of intensity $\nu$ on $\mathbb{R}^{d}$ is a
random set of countable points $P_{\nu}^{d}\subset\mathbb{R}^{d}$. The set
$P_{\nu}^{d}$ is such that for disjoint and measurable sets
$S_{1},S_{2}\in\mathbb{R}^{d}$ , we have $|(P_{\nu}^{d}|\cap
S_{1})=\text{Poisson}(\mu(S_{1})\nu)$ and $|(P_{\nu}^{d}|\cap
S_{2})=\text{Poisson}(\mu(S_{2})\nu)$ with $\text{Poisson}(\mu(S_{2})\nu)$ and
$\text{Poisson}(\mu(S_{1})\nu)$ independent.

######  Lemma 8.

([29]) Given a sequence of points $(x_{i})_{i=1}^{\infty}$ drawn independently
and uniformly from a measurable set $L\subseteq\mathbb{R}^{d}$. The set
$\\{x_{1},x_{2},\dots,x_{\text{Poisson}(\tau)}\\}$ is the restriction to $L$
of a homogeneous Poisson point process of intensity $\frac{\tau}{\mu(L)}$

Let the set of paths in the graph generated by the $\text{GSE}^{\star}$
algorithm in $n$ iterations be $\mathfrak{P}_{n}$. Let us define
$\sigma_{n}^{{}^{\prime}}\triangleq\underset{\sigma^{{}^{\prime}}\in\mathfrak{P}_{n}}{\operatorname{argmin}}\|\sigma_{n}-\sigma^{\prime}\|_{\text{BV}}$.
It now remains to be shown that the sequence of paths
$\sigma_{n}^{{}^{\prime}}$ converges to the path $\sigma^{\star}$.

######  Theorem 2.

With the sequence $(\sigma^{\prime}_{n})_{n=1}^{\infty}$ defined as above,
$\mathbb{P}({\underset{n\rightarrow\infty}{\lim}\|\sigma_{n}-\sigma^{\prime}_{n}\|_{\text{BV}}=0})=1$

###### Proof.

We establish this by showing that
$\sum_{n=1}^{\infty}\mathbb{P}(\|\sigma_{n}-\sigma^{\prime}_{n}\|_{\text{BV}}>\epsilon)$
is finite for any $\epsilon>0$. Thus, by a straightforward application of the
Borel-Cantelli lemma, we can conclude that
$\mathbb{P}({\underset{n\rightarrow\infty}{\lim}\|\sigma_{n}-\sigma_{n}^{\prime}\|=0})=1$.

(a) GSE for $m=4$. Cost: 3.64

(b) GSE⋆ for $m=4$. Cost: 3.61

(c) GSE for $m=4$. Cost: 84.32

(d) GSE⋆ for $m=4$. Cost: 81.37

Figure 7: Illustration of the shortest feasible paths (red lines) found by the
GSE and GSE⋆ algorithms in 50 iterations, in 2-D and 3-D environments with 4
obstacles.

(a) Convergence plot for $m=4$

(b) Convergence plot for $m=16$

Figure 8: Convergence to the optimal cost for the GSE and GSE* algorithms

We fix $\epsilon>0$, Let $\alpha,\beta\in(0,1)$ be two constants independent
of $n$. Define $\beta B_{n,m}$ to be a ball concentric with $B_{n,m}$, and has
a radius of $\beta q_{n}$. Fig. 5 provides a representation of the covering
balls and paths.

We define the indicator variables $I_{n,m}$ as follows,

$I_{n,m}\triangleq\begin{cases}0&\text{if there is a vertex of GSE* in the
ball $\beta B_{n,m}$ }\\\ 1&\text{otherwise}\end{cases}$

We define $K_{n}=\sum_{m=1}^{M_{n}}I_{n,m}$. This measures the number of balls
$\beta B_{n,m}$ which do not contain a vertex from the graph constructed by
the $\text{GSE}^{\star}$ algorithm. Let $L$ be an upper bound on the lengths
of the paths $\sigma_{n}$. We can conclude that if $K_{n}\leq\alpha M_{n}$,
then
$\|\sigma_{n}-\sigma^{\prime}_{n}\|_{\text{BV}}\leq(\sqrt{2}\alpha+\beta(1-\alpha))L$.
Hence,

$\\{K_{n}\leq\alpha
M_{n}\\}\subseteq\\{\|\sigma_{n}-\sigma^{\prime}_{n}\|_{\text{BV}}\leq\sqrt{2}L(\alpha+\beta)\\}$
(36)

This allows us to conclude that,

$\mathbb{P}(\\{K_{n}\geq\alpha
M_{n}\\})\geq\mathbb{P}(\\{\|\sigma_{n}-\sigma^{\prime}_{n}\|_{\text{BV}}\geq\sqrt{2}L(\alpha+\beta)\\})$
(37)

Thus, to prove the theorem, we need to show that
$\sum_{n=1}^{\infty}\mathbb{P}(\\{K_{n}\geq\alpha M_{n}\\})<\infty$. In order
to show this, we first poissonize the sampling process, to obtain a
homogeneous Poisson point process as in Def. 5 and Lemma 8. Subsequently we
de-poissonize the process to obtain the relevant bounds on
$\mathbb{P}(\\{K_{n}\geq\alpha M_{n}\\})$.

Consider $\lambda<1$, independent of $n$. Consider the random variable
$\text{Poisson}(\lambda n)$. Let the points sampled by $\text{GSE}^{\star}$
(independently and uniformly at random) be $\\{x_{1},x_{2},\dots,x_{n}\\}$. By
Lemma 8, we can conclude that the set $P_{\lambda
n}^{d}=\\{x_{1},x_{2},\dots,x_{{\text{Poisson}(\lambda n)}}\\}$ is a
homogeneous Poisson point process of intensity $\frac{\lambda
n}{\mu(\mathbb{X}_{\text{free}})}$. Let us denote by $\widehat{K}_{n}$ to be
the number of balls in $\beta\mathfrak{B}_{n,q_{n},l_{n}}={\beta B_{n,1},\beta
B_{n,2}\dots\beta B_{n,M_{n}}}$ that fail to have a point from the set
$P_{\lambda n}^{d}$ within radius $\beta q_{n}$ of their centres. Notice that
the underlying process is different from the process for the original variable
$K_{n}$. Relating the original process and the poissonized version (following
[30]),

$\displaystyle\mathbb{P}(K_{n}\geq\alpha
M_{n})\leq\mathbb{P}(\widehat{K}_{n}\geq\alpha
M_{n})+\mathbb{P}(\text{Poisson}(\lambda n)\geq n)$ (38)

We can directly offer the bound $\mathbb{P}(\\{\text{Poisson}(\lambda n)\geq
n\\})\leq e^{-cn}$ where $c>0$ is a constant, by the definition of a Poisson
random variable. In order to bound $\mathbb{P}(\widehat{K}_{n}\geq\alpha
M_{n})$, observe that for sufficiently small $\beta$, the sets $\beta B_{n,m}$
are disjoint. Thus, the events that each of these balls do not contain a point
from $P_{\lambda n}^{d}$ (which is a homogeneous Poisson point process) are
mutually independent. The probability $p_{n}$ that any given set does not
contain a ball is therefore given as follows

$\displaystyle p_{n}=e^{-\frac{\lambda n}{\mu(\mathbb{X}_{\text{free}})}}$
(39)

The variable $\widehat{K}_{n}$ can therefore be compared to a binomial random
variable (with parameters $M_{n}$ and $p_{n}$) as follows

$\displaystyle\mathbb{P}(\widehat{K}_{n}\geq\alpha
M_{n})\leq\mathbb{P}(\text{Binomial}(M_{n},p_{n})\geq\alpha M_{n})\leq
e^{-M_{n}p_{n}}$ (40)

Therefore from Eq. (40), we can conclude the following

$\sum_{n=1}^{\infty}\mathbb{P}(K_{n}\geq\alpha
M_{n})\leq\sum_{n=1}^{\infty}(e^{-cn}+e^{-M_{n}p_{n}})\leq\infty$ (41)

Notice that the constants $\alpha,\beta$ are arbitrary. Consequently we can
set $\epsilon=\sqrt{2}(\alpha+\beta)L)$, for arbitrary $\epsilon>0$ From the
inequality (37) and the equation above we know that

$\sum_{n=1}^{\infty}\mathbb{P}(\\{\|\sigma_{n}-\sigma^{\prime}_{n}\|_{\text{BV}}>\epsilon\\})<\infty$
(42)

By the Borel-Cantelli lemma and the equation above, we have
$\mathbb{P}(\\{\|\sigma_{n}-\sigma^{\prime}_{n}\|_{\text{BV}}=0)=1$.

Since $\underset{n\rightarrow\infty}{\lim}\sigma_{n}=\sigma^{\star}$ from
Lemma 4, by application of the triangle inequality,

$\displaystyle\mathbb{P}(\\{\underset{n\rightarrow\infty}{\lim}c(\sigma^{\prime}_{n})=c(\sigma^{\star})\\})=1$
(43)

Thus, the asymptotic optimality of the GSE⋆ is proved. ∎

######  Observation 2.

As a corollary of Theorem 2, we notice that
$\|\sigma^{\prime}_{n}-\sigma^{\star}\|_{\text{BV}}\leq\|\sigma^{\prime}_{n}-\sigma_{n}\|_{\text{BV}}+\|\sigma_{n}-\sigma^{\star}\|_{\text{BV}}$,
and hence the
$\mathbb{P}(\|\sigma^{\prime}_{n}-\sigma^{\star}\|_{\text{BV}}>\epsilon)$
decays with increasing $n$ for all $\epsilon>0$, exponentially.

##  V Simulation Results

In this section, we present numerical validation of the properties of the
algorithms GSE and GSE⋆ analyzed in the paper. Simulation studies have been
carried out in 2-D and 3-D environments having 4 and 16 obstacles representing
low to high obstacle densities using MATLAB R2020a on Intel Core i7 2.2GHz
processor. Simulations and comparison studies related to probabilistic
completeness of the GSE have already been provided in [26] and hence, are
omitted here for brevity. First, feasible shortest paths generated using the
GSE and GSE⋆ algorithms over 50 iterations in the 2-D and 3-D environments
each having both 4 and 16 obstacles are illustrated in Figs. 7(a), 7(b) and
Figs. 7(c), 7(d), respectively.

Throughout the simulations, the cost function is taken to be the Euclidean
path length. Since PRM⋆ is asymptotically optimal, the approximate optimal
cost ($c^{\star}$) is found using the PRM⋆ which is run over 8000 iterations.
Figs. 8(a) and 8(b) depict the convergence of costs to the optimal one for 4
and 16 obstacles in a 3-D workspace. For a given number of obstacles, four
different randomly generated workspaces are considered. For each workspace,
100 simulations, of 500 iterations in each simulation, have been run for each
algorithm. The cost obtained is averaged over each set of 100 simulations for
a given workspace. This average cost is again averaged over the four different
workspaces, thus providing the costs depicted in the convergence plots in
Figs. 8(a) and 8(b).

The numerical simulations are primarily aimed at demonstrating the performance
of the GSE⋆ algorithm in comparison with the original version of the GSE
algorithm in terms of the convergence of cost function to the optimal cost. It
is observed that the GSE⋆ converges to the optimal cost in around 150 and 300
iteration for 4 and 16 obstacles, respectively, whereas the GSE is found not
to converge even in 500 iterations to the optimal cost. This justifies the
claim of asymptotic non-optimality of the GSE in Section III-C and the
asymptotic optimality of the GSE⋆ algorithm in Section IV-C.

##  VI Conclusion

The recently-proposed Generalized Shape Expansion (GSE) algorithm was shown to
be probabilistically complete. In this paper, we have extended the study on
the GSE to show that it is not asymptotically optimal by observing its non-
zero probability of generating low-quality solutions to the promenade problem.
In order to improve in the context of asymptotic optimality, a modified GSE
algorithm, namely the $\text{GSE}^{\star}$ algorithm, has been presented. A
detailed mathematical analysis has been given to prove both probabilistic
completeness and asymptotic optimality of the GSE⋆. Further, simulation
studies comparing the performance of these algorithms in various environments
have shown marked improvement in asymptotic convergence to the optimal cost by
the GSE⋆ over the GSE, which justifies the presented theoretical results. The
key feature of the generalized shape expansion-based algorithms is the use of
the generalized shape to generate collision-free paths. Leveraging these
shapes have been found to greatly reduce the number of iterations required to
obtain a feasible path attaining a minimally connected graph. While
maintaining the notion of the GSE⋆ to gain computational advantages could be a
potential future scope of research.

## References

  * [1] O. Khatib, “Real-time obstacle avoidance for manipulators and mobile robots,” _IEEE International Conference on Robotics and Automation_ , pp. 500–505, 1985.
  * [2] Y. Koren and J. Borenstein, “Potential field methods and their inherent limitations for mobile robot navigation,” _International Conference on Robotics and Automation_ , pp. 1398–1404, 1991.
  * [3] P. Fiorini and Z. Shiller, “Motion planning in dynamic environments using velocity obstacles,” _The International Journal of Robotics Research_ , vol. 17, no. 7, pp. 760–772, 1998.
  * [4] J. Van Den Berg, S. J. Guy, M. Lin, and D. Manocha, “Reciprocal n-body collision avoidance,” in _Robotics research_. Springer, 2011, pp. 3–19.
  * [5] A. Chakravarthy and D. Ghose, “Collision cones for quadric surfaces,” _IEEE Transactions on Robotics_ , vol. 27, no. 6, pp. 1159–1166, 2011.
  * [6] A$.\;$Chakravarthy and D$.\;$Ghose, “Generalization of the collision cone approach for motion safety in 3-d environments,” _Autonomous Robots_ , vol. 32, no. 3, pp. 243–266, 2012.
  * [7] J. Wilhelm, G. Clem, and D. Casbeer, “Circumnavigation and obstacle avoidance guidance for uavs using gradient vector fields,” in _AIAA Scitech 2019 Forum_ , 2019, p. 1791.
  * [8] Q. Gong, R. Lewis, and M. Ross, “Pseudospectral motion planning for autonomous vehicles,” _Journal of Guidance, Control, and Dynamics_ , vol. 32, no. 3, pp. 1039–1045, 2009.
  * [9] S. Ghosh, D. Davis, and T. Chung, “A guidance law for avoiding specific approach angles against maneuvering targets,” _IEEE Conference on Decision and Control, Las Vegas, USA_ , pp. 4142–4147, 2018.
  * [10] S. Ghosh, , O. A. Yakimenko, D. T. Davis, and T. H. Chung, “Unmanned aerial vehicle guidance for an all-aspect approach to a stationary point,” _Journal of Guidance, Control, and Dynamics_ , vol. 40, no. 11, pp. 2871–2888, 2017.
  * [11] R. Brooks and T. Lozano-Perez, “A subdivision algorithm in configuration space for findpath with rotation,” _IEEE Transactions on Systems, Man, and Cybernetics_ , vol. SMC-15, pp. 224–233, 1985.
  * [12] J. S. B. Mitchell, G. Rote, and G. Woeginger, “Minimum-link paths among obstacles in the plane,” _Algorithmica_ , vol. 8, no. 1, pp. 431–459, Dec 1992.
  * [13] L. Kavraki, P. Svestka, J. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high- dimensional configuration spaces,” _Transactions on Robotics and Automation_ , vol. 12, no. 4, pp. 566–580, 1996\.
  * [14] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” _The international journal of robotics research_ , vol. 30, no. 7, pp. 846–894, 2011.
  * [15] Y. Chen, M. Cutler, and J. P. How, “Decoupled multiagent path planning via incremental sequential convex programming,” in _IEEE International Conference on Robotics and Automation_. IEEE, 2015, pp. 5954–5961.
  * [16] F. Augugliaro, A. P. Schoellig, and R. D’Andrea, “Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach,” in _International Conference on Intelligent Robots and Systems_. IEEE, 2012, pp. 1917–1922.
  * [17] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor, and V. Kumar, “Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments,” _IEEE Robotics and Automation Letters_ , vol. 2, no. 3, pp. 1688–1695, 2017.
  * [18] S. M. LaValle and J. J. Kuffner Jr, “Randomized kinodynamic planning,” _The international journal of robotics research_ , vol. 20, no. 5, pp. 378–400, 2001.
  * [19] L. Janson, E. Schmerling, A. Clark, and M. Pavone, “Fast marching tree: a fast marching sampling-based method for optimal motion planning in many dimensions,” 2015.
  * [20] F. Baldini, S. Bandyopadhyay, R. Foust, S.-J. Chung, A. Rahmani, J.-P. de la Croix, A. Bacula, C. M. Chilan, and F. Hadaegh, “Fast motion planning for agile space systems with multiple obstacles,” in _AIAA/AAS astrodynamics specialist conference_ , 2016, p. 5683.
  * [21] V. Zinage and S. Ghosh, “An efficient motion planning algorithm for uavs in obstacle-cluttered environment,” _American Control Conference, IEEE, Philadelphia, PA_ , pp. 2271–2276, July 2019.
  * [22] V. V. Zinage and S. Ghosh, “Generalized shape expansion-based motion planning in three-dimensional obstacle-cluttered environment,” _Journal of Guidance, Control, and Dynamics_ , pp. 1–11, 2020.
  * [23] V. Zinage and S. Ghosh, “Generalized shape expansion-based motion planning for uavs in three dimensional obstacle-cluttered environment,” in _AIAA Scitech 2020 Forum_ , 2020, p. 0860.
  * [24] V$.\;$Zinage and S$.\;$Ghosh, “Directional sampling-based generalized shape expansion for accelerated motion planning in 2-d obstacle-cluttered environments,” _IEEE Control Systems Letters_ , vol. 5, no. 3, pp. 1067–1072, 2020.
  * [25] L. Kavraki, M. Kolountzakis, and J. Latombe, “Analysis of probabilistic roadmaps for path planning,” _IEEE Transactions on Robotics and Automation_ , vol. 14, no. 1, pp. 166–171, 1998.
  * [26] A. Ramkumar, V. Zinage, and S. Ghosh, “On probabilistic completeness of the generalized shape expansion-based motion planning algorithm,” in _accepted at Conference on Decision and Control_. IEEE, 2020.
  * [27] O. Nechushtan, B. Raveh, and D. Halperin, “Sampling-diagram automata: A tool for analyzing path quality in tree planners,” vol. 68, 01 2010, pp. 285–301.
  * [28] O$.\;$Nechushtan, B$.\;$Raveh, and D$.\;$Halperin, “Sampling-diagram automata: A tool for analyzing path quality in tree planners,” in _Algorithmic Foundations of Robotics IX_. Springer, 2010, pp. 285–301.
  * [29] S. Chiu, D. Stoyan, W. Kendall, and J. Mecke, _Stochastic Geometry and Its Applications_ , ser. Wiley Series in Probability and Statistics. Wiley, 2013.
  * [30] M. Penrose, _Random Geometric Graphs_ , ser. Oxford studies in probability. Oxford University Press, 2003\.