Motion Planning for Active Data Association and Localization in Non-Gaussian Belief Spaces

Abstract

This paper presents a method for motion planning under uncertainty to resolve situations where ambiguous data associations result in a multimodal hypothesis on the robot state. Simultaneous localization and planning for a lost (or kidnapped) robot requires that given little to no a priori pose information, a planner should generate actions such that future observations allow the localization algorithm to recover the correct pose of a mobile robot with respect to a global reference frame. We present a Receding Horizon approach, to plan actions that sequentially disambiguate a multimodal belief to achieve tight localization on the correct pose in finite time. In our method, disambiguation is achieved through active data associations by picking target states in the map which allow distinctive information to be observed for each belief mode and creating local feedback controllers to visit the targets. Experimental results are presented for a kidnapped physical ground robot operating in an artificial maze-like environment.