MoveIt 运动规划：CHOMP 轨迹优化与避障调参实战

一、先搞清楚 CHOMP 在 MoveIt 里到底是"谁"

我见过太多人第一次翻到moveit_config/config/目录下的chomp_planning.yaml，第一反应是"哦，又一个 Planner，跟 OMPL 里的 RRTConnect、BiTRRT 是一类东西，换一个名字而已"。这个理解偏差不大但足够致命——它会让后面每一步调试都往错误方向使劲：你会去 RViz 的 MotionPlanning 面板里找"CHOMP"这个 Planning Library 选项，找不到，然后开始怀疑自己装的包不全。

真实情况是：在 ROS Noetic 搭配 MoveIt 1 的常规配置里，CHOMP（Covariant Hamiltonian Optimization for Motion Planning）绝大多数时候不是以独立规划器插件的身份被调用的，而是挂在规划管线的末端，作为一个 planning request adapter，去优化采样规划器已经吐出来的那条轨迹。换句话说，它吃进去的是 OMPL 给的"能走但很丑"的关节轨迹，吐出来的是"能走而且平滑、还离障碍物更远"的关节轨迹。理解了这一层，后面所有的配置项、日志、踩坑才有落脚点。

适合读这篇的人大致三类：一是正在用 MoveIt 做机械臂运动规划、被 RRT 那种随机折线轨迹折磨的工程师；二是准备把演示级 demo 推向实际工况、开始在意轨迹平滑度和关节冲击的人；三是刚接触 planning pipeline 概念、搞不清 planner 和 adapter 区别的初学者。第三类读者建议从第 1 节完整看，前两类可以直接跳到第 3 节的配置拆解和第 5 节的踩坑记录。

二、CHOMP 的真实身份：规划管线的"最后一道工序"

2.1 planner 与 adapter 是两种不同的东西

MoveIt 的规划管线可以粗略拆成三段。第一段是planning_plugin，负责从无到有生成一条满足约束的轨迹，典型实现是ompl_interface/OMPLPlanner。第二段是planning_adapters，负责在轨迹生成之后做各种加工，比如补时间参数化、修正起点碰撞、裁剪到工作空间内。第三段才是把结果塞进MotionPlanResponse返回给调用方。

CHOMP adapter 就属于第二段。它在管线里的名字是chomp/OptimizerAdapter，作用时机是"OMPL 已经给出了一条无碰撞轨迹之后"。它拿到这条轨迹，把它当作优化的初始解，然后跑一轮梯度下降，让整条轨迹同时变得更平滑、离障碍物更远。

大部分 MoveIt 配置向导生成的模板走的是 adapter 模式，因为职责清晰：出问题的要么是 OMPL 没给出种子，要么是 CHOMP 优化崩了，排查方向不模糊。

2.2 采样规划器为什么需要 CHOMP 来收尾

RRTConnect 这类采样规划器的原理是在关节空间里随机撒点、连边、搜连通性。它能给你一条无碰撞路径，但这条路径的本质是"一连串随机中途点"，形状上像一根被反复折过的铁丝。对六轴或七轴机械臂来说，这条轨迹的关节角速度和加速度在相邻路点之间是跳变的，直接下发给控制器，你会听到减速机发出的那种闷响，末端相机跟着抖，标定参数慢慢漂。

传统做法是后处理：随机裁剪（shortcut）、样条拟合、B 样条平滑。这些方法确实能让轨迹看着顺眼，但它们是启发式的——裁剪只看路径长度，不管离障碍物多远；样条拟合可能把轨迹推进障碍物里，还得再做一轮碰撞检查然后回退。CHOMP 的价值在于，它把"平滑"和"避障"放进同一个目标函数里一起优化，不是一个做完再做另一个。

2.3 目标函数长什么样

CHOMP 把整条轨迹ξ当作一个高维变量，最小化下面这个泛函：

U(ξ) = F_smooth(ξ) + F_obs(ξ)

平滑项F_smooth是轨迹的高阶导数沿时间的积分。MoveIt 把这部分拆成三个可配的权重：速度项smoothness_cost_velocity、加速度项smoothness_cost_acceleration、加加速度项smoothness_cost_jerk，默认只有加速度项是 1.0，另外两个是 0.0。加速度项权重非零意味着优化器会惩罚"关节加速度突变"，这正是消除抖动最直接的手段。

障碍项F_obs是把工作空间里每个机器人点的净空距离映射成一个代价，再沿轨迹积分。MoveIt 用的代价塑形大致是这样的分段函数（d是净空距离，ε就是collision_threshold）：

这个表格解释了collision_threshold的真正含义：它不是"多少米算碰撞"，而是"多少米以内开始产生避障压力"。碰撞判定本身是另一套碰撞检测模块在做的事。

2.4 一条实操心得

提示：如果你的机械臂末端装有比较贵的设备（相机、力传感器），把collision_threshold适当调大（比如从 0.07 调到 0.10），能让 CHOMP 主动把轨迹往外推一点，比碰撞后的急停保护要划算得多。代价是规划成功率会下降，因为可行空间被压缩了。

三、协变泛函梯度：那一步更新到底在干什么

3.1 普通梯度下降在这里为什么不работает

最朴素的想法是：把轨迹上每个路点的关节角当独立变量，对目标函数求梯度，然后每个路点各自往下走一小步。问题在于，相邻路点的位置是强耦合的——你把第 50 个路点往左推 1 厘米，第 49 和第 51 个路点的加速度立刻就炸了。为了不让轨迹自毁，步长必须压得极小，收敛慢到没法用。

CHOMP 的解法是换一个"度量"。它不用欧氏度量，而是用轨迹的平滑度本身作为度量矩阵，这就是名字里 Covariant（协变）的来源。

3.2 更新公式和我实际怎么理解它

更新规则可以写成：

ξ_{k+1} = ξ_k - η · A⁻¹ · ∇U(ξ_k)

其中A是平滑度泛函的 Hessian 矩阵，η是learning_rate。不严格地类比一下：普通梯度下降像是让每个人各自往低处挪，队伍瞬间散架；协变梯度下降像是让整条橡皮筋整体变形，局部想乱动会被橡皮筋的张力拽回来。所以它能用大得多的步长，收敛也快得多。

A其实是有限差分算子拼出来的矩阵，维度是"路点数 × 关节数"，直接求逆代价太高，工程上通过解线性方程组来实现A⁻¹的作用。这里就出现了配置里那个看起来莫名其妙的参数：ridge_factor。它做的是

(A + λI)⁻¹

也就是往矩阵对角线上加一个小量，避免矩阵接近奇异时数值解炸掉。默认 0.01。当你的轨迹路点数很少、或者关节数很少的时候，A本身条件数很好，这个值可以调到 1e-4 级别；反过来，如果规划时经常出现轨迹剧烈振荡甚至数值异常，把它调到 0.1 试试。

use_pseudo_inverse和pseudo_inverse_ridge_factor是另一条路：当矩阵病态到加 ridge 也救不回来时，改用伪逆求解，伪逆同样需要一个 ridge 因子，默认 1e-4。这两个参数我在实际项目里几乎没动过默认值，只有在七轴冗余机械臂上做长轨迹优化时开过一次，效果不明显。

3.3 障碍代价的梯度从哪来

平滑项的梯度好求，纯代数。障碍项的梯度需要绕一圈：先在工作空间里算出机器人各碰撞体上离障碍物最近的那些点，对这些点求距离场的空间梯度；再通过雅可比矩阵把这些工作空间梯度映射回关节空间。这就是"协变"的另一半含义——工作空间的力被雅可比转置拉回到关节空间，变成一组虚拟力矩。

MoveIt 里的距离场是一张覆盖工作空间的规则网格，每个格点存着到最近障碍物的有符号距离。这带来两个非常现实的后果：第一，距离场的分辨率决定了它能感知多细的障碍物；第二，距离场的范围决定了内存占用和每次查询的开销。

注意：距离场是"体素化"的。一块 2 毫米厚的薄板在距离场里可能整个消失，CHOMP 会毫无察觉地穿过去，而传统的碰撞检测模块却能正确报出碰撞。这是"规划出来的轨迹撞了障碍"最常见的隐藏原因之一，第 5 节会详细展开。

3.4 什么时候停

迭代终止条件有三个，配置里对应三个参数：

• max_iterations：迭代次数硬上限，默认 200。
• max_iterations_after_collision_free：一旦当前轨迹变成无碰撞，再做几次迭代就收工。默认 5。
• planning_time_limit：墙钟时间上限。

第二条特别值得说。为什么轨迹无碰撞之后不继续优化到极致？因为障碍代价一旦归零，剩下的只有平滑项在起作用，优化器会把轨迹越拉越直——直着走确实更平滑，但很可能擦着障碍物边缘过去，或者干脆又把自己推回障碍物里。留 5 次迭代作为"缓冲区"是个挺务实的折中。

use_stochastic_descent: true打开时，每次迭代不是对所有路点求梯度，而是随机挑一部分路点算有限差分。这个做法在碰撞检测开销占主导时特别划算，因为碰撞检测是整条管线里最贵的操作。代价是收敛过程带随机性，同样输入两次规划结果可能略有差异。

四、在 Noetic + MoveIt 1 里把 CHOMP 接上

4.1 先确认你手上的 MoveIt 带不带 CHOMP

不同发行版、不同安装方式下包名会有差异，不要凭记忆硬写。两条命令确认：

rospack find moveit_planners_chomp rospack find moveit_chomp_optimizer_adapter

两条都能返回路径，说明插件齐了。如果某一条报 "package not found"，用 apt 补上对应的规划器包即可。接着确认插件描述文件里注册的类名，这个类名后面要写进 launch：

grep -r "OptimizerAdapter\|ChompPlanner" \ $(rospack find moveit_chomp_optimizer_adapter) \ $(rospack find moveit_planners_chomp) \ --include=*.xml

正常会看到类似<name>chomp/OptimizerAdapter</name>和<name>chomp/ChompPlanner</name>的登记项。记下前者，它就是我们要挂进管线的那个。

4.2 改 launch：走 CHOMP 管线

MoveIt 配置向导生成的move_group.launch里通常已经有管线的分支逻辑，靠一个pipeline参数切换。你需要确认三件事。

第一，move_group.launch里引入了 CHOMP 管线的描述文件：

<include file="$(find your_robot_moveit_config)/launch/chomp_planning_pipeline.launch.xml"> <arg name="start_state_max_bounds_error" value="0.1" /> <arg name="jiggle_fraction" value="0.05" /> </include>

第二，chomp_planning_pipeline.launch.xml里同时加载了 OMPL 的参数（出种子用）和 CHOMP 的参数，并且把 adapter 挂上：

<launch> <arg name="planning_plugins" default="ompl_interface/OMPLPlanner" /> <arg name="planning_adapters" default="chomp/OptimizerAdapter" /> <rosparam command="load" file="$(find your_robot_moveit_config)/config/ompl_planning.yaml" /> <rosparam command="load" file="$(find your_robot_moveit_config)/config/chomp_planning.yaml" /> <param name="planning_plugin" value="$(arg planning_plugins)" /> <param name="request_adapters" value="$(arg planning_adapters)" /> <rosparam command="load" file="$(find your_robot_moveit_config)/config/kinematics.yaml" /> </launch>

第三，启动时显式指定管线，避免默认值把你带偏：

roslaunch your_robot_moveit_config demo.launch pipeline:=chomp

提示：不同版本的 MoveIt 配置模板里，planning_adapters和request_adapters两个参数名可能只出现一个，甚至有版本把 CHOMP adapter 直接写在move_group.launch的default_planner_request_adapters里。以你自己moveit_config里那份模板为准，不要照抄别人的。

4.3chomp_planning.yaml参数逐条拆解

下面这张表是我按"改动的性价比"排序的，前三行覆盖了 80% 的调优场景。

关于文件结构还有一点容易踩：有些版本的chomp_planning.yaml是按规划组分节的，形如：

manipulator: planner_plugin_name: chomp/ChompPlanner planning_time_limit: 5.0 max_iterations: 150 smoothness_cost_weight: 0.1 ...

而有些版本是平铺的、不带组名前缀。两种都能工作，取决于你的 MoveIt 版本和配置向导的行为。判断方法很简单：看你的ompl_planning.yaml是不是按组名分节，两份文件通常保持同一种风格。

4.4 怎么确认 CHOMP 真的生效了

别靠肉眼在 RViz 里看轨迹形状——那个差别不够显著，容易自我暗示。用数据说话，我一般走这三步。

第一步，确认参数确实被加载进了/move_group：

rosparam dump /tmp/mg.yaml /move_group grep -n -i -A3 "chomp" /tmp/mg.yaml

第二步，用 rqt_console 过滤 "chomp" 关键字，正常会看到 adapter 加载和优化过程的日志；如果一片空白，说明根本没挂上。

第三步，也是最关键的一步：同一条规划请求，分别在pipeline:=ompl和pipeline:=chomp下跑，用脚本比较轨迹的平滑度指标。指标我一般算两个：相邻路点关节角差值的平方和（速度能量），以及二阶差分的平方和（加速度能量）。

import numpy as np def traj_energy(traj_points): """traj_points: list of trajectory_msgs/JointTrajectoryPoint""" q = np.array([p.positions for p in traj_points]) d1 = np.diff(q, axis=0) d2 = np.diff(d1, axis=0) return { "velocity_energy": float(np.sum(d1 ** 2)), "acceleration_energy": float(np.sum(d2 ** 2)), "n_points": q.shape[0], }

念一下这两个数：CHOMP 生效时，加速度能量应该显著低于 OMPL 的原始结果，常常能降一个数量级；速度能量则可能略有上升，因为 CHOMP 会把轨迹拉长去绕开障碍。如果两个数几乎一样，那 CHOMP 肯定没起作用。

五、调参的正确顺序：先通、再净、最后快

调 CHOMP 参数最容易犯的错是一次改五六个值然后观察结果。参数之间是耦合的，你分不清是哪个起了作用。我固定按下面三个阶段来。

5.1 第一阶段：让规划别再失败

这个阶段的目标只有一个——plan()能稳定返回轨迹。先把平滑相关的参数全部放回默认，动这几个：

先把enable_failure_recovery打开，max_recovery_attempts设 5。这个机制的工作原理是：优化失败时，扰动起点状态再重新来一遍。对"起点刚好卡在障碍物边缘"这类问题特别有效。

然后检查planning_time_limit。很多人把它和 OMPL 的 timeout 搞混。OMPL 的 timeout 管的是找种子轨迹的时间，CHOMP 的planning_time_limit管的是优化阶段的时间。如果日志里显示"OMPL 找不到解"，调这个参数毫无意义，你要去调ompl_planning.yaml里的timeout或者换用 RRTConnect。

接着把joint_update_limit从 0.1 降到 0.05，learning_rate从 0.01 降到 0.005。这两步会让单次迭代更保守，成功率上升，代价是收敛变慢。等成功率稳住了，再一点点往回加。

最后确认初始轨迹来源。如果你的场景里 OMPL 经常给不出解，可以在chomp_planning.yaml里用trajectory_initialization_method指定用插值填充的方式生成初始轨迹（比如线性插值或五次样条插值），绕过对 OMPL 的强依赖。注意走这条路时，初始轨迹大概率是有碰撞的，CHOMP 需要更多迭代才能把轨迹推出障碍物，max_iterations建议不低于 300。

5.2 第二阶段：消除穿透障碍和关节跳变

成功率稳定之后，开始处理质量问题。按我遇到的频率排：

穿透障碍。先怀疑碰撞体和距离场，而不是参数。检查 Planning Scene 里障碍物是否真的加进去了（RViz 里能看到才算），检查障碍物有没有薄壁结构。确认无误再动collision_threshold——从 0.07 往上加到 0.10 或 0.12，让避障压力区变宽。

轨迹贴障碍。提高obstacle_cost_weight到 2.0 甚至 3.0。同时把smoothness_cost_weight从 0.1 降到 0.05，因为平滑项的"拉直"效应会让轨迹抄近路。这两个参数是一对跷跷板，一边加一边减是常态。

关节角度跳变。典型的信号是相邻路点某个关节角差了几十度，RViz 里看起来轨迹突然"抽"一下。这通常是joint_update_limit太大，优化器一次推得太猛，把轨迹推到了解空间的另一侧。降到 0.02~0.05 能明显改善。

残余高频抖动。如果轨迹大体平滑但仍有细碎抖动，把smoothness_cost_jerk从 0 开到 0.05，会给"加加速度突变"额外加一道惩罚。这个操作在高精度装配场景下值得做，普通搬运场景收益不明显。

5.3 第三阶段：压缩规划时间

CHOMP 单次优化的耗时通常在 0.5 到 3 秒之间，取决于迭代次数、路点数、碰撞检测开销。慢的时候我按这个顺序排查：

先降max_iterations。默认 200 往往偏保守，实测 100~150 大多数场景够了。注意降这个值会同时影响轨迹质量，属于用质量换时间的交易。

再降max_iterations_after_collision_free。如果轨迹一旦无碰撞就急着收工，设成 1 也能用。

然后是最有效但最少人想到的一招：换掉高面数网格碰撞体。距离场的构建和查询开销跟碰撞几何体的复杂度强相关。我做过一次实测，把一条机械臂的 STL 网格（几万个三角面）换成由十几个 box 和 cylinder 拼出的近似外形，规划时间从 2.8 秒降到了 0.4 秒，轨迹质量肉眼几乎没差别。碰撞检测的保守性还下降了，因为包围盒近似通常比原始网格大一点点，反倒更安全。

最后是缩小工作空间。距离场覆盖整个 Planning Scene 里声明的 workspace bounds，你把范围从 3 米见方缩到 1.5 米，网格数少四倍，内存和查询开销都跟着降。这个参数由工作空间边界和FixWorkspaceBoundsadapter 决定。

5.4 一份可以直接抄的起步配置

这是我常用的起点，六轴机械臂、搬运/装配场景，实测通过率不错：

planner_plugin_name: chomp/ChompPlanner planning_time_limit: 5.0 max_iterations: 150 max_iterations_after_collision_free: 5 smoothness_cost_weight: 0.1 obstacle_cost_weight: 1.5 learning_rate: 0.01 smoothness_cost_velocity: 0.0 smoothness_cost_acceleration: 1.0 smoothness_cost_jerk: 0.0 ridge_factor: 0.01 use_pseudo_inverse: false pseudo_inverse_ridge_factor: 1.0e-4 joint_update_limit: 0.05 collision_clearance: 0.2 collision_threshold: 0.10 use_stochastic_descent: true enable_failure_recovery: true max_recovery_attempts: 5

改完记得核对一下ompl_planning.yaml里同一个规划组的 timeout，别让 OMPL 那边先超时——种子的质量直接决定 CHOMP 的起点好坏。

六、我踩过的六个坑，以及完整的排查链路

这一节我按"现象 → 排查动作 → 根因 → 修复"的顺序写，你可以照着复现排查思路。

6.1 坑一：选了 CHOMP 却完全没效果

现象：改完 launch、加了配置、重启节点，规划出来的轨迹和以前一模一样，且chomp_planning.yaml里的参数怎么改都没反应。

排查链路：先在 rqt_console 里按 "chomp" 过滤日志，一片空白——这就是第一个线索。然后rosparam dump看/move_group下有没有 CHOMP 那一坨参数，发现也没有。说明rosparam command="load"那行根本没执行到，多半是move_group.launch里的分支逻辑没走到 CHOMP 管线，或者你改的是chomp_planning_pipeline.launch.xml，但启动时pipeline参数仍是默认的ompl。

修复：启动时显式带上pipeline:=chomp，或者去改move_group.launch里pipeline的默认值。改完再用rosparam dump确认参数到位。这一步验证成本极低，但能省掉后面半小时的无效调参。

顺带一提：因为是 adapter 模式，你在 RViz 里选 RRTConnect 还是 BiTRRT，最终都会走 CHOMP 优化。这一点反直觉，但却是判断 adapter 是否生效的一个快捷方法——把 planner 从 RRTConnect 换成另一个，如果优化后轨迹的平滑度指标几乎不变，说明 CHOMP 确实在管事。

6.2 坑二：规划直接返回空轨迹

现象：plan()返回 False，日志里出现 CHOMP 优化失败的提示。

排查链路：先分清是哪个环节失败。抓两条日志关键词：OMPL 的 "Unable to find a solution" 和 CHOMP 的优化失败提示。前者说明没种子，后者说明有种子但优化崩了。

如果落在 OMPL 侧，检查四件事：起点状态是不是在碰撞（可以用FixStartStateCollisionadapter 自动微调）、目标位姿是否可达（用笛卡尔路径接口试着走一小段，看看 IK 解不算不出来）、ompl_planning.yaml里该组的 timeout 是不是被设成了 0.5 秒这种极限值、还有 Allowed Collision Matrix 是不是把某些本该检测的连杆对给禁掉了。

如果落在 CHOMP 侧，多半是learning_rate太大导致发散，或者joint_update_limit太大把轨迹推到关节限位外。先把这两个值减半再试。

6.3 坑三：轨迹看着没问题，但实际执行时撞了

现象：RViz 里轨迹和障碍物完全没交集，实机上却擦了。或者反过来，RViz 里看着贴得极近，实机反而没事。

排查链路：这个坑我花了最久才定位。核心是距离场的体素化问题。距离场的分辨率是有限的，一块薄板、一根细管、一片护栏，可能在网格上只占不到一个体素，被直接抹掉。传统碰撞检测用的是精确几何求交，能感知到毫米级的厚度，两套机制的能力边界完全不同。

验证方法很直接：临时把障碍物的尺寸撑大一圈（比如 2 毫米的薄板换成 20 毫米的方块），重新规划。如果瞬间不撞了，基本就是体素化的问题。

修复：在生成碰撞体时主动做加厚处理。对薄板类物体，不要直接把网格丢进 Planning Scene，用一个稍微大一点的外接 box 近似；对细长杆件，用 cylinder 包一层。这是个纯粹的工程妥协——牺牲一点规划空间换取确定性。

另一个相关但不同的坑是自碰撞。距离场主要覆盖环境障碍物，机械臂自身的碰撞要靠碰撞检测模块兜底。如果你在 SRDF 里图省事把大量连杆对都设成disable_collisions，CHOMP 那边是感知不到的，它会很大方地让相邻连杆穿模。我的做法是：只禁用真正相邻、几何上不可能互碰的连杆对，其余全部保留检测。宁可多花点碰撞检测时间。

6.4 坑四：规划时间从 1 秒变成 8 秒

现象：加了 CHOMP 之后，整个规划调用从 1 秒变成 8 秒，节拍完全撑不住。

排查链路：先用 rosconsole 的计时输出定位耗时在哪一段。如果时间花在优化迭代上，看max_iterations和路点数；如果时间花在碰撞检测上，看碰撞几何体的面数和碰撞矩阵的大小。

修复，按性价比排序：

一是换简化碰撞体，前面说过，收益最大。二是砍max_iterations到 100~150。三是确认use_stochastic_descent是 true，这个开关在碰撞检测昂贵时能省掉大量梯度计算。四是缩小工作空间范围，减少距离场网格数。五是检查有没有把无关的物体也丢进了 Planning Scene——见过有人把整个厂房模型都加进去，距离场网格数直接爆掉。

6.5 坑五：轨迹没有时间参数化，速度超限

现象：轨迹能规划出来，但下发给控制器时报速度超限，或者发现轨迹点的time_from_start全是零。

排查链路：

rostopic echo -n1 /move_group/display_planned_path \ | grep -A5 "time_from_start"

如果所有时间戳都是 0，说明时间参数化那一步没做。

修复：确认AddTimeParameterizationadapter 在你的 adapter 链里。注意这里有个容易搞混的地方——在 adapter 模式下，时间参数化通常由另外的 adapter 负责，跟chomp/OptimizerAdapter是两个独立的条目；如果你自定义了planning_adapters列表，很容易把时间参数化那一条覆盖掉。检查方法就是把request_adapters的完整值打印出来看一遍。

时间参数化补上之后，还要看速度缩放因子。MoveIt 默认按关节速度上限跑，如果你的控制器实际承受不了，得在调用端设max_velocity_scaling_factor。

6.6 坑六：需要末端走直线，CHOMP 却不给面子

现象：任务要求末端沿直线插入或沿直线扫描，CHOMP 优化出来的轨迹末端路径是弯的，插入时刮到工装。

排查链路：这不是 bug，是原理决定的。CHOMP 优化的目标函数里只有关节空间的平滑度和工作空间的障碍距离，完全没有"末端走直线"这一项。它只关心关节轨迹好不好看、离障碍远不远，末端轨迹是什么形状它不负责。

修复：别让 CHOMP 管直线段。把任务拆成两类动作——"走位段"（从 A 位姿挪到 B 位姿，路径不关心）交给 OMPL + CHOMP；"工艺段"（直线插入、直线扫描）用笛卡尔路径接口逐段做 IK 插值，或者用工业运动规划器那类确定性方案直接生成 PTP/LIN/CIRC 指令。两段之间拼接时注意接口处的速度连续性，我一般会在拼接点前后各留一小段过渡，让速度缩放因子平滑过渡过去。

提示：如果你在管线里同时挂了FixStartStatePathConstraintsadapter，注意它和 CHOMP 优化的顺序关系。这个 adapter 会在起点不满足路径约束时尝试修正，而它的修正结果可能覆盖掉 CHOMP 的优化成果。调试路径约束相关问题时，先把 CHOMP 摘掉，确认约束链路本身没问题，再把 CHOMP 加回来。

七、CHOMP、OMPL、STOMP，还有"别人家的 planner"

7.1 一张表看清分工

选型逻辑其实很清楚。"从哪到哪"用 OMPL，"怎么走得漂亮"用 CHOMP。这两个是接力关系不是替代关系。STOMP 的定位跟 CHOMP 类似，区别在它用随机采样估计梯度，对不可导或者带硬约束的代价函数更宽容，但收敛稳定性不如 CHOMP。确定性工业规划器解决的问题层次又不一样，它保证末端轨迹形状可预测，代价是几乎不做避障优化——所以工业现场通常是"确定性规划器走工艺段 + 采样规划器走过渡段"的组合。

7.2 别被同名热词带偏

搜索planner这个词会撞出一大堆不同层面的东西，顺手澄清一下，省得你在错误的文档里绕圈。

机械臂领域的 planner（OMPL、CHOMP、STOMP）做的是关节空间运动规划，输入是关节角起点和末端目标位姿，输出是一串关节角度序列。这是本文讨论的层次。

无人机领域的局部轨迹规划器（比如 EGO 这一类）做的是工作空间中的连续轨迹优化，通常在 ESDF 距离场上做 B 样条优化，同时处理动力学约束和障碍物。它有"优化"的骨架，但状态空间是位姿加动力学，不是纯关节角，跟 CHOMP 解决的数学问题形态不同。

地面站软件里的航线规划（Mission Planner 这类工具）做的是任务级路径规划，在二维地图上规划一条经过若干航点的航线，跟"动作怎么执行"完全不在一个层面。

扫描规划（scan planner）通常指覆盖路径规划，解决的是"用最小代价把一片区域覆盖完"，是组合优化问题，跟避障平滑没关系。

搞清楚这个分类，你在搜资料时就不会拿无人机轨迹优化的参数去套机械臂的 CHOMP 配置了。

7.3 我的组合策略

实际项目里我用的是一条固定链路：RRTConnect 出种子（timeout 给 1~2 秒）→ CHOMP 优化（150 次迭代以内）→ 时间参数化 → 速度缩放 → 下发给控制器。中间如果 CHOMP 优化失败，降级直接用 OMPL 的原始轨迹加一道 B 样条平滑，保证系统不会因为轨迹太丑就完全停摆。

工艺段（直线插入、螺旋拧紧、圆弧涂胶）单独走笛卡尔路径接口，不经过 CHOMP。这样两套逻辑各自简单，出了问题也好定位。

八、最后分享几个我在实际使用中攒下的小经验

第一，参数改动一定要做 A/B 记录。我习惯每次只改一个参数，把「参数值 / 规划成功率 / 平均耗时 / 加速度能量」四个数记在一个表格里。跑上二三十组之后，你会发现有些参数的影响方向和直觉是反的——比如把obstacle_cost_weight从 5 提到 20，成功率不是提高而是断崖式下跌，因为避障压力太大把轨迹逼到解空间边缘，优化直接发散。

第二，距离场的分辨率是硬约束，改参数救不回来。如果确认障碍物是薄壁、细杆、网格这类结构，第一步永远是简化碰撞体，不是调collision_threshold。我在这上面浪费过整整两天。

第三，给 CHOMP 一个"热身"的机会。如果是重复性任务，第一次规划走得慢一点没关系，把结果缓存下来复用，效率提升立竿见影。轨迹缓存这个思路在节拍敏感的产线上比调参数管用得多。

第四，调试时把 OMPL 的种子轨迹 dump 出来看一眼。CHOMP 是局部优化，种子质量差的时候它只能在附近打转。我遇到过一次规划结果始终贴障碍，最后发现是 RRTConnect 给了一条本来就贴着障碍的种子路径，CHOMP 只做了平滑，没能力把它整体搬走。换了种子来源，问题自己就没了。