Hierarchical Terrain-Aware Navigation via Bayesian-Guided RRT* and Flat-Corridor CiLQR

Abstract

Autonomous navigation in unstructured, off-road environments remains a critical challenge due to complex terrain and limited prior knowledge. This letter presents a hierarchical framework to generate paths across locally flat regions and optimize spatio-temporal trajectories under vehicle kinodynamics. The first step employs a terrain-aware RRT* that integrates Bayesian-learning-guided sampling with a directional reachability index, enabling efficient exploration in complex environments while avoiding dead-end traps. The resulting path explicitly accounts for local roughness and slope. The second step applies a constrained iterative linear quadratic regulator (CiLQR) trajectory optimizer that refines the path within a dynamic flat-corridor, where terrain risk is embedded into the optimization. The optimizer enforces vehicle stability constraints and adaptively regulates speed based on terrain risk and curvature, yielding dynamically feasible trajectory. Extensive simulations in off-road scenarios demonstrate that the proposed framework outperforms state-of-the-art methods, producing feasible trajectories with up to 34 % lower vehicle jolt. Furthermore, real-world tests on a 40-ton articulated truck in the Alxa Desert validate the framework's robustness and practical effectiveness.