Autonomous Navigation on a Legged Platform

mapping, exploration and safe local planning for a legged robot

Sensors in, a velocity command out. This is an autonomy stack for a legged robot that takes a stereo pair and an IMU and produces a body velocity command, built so that a single map correction never forces the geometry to be rebuilt. Two loops run at different triggers rather than at two fixed rates: tracking runs every tick, producing a velocity command over the current route and reacting to what just came into view, while global planning runs on demand, triggered when the tracker nears the end of the route it was given.

The diagrams below are the ones from the talk, live rather than exported — the ones that move, move.

System at a Glance

Five stages, from raw sensing to the command the locomotion controller receives.

Unified Autonomy Stack [1]
sensors
stereo · IMU · depth
→
estimate
keyframe VI-SLAM
→
map
occupancy · ESDF · elevation
→
plan
RRG → corridor
→
track + filter
MPPI · CBF
→
u = (vx, vy, ω)
velocity command

Sensors in, a velocity command out.

Two loops, but not two fixed rates. Tracking runs every tick: a velocity command over the current route, reacting to what just came into view. Global planning runs on demand — triggered when the tracker nears the end of the route it was given.

Keyframe Visual-Inertial SLAM

The frontend runs per frame; loop closing runs asynchronously and re-anchors the map when it lands.

OKVIS2-X [2]
FRONTEND · PER FRAME
stereo pair
→
SuperPoint [3]
→
match + RANSAC
parallel per camera
INERTIAL
IMU
→
preintegration
between keyframes
REALTIME GRAPH
keyframesoptimisation window
LOOP CLOSING · ASYNC
place recognition
DBoW vocabulary
→
full-graph BA
re-anchors submaps
DEPTH · MAPPING
stereo network (S2M2) [4]
→
integration
log-odds
3D OCCUPANCY GRID
keyframe-anchored submaps · multi-resolution
free
occupied
unknown
a pose-graph update relocates geometry — no re-integration

Keyframe-Anchored Submaps

The representation is what makes loop closure affordable: geometry lives in each keyframe’s frame, not the world’s.

BEFORE CLOSURE
drifted
trajectory
AFTER CLOSURE
submaps follow
their anchors
FIG 1 · The line is the trajectory through the keyframe anchors. Geometry is stored in each keyframe's frame, so when closure corrects the trajectory the submaps move with it — no re-integration.

Depth is integrated as multi-resolution occupancy into submaps anchored to keyframes rather than to the world.

Each submap also carries an axis-aligned bounding box, so a query can discard every submap that cannot contain the point before touching any voxels.

Once a submap is finalized its occupancy grid is extracted and aligned along the gravity direction.

Signed Distance and Gradient

With distance to the nearest surface stored everywhere, clearance becomes a lookup rather than a search.

nvblox [5]
  • Every point in space stores how far the nearest obstacle surface is — large in the open, small beside a wall, negative inside an obstacle.
  • Clearance becomes a lookup, not a search.
  • Each point also contains the gradient direction away from the obstacle.
  • Distances come from a Euclidean distance transform swept over all three axes.
  • ∂d/∂x ≈ [ d(i+1, j) − d(i−1, j) ] / 2Δ
    ∂d/∂y ≈ [ d(i, j+1) − d(i, j−1) ] / 2Δ
    central differences on neighbouring voxels · Δ = voxel size
0.7
0.6
0.4
0.4
0.4
0.4
0.4
0.6
0.7
0.8
0.8
0.6
0.4
0.3
0.2
0.2
0.2
0.3
0.4
0.6
0.6
0.6
0.6
0.4
0.2
-0.2
-0.2
-0.2
0.2
0.4
0.4
0.4
0.4
0.6
0.4
0.2
-0.2
-0.2
-0.2
0.2
0.3
0.2
0.2
0.3
0.6
0.4
0.3
0.2
0.2
0.2
0.3
0.2
-0.2
-0.2
0.2
0.7
0.6
0.4
0.4
0.4
0.4
0.4
0.2
-0.2
-0.2
0.2
0.8
0.7
0.6
0.6
0.6
0.6
0.4
0.3
0.2
0.2
0.3
inside · d < 0
near
open
query voxel · ∇d
FIG 2 · One horizontal slice of the voxel grid, 0.2 m resolution. Every voxel stores metres to the nearest surface — negative inside — and the arrow is that direction.
ESDF CONTAINS
Q(p) = { d(p) ∈ ℝ, ∇d(p) ∈ ℝ³, o(p) ∈ {0,1} }
d > 0 outside obstacles, < 0 inside · ∇d points toward increasing distance · o marks whether the point falls inside observed bounds

Occupancy → Elevation

A column of voxels collapses to two cells — the ground you stand on and the top that caps the space above it.

elevation_mapping_cupy [6]

Scanning the column returns two cells, not one — the ground you stand on and the top that caps the space above it.

1 · BIN

Divide the column at (x, y) into z cells.

2 · GROUND

The ground is the lowest occupied cell with an unbroken run of free cells above it — free space is what separates a floor from a ceiling.

3 · TOP

The top is the occupied cell that closes that run. The pair gives each column a height and a headroom h.

COLUMN AT (X,Y)
z
shelf
floor
→
GROUND & TOP
free above
the top is a
separate slab
→ top
→ ground
h
→
TWO LAYERS OUT
θ
no top
(x, y)
4 · SLOPE

Fitting a cell's ground height against its neighbours gives the normal, and the angle θ to gravity.

zx = [z(i+1,j) − z(i−1,j)] / 2Δ    zy = [z(i,j+1) − z(i,j−1)] / 2Δ
n = (1, 0, zx) × (0, 1, zy)    cos θ = n̂ · (0, 0, 1)
OCCUPIED
FREE
NEVER OBSERVED
GROUND OUT
SLOPE

Planning and Navigation

The navigation decision and how it is timed against the tracking loop.

NAVIGATION DECISION · ON DEMAND
occupancy
grid
ground + top
per column
ESDF
build RRG
≤500 iters
score gain
vertex × yaw
EXPLORE
dense local RRG
maximise volumetric gain
GLOBAL PLANNER
sparse global graph
reposition · return-to-home
corridor
→ main loop
scan
distance · collision
gain above threshold
exhausted
time limit · homing
gain?
THREADS · TIMING
u
NAVIGATION
decision
CORRIDOR
MAIN LOOP
local planner
MPPI + CBF
EXPLORE — replans
GLOBAL / RETURN-TO-HOME — solves once
RRG · gain
RRG · gain
global solve
progress trigger
gain exhausted
k
k+1
k+2
whole path to target
no re-trigger until arrival

The Rapidly-Exploring Random Graph

One sampler, two roles: a dense local graph rebuilt every replan, and a sparse global graph that persists.

frontier · gain per vertex × yaw sampling box · shrunk to observed reach obstacles best branch robot
EXTENT
explorebox around robot, observed reach
globaleverywhere visited
DENSITY
exploredense — up to 500 samples per build
globalsparse — one vertex per waypoint
LIFETIME
explorerebuilt every replan, then discarded
globalpersists, never rebuilt
OBJECTIVE
exploremaximise gain, vertex × yaw
globalshortest path to a known target
OUTPUT
explorecorridor to the best branch
globalwhole path to target

Traversability

Every sample and every candidate edge has to pass the same set of geometric tests before it enters the graph.

VERTEX — every sample
EDGE — every candidate connection
every neighbour within nearest_range repeats this
inside bound − radius
ground beneath it
footprint slope ≤ limit
cylinder free
vertex accepted
edge length ≥ min
ground at every sample
segment slope ≤ limit
swept cylinder free
traversable — insert
out of bounds
hanging
steep
occupied / unknown
short edge
hanging
steep
occupied / unknown
resample at fixed spacing
BOUNDthe mission volume, anchored at the start pose, eroded by the body radius so an accepted vertex never overhangs it
BODYthe robot is a cylinder — radius and height, inflated by a clearance margin — so the check is yaw-invariant and the swept volume along an edge is a capsule
SLOPEmeasured across the footprint for a vertex, between consecutive samples for an edge

Volumetric Gain

What a vertex is worth is the volume it would reveal, discounted by how far away it is and how far the robot has to turn to face it.

g(x,y,φ) = ∑ w(q) over q ∈ R(x,y,φ)
= nunk·wu + nfree·wf + nocc·wo
score(P) = [ ∑v∈P g(v)·e−λd(v) ] · e−λ₂Δ(P)
R — visible set at yaw φ, capped at dmax
t* = min{ dmax, min{ t : c(p + t·u) = occupied } }
V ← V ∪ { voxels on p + t·u, t < t* }
c(p + t·u) — occupancy at distance t along ray u
PER VERTEX — g SWEEPS WITH HEADING
occlusion shortens the frustum
facing the most unknown
PER PATH — DISTANCE AND HEADING CUT IT DOWN
short in straight line, blocked
F1 F2 — chosen F3 φ
TERMS
g(v)
volume v reveals at its best φ
e−λd(v)
discount by distance along the graph
e−λ₂Δ(P)
penalty for turning away from φ

Working Diagram

Which representation each consumer actually reads — and where the safety path deliberately skips the fused layers.

SENSING
REPRESENTATION
CONSUMER
OUTPUT
learned stereo
depth + σ, per frame
depthToLocalEsdf
body grid, per tick
raw depth pixels
no field built
CBF-QP
envelope constraints
u_safe
→ velocity command
supereight submaps
occupancy, per keyframe
elevation grid
height + normal
ESDF
distance + gradient
RRG planner
sample · project · test edge
MPPI
K rollouts × H steps
corridor
u_nom
The safety path skips the fused layers and reads raw depth.

MPPI — Sampling

Perturb the previous plan, roll every sample through the dynamics, score it, and re-average.

SAMPLE — PERTURB THE PREVIOUS PLAN, SCORE, RE-AVERAGE robot weighted average → u K sampled rollouts, shaded by cost
1shift last tick’s plan forward one step — the warm start
2draw K perturbations, roll each through the dynamics
3score every rollout to one scalar C
4re-average by softmax weight, send the first control, repeat
ut(k) = ūt + εt(k), ε ~ N(0, Σ)
wk = softmax(−Ck/λ), ū ← ∑k wk u(k)
Σ how far the fan spreads
λ how sharply the best rollouts dominate

MPPI — The Obstacle Cost

Clearance is queried on the robot’s envelope, priced in two tiers, and scaled by whether the robot is actually closing on the surface.

1 — QUERY THE ESDF ON THE BODY, NOT AT A POINT
r_body envelope
clearance = d(p) − rbody
unobserved → clearance = ∞
d(p) — ESDF lookup, one per envelope sample:
distance to the nearest surface
∇d is its gradient — the direction away from that surface
2 — PRICE IT IN TWO TIERS
d_hard d_safe clearance → quadratic linear
THE PENALTY
pen = fdanger·whard·max(0, dhard−clearance)²
    + fcomfort·wsoft·max(0, dsafe−clearance)
Cobs = ∑t ∑envelope pen(clearance)
3 — SCALE BY WHETHER YOU ARE CLOSING
closing → 1, full cost passing → 0, comfort term vanishes
closing = max(0, −v̂·∇d)
fcomfort = closing
fdanger = φ + (1−φ)·closing

MPPI — The Other Five Terms

Progress and cross-track in the corridor’s Frenet frame, traversability, smoothness, and effort.

TERM BY TERM
Cgoal = w ∑t (L − st) + w′ ∥pT − g∥²
st arc length reached at step t, L total corridor length,
pT final rollout pose, g goal point → distance still to run,
added up over the horizon, plus a squared miss at the end
Ccross = w ∑t et2
et perpendicular offset from the centreline at step t →
squared and summed: total lateral drift over the horizon
Ctrav = w ∑t [ max(0, α−αlim)² + step² + drop² ]
α slope under the robot, Δz height change per step →
each term is 0 inside the limit, squared excess outside
Csmooth = w ( ∑t ∥ut+1−ut∥² + ∥u0−uprev∥² )
ut control at step t, uprev the command sent last tick →
total change within the plan, plus the jump across ticks
Ceff = w ∑t ∥ut∥²
squared magnitude of every control → total effort spent
THE FRENET FRAME TURNS A CORRIDOR INTO TWO SCALARS
e goal string-pulled centreline s L − s — what C_goal sums
WHAT EACH TERM NEEDS
obsthe ESDF
goal, crossthe corridor, in its Frenet frame
travthe surface model
smooth, effthe controls alone

Control Barrier Function Filter

One linear constraint per observed envelope point, and an objective that changes the nominal command as little as possible.

1 — A BARRIER PER ENVELOPE POINT
clearancei = di − rbody
hi = clearancei − dsafe
h > 0 is the safe set. Two margins: the body radius, then a
separate standoff. The filter keeps h non-negative, not large.
2 — DIFFERENTIATE ALONG THE DYNAMICS
ḣ = ∇d · v
∇d points away from the obstacle, so the sign reads directly:
toward  ∇d·v < 0  clearance falling
parallel ∇d·v ≈ 0  clearance steady
away    ∇d·v > 0  clearance rising
ḣ ≥ −γh  ⇔  ∇d · v ≥ −γh
clearance may shrink, but no faster than γ times what
is left → as h → 0 the allowed closing speed → 0, so h never crosses zero
h is nonlinear in position, ḣ is linear in v → one row per point
THE QP
minu,δ ∥u − unom∥² + ρ ∑ δi2
s.t. ∇di·(vx, vy) + δi ≥ −γhi per observed point
     |vx| ≤ vmax, |vy| ≤ vmax, |ω| ≤ ωmax
     δi ≥ 0
minimal intervention — of all safe commands, take the
one least changed. Not the safest, not the slowest.
∥u−unom∥² squared → correction is an orthogonal projection,
so a safe nominal passes untouched — free when idle
δi slack: how far row i was broken, priced at ρ. Keeps the
QP feasible when rows conflict; unobserved points give no row
3 — PROJECT THE NOMINAL ONTO THE SAFE SET
box limits h < 0 h ≥ 0 u nominal u safe
STRUCTURAL GAP
yaw is unconstrained

What This Builds On

  1. A. Dharmadhikari et al., “The Unified Autonomy Stack: Toward a Blueprint for Generalizable Robot Autonomy,” arXiv, 2026
  2. S. Boche et al., “OKVIS2-X: Open Keyframe-based Visual-Inertial SLAM Configurable with Dense Depth or LiDAR, and GNSS,” IEEE Transactions on Robotics, 2026
  3. D. DeTone, T. Malisiewicz, and A. Rabinovich, “SuperPoint: Self-Supervised Interest Point Detection and Description,” CVPR Workshops, 2018
  4. J. Min et al., “S2M2: Scalable Stereo Matching Model for Reliable Depth Estimation,” ICCV, 2025
  5. A. Millane et al., “nvblox: GPU-Accelerated Incremental Signed Distance Field Mapping,” ICRA, 2024
  6. T. Miki, L. Wellhausen, R. Grandia, F. Jenelten, T. Homberger, and M. Hutter, “Elevation Mapping for Locomotion and Navigation using GPU,” IROS, 2022
  7. A. Sriganesh et al., “Modular, Resilient, and Scalable System Design Approaches — Lessons Learned in the Years after DARPA Subterranean Challenge,” ICRA Workshop, 2024
  8. M. Steinbrink et al., “Rapidly-Exploring Random Graph Next-Best View Exploration for Ground Vehicles,” ECMR, 2021
  9. M. Kulkarni et al., “Autonomous Teamed Exploration of Subterranean Environments using Legged and Aerial Robots,” ICRA, 2022
  10. G. Williams et al., “Information-Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving,” IEEE Transactions on Robotics, 2018
  11. A. D. Ames, S. Coogan, M. Egerstedt, G. Notomista, K. Sreenath, and P. Tabuada, “Control Barrier Functions: Theory and Applications,” European Control Conference (ECC), 2019