# SDP Synthesis of Maximum Coverage Trees for Probabilistic Planning under
Control Constraints

Naman Aggarwal, and Jonathan P. How
Laboratory for Information and Decision Systems, Massachusetts Institute of
Technology

###### Abstract

The paper presents Maximal Covariance Backward Reachable Trees (MAXCOVAR BRT),
which is a multi-query algorithm for planning of dynamic systems under
stochastic motion uncertainty and constraints on the control input with
explicit coverage guarantees. In contrast to existing roadmap-based
probabilistic planning methods that sample belief nodes randomly and draw
edges between them [1], under control constraints, the reachability of belief
nodes needs to be explicitly established and is determined by checking the
feasibility of a non-convex program. Moreover, there is no explicit
consideration of coverage of the roadmap while adding nodes and edges during
the construction procedure for the existing methods. Our contribution is a
novel optimization formulation to add nodes and construct the corresponding
edge controllers such that the generated roadmap results in provably maximal
coverage under control constraints as compared to any other method of adding
nodes and edges. We characterize formally the notion of coverage of a roadmap
in this stochastic domain via introduction of the h-$\operatorname{BRS}$
(Backward Reachable Set of Distributions) of a tree of distributions under
control constraints, and also support our method with extensive simulations on
a 6 DoF model.

##  I Introduction

Multi-query motion planning entails the design of plans that can be reused
across different initial configurations of the system. This is typically done
via the offline construction of a roadmap of feasible trajectories in the
state space, such that in real-time, for a pair of initial and goal
configurations of the system, planning proceeds by connecting the initial and
goal configurations to the roadmap followed by graph search to find a path [2,
3]. The pre-computation of a roadmap is beneficial since it avoids the
computational burden of finding plans from scratch for new configurations, and
reuses search effort across queries. Such an approach of constructing a
roadmap of feasible trajectories facilitates robust planning since a plan to
meet the mission requirements could be computed quickly if the system finds
itself in unexpected configurations due to unmodeled dynamics or disturbances
during online operation. An important design consideration for any roadmap
based planning algorithm is the coverage of the roadmap – it is desirable to
be able to re-use search effort across as many queries as possible, therefore,
reasoning explicitly about the coverage of the roadmap becomes an important
algorithmic design consideration.

There is extensive work on motion planning via the construction of reusable
roadmaps for deterministic systems [4, 5, 6, 7]. The work proposed in [4], for
instance, builds a tree of funnels, which are regions of finite-time
invariance around trajectories with associated feedback controllers, backwards
from the goal in a space-filling manner. Paths to the goal could be found from
different regions of the state space by traversing the branches of the
constructed tree. Ref. [6] takes a different approach by populating a library
with a finite set of feasible trajectories with associated funnels, such that
the online planning proceeds through the sequential composition of the
available funnels in the library.

Figure 1: An illustration of the recursive satisfaction of constraints (goal
distribution reaching constraint, and chance constraints on the state and
control input) for the overall trajectory initialized at the
$(\mu_{i},\Sigma_{i})$ distribution through sequential composition of two
$N$-step controllers, $\mathscr{C}_{i,j}$ (designed for the $i\rightarrow j$
maneuver) and $\mathscr{C}_{j,k}$ (designed for the
$j_{\mathrm{max}}\rightarrow k$ maneuver).

In this paper, we are concerned with planning for systems with stochastic
dynamics. A stochastic framework helps by making informed plans considering
the probability of constraint violation and leads to a lesser conservative
approach as compared to a deterministic framework considering worst-case
uncertainty in the dynamics or enforcing hard constraints. CC-RRT [8] is a
popular framework for single-query motion planning in stochastic systems. The
algorithm proceeds by growing a forwards search tree of distributions over the
state space, such that the obstacle avoidance constraints are satisfied
probabilistically along each edge. The tree expansion phase involves randomly
sampling open-loop control signals and simulating the system dynamics forward
until the chance constraints on obstacle avoidance are satisfied, and adding
the forward simulated state distribution to the tree as a node with the
associated open-loop control sequence as the edge. A major limitation of
constructing a search tree via sampling open-loop control signals is that the
the second order moment of the state distributions (covariance) cannot be
controlled via open-loop control even for the simplest case of linear-Gaussian
systems. CC-RRT comes with a probabilistic completeness guarantee such that
the algorithm finds paths to all goal distributions for which open-loop paths
would exist [9]. However, the set of all possible distributions that could be
reached with open-loop control is a subset of the set of all distributions
that could be reached with a richer control scheme such as feedback control.
Particularly, for a mission with tight goal reaching constraints (a specific
size of the goal covariance as required by the mission), CC-RRT might fail to
find a path even though a closed-loop path might exist.

While CC-RRT is a single-query planner, there exist other roadmap based
methods for planning in stochastic domains. FIRM [10] is a planning framework
that builds a roadmap in the belief space by sampling stationary belief nodes
and drawing belief stabilizing controllers as edges between the nodes. The
closest in approach to our work, however, is the recently proposed CS-BRM [11]
that provides a faster planning scheme as compared to FIRM by allowing
sampling of non-stationary belief nodes and adding finite-time feedback
controllers as edges. CS-BRM is built on top of a series of latest work on
covariance steering that allows finite-time satisfaction of terminal
covariance constraints [12, 13, 14]. CS-BRM, however, does not assume control
input constraints, and adds edges between randomly sampled belief nodes. In
the absence of control constraints, the distribution steering problem
decouples into the mean steering and covariance steering problem: both of
which are tractable to solve [14]. In the presence of control input
constraints however, the reachability of belief nodes needs to be explicitly
established. Even for the simplest case of a controllable linear-Gaussian
system, it is no longer possible to drive arbitrary distributions to arbitrary
distributions, and the existence of a steering maneuver for a pair of belief
nodes must be determined by checking the feasibility of a nonconvex program.
Therefore, CS-BRM is not efficient for settings with constraints on the
control input due to unnecessary sampling of belief nodes that would not be
added to the roadmap due to the non-existence of a steering control.

Moreover, even for successful insertion of belief nodes, there is no explicit
consideration of coverage while selecting nodes during the roadmap
construction procedure, neither a post-analysis on the coverage
characteristics of the roadmap generated by CS-BRM. We provide a systematic
procedure that reasons about the coverage of the to-be-generated roadmap while
adding nodes through a novel optimization formulation. The contributions of
the paper are as follows:

  * •

We characterize the notion of coverage of a roadmap in the stochastic domain
formally via what we call h-$\operatorname{BRS}$ (Backward Reachable Set of
Distributions) of the tree, which is the set of all distributions that can
reach the goal in h-hops where 1 hop is a finite-time horizon of N-steps under
control constraints.

  * •

We propose a novel method of edge construction such that the BRT constructed
following our method finds paths from provably the largest set of initial
distributions for the overall planning problem, i.e. provides maximal
coverage.

  * •

We supplement our approach with extensive simulations on a 6 DoF model.

##  II Problem Statement

Consider a discrete-time stochastic linear system represented by the following
difference equation

$\mathbf{x}_{t+1}=A\mathbf{x}_{t}+B\mathbf{u}_{t}+D\mathbf{w}_{t}$ (1)

where $\mathbf{x}_{t}\in\mathcal{X}$, $\mathbf{u}_{t}\in\mathcal{U}$ are the
state and control inputs at time $t$, respectively,
$\mathcal{X}\subseteq\mathbb{R}^{n}$ is the state space,
$\mathcal{U}\subseteq\mathbb{R}^{m}$ is the control space, $n,m\in\mathbb{N}$,
$D\in\mathbb{R}^{n\times n}$ and $\mathbf{w}_{t}$ represents the stochastic
disturbance at time $t$ that is assumed to have a Gaussian distribution with
zero mean and unitary covariance. We also assume that $\\{w_{t}\\}$ is an
i.i.d. process, and that $A$ is non-singular. Note that we use the notation
$\mathcal{N}(\mu,\Sigma)$ to denote a Gaussian distribution with mean $\mu$
and covariance $\Sigma$.

We define a finite-horizon optimal control problem, $\operatorname{OPT-
STEER}(\mathcal{I},\mathcal{G},N)$, where $\mathcal{I}$, $\mathcal{G}$, $N$
are the initial distribution, goal distribution, and the time-horizon
corresponding to the control problem respectively. $\operatorname{OPT-
STEER}(\mathcal{I},\mathcal{G},N)$ solves for a control sequence that is
optimal with respect to a performance index $J$, and that steers the system
from an initial distribution $\mathcal{I}$ to a goal distribution
$\mathcal{G}$ in a time-horizon of length $N$ while avoiding obstacles such
that the control sequence respects the prescribed constraints on the control
input at each time-step:

$\min_{\Phi_{k}}\quad
J=\mathbb{E}\left[\sum_{k=0}^{N-1}\mathbf{x}_{k}^{\top}Q_{k}\mathbf{x}_{k}+\mathbf{u}_{k}^{\top}R_{k}\mathbf{u}_{k}\right]$
(OPT-STEER)

such that, for all $k=0,1,\cdots N-1$,

$\displaystyle\mathbf{x}_{k+1}=A\mathbf{x}_{k}+B\mathbf{u}_{k}+D\mathbf{w}_{k},$
(2a)
$\displaystyle\mathbf{x}_{0}\sim\mathcal{N}(\mu_{\mathcal{I}},\Sigma_{\mathcal{I}}),$
(2b) $\displaystyle\mathbf{x}_{N}\sim\mathcal{N}(\mu_{N},\Sigma_{N}),$ (2c)
$\displaystyle\mu_{N}=\mu_{\mathcal{G}},$ (2d)
$\displaystyle\Sigma_{N}\preceq\Sigma_{\mathcal{G}},$ (2e)
$\displaystyle\mathbb{P}(\mathbf{x}_{k}\in\mathcal{X})\geq 1-\epsilon_{x},$
(2f) $\displaystyle\mathbb{P}(\mathbf{u}_{k}\in\mathcal{U})\geq
1-\epsilon_{u},$ (2g)

where $\Phi_{k}(.)$ is the parameterization for the finite-horizon controller
such that $\mathbf{u}_{k}\coloneqq\Phi_{k}(\mathbf{x}_{k})$.

Under constraints on the control input of the form (2g), not all instances of
$\operatorname{OPT-STEER}(\mathcal{I},\mathcal{G},N)$ have a solution where an
instance is described by the tuple $(\mathcal{I},N)$ of the initial
distribution and the planning horizon for a fixed goal distribution
$\mathcal{G}$. In particular, $\operatorname{OPT-
STEER}(\mathcal{I},\mathcal{G},N)$ has a solution if and only if the
constraint set of the optimization problem defined by the set of equations
(2a)-(2g) is non-empty in the space of the decision variables
$\\{\Phi_{k}\\}_{k=0}^{N-1}$. Alternatively, under control constraints, the
existence of an $N$-step controller that steers the system from an initial
distribution to a goal distribution needs to be established explicitly by
solving a feasibility question even when the underlying system $(A,B)$ is
controllable. This existence (or not) of a control sequence for a planning
instance defined by the triplet $\mathcal{I},\mathcal{G},N$ is a special
artifact of the presence of control input constraints in distribution steering
(see Section III-A). This paper presents solutions to the following problem:

######  Problem II.1.

Find paths to the goal distribution $\mathcal{G}$ from all query initial
distributions $\mathcal{I}$ for which paths (respecting all the constraints)
exist.

Note that we use the terms path and control sequence interchangeably in this
paper. Problem II.1 is an instance of multi-query planning, and is a difficult
problem to solve in it’s full generality. We employ graph-based methods to
build a roadmap in the space of distributions of the state of the system such
that the roadmap could be used across query initial distributions.

We propose a novel method of edge construction such that the edge controller
can be re-used across a wider set of initial distributions for the edge, and
such that the BRT constructed following the novel method of edge construction
finds paths from provably the largest set of initial distributions for the
overall planning task. We supplement our approach with extensive simulations.

We begin with a discussion on the feasibility of a covariance steering problem
instance in Section III-A, followed by a review of an existing tractable
method for covariance steering in Section III-B. We discuss our approach in
Section IV followed by a discussion on experiments in Section V.

##  III Finite Horizon Covariance Steering under Control Constraints

In this section, we discuss the finite horizon covariance steering problem,
and the feasibility of any covariance steering problem instance.

###  III-A Feasibility

The development in this subsection follows closely [13] [14]. We consider a
linear state feedback parameterization for the controller to solve (OPT-STEER)
as follows,

$\mathbf{u}_{k}=K_{k}(\mathbf{x}_{k}-\mu_{k})+v_{k}$ (3)

where $K_{k}\in\mathbb{R}^{n\times m}$ is the feedback gain that controls the
covariance dynamics, and $v_{k}\in\mathbb{R}^{n}$ is the feedforward gain that
controls the mean dynamics. Under a linearly parameterized controller as
described in (3), the state distribution remains Gaussian at all times and we
can express the objective $J$ (OPT-STEER) completely in terms of the first and
second order moments of the state process:

$J\\!\\!=\\!\\!\sum_{k=0}^{N-1}\operatorname{tr}({Q_{k}\Sigma_{k}})+\operatorname{tr}(R_{k}K_{k}\Sigma_{k}K^{\intercal}_{k})+\mu^{\intercal}_{k}Q_{k}\mu_{k}+v_{k}^{\intercal}R_{k}v_{k},$

We consider polytopic state and control constraints of the form
$\mathcal{X}\coloneqq\\{x_{k}\in\mathbb{R}^{n}\ |\
\alpha^{\intercal}_{x}x_{k}\leq\beta_{x}\\}$,
$\mathcal{U}\coloneqq\\{u_{k}\in\mathbb{R}^{m}\ |\
\alpha^{\intercal}_{u}u_{k}\leq\beta_{u}\\}$ such that,

$\displaystyle\mathbb{P}(\alpha^{\intercal}_{x}\mathbf{x}_{k}\leq\beta_{x})$
$\displaystyle\geq 1-\epsilon_{x},$ (4)
$\displaystyle\mathbb{P}(\alpha^{\intercal}_{u}\mathbf{u}_{k}\leq\beta_{u})$
$\displaystyle\geq 1-\epsilon_{u},$ (5)

where $\alpha_{x}\in\mathbb{R}^{n}$, $\alpha_{u}\in\mathbb{R}^{m}$, and
$\beta_{x},\beta_{u}\in\mathbb{R}$.
$\epsilon_{x},\epsilon_{u}\in\left[0,0.5\right]$ represent the tolerance
levels with respect to state and control constraint violation respectively,
and $\\{\alpha_{x}\mathbf{x}_{k}\\}^{N-1}_{k=0}$ and
$\\{\alpha_{u}\mathbf{u}_{k}\\}^{N-1}_{k=0}$ are univariate random variables
with first and second order moments,

$\displaystyle\mathbb{E}(\alpha_{x}\mathbf{x}_{k})=\alpha_{x}\mu_{k}$ (6)
$\displaystyle\mathbb{E}(\alpha_{u}\mathbf{u}_{k})=\alpha_{u}v_{k}$ (7)
$\displaystyle\mathbb{E}(\alpha^{\intercal}_{x}(\mathbf{x}_{k}-\mu_{k})(\mathbf{x}_{k}-\mu_{k})^{\intercal}\alpha_{x})=\alpha^{\intercal}_{x}\Sigma_{k}\alpha_{x}$
(8)
$\displaystyle\mathbb{E}(\alpha^{\intercal}_{u}(\mathbf{u}_{k}-v_{k})(\mathbf{u}_{k}-v_{k})^{\intercal}\alpha_{u})=\alpha^{\intercal}_{u}K_{k}\Sigma_{k}K^{\intercal}_{k}\alpha_{u}.$
(9)

Ref. [12] shows that the chance constraints can be written as,

$\displaystyle\Phi^{-1}(1-\epsilon_{x})\sqrt{\alpha^{\intercal}_{x}\Sigma_{k}\alpha_{x}}+\alpha^{\intercal}_{x}\mu_{k}-\beta_{x}\leq
0,$ (10a)
$\displaystyle\Phi^{-1}(1-\epsilon_{u})\sqrt{\alpha^{\intercal}_{u}K_{k}\Sigma_{k}K^{\intercal}_{k}\alpha_{u}}+\alpha^{\intercal}_{u}v_{k}-\beta_{u}\leq
0,$ (10b)

where $\Phi^{-1}(.)$ is the inverse cumulative distribution function of the
normal distribution. Therefore, the optimization problem OPT-STEER can be
recast as the nonlinear program,

$\displaystyle\min_{\Sigma_{k},K_{k},\mu_{k},v_{k}}J=\sum_{k=0}^{N-1}$
$\displaystyle\operatorname{tr}({Q_{k}\Sigma_{k}})+\operatorname{tr}(R_{k}K_{k}\Sigma_{k}K^{\intercal}_{k})$
$\displaystyle+\mu^{\intercal}_{k}Q_{k}\mu_{k}+v_{k}^{\intercal}R_{k}v_{k},$
(11)

such that for all $k=0,1,\cdots,N-1$,

$\displaystyle\begin{aligned}
\Sigma_{k+1}=A&\Sigma_{k}A^{\intercal}+BK_{k}\Sigma_{k}A^{\intercal}+A\Sigma_{k}K^{\intercal}_{k}B^{\intercal}\\\
&+BK_{k}\Sigma_{k}K^{\intercal}_{k}B^{\intercal}+DD^{\intercal},\end{aligned}$
(11a) $\displaystyle\Sigma_{0}=\Sigma_{\mathcal{I}},$ (11b)
$\displaystyle\Sigma_{N}\preceq\Sigma_{\mathcal{G}},$ (11c)
$\displaystyle\mu_{k+1}=A\mu_{k}+Bv_{k},$ (11d)
$\displaystyle\mu_{0}=\mu_{\mathcal{I}},$ (11e)
$\displaystyle\mu_{N}=\mu_{\mathcal{G}},$ (11f)
$\displaystyle\Phi^{-1}(1-\epsilon_{x})\sqrt{\alpha^{\intercal}_{x}\Sigma_{k}\alpha_{x}}+\alpha^{\intercal}_{x}\mu_{k}-\beta_{x}\leq
0,$ (11g)
$\displaystyle\Phi^{-1}(1-\epsilon_{u})\sqrt{\alpha^{\intercal}_{u}K_{k}\Sigma_{k}K^{\intercal}_{k}\alpha_{u}}+\alpha^{\intercal}_{u}v_{k}-\beta_{u}\leq
0.$ (11h)

The existence of an $N$-step steering control that drives the state
distribution from $\mathcal{I}$ to $\mathcal{G}$ satisfying all the
constraints is determined by the feasibility of the set of equations
(11a)-(11h) which represents a non-convex set in the space of the decision
variables $\\{K_{k}\\}_{k=0}^{N-1},\\{v_{k}\\}_{k=0}^{N-1}$. Determining if a
non-convex set is empty or not from it’s algebraic description is an NP-hard
problem in general, and the complexity of this feasibility check scales with
the time-horizon since the problem size becomes larger and the number of
variables increase. Problem II.1 is concerned with finding feasible paths from
all possible initial distributions, and our solution methodology proceeds by
building a backward reachable tree of feasible paths from the goal
distribution, and sequencing them together to find a feasible path at run-time
for the query initial distribution, hence avoiding solving for the feasible
path of the query distribution from scratch (see Section IV for details).

###  III-B Convex Relaxation

Following the development in [13] [14], making the change of variables
$U_{k}=K_{k}\Sigma_{k}$ and introducing an auxiliary variable $Y_{k}$, we can
relax (11) to a convex semidefinite program as,

$\displaystyle\min_{\Sigma_{k},U_{k},Y_{k},\mu_{k},v_{k}}J=\sum_{k=0}^{N-1}$
$\displaystyle\operatorname{tr}({Q_{k}\Sigma_{k}})+\operatorname{tr}(R_{k}Y_{k})$
$\displaystyle+\mu^{\intercal}_{k}Q_{k}\mu_{k}+v_{k}^{\intercal}R_{k}v_{k},$
(13)

such that for all $k=0,1,\cdots,N-1$,

$\displaystyle C_{k}$ $\displaystyle\triangleq
U_{k}\Sigma_{k}^{-1}U^{\intercal}_{k}-Y_{k}\preceq 0,$ (13a) $\displaystyle
G_{k}$ $\displaystyle\triangleq
A\Sigma_{k}A^{\intercal}+BU_{k}A^{\intercal}+AU^{\intercal}_{k}B^{\intercal}$
$\displaystyle~{}~{}+BY_{k}B^{\intercal}+DD^{\intercal}-\Sigma_{k+1}=0,$ (13b)
$\displaystyle\text{subject to (\ref{eq:covar_init_nlp}) -
(\ref{eq:mean_goal_nlp})},$ (13c)

where the constraint (13a) can be expressed as an LMI using the Schur
complement as follows, $\left[\begin{array}[]{ll}\Sigma_{k}&U_{k}^{\top}\\\
U_{k}&Y_{k}\end{array}\right]\succeq 0.$ From (13a), (10b) is further relaxed
to,

$\displaystyle\Phi^{-1}(1-\epsilon_{u})\sqrt{\alpha^{\intercal}_{u}Y_{k}\alpha_{u}}+\alpha^{\intercal}_{u}v_{k}-\beta_{u}\leq
0.$ (15)

Due to the presence of the square root, neither of (10a) and (15) are convex.
Ref. [14] proposes a linearization of (10a) and (15), and since the square
root is a concave function, the tangent line serves as the global
overestimator,

$\sqrt{x}\leq\frac{1}{2\sqrt{x_{0}}}x+\frac{\sqrt{x_{0}}}{2},\quad\forall
x,x_{0}>0$ (16)

Therefore, the constraints (10a) and (10b) are finally approximated as,

$\displaystyle\begin{aligned}
\Phi^{-1}\left(1-\epsilon_{x}\right)&\frac{1}{2\sqrt{\alpha_{x}^{\top}\Sigma_{r}\alpha_{x}}}\alpha_{x}^{\top}\Sigma_{k}\alpha_{x}+\alpha_{x}^{\top}\mu_{k}\\\
&-\left(\beta_{x}-\Phi^{-1}\left(1-\epsilon_{x}\right)\frac{1}{2}\sqrt{\alpha_{x}^{\top}\Sigma_{r}\alpha_{x}}\right)\leq
0\end{aligned}$ (17) $\displaystyle\begin{aligned}
\Phi^{-1}\left(1-\epsilon_{u}\right)&\frac{1}{2\sqrt{\alpha_{u}^{\top}Y_{r}\alpha_{u}}}\alpha_{u}^{\top}Y_{k}\alpha_{u}+\alpha_{u}^{\top}v_{k}\\\
&-\left(\beta_{u}-\Phi^{-1}\left(1-\epsilon_{u}\right)\frac{1}{2}\sqrt{\alpha_{u}^{\top}Y_{r}\alpha_{u}}\right)\leq
0\end{aligned}$

where $\Sigma_{r},Y_{r}$ are some reference values. The linearized constraints
now form a convex set.

The finite horizon constrained covariance steering problem (OPT-STEER) is
finally approximated as the following convex semidefinite program,

$\displaystyle\min_{\Sigma_{k},U_{k},Y_{k},\mu_{k},v_{k}}J$ (18)

such that for all $k=0,1,\cdots,N-1$,

$\displaystyle\mu_{k+1}=A\mu_{k}+Bv_{k},$ (18a) $\displaystyle
C_{k}(\Sigma_{k},U_{k},Y_{k})\preceq 0,$ (18b) $\displaystyle
G_{k}(\Sigma_{k+1},\Sigma_{k},Y_{k},U_{k})=0,$ (18c)
$\displaystyle\ell^{\intercal}\Sigma_{k}\ell+\alpha^{\intercal}_{x}\mu_{k}-\overline{\beta}_{x}\leq
0,$ (18d) $\displaystyle
e^{\intercal}Y_{k}e+\alpha^{\intercal}_{u}v_{k}-\overline{\beta}_{u}\leq 0,$
(18e)

where $J$ is defined in (13).

##  IV MAXCOVAR BRT: A MAXIMUM COVERAGE TREE FOR PROBABILISTIC PLANNING

We solve Problem II.1 by constructing a Backward Reachable Tree (BRT) of
distributions that verifiably reach the goal distribution $\mathcal{G}$ under
constraints on the control input. As discussed previously, in the presence of
control constraints, existence of a control sequence that steers the system
from an initial distribution to a goal distribution is established by solving
a feasibility problem. The size of this feasibility check scales with the
time-horizon and the BRT enables a faster feasibility check on a long time-
horizon by checking the feasibility of reaching any existing node on the BRT
instead of directly checking feasibility against the goal distribution.

We refer to this idea as recursive feasibility since the branches of the tree
can be thought of as carrying a certificate of feasibility along its edges
from the goal in a backwards fashion s.t. guaranteeing feasibility to any of
the children nodes in the tree guarantees feasibility to all the upstream
parent nodes and consequently the root node that corresponds to the goal
distribution.

We introduce a novel objective function MAX-COVAR, as discussed in the
following subsection, for adding nodes and constructing edge controllers such
that the resulting tree provides maximum coverage. We also characterize
formally the notion of coverage mathematically in this section.

###  IV-A MAX-COVAR: Novel Objective for Construction of the Edge Controller

We define a procedure to construct an $N$-step edge controller as follows. The
procedure $\operatorname{MAXCOVAR}$ takes in as input a candidate initial mean
$\mu_{\mathrm{initial}}$, a target distribution at the end of the $N$-step
steering maneuver $(\mu_{\mathrm{target}},\Sigma_{\mathrm{target}})$ and
computes an initial covariance $\Sigma_{\mathrm{initial}}$ in a maximal sense,
henceforth referred to as $\Sigma_{\mathrm{initial,max}}$, and the associated
control sequence $\mathscr{C}\coloneqq\\{\mathscr{C}_{k}\\}_{k=0}^{N-1}$ that
achieves the corresponding steering maneuver. The control at time $k$ is a
tuple of the feedback and the feedforward term s.t.
$\mathscr{C}_{k}=(K_{k},v_{k})$.

$\displaystyle\min_{\Sigma_{k},K_{k},\mu_{k},v_{k}}J_{\mathrm{MAX\textunderscore
COVAR}}=-\lambda_{\mathrm{min}}(\Sigma_{0})$ (MAX-COVAR)

such that for all $k=0,1,\cdots,N-1$,

$\displaystyle\begin{aligned}
\Sigma_{k+1}=A&\Sigma_{k}A^{\intercal}+BK_{k}\Sigma_{k}A^{\intercal}+A\Sigma_{k}K^{\intercal}_{k}B^{\intercal}\\\
&+BK_{k}\Sigma_{k}K^{\intercal}_{k}B^{\intercal}+DD^{\intercal},\end{aligned}$
$\displaystyle\lambda_{\mathrm{max}}(\Sigma_{N})\leq\lambda_{\mathrm{min}}(\Sigma_{\mathcal{G}}),$
(20a) $\displaystyle\mu_{k+1}=A\mu_{k}+Bv_{k},$ (20b)
$\displaystyle\mu_{0}=\mu_{i},\ \ \mu_{N}=\mu_{\mathcal{G}}$ (20c)
$\displaystyle\Phi^{-1}(1-\epsilon_{x})\sqrt{\alpha^{\intercal}_{x}\Sigma_{k}\alpha_{x}}+\alpha^{\intercal}_{x}\mu_{k}-\beta_{x}\leq
0,$ (20d)
$\displaystyle\Phi^{-1}(1-\epsilon_{u})\sqrt{\alpha^{\intercal}_{u}K_{k}\Sigma_{k}K^{\intercal}_{k}\alpha_{u}}+\alpha^{\intercal}_{u}v_{k}-\beta_{u}\leq
0,$ (20e)

where $\lambda_{\mathrm{min}}(.)$ is the minimum eigenvalue operator.
$\lambda_{\mathrm{min}}(A)$ is a concave function of the positive semidefinite
matrix variable $A$, therefore MAX-COVAR is a convex minimization objective.
The feasible region of the above optimization problem is non-convex and we use
the similar lossless convexification as described in Section III-B to tackle
MAX-COVAR.

Notation: We define a predicate
$\operatorname{FEASIBLE}\coloneqq\operatorname{FEASIBLE}(q,p,N)$ that returns
a boolean TRUE or FALSE if there exists a feasible $N$-step control sequence
such that the system of equations (20a)-(20e) defined for
$(\mu_{\mathcal{I}},\Sigma_{\mathcal{I}})=(\mu_{q},\Sigma_{q})$, and
$(\mu_{\mathcal{G}},\Sigma_{\mathcal{G}})=(\mu_{p},\Sigma_{p})$ is feasible.
Also, we use the notation $q\xrightarrow[N]{\mathscr{C}}p$ to denote that the
mean and covariance dynamics initialized at $(\mu_{q},\Sigma_{q})$ and driven
by the N-step control sequence $\mathscr{C}$ satisfy the state and control
chance constraints (20d)-(20e) at all time-steps and the terminal goal
reaching constraint (20a) corresponding to
$(\mu_{\mathcal{G}},\Sigma_{\mathcal{G}})=(\mu_{p},\Sigma_{p})$.

######  Remark 1.

(Reuse) Let $\mathscr{C}$ be a $N$-step control sequence s.t.
$\mathcal{I}\xrightarrow[N]{\mathscr{C}}\mathcal{G}$, then it follows that
$\mathcal{I}^{-}\xrightarrow[N]{\mathscr{C}}\mathcal{G}$ for all
$\mathcal{I}^{-}$ s.t. $\mu_{\mathcal{I}^{-}}=\mu_{\mathcal{I}}$, and
$\Sigma_{\mathcal{I}^{-}}\preceq\Sigma_{\mathcal{I}}$. In other words, a
control sequence computed for the steering maneuver from $\mathcal{I}$ to
$\mathcal{G}$ remains a feasible maneuver from $\mathcal{I}^{-}$ to
$\mathcal{G}$ and thus could be reused.

The above directly follows from an analysis of equations (20a)-(20e) and we
omit a formal proof.

Rationale behind $\operatorname{MAX-COVAR}$: MAX-COVAR is based on the
maximization of the minimum eigenvalue of $\Sigma_{\mathrm{initial}}$ for a
given initial mean, and a desired target distribution. It is based on the
observation that if $\mathcal{I}\xrightarrow{\mathscr{C}}\mathcal{G}$, then
$\mathcal{I}^{\prime}\xrightarrow{\mathscr{C}}\mathcal{G}\ \ \forall\ \
\mathcal{I}^{\prime}\text{ s.t.
}\mu_{\mathcal{I}}=\mu_{\mathcal{I}^{\prime}}\text{ and
}\Sigma_{\mathcal{I}^{\prime}}\prec\Sigma_{\mathcal{I}}$. In other words, if
the system trajectory initialized at $\mathcal{I}$ respects all the
constraints and reaches the target distribution $\mathcal{G}$ under control
sequence $\mathscr{C}$, then for the system initialized at
$\mathcal{I}^{\prime}$ such that
$\mu_{\mathcal{I}^{\prime}}=\mu_{\mathcal{I}}$ and
$\Sigma_{\mathcal{I}^{\prime}}\prec\mathcal{I}$, the system satisfies all the
constraints under the same control sequence $\mathscr{C}$.

Therefore, we aim to find a $(\Sigma_{\mathrm{initial}},\mathscr{C})$ such
that the computed $\mathscr{C}$ could be reused across largest possible number
of initial distributions, i.e., find $\mathcal{I}$ such that
$\\{\mathcal{I}^{\prime}\ |\ \mathcal{I}^{\prime}\prec\mathcal{I}\\}$ is the
largest. This leads to the maximization of the minimum eigenvalue of the
initial covariance as a natural objective function for our search.

Significance of MAX-COVAR: $\operatorname{MAX-COVAR}$ provides a certificate
of reachability for any goal distribution in terms of the maximum permissible
value of the minimum eigenvalue of the covariance at any query mean for which
there exists a feasible control sequence that can achieve the corresponding
steering maneuver under control constraints. For instance, consider a goal
distribution $(\mu_{\mathcal{G}},\Sigma_{\mathcal{G}})$ and a query mean
$\mu_{q}$, and let $\Sigma_{q,\mathrm{max}}$, $\mathscr{C}_{q,\mathrm{max}}$
be such that,

$\displaystyle\Sigma_{q,\mathrm{max}},\mathscr{C}_{q,\mathrm{max}}\longleftarrow\operatorname{MAXCOVAR}(\mu_{q},(\mu_{\mathcal{G},\Sigma_{\mathcal{G}}}),N).$

It follows from the above that $\forall\ \Sigma\succ 0$ s.t.
$\lambda_{\mathrm{min}}(\Sigma)>\lambda_{\mathrm{min}}(\Sigma_{q,\mathrm{max}})$,
$\nexists\ \mathscr{C}$ s.t.
$(\mu_{q},\Sigma)\xrightarrow[N]{\mathscr{C}}\mathcal{G}$ by definition of
MAX-COVAR otherwise $\lambda_{\mathrm{min}}(\Sigma_{q,\mathrm{max}})$ is not
the maximum possible minimum eigenvalue of the initial covariance for the
existence of a feasible path and we arrive at a contradiction. On the other
hand, all matrices $\Sigma\succ 0$ such that their minimum eigenvalue is less
than the minimum eigenvalue of the covariance matrix computed in the maximal
sense i.e.
$\lambda_{\mathrm{min}}(\Sigma)<\lambda_{\mathrm{min}}(\Sigma_{q,\mathrm{max}})$
have lesser coverage than the maximal covariance matrix, a fact that is
formalized later in Lemma 2.

###  IV-B Construction of the MAXCOVAR BRT

The algorithm proceeds by building a tree $\mathcal{T(\mathcal{G})}$
represented through a set of nodes $\\{\mathcal{\nu}_{i}\\}$, and a set of
edge controllers $\\{\varepsilon_{i,j}\\}$. Each node $i$, $\nu_{i}$, is a
tuple $\left(\mu_{i},\Sigma_{i},p_{i},\mathscr{C}_{i},\mathrm{ch}_{i}\right)$
where $\mu_{i},\Sigma_{i}$ are the mean and the covariance of the distribution
stored in the node, $p_{i}$ is a pointer to the parent node,
$\mathscr{C}_{i}\coloneqq\\{K^{i,p_{i}}_{t},v^{i,p_{i}}_{t}\\}_{t=0}^{N-1}$ is
the $N$-step control sequence stored at the node that steers the state
distribution from $(\mu_{i},\Sigma_{i})$ to $(\mu_{p_{i}},\Sigma_{p_{i}})$,
and $\mathrm{ch}_{i}$ is the list of pointers of all the children node of node
$i$ in the tree $\mathcal{T}(\mathcal{G})$.

$\varepsilon$ is another data structure that stores all the edge information
for the tree $\mathcal{T}(\mathcal{G})$, such that,
$\varepsilon_{i,j}\coloneqq\\{K^{i,j}_{t},v^{i,j}_{t}\\}_{t=0}^{N-1}$ stores
the $N$-step control sequence that steers the state distribution from node $i$
to node $j$ if such an edge exists, and is empty otherwise.

Algorithm 1 Constructing the MAXCOVAR BRT

1:$\mathcal{G}$, $N$, $n_{\mathrm{iter}}$

2:$\mathcal{T}$

3:$\nu\leftarrow\phi$, $\varepsilon\leftarrow\phi$

4:$\nu_{0}\leftarrow\operatorname{CREATENODE}(\mu_{\mathcal{G}},\Sigma_{\mathcal{G}},\operatorname{NONE},\operatorname{NONE},\\{\\})$

5:$\nu\leftarrow\nu\cup\\{\nu_{0}\\}$

6:for $i\leftarrow 1\textrm{ to }n_{\mathrm{iter}}$ do

7: $\nu_{k}\leftarrow\operatorname{RAND}(\nu)$

8:
$\mu_{q}\leftarrow\operatorname{RANDMEANAROUND}(\nu_{k},r_{\textrm{sample}})$

9:
$\textrm{status},\Sigma_{\mathrm{max}},\mathscr{C}_{q}\leftarrow\operatorname{MAXCOVAR}(\mu_{q},(\mu_{k},\Sigma_{k}),N)$

10: if $\textrm{status}\neq\textrm{infeasible}$ then

11: $\textrm{idx}\leftarrow\operatorname{size}(\mathcal{\nu})+1$

12: $\mathrm{ch}_{\mathrm{idx}}\leftarrow\\{\\}$

13:
$\nu_{\mathrm{new}}\leftarrow\operatorname{CREATENODE}(\mu_{q},\Sigma_{\mathrm{max}},\textrm{idx},k,\mathrm{ch}_{\mathrm{idx}})$

14: $\mathrm{ch}_{k}\leftarrow\mathrm{ch}_{k}\bigcup\\{\textrm{idx}\\}$

15: $\varepsilon_{\textrm{idx},k}\leftarrow\mathscr{C}_{q}$

16:
$\varepsilon\leftarrow\varepsilon\bigcup\\{\varepsilon_{\textrm{idx},k}\\}$

17:$\mathcal{T}\leftarrow\\{\nu,\varepsilon\\}$

18:$\textrm{{Return}}\ \ \mathcal{T}$

Now, we discuss the essential sub-routines of the above algorithm.

####  IV-B1 Node selection

The tree is grown in the spirit of finding paths from all query initial
distributions for which paths would exist to the goal distribution. In our
implementation, the nodes are selected randomly according to the Voronoi bias
(of the first order moment of the nodes) to bias population of the $1-$BRS of
the node distributions whose corresponding Voronoi regions are relatively
unexplored in the sense of the first order moment of the distributions.

####  IV-B2 Node expansion

Once a node to expand has been selected, a query mean is sampled from a
neighbourhood of some radius around it and a connection is attempted through
the $\operatorname{MAXCOVAR}$ method for edge construction. Let’s say the
$k$th node on the tree containing the distribution $(\mu^{(k)},\Sigma^{(k)})$
has been selected to expand, and let $\mu_{q}$ be the query mean sampled from
a neighbourhood around $\mu^{(k)}$ through the
$\operatorname{RANDMEANAROUND}(.,r)$ module where $r$ is some sampling radius.
We solve the following optimization to construct the edge,

$\displaystyle\Sigma_{\mathrm{max}},\mathscr{C}_{\mathrm{max}}\longleftarrow\operatorname{MAXCOVAR}(\mu_{q},(\mu^{(k)},\Sigma^{(k)}),N)$
(21)

$(\mu_{q},\Sigma_{\mathrm{max}})$ is added as a node to the tree with the edge
controller $\mathscr{C}_{\mathrm{max}}$ and $(\mu^{(k)},\Sigma^{(k)})$ as the
parent if the status of the above optimization problem (as returned by the
solver) is not infeasible.

Definitions: We now define the following mathematical objects that will aid
the analysis and further discussion of our proposed approach. The $h$ hop
backward reachable set of distributions for a distribution $p$,
h-$\operatorname{BRS}(p)$ is defined as follows,

$\displaystyle
h-\operatorname{BRS}(p)=\\{(\mu_{q},\Sigma_{q})|\operatorname{FEASIBLE}(q,p,hN)=\operatorname{TRUE}\\}.$
(22)

We also define the $h$-$\operatorname{BRS}$ of a tree of distributions
$\mathcal{T}$ as,

$\displaystyle\text{h-}\operatorname{BRS}(\mathcal{T})=\bigcup_{i\in\nu(\mathcal{T})}\text{($h-d_{i}$)-}\operatorname{BRS}(\nu_{i}),$
(23)

where $\nu(\mathcal{T})$ is the set of all vertices in the tree, and $d_{i}$
is the distance of the $i$-th node from the root node in terms of the number
of hops.

Concatenation of control sequences: A concatenation $\mathscr{C}_{A,B}$ of two
control sequences $\mathscr{C}_{A}$, $\mathscr{C}_{B}$ of lengths
$N_{A},N_{B}$ respectively is a control sequence of length $N_{A}+N_{B}$
represented through the $\bigcup$ operator i.e.
$\mathscr{C}_{A,B}\coloneqq\mathscr{C}_{A}\bigcup\mathscr{C}_{B}$, s.t.
$\mathscr{C}_{A,B}(t)\coloneqq C_{A}(t)\ \forall\ t=0,1,\cdots N_{A}-1$, and
$\mathscr{C}_{A,B}(t)\coloneqq C_{B}(t-N_{A})\ \forall\ t=N_{A},N_{A}+1,\cdots
N_{A}+N_{B}-1$. Note that the concatenation operator $\bigcup$ is non-
commutative in the two argument control sequences, i.e.
$\mathscr{C}_{A}\bigcup\mathscr{C}_{B}\neq\mathscr{C}_{B}\bigcup\mathscr{C}_{A}$.

To establish / guarantee feasibility of the query distribution to the goal
distribution, it is sufficient to guarantee feasibility to any distribution
that is already verified to reach the goal. This idea can be seen in the
following lemma on sequential composition of control sequences ensuring
satisfaction of state and control chance constraints along the overall
concatenated trajectory,

######  Lemma 1.

(Feasibility through Sequential Composition) Let
$(\mu_{i},\Sigma_{i}),(\mu_{j},\Sigma_{j}),(\mu_{k},\Sigma_{k}),\mathscr{C}_{i,j},\mathscr{C}_{j,k},h_{i,j},h_{j,k}$
be such that
$(\mu_{i},\Sigma_{i})\xrightarrow[h_{i,j}N]{\mathscr{C}_{i,j}}(\mu_{j},\Sigma_{j})$,
and
$(\mu_{j},\Sigma_{j})\xrightarrow[h_{j,k}N]{\mathscr{C}_{j,k}}(\mu_{k},\Sigma_{k})$.
It follows that
$(\mu_{i},\Sigma_{i})\xrightarrow[(h_{i,j}+h_{j,k})N]{\mathscr{C}_{i,k}}(\mu_{k},\Sigma_{k})$
where $\mathscr{C}_{i,k}\coloneqq\mathscr{C}_{i,j}\bigcup\mathscr{C}_{j,k}$.

###### Proof.

We want to prove that the system initialized at the distribution
$(\mu_{i},\Sigma_{i})$ and driven by the control sequence $\mathscr{C}_{i,k}$
which is obtained from the concatenation of the two control sequences
$\mathscr{C}_{i,j}$, and $\mathscr{C}_{j,k}$ i.e.
$\mathscr{C}_{i,k}=\mathscr{C}_{i,j}\bigcup\mathscr{C}_{j,k}$ satisfies all
the state and control chance constraints of the form (20d)-(20e) for a
trajectory of length $(h_{i,j}+h_{j,k})N$, and the terminal goal reaching
constraints of the form (20c) and (20a) for the goal distribution
$(\mu_{k},\Sigma_{k})$.

The mean and covariance dynamics at any time $t$ as a function of the intial
distribution and the control sequence are represented as
$\mu_{t}(\mu_{i},\mathscr{C}_{i,k})$ and
$\Sigma_{t}(\Sigma_{i},\mathscr{C}_{i,k})$ respectively.

For $t=0,1,\cdots h_{i,j}N-1$,
$\mu_{t}(\mu_{i},\mathscr{C}_{i,k})=\mu_{t}(\mu_{i},\mathscr{C}_{i,j})$, and
$\Sigma_{t}(\Sigma_{i},\mathscr{C}_{i,k})=\Sigma_{t}(\Sigma_{i},\mathscr{C}_{i,j})$.
Since $\mathscr{C}_{i,j}$ is a feasible control sequence for the $(i,j)$
maneuver s.t.
$(\mu_{i},\Sigma_{i})\xrightarrow[N_{i,j}]{\mathscr{C}_{i,j}}(\mu_{j},\Sigma_{j})$,
all the state and control chance constraints are satisfied by
$\mu_{t}(\mu_{i},\mathscr{C}_{i,j})$ and
$\Sigma_{t}(\Sigma_{i},\mathscr{C}_{i,j})$ for $t=0,1,\cdots N_{i,j}-1$, and
$\Sigma_{N_{i,j}}(\Sigma_{i},\mathscr{C}_{i,k})$ is s.t.
$\lambda_{\mathrm{max}}(\Sigma_{N_{i,j}}(\Sigma_{i},\mathscr{C}_{i,k}))\leq\lambda_{\mathrm{min}}(\Sigma_{j})$
which implies
$\Sigma_{N_{i,j}}(\Sigma_{i},\mathscr{C}_{i,k})\preceq\Sigma_{j}$.

The rest of the maneuver for $t=N_{i,j},N_{i,j}+1,\cdots N_{i,j}+N_{j,k}-1$
could be thought of as an $N_{j,k}$ step maneuver initialized at
$\mu_{N_{i,j}}$ and $\Sigma_{N_{i,j}}$. $\mathscr{C}_{j,k}$ is s.t.
$(\mu_{j},\Sigma_{j})\xrightarrow[N_{j,k}]{\mathscr{C}_{j,k}}(\mu_{k},\Sigma_{k})$,
therefore from Remark 1,
$(\mu_{j},\Sigma_{N_{i,j}}(\Sigma_{i},\mathscr{C}_{i,k}))\xrightarrow[N_{j,k}]{\mathscr{C}_{j,k}}(\mu_{k},\Sigma_{k})$
since $\Sigma_{N_{i,j}}(\Sigma_{i},\mathscr{C}_{i,k})\preceq\Sigma_{j}$. ∎

###  IV-C Planning through the BRT

In this section, we discuss our approach to find feasible paths to the goal
through a BRT.

Finding a feasible path: To find a feasible path to the goal for a query
distribution $q\coloneqq(\mu_{q},\Sigma_{q})$, single hop connections are
attempted one-by-one to $M$ nearest nodes on the BRT for some hyperparameter
$M$. For a candidate node $\nu_{k}$ on the BRT, the following problem is
solved,

$\displaystyle\mathscr{C}_{q}\longleftarrow\operatorname{OPT-
STEER}((\mu_{q},\Sigma_{q}),(\mu^{(k)},\Sigma^{(k)}),N).$ (24)

The search for a feasible path terminates once a connection has successfully
been established to one of the existing nodes on the BRT, and is given by a
concatenation of the above computed control sequence $\mathscr{C}_{q}$ with
the pre-computed controllers stored in the sequence of edges of the tree from
the $k$th node to the root node. Let $\nu_{k}$ be a distance of $d_{k}$ hops
away from the goal s.t.
$\mathrm{idx}_{0},\mathrm{idx}_{1},\cdots,\mathrm{idx}_{d_{k}}$ be the
sequence of nodes encountered from the $k$th node to the root node where
$\mathrm{idx}_{0}=k$ and $\mathrm{idx}_{d_{k}}=0$. Thererfore, the feasible
path from $q$ to the goal $\mathcal{G}$ is obtained as,
$\mathscr{C}_{q,\mathcal{G}}=\mathscr{C}_{q}\bigcup\mathscr{C}_{\mathrm{idx}_{0},\mathrm{idx}_{1}}\bigcup\mathscr{C}_{\mathrm{idx}_{1},\mathrm{idx}_{2}}\cdots\bigcup\mathscr{C}_{\mathrm{idx}_{d_{k}-1},\mathrm{idx}_{d_{k}}}$
s.t. $q\xrightarrow[(d_{k}+1)N]{\mathscr{C}_{q,\mathcal{G}}}\mathcal{G}$,
which follows from Lemma 1.

Implication of recursive feasibility on the speed-up in computing a feasible
path: Say that the query distribution $q$ is such that
$q\in\text{t}-\operatorname{BRS}(\mathcal{G})$ and
$q\notin\text{t'}-\operatorname{BRS}(\mathcal{G})\ \forall\ t^{\prime}<t$,
i.e. a path from $q$ to the goal shorter than $t$-hops does not exist.
Therefore, to compute a feasible path that steers the system from $q$ to
$\mathcal{G}$ without reusing any of the pre-computed controllers from the
BRT, a feasibility instance $\operatorname{FEASIBLE}(q,\mathcal{G},tN)$ of
size $tN$ needs to be solved. Alternatively, if the search for a feasible path
is carried through attempting connections to the BRT, the expenditure on
compute is that of solving a feasibility instance of size $N$ a maximum $M$
number of times resulting in an order of magnitude savings in computation.
More details about the empirical experiments and results can be found in
Section V.

Maximum Coverage of the MAXCOVAR BRT: As discussed earlier, re-use of
controllers stored along the edges of a BRT can lead to significant speedup in
the computation of a feasible path to the goal. Therefore, it is a desirable
property for a BRT to be such that paths from as large a number of query
initial distributions as possible can be found to the tree. This property is
characterized in terms of the coverage of the BRT, and for any given BRT
$\mathcal{T}$, its coverage is quantified through the set of all distributions
that can reach the tree in $h$ hops i.e.

$\displaystyle\operatorname{Cover}(\mathcal{T})\coloneqq\text{h}-\operatorname{BRS}(\mathcal{T}).$
(25)

We now show that the BRT constructed through the novel objective function for
edge construction defined in Section IV-A henceforth referred to as MAXCOVAR
BRT discovers feasible paths from provably the largest possible set of query
initial distributions compared to any other procedure of edge construction.
This is formalized in Theorem 1 below. We proceed by first proving Lemma 2
that talks about the coverage of a single node and is used as a building block
for Theorem 1 that concerns the coverage of a tree (multiple nodes).

######  Lemma 2.

For $\Sigma_{\mathrm{max}},\Sigma^{-}_{\mathrm{max}}\succ 0$, s.t.
$\lambda_{\mathrm{min}}(\Sigma_{\mathrm{max}})>\lambda_{\mathrm{min}}(\Sigma^{-}_{\mathrm{max}})$,
$h\text{-}\operatorname{BRS}(\mu,\Sigma_{\mathrm{max}})\supseteq
h\text{-}\operatorname{BRS}(\mu,\Sigma^{-}_{\mathrm{max}})\ \forall\ h\geq 1$
for all planning scenes. Also, there exist planning scenes
$\\{(\alpha_{x},\beta_{x},\epsilon_{x}),(\alpha_{u},\beta_{u},\epsilon_{u})\\}$
s.t. $h\text{-}\operatorname{BRS}(\mu,\Sigma_{\mathrm{max}})\supset
h\text{-}\operatorname{BRS}(\mu,\Sigma^{-}_{\mathrm{max}})\ \forall\ h\geq 1$.

###### Proof.

Proof relegated to Appendix References. ∎

######  Theorem 1 (Maximum Coverage).

$\text{h-}\operatorname{BRS}(\mathcal{T}^{(r)}_{\mathrm{MAXCOVAR}})\supseteq\text{h-}\operatorname{BRS}(\mathcal{T}^{(r)}_{\mathrm{ANY}})$
for all planning scenes, and there always exist planning scenes such that
$\text{h-}\operatorname{BRS}(\mathcal{T}^{(r)}_{\mathrm{MAXCOVAR}})\supset\text{h-}\operatorname{BRS}(\mathcal{T}^{(r)}_{\mathrm{ANY}})\
\forall\ r\geq 1,\ \forall\ h\geq 1$.

###### Proof.

Proof relegated to Appendix References. ∎

##  V Experiments

To illustrate our method, we conduct experiments for the motion planning of a
quadrotor in a 2D plane. The lateral and longitudinal dynamics of the
quadrotor are modeled as a triple integrator leading to a 6 DoF model with
state matrices,

$A=\left[\begin{array}[]{ccc}I_{2}&\Delta TI_{2}&0_{2}\\\ 0_{2}&I_{2}&\Delta
TI_{2}\\\
0_{2}&0_{2}&I_{2}\end{array}\right],~{}B=\left[\begin{array}[]{c}0_{2}\\\
0_{2}\\\ \Delta TI_{2}\end{array}\right],~{}D=0.1I_{6},$

a time step of 0.1 seconds, a horizon of $N=20$, and a goal distribution
$\mathcal{G}$ for the planning task as follows:

$\displaystyle\mu_{\mathcal{G}}=\mathbf{0}_{6\times
1},\quad\Sigma_{\mathcal{G}}=0.1*\mathbf{I}_{6\times 6}.$

The control input space is characterized by a bounding box represented as
$\alpha_{u}=\left\\{\left[\pm 1,0\right]^{\top},\left[0,\pm
1\right]^{\top}\right\\}$, $\beta_{u}=\\{\pm 25,\pm 25\\}$. The chance
constraint linearization is performed around $\Sigma_{r}=1.2\
\mathbf{I}_{6\times 6}$, and $Y_{r}=15\ \mathbf{I}_{2\times 2}$. All the
optimization programs are solved in Python using cvxpy [15].

Figure 2: Coverage experiment setup: MAXCOVAR and RANDCOVAR BRTs

Construction of the BRTs: For the tree construction procedure, a sampling
radius of $r_{\mathrm{sample}}=\left[\pm 5,\pm 5,\pm 2.5,\pm 2.5,\pm 1.25,\pm
1.25\right]$ was used. Fig. 2 shows the constructed MAXCOVAR and the RANDCOVAR
trees corresponding to the goal distribution $\mathcal{G}$ that were used for
the experiments on coverage. The figure displays the first two states of the 6
DoF model: the x ($\operatorname{state}[0]$) and y ($\operatorname{state}[1]$)
locations of the quadrotor on the x-y plane. Each node on the plot denotes the
first two dimensions of the state distribution mean with edges between the
nodes displayed as directed arrows such that the corresponding $N$-step
control sequences are stored offline. The blue annulus around the two trees in
Figure 2 represents the region over which the query means were uniformly
sampled. Both the trees in Fig. 2 consist of 265 nodes and were generated with
the same random seed.

The construction procedure for the MAXCOVAR tree was described in Algorithm 1.
For any existing node $\nu_{k}$ on the tree that’s selected to expand, and for
any query mean $\mu_{q}$ sampled from a box around it, the new node covariance
$\Sigma_{q,\mathrm{max}}$ and the corresponding edge controller
$\mathscr{C}_{q,\mathrm{max}}$ is the result of solving the MAXCOVAR
optimization problem.

For the construction of the RANDCOVAR tree, the node covariance
$\Sigma_{\mathrm{rand}}$ was randomly sampled from the space of positive
definite matrices, similar in spirit to [1], and the edge controller was given
by the optimal steering control from $(\mu_{q},\Sigma_{\mathrm{rand}})$ to
$(\mu_{k},\Sigma_{k})$. To construct samples from the positive definite matrix
space, the eigenvalues and orthonormal eigenvectors that constitute a positive
definite matrix are sampled separately. It was observed empirically that
randomly sampling node covariances resulted in rejecting a lot of candidate
nodes due to the non-existence of a corresponding steering maneuver.
Therefore, eigenvalues of the candidate node covariance
$\Sigma_{\mathrm{rand}}$ were sampled to ensure that
$\lambda_{\mathrm{min}}(\Sigma_{\mathrm{rand}})\leq\lambda_{\mathrm{min}}(\Sigma_{\mathrm{max}})$.

Figure 3: Coverage experiment metrics

Coverage Experiment: The coverage experiment proceeds by sampling query
distributions and attempting connects to the two trees to find paths. Query
means were sampled from the blue annulus as shown in Fig. 2, and the query
covariances were considered to be diagonal matrices where the diagonal entries
were sampled uniformly over an interval. The experiment was repeated for
different intervals of sampling the diagonal entries of the query covariance
as shown in the x-axis of Fig. 3. For each interval, the experiment was
repeated 250 times and the percentage of times a path was found to the two
trees was reported.

Interpretation of the Coverage Experiment: From Fig. 3, it can be seen that
for intervals corresponding to higher candidate eigenvalues of the query
covariance, the RANDCOVAR tree with randomly sampled node covariances is not
able to find paths in contrast to MAXCOVAR tree where the node covariances and
the edges were constructed explicitly to provide maximal coverage. This is a
direct consequence of Lemma 2 which says that for two positive definite
matrices $\Sigma$ and $\Sigma^{-}$, there exist planning scenes such that
distributions with a larger spectral radius of the covariance reach $\Sigma$
as compared to $\Sigma^{-}$ (Lemma 2 follows a proof by construction, see
Appendix References).

Figure 4: Real-time planning through the pre-computed MAXCOVAR BRT

Real-time planning through the MAXCOVAR BRT: The generated tree and the
associated control sequences were stored offline. For real-time planning, a
query mean $\mu_{q}$ was randomly sampled in a box around the origin with a
sampling radius of $[\pm 25,\pm 25,\pm 25,\pm 25,0,0]$ with a query covariance
of $\Sigma_{q}=0.1\ \mathbf{I}_{6\times 6}$. Fig. 4 shows one such run that
resulted in a $9$-hop chained maneuver that drives the sampled query mean to
the goal (a total of $9\times N=180$ timesteps). It took $1$ second to find a
feasible path through the offline generated BRT, and around $2$ minutes to
find a path through OPT-STEER($q,\mathcal{G},N$) for $N=180$. In Fig. 4, the
red and green ellipses correspond to the $3$-sigma confidence interval of the
sampled query node and the goal node respectively. The blue ellipses
correspond to the distributions stored in the nodes of the discovered feasible
path. Monte carlo simulations were performed and the system trajectories are
represented by black lines. The only real-time computation that was done was
to find a control sequence that drives the system from the red ellipse to the
first blue ellipse in the sequence, the rest of the trajectories were
propagated using controllers stored along each edge of the discovered feasible
path of the BRT.

## Conclusion

In this paper we introduced Maximal Covariance Backward Reachable Trees
(MAXCOVAR BRT), a multi-query algorithm for probabilistic planning under
constraints on the control input with explicit coverage guarantees. Our
contribution was a novel optimization formulation to add nodes and construct
the corresponding edge controllers such that the generated roadmap results in
provably maximal coverage. The notion of coverage of a roadmap was
characterized formally via h-$\operatorname{BRS}$ (Backward Reachable Set of
Distributions) of a tree of distributions, and our proposed method was
supported by theoretical analysis as well as extensive simulation on a 6 DoF
model.

## References

  * [1] D. Zheng, J. Ridderhof, Z. Zhang, P. Tsiotras, and A.-A. Agha-mohammadi, “Cs-brm: A probabilistic roadmap for consistent belief space planning with reachability guarantees,” IEEE Transactions on Robotics, 2024.
  * [2] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” IEEE transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580, 1996.
  * [3] V. N. Hartmann, M. P. Strub, M. Toussaint, and J. D. Gammell, “Effort informed roadmaps (eirm*): Efficient asymptotically optimal multiquery planning by actively reusing validation effort,” in The International Symposium of Robotics Research, pp. 555–571, Springer, 2022.
  * [4] 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.
  * [5] A. Majumdar, M. Tobenkin, and R. Tedrake, “Multi-query feedback motion planning with lqr-roadmaps,” 2011.
  * [6] A. Majumdar and R. Tedrake, “Funnel libraries for real-time robust feedback motion planning,” The International Journal of Robotics Research, vol. 36, no. 8, pp. 947–982, 2017.
  * [7] M. K. M Jaffar and M. Otte, “Pip-x: Online feedback motion planning/replanning in dynamic environments using invariant funnels,” The International Journal of Robotics Research, p. 02783649231209340, 2023.
  * [8] B. Luders, M. Kothari, and J. How, “Chance constrained rrt for probabilistic robustness to environmental uncertainty,” in AIAA guidance, navigation, and control conference, p. 8160, 2010.
  * [9] B. D. Luders, Robust sampling-based motion planning for autonomous vehicles in uncertain environments.  PhD thesis, Massachusetts Institute of Technology, 2014.
  * [10] 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.
  * [11] D. Zheng, J. Ridderhof, Z. Zhang, P. Tsiotras, and A.-A. Agha-mohammadi, “Cs-brm: A probabilistic roadmap for consistent belief space planning with reachability guarantees,” IEEE Transactions on Robotics, 2024.
  * [12] K. Okamoto, M. Goldshtein, and P. Tsiotras, “Optimal covariance control for stochastic systems under chance constraints,” IEEE Control Systems Letters, vol. 2, no. 2, pp. 266–271, 2018.
  * [13] F. Liu, G. Rapakoulias, and P. Tsiotras, “Optimal covariance steering for discrete-time linear stochastic systems,” arXiv preprint arXiv:2211.00618, 2022.
  * [14] G. Rapakoulias and P. Tsiotras, “Discrete-time optimal covariance steering via semidefinite programming,” arXiv e-prints, pp. arXiv–2302, 2023.
  * [15] S. Diamond and S. Boyd, “Cvxpy: A python-embedded modeling language for convex optimization,” The Journal of Machine Learning Research, vol. 17, no. 1, pp. 2909–2913, 2016.

###### Proof of Lemma 2.

We want to show that $\ \forall\ p\in
h\text{-}\operatorname{BRS}(\mu,\Sigma^{-}_{\mathrm{max}});\ \exists\
\mathscr{C}_{p}$ s.t.
$p\xlongrightarrow[hN]{\mathscr{C}_{p}}(\mu,\Sigma_{\mathrm{max}})$, what we
refer to as the forward side of the argument, and also that $\ \exists\ q$
s.t. $q\in h\text{-}\operatorname{BRS}(\mu,\Sigma_{\mathrm{max}})$ and
$\nexists\ \mathscr{C}_{q}$ s.t.
$q\xlongrightarrow[hN]{\mathscr{C}_{q}}(\mu,\Sigma^{-}_{\mathrm{max}})$, what
we refer to as the backward side of the argument.

We first prove the forwards side of the argument. Let $p$ be any element of
$h\text{-}\operatorname{BRS}(\mu,\Sigma^{-}_{\mathrm{max}})$. Therefore,
$\exists\ \mathscr{C}_{p}$ s.t. the equations (20a)-(20e) are satisfied. From
(20a), $\Sigma_{{hN}}(p,\mathscr{C}_{p})$ s.t.
$\lambda_{\mathrm{max}}(\Sigma_{hN}(p,\mathscr{C}_{p}))\leq\lambda_{\mathrm{min}}(\Sigma^{-}_{\mathrm{max}})$.
Since
$\lambda_{\mathrm{min}}(\Sigma^{-}_{\mathrm{max}})<\lambda_{\mathrm{min}}(\Sigma_{\mathrm{max}})$,
$\lambda_{\mathrm{max}}(\Sigma_{hN}(p,\mathscr{C}_{p}))\leq\lambda_{\mathrm{min}}(\Sigma_{\mathrm{max}})$.
Therefore, $p\in h\text{-}\operatorname{BRS}(\mu,\Sigma_{\mathrm{max}})$ under
the same control sequence $\mathscr{C}_{p}$ that drives $p$ to
$(\mu,\Sigma^{-}_{\mathrm{max}})$. Since the above argument can be reproduced
for any arbitrary distribution that reaches $(\mu,\Sigma^{-}_{\mathrm{max}})$,
it follows that $\forall\ p\in
h\text{-}\operatorname{BRS}(\mu,\Sigma^{-}_{\mathrm{max}}),p\in
h\text{-}\operatorname{BRS}(\mu,\Sigma_{\mathrm{max}})$.

Now, we prove the backwards side of the argument. Specifically, we construct
$q$ and a planning scene
$\\{(\alpha_{x},\beta_{x},\epsilon_{x}),(\alpha_{u},\beta_{u},\epsilon_{u})\\}$
such that $q\in h\text{-}\operatorname{BRS}(\mu,\Sigma_{\mathrm{max}})$ and
$q\notin h\text{-}\operatorname{BRS}(\mu,\Sigma^{-}_{\mathrm{max}})$. Consider
$q(\mu_{q},\Sigma_{q})$ such that,

$\displaystyle\Sigma_{q},\mathscr{C}_{q}\longleftarrow\operatorname{MAX-
COVAR}(\mu_{q},(\mu,\Sigma^{-}_{\mathrm{max}}),hN),$ (26)

and $\lambda_{\mathrm{max}}(\Sigma_{q})<\infty$. We also assume that for
$(\mu_{q},\Sigma_{q})$ and $\mathscr{C}_{q}$, the corresponding state and
control chance constraints (20d)-(20e) are non-tight, i.e., are strict
inequalities. This is a mild assumption and can be shown to hold by
constructing $\mu_{q}$ in an appropriate manner.

From (26), $\nexists\ \Sigma_{q^{+}},\mathscr{C}_{q^{+}}$ s.t.
$\lambda_{\mathrm{min}}(\Sigma_{q^{+}})>\lambda_{\mathrm{min}}(\Sigma_{q})$
and
$(\mu_{q},\Sigma_{q^{+}})\xlongrightarrow[kN]{\mathscr{C}_{q^{+}}}(\mu,\Sigma^{-}_{\mathrm{max}})$
$\ast$.

Now, consider a perturbation to $q$ such that
$\Sigma(\epsilon)=\Sigma_{q}+\epsilon\mathrm{I}$ for some $\epsilon>0$. From
$(\ast)$ SDP Synthesis of Maximum Coverage Trees for Probabilistic Planning
under Control Constraints, $(\mu_{q},\Sigma(\epsilon))\notin
h\text{-}\operatorname{BRS}(\mu,\Sigma^{-}_{\mathrm{max}})$ since
$\lambda_{\mathrm{min}}(\Sigma(\epsilon))>\lambda_{\mathrm{min}}(\Sigma_{q})$
for $\epsilon>0$. We show that $\exists$ planning scenes
$\\{(\alpha_{x},\beta_{x},\epsilon_{x}),(\alpha_{u},\beta_{u},\epsilon_{u})\\}$
$\exists\ \epsilon>0$ s.t. $(\mu_{q},\Sigma(\epsilon))\in
h\text{-}\operatorname{BRS}(\mu,\Sigma_{\mathrm{max}})$.

Consider $\mathscr{C}(\lambda)$ as the candidate control sequence s.t.
$(\mu_{q},\Sigma(\epsilon))\xlongrightarrow[hN]{\mathscr{C}(\lambda)}(\mu,\Sigma_{\mathrm{max}})$
where $\lambda\coloneqq\\{\lambda_{k}\\}_{t=0}^{hN-1}$ is a perturbation to
$\mathscr{C}_{q}$ defined as follows,

$\displaystyle K_{k}(\mathscr{C}(\lambda))$
$\displaystyle=(1-\lambda_{k})K_{k}(\mathscr{C}_{q}),$ $\displaystyle
v_{k}(\mathscr{C}(\lambda))$ $\displaystyle=v_{k}(\mathscr{C}_{q}),$

$\forall\ k=0,1,\cdots hN-1.$ Under the perturbed control sequence
$\mathscr{C}(\lambda)$, the mean dynamics are unaffected since the feedforward
term $v_{k}(\mathscr{\lambda})=v_{k}(\mathscr{C}_{q})$ is unperturbed, i.e.
$\mu_{k}(\mathscr{C}(\lambda))=\mu_{k}(\mathscr{C}_{q})$. We now express the
dynamics of the state covariance under the perturbed initial covariance and
control sequence $\Sigma(\epsilon)$ and $\mathscr{C}(\lambda)$ as
$\Sigma_{k}(\Sigma(\epsilon),\mathscr{C}(\lambda))$. We have,

$\displaystyle\Sigma_{1}(\Sigma,\mathscr{C})$
$\displaystyle=(A+BK_{0}(\mathscr{C}))(\Sigma_{q}+\epsilon\mathrm{I})(A+BK_{0}(\mathscr{C}))^{\intercal}+DD^{\intercal}$
$\displaystyle=\Sigma_{1}(\Sigma_{q},\mathscr{C})+\epsilon\gamma_{1}(\mathscr{C})$

where
$\Sigma_{1}(\Sigma_{q},\mathscr{C})=(A+BK_{0}(\mathscr{C}))\Sigma_{q}(A+BK_{0}(\mathscr{C}))^{\intercal}$
and
$\gamma_{1}(\mathscr{C})=(A+BK_{0}(\mathscr{C}))(A+BK_{0}(\mathscr{C}))^{\intercal}$.
Through induction, we can express $\Sigma_{k}(\Sigma,\mathscr{C})$ as,

$\displaystyle\Sigma_{k}(\Sigma,\mathscr{C})=\Sigma_{k}(\Sigma_{q},\mathscr{C})+\epsilon\gamma_{k}(\mathscr{C}),$
(27)

where $\Sigma_{0}(\Sigma_{q},\mathscr{C})=\Sigma_{q}$,
$\gamma_{k}(\mathscr{C})=(A+BK_{k-1}(\mathscr{C}))\gamma_{k-1}(\mathscr{C})(A+BK_{k-1}(\mathscr{C}))^{\intercal}$,
and $\gamma_{0}(\mathscr{C})=\mathrm{I}$.

Writing the state chance constraints (20d) corresponding to $\Sigma(\epsilon)$
and $K(\mathscr{C}(\lambda))$,

$\displaystyle\mathscr{S}_{k}(\Sigma,\mathscr{C})$
$\displaystyle=\Phi^{-1}(1-\epsilon_{x})\sqrt{\alpha_{x}^{\intercal}\Sigma_{k}(\Sigma,\mathscr{C})\alpha_{x}}+\alpha_{x}^{\intercal}\mu_{k}(\mathscr{C})-\beta_{x}.$
(28)

From (27) and (29), and using $\sqrt{a+b}\leq\sqrt{a}+\sqrt{b}\ \forall\
a,b\geq 0$,

$\displaystyle\mathscr{S}_{k}(\Sigma,\mathscr{C})$
$\displaystyle\leq\Phi^{-1}(1-\epsilon_{x})\left[\sqrt{\alpha_{x}^{\intercal}\Sigma_{k}(\Sigma_{q},\mathscr{C})\alpha_{x}}+\sqrt{\epsilon}\sqrt{\alpha_{x}^{\intercal}\gamma_{k}(\mathscr{C})\alpha_{x}}\right]$
$\displaystyle+\alpha_{x}^{\intercal}\mu_{k}(\mathscr{C})-\beta_{x}.$ (29)

From the state chance constraint corresponding to $\Sigma_{q}$ and
$\mathscr{C}_{q}$,

$\displaystyle\mathscr{S}_{k}(\Sigma_{q},\mathscr{C}_{q})=\Phi^{-1}(1-\epsilon_{x})\sqrt{\alpha_{x}^{\intercal}\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})\alpha_{x}}+\alpha_{x}^{\intercal}\mu_{k}(\mathscr{C}_{q})-\beta_{x}<0.$

Therefore,

$\displaystyle\alpha_{x}^{\intercal}\mu_{k}(\mathscr{C}_{q})-\beta_{x}=-z_{s,k}-\left[\Phi^{-1}(1-\epsilon_{x})\sqrt{\alpha_{x}^{\intercal}\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})\alpha_{x}}\right],$
(30)

where $z_{s,k}>0$ is the slack variable associated with the state chance
constraint at the $k$-th time-step for the system initialized at $\Sigma_{q}$
and evolving under $\mathscr{C}_{q}$. Substituting (30) in (29), and since
$\mu_{k}(\mathscr{C})=\mu_{k}(\mathscr{C}_{q})$,

$\displaystyle\mathscr{S}_{k}(\Sigma,\mathscr{C})$
$\displaystyle\leq\Phi^{-1}(1-\epsilon_{x})\left[\sqrt{\alpha_{x}^{\intercal}\Sigma_{k}(\Sigma_{q},\mathscr{C})\alpha_{x}}-\sqrt{\alpha_{x}^{\intercal}\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})\alpha_{x}}\right]$
$\displaystyle+\sqrt{\epsilon}\sqrt{\alpha_{x}^{\intercal}\gamma_{k}(\mathscr{C})\alpha_{x}}-z_{s,k}.$
(31)

We construct perturbations $\epsilon$ and $\lambda$ s.t.
$\mathscr{S}_{k}(\Sigma,\mathscr{C})\leq 0\ \forall\ k=0,1,\cdots,hN-1$. The
following holds for the desired value of the perturbations,

$\displaystyle\sqrt{\epsilon}\sqrt{\alpha_{x}^{\intercal}\gamma_{k}(\mathscr{C})\alpha_{x}}\leq
z_{s,k}$
$\displaystyle-\Phi^{-1}(1-\epsilon_{x})\left[\sqrt{\alpha_{x}^{\intercal}\Sigma_{k}(\Sigma_{q},\mathscr{C})\alpha_{x}}\right.$
$\displaystyle-\left.\sqrt{\alpha_{x}^{\intercal}\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})\alpha_{x}}\right]$
(S)

Following a similar analysis as the state chance constraints above, we can
express the control chance constraints (20e) corresponding to
$\Sigma(\epsilon)$ and $\mathscr{C}(\lambda)$ as,

$\displaystyle\mathcal{U}_{k}(\Sigma,\mathscr{C})$
$\displaystyle\leq\Phi^{-1}(1-\epsilon_{u})\left[\sqrt{\alpha_{u}^{\intercal}K_{k}(\mathscr{C})\Sigma_{k}(\Sigma_{q},\mathscr{C})K_{k}^{\intercal}(\mathscr{C})\alpha_{u}}\right.$
$\displaystyle\left.-\sqrt{\alpha_{u}^{\intercal}K_{k}(\mathscr{C})\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})K_{k}^{\intercal}(\mathscr{C})\alpha_{u}}\right]$
$\displaystyle+\sqrt{\epsilon}\sqrt{\alpha_{u}^{\intercal}K_{k}(\mathscr{C})\gamma_{k}(\mathscr{C})K_{k}^{\intercal}(\mathscr{C})\alpha_{u}}-z_{u,k},$
(32)

where $z_{u,k}>0$ is the slack variable associated with the control chance
constraint at the $k$-th time-step. For the desired values of the
perturbations, $\mathcal{U}_{k}(\Sigma,\mathscr{C})\leq 0$, and we get the
following,

$\displaystyle\sqrt{\epsilon}$
$\displaystyle\sqrt{\alpha_{u}^{\intercal}K_{k}(\mathscr{C})\gamma_{k}(\mathscr{C})K_{k}^{\intercal}(\mathscr{C})\alpha_{u}}\leq$
$\displaystyle\Phi^{-1}(1-\epsilon_{u})\left[\sqrt{\alpha_{u}^{\intercal}K_{k}(\mathscr{C})\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})K_{k}^{\intercal}(\mathscr{C})\alpha_{u}}\right.$
$\displaystyle-\left.\sqrt{\alpha_{u}^{\intercal}K_{k}(\mathscr{C})\Sigma_{k}(\Sigma_{q},\mathscr{C})K_{k}^{\intercal}(\mathscr{C})\alpha_{u}}\right]+z_{u,k}.$
(U)

For there to always exist some $\epsilon>0$ s.t. (S) and (U) hold true, it is
a sufficient condition that
$\sqrt{\alpha_{x}^{\intercal}\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})\alpha_{x}}-\sqrt{\alpha_{x}^{\intercal}\Sigma_{k}(\Sigma_{q},\mathscr{C}(\lambda))\alpha_{x}}\geq
0$, and
$\sqrt{\alpha_{u}^{\intercal}K_{k}(\mathscr{C})\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})K^{\intercal}_{k}(\mathscr{C})\alpha_{u}}-\sqrt{\alpha_{u}^{\intercal}K_{k}\Sigma_{k}(\Sigma_{q},\mathscr{C}(\lambda))K^{\intercal}_{k}\alpha_{u}}\geq
0$, since $z_{s,k}$ and $z_{u,k}$ are strictly positive $\forall\
k=0,1,\cdots,hN-1$. Therefore, we construct $\lambda$ such that
$\Sigma_{k}(\Sigma_{q},\mathscr{C}(\lambda))\prec\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})\
\forall\ k=0,1,\cdots,hN-1$. Writing
$\Sigma_{1}(\Sigma_{q},\mathscr{C}(\lambda))$ in terms of
$\Sigma_{1}(\Sigma_{q},\mathscr{C}_{q})$,

$\displaystyle\Sigma_{1}(\Sigma_{q},\mathscr{C}(\lambda))$
$\displaystyle=(A+BK_{0}(\lambda))\Sigma_{q}(A+BK_{0}(\lambda))^{\intercal}+DD^{\intercal}$
$\displaystyle=\begin{aligned}
\Sigma_{1}(&\Sigma_{q},\mathscr{C}_{q})+\lambda^{2}_{0}BK_{0}\Sigma_{q}(BK_{0})^{\intercal}\\\
&\qquad-\lambda_{0}[BK_{0}\Sigma_{q}(A+BK_{0})^{\intercal}\\\
&\qquad\qquad+(A+BK_{0})\Sigma_{q}(BK_{0})^{\intercal}].\end{aligned}$ (33)

From (33),
$\lambda^{2}_{0}BK_{0}\Sigma_{q}(BK_{0})^{\intercal}\prec\lambda_{0}[BK_{0}\Sigma_{q}(A+BK_{0})^{\intercal}+(A+BK_{0})\Sigma_{q}(BK_{0})^{\intercal}]$
is a sufficient condition to ensure
$\Sigma_{1}(\Sigma_{q},\mathscr{C}(\lambda))\prec\Sigma_{1}(\Sigma_{q},\mathscr{C}_{q})$.
This is done by constructing $\lambda_{0}$ s.t.
$\lambda_{\mathrm{max}}(\lambda^{2}_{0}BK_{0}\Sigma_{q}(BK_{0})^{\intercal})<\lambda_{\mathrm{min}}(\lambda_{0}[BK_{0}\Sigma_{q}(A+BK_{0})^{\intercal}+(A+BK_{0})\Sigma_{q}(BK_{0})^{\intercal})]$.
Therefore,

$\displaystyle\lambda_{0}<\frac{\lambda_{\mathrm{min}}(BK_{0}\Sigma_{q}(A+BK_{0})^{\intercal}+(A+BK_{0})\Sigma_{q}(BK_{0})^{\intercal})}{\lambda_{\mathrm{max}}(BK_{0}\Sigma_{q}(BK_{0})^{\intercal})}.$
(34)

Repeating the above arguments recursively for
$\Sigma_{k}(\Sigma_{q},\mathscr{C}(\lambda))$ and
$\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})$,
$\Sigma_{k+1}(\Sigma_{q},\mathscr{C}(\lambda))=(A+BK_{k}(\lambda))\Sigma_{k}(\Sigma_{q},\mathscr{C}(\lambda))(A+BK_{k}(\lambda))^{\intercal}+DD^{\intercal}$.
Assuming $\lambda_{k-}$ is such that
$\Sigma_{k}(\Sigma_{q},\mathscr{C}(\lambda))\prec\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})$,
$\lambda^{2}_{k}BK_{k}\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})(BK_{k})^{\intercal}\preceq\lambda_{k}[BK_{k}\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})(A+BK_{k})^{\intercal}+(A+BK_{k})\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})(BK_{k})^{\intercal}]$
is a sufficient condition to ensure
$\Sigma_{k+1}(\Sigma_{q},\mathscr{C}(\lambda))\preceq\Sigma_{k+1}(\Sigma_{q},\mathscr{C}_{q})$.
Therefore,

$\displaystyle\lambda_{k}<=\frac{\mathscr{N}^{\Sigma}_{k}}{\lambda_{\mathrm{max}}(BK_{k}\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})(BK_{k})^{\intercal})}$
(35)

$\forall\ k=0,1,\cdots,hN-1$, where
$\mathscr{N}^{\Sigma}_{k}\coloneqq\lambda_{\mathrm{min}}(BK_{k}\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})(A+BK_{k})^{\intercal}+(A+BK_{k})\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q})(BK_{k})^{\intercal})$.

We define quantities $\epsilon_{s,k}(\alpha_{x},\epsilon_{x};\lambda)$, and
$\epsilon_{u,k}(\alpha_{u},\epsilon_{u};\lambda)$ as follows,

$\displaystyle\epsilon_{s,k}(\alpha_{x},\epsilon_{x};\lambda)\coloneqq\frac{z_{s,k}}{\lVert\alpha_{x}\rVert\sqrt{\lambda_{\mathrm{max}}(\gamma_{k}(\mathscr{C}(\lambda)))}}$
$\displaystyle+\Phi^{-1}(1-\epsilon_{x})\frac{\sqrt{\lambda_{\mathrm{min}}(\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q}))}-\sqrt{\lambda_{\mathrm{max}}(\Sigma_{k}(\Sigma_{q},\mathscr{C}(\lambda)))}}{\sqrt{\lambda_{\mathrm{max}}(\gamma_{k}(\mathscr{C}(\lambda)))}},$
(36)
$\displaystyle\epsilon_{u,k}(\alpha_{u},\epsilon_{u};\lambda)\coloneqq\frac{z_{u,k}}{\lvert(1-\lambda_{k})\rvert\lVert
K^{\intercal}_{k}(\mathscr{C}_{q})\alpha_{u}\rVert\sqrt{\lambda_{\mathrm{max}}(\gamma_{k}(\mathscr{C}(\lambda)))}}$
$\displaystyle+\Phi^{-1}(1-\epsilon_{u})\frac{\sqrt{\lambda_{\mathrm{min}}(\Sigma_{k}(\Sigma_{q},\mathscr{C}_{q}))}-\sqrt{\lambda_{\mathrm{max}}(\Sigma_{k}(\Sigma_{q},\mathscr{C}(\lambda)))}}{\sqrt{\lambda_{\mathrm{max}}(\gamma_{k}(\mathscr{C}(\lambda)))}}.$
(37)

$\epsilon_{s,k}(\alpha_{x},\epsilon_{x};\lambda)$, and
$\epsilon_{u,k}(\alpha_{u},\epsilon_{u};\lambda)$ are lower bounds on the RHS
of (S) and (U) respectively. Therefore, a sufficient condition for (S) and (U)
to hold is the following,

$\displaystyle\epsilon\leq\epsilon_{s}(\alpha_{x},\epsilon_{x};\lambda)\coloneqq\min_{k}\epsilon_{s,k}(\alpha_{x},\epsilon_{x};\lambda),$
(38)
$\displaystyle\epsilon\leq\epsilon_{u}(\alpha_{u},\epsilon_{u};\lambda)\coloneqq\min_{k}\epsilon_{u,k}(\alpha_{u},\epsilon_{u};\lambda).$
(39)

For specified $\lambda$, $\epsilon_{s,k}(\alpha_{x},\epsilon_{x};\lambda)$ and
$\epsilon_{u,k}(\alpha_{u},\epsilon_{u};\lambda)$ are functions of the
planning scene $\\{(\alpha_{x},\epsilon_{x}),(\alpha_{u},\epsilon_{u})\\}$
s.t. $\epsilon_{s,k}(\alpha_{x},\epsilon_{x};\lambda)$ and
$\epsilon_{u,k}(\alpha_{u},\epsilon_{u};\lambda)$ are monotonically decreasing
functions of $\lVert\alpha_{x}\rVert,\lVert\alpha_{u}\rVert\neq 0$
respectively.

From $\lambda$ specified earlier, the above inequalities are a function of
just the planning scene
$\\{(\alpha_{x},\epsilon_{x}),(\alpha_{u},\epsilon_{u})\\}$. We now derive a
condition on $\epsilon$ from the terminal goal reaching constraint, and show
that there always exists some $\epsilon>0$ such that all the conditions hold
by choosing the planning scene in a careful manner. Writing the desired
terminal covariance constraint under the perturbations,

$\displaystyle\lambda_{\mathrm{min}}(\Sigma^{-}_{\mathrm{max}})<\lambda_{\mathrm{max}}(\Sigma_{hN}(\Sigma,\mathscr{C}))\leq\lambda_{\mathrm{min}}(\Sigma_{\mathrm{max}}).$
(40)

We define the function $f:[0,\infty)\rightarrow\mathbb{R}^{+}$ as the
following,

$\displaystyle
f(\epsilon;\lambda)\coloneqq\lambda_{\mathrm{max}}(\Sigma_{hN}(\Sigma,\mathscr{C}))=\lambda_{\mathrm{max}}(\Sigma_{hN}(\Sigma_{q},\mathscr{C})+\epsilon\gamma_{k}(\mathscr{C})),$
(41)

where for a specified $\lambda$, $f(.;\lambda)$ is a function of $\epsilon$
alone. In particular, $f(.)$ is a continuous, strictly increasing (under mild
assumptions on $\mathscr{C}_{q}$ i.e. $(A+BK_{k})$ is full rank $\forall\ k$),
and a convex function of $\epsilon$.
$f(0;\lambda)=\lambda_{\mathrm{max}}(\Sigma_{hN}(\Sigma_{q},\mathscr{C}))\leq\lambda_{\mathrm{max}}(\Sigma_{hN}(\Sigma_{q},\mathscr{C}_{q}))$.
Also from (20a),
$\lambda_{\mathrm{max}}(\Sigma_{hN}(\Sigma_{q},\mathscr{C}_{q}))\leq\lambda_{\mathrm{min}}(\Sigma^{-}_{\mathrm{max}})$.
Therefore, $\exists\ \epsilon^{+}$ s.t.
$f(\epsilon^{+};\lambda)=\lambda_{\mathrm{min}}(\Sigma^{-}_{\mathrm{max}})$
s.t. $f(\epsilon;\lambda)>\lambda_{\mathrm{min}}(\Sigma^{-}_{\mathrm{max}})\
\forall\ \epsilon>\epsilon^{+}$.

From the specified $\lambda$ as before (34) (35), and the existence of
$\epsilon^{+}$ (note that the existence of such an $\epsilon^{+}$ and it’s
value is independent of the choice of the planning scene parameters), we
choose $\\{(\alpha_{x},\epsilon_{x}),(\alpha_{u},\epsilon_{u})\\}$ s.t.
$\epsilon_{s,k}(\alpha_{x},\epsilon_{x};\lambda),\epsilon_{u,k}(\alpha_{u},\epsilon_{u};\lambda)>\epsilon^{+}\
\forall\ k$.

For such a choice of $\lambda$,
$\\{(\alpha_{x},\epsilon_{x}),(\alpha_{u},\epsilon_{u})\\}$, and
$\epsilon\in(\epsilon^{+},\operatorname{min}(\epsilon_{s}(\alpha_{x},\epsilon_{x};\lambda),\epsilon_{u}(\alpha_{u},\epsilon_{u};\lambda))$,
$(\mu_{q},\Sigma(\epsilon))\xlongrightarrow[kN]{\mathscr{C}(\lambda)}(\mu_{\mathrm{cand}},\Sigma_{\mathrm{max}})$
and $\nexists\ \mathscr{C}_{q^{+}}$ s.t.
$(\mu_{q},\Sigma(\epsilon))\xlongrightarrow[hN]{\mathscr{C}_{q^{+}}}(\mu_{\mathrm{cand}},\Sigma^{-}_{\mathrm{max}})$.
Hence, proved.

∎

###### Proof of Theorem 1.

We define the h-$\operatorname{BRS}$ of a distribution $p$ as the following:
h-$\operatorname{BRS}(p)=\\{(\mu_{q},\Sigma_{q})|\operatorname{FEASIBLE}(q,p,hN)=\operatorname{TRUE}\\}$.
Using the above definition, the h-$\operatorname{BRS}(\mathcal{T})$ of a tree
of distributions $\mathcal{T}$ is defined as,

$\displaystyle\text{h-}\operatorname{BRS}=\bigcup_{i\in\nu(\mathcal{T})}\text{($h-d_{i}$)-}\operatorname{BRS}(\nu_{i})$
(42)

where $d_{i}$ is the distance of the $i$-th node from the goal node in terms
of the number of hops. Let $\mathcal{T}^{(n)}$ be the state of the tree at the
end of $n$ iterations of the $\operatorname{ADD}$ procedure. We first show
that $\exists\
\\{(\alpha_{x},\beta_{x},\epsilon_{x}),(\alpha_{u},\beta_{u},\epsilon_{u})\\}$
s.t.
h-$\operatorname{BRS}(\mathcal{T}^{(n+1)}_{\mathrm{MAXCOVAR}})\supset\text{h-}\operatorname{BRS}(\mathcal{T}^{(n+1)}_{\mathrm{ANY}})$
where $\mathcal{T}^{(n+1)}_{\mathrm{MAXCOVAR}}$,
$\mathcal{T}^{(n+1)}_{\mathrm{ANY}}$ are trees obtained from a common
$\mathcal{T}^{(n)}$ by following one more iteration of the
$\operatorname{ADD}$ procedure with the edge constructed according to the
$\operatorname{MAXCOVAR}$ algorithm that explicitly maximizes the minimum
eigenvalue of the initial covariance versus any other algorithm for edge
construction $\operatorname{ANY}$. The $\operatorname{ANY}$ procedure could
for instance sample belief nodes randomly, or add edges that optimize for a
performance regularized objective of the minimum eigenvalue of the initial
covariance matrix.

The $(n+1)$-th iteration proceeds by attempting to add a sampled mean
$\mu_{q}$ to an existing node $\nu^{(n)}$ on the tree, where $\nu^{(n)}$ is
the node sampled on the $(n+1)$-th iteration. We have,

$\displaystyle\Sigma_{\mathrm{max}},\mathcal{C}_{\mathrm{max}}\longleftarrow\operatorname{MAXCOVAR}(\mu_{q},(\mu^{(n)},\Sigma^{(n)}),N),$
(43)

and,

$\displaystyle\Sigma_{\mathrm{any}},\mathcal{C}_{\mathrm{any}}\longleftarrow\operatorname{ANY}(\mu_{q},(\mu^{(n)},\Sigma^{(n)}),N),$
(44)

s.t.
$\lambda_{\mathrm{min}}(\Sigma_{\mathrm{max}})>\lambda_{\mathrm{min}}(\Sigma_{\mathrm{any}})$.
Consider the two nodes added at the $(n+1)$-th iteration:
$\nu^{(n+1)}_{\mathrm{MAXCOVAR}}=(\mu_{q},\Sigma_{\mathrm{max}})$; and
$\nu^{(n+1)}_{\mathrm{ANY}}=(\mu_{q},\Sigma_{\mathrm{any}})$ with distance
from the goal node as $d^{(n+1)}$. From Lemma 2, $\exists\
\\{(\alpha_{x},\beta_{x},\epsilon_{x}),(\alpha_{u},\beta_{u},\epsilon_{u})\\}$
s.t.
$\text{t-}\operatorname{BRS}(\nu^{(n+1)}_{\mathrm{MAXCOVAR}})\supset\text{t-}\operatorname{BRS}(\nu^{(n+1)}_{\mathrm{ANY}})\
\forall\ t\geq 1$. Therefore,

$\displaystyle\text{$(h-d^{(n+1)})$-}\operatorname{BRS}(\nu^{(n+1)}_{\mathrm{MAXCOVAR}})\supset\text{$(h-d^{(n+1)})$-}\operatorname{BRS}(\nu^{(n+1)}_{\mathrm{ANY}}),$
(45)

$\forall\ h>d^{(n+1)}$. From (42),

$\displaystyle\text{h-}\operatorname{BRS}(\mathcal{T}^{(n+1)}_{\mathrm{MAXCOVAR}})$
$\displaystyle=\text{h-}\operatorname{BRS}(\mathcal{T}^{(n)})\bigcup$
$\displaystyle(h-d^{(n+1)})-\operatorname{BRS}(\nu^{(n+1)}_{\mathrm{MAXCOVAR}}),$
(46)

and,

$\displaystyle\text{h-}\operatorname{BRS}(\mathcal{T}^{(n+1)}_{\mathrm{ANY}})$
$\displaystyle=\text{h-}\operatorname{BRS}(\mathcal{T}^{(n)})\bigcup$
$\displaystyle(h-d^{(n+1)})-\operatorname{BRS}(\nu^{(n+1)}_{\mathrm{ANY}}).$
(47)

From (45), it follows that there exist planning scenes such that
$\text{h-}\operatorname{BRS}(\mathcal{T}^{(n+1)}_{\mathrm{MAXCOVAR}})\supset\text{h-}\operatorname{BRS}(\mathcal{T}^{(n+1)}_{\mathrm{ANY}})$
when starting from a common tree state $\mathcal{T}^{(n)}$. Considering the
common tree state as just a singleton set of the goal distribution,
$\mathcal{T}^{(0)}=\\{\mathcal{G}\\}$. Therefore, there exist planning scenes
such that
$\text{h-}\operatorname{BRS}(\mathcal{T}^{(1)}_{\mathrm{MAXCOVAR}})\supset\text{h-}\operatorname{BRS}(\mathcal{T}^{(1)}_{\mathrm{ANY}})$.
The result for a general $r$ follows from the above. Hence, proved.

∎