Nav2 返回 SUCCEEDED，小车也停在目标点附近。按普通导航演示的标准，这个节点已经可以收尾了。

可我们给这次运行的最终判定却是 FAIL。

原因不在路径规划：托盘在运输过程中相对车载甲板滑了 5.2158 mm，超过预先设定的 5 mm 保持门限。它随后又被保持机构拉回到约 2.5 mm，但最大滑移已经发生，不能拿终点的一帧画面把这次越界抹掉。

这篇文章记录的，就是一次“导航成功，搬运失败”的本地 MuJoCo + ROS 2 Nav2 实验，以及我们如何把失败定位到车辆、甲板、托盘和验收逻辑之间。

一、先说清楚实验范围

场景是一台搭载托盘的 AMR，在同一 MuJoCo 物理世界中接受 Nav2 速度命令，完成一段约 0.6 m 的站内短程导航。报告来自本地运行 p4-nav2-v7-03，不是根据预期日志编造的例子。

但有一条边界必须放在前面：这次运行的货物是场景初始化时放好的，不是人形机器人供盘、机械臂装料、车辆接货之后接着跑出的完整订单。地图来自已知场景的静态先验，也不能把它称为这次重新完成了 SLAM。本文只讨论带初始载荷的导航与验收。

导航系统回答的是“车有没有执行目标”；搬运系统还要回答“货有没有跟着车、有没有安全停下”。我们为后者单独设了物理判据。

二、第一层结果：导航确实到了

这次运行的 Nav2 worker 报告为 SUCCEEDED，目标被接受，也确实产生了非零运动命令。独立的仿真真值检查给出：

项目	本地实测	判定
Nav2 action	SUCCEEDED	PASS
到站位置误差	0.14786 m	小于 0.25 m，PASS
车辆停车速度	0.00000235 m/s	小于 0.01 m/s，PASS
车辆停车窗口位移	0.000000043 m	小于 0.005 m，PASS
运输姿态许可	全程有效	PASS

也就是说，不应把这次失败甩给“Nav2 没到点”。车动了、到了、停了；真正没有通过的是载货判据。

当次报告中的关键字段可以压缩成这样：

nav2_succeeded                 PASS
independent_arrival           PASS
physical_stop                 PASS
transport_posture_authorized  PASS
tray_retained                 FAIL
commissioning_result          FAIL

三、第二层结果：车停住了，不代表托盘始终稳住了

如果只计算托盘的世界坐标位移，车一开动，托盘当然也在移动；这个数值不能说明它相对甲板有没有滑动。我们的判据先把托盘位置转到甲板局部坐标系，再与起始位置比较：

p_rel(t) = R_deck(t)^T · [p_tray(t) - p_deck(t)]
slip_max = max_t ‖p_rel,xy(t) - p_rel,xy(0)‖

项目中的核心计算相当于下面这段（为便于阅读做了变量整理，非独立运行脚本）：

def tray_position_in_deck(data):
    tray = data.body("c_payload")
    deck = data.body("c_deck")
    rotation = deck.xmat.reshape(3, 3)
    return (tray.xpos - deck.xpos) @ rotation

initial = tray_position_in_deck(data).copy()

# 每次物理采样后记录当前位置，整段运行结束再取最大值。
slip_max = max(
    np.linalg.norm(sample[:2] - initial[:2])
    for sample in tray_positions_in_deck
)
tray_retained = slip_max <= 0.005

这里看的是整段运输的峰值，不是终点位移。p4-nav2-v7-03 在仿真时间约 7.41 s 时出现 5.2158 mm 的起步尖峰，后来被保持机构拉回约 2.5 mm。如果只拍终点截图，读者很可能以为“一直很稳”；如果只看峰值，不交代回收过程，又会误以为托盘持续滑走。两种写法都不准确。

我们选择保留原来的 5 mm 门限，所以结论很直接：车辆到站 PASS，托盘保持 FAIL，整体 FAIL。

四、我以为“开慢一点”就能解决，结果更糟

第一反应很自然：降低运动加速度，托盘应该滑得更少。于是我们保留世界、货物和原验收门限，换成一个低加速度候选，再跑一次。结果并没有按直觉走。

运行	到站误差	最大托盘相对滑移	现象
v7 基础档 p4-nav2-v7-03	0.14786 m	5.2158 mm	起步尖峰，随后部分回收
v7 低加速度档 p4-nav2-v7-04-retained	0.07374 m	约 11.46 mm	逐步累积，末端没有回收

到站误差变小了，滑移却扩大到两倍多。低加速度档不是“同一种失败更轻一点”，而是换了一种失败形态：托盘相对甲板一点点单向爬移，最终越过保持机构的恢复范围。

这至少否定了一个过于简单的解释：不能仅凭一次失败就认定是“加速度太大”，更不能把 Nav2 的速度参数调低当作已经验证的修复。

五、长距离失败，又差点把我们带向另一个错误结论

随后，车辆尝试驶向更远的接收工位。在约 4.63 m 行程的运行中，甲板偏航达到 −0.011011 rad，越过原定的 ±0.01 rad 姿态门限。保护逻辑随即锁存并停下任务。

这里也有一个容易写错的地方：停下时车辆距目标约 0.6077 m，不代表 Nav2 规划失败或“只能开到这里”。这次目标尚未被完整执行，是姿态保护门先拒绝了继续运输。

起初，长行程伴随更大滑移，很容易让人下结论：“路越长，托盘越容易滑。”我们又做了两组受控对照，都走约 4.63 m：

对照	控制方式	托盘相对滑移
D1	单段纯直行	0.133 mm
D2	纯直行拆成八段，包含七次段间停车	0.078 mm

两组对照的甲板偏航也都接近零。于是“单纯因为距离长”以及“单纯因为启停次数多”都站不住。更值得怀疑的是 Nav2 实际命令流里的转向分量，与被动甲板、托盘保持机构的耦合。

但继续压低角速度也没有直接解决问题：另一档 Nav2 转向保守配置到站误差 0.1545 m、到站仍 PASS，滑移却达到 8.362 mm。因此，这里可以说“转向命令流相关”，不能说已经找到一个调小角速度就能治好的参数。

六、后续改进到哪里了？

后续机构候选 v10 在另一次带初始载荷的导航运行中，把最大相对滑移降到 4.812 mm，tray_retained 首次通过原 5 mm 门限，Nav2 到站误差为 0.1455 m。这说明“托盘保持”这一项并非永远无解。

但那次报告的整体结果仍是 FAIL：其当时使用的货物停车判据未通过。更晚的证据审计发现，旧版以货物接触点的速度上界直接代表“货物是否停止”可能高估贴着工装的振动，因此项目后来把该门改为基于货物本体中心的位移差分速度，同时保留旧速度上界作为诊断字段。本文不把旧报告的 0.0454 m/s 或 0.0209 m/s 写成“货物本体真的以该速度持续滑动”。

这也意味着：v10 通过了托盘保持门，不等于整套订单已完成；新口径也不能反过来把旧报告自动改判为 PASS。每一次判定，都必须绑定当次代码、世界和报告。

七、这次实验真正给我的启发

机器人项目里的“成功”至少有三层：

1. 导航成功：Nav2 action 完成，车辆真值到站。
2. 载货成功：运输全程托盘不越过相对滑移门，姿态与支撑持续有效。
3. 任务成功：停车后货物状态合格，并真正完成卸货、交接和订单确认。

把第 1 层当成第 3 层，演示会很好看，工程却可能过不了下一站。我们的 p4-nav2-v7-03 正好是一个反例：Nav2 到站、车辆停稳都是真的；托盘曾经越过 5 mm 门，也是真的。让两个事实同时留在报告里，比选一个好看的结论更有用。

当前异构机器人工作站仍在继续开发。本文的运行是初始化载荷的短程验证，不是从人形供盘到接收端卸货的完整连续订单，更不是实机安全认证。

---