iPolicy: Incremental Policy Algorithms for Feedback Motion Planning

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 map** 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. $\mathcal{X}$ and $\mathcal{U}$ are non-empty and compact;

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

3. $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. $\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,

$$\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

$$(\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

$$F_{k}(x)\triangleq (x+\epsilon_{k}\bigcup_{u\in\mathcal{U}}f(x,u)+\rho(d_{k},\epsilon_{k})\mathcal{B})$$ (3)

$$\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:

$$(\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. $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. $\epsilon_{k}$ and $d_{k}/\epsilon_{k}$ monotonically decreases to $0$ as $k\to+\infty$;

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

4. $\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

$$\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:

$$\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:

$$\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

$$\|\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)

$$\leq \|\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

$$\|\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

$$\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.,

$$\|\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,

$$|\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:

$$|\mathfrak{T}_{k}(\varTheta_{k})(x)-\varTheta_{k}^{*}(x)|$$ (8)

$$\leq \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}}$$

$$\leq \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:

  $$|\mathfrak{T}_{k}(\varTheta_{k})(x)-\mathfrak{T}_{k}(\varTheta^{*}_{k})(x)|$$ (9)

  $$= \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{|}$$

  $$= \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{|}$$

  $$\leq \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

$$|\mathfrak{T}_{k}(\varTheta_{k})(x)-\mathfrak{T}_{k}(\varTheta^{*}_{k})(x)|$$ (10)

$$= \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{|}$$

$$\leq \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

$$\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 (4) in Lemma VI.2, (10) renders with probability one at

$$\|\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)

$$\leq \|\min_{x^{\prime}\in(x+\rho(d_{k},\epsilon_{k})\mathcal{B})\cap V_{k}}\varTheta_{k}(x^{\prime})$$

$$\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 (4) again and (11) renders at

$$\|\min_{x^{\prime}\in(x+\rho(d_{k},\epsilon_{k})\mathcal{B})\cap V_{k}}\varTheta_{k}(x^{\prime})$$ (12)

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

$$\leq \|\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 (4) and (12) that the following holds with probability one:

$$\|\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}}$$

$$\leq \|\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

$$\|\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$$

$$\leq \|\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$$

$$\leq \|\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:

$$|\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

$$|\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

$$\|\varTheta^{1}_{k}-\varTheta^{*}_{k}\|_{V_{k}}$$ (14)

$$\leq \beta_{k}\|\min_{x^{\prime}\in(x+\rho(d_{k},\epsilon_{k})\mathcal{B})\cap V_{k}}\varTheta^{0}_{k}(x^{\prime})$$

$$-\min_{x^{\prime}\in(x+\rho(d_{k},\epsilon_{k})\mathcal{B})\cap V_{k}}\varTheta^{*}_{k}(x^{\prime})\|_{\mathcal{X}\setminus\mathcal{Z}}$$

$$\leq \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}}$$

$$= \beta_{k}\|\tilde{\varTheta}_{k}^{0,-}-\tilde{\varTheta}_{k}^{*,-}\|_{\mathcal{X}\setminus\mathcal{Z}}.$$

The last inequality follows from (4) 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

$$\|\tilde{\varTheta}_{k}^{0,-}-\tilde{\varTheta}_{k}^{*,-}\|_{\mathcal{X}\setminus\mathcal{Z}}$$ (15)

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

$$\leq \|\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

$$\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)

$$= \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

$$\|\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}\setminus(x_{k}+d_{k-1}\mathcal{B})}+c_{k}$$ (17)

$$\leq \|\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):

$$\|\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

$$\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

$$\|\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

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

$$\leq \|\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:

$$\|\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:

$$|\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

$$|\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}$$

$$\leq \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}$$

$$\leq \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

$$\|\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)

$$= \|\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}$$

$$\leq \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:

$$|\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)

$$\leq |\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

$$|\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

$$|\varTheta^{m_{\hat{k}}}_{\hat{k}}(x)-\varTheta^{*}_{\hat{k}}(x)|\leq \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)

$$= \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}$,

$$|\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

$$|\varTheta^{m_{\bar{k}+P}}_{\bar{k}+P}(x)-\varTheta^{*}_{\bar{k}+P}(x)|$$ (26)

$$\leq \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

$$\mathfrak{A}_{\bar{k}+P}=\|\varTheta^{m_{\bar{k}+P}}_{\hat{k}+P}(x)-\varTheta^{*}_{\bar{k}+P}(x)\|_{V_{\bar{k}+P}}$$ (27)

$$\leq \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:

$$\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

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

$$\leq \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

$$\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

$$\mathfrak{A}_{\breve{k}(\tau)}\leq \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

$$\mathfrak{A}_{\breve{k}((\zeta+1)T)}$$ (29)

$$\leq \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})}$$

$$\leq \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

$$\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}}$$

$$= \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}}$$

$$\leq \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. Can iPolicy recover the optimal value function for systems with differential constraints?

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

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:

$$\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].

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, Bei**g, 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, Bei**g, 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}^{*})$,

$$\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

$$\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)

$$\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,

$$\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

$$\tilde{\mathds{T}}^{\infty}_{p}(x)=\inf\{t|(x,t)\in\mathcal{E}pi(\mathds{T}^{\infty}_{p})+\eta_{p}\mathcal{B}\}$$ (32)

$$= \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

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

$$\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})$:

$$\tilde{\mathds{T}}^{\infty}_{p}(x)=\inf\{t|\forall x^{\prime}\in(x+\eta_{p}\mathcal{B})\cap K_{d_{p}}$$ (33)

$$\quad\quad\quad\quad\quad\text{ s.t. }\|(x,t)-(x^{\prime},\mathds{T}^{\infty}_{p}(x^{\prime}))\|\leq\eta_{p}\}$$

$$= \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}\}$$

$$\leq \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}^{*})$,

$$\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

$$\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

$$\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

$$\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

$$\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)$$

$$\leq \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:

$$\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.,

$$(\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}^{*})$:

$$\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}$,

$$\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.,

$$\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. ∎