HJ reachability

In this chapter, we will introduce reachability analysis, and show that we can frame reachability as an optimal control problem and utilize the same machinery discussed in the previous chapter.

Reachability analysis

Reachability analysis, as the name suggests, is the study of the set of states a system can (or cannot) reach within some fixed time. Generally, we refer to this set of states as a reachable set. The reachable set depends on a number of factors including

• the dynamics of the system

• initial conditions

• set of controls

• disturbances affecting the system

Reachability analysis is useful for a number of applications such as

• safety-critical control: knowing whether it is possible (or inevitable) for the system to hit an obstacle. If so, evasive control should be executed to avoid entry into such a set.

• robust control: if there is uncertainty or disturbances present, it is important to know where the system could possibly reach and ensure that it does not intersect with obstacles.

• verification: computing the reachable sets based on the dynamics and assumptions about the system can help provide a certificate or guarantee on whether a system can reach a goal or can avoid obstacles.

Types of reachability

There are two types of reachability problem: Forward reachability and backward reachability.

Forward reachability

Given system dynamics, and the set of all allowable controls it can take. Then the forward reachable set is the set of the states the system could be in at some future time. Or differently phrased, starting from an initial set of states, under any allowed input, where could the system end up within some time?

Let’s define this mathematically. Let \(\mathbb{U}[0,t]:=\lbrace u \mid [0,t] \rightarrow \mathcal{U} \rbrace\) be the set of all possible control signals over time interval \([0,t]\). Given dynamics \(\dot{x} = f(x,u,t)\), let \(\xi_{x_0, 0}^{u(\cdot)}(\tau) = x(\tau)\) be the state the system will be in at time \(t=\tau\) if starting at \(x_0\) and \(t=0\) and executes some control signal \(u(\cdot)\in\mathbb{U}[0,\tau]\).

Then we define the forward reachable set \(\mathcal{F}(x_0, \tau)\) as follows,

\[\mathcal{F}(\mathcal{X}_0, \tau) = \lbrace x \in \mathcal{X} \mid \exists u(\cdot) \in \mathbb{U}[0, t], \: \exists x_0 \in \mathcal{X}_0, \: x = \xi_{x_0,0}^{u(\cdot)}(\tau) \rbrace\]

which translates to, all the states in the domain where there exists a control signal that can make the system be at that state within time \(\tau\) when starting from inside \(\mathcal{X}_0\).

Backward reachability

Given a target set, the backward reachable set is the set of states where it is possible reach the target set at some future time. Or differently phrased, where must the system be now so that it can reach the target set in the future?

Let’s define this mathematically. Let \(\mathcal{T}\) be a target set that the system is interested in reaching. Without loss of generality, let us assume that \(t=0\) corresponds to the time when we want to check whether or not the system has reached \(\mathcal{T}\). We want to determine what are at the states where within some horizon length \(|\tau|\), we will reach \(\mathcal{T}\). Since we have defined \(t=0\) to be the end of the horizon, we are interested in the set of states at \(t=-|\tau|\) where in \(|\tau|\) seconds later (\(t=-|\tau| + |\tau| = 0\)), the system will be inside \(\mathcal{T}\). For simplicity, we will assume \(\tau >0\) and drop the absolute value symbols. The backward reachable set is defined as

Backward Reachable Set (Reach Case)

()\[\mathcal{R}(\mathcal{T}, -\tau) = \lbrace x\in\mathcal{X} \mid \exists u(\cdot) \in \mathbb{U}[0,\tau] \: \text{s.t.} \: \xi_{x, -\tau}^{u(\cdot)}(0) \in \mathcal{T} \rbrace.\]

This translates to, for all initial states in the domain, there exists a control signal where it will lead the system to be inside \(\mathcal{T}\) in \(\tau\) seconds.

The above assume that reaching the target set \(\mathcal{T}\) is a good thing. That is, we are seeking whether there exists at least one control signal that would get us there in \(\tau\) seconds. As such, we use \(\mathcal{R}\) to denote the intention to reach \(\mathcal{T}\).

However, we can also treat \(\mathcal{T}\) as an undesirable set (e.g., collision set), and ask the question whether it is possible to avoid \(\mathcal{T}\) in the future. To check whether it is possible to avoid \(\mathcal{T}\), we can equivalently determine whether all control signals would lead to the system being inside \(\mathcal{T}\). If all control signals lead to the system entering \(\mathcal{T}\), then that means that there is no control signal that avoids \(\mathcal{T}\).

Obviously, being inside this set would be undesirable because it would mean that all control signals would always lead the system to end up in \(\mathcal{T}\), i.e., there is nothing that the system can do to avoid entering \(\mathcal{T}\). We can define this set in a similar way as above. We refer to this as the avoid set and use \(\mathcal{A}\) to denote it. This avoid set is also often referred to as the Inevitable Collision Set (ICS) if \(\mathcal{T}\) represents a collision set.

Backward Reachable Set (Avoid Case)

()\[\mathcal{A}(\mathcal{T}, -\tau) = \lbrace x_0\in\mathcal{X} \mid \forall u(\cdot) \in \mathbb{U}[-\tau, 0] \: \text{s.t.} \: \xi_{x_0, -\tau}^{u(\cdot)}(0) \in \mathcal{T} \rbrace\]

Backward reachable tubes

Great! We just defined BRS as sets that determine whether is it possible reach a target set (reach case), and whether it is impossible to avoid the target set (avoid case). However, the above definitions only considers whether the system is inside the target set only at the end of the horizon. It is possible that the system enters \(\mathcal{T}\) during the horizon but exits it by the end of the horizon. Thus a state may be outside of the BRS, but can still enter \(\mathcal{T}\) in the future. Indeed, if \(\mathcal{T}\) represents a collision set, then entering \(\mathcal{T}\) before the end of the horizon is still bad and is something we would like to capture.

As such, we should also define sets that consider entry into the target set \(\mathcal{T}\) anytime between \(t=-\tau\) and \(t=0\). These sets that consider entry into the target set before the end of the horizon are referred to as Backward Reachable Tubes (BRT) and has a very similar definition as above, but considers additional entry into \(\mathcal{T}\) for any time \(s\in[-\tau, 0]\).

Backward Reachable Tube (Reach Case)

()\[\widetilde{\mathcal{R}}(\mathcal{T}, -\tau) = \lbrace x_0\in\mathcal{X} \mid \exists u(\cdot) \in \mathbb{U}[-\tau, 0],\: \exists s\in[-\tau, 0], \: \text{s.t.} \: \xi_{x_0, -\tau}^{u(\cdot)}(s)) \in \mathcal{T} \rbrace.\]

Backward Reachable Tube (Avoid Case)

()\[\widetilde{\mathcal{A}}(\mathcal{T}, -\tau) = \lbrace x_0\in\mathcal{X} \mid \forall u(\cdot) \in \mathbb{U}[-\tau, 0], \: \exists s\in[-\tau,0], \: \text{s.t.} \: \xi_{x_0, -\tau}^{u(\cdot)}(s) \in \mathcal{T} \rbrace\]

Computing BRS

Great! We just defined what backward reachable sets and backward reachable tubes are. But how do we go about actually computing them? Let’s focus our attention to BRS for the moment. We are concerned with whether we end up in \(\mathcal{T}\) at the end of the time horizon, and we must reason backward in time to determine what is the set of states where it is possible to enter \(\mathcal{T}\) in \(\tau\) time. Or differently put, given some horizon and starting state, does there exist a control signal where the state at the end of horizon is in \(\mathcal{T}\)? When we consider “does there exists”, it should make you think “optimization”! If we just need to find at least one, then we can try to find the “best”, i.e., optimal, control signal that drives the final state as far inside \(\mathcal{T}\) as possible. If the optimal control sequence cannot reach \(\mathcal{T}\), then no other control sequence can.

This aspect of reasoning backward in time and the “does there exist” question strongly hints that we need to perform dynamic programming to determine the BRS. In fact, we can pose this reachability problem as an optimal control problem and solve it using the HJB equation (since we are considering a continuous-time setting).

Note that in the BRS definition, we only care if we enter \(\mathcal{T}\) at the end of the horizon and nothing else. We don’t care about the total control effort used nor what happens during the trajectory. We just care about what happens at the end. With this idea, we treat the running cost as zero, an keep the terminal cost non-zero. In fact, define the terminal cost based on the \(\mathcal{T}\). Let us define a terminal cost function \(g:\mathbb{R}^n \rightarrow \mathbb{R}\) such that,

\[ \mathcal{T} = \lbrace x\in\mathcal{X} \mid g(x) \leq 0 \rbrace \]

which means that the \(g(x)\) is negative if \(x\in\mathcal{T}\), or \(g(x)=0\) if \(x\in\partial\mathcal{T}\) (i.e., on the boundary of \(\mathcal{T}\)). A natural choice for \(g\) is the signed distance function, a function that measures the closest distance to the boundary of \(\mathcal{T}\), where negative distances denotes inside the set, while positive distance denotes outside the set.

Below is an optimal control problem with the described cost structure, and no additonal constraints aside from ones constraining the dynamics and allowable state/control sets.

\[\begin{split} \begin{align*} \min_{u(\cdot) \in \mathbb{U}[0,\tau]} &\: g(x(T))\\ \text{subject to} &\: \dot{x} = f(x,u,t)\\ &\: u(t) \in \mathcal{U}, x(t) \in \mathcal{X} \end{align*} \end{split}\]

With this cost structure, note that finding an optimal control policy that minimizes the cost is equivalent to finding an optimal control policy that drives the system into as far into \(\mathcal{T}\) as possible, assuming that \(g\) is defined where the more negative it is, the farther inside \(\mathcal{T}\) it is.

Recall that the (optimal) value function measures the future accumulated cost associated with state \(x\) if the system follows the (optimal) policy. With this particular choice of cost structure described above (zero running cost and terminal cost to have a zero sub-level set be \(\mathcal{T}\)), the value function becomes:

\[ V^*(x,-\tau) = \min_{u(\cdot) \in \mathbb{U}[-\tau, 0]}g(\xi_{x,-\tau}^{u(\cdot)}(0)), \qquad \pi^*(\cdot) = \mathrm{arg}\min_{u(\cdot) \in \mathbb{U}[-\tau,0]}g(\xi_{x,-\tau}^{u(\cdot)}(0)) \]

which is simply the terminal cost evaluated at the state at the end of the horizon if starting from state \(x\), and following policy \(\pi^*\). Since \(g(x) \leq 0\) implies \(x\in \mathcal{T}\), this means that if \(V(x,t) \leq 0\), then following the optimal policy from state \(x\) and time \(t=-\tau\) will lead the system to be inside \(\mathcal{T}\) at time \(t=0\) (i.e., at the end of the horizon). As such, this means that the zero sub-level set of \(V(\cdot, t)\) is precisely the BRS for a horizon of \(\tau\)! Concretely, we have,

Backward Reachable Set as a Value Function (Reach Case)

()\[\mathcal{R}(\mathcal{T}, -\tau) = \lbrace x\in \mathcal{X} \mid V^*(x,-\tau) \leq 0 \rbrace, \qquad \text{where the planning horizon is }\tau\]

What about the avoid case? Recall that the BRS for the avoid case is to determine the set of states where it is impossible to avoid \(\mathcal{T}\) despite the system’s best effort. With the same cost described above, if the system instead aimed to maximize the terminal cost rather than minimize, then it is trying its best to avoid \(\mathcal{T}\), but if \(V^*(x,-\tau) \leq 0\) it means that the system will end up inside \(\mathcal{T}\) (despite the system’s best effort).

As such, the BRS for the avoid case is the same as the reach case, except that we are performing a maximization problem instead of a minimization one. After solving the for the value function with the maximization, then the BRS (avoid case) is simply the sub-zero level set of the value function, just like what we saw above.

Great! Now we have now posed the backward reachability problem as an optimal control problem and discussed how to extract the BRS from the computed value function. So how do we go about solving for the value function? We solve the Hamilton-Jacboi-Bellman equation! But the stage cost is set to zero.

HJB equation for BRS

()\[\begin{split}\frac{\partial V}{\partial t}(x,t) + \min_{u\in\mathcal{U}} \nabla V(x,t)^T f(x,u,t) = 0 \qquad \text{Reach case}\\ \frac{\partial V}{\partial t}(x,t) + \max_{u\in\mathcal{U}} \nabla V(x,t)^T f(x,u,t) = 0 \qquad \text{Avoid case}\\ V(x,0) = g(x) \qquad t\in[-\tau, 0]\\\end{split}\]

Computing BRT

What about the backward reachable tube? What want to check whether the system enters \(\mathcal{T}\) any time during the horizon. As such, we don’t want to compute the terminal cost at the end of the horizon, but rather, determine the lowest value anytime over the horizon. Mathematically, the value function becomes,

\[ V^*(x,t) = \min_{u(\cdot) \in \mathbb{U}[-\tau, 0]} \min_{s\in[-\tau, 0]} J_T(\xi_{x,-\tau}^{u(\cdot)}(s)), \qquad \pi^*(\cdot) = \mathrm{arg}\min_{u(\cdot) \in \mathbb{U}[-\tau, 0]} \min_{s\in[-\tau, 0]} J_T(\xi_{x,-\tau}^{u(\cdot)}(s)). \]

We change the HJB PDE slightly to reflect the minimization over time,

HJB equation for BRT

()\[\begin{split}\frac{\partial V}{\partial t}(x,t) + \min\biggl(0, \min_{u\in\mathcal{U}} \nabla V(x,t)^T f(x,u,t)\biggl) = 0 \qquad \text{Reach case}\\ \frac{\partial V}{\partial t}(x,t) + \min\biggl(0, \max_{u\in\mathcal{U}} \nabla V(x,t)^T f(x,u,t)\biggl) \qquad \text{Avoid case}\\ V(x,0) = g(x) \qquad t\in[-\tau, 0],\\\end{split}\]

which looks very similar to the standard HJB equation, except there is a \(\min\) with zero. Intuitively, if \(\min_{u\in\mathcal{U}} \nabla V(x,t)^T f(x,u,t) > 0 \) (or \(\max\)), it means the system is moving in a direction where \(V\) increases (i.e., moving away from \(\mathcal{T}\)). Since we want to find the minimum value of \(V\) over the horizon, whenever \(V\) is increasing, we want to “freeze” the value of \(V\), essentially forcing \(\frac{\partial V}{\partial t}=0\). Alternatively, if \(\min_{u\in\mathcal{U}} \nabla V(x,t)^T f(x,u,t) < 0 \), it means the system is moving towards \(\mathcal{T}\) and we want to keep track of this and not “freeze” the value of \(V\).

Presence of disturbances

What if now there are disturbances added to the system? Assuming our dynamics are now \(\dot{x} = f(x,u,d)\) and there is an external (bounded) disturbance input \(d\in\mathcal{D}\) that affects the system. Then we want to ensure that it is still possible to reach the target set \(\mathcal{T}\), or whether it can still be avoided despite any and all disturbance signals that could be applied.

For the reach case, we want to consider whether the system can still reach \(\mathcal{T}\) regardless of any disturbances possible. This means that the BRS/BRT should be defined such that the system can reach \(\mathcal{T}\) for all disturbances.

Backward Reachable Set/Tube with Disturbances (Reach Case)

()\[ \begin{align}\begin{aligned}\begin{split}\mathcal{R}_d(\mathcal{T}, -\tau) = \lbrace x_0\in\mathcal{X} \mid \exists u(\cdot) \in \mathbb{U}[-\tau,0], \: \forall d(\cdot) \in \mathbb{D}[-\tau, 0],\: \text{s.t.} \: \xi_{x_0, -\tau}^{u(\cdot)}(0) \in \mathcal{T} \rbrace.\\\end{split}\\\widetilde{\mathcal{R}}_d(\mathcal{T}, -\tau) = \lbrace x_0\in\mathcal{X} \mid \exists u(\cdot) \in \mathbb{U}[-\tau,0], \: \forall d(\cdot) \in \mathbb{D}[-\tau, 0],\: \exists s\in[-\tau,0], \: \text{s.t.} \: \xi_{x_0, -\tau}^{u(\cdot)}(s) \in \mathcal{T} \rbrace.\end{aligned}\end{align} \]

For the avoid case, we want to consider whether there is at least one disturbance that would always lead the system into \(\mathcal{T}\) regardless of any possible control signal. In other words, the BRS/BRT is considering whether there is a disturbance signal that makes it impossible for the system to about \(\mathcal{T}\). This means that the BRS/BRT should be defined such that there exists a disturbance signal such that the system will reach \(\mathcal{T}\) regardless of any control signals.

Backward Reachable Set/Tube with Disturbances (Avoid Case)

()\[ \begin{align}\begin{aligned}\mathcal{A}_d(\mathcal{T}, -\tau) = \lbrace x_0\in\mathcal{X} \mid \forall u(\cdot) \in \mathbb{U}[-\tau,0], \: \exists d(\cdot) \in \mathbb{D}[-\tau, 0],\: \text{s.t.} \: \xi_{x_0, -\tau}^{u(\cdot)}(0) \in \mathcal{T} \rbrace.\\\widetilde{\mathcal{A}}_d(\mathcal{T}, -\tau) = \lbrace x_0\in\mathcal{X} \mid \forall u(\cdot) \in \mathbb{U}[-\tau,0], \: \exists d(\cdot) \in \mathbb{D}[-\tau, 0],\: \exists s\in[-\tau,0], \: \text{s.t.} \: \xi_{x_0, -\tau}^{u(\cdot)}(s) \in \mathcal{T} \rbrace.\end{aligned}\end{align} \]

So now, we have updated the definition of BRS/BRT with disturbances present. We can similarly update the HJB PDE to include the disturbance term. Since the disturbance is acting in a manner that tries to oppose the system’s best effort to reach/avoid \(\mathcal{T}\), if the system is trying perform a \(\max\), then the disturbance will perform a \(\min\), and vice versa. But there is a question as to which order we should compute this sequence of operations. Or differently put, what kind of information pattern we should assume? Does the system get to see what the disturbance is first when deciding the optimal control, or the other way around? Generally, we are using reachability analysis to be robust against disturbances/uncertainties, and therefore would like to take on a more conservative stance. That is, we would like to choose the ordering that would assume that the information pattern would benefit the disturbance.

There is a max-min inequality that describes that the player acting second has the advantage.

Ordering

To interpret \(\max_{z\in Z} \min_{w\in W} g(z,w)\), we can consider it as \(\max_{z\in Z} \biggl(\min_{w\in W} g(z,w)\biggl)\). So when \(\max_{z\in Z}\) is performed (i.e., going first), the \(\min_{w\in W}\) will be applied after (i.e., going second).

Max-min inequality

\[\sup_{z\in Z} \inf_{w\in W} f(z,w) \leq \inf_{w\in W} \sup_{z \ in Z} f(z,w)\]

With this max-min inequality, we assume the system chooses a control first, and then the disturbance will act second. With the addition of the disturbance term, the HJB equation becomes the Hamilton-Jacboi-Isaacs PDE.

HJI equation for BRS

()\[\begin{split}\frac{\partial V}{\partial t}(x,t) + \min_{u\in\mathcal{U}}\max_{d\in\mathcal{D}} \nabla V(x,t)^T f(x,u,d,t) = 0 \qquad \text{Reach case}\\ \frac{\partial V}{\partial t}(x,t) + \max_{u\in\mathcal{U}}\min_{d\in\mathcal{D}} \nabla V(x,t)^T f(x,u,d,t)\qquad \text{Avoid case}\\ V(x,0) = g(x) \qquad t\in[-\tau, 0],\\\end{split}\]

HJI equation for BRT

()\[\begin{split}\frac{\partial V}{\partial t}(x,t) + \min\biggl(0, \min_{u\in\mathcal{U}}\max_{d\in\mathcal{D}} \nabla V(x,t)^T f(x,u,d,t)\biggl) = 0 \qquad \text{Reach case}\\ \frac{\partial V}{\partial t}(x,t) + \min\biggl(0, \max_{u\in\mathcal{U}}\min_{d\in\mathcal{D}} \nabla V(x,t)^T f(x,u,d,t)\biggl) \qquad \text{Avoid case}\\ V(x,0) = g(x) \qquad t\in[-\tau, 0],\\\end{split}\]

HJ reachability toolbox

This hj_reachability toolbox can be used to solve the corresponding HJI equation. The repo has a nice quickstart notebook. Also check out this demo notebook

A few remarks and comments

• It is assumed that the control and disturbance sets are bounded. The formulation considers a worst-case analysis on the disturbance input. As such, given a bounded disturbance set, the HJ reachability formulation will consider the worst disturbance in that set, even if it may be unlikely that disturbance will occur. This resulting computing would be conservative, but perhaps too conservative. The size of the sets could be reduced, but then that would compromise on the robustness of the computed BRS/BRT.

• If the dynamics are control and disturbance affine \(\dot{x} = f_0(x) + B_u(x)u + B_d(x)d\), and the control and disturbances sets are bounded, then computing the \(\min\)/\(\max\) is relatively straight forward. Simply determine the sign of \(\nabla V(x,t)^TB_u(x)\) and \(\nabla V(x,t)^TB_d(x)\) and consider the largest/smallest values of \(u\) and \(d\) depending on whether you want \(\nabla V(x,t)^TB_u(x)u\) and \(\nabla V(x,t)^TB_d(x)d\) to be maximized or minimized. The resulting optimal control is bang-bang. This is not surprising since we have zero running cost, there is not penalty on control effort. So the optimal thing to do to reach/avoid \(\mathcal{T}\) would be to take the most extreme action, always.

• If the dynamics and control/disturbance sets are not nice (e.g., not affine, non-convex etc), then solving the HJB/HJI PDE involves solving a difficult optimization problem.

• The hj_reachability toolbox solves the PDEs explicity. That is, it discretizes the state space (i.e., creates a grid over the state space) and solves the PDE by considering each grid point. This explicit approach suffers from the curse of dimensionality—as the state dimension of the problem grows, the more grid points the solver must consider.

TODOs

• add figures

• add papers

• add further readings

• add reach-avoid