Risk-aware planners score paths on uncertain elevation maps using the path cost's mean and standard deviation. Modeling rigid contact, however, requires computing a maximum over several uncertain cells. First-order propagation loses accuracy here by differentiating at only a single cell, while Monte Carlo sampling requires a full path evaluation per draw. We compute the moments of that contact maximum in closed form using Clark's pairwise recursion. By tracking each contact's covariance against the shared map cells, we propagate the smooth remainder using exact Gaussian quadratic-form identities. A contest-depth calibration, fitted once on two design traverses, closes the aggregate standard-deviation shortfall that remains. On 317 held-out rover path segments, scored against a Monte Carlo reference from the same belief, every pre-registered criterion was met. The corrected Clark fold cuts the median error of the mean from linearization's 2.3% to 0.19% and attains the lowest error in the conditional value at risk (CVaR) at the 90% level of every method tested. A benchmark plan costs just 5.5 microseconds on a GPU. These accuracy gains concentrate at contested contacts. While they seldom change which path is chosen on this terrain, the fold still selects the reference-best path in 98% of decisions against linearization's 93 to 96%. On a second dataset the mean transfers, though the risk number does not.
Figures & tables
Fig. 1: (a) A wheel over Gaussian terrain heights (per-cell mean and ±2σ band) settles on the cell that maximizes its elevation, adjusted for wheel geometry. The contact height e is thus a maximum over underlying cells, five of the seven here contesting it; the five are ringed, and in pink the one highest in the mean map, the only cell linearization considers. (b) The distribution of e (a constructed example, its two standard-deviation ratios matched to the medians at this contest depth, Section III-H ). The bars are 200,000 draws; the curve is the Gaussian carrying their mean and standard deviation ( 19.6±3.1 cm). Linearization misses both, underestimating the mean by 3.8 cm and overestimating the standard deviation by 29% ( 15.8±4.0 ); the Clark fold reproduces both. The deficit the corrected fold repairs is not here but in the aggregation over a plan (Section III-G ).
Fig. 2: (a) A synthetic 1-D environment ( 35 cm step, 25 cm drop) sampled with σ=2 cm range noise ( 25 samples per 10 cm cell) and rasterized into a Gaussian belief. At the step, samples straddle the discontinuity; the mean lands mid-air, and the standard deviation grows eightfold. (b) Error in E[max] against a 20,000 -draw Monte Carlo reference for a flat 3 -cell footprint swept along the same strip, so the step in (a) stands over the cells it spoils. The mean map is systematically optimistic, and log-sum-exp at its best-fit temperature ( τ=1.6 cm) returns the mean map’s own value to within 0.002 cm, its weight reading the gap between the means rather than the spread of the margin (Section III-C ). RMS error: 0.14 cm for the fold, 0.59 for LSE, 1.00 for the mean map.
Fig. 3: The standard deviation deficit and correction on held-out windows. These plots show the ratio of each estimator’s standard deviation to the Monte Carlo reference, plotted against the window’s contest depth αw . The data is visualized as medians and interquartile ribbons across six αw -quantile bins, with the fitted correction law d(αw) overlaid. (a) Foresight: the estimators and the deficit correction under the causal map available at decision time. (b) Hindsight: the same under the fully fused map. In (b) the uncorrected fold’s medians sit above the frozen law by 0.004 to 0.025 ; in (a) they cross it.
Fig. 4: The corrected fold on a BASEPROD elevation map. (a) The belief mean across a 4 m foresight window of a design traverse, rendered orthographically at true vertical scale. The path is the window’s own track (the 35 of 39 steps that pass the quality flags), colored by the corrected fold’s CVaR0.9 at each step. A 3 D model of the robot is placed where this risk peaks, its wheels seated on the support heights the fold returns there: the rear wheel’s cylinder element evaluates K=7 cells, where the top two candidates are nearly tied, α=0.01 standard deviations of their difference apart. The vehicle’s 20∘ roll, derived from these three supports, is computed but not drawn. (b) A 1.96 m wide rear view of the contested rear contact. It reveals the ground beneath that wheel, which the chassis and the front wheels obscure in panel (a). The ring in (a) marks this contact. (c) The per-step mean E[ℓt] alongside its ±1σ band, the ribbon colored by the CVaR0.9 risk. The risk spikes to 0.24 rad 2 at the contested step, against a median of 0.0094 along the plan.
med. rel. err. E[ℓ]
med. rel. err. sd[ℓ]
clark-corr (ours)
1.9⋅10−3 / 8.5⋅10−4
0.028 / 0.018
clark
1.9⋅10−3 / 8.5⋅10−4
0.061 / 0.035
fosm
2.3⋅10−2 / 1.5⋅10−2
0.038 / 0.023
mc-32
1.2⋅10−2 / 9.4⋅10−3
0.093 / 0.097
TABLE I: Claim 1 : median relative error against the MC reference, foresight / hindsight ( 317 / 323 windows). Bold marks the best in each column; the two Clark arms share one mean.
method
F med.
F p95
H med.
H p95
clark-corr (ours)
0.0045
0.124
0.0022
0.152
clark
0.0088
0.234
0.0054
0.257
mc-32
0.0085
0.322
0.0068
0.484
fosm
0.012
0.823
0.0074
0.778
lse
0.015
0.179
0.0090
0.196
ut
0.027
1.216
0.016
1.028
TABLE II: Claim 2 : ∣CVaR0.9(method)−CVaR0.9(MC)∣ (rad 2 ), median and 95th percentile, foresight (F) and hindsight (H), on the 317 / 323 held-out windows (reference median 0.50 / 0.63 ); the lse and ut rows use all 363 / 368 windows and a re-drawn reference, where clark-corr is 0.0045 / 0.0019 .
Legged robots maintain dynamic feasibility through multicontact interactions with terrain. Learned foothold prediction can provide feasibility-aware costs for motion planning and path selection, but accurately predicting future contacts from perceptual inputs such as height scans remains challenging on highly unstructured terrain, even with a repetitive gait cycle. In this work, we show that modeling epistemic uncertainty in predicted footholds, conditioned on terrain observations and commanded motion, distinguishes in-distribution from out-of-distribution operating regimes in simulation and real-world settings. This allows a single learned model, trained on limited data distributions, to express uncertainty caused by missing training coverage. We use this learned uncertainty to detect OOD regions and incorporate them into a unified costmap-generation framework for uncertainty-aware path planning. Using these uncertainty-aware costmaps, we evaluate feasibility error across in-distribution and OOD terrains in simulation and real-world settings. The results show improved OOD detection, up to a 37% reduction in simulation feasibility error, and more reliable planning behavior than geometry-only baselines.
For autonomous space exploration, robotic agents need to perform motion planning in which environmental interactions may be unknown. Learning these interactions, such as terrain mechanics for wheeled robots, can introduce uncertainties that lead to risky motion plans and potentially hazardous operations or mission failures. Moreover, uncertainties induced by perception-based systems can exacerbate the problem of safe motion planning. In this letter, we address the problem of performing cost-optimal kinodynamic motion planning with risk awareness. We approach this in two steps. First, a sampling-based planner (AO-RRT) generates a dynamically feasible, risk-aware, and asymptotically cost-optimal trajectory. Second, we formulate motion planning as a nonlinear optimization problem and solve it using sequential convex programming (SCP), using the AO-RRT trajectory as an initial solution. By quantifying risk using conditional value-at-risk (CVaR), we demonstrate a reduction in risk by over ∼97% across trajectories in simulation and hardware experiments.
We present MC-Risk, a planner-aligned, multi-component risk field on a bird's-eye-view grid that yields early, calibrated, and class-aware risk localization. MC-Risk linearly composes three interpretable modules: (i) a motorized-agent field that fuses a black-box multimodal trajectory predictor with an analytic Gaussian-torus construction whose lateral width grows with speed/curvature and whose height attenuates with look-ahead; (ii) a VRU risk field that replaces isotropic pedestrian blobs with a forward-biased anisotropic kernel aligned to heading and speed; and (iii) a road penalty field that exploits full HD-map topology, imposing an off-road penalty and lane-aware risk exposure for same/opposite directions. We conduct, to our knowledge, the first standardized quantitative evaluation of a risk-field formulation on RiskBench's collision subset. MC-Risk attains the best overall risk localization and the earliest hazard indication. Finally, we demonstrate a plug-and-play planning interface by using the field as an MPC cost density, enabling risk-aware trajectory generation without additional training.
Maximilian Link, Yingjie Xu, Yingbai Hu +1
Technical University of Munich · Munich, Germany · The Chinese University of Hong Kong +3