Two parallel lines of work. Line 1 is the core navigation
stack — LiDAR SLAM plus the SCAN-Planner occupancy planner; more real-robot
testing this week turned "it sometimes fails" into four specific, separable
failure modes, three with fixes and one under investigation.
Line 2 is the perception line toward speech-driven,
semantic navigation; this week the camera went from streaming-but-unused to
wired into the SLAM — full-rate throughput, a solved camera-to-LiDAR
calibration, and camera-LiDAR fusion running — and the semantic-mapping
approach was chosen.
Recap — the navigation stack (SCAN-Planner)
The core stack navigates the legged robot with SCAN-Planner. Unlike a
flat 2D-costmap planner, SCAN keeps its own 3D occupancy grid and
optimises a smooth 3D B-spline trajectory through it, tracked by a
closed-loop controller. Because both the map and the path are 3D, it can reason about a
quadruped-traversable route over height — steps, ramps, thresholds — not just
around obstacles on a plane. The command still passes through the one mux and safety
gate that everything does.
SCAN-Planner pipeline. The 3D occupancy grid and 3D B-spline path are what
make a quadruped-traversable, multilevel route representable — where a flat 2D costmap
could only plan around obstacles on the floor.
Line 1Core navigation stack — four failure modes
More time driving the real robot (recordings available) separated the
intermittent SLAM/planner failures into four distinct causes. Naming them
individually is the point: each has its own fix, and lumping them together
was hiding that.
Failure mode
What happens · fix
Status
1. Compute starvation
The estimator misses its 10-per-second deadline, its input queue
backs up, and the estimate degrades. Fix: thin the LiDAR points
(per-scan filter 3→1) and keep the estimator on the fast CPU cores in
performance mode — frame time ~200 ms → ~50 ms.
fixed, verified
2. Network transport
Streaming raw LiDAR from the robot to the workstation adds latency on
the critical path. Plan: move the SLAM onboard immediately, so the
network leaves the loop; then, once the semantic-navigation line is
settled, move the full stack onto a more capable onboard computer
(Jetson AGX Orin). Sensor drivers already moved onboard last week.
planned
3. Turning drift indoors
Turning in the pit could send the estimate into a runaway (up to
84 m/s). Fix: the compute fix above, plus the camera-LiDAR fusion from
Line 2 — on the exact recording that used to diverge, the fused
estimate stays bounded.
fix in hand
4. Planner fences itself in over a long run
On a longer session, one goal became unreachable: ground points had
accumulated as obstacles in the planner's map. The same goal from a
fresh map planned fine — so it is accumulation over the run, not an
infeasible goal. Consistent with map cells not being retracted as the
estimate refines. Still being root-caused.
under investigation
Current limitations
Mode 1 is verified; mode 2 is a direction, not yet built; mode 3's
camera fix is shown on one recording (see Line 2); mode 4 is not yet
root-caused.
Line 2Camera line — toward speech-driven, semantic navigation
The camera earns its place on two fronts: it stabilises the LiDAR SLAM (Line
1, mode 3), and it is the sensor for the interaction line — speak a
destination in plain language, and build a map the robot understands by
object. It can serve both; we track it here, on Line 2. This week moved it
from "streaming but used by nothing" to "wired into the SLAM".
Getting the camera in — the three things figured out
Throughput. Brought the camera to its full 30 frames per
second (from ~13) after a cable change — the bandwidth the fusion needs.
Calibration. The LiDAR maker's own targetless tool failed
on our data (it returned a physically impossible answer). Switched to an
automatic targetless method (koide3) that solved the camera-to-LiDAR
geometry with no manual measurement — checked two independent ways: against
the physical mount (about 20 cm apart, an ~11° tilt) and by projecting the
LiDAR onto the photo.
Wired into the SLAM. Brought up the fusion (FAST-LIVO2)
with the calibration in place. On the recording that broke the LiDAR-only
estimator it tracks cleanly, and it runs live producing a colour 3D map.
Done.
Calibration check: LiDAR points projected onto the camera photo
with the solved geometry — the board's outline matches. A wrong calibration
shifts them off it.Live camera-LiDAR fusion, top-down: the whole arena colour-mapped
as the robot moves, with its path (green) through the centre. On the recording
that used to diverge, the estimate stays bounded — 0.36 m and 0.47 m/s, versus
the LiDAR-only 141 m and 84.7 m/s.The same fusion from the robot's viewpoint — the LiDAR cloud
coloured by the camera (an RGB-D point cloud). Grey is geometry the LiDAR sees
all around; colour is what the camera can label. This colour output is what
feeds the semantic map.
Why the fusion holds where LiDAR-only breaks
The turning runaway (Line 1, mode 3) is a LiDAR failure: while rotating, the scan being
matched is stale relative to the pose and the geometric match fails. FAST-LIVO2 adds a
second, independent constraint — it aligns the camera image directly
(photometric), using the LiDAR points as the 3D structure. When the geometry is
ambiguous, the visual term still anchors the pose on scene texture, so the fused
estimate does not run away. The camera supplies appearance, not depth; the LiDAR still
supplies all geometry.
Two independent constraints reach one estimator. When the geometric (LiDAR)
term is ambiguous — turning against a stale scan — the visual term still holds, so the
fused pose stays bounded instead of running away.
Semantic mapping — approach chosen
Surveyed what actually runs on the laptop and is stable enough to demo, and
converged on the scene-graph approach. Chose DualMap — a
popular, lightweight open-vocabulary mapper (mobile-sized vision models) that
builds a map you can query in plain language. Rejected ConceptGraphs: its
full-size models need more GPU memory than the laptop has. DualMap consumes
the fusion's pose and colour output, so it slots straight on top of Line 2.
Speech-driven semantic navigation: speak a destination → Parakeet
transcribes it → grounding resolves it against the DualMap open-vocabulary map
(built from the fusion's RGB-D and pose) → a goal pose enters the same chain as an
operator-clicked goal. Nothing here drives the platform directly.
Current limitations
Fusion runs as a standalone process — not yet a switchable option inside
the navigation stack.
Fusion accuracy is shown as non-divergence on one recording plus a live
run, not a ground-truth number.
The semantic layer is chosen, not yet built.
Project timeline
Where the two lines are heading, in three stages:
Next
Integrate the fusion into the stack as a switchable localization option,
with a written decision record and tests, verified disarmed first.
Root-cause Line 1 mode 4 (planner map accumulating ground as obstacles).
Stand up the DualMap semantic layer on the fusion output, then speak-a-goal
navigation on top — an observation-only perception line, clear of the
motion path.
Sensing direction for full-surround semantics: a 360° camera dedicated to
the perception line and kept out of the tight SLAM loop (the forward
camera stays); the LiDAR already gives 360° geometry.
Carried forward: benchmark fusion accuracy against ground truth; a driven
closed-loop lap; a runtime detector for estimator divergence; the
compute-headroom test with the planner also running.
