PaperlayerView exact revision on arXiv

Source-linked AI summary

BLT*: Informed Belief Localization Trees for Uncertainty-Aware Planning on Digital Twins

Elliot Preston-Krebs, Abhishek Goudar, Timothy D. Barfoot

arXiv:2610.01972v1cs.RO

TL;DR

Poorly observable regions can let localization uncertainty grow until collision-avoidance or goal-reaching constraints are not met. Informed BLT* plans over isotropic Gaussian beliefs using W2 path length and point-cloud information; experiments report faster initial solution discovery in most maps with competitive final costs.

  • Problem

    Geometrically short paths can pass through poorly observable regions, where growing uncertainty may violate probabilistic collision-avoidance or goal-reaching constraints.

    Source: §I · p. 1

  • Method

    Informed BLT* uses W2 path length to plan over beliefs, with reachability-based steering and rewiring, reusable point-cloud information, and a semantic digital-twin pipeline.

    Source: §I · p. 1, D. Belief State Reachability & Rewiring · p. 4

  • Results

    The proposed planners generally find solutions faster than baselines, with fastest initial solutions on all maps except Narrow, and achieve competitive final costs after 100 s.

    Source: D. Results and Discussion · pp. 7–8, §VII · p. 8

  • Takeaways & Limitations

    In digital twins, BLT* variants found solutions in all trials, while some baselines had substantial failures in larger environments with more computationally expensive point-cloud observations.

    Source: §VII · p. 8

    The proposed planners did not consistently attain the lowest cost after 100 s on random maps; Informed BLT*’s mean final cost was within 1.34% of the lowest mean final cost obtained by any planner.

    Source: D. Results and Discussion · p. 8

Abstract

from arXiv · show

We present Informed Belief Localization Trees* (Informed BLT*), a sampling-based belief space planning (BSP) algorithm that scales to large outdoor digital twins with point-cloud observations. We adapt RRT* and Informed RRT* to belief space using the $2$-Wasserstein ($W_2$) metric. Assuming isotropic Gaussian beliefs, sampled belief states can be connected efficiently while accounting for available information and probabilistic collision constraints. This enables steering and rewiring without repeatedly propagating observations, and allows previously computed measurement information to be reused. We present a framework to generate semantically labelled digital twins for planning in real-world environments with point-cloud-based localization. Experiments in simulated environments and digital twins show faster initial solution discovery in most maps with competitive cost convergence.

I. INTRODUCTION

Informed BLT* plans over robot beliefs because geometrically short paths can pass through poorly observable regions where uncertainty undermines collision avoidance or goal attainment. It uses W2 path length to guide belief-space planning and presents a point-cloud digital-twin pipeline alongside experiments in simulated and reconstructed environments.

I. INTRODUCTION

  • Poorly observable regions can make uncertainty grow until probabilistic collision-avoidance or goal-reaching constraints are no longer satisfied.Belief-space planning addresses this coupling by planning over distributions of robot states and accounting for motion and localization uncertainty.Source: §I · p. 1
  • Informed BLT* minimizes accumulated W2 path length for isotropic Gaussian beliefs and derives closed-form reachability for steering and rewiring without repeatedly propagating control sequences.Previously evaluated point-cloud information can be reused during later connection and rewiring.Source: §I · p. 1
  • The pipeline uses semantic preprocessing to select localization-relevant geometry and the ICP Hessian as an information matrix for belief updates.The evaluation compares informed and uninformed variants with three BSP baselines across 16 simulated and photogrammetry-reconstructed environments.Source: §I · p. 1

II. RELATED WORK

  • Prior environment-aware planners use spatial localizability or predicted photometric uncertainty [6], while this work extracts localization information from map geometry for digital-twin planning.Source: §II · p. 2

III. PRELIMINARIES

  • Belief states are Gaussian distributions parameterized by mean and covariance, and the paper adopts W2 as their metric and represents planning trees as directed belief-state graphs.The related-work discussion notes that Belief-A uses W2 for proximity while retaining a separate trajectory cost; this work uses W2 as its path objective, enabling cost-informed sampling.Source: §III · p. 2, §II · p. 2

IV. PROBLEM FORMULATION

The problem is to find a feasible belief-space path that minimizes W2 length while satisfying goal and collision-avoidance constraints. The formulation uses discrete-time motion and observation models and restricts beliefs to isotropic Gaussians.

A. Belief Space Shortest Path Problem

B. System Dynamics

  • The system uses discrete-time motion and measurement models; for a holonomic system in R2, these are expressed as linear motion and observation models.Process and observation noises have covariances Q_k and R_k, and u_k is the control input.Source: B. System Dynamics · p. 2

C. Isotropic Gaussian Assumption

  • Anisotropic covariances are conservatively bounded by isotropic covariances determined by their maximum eigenvalues.This parameterizes each belief covariance by a single standard deviation; the formulation also assumes isotropic process and observation noise.Source: C. Isotropic Gaussian Assumption · pp. 2–3
  • Under isotropic Gaussian beliefs, W2 admits an isometric embedding into R^(d+1), mapping each mean and standard deviation to an augmented Euclidean coordinate.For the paper's d = 2 setting, the embedding uses x, y, and σ coordinates.Source: C. Isotropic Gaussian Assumption · p. 3
  • The authors state that the holonomic motion model and isotropic uncertainty assumptions suffice for the use cases in their experiments.Source: C. Isotropic Gaussian Assumption · p. 3

V. METHODOLOGY

This section outlines how belief states evolve under the motion and observation models.

V. METHODOLOGY

  • The section outlines belief-state evolution under the motion and observation models.Source: §V · p. 3

A. Belief Dynamics

Belief dynamics use prediction and information-form correction, with point-cloud information incorporated into updates. The point-cloud model derives a planar information matrix from the ICP Hessian.

A. Belief Dynamics

  • The prediction step uses the motion model, and under the maximum-likelihood observation assumption the zero-innovation correction is expressed in information form.The notation distinguishes prior and posterior quantities, and the covariance is bounded isotropically using maximum eigenvalues.Source: A. Belief Dynamics · p. 3

B. Point-Cloud Observation Model

  • For point-cloud observations, the point-to-plane ICP Hessian approximates observation information even when alignment at the predicted state gives zero nominal residual.Source: B. Point-Cloud Observation Model · p. 3
  • Partitioning the ICP Hessian isolates xy translation in H_aa,k, and the marginal planar information matrix Ω_k assumes H_bb,k is nonsingular.Source: B. Point-Cloud Observation Model · p. 3

C. Continuous Motion Model Parametrization

The motion model interpolates uncertainty between discrete updates and derives the least reachable uncertainty for travel between states. Reachability then connects directly sampled beliefs while allowing uncertainty-increasing motion or partial observations and checking collision validity along each edge.

C. Continuous Motion Model Parametrization

  • Variance accumulates linearly between discrete updates, motivating continuous-time interpolation for arbitrary offsets.The interpolation assumes a fixed discrete update interval.Source: C. Continuous Motion Model… · p. 3
  • Maximum admissible velocity toward the target minimizes prediction steps and thus the uncertainty incurred during travel.This follows under the constant-velocity single-integrator model and uniform process-noise accumulation.Source: C. Continuous Motion Model… · p. 3
  • The resulting reachable-standard-deviation bound is tight when motion uses maximum velocity along the direct path.The bound follows by substituting travel time into the continuous-time variance model.Source: C. Continuous Motion Model… · p. 3

D. Belief State Reachability & Rewiring

  • The planner samples beliefs directly and accepts a sample when its uncertainty meets the reachable bound, using higher inflation or partial measurement when needed.Higher inflation subsumes alternatives such as increased process noise, shorter steps, or lower velocity; collision validity is checked at intermediate beliefs along each edge.Source: D. Belief State Reachability & Rewiring · p. 4

E. Belief State Steering

Steering searches nearby tree nodes for a reachable parent and validates the resulting connection. Collision and goal tests enforce probability-based clearance, while informed sampling uses a prolate hyperspheroid to approximate the region of potentially improving solutions.

E. Belief State Steering

  • Steering first tests the nearest node, then searches nearby nodes for a reachable parent, retaining the closest achievable uncertainty as fallback.If no exact target is reachable, it may apply a full observation to produce the closest achievable belief; the returned connection is accepted only if valid.Source: E. Belief State Steering · p. 4

F. Obstacle & Goal Region Interaction

  • A belief is collision-safe when signed obstacle clearance exceeds robot radius plus k_interσ, guaranteeing collision freedom with probability at least P_inter.The condition is evaluated at intermediate beliefs along a connection.Source: F. Obstacle & Goal Region Interaction · p. 4
  • A belief meets the goal condition when its clearance from the goal exterior exceeds robot radius plus k_interσ, at probability at least P_inter.Source: F. Obstacle & Goal Region Interaction · p. 4

G. Informed Sampling for Convex Goal Region

  • For a convex goal, the method seeks a single prolate hyperspheroid as an outer approximation of the informed region.The hyperspheroid is defined by two foci and a major-axis length.Source: G. Informed Sampling for Convex Goal… · p. 4

1) Approximate informed region:

The method approximates the informed region with one prolate hyperspheroid and tightens its major-axis length by minimizing the worst-case distance to the goal. Sampling uses this construction in the embedded belief space, but containment guarantees remain unestablished.

1) Approximate informed region:

  • The prolate hyperspheroid is used as an empirical outer approximation that provided sufficient informed-region samples in experiments.Establishing containment guarantees remains future work.Source: 1) Approximate informed region · p. 4
  • The approximation minimizes c(y), the worst-case distance from y to the convex goal set.Source: 2) Minimax tightening · p. 4

2) Minimax tightening:

  • The minimax construction minimizes the outer bound’s major-axis length, but not necessarily its volume.Source: 2) Minimax tightening · p. 5
  • The construction is sampled as an R3 prolate hyperspheroid in embedded belief coordinates, rejecting samples outside the true informed region or belief bounds and those with negative uncertainty.Clamping or reflecting negative uncertainties would produce non-uniform samples.Source: 2) Minimax tightening · p. 5

H. Informed Belief Localization Trees*

Informed BLT* and BLT* adapt RRT* structure to belief dynamics, with informed sampling added to the informed variant after a solution is found. Their edge costs can use either W2 distance across the full belief trajectory or ℓ2 distance over mean motion, while sharing belief-space feasibility constraints.

H. Informed Belief Localization Trees*

  • Informed BLT* adds cost-informed sampling after a solution is available, while BLT* follows the same adapted RRT* structure without that added step.Both adapt steering, connection validity, and rewiring to the belief dynamics.Source: H. Informed Belief Localization Trees* · p. 5
  • Under W2, edge cost accumulates distance along motion and observation updates, preserving the full belief-space trajectory.Source: H. Informed Belief Localization Trees* · p. 5
  • Under ℓ2, edge cost counts only configuration-space motion of the belief mean; uncertainty still determines feasibility, collision validity, and goal satisfaction.Parent selection and rewiring follow the selected objective while sharing belief-space feasibility constraints.Source: H. Informed Belief Localization Trees* · p. 5

I. Digital Twin Preprocessing

The digital-twin pipeline combines semantic labels and point-cloud geometry to produce a planning map and localization point clouds. It represents terrain height and occupancy continuously for planning queries.

I. Digital Twin Preprocessing

  • The pipeline produces a 2.5D planning map and localization point clouds, using semantic classes as a proxy for localization reliability.Classes designate geometry as traversable, non-traversable, or observable for localization.Source: §I · p. 5

1) Semantic Labelling:

  • Semantic labels are projected from segmented image views onto mesh faces and point-cloud points, then assigned by majority vote across views.Nearby points can inherit mesh-face votes, and users select classes retained for ICP localization.Source: 1) Semantic Labelling · p. 6

2) 2.5D Map Representations:

  • The 2.5D grid marks cells with traversable points free, cells with only non-traversable points as obstacles, and empty cells unknown; ties favor obstacles.Occupancy is computed on a grid finer than the 5 cm point-cloud resolution.Source: 2) 2.5D Map Representations · p. 6
  • Free-cell height is the mean observed elevation, while unobserved nearby cells inherit the nearest observed height; the signed distance field is positive in free space and negative inside obstacles.Unknown cells count as obstacles, and height and signed distance are queried continuously using bilinear interpolation.Source: 2) 2.5D Map Representations · p. 6

3) Integration with Planners:

The planner integration caches localization information at collision-free configurations for reuse during connection checks. Evaluation spans simulated maps and digital twins, with custom planar maps illustrating distinct route and information-gathering demands.

3) Integration with Planners:

  • ICP information is evaluated after a candidate configuration passes collision checking, stored with its tree node, and reused during later connection checks.The same caching is applied along RRBT graph edges; action-sampling planners often generate new configurations, making reuse less direct.Source: 3) Integration with Planners · p. 6

B. Simulated Planning Environments

  • Figure 4 orders the environments as Empty, Block, Narrow, and Random, marking the start, its covariance, and the goal; Table I and Table II report planner metrics under W2 and ℓ2.Table I includes mean first-solution cost and time, costs at 10 and 100 s, and success; Table II reports first-solution and later-cost metrics for Random maps.Source: Figure 4 · p. 6, Table I · p. 7, Table II · p. 7

A. Setup

  • Each of five planners ran for 100 s under ℓ2 and W2 objectives in 50 seeded trials per map, with matched seeds across planners.Runs were sequential, limited to 4096 MB, and monitored for objective value every 1 ms.Source: A. Setup · p. 6

B. Simulated Planning Environments

  • The evaluation covered 16 maps—three custom R2 maps, ten random R2 maps, and three photogrammetric digital twins—for 8000 planner runs.The total spans five planners, 50 trials, and two optimization objectives.Source: B. Simulated Planning Environments · pp. 6–7
  • The robot is modelled as a point with observation-noise standard deviation 0.1 m and uncertainty bounded by 5 m; propagation steps are limited to 1 m displacement.Collision checks occur at intervals of at most 1 m, with a 3σ collision margin.Source: B. Simulated Planning Environments · p. 7

1) Custom Maps in R2:

  • Empty offers a direct, observable route, Block has two solution classes with information gathering on the shorter route, and Narrow requires uncertainty reduction before corridor traversal.All three custom maps use a 5 m start uncertainty.Source: 1) Custom Maps in R2 · p. 7

2) Random Maps in R2:

The proposed planners generally find initial solutions faster and achieve competitive final costs, with the clearest advantages in larger digital twins. Informed sampling improves cost reduction when it can exclude a substantial part of the sampling domain.

2) Random Maps in R2:

  • The evaluation used ten random maps, each with 75 obstacles and 75 measurement regions in a 100 × 100 m workspace.Measurement-region dimensions were sampled uniformly from m.Source: 2) Random Maps in R2 · p. 7

3) Digital Twins:

  • The three digital twins vary in size and observable geometry, with Rural especially sparse and Office the largest.Simulated point-cloud observations have a 20 m radius; observable classes differ across maps.Source: 3) Digital Twins · p. 7

C. Baseline Standardization

  • For consistent baseline comparisons, all planners used the xy marginal covariance for belief representation, propagation, and collision checking, with point-cloud information matrices added.This matches the paper’s open-loop setting without a feedback controller.Source: C. Baseline Standardization · p. 7

D. Results and Discussion

  • The proposed planners found initial solutions fastest on every map except Narrow, while Informed BLT*’s mean final cost was within 1.34% of the best planner’s on random maps.They did not consistently attain the lowest random-map cost after 100 s.Source: D. Results and Discussion · p. 8
  • Informed sampling reduced median solution cost faster on Campus and improved results on random maps, where the informed region often occupied a smaller share of the map.Its effect depends on how much of the original sampling domain can be excluded.Source: D. Results and Discussion · p. 8

VII. CONCLUSION & FUTURE WORK

  • BLT* variants found solutions in every digital-twin trial, while some baselines had substantial failures in larger environments with more computationally expensive point-cloud observations.The conclusion describes the approach as scalable in these digital-twin experiments.Source: §VII · p. 8
Loading 2610.01972v1…