自动驾驶 | SSTG-Nav 深度解析：把一次环境测绘变成可反复调用的物体导航基础设施

转载自公众号：敢敢AUTOHUB

简介

SSTG-Nav 研究的是长期驻留在同一栋房子、同一层办公楼里的服务机器人如何完成物体导航（ObjectNav）：用户说“去沙发那里”，机器人要走到一个合法位置并主动停下。主流方法把每次请求都当成一次陌生环境探索，而真实部署里更致命的问题在于，相机隔着门框、栏杆认出了沙发，并不等于机器人知道该在哪里停。作者的做法是先做一次与任务无关的 RGB-D 环境测绘，把视觉大模型（VLM）的逐视图检测通过深度反投影变成可到达的物体中心停靠点（standoff），再用可达性约束下的多视角融合合并证据，查询时按置信度排序、失败后切换空间上分开的备用点，并在抵达后用新拍的 RGB-D 复核。相关代码已经在github上了。

论文地址：https://arxiv.org/html/2608.00527v1
GitHub主页：https://github.com/DaojiePENG/sstg-nav-bench

1. 研究问题：长期驻留的机器人需要什么样的地图

1.1 一次性探索范式在固定场景里的代价

ObjectNav 的标准设定来自 Habitat 系列基准：智能体被放进一个从未见过的场景，接到“找到一把椅子”这样的类别指令后，需要边探索边识别，最后在距离目标合法视点 1 米以内的位置发出 STOP。这个设定催生了语义探索策略、视觉语言前沿地图（VLFM）、常识约束搜索、在线场景图等一大批方法，它们的共同出发点是环境未知。可是家庭、医院、写字楼里的服务机器人往往一驻就是几个月，同一个空间会被反复请求上百次。每次都从零探索，意味着之前花大量算力看到的东西全部作废，完成时间也随探索路径的好坏剧烈波动。核心问题在于，在这种场景里，评价标准应该从“单次探索有多高效”换成“整个服役周期内每次请求能否稳定完成”。

进一步看，持久化地图能带来几个在线方法给不了的性质：昂贵的感知推理只付一次；新的语言查询可以立刻得到候选位置；对一个模棱两可的物体可以同时保留多个假设；某个候选被否定后，机器人可以直接去下一个，而不必重新开始搜索。论文把这一设定单独定义为可复用物体导航（reusable ObjectNav），并把研究重点放在“记忆如何变得可执行”上，也就是地图里存的东西必须能直接告诉机器人在哪里停，而不只是记录相机曾经认出过什么。

1.2 观测位姿与停靠位姿之间的错位

这篇论文最值得记住的一个观察是observation-pose / STOP-pose mismatch。机器人在某个拓扑节点上拍到了一台电视，这台电视可能是透过门洞看到的，可能隔着一道楼梯栏杆，也可能位于家具的另一侧。若直接把“拍到电视的相机位置”当作导航终点，机器人确实能开过去，可那个位置离电视的合法视点可能有好几米，按 1 米阈值判定就是失败。换句话说，识别成功和导航成功之间隔着一层几何转换，过去很多语义地图工作把这层转换当作理所当然，或者干脆用最近的拓扑节点代替。

直觉理解：这有点像在商场二楼的栏杆边看到一楼有家咖啡店。你知道咖啡店在哪，但“我看见它的地方”并不是“我该走到的地方”，真正的目的地在一楼咖啡店门口，而通往那里的路要先绕到扶梯。SSTG-Nav 做的事情，就是把每一次“看见”换算成“该站在哪”。

这个错位具体长什么样，值得单独画一张图。论文图 2 给的是一次真实沙发观测的审计截图，信息密集但不够直观，下面这张示意图把三种典型失败情形抽出来并排放在一起，读者可以先在脑子里建立“观测点不等于停靠点”的几何图像，再回头看第 4 节的公式。

论文还指出一个容易被忽视的评测陷阱：如果地图节点本身就是从官方目标视点附近采样的，那么地图里天然包含成功的停靠位置，此时得到的高分其实来自目标信息泄露。作者把这种情形称为 target-view coverage，只把它当作受控上界，而把真正要解决的问题设定为：建图时完全不知道未来任务，事后再把视觉证据转换成可到达、经多方印证、到达后可复核的物体目的地。

1.3 三套评测协议划清信息边界

为了让读者能分清每个数字背后用到了多少特权信息，论文设计了三套协议，信息边界见下表。第一套 target-view coverage 用官方目标视点决定拍摄位置，只用来测量“覆盖充分时语义检索还能差多少”；第二套 independent oracle 用与目标无关的拓扑，但语义标签由评测器事后根据真值赋予，用来测量纯几何覆盖的天花板；第三套 independent real 是论文的主协议，从拓扑构建、RGB-D 采集、VLM 推理、投影、融合到检索，全程看不到任何起点、类别、物体标注和目标视点，这些信息只在候选选定之后才被评测器读取，用于计算成功率与距离误差。

这里要厘清的是，三套协议的数字不能横着比。target-view 的 0.990 和 independent oracle 的 0.994 都是上界，前者靠目标视点，后者靠真值标签；只有 independent real 的 0.926 与 0.975 才对应可部署系统。仓库的 docs/core-pipeline.md 也专门列了一张“哪些模块在何时读取官方 goal”的表，并明确写出 Top-K 的 Success@K 假设每次访问后评测器会告诉智能体是否成功，属于诊断上限，而自主 STOP 的结果要看 arrival_evaluate 的输出。

2. 整体框架：一次测绘，多次查询

2.1 离线测绘与在线查询的分工

SSTG-Nav 把系统拆成两条泳道。离线泳道只跑一次：在场景里撒点构建拓扑图，在每个节点拍四个方向的 RGB-D，送进 VLM 做逐视图检测，再把检测框通过深度投影成三维物体估计和可到达的停靠点，最后按类别做融合。在线泳道对每条用户请求都要跑：把自然语言解析成类别，从持久地图里取出候选并排序，在拓扑图上规划路径，交给 Nav2 执行，到达后拍新的 RGB-D 让 VLM 复核，通过则 STOP，不通过就前往下一个候选。论文图 1 把这两条泳道画得很清楚，评测器只出现在最右侧单独的一条通道里，在决策结束后才接触目标标注。

图 2：SSTG-Nav 系统总览，一次与目标无关的测绘生成持久的度量语义拓扑，支撑反复的自然语言导航（取自原论文 Figure 1）

从图中可以读出一个关键设计：交互管理器在每次请求中构造结构化语义查询，检索并排序多个候选，协调图规划与 Nav2 执行，再根据到达视图决定停还是走。被否定的假设会直接触发去下一个候选，澄清、进度、恢复、完成都通过双向自然语言接口反馈给用户。这意味着系统从设计上就假设“第一个候选可能是错的”，恢复不是补丁，而是主流程的一部分。

2.2 地图里到底存了什么

形式化地，预先测绘的场景包含可通行空间  和图 。每个导航节点  记录三维位置 、朝向  和该点拍到的 RGB-D 视图集合 ；边连接局部可达的节点，权重是 navmesh 上的测地线距离。一次查询给出类别  和起点 ，系统返回一个排好序的度量候选  以及从起点到它的最短可行路径。成功判定沿用 ObjectNav 官方标准， 是该类别所有合法目标视点：

路径效率用 SPL 衡量， 是官方最短路径长度， 是实际规划路程，成功时取 ，失败记 0；DTG 则是最终停靠点到最近合法视点的测地线距离，越小说明停得越准。三个指标各管一面：SR 看能不能到，SPL 看绕没绕远路，DTG 看停的位置离合法区域有多近。后面的消融里经常出现 SR 上升而 SPL 略降的情况，读的时候要把它理解成“为了更可靠，选了稍远的目标”，而不是系统退步。

和 VLMaps、ConceptFusion 这类把语言特征稠密融合进三维重建的路线相比，SSTG-Nav 的表示刻意更轻：它只存导航节点加上度量化的物体候选，不存逐像素或逐体素的特征场。这里值得注意的取舍是，轻量表示牺牲了开放词汇的空间关系查询能力（比如“沙发和电视之间”），换来的是候选可以直接作为导航目标使用，并且每个候选都能追溯到它来自哪张图、哪个框、哪个深度值。

工程价值：每个候选在仓库里都带着 source_rgb_path、source_depth_path、bbox_norm、depth_m 和 source_topology_node 字段。出错时可以一路倒查到原始像素，这对长期运行的机器人做故障审计很重要，稠密特征场很难做到这一点。

3. 第一步：与目标无关的空间拓扑

3.1 最远点采样保证覆盖半径

建图的第一步是在可通行空间里布点。作者没有沿用“在目标附近放节点”的做法，而是用固定随机种子从 navmesh 上抽 12000 个可行走点作为候选池，再做贪心最远点采样：从最靠近池子质心的点开始，每次加入离已选集合最远的那个点，直到池中任意一点到最近已选点的距离都不超过半径  米。整个过程不读取任何 episode 起点、类别或目标视点，输出的 JSON 里还写死了 goal_annotations_used: false 作为审计标记。

# sstg_bench/uniform_map.py
def farthest_cover(pool: np.ndarray, radius: float, max_nodes: int) -> tuple[np.ndarray,float]:
    """Greedy farthest-point cover over an annotation-independent sample pool."""
    center=np.mean(pool,axis=0)
    first=int(np.argmin(np.sum((pool-center)**2,axis=1)))
    selected=[first]
    nearest_sq=np.sum((pool-pool[first])**2,axis=1)
    while len(selected)<max_nodes:
        index=int(np.argmax(nearest_sq))
        if nearest_sq[index] <= radius*radius:
            break
        selected.append(index)
        nearest_sq=np.minimum(nearest_sq,np.sum((pool-pool[index])**2,axis=1))
    return pool[selected],float(math.sqrt(float(np.max(nearest_sq))))

这段代码的实现很朴素，但有两个细节值得留意。其一，nearest_sq 维护的是池中每个点到已选集合的最近平方距离，每加一个新点只需和它做一次 np.minimum 更新，单轮复杂度 ，总复杂度 ，其中  是池大小、 是最终节点数。其二，停止条件不是固定节点数，而是“最远的那个点也已经在半径之内”，所以小户型和大户型会自动得到不同数量的节点。论文报告整个验证集 36 个场景共得到 6642 个节点、21845 条边，单场景从 74 到 399 个节点不等，说明覆盖规则确实在随场景尺度自适应。

3.2 建边与四方位 RGB-D 采集

节点选好后，每个节点只考虑欧氏距离最近的 8 个邻居，并且要求欧氏距离不超过 2.4 米、navmesh 上存在路径、测地线长度不超过 3.84 米（即 2.4 的 1.6 倍）才连边。这个 1.6 倍系数的作用是过滤掉“欧氏上很近、实际要绕一大圈”的点对，比如墙两侧的两个节点。随后 rgbd_capture.py 在每个节点按 0°、90°、180°、270° 四个方向各拍一张 RGB 和一张浮点深度图，相机安装在机身上方 1.25 米处，主实验使用 640×640、水平视场 120° 的广角配置。

# sstg_bench/uniform_map.py
def build_edges(sim, points: np.ndarray, neighbors: int, max_edge: float):
    edges=[]
    for i,p in enumerate(points):
        distances=np.linalg.norm(points-p,axis=1)
        for j in np.argsort(distances)[1:neighbors+1]:
            j=int(j)
            if j<=i or distances[j]>max_edge:
                continue
            ok,geodesic,_=shortest(sim,p,points[j])
            if ok and math.isfinite(geodesic) and geodesic<=max_edge*1.6:
                edges.append({"source":i,"target":j,"geodesic_distance":geodesic})
    return edges

这里的关键是四个视图分开送给 VLM，而不是拼成全景图让模型在全景坐标里框物体。拼接全景会让检测框横跨两个相机、坐标系含混，后续深度反投影就没法用单一内参算。论文附录也记录了一个反例：本地 Qwen2.5-VL-3B 在 2×2 拼图上偶尔会输出退化的非 JSON 文本，作者不得不对失败节点改回逐视图独立提问来修复。逐视图处理看起来多花了一点 token，但保住了每张图干净的相机模型。

难点提示（为什么要分开四个视图）：可以把四张图想象成四台独立的监控摄像头，每台都有自己的镜头参数和朝向。让模型在一张拼出来的大图里画框，等于要求它理解“这个框的左半边属于 1 号摄像头、右半边属于 2 号摄像头”，深度图也就对不上了。逐视图输出的框只属于一台相机，反投影公式才能直接套用。

4. 度量接地：从检测框到可停靠点

4.1 深度反投影出物体表面点

VLM 对每张图返回的检测是一个四元组 ，分别是类别、归一化检测框、置信度和视图编号。SSTG-Nav 取框中心 ，在深度图对应位置的 7×7 小块里取有效深度的中位数 （只接受 0.2 到 6.0 米的值），再用相机内参  和世界到相机的变换  反投影出物体表面点：

取中位数而不是单像素，是为了抗住框中心刚好落在物体边缘、背后墙面或深度空洞上的情况。仓库里的实现没有显式构造矩阵 ，而是直接用视场角换算：水平视场  已知，垂直视场由宽高比推出，。标准相机 640×360、90° 水平视场对应约 59° 垂直视场，方形广角相机两个方向都是 120°。

# sstg_bench/rgbd_fusion.py
def median_depth(path,center,max_depth):
    depth=np.load(path,mmap_mode="r");height,width=depth.shape
    x=int(round(center[0]*(width-1)));y=int(round(center[1]*(height-1)))
    patch=np.asarray(depth[max(0,y-3):min(height,y+4),max(0,x-3):min(width,x+4)])
    values=patch[np.isfinite(patch)&(patch>.2)&(patch<max_depth)]
    return float(np.median(values)) if values.size else None

def project(node,view,detection,depth):
    # Habitat depth is camera-forward Z depth. Camera frame is +X right,
    # +Y up and -Z forward; the sensor is 1.25 m above the agent state.
    width,height=view.get("resolution",[640,360]);hfov=math.radians(float(view.get("hfov_deg",90)))
    vfov=2*math.atan((height/width)*math.tan(hfov/2))
    x=(detection["center"][0]-.5)*2*math.tan(hfov/2)*depth
    y=-(detection["center"][1]-.5)*2*math.tan(vfov/2)*depth;z=-depth
    rotation=node.get("rotation",[0,0,0,1]);base_yaw=2*math.atan2(float(rotation[1]),float(rotation[3]))
    yaw=base_yaw+math.radians(view["yaw_offset_deg"]);c=math.cos(yaw);s=math.sin(yaw)
    px,py,pz=node["position"]
    return np.asarray([px+c*x+s*z,py+1.25+y,pz-s*x+c*z],dtype=np.float32)

project 里有几处 Habitat 特有的约定，复现时很容易踩坑。Habitat 的深度是沿相机前向的  深度而非射线长度，所以横向偏移直接用  计算；相机坐标系是 +X 向右、+Y 向上、-Z 向前，因此 z=-depth；图像纵坐标向下增长，所以  前面有负号。最后把节点自身偏航角和视图偏航偏移相加，绕竖直轴旋转到世界系，并加上 1.25 米的相机安装高度。这意味着整个几何链条只依赖节点位姿和视场角两个量，换一台机器人只要改这两个参数。

4.2 沿观察方向后撤 0.8 米并吸附到 navmesh

拿到物体表面点后，SSTG-Nav 并不把它当终点，因为物体表面本身通常不可通行，椅面、床垫、沙发靠背都不在 navmesh 上。作者在水平面上沿“物体指向来源相机”的方向后撤  米，选这个方向的理由很直接：相机既然能从那一侧看到物体，那一侧大概率是开阔的、可以站人的。公式中  表示去掉竖直分量，只在地面平面内移动：

得到的期望点  会被吸附（snap）到最近的可通行网格上；如果从来源节点出发到这个点没有路径，就直接丢掉这条检测。论文强调这一步只用深度做几何，语义置信度仍然是 VLM 的原始输出，没有被深度信息“加工”过，所以后面的消融可以干净地把几何增益和语义增益分开。

# sstg_bench/rgbd_fusion.py
def candidate_from_detection(sim,root,node,detection,standoff,max_depth):
    view=node["rgbd_views"][detection["view_index"]];depth=median_depth(root/view["depth_path"],detection["center"],max_depth)
    if depth is None:return None
    object_position=project(node,view,detection,depth);source=np.asarray(node["position"],dtype=np.float32)
    direction=source[[0,2]]-object_position[[0,2]];norm=float(np.linalg.norm(direction))
    if norm<.2:return None
    desired=object_position.copy();desired[[0,2]]+=direction/norm*standoff;desired[1]=source[1]
    snapped=np.asarray(sim.pathfinder.snap_point(desired),dtype=np.float32)
    if not np.isfinite(snapped).all():return None
    ok,_,_=shortest(sim,source,snapped)
    if not ok:return None
    return {"position":snapped.tolist(), "object_estimate":object_position.tolist(),
            "source_topology_node":node["id"], "category":detection["category"],
            "confidence":detection["confidence"], "depth_m":depth, ...}

代码里 [[0,2]] 取的是 Habitat 坐标系下的 x 和 z 两个水平分量，正好对应公式里的 。norm<.2 这一行处理一个退化情形：物体估计点几乎就在相机正下方时，后撤方向没有意义，直接丢弃。期望点的高度被设成来源节点的高度 desired[1]=source[1]，避免物体在桌面上时停靠点也被抬到桌面高度。三层过滤（深度无效、方向退化、吸附后不可达）叠在一起，保证进入地图的每个候选都是机器人真能走到的位置。

图 3：一次经过审计的沙发观测，VLM 逐视图检测框索引对齐深度，中位表面点经反投影后后撤到 0.8 米处的可达停靠点（取自原论文 Figure 2）

一句话理解：检测框告诉你“是什么”，深度告诉你“有多远”，后撤加吸附告诉你“该站哪”。论文在固定同一批 GPT-5.4 检测结果的前提下，只加这一步就把成功率从 0.835 提到 0.920，配对比较中 93 条从失败变成功、只有 8 条反过来，精确 McNemar 检验 。

5. 可达性感知的软融合

5.1 三重约束防止穿墙合并

同一把椅子会被周围好几个节点、好几个视图同时看到，如果每次检测都单独当一个候选，地图会被大量重复点淹没，排序也会被单视图误检干扰。SSTG-Nav 按类别做聚类，两个候选被连成一组需要同时满足三个条件：物体估计的水平距离不超过  米，竖直高度差不超过  米，两个停靠点之间存在路径且测地线距离不超过  米。前两条是常规的空间邻近，第三条才是这篇工作的特色。

# sstg_bench/rgbd_fusion.py
def components(sim,candidates,radius,vertical_tolerance,max_stop_geodesic):
    parent=list(range(len(candidates)))
    def find(i):
        while parent[i]!=i:parent[i]=parent[parent[i]];i=parent[i]
        return i
    def union(i,j):
        a,b=find(i),find(j)
        if a!=b:parent[b]=a
    for i,a in enumerate(candidates):
        pa=np.asarray(a["object_estimate"])
        for j in range(i):
            pb=np.asarray(candidates[j]["object_estimate"])
            if abs(pa[1]-pb[1])>vertical_tolerance or np.linalg.norm(pa[[0,2]]-pb[[0,2]])>radius:
                continue
            # Euclidean proximity alone can merge detections through walls or
            # across disconnected floor islands.  Such candidates must remain
            # independent navigation hypotheses.
            reachable,geodesic,_=shortest(sim,a["position"],candidates[j]["position"])
            if reachable and math.isfinite(geodesic) and geodesic<=max_stop_geodesic:
                union(i,j)
    ...

实现上用的是带路径压缩的并查集，连通分量就是聚类结果。两重循环是  的，但欧氏距离和高度差是廉价的前置过滤，只有通过前置过滤的点对才会调用 navmesh 最短路查询，所以实际开销集中在少量近邻对上。源码注释点明了第三条约束存在的理由：只看欧氏距离，墙两侧的两次检测、或者分属两层楼的两次检测都可能被合并成一个物体，导航时就会出现“目标在墙那边”的错误。

难点提示（为什么要在停靠点上检查可达性）：两个人在同一栋楼里报告“我看到一台电视”，相隔不到一米，但一个在卧室、一个在隔壁客厅，中间隔着一堵墙。坐标上看他们说的是同一台电视，走路过去却是两个完全不同的地方。用停靠点之间的测地线距离做门槛，相当于问“从这个站位走到那个站位要不要绕一大圈”，绕得远就说明不是同一个可用目标。

5.2 Noisy-OR 打分与独立来源计数

聚类之后要给每个簇打一个置信分。直接把簇里所有检测的置信度累加或取平均都有问题：同一张图里 VLM 可能对同一物体重复输出好几个框，这些框不是独立证据，不该让分数虚高。SSTG-Nav 先按来源拓扑节点分组，每个节点只保留它在该簇里的最高置信度，然后对这些独立来源做 noisy-OR：

其中  是观测到簇  的独立节点集合， 是节点  贡献的检测。直观理解是，只要有一个独立来源说对了，这个物体就存在；三个来源各自 0.7 的置信度，合起来就是 。代码里额外把单项置信度截断在 0.99，避免某一个来源报出 1.0 后把乘积直接压成 0，导致后续排序全部饱和。

# sstg_bench/rgbd_fusion.py
for group in components(sim,category_items,args.cluster_radius,args.vertical_tolerance,args.max_stop_geodesic):
    support=len({item["source_topology_node"] for item in group});best=max(group,key=lambda x:x["confidence"])
    if support<args.min_support and best["confidence"]<args.keep_single_confidence:continue
    # Independent observations increase confidence without allowing
    # duplicate boxes from the same frame to inflate support.
    source_confidence={}
    for item in group:
        source_node_id=item["source_topology_node"]
        source_confidence[source_node_id]=max(source_confidence.get(source_node_id,0.),item["confidence"])
    score=1.-float(np.prod([1.-min(.99,value) for value in source_confidence.values()]))
    representative=as_node(best,cluster_id,score,support,len(group))
    fused.append(representative)
    for member in group:                       # 保留簇内全部停靠点作为备用几何
        multi_standoff.append(as_node(member,len(multi_standoff),score,support,len(group)))

保留规则是：至少两个独立节点支持，或者是置信度不低于 0.92 的高置信单例（全量验证集设置）。每个保留簇用置信度最高的成员作为代表停靠点，进入融合主库 ；所有通过深度过滤的原始停靠点另存为残差库 ，在代码里对应 raw_maps/。全量 GPT-5.4 运行中，20107 个有效深度候选被压缩成 1329 个融合假设，压缩率 93.4%，DTG 从 0.879 米降到 0.747 米。

三重约束加 noisy-OR 这套机制，文字和代码读起来都比实际几何更绕，所以这里补一张示意图。论文附录 Figure 1 画的是两个 minival 场景融合前后的俯视点云对比，能看出压缩效果，但看不出一个簇是怎么被判定成立、分数又是怎么算出来的，下面这张图专门拆这两件事。

5.3 双层存储：主库求稳，残差库保多样

为什么不只保留融合后的代表点？论文给出的理由很实在：一个簇只留一个代表，等于把这个物体所有可能的停靠侧都压成了一个点，如果这个点恰好在沙发靠墙的一侧、或者床的背面，那么即便语义是对的，导航也会失败，而此时其余站位已经被丢掉了。双层存储让主库负责“该去哪个物体”，残差库负责“换个角度站”。附录的配对分析也印证了这种分工的必要性：在全量数据上，raw 到 fused 的单目标比较是 28 条变好、22 条变差，SR 只净增 0.006，SPL 还略降 0.017，说明融合本身更像是一次“去噪加压缩”，它的价值要在多候选恢复里才完全兑现。

解读：融合把候选从两万压到一千出头，主要好处是让排序更稳、误检更难排到第一位；代价是丢掉了簇内的几何多样性。把原始停靠点留作备用库，正好补上这个代价，第 7 节的 Top-3 实验会给出直接证据。

6. 查询、排序与执行

6.1 字典序排序：语义支持优先，路程只做平局裁决

用户请求  先被语言规范器映射成类别 ，比如“帮我去看看电视那边”会被归一化成 tv_monitor。然后系统从融合主库里取出同类、且从当前起点可达的候选集合 从可达。这一步完全不涉及视觉推理，只是查表加一次可达性检查，所以响应几乎是即时的。排序规则是一个字典序排列：

也就是先按融合分  从高到低排，分数相同时才比较从起点出发的图上路程 。这个看起来平淡的选择背后有明确考虑：如果把路程放在首位或者做加权，一个就在机器人旁边、只有单视图支持的误检，很容易排到多视角印证的真目标前面。作者也没有在查询端引入任何学习得到的打分策略，整个排序是确定性的，可复现、可解释。

# sstg_bench/topk.py
RANKING_STRATEGIES=("category_score","support_confidence","confidence_support")

def candidate_rank_key(node,category,distance,strategy="category_score"):
    """Return a stable query-time ordering key without evaluator signals."""
    score=float(node.get("category_scores",{}).get(category,0.))
    confidence=float(node.get("confidence",score))
    support=int(node.get("cluster_support",1))
    if strategy=="category_score":return (-score,distance)
    if strategy=="support_confidence":return (-support,-confidence,distance)
    if strategy=="confidence_support":return (-confidence,-support,distance)
    raise ValueError(f"unknown ranking strategy: {strategy}")

仓库实现了三种排序键，主库固定用 category_score，也就是公式里的 ；残差库用 confidence_support，先比单视图置信度、再比簇支持数、最后比距离。Python 的元组比较天然就是字典序，所以这里返回元组再统一 sort 即可。仓库测试 tests/test_topk_ranking.py 里有一个用例专门验证：两个候选融合分都饱和到 1.0 时，support_confidence 会让支持数 12、距离 8 米的远处候选排在支持数 2、距离 2 米的近处候选之前，这正是“证据优先于路程”的设计意图。

6.2 融合感知的 Top-3 候选序列

第一个访问点来自主库排序后的 ；后面最多两个备用点来自残差库中同类的 ，按置信度和路程排序，同时强制与所有已选访问点的欧氏距离不小于  米。如果主库里该类别为空，就直接拿残差库里最好的可达点当第一个访问点。对照组 fused-only Top-3 则三次都从主库里取。两者的差别在于，前者第二、第三次尝试换的是“站位”，后者换的是“物体”。

# sstg_bench/topk.py
def append_distinct(selected,ranked,count,max_k,min_separation):
    added=0
    for _,_,node in ranked:
        position=np.asarray(node["position"],dtype=float)
        if any(np.linalg.norm(position-np.asarray(previous["position"],dtype=float))<max(1e-4,min_separation)
               for previous in selected):continue
        selected.append(node);added+=1
        if added>=count or len(selected)>=max_k:break
    return selected

def hierarchical_candidates(primary_ranked,fallback_ranked,max_k,min_separation,primary_k,secondary_ranked=None,secondary_k=0):
    """Select primary representatives, optional middle-tier candidates, then distinct residual standoffs."""
    selected=diverse_candidates(primary_ranked,min(primary_k,max_k),min_separation)
    if secondary_ranked and secondary_k>0:
        append_distinct(selected,secondary_ranked,secondary_k,max_k,min_separation)
    return append_distinct(selected,fallback_ranked,max_k-len(selected),max_k,min_separation)

append_distinct 里的 max(1e-4,min_separation) 是一个容易被忽略的细节：即使把分隔距离设成 0，也会跳过与已选点完全重合的候选。这是因为融合代表点本身就是残差库里的某一个原始点，不加这道保护，第二次尝试可能原地不动地再访问一次第一个点。论文 headline 配置对应 primary-k=1、残差库 confidence_support 排序、min-separation=2.0，由 tools/run_full_rgbd_benchmark.sh 复现；中间那一层 secondary 是代码额外支持的诊断选项，不影响主结果。

6.3 后退式执行：失败不重来

执行阶段是一个后退式的候选序列，而不是几次互相独立的“起点到目标”试验。这个区别在评测上很重要：如果每次尝试都从起点重新出发，第二、三次尝试的路程就被低估了，SPL 会显得比实际更好。SSTG-Nav 的做法更贴近真实机器人，被否定后就在原地重新规划。设  为请求起点，第  段路径从上一个候选位置出发：

实际走过的总路径是 ，其中  是第一个通过到达复核的候选编号。这意味着 SPL@K 里的路程  是所有尝试段的累加，被否定的候选会真实地拉低路径效率，评测不会因为“多试几次”而免费获益。仓库里 topk.py 的 evaluate_episode 和 arrival_evaluate.py 都用 current=node["position"] 加 cumulative+=leg 实现这一点。

论文还给出了一个摊销成本的表达，用来说明复用地图的经济性。这个公式本身很简单，但它把论文的立论基础写清楚了：可复用范式之所以划算，是因为最贵的那部分开销只付一次，而在线方法每条请求都要重新付感知和探索的钱。若一次测绘服务  次请求，每次请求的非运动开销为

VLM 推理和深度接地只付一次，摊到每次请求上随  增大而趋于零；之后每次请求只剩检索、图搜索和到达复核。检索排序是 ， 是该类别候选数，图搜索是 。参考附录数据，全量验证集一次测绘调用 GPT-5.4 共 6642 次请求、消耗约 4450 万 token，这笔一次性投入在后续上千次查询里被摊薄。

直觉理解：这和外卖骑手熟悉片区的过程很像。第一周跑单要边看导航边找楼栋入口，成本很高；跑熟之后，接到单子脑子里直接浮现“3 号楼的门在背面”，剩下的只是骑过去。SSTG-Nav 的测绘就是那第一周，摊销公式说的是跑的单越多，第一周的代价越不值一提。

7. 到达复核：拿新鲜 RGB-D 决定 STOP

7.1 四个条件同时满足才停

前面所有步骤都只产生“候选”，真正决定停不停的是到达后的复核。机器人抵达第  个候选后，重新拍四张 120° 的 RGB-D，第 0 张正对地图里存的物体估计方向，其余依次旋转 90°。GPT-5.4 同时收到这四张新图和一张画了品红色框的建图期参考图，返回目标是否可见、停靠侧是否合理的判断 、到达视图里的框以及置信度 。系统再用返回框的中心区域去对齐深度图，取中位深度 。STOP 条件写成：

四个条件里，可见性和停靠侧来自 VLM 的语义判断，置信度阈值过滤犹豫的回答，深度区间则是一项纯几何检查，保证目标确实在一个合理的近距离范围内。被否定时，机器人从当前位置规划到下一个候选。整个决策过程不读取任何官方目标，目标视点只在决策结束后用来打分和统计复核器的混淆矩阵。

# sstg_bench/arrival_vlm.py
def depth_median(depth_path: Path, bbox: list[float]) -> float | None:
    depth = np.load(depth_path)
    height, width = depth.shape[:2]
    x1, y1, x2, y2 = bbox
    # Use the central 50% of the returned box to reduce background leakage.
    px1 = int(np.clip((0.75 * x1 + 0.25 * x2) * width, 0, width - 1))
    px2 = int(np.clip((0.25 * x1 + 0.75 * x2) * width, px1 + 1, width))
    py1 = int(np.clip((0.75 * y1 + 0.25 * y2) * height, 0, height - 1))
    py2 = int(np.clip((0.25 * y1 + 0.75 * y2) * height, py1 + 1, height))
    patch = depth[py1:py2, px1:px2]
    valid = patch[np.isfinite(patch) & (patch > 0.05)]
    return float(np.median(valid)) if valid.size else None

def finalize_decision(parsed: dict, candidate: dict, root: Path, args) -> dict:
    ...
    result["checks"] = {
        "visible": parsed["target_visible"],
        "confidence": parsed["confidence"] >= args.min_confidence,
        "depth_valid": depth is not None and args.min_depth <= depth <= args.max_depth,
    }
    result["checks"]["vlm_stop_geometry_valid"] = parsed["stop_geometry"] == "valid"
    required = ("visible", "confidence", "depth_valid")
    if args.require_vlm_geometry:
        required += ("vlm_stop_geometry_valid",)
    result["accept"] = all(result["checks"][key] for key in required)

建图阶段用的是框中心 7×7 小块，到达复核用的是框中间 50% 的区域，后者面积更大，因为到达时物体通常离得更近、在画面里占比更大，取中心一半既能多拿样本，又能避开框边缘漏进来的背景墙面。require_vlm_geometry 开关对应论文表 10 的两种策略：严格的“双重几何”同时要求 VLM 说停靠侧有效和深度区间合法；另一种只看类别加 RGB-D 深度区间，忽略 VLM 的停靠侧判断。

7.2 复核器的提示词设计

复核提示词写得非常防御性。它明确告诉模型参考图里的框可能本身就是误检、倒影或者屏幕里的画面；只有当目标类别的真实物理实例在到达视图里清晰可见，且机器人看起来位于一个无遮挡、合理的停靠侧时才接受；如果目标只出现在参考图里、出现在电视或海报上、隔着墙或玻璃或栏杆，一律拒绝；不允许凭房间上下文推断，也不要求模型猜距离，因为深度由机器人另行检查。输出被限定为固定字段的 JSON，reason_code 还区分了 not_visible、wrong_category、reference_false_positive、barrier 等失败原因，方便事后统计。

这里要强调的是语义与几何的分工：VLM 擅长回答“这是不是一把真椅子、我是不是隔着栏杆”，却不擅长估计“离我 1.8 米还是 3.2 米”；深度图正好相反。把距离判断从提示词里剥离出去交给深度，是这套复核器准确率较高的原因之一。全量验证中，严格策略的复核精确率为 0.964、召回率 0.915，平均每条任务只尝试约 1.11 个候选；656 个唯一候选的复核延迟均值约 20.5 秒，消耗约 473 万 token。

难点提示（复核器的指标为什么比 Top-K 上界低）：Top-K 表里的 Success@3 假设每走到一个候选，评测器都会如实告诉机器人“对了”还是“错了”，这是候选列表本身的潜力上限。自主复核要靠 VLM 和深度自己判断，会有误拒（真目标被否掉）和误收（假目标被接受）。所以严格自主复核的 S@3 是 0.912，比 Top-K 的 0.975 低，这个差距衡量的是“停止信号”的质量，而不是“候选地图”的质量。

8. 完整流程串讲：一条请求从输入到停下

8.1 离线五步

把前面几节串起来，离线阶段按顺序做五件事。第一，uniform_map 在 navmesh 上最远点采样出覆盖半径 0.8 米的拓扑并连边；第二，rgbd_capture 在每个节点拍四方位 RGB-D；第三，peterai_rgbd_semantics 等语义模块把四张图一次性发给 VLM，拿回逐视图的类别、框和置信度，响应会被原子写入缓存以便断点续跑；第四，rgbd_fusion 做深度反投影、后撤、吸附和可达性检查，得到原始停靠点；第五，同一个脚本按类别聚类、做 noisy-OR 打分，输出 raw_maps/、clustered_maps/ 和 multi_standoff_maps/ 三类地图。论文算法 1 描述的就是这五步，它的输入里没有任何查询或 episode 字段。

为了让读者看清这五步和在线环节怎么衔接，下面补一张流程示意图。论文图 1 侧重系统模块，没有把“离线产物如何被在线查询逐层消费”单独画出来，这张图专门补这一块。读图时可以留意两个地方：中间的存储层同时存了融合主库和残差库，这是第 5.3 节双层设计的直接体现；在线层复核之后有一条指回“下一个候选”的回环箭头，它对应的就是第 6.3 节的后退式执行。

8.2 在线三步与工程部署

在线阶段对每条请求只做三件事：检索排序、规划执行、到达复核。在 ROS 2 实体系统里，作者把它落在一台 Yahboom X3 移动底盘上，传感器是 Gemini 336L RGB-D 相机和 RPLIDAR S2 激光雷达，slam_toolbox 负责占据栅格建图与定位，Nav2 负责执行度量目标。持久管理器跨导航段保存拓扑图和交互状态，语言接口负责类别解析并把支持该候选的视图展示给用户。论文图 5 展示了一次完整演示：查询“玩具”后在 Node 22 以 0.81 的置信度检索到目标，下发给 Nav2 后机器人抵达玩具旁。

9. 总结

SSTG-Nav 把物体导航从“每次都在陌生环境里重新找”换成“先认真测绘一次，然后反复调用”。它的技术链条可以压缩成四步：与目标无关的最远点拓扑保证覆盖，深度反投影加后撤把检测换成可停靠点，可达性约束下的 noisy-OR 融合把两万个噪声候选压成一千多个可信假设，双层候选加到达复核让第一次站错时能就近换个站位再试。在 HM3D-v2 全量验证上，这条链条把成功率从 0.835 依次推到 0.920、0.926、0.975，而几何覆盖上限是 0.994，剩下的差距主要落在语义可靠性上。

对于正在做服务机器人落地的团队，这篇工作给出的启示很具体：先把一次高质量测绘做扎实，把每个检测都换算成可执行的站位并保留出处，再在到达时用新鲜观测兜底。它没有依赖专门训练的导航策略，所有组件都可以在 ROS 2 和 Nav2 生态里找到对应。一句话总结：对长期驻留的服务机器人而言，地图要记住的是该站在哪里，以及站错时下一步去哪。