Belief Space Planning: A Covariance Steering Approach

TL;DR

This paper introduces CS-BRM, a belief space planning algorithm that extends probabilistic roadmaps with covariance steering to efficiently plan under motion and observation uncertainties.

Contribution

It develops a novel multi-query belief space planning algorithm based on covariance steering, enabling guaranteed belief constraint satisfaction in finite time.

Findings

CS-BRM outperforms previous methods in planning efficiency.

The algorithm guarantees belief constraints in finite time.

CS-BRM effectively explores velocity space for better plans.

Abstract

A new belief space planning algorithm, called covariance steering Belief RoadMap (CS-BRM), is introduced, which is a multi-query algorithm for motion planning of dynamical systems under simultaneous motion and observation uncertainties. CS-BRM extends the probabilistic roadmap (PRM) approach to belief spaces and is based on the recently developed theory of covariance steering (CS) that enables guaranteed satisfaction of terminal belief constraints in finite-time. The nodes in the CS-BRM are sampled in belief space and represent distributions of the system states. A covariance steering controller steers the system from one BRM node to another, thus acting as an edge controller of the corresponding belief graph that ensures belief constraint satisfaction. After the edge controller is computed, a specific edge cost is assigned to that edge. The CS-BRM algorithm allows the sampling of non-stationary belief nodes, and thus is able to explore the velocity space and find efficient motion plans. The performance of CS-BRM is evaluated and compared to a previous belief space planning method, demonstrating the benefits of the proposed approach.