Belief Space Planning: a Covariance Steering Approach

Dongliang Zheng, Jack Ridderhof, Panagiotis Tsiotras, Ali-akbar Agha-mohammadi

TL;DR

The authors introduce CS-BRM (Covariance Steering Belief RoadMap), a motion planning algorithm that allows robots to navigate uncertain environments without having to "stop and settle" at every waypoint. By leveraging covariance steering theory, the algorithm ensures a robot can transition between specific probability distributions (beliefs) in finite time, enabling smoother, faster, and more efficient paths than previous state-of-the-art methods.

What Problem Does This Paper Address?

In the real world, robots never know their exact state; they deal with "beliefs"—probability distributions over possible positions and velocities. Planning in this "belief space" is notoriously difficult because of the "curse of history." In traditional belief roadmaps, the uncertainty at one point depends on every action taken previously, which breaks the mathematical properties needed for efficient pathfinding algorithms like A*.

To solve this, previous methods (like FIRM) forced robots to reach "stationary" belief nodes—essentially requiring the robot to stop at every waypoint until its uncertainty settled into a predictable, localized state. While this makes the math work, it results in jerky, "stop-and-go" motion that is highly inefficient in terms of time and energy. The challenge addressed here is: How can we plan through belief space while allowing the robot to maintain velocity and move smoothly between waypoints?

Methods

The researchers propose the Covariance Steering Belief RoadMap (CS-BRM). The core innovation lies in using Covariance Steering (CS) as the "edge controller" for a roadmap graph.

  1. Roadmap Construction: Like a standard Probabilistic RoadMap (PRM), the algorithm samples nodes. However, these nodes are sampled in belief space (representing a mean state and a specific covariance/uncertainty).
  2. Covariance Steering (CS): This is the "secret sauce." CS is a control technique that guarantees a system will move from one Gaussian distribution to another in a fixed amount of time. By using CS to connect nodes, the authors ensure that the robot reaches the exact target belief at the end of an edge, regardless of its history.
  3. Compatible Nominal Trajectory (CNT): For nonlinear systems (like aircraft), the math gets harder. The authors developed the CNT algorithm, which iteratively finds a "nominal" path that is both dynamically feasible and optimal, ensuring the linearized control laws used for steering remain accurate.
  4. Graph Search: Because CS makes each edge independent, the authors can use standard graph search techniques to find the global path that minimizes a combination of control effort and collision risk.

Datasets

As is common in robotics control research, the paper evaluates the algorithm using simulated environments rather than static datasets:

  • 2D Double Integrator: A linear system where a point mass must navigate obstacles. This was used to compare CS-BRM directly against the stationary SLQG-FIRM method.
  • 3D Fixed-Wing Aircraft: A complex, nonlinear system governed by aerodynamic equations. The environment included spherical obstacles and landmarks used for position sensing.

Key Insights and Ideas

  • Breaking the Zero-Velocity Constraint: By allowing nodes to have non-zero velocity, the planner can explore the "velocity space." This allows the robot to "swing through" waypoints rather than stopping at them.
  • Reachability as a Solution to History: The "curse of history" is essentially a reachability problem. By proving that any sampled belief can be reached from a neighbor in finite time using CS, the authors restore the "optimal substructure" property required for efficient graph searching.
  • Separation of Concerns: The paper elegantly separates the problem into mean control (getting the robot to the right place) and covariance control (managing the "size" of the uncertainty bubble).
  • Collision Awareness: The edge costs aren't just about distance; they incorporate Monte Carlo simulations to estimate the probability of collision given the robot's uncertainty along that specific path.

Key Results

  • Significant Cost Reduction: In the 2D double integrator test, CS-BRM produced a path with a cost of 104.87, compared to 239.36 for the stationary method. This represents a massive improvement in efficiency primarily because the robot didn't have to decelerate to zero at every node.
  • Smoothness: Qualitative results show that CS-BRM paths are significantly smoother. The robot maintains momentum, which is critical for systems like fixed-wing aircraft that cannot easily hover or stop mid-air.
  • Nonlinear Robustness: The CNT algorithm successfully converged in just eight iterations for the aircraft model, proving that the approach is computationally viable for complex, real-world dynamics.

Limitations and Future Work

  • Offline Computation: While the resulting paths are efficient, the "offline" phase—building the roadmap and calculating the CS controllers for every edge—is computationally intensive.
  • Gaussian Assumption: The current framework relies on Gaussian distributions. While common, this might struggle in environments with highly non-Gaussian noise or "multi-modal" uncertainties (e.g., when a robot isn't sure if it's on the left or right side of a wall).
  • Future Directions: The authors suggest extending this to real-time replanning, where the roadmap could be updated on-the-fly as the robot discovers new obstacles or experiences unexpected sensor failures.

Summary updated

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.
BibTeX
@inproceedings{zheng2022belief,
  author = {Dongliang Zheng and Jack Ridderhof and Panagiotis Tsiotras and Ali-akbar Agha-mohammadi},
  title = {{Belief Space Planning: a Covariance Steering Approach}},
  booktitle = {IEEE International Conference on Robotics and Automation},
  year = {2022},
  doi = {10.1109/icra46639.2022.9811560},
}

Loading related content...

Figure