PAPER DEEP DIVE
Learning Adaptive Safety Margins for Visual Navigation
Robots in cluttered indoor spaces often fail not because they cannot generate collision-free paths, but because a fixed safety margin is mis-calibrated: conservative margins cause detours and timeouts, while permissive margins lead to near-boundary shortcuts under perception bias. Diffusion-based planners propose diverse trajectory candidates from egocentric RGB-D, yet reliable selection remains the bottleneck. We propose a context-conditioned safety critic that learns an adaptive clearance preference for ranking diffusion proposals, decomposed into three complementary terms: (i) a safety term with a clearance-budget penalty and a control-barrier-function residual for waypoint- and transition-wise safety, (ii) an efficiency term combining a smoothness penalty with a safety-gated detour-ratio penalty that avoids detours without incentivizing risky shortcuts, and (iii) a distance-constraint matching term that anchors the learned budget to realized ESDF clearances to prevent margin collapse. We train the critic with privileged ESDF geometry in simulation and distill it into a perception-only selector via a two-stage teacher-student procedure. On PointGoal navigation in HM3D and MP3D, including cross-dataset transfer, our method achieves the highest success rate (SR) and success weighted by path length (SPL) among strong diffusion, optimization, and RL baselines. Trained purely in simulation, it transfers to a Unitree G1 humanoid and navigates cluttered indoor scenes without task-specific tuning.
TL;DR
A context-conditioned safety critic replaces fixed safety margins with a learnable trajectory-dependent clearance budget, decomposed into safety (CBF residual), efficiency (safety-gated detour penalty), and balance (ESDF clearance matching) terms. Trained via teacher-student distillation from simulation ESDF to RGB-D-only deployment, achieving highest SR and SPL on HM3D/MP3D, with zero-tuning sim-to-real transfer to Unitree G1 humanoid.
Figure 1: Comparison between fixed-margin critic and adaptive safety critic. Fixed margins lead to conservative detours or brittle near-boundary choices; adaptive critic learns trajectory-dependent clearance budget for robust navigation.
1. Abstract
Robots in cluttered indoor spaces often fail not because they cannot generate collision-free paths, but because a fixed safety margin is mis-calibrated: conservative margins cause detours and timeouts, while permissive margins lead to near-boundary shortcuts under perception bias. We propose a context-conditioned safety critic that learns an adaptive clearance preference for ranking diffusion proposals, decomposed into three complementary terms: a safety term with a clearance-budget penalty and a control-barrier-function residual for waypoint- and transition-wise safety, an efficiency term combining a smoothness penalty with a safety-gated detour-ratio penalty, and a distance-constraint matching term that anchors the learned budget to realized ESDF clearances to prevent margin collapse. We train the critic with privileged ESDF geometry in simulation and distill it into a perception-only selector via a two-stage teacher–student procedure. On PointGoal navigation in HM3D and MP3D, including cross-dataset transfer, our method achieves the highest success rate (SR) and success weighted by path length (SPL) among strong diffusion, optimization, and RL baselines. Trained purely in simulation, it transfers to a Unitree G1 humanoid and navigates cluttered indoor scenes without task-specific tuning.
2. Background & Motivation
Mobile robots in human-centric indoor environments must maintain sufficient clearance during trajectory execution. The required clearance is context-dependent: local clutter, maneuvering demands, and momentary perception reliability make the same measured distance safe in one situation and risky in another. This is amplified in vision-based navigation where depth and egomotion estimates vary with viewpoint, motion, and sensing artifacts.
Hard selection rules with globally fixed clearance thresholds oscillate between two failure modes: overly conservative behavior in open areas (detours and timeouts) and brittle behavior in dense clutter (risky shortcuts under perception bias). Diffusion policies enable a generate-select paradigm, but the selector remains the bottleneck—most still use hand-tuned, globally fixed safety margins.
3. Core Method
3.1 Task Formulation
Given RGB-D observations, current pose $\mathbf{p}^{R}_{t}=(x_{0},z_{0},\theta_{0})\in\mathbb{R}^{3}$, goal pose $\mathbf{p}^{G}_{t}$, and obstacle set $\mathcal{Q}_{\text{o}}$, compute finite-horizon trajectory $\tau_{t}=\{\mathbf{p}_{j}\}_{j=0}^{T}$ from $\mathbf{p}_{0}=\mathbf{p}^{R}_{t}$ to $\mathbf{p}^{G}_{t}$, collision-free. Trajectory parameterized by ego-motion offsets $\mathbf{u}_{j}=(\Delta x_{j},\Delta z_{j},\Delta\theta_{j})$, poses reconstructed as $\mathbf{p}_{j}=\mathbf{p}_{0}\oplus\mathbf{u}_{j}$.
Figure 2: System architecture. Diffusion policy samples candidate trajectories from RGB-D and goal at inference. ESDF used only offline to supervise teacher training; deployed selector requires no map building.
3.2 Diffusion Trajectory Generator
Shared transformer encoder processes RGB-D and optional goal token to produce context; diffusion head generates trajectories (default $K=16$). DDPM scheduler with squared-cosine noise schedule. Diffusion loss:
$$\mathcal{L}_{\mathrm{diff}}=\mathbb{E}_{t,\mathbf{x}_{0},\epsilon}\left[w_{\mathrm{SNR}}(t)\,\big\|\hat{\epsilon}_{\theta}(\mathbf{x}_{t},t\,|\,\text{cond})-\epsilon\big\|_{2}^{2}\right]$$
where SNR weighting $w_{\mathrm{SNR}}(t)=\min\!\big(\mathrm{SNR}(t),\,\tau_{\mathrm{SNR}}\big)$ avoids over-weighting early steps. ESDF not used in generator training or inference.
3.3 Learnable Safety Critic
Decomposed into three terms: $V(\tau)=V_{\mathrm{safe}}(\tau)+V_{\mathrm{efficient}}(\tau)+V_{\mathrm{balance}}(\tau)$.
Adaptive safety budget: Context-conditioned margin head predicts time-varying safety budget:
$$d_{\min,j}=d_{\mathrm{safe}}+\operatorname{softplus}\bigl(q_{\eta}(\mathbf{f}_{j})\bigr)$$
where $d_{\mathrm{safe}}=0.1$ is fixed physical safety floor, $\mathbf{f}_{j}$ is lightweight context feature (local clearance $d_j$, ESDF gradient magnitude, normalized step index $j/T$), guaranteeing $d_{\min,j}\geq d_{\mathrm{safe}}$.
Safety term: Clearance-budget penalty and CBF residual. CBF residual:
$$r^{\mathrm{cbf}}_{j}=(1-\rho)h(\mathbf{p}_{j})-h(\mathbf{p}_{j+1})$$
where $h(p)=d(p)-d_{\mathrm{safe}}$, $\rho=0.1$. Safety term:
$$V_{\mathrm{safe}}(\tau)=-\sum_{j=0}^{T}\mathbb{I}\!\bigl(d_{j}<d_{\min,j}\bigr)-\lambda_{\mathrm{cbf}}\,\mathcal{L}_{\mathrm{cbf}}(\tau)$$
Efficiency term: Smoothness penalty and safety-gated detour penalty. Detour ratio:
$$\mathcal{L}_{\mathrm{detour}}(\tau)=\left[\frac{L_{\mathrm{path}}(\tau)}{D_{\mathrm{chord}}(\tau)+\varepsilon}-1\right]_{+}$$
Safety-gated weight $w_{j}=\sigma\!\left(\kappa\left(d_{j}-d_{\min,j}\right)\right)$ activates detour penalty only when clearance is available:
$$V_{\mathrm{efficient}}(\tau)=-\beta\sum_{j=1}^{T-1}\left\|\Delta^{2}\pi(\mathbf{p}_{j})\right\|_{2}-\mu\,\mathcal{L}^{\mathrm{gate}}_{\mathrm{detour}}(\tau)$$
Balance term: Anchors learned budget to realized ESDF clearances, preventing margin collapse.
3.4 Teacher-Student Training
Two stages: Stage 1 trains teacher critic with ESDF supervision in adversarial classification form (expert vs non-expert trajectories); Stage 2 trains student selector to imitate teacher from RGB-D observations. Teacher adversarial loss:
$$\mathcal{L}_{\mathrm{adv}}=-\mathbb{E}_{\tau^{+}}[\log D(\tau^{+})]-\mathbb{E}_{\tau^{-}}[\log(1-D(\tau^{-}))]$$
Student selector regresses teacher scores: $\mathcal{L}_{\mathrm{distill}}=\mathbb{E}[(s_{\phi}(\cdot)-V_{\mathrm{teacher}}(\cdot))^2]$. At inference, selector scores diffusion candidates from RGB-D and trajectory tokens without ESDF reconstruction.
graph LR
RGBD[RGB-D Observation] --> DIFF[Diffusion Generator]
GOAL[Goal Pose] --> DIFF
DIFF -->|K candidate trajectories| SEL[Student Selector]
RGBD --> SEL
SEL -->|score & rank| BEST[Best Trajectory]
BEST --> ROBOT[Robot Execution]
subgraph Offline Training
SCENE3D[3D Scenes] --> ASTAR[A* Planning + Spline]
ASTAR --> EXPERT[Expert Trajectories]
ASTAR -->|randomized edge costs| NONEXP[Non-Expert Trajectories]
EXPERT --> TEACHER[Teacher Critic]
NONEXP --> TEACHER
ESDF[ESDF Geometry] --> TEACHER
TEACHER -->|distill| SEL
end
4. Key Experiments
4.1 Simulation Experiments
| Method | HM3D SR | HM3D SPL | MP3D SR | MP3D SPL | HM3D→MP3D SR | HM3D→MP3D SPL |
|---|---|---|---|---|---|---|
| NavDP | 0.710 | — | 0.627 | — | 0.523 | — |
| PointNav (RL) | 0.547 | — | — | — | — | — |
| Ours | 0.783 | 0.611 | 0.680 | 0.523 | 0.597 | 0.465 |
Table 1: PointGoal navigation simulation results. Our method achieves highest SR and SPL across all settings, with largest advantage under cross-dataset transfer.
HM3D SR 0.783 (+7.3 over NavDP, +23.6 over PointNav), SPL 0.611. MP3D SR 0.680 (+5.3 over NavDP). Cross-dataset SR 0.597/SPL 0.465 (+7.4/+11.6 over NavDP), maintaining safe and efficient navigation under domain shift.
4.2 Real-World Experiments
Figure 3: Real-world test result visualization. Highest and lowest scored trajectories projected onto robot first-person view, blue-to-red representing low-to-high score (dangerous to safe).
| Method | Corridor (easy) | Apartment (medium) | Lab (hard) |
|---|---|---|---|
| NavDP | 10/10 (1.0) | 9/10 (0.9) | 6/10 (0.6) |
| ViPlanner | 9/10 (0.9) | 6/10 (0.6) | 4/10 (0.4) |
| PointNav | 5/10 (0.5) | 5/10 (0.5) | 0/10 (0.0) |
| EGO-Planner | 5/10 (0.5) | 4/10 (0.4) | 1/10 (0.1) |
| Ours | 10/10 (1.0) | 10/10 (1.0) | 8/10 (0.8) |
Table 2: Real-world PointGoal results (10 episodes/scene). Pure simulation training, zero-tuning transfer to Unitree G1 humanoid.
Deployed on Unitree G1 with Intel RealSense D435i, no task-specific tuning. Three difficulty levels: easy (wide corridor), medium (tight doorways apartment), hard (cluttered lab with narrow passages). Ours best across all scenes, especially lab 8/10 vs NavDP 6/10.
4.3 Narrowness Adaptability Test
Figure 4: Narrowness test. Checkpoints A and B with DoN ≈ 0.75. NavDP fails at both; our method successfully passes cluttered obstacles.
Degree of Narrowness (DoN) probes adaptability of learned constraint. At DoN≈0.75, our policy passes by tightening learned constraint while keeping clearance above budget; NavDP with fixed margin judges corridor non-passable and stalls. When clearance falls below safe envelope, critic refuses to further relax $d_{\min}$, showing learned margin balances adaptability with safety rather than shrinking below physically meaningful floor.
4.4 Ablation Study
| Model | Successes | SR | SPL |
|---|---|---|---|
| Remove safety term (A) | 207/300 | 0.690 | 0.572 |
| Remove efficiency term (B) | 212/300 | 0.707 | 0.468 |
| Remove balance term (C) | 190/300 | 0.633 | 0.585 |
| Full model | 235/300 | 0.783 | 0.611 |
Table 3: Safety-critic structure ablation. All three terms needed; full model best. Removing efficiency drops SPL to 0.468 (over-conservative detours); removing balance drops SR to 0.633 (margin collapse).
Removing efficiency term (B) raises SR to 0.707 but drops SPL to 0.468—without explicit efficiency objective, selector over-selects conservative detours and times out. Removing balance term (C) drops SR to 0.633—budget-clearance anchor lost, miscalibration distorts geometry. Full model achieves best SR 0.783 and SPL 0.611.
5. Limitations & Future Work
- Offline ESDF dependency: Teacher training requires privileged ESDF geometry; student inference is ESDF-free but teacher quality bounded by ESDF precision.
- PointGoal navigation only: Currently evaluated only on PointGoal; extension to semantic goal navigation or social navigation needs further validation.
- Fixed safety floor: $d_{\mathrm{safe}}=0.1$ is a fixed physical floor that may need adjustment for different robot morphologies.
- Limited real-world scale: Only 10 trials per real-world scene; larger-scale validation would strengthen statistical confidence.
6. Conclusion
This work introduces a generate-select framework replacing fixed margins with a learnable, context-conditioned safety critic fusing safety and efficiency in balance. The safety critic comprises a safety term (clearance-budget penalty + CBF residual), an efficiency term (smoothness penalty + safety-gated detour penalty), and a balance term (ESDF clearance matching), trained via teacher-student distillation from simulation ESDF to RGB-D-only deployment. Across simulation and real-world tests it achieves the highest SR and SPL, and transfers from pure simulation to a real humanoid in cluttered indoor scenes without task-specific tuning. Ablations confirm each critic term contributes to a favorable safety-efficiency trade-off with consistent gains over fixed-threshold baselines. This work demonstrates that safety margins should not be globally fixed constants but learnable functions that adapt to trajectory and geometric context.
SOURCE LINKS
