Belief Space Planning: A Covariance
Steering Approach

Motion uncertainty and measurement noise arise in all real-world robotic applications. When evaluating the safety of a robot under motion and estimation uncertainties, it is no longer sufficient to rely only on deterministic indicators of performance, such as whether the robot is in collision-free or in-collision status. Instead, the state of the robot is best characterized by a probability distribution function (pdf) over all possible states, which is commonly referred to as the belief or information state [1, 2, 3]. Explicitly taking into account the motion and observation uncertainties thus requires planning in the belief space, which allows one to compute the collision probability and thus make more informed decisions. Planning under motion and observation uncertainties is referred to as belief space planning, which can be formulated as a partially observable Markov decision process (POMDP) problem [4]. Solving POMDP in general domains, however, is very challenging, despite some recent progress in terms of more efficient POMDP solvers [5, 6, 7, 8, 9]. Planning in infinite-dimensional distributional (e.g., belief) spaces can become more tractable by the use of roadmaps, that is, graphs constructed by sampling. Since their introduction [10, 11], such belief roadmaps (BRMs) have increased in popularity owing to their simplicity and their ability to avoid local minima.

Sampling-based motion planning algorithms such as probabilistic roadmaps (PRM) [12] and rapidly exploring random trees (RRTs) [13] can be used to solve planning problems in high-dimensional continuous state spaces, by building a roadmap or a tree incrementally through sampling the search space. However, traditional PRM-based methods only address deterministic systems. PRM methods have been extended to belief space planning using belief space roadmaps (BRMs) [11, 14, 15]. One of the main challenges of belief space roadmap (BRM) methods is that the costs of different edges depend on each other, resulting in the “curse of history” problem for POMDPs [16, 14]. This dependence between edges breaks the optimal substructure property, which is required for search algorithms such as Dijkstra’s algorithm or A*. This problem arises from the unreachability of the belief nodes; even if the robot has full control of its mean, it is difficult to reach higher order moments (e.g., a specified covariance). Since the nodes in BRM are sampled in the belief space, the edges in BRM should ideally steer the robot from one distribution to another. If reachability of BRM nodes is not achieved, an edge in the BRM depends on all preceding edges along the path.

In general, in the literature, the belief may be classified into (i) the estimation belief (e-belief), describing the output of the estimator, i.e., the pdf over the error between the estimated value and true value of the state; (ii) control belief (c-belief), which refers to the pdf over “separated control” error, i.e., the error between the estimated state and the desired state; and (iii) the full belief (f-belief), that is, the belief over the true state. We discuss these different beliefs in greater detail in Section III. We just note here that e-belief planning tries to obtain better state estimates and finds a path with minimum estimation uncertainty. The f-belief planning aims at minimizing the full uncertainty that takes into account the impact of the controller as well. In this paper, we consider the f-belief planning problem and will call it belief planning for simplicity.

Planning in e-belief space was studied in [11, 17], where the goal was to find the minimum estimation uncertainty path for a robot from a starting position to a goal position. In [15], a linear-quadratic Gaussian (LQG) controller was used for motion planning within the BRM context. By taking into account the controller and the sensors used, [15] computes the true a priori probability distribution of the state of the robot. Thus, [15] studies the f-belief planning problem. However, the independence between edges is not satisfied. Similarly, [18] studies the f-belief planning problem using a tree.

The state-of-the-art in terms of BRM methods is probably [14], which tackles the “curse of history” problem. The proposed SLQG-FIRM method achieves node reachability using a stationary LQG controller. One limitation of this method is that the nodes have to be stationary. That is, the nodes in the BRM graph need to be sampled in the equilibrium space of the robot, which usually means zero velocity. Thus, this method cannot explore the velocity space and the resulting paths are suboptimal. Secondly, a converging process is required at every node. That is, the robot will have to “wait” at each node, which will increase the time required for the robot to reach the goal. Some remedies are introduced in [19] for systems (e.g., a fixed-wing aircraft) that cannot reach zero velocity by using periodic trajectories and periodic controllers. The periodic controller is applied repeatedly until the trajectory of the vehicle converges to the periodic trajectory. Thus, a “waiting” procedure is still required in these approaches. Online replanning in belief space is studied in [20]. The method in [20] improves the online phase by recomputing the local plans, which includes adding a virtual belief node and local belief edges to the FIRM graph, and solving a dynamic program at every time-step. The offline roadmap construction phase is the same as FIRM [14].

Recent developments in explicitly controlling the covariance of a linear system [21, 22] provide an appealing approach to construct the BRM with guarantees of node reachability. In particular, for a discrete-time linear stochastic system, covariance steering theory designs a controller that steers the system from an initial Gaussian distribution to a terminal Gaussian distribution in finite-time [23, 24, 25]. Reference [21] formulated the finite-horizon covariance control problem as a stochastic optimal control problem. In [23], the covariance steering problem is formulated as a convex program. Additional state chance constraints are considered in [25] and nonlinear systems are considered in [26] using iterative linearization. The covariance steering problem with output feedback has also been studied in [27, 28, 29].

In this paper, we propose the CS-BRM algorithm, which uses covariance steering as the edge controller of a BRM to ensure a priori node reachability. Since the goal of covariance steering is to reach a given distribution of the state, it is well-suited for reaching a belief node. In addition, covariance steering avoids the limitation of sampling in the equilibrium space, and thus the proposed CS-BRM method allows sampling of non-stationary belief nodes. Our method allows searching in the velocity space and thus finds paths with lower cost, as demonstrated in the numerical examples in Section VI.

Extending belief planning to nonlinear systems requires a nominal trajectory for each edge [14]. In the SLQG-FIRM framework, these nominal trajectories are either assumed to be given or being approximated by simple straight lines [15, 14]. However, the nominal trajectory has to be dynamically feasible in order to apply a time-varying LQG controller. Also, the nominal trajectory must be optimal if one wishes to generate optimal motion plans. Finding an optimal nominal trajectory requires solving a two-point boundary value problem (TPBVP), which for general nonlinear systems can be computationally expensive [30]. In this paper, we develop a simple, yet efficient, algorithm to find suitable nominal trajectories between the nodes of the BRM graph to steer the mean states in an optimal fashion.

The contributions of the paper are summarized as follows.

• $\bullet$

  A new belief space planning method, called CS-BRM, is developed, to construct a roadmap in belief space by using the recently developed theory of finite-time covariance control. CS-BRM achieves finite-time belief node reachability using covariance steering and overcomes the limitation of sampling stationary nodes.

• $\bullet$

  The concept of compatible nominal trajectory is introduced, which aims to improve the performance of linearization-based control methods to control nonlinear systems. An efficient algorithm, called the CNT algorithm, is proposed to compute nominal feasible trajectories for nonlinear systems.

The paper is organized as follows. The statement of the problem is given in Section II. In Section III, the output-feedback covariance steering theory for linear systems is outlined. In Section IV, the method of computing nominal trajectory for nonlinear systems is introduced. The main algorithm, CS-BRM, is given in Section V. The numerical implementation of the proposed algorithm is presented in Section VI. Finally, Section VII concludes the paper.

We consider the problem of planning for a nonholonomic robot in an uncertain environment which contains obstacles. The uncertainty in the problem stems from model uncertainty, as well from sensor noise that corrupts the measurements. We model such a system by a stochastic difference equation of the form

where $k=0,1,\ldots,N-1$ are the discrete time-steps, $x_{k}\in\mathbb{R}^{n_{x}}$ is the state, and $u_{k}\in\mathbb{R}^{n_{u}}$ is the control input. The steps of the noise process $w_{k}\in\mathbb{R}^{n_{w}}$ are i.i.d standard Gaussian random vectors. The measurements are given by the noisy and partial sensing model

where $y_{k}\in\mathbb{R}^{n_{y}}$ is the measurement at time step $k$, and the steps of the process $v_{k}\in\mathbb{R}^{n_{y}}$ are i.i.d standard Gaussian random vectors. We assume that $(w_{k})_{k=0}^{N-1}$ and $(v_{k})_{k=0}^{N-1}$ are independent.

The objective is to steer the system (1) from some initial state $x_{0}$ to some final state $x_{N}$ within $N$ time steps while avoiding obstacles and, at the same time, minimize a given performance index. The controller $u_{k}$ at time step $k$ is allowed to depend on the whole history of measurements $y_{\ell},~{}\ell=0,\ldots,k$ up to time $k$ but not on any future measurements.

This is a difficult problem to solve in its full generality. Here we use graph-based methods to build a roadmap in the space of distributions of the states (e.g., the belief space) for multi-query motion planning. Planning in the belief space accounts for the uncertainty inherent in system (1) and (2) owing to the noises $w_{k}$ and $v_{k}$, as well as uncertainty owing to the distribution of the initial states. Each node sampled in the belief space is a distribution over the state. The edges in a BRM steer the state from one distribution to another. In the next section, we describe a methodology to design BRM edge controllers that allow to steer from one node (i.e., distribution) to another. Similar to previous works [11, 14, 15], we consider Gaussian distributions where the belief is given by the state mean and the state covariance.

In this section, we give a introduction of covariance steering and outline some key results. Some of these results are explicitly used in subsequent sections. The derivation in this section follows closely [27, 25].

Given a nominal trajectory $(n_{k})_{k=0}^{N-1}$, where $n_{k}=(x_{k}^{r},u_{k}^{r})$, we can construct a linear approximation of (1)-(2) around $(n_{k})_{k=0}^{N-1}$ via linearization as follows

where $h_{k}\in\mathbb{R}^{n_{x}}$ is the drift term, $A_{k}\in\mathbb{R}^{n_{x}\times n_{x}}$, $B_{k}\in\mathbb{R}^{n_{x}\times n_{u}}$, and $G_{k}\in\mathbb{R}^{n_{x}\times n_{x}}$ are system matrices, and $C_{k}\in\mathbb{R}^{n_{y}\times n_{x}}$ and $D_{k}\in\mathbb{R}^{n_{y}\times n_{y}}$ are observation model matrices.

We define the covariance steering problem as follows.

Problem 1: Find the control sequence $u=(u_{k})_{k=0}^{N-1}$ such that the system given by (3) and (4), starting from the initial state distribution $x_{0}\sim\mathcal{N}(\bar{x}_{0},P_{0})$, reaches the final distribution $x_{N}\sim\mathcal{N}(\bar{x}_{N},P_{N})$ where $P_{N}\preceq P_{f}$, while minimizing the cost functional

where $(m_{k})_{k=0}^{N-1}$ is a given reference trajectory of the states, $\bar{x}_{k}=\mathbb{E}(x_{k})$ is the mean of the state $x_{k}$, $P_{0}$ and $P_{N}$ are the covariance matrices of $x_{0}$ and $x_{N}$, respectively, the terminal covariance $P_{N}$ is upper bounded by a given covariance matrix $P_{f}$, and the matrices $(Q_{k}\succeq 0)$ and $(R_{k}\succ 0)$ are given.

In this section, we develop an efficient algorithm to find nominal trajectories for nonlinear systems using the mean controller (18). A deterministic nonlinear dynamic model, i.e., with the noise $w_{k}$ set to zero, is considered when designing the nominal trajectory. To this end, consider the deterministic nonlinear system

Let $(x_{k}^{r})_{k=0}^{N-1}$ and $(u_{k}^{r})_{k=0}^{N-1}$ be a nominal trajectory for this system. The linearized model (3) along this nominal trajectory will be used by the mean controller (18) to compute a control sequence $(u_{k}^{c})_{k=0}^{N-1}$ and the corresponding state sequence $(x_{k}^{c})_{k=0}^{N-1}$.

Definition 1: A nominal trajectory is called a compatible nominal trajectory for system (23) if $x_{k}^{r}=x_{k}^{c}$ and $u_{k}^{r}=u_{k}^{c}$ for $k=0,\dots,N-1$.

Linearizing along a compatible nominal trajectory ensures that the nonlinear system is linearized at the “correct” points. When applying the designed control to the linearized system, the resulting trajectory is the same as the nominal trajectory, which means that the system reaches the exact points where the linearization is performed. On the other hand, there will be extra errors caused by the linearization if the system is linearized along a non-compatible nominal trajectory.

The iterative algorithm to find a compatible nominal trajectory is given in Algorithm 1.

When the algorithm converges, the nominal trajectory is the same as the mean optimal trajectory, which, by definition, is a compatible nominal trajectory. Note that $(\ref{S3Eq29})$ gives the analytical solution of the mean control, which can be quickly computed. Each iteration of the algorithm has a low computational load and the overall algorithm may be solved efficiently. As seen later in Section VI, the algorithm typically convergences within a few iterations.

The nodes in the CS-BRM are sampled in the belief space. In a partially observable environment, the belief $b_{k}$ at time-step $k$ is given by the conditional probability distribution of the state $x_{k}$, conditioned on the history of observations $(y_{i})_{i=0}^{k}$ and the history of control inputs $(u_{i})_{i=0}^{k-1}$, that is, $b_{k}=\mathbb{P}(x_{k}|(y_{i})_{i=0}^{k},(u_{i})_{i=0}^{k-1})$. In a Gaussian belief space, $b_{k}$ can be equivalently represented by the estimated state, $\hat{x}_{k}$, and the estimation error covariance, $\tilde{P}_{k}$, that is, $b_{k}=(\hat{x}_{k},\tilde{P}_{k})$ [18, 14]. The state estimate $\hat{x}_{k}$ is Guassian, and is given by $\hat{x}_{k}\sim\mathcal{N}(\bar{x}_{k},\hat{P}_{k})$. Hence, the Gaussian belief can be also written as $b_{k}=(\bar{x}_{k},\hat{P}_{k},\tilde{P}_{k})$. The main idea of CS-BRM is to use covariance steering theory to design the edge controller to achieve node reachability.

An illustration of the steps for building the CS-BRM is shown in Fig. 1. The algorithm for constructing the CS-BRM is given in Algorithm 2. The following procedures are used in the algorithm.
Sample Nodes: The function SampleNodes$(n)$ samples $n$ CS-BRM nodes. A node in the CS-BRM is represented by the tuple $(\bar{x},\hat{P}_{{}^{\mbox{\scriptsize{-}}}},\tilde{P}_{{}^{\mbox{\scriptsize{-}}}})$, where $\bar{x}$ is the state mean, $\hat{P}_{{}^{\mbox{\scriptsize{-}}}}$ is the prior estimated state covariance, and $\tilde{P}_{{}^{\mbox{\scriptsize{-}}}}$ is the prior state estimation error covariance. Since $P=\hat{P}_{{}^{\mbox{\scriptsize{-}}}}+\tilde{P}_{{}^{\mbox{\scriptsize{-}}}}$, the node can also be equivalently represented by $(\bar{x},P,\tilde{P}_{{}^{\mbox{\scriptsize{-}}}})$. Note that we can compute the a posteriori convariances $\hat{P}$ and $\tilde{P}$ using $\hat{P}_{{}^{\mbox{\scriptsize{-}}}}$ and $\tilde{P}_{{}^{\mbox{\scriptsize{-}}}}$. For constructing node $j$, the algorithm starts by sampling the free state space, which provides the mean $\bar{x}_{j}$ of the distribution. Then, an estimated state covariance $\hat{P}_{j^{\mbox{\scriptsize{-}}}}$ is sampled from the space of symmetric and positive definite matrix space. Similarly, a state estimation error covariance $\tilde{P}_{j^{\mbox{\scriptsize{-}}}}$ is also sampled at node $j$. $\tilde{P}_{j^{\mbox{\scriptsize{-}}}}$ is the initial condition of the Kalman filter update for the edges that are coming out of node $j$. In Fig. 1(a), for each node, $\bar{x}$ is shown as a black dot, $P$ is shown as a solid ellipse, and $\tilde{P}_{{}^{\mbox{\scriptsize{-}}}}$ is shown as a dashed ellipse.
Neighbor: The function Neighbor$(V_{r},nod_{i})$ finds all the nodes in $V_{r}$ that are within a given distance $d_{1}$ to node $nod_{i}$, where $V_{r}$ is a node set containing all nodes in the CS-BRM.
Mean Trajectory: Given two nodes $nod_{i}$ and $nod_{j}$, MTraj$(nod_{i},nod_{j})$ uses the mean controller (18) and Algorithm 1 to find the compatible nominal trajectory, which is also the mean trajectory from $nod_{i}$ to $nod_{j}$. The function returns the mean control $\bar{U}_{ij}$, mean trajectory $\tau_{ij}$, and the cost of the mean control $MCost_{ij}$.
Obstacle Checking: The function ObstacleFree$(\tau_{ij})$ returns true if the mean trajectory $\tau_{ij}$ is collision free.
Kalman Filter: Given two nodes $nod_{i}$ and $nod_{j}$, KF$(nod_{i},\ nod_{j})$ returns the prior estimation error covariance at the last time step of the trajectory from $nod_{i}$ to $nod_{j}$.
Covariance Control: CovControl$(nod_{i},nod_{j})$ solves the covariance control problem from $nod_{i}$ to $nod_{j}$. It returns the control $\tilde{U}_{ij}$ and the cost $CovCost_{ij}$.
Monte Carlo: We use Monte Carlo simulations to calculate the probability of collision of the edges. For edge $E_{ij}$, the initial state $x_{0}$ and initial state estimate $\hat{x}_{0^{\mbox{\scriptsize{-}}}}$ are sampled from their corresponding distributions. The state trajectory is simulated using the mean control $\bar{U}_{ij}$ and covariance control $\tilde{U}_{ij}$. Then, collision checking is performed on the simulated state trajectory. By repeating this process, we approximate the probability of collision of this edge. In our implementation the collision cost $CollisionCost_{ij}$ is taken to be proportional to the probability of collision along that edge.

Algorithm 2 starts by sampling $n$ nodes in the belief space using SampleNodes (Line 1). Lines 3-17 are the steps to add CS-BRM edges. Given two neighboring nodes $nod_{i}$ and $nod_{j}$, Lines 6-13 try to construct the edge $E_{ij}$ and Lines 14-16 try to construct the edge $E_{ji}$.

Every edge in the CS-BRM is constructed by solving a covariance steering problem. Each node tries to connect to its neighboring nodes if possible. First, for each edge, we use Algorithm 1 to find the mean trajectory of that edge (Line 6). Next, we check if the mean trajectory is collision-free. Only if the mean trajectory is collision-free do we proceed to the next step of edge construction. Then, the Kalman filter updates are simulated, which give the state estimation error covariance at every time step along that edge. The state estimation error covariances of the edges $\overline{ab}$ and $\overline{ac}$ are shown as red dash ellipses in Figure 1(c). Take the edge $\overline{ab}$ as an example. The initial condition of the Kalman filter is $\tilde{P}_{0^{\mbox{\scriptsize{-}}}}=\tilde{P}_{a^{\mbox{\scriptsize{-}}}}$. The prior estimation error covariance at the final time-step, $\tilde{P}_{N^{\mbox{\scriptsize{-}}}}$, is compared with the state estimation error covariance of node $b$, $\tilde{P}_{b^{\mbox{\scriptsize{-}}}}$. If $\tilde{P}_{N^{\mbox{\scriptsize{-}}}}\preceq\tilde{P}_{b^{\mbox{\scriptsize{-}}}}$ (Line 9), the algorithm proceeds to solve the covariance control problem and this edge is added to the graph. For the covariance control problem of edge $\overline{ab}$, the initial constraint and the terminal constraint are given by $\hat{P}_{0^{\mbox{\scriptsize{-}}}}=\hat{P}_{a^{\mbox{\scriptsize{-}}}}$ and $\hat{P}_{N}\preceq\hat{P}_{b}$. The covariance $\hat{P}_{b}$ is computed using $\hat{P}_{b^{\mbox{\scriptsize{-}}}}$ and $\tilde{P}_{b^{\mbox{\scriptsize{-}}}}$. The final edge controller is the combination of the mean controller, covariance controller, and the Kalman filter.

In addition to the edge controller, the edge cost is computed for each edge. The edge cost $EdgeCost_{ij}$ is a weighted sum of $MCost_{ij}$, $CovCost_{ij}$, and $CollisionCost_{ij}$. With covariance steering serving as the lower-level controller, the higher-level motion planning problem using the roadmap is a graph search problem similarly to a PRM. Thus, the covariance steering approach transforms the belief space roadmap into traditional PRM with specific edge costs.

In CS-BRM, each edge is an independent covariance steering problem and the planned path using CS-BRM consists of a concatenation of edges. Since the terminal estimated state covariance satisfies an inequality constraint (17) and the terminal state estimation error covariance satisfies an inequality relation by construction (Line 9), it is important to verify that the covariance constraints at all nodes are still satisfied by concatenating the edges.

To this end, consider the covariance steering problem from node $a$ to node $b$. For the edge $\overline{ab}$, we have the initial constraints $\hat{P}_{0^{\mbox{\scriptsize{-}}}}=\hat{P}_{a^{\mbox{\scriptsize{-}}}}$, $\tilde{P}_{0^{\mbox{\scriptsize{-}}}}=\tilde{P}_{a^{\mbox{\scriptsize{-}}}}$, and the terminal covariance constraint $\hat{P}_{N}\preceq\hat{P}_{b}$. Note that the constraint $\tilde{P}_{N^{\mbox{\scriptsize{-}}}}\preceq\tilde{P}_{b^{\mbox{\scriptsize{-}}}}$ is satisfied for $\overline{ab}$ (Line 9, Algorithm 2). Suppose that the solution of the covariance control problem of edge $\overline{ab}$ is the feedback gain $K$. By concatenating the edges, the system may not start exactly at node $a$. Instead, it will start at some node $a^{{}^{\prime}}=(\bar{x}_{a},\hat{P}_{a^{\mbox{\scriptsize{-}}}}^{{}^{\prime}},\tilde{P}_{a^{\mbox{\scriptsize{-}}}}^{{}^{\prime}})$ that satisfies $\hat{P}_{a^{\mbox{\scriptsize{-}}}}^{{}^{\prime}}\preceq\hat{P}_{a^{\mbox{\scriptsize{-}}}}$ and $\tilde{P}_{a^{\mbox{\scriptsize{-}}}}^{{}^{\prime}}\preceq\tilde{P}_{a^{\mbox{\scriptsize{-}}}}$. Next, we show that by applying the pre-computed feedback gain $K$, the terminal covariance still satisfies $\hat{P}_{N}^{{}^{\prime}}\preceq\hat{P}_{b}$ for the new covariances $\hat{P}_{a^{\mbox{\scriptsize{-}}}}^{{}^{\prime}}$ and $\tilde{P}_{a^{\mbox{\scriptsize{-}}}}^{{}^{\prime}}$. Before verifying this result, we provide a property of the covariance of the Kalman filter. Given an initial estimation error covariance $\tilde{P}_{0^{\mbox{\scriptsize{-}}}}$, we obtain a sequence of error covariances $(\tilde{P}_{k^{\mbox{\scriptsize{-}}}})_{k=1}^{N}$ using the Kalman filter updates (7). Similarly, given the new initial error covariance $\tilde{P}_{0^{\mbox{\scriptsize{-}}}}^{{}^{\prime}}$ which satisfies $\tilde{P}_{0^{\mbox{\scriptsize{-}}}}^{{}^{\prime}}\preceq\tilde{P}_{0^{\mbox{\scriptsize{-}}}}$, we obtain a new sequence of error covariances $(\tilde{P}_{k^{\mbox{\scriptsize{-}}}}^{{}^{\prime}})_{k=0}^{N}$. We have the following lemma.

The proof of this lemma is omitted. A proof of a similar result can be found in [18]. From Lemma 1, it is straightforward to show that, if $\tilde{P}_{0^{\mbox{\scriptsize{-}}}}^{{}^{\prime}}\preceq\tilde{P}_{0^{\mbox{\scriptsize{-}}}}$, we also have $\tilde{P}_{k}^{{}^{\prime}}\preceq\tilde{P}_{k}$ for all $k=0,\ldots,N$.

The proof of this proposition is straightforward and is omitted. Proposition 2 guarantees that, when planning on the CS-BRM roadmap, the covariance at each arrived node is always smaller than the assigned fixed covariance at the corresponding node of the CS-BRM roadmap.

In this section, we first illustrate our theoretical results for the motion planning problem of a 2-D double integrator. We compare our method with the SLQG-FIRM algorithm [14], and we show that our method overcomes some of the limitations of SLQG-FIRM. Subsequently, the problem of a fixed-wing aerial vehicle in a 3-D environment is studied.

A belief space roadmap (BRM) algorithm is developed in this paper. The nodes in BRM represent distributions of the state of the system and are sampled in the belief space. The main idea is to use covariance steering to design the edge controllers of the BRM graph to steer the system from one distribution to another. Compared to [14], the proposed method allows sampling nonstationary belief nodes, which has the advantage of more complete exploration of the belief space. For covariance steering of nonlinear systems, we introduce the concept of compatible nominal trajectories, which aim to better approximate the nonlinear dynamics through successive linearization. We also propose an efficient algorithm to compute compatible nominal trajectories. Compared to the standard PRM, the additional computation load comes from the computation of the edge controllers and edge cost evaluations, which, however, are done offline. By explicitly incorporating motion and observation uncertainties, we show that the proposed CS-BRM algorithm generates efficient motion plans that take into account both the control effort and collision probability.

This work has been supported by NSF awards IIS-1617630 and IIS-2008695, NASA NSTRF Fellowship 80NSSC17K0093, and ARL under CRA DCIST W911NF-17-2-0181. The authors would also like to thank Dipankar Maity for many insightful discussions regarding the output-feedback covariance steering problem.