Optimal Kinodynamic Motion Planning Through Anytime Bidirectional Heuristic Search with Tight Termination Condition

Abstract: This paper introduces Bidirectional Tight Informed Trees (BTIT*), an asymptotically optimal kinodynamic sampling-based motion planning algorithm that integrates an anytime bidirectional heuristic search (Bi-HS) and ensures the \emph{meet-in-the-middle} property (MMP) and optimality (MM-optimality). BTIT* is the first anytime MEET-style algorithm to utilize termination conditions that are efficient to evaluate and enable early termination \emph{on-the-fly} in batch-wise sampling-based motion planning. Experiments show that BTIT* achieves strongly faster time-to-first-solution and improved convergence than representative \emph{non-lazy} informed batch planners on two kinodynamic benchmarks: a 4D double-integrator model and a 10D linearized Quadrotor. The source code is available here.

• The paper introduces BTIT, a kinodynamic motion planner that combines anytime bidirectional heuristic search with a MEET-style tight termination condition to ensure optimal meet-in-the-middle solutions.
• The paper establishes theoretical guarantees of asymptotic optimality and probabilistic completeness, validated across 4D and 10D robotic planning benchmarks.
• The paper demonstrates significant speedups—up to 8.1× faster than prior methods—by focusing on informed sampling and efficient dual-tree heuristic updates.

Optimal Kinodynamic Motion Planning via Anytime Bidirectional Heuristic Search with Tight Termination Condition

Introduction

This work presents Bidirectional Tight Informed Trees (BTIT), an asymptotically optimal kinodynamic sampling-based motion planning algorithm that integrates an anytime bidirectional heuristic search (Bi-HS) into the random geometric graph (RGG) framework. BTIT enforces a tight, efficient, and easy-to-evaluate termination condition that ensures both the meet-in-the-middle property (MMP) and solution optimality (MM-optimality), addressing limitations of existing MEET-style approaches in realistic kinodynamic settings.

Related Work

Sampling-based motion planning (SBMP), including PRM, RRT, RRT*, and their batch-wise variants such as BIT*, AIT, and EIT, is the de facto standard for high-dimensional robotic planning under differential constraints. Bidirectional search strategies promise an exponential reduction in expanded states by propagating searches from both start and goal configurations; however, ensuring optimal coordination between dual trees and enforcing minimal cost-to-come has been a persistent technical barrier. MEET [MEET] established a provably tighter (yet expensive) termination condition for Bi-HS. BLIT* introduced anytime, incremental, lazy Bi-HS in batch-wise SBMP but without MEET-style guarantees. BTIT isolates the effect of tight, MEET-like conditions, enabling on-the-fly, batch-wise termination in kinodynamic SBMP.

Algorithmic Framework

BTIT operates in the anytime batch-wise SBMP paradigm: (1) it samples batches of informed states, defining an implicit RGG over the pruned subset of the state space; (2) it executes a bidirectional, heuristic-guided search exploiting MEET-style tight termination; and (3) it iteratively updates the search until the planning time budget is exhausted. Three pillars facilitate efficiency:

1. Adaptive On-the-fly Heuristic Updates: The heuristic is dynamically refined using dual-tree cost-to-come information, strictly enforcing search front expansion only where solutions below the current incumbent are possible.
2. MEET-based Tight Termination Condition: The search is terminated both at the first intersection of the forward and backward frontiers and if any MEET-style criteria deem improvement impossible.
3. Informed Subset Sampling: Sampling focuses directly on regions able to improve the incumbent, reducing unnecessary exploration and focusing computation on the prolate hyperspheroid defined by current solution cost.

   Figure 1: Experimental environment for the double-integrator robot (DIR) in a 2D workspace, showcasing enclosures, narrow passages, and multi-modal topologies.

Each iteration, BTIT builds two search trees rooted at the start and goal, respectively, expanding connections via nearest-neighbor (NN) or fixed-radius strategies following RRT*/PRM* theory. Expansion propagates the lowest admissible-cost connections, updating parent relationships and cost-to-come, and terminates upon the first frontier intersection. Pruning and queue resets maintain search tractability as solution quality improves. The algorithm guarantees optimality with asymptotic convergence and probabilistic completeness.

Theoretical Properties

Two primary theorems are proven within the MEET analytic framework:

• Admissibility: Upon triggering the termination condition, BTIT returns the lowest-cost solution in the induced RGG for the sampled batch.
• Asymptotic Optimality & Probabilistic Completeness: As the sample set densifies, the returned solution cost converges almost surely to the global optimum, and a feasible solution is found whenever one exists.

Experimental Setup

BTIT is evaluated against BIT*, ABIT*, AIT*, and EIT* in OMPL. The environments include:

• 4D Double Integrator Robot (DIR): Planar position and velocity in a structured lab domain with dynamic feasibility constraints and multiple narrow passages (Figure 1).
• 10D Linearized Quadrotor (LQ): Position, velocity, attitude, and angular velocity in a 3D workspace with non-convex corridors and connectivity bottlenecks.

  Figure 2: The 10D linearized quadrotor (LQ) environment, highlighting complex, high-dimensional kinodynamic planning with non-convex passageways.

All planners employ an optimal kinodynamic controller as a steering function, and edge costs and heuristics are derived from exact optimal control solutions.

Empirical Results

In the DIR benchmark, BTIT achieves a median initial solution time 3.6× faster than ABIT* and 8.1× faster than BIT*. Notably, both EIT* and AIT* fail to achieve full coverage due to difficulties in narrow passages, obtaining only 89% and 70% success rates, respectively.

In the LQ (10D) benchmark, AIT* and EIT* frequently fail due to the amplified nearest-neighbor and edge evaluation costs inherent to high-dimensional spaces. BTIT finds the median initial solution 1.48× faster than ABIT* and 2.37× faster than BIT*.

Figure 3: Comparative performance between planners: solution cost and success rate as functions of time, illustrating BTIT's rapid convergence and reliability over 100 Monte Carlo trials for both DIR and LQ domains.

The results underscore the value of (1) truly tight batch-wise termination, (2) focused search via dynamic pruning, and (3) robust meet-in-the-middle front intersection detection. BTIT not only reliably finds initial solutions faster, but also converges more rapidly and consistently, highlighting the effects of the proposed termination condition on batch efficiency and search effectiveness.

Implications and Future Directions

The BTIT framework conclusively demonstrates that adopting tight, efficient-to-evaluate termination conditions in bidirectional, batch-wise heuristic search leads to significant practical speedups and enhanced reliability for kinodynamic motion planning in high-dimensional, challenging environments. The approach is cleanly modular, allowing for future integration with lazy edge evaluation, advanced informed sampling techniques, alternative explicit kinodynamic steering, and real-time replanning frameworks.

From a theoretical standpoint, BTIT strengthens the link between graph-based bidirectional heuristic search guarantees and sampling-based kinodynamic planners, suggesting a productive avenue for further cross-fertilization between discrete graph search algorithms (such as MEET, MM, and BLIT*) and continuous-space motion planning.

Conclusion

Bidirectional Tight Informed Trees (BTIT) represent a decisive advance in sampling-based kinodynamic motion planning by integrating a principled, efficient batch termination criterion into an anytime bidirectional heuristic search framework. Empirical performance in both moderate and high-dimensional kinodynamic domains establishes BTIT as a leading approach for fast, reliable, and asymptotically optimal batch-wise motion planning under differential constraints.

---

References

For full details, see "Optimal Kinodynamic Motion Planning Through Anytime Bidirectional Heuristic Search with Tight Termination Condition" (2604.11587).