Terrain-Aware Spatio-Temporal Trajectory Planning for Ground Vehicles in Off-Road Environment

Autonomous navigation of ground vehicles in off-road environments with uneven terrain is crucial for various applications. This paper proposes a spatio-temporal trajectory optimization framework that considers off-road terrain. Initially, the framework assesses the static and dynamic stability of the vehicle by constructing a multi-layer map and employing a vehicle pose estimation method on uneven terrain. Subsequently, the optimal coarse trajectory is searched in the spatio-temporal state space via dynamic programming, incorporating a cost function designed to address vehicle stability metrics quantitatively. Finally, the trajectory planning problem is formulated in terms of optimal control, introducing the terrain curvature cost term into the objective function, and iteratively solving and updating the speed limit under terrain constraints. The resulting trajectories demonstrate smoothness, high quality, and adherence to the safety requirements imposed by the terrain. Extensive testing on public datasets and real-world experiments validates our method, demonstrating its capability to generate more traversable trajectories with higher achievable velocity compared to existing approaches.