Motion Planning in Non-Gaussian Belief Spaces for Mobile Robots Saurav Agarwal Amirhossein Tamjidi Suman Chakravorty Abstract—In this paper we propose a method for motion planning under uncertainty that can deal with situations where ambiguous data associations result in a multimodal hypothesis in belief space. Unlike conventional methods, planning based on the most-likely hypothesis is no longer possible in such cases. We adopt a Gaussian Mixture Model (GMM) for belief representation and use a Receding Horizon Planning approach, called a MultiModal Motion Planner (M3P), to plan actions that sequentially disambiguate the multimodal belief to a unimodal Gaussian and achieve tight localization on the true state. We demonstrate the performance of our method on a physical ground robot that is started in an unknown location in a symmetric environment wherein the planner successfully guides the belief to a unimodal distribution. I. I NTRODUCTION In the domain of motion planning for mobile robots, certain situations can arise where data association between what is observed and the robot’s a-priori map leads to a multimodal hypothesis. An example is the data association problem for a kidnapped robot operating in a symmetric environment. Imagine a mobile robot equipped with a monocular camera operating in a world where there are identical rooms as shown in Fig. 1. To the camera, each and every room appears identical. Thus, if the robot is switched off and placed randomly in one of the rooms, on waking up it has no way of knowing exactly where it is. In a sense, based on the observations, the robot would infer that it could be in either of the four rooms at that instant since the camera images would look alike. We can extend the previous statement to say that when sensory information leads to ambiguous data associations, the robot may believe itself to be in one of multiple states at the same time. Such a situation implies that the pdf of the robot’s state can no longer be sufficiently represented by a unimodal Gaussian. In general, motion planning for mobile robots involves deal- ing with the uncertain nature of physical systems, i.e., noisy actuators and sensors as well as changes in the environment in which the robot operates. The motion or actuator uncertainty makes it difficult to execute precise actions and sensing un- certainty makes it impossible to determine the exact state of the robot. Further, changes in the environment can reduce the effectiveness of plans computed offline. Thus, unless a plan can be updated on the fly to account for new constraints, it might fail. A significant amount of research has gone into developing Saurav Agarwal (sauravag@tamu.edu), Amirhossein Tamjidi (ahtamjidi@tamu.edu) and Suman Chakravorty (schakrav@tamu.edu) are with the Department of Aerospace Engineering, Texas A&M University, College Station, TX 77840. Fig. 1: A kidnapped robot scenario in a world with 4 identical rooms. The actual robot is depicted by the blue outlined disk, whereas the hypothesis are depicted by red disks. All hypothesis are equally likely. The dashed green arrows show a possible control action based on the hypothesis in the top- right room that can result in collision for the actual robot. probabilistic methods to achieve robust performance for practi- cal systems. In the probabilistic approach to motion planning, the aim is to develop methods that maximize the probability of achieving a desired state while avoiding obstacles. State of the art methods ([20], [6], [8], [21], [14], [4], [18]) rely on a probability distribution over the system’s state (referred to as the belief) and develop solutions in the belief space that enable us to drive the system belief from an initial to a desired belief. It is well known that planning for systems under uncertainty belongs to the class of Partially-Observable Markov Decision Process (POMDP) problems which are known to be computationally intractable [15]. A. Related Work Recent work, in particular, sampling-based methods, have shown varying degrees of success in solving the problem of motion planning under uncertainty. In [20], the authors rely on covariance factorization to devise an efficient belief space planner. Their method generates plans that result in information gathering actions which minimize state uncertainty at the goal. In [6], a graph is constructed in belief space and pruned suc- cessively, ultimately resulting in a tree in belief space. These methods along with others [8], [21], [14] provide solutions that are single-query, i.e., dependent on the initial belief. This makes them computationally inefficient for real-time replanning wherein new queries are submitted constantly. Recent devel- opments in [4] extend belief space planning to multi-query settings by creating a belief space variant of a Probabilistic RoadMap (PRM) [13], this way, it enables re-use of the offline arXiv:1511.04634v1 [cs.RO] 14 Nov 2015