Humanoid dynamic navigation
LiDAR-centric predictive navigation that turns sparse 3D returns into topology-aware motion—and motion into stable biped execution.
Overview
Kuavo-TopoNav is an end-to-end navigation framework for a slow, dynamically constrained humanoid. Physical obstacle geometry is observed only through Livox returns, separated from traversable terrain, tracked over time, and extrapolated as uncertain future motion. Costmaps and a topology-guided parallel optimizer jointly turn this evidence into a selected trajectory.
The system then checks the first action against the future obstacle corridor, preserves command direction through the velocity guard, and stabilizes gait-state transitions during a predictive wait. The result is a perception-to-control chain designed around the realities of biped locomotion—not a planner visualized in isolation.
System contributions
The work focuses on the failure modes that appear when topology-aware trajectory optimization meets sparse LiDAR, sloped terrain, moving obstacles, parallel callbacks, a low-speed biped, and real command arbitration.
Finite-value cleaning and region filtering occur before voxelization. Terrain-aware segmentation combines a progressive ground model with local-surface compensation, so a traversable 8° ramp does not become a false wall while vertical objects remain detectable.
Odometry-frame association produces 20 future means with Gaussian extents. Coherent displacement is required before a track becomes dynamic; stopped objects leave predictive slots but remain represented in the costmaps.
Prediction updates and parallel solvers share a synchronized snapshot. A future-corridor gate checks the actual first control, while numerical infeasibility becomes a valid zero-velocity wait rather than a blind fallback or controller abort.
Angular reversals cross zero before changing sign, and gait/stance transitions use temporal hysteresis. Success is accepted only after action completion, truth terminal accuracy, obstacle separation, and a 20 s stable posture window.
Architecture
Gazebo truth is barred from planning. It enters only the independent evaluator, shown by the dashed amber path.
Qualitative result
The Gazebo view shows physical scene motion; the synchronized RViz view shows only the navigation stack's perception and planning products.
Closed-loop validation
The final round passed a single automated protocol spanning source ownership, perception, prediction, planning, command execution, arrival, and biped stability.
| Measured quantity | Round 16 |
|---|---|
| MoveBase action state | 3 · SUCCEEDED |
| Truth terminal error | 0.2985 m |
| Minimum center distance | 0.7865 m |
| Perception position error, P95 | 0.5641 m |
| Prediction speed error, P95 | 0.3256 m/s |
| Global-path safe-point ratio | 0.9674 |
| Raw / guarded yaw-sign agreement | 0.9774 |
| Post-arrival stable observation | 20.020 s |
| Maximum roll / pitch | 0.0248 / 0.0639 rad |
Claims matrix