Navigation intelligence is measured in executed motion, recovery behavior, and auditable uncertainty.
A Local Planner With Commitment Issues
Navigation as embodied intelligence turns maps and goals into constrained motion. The planner is not searching for a pretty line; it is choosing a feasible commitment under geometry, dynamics, uncertainty, moving obstacles, and recovery rules.
Problem First
Navigation is where the agent's world model becomes a physical commitment. A route that ignores localization uncertainty, moving people, controller limits, or recovery policy is not a plan; it is a wish drawn on a map.
A navigation stack has layers: global route, local trajectory, costmap, controller, safety monitor, and recovery behavior. Each layer can be correct locally and still fail globally if the interfaces are underspecified.
The best path is the one the robot can localize, track, and recover from.
Formal Model
Most navigation methods can be read as constrained search or optimization:
$$ \pi^*=\arg\min_{\pi}\sum_t c(x_t,u_t)\quad\text{s.t.}\quad x_{t+1}=f(x_t,u_t),\; x_t\in X_{\mathrm{free}} $$
The objective names progress to a goal; constraints name the robot reality: footprint, clearance, speed, acceleration, curvature, localization confidence, and safety monitors.
- Convert the goal into a map frame and validate localization confidence.
- Plan a global path through known traversable space.
- Generate local velocity commands that respect dynamics and current obstacles.
- Trigger replanning or recovery when the state estimate, costmap, or controller error violates its bound.
Worked Diagnostic
Code Fragment 1 isolates the navigation loop: state estimate, goal, costmap, local command, and recovery trigger. The point is to test the executed-motion contract before Nav2 or OMPL adds plugins.
# Score a navigation run with execution-aware fields.
# A short path loses if it creates low clearance and many recoveries.
runs = [
{"name": "short", "meters": 12.0, "clearance": 0.18, "recoveries": 2},
{"name": "safe", "meters": 14.0, "clearance": 0.42, "recoveries": 0},
]
for run in runs:
cost = run["meters"] + 8 / run["clearance"] + 5 * run["recoveries"]
print(run["name"], round(cost, 1))Expected output interpretation. The shorter route loses by a wide margin because low clearance and two recoveries are expensive in the execution-aware score. The output should be read as a planning audit: path length alone would have selected the wrong behavior for a real robot.
Tool Workflow
Nav2 provides the deployed pattern: planner server, controller server, behavior tree navigator, costmaps, lifecycle nodes, and recovery behaviors. Habitat and Isaac-style simulators help test policies before field deployment.
Keep the small navigation example as a regression test for interface semantics. Use Nav2, OMPL, costmap plugins, behavior trees, and replay logs for deployment scale.
Replay blocked corridors, localization jumps, stale costmaps, moving people, actuator saturation, and failed recovery. A navigation system earns trust by explaining which layer noticed the problem first.
A delivery robot should log global plan, local command, costmap snapshot, localization confidence, controller error, nearest obstacle, and recovery action. Those fields distinguish bad navigation from stale perception.
Before comparing navigation stacks, freeze footprint, inflation radius, map resolution, velocity limits, localization source, controller rate, and recovery policy. Otherwise the comparison mixes navigation quality with robot configuration.
Navigation research is converging on classical planning plus learned perception, foundation-model goal interpretation, and explicit safety monitors. The field is rediscovering that end-to-end policies still need replayable failures and comparable baselines.
Navigation intelligence is measured in executed motion, recovery behavior, and auditable uncertainty.
Can you state the search space, cost function, constraints, replanning trigger, controller interface, and failure metric for navigation as embodied intelligence? If not, the planner is not specified enough to deploy.
Navigation as embodied intelligence is ready for embodied use when route quality, dynamic feasibility, local control, and recovery behavior are measured in the same replay.
Run a three-scenario panel with open route, newly blocked route, and moving obstacle. Report success, path length, minimum clearance, recovery action, and whether the robot waited for localization before moving.
What's Next?
Continue to Section 30.2: Graph search, where this planning contract connects to the next embodied capability.
Section References
LaValle, S. M. "Planning Algorithms." Cambridge University Press, 2006. http://lavalle.pl/planning/
Open textbook reference for graph search, sampling-based planning, configuration spaces, and kinodynamic planning.
OMPL Project. "Open Motion Planning Library." Official documentation. https://ompl.kavrakilab.org/
Primary tool reference for sampling-based planners such as RRT, RRTstar, PRM, and kinodynamic variants.
ROS 2 Navigation Project. "Nav2 documentation." Official documentation. https://navigation.ros.org/
Primary documentation for global planners, controllers, costmaps, behavior trees, and recovery behaviors.