FIRM: Sampling-based feedback motion-planning under motion uncertainty and imperfect measurements

Abstract

In this paper we present FIRM (Feedback-based Information RoadMap), a multi-query approach for planning under uncertainty, that is a belief-space variant of Probabilistic Roadmap Methods (PRMs). The crucial feature of FIRM is that the costs associated with the edges are independent of each other, and in this sense it is the first method that generates a graph in belief space that preserves the optimal substructure property. From a practical point of view, FIRM is a robust and reliable planning framework. It is robust since the solution is a feedback and there is no need for expensive replanning. It is reliable because accurate collision probabilities can be computed along the edges. In addition, FIRM is a scalable framework, where the complexity of the planning with FIRM is a constant multiplier of the complexity of planning with PRM. In this paper, FIRM is introduced as an abstract framework. As a concrete instantiation of FIRM, we adopt Stationary Linear Quadratic Gaussian (SLQG) controllers as belief stabilizers and introduce the so-called SLQG-FIRM. In SLQG-FIRM we focus on kinematic systems and then extend to dynamical systems by sampling in the equilibrium space. We investigate the performance of SLQG-FIRM in different scenarios.

Sampling-Based Reactive Motion Planning with Temporal Logic Constraints and Imperfect State Information

“…To alleviate this problem, we use feedback-based information roadmaps (FIRMs) [1]. FIRMs generalise probabilistic roadmaps (PRMs) [12] to account for motion and sensing uncertainty.…”

“…Without loss of generality, we use SLQG-FIRMs [1], where stationary linear quadratic Gaussian (SLQG) controllers are used as belief stabilisers. Any other type of controller can be used provided that the reachability of a belief is guaranteed.…”

“…Under the assumption that the pairs (A, B) and (A, H) are controllable and observable, respectively, the SLQG controller stabilises the system to an expected belief b ν = (ν, P ν ), where the covariance P ν can be determined offline for each node ν [1]. Hence, a belief node is defined as b = {b : b − b ν b < ε}, where · b is a suitable norm in B and ε determines the size of the belief node.…”

“…Because during the transition s to s ′ in P, more than one DRA state can be reached, in order to find a run on P satisfying a specification, each transition in P is associated with a probability of visiting a state in a pair (L i , K i ) ∈ F. These probabilities are denoted as P L i s,s ′ and P K i s,s ′ , respectively. Computing probabilities of transitioning from s to s ′ ∈ S is computationally expensive [1]. In this work, we approximate them using particle-based methods.…”

“…This is caused by two main sources of complexity: (i) the so-called curse of dimensionality, for a system with n states, the belief space is an (n − 1)-dimensional continuous space; and (ii) the curse of history [16], the number of distinct action-observation histories grows exponentially with the planning horizon. To alleviate this problem, sampling-based methods have been proposed, e.g., [1,5,17]. In these works, the objective is usually to optimally drive the system from an initial to a final state.…”