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

Ali-akbar Agha-mohammadi, Saurav Agarwal, Sung Kyun Kim, Suman Chakravorty and Nancy M. Amato
Journal Article, IEEE Transactions on Robotics, Vol. 34, No. 5, pp. 1195-1214, October, 2018

View Publication

Copyright notice: This material is presented to ensure timely dissemination of scholarly and technical work. Copyright and all rights therein are retained by authors or by other copyright holders. All persons copying this information are expected to adhere to the terms and constraints invoked by each author's copyright. These works may not be reposted without the explicit permission of the copyright holder.


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.

author = {Ali-akbar Agha-mohammadi and Saurav Agarwal and Sung Kyun Kim and Suman Chakravorty and Nancy M. Amato},
title = {SLAP: Simultaneous Localization and Planning Under Uncertainty via Dynamic Replanning in Belief Space},
journal = {IEEE Transactions on Robotics},
year = {2018},
month = {October},
volume = {34},
number = {5},
pages = {1195-1214},
} 2018-12-05T08:09:22-04:00