osmAG-Nav：面向大型多层室内环境的语义拓扑导航栈

1. 为什么传统栅格导航在大楼里会吃力室内移动机器人过去大量依赖 2D Occupancy Grid也就是把环境切成固定分辨率的小格子每个格子记录可通行、不可通行或未知状态。这种表达方式直观、通用也很适合局部避障和短距离路径搜索但当面积扩展到万平方米、楼层扩展到多层、任务持续到数周甚至数月时问题会被放大。栅格地图的存储量随面积和分辨率增长规划器在大地图上做 A* 搜索时需要展开大量格子长距离任务很容易出现延迟暴涨。更麻烦的是格子只描述“这里能不能走”并不知道这是走廊、房间、电梯厅还是楼梯间。多楼层场景会进一步暴露栅格地图的结构缺陷。二维栅格天然表示平面空间但真实楼宇存在电梯、楼梯、连廊和楼层切换机器人需要知道“从二楼房间出发先经过走廊再进入电梯切到一楼后继续去目标房间”。如果这些关系不在地图模型中作为一等对象存在工程系统只能在导航栈外再写状态机或规则脚本来处理楼层切换。短期看这种补丁可用长期看会增加调试难度也会让定位、规划和任务调度之间的接口变得脆弱。长期运行还会遇到定位退化。传统 SLAM 或栅格地图往往会把家具、临时货架、行人遮挡后的轮廓也固化进地图几周后环境变化激光扫描与旧地图不再匹配AMCL 这类基于栅格的定位方法就容易被动态杂波干扰。在长走廊、玻璃墙、重复门洞等几何退化场景中机器人还可能沿走廊方向产生漂移。osmAG-Nav 并不是否定栅格地图而是把它限制在更适合的位置局部、短时、围绕机器人实时生成全局层则换成更稀疏、更稳定、更有语义的表示。2. osmAG 地图用“区域和通道”重写室内空间osmAG 的全称是 OpenStreetMap Area Graph可以理解为兼容 OSM 数据模型的室内区域图。公开的 OpenStreetMap XML 资料说明OSM 文件通常由 node、way、relation 以及 tag 组成节点保存 WGS84 坐标way 引用节点形成线或面tag 则承载语义属性。osmAG 借用了这种成熟的 XML 生态但把室内机器人关心的对象抽象成两类Area 和 Passage。Area 表示房间、走廊、电梯、楼梯、楼层、建筑等区域Passage 表示门、开口、电梯跨层连接等通道。这样一来地图不再只是像素级障碍物而是带有“空间组织关系”的图。从数学上看osmAG 是一个无向区域图区域是顶点通道是边。它的关键不是“把栅格压缩成图”而是直接用建筑物本来的层次来组织地图建筑包含楼层楼层包含区域区域之间通过门、走廊开口、电梯等 Passage 相连。论文中特别强调电梯和楼梯不是 planner 里的特殊异常而是标准区域类型和标准通道关系。这意味着跨层路径不再是额外状态机而是普通图搜索的一部分只要电梯区域与不同楼层的入口被正确编码规划器就能把垂直移动纳入同一条路径。下面是一个简化后的 osmAG/OSM XML 片段用来展示语义标签的形态。真实地图会包含更多坐标点、父子区域关系、楼层高度和通道几何但核心思想就是用标准 OSM 元素承载机器人需要的空间结构。osmversion0.6generatorosmag_editornodeid1lat31.1770lon121.5900tagknamevroot//nodewayid1001ndref11/ndref12/ndref13/ndref14/ndref11/tagknamevF2_Corridor_A/tagkosmAG:typevarea/tagkosmAG:areaTypevcorridor/tagkosmAG:parentvFloor_2/tagklevelv2//waywayid2001ndref21/ndref22/tagknamevDoor_R201_to_Corridor/tagkosmAG:typevpassage/tagkosmAG:fromvRoom_201/tagkosmAG:tovF2_Corridor_A/tagkosmAG:passage_typevdoor/tagklevelv2//way/osm这种表达方式的直接收益是轻量。论文在上海科技大学约 11025 平方米的多层环境中比较了不同地图表示0.05 米分辨率的 2D 栅格约 48.5MB0.1 米体素下采样的 3D 点云约 1536MB而 osmAG 矢量地图约 1.4MB。这个差异不只是硬盘占用问题还会影响网络传输、启动加载、缓存构建和长期部署维护。对机器人系统来说地图越稀疏、语义越明确全局规划和任务层越容易做出稳定决策。3. 系统架构osmAG-Nav 的架构可以概括为“System of Systems”也就是把多个成熟子系统通过稳定接口组合起来而不是写一个巨大的导航节点。论文将系统拆成环境层、决策层和执行层。环境层负责保存 osmAG 全局语义拓扑地图并在机器人附近生成固定大小的局部栅格决策层负责结构感知定位、分层拓扑规划和分段目标管理执行层则继续使用标准 ROS2 Nav2 组件处理局部路径、控制和恢复行为。这个设计很务实因为 Nav2 在局部控制和行为树执行上已经很成熟osmAG-Nav 没有重写这部分而是替换了全局地图和长距离推理方式。Nav2 官方文档中NavigateToPose 是通过行为树导航器调用的 ROS2 actionLifecycle Nodes 用于管理节点从 Unconfigured、Inactive、Active 到 Finalized 的状态转换。osmAG-Nav 借用了这套接口边界Rolling Window 地图以nav_msgs/OccupancyGrid形式发布给 Nav2 的 costmap 层分段目标通过NavigateToPoseaction 下发定位结果通过 TF 和 pose 接口连接AGmap、map、odom与base_link。换句话说Nav2 仍然看到的是熟悉的局部栅格和目标点但更高层已经不再被全局栅格地图束缚。一个典型的数据流是这样的系统启动后osmAG parser 读取 OSM/XML 地图构建 AreaGraph 并发布结构信息定位模块用激光雷达与永久建筑结构匹配估计机器人在AGmap中的位置Rolling Window 根据当前位姿和楼层上下文把附近 50m × 50m 的矢量区域动态栅格化并发布为/local_AGgridmap分层规划器在全局语义图上计算通道序列再把下一个可执行的 passage 中心或局部边界投影点发送给 Nav2Nav2 在局部窗口内完成避障和控制。这个闭环让全局保持稀疏局部保持精细。下面是一个简化的 Nav2 静态层配置片段展示 Rolling Window 地图如何被当作局部静态地图输入。真实工程中还需要配置 footprint、inflation layer、controller、behavior tree 和 TF但这段已经能体现 osmAG-Nav 与 Nav2 的接口关系。local_costmap:local_costmap:ros__parameters:global_frame:maprobot_base_frame:base_linkrolling_window:falseplugins:[static_layer,obstacle_layer,inflation_layer]static_layer:plugin:nav2_costmap_2d::StaticLayermap_topic:/local_AGgridmapmap_subscribe_transient_local:truesubscribe_to_updates:trueobstacle_layer:plugin:nav2_costmap_2d::ObstacleLayerobservation_sources:scan4. Rolling Window让局部栅格不随地图规模膨胀Rolling Window 是 osmAG-Nav 连接语义拓扑地图和 Nav2 执行层的关键桥梁。它不是提前把整栋楼渲染成一张巨大的栅格而是只围绕机器人当前位置生成固定窗口的占据栅格。论文实现中窗口大小为 50m × 50m更新频率为 10Hz。由于窗口尺寸固定局部 costmap 的内存占用与总场景面积无关从大 O 角度看就是相对于环境规模的 O(1) 局部地图内存。对大楼、园区和多层建筑来说这是从根上避免全局 costmap 爆炸的办法。Rolling Window 的生成过程可以理解为“按需栅格化”。系统先根据机器人所在楼层和窗口边界筛选可能相交的 Area 多边形然后只扫描这些多边形的局部包围盒用点在多边形内的测试把可通行区域填成 free cell非通道边界画成 occupiedPassage 对应的门洞或开口再显式打开。这样 Nav2 得到的地图仍然是它能理解的 OccupancyGrid但这张图只代表机器人附近的可执行空间而不是整栋楼。对局部控制器来说这已经足够因为它关心的是接下来几米到几十米内如何安全行驶。下面的伪代码展示 Rolling Window 的核心思路。它不是论文源码只是帮助理解实现流程输入是矢量区域、通道集合和当前机器人位姿输出是局部占据栅格。defbuild_local_ag_grid(area_graph,robot_pose,window_size50.0,resolution0.05):windowmake_square_window(centerrobot_pose.xy,sizewindow_size)gridOccupancyGrid(window,resolution,defaultUNKNOWN)candidate_areas[]forareainarea_graph.areas:ifarea.levelrobot_pose.levelandpolygon_intersects(area.boundary,window):candidate_areas.append(area)forareaincandidate_areas:forcellingrid.cells_in_bbox(area.boundary.bounds):pointgrid.cell_center(cell)ifpoint_in_polygon(point,area.boundary):grid[cell]FREEforwall_segmentinarea.non_passage_boundaries:grid.draw_line(wall_segment,valueOCCUPIED)forpassageinarea_graph.passages_on_level(robot_pose.level):ifsegment_intersects(passage.geometry,window):grid.draw_line(passage.geometry,valueFREE,thicknesspassage_width)returngrid5. 分层规划从通道图到 LCA 搜索osmAG-Nav 的全局规划不是在格子上搜索而是在 Passage-centric graph 上搜索。通道图的顶点是门、走廊开口、电梯接口等实际约束机器人通行的元素边表示在同一个叶子区域内从一个通道走到另一个通道的代价。这里有一个重要细节边权不是简单欧氏距离而是先把叶子区域局部栅格化再用栅格 A* 估计两个 passage 之间的可通行代价。这样可以避免“直线很近但中间隔着墙”的错误也能把柱子、隔断、狭窄开口造成的绕行反映到拓扑边权里。论文把边权写成三种情况普通平面区域使用 RasterAStarCost电梯或楼梯等垂直转移使用固定的 vertical transition cost局部栅格搜索失败时才退回欧氏距离。这个设计很关键因为它在全局层保留了拓扑稀疏性同时又没有完全丢掉度量空间中的真实可通行性。通道图因此不是抽象到只剩“房间 A 连房间 B”而是带着室内几何约束的 topometric graph。ifedgeisinside a planar leaf area:weightRasterAStarCost(area_raster,passage_i,passage_j)elifedge represents elevatororstair transition:weightvertical_transition_costelse:weighteuclidean_distance(passage_i,passage_j)真正让长距离规划保持低延迟的是 LCA 分层规划。LCA 是 Lowest Common Ancestor即最低公共祖先。假设起点在二楼某个房间终点在一楼某个走廊二者在地图树中的祖先可能是“房间 → 楼层 → 建筑”。规划器不需要在全图上盲目搜索而是先把起点和终点注入为临时 passage再从各自叶子区域 attach 到局部紧凑图然后沿父级 lift 到共同祖先附近在共同父级的 compact graph 上跑一次 A*最后把紧凑路径 expand 回完整 passage 序列。这个过程可以概括为Attach、Lift、Common-Parent A*、Expand。下面的伪代码展示分层规划的主流程。它省略了缓存、失败回退和同楼层约束等工程细节但保留了读者理解系统的关键路径。需要注意的是论文实现会缓存 area compact graphs 和 parent lift graphs在线查询时复用缓存而不是每次临时重建实验中的 cache ablation 也说明缓存是低毫秒级查询的关键原因之一。PassagePathplanHierarchical(constPosestart,constPosegoal,constOsmagMapmap){auto[s,start_area]injectVirtualPassage(start,map);auto[g,goal_area]injectVirtualPassage(goal,map);AreaId commonlowestCommonAncestor(start_area,goal_area,map.hierarchy());Frontier fsattachToLeafCompactGraph(s,start_area);Frontier fgattachToLeafCompactGraph(g,goal_area);while(parent(start_area)!common){fsliftThroughParentGraph(fs,start_area);start_areaparent(start_area);}while(parent(goal_area)!common){fgliftThroughParentGraph(fg,goal_area);goal_areaparent(goal_area);}CompactGraph query_graphassembleCommonParentGraph(common,fs,fg);CompactPath compact_pathaStar(query_graph,s.id,g.id);returnexpandCompactPath(compact_path,map.cached_traces());}6. 分段执行不是一次扔给 Nav2 一个超远终点全局规划器输出的是 passage 序列和对应的几何路径但 Nav2 的局部 costmap 只看得到 Rolling Window 内的空间。如果直接把几百米外的终点交给局部执行层Nav2 很可能因为目标不在当前地图中而无法处理。因此 osmAG-Nav 使用 Segmented Execution高层规划器保留整条全局路线控制权只把当前阶段最合适的中间目标发给 Nav2。机器人接近当前 passage 后规划器再切换到下一个 passage形成连续交接。当下一个 passage 超出当前局部栅格边界时系统会做 boundary-aware goal projection也就是沿全局路径寻找仍在当前/local_AGgridmap内的最远可行点把它作为代理目标发送给 Nav2。随着机器人前进Rolling Window 更新代理目标也会继续向真实 passage 推进。这个机制解决了全局路径长、局部地图短之间的矛盾同时不破坏 Nav2 的局部规划假设。论文的长程任务实验也观察到在地图切换和目标切换事件附近机器人速度没有出现明显坍塌说明分段交接不会天然导致走走停停。一个简化的目标调度逻辑如下。工程实现中还要考虑 action 状态、TF 变换、恢复行为、目标朝向和任务取消但核心就是“全局路径由 osmAG-Nav 管局部运动由 Nav2 管”。defdispatch_segmented_goals(route,robot_pose,local_grid):activeroute.current_passage()ifdistance(robot_pose.xy,active.center)goal_reach_threshold:route.advance()activeroute.current_passage()iflocal_grid.contains(active.center):goalmake_pose(active.center,heading_to_next_route_point(route))else:proxyfarthest_point_inside_grid(route.polyline,local_grid)goalmake_pose(proxy,heading_to_next_route_point(route))nav2_client.send_goal(goal)7. AGLoc只相信稳定建筑结构的定位…详情请参照古月居