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

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
$\mathbf{x}_{k}$	Planar query state $(x_{k},y_{k},\alpha_{k})$ at time step $k$ .
$\mathbf{p}_{k}$	Planar position $(x_{k},y_{k})$ at time step $k$ .
$\boldsymbol{\xi}_{k}$ , $\boldsymbol{\xi}_{k}^{*}$	Terrain-dependent best-fit state: elevation, pitch, roll, and candidate wheel–terrain contact points, and its converged least-squares estimate.
$f_{\boldsymbol{\lambda}}(x,y)$	Terrain elevation model parameterized by Fourier coefficients.
$\boldsymbol{\lambda},\,\boldsymbol{\lambda}^{*}$	Terrain parameter vector and its converged least-squares estimate.
$\boldsymbol{\Sigma}_{\lambda}$	Regularized inverse-Hessian terrain-parameter uncertainty estimate.

TABLE I : Important symbols.

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

c_{unc}=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

c_{unc}=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

c_{unc}=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

c_{unc}=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 ( ∘ )
Terrain	Method	Runs	Failures	Failure Rate (%)	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 ( $c_{unc}=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^{\circ}

. 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

c_{unc}=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_{\kappa}$	Curvature-cost weight	0.01	$\mathrm{m}^{2}$
$w_{a}$	Acceleration-cost weight	10.0	$\mathrm{s}^{4}\mathrm{m}^{-2}$
$w_{n}$	Surface-normal-cost weight	10.0	dimensionless
$w_{p}$	Pose-cost weight	10.0	$\mathrm{rad}^{-2}$
$\rho_{n}$	Normal-uncertainty penalty coefficient	1.0	dimensionless
$\rho_{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 $h_{i}$	$l{=}0.210$ m, $w{=}0.272$ m, $h_{i}{=}0.260$ m ( $i{=}1,\ldots,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^{-3}\,\mathbf{I}$
Terrain LM	Damping, stopping tolerance, and iteration limit	damping $10^{-6}$ ; gradient tol. $10^{-3}$ ; 1 iteration (online refinement)
Pose/contact NLS	Stopping tolerance and iteration limit	gradient-norm tol. $10^{-3}$ ; 30 iterations
Scenario generation	Start–goal sampling law and admissibility rules	goal uniform in $90^{\circ}$ forward cone, 6–8 m range, inside map
Goal condition	Goal radius and closed-loop episode timeout	0.75 m; 120 s

TABLE V : Planner and closed-loop evaluation protocol.

Category	Required information	Value
Terrain data	Point clouds and split	3,185 patches of 35,000 points; shuffled 90/10 split (2,866/319)
Terrain network	CNN/DiT dimensions and size	CNN 64–128–256 $\to$ 256; DiT width 256, depth 4, 8 heads, MLP 1024; 5.47 M parameters
Normalization	Input/target scaling	each input shifted/scaled by one training-set mean and std.; targets (Fourier coefficients) in raw units
Optimization	Optimizer, lr, batch, epochs, precision	AdamW, weight decay $10^{-2}$ , constant lr: $10^{-3}$ , batch 1,000, $\leq$ 10,000 epochs, float32
Model selection	Checkpoint criterion	lowest training loss
ODE inference	Integrator and batching	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

c_{\mathrm{unc}}=0

ablation. The complete framework outcome is listed first; S and F denote success and failure, respectively.

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

Abstract

Figures & tables

Explore similar work

VertiAdaptor: Online Kinodynamics Adaptation for Vertically Challenging Terrain

A Terrain-Adaptive epsilon-Constraint MPC for Uneven Terrain Kinodynamic Planning

Feeling Terrain Before Crossing: World Models for Off-Road Navigation