A new belief space planning algorithm, called covariance steering Belief RoadMap (CS-BRM), is introduced, analyzed, and numerically and experimentally tested. CS-BRM is a multi-query algorithm for motion planning for dynamical systems under simultaneous motion and observation uncertainties. CS-BRM extends the probabilistic roadmap (PRM) approach to belief spaces 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 the 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 much more efficient trajectories than previous BRM methods. The performance of CS-BRM is evaluated and compared to previous belief space planning approaches using several numerical examples and experimental demonstrations, illustrating the benefits of the proposed approach.

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 finitetime. 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. We evaluate CS-BRM in different planning problems and demonstrate the benefits of the proposed approach.