第 4 章运动规划¶

规划器的算法机制（树怎么长、图怎么连、代价怎么估）属于 robotics-note 第 9 章；这一章只解决工程侧的三个问题：请求里哪些字段真正改变结果、时间参数化把解变成什么样的轨迹、失败时怎么在错误码与配置之间建立因果。读数均来自运行环境 ROS 2 Humble + MoveIt 2 2.5.10 + OMPL 1.7.0，样本臂 panda，场景是第 3 章那种手工方块环境。算法层面的取舍不在此处展开。

请求进管线到轨迹出管线的四段：

flowchart LR
    req["MotionPlanRequest"] --> adapt["适配器链<br/>起点 / 目标容差"]
    adapt --> planner["规划插件<br/>OMPL / Pilz"]
    planner --> resp["轨迹 + 错误码"]
    resp --> tparam["时间参数化"]

1. 管线结构与请求字段¶

一次规划请求由管线（planning pipeline）消化：管线 = 一个规划插件 + 一串请求适配器（request adapter），插件负责搜索、适配器负责后置处理。panda 样本配置里 OMPL 管线的定义就是这样：

## ompl_planning.yaml（节选）：插件 + 适配器链 + 起点容差
planning_plugin: ompl_interface/OMPLPlanner
request_adapters: >-
  default_planner_request_adapters/AddTimeOptimalParameterization
  default_planner_request_adapters/ResolveConstraintFrames
  default_planner_request_adapters/FixWorkspaceBounds
  default_planner_request_adapters/FixStartStateBounds
  default_planner_request_adapters/FixStartStateCollision
  default_planner_request_adapters/FixStartStatePathConstraints
start_state_max_bounds_error: 0.1     # 起点越出限位 0.1 rad 以内可被修正，超出即判非法

这些适配器各自带默认参数，运行时从进程输出里可直接读到：path_tolerance 0.1、resample_dt 0.1 s、default_workspace_bounds 10.0、start_state_max_bounds_error 0.1、start_state_max_dt 0.5、jiggle_fraction 0.02、max_sampling_attempts 100。

链条的顺序有意义：先 FixWorkspaceBounds / FixStartStateBounds / FixStartStateCollision 把起点收拾合法，再交给 OMPLPlanner 搜索，最后由 AddTimeOptimalParameterization 做时间参数化。所以“时间参数化失败”和“搜索失败”在同一根管线上是两个环节，返回的码也不同——这是下一节归因表的基础。

请求侧真正改变结果的字段：

字段	作用	边界与坑
`group_name`	选组，决定决策变量维度	名字写错返回 `FAILURE`，不报“组不存在”
`pipeline_id`	选管线	只能选配置里声明过的管线，demo 装了 `ompl` / `chomp` / `pilz_industrial_motion_planner` 三条
`planner_id`	选管线内的规划器配置名	要写 `planner_configs` 下的完整键名（如 `RRTConnectkConfigDefault`）；只写 `RRTConnect` 会走“找不到配置、改用默认”的路径，表面上仍出解，参数却不是预期的那套
`allowed_planning_time`	单次尝试的时间预算（s）	预算不足直接失败，与场景难度无关
`num_planning_attempts`	尝试次数	采样规划器的成功率靠它累加
`max_velocity_scaling_factor` / `max_acceleration_scaling_factor`	相对上限的缩放（0–1]	缺省值是 0，不同管线处理不同
`goal_constraints`	目标：关节约束或位姿约束	位姿约束是“区域”而非点，容差决定可行采样

最后一条要展开：位姿目标在消息里是 PositionConstraint（一个包围体区域，通常是半径几毫米的球）加 OrientationConstraint（逐轴角容差）。把容差收紧会让可行采样变少、规划变慢；容差过宽则解出来的末端位置可能偏出工艺要求。抓取侧的“到点精度”通常由这一对容差决定，而不是由控制器的定位精度决定。

2. 缩放因子缺省值的管线差异¶

max_velocity_scaling_factor 在消息里的缺省值是 0.0，语义上应当是“不缩放”。两条管线对它的处理不一致：同一份 Pilz 请求（缺省缩放因子）返回 INVALID_MOTION_PLAN，而把字段显式写成 1.0 后同一请求正常出解；OMPL 管线对 0 值不报错。

## 同一请求（Pilz 管线，PTP → 关节目标 extended），仅缩放因子不同
scaling 未显式给出: code=-2  INVALID_MOTION_PLAN，0 个航点
scaling = 1.0:      code= 1  航点 24，时长 2.242 s

排查这类问题的顺序应当是：先在请求里把所有缩放因子显式写出，再调规划器与预算。跨发行版这一处理还会变化（新版本把请求侧的缺省值统一交给管线适配器处理），因此不把它当作通用结论，只作为“缺省值不可依赖”的实例。

3. 采样规划器的适用分界与成功率¶

用一根立柱挡住起点与目标之间的直线路径（柱 0.10×0.25×0.30 m，中心 (0.45, −0.10, 0.30)；目标 (0.45, −0.30, 0.30)），每个规划器在每个预算下跑 20 次（每次 1 尝试），给出成功率与 Wilson 95% 区间：

规划器	0.05 s 预算	0.20 s 预算	出解时轨迹时长
RRTConnect	0/20 = 0.00 [0.00, 0.16]	2/20 = 0.10 [0.03, 0.30]	2.95 s
PRM	3/20 = 0.15 [0.05, 0.36]	2/20 = 0.10 [0.03, 0.30]	2.98 s
BiTRRT	1/20 = 0.05 [0.01, 0.24]	3/20 = 0.15 [0.05, 0.36]	2.47 s
EST	0/20 = 0.00 [0.00, 0.16]	7/20 = 0.35 [0.18, 0.57]	2.69 s
LBKPIECE	0/20 = 0.00 [0.00, 0.16]	2/20 = 0.10 [0.03, 0.30]	2.75 s

这张表能读出三件事，第一件是否定性的：五个规划器的区间大量重叠，换一批 20 次（同一场景、同一预算，先跑过一轮）排名就变了——前一轮里 RRTConnect 在 0.05 s 拿到 2/10 而 BiTRRT 0/10，这一轮 0.05 s 下 PRM 反而最高。所以小样本不足以支撑“这台机器该用哪个规划器”的结论，任何排序类断言都要先把样本量提到 30 次以上并给区间。

第二件是预算的主导作用：0.05 s 下四个规划器全部 0/20；把预算给到 2 s、单次尝试，RRTConnect / BiTRRT / LBKPIECE 都能出解。这一类失败的第一位原因是算力配额，而不是算法选择。

第三件是出解质量的差异被掩盖：成功样本的轨迹时长在 2.0–3.0 s、航点 22–32 之间波动，同一条请求两次结果不同——这是采样规划器的本性（概率完备但不保证最优）。要可复现的路径形状，应当使用第 5 节的笛卡尔通路或固定起点目标的对照实验；要更短的解，用 simplify solutions 类适配器或增加尝试次数取最优，而不是换规划器。

OMPL 侧还有两类参数值得单独调：longest_valid_segment_fraction（两点间插值检查的步长，过大就漏检碰撞）与 range / goal_bias（决定探索与向目标收敛的平衡）。它们的语义与算法效果归 robotics-note 第 9 章，此处只列“调什么”。

GPU 并行规划（cuRobo 一类）把搜索与碰撞检查批量搬到 GPU 上展开。它的形态是按机器人配置初始化一个批量规划器，再对多组起/终状态一次求：

from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig

mg = MotionGen(MotionGenConfig.load_from_robot_config(robot_cfg, world_cfg))
mg.warmup()                                                    # 首次编译/预热
result = mg.plan_single(start_state, goal_pose, MotionGenPlanConfig())
plan = result.get_interpolated_plan()                          # 插值后的轨迹

适用面是多目标/多初值的批量查询（抓取候选筛选、轨迹库生成）；单臂单次请求的收益取决于数据搬运与预处理开销，不属于“换个库就快”的场景。API 随版本演进，以仓库示例为准。

4. 时间参数化与缩放因子¶

采样规划器给出的是无时间的路径点序列（几何折线）。时间参数化（time parameterization）按关节速度与加速度上限把它重定时，得到可执行的轨迹；MoveIt 默认的适配器是 TOTG（time-optimal trajectory generation，时间最优轨迹生成）。同一几何路径（起点 ready → 组态 extended）在不同缩放因子下的轨迹：

速度缩放	加速度缩放	轨迹时长	航点数	峰值关节速度
1.00	1.00	1.781 s	19	2.175 rad/s
0.50	1.00	2.514 s	27	1.088 rad/s
1.00	0.20	3.878 s	40	1.195 rad/s
0.25	0.25	5.017 s	52	0.544 rad/s

三行事实值得记住。第一行的峰值 2.175 rad/s 恰好等于第 1 章 joint_limits.yaml 里的速度上限——重定时把上限吃满了，说明规划侧用的是覆写值而非 URDF 值。第二行速度缩放到 0.5，峰值精确减半（1.088），时长却只增加 41%（不是 100%），因为这条路径里有一段本来就没吃满速度上限。第三行只压加速度（0.2）时，峰值速度也被拖低到 1.195，时长增加 118%，且航点从 19 增到 52——加速度约束改变的是速度剖面与采样密度，不只是时长。

航点数由重定时决定，不由几何路径决定，这也是为什么同一目标在不同缩放下航点数差别这么大；执行侧（第 7 章）看到的插值密度因此直接受这里影响。

5. 笛卡尔通路与三种指令¶

抓取与放置大量要求末端走直线（贴近工件、插入、抽出）。MoveIt 提供两种笛卡尔通路，二者在“绕障”上的行为差别很大。

/compute_cartesian_path（moveit_msgs/srv/GetCartesianPath）：把目标位姿按 max_step 离散成一串中间位姿，逐个求逆解并检查碰撞，串起来即路径。它做的是“沿直线走”，不会绕障——遇到障碍就在某一步停下并报告完成度：

无障碍的直线下降 0.1 m        : 完成度=1.00，18 航点，直线度最大偏差 0.014 mm
同一路径，直线穿过立柱        : 完成度=0.39，12 航点（走到柱面即停）
同一路径，关闭碰撞检查        : 完成度=1.00，21 航点（直接穿柱）

完成度（fraction）是“走完了多大比例的航点”，不是“成功率”；返回码仍是成功。这决定了用法：先按完成度判断是否真的走到了，再决定是否下发执行；avoid_collisions=False 时它照样出解，只是解会穿模。直线度偏差 0.014 mm 说明这条通路的几何精度足够，风险全在完成度这一侧。

Pilz 工业规划器提供三种工业指令，PTP（point-to-point）、LIN（直线）、CIRC（圆弧）：

PTP → 关节目标 extended: 24 航点，2.242 s，峰值关节速度 2.039 rad/s
      各关节首次到达终值的时间比例 = [0.00, 0.94, 0.00, 0.98, 0.00, 0.00, 0.00]
LIN → 直线下降 0.1 m:  26 航点，2.445 s，直线度最大偏差 0.002 mm
      笛卡尔速度 min/中位/max = 2.6 / 50.1 / 50.1 mm/s
      中段（25%–75%）速度极差 = 0.00 mm/s
      同一目标 vs=0.25: 时长 8.115 s

PTP 的比例里那些 0.00 是因为对应关节本来就在目标值上（ready 与 extended 只有 joint2、joint4 不同）；真正运动的两个关节在 0.94 与 0.98 处到位，即同时到达——这是 PTP 的定义性特征，代价是路径形状不可控（不是直线）。LIN 则相反：直线度 0.002 mm、中段速度极差 0.00 mm/s，即匀速直线段，起止处是加减速斜坡（min 2.6 mm/s 就是起点段）。这三条结果正是“工业控制器行为”的刻画，也是选 Pilz 而非 OMPL 的唯一理由：需要直线 + 匀速时。

CIRC 没有可用的消息级通路：Humble 里只有 moveit_msgs/srv/GetCartesianPath（请求是位姿序列，不含圆心/途经点），三点圆弧的指令只在 C++ 侧的接口上提供。因此本章不写 CIRC 的行为。

三种指令都不做绕障规划：需要绕开障碍又必须保持直线时，正确做法是把任务拆成“采样规划到直线起点 → LIN 走完直线”两段（第 5 章的 stage 编排正是为此）。

6. 失败归因清单¶

同一套接口上，不同成因落在不同错误码上，且耗时形态也不同（码值取自 moveit_msgs/MoveItErrorCodes）：

现象	错误码	名目	耗时形态
目标位姿不可达（x=2.0 m）	−27	`GOAL_STATE_INVALID`	烧满预算（5 s 请求 → 5051 ms）
目标位形与障碍相交	−27	`GOAL_STATE_INVALID`	烧满预算（5064 ms）
目标取 SRDF 组态 `transport`	−27	`GOAL_STATE_INVALID`	立即失败（41 ms）
起点越出限位（joint4=1.0，上限 0.0873）	−26	`START_STATE_INVALID`	立即失败（32 ms）
组名写错	99999	`FAILURE`	立即失败（27 ms）
预算过小而场景需绕行	−27	`GOAL_STATE_INVALID`	223 ms
Pilz 请求缺省缩放因子	−2	`INVALID_MOTION_PLAN`	立即失败

三条使用要点。

目标非法有两种表现，区别在“能不能立刻判定”。transport 组态的失败只用 41 ms：因为它是一组确定的关节值，合法性可以一步查出——该组态本身就让 panda_link7 与 panda_link5 相交 4.4 mm（用第 3 章的校验服务单独确认过，无需世界物体即非法）。而位姿目标要经过逆解与采样，不可达时会反复尝试直到烧满预算。判断“某组态是否合法”应直接用校验服务，不要用规划请求去试，后者会把 5 s 预算全花掉。

耗时形态本身就是线索。返回失败且几乎立即返回 → 起点/目标/配置层的确定性问题（越限、非法、名字错）；返回失败且耗时接近预算 → 搜索层问题（不可达、需绕行、预算不足）。这一条比错误码更快指向原因。

同一现象在不同接口上的码可能不同。transport 组态作为目标，在第 1 章的进程内接口上返回 FAILURE 与 INVALID_MOTION_PLAN，在本章的动作接口上返回 GOAL_STATE_INVALID。写排障清单时要按接口层分别记录，不能混成一条“规划失败”。

7. 要点¶

管线 = 一个插件 + 一串请求适配器；起点修正适配器在搜索之前，时间参数化在搜索之后，所以同一管线的失败环节不同、错误码也不同。
真正改变结果的请求字段是组名、管线、规划器配置名、时间预算、尝试次数、两个缩放因子与目标约束的容差；缩放因子缺省值 0 在 OMPL 与 Pilz 上处理不一致，显式写出是唯一稳妥做法。
五个采样规划器在同一绕障场景下的成功率区间大量重叠，且换一批样本排名就变：小样本不足以排序，预算与尝试次数才是第一位原因（2 s 预算下多数规划器都能出解）。
时间参数化吃满覆写后的速度上限（峰值恰为 2.175 rad/s）；压速度上限不会等比例拉长时长，压加速度会同时改变速度剖面与航点密度。
笛卡尔通路不做绕障：/compute_cartesian_path 用 fraction 报告走到哪儿（障碍下 0.39），关闭碰撞检查照样 1.00 但穿模；Pilz 的 LIN 给出 0.002 mm 直线度与中段匀速，PTP 给出同时到位——需要“绕行 + 直线”时必须拆成两段。
失败归因优先看耗时形态：立即失败指向起点/目标/配置层，烧满预算指向搜索层；判断组态合法性用校验服务而不是发规划请求。
SRDF 的具名组态不保证合法：panda 样本的 transport 就让 link7 与 link5 相交 4.4 mm，作为目标会直接判非法。

参考文献与延伸阅读¶

Şucan, I. A., Moll, M., & Kavraki, L. E. The Open Motion Planning Library. IEEE Robotics & Automation Magazine, 2012. DOI
LaValle, S. M. Planning Algorithms. Cambridge University Press, 2006. 书籍主页
Karaman, S., & Frazzoli, E. Sampling-Based Algorithms for Optimal Motion Planning. The International Journal of Robotics Research, 2011. DOI

第 4 章 运动规划¶