Reachability-Based Safety Guarantees using Efficient Initializations

TL;DR

This paper introduces a warm-start approach for Hamilton-Jacobi-Isaacs reachability analysis, significantly reducing computation time while maintaining safety guarantees, especially useful for dynamic and uncertain autonomous systems.

Contribution

The paper proposes a novel warm-start method for reachability analysis that guarantees conservative solutions and accelerates convergence compared to standard techniques.

Findings

Warm-starting results in guaranteed conservative solutions.

The method achieves up to 6.2 times faster computation than discounted reachability.

Demonstrated effectiveness on a 10D quadcopter model with changing conditions.

Abstract

Hamilton-Jacobi-Isaacs (HJI) reachability analysis is a powerful tool for analyzing the safety of autonomous systems. This analysis is computationally intensive and typically performed offline. Online, however, the autonomous system may experience changes in system dynamics, external disturbances, and/or the surrounding environment, requiring updated safety guarantees. Rather than restarting the safety analysis, we propose a method of "warm-start" reachability, which uses a user-defined initialization (typically the previously computed solution). By starting with an HJI function that is closer to the solution than the standard initialization, convergence may take fewer iterations. In this paper we prove that warm-starting will result in guaranteed conservative solutions by over-approximating the states that must be avoided to maintain safety. We additionally prove that for many common problem formulations, warm-starting will result in exact solutions.We demonstrate our method on several illustrative examples with a double integrator, and also on a more practical example with a 10D quadcopter model that experiences changes in mass and disturbances and must update its safety guarantees accordingly. We compare our approach to standard reachability and a recently proposed "discounted" reachability method, and find for our examples that warm-starting is 1.6 times faster than standard and 6.2 times faster than (untuned) discounted reachability.

Tables

Table 1: Runtime Analysis for Reachability Methods
	Standard	Warm-Start	Discounted
a) Increasing $ℒ$ (exact)	6.4s, 115 steps	6.0s, 109 steps	12.5s, 231 steps
b) Decreasing $ℒ$ (conserv)	6.0s 110 steps	9.3s, 169 steps	21.2s, 385 steps
d) Decreasing $𝒰$ (exact)	6.5s, 124 steps	6.0s, 111 steps	20.3s, 374 steps
e) Increasing $𝒰$ (conserv)	6.8s, 124 steps	6.1s, 111 steps	20.5s, 374 steps
c) Increasing $𝒟$ (exact)	21.4s, 311 steps	7.7s, 112 steps	13.3s, 195 steps
d) Decreasing $𝒟$ (conserv)	6.0s, 110 steps	11.7s, 213 steps	19.0s, 346 steps
e) 10D quad increasing m, $𝒟$ (exact)	3.7hr, 86 steps	2.8hr, 65 steps	>18 hr, >401 steps
f) 10D quad decreasing m, $𝒟$ (conserv)	.68hr, 50 steps	.67hr, 48 steps	1.12hr, 82 steps

Equations

$$\dot{x}=f(x,u,d),u\in \mathcal{U},d\in \mathcal{D}.$$

$$\dot{x}=\left[\begin{array}{c}\dot{p}\\ \dot{v}\end{array}\right]=\left[\begin{array}{c}v+d\\ ub\end{array}\right],$$

$$J(x,t,u(\cdot ),d(\cdot ))=\underset{\tau \in [t,0]}{\min }l({\xi }_{x,t}^{u,d}(\tau )),t\le 0.$$

$$V(x,t)=\inf_{\gamma[u(\cdot)](\cdot)}\sup_{u(\cdot)}\Big{\{}J\Big{(}x,t,u(\cdot),\gamma[u(\cdot)](\cdot)\Big{)}\Big{\}}.$$

$$\displaystyle D_{t}V(x,t)+H\Big{(}V(x,t),f(x,u,d)\Big{)}=0.$$

$$\displaystyle H\Big{(}V(x,t),f(x,u,d)\Big{)}=$$

$$\underset{u}{\max }\underset{d}{\min }$$

$$\displaystyle\min\Big{\{}D_{t}V(x,t)+H\Big{(}V(x,t),$$

$$\displaystyle l(x)-V(x,t)\Big{\}}=0.$$

$$\displaystyle V(x,t)=\max_{u(\cdot)}\min_{d(\cdot)}\min\Big{\{}$$

$$\displaystyle V\Big{(}\xi_{x,t}^{u,d}(t+dt),t+dt\Big{)}\Big{\}}.$$

$$u^{\ast }=\arg \underset{u}{\max }\underset{d}{\min }⟨\nabla V^{\ast }(x),f(x,u,d)⟩.$$

$$\displaystyle V(x,t)=\max_{u(\cdot)}\min_{d(\cdot)}\min\Big{\{}$$

$$\displaystyle\exp{(\lambda\cdot dt)}V\Big{(}\xi_{x,t}^{u,d}(t+dt),t+dt\Big{)}\Big{\}},$$

$$\begin{split}V_{l}(x,t)&=\max_{u(\cdot)}\min_{d(\cdot)}J_{l}(x,t,u(\cdot),d(\cdot))\\ &=\max_{u(\cdot)}\min_{d(\cdot)}\min\Big{\{}\inf_{\tau\in[t,0)}l(\xi_{x,t}^{u,d}(\tau)),V_{l}(\xi_{x,t}^{u,d}(0),0)\Big{\}},\\ &=\max_{u(\cdot)}\min_{d(\cdot)}\min\Big{\{}\inf_{\tau\in[t,0)}l(\xi_{x,t}^{u,d}(\tau)),l(\xi_{x,t}^{u,d}(0))\Big{\}},\end{split}$$

$$\begin{split}0=\min\Big{\{}&D_{t}V_{l}(x,t)+H_{l}\Big{(}V_{l}(x,t),f(x,u,d)\Big{)},\\ &l(x)-V_{l}(x,t)\Big{\}},\\ H_{l}(V_{l}(x,t)&,f(x,u,d))=\\ &\max_{u}\min_{d}<\nabla V_{l}(x,t),f(x,u,d)>,\\ V_{l}(x,0)&=l(x,0).\end{split}$$

$$J_{k}(x,t,u(\cdot),d(\cdot))=\min\Big{\{}\inf_{\tau\in[t,0)}l(\xi_{x,t}^{u,d}(\tau)),k(\xi_{x,t}^{u,d}(0))\Big{\}}$$

$$\begin{split}V_{k}(x,t)&=\max_{u(\cdot)}\min_{d(\cdot)}J_{k}(x,t,u(\cdot),d(\cdot))\\ &=\max_{u(\cdot)}\min_{d(\cdot)}\min\Big{\{}\inf_{\tau\in[t,0)}l(\xi_{x,t}^{u,d}(\tau)),k(\xi_{x,t}^{u,d}(0))\Big{\}}.\end{split}$$

$$\forall x,t<0,V_k(x,t)\le V_l(x,t)$$

$$V_k(x,t)$$

$$\displaystyle\leq\max_{u(\cdot)}\min_{d(\cdot)}\min\Big{\{}\inf_{\tau\in[t,0)}l(\xi_{x,t}^{u,d}(\tau)),l(\xi_{x,t}^{u,d}(0))\Big{\}},$$

$$=V_l(x,t).$$

$$V_k(x,t)$$

$$=\min \{l({\xi }_{x,t}^{u,d}(0^{-})),k({\xi }_{x,t}^{u,d}(0)\}$$

$$=l({\xi }_{x,t}^{u,d}(0^{-}))=V_l(x,t).$$

$$V_k^{\ast }(x)=V_l^{\ast }(x)$$

$$V_k(x,t)\ge V_l^{\ast }(x)\forall x,t<0$$

$$\begin{split}V_{k^{\prime}}(x,t)&=\max_{u(\cdot)}\min_{d(\cdot)}\min\Big{\{}\inf_{\tau\in[t,0)}l(\xi_{x,t}^{u,d}(\tau)),V_{k^{\prime}}(\xi_{x,t}^{u,d}(0),0)\Big{\}},\\ &=\max_{u(\cdot)}\min_{d(\cdot)}\min\Big{\{}\inf_{\tau\in[t,0)}l(\xi_{x,t}^{u,d}(\tau)),k^{\prime}(\xi_{x,t}^{u,d}(0))\Big{\}}.\end{split}$$

$$\begin{array}{rl}{V}_k^{\ast }(x)& \le V_l^{\ast }(x)\text{(}Theorem\text{ref}thm:allk)\\ V_k^{\ast }(x)& \ge V_l^{\ast }(x)\text{(}Lemma\text{ref}lem:kgreaterV)\\ \Rightarrow V_k^{\ast }(x)& =V_l^{\ast }(x)\end{array}\text{vspace}-.5em$$

$$\displaystyle\begin{array}[]{c}\left[\begin{array}[]{c}\dot{p}_{x}\\ \dot{v}_{x}\\ \dot{\theta}_{x}\\ \dot{\omega}_{x}\\ \dot{p}_{y}\\ \dot{v}_{y}\\ \dot{\theta}_{y}\\ \dot{\omega}_{y}\\ \dot{p}_{z}\\ \dot{v}_{z}\end{array}\right]=\left[\begin{array}[]{c}v_{x}+d_{x}\\ g\tan\theta_{x}\\ -d_{1}\theta_{x}+\omega_{x}\\ -d_{0}\theta_{x}+n_{0}S_{x}\\ v_{y}+d_{y}\\ g\tan\theta_{y}\\ -d_{1}\theta_{y}+\omega_{y}\\ -d_{0}\theta_{y}+n_{0}S_{y}\\ v_{z}+d_{z}\\ (k_{T}/m)T_{z}-g\end{array}\right].\end{array}$$

$$\underset{t\to -\infty }{\lim }{V}_k(x,t)=V_{l^{\prime }}^{\ast }(x)$$

$$\begin{array}{rl}{V}_{l^{\prime }}(x,t)& =\underset{u(\cdot )}{\max }\underset{d(\cdot )}{\min }\underset{\tau \in [t,0]}{\inf }{l}^{\prime }({\xi }_{x,t}^{u,d}(\tau ))\\ & \le \underset{u(\cdot )}{\max }\underset{d(\cdot )}{\min }\underset{\tau \in [t,0]}{\inf }l({\xi }_{x,t}^{u,d}(\tau ))=V_l(x,t)\end{array}$$

$$\underset{t\to -\infty }{\lim }{V}_k(x,0)=V_{l^{\prime }}^{\ast }(x)$$

$$\begin{split}V_{l^{\prime}}(x,t)&=\max_{u\in\mathcal{U}^{\prime}}\min_{d\in\mathcal{D}}\min\Big{\{}\inf_{\tau\in[t,0)}l(\xi_{x,t}^{u,d}(\tau)),l(\xi_{x,t}^{u,d}(0))\Big{\}}\\ &\leq\max_{u\in\mathcal{U}}\min_{d\in\mathcal{D}}\min\Big{\{}\inf_{\tau\in[t,0)}l(\xi_{x,t}^{u,d}(\tau)),l(\xi_{x,t}^{u,d}(0))\Big{\}}\\ &=V_{l}(x,t)\end{split}$$

Full text

In this paper we prove that warm-starting will result in guaranteed conservative solutions by over-approximating the states that must be avoided to maintain safety. We additionally prove that for many common problem formulations, warm-starting will result in exact solutions. We demonstrate our method on several illustrative examples with a double integrator, and also on a more practical example with a 10D quadcopter model that experiences changes in mass and disturbances and must update its safety guarantees accordingly. We compare our approach to standard reachability and a recently proposed “discounted” reachability method, and find for our examples that warm-starting is $1.6$ times faster than standard and $6.2$ times faster than (untuned) discounted reachability.

††This research is supported by an NSF CAREER award, Sylvia Herbert’s NSF GRFP, NSF’s CPS FORCES and VeHICaL projects, the UC-Philippine-California Advanced Research Institute, the ONR MURI Embedded Humans, the DARPA Assured Autonomy program, and the SRC CONIX Center.

All authors are with the Department of Electrical Engineering and Computer Sciences at UC Berkeley. Contact info: {sylvia.herbert, shromona.ghosh, somil, tomlin}@berkeley.edu

I Introduction

As humanity increasingly relies on autonomous systems, ensuring provable safety guarantees and controllers for these systems is vital. To achieve safety for nonlinear systems, tools such as Hamilton-Jacobi-Isaacs (HJI) reachability analysis can provide both a guarantee and a corresponding control input [1, 2]. Applications include collision avoidance [2, 3], safe tracking of online motion planners [4, 5], stormwater management [6], and administering anesthesia [7]. HJI reachability analysis is based on assumptions about system dynamics, external disturbances, and the surrounding environment. However in reality the dynamics, the disturbance bounds, or the environment may differ from the assumptions. In these situations the safety analysis must be adapted.

Unfortunately, performing HJI reachability analysis is computationally intensive for large systems and cannot be computed efficiently as new information is acquired, thus and area of current research interest is developing frameworks for efficiently updating the computation as new information is acquired. There are some methods for speeding up this computation using decomposition [8], and there are other efficient approaches that require simplified problem formulations and/or dynamics [9, 10, 11, 12, 13, 14, 15]. The methods in [16, 17, 18, 9, 19], can handle more complex dynamics, but may be less scalable or unable to represent complex sets. Efficient reachability analysis remains challenging for general system dynamics and problem setups.

Warm-starting in the optimization community involves using a initialization that acts as a “best guess” of the solution, and therefore may converge in fewer iterations (if convergence can be achieved). Recent work applied this warm-starting idea to create a “discounted reachability” formulation for infinite-time horizon problems [20, 21]. By using a discount factor, this formulation guarantees convergence regardless of the initialization. However, due to this discount factor convergence rates can be very slow, and in practice the analysis may not converge numerically when convergence thresholds are too tight, or may converge incorrectly when convergence thresholds are too lenient. In addition, parameter tuning of the discount factor can be time-intensive. These issues reduce the computational benefit of warm-start reachability.

Until now there were no guarantees of convergence for warm-starting HJ reachability without using a discount factor. In this paper we prove that warm-start reachability with no discount factor will in general result in guaranteed conservative safety analyses and controllers (i.e. the analysis over-approximates the set of states that are unsafe to enter). Moreover, if the initialization is over-optimistic and therefore dangerous (i.e. the initialization underestimates the set of states that are unsafe to enter), we prove that warm-starting is guaranteed to converge exactly to the true solution (here we use “exact” to mean numerically convergent [22]).

In addition to these proofs, we provide several common problem classes for which we can prove this exact convergence. We demonstrate these results on an illustrative example with a double integrator, and a more practical example of a realistic 10D quadcopter model that experiences changes in mass and disturbances and must update its safety guarantees accordingly. In these examples warm-start reachability is $1.6$ times faster than standard reachablity and $6.2$ times faster than (untuned) discounted reachability formulation.

II Problem Formulation

Consider an autonomous agent in an environment in the presence of external disturbance. This environment contains a target set $\mathcal{L}$ that is meaningful to the agent: it can be either a set of goal states, or a set of unsafe states. HJI reachability seeks to find the set of initial states for which the system acting optimally and under worst-case disturbances will end up in the target set $\mathcal{L}$ either at a particular time (backward reachable set, or BRS) or within a time horizon (backward reachable tube, or BRT). Optimal behavior of the system depends on the nature of the target set and can be formulated as a game: for a goal set, the control will seek to minimize distance to the goal whereas the worst-case disturbance will maximize distance to the goal. For an unsafe set, the control will maximize and the disturbance will minimize. Both cases can be solved using HJI reachability analysis. Note that there are reach-avoid problems that seek to reach a goal set while avoiding unsafe sets. There are also problems that use forward reachable sets and tubes. More information can be found in [1].

The theory in this paper applies to BRTs with infinite-time horizons. Typically this scenario is more interesting in the avoid case (where the system seeks to avoid an unsafe set of states forever), and will therefore be the focus of this paper. In this section we define the agent’s dynamics and formally introduce HJI reachability analysis.

II-A Dynamic System Model

We assume that the autonomous system (i.e. agent) has initial state $x\in\mathbb{R}^{n}$ and initial time $t$, and evolves according to the ordinary differential equation (ODE):

[TABLE]

Here the system has a control $u$ and disturbance $d$. We assume that these inputs are drawn from compact sets ($\mathcal{U}$, $\mathcal{D}$), and their signals over time ($u(\cdot)$, $d(\cdot)$) are drawn from the set of measurable functions $\mathbb{U}:[t,0]\rightarrow\mathcal{U}$ and $\mathbb{D}:[t,0]\rightarrow\mathcal{D}$.

We assume that the flow field $f:\mathbb{R}^{n}\times\mathcal{U}\times\mathcal{D}\rightarrow\mathbb{R}^{n}$ is uniformly continuous in time and Lipschitz continuous in $x$ for fixed $u$ and $d$. Under these assumption there exists a unique solution of these system dynamics for a given $u(\cdot),d(\cdot)$ [23], providing trajectories of the system: $\xi(\tau;x,t,u(\cdot),d(\cdot)).$ This notation can be read as the state achieved at time $\tau$ by starting at initial state $x$ and initial time $t$, and applying input functions $u(\cdot)$ and $d(\cdot)$ over $[t,\tau]$. For compactness we will refer to trajectories using $\xi_{x,t}^{u,d}(\tau).$ Because we tend to solve reachability problems backwards in time, we use the notation that forward trajectories end at final time $\tau=0$, and start at an initial negative time $t$.

Running example: In this paper we use double integrator as a running example with dynamics,

[TABLE]

with states position $p$ and velocity $v$, where $u\in[-1,1]$ is acceleration. By default the disturbance is $d=[0,0]$, and there is a default model parameter of $b=1$. In later examples we will change the disturbance bound and model parameter.

II-B Hamilton-Jacobi-Isaacs Reachability

II-B1 Defining the Value Function

We define a target function $l(x)$ whose subzero level set is the target set $\mathcal{L}$ describing the unsafe states, i.e. $\mathcal{L}=\{x:l(x)\leq 0\}$. Typically $l(x)$ is defined as a signed distance function that measures distance to $\mathcal{L}$. This can be considered as measure of reward, with positive reward outside of the unsafe set and negative reward inside.

This problem formulation seeks to find all trajectories that will enter $\mathcal{L}$ at any point in the time horizon, and therefore become unsafe:

[TABLE]

More specifically, the goal is to capture this minimum reward for optimal trajectories of the system. To do this we optimize for the optimal control signal that maximizes the reward (and drives the system away from the unsafe target set) and the worst-case disturbance signal that minimizes the reward. This leads to the value function:

[TABLE]

Note that the disturbance is $\gamma=\{d:\mathcal{U}\rightarrow\mathcal{D}\}$, which maps control inputs to disturbance inputs. As in [2], we restrict the disturbance to draw from nonanticipative strategies.

Level sets of the value function correspond to level sets of the target function. If a state has a negative value, optimal trajectories starting from that state have entered the target set $\mathcal{L}$ sometime within the time horizon. Therefore, the sub-zero level set of the value function comprises the backward reachable tube (BRT), notated as $\mathcal{V}$: the set of states from which the system is guaranteed to enter the target set within the time horizon under optimal control and worst-case disturbance. For the infinite-time avoid BRT, we define the converged value function as $V^{*}(x)=\lim_{t\rightarrow-\infty}V(x,t)$. The subzero level set of this converged value function is the infinite-time avoid backwards reachable tube: $\mathcal{V}^{*}=\{x:V^{*}(x)\leq 0\}$. Trajectories initialized from states in this set will eventually enter the unsafe target set despite the control’s best effort. The complement of this set is therefore the safe set.

Running example: In the running example the target set $\mathcal{L}=\{(p,v):|p|\leq 2,|v|<\infty\}$. The target function $l(x)$ and its corresponding target set $\mathcal{L}$ can be seen in Fig. 1 in green. The converged BRT $\mathcal{V}^{*}$ and value function $V^{*}(x)$ are in cyan. If the system starts inside $\mathcal{V}^{*}$, it will eventually enter the unsafe target set even while applying the optimal control (i.e. decelerating/accelerating as much as possible).

II-B2 Solving for the Value Function

To solve this optimization problem for the value function, we discretize the state space and initialize the value function to be equal to the target function, $V(x,0)=l(x)$. We work backwards in time while updating the value function until the initial time has been reached (or in the infinite-horizon case, until convergence). The change in $V(\cdot,\cdot)$ for each state backwards in time satisfies the Hamilton-Jacobi-Isaacs partial differential equation (HJI PDE) (5), which is used for solving backward reachable sets (BRSs): the set of initial states that will lead to entering the target set at exactly $\tau=0$:

[TABLE]

where $H$ is the Hamiltonian defined in (6) optimizes over the inner product between the spatial gradients of the value function and the flow field of the dynamics to compute the optimal control and disturbance inputs.

[TABLE]

Running example: For the running example the initial spatial gradients for $V(x,0)=l(x)$ can be seen as black arrows in Fig. 1. The Hamiltonian (6) will optimize over the inner product between these gradients and the flow field of the dynamics $f(x,u,d)$, seen as blue arrows.

Note that the HJI PDE (5) solves for $J(x,t,u(\cdot),d(\cdot))=l(\xi_{x,t}^{u,d}(0))$. Because we are solving a BRT and want to capture the minimum value over the entire time horizon (as in (3)), we must include a minimization term, converting the HJI PDE to an HJI variational inequality (HJI VI).

[TABLE]

The term $l(x)-V(x,t)$ restricts the value function from becoming more positive than the target function, effectively enforcing that all trajectories that achieve negative reward at any time will continue to have negative reward for the rest of the time horizon. For more details on the derivation of this HJI VI and variations that include forward reachability and reach-avoid scenarios, please refer to [24, 25, 1, 2]. We solve this HJI VI recursively using dynamic programming:

[TABLE]

We use (8) to update the value of each discretized state backwards in time using the level set method toolbox and associated helperOC toolbox [22, 26]. At convergence the result is the infinite-horizon value function, whose subzero level set $\mathcal{V}^{*}$ corresponds to the set of states that should be avoided in order to remain safe for all time. Online the system avoids these states by solving for the instantaneous optimal control at state $x$ using the Hamiltonian and inifinite-horizon value function:

[TABLE]

II-C Discounted Reachability

In [20] and [21], the authors introduced a discounting factor $\lambda$ into the cost function (3) motivated by the sum of discounted rewards in reinforcement learning. Introducing $\lambda$, now updates the dynamic programming in 8 to solve,

[TABLE]

where $t\leq 0$. The authors show that this is a contraction mapping and, hence, $V(x,0)$ can be initialized to any function (not just $l(x)$). The discounting allows $V_{l}(x,t)$ to forget any incorrect initializations over longer time horizon. However, this formulation can still result in a slow convergence without careful tuning of $\lambda$, as we demonstrate in Section V.

III Warm-Start Reachability

When there are minor changes to the problem formulation, such as changes to the model parameters, external disturbances, or target sets, computing $V^{*}(\cdot,\cdot)$ requires recomputing the entire value function starting with the target function ($V(x,0)=l(x)$). Instead, we initialize with a previous computed (converged) value function. We define this warm-starting function as $k(x)$, with subzero level set $\mathcal{K}=\{x:k(x)<0\}$.

To develop the theory, we revisit the cost function (3). We rewrite, J_{l}(x,t,u(\cdot),d(\cdot))=\min\Big{\{}\inf_{\tau\in[t,0)}l(\xi_{x,t}^{u,d}(\tau)),l(\xi_{x,t}^{u,d}(0))\Big{\}} and $V_{l}(x,t)$ is defined as in (4) by replacing $J$ by $J_{l}$,

[TABLE]

$V_{l}(x,t)$ is the solution to the following HJI-VI,

[TABLE]

The converged value function is defined as, $V^{*}_{l}(x)=\lim_{t\rightarrow-\infty}V_{l}(x,t)$.

When we warm-start the computation of value function using $k$, the cost function is given by,

[TABLE]

$V_{k}$ can be defined as in (4) with $J=J_{k}$, i.e,

[TABLE]

$V_{k}$ is the solution to the HJI-VI defined similarly as in (12) with $V_{k}(x,0)=k(x)$. The converged value function is defined as, $V^{*}_{k}(x)=\lim_{t\rightarrow-\infty}V_{k}(x,t)$.

In this section we prove that the converged value function $V^{*}_{k}(x)$ that is initialized as above $V_{k}(x,0)=k(x)$ will always be more negative than the value function $V^{*}_{l}(x)$ achieved by standard reachability (i.e. initialized as $V_{l}(x,0)=l(x)$). For the case of avoiding an unsafe set, this means that the relationship between the functions’ BRTs (i.e. subzero level sets) is $\mathcal{V}_{k}^{*}\supseteq\mathcal{V}_{l}^{*}$. In other words, $\mathcal{V}_{k}^{*}$ is a conservative over-approximation of $\mathcal{V}_{l}^{*}$. We will prove that for certain conditions (more explicitly, when $k(x)\geq V^{*}_{l}(x)$), we can guarantee that the resulting value function and BRT will be exact.

III-A Conservative Warm-Start Reachability

If $[V_{k}(x,0)=k(x)]\leq V^{*}_{l}(x)$, a contraction mechanism is required to raise $V_{k}^{*}(x)$ towards the true solution $V^{*}_{l}(x)$. Recall the HJI VI from (7). Contraction may happen naturally, when the left hand side of the minimization (the HJI PDE) “pulls the system up” due to the Hamiltonian. However, there are no guarantees that this contraction will happen, and the new value function may get stuck in a local solution, $V_{k}^{*}(x)\leq V_{l}^{*}(x)$. This will result in a conservative BRT.

Theorem 1

For all initializations of $V_{k}(x,0)=k(x)$, the result will be conservative, i.e.

[TABLE]

Proof:

We prove that $V_{k}(x,t)\leq V_{l}(x,t)$ for two cases, (a) $k(x)<l(x)$ and (b) $k(x)\geq l(x)$.

$k(x)<l(x)$

For $\forall x,t<0$, let $V_{l}(x,t)$ be defined as (11) and $V_{k}(x,t)$ be defined as (14). At $t=0$, we have \Big{[}V_{k}(x,0)=k(x)\Big{]}<\Big{[}l(x)=V_{l}(x,0)\Big{]}\Rightarrow V_{k}(x,0)<V_{l}(x,0). For any $t<0$:

[TABLE]

The second inequality follows from the fact that $k(x)<l(x)~{}\forall x\in\mathbb{R}^{n}$. Hence, $\forall x,t$, we have $V_{k}(x,t)\leq V_{l}(x,t)$.

Finally, $t\rightarrow-\infty$, we have $V^{*}_{k}(x)\leq V^{*}_{l}(x)$.

$k(x)\geq l(x)$

When $t=0$, \Big{[}V_{k}(x,0)=k(x)\Big{]}\geq\Big{[}l(x)=V_{l}(x,0)\Big{]}. For a time instance $t=0^{-}$,

[TABLE]

We can re-write (11) and (14) by replacing [math] by $0^{-}$. The rest follows from proof of case (a). Here $0^{-}$ implies an infinitesimally small change in time and we are effectively computing $V_{k}(x,0^{-})=\min(k(x),l(x))$ and treating $V_{k}(x,0)=V_{k}(x,0^{-})$. One could derive the same proof by considering $V_{k}(x,0)=\min(k(x),l(x))$. ∎ In other words, the converged warm-starting solution will never be more conservative than the initialization, and at least as conservative as the exact solution.

III-B Exact Warm-start Reachability

In the case in which $k(x)\geq V^{*}_{l}(x)$, we are additionally guaranteed to recover the exact solution.

Theorem 2

If we warm-start with $V_{k}(x,0)=k(x)$, such that $\forall x\;k(x)\geq V^{*}_{l}(x)$, then,

[TABLE]

The proof of Theorem 2 follows from Theorem 1 and Lemma 1, defined as:

Lemma 1

If $\forall x\;k(x)\geq V^{*}_{l}(x)$, we have,

[TABLE]

Proof:

To prove Lemma 1, let us consider $k^{\prime}(x)=V^{*}_{l}(x)$. For $t<0$, using dynamic programming we have,

[TABLE]

For any $t<0$, we can follow the same logic as in (16). Hence, $\forall x,t$, we have $V_{k^{\prime}}(x,t)\leq V_{k}(x,t)$. Moreover, since $V_{k^{\prime}}(x,0)=V^{*}_{l}(x)$, we know that $V_{k^{\prime}}(x,t)=V^{*}_{l}(x)\quad\forall t$ since $V^{*}_{l}(x)$ is the converged value function corresponding to $l(x)$. Hence, $V_{k}(x,t)\geq V^{*}_{l}(x)\quad\forall x,t<0$. Since this holds for all time, it also holds for $t\rightarrow-\infty$: $V^{*}_{k}(x)\geq V^{*}_{l}(x)$. ∎

Proof:

To prove Theorem 2, we have $\forall x$,

[TABLE]

∎

IV Conservative Warm-Start Examples

Below we demonstrate several scenarios using the running example that result in conservative solutions. All experiments were run on a desktop computer with an Intel Core i7-5280K CPU $@3.30$GHz $\times 12$ processor and $12.8$GB of memory. For all examples the value function is considered converged when the maximum change of value in one time step ($dt=0.01$) is less than $0.001$.

IV-A Conservative Initialization with Exact Results

In practice we find that frequently the value function converges to the exact solution even when initialized below the converged value function, i.e. when $k(x)<V^{*}_{l}(x)$. Fig. 2a demonstrates one such example. The warm-start function $k(x)$ (seen in blue) is initialized to be the original value function acquired when $u\in[-.7,.7]$. If the control authority increases to $u\in[-1,1]$, standard reachability converges to the cyan value function $V^{*}_{l}(x)$. In black is the value function under $V^{*}_{k}(x)$ that was initialized by $k(x)$ instead of $l(x)$. Convergence occurs due to the Hamiltonian in (7) contracting the value function until the solution has been reached.

IV-B Conservative Initialization with Conservative Results

To find a result that does not converge exactly and instead results in a conservative solution, we initialize with $k(x)<V^{*}_{l}(x)$ that has incorrect gradients everywhere, as shown in blue in Fig 2b. Note that this is a fairly unrealistic initial estimate for the true value function, as as the subzero level set $\mathcal{K}$ is the entire state space. As the Hamiltonian contracts the function, convergence occurs at a local solution when the gradients of the value function approach zero. In black we see that $V^{*}_{k}(x)<V^{*}_{l}(x)$, and the BRT $\mathcal{V}_{k}^{*}$ is the entire state space.

IV-C Mixed Initalization with Conservative Results

In Fig. 2c we initialize the warm-starting function as $[V_{k}(x,0)=k(x)]=0$ (blue) so that $k(x)\geq V^{*}_{l}(x)$ for a subset of the state space. Where $k(x)\geq V^{*}_{l}(x)$ convergence is nearly exactly (black), with slight conservativeness introduced at the boundary of where $k(x)=V^{*}_{l}(x)$. Where $k(x)<V^{*}_{l}(x)$ the warm-start solution remains flat at $V^{*}_{k}(x)=0$. The resulting BRT $\mathcal{V}_{k}^{*}$ is a slight over-approximation of $\mathcal{V}_{l}^{*}$.

IV-D Random Initialization with Exact Results

Though we are able to find cases that lead to conservative results, these cases are hard to come by. In almost all initializations the correct value function was achieved exactly. Fig. 2d demonstrates this by initializing $\mathcal{V}_{k}$ with randomly spaced and sized circles. Similar exact results were found for a variety of system dynamics and problem formulations.

V Exact Warm-Start Examples

Though in general we may not know if $k(x)\geq V^{*}_{l}(x)$, there are some cases in which this can be proved, and therefore the exact solution can be recovered. For all following examples $V^{*}_{l}(x),$ is the original value function and $\mathcal{V}_{l}^{*}$ is the corresponding BRT acquired from standard reachability using the default running example. Each subsection introduces changes to the problem formulations, resulting in a new $V^{*}_{l^{\prime}}(x),\mathcal{V}_{l^{\prime}}^{*}$ acquired from standard reachability. Finally, $V^{*}_{k}(x),\mathcal{V}^{*}_{k}$ are the value function and BRT acquired by warm-starting with $k(x)=V^{*}_{l}(x)$ with the changed problem formulation. We further show what happens when the conditions that lead to exact results are reversed. In these cases we cannot guarantee exact convergence, but can guarantee that in each iteration the function will either reduce conservativeness or remain in a local solution (i.e. $k\leq V_{k}(x,t)\leq V_{l}^{*}(x)\;\forall x,t$). We show in Table 1 a time comparison for each example to standard and discounted reachability, shown both in runtime and number of iteration steps. For the exact cases we find that warm-starting is consistently faster. For comparison to discounted reachability, we used a discount factor of 0.999 and annealed to a discount factor of 1 once convergence was reached (see [20] for details).

V-A Changing Target Set

When the target set increases ($\mathcal{L}^{\prime}\supseteq\mathcal{L}$), setting the initialization to the previously converged value results in $[k(x)=V^{*}_{l}(x)]\geq V^{*}_{l^{\prime}}(x)$ and therefore exact convergence is guaranteed. Refer to Proposition 1 and its proof is in Appendix VIII-A. We demonstrate this in Fig. 3a, where the target sets are in green (solid for $\mathcal{L}$, dashed for $\mathcal{L}^{\prime}$). When warm-starting from the original BRT $\mathcal{V}_{l}^{*}$ (cyan), we are able to recover the new BRT $\mathcal{V}_{l^{\prime}}^{*}$ (red) exactly, resulting in $\mathcal{V}_{k}^{*}$ (black). We similarly show the reverse case for a decreasing target set in Fig. 3b. See Remark 1 for details.

V-B Changing Control Authority

In many applications the control authority can change over time. This can happen because of several reasons; for example, increasing the mass of a quadrotor leads to a reduction in its effective control authority. We can explicitly modify $\mathcal{U}$ when there is a change in the control bounds or in a model parameter which updates the effective control authority. When the control space is decreased, i.e. $\mathcal{U}^{\prime}\subseteq\mathcal{U}$, initializing with the previously converged value function will lead to $[k(x)=V^{*}_{l}(x)]\geq V^{*}_{l^{\prime}}(x)$ and therefore exact convergence is guaranteed. Refer to Proposition 2 and its proof in Appendix VIII-B.

To demonstrate this case of reduced control authority we vary the parameter $b$ in the system model (2). When $b$ decreases, the effective control authority decreases. In Fig. 3c we compute the value function for $b=1$ (cyan). We then compute the value function for $b=.8$ (red). Finally, we warm-start from the original cyan value function and reach the new red value function exactly, as shown in black. We similarly show the reverse case for an increasing control authority in Fig. 3d. See Remark 2 for details.

V-C Changing Disturbance Authority

Following similar logic to the previous example, we find that increasing $\mathcal{D}$ to a larger $\mathcal{D}^{\prime}$ has the same effect on the value function as decreasing $\mathcal{U}$ to $\mathcal{U}^{\prime}$. To demonstrate this, we change the disturbance bounds in our model (2). In Fig. 3e we compute the value function for $d\in[0,0]$, shown in blue. We then compute the value function for $d\in[-4,4]$. Finally, we warm-start from the original cyan value function and reach the new red value function exactly, as shown in black. We similarly show the reverse case for a decreasing disturbance authority in Fig. 3f.

VI high-dimensional example

The strength of warm-starting in reducing computation time is best seen in high-dimensional examples. In this example we perform reachability analysis to provide safety guarantees for a 10D nonlinear near-hover quadcopter model from [27, 8]. When the quadcopter experiences changes to its environment constraints or system dynamics (e.g. changes in mass or disturbances), it must update its safety guarantees appropriately.

The 10D near-hover quadcopter dynamics has states $(p_{x},p_{y},p_{z})$ denoting the position, $(v_{x},v_{y},v_{z})$ for velocity, $(\theta_{x},\theta_{y})$ for pitch and roll, and $(\omega_{x},\omega_{y})$ for pitch and roll rates. Its controls are $(S_{x},S_{y})$, which respectively represent the desired pitch and roll angle, and $T_{z}$, which represents the vertical thrust. The disturbances are $(d_{x},d_{y},d_{z})$ which represents wind, and $g$ is gravity. Its model is:

[TABLE]

The parameters $d_{0},d_{1},n_{0},k_{T}$, as well as the control bounds $\mathcal{U}$ that we used were $d_{0}=10,d_{1}=8,n_{0}=10,k_{T}=4.55,|u_{x}|,|u_{y}|\leq 10\text{ degrees},0\leq u_{z}\leq 2g$. As in [8], we can decompose this into two 4D systems and one 2D system.

In this example the initial mass is $m=5$ and initial disturbances are $|d_{x}|,|d_{y}|\leq 1,|d_{z}|\leq 1$. As the quadcopter is flying, the mass increases to $m=5.25$ (say, due to rain accumulation or picking up a package), effectively decreasing the control bounds. In addition, disturbance bounds go up: $|d_{x}|,|d_{y}|\leq 1.5$. In this scenario we can warm-start from the previously computed value function to update the safety guarantees exactly. The value function converges to the true solution (max error of 0.189 in the $p_{x},p_{y}$ subsystems and 0.003 in the $p_{z}$ subsystem) in 66 steps (2.8 hours) instead of 87 steps (3.65 hours) for standard reachability. Discounted reachability still hadn’t converged after 400 steps (18+ hours), with an error of 0.0034 in $p_{x},p_{y}$ and .325 in the $p_{z}$ subsystem.

If the mass and disturbances instead go down (say, to $m=4.8,|d_{x}|,|d_{y}|\leq.95$), we can guarantee that the warm-start solution will at best be exactly the new solution, and at worst will be a conservative solution. As demonstrated in Sec. IV, in practice we almost always converge to the correct solution, and this 10D example converges correctly as well (max error of $2.7e-05$ in the $p_{x},p_{y}$ subsystems and .074 in the $p_{z}$ subsystem). Our warm-starting method took 48 steps, compared to 50 for standard reachability and 82 for discounting (with errors of $8.6e-06$ in the the $p_{x},p_{y}$ subsystems and .004 in the $p_{z}$ subsystem). Though warm-starting does not provide much computational benefit in this case, every iteration toward convergence provides a guaranteed safe over-approximation of the BRT, which is not true for standard reachability.

VII Discussion & Conclusion

Warm-starting infinite-horizon HJI reachability computations with intelligent initializations is beneficial because it may lead to a sizable reduction in computation time by reducing the number of iterations required for convergence. In this paper we proved that warm-starting will provide guaranteed conservative safety analyses and controllers. Moreover, when the initialization is under-conservative (i.e. $k(x)\geq V_{l}^{*}(x)$), we proved that the reachability analysis is guaranteed to converge to the true solution. We also showed several conditions for which exact convergence is achieved, and several cases that will either move closer to the correct safety guarantees or remain conservative with every iteration. In practice we frequently converge to the correct solution regardless of the conservativeness of the initialization.

We demonstrated these results through several examples, including a 10D quadcopter model experiencing changes in mass and disturbance bounds. We were able to accurately recover the updated value function representing the backwards reachable tube in fewer iterations than standard or discounted reachability. For our examples we find that warm-start reachability is $1.6$ times faster than standard reachablity and $6.2$ times faster than (untuned) discounting.

This new formulation opens the door to many different methods for solving HJI reachability problems efficiently. One direction would be to numerically parameterize the value function (for example, by different masses), then warm-start online using an interpolated intialization based on updated problem information. Another exciting direction is to update the value function locally for local changes in the environment, or to use sparse or adaptive gridding of the state space for fast initializations. Finally, we could use the conclusions drawn from this paper to inform a more tractable formulation of discounted reachability.

VIII Appendix

VIII-A Changing the Target Set

For a target function, $l^{\prime}(x)$, we define $J_{l^{\prime}}$ and $V_{l^{\prime}}(x,t)$ as (3) and (11) by replacing $l(x)$ by $l^{\prime}(x)$. We define $V_{k}(x,t)$ as the value function for $\mathcal{L}^{\prime}$ when we warm-start from $V^{*}_{l}(x)$.

Proposition 1

If $\mathcal{L}\subseteq\mathcal{L}^{\prime}$ (and hence $l(x)\leq l^{\prime}(x)$) and we warm-start the value function computation for $\mathcal{L}^{\prime}$ with $k(x)=V^{*}_{l}(x)$, i.e., $V_{k}(x,0)=V^{*}_{l}(x)$, then

[TABLE]

Proof:

To prove this, it suffices to prove that $\forall x\;V^{*}_{l^{\prime}}(x)\leq V^{*}_{l}$. We have $\forall x,t<0$,

[TABLE]

As $t\rightarrow-\infty$, $V^{*}_{l^{\prime}}(x)\leq V^{*}_{l}(x)$. From Theorem 2, if we warm-start, $V_{l^{\prime}}(x,0)=k(x)$ where $k(x)\geq V^{*}_{l^{\prime}}(x)$, then $V^{*}_{k}(x)=V^{*}_{l}$. Since, $k(x)=V^{*}_{l}(x)\geq V^{*}_{l^{\prime}}(x)$, Theorem 2 holds. ∎

Remark 1

By reversing the proof with conditions $\mathcal{L}\supseteq\mathcal{L}^{\prime}$, then $k(x)\leq V_{k}(x,t)\leq V_{l}^{*}(x)\;\forall x,t$.

VIII-B Changing the Control Authority

For a control domain, $\mathcal{U}^{\prime}$, we define $V_{l^{\prime}}(x,t)$ similar to (11) by replacing $u\in\mathcal{U}$ by $u\in\mathcal{U}^{\prime}$. We define $V_{k}(x,t)$ as the value function for $\mathcal{U}^{\prime}$ when we warm-start from $V^{*}_{l}(x)$.

Proposition 2

If the effective control authority $\mathcal{U}^{\prime}$ reduces, i.e., $\mathcal{U}^{\prime}\subseteq\mathcal{U}$ and if $V_{k}(x,0)=k(x)=V^{*}_{l}(x)$, then

[TABLE]

Proof:

To prove this, we need only prove that, $\forall x\;V^{*}_{l^{\prime}}(x)\leq V^{*}_{l}(x)$. We have, $\forall x,t<0$,

[TABLE]

As $t\rightarrow-\infty$, $V^{*}_{l^{\prime}}(x)\leq V^{*}_{l}(x)$. From Theorem 2, if we warm-start with $V_{k}(x,0)=k(x)$ with $k(x)\geq V^{*}_{l}(x)$, then $V^{*}_{k}(x)=V^{*}_{l^{\prime}}(x)$. Hence, $k(x)=V^{*}_{(l}(x)$ satisfies Theorem 2.∎

Remark 2

By reversing the proof with conditions $\mathcal{U}\subseteq\mathcal{U}^{\prime}$, then $k(x)\leq V_{k}(x,t)\leq V_{l}^{*}(x)\;\forall x,t$.