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

Can sampling-based belief-space planning scale to large outdoor digital twins with point-cloud observations while using a 2-Wasserstein path objective and still produce fast initial solutions and competitive convergence?

UGVs in mapped environments need paths that keep localization uncertainty low enough to satisfy probabilistic collision and goal constraints. Existing sampling-based belief-space planners propagate beliefs or sample actions and repeatedly evaluate observation information, which is computationally expensive with point-cloud observations and causes failures or long solution times in large digital twins.

Earlier graph/tree belief-space planners such as BRM, FIRM, CS-BRM, RRBT and IBBT, and direct belief-space planners such as Belief-A, B-RRT and B-SST, propagated Gaussian beliefs over sampled controls or nominal trajectories. They typically separate W2 proximity from trajectory cost and re-evaluate observation information along every new candidate trajectory. In this paper's baselines, B-RRT and B-SST had 0% success on the large Office digital twin, and RRBT solved only 24% of trials by 100 s.

BLT* samples isotropic Gaussian belief states in an isometric 3D embedding [x, y, √2σ] and uses W2 as both metric and path cost. It derives a closed-form lower bound on reachable σ under a holonomic single-integrator model, enabling direct belief-space steering and rewiring without per-edge control sampling; it caches point-cloud ICP Hessian information at nodes for reuse. Informed BLT* adds a single prolate hyperspheroid sampling region for a convex goal. The digital-twin pipeline labels reconstructed geometry with SAM3, selects stable classes for localization, builds a 2.5D signed-distance/height map, and uses point-to-plane ICP information matrices as observations.

Evaluation covered 16 maps (3 custom planar, 10 random planar, 3 photogrammetry digital twins), 5 planners, 50 seeds per map, and 2 objectives, totaling 8000 planner runs. Robot parameters were u_max=10 m/s, Δτ=0.1 s, σ_r=0.1 m, and a 3σ collision bound; digital twins used 20 m lidar-range point clouds and a 0.25 m robot radius. First-solution times: on Empty, BLT* averaged 0.12 ms versus 2.51–4.11 ms for baselines; on Campus, 0.09 s versus 2.38–5.71 s; on Office, only BLT* variants and RRBT solved, with BLT* at about 3.4 s versus 75.35 s for RRBT, while B-RRT and B-SST had 0% success. On Narrow, B-SST (6.15 ms) beat BLT* (19.19 ms), an exception. Final cost: on the 10 Random maps, Informed BLT* mean 100 s W2 cost was 66.40 m, within 1.34% of the lowest baseline (B-SST at 65.52 m) and far below B-RRT at 119.12 m. On Campus, Informed BLT* final cost was 102.70 m versus 141.86–182.30 m for baselines; on Office it was 445.92 m versus 502.70 m for RRBT; on Rural it was close but not lowest (130.80 m versus 125.21 m for RRBT).

A stated limitation is that optimality and informed-region containment guarantees are future work; the isotropic Gaussian bound is conservative and may reject feasible plans. The motion model is holonomic and 2D/2.5D, and collision checks are open-loop without feedback control. Evaluation is simulation-only, with no physical robot trials; digital twins are three static environments. The paper does not state a code release. The method depends on reliable semantic segmentation and ICP localization, so dynamic objects, vegetation, or reconstruction artifacts could degrade performance.