Motion Planning in Non-Gaussian Belief Spaces (M3P): The Case of a Kidnapped Robot

TL;DR

This paper introduces M3P, a motion planning algorithm for robots operating under uncertainty with multi-modal beliefs, enabling effective localization in ambiguous environments by disambiguating hypotheses through sequential actions.

Contribution

The paper presents a novel Receding Horizon Planning algorithm, M3P, that disambiguates multi-modal beliefs to achieve accurate localization, extending Gaussian-based methods to more complex belief representations.

Findings

M3P effectively localizes a robot in multi-modal belief scenarios.

Simulation results show improved localization accuracy over existing methods.

The approach reduces uncertainty in environments with ambiguous data associations.

Abstract

Planning under uncertainty is a key requirement for physical systems due to the noisy nature of actuators and sensors. Using a belief space approach, planning solutions tend to generate actions that result in information seeking behavior which reduce state uncertainty. While recent work has dealt with planning for Gaussian beliefs, for many cases, a multi-modal belief is a more accurate representation of the underlying belief. This is particularly true in environments with information symmetry that cause uncertain data associations which naturally lead to a multi-modal hypothesis on the state. Thus, a planner cannot simply base actions on the most-likely state. We propose an algorithm that uses a Receding Horizon Planning approach to plan actions that sequentially disambiguate the multi-modal belief to a uni-modal Gaussian and achieve tight localization on the true state, called a Multi-Modal Motion Planner (M3P). By combining a Gaussian sampling-based belief space planner with M3P, and introducing a switching behavior in the planner and belief representation, we present a holistic end-to-end solution for the belief space planning problem. Simulation results for a 2D ground robot navigation problem are presented that demonstrate our method's performance.