This article develops a model-based reinforcement learning (RL) approach to the closed-loop control of nonlinear dynamical systems with a partial nonlinear observation model. We propose an "information-state"-based approach to rigorously transform the partially observed problem into a fully observed problem where the information state consists of the past several observations and control inputs. We further show the equivalence of the transformed and the initial partially observed optimal control problems and provide the conditions to solve for the deterministic optimal solution. We develop a data-based generalization of the iterative linear quadratic regulator (ILQR) for the RL of partially observed systems using a local linear time-varying model of the information-state dynamics approximated by an autoregressive-moving-average (ARMA) model that is generated using only the input-output data. This approach allows us to design a local perturbation feedback control law that provides an optimum solution to the partially observed feedback design problem locally. The efficacy of the developed method is shown by controlling complex high-dimensional nonlinear dynamical systems in the presence of model and sensing uncertainty.

This paper presents a data-based approach to control robotic systems with partially-observed feedback. First, an open-loop optimization problem is solved to generate the nominal trajectory and then a linear time-varying Autoregressive–Moving-Average (ARMA) model of the system is calculated along the trajectory from the output measurement data. The system is then described in information state, which contains input-output information of the past few steps. Finally, a feedback gain which is calculated by solving a specific LQG problem along the nominal trajectory. The separate design of the open-loop and the closed-loop problem is used following the Decoupled Data-based Control (D2C) approach. Simulation results are also shown for complex models with fluid-structure interaction in the presence of both process and measurement noise.

This article presents and studies a recursive information consensus filter for decentralized dynamic state estimation under circumstances in which the communication network is unreliable. Local estimators are assumed to have access only to local information, and no structure is assumed about the topology of the communication network, which need not be connected at all times. The filter is a hybrid approach: it uses iterative covariance intersection to reach consensus over priors, which might become correlated, while consensus over new information is handled using weights based on a Metropolis–Hastings Markov chain. We establish bounds for estimation performance and show that this hybrid method produces unbiased conservative estimates that are better than covariance intersection. The performance of the hybrid method is evaluated extensively, including comparisons with competing algorithms, with a hypothetical “full history” yardstick, and centralized performance. We conduct an assessment on a realistic atmospheric dispersion problem and also on more carefully crafted settings to help characterize particular aspects of the performance.

In our previous work, we proposed a particle Gaussian mixture (PGM-I) filter for nonlinear estimation. The PGM-I filter uses the transition kernel of the state Markov chain to sample from the propagated prior. It constructs a Gaussian mixture representation of the propagated prior density by clustering the samples. The measurement data is incorporated by updating individual mixture modes using the Kalman measurement update. However, the Kalman measurement update is inexact when the measurement function is nonlinear and leads to the restrictive assumption that the number of modes remain fixed during the measurement update. In this paper, we introduce an alternate PGM-II filter that employs parallelized Markov Chain Monte Carlo sampling to perform the measurement update. The PGM-II filter update is asymptotically exact and does not enforce any assumptions on the number of Gaussian modes. The PGM-II filter is employed in the estimation of two test case systems. The results indicate that the PGM-II filter is suitable for handling nonlinear/non-Gaussian measurement update.

Simultaneous localization and planning (SLAP) is a crucial ability for an autonomous robot operating under uncertainty. In its most general form, SLAP induces a continuous partially observable Markov decision process (POMDP), which needs to be repeatedly solved online. This paper addresses this problem and proposes a dynamic replanning scheme in belief space. The underlying POMDP, which is continuous in state, action, and observation space, is approximated offline via sampling-based methods, but operates in a replanning loop online to admit local improvements to the coarse offline policy. This construct enables the proposed method to combat changing environments and large localization errors, even when the change alters the homotopy class of the optimal trajectory. It further outperforms the state-of-the-art Feedback-based Information RoadMap (FIRM) method by eliminating unnecessary stabilization steps. Applying belief space planning to physical systems brings with it a plethora of challenges. A key focus of this paper is to implement the proposed planner on a physical robot and show the SLAP solution performance under uncertainty, in changing environments and in the presence of large disturbances, such as a kidnapped robot situation.

The SLAM problem is known to have a special property that when robot orientation is known, estimating the history of robot poses and feature locations can be posed as a standard linear least squares problem. In this work, we develop a SLAM framework that uses relative feature-to-feature measurements to exploit this structural property of SLAM. Relative feature measurements are used to pose a linear estimation problem for pose-to-pose orientation constraints. This is followed by solving an iterative non-linear on-manifold optimization problem to compute the maximum likelihood estimate for robot orientation given relative rotation constraints. Once the robot orientation is computed, we solve a linear problem for robot position and map estimation. Our approach reduces the computational complexity of non-linear optimization by posing a smaller optimization problem as compared to standard graph-based methods for feature-based SLAM. Further, empirical results show our method avoids catastrophic failures that arise in existing methods due to using odometery as an initial guess for non-linear optimization, while its accuracy degrades gracefully as sensor noise is increased. We demonstrate our method through extensive simulations and comparisons with an existing state-of-the-art solver.

Planning under motion and observation uncertainties requires the solution of a stochastic control problem in the space of feedback policies. In this paper, by restricting the policy class to the linear feedback polices, we reduce the general (n2+ n)-dimensional belief space planning problem to an (n)-dimensional problem. As opposed to the previous literature that search in the space of open-loop optimal control policies, we obtain this reduction in the space of closed-loop policies by obtaining a Linear Quadratic Gaussian (LQG) design with the best nominal performance. Then, by taking the entire underlying trajectory of the LQG controller as the decision variable, we pose a coupled design of the trajectory and estimator (while keeping the design of the controller separate) as a NonLinear Program (NLP) that can be solved by a general NLP solver. We prove that under a first-order approximation and a careful usage of the separation principle, our approximations are valid. We provide an analysis on the existing major belief space planning methods and show that our algorithm keeps the lowest computational burden while searching in the policy space. Finally, we extend our solution to contain general state and control constraints. Our simulation results support our design.

Belief space planning is concerned with the problem of finding the control policy under process and measurement uncertainties. Formulated as a stochastic control problem, the solution of a general Decentralized Partially Observed Markov Decision Process (Dec-POMDP) is a collection of feedback policies for individual agents, maximizing a joint value function. In this paper, we design (m) number of Linear Quadratic Gaussian (LQG) policies for (m) number of agents maximizing the joint performance of the team. Casting the problem as a NonLinear Program (NLP), we propose a framework that reduces the optimization dimension from ((mn)2+ mn) to (mn) with (n) referring to the dimension of each individual agent's state space. As a result, the proposed method reduces the formidable generic Dec-POMDP to a computationally tractable multi-agent planning under uncertainty. Our results in 2D and 3D environments demonstrate the performance of the algorithm and its ability to predict and avoid inter-agent collisions.

Planning under process and measurement uncertainties is a challenging problem. In its most general form, it can be modeled as a Partially Observed Markov Decision Process (POMDP) problem. However POMDPs are generally difficult to solve when the underlying spaces are continuous, particularly when beliefs are non-Gaussian, and the difficulty is further exacerbated when there are also non-convex constraints on states. Existing algorithms to address such challenging POMDPs are expensive in terms of computation and memory. In this paper, we provide a feedback policy in non-Gaussian belief space by solving a convex program for common non-linear observation models. The solution involves a Receding Horizon Control strategy using particle filters for the non-Gaussian belief representation. We develop a way of capturing non-convex constraints in the state space and adapt the optimization to incorporate such constraints, as well. A key advantage of this method is that it does not introduce additional variables in the optimization problem and is therefore more scalable than existing constrained problems in belief space. We demonstrate the performance of the method on different scenarios.

This paper presents a new recursive information consensus filter for decentralized dynamic-state estimation. Local estimators are assumed to have access only to local information and no structure is assumed about the topology of the communication network, which need not be connected at all times. Iterative Covariance Intersection (ICI) is used to reach consensus over priors which might become correlated, while consensus over new information is handled using weights based on a Metropolis Hastings Markov Chain (MHMC). We establish bounds for estimation performance and show that our method produces unbiased conservative estimates that are better than CI. The performance of the proposed method is evaluated and compared with competing algorithms on an atmospheric dispersion problem.

In this paper, we present a receding horizon solution to the problem of optimal scheduling for multiple sensors monitoring a group of dynamical targets. The term target is used here in the classic sense of being the object that is being sensed or observed by the sensors. This problem is motivated by the space situational awareness (SSA) problem. The multisensor optimal scheduling problem can be posed as a multiagent Markov decision process on the information space which has a dynamic programming (DP) solution. We present a simulation-based stochastic optimization technique that exploits the structure inherent in the problem to obtain variance reduction along with a distributed solution. This stochastic optimization technique is combined with a receding horizon approach which uses online solution of the control problems to obviate the need to solve the computationally intractable multiagent information space DP problem and hence, makes the technique computationally tractable. The technique is tested on a moderate scale SSA example which is nonetheless computationally intractable for existing solution techniques.

Motion planning in belief space (under motion and sensing uncertainty) is a challenging problem due to the computational intractability of its exact solution. The Feedback-based Information RoadMap (FIRM) framework made an important theoretical step toward enabling roadmap-based planning in belief space and provided a computationally tractable version of belief space planning. However, there are still challenges in applying belief space planners to physical systems, such as the discrepancy between computational models and real physical models. In this paper, we propose a dynamic replanning scheme in belief space to address such challenges. Moreover, we present techniques to cope with changes in the environment (e.g., changes in the obstacle map), as well as unforeseen large deviations in the robot's location (e.g., the kidnapped robot problem). We then utilize these techniques to implement the first online replanning scheme in belief space on a physical mobile robot that is robust to changes in the environment and large disturbances. This method demonstrates that belief space planning is a practical tool for robot motion planning.

In this paper, we present a receding horizon solution to the optimal sensor scheduling problem. The optimal sensor scheduling problem can be posed as a partially observed Markov decision problem whose solution is given by an information space (I-space) dynamic programming (DP) problem. We present a simulation-based stochastic optimization technique that, combined with a receding horizon approach, obviates the need to solve the computationally intractable I-space DP problem. The technique is tested on a sensor scheduling problem, in which a sensor must choose among the measurements of N dynamical systems in a manner that maximizes information regarding the aggregate system over an infinite horizon. While simple, such problems nonetheless lead to very high dimensional DP problems to which the receding horizon approach is well suited.

This paper extends the concept of “probabilistic completeness” defined for motion planners in state space (or configuration space) to the concept of “probabilistic completeness under uncertainty” for motion planners in belief space. Accordingly, an approach is proposed to verify the probabilistic completeness of the sampling-based planners in belief space. Finally, through the proposed approach, it is shown that under mild conditions the sampling-based methods constructed based on the abstract framework of FIRM (Feedback-based Information Roadmap Method) are probabilistically complete under uncertainty.

In roadmap-based methods, such as the Probabilistic Roadmap Method (PRM) in deterministic environments or the Feedback-based Information RoadMap (FIRM) in partially observable probabilistic environments, a stabilizing controller is needed to guarantee node reachability in state or belief space. In belief space, it has been shown that belief-node reachability can be achieved using stationary Linear Quadratic Gaussian (LQG) controllers, for linearly controllable systems. However, for nonholonomic systems such as a unicycle model, belief reachability is a challenge. In this paper, we construct a roadmap in information space, where the local planners in partially-observable space are constructed by utilizing a Kalman filter as an estimator along with a Dynamic Feedback Linearization-based (DFL-based) controller as the belief controller. As a consequence, the task of belief stabilization to pre-defined nodes in belief space is accomplished even for nonholonomic systems. Therefore, a query-independent roadmap is generated in belief space that preserves the “principle of optimality”, required in dynamic programming solvers. This method serves as an offline POMDP solver for motion planning in belief space, which can seamlessly take obstacles into account. Experimental results show the efficiency of both individual local planners and the overall planner over the information graph for a nonholonomic model.

In this paper, the generalized motion planning algorithm (Generalized PRM: GRPM [1, 2, 3, 4]) is extended to a class of multi-agent motion planning problem in presence of process uncertainty and stochastic maps. The proposed algorithm is a hierarchical approach towards constructing a passive coordination strategy which utilizes an existing multiple traveling salesman problem (MTSP) solution methodology in conjunction with the GPRM framework to solve the multi-agent motion planning problem. The proposed algorithm is generalized to tackle multi-agent problems involving heterogeneous agents. The algorithm is used to solve multi-agent motion planning problems involving 2-dimensional (2D) and 3-dimensional(3D) agents in stochastic maps with uncertainty in the motion model. Results indicate that the algorithm successfully solves the problem under uncertainty, and generates a solution having high probability of success. It also demonstrates that the algorithm is scalable in terms of number of start and goal locations, the number of agents and their dynamics.

In this paper, we present a receding horizon solution to the problem of optimal sensor scheduling problem. The optimal sensor scheduling problem can be posed as a Partially Observed Markov Decision Process (POMDP) whose solution is given by an Information Space (I-space) Dynamic Programming (DP) problem. We present a simulation based stochastic optimization technique that, combined with a receding horizon approach, obviates the need to solve the computationally intractable I-space DP problem. The technique is tested on a simple sensor scheduling problem where a sensor has to choose among the measurements of N dynamical systems such that the information regarding the aggregate system is maximized over an infinite horizon.

Direct transformation of sampling-based motion planning methods to the Information-state (belief) space is a challenge. The main bottleneck for roadmap-based techniques in belief space is that the incurred costs on different edges of the graph are not independent of each other. In this paper, we generalize the Probabilistic RoadMap (PRM) framework to obtain a Feedback controller-based Information-state RoadMap (FIRM) that takes into account motion and sensing uncertainty in planning. The FIRM nodes and edges lie in belief space and the crucial feature of FIRM is that the costs associated with different edges of FIRM are independent of each other. Therefore, this construct essentially breaks the “curse of history” in the original Partially Observable Markov Decision Process (POMDP), which models the planning problem. Further, we show how obstacles can be rigorously incorporated into planning on FIRM. All these properties stem from utilizing feedback controllers in the construction of FIRM.

In this paper, generalized versions of the probabilistic sampling-based planners, i.e., probabilistic roadmaps and rapidly exploring random tree, are presented. The generalized planners, i.e., generalized probabilistic roadmap and the generalized rapidly exploring random tree, result in hybrid hierarchical feedback planners that are robust to the uncertainties in the robot motion model and in the robot map or workspace. The proposed planners are analyzed and shown to probabilistically be complete. The algorithms are tested on fully actuated and underactuated robots on several maps of varying degrees of difficulty, and the results show that the generalized methods have a significant advantage over the traditional methods when planning under uncertainty.

In this paper, generalized versions of the probabilistic sampling based planners, Probabilisitic Road Maps (PRM) and Rapidly exploring Random Tree (RRT), are presented. The generalized planners, Generalized Proababilistic Road Map (GPRM) and the Generalized Rapidly Exploring Random Tree (GRRT), are designed to account for uncertainties in the robot motion model as well as uncertainties in the robot map/ workspace. The proposed planners are analyzed and shown to be probabilistically complete. The algorithms are tested by solving the motion planning problem of a nonholonomic unicycle robot in several maps of varying degrees of difficulty and results show that the generalized methods have excellent performance in such situations.

A hybrid Bayesian/ frequentist approach is presented for the simultaneous localization and mapping problem (SLAM). A frequentist approach is proposed for mapping with time varying robotic poses and is generalized to the case when the robotic pose is both time varying and uncertain. The SLAM problem is then solved in two steps: 1) the robot is localized with respect to a sparse set of landmarks in the map using a Bayes filter and a belief on the robot pose is formed, and 2) this belief on the robot pose is used to map the rest of the map using the frequentist estimator. The hybrid methodology is shown to have complexity linear in the map components, is robust to the data association problem and is provably consistent.