osmAG-Nav: A Hierarchical Semantic Topometric Navigation Stack for Robust Lifelong Indoor Autonomy

Yongqi Zhang1,†, Jiajie Zhang1,†, Chengqian Li1, Fujing Xie1, Sören Schwertfeger1,*
1School of Computer Science and Technology, ShanghaiTech University, Shanghai, China
† Equal contribution. * Corresponding author: soerensch@shanghaitech.edu.cn
Paper arXiv Code (Upcoming...)
osmAG-Nav teaser

osmAG-Nav performs topology-centric indoor navigation across large multi-floor environments, routing through semantic passages such as elevators while keeping the runtime local metric map memory footprint bounded independently of total map size.

At a Glance

Campus Benchmark

>11,025 m2

Real-world multi-story indoor-outdoor campus-scale evaluation environment at ShanghaiTech University.

Map Storage

1.4 MB

Compared with 48.5 MB for a 2D occupancy grid and 1536 MB for a downsampled 3D point cloud.

Planning Speedup

7816x

Speedup over Grid A* on long same-floor routes, with 1995x speedup on medium routes.

Corridor ATE RMSE

0.36 m

Reduced from 1.26 m with the original AGLoc baseline in degenerate corridors.

Long-Range Mission

235 m

Single-floor long-range mission used to validate end-to-end integrated stack reliability.

Mission Success

9 / 10

Repeated end-to-end trials with an average mission duration of 7 min 37 sec.

Abstract

The deployment of mobile robots in large-scale, multi-floor environments demands navigation systems that achieve spatial scalability without compromising local kinematic precision. Traditional navigation stacks, predominantly reliant on monolithic occupancy grid maps, face severe bottlenecks in storage efficiency, cross-floor reasoning, and long-horizon planning.

To address these systemic limitations, this paper presents osmAG-Nav, a complete, open-source navigation stack for ROS2 built upon the hierarchical semantic topometric OpenStreetMap Area Graph (osmAG) map standard. The system follows a "System of Systems" architecture that decouples global topological reasoning from local metric execution. A Hierarchical osmAG planner replaces dense grid searches with an LCA-anchored planning pipeline on a passage-centric graph whose edge costs are derived from local raster traversability rather than Euclidean distance, yielding low-millisecond planning even on long campus-scale routes. A Rolling Window mechanism rasterizes only a fixed-size local metric grid around the robot, keeping the local costmap memory footprint independent of the total mapped area, while a Segmented Execution strategy dispatches intermediate goals sequentially to standard ROS2 controllers for smooth, continuous handoffs.

System robustness is reinforced by a structure-aware LiDAR localization framework that filters dynamic clutter against permanent architectural priors. Extensive experiments on a real-world multi-story indoor-outdoor campus (>11,025 m2) show that, on the comparable same-floor benchmark subset, osmAG-Nav delivers up to 7816x lower planning latency than a grid-based baseline on long routes while maintaining low path-length overhead and lifelong localization stability. A single-floor long-range robot mission further validates integrated stack reliability. The full stack is released as modular ROS2 Lifecycle Nodes.

Video

Method Overview

System of Systems: osmAG-Nav replaces a monolithic occupancy-grid world model with a decoupled stack in which the environment layer maintains the persistent semantic-topological map, the deliberative layer performs hierarchical passage-centric planning and structure-based localization, and the execution layer remains standard Nav2.

Rolling Window: a fixed-size local grid is rasterized online from the vector map and published as /local_AGgridmap, keeping local costmap memory bounded independently of building scale.

Hierarchical Planning and Segmented Execution: the planner reasons over passages instead of dense global grids, performs LCA-based attach-lift-expand planning, and dispatches intermediate goals sequentially through NavigateToPose.

Structure-Aware Localization: AGLoc++ filters dynamic clutter against permanent architectural structure and fuses scan matching with odometry to improve long-term robustness in dynamic and geometrically degenerate corridors.

System architecture of osmAG-Nav

Hierarchical Planning Engine

A core contribution of osmAG-Nav is its LCA-anchored hierarchical planner. Instead of searching the full global graph uniformly, the planner first identifies the common parent of the start and goal areas, attaches the query to the leaf graph, lifts the frontier through the hierarchy, performs compact A* at the reduced graph level, and finally expands the compact trace back to the full passage sequence.

Hierarchical planning engine

Rolling Window and Segmented Execution

The topology-to-metric bridge couples global semantic routing in AGmap with bounded local execution in Nav2. When the next passage lies beyond the current local map, the planner projects a proxy goal to the local boundary and refreshes it as the rolling window advances.

Rolling window and segmented execution

Automated Map Generation

Complementing the runtime stack, osmAG-Nav includes an automated CAD-to-osmAG pipeline that extracts structure from architectural .dxf input, segments navigable areas, and outputs a semantic vector map ready for deployment.

Automated map generation pipeline

Experimental Results

The paper evaluates osmAG-Nav on a real-world multi-story ShanghaiTech campus environment covering more than 11,025 m2, with experiments spanning map storage efficiency, global planning scalability, localization robustness, and long-range integrated navigation.

Planning Scalability

On the common same-floor comparable subset, the hierarchical planner remains in the low-millisecond regime and achieves 1995x and 7816x speedups over Grid A* on medium and long routes, respectively, while keeping only small path-length overhead relative to the grid baseline.

Planning scalability benchmark

Localization Robustness

In geometrically degenerate corridors, AGLoc++ reduces corridor ATE RMSE from 1.26 m to 0.36 m while preserving strong laboratory accuracy, substantially improving lifelong localization stability over both AMCL and the original AGLoc baseline.

Localization comparison in a corridor

Real-World Planning Example

An actual deployed campus-scale route visualized in RViz shows that osmAG-Nav handles multi-building and multi-floor navigation as one unified hierarchical planning problem, including outdoor connectors and elevator transitions as explicit semantic passages.

RViz visualization of an actual hierarchical route

Key Results

Storage efficiency: for the 11,025 m2 campus map, osmAG stores the full environment in just 1.4 MB, compared with approximately 48.5 MB for a 2D occupancy grid and 1536 MB for a downsampled 3D point cloud.

Long-range planning: on long same-floor routes, the Hierarchical osmAG planner achieves up to 7816x speedup over Grid A* while maintaining low path-length overhead.

Localization stability: the corridor-average ATE RMSE drops from 1.26 m with AGLoc to 0.36 m with AGLoc++, showing materially stronger robustness in feature-poor environments.

Integrated autonomy: in a 235 m long-range single-floor mission, osmAG-Nav succeeds in 9/10 trials with an average mission duration of 7 min 37 sec.

BibTeX

@misc{zhang2026osmagnavhierarchicalsemantictopometric,
  title={osmAG-Nav: A Hierarchical Semantic Topometric Navigation Stack for Robust Lifelong Indoor Autonomy},
  author={Yongqi Zhang and Jiajie Zhang and Chengqian Li and Fujing Xie and Sören Schwertfeger},
  year={2026},
  eprint={2603.28271},
  archivePrefix={arXiv},
  primaryClass={cs.RO},
  url={https://arxiv.org/abs/2603.28271},
}