BOWConnect: Parallel Bayesian Optimization over Windows with Learned Local Cost Maps for Sample-Efficient Kinodynamic Motion Planning

Abstract: This paper presents BOWConnect, a bidirectional parallel kinodynamic motion planner that addresses three fundamental limitations of existing sampling-based methods: sample inefficiency in high-dimensional state spaces, unreliable cost heuristics under dynamic constraints, and poor performance in narrow passage environments. Unlike classical planners that rely on random control sampling and geometric distance heuristics, BOWConnect integrates Bayesian Optimization over Windows (BOW) as a learning-based steering function within a parallel tree-based exploration framework, enabling each worker to learn local cost maps and constraints to guide sampling toward dynamically feasible and collision-free controls. A bidirectional architecture simultaneously grows forward and backward trees from the start and goal regions in parallel threads, with a spatial hashing mechanism enabling fast connection queries and a boundary value problem solver generating kinodynamically consistent bridge trajectories. Extensive evaluations across ten benchmark environments demonstrate that BOWConnect achieves 100\% success while delivering the fastest or near-fastest planning time in complex scenarios, including narrow passages and non-convex spaces where state-of-the-art planners fail or degrade substantially. Real-world deployment on a ground vehicle and a quadrotor confirms real-time planning with no collisions. Videos of real-world and simulated experiments, high-resolution versions of the figures, and the open-source code are available at https://bow-connect.github.io/.

• The paper introduces a parallel bidirectional planner that integrates Bayesian optimization with learned local cost maps to efficiently overcome narrow passage challenges.
• The paper demonstrates significant improvements in planning speed, trajectory smoothness, and success rates compared to classical SBMPs under stringent dynamic constraints.
• The paper validates BOWConnect’s robust performance in both simulated and real-world environments, successfully adapting to various robot models including unicycle, bicycle, and quadrotor.

BOWConnect: Parallel Bayesian Optimization over Windows with Learned Local Cost Maps for Sample-Efficient Kinodynamic Motion Planning

Motivation and Contributions

BOWConnect advances kinodynamic motion planning by addressing the sample inefficiency and heuristic limitations of contemporary SBMPs in high-dimensional and highly constrained environments. Traditional planners suffer from slow convergence and suboptimal performance in narrow passages due to reliance on uniform control sampling and geometric heuristics. BOWConnect integrates a parallel bidirectional architecture with online Bayesian Optimization over Windows (BOW) and local cost map learning, enabling efficient, collision-free, and dynamically feasible trajectory generation. The algorithm establishes multiple independent worker threads, each growing motion trees from diverse start and goal states using GP-guided acquisition, and leverages spatial hashing for rapid inter-tree connection discovery.

The core contributions include:

• Embedding constrained Bayesian optimization as the steering function, learning local reward and feasibility cost maps in continuous state-control spaces.
• A parallel bidirectional tree expansion strategy, outperforming serial and unidirectional planners especially in environments with narrow passages and nonholonomic constraints.
• Integration of spatial hashing and boundary value problem (BVP) solvers for efficient kinodynamic connections between trees.
• Comprehensive empirical validation on both simulated and real-world ground and aerial platforms, with benchmark studies demonstrating robust performance under varying motion models.

Algorithmic Framework

BOWConnect's workflow initiates by sampling multiple collision-free start and goal regions, each with randomized heading orientations, to mitigate connection ambiguity in $\mathcal{X}$. Each worker maintains a tree and utilizes BOW for steering. At each iteration, the worker samples a target state, queries the nearest tree node, and invokes Bayesian optimization over the local control window. The reward is defined as the negative distance to target position for collision-free rollouts; constraints are encoded via collision predicates. Gaussian Process models for both reward and feasibility are updated online, and acquisition is computed as:

$$\alpha(\mathbf{u}) = [\mu_r(\mathbf{u}) - \kappa \sigma_r(\mathbf{u})] \times P_{\text{feas}}(\mathbf{u})$$

Spatial hashing enables rapid candidate pair identification across trees, and a sequential feasibility check (including heading and distance constraints) precedes BVP solving. BVP utilizes feedback control primitives to generate kinodynamically consistent bridges, and the minimum-length trajectory is prioritized for merging.

Figure 1: Parallel BOWConnect tree growth with multi-threaded exploration and GP-guided controls in a cluttered planar environment.

Empirical Evaluation

Ground Vehicle Benchmarks

Evaluation across six canonical UGV environments shows BOWConnect is consistently the only planner to achieve 100% success with both Unicycle and Bicycle models, in environments where baselines degrade or fail entirely. BOWConnect demonstrates strong numerical results, often solving problems in $0.019$~s to $2.133$~s, compared to $6.886$ to $42$~s for classical planners. In Bugtrap, BOWConnect solved in $0.035$~s (Unicycle) and $0.021$~s (Bicycle), with shorter and smoother trajectories compared to all baselines. Under stricter nonholonomic constraints (Bicycle), EST and KPIECE failed to generate solutions in any scenario, while RRT and SST dropped below $50\%$ success in narrow passage and nonconvex environments; BOWConnect maintained full success.

BOWConnect produces shorter trajectories in environments with complex connectivity (e.g., Nonconvex and Forest), resulting directly from its superior global exploration and backward propagation. Average jerk values are markedly lower than for SBMPs, confirming the smoothness of GP-guided controls.

Quadrotor UAV Benchmarks

BOWConnect achieves 100% success in all tested environments, with planning times consistently under $0.12$~s. SST, EST, and KPIECE require $$\alpha(\mathbf{u}) = [\mu_r(\mathbf{u}) - \kappa \sigma_r(\mathbf{u})] \times P_{\text{feas}}(\mathbf{u})$$0–$$\alpha(\mathbf{u}) = [\mu_r(\mathbf{u}) - \kappa \sigma_r(\mathbf{u})] \times P_{\text{feas}}(\mathbf{u})$$1~s. BOWConnect yields the lowest jerk and shortest trajectories in several environments, confirming efficacy in high-dimensional systems.

Real-World Validation

Physical deployment on a differential-drive UGV and a 6-DOF quadrotor in cluttered laboratory environments demonstrates real-time planning ($$\alpha(\mathbf{u}) = [\mu_r(\mathbf{u}) - \kappa \sigma_r(\mathbf{u})] \times P_{\text{feas}}(\mathbf{u})$$2~s) and sim-to-real consistency, with trajectories executed with no collisions or tracking instabilities. Multiple trials confirm robustness and kinodynamic feasibility.

Figure 2: BOWConnect kinodynamic planning and execution in a real-world indoor UGV scenario, illustrating sim-to-real consistency.

Figure 3: Real-world quadrotor trajectory execution using BOWConnect in a 3D cluttered workspace.

Theoretical and Practical Implications

The parallelism and local learning-based steering of BOWConnect overcome the curse of dimensionality and the sample inefficiency plaguing SBMPs. The GP surrogate's uncertainty-aware acquisition function prevents local minima in narrow or infeasible regions, enabling rapid convergence to collision-free, dynamically feasible paths. Spatial hashing enables scalable connection queries, making the method amenable to further hardware acceleration and distributed computation.

BOWConnect's architecture is agnostic to robot dynamics, evidenced by consistent success in both unicycle/bicycle models and high-dimensional quadrotor planning. It challenges the dominant paradigm of geometric heuristics and uniform control sampling, demonstrating that local cost map learning and parallelism yield superior exploration and planning in continuous state/control spaces.

Prospects for Future Research

Potential future directions include:

• Adaptive worker allocation and dynamic task partitioning to optimize computational resources.
• Online uncertainty modeling for robust planning in non-static and stochastic environments, including moving obstacles.
• Integration with task-level planning and hierarchical architectures.
• Extension to multi-agent kinodynamic planning, leveraging coordinated local cost map learning and constrained Bayesian optimization.

Conclusion

BOWConnect represents a substantive advancement in kinodynamic motion planning, introducing a parallel bidirectional framework with GP-guided control sampling and spatially efficient connection strategies. The algorithm delivers strong empirical performance, consistently achieving full success rates and rapid planning in environments and robot modalities where prior SBMPs fail. Practical deployment has demonstrated real-time feasibility on both ground and aerial robots with sim-to-real consistency. The approach is theoretically significant, shifting the focus from geometric heuristics to learning-based steering and scalable parallelism, with rich avenues for future extension in dynamic and multi-agent domains (2606.27292).