Aghamohammadi, Aliakbar (2014-05). Feedback-based Information Roadmap (FIRM): Graph-based Estimation and Control of Robotic Systems Under Uncertainty. Doctoral Dissertation.
This dissertation addresses the problem of stochastic optimal control with imperfect measurements. The main application of interest is robot motion planning under uncertainty. In the presence of process uncertainty and imperfect measurements, the system's state is unknown and a state estimation module is required to provide the information-state (belief), which is the probability distribution function (pdf) over all possible states. Accordingly, successful robot operation in such a setting requires reasoning about the evolution of information-state and its quality in future time steps. In its most general form, this is modeled as a Partially-Observable Markov Decision Process (POMDP) problem. Unfortunately, however, the exact solution of this problem over continuous spaces in the presence of constraints is computationally intractable. Correspondingly, state-of-the-art methods that provide approximate solutions are limited to problems with short horizons and small domains. The main challenge for these problems is the exponential growth of the search tree in the information space, as well as the dependency of the entire search tree on the initial belief. Inspired by sampling-based (roadmap-based) methods, this dissertation proposes a method to construct a "graph" in information space, called Feedback-based Information RoadMap (FIRM). Each FIRM node is a probability distribution and each FIRM edge is a local controller. The concept of belief stabilizers is introduced as a way to steer the current belief toward FIRM nodes and induce belief reachability. The solution provided by the FIRM framework is a feedback law over the information space, which is obtained by switching among locally distributed feedback controllers. Exploiting such a graph in planning, the intractable POMDP problem over continuous spaces is reduced to a tractable MDP (Markov Decision Process) problem over the graph (FIRM) nodes. FIRM is the first graph generated in the information space that preserves the principle of optimality, i.e., the costs associated with different edges of FIRM are independent of each other. Unlike the forward search methods on tree-structures, the plans produced by FIRM are independent of the initial belief (i.e., plans are query-independent). As a result, they are robust and reliable. They are robust in the sense that if the system's belief deviates from the planned belief, then replanning is feasible in real-time, as the computed solution is a feedback over the entire belief graph. Computed plans are reliable in the sense that the probability of violating constraints (e.g., hitting obstacles) can be seamlessly incorporated into the planning law. Moreover, FIRM is a scalable framework, as the computational complexity of its construction is linear in the size of underlying graph as opposed to state-of-the-art methods whose complexity is exponential in the size of underlying graph. In addition to the abstract framework, we present concrete FIRM instantiations for three main classes of robotic systems: holonomic, nonholonomic, and non-pointstabilizable. The abstract framework opens new avenues for extending FIRM to a broader class of systems that are not considered in this dissertation. This includes systems with discrete dynamics or in general systems that are not well-linearizable, systems with non-Gaussian distributions, and systems with unobservable modes. In addition to the abstract framework and concrete instantiations of it, we propose a formal technique for replanning with FIRM based on a rollout-policy algorithm to handle changes in the environment as well as discrepancies between actual and computational models. We demonstrate the performance of the proposed motion planning method on different robotic systems, both in simulation and on physical systems. In the problems we consider, the system is subject to motion and sensing noise. Our results demonstrate a significant advance over existing approaches for motion planning in i