Motion Planning in Non-Gaussian Belief Spaces (M3P): The Case of a Kidnapped Robot
This addresses the challenge of planning under non-Gaussian uncertainty for robots in symmetric environments, offering a domain-specific solution that is incremental as it builds on existing Gaussian planners.
The paper tackles the problem of motion planning for robots with multi-modal beliefs, where uncertainty arises from ambiguous data associations, by proposing M3P, an algorithm that plans actions to disambiguate beliefs to a uni-modal Gaussian and achieve tight localization. Simulation results in 2D navigation show the method's performance, though no specific numerical gains are provided.
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.