Occlusion-Aware, Quasi-Static, Stability-Oriented Trajectory Planning on Uneven Terrain

2026-09-30

AI Summary

This study addresses the challenges of epistemic uncertainty caused by terrain occlusion and vehicle stability planning in unstructured off-road environments by proposing an end-to-end differentiable framework. Methodologically, it employs fixed-feature Fourier representations to model occlusion uncertainty, propagating it into trajectory optimization via implicit differentiation. A nonlinear least-squares contact model is combined with the cross-entropy method (CEM) to solve for stable trajectories, while Flow Matching is introduced to accelerate terrain fitting and provide warm starts. Experimental results demonstrate that the proposed approach reduces the failure rate to 18.9% across six types of synthetic terrains, significantly outperforming baselines. Furthermore, hardware validation conducted in six real-world outdoor scenarios confirms that the framework effectively balances planning accuracy with real-time performance.

Abstract

Autonomous navigation in unstructured off-road environments requires reasoning about both vehicle--terrain interaction and environmental unknowns. We propose a model-based framework for generating quasi-static, stability-oriented reference trajectories for rigid, non-articulated four-wheeled vehicles on highly uneven terrain. Our work makes three primary contributions. First, we model blind spots caused by terrain occlusion as coverage-induced epistemic uncertainty in a fixed-feature Fourier terrain representation, quantified through a regularized inverse-Hessian estimate. Second, we propagate this uncertainty through the Nonlinear Least-Squares (NLS) pose/contact model using implicit differentiation and incorporate the resulting pose, contact-point, and per-wheel surface-normal uncertainty terms into trajectory optimization based on the Cross-Entropy Method (CEM). Third, we introduce a Flow Matching model that warm-starts terrain fitting, and we evaluate its fitting-accuracy--latency trade-off while retaining model-based refinement. Across six synthetic terrains with 30 matched start--goal pairs per terrain, the complete framework produced an observed failure rate of 18.9%, compared with 46.1% and 41.7% for two representative baselines and 34.4% for an ablation that removed the propagated-uncertainty scoring. Hardware evaluations span six distinct outdoor environments, with two representative executions presented in the paper and four additional executions included in the supplementary video. The evaluation also reports the accuracy--latency trade-off for the Flow Matching warm start.

Problem

• Autonomous navigation
• Uneven terrain
• Trajectory planning
• Occlusion uncertainty
• Vehicle stability

Innovation

• Trajectory Planning
• Epistemic Uncertainty
• Implicit Differentiation
• Cross-Entropy Method
• Flow Matching