#  iPolicy: Incremental Policy Algorithms for Feedback Motion Planning

Guoxiang Zhao1,∗ Devesh K. Jha2,∗ Yebin Wang2 Minghui Zhu3
∗ Equal contributions.1 Guoxiang Zhao is with School of Future Technology and
Institute of Artificial Intelligence, Shanghai University, Shanghai, 200444,
China. (email: gxzhao@shu.edu.cn)2 Devesh K. Jha and Yebin Wang are with
Mitsubishi Electric Research Laboratories, Cambridge, MA 02139, USA. (email:
<EMAIL_ADDRESS>yebinwang@ieee.org)3 Minghui Zhu is with School of Electrical
Engineering and Computer Science, Pennsylvania State University, University
Park, PA 16802, USA. This work by M. Zhu was partially supported by the grant
U.S. NSF CNS1830390. (email<EMAIL_ADDRESS>
###### Abstract

This paper presents policy-based motion planning for robotic systems. The
motion planning literature has been mostly focused on open-loop trajectory
planning which is followed by tracking online. In contrast, we solve the
problem of path planning and controller synthesis simultaneously by solving
the related feedback control problem. We present a novel incremental policy
(iPolicy) algorithm for motion planning, which integrates sampling-based
methods and set-valued optimal control methods to compute feedback controllers
for the robotic system. In particular, we use sampling to incrementally
construct the state space of the system. Asynchronous value iterations are
performed on the sampled state space to synthesize the incremental policy
feedback controller. We show the convergence of the estimates to the optimal
value function in continuous state space. Numerical results with various
different dynamical systems (including nonholonomic systems) verify the
optimality and effectiveness of iPolicy.

##  I Introduction

Informally speaking, given a robot with a description of its dynamics, a
description of its environment and a set of goal states, the motion planning
problem is to find a sequence of control inputs so as to guide the robot from
the initial state to one of the goal states while avoiding collision in the
cluttered environment. It is well-known that robotic motion planning is at
least as difficult as the generalized piano mover’s problem, which has been
proven to be PSPACE-hard [1]. Many planning algorithms have been proposed. The
discrete approaches, such as Dijkstra’s Algorithm and A* [2], usually depend
on the structured environment and apply graph search to find the shortest
path. The optimality of their returned trajectories are formally guaranteed
for discrete problems but their heavy dependency on structured environment
makes them suffer from inaccuracies and infeasibility of the solution when
dealing with continuous systems [3]. Recently, sampling-based geometric
planning algorithms such as the rapidly-exploring random trees (RRT) [4] and
its optimal variant RRT* [5] are arguably the most influential and widely-used
motion planning algorithms since the last two decades. They are shown to
compute quickly in high-dimensional continuous space and possess theoretical
guarantees such as probabilistic completeness and optimality.

Robots, in most of the practical applications, have stringent differential
constraints on their motion which need to be properly incorporated during
motion planning. Motion planning for dynamical systems has a rich history and
much work has been done on this topic [6, 7, 2, 8]. However, the problem of
motion planning for nonholonomic systems is still open in many aspects [9].
Sampling-based algorithms have received a lot of attention for their
efficiency in handling obstacle-cluttered environments. There are two main
bodies of research to make these methods more suitable for dealing with
differential constraints: one develops steering functions for dynamical
systems using concepts from feedback control theory (see e.g., [10, 11]); the
other direction of work improves the computational effectiveness of the
algorithms using geometric planning (see e.g., [9, 12, 13]). Despite the
tremendous body of work on this topic, most of the work focuses on planning
open-loop trajectories.

In practical implementation, the planned open-loop trajectories are tracked by
a feedback controller. [14]. The presence of differential, state and input
constraints, however, makes the design of the feedback tracking controller for
highly nonlinear and complex robots still a difficult problem. Furthermore,
the performance guarantees of the closed-loop trajectories could be lost in
the presence of these constraints, especially in terms of minimizing the
aggregated cost. It necessitates feedback motion planning which explicitly
takes into account the feedback tracking during the planning process. The
feedback planner computes the mapping from state space to control space
subject to constraints and equivalently searches for the solution for every
initial condition of the robot; this facilitates the completeness and
performance guarantee of the planner. However, the feedback motion planning
problem is challenging in various aspects and most of the computational issues
involving feedback planning are still unexplored and open. Interested readers
are referred to [3] for a comprehensive survey of the challenges in feedback
motion planning. Most of the previous approaches for feedback motion planning
struggle with computational tractability in continuous state-space or local
minima issues [2]. In this paper, we present a method for feedback motion
planning of dynamical systems which makes use of sampling-based methods and
value iterations to recover the optimal feedback controller. As a synthesis of
the computational advantages of sampling-based algorithms and the
approximation consistency of set-valued algorithms [15], the proposed iPolicy
algorithm is an incremental and anytime feedback motion planner with formal
guarantee of asymptotic optimality.

Contributions. This paper presents a sampling-based algorithm for feedback
motion planning for a class of nonlinear robotic systems. In this work, we
leverage the existing set-valued analysis tool for motion planning and
asynchronous value iterations to synthesize feedback motion planners in an
incremental fashion. In particular, the continual sampling creates an
approximation of the original minimal time problem in every iteration and the
policy on the discretized space provides an incrementally refined estimate of
the value function. Using value iterations in an asynchronous and incremental
manner limits the computational requirements while retaining optimality
conditions. We show the convergence of the estimated value functions to the
optimal value function for the motion planning problem in the continuous state
space. For clarification of presentation, the various salient features of the
algorithms are demonstrated with simulations using point mass system. Some
numerical simulations are then provided for feedback motion planning of simple
car and Dubins car (these are well studied nonholonomic systems [2, 8]) using
the proposed motion planning algorithm. Several simulations are provided to
show the value functions calculated by the proposed algorithm and trajectories
under the guidance of the computed controller in cluttered environments.
Through simulation study of various different dynamical systems, we show the
applicability of the proposed algorithms across a wide range of robotic
systems (two different classic nonholonomic systems).

Organization. This paper is organized in eight sections including the current
one. We present work related to our proposed problem in Section II. Commonly
sued notations and notions are clarified in Section III. In Section IV, we
present a formal statement of the problem solved in this paper. The main
algorithms of the paper are presented in Section V followed by analysis of the
same in Section VI. Numerical results for the proposed algorithm are presented
in Section VII with related interpretation and discussion. The paper is
finally concluded with a summary and future work in Section VIII.

##  II Related Work

Motion planning is central to robotics. Consequently, it has received a lot of
attention in robotics and controls community. Broadly speaking, there are
mostly three kinds of approaches for motion planning – optimization-based
methods, gradient-based methods and sampling-based methods.

Some of the most popular optimization-based approaches could be found in [16,
17, 18, 19, 20]. The main idea of these approaches is to formulate a dynamic
optimization problem with smooth formulation of constraints (like collision,
etc.). The resulting optimization problem can then be solved to generate an
optimal trajectory. However, the main shortcoming of these optimization-based
techniques is that they struggle to find solution in obstacle cluttered, non-
convex spaces. Trajectory optimization techniques using optimal control
literature can generate control trajectories in presence of state and input
constraints for nonlinear dynamical systems – however, they may not be able to
find feasible trajectories in the presence of arbitrary obstacles [21, 22].
These methods are mostly used to compute trajectories under dynamical
constraints which can be followed online using a trajectory tracking
controller using desirable constraints for execution [23, 24, 25].

Gradient-based approaches, such as potential fields [26, 27] and navigation
functions [28], consider the composition of a repulsive component for
collision avoidance and an attractive component for reaching goal, and derive
the control law based on the gradient of the synthesized field. The
computations of both fields usually only depend on local information and this
brings computational efficiency to gradient-based approaches; however, the
cancellation of different components in the composition can also trap the
robot at local minima [2]. Thus, these approaches tend to struggle when a
robot has to operate in obstacle cluttered environment.

Sampling-based approaches are shown to be able to successfully address
irregularly shaped environments and arguably the most widely used methods for
motion planning in robotics [2, 29]. However, most of these methods are used
to compute open-loop plans for the robot to follow. It is, in general,
desirable that robots operate in a feedback fashion using state estimates
during execution of a task. Consequently, there has been some work on using
sampling-based algorithms to develop feedback planners by using different
metrics to select the best path. Some methods to solve for feedback planners
are presented in [30, 31, 10, 32, 33]. However, very little work has been done
on using these algorithms to incrementally build the state-space of systems to
solve for continuous time and space optimal control problems for the dynamical
systems. So, even though the motion planning problem has been thoroughly
studied in literature, there seems to be lack of techniques and algorithms
which can compute feedback motion planners for dynamical systems. We make an
attempt to address this problem in this paper.

Our approach is closely related to the value iterations-based approach in
[34]; however, we use asynchronous value iterations, provide results for
nonholonomic systems and also provide rigorous numerical simulation results.
Our results on optimal performance do not require the explicit description of
steering functions between any two states of the system in the collision-free
space. As such, we expect our results to cater to a rich set of robotic motion
planning problems. Our approach is also related to [35] but differs in the way
to solve the approximate motion planning problem, where [35] spends heavy
computations searching for the optimal solution over the entire graph before
refinement while our approach swiftly solves for a small subset of the graph
and proceeds to finer graphs. This brings the continuously increasing
optimality to our approach and it incrementally improves solutions once more
computational resources are given. The problem and algorithms presented in
this paper are different from some other sampling-based feedback planning like
[36], and the proposed algorithms have the advantage of being incremental
(instead of batch). Furthermore, optimal performance guarantees for the
feedback control problem have been provided.

##  III Notations and notions

Let $\|\cdot\|$ be the $2$-norm in n. Define the distance from a point
$x\in^{n}$ to a set $\mathcal{X}\subseteq^{n}$ by
$\operatorname{dist}(x,\mathcal{X})\triangleq\inf_{x^{\prime}\in\mathcal{X}}\|x-x^{\prime}\|$.
Given a compact set $\mathcal{X}\subseteq^{n}$ and a function
$v:\mathcal{X}\to$, denote the supremum norm over $\mathcal{X}$ by
$\|v\|_{\mathcal{X}}\triangleq\sup_{x\in\mathcal{X}}|v(x)|$. Let the Lebesgue
measure of the set $\mathcal{X}$ be $\mu(\mathcal{X})$. Denote the unit ball
in n by $\mathcal{B}_{n}$ and the volume of the unit ball by
$C_{n}\triangleq\frac{\pi^{n/2}}{(n/2)!}$. When no ambiguity is caused, the
dimensionality in the subscript may be omitted. Define the Minkowski sum in n
for $\mathcal{X}\subseteq^{n}$ and $\mathcal{Y}\subseteq^{n}$ by
$\mathcal{X}+\mathcal{Y}\triangleq\\{x+y|x\in\mathcal{X},y\in\mathcal{Y}\\}$.
When $\mathcal{X}=\\{x\\}$ is a singleton, with slight abuse of notations, the
Minkowski sum can be written as
$x+\mathcal{Y}\triangleq\\{x+y|y\in\mathcal{Y}\\}$. Define the value
assignment operator by $a\leftarrow b$ where the value of $b$ is assigned to
$a$.

##  IV Problem Formulation

Consider a robot associated with a time-invariant dynamic system governed by
the following differential equation:

$\dot{x}(t)=f(x(t),u(t)),$ (1)

where $x(t)\in\mathcal{X}\subseteq^{n}$ is the state and
$u(t)\in\mathcal{U}\subseteq^{m}$ is the control of robot. The following mild
assumptions are imposed throughout the paper and they are inherited from [15].

######  Assumption IV.1.

The following assumptions hold for (1):

  1. (A1)

$\mathcal{X}$ and $\mathcal{U}$ are non-empty and compact;

  2. (A2)

$f$ is continuous in $u$ and Lipschitz continuous in $x$ for any
$u\in\mathcal{U}$ with a Lipschitz constant $l$;

  3. (A3)

$f$ is linear growth; i.e., $\exists c\geq 0$ s.t. $\forall
x\in\mathcal{X},u\in\mathcal{U},\|f(x,u)\|\leq c(\|x\|+\|u\|+1)$;

  4. (A4)

$\bigcup_{u\in\mathcal{U}}f(x,u)$ is a convex set for any $x\in\mathcal{X}$.

Assumptions (A1) and (A2) imply $\|f(x,u)\|$ is bounded. Denote the upper
bound of $\|f(x,u)\|$ by
$M\triangleq\max_{x\in\mathcal{X},u\in\mathcal{U}}\|f(x,u)\|$.

Let $\mathcal{X}_{\mathrm{obs}}$ and $\mathcal{X}_{\mathrm{goal}}$ be the
obstacle region and the goal region, respectively. Define the obstacle free
region as
$\mathcal{X}_{\mathrm{free}}\triangleq\mathcal{X}\setminus\mathcal{X}_{\mathrm{obs}}$.
Denote the trajectory of system (1) as $\phi(\cdot;x,\pi):\to\mathcal{X}$
given the initial state $x$ and a state feedback controller
$\pi:\mathcal{X}\to\mathcal{U}$. Let $\mathds{T}^{\pi}(x)$ be the first time
when the trajectory $\phi(\cdot;x,\pi)$ hits $\mathcal{X}_{\mathrm{goal}}$
while staying in $\mathcal{X}_{\mathrm{free}}$ before $\mathds{T}^{\pi}(x)$;
i.e.,

$\begin{split}\mathds{T}^{\pi}(x)&\triangleq\inf\\{t\geq
0|\phi(t;x,\pi)\in\mathcal{X}_{\mathrm{goal}},\\\
&\phi(\tau;x,\pi)\in\mathcal{X}_{\mathrm{free}},\forall\tau\in[0,t]\\}.\end{split}$

The problem of interest in this paper is to find the control policy
$\pi^{*}:\mathcal{X}\to\mathcal{U}$ for system (1) that incurs the minimal
traveling time $\mathds{T}^{*}$ for every $x\in\mathcal{X}$; that is,

$\displaystyle\mathds{T}^{*}(x)=\min_{\pi}\mathds{T}^{\pi}(x),\forall
x\in\mathcal{X}$

and the minimizer $\pi^{*}$ is referred to as the optimal policy.

Notice that the minimal traveling time function $\mathds{T}^{*}$ is not
necessarily bounded, as $\mathcal{X}_{\mathrm{goal}}$ may not be reachable for
every $x\in\mathcal{X}_{\mathrm{free}}$, and $\mathds{T}^{*}(x)$ would be
positive infinity for such states. The positive infinity can cause
difficulties in both computational expressions and mathematical analysis, and
we address this issue by leveraging the Kruzhkov transform $\Psi:_{\geq
0}\bigcup\\{+\infty\\}\to[0,1]$, defined as

$\displaystyle(\Psi\circ\mathds{T})(x)\triangleq 1-\exp(-\mathds{T}(x)),$

and we let the transformed value function be denoted by
$\varTheta\triangleq\Psi\circ\mathds{T}$ inheriting all subscripts and
superscripts of $\mathds{T}$. Notice that the Kruzhkov transform is bijective
and monotonically increasing. The positive infity is transformed to $1$ after
the Kruzhkov transform while $0$ remains the same. The objective of this paper
is equivalently to find the transformed minimal traveling time function
$\varTheta^{*}$ and the optimal policy $\pi^{*}$.

##  V The Incremental Policy (iPolicy) Algorithm

In this section, we present the incremental policy algorithm iPolicy. We
leverage sampling-based methods in [5, 12, 37] and set-valued methods in [15,
35] along with asynchronous value iterations to incrementally approximate the
minimal traveling time function. In particular, iPolicy consists of two
components: graph expansion and minimal traveling time estimation. In graph
expansion, iPolicy continually samples the free region
$\mathcal{X}_{\mathrm{free}}$ and constructs a graph using the set-valued
method to approximate the continuous time dynamic of system (1); in minimal
traveling time estimation, asynchronous value iterations are executed over the
approximate graph in a back propogation fashion, where only stale values are
updated to save computations, to obtain an estimate of the minimal traveling
time function. As more samples are added, iPolicy iteratively expands graph
and estimates the minimal traveling time function until a certain limit (e.g.,
time limit, approximation error threshold) is reached. Denote the set of
sampled states from $\mathcal{X}_{\mathrm{free}}$ after $k$ iterations of
iPolicy by $V_{k}$. With slight abuse of notations, denote the estimate of
minimal traveling time, also known as the value function, after $k$ iterations
by $\mathds{T}_{k}:V_{k}\to[0,+\infty]$. The staleness function
$\mathcal{F}_{k}:V_{k}\to\mathbb{N}_{\geq 0}$ is defined as the number of
iterations since the last update of a sample $x\in V_{k}$. We proceed to
explain iPolicy in the rest of this section.

1 Initialization

2 for  _$x\in V_{0}$_ do

3 $\varTheta_{0}(x)\leftarrow 0$;

4 $\mathcal{F}_{0}(x)\leftarrow P$;

5

6 $k\leftarrow 1$;

7 Main Loop

8 while  _$k <K$_ do

9
$x_{\mathrm{new}}\leftarrow\texttt{Sample}(\mathcal{X}_{\mathrm{free}}+d_{k}\mathcal{B})$;

10 $V_{k}\leftarrow V_{k-1}\cup\\{x_{\mathrm{new}}\\}$;

11 if
_$x_{\mathrm{new}}\in\mathcal{X}_{\mathrm{goal}}+(M\epsilon_{k}+d_{k})\mathcal{B}$_
then

12 $\hat{\varTheta}_{k}(x_{\mathrm{new}})\leftarrow 0$;

13

14 else

15 $\hat{\varTheta}_{k}(x_{\mathrm{new}})\leftarrow 1$;

16

17 $\hat{\mathcal{F}}_{k}(x_{\mathrm{new}})\leftarrow P$;

18 for  _$x\in V_{k-1}$_ do

19 $\hat{\varTheta}_{k}(x)\leftarrow\varTheta_{k-1}(x)$;

20 $\hat{\mathcal{F}}_{k}(x)\leftarrow\mathcal{F}_{k-1}(x)$;

21 Compute $F_{k}(x)$ in (3);

22

23
$(\varTheta_{k},\mathcal{F}_{k})\leftarrow{\texttt{ValueIteration}}{(V_{k},\hat{\varTheta}_{k},\hat{\mathcal{F}}_{k},m_{k})}$;

24 $k\leftarrow{k+1}$;

25return _$V_{k}$_ ;

Algorithm 1 Incremental Policy (iPolicy) Algorithm

1 Input: Samples $V$, transformed value function $\varTheta$, staleness
function $\mathcal{F}$, recursion allowance $m$;

2 def  _ValueIteration($V,\varTheta,\mathcal{F},m$)_:

3 $\Delta\leftarrow\Psi(\epsilon-d),\beta=1-\Delta$;

4 $S\leftarrow\\{x\in
V|\mathcal{F}(x)=P,x\in\mathcal{X}_{\mathrm{free}}+d\mathcal{B}\\}$;

5 for  _$x\in S$_ do

6 $\varTheta(x)\leftarrow\texttt{BackProp}(x,m,\varTheta)$;

7 $\mathcal{F}(x)\leftarrow 0$;

8

9 for  _$x\in V\setminus S$_ do

10 $\mathcal{F}(x)\leftarrow\mathcal{F}(x)+1$;

11

12 return _$(\varTheta,\mathcal{F})$_

Algorithm 2 The ValueIteration procedure

1 Input: State $x\in V$, recursion allowance $m$, transformed value function
$\varTheta^{m}$;

2 def  _BackProp($x$, $m$, $\varTheta^{m}$)_:

3 if  _$x\in\mathcal{X}_{\mathrm{goal}}+(M\epsilon+d)\mathcal{B}$ or $m=0$_
then

4 return $\varTheta^{m}(x)$;

5

6 for  _$x^{\prime}\in F(x)$_ do

7
$\varTheta^{m-1}(x)\leftarrow\texttt{BackProp}(x^{\prime},m-1,\varTheta^{m})$;

8

9 $\displaystyle\varTheta^{m}(x)\leftarrow\Delta+\beta\min_{x^{\prime}\in
F(x)}\varTheta^{m-1}(x^{\prime})$;

10 return $\varTheta^{m}(x)$

Algorithm 3 The BackProp procedure

###  V-A Algorithm statement

We initialize iPolicy with a set of states $V_{0}$, which we assume it
intersects with the goal set $\mathcal{X}_{\mathrm{goal}}$ to ensure the
availability of boundary condition in value iteration execution. Each sampled
state $x\in V_{0}$ is initialized with its value set by $\varTheta_{0}(x)=0$
and the its staleness by a staleness threshold $P\geq 0$, meaning their values
shall be updated at the beginning of the main loop of iPolicy. See lines 1-1
in Algorithm 1.

Then iPolicy enters the main loop in line 1. Similar to exploration, the main
loop samples $\mathcal{X}_{\mathrm{new}}$ and add newly sampled state
$x_{\mathrm{new}}$ to $V_{k}$ as line 1. Then the value and staleness of
$x_{\mathrm{new}}$ are initialized in the same way as those in the
initialization stage, while for sampled states from the last grid $V_{k-1}$,
we retain their staleness and values as $\mathcal{F}_{k-1}$ and
$\varTheta_{k-1}$. Since the new sample may incur paths with lower cost, value
iterations shall be executed on $V_{k}$ as line 1 in Algorithm 1 and Algorithm
2 to obtain a better estimate of the minimal traveling time $\mathds{T}_{k}$.
The conventional value iteration is executed in a synchronous fashion, where
the value at every sampled state on $V_{k}$ is updated in every iteration, and
its computations grow quickly as more and more samples are added to $V_{k}$.
We employ the asynchronous value iteration to mitigate the computational
complexity. Particularly, the value at $x$ is updated when two circumstances
occur: first, a sampled state is newly added to $V_{k}$, since its initial
value may not be accurate; second, the value of a sampled state is too stale
to reflect the true estimate of the minimal traveling time. Recall the
staleness function $\mathcal{F}_{k}:V_{k}\to\mathbb{N}_{\geq 0}$ represents
the number of iterations since the last update of a sample $x\in V_{k}$. Let
$\hat{\mathcal{F}}_{k}$ be the staleness function before the update at
iteration $k$. Then $\hat{\mathcal{F}}_{k}(x)=\mathcal{F}_{k-1}(x),\forall
x\in V_{k-1}$ and for $x\in V_{k}\setminus V_{k-1}$, we have
$\hat{\mathcal{F}}_{k}(x)=P$. The staleness threshold $P\in\mathbb{N}_{\geq
0}$ is the maximum number of value iterations a sampled state can skip. When
$P=0$, the asynchronous update is identical to the synchronous update, while
when $P$ is positive infinity, each sample will only be updated once. In
Algorithm 2 ValueItertion, the set of stale samples $S_{k}\triangleq\\{x\in
V_{k}|\hat{\mathcal{F}}_{k}(x)=P,x\in\mathcal{X}_{\mathrm{free}}+d_{k}\mathcal{B}\\}$
are updated using Algorithm 3 BackProp that recursively searches for the
minimum values from one’s neighbors. After updates, the staleness for the
samples in $S_{k}$ will be cleared as line 2 in Algorithm 2 as they have been
freshly updated while staleness of other samples $V_{k}\setminus S_{k}$ would
be increased by $1$ as line 2.

Algorithm 3 BackProp leverages the Bellman equation to recursively update
values and partially solve for the estimate of minimal traveling time on
$V_{k}$. For simplicity, we drop the dependency on $k$ in the psuedo codes of
BackProp. The vanilla update procedure of value iteration is summarized as the
Bellman operator $\mathcal{T}_{k}$:

$(\mathcal{T}_{k}\circ\mathds{T}_{k})(x)\triangleq\begin{cases}&\displaystyle\epsilon_{k}-d_{k}+\min_{x^{\prime}\in
F_{k}(x)}\mathds{T}_{k}(x^{\prime}),\\\ &\quad\text{if }x\in
V_{k}\setminus(\mathcal{X}_{\mathrm{goal}}+(M\epsilon_{k}+d_{k})\mathcal{B});\\\
&\mathds{T}_{k}(x),\text{otherwise},\end{cases}$ (2)

where

$\displaystyle F_{k}(x)\triangleq$
$\displaystyle(x+\epsilon_{k}\bigcup_{u\in\mathcal{U}}f(x,u)+\rho(d_{k},\epsilon_{k})\mathcal{B})$
(3) $\displaystyle\cap(\mathcal{X}_{\mathrm{free}}+d_{k}\mathcal{B})\cap
V_{k}$

is the set of one-hop neighbors of $x$ using Euler discretiztion and
$\rho(d,k)\triangleq 2d_{k}+l\epsilon_{k}(d_{k}+M\epsilon_{k})$ is the
perturbation radius inherited from [15]. See Figure 1 for the visualization of
the discretization, where the robot transits from $x$ to $x^{\prime}$ after
applying a constant control $u$ for a fixed time $\epsilon_{k}$, and we treat
all sampled states (orange dots) in the circle of radius $\rho$ centered at
$x^{\prime}$ as the one-hop neighbors of $x$, as $x^{\prime}$ is not
necessarily in $V_{k}$. With the Kruzhkov transform, one may rewrite the
update procedure (2) and define a tranformed Bellman operator as:

$\displaystyle(\mathfrak{T}_{k}\circ\varTheta_{k})(x)\triangleq\begin{cases}&\displaystyle\Delta_{k}+\beta_{k}\min_{x^{\prime}\in
F_{k}(x)}\varTheta_{k}(x^{\prime}),\\\ &\quad\text{if }x\in
V_{k}\setminus(\mathcal{X}_{\mathrm{goal}}+(M\epsilon_{k}+d_{k})\mathcal{B});\\\
&\varTheta_{k}(x),\text{ otherwise},\end{cases}$

where $\Delta_{k}\triangleq\Psi(\epsilon_{k}-d_{k})$ is the transformed
running cost, $\beta_{k}\triangleq 1-\Delta_{k}$ is the total discount factor
in the transformed form and $\mathfrak{T}_{k}$ is the transformed Bellman
operator. For each $x\in S$, BackProp computes $\varTheta^{m_{k}}(x)$ in a
recursive manner. See Figure 1 again for the visualization of BackProp.
Algorithm 3 BackProp first checks whether the goal region
$\mathcal{X}_{\mathrm{goal}}$ is reached or the recursion allowance $m_{k}=0$
runs out as line 3, which are the boundary condition of Bellman equation and
the limit of computations for each iteration, respectively. If neither is
satisfied, BackProp will check and compute $\varTheta^{m_{k}-1}(x^{\prime})$
for each neighbor $x^{\prime}\in F_{k}(x)$ by calling itself as line 3. This
will temporarily halt the exeuction of current BackProp (recursion allowance
$m_{k}$) with all values saved, while processor will create a duplicate of
BackProp with a reduced recursion allowance $m_{k}-1$ and continues executing
the new BackProp When the new BackProp finishes according to the boundary
conditions in line 3, the processor will exit it and resume the execution of
the old BackProp with recursion allowance $m_{k}$ with its previously saved
values and the returned value at a one-hop neighbor
$\varTheta^{m_{k}-1}(x^{\prime})$ from the new BackProp. With all values at
one-hop neighbors being ready, BackProp computes $\varTheta^{m}(x)$ using the
Bellman operator as line 3. In other words, a recursion with maximum allowance
$m_{k}$ starting from $x$ updates the value at $x$ by propogating values at
neighbors of $x$ within $m_{k}$ hops. This limits the search to a subset of
$V_{k}$ to save computations. When $m_{k}\geq|V_{k}|$, BackProp updates all
sampled states on $V_{k}$ in the depth-first search manner.

Figure 1: Illustration of the approximation of dynamic and BackProp. Orange
dots are sampled states, orange crosses are resting states when applying
constant control $u$ for time $\epsilon$ and blue arrows imply discrete time
transition.

The consistency of iPolicy follows the discretization scheme in [15], where
convergence rate conditions are imposed for the temporal and spatial
resolutions. Specifically, let $d_{k}$ be a conservative estimate of the
spatial dispersion, $\epsilon_{k}$ be the associated temporal resolution and
$\rho(\epsilon_{k},d_{k})$ be the perturbation radius. Then $d_{k}$ is a
distance within which every point $x\in\mathcal{X}$ can find a sampled state
in $V_{k}$ and $\epsilon_{k}$ would be the minimal time elapse system (1) can
transit. Since system (1) may not fall on a sample in $V_{k}$ if it takes a
constant control for $\epsilon_{k}$ time units, the discrepancy of
approximation of the dynamic system is accommodated by the perturbation
$\rho(d_{k},\epsilon_{k})$ so that the one-hop neighbors of state $x$ with
pertubation would surely fall on $V_{k}$; i.e.,
$(x+\epsilon_{k}f(x,u)+\rho(d_{k},\epsilon_{k})\mathcal{B})\cap
V_{k}\neq\emptyset$. The choice of $d_{k}$ and $\epsilon_{k}$ is not unique
but is subject to the following assumption for the purpose of consistent
approximation.

######  Assumption V.1.

The selection of $d_{k}$ and $\epsilon_{k}$ satsifies the following relations:

  1. (A5)

$d_{k}\geq B\Big{(}\frac{\log|V_{k}|}{|V_{k}|}\Big{)}^{1/n},\forall
k\in\mathbb{N}_{\geq 0}$, where $B>(\mu(\mathcal{X})/C_{n})^{1/n}$;

  2. (A6)

$\epsilon_{k}$ and $d_{k}/\epsilon_{k}$ monotonically decreases to $0$ as
$k\to+\infty$;

  3. (A7)

$\epsilon_{k}>d_{k},\forall k\in\mathbb{N}_{\geq 0}$;

  4. (A8)

$\rho(d_{k},\epsilon_{k})\geq d_{k-1},\forall k\in\mathbb{N}$.

Assumption (A5) follows from [5] and implies the lower bound of the spatial
resolution that the union of balls centered at every sample $x\in V_{k}$ with
radius $d_{k}$ covers the whole region $\mathcal{X}$. Assumption (A6)
characterizes the convergence rate for temporal and spatial resolutions.
Notice that the perturbation radius $\rho(d_{k},\epsilon_{k})$ is dimishing
faster than $\epsilon_{k}$; this ensures the discrete time transition always
falls on the graph for sufficiently small temporal-spatial resolutions. In
Assumption (A6), the faster convergence of $d_{k}$ compared to $\epsilon_{k}$
indicates the discrete time transition on $V_{k}$ would be close to the
discrete time transition on $\mathcal{X}$ for sufficiently small
$\epsilon_{k}$; and $\epsilon_{k}\to 0$ finalizes the consistency of
approximation. Assumptions (A7) and (A8) are purely for the validity of
computations and analysis of iPolicy. Assumption (A7) trivially holds for
sufficiently large $k$ by Assumption (A6) while assumption (A8) holds when
$d_{k}$ takes the lower bound in Assumption (A5).

###  V-B Performance guarantee

The consistency of the estimates requires proper selection of temporal
resolution $\epsilon_{k}$ and sufficient value iterations ${m_{k}}$ executed
on $V_{k}$. The following assumption is adapted from Assumption IV.1 in [35]
and it imposes the minimum required value iterations $\\{m_{k}\\}$ executed on
each graph with convergence guarantee.

######  Assumption V.2.

There exists a graph index $k_{0}$, an upper bound of intervals $T\geq 0$ and
an upper bound of accumulated discount $\bar{\beta}\in(0,1)$ such that
$\forall\bar{k}\geq k_{0}$, the total numbers of value iterations on each
graph $\\{m_{k}\\}$ satisfy

$\displaystyle\prod_{t=0}^{T}\max_{\ell\in\\{0,\dots,P\\}}\beta_{\breve{k}(t,\ell)}^{m_{\breve{k}(t,\ell)}}\leq\bar{\beta}<1,$

where the graph index is denoted by
$\breve{k}(t,\ell)\triangleq\bar{k}+t(P+1)+\ell$.

In particular, Assumption V.2 partitions the graph indices into intervals of
equal length $P+1$, and the aggregation of the worst-case discount on each
interval $\breve{k}\in[\bar{k}+t(P+1),\bar{k}+t(P+1)+P]$ over $T+1$
consecutive intervals should be strictly lower than $1$. This implies the
distance between the estimated value function $\varTheta_{k}^{m_{k}}$ and the
fixed point $\varTheta_{k}^{*}$ is strictly decreasing in a window of
$(T+1)(P+1)$ graphs.

The main result is summarized below, where the last estimate of minimal
traveling time $\varTheta^{m_{k}}_{k}$ converges to the ground truth pointwise
with probability one.

######  Theorem V.1.

Suppose Assumptions IV.1, V.1 and V.2 hold. Then the following holds with
probability one:

$\displaystyle\lim_{k\to+\infty}\min_{x^{\prime}\in(x+d_{k}\mathcal{B})\cap
V_{k}}\varTheta^{m_{k}}_{k}=\varTheta^{*}(x),\forall
x\in\mathcal{X}_{\mathrm{free}}\setminus\mathcal{Z},$

where $\mathcal{Z}$ is a set of sufficiently small measure.

##  VI Analysis

In this section, we analyze the performance of iPolicy. Towards the proof of
Theorem V.1, we proceed with the following result with a supplementary
definition.

######  Theorem VI.1.

Suppose Assumptions IV.1, V.1 and V.2 hold and let
$\varTheta_{k}^{*}(x)=\varTheta_{k}^{m}(x)=1,\forall
x\in\mathcal{X}\setminus(\mathcal{X}_{\mathrm{free}}+d_{k}\mathcal{B}),k\geq
1$ and $0\leq m\leq m_{k}$. Then it holds with probability one:

$\displaystyle\lim_{k\to+\infty}\min_{x^{\prime}\in(x+d_{k}\mathcal{B})\cap
V_{k}}\varTheta^{m_{k}}_{k}=\varTheta^{*}(x),\forall
x\in\mathcal{X}\setminus\mathcal{Z},$

where $\mathcal{Z}$ is a set of sufficiently small measure.

######  Remark VI.1.

Theorem VI.1 extends Theorem V.1 to the whole state space $\mathcal{X}$ by
manually setting $\varTheta_{k}^{m_{k}}(x)$ as $1$ for every
$x\in\mathcal{X}_{\mathrm{obs}}$, indicating there is no feasible path
connecting the goal set $\mathcal{X}_{\mathrm{goal}}$. This practice
consolidates the obstacle regions $\mathcal{X}_{\mathrm{obs}}$ where
$\varTheta^{m}_{k}$ is only defined with the free regions
$\mathcal{X}_{\mathrm{free}}$ and simplifies analysis by reducing the total
number of cases considered in the analysis.

The following lemma shows the supplementary definition in Theorem VI.1 is not
involved in any part in the execution of the proposed algorithms.

######  Lemma VI.1.

For any $k\geq 0$ and
$x\in\mathcal{X}\setminus(\mathcal{X}_{\mathrm{free}}+d_{k}\mathcal{B})$, it
holds that $x\notin S_{\kappa}$ and $x\notin F_{\kappa}(x^{\prime})$,
$\forall\kappa\geq k$ and $x^{\prime}\in V_{k}\setminus\\{x\\}$.

###### Proof.

It follows from Assumption V.1 that $d_{k}$ is monotonically decreasing. Then
for every
$x\in\mathcal{X}\setminus(\mathcal{X}_{\mathrm{free}}+d_{k}\mathcal{B})$,
$x\notin\mathcal{X}_{\mathrm{free}}+d_{\kappa}\mathcal{B},\forall\kappa\geq
k$. This implies $x$ will never be included in $S_{\kappa},\forall\kappa\geq
k$ per line 2 in Algorithm 2. It follows from the definition of $F_{k}(x)$ in
(3) that $x\notin F_{\kappa}(x^{\prime}),\forall x^{\prime}\in V_{\kappa}$ and
$\kappa\geq k$. This completes the proof. ∎

Lemma VI.1 shows that the supplementary definition of $\varTheta_{k}^{m}$ and
$\varTheta_{k}^{*}$ over
$\mathcal{X}\setminus(\mathcal{X}_{\mathrm{free}}+d_{k}\mathcal{B})$ will not
affect computations of $\varTheta_{k}^{m}$ and $\varTheta_{k}^{*}$ over the
free region, and Theorem VI.1 is identical to Theorem V.1 over
$\mathcal{X}_{\mathrm{free}}+d_{k}\mathcal{B}$. Throughout the analysis in
this section, we assume all conditions in Theorem VI.1 hold.

###  VI-A Preliminaries

In this subsection, we introduce a couple of preliminary results that
facilicate the proof of our main theorem. The following lemma is borrowed from
Lemma VII.4 in [35] and is listed below for the completeness of the paper.

######  Lemma VI.2 (Lemma VII.4 in [35]).

Given $\mathcal{X}_{1},\mathcal{X}_{2}\subseteq\mathcal{X}$, consider
$V_{i}:\mathcal{X}\to[0,1]$ and $\eta_{i}>0$ s.t.
$(x+\eta_{i}\mathcal{B})\cap\mathcal{X}_{i}\neq\emptyset,\forall
x\in\mathcal{X}$, where $i\in\\{1,2\\}$. Then for any set-valued map
$Y:\mathcal{X}\rightrightarrows\mathcal{X}$ s.t. $Y(x)\neq\emptyset,\forall
x\in\mathcal{X}$, we have

$\displaystyle\|\min_{\tilde{x}\in(Y(x)+\eta_{1}\mathcal{B})\cap\mathcal{X}_{1}}V_{1}(\tilde{x})-\min_{\tilde{x}\in(Y(x)+\eta_{2}\mathcal{B})\cap\mathcal{X}_{2}}V_{2}(\tilde{x})\|_{\mathcal{X}}$
(4) $\displaystyle\leq$
$\displaystyle\|\min_{\tilde{x}\in(x+\eta_{1}\mathcal{B})\cap\mathcal{X}_{1}}V_{1}(\tilde{x})-\min_{\tilde{x}\in(x+\eta_{2}\mathcal{B})\cap\mathcal{X}_{2}}V_{2}(\tilde{x})\|_{\mathcal{X}}$

If $\mathcal{X}_{1}=\mathcal{X}_{2}\triangleq\bar{\mathcal{X}}$ and
$\eta_{1}=\eta_{2}\triangleq\eta$, then

$\displaystyle\|\min_{\tilde{x}\in(x+\eta\mathcal{B})\cap\bar{\mathcal{X}}}V_{1}(\tilde{x})-\min_{\tilde{x}\in(x+\eta\mathcal{B})\cap\bar{\mathcal{X}}}V_{2}(\tilde{x})\|_{\mathcal{X}}\leq\|V_{1}-V_{2}\|_{\bar{\mathcal{X}}}.$
(5)

The following lemma shows that the maximum over a finite horizon of a
convergent sequence is also convergent.

######  Lemma VI.3.

Given a sequence $\\{a_{k}\\}$ s.t. $\lim_{k\to+\infty}a_{k}=0$, consider a
new sequence $\\{b_{k}\\}$ s.t. $b_{k}=\max_{\ell\in\mathcal{I}}a_{k+\ell}$,
where $\mathcal{I}\subseteq\mathbb{N}_{\geq 0}$ is finite set of integers.
Then $\lim_{k\to+\infty}b_{k}=0$.

###### Proof.

Since $\lim_{k\to+\infty}a_{k}=0$, then $\forall\epsilon>0$, $\exists K>0$
s.t. $\forall k\geq K$, $|a_{k}|\leq\epsilon$. Then it holds that
$|b_{k}|=|\max_{\ell\in\mathcal{I}}a_{k+\ell}|\leq\max_{\ell\in\mathcal{I}}|a_{k+\ell}|\leq\epsilon$.
This implies $\lim_{k\to+\infty}b_{k}=0$. ∎

The following lemma from Lemma 7.2 in [38] characterizes the upper bound of
the spatial resolution $d_{k}$.

######  Lemma VI.4.

Consider an estimate of spatial dispersion $d\geq
B(\frac{\log|V|}{|V|})^{1/n}$. Then it holds that

$\displaystyle\limsup_{|V|\to+\infty}\mathbb{P}[\exists x\in\mathcal{X}\text{
s.t. }(x+d\mathcal{B})\cap V=\emptyset]=0.$

######  Remark VI.2.

Lemma VI.4 indicates that for any $x\in\mathcal{X}$, there always is at lease
one node in $V_{k}$ within the spatial dispersion $d_{k}$ of $x$ with
probability one in an asymptotic sense. Via the spatial dispersion, $V_{k}$
can be treated as a grid in [15] and we can leverage all set-valued tools for
analysis.

The following lemma shows the whole graph will be periodically updated within
an interval of length $P$.

######  Lemma VI.5.

It holds that $V_{k+P}\subseteq\bigcup_{\ell=k}^{k+P}S_{\ell}$.

###### Proof.

Fix $x\in V^{k+P}$ and three cases arise:

  * •

Case 1: $x\in S_{k}$. Then the lemma trivially holds;

  * •

Case 2: $x\in V_{k}\setminus S_{k}$. This implies
$\hat{\mathcal{F}}_{k}(x)<P$. Then the staleness of $x$ increases to $P$ at
iteration $P-\hat{\mathcal{F}}_{k}(x)+k$. With that being said, $x\in
S_{P-\hat{\mathcal{F}}_{k}(x)+k}\subseteq\bigcup_{\ell=k}^{k+P}S_{\ell}$ and
this completes the proof in Case 2;

  * •

Case 3: $x\in V_{k+P}\setminus V_{k}$. Then $x$ is a newly added node at some
iteration $k^{\prime}\in[k+1,k+P]$ and $x\in
S_{k^{\prime}}\bigcup_{\ell=k}^{k+P}S_{\ell}$. This completes the proof in
Case 3.

Since the above holds $\forall x\in V^{k+P}$, proof is completed. ∎

The following lemma characterizes the values of nodes within goal regions.

######  Lemma VI.6.

Given $k\geq 1$ and any $x\in\mathcal{X}+(M\epsilon_{k}+d_{k})\mathcal{B}$, it
holds that $\varTheta_{k}^{\ell}(x)=\varTheta_{k}^{*}(x)=0$.

###### Proof.

Following the definition of the Bellman operator $\mathfrak{T}_{k}$ and the
initial values of $\varTheta_{0}$, one sees that
$\varTheta_{k}^{\ell}(x)=\varTheta_{k}^{\ell-1}(x)=\cdots=\varTheta_{k}^{0}(x)=0$.
The same holds for $\varTheta_{k}^{*}(x)$. This completes the proof. ∎

###  VI-B Contraction property on a fixed graph

In this subsection, we analyze the monotonicity of the Bellman operator
$\mathfrak{T}_{k}$ on a fixed graph, which is summarized in the following
lemma.

######  Lemma VI.7.

The distance between a value function $\varTheta_{k}$ and the fixed point
$\varTheta^{*}_{k}$ is monotonically decreasing over the Bellman operator
$\mathfrak{T}_{k}$; i.e.,

$\displaystyle\|\mathfrak{T}_{k}(\varTheta_{k})-\varTheta_{k}^{*}\|_{V_{k}}\leq\|\varTheta_{k}-\varTheta_{k}^{*}\|_{V_{k}}.$
(6)

Specially, strict monotonic decrease holds $\forall x\in S_{k}$; that is,

$\displaystyle|\mathfrak{T}_{k}(\varTheta_{k})(x)-\varTheta_{k}^{*}(x)|\leq\beta_{k}\|\varTheta_{k}-\varTheta_{k}^{*}\|_{F_{k}(x)}.$
(7)

Moreover, the perturbed version of the above special case holds with
probablity one in the asymptotic sense:

$\displaystyle|\mathfrak{T}_{k}(\varTheta_{k})(x)-\varTheta_{k}^{*}(x)|$ (8)
$\displaystyle\leq$
$\displaystyle\beta_{k}\|\min_{x^{\prime}\in(x+d_{k}\mathcal{B})\cap
V_{k}}\varTheta_{k}(x^{\prime})-\min_{x^{\prime}\in(x+d_{k}\mathcal{B})\cap
V_{k}}\varTheta^{*}_{k}(x^{\prime})\|_{\mathcal{X}\setminus\mathcal{Z}}$
$\displaystyle\leq$
$\displaystyle\beta_{k}\|\varTheta_{k}-\varTheta_{k}^{*}\|_{V_{k}},$

where $\mathcal{Z}$ is a set of sufficiently small measure.

###### Proof.

We first proceed to show the proofs of (7) and (8). Once (7) and (8) are
proven, we extend the results to (6).

######  Claim VI.1.

Inequality (7) holds for every $x\in S_{k}$.

###### Proof.

Notice that $\mathfrak{T}_{k}(\varTheta^{*}_{k})=\varTheta^{*}_{k}$, since
$\varTheta^{*}_{k}$ is the fixed point of $\mathfrak{T}_{k}$. Two cases arise.

  * •

Case 1, $x\in\mathcal{X}_{\mathrm{goal}}+(M\epsilon_{k}+d_{k})\mathcal{B}$.
Then it follows from Lemma VI.6 that
$|\mathfrak{T}_{k}(\varTheta_{k})(x)-\mathfrak{T}_{k}(\varTheta^{*}_{k})(x)|=0\leq\beta_{k}\|\varTheta_{k}-\varTheta_{k}^{*}\|_{V_{k}}$.

  * •

Case 2, $x\in
S_{k}\setminus(\mathcal{X}_{\mathrm{goal}}+(M\epsilon_{k}+d_{k})\mathcal{B})$.
Then the following inequality holds:

$\displaystyle|\mathfrak{T}_{k}(\varTheta_{k})(x)-\mathfrak{T}_{k}(\varTheta^{*}_{k})(x)|$
(9) $\displaystyle=$
$\displaystyle\Big{|}\Delta_{k}+\beta_{k}\min_{x^{\prime}\in
F_{k}(x)}\varTheta(x^{\prime})-\Delta_{k}-\beta_{k}\min_{x^{\prime}\in
F_{k}(x)}\varTheta^{*}_{k}(x^{\prime})\Big{|}$ $\displaystyle=$
$\displaystyle\beta_{k}\Big{|}\min_{x^{\prime}\in
F_{k}(x)}\varTheta(x^{\prime})-\min_{x^{\prime}\in
F_{k}(x)}\varTheta^{*}_{k}(x^{\prime})\Big{|}$ $\displaystyle\leq$
$\displaystyle\beta_{k}\|\varTheta_{k}-\varTheta_{k}^{*}\|_{F_{k}(x)}.$

In summary, we have
$\|\mathfrak{T}_{k}(\varTheta_{k})-\mathfrak{T}_{k}(\varTheta^{*}_{k})\|_{S_{k}}\leq\beta_{k}\|\varTheta_{k}-\varTheta_{k}^{*}\|_{V_{k}}$.
This completes the proof of Claim VI.1 and (7) is proven. ∎

######  Claim VI.2.

Inequality (8) holds with probability one in the asymptotic sense.

###### Proof.

Following the argument towards Claim VI.1, Case 1 trivially holds for Claim
VI.2. We rewrite (9) for $x\in
S_{k}\setminus(\mathcal{X}_{\mathrm{goal}}+(M\epsilon_{k}+d_{k})\mathcal{B})$
and arrive at

$\displaystyle|\mathfrak{T}_{k}(\varTheta_{k})(x)-\mathfrak{T}_{k}(\varTheta^{*}_{k})(x)|$
(10) $\displaystyle=$ $\displaystyle\beta_{k}\Big{|}\min_{x^{\prime}\in
F_{k}(x)}\varTheta_{k}(x^{\prime})-\min_{x^{\prime}\in
F_{k}(x)}\varTheta^{*}_{k}(x^{\prime})\Big{|}$ $\displaystyle\leq$
$\displaystyle\beta_{k}\Big{\|}\min_{x^{\prime}\in
F_{k}(x)}\varTheta_{k}(x^{\prime})-\min_{x^{\prime}\in
F_{k}(x)}\varTheta^{*}_{k}(x^{\prime})\Big{\|}_{V_{k}}.$

It follows from the supplementary definition in Theorem VI.1 that
$\varTheta_{k}(x)=1,\forall
x\in\mathcal{X}\setminus(\mathcal{X}_{\mathrm{free}}+d_{k}\mathcal{B})$. Then
it follow from the definition of $F_{k}(x)$ in (3) that

$\displaystyle\min_{x^{\prime}\in
F_{k}(x)}\varTheta_{k}(x^{\prime})=\min_{x^{\prime}\in(x+\epsilon_{k}\bigcup_{u\in
U}f(x,u)+\rho(d_{k},\epsilon_{k})\mathcal{B})\cap
V_{k}}\varTheta(x^{\prime}).$

Same holds for $\varTheta_{k}^{*}$. Then by Lemma VI.4 and
(LABEL:eq:perturbed2perturbed) in Lemma VI.2, (10) renders with probability
one at

$\displaystyle\|\min_{x^{\prime}\in
F_{k}(x)}\varTheta_{k}(x^{\prime})-\min_{x^{\prime}\in
F_{k}(x)}\varTheta^{*}_{k}(x^{\prime})\|_{V_{k}}$ (11) $\displaystyle\leq$
$\displaystyle\|\min_{x^{\prime}\in(x+\rho(d_{k},\epsilon_{k})\mathcal{B})\cap
V_{k}}\varTheta_{k}(x^{\prime})$
$\displaystyle\quad-\min_{x^{\prime}\in(x+\rho(d_{k},\epsilon_{k})\mathcal{B})\cap
V_{k}}\varTheta^{*}_{k}(x^{\prime})\|_{V_{k}}.$

Since $\rho(d_{k},\epsilon_{k})\geq d_{k}$, we apply
(LABEL:eq:perturbed2perturbed) again and (11) renders at

$\displaystyle\|\min_{x^{\prime}\in(x+\rho(d_{k},\epsilon_{k})\mathcal{B})\cap
V_{k}}\varTheta_{k}(x^{\prime})$ (12)
$\displaystyle\quad-\min_{x^{\prime}\in(x+\rho(d_{k},\epsilon_{k})\mathcal{B})\cap
V_{k}}\varTheta^{*}_{k}(x^{\prime})\|_{V_{k}}$ $\displaystyle\leq$
$\displaystyle\|\min_{x^{\prime}\in(x+d_{k}\mathcal{B})\cap
V_{k}}\varTheta_{k}(x^{\prime})-\min_{x^{\prime}\in(x+d_{k}\mathcal{B})\cap
V_{k}}\varTheta^{*}_{k}(x^{\prime})\|_{V_{k}}.$

Since $\mathcal{Z}$ is a set of sufficiently small measure, a new node falls
into $\mathcal{Z}$ with sufficiently small probability; i.e.,
$\mathbb{P}[V_{k}\cap\mathcal{Z}\neq\emptyset]=0$. With that being said, it
follows again from (LABEL:eq:perturbed2perturbed) and (12) that the following
holds with probability one:

$\displaystyle\|\min_{x^{\prime}\in(x+d_{k}\mathcal{B})\cap
V_{k}}\varTheta_{k}(x^{\prime})-\min_{x^{\prime}\in(x+d_{k}\mathcal{B})\cap
V_{k}}\varTheta^{*}_{k}(x^{\prime})\|_{V_{k}}$ $\displaystyle\leq$
$\displaystyle\|\min_{x^{\prime}\in(x+d_{k}\mathcal{B})\cap
V_{k}}\varTheta_{k}(x^{\prime})-\min_{x^{\prime}\in(x+d_{k}\mathcal{B})\cap
V_{k}}\varTheta^{*}_{k}(x^{\prime})\|_{\mathcal{X}\setminus\mathcal{Z}}.$

Then it follows from (5), the above inequality renders at

$\displaystyle\|\min_{x^{\prime}\in(x+d_{k}\mathcal{B})\cap
V_{k}}\varTheta_{k}(x^{\prime})-\min_{x^{\prime}\in(x+d_{k}\mathcal{B})\cap
V_{k}}\varTheta^{*}_{k}(x^{\prime})\|_{\mathcal{X}\setminus\mathcal{Z}}\allowdisplaybreaks$
$\displaystyle\leq$
$\displaystyle\|\min_{x^{\prime}\in(x+d_{k}\mathcal{B})\cap
V_{k}}\varTheta_{k}(x^{\prime})-\min_{x^{\prime}\in(x+d_{k}\mathcal{B})\cap
V_{k}}\varTheta^{*}_{k}(x^{\prime})\|_{\mathcal{X}}\allowdisplaybreaks$
$\displaystyle\leq$
$\displaystyle\|\varTheta_{k}-\varTheta^{*}_{k}\|_{V_{k}}.$

Combining all above relations completes the proof. ∎

######  Claim VI.3.

Inequality (6) holds true.

###### Proof.

It follows from Lemma VI.6 that for $x\in
S_{k}\setminus(\mathcal{X}_{\mathrm{goal}}+(M\epsilon_{k}+d_{k})\mathcal{B})$,
$|\mathfrak{T}_{k}(\varTheta_{k})(x)-\mathfrak{T}_{k}(\varTheta_{k}^{*})(x)|=0\leq\|\varTheta_{k}-\varTheta_{k}^{*}\|_{V_{k}}$.
Then it follows from Claim VI.1 that (6) holds for $x\in S_{k}$. This
completes the proof. ∎

It follows from Claims VI.1, VI.2 and VI.3 that Lemma VI.7 holds. This
completes the proof. ∎

######  Remark VI.3.

The sufficiently small set $\mathcal{Z}$ is to accommodate the result of
Corollary .2 in the proof of Theorem V.1, where the uniform convergence of the
Kruzhkov transformed functions holds in an almost sure sense.

###  VI-C Asynchronous contraction over graphs

Following Algorithm 3, we recursively define the set of updated nodes by
$S_{k}^{m_{k}}\triangleq S_{k}$, and $S_{k}^{\ell}=\bigcup_{x\in
S_{k}^{\ell+1}}F_{k}(x),\forall\ell\in\\{0,\dots,m_{k}-1\\}$. Correspondingly,
define the value function by
$\varTheta^{\ell}_{k}(x)\triangleq\mathfrak{T}_{k}(\varTheta^{\ell-1}_{k})(x),\forall
x\in S_{k}^{\ell}$ and $\ell\in\\{1,\dots,m_{k}\\}$, and
$\varTheta^{0}_{k}(x)=\hat{\varTheta}_{k}(x),\forall x\in V_{k}$. Starting
from this subsection, we consider two perturbed estimates of the minimal
traveling time function
$\displaystyle\tilde{\varTheta}_{k}^{*}(x)\triangleq\min_{x^{\prime}\in(x+d_{k}\mathcal{B})\cap
V_{k}}\varTheta_{k}^{*}(x^{\prime})$ and
$\displaystyle\tilde{\varTheta}_{k}^{*,-}(x)\triangleq\min_{x^{\prime}\in(x+d_{k-1}\mathcal{B})\cap
V_{k}}\varTheta_{k}^{*}(x^{\prime})$. Notice that the perturbation radii
$d_{k}$ and $d_{k-1}$ below the minimization operator are different. Similar
notations apply to $\tilde{\varTheta}_{k}^{m}$ and
$\tilde{\varTheta}_{k}^{m,-}$ respectively for $m\in\mathbb{N}_{\geq 0}$. The
following lemma characterizes the contraction property between two consecutive
graphs.

######  Lemma VI.8.

Consider $x\in S_{k}$. It holds with probability one:

$\displaystyle|\varTheta^{m_{k}}_{k}(x)-\varTheta^{*}_{k}(x)|\leq\beta_{k}^{m_{k}}\|\varTheta^{m_{k-1}}_{k-1}-\varTheta^{*}_{k-1}\|_{V_{k-1}}+\beta_{k}^{m_{k}}c_{k},$

where
$c_{k}\triangleq\|\tilde{\varTheta}^{*}_{k-1}-\tilde{\varTheta}^{*,-}_{k}\|_{\mathcal{X}\setminus\mathcal{Z}}$
and $\mathcal{Z}$ is sufficiently small.

###### Proof.

Fix $x\in S_{k}$. By line 2 in Algorithm 2, we apply (7) in Lemma VI.7 for
$m_{k}-1$ times and it renders at

$\displaystyle|\varTheta^{m_{k}}_{k}(x)-\varTheta^{*}_{k}(x)|\leq\beta_{k}^{m_{k}-1}\|\varTheta^{1}_{k}-\varTheta^{*}_{k}\|_{V_{k}}.$
(13)

Then it again follows from (6), (8) in Lemma VI.7 and Assumption (A8) that

$\displaystyle\|\varTheta^{1}_{k}-\varTheta^{*}_{k}\|_{V_{k}}$ (14)
$\displaystyle\leq$
$\displaystyle\beta_{k}\|\min_{x^{\prime}\in(x+\rho(d_{k},\epsilon_{k})\mathcal{B})\cap
V_{k}}\varTheta^{0}_{k}(x^{\prime})$
$\displaystyle-\min_{x^{\prime}\in(x+\rho(d_{k},\epsilon_{k})\mathcal{B})\cap
V_{k}}\varTheta^{*}_{k}(x^{\prime})\|_{\mathcal{X}\setminus\mathcal{Z}}$
$\displaystyle\leq$
$\displaystyle\beta_{k}\|\min_{x^{\prime}\in(x+d_{k-1}\mathcal{B})\cap
V_{k}}\varTheta_{k}^{0}(x^{\prime})-\min_{x^{\prime}\in(x+d_{k-1}\mathcal{B})\cap
V_{k}}\varTheta^{*}_{k}(x^{\prime})\|_{\mathcal{X}\setminus\mathcal{Z}}$
$\displaystyle=$
$\displaystyle\beta_{k}\|\tilde{\varTheta}_{k}^{0,-}-\tilde{\varTheta}_{k}^{*,-}\|_{\mathcal{X}\setminus\mathcal{Z}}.$

The last inequality follows from (LABEL:eq:perturbed2perturbed) in Lemma VI.2.
Let $x_{k}\in V_{k}\setminus V_{k-1}$ and two cases arise. We proceed to show
both cases render at the same result, summarized in (20).

Case 1,
$x_{k}\in\mathcal{X}\setminus(\mathcal{X}_{\mathrm{goal}}+(M\epsilon_{k}+d_{k})\mathcal{B})$.
It follows from Lemma VI.6 that
$\varTheta_{k}^{0}(x_{k})=\varTheta_{k}^{*}(x_{k})=0$; that is,
$\tilde{\varTheta}_{k}^{0,-}(x)=\tilde{\varTheta}_{k}^{*,-}(x)=0,\forall x\in
x_{k}+d_{k-1}\mathcal{B}$. Then we have

$\displaystyle\|\tilde{\varTheta}_{k}^{0,-}-\tilde{\varTheta}_{k}^{*,-}\|_{\mathcal{X}\setminus\mathcal{Z}}$
(15) $\displaystyle=$
$\displaystyle\|\tilde{\varTheta}_{k}^{0,-}-\tilde{\varTheta}_{k}^{*,-}\|_{\mathcal{X}\setminus(\mathcal{Z}\cup(x_{k}+d_{k-1}\mathcal{B}))}\allowdisplaybreaks$
$\displaystyle\leq$
$\displaystyle\|\tilde{\varTheta}_{k}^{0,-}-\tilde{\varTheta}_{k-1}^{*}\|_{\mathcal{X}\setminus(x_{k}+d_{k-1}\mathcal{B})}+c_{k}.$

We focus on the first term. For
$x\in\mathcal{X}\setminus(\mathcal{Z}\cup(x_{k}+d_{k-1}\mathcal{B}))$, since
no changes are made to values at $x\in V_{k-1}$, we have

$\displaystyle\tilde{\varTheta}^{0,-}_{k}(x)=\min_{x^{\prime}\in(x+d_{k-1}\mathcal{B})\cap
V_{k}}\varTheta^{0}_{k}(x^{\prime})\allowdisplaybreaks$ (16) $\displaystyle=$
$\displaystyle\min_{x^{\prime}\in(x+d_{k-1}\mathcal{B})\cap
V_{k-1}}\varTheta^{m_{k-1}}_{k-1}(x^{\prime})=\tilde{\varTheta}^{m_{k-1}}_{k-1}(x).$

Combining (15) and (16) renders at

$\displaystyle\|\tilde{\varTheta}_{k}^{0,-}-\tilde{\varTheta}_{k}^{*,-}\|_{\mathcal{X}\setminus\mathcal{Z}}\leq$
$\displaystyle\|\tilde{\varTheta}^{m_{k-1}}_{k-1}-\tilde{\varTheta}^{*}_{k-1}\|_{\mathcal{X}\setminus(x_{k}+d_{k-1}\mathcal{B})}+c_{k}$
(17) $\displaystyle\leq$
$\displaystyle\|\tilde{\varTheta}^{m_{k-1}}_{k-1}-\tilde{\varTheta}^{*}_{k-1}\|_{\mathcal{X}}+c_{k}.$

Case 2,
$x_{k}\in\mathcal{X}_{\mathrm{goal}}+(M\epsilon_{k}+d_{k})\mathcal{B}$. Then
by the triangular inequality of supremum norm, one has a result similar to
(15):

$\displaystyle\|\tilde{\varTheta}_{k}^{0,-}-\tilde{\varTheta}_{k}^{*,-}\|_{\mathcal{X}\setminus\mathcal{Z}}\leq\|\tilde{\varTheta}_{k}^{0,-}-\tilde{\varTheta}_{k-1}^{*}\|_{\mathcal{X}}+c_{k}.$
(18)

We focus on the first term
$\|\tilde{\varTheta}^{0,-}_{k}-\tilde{\varTheta}^{*}_{k-1}\|_{\mathcal{X}}$.
For any $x\in\mathcal{X}\setminus(x_{k}+d_{k-1}\mathcal{B})$, it renders at
the identical result as (16). For any $x\in x_{k}+d_{k-1}\mathcal{B}$, since
$x_{k}\in\mathcal{X}\setminus(\mathcal{X}_{\mathrm{goal}}+(M\epsilon_{k}+d_{k})\mathcal{B})$,
it follows line 1 in Algorithm 1 that
$\min_{x^{\prime}\in(x+d_{k-1}\mathcal{B})\cap
V_{k}}\varTheta^{0}_{k}(x^{\prime})\leq\varTheta^{0}_{k}(x_{k})=1$. Then we
have

$\displaystyle\tilde{\varTheta}^{0,-}_{k}(x)=\min_{x^{\prime}\in(x+d_{k-1}\mathcal{B})\cap
V_{k-1}}\varTheta^{m_{k-1}}_{k-1}(x^{\prime})=\tilde{\varTheta}^{m_{k-1}}_{k-1}(x).$
(19)

Combining (18) and (19) renders at

$\displaystyle\|\tilde{\varTheta}_{k}^{0,-}-\tilde{\varTheta}_{k}^{*,-}\|_{\mathcal{X}\setminus\mathcal{Z}}\leq\|\tilde{\varTheta}^{m_{k-1}}_{k-1}-\tilde{\varTheta}^{*}_{k-1}\|_{\mathcal{X}}+c_{k},$
(20)

which is identical to (17). This verifies that the two cases on $x_{k}$ render
at the same result.

Notice that (20) can be fruther relaxed as

$\displaystyle\|\tilde{\varTheta}^{0,-}_{k}-\tilde{\varTheta}^{*}_{k-1}\|_{\mathcal{X}}=$
$\displaystyle\|\tilde{\varTheta}^{m_{k-1}}_{k-1}-\tilde{\varTheta}^{*}_{k-1}\|_{\mathcal{X}}$
(21) $\displaystyle\leq$
$\displaystyle\|\varTheta^{m_{k-1}}_{k-1}-\varTheta^{*}_{k-1}\|_{V_{k-1}},$

where the inequality follows from (5) in Lemma VI.2. Combining (13) to (21)
completes the proof. ∎

The following corollary characterizes the upper bound of the approximation
errors on $V_{k}$ using the error on $G_{k-1}$.

######  Corollary VI.1.

The following inequality holds for any $\ell\in\\{1,\dots,m_{k}\\}$ with
probability one:

$\displaystyle\|\varTheta^{\ell}_{k}-\varTheta^{*}_{k}\|_{V_{k}}\leq\|\varTheta^{m_{k-1}}_{k-1}-\varTheta^{*}_{k-1}\|_{V_{k-1}}+c_{k},$

where
$c_{k}\triangleq\|\tilde{\varTheta}^{*}_{k-1}-\tilde{\varTheta}^{*,-}_{k}\|_{\mathcal{X}\setminus\mathcal{Z}}$
and $\mathcal{Z}$ is sufficiently small.

###### Proof.

Fix $x\in V_{k}$. When $x\in S_{k}$, it follows from Lemma VI.8 that
$|\varTheta^{\ell}_{k}(x)-\varTheta^{*}_{k}(x)|\leq\beta_{k}\|\varTheta^{\ell-1}_{k}-\varTheta^{*}_{k}\|_{V_{k}}+\beta_{k}c^{k}_{k-1}\leq\|\varTheta^{\ell-1}_{k}-\varTheta^{*}_{k}\|_{V_{k}}+c_{k}$.
When $x\in V_{k}\setminus S_{k}$, the worst case is that no updates are
imposed on $x$; in such case, we have
$\varTheta_{k}^{\ell}(x)=\varTheta_{k}^{0}(x)$. Therefore,
$|\varTheta^{\ell}_{k}(x)-\varTheta^{*}_{k}(x)|=|\varTheta^{0}_{k}(x)-\varTheta^{*}_{k}(x)|\leq\|\varTheta^{0}_{k}-\varTheta^{*}_{k}\|_{V_{k}}+c_{k}$.
This completes the proof. ∎

The following corollary characterizes the contraction property over $P$
consecutive graphs.

######  Corollary VI.2.

Consider $k\in\\{\bar{k}+1,\cdots,\bar{k}+P\\}$ for
$\bar{k}\in\mathbb{N}_{\geq 0}$ and $x\in S_{k}$. The following holds with
probability one:

$\displaystyle|\varTheta^{m_{k}}_{k}(x)-\varTheta^{*}_{k}(x)|\leq\beta_{k}^{m_{k}}\|\varTheta^{m_{\bar{k}}}_{\bar{k}}-\varTheta^{*}_{\bar{k}}\|_{V_{\bar{k}}}+\beta_{k}^{m_{k}}\sum_{\kappa=\bar{k}}^{k-1}c_{\kappa},$

where
$c_{\kappa}\triangleq\|\tilde{\varTheta}^{*}_{\kappa-1}-\tilde{\varTheta}^{*,-}_{\kappa}\|_{\mathcal{X}\setminus\mathcal{Z}}$
and $\mathcal{Z}$ is sufficiently small.

###### Proof.

Fix $x\in S_{k}$. Applying Lemma VI.8 once and iteratively applying Corollary
VI.1 to the left side renders at

$\displaystyle|\varTheta^{m_{k}}_{k}(x)-\varTheta^{*}_{k}(x)|$
$\displaystyle\leq$
$\displaystyle\beta_{k}^{m_{k}}\|\varTheta^{m_{k-1}}_{k-1}-\varTheta^{*}_{k-1}\|_{V_{k-1}}+\beta_{k}^{m_{k}}c_{k}$
$\displaystyle\leq$
$\displaystyle\beta_{k}^{m_{k}}\|\varTheta^{m_{k-2}}_{k-2}-\varTheta^{*}_{k-2}\|_{V_{k-2}}+\beta_{k}^{m_{k}}c_{k-1}+\beta_{k}^{m_{k}}c_{k}$
$\displaystyle\leq$
$\displaystyle\cdots\leq\beta_{k}^{m_{k}}\|\varTheta^{m_{\bar{k}}}_{\bar{k}}-\varTheta^{*}_{\bar{k}}\|_{V_{\bar{k}}}+\beta_{k}^{m_{k}}\sum_{\kappa=\bar{k}}^{k-1}c_{\kappa+1}.$

This completes the proof. ∎

###  VI-D Proof of Theorem VI.1

In this subsection, we present the proof of Theorem VI.1.

###### Proof of Theorem VI.1.

Let $\bar{k}\in\mathbb{N}$ and consider an interval
$\\{\bar{k},\bar{k}+1,\dots,\bar{k}+P\\}$. We start with
$\|\min_{x^{\prime}\in(x+d_{\bar{k}+P}\mathcal{B})\cap
V_{\bar{k}+P}}\varTheta^{m_{\bar{k}+P}}_{\bar{k}+P}-\varTheta^{*}\|_{\mathcal{X}\setminus\mathcal{Z}}$.
It follows from the triangular inequality of the maximum norm that

$\displaystyle\|\min_{x^{\prime}\in(x+d_{\bar{k}+P}\mathcal{B})\cap
V_{\bar{k}+P}}\varTheta^{m_{\bar{k}+P}}_{\bar{k}+P}-\varTheta^{*}\|_{\mathcal{X}\setminus\mathcal{Z}}$
(22) $\displaystyle=$
$\displaystyle\|\tilde{\varTheta}^{m_{\bar{k}+P}}_{\bar{k}+P}-\varTheta^{*}\|_{\mathcal{X}\setminus\mathcal{Z}}\leq\|\tilde{\varTheta}^{m_{\bar{k}+P}}_{\bar{k}+P}-\tilde{\varTheta}^{*}_{\bar{k}+P}\|_{\mathcal{X}\setminus\mathcal{Z}}+\mathfrak{B}_{\bar{k}+P}$
$\displaystyle\leq$
$\displaystyle\mathfrak{A}_{\bar{k}+P}+\mathfrak{B}_{\bar{k}+P},$

where
$\mathfrak{A}_{\bar{k}+P}\triangleq\|\varTheta^{m_{\bar{k}+P}}_{\bar{k}+P}-\varTheta^{*}_{\bar{k}+P}\|_{V_{\bar{k}+P}}$
and
$\mathfrak{B}_{\bar{k}+P}\triangleq\|\tilde{\varTheta}^{*}_{\bar{k}+P}-\varTheta^{*}\|_{\mathcal{X}\setminus\mathcal{Z}}$
and the last inequality follows from (5) in Lemma VI.2.

We fix $x\in V_{\bar{k}+P}$ and let
$\hat{k}\triangleq\operatorname{argmax}_{\bar{k}\leq
k\leq\bar{k}+P}\hat{\mathcal{F}}_{k}(x)$ be the last time when value at $x$ is
updated. Notice that $\hat{k}$ is a function of $x$, but for the conciseness
of the proof, we omit the depnedency on $x$ in notation. It follows from Lemma
VI.5 that $\hat{k}\geq\bar{k}$ and $x\in S_{\hat{k}}$. Then the following
holds:

$\displaystyle|\varTheta^{m_{\bar{k}+P}}_{\bar{k}+P}(x)-\varTheta^{*}_{\bar{k}+P}(x)|=|\varTheta^{m_{\hat{k}}}_{\hat{k}}(x)-\varTheta^{*}_{\bar{k}+P}(x)|$
(23) $\displaystyle\leq$
$\displaystyle|\varTheta^{m_{\hat{k}}}_{\hat{k}}(x)-\varTheta^{*}_{\hat{k}}(x)|+|\varTheta^{*}_{\hat{k}}(x)-\varTheta^{*}_{\bar{k}+P}(x)|,$

where the inequality is a direct result of the triangular inequality of the
maximum norm.

We first analyze the first term in (23). Since $x\in S_{\hat{k}}$, it follows
from Lemma VI.8 that

$\displaystyle|\varTheta^{m_{\hat{k}}}_{\hat{k}}(x)-\varTheta^{*}_{\hat{k}}(x)|\leq\beta_{\hat{k}}^{m_{\hat{k}}}\|\varTheta^{m_{\hat{k}-1}}_{\hat{k}-1}-\varTheta^{*}_{\hat{k}-1}\|_{V_{\hat{k}-1}}+\beta_{\hat{k}}^{m_{\hat{k}}}c_{\hat{k}}.$

Then we apply Corollary VI.2 and it arrives at

$\displaystyle|\varTheta^{m_{\hat{k}}}_{\hat{k}}(x)-\varTheta^{*}_{\hat{k}}(x)|\leq$
$\displaystyle\beta_{\hat{k}}^{m_{\hat{k}}}\|\varTheta^{m_{\bar{k}-1}}_{\bar{k}-1}-\varTheta^{*}_{\bar{k}-1}\|_{V_{\bar{k}-1}}+\mathfrak{C}_{\bar{k}-1}(x)$
(24) $\displaystyle=$
$\displaystyle\beta_{\hat{k}}^{m_{\hat{k}}}\mathfrak{A}_{\bar{k}-1}+\mathfrak{C}_{\bar{k}-1}(x),$

where
$\mathfrak{C}_{\bar{k}}(x)\triangleq\beta_{\hat{k}}^{m_{\hat{k}}}\sum_{\kappa=\bar{k}}^{\hat{k}}c_{\kappa}$
is a function of $x$, as $\hat{k}$ inside the definition is a function of $x$.

We then analyze the second term. It follows from the triangular inequality
that the second term renders at $\forall x\in V_{k}$,

$\displaystyle|\varTheta^{*}_{\hat{k}+1}(x)-\varTheta^{*}_{\bar{k}+P}(x)|\leq\bar{\mathcal{C}}_{\hat{k}+1}(x)+\bar{\mathcal{C}}_{\bar{k}+P}(x),$
(25)

where
$\bar{\mathcal{C}}_{k}(x)\triangleq|\varTheta^{*}_{k}(x)-\varTheta^{*}(x)|,\forall
x\in V_{k}$ and $\bar{\mathcal{C}}_{k}(x)=0$ otherwise. Combining (24) and
(25), we have

$\displaystyle|\varTheta^{m_{\bar{k}+P}}_{\bar{k}+P}(x)-\varTheta^{*}_{\bar{k}+P}(x)|$
(26) $\displaystyle\leq$
$\displaystyle\beta_{\hat{k}}^{m_{\hat{k}}}\mathfrak{A}_{\bar{k}-1}+\mathfrak{C}_{\bar{k}-1}(x)+\bar{\mathcal{C}}_{\hat{k}+1}(x)+\bar{\mathcal{C}}_{\bar{k}+P}(x).$

Since (26) holds for every $x\in V_{\bar{k}+P}$, taking the maximum over
$V_{\bar{k}+P}$ renders at

$\displaystyle\mathfrak{A}_{\bar{k}+P}=\|\varTheta^{m_{\bar{k}+P}}_{\hat{k}+P}(x)-\varTheta^{*}_{\bar{k}+P}(x)\|_{V_{\bar{k}+P}}$
(27) $\displaystyle\leq$
$\displaystyle\bar{\beta}_{\bar{k}}\mathfrak{A}_{\bar{k}-1}+\max_{x\in
V_{\bar{k}+P}}\Big{[}\mathfrak{C}_{\bar{k}-1}(x)+\bar{\mathcal{C}}_{\hat{k}+1}(x)+\bar{\mathcal{C}}_{\bar{k}+P}(x)\Big{]},$

where $\displaystyle\bar{\beta}_{\bar{k}+P}\triangleq\max_{\bar{k}\leq
k\leq\bar{k}+P}\beta_{k}^{m_{k}}$. Now we would like to show the maximum term
in (27) converges to zero by Lemma VI.3.

######  Claim VI.4.

The following holds true:

$\displaystyle\lim_{\bar{k}\to\infty}\max_{x\in
V_{\bar{k}+P}}\Big{[}\mathfrak{C}_{\bar{k}-1}(x)+\bar{\mathcal{C}}_{\hat{k}+1}(x)+\bar{\mathcal{C}}_{\bar{k}+P}(x)\Big{]}=0.$

###### Proof.

We first show $\mathfrak{C}_{\bar{k}-1}(x)$ converges to zero for every $x\in
V_{\bar{k}+P}$. For notational simplicity, we use $\mathfrak{C}_{\bar{k}}(x)$
instead of $\mathfrak{C}_{\bar{k}-1}(x)$ in this proof. Fix $x\in
V_{\bar{k}+P}$. Since $\beta_{k}\leq 1$, we have
$\mathfrak{C}_{\bar{k}}(x)\leq\sum_{\kappa=\bar{k}}^{\bar{k}+P}c_{\kappa}$, a
finite sum of $c_{\kappa}$. It follows from Corollary .2 that

$\displaystyle\lim_{\kappa\to+\infty}c_{\kappa}=\lim_{\kappa\to+\infty}\|\tilde{\varTheta}^{*}_{\kappa-1}-\tilde{\varTheta}^{*,-}_{\kappa}\|_{\mathcal{X}\setminus\mathcal{Z}}$
$\displaystyle\leq$
$\displaystyle\lim_{\kappa\to+\infty}\|\tilde{\varTheta}^{*}_{\kappa-1}-\varTheta^{*}\|_{\mathcal{X}\setminus\mathcal{Z}}+\lim_{\kappa\to+\infty}\|\varTheta^{*}-\tilde{\varTheta}^{*,-}_{\kappa}\|_{\mathcal{X}\setminus\mathcal{Z}}=0.$

Then we have

$\displaystyle\lim_{\bar{k}\to+\infty}\bar{\mathfrak{C}}_{\bar{k}}(x)\leq\lim_{\bar{k}\to+\infty}\sum_{\kappa=\bar{k}}^{\bar{k}+P}c_{\kappa}=\sum_{i=0}^{P}\lim_{\bar{k}\to+\infty}c_{\bar{k}+i}=0.$

Since $\bar{\mathfrak{C}}_{\bar{k}}(x)\geq 0$, this completes the proof of
$\lim_{\bar{k}\to+\infty}\mathfrak{C}_{\bar{k}}(x)=0$.

Then we show the rest two terms also converge to zero. It follows from
Corollary .1 that both $\bar{\mathcal{C}}_{\hat{k}+1}(x)$ and
$\bar{\mathcal{C}}_{\bar{k}+P}(x)$ converges to zero for every $x\in
V_{\bar{k}+P}$ as $\bar{k}\to+\infty$. Then it follows from Lemma VI.3 that
$\lim_{\bar{k}\to\infty}\max_{x\in
V_{\bar{k}+P}}\Big{[}\mathfrak{C}_{\bar{k}-1}(x)+\bar{\mathcal{C}}_{\hat{k}+1}(x)+\bar{\mathcal{C}}_{\bar{k}+P}(x)\Big{]}=0.$
This completes the proof. ∎

For brevity, we rewrite (27) as

$\displaystyle\mathfrak{A}_{\breve{k}(\tau)}\leq$
$\displaystyle\bar{\beta}_{\breve{k}(\tau)}\mathfrak{A}_{\breve{k}(\tau-1)}+\mathfrak{D}_{\breve{k}(\tau)},$
(28)

where $\breve{k}(\tau)\triangleq\bar{k}+\tau(P+1)$ and
$\displaystyle\mathfrak{D}_{\bar{k}+P}\triangleq\max_{x\in
V_{\bar{k}+P}}\Big{[}\mathfrak{C}_{\bar{k}-1}(x)+\bar{\mathcal{C}}_{\hat{k}+1}(x)+\bar{\mathcal{C}}_{\bar{k}+P}(x)\Big{]}$.
Notice that $\lim_{k\to+\infty}\mathfrak{D}_{k}=0$ as a result of Claim VI.4.
By Assumption V.2, we continue to relax $\mathfrak{A}_{\breve{k}(\tau-1)}$
over $T$ intervals, each of which consists of $P+1$ graphs, and it holds for
any $\zeta\in\mathbb{N}_{\geq 0}$ that

$\displaystyle\mathfrak{A}_{\breve{k}((\zeta+1)T)}$ (29) $\displaystyle\leq$
$\displaystyle\prod_{\tau=\zeta
T+1}^{(\zeta+1)T}\bar{\beta}_{\breve{k}(\tau)}\mathfrak{A}_{\breve{k}(\zeta
T)}+\sum_{\tau=\zeta
T+1}^{(\zeta+1)T}\mathfrak{D}_{\breve{k}(\tau)}\prod_{\tau^{\prime}=\tau+1}^{(\zeta+1)T}\bar{\beta}_{\breve{k}(\tau^{\prime})}$
$\displaystyle\leq$ $\displaystyle\bar{\beta}\mathfrak{A}_{\breve{k}(\zeta
T)}+\breve{\mathfrak{D}}_{\zeta T},$

where $\breve{\mathfrak{D}}_{\zeta T}\triangleq\sum_{\tau=\zeta
T+1}^{(\zeta+1)T}\mathfrak{D}_{\breve{k}(\tau)}\prod_{\tau^{\prime}=\tau+1}^{(\zeta+1)T}\bar{\beta}_{\breve{k}(\tau^{\prime})}$
is a finite sum of $\mathfrak{D}_{k}$. Since
$\displaystyle\lim_{\tau\to+\infty}\mathfrak{D}_{\breve{k}(\tau)}=\lim_{k\to+\infty}\mathfrak{D}_{k}=0$,
$\breve{\mathfrak{D}}_{\zeta T}$ converges to $0$ as $\zeta\to+\infty$. It
follows from Lemma VII.5 in [35] that
$\displaystyle\lim_{k\to+\infty}\mathfrak{A}_{k}=\lim_{\zeta\to+\infty}\mathfrak{A}_{\breve{k}(\zeta
T)}=0$. It follows from Corollary .2 that
$\lim_{\bar{k}\to+\infty}\mathfrak{B}_{\bar{k}}=0$. Together with (22), we
have

$\displaystyle\lim_{k\to+\infty}\|\min_{x^{\prime}\in(x+d_{k}\mathcal{B})\cap
V_{k}}\varTheta^{m_{k}}_{k}-\varTheta^{*}\|_{\mathcal{X}\setminus\mathcal{Z}}$
$\displaystyle=$
$\displaystyle\lim_{\bar{k}\to+\infty}\|\min_{x^{\prime}\in(x+d_{\bar{k}+P}\mathcal{B})\cap
V_{\bar{k}+P}}\varTheta^{m_{\bar{k}+P}}_{\bar{k}+P}-\varTheta^{*}\|_{\mathcal{X}\setminus\mathcal{Z}}$
$\displaystyle\leq$
$\displaystyle\lim_{\bar{k}\to+\infty}\mathfrak{A}_{\bar{k}+P}+\lim_{\bar{k}\to+\infty}\mathfrak{B}_{\bar{k}+P}=0.$

This completes the proof. ∎

##  VII Numerical Results and Discussion

In this section, we first present a computation-saving adaptation of the
proposed iPolicy algorithm for a class of simpler but widely used dynamic
systems. Then we present results of numerical experiments for three
differential systems - point mass, simple car and Dubins car. Using the point
mass, we show the correctness of the theoretical results. We show several
simulations for unicycle robots in obstacle-free as well as cluttered
environment to perform complex maneuvers like automatic parking. The
experiments presented in this section are done to investigate the following
characteristics of the proposed iPolicy algorithm.

  1. 1.

Can iPolicy recover the optimal value function for systems with differential
constraints?

  2. 2.

Can the state-based feedback controllers handle complex motion maneuvers in
the presence of obstacles?

  3. 3.

Can we demonstrate the anytime and incremental nature of the algorithm? Is the
algorithm able to monotonically improve the plan towards the goal?

In the rest of this section, we will try to answer the above questions with
systems of increasing complexity.

(a) The estimated value function with $761$ samples after $21.8$ seconds.

(b) The estimated value function with $1,934$ samples after $205.7$ seconds.

(c) The estimated value function with $16,979$ samples after $20062.7$
seconds.

(d) The root of mean squared errors over total number of samples on $5$ runs.

Figure 2: The estimated value function obtained for a point mass in the
presence of obstacles and its convergence with the computational time. Goal
region for the point-mass is centered at $(0,0)$ with red dash line. The
colorbar in the plots represent the approximate time to the goal region.
Figure 2d shows the errors over $5$ independent runs. As seen in Figure 2d,
iPolicy achieves faster convergence compared to the multigrid method.

###  VII-A Computation-saving query

In this subsection, we introduce a class of simpler but widely used dynamical
systems in the robotics community and an adaptation for iPolicy that
significantly reduces computations in practice but maintains the optimality in
Theorem V.1. Consider a class of systems that, in addition to the satisfaction
of Assumption IV.1, also satisfy the following assumption:

######  Assumption VII.1.

System (1) satisfies that for any $x\in\mathcal{X},\exists u\in\mathcal{U}$
s.t. $f(x,u)=0$.

In other words, Assumption VII.1 indicates the robot can instantly stop at any
state. The following lemma shows the one-hop neighbors $F_{k}(x)$ is
monotonically decreasing in $k$.

######  Lemma VII.1.

Suppose Assumptions IV.1, V.1 and VII.1 hold. Then it holds that
$F_{k^{\prime}}(x)\subseteq F_{k}(x)\cup(V_{k^{\prime}}\setminus
V_{k}),\forall x\in\mathcal{X}_{\mathrm{free}}$ and $k^{\prime}\geq k$.

###### Proof.

It follows from Assumption V.1 that $d_{k},\epsilon_{k}$ and
$\rho(d_{k},\epsilon_{k})$ are monotonically decreasing in $k$. Recall the
definition of $F_{k}(x)$ in (3) follows
$F_{k}(x)=(x+\epsilon_{k}\bigcup_{u\in\mathcal{U}}f(x,u)+\rho(d_{k},\epsilon_{k})\mathcal{B})\cap
V_{k}\cap(\mathcal{X}_{\mathrm{free}}+d_{k}\mathcal{B})$. Fix
$x\in\mathcal{X}_{\mathrm{free}}$ and $x^{\prime}\in F_{k^{\prime}}(x)$. We
proceed to prove $x^{\prime}\in F_{k}(x)\cup(V_{k^{\prime}}\setminus V_{k})$
by showing $x^{\prime}$ is included in each part of $F_{k}(x)$

We first show $x^{\prime}\in
x+\epsilon_{k}\bigcup_{u\in\mathcal{U}}f(x,u)+\rho(d_{k},\epsilon_{k})\mathcal{B}$.
Since $x^{\prime}\in
x+\epsilon_{k^{\prime}}\bigcup_{u\in\mathcal{U}}f(x,u)+\rho(d_{k^{\prime}},\epsilon_{k^{\prime}})\mathcal{B}$,
there exist $u^{\prime}\in\mathcal{U},b\in\mathcal{B}$ and
$\alpha\in[0,\rho(d_{k^{\prime}},\epsilon_{k^{\prime}})]$ s.t.
$x^{\prime}=x+\epsilon_{k^{\prime}}f(x,u^{\prime})+\alpha b$. It follows from
Assumptions (A4) and VII.1 that for any $u\in\mathcal{U}$ and $\zeta\in[0,1]$,
$\zeta f(x,u)\in\bigcup_{u\in\mathcal{U}}f(x,u)$. By the decreasing
monotonicity of $\epsilon_{k}$ in $k$, this implies
$\epsilon_{k^{\prime}}f(x,u^{\prime})=\frac{\epsilon_{k^{\prime}}}{\epsilon_{k}}\epsilon_{k}f(x,u^{\prime})\in\epsilon_{k}\bigcup_{u\in\mathcal{U}}f(x,u)$.
It again follows from the decreasing monotonicity of
$\rho(d_{k},\epsilon_{k})$ that $\alpha
b\in\rho(d_{k},\epsilon_{k})\mathcal{B}$. In summary, we have $x^{\prime}\in
x+\epsilon_{k}\bigcup_{u\in\mathcal{U}}f(x,u)+\rho(d_{k},\epsilon_{k})\mathcal{B}$.

Then we show $x^{\prime}\in V_{k}\cup(V_{k^{\prime}}\setminus V_{k})$. Since
$V_{k}\cup(V_{k^{\prime}}\setminus V_{k})=V_{k^{\prime}}$, this trivially
holds.

Finally, we show $x^{\prime}\in\mathcal{X}_{\mathrm{free}}+d_{k}\mathcal{B}$.
Since
$x^{\prime}\in\mathcal{X}_{\mathrm{free}}+\rho(d_{k^{\prime}},\epsilon_{k^{\prime}})\mathcal{B}$,
there exist $x_{f}\in\mathcal{X}_{\mathrm{free}},b\in\mathcal{B}$ and
$\alpha\in[0,d_{k^{\prime}}]$ that $x^{\prime}=x_{f}+\alpha b$. It follows
from the decreasing monotonicity of $d_{k}$ that $\alpha b\in
d_{k}\mathcal{B}$. The proof of
$x^{\prime}\in\mathcal{X}_{\mathrm{free}}+d_{k}\mathcal{B}$ is completed.

The above arguments show that $x^{\prime}\in F(x)\cup(V_{k^{\prime}}\setminus
V_{k})$. Since this holds for every $x^{\prime}\in F_{k^{\prime}}(x)$ and
$x\in\mathcal{X}_{\mathrm{free}}$, the proof is completed. ∎

1 Input: Graph index $k$, newly added sample $x_{\mathrm{new}}$;

2 Compute $F(x_{\mathrm{new}})$ as (3);

3 for  _$x\in V_{k}\setminus\\{x_{\mathrm{new}}\\}$_ do

4 if  _$x_{\mathrm{new}}\in
x+\epsilon_{k}\bigcup_{u\in\mathcal{U}}f(x,u)+\rho(d_{k},\epsilon_{k})\mathcal{B}$_
then

5 $F(x)\leftarrow F(x)\cup\\{x_{\mathrm{new}}\\}$;

6

7

8 for  _$x\in V_{k}\setminus\\{x_{\mathrm{new}}\\}$_ do

9 for  _$x^{\prime}\in F(x)$_ do

10 if
_$x^{\prime}\notin(x+\epsilon_{k}\bigcup_{u\in\mathcal{U}}f(x,u)+\rho(d_{k},\epsilon_{k})\mathcal{B})$_
then

11 $F(x)\leftarrow F(x)\setminus\\{x^{\prime}\\}$;

12

13 if  _$x^{\prime}\notin(\mathcal{X}_{\mathrm{free}}+d_{k}\mathcal{B})$_ then

14 $F(x)\leftarrow F(x)\setminus\\{x^{\prime}\\}$;

15

16

17

return $F$

Algorithm 4 Computation-saving query of $F$

Based on Lemma VII.1, we can construct $F_{k}(x)$ in line 3 of Algorithm 3
BackProp in a computationally efficient manner, where the execution of the
computationally heaviest part, querying $V_{k}$ within a range of given a
random state $x$, is reduced. Specifically, we treat $F_{k}(x)$ as a set of
samples independent of graph index $k$; i.e., $F_{k}(x)=F(x)$. See Algorithm
4. When $x$ is added to $V_{k}$ as $x_{\mathrm{new}}$, we initialize $F(x)$ as
its definition (3) using the resolutions $\epsilon_{k}$ and $d_{k}$ at that
time, and update the one-hop neighbors of every other sampled state as lines 4
to 4; when querying for the one-hop neighbors of $x$ in a later iteration, we
update $F(x)$ by removing faraway one-hop neighbors according to new
resolutions as lines 4 to 4. In contrast to the vanilla query command as line
3 in BackProp, where searching the whole $V_{k}$ is executed every time when
$F(x)$ is queried, the new adaptation skips this step and saves computations.
This adaptation is leveraged throughout the results in Section VII.

###  VII-B Point mass

In this subsection, we try to show the correctness of the algorithm and the
rate of convergence through a number of numerical simulations for a point
mass. The point mass dynamic follows a single integrator
$\begin{bmatrix}\dot{x}\\\ \dot{y}\end{bmatrix}=\begin{bmatrix}u_{1}\\\
u_{2}\end{bmatrix}$, where the velocity on each coordinate can be directly
manipulated. The control set is a unit disc; i.e., the maximum velocity is
restricted to $1$. The environment is a square of size $20\times 20$ populated
with a rectangular obstacle and a circular obstacle, and the robot desires to
rest at a central circle. We used $\epsilon_{k}=(5d_{k})^{2/3}$ and
$\rho(\epsilon_{k},d_{k})=2d_{k}$ for computational simplicity. The experiment
used $P=50$ and $m_{k}=500$; i.e., every vertex was updated at-most after $50$
iterations and the recursion depth is a constant of $500$. Notice that a
constant recursion depth does not necessarily satisfy Assumption V.2, but the
results still show its applicability in practice.

Figures 2a to 2c qualitatively show the estimated value function over the
whole environment after the computations of $21.8$ seconds, $205.7$ seconds
and $20062.7$ seconds, respectively, where the goal region is marked by the
red dash circle. It is observed that the estimate of minimal traveling time to
the goal region is monotonically improving.

Quantitative characterization of the approximation error is available, as the
minimal traveling time function $\mathds{T}^{*}$ can be analytically computed
for a point mass in the presence of regularly shaped obstacles. The
approximation error is computed between the estimated value function
$\mathds{T}_{k}$ and the ground truth $\mathds{T}^{*}$ and is characterized by
the root of mean squared error (RMSE), which follows as
$RMSE\triangleq\sqrt{\frac{1}{|V_{k}|}\sum_{x\in
V_{k}}|\mathds{T}_{k}(x)-\mathds{T}^{*}(x)|^{2}}$. Notice that the estimated
value function $\mathds{T}_{k}$ can be positive infinity when value iterations
are not sufficiently executed and we exclude them in the comparison. We
compare iPolicy with the multi-grid method, the single-robot version of [35].
We use $4$ grids with spatial resolutions $\\{0.8,0.4,0.2,0.1\\}$ and compute
the temporal resolutions in the same way as [35] for the multi-grid method.
All other parameters share the same values with those of iPolicy. Figure 2d
quantitatively visualizes RMSEs over computational times for $5$ independently
executed experiments. It is shown that the approximation error of iPolicy
monotonically and continuously decreases in general as more computational time
is consumed, and iPolicy obtains lower approximation errors compared to the
multi-grid method within the allowed computational resources.

###  VII-C Simple car

In this subsection, we first show the optimality results with the simple car
model. The simple car model follows the unicycle dynamic, a well-known
driftless affine system in the configuration space
${}^{2}\times\mathds{S}^{1}$ [2]. The unicycle dynamics is given below:

$\displaystyle\begin{bmatrix}\dot{x}\\\ \dot{y}\\\
\dot{\theta}\end{bmatrix}=\begin{bmatrix}\cos\theta&0\\\ \sin\theta&0\\\
0&1\end{bmatrix}\begin{bmatrix}u_{1}\\\ u_{2}\end{bmatrix},$

where $\theta$ is the steering angle and is a circle $\mathds{S}^{1}$. For
computational simplicity, we redefine $\mathds{S}^{1}$ by
$\theta\in[-\Theta,\Theta]/\sim$, where $\sim$ indicates $-\Theta$ and
$\Theta$ are identical, and by using the Carnot-Carathéodory distance. To
avoid the bias over different coordinates in computing distances, we let the
region of interest be a square of $20\times 20$ and let $\Theta=10$. The
control set of the simple car is given by $U=[-1,1]\times[-1,1]$, and the goal
set is a sub-Riemannian ball of radius $1$ centered at $(0,0,0)$. Figure 3
shows the estimated value function after approximately $13$k seconds of
computations over $5,000$ samples on the plane, where the orientation of the
car is roughly parallel to the orientation at goal
($\theta_{\mathrm{goal}}=0$) with a maximum difference of $10^{\circ}$. The
shape of the level sets around the goal depict that the minimal traveling time
is higher in the transverse direction when compared to that along the
longitudinal direction parallel to the orientation of the goal state, as the
car has a non-zero turn radius and it cannot move in the transverse direction.

Figure 3: The estimated value function obtained by the proposed incremental
algorithm for simple car obtained over $5,000$ samples with orientation
$\theta=0$.

(a) The path of head-in parking

(b) The path of parallel parking

Figure 4: Trajectory of the simple car accomplishing automated parking in the
cluttered environment. Yellow arrows denote the orientation of the car while
red stars denote the center. Goal region is marked by a pink dash circle.

In another simulation, we demonstrate the capability of iPolicy of handling
automated parking, including head-in parking and parallel parking, of the
simple car in the cluttered environment and show anytime property of the
algorithm by visualizing the resulting trajectory under the guidance of the
computed controller. See Figure 4a and Figure 4b. In the first simulation, a
simple car desires to park in a one-way $90^{\circ}$ head-in-only parking lot,
and it starts from $(-6,0,0)$ and needs to safely stop at $(4,7,\pi/2)$. In
the second simulation, we simulate the parallel parking of a simple car on a
two-way street, where the car starts from $(-7,-2,0)$ and needs to park at
$(4,7,\pi/2)$ without causing any collisions. In Figure 4a, iPolicy uses $290$
samples and runs $53.71$ seconds to compute a safe and feasible controller for
head-in parking in Figure 4a. In Figure 4b, iPolicy uses $200$ samples and
runs $22.23$ seconds to command the simple car to safely park in parallel.
Both cases imply the quick feasibility of iPolicy. Together with Theorem V.1
and Figure 2d, where the increasing optimality is verified, the anytime
property of iPolicy is demonstrated. That is, iPolicy can produce a feasible
solution given a short computational time, and can continuously optimize its
solution if more computational time is given.

Figure 5: The estimated value function obtained by iPolicy for Dubins car
system obtained over $7500$ samples. As the Dubins car can’t move backwards,
the value functions are discontinuous and it results in more complex reachable
sets in the sub-Riemannian manifold.

###  VII-D Dubins car

In this subsection, we show Algorithm 1 can handle more complex dynamic
models. In particular, we also show the estimate of the value functions
obtained using iPolicy for the Dubins car whose dynamics also lies in the
${}^{2}\times\mathds{S}^{1}$ space and is described by the same set of
equations as the simple car; however, it can only move forward. Different from
the Dubins car in [39] that can instantly stop, the Dubins car can only move
forward at a constant speed, making its control set a singleton
$U=\\{1\\}\times[-1,1]$. The Dubins car is another example of nonholonomic
system but without a reverse gear or a brake [12, 2]. Moreover, the Dubins car
does not satisfy Assumption VII.1; however, we still apply Algorithm 4 in
implementation and later result will show this quick adaptation does not
affect the The goal set in this case is the sub-Riemannian ball of radius $1$
around the point $(0,0,0)$ and $R=1$ (again the goal is the point where we
reach first inside the ball). In Figure 5, the estimated cost-to-go on a plane
are shown where the orientation of the car is parallel to the orientation at
goal $\theta_{\rm goal}=0$ with a maximum difference of $10^{\circ}$. The
estimate of the value functions show the more complex level sets obtained for
the Dubins car as it can not move backwards. Unlike the simple car, the level
sets are not rotationally symmetric about the origin. Notice that area on the
right-hand side of $x=7.5$ in Figure 5 is mostly empty; this indicates iPolicy
cannot find a solution in this area, mostly because the Dubins car starting
from this area cannot reach the goal region without exiting the region of
interest.

######  Remark VII.1.

Applying iPolicy to larger size environment does not require any additional
modifications. However, at the early stage of computations, smaller size of
samples may not display the desired probabilistic properties and some minor
revisions may be needed for quick feasibility. Specifically, we can revise
$\epsilon_{k}$ s.t. the one-hop neighbor of a sample does not exit the region
of interest; i.e.,
$x+f(x,u)\epsilon_{k}+\rho(\epsilon_{k},d_{k})\mathcal{B}\subseteq\mathcal{X}$
for most $x\in V_{k}$ and $u\in\mathcal{U}$.

##  VIII Conclusions and Future Work

In this paper we presented an algorithm for feedback motion planning of
dynamical systems. Using results from set-valued control theory and sampling-
based algorithms, we presented the iPolicy algorithm which can be used for
feedback motion planning of robotic systems and guarantees asymptotic
optimality of the value functions. Numerical experiments with point mass, a
simple car model and the Dubins car model are presented where the time-to-go
costs are recovered using iPolicy. We discussed an asynchronous update rule of
the value functions for computational efficiency of the algorithms and proved
that it retains asymptotic optimality guarantees.

It was found that majority of the time of the algorithm is used in local
connection and collision checking using the sub-Riemannian metric.
Computational time is a critical bottleneck for the class of algorithms
presented here and needs further research. Our future work includes the
research on speeding up the calculations. Using parallel machine learning
algorithms which can assist in connecting the states sampled during state-
space construction using sampling could potentially help in relaxing the
compute times. Learning-based algorithms [40] are shown to be efficient in
addressing high-dimensional problems, which has the potential to help reduce
computations of planners and makes it more applicable in practice.
Furthermore, use of iPolicy for sub-optimal, trajectory-centric control of
underactuated systems [41, 42] could also be an interesting topic of future
research. Extension to affine or complementarity dynamical systems could be
another interesting direction of research [43, 44, 45].

## References

  * [1] J. Reif, “Complexity of the mover’s problem and generalizations,” in Proceedings of the 20th Annual IEEE Conference on Foundations of Computer Science, pp. 421–427, 1979.
  * [2] S. M. LaValle, Planning algorithms.  Cambridge university press, 2006.
  * [3] S. LaValle, “Motion planning,” Robotics Automation Magazine, IEEE, vol. 18, pp. 79–89, March 2011.
  * [4] S. M. LaValle and J. J. Kuffner, “Randomized kinodynamic planning,” The International Journal of Robotics Research, vol. 20, no. 5, pp. 378–400, 2001\.
  * [5] 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.
  * [6] R. M. Murray and S. S. Sastry, “Nonholonomic motion planning: Steering using sinusoids,” Automatic Control, IEEE Transactions on, vol. 38, no. 5, pp. 700–716, 1993.
  * [7] J.-P. Laumond, S. Sekhavat, and F. Lamiraux, Guidelines in nonholonomic motion planning for mobile robots.  Springer, 1998.
  * [8] Y. Wang, D. K. Jha, and Y. Akemi, “A two-stage RRT path planner for automated parking,” in 2017 13th IEEE Conference on Automation Science and Engineering (CASE), pp. 496–502, IEEE, 2017.
  * [9] E. Schmerling, L. Janson, and M. Pavone, “Optimal sampling-based motion planning under differential constraints: the driftless case,” in 2015 IEEE International Conference on Robotics and Automation (ICRA), pp. 2368–2375, IEEE, 2015.
  * [10] A. Perez, R. Platt, G. Konidaris, L. Kaelbling, and T. Lozano-Perez, “LQR-RRT*: Optimal sampling-based motion planning with automatically derived extension heuristics,” in Robotics and Automation (ICRA), 2012 IEEE International Conference on, pp. 2537–2542, IEEE, 2012.
  * [11] D. J. Webb and J. van den Berg, “Kinodynamic RRT*: Asymptotically optimal motion planning for robots with linear dynamics,” in Robotics and Automation (ICRA), 2013 IEEE International Conference on, pp. 5054–5061, IEEE, 2013.
  * [12] S. Karaman and E. Frazzoli, “Sampling-based optimal motion planning for non-holonomic dynamical systems,” in Robotics and Automation (ICRA), 2013 IEEE International Conference on, pp. 5041–5047, IEEE, 2013.
  * [13] K. Hauser, “Lazy collision checking in asymptotically-optimal motion planning,” in 2015 IEEE international conference on robotics and automation (ICRA), pp. 2951–2957, IEEE, 2015.
  * [14] H. Peng, F. Li, J. Liu, and Z. Ju, “A symplectic instantaneous optimal control for robot trajectory tracking with differential-algebraic equation models,” IEEE Transactions on Industrial Electronics, vol. 67, no. 5, pp. 3819–3829, 2020.
  * [15] P. Cardaliaguet, M. Quincampoix, and P. Saint-Pierre, “Set-valued numerical analysis for optimal control and differential games,” in Stochastic and differential games: theory and numerical methods, pp. 177–247, Birkhäuser Boston Boston, MA, 1999.
  * [16] M. Kalakrishnan, S. Chitta, E. Theodorou, P. Pastor, and S. Schaal, “Stomp: Stochastic trajectory optimization for motion planning,” in 2011 IEEE International Conference on Robotics and Automation, pp. 4569–4574, 2011.
  * [17] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “Chomp: Gradient optimization techniques for efficient motion planning,” in 2009 IEEE International Conference on Robotics and Automation, pp. 489–494, 2009.
  * [18] M. Kelly, “An introduction to trajectory optimization: How to do your own direct collocation,” SIAM Review, vol. 59, no. 4, pp. 849–904, 2017.
  * [19] J. Schulman, Y. Duan, J. Ho, A. Lee, I. Awwal, H. Bradlow, J. Pan, S. Patil, K. Goldberg, and P. Abbeel, “Motion planning with sequential convex optimization and convex collision checking,” The International Journal of Robotics Research, vol. 33, no. 9, pp. 1251–1270, 2014.
  * [20] A. U. Raghunathan, D. K. Jha, and D. Romeres, “PYROBOCOP : Python-based robotic control & optimization package for manipulation and collision avoidance,” CoRR, vol. abs/2106.03220, 2021.
  * [21] X. Wang, J. Liu, Y. Zhang, B. Shi, D. Jiang, and H. Peng, “A unified symplectic pseudospectral method for motion planning and tracking control of 3d underactuated overhead cranes,” International Journal of Robust and Nonlinear Control, vol. 29, no. 7, pp. 2236–2253, 2019.
  * [22] Y. Tassa, T. Erez, and E. Todorov, “Synthesis and stabilization of complex behaviors through online trajectory optimization,” in 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 4906–4913, IEEE, 2012.
  * [23] K. Zhang, Q. Sun, and Y. Shi, “Trajectory tracking control of autonomous ground vehicles using adaptive learning mpc,” IEEE Transactions on Neural Networks and Learning Systems, vol. 32, no. 12, pp. 5554–5564, 2021.
  * [24] J. De Schutter, M. Zanon, and M. Diehl, “Tunempc—a tool for economic tuning of tracking (n) mpc problems,” IEEE Control Systems Letters, vol. 4, no. 4, pp. 910–915, 2020.
  * [25] A. Majumdar and R. Tedrake, “Funnel libraries for real-time robust feedback motion planning,” The International Journal of Robotics Research, vol. 36, no. 8, pp. 947–982, 2017.
  * [26] O. Khatib, “Real-time obstacle avoidance for manipulators and mobile robots,” in Autonomous Robot Vehicles, pp. 396–404, Springer, 1986.
  * [27] J. Borenstein and Y. Koren, “The vector field histogram - fast obstacle avoidance for mobile robots,” IEEE Journal of Robotics and Automation, vol. 7, no. 3, pp. 278–288, 1991.
  * [28] D. E. Koditschek and E. Rimon, “Robot navigation functions on manifolds with boundary,” Advances in applied mathematics, vol. 11, no. 4, pp. 412–442, 1990.
  * [29] A. Orthey, C. Chamzas, and L. E. Kavraki, “Sampling-based motion planning: A comparative review,” Annual Review of Control, Robotics, and Autonomous Systems, vol. 7, no. 1, p. null, 2024.
  * [30] R. Tedrake, I. R. Manchester, M. Tobenkin, and J. W. Roberts, “Lqr-trees: Feedback motion planning via sums-of-squares verification,” The International Journal of Robotics Research, vol. 29, no. 8, pp. 1038–1052, 2010\.
  * [31] R. Tedrake, “LQR-trees: Feedback motion planning on sparse randomized trees,” in Proceedings of Robotics: Science and Systems, (Seattle, USA), June 2009.
  * [32] G. J. Maeda, S. P. Singh, and H. Durrant-Whyte, “A tuned approach to feedback motion planning with rrts under model uncertainty,” in Robotics and Automation (ICRA), 2011 IEEE International Conference on, pp. 2288–2294, IEEE, 2011.
  * [33] V. A. Huynh, S. Karaman, and E. Frazzoli, “An incremental sampling-based algorithm for stochastic optimal control,” in Robotics and Automation (ICRA), 2012 IEEE International Conference on, pp. 2865–2872, IEEE, 2012.
  * [34] D. K. Jha, M. Zhu, and A. Ray, “Game theoretic controller synthesis for multi-robot motion planning-part II: Policy-based algorithms,” IFAC-PapersOnLine, vol. 48, no. 22, pp. 168–173, 2015.
  * [35] G. Zhao and M. Zhu, “Pareto optimal multirobot motion planning,” IEEE Transactions on Automatic Control, vol. 66, no. 9, pp. 3984–3999, 2020.
  * [36] A.-A. Agha-Mohammadi, S. Chakravorty, and N. M. Amato, “Firm: Sampling-based feedback motion-planning under motion uncertainty and imperfect measurements,” The International Journal of Robotics Research, vol. 33, no. 2, pp. 268–304, 2014.
  * [37] E. Schmerling, L. Janson, and M. Pavone, “Optimal sampling-based motion planning under differential constraints: the driftless case,” arXiv preprint arXiv:1403.2483, 2014.
  * [38] E. Mueller, M. Zhu, S. Karaman, and E. Frazzoli, “Anytime computation algorithms for approach-evasion differential games,” arXiv preprints, 2013.  http://arxiv.org/abs/1308.1174.
  * [39] S. Levine and P. Abbeel, “Learning neural network policies with guided policy search under unknown dynamics,” in Advances in Neural Information Processing Systems, pp. 1071–1079, 2014.
  * [40] C. Chi, S. Feng, Y. Du, Z. Xu, E. Cousineau, B. Burchfiel, and S. Song, “Diffusion policy: Visuomotor policy learning via action diffusion,” arXiv preprint arXiv:2303.04137, 2023.
  * [41] A. Majumdar, A. A. Ahmadi, and R. Tedrake, “Control design along trajectories with sums of squares programming,” in 2013 IEEE International Conference on Robotics and Automation, pp. 4054–4061, IEEE, 2013.
  * [42] P. Kolaric, D. K. Jha, A. U. Raghunathan, F. L. Lewis, M. Benosman, D. Romeres, and D. Nikovski, “Local policy optimization for trajectory-centric reinforcement learning,” International Conference on Robotics and Automation (ICRA), 2020.  https://arxiv.org/pdf/2001.08092.
  * [43] Y. Shirai, D. K. Jha, and A. U. Raghunathan, “Covariance steering for uncertain contact-rich systems,” arXiv preprint arXiv:2303.13382, 2023\.
  * [44] Y. Shirai, D. K. Jha, A. U. Raghunathan, and D. Romeres, “Robust pivoting: Exploiting frictional stability using bilevel optimization,” in 2022 International Conference on Robotics and Automation (ICRA), pp. 992–998, 2022\.
  * [45] G. Yang, M. Cai, A. Ahmad, C. Belta, and R. Tron, “Efficient LQR-CBF-RRT*: Safe and optimal motion planning,” arXiv preprint arXiv:2304.00790, 2023\.
  * [46] H. L. Royden and P. Fitzpatrick, Real analysis.  Prentice Hall, 4 ed., 2010.

|  Guoxiang Zhao is currently an Associate Professor in the School of Future
Technology at Shanghai University, Shanghai, China. He received PhD in
Electrical Engineering from Pennsylvania State University in May 2022. He
received a M.S. degree in Mechanical Engineering from Purdue University, West
Lafayette, IN, USA and a B.E. degree from Shanghai Jiao Tong University,
Shanghai, China. He was a Software Engineer from 2022 to 2023 with TuSimple,
Inc. in San Diego, CA, USA. His research interests are in the areas of motion
planning and multi-robot systems.
---|---
|  Devesh K. Jha is currently a Principal Research Scientist at Mitsubishi
Electric Research Laboratories (MERL) in Cambridge, MA, USA. He received PhD
in Mechanical Engineering from Penn State in Decemeber 2016. He received M.S.
degrees in Mechanical Engineering and Mathematics also from Penn State. His
research interests are in the areas of Machine Learning, Robotics and Deep
Learning. He is a recipient of several best paper awards including the Kalman
Best Paper Award 2019 from the American Society of Mechanical Engineers (ASME)
Dynamic Systems and Control Division. He is a senior member of IEEE and an
associate editor of IEEE Robotics and Automation Letters (RA-L).
---|---
|  Yebin Wang (M’10-SM’16) received the B.Eng. degree in Mechatronics
Engineering from Zhejiang University, Hangzhou, China, in 1997, M.Eng. degree
in Control Theory & Control Engineering from Tsinghua University, Beijing,
China, in 2001, and Ph.D. in Electrical Engineering from the University of
Alberta, Edmonton, Canada, in 2008. Dr. Wang has been with Mitsubishi Electric
Research Laboratories in Cambridge, MA, USA, since 2009, and now is a Senior
Principal Research Scientist. From 2001 to 2003 he was a Software Engineer,
Project Manager, and Manager of R&D Dept. in industries, Beijing, China. His
research interests include nonlinear control and estimation, optimal control,
adaptive systems and their applications including mechatronic systems.
---|---
|  Minghui Zhu is an Associate Professor in the School of Electrical
Engineering and Computer Science at the Pennsylvania State University. Prior
to joining Penn State in 2013, he was a postdoctoral associate in the
Laboratory for Information and Decision Systems at the Massachusetts Institute
of Technology. He received Ph.D. in Engineering Science (Mechanical
Engineering) from the University of California, San Diego in 2011. His
research interests lie in distributed control and decision-making of multi-
agent networks with applications in robotic networks, security and the smart
grid. He is the co-author of the book ”Distributed optimization-based control
of multi-agent networks in complex environments” (Springer, 2015). He is a
recipient of the award of Outstanding Graduate Student of Mechanical and
Aerospace Engineering at UCSD in 2011, the Dorothy Quiggle Career Development
Professorship in Engineering at Penn State in 2013, the award of Outstanding
Reviewer of Automatica in 2013 and 2014, and the National Science Foundation
CAREER award in 2019. He is an associate editor of the IEEE Open Journal of
Control Systems, the IET Cyber-systems and Robotics and the Conference
Editorial Board of the IEEE Control Systems Society.
---|---

This section contains preliminary results towards the complete proof of
Theorem V.1. Most notations are borrowed from [15] but we make the following
changes for the consistency throughout this paper. Let $K=\mathcal{X}$ be a
closed set and $C=\mathcal{X}_{\mathrm{goal}}$ be a closed target set of $K$
following the definition in [15]. We rewrite the minimal time function
$\vartheta^{K}_{C}$, the estimated value function $T$ and the the spatial
resolution $h_{p}$ in [15] as $\mathds{T}^{*}$, $\mathds{T}$ and $d_{p}$
respectively. Notice that all superscripts and subscripts of $T$ are kept in
$\mathds{T}$. Denote the domain of $\mathds{T}^{*}$ by
$\mathcal{D}(\mathds{T}^{*})\triangleq\\{x\in K|\mathds{T}^{*}(x)<+\infty\\}$.
The following theorem proves a slightly stronger result compared to Corollary
3.7 in [15], where the perturbation radius can be arbitrarily large.

######  Theorem .1.

Given the convergence in the epigraphic sense, i.e.,
$\mathcal{E}pi(\mathds{T}^{*})=\operatorname*{Lim}_{p\to+\infty}\mathcal{E}pi(\mathds{T}^{\infty}_{p})$,
$\mathds{T}^{\infty}_{p}$ converges pointwise to $\mathds{T}^{*}$, i.e.,
$\forall x\in\mathcal{D}(\mathds{T}^{*})$,

$\displaystyle\mathds{T}^{*}(x)=\lim_{p\to+\infty}\min_{x^{\prime}\in(x+\eta_{p}\mathcal{B}\cap
K_{d_{p}})}\mathds{T}^{\infty}_{p}(x^{\prime}),$

where $\lim_{p\to+\infty}\frac{\eta_{p}}{d_{p}}\in[1,+\infty)$ and
$\eta_{p}\geq d_{p}$.

###### Proof.

Define $\tilde{\mathds{T}}^{\infty}_{p}$ s.t.
$\mathcal{E}pi(\mathds{T}^{\infty}_{p})+\eta_{p}\mathcal{B}=\mathcal{E}pi(\tilde{\mathds{T}}^{\infty}_{p})$.

######  Claim .1.

The upper and lower bound of $\mathds{T}^{\infty}_{p}(x)$ for
$x\in\mathcal{D}(\mathds{T}^{*})$ follows respectively

$\displaystyle\tilde{\mathds{T}}^{\infty}_{p}(x)+\eta_{p}\geq\min_{x^{\prime}\in(x+\eta_{p}\mathcal{B})\cap
K_{d_{p}}}\mathds{T}^{\infty}_{p}(x^{\prime}),$ (30)
$\displaystyle\tilde{\mathds{T}}^{\infty}_{p}(x)\leq\min_{x^{\prime}\in(x+\eta_{p}\mathcal{B})\cap
K_{d_{p}}}\mathds{T}^{\infty}_{p}(x^{\prime}).$ (31)

###### Proof.

For any $x\in\mathcal{D}(\mathds{T}^{*})$ and
$(x,\tilde{\mathds{T}}^{\infty}_{p}(x))\in\mathcal{E}pi(\tilde{\mathds{T}}^{\infty}_{p})$,
there exists
$(x^{\prime},t^{\prime})\in\mathcal{E}pi(\mathds{T}^{\infty}_{p})$ s.t.
$\|(x,\tilde{\mathds{T}}^{\infty}_{p}(x))-(x^{\prime},t^{\prime})\|\leq\eta_{p}$.
This implies $\|x-x^{\prime}\|\leq\eta_{p}$ and
$|\tilde{\mathds{T}}^{\infty}_{p}(x)-t^{\prime}|\leq\eta_{p}$. Therefore,

$\displaystyle\tilde{\mathds{T}}^{\infty}_{p}(x)+\eta_{p}\geq
t^{\prime}\geq\mathds{T}^{\infty}_{p}(x^{\prime})\geq\min_{x^{\prime}\in(x+\eta_{p}\mathcal{B})\cap
K_{d_{p}}}\mathds{T}^{\infty}_{p}(x^{\prime}).$

This completes the proof of (30).

Following the definition of $\tilde{\mathds{T}}^{\infty}_{p}$, we have

$\displaystyle\tilde{\mathds{T}}^{\infty}_{p}(x)=\inf\\{t|(x,t)\in\mathcal{E}pi(\mathds{T}^{\infty}_{p})+\eta_{p}\mathcal{B}\\}$
(32) $\displaystyle=$ $\displaystyle\inf\\{t|\exists x^{\prime}\in
K_{d_{p}},t^{\prime}\geq\mathds{T}^{\infty}_{p}(x^{\prime})\text{ s.t.
}\|(x,t)-(x^{\prime},t^{\prime})\|\leq\eta_{p}\\}.$

Notice that
$\|(x,t)-(x^{\prime},t^{\prime})\|\leq\eta_{p}\Rightarrow\|x-x^{\prime}\|\leq\eta_{p}$.
We can simplify the conditions on $(x,t)$ as

$\displaystyle\tilde{\mathds{T}}^{\infty}_{p}(x)=$
$\displaystyle\inf\\{t|\forall x^{\prime}\in(x+\eta_{p}\mathcal{B})\cap
K_{d_{p}},t^{\prime}\geq\mathds{T}^{\infty}_{p}(x^{\prime})$
$\displaystyle\quad\text{ s.t.
}\|(x,t)-(x^{\prime},t^{\prime})\|\leq\eta_{p}\\}.$

It again follows from (32) that $|t-t^{\prime}|\leq\eta_{p}$. Since
$\tilde{\mathds{T}}^{\infty}_{p}(x)$ searches for an infimum of $t$, taking an
infimum of $t^{\prime}$ would render at the same result but remove
unattainable candidate minimizers. Thus, the following holds by letting
$t^{\prime}=\mathds{T}^{\infty}_{p}(x^{\prime})$:

$\displaystyle\tilde{\mathds{T}}^{\infty}_{p}(x)=\inf\\{t|\forall
x^{\prime}\in(x+\eta_{p}\mathcal{B})\cap K_{d_{p}}$ (33)
$\displaystyle\quad\quad\quad\quad\quad\text{ s.t.
}\|(x,t)-(x^{\prime},\mathds{T}^{\infty}_{p}(x^{\prime}))\|\leq\eta_{p}\\}$
$\displaystyle=$ $\displaystyle\inf\\{t|\forall
x^{\prime}\in(x+d_{p}\mathcal{B})\cap
K_{d_{p}},(x,t)\in(x^{\prime},\mathds{T}^{\infty}_{p}(x^{\prime}))+\eta_{p}\mathcal{B}\\}$
$\displaystyle\leq$
$\displaystyle\min_{x^{\prime}\in(x+\eta_{p}\mathcal{B})\cap
K_{d_{p}}}\mathds{T}^{\infty}_{p}(x^{\prime}).$

where the last inequality is a direct result of expansion of
$d_{p}\mathcal{B}$ on $(x^{\prime},\mathds{T}^{\infty}_{p}(x^{\prime}))$. This
completes the proof of (31). ∎

We proceed to show that $\mathds{T}^{*}(x)$ is bounded both from below and
above by $\mathds{T}^{\infty}_{p}(x)$. The following claim shows
$\mathds{T}^{*}(x)$ is lower bounded by $\mathds{T}^{\infty}_{p}(x)$.

######  Claim .2.

It holds that for any $x\in\mathcal{D}(\mathds{T}^{*})$,

$\displaystyle\mathds{T}^{*}(x)\geq\min_{x^{\prime}\in(x+\eta_{p}\mathcal{B})\cap
K_{d_{p}}}\mathds{T}^{\infty}_{p}(x^{\prime})-\eta_{p}.$

###### Proof.

Following the argument in the proof of Corollary 3.7 in [15], we have
$\mathcal{E}pi(\mathds{T}^{*})\subseteq\mathcal{E}pi(\tilde{\mathds{T}}^{\infty}_{p})$
and it renders at $\tilde{\mathds{T}}^{\infty}_{p}(x)\leq\mathds{T}^{*}(x)$.
Then it follows from (30) in Claim .1 that
$\min_{x^{\prime}\in(x+\eta_{p}\mathcal{B})\cap
K_{d_{p}}}\mathds{T}^{\infty}_{p}(x^{\prime})\leq\tilde{\mathds{T}}^{\infty}_{p}(x)+\eta_{p}\leq\mathds{T}^{*}(x)+\eta_{p}$.
This completes the proof. ∎

The following claim shows the $\mathds{T}^{*}(x)$ is upper bounded by the
limit of $\mathds{T}^{\infty}_{p}(x)$.

######  Claim .3.

For any $x\in\mathcal{D}(\mathds{T}^{*})$, it holds that

$\displaystyle\mathds{T}^{*}(x)\leq\liminf_{p\to+\infty}\min_{x^{\prime}\in(x+\eta_{p}\mathcal{B})\cap
K_{d_{p}}}\mathds{T}^{\infty}_{p}(x^{\prime}).$

###### Proof.

Following the argument in the proof of Corollary 3.7 in [15], we have
$\operatorname*{Limsup}_{p\to+\infty}\mathcal{E}pi(\tilde{\mathds{T}}^{\infty}_{p})\subseteq\mathcal{E}pi(\mathds{T}^{*})$.
By the definition of $\operatorname*{Limsup}$, we have
$\forall(x,t)\in\operatorname*{Limsup}_{p\to+\infty}\mathcal{E}pi(\tilde{\mathds{T}}^{\infty}_{p})$,
$\liminf_{p\to+\infty}\operatorname{dist}((x,t),\mathcal{E}pi(\tilde{\mathds{T}}^{\infty}_{p}))=0$.
In other words, there exists a sequence
$\\{(x_{p},t_{p})\in\mathcal{E}pi(\tilde{\mathds{T}}^{\infty}_{p})\\}$ s.t.
$\liminf_{p\to+\infty}\operatorname{dist}((x,t),(x_{p},t_{p}))=0$.

Fix $x\in\mathcal{D}(\mathds{T}^{*})$ and consider a sequence
$\\{(x_{p},t_{p})\\}$ s.t.
$x_{p}=x,t_{p}=\tilde{\mathds{T}}^{\infty}_{p}(x_{p})$. It follows from Claim
.2 that $t_{p}\leq\mathds{T}^{*}(x)<+\infty$, where the last inequality
follows from the definition of $\mathcal{D}(\mathds{T}^{*})$. Therefore,
$\\{(x_{p},t_{p})\\}$ is bounded. Then it follows from the supplementary
statement about the definition of $\operatorname*{Limsup}$ on page 236 in [15]
that $\mathrm{Limsup}_{p\to+\infty}\\{(x_{p},t_{p})\\}\neq\emptyset$. Since
$(x_{p},t_{p})\in\mathcal{E}pi(\tilde{\mathds{T}}^{\infty}_{p})$, we have

$\displaystyle\operatorname*{Limsup}_{p\to+\infty}\\{(x_{p},t_{p})\\}\subseteq\operatorname*{Limsup}_{p\to+\infty}\mathcal{E}pi(\tilde{\mathds{T}}^{\infty}_{p})\subseteq\mathcal{E}pi(\mathds{T}^{*}).$

Let $(x^{*},t^{*})\in\operatorname*{Limsup}_{p\to+\infty}\\{(x_{p},t_{p})\\}$
s.t. $t^{*}\leq
t,\forall(x,t)\in\operatorname*{Limsup}_{p\to+\infty}\\{(x_{p},t_{p})\\}$.
Therefore, $t^{*}\geq\mathds{T}^{*}(x^{*})=\mathds{T}^{*}(x)$. Then we have
$\liminf_{p\to+\infty}\tilde{\mathds{T}}^{\infty}_{p}(x)=t^{*}\geq\mathds{T}^{*}(x)$.
It follows from (31) in Claim .1 that

$\displaystyle\mathds{T}^{*}(x)\leq\liminf_{p\to+\infty}\min_{x^{\prime}\in(x+\eta_{p}\mathcal{B})\cap
K_{d_{p}}}\mathds{T}^{\infty}_{p}(x^{\prime}).$

This completes the proof. ∎

Combining Claims .2 and .3 renders at

$\displaystyle\min_{x^{\prime}\in(x+\eta_{p}\mathcal{B})\cap
K_{d_{p}}}\mathds{T}^{\infty}_{p}(x^{\prime})-\eta_{p}\leq\mathds{T}^{*}(x)$
$\displaystyle\leq$
$\displaystyle\liminf_{p\to+\infty}\min_{x^{\prime}\in(x+\eta_{p}\mathcal{B})\cap
K_{d_{p}}}\mathds{T}^{\infty}_{p}(x^{\prime}).$

By taking $p\to+\infty$ on the left side, both $\eta_{p}$ and $d_{p}$
disappear at the same rate and we complete the proof. ∎

We proceed to show the convergence of the value function estimate on each
point $x\in K_{d_{p}}$. Different from Theorem .1, we remove the perturbation
$x^{\prime}\in(x+\eta_{p}\mathcal{B})\cap K_{d_{p}}$ in the theorem statement
and directly analyze the values $\mathds{T}_{p}^{\infty}(x),\forall x\in
K_{d_{p}}$. Towards this end, we first prove the following lemma quantifying
the lowerbound of $\mathds{T}^{*}(x),\forall x\in K_{d_{p}}$.

######  Lemma .1.

Given all conditions in Theorem .1, the following relationship holds:

$\displaystyle\mathds{T}^{*}(x)\geq\mathds{T}^{\infty}_{p}(x)-d_{p},\forall
x\in K_{d_{p}}.$

###### Proof.

It follows from Proposition 2.21 in [15] that
$(Viab_{F}(\mathcal{H})+d_{p}\mathcal{B})\cap\mathcal{H}_{d_{p}}$ is a
discrete viability domain for $\Gamma_{\epsilon_{p},d_{p}}$. Thus, it holds
that $(Viab_{F}(\mathcal{H})+d_{p}\mathcal{B})\cap\mathcal{H}_{d_{p}}\subseteq
Viab_{\Gamma_{\epsilon_{p},d_{p}}}(\mathcal{H}_{d_{p}})$. It follows from
Theorem 3.2 and Lemma 3.6 that we can replace the discrete viability kernals
with epigraphs of traveling time functions; i.e.,

$\displaystyle(\mathcal{E}pi(\mathds{T}^{*})+d_{p}\mathcal{B})\cap\mathcal{H}_{d_{p}}\subseteq\mathcal{E}pi(\mathds{T}^{\infty}_{p}).$

Fix $x\in K_{d_{p}}$ and there exists $t\in\mathds{T}^{*}(x)+d_{p}\mathcal{B}$
s.t. $(x,t)\in\mathcal{E}pi(\mathds{T}^{\infty}_{p})$; that is,
$t\geq\mathds{T}^{\infty}_{p}(x)$. This implies
$\mathds{T}^{*}(x)+d_{p}\geq\mathds{T}^{\infty}_{p}(x)$. This completes the
proof. ∎

The following corollary characterized the pointwise convergence of the value
function estimate.

######  Corollary .1.

Given the conditions in Theorem .1 and $x\in
K_{d_{\bar{p}}}\cap\mathcal{D}(\mathds{T}^{*})$ for some $\bar{p}\geq 1$. Then
the following holds for every $x\in
K_{d_{\bar{p}}}\cap\mathcal{D}(\mathds{T}^{*})$:

$\displaystyle\mathds{T}^{*}(x)=\lim_{p\to+\infty}\mathds{T}^{\infty}_{p}(x).$

###### Proof.

The proof generally follows the arguments towards Theorem .1. For
$\tilde{T}_{p}^{\infty}$ defined as
$\mathcal{E}pi(\tilde{T}_{p}^{\infty})=\mathcal{E}pi(\mathds{T}_{p}^{\infty})+\eta_{p}\mathcal{B}$,
$\tilde{T}_{p}^{\infty}(x)\leq\mathds{T}_{p}^{\infty}(x)$. Then it follows
from Claim .3 that
$\liminf_{p\to+\infty}\mathds{T}_{p}^{\infty}(x)\geq\liminf_{p\to+\infty}\tilde{T}_{p}^{\infty}(x)\geq\mathds{T}^{*}(x)$.
It follows from Lemma .1 that
$\mathds{T}^{\infty}_{p}(x)\leq\mathds{T}^{*}(x)+d_{p}$. Taking limits
$p\to+\infty$, $d_{p}\to 0$ completes the proof. ∎

Notice that Theorem .1 only characterizes the pointwise convergence over
$\mathcal{D}(\mathds{T}^{*})$. On $K\setminus\mathcal{D}(\mathds{T}^{*})$, the
value of $\mathds{T}^{*}$ is not well defined and the convergence cannot be
analyzed. The Kruzhkov transform can address this issue and let
$\varTheta^{*}_{k}(x)=1,\forall x\in K\setminus\mathcal{D}(\mathds{T}^{*})$.
Without proof, the theorem is stated below.

######  Lemma .2.

Given the convergence in the epigraphic sense, i.e.,
$\mathcal{E}pi(\mathds{T}^{*})=\operatorname*{Lim}_{k\to+\infty}\mathcal{E}pi(\mathds{T}^{*}_{k})$,
$\varTheta^{*}_{k}$ converges pointwise to $\varTheta^{*}$, i.e., $\forall
x\in\mathcal{X}$,

$\displaystyle\varTheta^{*}(x)=\lim_{k\to+\infty}\min_{x^{\prime}\in(x+\ell_{k}\mathcal{B})\cap
V_{k}}\varTheta^{*}_{k}(x^{\prime}),$

where $\lim_{k\to+\infty}\frac{\ell_{k}}{d_{k}}\in[1,+\infty)$ and
$\ell_{k}\geq d_{k}$.

The following corollary shows the uniform convergence for the transformed
value function estimates.

######  Corollary .2.

The transformed value function estimate $\\{\varTheta^{*}_{k}\\}$ converges
uniformly to $\varTheta^{*}$ almost everywhere on $\mathcal{X}$; i.e.,

$\displaystyle\lim_{k\to+\infty}\|\varTheta^{*}-\min_{x^{\prime}\in(x+\ell_{k}\mathcal{B})\cap
V_{k}}\varTheta^{*}_{k}(x^{\prime})\|_{\mathcal{X}\setminus\mathcal{Z}}=0,$

where $\mathcal{Z}$ has sufficiently small measure,
$\lim_{k\to+\infty}\frac{\ell_{k}}{d_{k}}\in[1,+\infty)$ and $\ell_{k}\geq
d_{k}$.

###### Proof.

The proof leverages Egoroff’s theorem [46]. Since $\mathcal{X}$ is compact,
$\mathcal{X}$ has finite measure. It follows from Theorem .1 that
$\\{\varTheta^{*}_{k}\\}$ are measurable functions converging on $\mathcal{X}$
pointwise to $\varTheta^{*}$. Then $\\{\varTheta^{*}_{k}\\}$ converges
uniformly to $\varTheta^{*}$ almost everywhere to
$\mathcal{X}\setminus\mathcal{Z}$ by the Egoroff’s theorem, where
$\mathcal{Z}$ has sufficiently small measure. This completes the proof of the
first relation. ∎