机器人软件与具身智能 · 专栏四：定位、建图与导航 · 第 6 篇

本专栏第 5 篇讨论怎样找到路径，本篇继续追踪路径如何进入执行过程。从八界前往桌边途中遇到新玩具讲起，把地图更新、路线搜索、速度控制、恢复策略和任务结果接成反馈循环。程序使用理想平面运动仿真，配套接口说明与教学模型分开表述。

八界已经算出前往桌边的路线，刚向前走了一小段，一只玩具被放到了原来的通道上。规划器曾经返回成功，为什么机器人仍然必须重新判断？如果停住不动，到底是正在等待、需要重规划、控制失败，还是整个任务已经结束？

这就是导航从“求一条路”进入“执行一个任务”的分界。路径只是依据某一时刻地图和起始状态得到的空间方案。环境会变化，定位会更新，控制器也会产生跟踪误差。导航系统需要持续比较当前观测与计划，再决定继续、调整、恢复或结束。

八界的桌边到达又与下一步操作有关：底盘停在哪里、朝向哪里、是否真正停稳，会影响机械臂接近物体时的空间条件。任务结果不能只来自一条曾经有效的全局路径。

图 1｜新的障碍观测改变了通行条件，导航需要把变化反馈到规划与执行。

1. 一次导航至少包含四种不同的“成功”

第一种是请求被接受：服务或 Action 服务端愿意处理目标。第二种是路径搜索成功：在当前地图和约束下找到一条候选路线。第三种是控制阶段完成：机器人已进入允许的目标区域，并满足相应运动状态要求。第四种才是业务任务完成：到达的位置、朝向和状态足以支持后续操作。

这些结果不能互相代替。八界公开 SDK 的导航接口区分同步等待与带回调的异步方式，后者可以先返回任务标识，再等待任务完成通知。[1] 得到任务 ID，意味着有了追踪对象，并不代表底盘已经到达。

公开 ROS 2 开发文档中的区域导航示例，也分别处理目标接受与结果回调。[2] 区域导航的业务结果字段，要按其实际接口定义解释，不能与另一个 Action 的状态码直接互换。

作为通用参照，Nav2 Humble 的 NavigateToPose 目标包含 PoseStamped，反馈包含当前位姿、导航耗时、剩余距离与恢复次数等字段，结果负载使用 std_msgs/Empty。[3] 空结果负载本身不能区分成功、取消或中止；调用方仍需检查 Action 的终态状态。本文采用这个版本的定义，不把其他版本新增字段混进来。

2. 全局规划器与局部控制器各自看到什么

全局规划器回答：从当前起点到目标，空间上怎样绕过障碍？它一般在地图或代价地图上搜索，输出路径。路径常是一串带参考系的位姿，但并不自动包含可直接执行的时间、速度和加速度。

局部控制器回答：根据现在的位置、局部环境与运动约束，下一小段时间应该给出怎样的命令？它可能使用路径跟踪、局部轨迹采样或优化方法。算法可以不同，共同点是每次计算都依赖更新后的状态。

代价地图负责把传感器与静态地图转成规划、控制所需的通行表达。它包含的障碍层、更新频率、清除方式与 footprint 都会影响结果。所谓“局部”不意味着可以忽略全局目标，“全局”也不意味着永远不变。

把八界的任务链写成一次循环，可以理解为：取得当前位姿与观测，验证坐标和时间，更新障碍表达，判断原路径是否仍适用，必要时重规划，再计算短时控制命令，最后根据执行后的新状态继续判断。

关键是反馈中的“新状态”。如果只把预设命令积分一遍，却没有重新读取传感器与定位，那么得到的是开环预测。真实八界还需要面对轮地接触、执行延迟、控制误差和定位不确定性；这些因素不会因为发布了速度命令就消失。

3. 先用一个足够小的模型，把循环跑通

配套程序 使用 0.1 m 分辨率的二维地图、半径 0.2 m 的教学圆形机器人，起点 (0.85,2.55,0)，目标 (6.55,2.55,0)。这些参数只服务于实验，不代表八界的 footprint 或限速。

为了把注意力放在闭环逻辑，路径规划采用四邻接、统一边代价的广度优先搜索。每个有效邻居都只增加一步，按层遍历可以得到这个离散图中的最少步数路径。它没有调用本专栏第 5 篇的模块，读者可以单独运行本篇。

程序将连续同方向的栅格段压缩成拐点。控制器追踪当前拐点：若朝向误差较大，先转向；误差较小时，以不超过 0.4 m/s 的速度前进。角速度根据误差比例计算，并限制在 ±1.2 rad/s。

e_θ = wrap(atan2(y_target−y, x_target−x) − θ)
ω = clip(2.5 e_θ, −1.2, 1.2)
|e_θ| < 0.2 时：v = min(0.4, 1.8 × 到当前拐点的距离)
否则：v = 0

接近最终位置后，切换为目标朝向调整。这个控制器允许原地转动，是解释反馈过程的理想单轮模型，并不是八界底盘控制器实现，也不能与本专栏第 5 篇只能前进、曲率受限的教学车辆直接混为同一种运动模型。

每个控制周期为 0.1 s。程序用定速圆弧积分更新位姿，并在每条命令内部检查六个子时刻的离散地图碰撞。它假设定位完全准确、速度命令即时实现、障碍观测即时可用，因此后面的时间只描述这段软件仿真。

图 2｜无障碍时沿直线到达；2.0 s 时新障碍进入地图，更新后的路线绕开阻挡。轨迹由本篇理想仿真生成。

无障碍实验只规划一次，得到起终点两个拐点，14.6 s 后满足完成条件，最终位置误差约 0.066881 m。误差不是零，是因为目标判定允许 0.07 m 的位置容差；不能把“成功”读成精确抵达数学上的一个点。

4. 地图变了，怎样保证用上的是新的路径

动态实验在第 20 个周期，即 2.0 s 时，将一个矩形障碍放进原通道。程序递增 map_revision，并比较路径生成时保存的 path_revision。二者不同，就根据当前位姿重新规划。

事件记录依次是：0 s 生成第一条路径；2 s 收到新的障碍状态并把地图版本改为 1；同一轮生成第二条路径；29.3 s 宣告完成。第二条路径包含七个拐点，实际轨迹的最大横向绕行约 0.803239 m。

版本号在这里表达的是一个简单不变量：控制器使用的路径，应当能追溯到已知的地图状态。真实系统可能不需要每个像素更新都全局重规划，而是结合路径受阻、周期触发、目标变化和局部控制失败等条件。但路径与其依据的信息之间仍应保持可解释的关系。

如果规划运行在另一个线程或进程，还会遇到结果晚到的问题：版本 3 的路径尚未算完，地图已经来到版本 4。调用方应判断这条结果是否仍适用，不能只因为“刚收到”就把它当成最新方案。

Nav2 Humble 提供的一份导航行为树使用 PipelineSequence，以 RateController hz="1.0" 包裹全局规划，并与路径跟随组织在同一流程中。[4] 这是该参考文件的具体行为，不代表所有 Nav2 部署或八界系统都以 1 Hz 重规划。本文示例则使用地图版本变化触发，便于把原因与结果对应起来。

图 3｜转向与直行交替出现，最终阶段继续收敛目标朝向和角速度。到目标的直线距离不是全局路径的剩余长度。

绕行时，到目标的直线距离短时间不下降，甚至增加，都可能合理。若监控只要求每个周期都更接近目标，就会把必要的绕障误判为失败。进展检查必须与使用的路径、局部运动和允许时间对应。

5. 数据过期时，继续跟踪旧路径为什么不够

设当前时刻为 t，最新障碍观测时间为 t_obs，定位变换时间为 t_tf。程序分别检查 t−t_obs 与 t−t_tf，而不是只看消息刚进入队列的时间。

教学阈值为：观测年龄不超过 0.3 s、变换年龄不超过 0.2 s。任何时间不是有限数、时间落在未来，或目标与地图参考系不一致，都不能进入 READY。这里默认各时间来自可比较的同一时钟域；如果时钟没有对齐，年龄计算本身就缺乏意义。

过期实验设置当前 10 s、观测来自 9.5 s，因此返回 STALE，策略输出 (v,ω)=(0,0)。另一组实验将地图帧设为 odom、目标帧设为 map，直接返回 FRAME_MISMATCH。生产系统可以先做正确的时间相关坐标变换；本例没有实现 TF 查询，因此宁可明确拒绝，也不隐式把两个帧视作相同。

图 4｜0.5 s 的观测年龄超过教学阈值；恢复尝试计数达到 2 后进入失败终态。时间门限和重试上限均是示例策略。

这两组是独立的状态与策略实验，没有在完整轨迹中模拟网络断流。完整闭环仿真每轮使用同一时刻的合成观测。这样可以分别检查判断逻辑与理想控制轨迹，也避免把没有模拟过的通信异常说成已经验证。

对八界来说，局部避障数据陈旧时，即使全局路线没有变化，也可能无法判断新出现的玩具是否占据通道。位姿更新陈旧时，控制器甚至不知道自己现在偏离路径多少。地图与定位的新鲜度，需要作为运动决策的输入。

6. 发出零速度，不等于物理上立即停下

软件输出零速度，只是改变了命令。真实机器人还要经过传输、调度、执行器响应和制动。考虑一维直行、延迟期间保持原速度、随后以恒定减速度 a 制动的简化模型：

d_required = v τ + v²/(2a) + margin

第一项是延迟阶段继续走过的距离，第二项来自匀减速关系 0=v²−2ad，第三项保留额外空间。教学参数 v=0.6 m/s、τ=0.25 s、a=0.4 m/s²、margin=0.1 m，结果为：

0.6×0.25 + 0.6²/(2×0.4) + 0.1
= 0.15 + 0.45 + 0.1
= 0.70 m

如果只按本体几何半径膨胀障碍，却忽略运动中的停车空间，距离查询通过也不能说明当前速度合适。速度翻倍时，延迟项翻倍，制动项增长四倍；提高控制频率只能影响部分延迟，无法取消机械制动过程。

图 5｜左侧是独立的恒减速预算，不是轨迹仿真的制动结果；右侧要求位置、朝向、线速度和角速度共同满足阈值，并持续三个周期。

本例的轨迹积分没有模拟加速度限制或制动，因此不能用“轨迹没穿过障碍”来证明上述 0.70 m 已经在运动中得到满足。八界的真实延迟、不同载荷下的减速度和停止行为，需要来自设备配置与测量；这里不把教学数值当作实机安全距离。

7. 恢复行为应当针对失败原因，并且有终点

规划失败可能来自目标落在障碍里、地图中确实无路、坐标错误或观测陈旧。控制失败可能来自路径无法跟随、局部阻挡或长时间没有足够进展。不同原因应走向不同的恢复动作。

清除代价地图可以消除缓存中的旧证据，但不会移动现实里的玩具。示例先把缓存清空，再用仍然存在的物理障碍刷新，障碍重新出现。若恢复逻辑反复清图，却不等待新观测和重新判断，就可能在短暂空白的地图上继续行驶。

Nav2 Humble 的同一份参考行为树包含局部恢复与外层恢复：规划或跟踪失败后分别尝试清除相关代价地图，外层恢复序列还可以组织旋转、等待和后退，并设置重试次数。[4] 这些是可配置的流程示例，不代表任何场景都应执行后退或旋转；恢复动作本身也要检查空间、数据和控制条件。

本篇只实现有限次数策略：尝试次数为 0、1 时返回 RECOVER，达到 2 时返回 FAILED；另外用贯穿整张地图的墙验证“确实无路”会被搜索器报告。没有模拟真实的旋转搜索、后退动作或代价地图服务调用。

进展检查也要理解具体插件语义。Humble 的 SimpleProgressChecker 根据平面位移与允许时间更新基准，纯原地旋转不会让它的平移距离条件成立。[5] 因而“正在认真调朝向”和“在该进展指标下没有足够移动”可能同时发生。调试时应先读清楚使用的进展定义，不能只把时间阈值无限调大。

8. 怎样判定八界已经到达，而不是恰好路过

教学程序要求四个条件同时成立：位置误差小于 0.07 m、朝向误差小于 0.05 rad、线速度绝对值小于 0.03 m/s、角速度绝对值小于 0.05 rad/s，并且连续满足三个周期。

其中速度在理想仿真里等于指令速度；真实系统需要检查可靠的测量或状态估计，而不是用刚发出的零命令冒充停稳证据。三个周期只演示抑制瞬时通过的策略，也不是八界产品的到达标准。

代码分别检查三个反例：位置相同但朝向相差 90°，不能完成；位姿正确但仍以 0.2 m/s 行进，不能完成；位姿正确但仍以 0.2 rad/s 转动，也不能完成。位置、朝向和运动状态必须共同服务于下一步操作的要求。

到达结果还必须属于当前任务。如果用户把目标从桌边改成门口，旧目标的结果晚到，不能用它结束新任务。示例用 active_id 与结果携带的 ID 比较，拒绝 old-goal 对 new-goal 的更新。这只检查身份策略，没有实现 ROS 2 Action 的完整取消握手。

同理，取消请求被接受与机器人已停止是不同事实。应用应追踪取消相关终态、控制输出和实际运动状态。示例报告中的取消输出 (0,0) 仅表达本地策略，不声称已经验证网络取消、底盘制动或服务端实现。

八界随后若要执行桌面抓取，还需要重新确认目标观测、底盘位姿与机械臂规划条件。导航成功提供一个明确的阶段结果，后续操作仍需自己的可执行性检查。

9. 调试时保留一条能串起来的证据链

一个有用的导航日志，应该能把目标 ID、地图身份与版本、位姿参考系、观测时间、路径生成时间、当前控制状态和恢复原因串在一起。不要只留下“规划成功”“控制失败”两个孤立字符串。

对本例，可以沿事件直接复盘：新障碍何时出现、地图版本何时递增、路径何时替换、控制器如何转弯，以及最终哪几个判据同时成立。对八界的真实接入，则应依据开放 SDK 与 ROS 接口保留相应证据，而不是臆测内部模块。[1][2]

专栏四的六篇到这里连成了从定位到执行的过程：第 1 篇分析里程计漂移，第 2、3 篇用状态估计与 SLAM 组织观测约束，第 4 篇讨论地图表示，第 5 篇搜索可行路径，第 6 篇把路径放进持续更新的导航任务。后续规划的专栏五“机械臂与操作”将接着讨论到达目标附近以后，怎样把末端目标变成可执行的操作。

欢迎关注科沃斯八界 AtomGit，相关代码将逐步释放，实际开放内容与使用说明以仓库为准。[6]

10. 运行八组闭环与反例实验

完整程序见 24_navigation_loop.py，环境与输出说明见 examples/README.md。在配套仓库根目录执行：

python3 -m venv .venv-nav
.venv-nav/bin/python -m pip install -r "04_定位建图与导航/24_导航系统如何闭环运行/examples/requirements.txt"
.venv-nav/bin/python "04_定位建图与导航/24_导航系统如何闭环运行/examples/24_navigation_loop.py"

程序检查正常闭环、新障碍重规划、观测过期、有限恢复、到达证据、任务身份、参考系检查和停车距离预算。默认只打印报告，--json 输出纯 JSON；不依赖 ROS、不读取实机状态，也不发布运动命令。

参考链接

1. 八界 Python SDK 使用说明

2. 八界机器人 ROS 2 Humble 高阶开发 SOP

3. Nav2 Humble：NavigateToPose.action 定义

4. Nav2 Humble：带重规划与恢复的导航行为树

5. Nav2 Humble：SimpleProgressChecker 实现

6. 科沃斯八界 AtomGit 开源组织