Efficient Belief Road Map for Planning Under Uncertainty

TL;DR

This paper introduces an efficient graph-based belief space planning method using covariance control to navigate uncertain environments, demonstrated through numerical experiments and available as open-source.

Contribution

It presents a novel belief roadmap construction technique that adaptively manages uncertainty with covariance control for improved planning under uncertainty.

Findings

Effective in various motion planning tasks

Balances control costs and uncertainty

Outperforms existing methods in experiments

Abstract

Robotic systems, particularly in demanding environments like narrow corridors or disaster zones, often grapple with imperfect state estimation. Addressing this challenge requires a trajectory plan that not only navigates these restrictive spaces but also manages the inherent uncertainty of the system. We present a novel approach for graph-based belief space planning via the use of an efficient covariance control algorithm. By adaptively steering state statistics via output state feedback, we efficiently craft a belief roadmap characterized by nodes with controlled uncertainty and edges representing collision-free mean trajectories. The roadmap's structured design then paves the way for precise path searches that balance control costs and uncertainty considerations. Our numerical experiments affirm the efficacy and advantage of our method in different motion planning tasks. Our open-source implementation can be found at https://github.com/hzyu17/VIMP/tree/BRM.