💻 computer science

Continuous-Time Gaussian Belief Trees for Motion Planning

This paper proposes a continuous-time Gaussian belief tree framework for motion planning under uncertainty that integrates hybrid belief propagation with a belief-barrier-function safety checker to guarantee probabilistic safety over entire trajectory segments, thereby overcoming the limitations of discrete-time methods in detecting inter-sample chance-constraint violations.

Original authors: Rayan Mazouz, Qi Heng Ho, Zachary N. Sunberg, Morteza Lahijanian

Published 2026-07-07

📖 4 min read☕ Coffee break read

CC BY 4.0

Original authors: Rayan Mazouz, Qi Heng Ho, Zachary N. Sunberg, Morteza Lahijanian

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are trying to guide a blindfolded drone through a crowded city full of narrow alleys and tall buildings. You can't see the drone perfectly; you only get a blurry, noisy GPS signal every few seconds. Your goal is to get the drone from point A to point B without hitting anything, even though you aren't 100% sure where the drone actually is at any given moment.

This paper presents a new "navigation strategy" for robots that are dealing with this exact kind of uncertainty. Here is how it works, broken down into simple concepts:

1. The Problem: The "Snapshots" Trap

Most current robot planners work like a flipbook. They take a "snapshot" of where the robot might be, make a decision, take another snapshot, and decide again. They only check for safety at these specific moments (the snapshots).

The Analogy: Imagine you are driving a car and only check your rearview mirror every 10 seconds. Between those checks, you might drift into a pothole or hit a pedestrian, but because you weren't looking at that exact second, your plan thinks you are safe.

In the real world, robots move continuously, but sensors only give data at discrete moments. The old methods miss the dangerous moments between the sensor updates. If a robot drifts into an obstacle between two "snapshots," the old planner doesn't know until it's too late.

2. The Solution: A Continuous "Belief" Stream

The authors created a new method called Continuous-Time Gaussian Belief Trees. Instead of thinking in snapshots, this method thinks in a smooth, flowing stream.

The "Belief": Since the robot doesn't know its exact location, it holds a "belief" about where it is. This belief isn't a single dot; it's a fuzzy cloud (a Gaussian distribution) that represents all the possible places the robot could be.
The Flow: Between sensor updates, this fuzzy cloud naturally expands and drifts (because the robot is moving and things are uncertain). The new math tracks this cloud's growth and movement continuously, like watching a balloon slowly inflate and float, rather than just checking its size once a minute.
The Jump: When a new sensor reading arrives, the cloud suddenly shrinks and snaps to a more accurate location (like a Kalman Filter update).

3. The Safety Net: The "Fence" Check

The biggest innovation is how they check for safety.

Old Way: Check if the center of the cloud is safe at the snapshot moments.
New Way: They use something called a Belief Barrier Function. Think of this as an invisible, flexible fence that surrounds the robot's "fuzzy cloud."

Instead of just checking the fence at the snapshot moments, this new method checks the entire path the fence takes between snapshots. It mathematically guarantees that the fuzzy cloud never touches the obstacles, even in the split seconds between sensor updates.

4. The Results: Narrow Passages

The authors tested this on robots moving through very tight spaces (narrow alleys).

The Old Planners (Discrete-Time): They often failed. They thought the robot was safe because the "snapshots" looked clear, but the robot actually crashed in the gaps between snapshots.
The New Planner (Continuous-Time): It succeeded almost every time. By watching the whole continuous path, it found routes that were truly safe, avoiding the "hidden" crashes that the old methods missed.

Summary

In short, this paper teaches robots to stop guessing based on snapshots and start planning based on a continuous, flowing understanding of their uncertainty. It's the difference between checking your blindfolded path by peeking every few seconds versus having a continuous, real-time awareness of your surroundings, ensuring you never bump into a wall even when you aren't looking directly at it.

Key Takeaway: This method makes robots safer and more reliable in uncertain, real-world environments, specifically by catching dangers that happen between sensor updates.

Technical Summary: Continuous-Time Gaussian Belief Trees for Motion Planning

Problem Formulation
This paper addresses the challenge of sampling-based motion planning for robotic systems operating under both process and measurement uncertainty in continuous time. While real-world robotic dynamics evolve continuously, sensors typically provide measurements only at discrete intervals. Existing belief-space planners often rely on discrete-time approximations, which evaluate safety only at specific time steps (nodes). This approach creates a critical vulnerability: the robot's state distribution (belief) grows continuously between discrete updates due to noise, potentially intersecting with obstacles during the inter-sample intervals even if the discrete checkpoints appear safe. The authors formulate a problem where the goal is to find a motion plan that satisfies probabilistic safety constraints (chance constraints) over the entire continuous-time trajectory, ensuring the probability of collision or control violation remains below a specified threshold $P_{safe}$ .

Methodology
The proposed framework, Continuous-Time Gaussian Belief Trees (CT-GBT), extends existing sampling-based planners (specifically RRT and SST) to handle continuous-time stochastic dynamics. The methodology consists of three core components:

Hybrid Belief Propagation Model:
The authors derive an offline belief propagation model that captures the hybrid nature of the system. Between measurement epochs, the belief (modeled as a Gaussian distribution) evolves according to continuous-time Ordinary Differential Equations (ODEs) governing the mean and the total covariance (the sum of estimation error covariance and estimate covariance). At discrete measurement times, the belief undergoes a discrete jump via a Kalman Filter update. This allows the planner to predict the evolution of uncertainty without relying on future measurements.
Continuous-Time Probabilistic Safety Checker:
To address the "inter-sample" safety gap, the paper introduces a belief-barrier-function-based collision checker. Unlike traditional methods that verify safety only at discrete nodes, this checker validates entire continuous trajectory segments.
- State Constraints: For each obstacle (modeled as a convex polytope), a separating half-space is constructed. A belief barrier function is defined to ensure the probability of the state lying within the obstacle remains below a risk budget.
- Control Constraints: Since the control input depends on the uncertain state estimate, it is treated as a stochastic variable. The framework derives chance constraints for control admissibility using the covariance of the state estimate.
- Verification: A sufficient condition for continuous-time probabilistic safety is established using a barrier function derivative condition ( $\frac{\partial h}{\partial b}\dot{b} \geq -h$ ). This condition is verified on a dense grid within each trajectory segment to certify safety between measurement updates.
Risk Allocation:
The framework allocates a total risk budget $P_{safe}$ across multiple state and control constraints using Boole's inequality, ensuring the joint probability of violation remains within limits.

Key Contributions
The paper makes four primary contributions:

Hybrid Belief Propagation: A derivation of continuous-time ODEs for estimation and estimate covariances, coupled with discrete Kalman filter updates, enabling accurate offline belief prediction.
Segment-Level Safety Certification: A continuous-time collision checker based on belief barrier functions that certifies safety over entire trajectory segments, detecting inter-sample violations missed by discrete-time methods.
Probabilistic Control Constraints: The integration of chance constraints for stochastic control inputs within the belief-space planning framework, a feature absent in prior Gaussian Belief Tree (GBT) methods.
Theoretical Guarantees: Proof of the algorithm's correctness (soundness) and probabilistic completeness with respect to conservatism, ensuring that if a valid trajectory exists, the algorithm will find it with probability approaching 1 as iterations increase.

Results
The method was evaluated on two benchmark systems (2D single integrator and 4D double integrator) across six environments, including narrow-passage scenarios. The results were compared against discrete-time (DT) GBT variants using both RRT and SST planners:

Success Rates: In Monte Carlo simulations (5,000 runs), the CT-GBT variants achieved success probabilities above 0.94 in all environments, including the most constrained narrow passages. In contrast, DT variants suffered sharp performance degradation, with success rates dropping to as low as 0.05–0.06 in complex environments.
Constraint Satisfaction: CT-GBT maintained high satisfaction probabilities for chance constraints (0.91–1.00), whereas DT planners frequently violated constraints (dropping to 0.00 in the most constrained cases) due to their inability to detect inter-sample unsafe behavior.
Efficiency vs. Accuracy: While DT planners sometimes exhibited lower planning times due to reduced computational burden, they produced trajectories with higher costs and significantly lower reliability. The CT approach demonstrated that reducing the discrete time-step alone is insufficient to recover the safety and performance benefits of the continuous-time formulation.

Significance
The paper claims that this work provides a principled framework for sampling-based belief planning that bridges the gap between real-world continuous-time dynamics and discrete-time planning models. By explicitly accounting for continuous-time uncertainty propagation and enforcing safety over entire trajectory segments, the proposed method offers robust enforcement of chance constraints. The authors emphasize that this is particularly critical in safety-critical scenarios (e.g., narrow passages) where discrete-time counterparts fail due to missed inter-sample unsafe behavior. The methodology establishes a foundation for safe and efficient motion planning under uncertainty, with potential future extensions to richer task specifications like signal temporal logic.