SLAP: Simultaneous Localization and Planning Under Uncertainty via Dynamic Replanning in Belief Space

Abstract

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 POMDP (partially-observable Markov decision process), 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 FIRM (Feedback-based Information RoadMap) 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.

Where to Map? Iterative Rover-Copter Path Planning for Mars Exploration

“…The pose belief is propagated with Eqs. (5) and (6) if the relative transformation is given in a deterministic form. In our problem, we plan rover's path on a map belief and the uncertainty accumulation only given as a stochastic form.…”

Distributed Hierarchical Control for State Estimation With Robotic Sensor Networks

“…This problem is known in the mobile robotics literature as distributed state estimation. The approach presented herein is similar to recent advancements in linear-Gaussian active sensing that operate in pose-covariance space [2], [3], except that here we propose a novel beliefspace discretization that admits exact value iteration and can be readily incorporated in a hierarchical multi-robot controller, enabling decentralized information acquisition of very large collections of hidden states by large teams of robots.…”

“…A preliminary version of this work can be found in [1] the covariance matrix. This is the approach followed, e.g., in mobile target tracking [4], [5], sparse landmark localization [1], [6], [7], and active SLAM [3], [8], [9]. Recently, [8] have shown that information-theoretic objectives may fail even to be monotonic in many active sensing tasks, removing performance guarantees for greedy control.…”

“…This is known as the "belief representation problem." This problem, in the context of information acquisition, has received a great deal of attention recently, both when the robot state is observable [2], [10]- [13] and when it is only partially observable [3], [14]- [21]. The most widely used approach is to grow a tree with a prior distribution of the hidden states at the root, sampling in this way several sequences of possible future observations to obtain the set of reachable belief states at the leaves.…”

“…Common in the vast majority of approaches discussed above is that mobile sensor planning relies on sampling-based strategies, e.g., forward search, for partially [3], [14], [15], [15]- [21] and fully observable [2], [13] robots, or belief space sampling in the policy domain [17]- [19]. Sampling-based approaches will typically run into scalability issues due to one or more of the following reasons: (i) sparsity of information in the environment, which forces longer planning horizons, (ii) high dimensional unknowns, which make observation sampling inefficient, or (iii) large teams of robots, which significantly increase the size of the action space.…”