# Kernel-based Diffusion Approximated Markov Decision Processes for Off-Road
Autonomous Navigation and Control

Junhong Xu1 Kai Yin2 Zheng Chen1 Jason M. Gregory3 Ethan A. Stump3 Lantao Liu1
1Luddy School of Informatics, Computing, and Engineering at Indiana
University, Bloomington, IN 47408, USA. E-mail: {xu14, zc11,
<EMAIL_ADDRESS>Group. E-mail<EMAIL_ADDRESS>Research
Laboratory. E-mail: {jason.m.gregory1<EMAIL_ADDRESS>
###### Abstract

We propose a diffusion approximation method to the continuous-state Markov
Decision Processes (MDPs) that can be utilized to address autonomous
navigation and control in unstructured off-road environments. In contrast to
most decision-theoretic planning frameworks that assume fully known state
transition models, we design a method that eliminates such a strong assumption
that is often extremely difficult to engineer in reality. We first take the
second-order Taylor expansion of the value function. The Bellman optimality
equation is then approximated by a partial differential equation, which only
relies on the first and second moments of the transition model. By combining
the kernel representation of the value function, we then design an efficient
policy iteration algorithm whose policy evaluation step can be represented as
a linear system of equations characterized by a finite set of supporting
states. We first validate the proposed method through extensive simulations in
$2D$ obstacle avoidance and $2.5D$ terrain navigation problems. The results
show that the proposed approach leads to a much superior performance over
several baselines. We then develop a system that integrates our decision-
making framework with onboard perception and conduct real-world experiments in
both cluttered indoor and unstructured outdoor environments. The results from
the physical systems further demonstrate the applicability of our method in
challenging real-world environments.

##  I Introduction

Decision-making of an autonomous mobile robot moving in unstructured
environments typically requires the robot to account for uncertain action
(motion) outcomes, and at the same time, maximize the long-term return. The
Markov Decision Process (MDP) is a very useful framework for formulating such
decision-theoretic planning problems [1]. Since the robot is moving in a
continuous space, directly employing the standard form of MDP needs a
discretized representation of the robot state and action. For example, in
practice the discretized robot states are associated with spatial tessellation
[2], and a grid-map like representation has been widely used for robot
planning problems where each grid is regarded as a discrete state; similarly,
actions are simplified as transitions to traversable grids which are usually
the very few number of adjacent grids in vicinity.

However, the discretization can be problematic. Specifically, if the
discretization is low in resolution (i.e., large but few number of grids), the
decision policy becomes a very rough approximation of the simplified
(discretized) version of the original problem; on the other hand, if the
discretization is high in resolution, the result might be approximated well,
but this will induce prohibitive computational cost and prevent the real-time
decision-making. Finally, the characteristics of state space might be complex
and it is inappropriate to conduct lattice-like tessellation which is likely
to result in sub-optimal solutions. See Fig. 1 for an illustration.

Figure 1:  In unstructured environments, the robot needs to make motion
decisions in the navigable space with spatially varying terrestrial
characteristics (hills, ridges, valleys, slopes). This is different from the
simplified and structured environments where there are only two types of
representations, i.e., either obstacle-occupied or obstacle-free. Evenly
tessellating the complex terrain to create a discretized state space cannot
effectively characterize the underlying value function used for computing the
MDP solution. (Picture credit: NASA)

Another critical issue lies in MDP’s transition model, which describes the
probabilistic transitions from a state to others. However, obtaining an
accurate probability distribution function for robot motion transitions is
oftentimes unrealistic even without considering spatiotemporal variability.
This is another important factor that significantly limits the applicability
of MDP in many real-world problems. Reinforcement learning [3] does not rely
on a transition model specification, but requires a large number of training
trials to learn the value function and the policy, which can be viewed as
another strong and difficult assumption in many robotic missions. Thus, it is
desirable that the demanding assumption of the known transition probability
function can be relaxed as some non-exact form and can be obtained without
cumbersome learning trials. This can be achieved by leveraging the major
characteristics of the transition probabilistic distribution (e.g., moments or
quantile of the distribution), which typically can be obtained (well
approximated) from lightweight historical data or offline tests [2]. If we
only assume such “partial knowledge" – first two moments – of the transition
model, we must re-design the modeling and solving mechanisms.

To address the above problems, we propose a diffusion-approximated and kernel-
based solution. Our primary focus is to develop a theoretically-sound and
practically-efficient solution to compute the optimal value function in
continuous-state MDPs without requiring a full probabilistic transition model.

Our contributions can be summarized as follows:

  * •

First, to relax the requirement of fully known transition functions, we apply
the second-order Taylor expansion to the value function. The Bellman-type
policy evaluation equation and Bellman optimality equation are then
approximated by a diffusion-type partial differential equation (PDE) which
only relies on the first and second moments of transition probability
distribution. The solution of the Taylored Bellman optimality equation can be
viewed as a infinite-horizon discounted reward diffusion process.

  * •

Second, to improve both the efficiency and the flexibility of the value
function approximation, we use kernel functions which can represent a large
number of function families for better value approximation. This approximation
can conveniently characterize the underlying value functions with a finite set
of discrete supporting states, leading to fast computation.

  * •

Finally, we develop an efficient policy iteration algorithm by integrating the
kernel value function representation and the Taylor-based diffusion-type
approximation to Bellman optimality equation. The policy evaluation step can
be represented as a linear system of equations with characterizing values at
the finite supporting states, and the only information needed is the first and
second moments of the transition function. This alleviates the need for
heavily searching in continuous/large state space and the need for carefully
modeling/engineering the transition probability.

The organization of the paper is as follows: we review the relevant literature
and provide background in formulating stochastic planning problems and value
function approximation methods in Section II and III, respectively. In Section
IV, we present the Taylored Bellman equation and the proposed policy iteration
algorithm. Section V provides evaluation results for various important
algorithmic properties of the proposed method. Then, we integrate the
perception into our framework to build an autonomy system and demonstrate
real-world experiments in Section VI–VIII. Finally, we conclude the paper and
discuss insights learned from the real-world experiments in Section IX.

##  II Related Work

In this section, we first provide a brief survey on the value function
approximation in Markov Decision Processes (MDPs). Then, we review
deterministic and stochastic path and motion planning methods developed in the
robotics community. We highlight our contributions at last.

###  II-A Value Function Approximation in Continuous-State MDPs

Motion planning under action uncertainty can be formulated as an MDP with
continuous state and action [4, 5]. Many existing works focus on value
function representation and approximation in large/continuous state space [3,
5]. Unlike in the discrete state space, the value function in the continuous
(or large) state space requires an infinite-sized table. Additionally,
computing state-values requires enumeration over every entry, which is
practically impossible [5]. In general, approximation of the value function is
needed to avoid this intractability when storing and computing the value
function over the continuous state space.

One simple approach to approximating the value function is by tessellating the
continuous state space into uniform grids. This method is popular in many
robotic applications [6, 7, 8, 9, 10]. However, this uniform tessellation
approach does not scale well to environments with complex geometric
structures, and it requires a large number of grids when the problem size
increases, known as the curse of dimensionaliy [11]. A more advanced
discretization technique that alleviates this problem is by adaptive
discretization [12, 13, 14, 15, 16].

Alternative methods tackle this challenge through representing and also
approximating the value function by a linear combination of basis functions or
some parametric functions [17, 5, 18]. The weights in the linear combination
can be optimized by minimizing the Bellman residual [19]. However, these
methods are not applicable in complicated problems because selecting a proper
set of basis functions is non-trivial. This weakness may be resolved through
kernel methods [20]. Because the weights in linear combination of basis
functions can be presented by their product (through so-called duel form of
least squares [21]), the product may be better replaced by kernel functions
(on so-called reproducing kernel Hilbert spaces) at supporting states. Once
value functions at supporting states are obtained, the approximation to value
function at any state is also determined. In [22], the authors use a Gaussian
process with a Radial Basis Kernel (RBF) to approximate the value function.
This approach to approximating the value function by kernel functions is
referred as the direct kernel-based method in this paper. There is a vast
literature on kernelized value function approximations in reinforcement
learning [23, 24, 25, 26]. But few studies in robotic planning problems
leveraged this approach. A recent application of work [23] on marine robots
can be found in [27]. In addition, due to the advancement in deep learning,
using neural networks to approximate value function has demonstrated successes
in a wide range of applications [28]. However, it is generally hard to find
suitable network architectures and hyperparameters to approximate the value
function [29] for different environments and robotic motion planning tasks.

Unfortunately, all these approaches rely on either fully known transitions in
MDP or the selection of basis functions, which are difficult to obtain in
practice. Therefore, the challenge becomes how to design a principled
methodology without explicitly relying on basis functions and without full
knowledge of transitions in MDP, which will be addressed in this work.

###  II-B Deterministic and Stochastic Planning in Robotics

Most of the traditional robotic planning methods (see [30] for a recent
survey) use a simplified deterministic robot motion model, e.g., holonomic
kinematics, to compute an open-loop path, e.g., sampling-based motion planners
such as probabilistic roadmap (PRM) and rapidly exploring random tree (RRT)
and their variants [16, 31]. Since the modeling error exists between the model
used in planning and the real world, the robot cannot execute the path
directly and requires a separate controller to track the path. To ensure the
path is trackable by the controller, refining the planned path into a
kinodynamically trackable trajectory [32] is necessary. One strategy to
generate such trajectories is based on trajectory optimization. Existing
trajectory optimization methods can be categorized into hard-constrained
methods and soft-constrained methods. Using hard-constrained optimization
technique to solve trajectory generation problem has been proposed by [33],
where piecewise polynomial trajectories are generated by solving a quadratic
programming (QP) problem. A closed-form solution is provided to solve an
unconstrained QP problem, and intermediate waypoints are added iteratively to
ensure the safety of the trajectories [34]. Free space represented by multiple
geometric volumes, e.g., cubes [35, 36], spheres [37, 38], and polyhedra [39,
40], are used to formulate a convex optimization problem, which generates
smooth trajectories within the volumes. However, hard-constrained methods
ignore the distance information to the obstacle boundaries and are prone to
generate trajectories close to the obstacles. Such safety issue motivates the
development of the soft-constrained methods which leverage the distance
gradient during optimization such that the trajectories could be adjusted to
stay far from obstacle boundaries while maintaining the smoothness property
[41, 42, 43].

The above design of the navigation system is based on deterministic models at
all levels, i.e., from path planning to trajectory optimization, and
uncertainties in the motion are entirely handled by the tracking controller.
Such decoupled design is brittle because it is possible that the feedback
controller cannot stabilize the open-loop path computed by the planner.
Therefore, to reduce the burden of designing and tuning separate controllers
for different environments, it is desirable to develop methods that directly
consider the action execution uncertainty during the planning phase if the
error between the planning model and the real-world action is large. This
problem is particularly obvious when the robot moves on rough terrains.

Early work of stochastic planning in the robotics community was mostly built
upon the sampling-based motion planning paradigm. In [44], the authors compute
local feedback controllers while adding new tree branches in the RRT algorithm
to stabilize the robot’s motion between two tree nodes. A similar idea has
also been exploited by [45] with a different planner and controller. This
method considers both the sensing and motion uncertainty and uses the linear-
quadratic Gaussian (LQG) controller [46] to stabilize the motion and sensing
uncertainties along the edges generated by a PRM algorithm. Alternatively,
[47] use the LQG controller to compute a distribution of paths, from which the
path is planned by the RRT algorithm that optimizes an objective function
based on the path distribution. Instead of modular design of the planning and
control components, [48] propose a more integrated approach to solving the
general continuous-time stochastic optimal control problem. The method first
approximates the original MDP by a discrete MDP through sampling points in the
state space, and then a closed-loop policy is computed in this discrete MDP.
The approximation and solution are then refined by iterating this sampling and
solving procedure.

Model predictive control (MPC), or receding horizon control, is another
strategy to resolve motion uncertainty [49, 50]. At each timestep, it solves a
finite horizon optimal control problem and applies the first action of the
computed open-loop trajectory online. To reduce computational burden, MPC
usually uses a deterministic model to predict future states [51]. Robust MPC
[52] explicitly deals with the model uncertainty by optimizing over feedback
policies rather than open-loop trajectories. However, this optimization is
computationally expensive. Thus, in practice, tube MPC that focuses only on a
sub-space is often used as an approximate solution to robust MPC [53]. Tube
MPC for nonlinear robot motion models is still an active research area. For
example, sum-of-squared programming [54, 55] and reachibility analysis [56,
57] are used for solving the nonlinear Tube MPC problem. More recently, Model
Predictive Path Integral (MPPI) control [58, 59, 60, 59, 61], a sampling-based
MPC approach to solve finite-horizon stochastic optimal control problems with
arbitrary cost function and nonlinear system dynamics is developed.

In general, robot motion planning essentially requires to reason about
uncertainties during motion control. However, because the prohibitive
computation prevents the robot from real-time decision, most of the
traditional approaches to generating feasible trajectories do not consider
uncertainties. Oftentimes some heuristics are leveraged to ensure the motion
safety during execution, e.g., using the soft-constrained methods. In
contrast, our proposed framework solves the stochastic planning problem in a
direct and integrative fashion. As a result, the computed policy can naturally
guide the robot with a large clearance from the obstacles, without adding
extra penalty in the objective function which can be hard to specify in
practice. In addition, most existing methods that solve the stochastic
planning problems avoid the difficulties in directly computing the optimal
value function through using arbitrary, e.g., non-linear or discontinuous,
reward and transition functions. Different from that, the proposed approach
makes the direct computation possible in practice, which opens a new line of
work to solve stochastic motion planning problems.

##  III Preliminary Background

###  III-A Markov Decision Processes

We formulate the robot decision-theoretic planning problem as an infinite
horizon discrete time discounted Markov Decision Process (MDP) with continuous
states and finite actions. It is defined by a 5-tuple
$\mathcal{M}\triangleq\langle\mathbb{S},\mathbb{A},T,R,\gamma\rangle$, where
$\mathbb{S}=\\{s\\}\subseteq\mathbb{R}^{d}$ is the $d$-dimensional continuous
state space and $\mathbb{A}=\\{a\\}$ is a finite set of actions. $\mathbb{S}$
can be thought of as the robot workspace in our study. A robot transits from a
state $s$ to the next $s^{\prime}$ by taking an action $a$ in a stochastic
environment and obtains a reward $R(s,a)$. Such transition is governed by a
conditional probability distribution $T(s,a,s^{\prime})\triangleq
p(s^{\prime}|s,a)$ which is termed as transition model (or transition
function); the reward $R(s,a)$, a mapping from a pair of state and action to a
scalar value, which specifies the one-step objective that the robot receives
by taking action $a$ at state $s$. The final element $\gamma\in(0,1)$ in
$\mathcal{M}$ is a discount factor which will be used in the expression of
value function.

We consider the class of deterministic policies $\Pi$, which defines a mapping
$\pi\in\Pi:\mathbb{S}\rightarrow\mathbb{A}$ from a state to an action. The
expected discounted cumulative reward for any policy $\pi$ starting at any
state $s$ is expressed as

$\displaystyle
v^{\pi}(s)=\mathbb{E}\left[\sum_{t=0}^{\infty}\gamma^{t}R(s_{t},\pi(s_{t}))|s_{0}=s\right].$
(1)

The state at the next time step, $s_{t+1}$, draws from distribution
$p(s_{t+1}|s_{t},\pi(s_{t}))$. Let us use $k$ to denote the computation epoch,
then we can rewrite the above equation recursively as follows

$\begin{split}v_{k+1}^{\pi}(s)=&R(s,\pi(s))+\gamma\,\mathbb{E}^{\pi}[v_{k}^{\pi}(s^{\prime})|s]\triangleq\mathcal{B}^{\pi}v_{k}^{\pi}(s),\end{split}$
(2)

where $\mathcal{B}^{\pi}$ is called the Bellman operator, and
$\mathbb{E}^{\pi}[v_{k}^{\pi}(s^{\prime})|s]=\int
p(s^{\prime}|s,\pi(s))v_{k}^{\pi}(s^{\prime})\,ds^{\prime}$. The computation
epoch $k$ is omitted in the rest of the paper for notation simplicity. Eq. (2)
is called Bellman equation. The function $v^{\pi}(s)$ is usually called the
state value function of the policy $\pi$. Solving an MDP amounts to finding
the optimal policy $\pi^{*}$ with the optimal value function which satisfies
the Bellman optimality equation

$v^{\pi^{*}}(s)=\max_{\pi}\left\\{R(s,\pi(s))+\gamma\,\mathbb{E}^{\pi}[v^{\pi}(s^{\prime})|s]\right\\}.$
(3)

###  III-B Approximate Policy Iteration via Value Function Representation

Value iteration and policy iteration are the most prevalent approaches to
solving an MDP. It has been shown that the value iteration and policy
iteration can both converge to the optimal policy in MDPs with discrete states
[4, 5]. Our work will be built upon policy iteration and here we provide a
summary of the common value function approximation methods used in policy
iteration when dealing with continuous states [62, 63, 64].

Policy iteration requires an initialization of the policy (can be random).
When the number of states in the MDP is finite, a system of finite number of
linear equations can be established based on the initial policy, where each
equation is exactly the value function (Eq. (2)). The solution to this linear
system yields state-values for all states of the initial policy [65]. This
step is called policy evaluation. The second step is to improve the current
policy by greedily improving local actions based on the incumbent values
obtained. This step is called policy improvement. Through iterating these two
steps, we can find the optimal policy and a unique solution to the value
function that satisfies Eq. (1) for every state.

If, however, the states are continuous or the number of states is infinite, it
is intractable to store and evaluate the value function at every state. One
must resort to approximate solutions by finding an appropriate representation
of the value function. Suppose that the value function can be represented by a
weighted linear combination of known functions where only weights are to be
determined, then a natural way to go is leveraging the Bellman-type equation,
i.e., Eq. (2), to compute the weights. Specifically, given an arbitrary
policy, the representation of value function can be evaluated at a finite
number of states, leading to a linear system of equations whose solutions can
be viewed as weights [66]. This obtained representation of value function can
be used to improve the current policy. The remaining procedure is then similar
to the standard policy iteration method. The final obtained value function
representation serves as an approximated optimal value function for the whole
continuous state space, and the corresponding policy can be obtained
accordingly.

Formally, let the value function approximation under policy $\pi$ be

$v^{\pi}(s)\simeq v(s;w^{\pi})=\sum_{i=1}^{m}w_{i}^{\pi}\cdot\phi_{i}(s),$ (4)

where $\phi_{i}\in\Phi\triangleq\\{\phi_{1},\ldots,\phi_{m}\\}$. The elements
in the set $\Phi$ are called the basis functions in literature [62], and these
basis functions are usually parametric functions with a fixed form. A finite
number of supporting states $\mathbf{s}=\\{s^{1},\ldots,s^{N}\\}$, $N>m$ can
be selected. The selection of supporting states $\mathbf{s}$ needs to take
into account the characteristics of the underlying value functions. In the
scenario of robot motion planning in complex terrains, it relates to the
properties of the terrain, e.g., geometry or texture. The solution to
$w^{\pi}$ can be calculated by minimizing the squared Bellman error over
$\mathbf{s}$, defined by
$\mathcal{L}(w^{\pi})=\sum_{i=1}^{N}({v(s^{i};w^{\pi})-\mathcal{B}^{\pi}v({s}^{i};w^{\pi}))^{2}}.$
And $w^{\pi}$ may have a closed form solution in terms of the basis functions,
transition probabilities, and rewards [66]. By policy iteration, the final
solution for $v(s;w^{\pi})$ can be obtained. Note that $v(s)$ may also be
approximated by any non-parametric nonlinear functions such as neural
networks.

##  IV Methodology

Our objective is to design a principled kernel-based policy iteration approach
by leveraging kernel methods to solve the continuous-state MDP. In contrast to
most decision-theoretic planning frameworks which assume fully known MDP
transition probabilities [1, 65], we propose a method that eliminates such a
strong premise which oftentimes is extremely difficult to engineer in
practice. To overcome this challenge, first we apply the second-order Taylor
expansion of the kernelized value function (Section IV-A). The Bellman
optimality equation is then approximated by a partial differential equation
which only relies on the first and second moments of transition probabilities
(Section IV-B). Combining the kernel representation of value function, this
approach efficiently tackles the continuous or large-scale state space search
with minimum prerequisite knowledge of state transition model (Sections IV-C
and IV-D).

###  IV-A Taylored Approximate Bellman Equation

Figure 2: Illustration of the boundary condition in Eq. (8a) using a 2D
example. The blue and green regions indicate the state space and the goal
region, respectively. The grey areas represent the infeasible space, e.g.,
obstacles, and the boundaries are shown as black curves. The normal vector
$\hat{\mathbf{n}}$ and the gradient of value function $\nabla v^{\pi}(s)$ at
three arbitrary boundary points are indicated by yellow and red arrows,
respectively.

To design an efficient approach for solving continuous-state MDP problems, we
essentially need to fulfill two requirements: a suitable representation of the
value function and efficient computation of the Bellman optimality equation.

For the first requirement, we may directly apply the basis functions to
approximate the value function and then solve the Bellman optimality equation.
Yet this approach faces difficulties of explicitly specifying basis functions.
If the set of basis functions is not rich enough, the approximation error can
be large. A better approach may be a direct application of kernel methods to
represent the value function (referred as the direct kernel-based method). We
will discuss this method later. But this representation does not satisfy the
second requirement, i.e., efficient evaluation of the Bellman optimality
equation. This is because a fully specified transition probability function
$p(\cdot|\cdot,\cdot)$ is required for computing the Bellman operator, but it
is usually hard to specify in the real world. In addition, even this
transition function can be obtained, the expectation
$\mathbb{E}^{\pi}[v^{\pi}_{k}(s^{\prime})]=\int
p(s^{\prime}|s,\pi(s))v_{k}^{\pi}(s^{\prime})ds^{\prime}$ generally does not
have a closed-form solution, and computationally expensive numerical
integration is typically entailed.

In contrast to directly solving Eq. (3) by value function approximation, we
consider an approximation to the Bellman-type equation at first and then apply
value function approximation. We approximate the Bellman equation by using
only first and second moments of transition functions. This will allow us to
obtain a nice property that a complete and accurate transition model is not
necessary; instead, only the important statistics such as mean and variance
(or covariance) will be sufficient for most real-world applications.
Additionally, we will see that evaluating the integration over the global
state space is not needed. From this perspective, our approximation can be
viewed as a local version of the original Bellman equation, i.e., it uses
local gradient information to approximate the integral of the value function
over possible next states.

Formally, we assume that the state space is of $d$-dimension and a state $s$
may be expressed as $s=[s_{1},s_{2},\ldots,s_{d}]^{T}$. Suppose that the value
function $v^{\pi}(s)$ for any given policy $\pi$ has continuous first and
second order derivatives (this can usually be satisfied with aforementioned
value function designs). We subtract both hand-sides by $v^{\pi}(s)$ from Eq.
(2) and then take Taylor expansions of value function around $s$ up to second
order [67]:

$\displaystyle-R(s,\pi(s))$
$\displaystyle=\gamma\left(\mathbb{E}^{\pi}[v^{\pi}(s^{\prime})\mid
s]-v^{\pi}(s)\right)-(1-\gamma)\,v^{\pi}(s)$ $\displaystyle=\gamma\int
p(s^{\prime}|s,\pi(s))(v^{\pi}(s^{\prime})-v^{\pi}(s))\,ds^{\prime}-(1-\gamma)\,v^{\pi}(s)$
$\displaystyle\simeq\gamma\,\Big{(}(\mu^{\pi}_{s})^{T}\,\nabla
v^{\pi}(s)+\frac{1}{2}\nabla\cdot\sigma_{s}^{\pi}\nabla
v^{\pi}(s)\Big{)}-(1-\gamma)\,v^{\pi}(s),$ (5)

where $\mu^{\pi}_{s}$ and $\sigma^{\pi}_{s}$ are the first moment (i.e., a
$d$-dimensional vector) and the second moment (i.e., a $d$-by-$d$ matrix) of
transition functions, respectively, with the following form

$\displaystyle(\mu^{\pi}_{s})_{i}$ $\displaystyle=\int
p(s^{\prime}|s,\pi(s))(s^{\prime}_{i}-s_{i})\,ds^{\prime},$ (6a)
$\displaystyle(\sigma^{\pi}_{s})_{i,j}$ $\displaystyle=\int
p(s^{\prime}|s,\pi(s))(s^{\prime}_{i}-s_{i})(s^{\prime}_{j}-s_{j})\,ds^{\prime},$
(6b)

for $i,j\in\\{1,\ldots,d\\}$; the operator
$\nabla\overset{\Delta}{=}[\partial/\partial s_{1},...,\partial/\partial
s_{d}]^{T}$ and the notation $\cdot$ in the last equation indicate an inner
product; the operator $\nabla\cdot\sigma^{\pi}_{s}\nabla$ is read as

$\displaystyle\nabla\cdot\sigma^{\pi}_{s}\nabla=\sum_{i,j}\sigma^{\pi}_{s_{i}s_{j}}\frac{\partial^{2}}{\partial
s_{i}\partial s_{j}}.$

To be concrete, we take a surface-like terrain for example and use that
surface as the decision-theoretic planning workspace, i.e.,
$s=[x,y]^{T}\,\overset{\Delta}{=}\,[s_{x},s_{y}]^{T}$. We have the expression
for the following operator

$\displaystyle\nabla\cdot\sigma^{\pi}_{s}\nabla=\sigma^{\pi}_{xx}\frac{\partial^{2}}{\partial
x^{2}}+\sigma^{\pi}_{xy}\frac{\partial^{2}}{\partial x\partial
y}+\sigma^{\pi}_{yx}\frac{\partial^{2}}{\partial y\partial
x}+\sigma^{\pi}_{yy}\frac{\partial^{2}}{\partial y^{2}}.$

Since Eq. (5) approximates calculation of Eq. (2) in the policy evaluation
stage, the solution to Eq. (5) thus provides the value function approximation
under current policy $\pi$. Eq. (5) also implies that we only need to use the
first $(\mu^{\pi}_{s})_{i}$ and second $(\sigma^{\pi}_{s})_{i,j}$ moments
instead of computing the expectation of the value function using the original
transition model $p(s^{\prime}|s,\pi(s))$ to evaluate the Bellman operator Eq.
(3) required in solving MDPs.

###  IV-B Approximate Bellman Optimality Equation via Diffusion-type PDE

Eq. (5) is a diffusion-type partial differential equation (PDE). It is an
approximation to the Bellman equation (7) through the second-order Taylor
expansion. We will need to solve this PDE to obtain an approximation
methodology to the Bellman optimality equation. Typically if solutions exit
for a PDE, there could be infinite solutions to satisfy the the PDE unless we
impose proper boundary conditions [68]. Therefore, we need to analyze the
necessary boundary conditions to Eq. (5).

In our problem settings, robots are not allowed to move out of free-space
boundaries. We first observe that the value function should not have values
outside of the feasible planning region, i.e., the state space $\mathbb{S}$,
and the value function should not increase towards the boundary of the region.
Otherwise it will result in actions that guide the robot outside the free
space. A practical boundary condition to impose can be that the directional
derivative of the value function with respect to the outward unit normal at
the boundary states is zero (see Fig. 2 for an illustration). It is not the
only condition that we can impose, but it is a relatively easier condition to
obtain solutions with desired behaviors. Second, in order to ensure that the
robot is able to reach the goal, we follow the conventional goal-oriented
decision-theoretical planning setup and constrain the value function at the
goal state to the maximum state-value in the state space $\mathbb{S}$. These
boundary conditions ensure that the policy does not control the robot outside
of the feasible regions (safety) and also leads the robot to the goal area.

Formally, let us denote the boundary of entire continuous planning region by
$\partial\mathbb{S}$ and the goal state by $s_{g}$. Suppose the value function
at $s_{g}$ is $v_{g}$. Section IV-A implies that the Bellman optimality
equation Eq. (3) can be approximated by the following PDE:

$\displaystyle 0$ $\displaystyle=$
$\displaystyle\max_{\pi}\Big{\\{}\gamma\,\Big{(}(\mu_{s}^{\pi})^{T}\nabla
v^{\pi}(s)+\frac{1}{2}\nabla\cdot\sigma_{s}^{\pi}\nabla v^{\pi}(s)\Big{)}+$
(7) $\displaystyle R(s,\pi(s))-(1-\gamma)\,v^{\pi}(s)\Big{\\}},$

with boundary conditions

$\displaystyle\sigma_{s}^{\pi}\,\nabla v^{\pi}(s)\cdot\hat{\bm{n}}$
$\displaystyle=$ $\displaystyle 0,\mbox{ on }\partial\mathbb{S}$ (8a)
$\displaystyle v^{\pi}(s_{g})$ $\displaystyle=$ $\displaystyle v_{g},$ (8b)

where $\hat{\bm{n}}$ denotes the unit vector normal to $\partial\mathbb{S}$
pointing outward, and condition (8a) constrains the directional derivative
with respect to this normal vector $\hat{\bm{n}}$ to be zero. The solution
$v^{\pi}$ of the above PDE can be interpreted as a diffusion process with
$\mu_{s}^{\pi}$ and $\sigma_{s}^{\pi}$ as drift and diffusion coefficients,
respectively. We show a 2D example of this boundary condition in Fig. 2. The
normal vectors $\hat{\bm{n}}$ at the boundary are perpendicular to the
gradient of the value function $\nabla v^{\pi}(s)$, which constrains the value
function from expanding outside the boundaries of the state space. In
addition, the direction of each gradient vector points toward the goal, and
this allows the policy to follow the value function gradient which guides the
robot to move in the goal direciton.

The condition (8a) is a type of homogeneous Neumann condition, and condition
(8b) can be thought of as a Dirichlet condition in literature [68]. This
elegantly approximates the classic Bellman optimality equation by a convenient
PDE representation. While Eq. (7) is a nonlinear PDE (due to the maximum
operator over all policies), our algorithm in the future section will allow to
solve a linear PDE for a fixed policy. In the next section, we will leverage
the kernelized representation of the value function to avoid difficulties of
directly solving PDE. The kernel method will help transform the problem to a
linear system of equations with unknown values at the finite supporting
states.

###  IV-C Kernel Taylor-Based Approximate Policy Evaluation

With aforementioned formulations, another critical research question is
whether the value function can be represented by some special functions that
are able to approximate large function families in a convenient way. We tackle
this question by using a kernel method to represent the value function. Thanks
to Eq. (7) which allows us to extend with kernelized policy evaluation for
Taylored value function approximation.

Specifically, let $k(\cdot,\cdot)$ be a generic kernel function† [20]. For a
set of selected finite supporting states
$\mathbf{s}=\\{s^{1},\ldots,s^{N}\\}$, let $\mathbf{K}$ be the Gram matrix
with $[\mathbf{K}]_{i,j}=k(s^{i},s^{j})$, and
$\mathbf{k}(\cdot,\mathbf{s})=[k(\cdot,s^{1}),\ldots,k(\cdot,s^{N})]^{T}$.
Given a policy $\pi$, assume the value functions at $\mathbf{s}$ are
$V^{\pi}=[v^{\pi}(s^{1}),\ldots,v^{\pi}(s^{N})]^{T}$. Then, for any state
$s^{\prime}$, the kernelized value function has the following form

$v^{\pi}(s^{\prime})=\mathbf{k}(s^{\prime},\mathbf{s})^{T}\,\left(\lambda\mathbf{I}+\mathbf{K}\right)^{-1}\,V^{\pi},$
(9)

where $\lambda\geq 0$ is a regularization factor. When $\lambda=0$, it links
to the kernel ordinary least squares estimation of $w^{\pi}$ in Eq. (4); when
$\lambda>0$, it refers to the ridge-type regularized kernel least squares
estimation [21]. Furthermore, Eq. (9) implies that as long as the values
$V^{\pi}$ are available, the value function for any state can be immediately
obtained. Now our objective is to get $V^{\pi}$ through Eq. (5) and boundary
conditions Eq. (8).

00footnotetext: † It is worth mentioning that our approach of utilizing kernel
methods is to approximate the function. This usage should be distinguished
from that in the machine learning literature where kernel methods are used to
learn patterns from data.

Plugging the kernelized value function representation into Eq. (5), we end up
with the following linear system:

$\left(\mathbf{M}^{\pi}\left(\lambda\mathbf{I}+\mathbf{K}\right)^{-1}-(1-\gamma)\,\mathbf{I}\right)\,V^{\pi}=\mathbf{R}^{\pi},$
(10)

where $\mathbf{I}$ is an identity matrix, $\mathbf{R}^{\pi}$ is a $N\times 1$
vector with element $[\mathbf{R}^{\pi}]_{i}=-R(s^{i},\pi(s^{i}))$, and
$\mathbf{M}^{\pi}$ is a matrix whose elements are:

$[\mathbf{M}^{\pi}]_{i,j}=\gamma\left((\mu^{\pi}_{s^{i}})^{T}\nabla_{s^{i}}+\frac{1}{2}\nabla_{s^{i}}\cdot\sigma_{s^{i}}^{\pi}\nabla_{s^{i}}\right)k(s^{i},s^{j}).$
(11)

Note that $\nabla_{s^{i}}$ indicates the derivatives with respect to $s^{i}$,
i.e., $\nabla_{s^{i}}\overset{\Delta}{=}[\partial/\partial
s^{i}_{1},\ldots,\partial/\partial s^{i}_{d}]^{T}$. Here, we provide a
concrete derivation of using the Gaussian kernel to our proposed kernel
Taylor-Based approximate method as it is a commonly used kernel in practice
and often used in the studies of kernel methods.

Gaussian kernel functions on states $s^{\prime}$ and $s$ have the form
$k(s^{\prime},s)=c\times\exp\left((-\frac{1}{2}(s^{\prime}-s)^{T}\Sigma^{-1}_{s}(s^{\prime}-s)\right)$,
where $c$ is a constant and $\Sigma_{s}$ is a covariance matrix. Note that
$\Sigma_{s}$ is referred to as the length-scale parameter in our work. The
lengthscale governs the “smoothness" of the function, and a large lengthscale
leads to a smooth function whereas a small lengthscale causes a rugged
function. Due to limited space, we only provide formula below for the first
and second derivatives of the Gaussian kernel functions. These formula are
necessary when Gaussian kernels are employed (for example, Eq.(11)). In fact,
we have

$\nabla_{s^{\prime}}k(s^{\prime},s)=-\Sigma_{s}^{-1}(s^{\prime}-s)k(s^{\prime},s),$
(12)

and

$\begin{split}&\nabla_{s^{\prime}}\cdot\sigma_{s}\nabla_{s^{\prime}}k(s^{\prime},s)=-tr(\sigma_{s}\,\Sigma_{s}^{-1})\,k(s^{\prime},s)\\\
&+(s^{\prime}-s)^{T}\Sigma_{s}^{-T}\sigma_{s}\,\Sigma_{s}^{-1}(s^{\prime}-s)k(s^{\prime},s),\end{split}$
(13)

where $tr(\cdot)$ denotes the trace of the matrix. By plugging Eq. (12) and
Eq. (13) into Eq. (11), we can obtain a closed-form expression for the
elements in the matrix $\mathbf{M}^{\pi}$.

The solutions to the system Eq. (10) yield values of $V^{\pi}$. These values
further allow us to obtain the value function (9) for any state under current
policy $\pi$. This completes modeling our kernel Taylor-based approximate
policy evaluation framework.

###  IV-D Kernel Taylor-Based Approximate Policy Iteration

Algorithm 1 Kernel Taylor-Based Approximate Policy Iteration

1: A set of supporting states
$\mathbf{s}=\\{\mathbf{s}^{1},...,\mathbf{s}^{N}\\}$; the kernel function
$k(\cdot,\cdot)$; the regularization factor $\lambda$; the MDP
$\langle\mathbb{S},\mathbb{A},T,R,\gamma\rangle$.

2:The kernelized value function Eq. (9) for every state and corresponding
policy.

3:Initialize the action at the supporting states.

4:Compute the matrix $\mathbf{K}+\lambda\mathbf{I}$ and its inverse.

5:repeat

6: // Policy evaluation step

7: Solve for $V^{\pi}$ according to Eq. (10) in Section IV-A.

8: // Policy improvement step

9: for $i=1,...,N$ do

10: Update the action at the supporting state $s^{i}$ based on Eq. (14).

11: end for

12:until actions at the supporting states do not change.

(a) Kernel Taylor-based PI

(b) Direct kernel-based PI

(c) NN

(d) Grid-based PI

Figure 3:  Evaluation with a traditional simplified scenario where obstacles
and goal are depicted as red and green blocks, respectively. We compare the
final value function and the final policy obtained from (a) kernel Taylor-
based PI, (b) direct kernel-based PI, (c) NN, and (d) grid-based PI. A
brighter background color represents a higher state value. The policies are
the arrows (vector fields), and each arrow points to some next waypoint.
Orange dots denote supporting states or the grid centers (in the case of grid-
based PI).

With the above formulations, our next step is to design an implementable
algorithm that can solve the continuous-state MDP efficiently. We extend the
classic policy iteration mechanism which iterates between the policy
evaluation step and the policy improvement step until convergence to find the
optimal policy as well as its corresponding optimal value function.

Because our kernelized value function representation depends on the finite
number of supporting states $\mathbf{s}$ instead of the whole state space, we
only need to improve the policy on $\mathbf{s}$. Therefore, the policy
improvement step in the $(k+1)$-th iteration is to improve the current policy
at each support state

$\displaystyle\pi_{k+1}(s)$
$\displaystyle=\operatorname*{arg\,max}_{a\in\mathbb{A}}\Big{\\{}R(s,a)+$ (14)
$\displaystyle\gamma\Big{(}(\mu_{s}^{a})^{T}\,\nabla+\frac{1}{2}\nabla\cdot\sigma_{s}^{a}\nabla\Big{)}v^{\pi_{k}}(s)\Big{\\}},$

where $s\in\mathbf{s}$, $\pi_{k}$ and $\pi_{k+1}$ are the current policy and
the updated policy, respectively. Note that $\mu_{s}^{a}$ and $\sigma_{s}^{a}$
depend on $a$ through the transition function $p(s^{\prime}|s,a)$ in Eq. (6).
Compared with the approximated Bellman optimality equation (Eq. (7)), Eq. (14)
drops the term $(1-\gamma)v^{\pi_{k}}(s)$. This is because $v^{\pi_{k}}(s)$
does not explicitly depend on action $a$. The value function of the updated
policy satisfies $v^{\pi_{k+1}}(s)\geq v^{\pi_{k}}(s)$ [4]. If the equality
holds, the iteration converges.

The final kernel Taylor-based policy iteration algorithm is pseudo-coded in
Alg. 1. It first initializes the actions at the finite supporting states and
then iterates between policy evaluation and policy improvement. Since the
supporting states as well as the kernel parameters do not change, the
regularized kernel matrix and its inverse are computed only once at the
beginning of the algorithm. This greatly reduces the computational burden
caused by matrix inversion. Furthermore, due to the finiteness of the
supporting states, the entire algorithm views the policy $\pi$ as a table and
only updates the actions at the supporting states using Eq. (14). The
algorithm stops and returns the supporting state values when the actions are
stabilized. We can then use these state-values to get the final kernel value
function that approximates the optimal solution. The corresponding policy for
every continuous state can then be easily obtained from this kernel value
function [69].

Intuitively, this proposed framework is efficient and powerful due to the
following reasons: (1) by approximating the Bellman-type equation using the
PDE, we eliminate the necessity in requiring a full transition function and
the difficulty in computing the expectation over the next state-values; (2)
rather than tackling the difficulties in solving the PDE , we use the kernel
representation to convert the problem to a system of linear equations with
characterizing values at the finite discrete supporting states. From this
viewpoint, our proposed method nicely balances the trade-off between searching
in finite states and that in infinite states. In other words, our approach
leverages the kernel methods and Bellman optimal conditions under practical
assumptions.

##  V Algorithmic Performance Evaluation

Before testing the applicability of our proposed method in real-world
environments, we conducted algorithmic evaluations in two sets of simulations
in order to validate the efficiency of the proposed framework.

To do so, we eliminate the real-world complexities that are not the focus of
the main method (e.g., perception, partial observability, and online re-
planning). The first evaluation (Section V-A) is a goal-oriented planning
problem in a simple environment with obstacle-occupied and obstacle-free
spaces. In the second evaluation (Section V-B), we demonstrate that our method
can be applied to a more challenging navigation scenario on Mars surface [70],
where the robot needs to take into account the elevation of the terrain
surface (i.e., “obstacles" become continuous and are implicit).

(a) $6\times 6$

(b) $7\times 7$

(c) $10\times 10$

(d) $11\times 11$

Figure 4:  The performance matrix obtained by the hyperparameter search using
(a) $6\times 6$; (b) $7\times 7$; (c) $10\times 10$; and (d) $11\times 11$
evenly-spaced supporting states. Rows and columns represent different Gaussian
kernel lengthscale and regularization parameters, respectively. The numbers in
the color map represent the average return of the final policy obtained using
the corresponding hyperparameter combination. The colorbar is shown on the
right side of each table.

###  V-A Plane Navigation

####  V-A1 Setup:

Our first test is a 2D plane navigation problem, where the obstacles and the
goal area are represented in a $10m\times 10m$ environment, as shown in Fig.
3. The state space for this task is a 2-dimensional Euclidean space, i.e.,
$s=[s_{x},s_{y}]^{T}$ and $s\in\mathbb{S}\subseteq\mathbb{R}^{2}$. The action
space is a finite set with a number of $Q$ points
$\mathbb{A}(s)=\\{a_{i}(s)|i\in\\{1,...,Q\\}\\}$. Each point
$a_{i}(s)=[s_{x}+r\cos(\frac{2\pi i}{Q}),s_{y}+r\sin(\frac{2\pi i}{Q})]^{T}$
is an action generated on a circle centered at the current state with a radius
$r$. In this evaluation, we set the number of actions $Q=12$ and the action
radius as $r=0.5m$. An action point can be viewed as the “carrot-dangling"
waypoint for the robot to follow, which serves as the input to the low-level
motion controller. For the reward function, we set the reward of arriving at
the goal and obstacle states to be $+1$ and $-1$, respectively. Since the
reward now depends on the next state, we use Monte-carlo sampling to estimate
the expectation of $R(s,a)$. The discount factor for the reward is set to
$\gamma=0.9$. We set the obstacle areas and the goal as absorbing states,
i.e., the robot cannot transit to any other states if they are in these
states. To satisfy the boundary condition mentioned in Section IV-B, we allow
the robot to receive rewards at the goal state, but it cannot receive any
reward if its current state is within an obstacle.

####  V-A2 Performance measure:

Since the ultimate goal of planning is to find the optimal policy, our
performance measure is based on the quality of the policy. A policy is better
if it achieves a higher expected cumulative reward starting from every state.
Because it is impossible to evaluate over an infinite number of states, we
numerically evaluate the quality of a policy using the average return
criterion [71]. In detail, we first uniformly sample $Q=10^{4}$ states to
ensure a thorough performance evaluation. Then, for each sampled state, we
execute the policy for multiple trials, i.e., generating multiple
trajectories, where each trajectory ends when it arrives at a terminal state
(goal or obstacle) or reaches an allowable maximal number of steps. This
procedure gives us an expected performance of the policy at any state by
averaging the discounted sum of rewards over all the trajectories starting
from it. Now, we can calculate the average return criterion by averaging over
the performance of sampled states. A higher value of the average return
implies that, on average, the policy gives better performance over the entire
state space.

####  V-A3 Results:

Figure 5:  The comparison of the average return of the policies computed from
the four algorithms. The x-axis is the number of supporting states/grids used
in computing the policy. The y-axis shows the average return.

(a)

(b)

Figure 6:  Computational time comparisons of the four algorithms with changing
number of states. (a) The computational time per iteration. (b) Number of
iterations to convergence.

We are interested in examining the performance trends under varying supporting
states and related parameters. To achieve this goal, we place evenly-spaced
supporting states (in a lattice pattern) with different spacing resolution.
Besides the number of states, the kernel lengthscale and the regularization
parameter $\lambda$ are the other two hyperparameters governing the
performance of our algorithm. We present the grid-based hyperparameter search
results using four different configurations of supporting states shown in Fig.
4. The lengthscale and regularization parameters are searched over a set of
values whose range is pre-estimated by the work-space dimensions and
configurations, $\\{0.5,1,1.5,2,2.5,3\\}$. By entry-wise comparison among the
four matrices in Fig. 4, we can observe that increasing the number of states
leads to improving performance in general. However, we can find that the best
performed policy is given by the $10\times 10$ supporting states configuration
(Fig. LABEL:sub@fig:taylor-pi-100) which is not the scenario with the best
spacing resolution. This indicates that a larger number of states can also
result in a deteriorating solution, and the performance of the algorithm is a
matter of how the supporting states are placed (distributed), instead of the
number (resolution) of state discretization. Furthermore, we can gain some
insights on how to select the hyperparameters based on the number of
supporting states. Low-performing entries (highlighted with red) occur more
often on the left side of the performance matrix when the number of supporting
states increases. It implies that with more supporting states, the algorithm
requires a stronger regularization (i.e., greater $\lambda$ described in
Section IV-C). On the other hand, high-performing policies (indicated by
yellow) appear more on the bottom of the performance matrix when a greater
number of supporting states present, which means that a smaller length scale
is generally required given a larger quantity of supporting states.

We further compare our kernelized value function representation against other
three variants of the value function approximations. Specifically, the first
one is the direct kernel-based approximation method using a Gaussian kernel.
This method is similar to the one in [24], but with a fully known transition
function. The second one uses neural networks (NNs) as the value function
approximator. We set up the NN configuration similar to a recent work [72]. In
detail, we use a shallow two-layer network with 100 hidden units in each
layer. Its parameters are optimized through minimizing the squared Bellman
error via gradient descent. Since there is no closed-form solution to compute
the expected next state value when using NN as a function approximator, we use
Monte-Carlo sampling to estimate the expected value at the next states
$\mathbb{E}[v^{\pi}(s^{\prime})|s]$. The third method is the discrete grid-
based approximation method. It first transforms the continuous MDP to a
discrete version, where each state is regarded as a grid. Then, it uses the
vanilla policy iteration to solve the discretized MDP.

The comparison among above methods aims at investigating two important
questions:

  1. 1.

How does the kernelized value function representation perform compared to
other representations (NN and grid-based) in terms of the final policy
performance?

  2. 2.

Compared to our method, the direct kernel-based method not only requires the
fully known transition function, but also restricts the transition to be a
Gaussian distribution. Can our method with only mean and variance obtain
similar performance as the direct kernel-based method?

To answer these questions, we choose the transition function as a Gaussian
distribution. Its mean is the selected (intended) next waypoint. We set the
standard deviation of the transition function to be $0.2m$ in both axes during
the experiment. The transition probability models the accuracy of the low-
level motion controller: more accurate controller leads to smaller
uncertainty. To perform fair comparisons, we use the same supporting states
and apply hyperparameter search across all the methods.

The results for four methods are shown in Fig. 5 in terms of the average
return. The first question is answered by the fact that the kernel-based
methods (our kernel Taylor-based and the direct kernel-based PI) consistently
outperform the other two methods. Moreover, our method has the performance as
good as the direct kernel-based method which however requires the prerequisite
full distribution information of the transition. This indicates that our
method can be applied to broader applications that do not have full knowledge
of transition functions. In contrast to the grid-based PI, the kernel-based
algorithms and NN can achieve moderate performance even with a small number of
supporting states. It implies that the continuous representation of the value
function is crucial when supporting states are sparse. However, increasing the
number of states does not improve the performance of the NN.

In Fig. 6, we compare the computational time and the number of iterations to
convergence. The computational time of our method is less than the grid-based
method as revealed in Fig. 6LABEL:sub@fig:computation-time. We notice that
there is a negligible computational time difference between our method and the
direct method. As a parametric method, NN has the least computational time,
and only linearly increases, but it does not converge as indicated by Fig.
6LABEL:sub@fig:convergence-iter.

The function values and the final policies are visualized in Fig. 3. All the
methods except for the NN obtain reasonable approximations to the optimal
value function. Compared to our method, the values generated by the grid-based
method are discrete “color blocks", thus the obtained policy is non-smooth.

(a)

(b)

(c)

(d)

Figure 7:  Supporting state distributions and the policies for evenly-spaced
selection and importance sampling-based selection. The 3D surface shows the
Mars digital terrain model obtained from HiRISE. Supporting states and
policies are shown in black dots and vector fields, respectively. The colored
lines represent sampled trajectories, which initiate from four different
starting positions. (a)(b) The evenly-spaced supporting states and the
corresponding policy and trajectories; (c)(d) The supporting states generated
by importance sampling and the corresponding policy and trajectories.

###  V-B Martian Terrain Navigation

In this evaluation, we consider the autonomous navigation task on the surface
of Mars with a rover. We obtain the Mars terrain data from High Resolution
Imaging Science Experiment (HiRISE) [73]. Since there is no explicitly
presented “obstacle", the robot only receives the reward when it reaches the
goal. If the rover attempts to move on a steep slope, it may be damaged and
trapped within the same state with probability proportional to the slope
angle. Otherwise, its next state is distributed around the desired waypoint
following by the current action. This indicates that the underlying transition
function should be the mixture of these two factors, and it is reasonable to
assume that the means of the two cases are given by the current state and the
next waypoint, respectively. We can similarly have an estimate of the
variances. The mean and the variance of the transition function can then be
computed using the law of total expectation and total variance, respectively.

Due to the complex and unstructured terrestrial features, evenly-spaced
supporting state points may fail to best characterize the underlying value
function. Also, to keep the computational time at a reasonable amount while
maintaining a good performance, we leverage the importance sampling technique
to sample the supporting states that concentrate around the dangerous regions
where there are steep slopes. This is obtained by first drawing a large number
of states uniformly covering the whole workspace. For each sampled state, we
then assign a weight proportional to its slope angle. Finally, we resample
supporting states based on the weights. To guarantee the goal state to have a
value, we always place one supporting state at the center of the goal area.

Fig. 7LABEL:sub@fig:even-support-state and Fig. 7LABEL:sub@fig:weighted-
support-state compare the two methods for supporting state selections. The
supporting states given by the importance sampling-based method are dense
around the slopes. These supporting states better characterize the potentially
high-cost and dangerous areas than the evenly-spaced selection scheme. We
selected four starting locations from where the rover needs to plan paths to
arrive at a goal location. For each starting location, we conducted multiple
trials following the produced optimal policies. The trajectories generated
with the importance sampling states in Fig. 7LABEL:sub@fig:weighted-3d-policy
attempt to approach the goal (green star) with minimum distances, and at the
same time, avoid dangerous high-elevation terrains. In contrast, the
trajectories obtained using the evenly-spacing states in Fig.
7LABEL:sub@fig:even-3d-policy approach the goal in a more aggressive manner
which can be risky in terms of safety. It indicates that a good selection of
supporting states can better capture the state value function and thus produce
finer solutions. This superior performance can also be reflected in Fig.
8LABEL:sub@fig:bar-chart. The policy obtained by the uniformly sampled states
shows similar performance to the one generated by the evenly-spacing states,
both of which yield smaller average return than the importance-sampled case. A
top-down view of the policy is shown in Fig. 8LABEL:sub@fig:terrain-policy-
map-weighted-2d where the background color map denotes the elevation of
terrain.

(a)

(b)

Figure 8:  (a) The top-down view of the Mars terrain surface as well as the
policy generated by our method with the importance sampling selection. The
color map indicates the height (in meters) of the terrain. (b) The comparison
of average returns among three supporting state selection methods using the
same number of states. Red, green, and blue bars indicate the performance of
importance sampling selection, evenly-spaced selection, and uniform
distribution sampling selection, respectively.

##  VI Autonomy System Design

Figure 9: An overview of the system for navigation in prior-unknown
environments. The red arrows represent the direction of the information flow.
The global planner module in the dashed block is optional.

Previous algorithmic evaluations assume that a well-defined MDP can be
abstracted and constructed from the real-world problem, and focus on
algorithmic property/performance assessments. However, in reality, specifying
an MDP for stochastic motion planning problems requires a known task region
for constructing the state space as well as detailed information of the
environment for transition and reward functions. These requirements are
generally very difficult to satisfy if we deploy the robots into the real
world, especially in the off-road environment where the task region is hard to
define and no prior map is provided. In such cases, the robot needs to use its
onboard sensors to acquire information and make decisions online.
Specifically, it needs to process the observations, construct a new MDP within
the observed area, compute the corresponding policy, and execute the policy.
This process should repeat until the task is completed. In this paper, we only
focus on generating a local policy within the observable area. Note that, the
global planning in any unknown environment might require information-driven
(active) sensing to achieve a trade-off between the exploration of unknown
space and the exploitation of observed space, which however is not the focus
in this work. In this section, we present our autonomy system that extracts
the MDP elements (supporting states, transition function, and reward function)
from sensor data and connect the perception to the action loop. Fig. 9
provides an overview of the system.

###  VI-A Perception

The perception module is responsible for processing sensor observation and
providing information for planning. It generates the robot state (pose) and a
map of the environment in the field of view. The pose information can be
estimated by any existing localization methods (e.g., [74]). The type of the
map is important as it represents the information about what factors in the
environment the robot needs to consider during planning. Because navigating on
uneven terrains requires both the occupancy information for identifying
feasible regions and the elevation information for reasoning about the
physical interaction between the robot and the terrain surface [75], we use a
cost map computed from an occupancy map and an elevation map, or a combination
of both to represent the environment.

###  VI-B MDP Construction

The second part of the system builds an MDP using the mapping information. In
the following, we provide detailed explanations of this construction process
for each MDP element.

####  VI-B1 State and Action Spaces:

The state and action spaces are pre-defined with the given vehicle type
(ground). Specifically, the state is represented as the robot pose with
position and orientation information $(x,y,\theta)$, and the action consists
of linear and angular speeds $(v,\omega)$. Since our method deals with the
discrete action space, we can tessellate the continuous action into evenly-
spaced intervals. The discretization resolution, as well as the minimum and
maximum values for the speeds, depend on the capability of the robot and the
complexity of the task.

####  VI-B2 Reward Function:

The reward function consists of two parts
$R(s)=(1-\mathbb{I}_{s_{g}}(s))c(s)+r_{g}\mathbb{I}_{s_{g}}(s)$, where the
first part $c(s)$ is the obstacle penalty provided by the cost map,
$\mathbb{I}_{s_{g}}(s)$ is an indicator function for checking whether the
state belongs to the goal region $s_{g}$, and $r_{g}$ is the goal reward. If
the cost map is not used, the obstacle penalty part is not included, and the
robot only receives a positive reward if it arrives at the goal. Since we
focus on planning within the observable space only, we need to generate a
dummy goal region $s_{g}$ in the space of the field of view. In other words,
it needs to set a temporary goal if the final goal region is outside of the
robot’s current observed area. We use two heuristic approaches to generate
this temporary goal. The first one is inspired by the frontier-based concept
which generates a temporary goal chosen at the boundary of the current
observable map and with the least distance towards the final goal [76, 77].
The second method utilizes the path generated by a global planner if available
(usually just for high level guidance). The temporary goal can be determined
by “cropping" a path segment from the global path within the field of view
only, where the path segment starts from the current robot pose and extends to
a pre-defined length along the path, and the final pose on the extracted path
segment serves as the temporary goal. Note that such a global planner is not
mandatory for our method as it can be replaced with the frontier-based goal
selection method.

####  VI-B3 First Two Moments of the Transition Function:

We assume that the robot motion is generated based on
$s^{\prime}=f(s,a,h)+\epsilon$, where $f(s,a,h)$ is a deterministic function
representing the discrete-time motion model related to vehicle dynamics, e.g.,
a differential drive model; $h(s)$ is the slope angle at state $s$ derived
from the elevation map; and $\epsilon$ is a noise term independent of the
state and action. If the elevation map is used, the motion model considers the
effect of the elevation on the robot’s motion. Otherwise, we treat $h(s)=0$
(flat surface) for all states in the current state space. To construct
$f(s,a,h)$, we modify the Dubin’s car model to take into consideration of the
terrain slope, $x^{\prime}=(\frac{\pi}{2}-h(s))(x+v\Delta
t\cos{\theta})+h(s)x$, $y^{\prime}=(\frac{\pi}{2}-h(s))(y+v\Delta
t\sin{\theta})+h(s)y$, and $\theta^{\prime}=\theta+\omega\Delta t$. Here,
$h(s)\in[0,\frac{\pi}{2}]$ is the slope angle at state $s$ and $\Delta t$ is
the time discretization resolution. Intuitively, this model penalizes the
distance traveled on the surface with larger slope angle. Based on this
formulation and Eq. (6), the first and second moments can be computed as
$\mu(s,a,h)=\Delta_{a}s+\mathbb{E}[\epsilon]$ and
$\sigma(s,a,h)=\mathbb{V}[\epsilon]+\mu(s,a,h)\mu(s,a,h)^{T}$, where
$\Delta_{a}s=f(s,a,h)-s$ is the state shift after applying action $a$. The
advantage of our method is obvious from the above modeling perspective. Since
we only need to model the mean and variance of the noise $\epsilon$, it is not
necessary to acquire the exact probability distribution of the noise.

####  VI-B4 Supporting States:

The last part of the construction is state sampling, which is responsible for
distributing the supporting states within the state space. The positions and
the number of the supporting states are critical as they determine the
accuracy of the value function and also the computational time. More
supporting states generally provide a better estimation of the optimal value
function but require more time to compute as shown in Section V-A. We consider
uniform sampling, importance sampling, and trajectory sampling strategies in
this work. All these sampling methods require a region to distribute the
supporting states. The first two methods introduced in Section V-A and Section
V-B need prior knowledge of the environment and specifying a sampling region
manually, e.g., a rectangular workspace. Uniform sampling tessellates a pre-
defined region into equal-sized cells and uses the cell vertices as the
supporting states. The importance sampler first uniformly samples a large
number of states and then selects them based on some weighting criteria, e.g.,
the slope of the terrain at a given state point. The trajectory sampler
utilizes the global path generated by a global planner as a heuristic to
define the planning and sampling region. It distributes the state samples
around the same path segment extracted from the global trajectory-based goal
generation method. This method is especially useful when the fast online
computation is required, since it does not need to search over the entire
planning region.

###  VI-C Policy Execution

With all the MDP elements ready, we can use Alg. 1 to compute a policy for the
constructed MDP. Then, this policy is used for generating control actions
$a=(v,\omega)=\pi(s)$. It is necessary to discuss how this system can achieve
online execution as this is important for local motion control. Most of the
computation is spent on the policy iterations, which require
$\mathcal{O}(|\mathbb{A}|\times|\mathbf{s}|^{2})$ for policy improvement
(iterating over every action at every supporting state) and at most
$\mathcal{O}(|\mathbf{s}|^{3})$ for policy evaluation (solving the linear
system in Eq. (10)). Fortunately, once the policy is computed, it is
relatively fast to obtain an action, because it only involves iterating over
all the actions and searching for the best one at one state. Depending on the
scenario, we provide two ways to perform online execution. In the first
method, the policy computation and execution are separate: when the robot
arrives at a temporary goal, it waits until the policy computation converges.
Then, the robot carries out the newly computed policy. The benefit of this
method lies in that since the policy computation process is separate from the
subsequent online execution, it is suitable for those tasks needing
sophisticated supporting state sampling schemes. However, this separation may
cause the robot to have a discontinuous behavior with periodical pauses. The
second method uses an MPC-based idea by combining policy computation and
execution into one process. At each time step (when the robot takes an
action), we construct a new MDP and computes the corresponding policy. Then
the robot only takes one action from the policy and repeats the computation.
This method is suitable for the task where a small number of supporting states
is enough to ensure online computation and safe maneuver. Thus, practically
one can combine this online execution with the trajectory sampling, where a
global planner can provide a heuristic for the supporting state selection.

##  VII Simulation with LiDAR Perception

(a) A snapshot of the simulator

(b) The top-down view of the environment

Figure 10: A snapshot and the top-down view of the high fidelity simulator

To accomplish the off-road navigation autonomy system described above, we
first integrate the perception module and also test the system performance. In
this way, the robot is no longer provided with a prior map of the environment,
and it can observe the environment and obtain state information from its
onboard LiDAR.

(a) Easy

(b) Medium

(c) Hard

Figure 11:  The mission maps and sampled robot trajectories for each
difficulty level. The blue circles are the waypoints. The number of waypoints
decreases as the difficulty increases (from left to right). For each
difficulty level, the robot starts from the initial position, navigates
sequentially to the next points, and returns to the initial position. Two
sampled robot trajectories for our method are shown in blue lines.

The testing scenario is to navigate the robot safely without a prior map in a
cluttered outdoor environment that contains buildings and random obstacles.
Fig. 10 shows a snapshot of the simulator and a bird-eye view of the simulated
environment. The simulator simulates a ClearPath Warthog differential drive
ground vehicle equipped with a $64$-beam LiDAR. We set the robot’s minimum and
maximum values for linear and angular speeds as $v_{min}=-1m/s$,
$v_{max}=3m/s$, $\omega_{min}=-1.5rad/s$ (turning right), and
$\omega_{max}=1.5rad/s$ (turning left). We consider a task where the robot
needs to arrive at a set of pre-defined waypoints sequentially and return back
to the initial position. We define three levels of difficulties: easy, medium,
hard. The difficulty level increases as the number of waypoints decreases.
This is because the robot receives less guidance and needs to make more
decisions on its own. Fig. 11 shows the waypoint positions, the constructed
cost map, and two sampled trajectories for each difficulty level using our
method.

(a) Completion Time

(b) Average Velocity

Figure 12:  Performance comparison between our method and NMPC. (a) and (b)
show the time of task completion and the velocity averaged over the task. For
both figures, the $x$-axis indicates the difficulty level. All the experiments
are averaged over $10$ trials.

We compare our method with a baseline local planner, nonlinear model
predictive control (NMPC) [78], which generally represents a set of
conventional, motion primitive-based planning algorithms from literature and
has demonstrated great applicability in real-world environments [79]. This
deterministic planner produces smooth trajectories that respect the robot’s
dynamics by optimizing a cost function that penalizes the path deviation from
a desired global trajectory using algorithms from the NLOPT library [80].
Here, an anytime version of A* (ARA*) [81] is employed as the global planner
to provide the desired reference trajectory and a PID tracking controller is
used to execute the optimized path. This pipeline is often adopted by the
traditional three-layer planning and control system. To make a fair
comparison, we also leverage the global planner to generate temporary goal
points and sampling supporting states, which are mentioned in Section VI. In
this experiment, we choose $6m$ as the length of trajectory sampling, which
means that the robot plans $2s$ in the future if it uses the maximum speed
(which is $3m/s$). For our method, the actions are discretized evenly into
$8\times 8$ intervals which are dense enough to approximate continuous
actions.

We use completion time and averaged linear velocities as the performance
metrics. The completion time is the total travel time for finishing the task,
and the smaller the better. The averaged velocities reflect the motion
smoothness of the robot (within speed limit) and thus the larger the better.
Compared to the averaged cumulative rewards, these metrics are more relevant
to the robot navigation task. The result is shown in Fig. 12. In general, our
method is more efficient and can complete the task in a shorter time as shown
in Fig. 12LABEL:sub@fig:time-comparison. Since the NMPC method attempts to
follow the global path exactly, the trajectory optimized by NMPC sometimes
oscillates around and overshoots the global path. This oscillation causes the
vehicle to reduce its speed due to frequent turning. In contrast, since our
method considers the uncertainty (via the second moment) of the robot’s
motion, it can reason over some areal coverage around the global path. As a
result, as long as the robot remains within the area supported by the sampled
states, it does not show the oscillation behavior. We can also observe this
result from Fig. 12LABEL:sub@fig:v-comparison where our method’s average
velocity is larger than the NMPC approach.

It is also worth pointing that, the robot uses a shorter time to complete the
task in the hard mission than in the easy one, but the linear velocity in the
hard mission is smaller than the one in the easy mission. This can be
explained by observing the trajectories the robot generated in the two
missions in Fig. 11. Although the easy mission provides denser waypoints, the
robot is restricted to follow these predefined points which are not
necessarily the shortest route. In contrast, the hard mission’s waypoints are
sparse and thus the mission has less restriction on which trajectory the robot
should follow. As a result, the robot chooses a shortcut to navigate from
Waypoint 2 to Waypoint 3 as shown in Fig. 11LABEL:sub@fig:market-loop-hard.

##  VIII Real-World Demonstrations in Indoor Cluttered and Outdoor
Unstructured Environments

(a)

(b)

(c)

(d)

(e)

(f)

(g)

(h)

(i)

(j)

(k)

(l)

Figure 13:  The indoor environment where our method is tested for validating
its basic obstacle-avoidance behavior. The first and third rows show the
vehicles’ behavior at different timesteps, and the second and fourth rows
depict the corresponding visualization results. The task for the robot is to
navigate through the gap between boxes (approximately $1.5m$) and arrive at
the green square shown in the second row, and then return back to the initial
position. The colored dots are the point cloud generated from the LiDAR
sensing. Upper and lower $2.5D$ color maps represent the value function and
the elevation map, respectively. The height of the value function represents
the state value. The planned expected trajectory is shown in black curves. The
axes represent the pose of the robot.

To demonstrate the applicability of our system in the real world, we conducted
extensive real-world trials in different scenarios. The goal is to validate
whether the proposed method can be used to generate effective policies that
navigate a ground vehicle in complex and unstructured environments. The robot
behaviors can be seen in video https://youtu.be/6Xc8s4Bongg.

###  VIII-A Hardware Setup

We implement our system on two ground vehicles, ClearPath Jackal and Husky.
Since Jackal is small in dimension and has a small wheel traction, it is only
used for indoor experiment. It is equipped with a 16-beam Velodyne LiDAR for
localization and mapping, a quad-core 2.7GHz CPU and 16GB RAM for the onboard
computation. Husky is used for navigating in outdoor environments, because it
is larger in size and provides more traction when moving on rough terrains.
Husky is equipped with a 64-beam Ouster LiDAR for more detailed terrain
mapping and a Lord Microstrain 3DM-GX4-25 IMU for more accurate localization
outdoor. It has an eight-core 2.7GHz Intel I7 CPU and 64GB RAM for the onboard
computation.

###  VIII-B Indoor Cluttered Environment

(a)

(b)

(c)

(d)

(e)

(f)

(g)

(h)

Figure 14:  Snapshots of navigating in the elevation-rich environment. The two
rows show the real system and the visualization, respectively. The traversed
trajectories and goals are shown in red curves and red circles.

(a)

(b)

(c)

(d)

(e)

(f)

(g)

(h)

Figure 15:  Snapshots of navigation in a trail using trajectory sampling to
sample the states and cost map to represent the environment. The visualization
in the second row shows the cost map as a $2D$ color map, where a darker color
means larger cost. The red trajectory represents the expected path by
executing the policy, and the black dots around the green global trajectory
are the sampled states. The goal for each time step is shown as a green
square. The final goal is the blue circle.

The setup is an indoor $5m\times 5m$ environment with random boxes as
obstacles, and the task is to navigate the ClearPath Jackal to a goal location
and return to the start position. The first aim is to validate the basic
obstacle avoidance behavior in a prior unknown environment. The second aim is
to demonstrate that our system can achieve efficient navigation behavior
without a global planner, as well as use only the elevation map representation
to construct the MDP and specify only a positive goal reward. Traditionally,
the obstacle penalty in the reward (or cost) function is a proxy for
indirectly expressing the consequence of collisions. With the elevation map
representation, the transition function can directly approximate how the
environment’s geometry (in our case, elevation) affects robot’s motion, which
includes the result of colliding with obstacles. This is because we formulate
the transition function to penalize the distance traveled on high-slope
surfaces as mentioned, and obstacles generally have large slope angles.

Since the planning region in this case is observable and can be pre-
specified111Note that it does not mean that the environment map is provided
a-prior., i.e., a $5m\times 5m$ square, we can evenly distribute $6$
supporting states in each of the $x,y$ dimensions, so that the distance
between two neighboring states is $1m$ in each dimension. However, because it
is difficult to predict what heading angles the robot may take during the task
execution, we need supporting states to cover one full rotation of the heading
angle, i.e.,$[-\pi,\pi)$. Specifically, we place $10$ equidistant states in
the $\theta$ dimension, so that the distance between two neighboring states
along the $\theta$ dimension is $\frac{\pi}{5}$. The total number of states is
$360$.

The range of linear and angular speeds are set to $v\in[-0.5m/s,1.5m/s]$ and
$\omega\in[-1.5rad/s,1.5rad/s]$, respectively. The environment, a sequence of
the robot’s snapshots, and detailed planning information are shown in Fig. 13.
The initial position of the robot is placed in front of the obstacles, and the
goal is between the two obstacles and is indicated by the green square on the
second row of Fig. 13. Initially, due to the occlusion by randomly placed
boxes, the robot can only build a partial elevation map using the observed
points. Then, it uses this initial map to calculate the value function within
the $5m\times 5m$ squared region. The value function is shown as the upper
$2.5D$ surface in the second row. To aid the visualization, we only draw the
value function over the position variables at the current robot’s angle, and
the height of the surface represents the state value. We can observe that the
value function correctly characterizes the geometry of the environment and the
task. Specifically, the goal region has the highest state value. Since the
policy always selects actions that maximize the state value, executing the
policy from this value function will allow the robot to converge at the goal
point. Additionally, on the value function surface, the narrow ridge between
the two obstacles reflects the navigable space correctly. Thus, the resulting
policy can navigate through the passage safely as shown in the first row of
Fig. 13. As the robot executes its planned policy, it receives more sensing
points and gradually builds a more detailed elevation map. We can see this
process from the second and fourth rows. After the robot arrives at the goal,
it re-plans and returns to the initial position, which is shown in the third
row. We can observe that the computed value function allows the robot to
reason about its nonholonomic motion through the transition function. Since
the forward velocity is larger than the backward velocity, the robot reverses
and turns to align its forward axis to the initial position to ensure it uses
the maximum speed. This also reveals that our method exhibits a speed-adaptive
behavior for the most efficient motion.

###  VIII-C Outdoor Unstructured Environment

We further test our system in two types of outdoor environments shown in Fig.
14 and Fig. 15. These environments are used to test our method’s performance
in more complex and unstructured environments.

Fig. 14 demonstrates the snapshots of the robot navigating on a terrain that
has varying elevations. This environment contains a dune with a maximum height
of approximately $1.0m$, a high slope in the middle, and a low slope surface
to the left of the robot’s initial position. In this environment, we use
elevation map and importance sampling to construct the MDP, where the weight
is given by the slope angle. The robot’s initial pose directly faces the part
of the terrain that has the highest slope angle. The goal is placed behind the
sand dune indicated as a red circle in the second row of Fig. 14. The shortest
path between the robot’s initial pose and the goal is a straight line crossing
the up-slope terrain, but it is difficult (and dangerous) for the vehicle to
traverse. We can observe in the first three columns of Fig. 14 that the robot
takes safe maneuver to avoid the dangerous terrain. This is because we
penalize the robot’s movement distance on high slope surfaces, the resulting
policy can guide the robot to avoid the dune. The last column shows that after
arriving at the current goal, the vehicle extends its elevation map, updates
the MDP, and computes a new policy.

Fig. 15 shows the robot navigating on a trail surrounded by various
vegetation. Since this environment does not contain elevated terrains, it is
only necessary to provide obstacle penalty using a cost map. As described in
Section VII, we use trajectory-based supporting state sampling and goal
generation for fast traversal in this scenario. The figure shows that our
system allows the robot to navigate through the trail safely without colliding
into the dense vegetation and other obstacles.

##  IX Conclusion and Discussion

This paper presents a new decision making framework for robot planning and
control in complex and unstructured environments such as the off-road
navigation. We propose a method to solve the continuous-state Markov Decision
Process by integrating the kernel value function representation and the
Taylor-based approximation to Bellman optimality equation. Our algorithm
alleviates the need for heavily searching in continuous state space and the
need for precisely modeling the state transition functions. We have validated
the proposed method through thorough evaluations in both simplified and
realistic planning scenarios. The experiments comparing with other baseline
approaches show that our proposed framework is powerful and flexible, and the
performance statistics reveal superior efficiency and accuracy of the
presented algorithm.

In addition to the theoretical contribution, our real-world experiments reveal
several challenges of applying the proposed method in practice. We present a
system (Section VI) which is mainly responsible for converting the raw sensor
data to an MDP problem by assuming that feasible/infeasible regions can be
distinguished and the features in the environment (elevation in our
experiment) can be obtained accurately from sensors. Since this MDP is used
for computing a policy, ensuring the MDP matches the real-world scenario is
paramount in the final performance of the system. However, the map built from
the noisy sensor measurement usually cannot accurately represent the geometry
of the environment, e.g., occupancy and elevation. The MDP derived from this
inaccurate map may deviate from the real environment. Thus, reasoning about
these inaccuracies in the planning method is essential for building a robust
navigation system. Additionally, an accurate physics model for planning is
necessary in complex environments, where detailed motion control strategies
are needed. By comparing the results of the Section V-A and Section VIII, we
can observe that the planning method can generate more efficient policies if
we use a physics model which can better describe the robot’s motion. In our
real-world experiments, our modeling of the first moment uses inaccurate
models to describe the physical interaction between the robot and the terrain.
Although these models can be used for planning in the particular environments
we tested, to generalize to more complex situations, it is necessary to
develop more accurate physical models that consider not only the elevation but
also terrain textures, which is our future work.

## Acknowledgement

This research was supported by the Army Research Office with grant number
W911NF-20-2-0099, the National Science Foundation with grant number 2006886,
and an Amazon Research Award.

## References

  * [1] C. Boutilier, T. Dean, and S. Hanks, “Decision-theoretic planning: Structural assumptions and computational leverage,” _Journal of Artificial Intelligence Research_ , vol. 11, pp. 1–94, 1999.
  * [2] S. Thrun, W. Burgard, and D. Fox, _Probabilistic robotics_. MIT press Cambridge, 2000, vol. 1.
  * [3] J. Kober, J. A. Bagnell, and J. Peters, “Reinforcement learning in robotics: A survey,” _The International Journal of Robotics Research_ , vol. 32, no. 11, pp. 1238–1274, 2013.
  * [4] D. P. Bertsekas, _Dynamic programming and optimal control_. Athena scientific Belmont, MA, 1995, vol. 1, no. 2.
  * [5] R. S. Sutton and A. G. Barto, _Reinforcement learning: An introduction_. MIT press, 2018.
  * [6] A. A. Pereira, J. Binney, G. A. Hollinger, and G. S. Sukhatme, “Risk-aware path planning for autonomous underwater vehicles using predictive ocean models,” _Journal of Field Robotics_ , vol. 30, no. 5, pp. 741–762, 2013\.
  * [7] M. Otte, W. Silva, and E. Frew, “Any-time path-planning: Time-varying wind field+ moving obstacles,” in _2016 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2016, pp. 2575–2582.
  * [8] Y. Fu, X. Yu, and Y. Zhang, “Sense and collision avoidance of unmanned aerial vehicles using markov decision process and flatness approach,” in _2015 IEEE International Conference on Information and Automation_. IEEE, 2015, pp. 714–719.
  * [9] W. H. Al-Sabban, L. F. Gonzalez, and R. N. Smith, “Wind-energy based path planning for unmanned aerial vehicles using markov decision processes,” in _2013 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2013, pp. 784–789.
  * [10] S. S. Baek, H. Kwon, J. A. Yoder, and D. Pack, “Optimal path planning of a target-following fixed-wing uav using sequential decision processes,” in _2013 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2013, pp. 2955–2962.
  * [11] R. E. Bellman, _Adaptive control processes: a guided tour_. Princeton university press, 2015, vol. 2045.
  * [12] A. A. Gorodetsky, S. Karaman, and Y. M. Marzouk, “Efficient high-dimensional stochastic optimal motion control using tensor-train decomposition.” in _Robotics: Science and Systems_ , 2015.
  * [13] L. Liu and G. S. Sukhatme, “A solution to time-varying markov decision processes,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 3, pp. 1631–1638, 2018.
  * [14] J. Xu, K. Yin, and L. Liu, “Reachable space characterization of markov decision processes with time variability,” in _Proceedings of Robotics: Science and Systems_ , FreiburgimBreisgau, Germany, June 2019.
  * [15] R. Munos and A. Moore, “Variable resolution discretization in optimal control,” _Machine learning_ , vol. 49, no. 2-3, pp. 291–323, 2002.
  * [16] S. M. LaValle, _Planning algorithms_. Cambridge university press, 2006.
  * [17] D. P. Bertsekas and J. N. Tsitsiklis, _Neuro-dynamic programming_. Athena Scientific Belmont, MA, 1996, vol. 5.
  * [18] R. Munos and C. Szepesvári, “Finite-time bounds for fitted value iteration,” _Journal of Machine Learning Research_ , vol. 9, no. May, pp. 815–857, 2008.
  * [19] A. Antos, C. Szepesvári, and R. Munos, “Learning near-optimal policies with bellman-residual minimization based fitted policy iteration and a single sample path,” _Machine Learning_ , vol. 71, no. 1, pp. 89–129, 2008.
  * [20] T. Hofmann, B. Schölkopf, and A. J. Smola, “Kernel methods in machine learning,” _The annals of statistics_ , pp. 1171–1220, 2008.
  * [21] J. Shawe-Taylor, N. Cristianini _et al._ , _Kernel methods for pattern analysis_. Cambridge university press, 2004\.
  * [22] M. P. Deisenroth, C. E. Rasmussen, and J. Peters, “Gaussian process dynamic programming,” _Neurocomputing_ , vol. 72, no. 7-9, pp. 1508–1524, 2009.
  * [23] Y. Engel, S. Mannor, and R. Meir, “Bayes meets bellman: The gaussian process approach to temporal difference learning,” in _Proceedings of the 20th International Conference on Machine Learning (ICML-03)_ , 2003, pp. 154–161.
  * [24] M. Kuss and C. E. Rasmussen, “Gaussian processes in reinforcement learning,” in _Advances in Neural Information Processing Systems_ , 2004, pp. 751–758.
  * [25] G. Taylor and R. Parr, “Kernelized value function approximation for reinforcement learning,” in _Proceedings of the 26th Annual International Conference on Machine Learning_ , 2009, pp. 1017–1024.
  * [26] X. Xu, D. Hu, and X. Lu, “Kernel-based least squares policy iteration for reinforcement learning,” _IEEE Transactions on Neural Networks_ , vol. 18, no. 4, pp. 973–992, 2007.
  * [27] J. Martin, J. Wang, and B. Englot, “Sparse gaussian process temporal difference learning for marine robot navigation,” _arXiv preprint arXiv:1810.01217_ , 2018.
  * [28] K. Arulkumaran, M. P. Deisenroth, M. Brundage, and A. A. Bharath, “A brief survey of deep reinforcement learning,” _arXiv preprint arXiv:1708.05866_ , 2017.
  * [29] X. Glorot and Y. Bengio, “Understanding the difficulty of training deep feedforward neural networks,” in _Proceedings of the Thirteenth International Conference on Artificial Intelligence and Statistics_ , 2010, pp. 249–256.
  * [30] J. D. Gammell and M. P. Strub, “Asymptotically optimal sampling-based motion planning methods,” _Annual Review of Control, Robotics, and Autonomous Systems_ , vol. 4.
  * [31] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” _The international journal of robotics research_ , vol. 30, no. 7, pp. 846–894, 2011.
  * [32] D. J. Webb and J. v. d. Berg, “Kinodynamic rrt*: Optimal motion planning for systems with linear differential constraints,” _arXiv preprint arXiv:1205.5088_ , 2012.
  * [33] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in _2011 IEEE international conference on robotics and automation_. IEEE, 2011, pp. 2520–2525.
  * [34] C. Richter, A. Bry, and N. Roy, “Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments,” in _Robotics research_. Springer, 2016, pp. 649–666.
  * [35] J. Chen, K. Su, and S. Shen, “Real-time safe trajectory generation for quadrotor flight in cluttered environments,” in _2015 IEEE International Conference on Robotics and Biomimetics (ROBIO)_. IEEE, 2015, pp. 1678–1685.
  * [36] F. Gao, W. Wu, Y. Lin, and S. Shen, “Online safe trajectory generation for quadrotors using fast marching method and bernstein basis polynomial,” in _2018 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2018, pp. 344–351.
  * [37] F. Gao and S. Shen, “Online quadrotor trajectory generation and autonomous navigation on point clouds,” in _2016 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR)_. IEEE, 2016, pp. 139–146.
  * [38] F. Gao, W. Wu, W. Gao, and S. Shen, “Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments,” _Journal of Field Robotics_ , vol. 36, no. 4, pp. 710–733, 2019.
  * [39] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor, and V. Kumar, “Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments,” _IEEE Robotics and Automation Letters_ , vol. 2, no. 3, pp. 1688–1695, 2017.
  * [40] R. Deits and R. Tedrake, “Efficient mixed-integer planning for uavs in cluttered environments,” in _2015 IEEE international conference on robotics and automation (ICRA)_. IEEE, 2015, pp. 42–49.
  * [41] B. Zhou, F. Gao, J. Pan, and S. Shen, “Robust real-time uav replanning using guided gradient-based optimization and topological paths,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2020, pp. 1208–1214.
  * [42] H. Oleynikova, M. Burri, Z. Taylor, J. Nieto, R. Siegwart, and E. Galceran, “Continuous-time trajectory optimization for online uav replanning,” in _2016 IEEE/RSJ international conference on intelligent robots and systems (IROS)_. IEEE, 2016, pp. 5332–5339.
  * [43] B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen, “Robust and efficient quadrotor trajectory generation for fast autonomous flight,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 4, pp. 3529–3536, 2019.
  * [44] R. Tedrake, I. R. Manchester, M. Tobenkin, and J. W. Roberts, “Lqr-trees: Feedback motion planning via sums-of-squares verification,” _The International Journal of Robotics Research_ , vol. 29, no. 8, pp. 1038–1052, 2010\.
  * [45] A.-A. Agha-Mohammadi, S. Chakravorty, and N. M. Amato, “Firm: Sampling-based feedback motion-planning under motion uncertainty and imperfect measurements,” _The International Journal of Robotics Research_ , vol. 33, no. 2, pp. 268–304, 2014.
  * [46] R. E. Kalman _et al._ , “Contributions to the theory of optimal control,” _Bol. soc. mat. mexicana_ , vol. 5, no. 2, pp. 102–119, 1960.
  * [47] J. Van Den Berg, P. Abbeel, and K. Goldberg, “Lqg-mp: Optimized path planning for robots with motion uncertainty and imperfect state information,” _The International Journal of Robotics Research_ , vol. 30, no. 7, pp. 895–913, 2011.
  * [48] V. A. Huynh, S. Karaman, and E. Frazzoli, “An incremental sampling-based algorithm for stochastic optimal control,” _The International Journal of Robotics Research_ , vol. 35, no. 4, pp. 305–333, 2016.
  * [49] J. B. Rawlings, D. Q. Mayne, and M. Diehl, _Model predictive control: theory, computation, and design_. Nob Hill Publishing Madison, WI, 2017, vol. 2.
  * [50] D. P. Bertsekas, “Dynamic programming and suboptimal control: A survey from adp to mpc,” _European Journal of Control_ , vol. 11, no. 4-5, pp. 310–334, 2005.
  * [51] D. Bertsekas, _Dynamic programming and optimal control: Volume I_. Athena scientific, 2012, vol. 1.
  * [52] A. Bemporad and M. Morari, “Robust model predictive control: A survey,” in _Robustness in identification and control_. Springer, 1999, pp. 207–226.
  * [53] W. Langson, I. Chryssochoos, S. Raković, and D. Q. Mayne, “Robust model predictive control using tubes,” _Automatica_ , vol. 40, no. 1, pp. 125–133, 2004.
  * [54] A. Majumdar and R. Tedrake, “Robust online motion planning with regions of finite time invariance,” in _Algorithmic foundations of robotics X_. Springer, 2013, pp. 543–558.
  * [55] ——, “Funnel libraries for real-time robust feedback motion planning,” _The International Journal of Robotics Research_ , vol. 36, no. 8, pp. 947–982, 2017.
  * [56] M. Althoff, O. Stursberg, and M. Buss, “Reachability analysis of nonlinear systems with uncertain parameters using conservative linearization,” in _2008 47th IEEE Conference on Decision and Control_. IEEE, 2008, pp. 4042–4048.
  * [57] S. Bansal, M. Chen, S. Herbert, and C. J. Tomlin, “Hamilton-jacobi reachability: A brief overview and recent advances,” in _2017 IEEE 56th Annual Conference on Decision and Control (CDC)_. IEEE, 2017, pp. 2242–2253.
  * [58] H. J. Kappen, V. Gómez, and M. Opper, “Optimal control as a graphical model inference problem,” _Machine learning_ , vol. 87, no. 2, pp. 159–182, 2012.
  * [59] E. Theodorou, J. Buchli, and S. Schaal, “A generalized path integral control approach to reinforcement learning,” _The Journal of Machine Learning Research_ , vol. 11, pp. 3137–3181, 2010.
  * [60] G. Williams, N. Wagener, B. Goldfain, P. Drews, J. M. Rehg, B. Boots, and E. A. Theodorou, “Information theoretic mpc for model-based reinforcement learning,” in _2017 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2017, pp. 1714–1721.
  * [61] G. Williams, B. Goldfain, P. Drews, K. Saigol, J. M. Rehg, and E. A. Theodorou, “Robust sampling based model predictive control with sparse objective information.” in _Robotics: Science and Systems_ , 2018.
  * [62] W. B. Powell, “Perspectives of approximate dynamic programming,” _Annals of Operations Research_ , vol. 241, no. 1-2, pp. 319–356, 2016.
  * [63] G. J. Gordon, “Approximate solutions to markov decision processes,” Carnegie-Mellon University School of Computer Science, Tech. Rep., 1999.
  * [64] D. P. Bertsekas, “Approximate policy iteration: A survey and some new methods,” _Journal of Control Theory and Applications_ , vol. 9, no. 3, pp. 310–335, 2011.
  * [65] M. L. Puterman, _Markov decision processes: discrete stochastic dynamic programming_. John Wiley & Sons, 2014\.
  * [66] M. G. Lagoudakis and R. Parr, “Least-squares policy iteration,” _Journal of machine learning research_ , vol. 4, no. Dec, pp. 1107–1149, 2003.
  * [67] A. Braverman, I. Gurvich, and J. Huang, “On the taylor expansion of value functions,” _Operations Research_ , vol. 68, no. 2, pp. 631–654, 2020.
  * [68] L. C. Evans, _Partial Differential Equations: Second Edition (Graduate Series in Mathematics)_. American Mathematical Society, 2010.
  * [69] J. Si, A. G. Barto, W. B. Powell, and D. Wunsch, _Handbook of learning and approximate dynamic programming_. John Wiley & Sons, 2004, vol. 2.
  * [70] M. Maurette, “Mars rover autonomous navigation,” _Autonomous Robots_ , vol. 14, no. 2-3, pp. 199–208, 2003.
  * [71] R. Islam, P. Henderson, M. Gomrokchi, and D. Precup, “Reproducibility of benchmarked deep reinforcement learning tasks for continuous control,” _arXiv preprint arXiv:1708.04133_ , 2017.
  * [72] N. Heess, G. Wayne, D. Silver, T. Lillicrap, T. Erez, and Y. Tassa, “Learning continuous control policies by stochastic value gradients,” in _Advances in Neural Information Processing Systems_ , 2015, pp. 2944–2952.
  * [73] A. S. McEwen, E. M. Eliason, J. W. Bergstrom, N. T. Bridges, C. J. Hansen, W. A. Delamere, J. A. Grant, V. C. Gulick, K. E. Herkenhoff, L. Keszthelyi _et al._ , “Mars reconnaissance orbiter’s high resolution imaging science experiment (hirise),” _Journal of Geophysical Research: Planets_ , vol. 112, no. E5, 2007.
  * [74] G. Bresson, Z. Alsayed, L. Yu, and S. Glaser, “Simultaneous localization and mapping: A survey of current trends in autonomous driving,” _IEEE Transactions on Intelligent Vehicles_ , vol. 2, no. 3, pp. 194–220, 2017.
  * [75] P. Fankhauser, M. Bloesch, and M. Hutter, “Probabilistic terrain mapping for mobile robots with uncertain localization,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 4, pp. 3019–3026, 2018.
  * [76] W. Burgard, M. Moors, C. Stachniss, and F. E. Schneider, “Coordinated multi-robot exploration,” _IEEE Transactions on robotics_ , vol. 21, no. 3, pp. 376–386, 2005.
  * [77] B. Yamauchi, “A frontier-based approach for autonomous exploration,” in _Proceedings 1997 IEEE International Symposium on Computational Intelligence in Robotics and Automation CIRA’97.’Towards New Computational Principles for Robotics and Automation’_. IEEE, 1997, pp. 146–151.
  * [78] F. Allgöwer and A. Zheng, _Nonlinear model predictive control_. Birkhäuser, 2012, vol. 26.
  * [79] J. Gregory, J. Fink, E. Stump, J. Twigg, J. Rogers, D. Baran, N. Fung, and S. Young, “Application of multi-robot systems to disaster-relief scenarios with limited communication,” in _Field and Service Robotics_. Springer, 2016, pp. 639–653.
  * [80] S. Johnson, “The NLopt Nonlinear-Optimization Package,” http://ab-initio.mit.edu/nlopt.
  * [81] M. Likhachev, D. I. Ferguson, G. J. Gordon, A. Stentz, and S. Thrun, “Anytime dynamic a*: An anytime, replanning algorithm.” in _ICAPS_ , vol. 5, 2005, pp. 262–271.