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.
Figures & tables
Fig. 1 : A wheeled robot navigating on off-road terrain. Safe path planning requires balancing a key trade-off: avoiding high-uncertainty regions caused by terrain-induced occlusions (highlighted in blue) while simultaneously steering clear of non-traversable areas, such as the pole on the left. The core idea behind our work is modeling terrain occlusion as coverage-induced epistemic uncertainty, which is propagated to the vehicle pose and contact points variable, and finally to the downstream planning cost. In this figure, the Grey surface represents our terrain reconstruction based on the partial point-cloud based observation of the terrain.
Symbol
Description
xk
Planar query state (xk,yk,αk) at time step k .
pk
Planar position (xk,yk) at time step k .
ξk , ξk∗
Terrain-dependent best-fit state: elevation, pitch, roll, and candidate wheel–terrain contact points, and its converged least-squares estimate.
fλ(x,y)
Terrain elevation model parameterized by Fourier coefficients.
λ,λ∗
Terrain parameter vector and its converged least-squares estimate.
Fig. 2 : Model-based core of the proposed framework. (a) Partial terrain observations are converted into uncertain terrain parameters and propagated through the pose/contact model. (b) CEM evaluates sampled trajectories using the resulting nominal states and uncertainty. The Flow Matching module introduced subsequently provides a warm start for terrain refinement without affecting the model-based evaluation shown here.
Fig. 3 : Likelihood-scaled terrain uncertainty from partial observations. (a) On-board sensing may fail to observe parts of the terrain because of occlusion. (b) The nominal Fourier fit can be unreliable where spatial observations are sparse or missing. Two sample fits are shown (blue and green), matching at points where data are available (red) and diverging in unseen areas. (c) The regularized inverse-Hessian variance is larger in directions that are weakly constrained by the observed point cloud.
Fig. 4 : Kinematic model of the wheeled robot, illustrating the global, body-fixed, and translating reference frames used for loop closure.
Fig. 5 : Terrain Parameter Network. A CNN encodes the Digital Elevation Model (DEM) constructed from the current point cloud. Conditioned on this encoding and the flow timestep, the DiT represents a velocity field whose ODE transports a Gaussian initial state to a Fourier-coefficient warm start; Levenberg–Marquardt subsequently refines this output. Abbreviations: Conv – convolutional layer; BN – batch normalization; ReLU – rectified linear unit; MaxPool – max pooling; GAP – global average pooling; FC – fully connected layer; pos embed – positional embedding; DiT – Diffusion Transformer; ModLN – modulated layer normalization; MHA – multi-head self-attention; MLP – multi-layer perceptron; SinCos – sinusoidal timestep encoding; ODE – ordinary differential equation.
Fig. 6 : Comparison with the propagated-uncertainty-scoring ablation. Both planners use the same CEM initialization. a) Planned trajectories overlaid on the terrain-uncertainty map. The cunc=0 planner (magenta) traverses the high-uncertainty region formed by occlusion behind the bump (blue rectangle), whereas the uncertainty-aware planner (black) routes around it. Blue circles identify the craters. b) Closed-loop Gazebo executions: the cunc=0 planner becomes immobilized, while the uncertainty-aware planner reaches the goal.
Fig. 7 : Second comparison with the propagated-uncertainty-scoring ablation. Both planners use the same CEM initialization. a) The cunc=0 trajectory (magenta) crosses a high-uncertainty, crater-adjacent region, while the uncertainty-aware trajectory (black) avoids the marked regions. b) In closed-loop Gazebo execution, the cunc=0 planner falls into a crater, whereas the uncertainty-aware planner reaches the goal.
Fig. 8 : Qualitative comparison in Scene 1. (a) Planned trajectories overlaid on ground-truth terrain points (green) and partial LiDAR observations (red), with a dashed blue circle and rectangle marking the crater and occluding bump. (b) Closed-loop Gazebo executions: the proposed method and GP-navigation reach the goal, whereas Bi-level-opt falls into the crater. (c) Planned trajectories overlaid on the proposed terrain-uncertainty map. (d) GP predictive-variance map with the GP-navigation path. Dashed yellow circles highlight spatially corresponding high-uncertainty regions in (c) and (d).
Fig. 9 : Qualitative comparison in Scene 2. (a) Planned trajectories overlaid on ground-truth terrain points (green) and partial LiDAR observations (red); dashed annotations mark the bump–crater occlusion. (b) Closed-loop Gazebo executions: the proposed planner and GP-navigation reach the goal, while Bi-level-opt becomes immobilized on the bumpy terrain. (c) Planned trajectories overlaid on the proposed terrain-uncertainty map. (d) GP predictive-variance map and GP-navigation path. Dashed yellow circles highlight corresponding high-uncertainty regions in (c) and (d).
Fig. 10 : Qualitative comparison in Scene 3. (a) Planned trajectories overlaid on ground-truth terrain points (green) and partial LiDAR observations (red). (b) Closed-loop Gazebo executions: the proposed planner avoids both marked hazards and reaches the goal; GP-navigation falls into the crater marked in blue, while Bi-level-opt falls into the crater marked in purple. (c) Planned trajectories overlaid on the proposed terrain-uncertainty map. (d) GP predictive-variance map and GP-navigation path. Dashed yellow circles highlight corresponding high-uncertainty regions in (c) and (d).
Fig. 11 : Synthetic terrains used for quantitative benchmarking. a) Terrain 1. b) Terrain 2. c) Terrain 3. d) Terrain 4. e) Terrain 5. f) Terrain 6.
Terrain
Method
Runs
Failures
Failure Rate (%)
Roll ( ∘ )
Pitch ( ∘ )
RMS
Max
RMS
Max
Terrain 1
Proposed
30
1
3.33
3.962
28.068
5.419
29.159
Bi-level-opt
30
1
3.33
6.440
31.468
6.313
34.315
GP-navigation
30
2
6.67
2.523
21.332
6.954
38.693
Ablation ( cunc=0 )
30
2
6.67
4.905
28.588
5.991
35.009
Terrain 2
Proposed
30
12
40.00
4.708
31.927
2.581
33.775
TABLE II : Simulation Statistics Across Terrains. A run is classified as a failure if the vehicle does not reach the goal within the allotted time, becomes immobilized, or exceeds a roll or pitch magnitude of 40∘ . The planner formulation represents pose angles in radians internally; roll and pitch are converted to degrees here for ease of interpretation. The RMS and Max values are taken from successful runs only. “Ablation” sets cunc=0 .
Fig. 12 : Illustrative Clearpath Husky execution near a coverage-induced blind spot. (a) Outdoor scene and robot execution. (b) Planned trajectory (black) and tracked trajectory (magenta) overlaid on the partial point cloud. The dashed cyan ellipse marks the region occluded by the tree. (c) Fitted terrain and point-cloud observations. (d) Coverage-induced terrain-uncertainty map. The selected route remains in the better-observed corridor and routes around the occlusion.
Fig. 13 : Illustrative Clearpath Jackal execution with varying observation support and modeled terrain traversability. (a) Outdoor scene and robot execution. (b) Planned trajectory (black) and tracked trajectory (magenta) overlaid on the partial point cloud. (c) Fitted terrain and point-cloud observations; the direct start–goal route crosses the elevated grassy region. (d) Coverage-induced terrain-uncertainty map. The direct route is observed but has a higher modeled surface-normal cost; the selected route moves toward the boundary of the observed footprint, where this cost is lower, before turning toward the goal.
Method
Fitting RMSE (mm)
Time (ms)
Network Prediction
1.99
3
One-Step Refinement
1.93
14
Nonlinear Solver
1.93
43
TABLE III : Terrain-parameter prediction performance averaged over 319 spatially disjoint held-out local point-cloud patches. One Flow Matching sample is used per patch, and computation time is measured on an NVIDIA RTX 5090 after JIT compilation. Lower values are better.
Symbol
Role
Value
Unit
wκ
Curvature-cost weight
0.01
m2
wa
Acceleration-cost weight
10.0
s4m−2
wn
Surface-normal-cost weight
10.0
dimensionless
wp
Pose-cost weight
10.0
rad−2
ρn
Normal-uncertainty penalty coefficient
1.0
dimensionless
ρp
Pose-uncertainty penalty coefficient
1.0
dimensionless
TABLE IV : Planner weights, penalty coefficients, and numerical stabilizers.
Category
Required setting
Value
Vehicle
Wheelbase l , width w , and wheel offsets hi
l=0.210 m, w=0.272 m, hi=0.260 m ( i=1,…,4 )
CEM sampling
Initial coefficient mean and covariance
boundary-consistent straight-line Bernstein coefficients; empirical covariance of Bernstein fits to 100 straight-line paths with smoothness-shaped lateral noise, +10−3I
fixed-step explicit midpoint; 16 uniform steps on [0,1] ; batched by vmap
TABLE VI : Flow Matching training and inference details.
Outcome (complete/ablation)
S/S
S/F
F/S
F/F
Scenarios
108
38
10
24
TABLE VII : Paired outcomes for the complete framework and the cunc=0 ablation. The complete framework outcome is listed first; S and F denote success and failure, respectively.
Autonomous driving in off-road environments presents significant challenges due to the dynamic and unpredictable nature of unstructured terrain. Traditional kinodynamic models often struggle to generalize across diverse geometric and semantic terrain types, underscoring the need for real-time adaptation to ensure safe and reliable navigation. We propose VertiAdaptor (VA), a novel online adaptation framework that efficiently integrates elevation with semantic embeddings to enable terrain-aware kinodynamic modeling and planning via function encoders. VA learns a kinodynamic space spanned by a set of neural ordinary differential equation basis functions, capturing complex vehicle-terrain interactions across varied environments. After offline training, the proposed approach can rapidly adapt to new, unseen environments by identifying kinodynamics in the learned space through a computationally efficient least-squares calculation. We evaluate VA within the Verti-Bench simulator, built on the Chrono multi-physics engine, and validate its performance both in simulation and on a physical Verti-4-Wheeler platform. Our results demonstrate that VA improves prediction accuracy by up to 23.9% and achieves a 5X faster adaptation time, advancing the robustness and reliability of autonomous robots in complex and evolving off-road environments.
Tong Xu, Chenhui Pan, Aniket Datar +1
Department of Computer Science, George Mason University
Kinodynamic planning for car-like vehicles on uneven terrain requires simultaneously optimizing competing objectives such as path efficiency and pose stability. This work presents an adaptive epsilon-constraint method integrated into a Model Predictive Control (MPC) framework, where the epsilon bounds are dynamically adjusted based on terrain descriptors to explore the Pareto front in real time. To capture vehicle-terrain dynamics, we develop a semi-parametric model combining analytical vehicle dynamics with a Sparse Gaussian Process (SGP) trained on the same terrain descriptors. The proposed epsilon-MPC is evaluated against MPPI and GAKD baselines, achieving a 94% navigation success rate while reducing maximum orientation deviation by 24% and improving multi-objective trade-off quality by 23%.
Laborat´orio de Engenharia de Sistemas e Rob´otica, Universidade Federal da Para´ıba (UFPB), Jo˜ao Pessoa, Para´ıba, 58055-000, Brazil · School of Computer Science, University of Lincoln, Lincoln, United Kingdom
Navigation world models plan by foresight, predicting the future that each candidate action sequence produces and selecting the best, rather than mapping observations to actions directly. Unlike urban settings where a predicted scene is a sufficient proxy, off-road navigation hinges on the robot--terrain interaction, so the prediction must cover not only what the camera will see but what the robot will feel. However, existing scene-focused models do not predict how much the robot will slip, tilt or shake along a planned trajectory. Proprioception captures these dynamics directly and, when used as input, improves the prediction of the physical future. We present Feel-WM, the first off-road navigation world model that conditions on proprioception and predicts what the robot will feel alongside what the camera will see. The physical future takes the form of a future proprioceptive state and a failure risk, both learned from the robot's own experience without human labels. The planner rolls out the physical future alongside the scene and weighs the predicted failure risk against goal similarity in a separable score. Experiments on real off-road data and in simulation demonstrate that Feel-WM outperforms visual-only navigation world models in open-loop planning and closed-loop rough-terrain navigation across wheeled and legged platforms. Deployed on a Husky on mountain trails, Feel-WM plans onboard, predicts rough ground ahead and steers around it, completing courses that an end-to-end policy fails.
E-In Son, Dong-Wook Kim, Ji-Hoon Hwang +4
Department of Electrical & Computer Engineering, Seoul National University, Republic of Korea