Bi-directional Value Learning for Risk-aware Planning Under Uncertainty: Extended Version

TL;DR

This paper introduces a bi-directional value learning method for risk-aware planning under uncertainty, combining forward and backward searches in belief space to improve safety and long-range decision-making in autonomous systems.

Contribution

It presents a novel bi-directional value learning approach that enhances risk assessment and policy optimization in POMDPs for continuous, long-horizon planning problems.

Findings

Effective long-range risk evaluation demonstrated

Scalable to continuous-space problems

Improves safety in autonomous navigation

Abstract

Decision-making under uncertainty is a crucial ability for autonomous systems. In its most general form, this problem can be formulated as a Partially Observable Markov Decision Process (POMDP). The solution policy of a POMDP can be implicitly encoded as a value function. In partially observable settings, the value function is typically learned via forward simulation of the system evolution. Focusing on accurate and long-range risk assessment, we propose a novel method, where the value function is learned in different phases via a bi-directional search in belief space. A backward value learning process provides a long-range and risk-aware base policy. A forward value learning process ensures local optimality and updates the policy via forward simulations. We consider a class of scalable and continuous-space rover navigation problems (RNP) to assess the safety, scalability, and optimality of the proposed algorithm. The results demonstrate the capabilities of the proposed algorithm in evaluating long-range risk/safety of the planner while addressing continuous problems with long planning horizons.

Equations

$$x_{k+1}=f(x_k,u_k,w_k),w_k\sim p(w_k|x_k,u_k)$$

$$z_k=h(x_k,v_k),v_k\sim p(v_k|x_k)$$

$$b_{k+1}$$

$$J(b;\pi )$$

$$=c(b,\pi (b))+\gamma \sum _{b^{\prime }\in \mathbb{B}}p(b^{\prime }|b,\pi (b))J(b^{\prime };\pi )$$

$$J(b;\pi )=\underset{u\in \mathbb{U}}{\min }Q(b,u;\pi )$$

$${\pi }^{\ast }(b)={\mathrm{argmin}}_{\pi \in \Pi }J(b;\pi ),\forall b\in \mathbb{B}$$

$$\rho (b_0;\pi )=1-\mathbb{E}\left[\prod _{k=0}^{\infty }(1-p(F|b_k,\pi (b_k))\right]$$

$$\rho (b;\pi )$$

$$=\sum _{b^{\prime }\in \mathbb{B}}p(b^{\prime }|b,\pi (b))\rho (b^{\prime };\pi )$$

$$\rho (b;\pi )=\underset{J^F\to \infty }{\lim }\frac{J(b;\pi )}{J^F}$$

$$\rho (b;\pi )$$

$$\underset{J^F\to \infty }{\lim }\frac{J(b_f;\pi )}{J^F}$$

$$\rho (b;\pi )$$

$$\underset{J^F\to \infty }{\lim }\frac{J(b;\pi )}{J^F}$$

$${\pi }^{\ast }(b)={\mathrm{argmin}}_{\pi \in \Pi }\underset{J^F\to \infty }{\lim }J(b;\pi ),\forall b\in \mathbb{B}$$

$${\pi }^{\ast }(b)={\mathrm{argmin}}_{\pi \in \Pi }\underset{J^F\to \infty }{\lim }\frac{J(b;\pi )}{J^F},\forall b\in \mathbb{B}$$

$${\pi }^{\ast }(b)={\mathrm{argmin}}_{\pi \in \Pi }\rho (b;\pi ),\forall b\in \mathbb{B}$$

$$\pi (\cdot )={\mathrm{argmin}}_{\Pi }\mathbb{E}\left[C^{sr}(b^{sr},\pi )+C^{br}(b^{br},\pi )+C^{lr}(b^{lr},\pi )\right]$$

$$\pi (\cdot )=$$

$$+\sum _{k=K^{sr}}^{K^{sr}+K^{br}-1}c(b_k,{\pi }_{(K^{sr}-1)}(b_k))+{\tilde{J}}^g(B^j)]$$

$$B^i$$

$${\tilde{C}}^g(B^i,{\mu }^{ij})=\sum _{k=0}^{{\mathcal{K}}^{ij}}c(b_k,{\mu }^{ij}(b_k))$$

$${\tilde{J}}^g(B^i;{\tilde{\pi }}^g)$$

$${\tilde{J}}^g(B^i;{\tilde{\pi }}^g)={\tilde{C}}^g(B^i,{\tilde{\pi }}^g(B^i))$$

$$+\sum _{B^j\in \mathbb{N}(B^i)}{\mathbf{P}}^g(B^j|B^i,{\tilde{\pi }}^g(B^i)){\tilde{J}}^g(B^j;{\tilde{\pi }}^g)$$

$${\tilde{\pi }}^{g^{\ast }}(\cdot )$$

$$u^{\ast }$$

$$Q^{\prime }(b_k,u_k)$$

$$R_k$$

$$Q_{init}(b,u^j)$$

$$J_{init}(b)$$

Full text

I Introduction

Consider a scenario where an autonomous mobile robot (e.g., a rover or flying drone) needs to navigate through an obstacle-laden environment under both motion and sensing uncertainty. In spite of these uncertainties, the robot needs to guarantee safety and reduce the risk of collision with obstacles at all times. This, in particular, is a challenge for safety-critical systems and fast moving robots as the vehicle traverses long distances in a short time horizon. Hence, ensuring system’s safety requires risk prediction over long horizons.

The above-mentioned problem is an instance of general problem of decision-making under uncertainty in the presence of risk and constraints, which has applications in different mobile robot navigation scenarios. This problem in its most general and principled form can be formulated as a Partially Observable Markov Decision Process (POMDP) [1, 2]. In particular, in this work, we focus on a challenging class of POMDPs, here referred to as RAL-POMDPs (Risk-Averse, Long-range POMDPs). A RAL-POMDP reflects some of challenges encountered in physical robot navigation problems, and is characterized with the following features:

Long planning horizons (beyond $10^{4}$ steps) without discounting cost over time, i.e., safety is equally critical throughout the plan. In RAL-POMDP, the termination of the planning problem is dictated by reaching the goal (terminal) state rather than reaching a finite planning horizon. 2. 2.

RAL-POMDP is defined via high-fidelity continuous state, action, and observation models. 3. 3.

RAL-POMDP incorporates computationally expensive costs and constraints such as collision checking. 4. 4.

RAL-POMDP requires quick policy updates to cope with local changes in the risk regions during execution.

In recent years, value learning in partially observable settings has seen impressive advances in terms of the complexity and size of solved problems. There are two major classes of POMDP solvers (see Fig. 1). The first class is forward search methods [3, 4, 5, 6]. Methods in this class (offline and online variants) typically rely on forward simulations to search the reachable belief space from a given starting belief and learn the value function. POMCP (Partially Observable Monte Carlo Planning) [5], DESPOT [7], and ABT [8] are a few examples of methods in this class that can efficiently learn and update the policy while executing a plan using Monte Carlo simulation.

The second class is approximate long-range solvers such as FIRM (Feedback-based Information RoadMap) [9, 10]. These methods typically address continuous POMDPs but under the Gaussian assumption. They typically rely on graph construction and feedback controllers to solve larger problems. Through offline planning, they can learn an approximate value function on the representative (sampled) graph.

The features of RAL-POMDP problems make them a challenging class of POMDPs for above-mentioned solvers. Forward search-based methods typically require cost discounting and a limited horizon (shorter than 100 steps) to be able to handle the planning problem. Also, they typically require at least one of the state, action, or observation space to be discrete. Continuous approximate long-range methods suffer from suboptimality since actions are generated based on a finite number of local controllers due to the underlying sparse sampling-based structure.

This work addresses RAL-POMDP problems induced by fast-moving robot navigation in safety-critical scenarios. In such systems, several seconds of operation can translate to thousands of decision making steps. The main objective of this work is to provide probabilistic safety guarantees for the long-horizon decision making process (beyond thousands of steps). The second objective of this work is to generate solutions for RAL-POMDPs that are closer to the globally optimal solution compared to the state-of-the-art methods. In parallel to these objectives, we intend to satisfy other requirements of the RAL-POMDP such as incorporating high-fidelity continuous dynamics and sensor models into the planning.

In this paper, we propose Bi-directional Value Learning (BVL) method, a POMDP solver that searches the belief space and learns the value function in a bi-directional manner. In the one thread (can be performed offline) we learn a risk-aware approximate value function backwards from the goal state toward the starting point. In the second thread (performed online), we expand a forward search tree from the start toward the goal. BVL significantly improves the performance (optimality) of the backward search methods by locally updating the policy through rapid online forward search during the actual execution. BVL also enhances the probabilistic guarantees on system’s safety by performing computationally intensive processes, such as collision checking, over long planning horizons in the offline phase.

In Section II, we go over the formal definition of POMDP problems and explain more details about RAL-POMDP problems. In Section III, we present the overall framework of BVL and its concrete instance based on POMCP [5] and FIRM [10]. Section IV provides various simulation experiments to validate the BVL method, and Section V concludes this paper.

II Preliminaries

II-A POMDP Problems

Let us denote the system state, action, and observation at the $k$-th time step by $x_{k}\in\mathbb{X}$, $u_{k}\in\mathbb{U}$, and $z_{k}\in\mathbb{Z}$. The motion model $f$ and observation model $h$ can be written as:

[TABLE]

where $w_{k}$ and $v_{k}$ denote the motion and sensing noises.

A belief state $b\in\mathbb{B}$ is a posterior distribution over all possible states given the past actions and observations $b_{k}=p(x_{k}|z_{0:k},u_{0:k-1})$, which can be updated recursively via Bayesian inference:

[TABLE]

A policy $\pi:\mathbb{B}\rightarrow\mathbb{U}$ maps each belief state $b$ to a desirable action $u$. Denoting the one-step cost function as $c(b,u)\in\mathbb{R}_{>0}$, the value function (or more precisely, the expected cost-to-go function) under policy $\pi$ is defined as follows.

[TABLE]

where $b_{0}=b$, $\gamma\in(0,1]$ is a discount factor that reduces the effect of later costs, and $p(b^{\prime}|b,u)$ is the transition probability from $b$ to $b^{\prime}$ under action $u$. Equation (5) in a recursive form is called a Bellman equation.

It is also convenient to define an intermediate belief-action function, or Q-value, as $Q(b,u;\pi)=c(b,u)+\gamma\sum_{b^{\prime}\in\mathbb{B}}p(b^{\prime}|b,u)J(b^{\prime};\pi)$, such that

[TABLE]

A POMDP problem can then be cast as finding the optimal value and policy.

[TABLE]

II-B RAL-POMDP

In this work, we focus on a RAL-POMDP as a special case of the above-mentioned POMDP problem. Formally, in a RAL-POMDP, $\mathbb{X}$, $\mathbb{U}$, and $\mathbb{Z}$ are continuous spaces, and $f$ and $h$ represent locally differentiable nonlinear mappings. There exists a goal termination set $B^{goal}\subset\mathbb{B}$ such that $J(b_{g})=0$ for $\forall b_{g}\in B^{goal}$. There also exists a failure termination set $F\subset\mathbb{B}$ which represents the risk region (e.g., obstacles in robot motion planning) such that $J(b_{f})\to\infty$ for $\forall b_{f}\in F$. As the risk is critical throughout the plan, a RAL-POMDP does not allow cost discounting, i.e., $\gamma=1$.

In our risk metric discussion, we follow definitions in [11, 12]. Accordingly, our risk metric falls in the category of risk for sequential decision making with deterministic policies, satisfying time-consistency (see [11, 12] for details). Specifically, we formalize the risk by compounding the failure probability, $p(F|b,\pi(b))=\sum_{b_{f}\in F}p(b_{f}|b,\pi(b))$, of each action along the sequence. Accordingly, the risk metric of a policy given a belief $b_{0}$ is measured as follows.

[TABLE]

The second term on the right-hand side is the expected probability to reach the goal without hitting the risk region. Note that $\rho(b_{g};\pi)=0$ for $\forall b_{g}\in B^{goal}$ and $\rho(b_{f};\pi)=1$ for $\forall b_{f}\in F$ for $\forall\pi\in\Pi$. It can be rewritten in a recursive form:

[TABLE]

Now we show that in RAL-POMDPs where $J(b_{f})=J^{F}\to\infty$ for $\forall b_{f}\in F$, the optimal policy $\pi^{*}$ in Eq. (7) also minimizes $\rho(b;\pi^{*})$ in Eq. (9) for $\forall b\in\mathbb{B}$.

Lemma 1**.**

In RAL-POMDPs where $J^{F}\to\infty$ and $\gamma=1$, the following is satified for $\forall b\in\mathbb{B}$.

[TABLE]

Proof.

We prove this by backward induction.

Consider the terminal beliefs first. Trivially, from Eq. (4) and Eq. (8), $\rho(b_{g};\pi)=\lim_{J^{F}\to\infty}\frac{J(b_{g};\pi)}{J^{F}}=0$ for $\forall b_{g}\in B^{goal}$, and $\rho(b_{f};\pi)=\lim_{J^{F}\to\infty}\frac{J(b_{f};\pi)}{J^{F}}=1$ for $\forall b_{f}\in F$. Thus, Eq. (10) is satified for terminal beliefs.

Next, consider a belief such that its every successor is either $b_{g}\in B^{goal}$ or $b_{f}\in F$, i.e., $\sum_{b_{g}\in B^{goal}}p(b_{g}|b,\pi(b))+\sum_{b_{f}\in F}p(b_{f}|b,\pi(b))=1$. Then from Eq. (9),

[TABLE]

and from Eq. (5) with $\gamma=1$ we have:

[TABLE]

Thus, all such belief $b$ satisfies Eq. (10).

Now we consider a belief $b$ such that its all successors $\{b^{\prime}|b,\pi(b)\}$ satisfy Eq. (10). By injecting Eq. (10) for the successors into Eq. (9),

[TABLE]

By dividing Eq. (5) by $J^{F}$,

[TABLE]

Thus, it satisfies Eq. (10).

Finally, by backward induction, Eq. (10) is satisfied for $\forall b\in\mathbb{B}$ in RAL-POMDPs. ∎

Theorem 1**.**

In RAL-POMDPs where $J^{F}\to\infty$ and $\gamma=1$, the optimal policy $\pi^{*}$ that minimizes $J(b;\pi^{*})$ also minimizes $\rho(b;\pi^{*})$ for $\forall b\in\mathbb{B}$.

Proof.

First, we can rewrite Eq. (7) as follows for RAL-POMDPs where $J^{F}\to\infty$.

[TABLE]

By dividing the objective function in Eq. (7) by a constant $J^{F}$, we have:

[TABLE]

Then by Lemma 1, we prove the theorem.

[TABLE]

∎

III Bi-directional Value Learning (BVL)

III-A Overall Framework

In this section, we provide the framework of BVL, the proposed bi-directional long-short-range POMDP solver, and its concrete instance based on POMCP [5] and FIRM [10]. Figure 1 conceptually shows how the combination of the forward short-range and backward long-range planner works. The short-range solver relies on the knowledge of the initial belief and is limited to its reachable belief subspace. It can find a locally near-optimal policy but may get stuck in local minima in the global perspective. The long-range solver can provide a global policy to reach to the goal, but it only considers (sampled) subspace, which results in the solution suboptimality. The main idea of BVL is to develop a bridging scheme between these two approaches to take advantage of both solvers while alleviating their drawbacks.

In BVL, the optimization in Eq. (7) is decomposed as:

[TABLE]

First term, $C^{sr}$, is the cost learned by the short-range planner. $C^{lr}$ is the cost computed by the long-range planner. $C^{br}$ is the cost learned by the bridge planner that connects the short-range policy to the long-range policy.

More concretely, Eq. (18) can be rewritten as follows for the instance of BVL based on POMCP and FIRM.

[TABLE]

where $K^{sr}$ is the fixed horizon of the short-range planner, and $K^{br}$ is a varying horizon of a bridge planner that takes the belief $b_{(K^{sr}+K^{br})}$ (at the end of bridging) to a node $B^{j}\!$ of the global long-range policy. $\tilde{J}^{g}(B^{j^{\scaleobj{0.8}{+}}})$ denotes the approximate estimate of the cost-to-go of $B^{j}\!$ computed offline.

In the following sections, we will discuss this decomposition in more detail using concrete instantiations of the short-range and long-range planners.

III-B Long-range Global Planner

For our long-range global policy, we utilize FIRM (Feedback-based Information Roadmap) [10]. FIRM is an offline, approximate long-range planner. FIRM locally approximates the system model with linear Gaussian models and generates a graph (see Fig. 1-top) of Gaussian distributions in the belief space. We formally describe the offline planning here (Algorithm 1): Let us define the $i$-th FIRM node $B^{i}$ as a set of belief states near a center belief $b_{c}^{i}\equiv(v^{i},P_{c}^{i})$, where $v^{i}$ is a sampled point in state space and $P_{c}^{i}$ is the node covariance.

[TABLE]

$\epsilon$ is the node size and $\mathbb{V}^{g}=\{B^{i}\}$ is the set of all FIRM nodes.

For a pair of neighboring nodes $B^{i}$ and $B^{j}$, a local closed-loop controller $\mu^{ij}:\mathbb{B}\rightarrow\mathbb{U}$ can be designed (e.g., Linear Quadratic Gaussian controllers) that can steer the belief from $B^{i}$ to $B^{j}$. We denote the set of all local controllers as $\mathbb{M}^{g}=\{\mu^{ij}\}$ and the set of all local controllers originated from $B^{i}$ as $\mathbb{M}(i)\subset\mathbb{M}^{g}$. After graph construction, FIRM associates a cost function to each edge by simulating the local controller, $\mu^{ij}$ from $B^{i}$ to $B^{j}$.

[TABLE]

where $b_{0}=b_{c}^{i}$. $\mathcal{K}^{ij}$ is the number of time steps it takes for controller $\mu^{ij}$ to take belief $b_{k}$ from $B^{i}$ to $B^{j}$.

A policy over FIRM graph is a mapping from nodes to edges, i.e., $\tilde{\pi}^{g}:\mathbb{V}^{g}\rightarrow\mathbb{M}^{g}$. Approximate cost-to-go for a given $\tilde{\pi}^{g}$ can be computed as follows.

[TABLE]

where $B_{0}=B^{i}$. We denote by $\mathbb{N}(B^{i})$ the set of neighbor FIRM nodes of $B^{i}$. Equation (22) can also be rewritten in a recursive form as follows.

[TABLE]

where $\mathbf{P}^{g}(B^{j}|B^{i},\tilde{\pi}^{g}(B^{i}))$ is the transition probability from $B^{i}$ to $B^{j}$ under $\mu^{ij}=\tilde{\pi}^{g}(B^{i})$. Note that since the transition probability is usually expensive to compute, approximation methods, such as Monte Carlo simulation, are being used.

Then the following optimization problem is solved by value iteration to find a global policy for the sampled subspace.

[TABLE]

III-C Short-range Local Planner

The short-range local planner is to find a locally near-optimal policy in an online manner. To tackle RAL-POMDP problems, we develop a variant of POMCP (Partially Observable Monte Carlo Planning) [5] here, referred to as J-POMCP for the current instance of BVL.

We start by a brief review of the original POMCP algorithm. POMCP is an online POMDP solver that uses Monte Carlo Tree Search (MCTS) in belief space and particle representation of belief states. POMCP’s action selection during Monte Carlo simulation is governed by two policies: a tree policy within the constructed belief tree, and a rollout policy beyond the tree and up to a pre-defined finite discount horizon.

The tree policy selects an action based on Partially Observable UCT (PO-UCT) algorithm as follows.

[TABLE]

where $Q(b,u)$ is as defined in Section II-A, and $N(b)$ and $N(b,u)$ are the visitation counts for a belief node and an intermediate belief-action node, respectively. $\eta_{q}$ is a constant for exploration bonus in the tree policy. As $\eta_{q}$ gets larger, PO-UCT becomes more explorative in action selection.

The rollout policy may be a random policy. If there is domain knowledge available, a preferred action set can be specified for the rollout policy to guide the Monte Carlo simulation toward a promising subspace.

After each Monte Carlo simulation, $Q(b_{k},u_{k})$ (corresponding to the belief-action pair on the $k$-th simulation step in tree) is updated as follows.

[TABLE]

where

[TABLE]

$Q^{\prime}(b_{k},u_{k})$ denotes the updated value of $Q(b_{k},u_{k})$, and $R_{k}$ is the accumulated return from the horizon of the short-range planner to the current simulation step $k$. Note that $J(b_{K^{sr}})$ should appear in Eq. (27) if the $K^{sr}$ is shorter than the problem’s planning horizon. Through iterative forward simulations, POMCP gradually learns the Q-value for each belief-action pair.

There are two major challenges for POMCP when applied to RAL-POMDP problems.

RAL-POMDPs are infinite horizon problems without cost discounting. Thus, POMCP needs an estimate of $J(b)$ for each $b$ on its finite horizon, possibly from naive heuristics or sophisticated global policy solvers. 2. 2.

RAL-POMDPs incorporates computationally expensive costs and constraints, such as collision checking by high-fidelity simulator, and thus, a higher number of forward simulations are discouraged. However, POMCP usually requires many simulations until convergence because its update rule in Eq. (27) does not bootstrap all the successors whose values are initialized by domain knowledge or global policy solvers.

The first challenge is addressed by the bridge planner that connects the short-range planner to the long-range global planner (see Section III-D). To handle the second challenge, we develop a variant of POMCP, referred to as J-POMCP.

J-POMCP follows exactly how POMCP selects actions to explore the belief space, but slightly differs in how the values are updated. More precisely, J-POMCP uses the following instead of Eq. (27) to compute $Q^{\prime}(b_{k},u_{k})$.

[TABLE]

Note that the second term on the right-hand side is $J(b_{k+1})$ in Eq. (6), hence the name J-POMCP. This is in fact how Q-learning updates the Q-value in a greedy manner by bootstrapping the initialized or learned values [14]. It creates a bias in value and converges faster if the initial values are informative.

In BVL, we have access to the approximate global policy computed by the long-range solver, which is much better than the heuristics computed under the assumption of deterministic or fully observable environments. J-POMCP can make the most use of the underlying global policy through bootstrapping.

This new update rule can be implemented as presented in Algorithm 4 (see Line 27, 31, and 32). As in Line 31, $J(b)$ is updated every time $Q(b,u)$ is updated for $\forall u\in\mathbb{U}$.

Algorithms 2–5 detail this online planning process.

III-D Bridging the Local and Global Policies

The major shortcomings of the traditional short range planners (e.g., POMCP) in RAL-POMDP problems are due to the lack of proper guidance beyond its horizon. These methods may fall into the local minima and lead to a highly risky or suboptimal solution. In contrast, BVL bootstraps the graph-based global policy to guide the forward search during online planning and improve the safety guarantees and optimality.

There are two places where the cost-to-go information from graph-based global policy (e.g., FIRM) is being used.

1. When a new belief node $b$ is added to the BVL’s forward search tree, it is initialized using the cost-to-go from the underlying global graph (line 14 and 17 in Algorithm 4).

[TABLE]

where $\mathbb{U}(b)=\{\mu^{\cdot j}(b)\,|\,\mu^{\cdot j}\!\in\!\mathbb{M}(b)\}$ and $\mathbb{M}(b)$ is a set of local controllers that steers the belief from $b$ to its neighboring FIRM nodes $B^{j}\in\mathbb{N}(b)$. $\mathscr{C}(b,B^{j})$ is the estimated edge cost from $b$ to $B^{j}$, and $\tilde{J}^{g}(B^{j})$ is the cost-to-go computed by FIRM in the offline phase. The visitation counts are initialized to zeros, i.e., $N_{init}(b,u^{j})=0$ and $N_{init}(b)=0$. Note that the action space $\mathbb{U}(b)$ is only a subset of the continuous action space which is based on local controllers toward neighboring FIRM nodes.

2. The rollout policy selects an action by random sampling from a probability mass function which is based on FIRM’s cost-to-go rather than a uniform distribution (Algorithm 5). For each $B^{j}\in\mathbb{N}(\mathcal{b})$, the weight $w^{j}$ is computed as

[TABLE]

where $\mathcal{b}$ denotes the sampled belief state in the current Monte Carlo simulation. $\eta_{w}$ is a constant for the exploration bonus in the rollout policy. As $\eta_{w}$ gets larger, the rollout policy becomes more explorative. $\eta_{w}=\infty$ will lead the rollout to pure exploration, i.e., random sampling from uniform distribution.

Based on the computed weight $w^{j}$, the rollout policy selects an action by random sampling $u^{*}\sim p(u^{j};\mathcal{b})$ from the following probability mass function.

[TABLE]

III-E Discussion

We briefly highlight a few properties of the proposed algorithm in terms of optimality and safety.

Optimality: We first consider a small problem where the goal belief (with the known cost-to-go of 0) is within the finite horizon of POMCP from the beginning. Stand-alone POMCP is guaranteed to converge to the optimal solution [5], and it is trivial to prove that BVL converges to the optimal. In larger problems, the global optimality depends on the cost-to-go estimation for the leaf nodes of the POMCP tree. Note that finding the accurate cost-to-go estimation is as difficult as the original problem. Simple heuristics such as Euclidean distance heuristic typically provide poor cost-to-go estimation compared to the approximate long-range solvers, such as FIRM, that take uncertainty into account. Hence, in terms of optimality, BVL mostly outperforms POMCP. It should also be noted that the cost-to-go estimation of FIRM gets closer to the optimal with more and more samples [10], which can improve the overall optimality of BVL.

Safety: As discussed in Sec. II-B, minimizing the risk (the expected failure probability along the whole trajectory) is encoded as a soft constraint in the cost-to-go minimization problem. Based on the optimality analysis, BVL can achieve smaller expected cost-to-go from the initial belief than POMCP in non-trivial problems, which effectively leads to policies with less risk than POMCP policies. Since BVL adapts to the current belief during the online planning phase, it can provide higher safety than (offline) FIRM planner.

IV Experiments

IV-A Rover Navigation Problem

As a representative RAL-POMDP problem, we consider the real-world problem of the Mars rover navigation under motion and sensing uncertainty. In Rover Navigation Problem (RNP) introduced here, the objective is to navigate a Mars rover from a starting point to a goal location while avoiding risk regions such as steep slopes, large rocks, etc. The rover is provided a map of the environment which is created by a Mars orbiter satellite [15] and a Mars helicopter [16] flying ahead of the rover. This global map contains the location of landmarks, which serve as information sources that the rover can use to localize itself on the global map. The map also contains the location of risk regions that the rover needs to avoid and regions of science targets which needs to be visited by the rover to collect data or samples.

Motion model: The motion of the rover is noisy due to factors like wheel slippage, unknown terrain parameters, etc. In RNP introduced here, we assume a nonlinear motion model (but still a holonomic one to provide a simple benchmark). Specifically, we use the model in [17], where the state $x=[\prescript{g}{}{x},\prescript{g}{}{y},\prescript{g}{}{\theta}]^{T}\in\mathbb{R}^{3}$ represents the 2D position and heading angle of the rover in the global world frame. Control input $u\in\mathbb{R}^{3}$ represents the velocity of each coordinate. Using [17], we obtain the discrete motion model as follows:

[TABLE]

where $w$ is motion noise drawn from a Gaussian distribution with zero mean.

Observation model: In RNP, we assume the rover can measure the range and bearing to each landmark. Denoting the displacement vector to a landmark $L^{i}$ by $d^{i}=[d^{i}_{x},d^{i}_{y}]^{T}\equiv L^{i}-p$, where $p=[\prescript{g}{}{x},\prescript{g}{}{y}]^{T}$ is the position of the robot, the observation model is given by:

[TABLE]

where $v^{i}\sim\mathcalorg{N}(0,R^{i})$. The measurement quality degrades as the distance of the robot from the landmark increases, and the weights $\xi_{r}$ and $\xi_{\theta}$ control this dependency. $\sigma^{r}_{b}$ and $\sigma^{\theta}_{b}$ are the bias standard deviations.

Cost and risk metrics: In RNP, we consider the localization accuracy as well as the mission completion time as the main elements of the cost function.

Specifically, we consider a cost function under the Gaussian assumption as follows.

[TABLE]

where $P_{k}=\textup{cov}(x_{k}|z_{0:k})$ represents the second moment of the belief distribution as a measure of state uncertainty, and $\Delta t$ is the time step size for each action. $\xi_{p}$ and $\xi_{T}$ denote weights to combine these different objectives. In the experiments, we used $\xi_{p}=10$, $\xi_{T}=1$, and $\Delta t=0.005$. The risk in RNP denotes the expected probability of failure (collision with obstacles) along the whole trajectory under a policy as described in Section II-B. Note that the action cost in Eq. (36) is not directly related to the risk metric.

RNP scalability: To test algorithms under different RNP complexities, we parameterize the Rover Navigation Problem as $\textit{RNP}{}_{s}(e,o)$, where $e$ represents the size of the environment, $o$ represents the size/density of obstacles, and $s$ is the environment type. We compare three key attributes (safety, scalability, and optimality) of each algorithm in three different environments ($\textit{RNP}{}_{\textit{InfoTrap}{}}$, $\textit{RNP}{}_{\textit{ObsWall}{}}$, and $\textit{RNP}{}_{\textit{Forest}{}}$).

IV-B Baseline Methods

As baseline methods, we consider three algorithms. From the class of forward search methods, we consider the POMCP method [5]. From the approximate long-range methods, we consider the FIRM [10] and its variant [18, 19] which is referred to as online graph-based rollout (OGR) here.

FIRM: FIRM is an execution of closed-loop controls returned by its offline planning algorithm. FIRM relies on belief-stabilizing local controllers at each graph node to ameliorate the curse of history. Hence, it can solve larger problems, but it is usually suboptimal compared to optimal online planners.

OGR: OGR is an online POMDP solver that improves the optimality of a base graph-based method (particularly, FIRM in our implementation). At every iteration, an OGR planner selects the next action by simulating all different possible actions and picking the best one. Compared to BVL, OGR expands the belief tree in a full-width but only for one-step look-ahead. While it can improve the performance of its base graph-based planner, it is prone to local minima due to the suboptimality in base planner’s cost-to-go and OGR’s myopic greedy policy. Additionally, OGR discards the performed forward simulation results in the next iteration, while BVL leverages them at each step to enrich the underlying tree structure.

URM-POMCP: We extend POMCP to make it work in larger and continuous spaces such as RAL-POMDPs. We refer to it as URM-POMCP (Uniform RoadMap POMCP).

URM-POMCP uses a heuristic cost-to-go function to cope with the finite horizon limitation in POMCP.

IV-C Safety

Reducing risk and ensuring system’s safety is the most important goal of the proposed framework. We compare the risk aversion capability of BVL with the baseline methods on $\textit{RNP}{}_{\textit{InfoTrap}{}}(e,o)$ shown in Fig. 2, where $e$ is the length of the environment and $o$ is the length of the obstacle.

In $\textit{RNP}{}_{\textit{InfoTrap}{}}$ problems the rover needs to reach the goal by passing through the narrow passage without colliding with any obstacles. As shown in Fig. 2, BVL reduces risk of collision by executing a longer trajectory that goes close to the landmarks (yellow diamonds) and reduces the localization uncertainty before entering the narrow passage. Since the URM-POMCP algorithm plans in a shorter horizon and depends on a heuristic cost-to-go estimation beyond the horizon, it takes a greedy approach to go towards the goal thus taking a higher risk of colliding with the obstacles. This can also be seen in Fig. 3 that shows the probability of collision of the rover as the length of the obstacle increases. The probability of collision here was estimated by running 20 Monte Carlo simulations of rover executing policies by different planners.

In this work, the heuristic cost-to-go function $\tilde{J}(b,x)$ is implemented as $\tilde{J}(b,x)\approx\frac{d(b,b_{g})}{\Delta\dot{x}_{max}}(\xi_{p}\text{tr}({P_{c}})+\xi_{T}\Delta t)$, where $d(b,b_{g})$ is the Euclidean distance from the current belief state to the goal belief state, $\Delta\dot{x}_{max}$ is the (approximate) maximum velocity of the rover, and $P_{c}$ is the stationary covariance of $m\in\mathbb{X}$ for the current belief $b=(m,P)$. This heuristic optimistically assumes that the belief can reach the goal by following the direct path at the maximum velocity without collision with the obstacles.

To construct a finite action set for URM-POMCP, we utilize a uniformly distributed roadmap in belief space. Each point in the uniformly distributed roadmap serves as the target point of a time-varying LQG controller, so that the controller can generate control inputs for a belief to move toward the point. This enables POMCP to utilize the Gaussian belief model in generating control inputs and updating the belief from observations.

Additionally, we penalize the actions to stay at the same state to prevent the robot from getting stuck at local minima indefinitely.

IV-D Scalability in Planning Horizon

Bi-directional learning of the value function enables the proposed planner to scale to infinite-horizon planning problems with terminal state. To compare this scalability with the baseline methods, we consider $\textit{RNP}{}_{\textit{ObsWall}{}}(e,o)$ problems shown in Fig. 4, where $e$ is the length of the environment and $o$ is the length of the obstacle shown in the figure.

Notice that in Fig. 5, as the obstacle gets larger, the local minimum gets deeper and the performance of URM-POMCP becomes worse. The number of time steps to get to the goal grows exponentially for URM-POMCP, while it grows linearly for BVL and others. This shows the effectiveness of guidance by long-range solver’s global policy in larger problems as opposed to a naive heuristic guidance in URM-POMCP.

IV-E Optimality

The fundamental contribution of this method is to achieve policies that are closer to the globally optimal policies while reducing the risk of collisions over long horizons. To compare the optimality of the planners, we consider $\textit{RNP}{}_{\textit{Forest}{}}(e,o)$ problems shown in Fig. 6, where $e$ represents the length of the environment and $o$ represents the number of obstacles. We vary the density of the underlying belief graph to demonstrate its effect on the proposed method.

As can be seen in Fig. 7, the performance of the FIRM solution improves as the density of the underlying graph gets higher. However, it will reach a maximum suboptimal bound due to its sampling-based nature (i.e., it requires stabilization of the belief to the stationary covariance of the graph nodes before leaving them). In this complex environment, OGR with myopic online replanning frequently gets stuck at local minima, while it sometimes outperforms FIRM. Its performance is brittle and subject to the coverage of the underlying belief graph. In contrast, BVL performs well even with a smaller number of nodes in the underlying graph.

While actions of the BVL are selected from local controllers connecting to the nodes of the underlying belief graph, online belief tree search process fundamentally improves its behavior such that it is much less dependent on the density and coverage of the underlying graph. BVL not only generates trajectories that are much closer to global optimum but also reduces the risk of collision over an infinite horizon.

V Conclusion

In this paper, we proposed BVL, a novel bi-directional value learning algorithm that incorporates locally near-optimal forward search methods and globally safety-guaranteeing approximate long-range methods to solve challenging RAL-POMDP problems. As shown in Fig. 8, BVL provides better probabilistic safety guarantees than forward search methods (URM-POMCP) and is closer to the optimal performance than approximate long-range methods (FIRM). It also shows more consistency in different environments compared to online graph-based rollout methods (OGR).

In future work, we will study the theoretical properties of this algorithm more rigorously and extend this work to more general and challenging robotic applications, such as mobile manipulation.

We will also investigate another instance of BVL using a heuristic search-based belief space planner that can connect to the approximate global policy using multi-goal planning techniques.