理解 MoveIt：机器人状态、规划场景和规划流水线

机器人软件与具身智能 · 专栏五：机械臂与操作 · 第 4 篇

前三篇分别计算位姿、局部运动和避障路径。本篇把这些计算放进同一次任务，追踪模型、状态、环境与规划结果之间的对应关系。

八界开始伸手前，规划器认为收纳筐在一侧。路径算好以后，筐被人挪到了机械臂前方；或者八界已经夹住玩具，但规划场景仍把夹爪当作空手。此时再换一种搜索算法，未必能解决问题。算法可能正确地回答了一个已经过期的问题。

理解 MoveIt，关键是知道每次计算使用了什么机器人模型、哪一份关节状态、哪一份环境，以及结果准备交给谁执行。plan 成功只完成了其中一段链路。

本文对照 MoveIt 2 的 Humble 文档解释实体与流程。配套 Python 程序是可独立运行的离线机制模型，使用二维圆盘和固定路径复现实例，不调用 MoveIt，也不冒充八界的控制接口。这样可以先把状态变化和失败条件逐个检查清楚。

图 1｜同一次规划需要机器人模型、当前状态、环境和任务目标共同参与。

1. 模型回答"是什么"，状态回答"现在在哪"

RobotModel 保存关节、连杆、关节组等模型关系；URDF 描述机构和几何，SRDF 补充规划组等语义。RobotState 在模型上保存一组具体变量，并据此计算连杆变换、查询运动学。把模型与状态分开，才能让同一机构同时表示起点、候选采样点和目标构型。 $1$

八界某个关节的轴线、父子 link 和固定工具变换属于模型；joint1=0.1 rad 属于某一时刻的状态。读入 URDF 不会自动获得编码器的当前值，收到关节消息也不会自动说明 TCP 选对了。

进一步看，关节位置数组本身没有完整表达时间有效性。上层还需要知道它来自哪次采样、是否覆盖所需关节、是否和本次规划的基准一致。专栏二讨论的时间戳、TF 和消息缓存，在这里直接影响规划起点。

PlanningScene 则把机器人状态、世界几何、附着物体和碰撞许可等组织起来，让碰撞与约束查询有完整上下文。 $2$ 同一个 q 放进两份不同场景，可以得到相反的有效性结论。

图 2｜模型提供结构，状态提供变量，场景提供环境与碰撞关系；监视器负责接收和组织更新。

对于八界抓取任务，可以把物体位姿、目标 TCP、起点关节和场景分别记录，并保留本次任务标识。这样发现错误时，能判断是哪一个对象改变了，而不是只看到最终的"规划失败"。

2. 场景被更新以后，旧路径为什么仍然看起来正常

PlanningSceneMonitor 接收机器人状态、场景和环境更新，为使用者提供受管理的规划场景。 $3$ 读写同步解决的是计算机内部的数据一致性，不能冻结真实世界。

例如规划线程读取场景时，一半物体是旧位置、一半是新位置，就可能得到从未真实存在过的混合状态。合理的锁定或快照让一次查询面对一致的对象集合。等这次计算完成，场景仍可能继续变化；一致快照不等于永久有效。

离线例子把障碍圆盘半径设为 0.08 m，工具圆盘半径为 0.025 m，固定测试路径沿 x 方向从 0 到 1 m，y 为零：

text 复制代码

场景 v1：障碍中心 (0.50,0.30) m
路径最小净空 = 0.30−0.08−0.025 = +0.195 m

场景 v2：障碍中心 (0.50,0.00) m
路径最小净空 = 0−0.08−0.025 = −0.105 m

深拷贝保存的 v1 仍返回正净空。它没有被更新"修正"，也没有算错；它描述的是旧环境。把 v1 路径交给 v2 场景执行，才是上下文连接出了问题。

图 3｜路径几何不变，障碍更新就能使净空从 +0.195 m 变为 −0.105 m。

示例给场景增加 revision，规划结果记录所用版本，执行前发现不一致就返回 SCENE_CHANGED_REVALIDATE。这是本文设计的应用层标识，不是宣称 MoveIt 的所有接口都带同名版本字段。真实系统也可以采用对象版本、更新时间或相关区域变化记录，但需要明确谁生成、谁检查。

发生版本变化后，可以重新规划，也可以按最新场景重新验证已有路径。仅看"变化是不是很小"不足以决定：几毫米变化发生在狭窄通道里，影响可能比远处一米的变化更大。

3. "消息发出去了"和"场景已经应用了"之间还差什么

MoveIt 的场景接口区分场景差分的发布与服务应用。官方教程展示了通过话题传播更新，以及使用 ApplyPlanningScene 服务把差分应用到目标节点的方式。 $4$

异步发布适合传播状态，但发布完成时，规划节点未必已经处理了该消息。如果八界的流程是"发布障碍 → 紧接着请求规划"，就需要建立二者的完成关系。等待固定几十毫秒只能描述一次经验，不构成接收与应用的确认。

服务返回成功确认的是对应目标已经应用这次更新，不代表所有其他订阅者都同时同步，也不代表之后没有新的变化。因此可以把任务过程组织为：形成目标场景差分，确认规划使用者已应用，读取一致上下文，再提交规划请求。最终执行前还要复核状态。

对象的稳定 ID 同样重要。用同一 ID 更新收纳筐位置，与每帧创建一个新 ID 表示新筐，是两种不同语义。后者若不清理旧对象，会让场景里积累一排不存在的障碍；先清空整个世界再添加，又可能在中间短暂暴露一个空场景。更新粒度和时序都应由明确的对象生命周期约束。

4. 抓住玩具是一场几何所有权转移

夹爪闭合以后，玩具的位置应随工具运动。世界坐标中的静态物体要转为附着到指定 link 的物体，其相对位姿需要重新表达：

text 复制代码

T_link_object = inverse(T_world_link) · T_world_object
T_world_object(t) = T_world_link(t) · T_link_object

第二个式子说明，附着关系保存的是相对变换。若直接把世界坐标数值当作 link 局部坐标，物体会随着夹爪出现错误偏移。教程中的附着场景差分把世界移除与附着添加放在一起表达，同时允许指定实际接触的 link。 $4$

随文二维例子把玩具初始中心设为 (0,0.05) m，工具中心为 (0,0) m，因此附着偏移为 (0,0.05) m。工具沿原路径移动时，玩具中心始终在工具上方 5 cm。

图 4｜附着物沿工具路径一起移动，空手净空为正的路段仍可能发生载物碰撞。

这次障碍中心为 (0.50,0.18) m、半径 0.08 m。空工具的净空为 +0.075 m；附着半径 0.09 m 的玩具后，最小净空为 0.13−0.09−0.08=−0.04 m。动图回放的是用于暴露错误的旧路径，红色重叠表示需要拒绝，不能把它继续交给执行器。

示例用 touch_links=['left_pad','right_pad'] 表达仅允许两侧指垫接触。这两个名字是机制模型中的教学标识。允许玩具接触指垫，不会自动允许它接触前臂。真实 MoveIt 配置需要使用机器人实际 link 名称，并检查允许碰撞矩阵中的具体对象对。

5. 规划流水线怎样把任务请求变成候选轨迹

规划请求需要说明关节组、起点、目标约束、路径约束和规划预算等。目标约束描述终点应落入的集合；路径约束要求过程满足条件。例如"最终把杯口朝上"和"整段搬运始终保持杯口朝上"不是同一个问题。 $5$

对八界而言，关节组只含六个臂关节时，规划器不会自动移动底盘或升降来救一个不可达目标。工具目标也需要明确 frame、link 和容差，否则看似相同的 xyz 可能对应不同的求解任务。

规划流水线可包含请求适配、规划器和结果处理。适配器承担配置明确的预处理或后处理，例如处理允许范围内的起点偏差、增加时间信息等；使用了哪些适配器、顺序是什么，需要以实际配置为准。 $5$

图 5｜请求、场景与配置共同决定规划输入；后处理之后仍需检查最终结果。

不要把适配器当作任意错误的修复器。起点轻微越界的容差处理，与机器人实际远离给定起点，是两类问题；后者需要重新获取当前状态。若处理后的起点发生变化，还应知道机器人怎样到达这个新起点。

上一篇已经看到，路径平滑能改变碰撞关系。时间参数化也可能采用会改变几何路径的算法。流水线末尾需要面对最终轨迹检查约束，不能仅复用搜索阶段的"有效"标记。本文的 make_plan() 只生成固定测试路径并检查净空，不实现上述搜索和适配器，代码注释明确保留了这个范围。

6. 关节数组必须按名字对齐

假设收到的顺序为：

text 复制代码

names = [joint3,joint1,joint6,joint2,joint5,joint4]
values = [0.3,0.1,0.6,0.2,0.5,0.4] rad

如果模型内部按 joint1 到 joint6 排列，就应得到 (0.1,0.2,0.3,0.4,0.5,0.6)。直接复制 values，会把各个关节的采样值互换，但数组长度、数值范围和数据类型可能全部"正常"。

图 6｜名称建立关节身份，数组位置只在约定的顺序内有意义。

示例的 ordered_positions() 先检查长度、重复名称、缺失名称和非有限值，再按目标关节序列取值。这里要求恰好覆盖六个关节，是为了让小实验的契约清楚；真实完整 JointState 还可能包含升降和夹爪，应按所选关节组提取，并独立检查必需关节是否齐全。

同理，规划结果中的 joint_names 必须和控制器接收的关节集合、顺序建立正确映射。模型能显示、逆解能算出，并不能证明控制器已经加载并接好了驱动链。鱼香 ROS 的中文教程在进入 MoveIt 实践前列出了控制器管理器、轨迹控制器和状态广播器等依赖，提醒我们这些组件需要实际准备。 $6$

7. 执行前要检查哪些仍然成立的条件

在本篇机制模型中，执行入口依次检查任务标识、frame、状态时间、起点偏差、场景版本和路径原有判定。这些条件不是重复规划，而是确认"准备执行的结果仍属于当前问题"。

本次教学时效阈值设为 0.10 s。当前时间取 10 s，采样时间分别为 9.95、9.8、10.01 s 和 NaN，返回：

text 复制代码

FRESH
STALE_SAMPLE
FUTURE_SAMPLE
INVALID_TIME

未来时间不能因为算出的 age 小于阈值就当作新鲜。它可能意味着时钟域不一致或时间异常；应先解释时间来源。0.10 s 是本实验设定，不是 ROS 或 MoveIt 的统一默认要求。

图 7｜计划属于哪次任务、使用什么状态和场景，需要在执行入口重新对应。

如果实际起点第一关节偏移 0.03 rad，超过本例 0.02 rad 容差，返回 START_STATE_CHANGED。如果任务已经从 pick-1 切换成 pick-2，旧结果返回 OBSOLETE_TASK；如果 plan 的 frame 与调用上下文不一致，返回 FRAME_MISMATCH。这些原因使上层能够采取不同处理，而不是把所有失败都归为"规划器不好用"。

全部通过后返回 READY_IN_MODEL，表示满足本机制实验的入口条件。真正执行还需要控制器接收、持续反馈、轨迹容差和停止行为等证据。八界举起物体时，环境与抓取状态仍在变化，执行入口的一次检查不能替代后续监视。

8. 复现状态变化，再连接实际系统

完整代码只用 Python 标准库，运行：

bash 复制代码

python3 04_planning_scene.py
python3 04_planning_scene.py --json

运行说明与八组预期结果分别解释快照、旧计划失效、物体附着、载物几何、局部接触许可、关节身份、采样时间和执行复核。程序不会启动 ROS 节点或控制器。

以后接入实际 MoveIt 系统时，可以逐项寻找对应证据：模型和规划组是否正确，关节状态是否完整新鲜，场景差分是否已应用，附着物是否跟随正确 link，规划结果是否仍与起点和任务对应。这里的每个问题都有明确对象，排查起来比反复修改规划算法更直接。

本专栏第 5 篇继续沿结果往下走：几何路径已经通过检查，怎样给它分配时间，又怎样判断控制器真的跟上了？

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

参考链接

模型、状态、关节组和运动学查询。MoveIt Humble Robot Model and Robot State
规划场景中的碰撞查询和允许碰撞关系。MoveIt Humble Planning Scene
场景监视器与状态、环境更新的关系。MoveIt Humble Planning Scene Monitor
场景差分、服务应用、附着物体与接触 link 的官方示例。MoveIt Humble Planning Scene ROS API
请求、规划流水线与请求适配器。MoveIt Humble Motion Planning
鱼香 ROS 的中文原创教程；此处只采用公开导言及依赖准备部分，不引用付费正文细节。MoveIt 2 实践准备