PaperlayerView exact revision on arXiv

Source-linked AI summary

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

Amith Manoharan, Chinmay Mundane, Aayush Bahukhandi, K. Madhava Krishna, Karel Zimmermann, Arun Kumar Singh

arXiv:2609.39105v1cs.RO

TL;DR

Occluded terrain leaves multiple plausible surfaces, but planning needs to estimate how those unknowns affect vehicle pose and wheel contacts. This paper propagates terrain uncertainty through a model-based planner and uses Flow Matching to warm-start terrain fitting. Across six synthetic terrains, the complete framework had an 18.9% observed failure rate, below the two reported baselines.

  • Problem

    Terrain occlusion leaves multiple plausible interpolations, whose different elevations and slopes can change predicted vehicle pose and wheel contacts.

    Source: §I · p. 2

  • Method

    The framework estimates Fourier terrain-parameter uncertainty, propagates it through an NLS pose/contact model, and incorporates the resulting uncertainty into CEM trajectory costs.

    Source: §I · p. 3

  • Results

    18.9% aggregate observed failure rate across six synthetic terrains, versus 46.1% for Bi-level-opt and 41.7% for GP-navigation.

    Source: §VI · p. 18

  • Takeaways & Limitations

    Flow Matching reduces terrain-fitting computation while retaining analytical terrain refinement and downstream model-based CEM planning.

    Source: §VI · p. 18

    The stated scope is rigid, non-articulated four-wheeled vehicles operating under quasi-static conditions.

    Source: §I · p. 1, §VI · p. 17

Abstract

from arXiv · show

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.

I. Introduction

Off-road trajectory planning must account for how uneven terrain changes vehicle pose and wheel contacts, while occlusion leaves plausible terrain shapes uncertain. The framework propagates that uncertainty through a vehicle–terrain model into trajectory costs, addressing limitations of deterministic pose prediction and map-level traversability scores.

I. Introduction

  • Occluded regions admit multiple plausible terrain interpolations, which can predict different wheel-contact heights, surface normals, and vehicle attitudes.A small blind spot beneath one wheel can affect predicted roll, pitch, or contact configuration more than a larger uncertain region away from the trajectory.Source: §I · p. 2
  • A regularized inverse-Hessian estimate represents uncertainty in fixed Fourier terrain parameters, assigning greater uncertainty to directions weakly constrained by occluded or sparse observations.The estimate is used as a local Gaussian surrogate for first-order uncertainty propagation.Source: §I · pp. 2–3
  • Implicit differentiation maps terrain-parameter uncertainty through the NLS pose/contact model to first-order pose, contact-point, and per-wheel surface-normal uncertainty terms for CEM trajectory costs.The objective combines uncertainty-aware pose and surface-normal costs with curvature and acceleration penalties.Source: §I · pp. 2–3
  • 18.9% aggregate observed failure rate, versus 46.1% for Bi-level-opt and 41.7% for GP-navigation, across six synthetic terrains.The ablation without propagated-uncertainty scoring had a 34.4% failure rate.Source: §I · p. 3

A. Pose Prediction for Vehicles on Uneven Terrain

  • Deterministic pose/contact predictors, including the NLS model, do not represent fitted-terrain uncertainty, whereas this framework propagates it into predicted vehicle states.High-fidelity contact simulation is described as too slow for the thousands of repeated placement queries used by online planning.Source: §I · pp. 1–2, A. Pose Prediction for Vehicles on… · p. 3

B. Traversability Analysis

  • Unlike map-level traversability methods, the planner evaluates terrain geometry and observation support through predicted body pose and four wheel contacts at each candidate configuration.Prior approaches use scalar costs from terrain patches or map cells, or traversability fields for path planning.Source: B. Traversability Analysis · pp. 3–4

C. Uncertainty-Aware Navigation

Prior uncertainty-aware planners often retain uncertainty as map-level risk, confidence, or mobility measures. This framework instead propagates terrain uncertainty through the predicted chassis pose and individual wheel contacts for each candidate configuration.

C. Uncertainty-Aware Navigation

  • Probabilistic mapping and exploration methods retain uncertainty in terrain maps or use it to guide observations, but do not propagate terrain-reconstruction uncertainty through a wheel–terrain pose/contact solution.The passage includes PTS-Map, risk-map planning, and belief-space or informative-sampling approaches [26]–.Source: C. Uncertainty-Aware Navigation · p. 4
  • GP-navigation predicts elevation and traversability from sparse data and masks cells whose elevation variance exceeds a threshold, independent of a candidate chassis’s wheel placement.The passage identifies it as the closest experimental baseline for sparse elevation observations.Source: C. Uncertainty-Aware Navigation · p. 4
  • Map-level risk and confidence methods do not estimate uncertainty in terrain-induced chassis pose or four wheel-contact locations, which this framework propagates through its interaction model.STEP assigns uncertainty to risk scores, while Yin et al. use mobility-failure probabilities.Source: C. Uncertainty-Aware Navigation · p. 4

D. Learning-Based Methods

Learning-based navigation uses learned outputs for terrain properties, traversability, policies, and computational acceleration. Here, Flow Matching has the narrower role of initializing terrain fitting while model-based refinement and trajectory evaluation remain in place.

D. Learning-Based Methods

  • Existing learning-based methods use learned predictions for vehicle attitude, rough-terrain control, traversability, elevation, uncertainty, or navigation policies.Their uncertainty remains at the perception, traversability, or policy output rather than being propagated through a three-dimensional pose and wheel-contact solution.Source: D. Learning-Based Methods · pp. 4–5
  • Flow Matching warm-starts terrain fitting, while Levenberg–Marquardt still refines terrain parameters against the current point cloud and CEM evaluates trajectories using model-based predictions.The learned module initializes terrain estimation; it does not replace the downstream planner.Source: D. Learning-Based Methods · p. 5

III. Methods

The method fits a terrain model from partial observations, propagates its uncertainty through vehicle pose and contact predictions, then evaluates sampled trajectories with CEM. A learned Flow Matching module warm-starts terrain refinement without changing this model-based evaluation.

III. Methods

  • The framework assumes a rigid, non-articulated four-wheeled vehicle on a single-valued elevation field moving slowly enough for quasi-static modeling.Slip and dynamic contact forces are not modeled.Source: A. Method Overview · p. 5
  • Least-squares fitting provides nominal Fourier terrain parameters, and a regularized inverse Hessian estimates their uncertainty as a local Gaussian surrogate rather than an exact Bayesian posterior.The estimate represents coverage-induced epistemic uncertainty.Source: A. Method Overview · p. 5
  • Implicit differentiation of converged NLS optimality conditions propagates terrain-parameter uncertainty into first-order uncertainty for the nominal pose and contact state.The NLS model predicts body elevation, pitch, roll, and candidate 3D contact points for each wheel.Source: A. Method Overview · p. 5
  • CEM evaluates sampled trajectories using curvature and acceleration penalties together with uncertainty-aware pose and per-wheel surface-normal costs.The costs account for nominal attitude and terrain-normal geometry, as well as uncertainty from incomplete terrain observations.Source: A. Method Overview · pp. 5–6, Figure 2 · p. 6
  • A conditional Flow Matching model predicts terrain parameters to warm-start refinement, while leaving downstream CEM evaluation unchanged.The figure distinguishes the model-based core from the subsequently introduced warm-start module.Source: A. Method Overview · p. 6

B. Uncertainty-Aware Representation of the Terrain

The section fits a continuous terrain surface from point-cloud elevations using fixed Fourier features, then uses fit curvature to estimate uncertainty where observations provide weak support. This produces pointwise latent-height uncertainty as a local, single-frame estimate.

Terrain fitting

  • Least-squares fitting uses retained, vehicle-frame point-cloud samples under a fixed homoscedastic Gaussian elevation-observation model.The observation scale is selected from converged-fit residuals and held constant across maps; the reported fitting RMSE is of order 10^-3 m.Source: B. Uncertainty-Aware Representation of… · pp. 6–7

Terrain representation

Online fitting

  • For instances with up to 35,000 retained observations, Flow Matching warm-starts terrain fitting, and one to two refinement iterations were empirically sufficient to recover converged parameters.Source: B. Uncertainty-Aware Representation of… · p. 7

Terrain uncertainty

  • A regularized inverse-Hessian covariance assigns greater parameter uncertainty to directions weakly constrained by the observed point cloud.The covariance uses the likelihood curvature and damping; parameter directions supported by many observations have smaller inverse curvature.Source: B. Uncertainty-Aware Representation of… · p. 7
  • Projecting parameter covariance through the terrain features estimates latent height variance, generally lower in dense observations and higher in occluded or sparse regions.The online planner uses the parameter covariance directly; a future noisy measurement would additionally include observation variance.Source: B. Uncertainty-Aware Representation of… · p. 7

Scope

  • The uncertainty estimate is local and single-frame; temporal belief updates, correlated sensor errors, and terrain-model mismatch beyond the inverse-Hessian approximation are outside the formulation.Source: B. Uncertainty-Aware Representation of… · p. 7

C. Uncertainty-Aware Pose and Contact-Point Prediction

The section predicts vehicle pose and wheel-contact estimates with an NLS model, propagates terrain uncertainty through its solution, and uses the resulting quantities in trajectory costs. The contact estimates are best fits, not guarantees of load-bearing contact.

Pose and contact prediction

  • The NLS model finds a best-fit pose and contact configuration from 16 residual components for 15 unknowns, but its contact estimates are not guaranteed load-bearing contacts.The solution minimizes loop-closure and terrain-gap discrepancies without requiring either residual group to vanish.Source: C. Uncertainty-Aware Pose and… · p. 8

Uncertainty propagation

  • Implicit differentiation of the converged NLS optimality conditions gives terrain-parameter sensitivities, which yield first-order marginal uncertainty estimates for predicted pose and contacts.The local sensitivity is defined when the stationary point is locally isolated and the relevant Jacobian is nonsingular.Source: C. Uncertainty-Aware Pose and… · pp. 8–9
  • Terrain parameters shared across time steps couple their uncertainty, while the planner uses per-step marginal estimates for efficient cost evaluation.Source: C. Uncertainty-Aware Pose and… · p. 9

Trajectory formulation

  • The planner uses a unicycle motion model; yaw follows the path tangent while moving and holds its last valid value below a speed threshold.A downstream path-tracking controller can perform stationary reorientation between the odometry yaw and the first moving path tangent.Source: D. Trajectory Optimization on Uneven… · p. 9
  • Trajectories have prescribed endpoint positions and planar velocities, and costs are accumulated over future samples with the measured initial state fixed.The implementation uses H = 100 samples at Δt = 0.2 s.Source: D. Trajectory Optimization on Uneven… · p. 9

Trajectory costs

E. Uncertainty-Aware Costs

The trajectory objective retains its nominal geometric costs and adds a penalty for uncertainty in predicted pose and surface normals. First-order Gaussian propagation makes these costs account for uncertainty from the terrain model.

E. Uncertainty-Aware Costs

  • The added uncertainty penalty covers both predicted pose and surface-normal effects of missing terrain information.The nominal geometric objective is retained alongside this separate penalty.Source: E. Uncertainty-Aware Costs · p. 10
  • The pose-uncertainty term makes the quadratic pose cost equal to its expectation under the first-order Gaussian approximation.The pose state is modeled with a marginal mean and covariance; the pitch and roll components and their covariance entries enter the quadratic cost.Source: 1) Uncertainty-Aware Pose Cost · p. 10
  • The normal-deviation uncertainty accounts for terrain parameters affecting normals both directly and indirectly through the contact-point solution.The normal-deviation vector's total derivative with respect to terrain parameters supports first-order covariance propagation.Source: 2) Uncertainty-Aware Surface-Normal Cost · p. 10
  • The additional normal-uncertainty correction remains sensitive when the nominal normal matches its reference but terrain-induced perturbations are uncertain.The normal covariance is propagated under a linearized Gaussian approximation; the correction complements the nominal normal cost.Source: 2) Uncertainty-Aware Surface-Normal Cost · pp. 10–11
  • At unit weights, nominal-plus-uncertainty pose and normal costs recover their first-order expected quadratic costs; larger weights add conservatism, while zero uncertainty penalty recovers the deterministic baseline.The nominal term penalizes geometrically difficult trajectories, while the uncertainty term discourages reliance on weakly observed terrain.Source: 2) Uncertainty-Aware Surface-Normal Cost · p. 11

F. Solution Process: Polynomial CEM

The planner represents candidate planar paths with degree-10 Bernstein polynomials and uses CEM to optimize their coefficients. Projection enforces endpoint position and velocity constraints, while trajectory evaluation uses the current terrain fit and propagated uncertainty.

F. Solution Process: Polynomial CEM

  • Degree-10 Bernstein polynomials parameterize the unicycle flat outputs, reducing the search to 11 coefficients per planar coordinate while enforcing endpoint position and velocity in coefficient space.A projection filter ensures every evaluated candidate satisfies the boundary conditions.Source: F. Solution Process: Polynomial CEM · p. 11
  • The Bernstein basis and its physical-time derivative matrices map 11 coefficients per coordinate to future positions and derivatives at the sampled horizon.The basis is evaluated row-wise at the future samples.Source: F. Solution Process: Polynomial CEM · p. 11
  • CEM maintains a Gaussian distribution over joint coefficient vectors and updates it from the lowest-cost candidates at each iteration.The derivative-dependent costs use the Bernstein position and derivative matrices.Source: F. Solution Process: Polynomial CEM · p. 11
  • CEM starts from a boundary-consistent straight-line mean and a covariance estimated from 100 noisy straight-line fits, regularized by 10^-3I.Terrain parameters and their uncertainty are computed once from the current point cloud and remain fixed during the search.Source: F. Solution Process: Polynomial CEM · p. 11
  • Each projected candidate is evaluated by solving the NLS pose/contact problem at every waypoint and computing the associated uncertainty terms.After the iterations, the planner returns the lowest-cost planar path and its terrain-surface representation.Source: F. Solution Process: Polynomial CEM · p. 11

IV. Flow Matching Warm Start for Terrain Fitting

A conditional Flow Matching model supplies a Fourier-coefficient warm start for terrain fitting. At deployment, it transports a Gaussian source through a learned velocity field, then Levenberg–Marquardt refinement produces the terrain parameters and uncertainty used by the planner.

IV. Flow Matching Warm Start for Terrain Fitting

  • The Flow Matching model is trained offline to generate a single initialization for local terrain-parameter refinement, reducing terrain-fitting latency.The Terrain Parameter Network uses a CNN to encode the DEM and a Diffusion Transformer to represent the conditional velocity field.Source: §IV · pp. 11–12, Figure 5 · p. 12
  • The network operates in a 2N-dimensional Fourier-coefficient space with frequencies fixed across terrain maps, so coefficient coordinates retain the same meaning across the training set.Training pairs each point-cloud DEM with a coefficient vector fitted offline.Source: §IV · p. 12
  • A Gaussian source state is transported by integrating the learned velocity field to produce a Fourier-parameter warm start.The model is conditioned on the DEM constructed from the current point cloud and the flow timestep.Source: §IV · p. 12
  • One to two Levenberg–Marquardt iterations refine the prediction against pointwise residuals, after which the refined parameters define the nominal surface and the regularized Hessian provides terrain uncertainty.These model-based quantities are used for pose/contact prediction and uncertainty-aware trajectory evaluation.Source: §IV · p. 12

V. Validation and Benchmarking

The validation section frames four questions covering uncertainty ablation, benchmark comparisons, hardware feasibility, and the Flow Matching accuracy–latency trade-off. It describes single-path closed-loop trials and the simulation and hardware evaluation setup.

V. Validation and Benchmarking

  • The evaluation is organized around uncertainty-removal effects, comparisons with two baselines, hardware feasibility, and the Flow Matching fitting-accuracy–latency trade-off.The listed questions point to separate simulation, benchmark, hardware, and warm-start evaluations.Source: §V · p. 12
  • The evaluation includes open-loop Python analyses and Gazebo closed-loop simulations, with synthetic terrains modeled in Blender.The implementation used JAX and JAXopt for GPU-accelerated computations.Source: A. Implementation Details · p. 13
  • Terrain fitting used a single point-cloud frame with LiDAR and depth-camera ranges capped at 10 m and 5 m, respectively; experiments fixed N = 100 Fourier frequencies and used a 20 s planning horizon.The CEM configuration used five iterations, 100 samples per iteration, and 25 elites.Source: A. Implementation Details · p. 13
  • The terrain-parameter network was trained on point-cloud data from Gazebo, the M2UD dataset, and the authors' Husky and Jackal hardware captures.The passage refers to an appendix table for additional dataset, training, and inference details.Source: A. Implementation Details · p. 13
  • Hardware demonstrations used Clearpath Husky and Jackal platforms with RealSense depth sensing, IMU-based state estimation, and an onboard laptop with an RTX 3090 GPU.Trajectories were tracked using a π-MPPI controller.Source: A. Implementation Details · p. 13
  • For each reported closed-loop simulation and hardware trial, the planner generated one terrain-aware reference path from the initial point-cloud frame, with no high-level replanning.A π-MPPI controller tracked the path in closed loop.Source: A. Implementation Details · p. 13

B. Baseline Methods and Evaluation Metrics

Across six synthetic terrains, the complete framework had a lower aggregate failure rate than both baselines and the propagated-uncertainty ablation. Evaluations also examined matched-scene behavior, outdoor hardware executions, and the Flow Matching warm start’s fitting-accuracy–latency trade-off.

B. Baseline Methods and Evaluation Metrics

  • Bi-level-opt uses a deterministic terrain fit and projected-gradient stability costs, while GP-navigation masks high-variance cells before RRT* planning.A failure includes not reaching the goal on time, immobilization, or roll or pitch exceeding 40°; RMS and peak attitude are reported for successful runs.Source: B. Baseline Methods and Evaluation… · p. 13

C. Effect of Propagated-Uncertainty Scoring

  • The uncertainty ablation comparison held CEM initialization and other planner components fixed, isolating the effect of propagated-uncertainty scoring on route choice.In the illustrated execution, the ablation became immobilized while the uncertainty-aware planner reached the goal.Source: C. Effect of Propagated-Uncertainty… · pp. 13–14

D. Qualitative Comparison with Baselines

  • Across three matched scenes, the proposed planner chose better-supported routes and reached the goal, while baselines entered weakly observed regions and sometimes failed.The scenes illustrate contrasting outcomes including immobilization and falls into unobserved terrain.Source: D. Qualitative Comparison with Baselines · pp. 14–15

E. Quantitative Results and Ablation Study

  • The cunc = 0 ablation had a 34.4% failure rate, 15.6 percentage points above the complete framework, and more failures on each terrain.It retained the same terrain representation, NLS predictor, nominal objective, CEM optimizer, start–goal pairs, and evaluation settings.Source: E. Quantitative Results and Ablation… · p. 16

F. Hardware Results

  • Hardware tests covered six outdoor environments; two representative executions show routes consistent with observation-support and vehicle–terrain interaction costs.Four additional executions are included in the supplementary video.Source: F. Hardware Results · p. 16

E. Quantitative Results and Ablation Study

G. Performance of the Terrain Parameter Network

  • Direct Flow Matching prediction took 3 ms versus 43 ms for the converged solver, with a 0.06 mm RMSE increase; one refinement iteration matched reported RMSE in 14 ms.The direct prediction yielded a 14.3× speedup; one iteration retained a 3.1× speedup.Source: G. Performance of the Terrain… · pp. 16–17, Table III · p. 17

VI. Conclusions

The conclusion presents an occlusion-aware, quasi-static planner that propagates terrain uncertainty into vehicle-interaction costs, reports lower synthetic failure rates than baselines, and demonstrates physical execution across six outdoor environments.

VI. Conclusions

  • A regularized inverse-Hessian estimate models coverage-induced terrain uncertainty, which implicit differentiation and first-order propagation carry through the NLS model to pose, wheel contacts, and surface-normal costs.CEM combines these uncertainty penalties with nominal geometric costs.Source: §VI · pp. 17–18
  • 18.9% aggregate observed failure rate, versus 46.1% for Bi-level-opt and 41.7% for GP-navigation, with uncertainty scoring included.The cunc = 0 ablation had a 34.4% failure rate, 15.6 percentage points above the complete framework.Source: §VI · p. 18
  • Hardware evaluations across six outdoor environments provided qualitative evidence of end-to-end execution; examples route around a tree occlusion and favor a lower surface-normal-cost path over a direct route.Two representative executions appear in the paper, with four more in the supplementary video.Source: §VI · p. 18, Figure 12 · p. 18, Figure 13 · p. 18
  • Direct terrain-parameter prediction was 14.3× faster than the converged nonlinear solver, with a 3.1% increase in fitting RMSE; one refinement iteration recovered the solver’s reported RMSE while remaining 3.1× faster.Flow Matching warm-starts terrain fitting without replacing model-based refinement.Source: §VI · p. 18
  • The framework assumes a rigid, non-articulated four-wheeled vehicle, a single-valued terrain height field, quasi-static interaction, and local first-order uncertainty propagation.The conclusion identifies dynamic stability, slip, larger repeated hardware campaigns, and active viewpoint selection as future work.Source: §VI · p. 18

Appendix Implementation Details

The appendix records planner and Flow Matching implementation settings, then compares the complete framework and uncertainty ablation on matched scenarios.

A. Planner Weights and Numerical Stabilizers

B. Flow Matching Network Details

  • Table VI reports the Flow Matching network’s training and evaluation settings, alongside subsection documentation of data provenance, numerical training configuration, and inference settings.Source: B. Flow Matching Network Details · p. 19

C. Paired Outcomes for the Uncertainty Ablation

  • 38 scenarios favored the complete framework over the ablation, while 10 favored the ablation; both agreed on 132 of 180 scenarios.The discordant outcomes account for the 28-failure difference between configurations.Source: C. Paired Outcomes for the Uncertainty… · p. 19
Loading 2609.39105v1…