Path-Tree Optimization in Discrete Partially Observable Environments using Rapidly-Exploring Belief-Space Graphs

TL;DR

This paper introduces the Path-Tree Optimization algorithm that plans in belief-space using rapidly-exploring graphs, enabling robots to efficiently navigate and manipulate in partially observable environments by balancing exploration and exploitation.

Contribution

The paper presents a novel PTO algorithm that constructs a belief-space path-tree with RRG expansion and dynamic programming, improving planning efficiency and optimality in partially observable settings.

Findings

Outperforms baseline TAMP in optimality and runtime

Successfully applied to navigation and mobile manipulation tasks

Balances exploration and exploitation effectively

Abstract

Robots often need to solve path planning problems where essential and discrete aspects of the environment are partially observable. This introduces a multi-modality, where the robot must be able to observe and infer the state of its environment. To tackle this problem, we introduce the Path-Tree Optimization (PTO) algorithm which plans a path-tree in belief-space. A path-tree is a tree-like motion with branching points where the robot receives an observation leading to a belief-state update. The robot takes different branches depending on the observation received. The algorithm has three main steps. First, a rapidly-exploring random graph (RRG) on the state space is grown. Second, the RRG is expanded to a belief-space graph by querying the observation model. In a third step, dynamic programming is performed on the belief-space graph to extract a path-tree. The resulting path-tree combines exploration with exploitation i.e. it balances the need for gaining knowledge about the environment with the need for reaching the goal. We demonstrate the algorithm capabilities on navigation and mobile manipulation tasks, and show its advantage over a baseline using a task and motion planning approach (TAMP) both in terms of optimality and runtime.