企业官网建设流程全解析

做机械臂控制这一行，最近几年绕不开的一个组合就是 ROS Noetic 加 MoveIt。我前前后后折腾过不少规划器，从最常用的 OMPL 采样族，到 STOMP，再到这篇文章要重点拆的 CHOMP Planner，中间踩的坑足够写一本小册子了。先把范围说清楚：这里讲的 planner 是机械臂的运动规划器（motion planner），跟无人机地面站那类软件完全不是一回事——你搜 “planner”“mission planner” 这类词，出来一大片都是航测、地面站相关的东西，很容易被带偏。找资料的时候把 “moveit_planners_chomp” 这个包名直接带上，命中率会高很多。CHOMP 全称是 Covariant Hamiltonian Optimization for Motion Planning，一句话概括：它不靠随机采样去“撞”出一条可行路径，而是先把一条很蠢的初始轨迹当成橡皮筋，然后一边拉直、一边往外推，用梯度下降把这条橡皮筋揉成一条又平滑又远离障碍的轨迹。它能解决的问题很具体——采样规划器给出的路径往往抖、贴障碍、需要一堆后处理，而 CHOMP 出来的东西天然平滑。适合谁看：手上有一个静态工作环境的机械臂项目（抓取台、打磨工位、实验室自动化），规划组自由度在 6 到 8 之间的朋友，看完能直接照着配起来跑。如果你做的是高速动态避障或者双臂协同，那得先看完它的局限那部分再决定要不要投入。

1. CHOMP 的核心思路到底特别在哪

1.1 三条技术路线，先搞明白自己站在哪一边

机械臂运动规划这块，主流做法大致能分成三类。第一类是采样规划，代表就是 MoveIt 里默认那一整套 OMPL 算法，RRTConnect、RRT*、PRM、BIT* 都算，思路是在关节空间里随机撒点、连边、建树，撞到障碍就重来，直到起点和目标点连通。第二类是优化规划，CHOMP、STOMP、TrajOpt 都属于这一支，它不是“找”路径，而是“改”路径，从一条已有的初始轨迹出发，不断迭代降低一个代价函数。第三类是搜索规划，在栅格或者离散状态空间上用 A*、D* 之类做搜索，移动机器人上用得多，高自由度机械臂上基本撑不起来。

为什么要费劲换到第二类？因为采样规划有个绕不过去的性格特点：它只保证概率完备，也就是“跑得够久，一定能找到解”，但它对“这条路径好不好”几乎不管。你可以试试让 RRTConnect 规划十次，十次的路径都不一样，有的绕大圈，有的贴着桌沿擦过去，有的在关节空间里来回抖。生产环境里这种不确定性很要命——同一个工位循环动作，每次轨迹都不一样，节拍没法算，减速机的磨损也没法评估。所以采样规划后面通常还要挂一串后处理：shortcut、简化、平滑、时间参数化，一层层修。CHOMP 的价值就在于，它把“平滑”和“离障碍远”这两件事写进了目标函数里，是从根上解决的，而不是事后补妆。

1.2 代价函数：一条轨迹的“不舒服程度”怎么量化

CHOMP 的整个逻辑建立在一个代价函数上，形式可以粗略写成平滑项加障碍项两类权重之和。平滑项负责让轨迹别乱扭，障碍项负责让轨迹别撞东西，两者用一个权重比例调和。平滑项最朴素的写法是整条轨迹上关节速度平方的积分，也就是说，轨迹越“弯”、速度变化越剧烈，代价越高；实际实现里还会把加速度、加加速度的平方也作为可选项加进去，分别对应“别突然加速”“别突然抖一下”这两种诉求。这部分理解起来不难，就是个能量泛函。

真正的巧思在障碍项。障碍项不是简单地对每个路点算一个“离障碍多远”然后求和，而是写成距离代价乘以该点的速度模长，再沿轨迹积分。为什么要乘速度模长？因为如果只对路点求和，那么轨迹上点密的地方权重天然就大，优化器会发现一个“作弊”的降代价方式：把点挪到点稀疏的地方去，而不是真的远离障碍。乘上速度模长之后，代价值就对齐到了轨迹弧长上，跟你怎么给轨迹打时间戳、怎么重采样都无关，这个性质在论文里叫重参数化不变性。这是我个人认为 CHOMP 设计上最漂亮的一笔，很多自己写轨迹优化的人第一次都会栽在这个地方，调出来的轨迹总有几段莫名其妙地贴着障碍，查半天才发现是代价定义里少了速度项。

距离代价本身是个截断二次函数：离障碍的距离小于一个阈值（也就是参数里的collision_threshold，默认量级在 7 厘米左右）时，代价按距离差的平方增长；距离大于阈值时，代价直接归零。这个设计意味着 CHOMP 只关心“危险区域”内的点，远处的点不再产生梯度，计算量一下子小了很多。阈值这个数就是你实际想要的安全余量，设多大取决于你的机械臂定位精度、工件公差、以及现场有没有人。我一般会把它设得比标称安全距离略大一点，留点余量给执行误差。

1.3 协变梯度下降：为什么整条轨迹能一起动

有了代价函数，下一步是求解。最直接的想法是梯度下降：算一下每个路点应该往哪个方向挪，挪一点，再算，再挪。但这里有两个坑。第一个坑是梯度尺度问题，轨迹重采样之后，同样的物理形状会得到完全不同的梯度幅值，步长没法统一设。第二个坑是每个点独立更新会导致轨迹被“揉碎”，相邻点各走各的，出来的结果比原来还抖。

CHOMP 的解法是协变更新，公式写出来大概是：新轨迹等于旧轨迹减去一个由度量矩阵的逆乘上梯度得到的修正量，前面还有一个步长系数。这个度量矩阵由平滑代价的二阶结构构成，再在对角线上加一个小量（就是ridge_factor，默认 0.001 这个量级）保证数值上可逆。直观理解：这个度量矩阵把相邻路点耦合在一起了，所以一次更新不是每个点各自为战，而是整条轨迹作为一个弹性体协调地移动。结果就是，平滑性不是在最后一步硬加上去的，而是从迭代过程里长出来的。这也是为什么 CHOMP 输出轨迹的质量对平滑权重的敏感度没有想象中那么高——它本身的更新方式就已经带了一层平滑。

那障碍的梯度从哪来？这里就要说到距离场了。MoveIt 里的 CHOMP 会先把规划场景里的环境物体栅格化，构建一个带符号的距离场，场里每个格点都存着“离最近障碍表面的距离”和“指向最近点的方向”。对机器人身上的每一个碰撞体，查一次场就能拿到距离和方向，再通过机械臂的雅可比矩阵把这个方向映射回关节空间，就得到障碍代价对关节角的梯度。距离场是预计算的，场景不动的话建一次能用很久，这就是 CHOMP 单次规划能做到几十毫秒量级的根本原因。同时也是它不擅长动态环境的根本原因——场景一变，距离场就得重建，重建的开销可比规划本身大多了。这个“预计算换速度”的取舍，决定了 CHOMP 的适用边界：适合固定工位反复规划，不适合人走来走去、物料随时被搬动的环境。

2. Noetic 环境下把 CHOMP 接进 MoveIt 的完整流程

2.1 依赖安装与插件类名的确认

先说版本选择。标题里写 Noetic 是有道理的，ROS1 Noetic 是 MoveIt 生态里 CHOMP 支持最完整、社区资料最多的一代。ROS2 那边的 MoveIt2 主推的规划器组合已经变了，CHOMP 的移植状态和参数体系跟 ROS1 不完全一致，所以如果你手上是 ROS2 项目，建议先确认清楚对应仓库的维护状态再动手。Noetic 这边装依赖就一条命令：

sudo apt install ros-noetic-moveit-planners-chomp

装完之后不要急着改配置，先做一件事——确认插件类名。因为不同小版本之间，插件导出的类名写法可能不一样，写错了 move_group 启动时不会报错，只会在规划器下拉框里默默少一项，能让你查半天。做法是先定位包路径，再看里面的插件描述文件：

rospack find moveit_planners_chomp cat $(rospack find moveit_planners_chomp)/chomp_interface_plugin_description.xml

里面会有一行类似<class name="chomp_interface/CHOMPPlanner" ... base_class_type="planning_interface::PlannerManager">的声明。name属性里的那个字符串，就是你等下要填进 YAML 的type值。老版本里可能直接写成CHOMPPlanner，Noetic 常见的是带命名空间前缀的写法。这一步花两分钟，能省掉后面半天排查。

2.2 chomp_planning.yaml 该怎么写

MoveIt 的规划器配置是放在 move_group 节点的私有参数命名空间里的，YAML 结构分两层：顶层planner_configs是全局的规划器定义表，下面按规划组名再列一份该组可用的规划器名单。下面这份是我在实际项目里用的骨架，参数都带了注释说明，你可以直接抄：

planner_configs: Chomp: type: chomp_interface/CHOMPPlanner # 以插件描述文件里的 name 为准 animate_path: false # true 会在 RViz 里动态画出迭代过程 max_iterations: 200 # 优化迭代上限 max_time: 10.0 # 单次优化时间上限，单位秒 collision_threshold: 0.07 # 障碍代价生效的距离带宽度，单位米 random_jump_amount: 1.0 # 失败恢复时的随机扰动幅值 joint_update_limit: 0.1 # 单次迭代单个关节的最大变化量，弧度 smoothness_cost_weight: 0.1 # 平滑项总权重 smoothness_cost_velocity: 1.0 # 平滑项里速度成分的权重 smoothness_cost_acceleration: 0.0 # 加速度成分权重 smoothness_cost_jerk: 0.0 # 加加速度成分权重 ridge_factor: 0.001 # 度量矩阵求逆的数值稳定项 min_clearance: 0.0 # 最终轨迹接受时的最小间隙要求 obstacle_cost_weight: 1.0 # 障碍项总权重 use_stochastic_descent: true # 是否用随机化的梯度估计 enable_failure_recovery: true # 迭代不收敛时是否重试 max_recovery_attempts: 5 # 最大重试次数 use_pseudo_inverse: false # 是否用伪逆替代稠密求解 pseudo_inverse_ridge_factor: 1.0e-4 use_hamiltonian_monte_carlo: false # 高级选项，一般别动 trajectory_initialization_method: quintic-spline resample_dt: 0.1 # 输出轨迹的重采样时间间隔 min_angle_change: 0.001 # 重采样时的最小角度变化 # 下面这一段的 key 必须和 SRDF 里的规划组名完全一致 panda_arm: planner_configs: - Chomp default_planner_config: Chomp

有几点必须提醒。第一，参数的默认值在不同小版本里可能有出入，尤其是max_time、max_recovery_attempts这种，建议配置完之后用rosparam get /move_group/planner_configs/Chomp看一下实际生效值，别凭记忆。第二，规划器参数是在 move_group 初始化时读进去的，改完 YAML 必须重启 move_group 才生效，ROS1 这边没有热重载。第三，也是最多人踩的坑：不要直接把 ompl_planning.yaml 替换成 chomp_planning.yaml。网上不少教程是这么写的，但那样你会在 RViz 里彻底失去 OMPL 那一整套规划器，做 A/B 对比的时候非常难受。正确的做法是把 CHOMP 的planner_configs条目合并进同一份 YAML，或者在组名的planner_configs列表里同时列上 OMPL 的配置和 Chomp，这样下拉框里两套规划器都在，随时切换对比。

2.3 在 move_group.launch 里挂载并验证

配置文件的加载位置在 move_group 的 launch 文件里，找到原来加载ompl_planning.yaml的那一行，在它后面加一行加载我们的文件：

<node name="move_group" ...> <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"/> ... </node>

注意两点：两次rosparam load的内容会做键级合并，不冲突的键会累加，所以只要两份文件里没有同名规划器配置就安全；但如果两份 YAML 的顶层键完全相同（比如都叫planner_configs并且里面有同名条目），后加载的会覆盖先加载的。合并写入同一份文件是最省心的做法，我一直是这么干的。

验证分三步。第一步，启动之后敲：

rosparam get /move_group/planner_configs

正常的话你会看到 Chomp 这个条目和它下面所有参数都列出来了。如果什么都没有，说明 YAML 没加载成功，检查文件路径和节点命名空间。第二步，打开 RViz 的 MotionPlanning 面板，在 Planning 标签页的 Planner 下拉框里找 “Chomp”，找到就说明插件注册成功了。第三步，切到 Chomp，随便给个目标位姿点 Plan，看能不能出轨迹。三步都过，环境就算搭好了。

注意：如果你的规划组里有 continuous 类型的关节（比如某些移动底盘的轮子、无限旋转的末端滚轮），强烈建议把这类关节从 CHOMP 的规划组里排除掉。这类关节在弧度和角度之间来回绕，距离场梯度映射回关节空间的时候容易出问题，表现出来就是规划时好时坏、偶尔报一些看不懂的矩阵错误。

3. 参数怎么调：从“能跑”到“好用”

3.1 平滑权重：先搞清楚它调的是哪一层

smoothness_cost_weight是平滑项的总权重，smoothness_cost_velocity、smoothness_cost_acceleration、smoothness_cost_jerk是平滑项内部的三个成分，三个乘起来才是最终权重。所以如果你只把总权重从 0.1 调到 0.5，但加速度成分是 0，那实际上你只是在“更用力地把轨迹拉直”，并没有额外惩罚加速度。这个概念我第一次看配置的时候也绕了一下，因为直觉上会以为三个参数是并列的。

实际调的时候我是这么做的：默认情况下只动总权重，先把smoothness_cost_velocity保持在 1，加速度和加加速度都留 0，把总权重在 0.05 到 1.0 之间扫一遍，看轨迹和规划时间的变化。总权重太小（比如 0.01）会出现轨迹在障碍附近来回摆动的现象，因为障碍项压过了平滑项；总权重太大（比如 5.0）会导致轨迹死活推不出去，明明前面没障碍它也要走一条特别保守的圆弧。找到那个既平滑又能正常避障的区间之后，如果执行时末端有明显抖动，再把加速度成分的权重从 0 加到 0.1 到 0.5 这个量级，专门压抖动。加加速度成分我一般在打磨、涂胶这种对速度连续性有要求的场景才开，开了之后规划时间会明显变长。

3.2 碰撞阈值与障碍权重：安全余量的主控开关

collision_threshold是我认为整个 CHOMP 里最值得先调的一个参数。它决定了候选轨迹离障碍多远的时候开始产生“推力”，直接对应你最终轨迹的贴障碍程度。设得太小，比如 0.02 米，轨迹会擦着障碍过，实际执行时机器的定位误差、工件装配误差一叠加就可能撞上；设得太大，比如 0.3 米，等于给整个工作空间套了一圈粗管子，稍微窄一点的缝隙就穿不过去，CHOMP 会直接报找不到可行解。

我的经验值是：先取机械臂重复定位精度的三到五倍作为起点。比如你的机械臂标称重复定位精度是正负 0.1 毫米，但实际带负载、带视觉标定误差之后综合误差可能到 3 到 5 毫米，那就把阈值设在 0.02 到 0.03 米起步。装配工位这种对干涉特别敏感的场合我会加到 0.05 米以上。另外注意这个值是有量纲的物理距离，跟你的机械臂尺寸要匹配——小型的桌面级机械臂用 0.07 米可能已经是整个臂展的一大截了。

obstacle_cost_weight控制障碍项在总代价里的比重，默认是 1.0。这个参数我不太建议乱动，因为平滑项和障碍项的平衡已经通过smoothness_cost_weight和collision_threshold调过了，再动障碍权重容易出现两个参数互相打架的情况。真要调的话，我一般是在某个特定场景下发现轨迹死活推不出障碍区，才会把它临时提到 2.0 试一下，确认是障碍项推力不够，然后再回过头去调阈值。

3.3 迭代次数、时间上限与失败恢复

max_iterations和max_time是两道保险，谁先触发就按谁停。默认 200 次迭代对大多数 7 自由度机械臂场景是够的，实测下来大部分情况 50 到 150 次就收敛了。但如果你发现规划结果总是不收敛、返回的轨迹还有碰撞，可以先把这个数字提到 500 试试。max_time我一般设在 5 到 10 秒，因为 CHOMP 单次迭代很快，真跑满 10 秒还没收敛，基本可以判定是掉进局部极小值了，继续等也没意义，不如让失败恢复机制上场。

enable_failure_recovery加max_recovery_attempts这一对是很有用的兜底。开了之后，CHOMP 如果在迭代上限内没把碰撞约束满足，会给初始轨迹加一个随机扰动然后重试，重试次数就是你设的那个值。random_jump_amount控制扰动的幅值。我的配置是开启恢复、重试 5 次，这个组合能把不少“差一点点就成”的情况救回来。但要注意，重试是有时间成本的，如果你的控制器对规划延迟敏感，比如要求 200 毫秒内出轨迹，那这个机制会直接让你的最坏延迟翻好几倍，这种场景下要么关掉它接受失败，要么把它作为上层重规划逻辑的一部分，而不是压在单次规划里。

use_stochastic_descent这个开关我在不同项目里试过两种设置。从命名和实现看，它是在梯度估计上引入随机性，用不完全精确的梯度来做更新，单次迭代更便宜，同时这种随机性有助于从浅层的局部极小里抖出来，代价是收敛曲线不再单调，可能出现代价先降后升再降的情况。追求极致速度的场景我开它；追求结果可复现、同一场景每次结果都要一致的场景我关掉它——关掉之后同样的输入基本能得到同样的输出，这对产线调试很重要。

3.4 轨迹初始化方式：决定你会不会掉进局部极小

trajectory_initialization_method决定 CHOMP 从什么样的初始轨迹开始优化，常见取值有quintic-spline（五次样条插值）、cubic（三次）、linear（直线）以及fillTrajectory这一类。默认的quintic-spline是起点和终点之间做五次多项式插值，形状比较自然，是大多数场景的首选。

为什么要关心这个？因为 CHOMP 是局部优化器，它只能把初始轨迹“推”到附近的一个局部最优解，推不到的地方它永远去不了。如果初始的那条直线正好从障碍物正中间穿过去，而且穿得很深，两边都有障碍，那 CHOMP 就可能把轨迹卡在障碍内部或者一侧出不来，表现为“优化跑完了但还是有碰撞”。这时候换初始化的插值方式有时候能救，因为五次样条在中段会略微不同，等于换了个起点。但更根本的解决办法是调整场景布置或者拆解目标点：让起点到终点的直线不要深穿障碍，或者把一次大跨度运动拆成两段。我知道有人想“先用 OMPL 规划一条，再拿给 CHOMP 当初值”，思路很对，但 MoveIt 暴露出来的 CHOMP 接口不支持从外部注入初始轨迹，这条路走不通，只能从初始化策略和场景设计上想办法。

3.5 参数调节的推荐顺序

参数一多就容易乱，我整理了一张速查表，按这个顺序调基本不会互相干扰：

4. 从启动到规划成功的实操记录

4.1 规划前的四项检查

每次调完配置重新启动，我都会按固定顺序过一遍这四项，能挡掉八成以上的“规划失败”。第一项，确认机器人当前关节状态和模型一致。规划请求里的起始状态用的是getCurrentState()，如果你的机器人上电后被人手动掰动过、或者标定时的零点和模型对不上，规划出来的轨迹第一步就会跳。第二项，确认robot_description和robot_description_semantic参数都在，这两个是 CHOMP 建运动学模型必需的，缺了会在初始化阶段直接报错。第三项，确认规划场景里的环境物体都已经添加到 collision world 里了，而且坐标是正确的——距离场是从 collision world 建的，场景里没东西，CHOMP 就以为自己在一个空房间里，规划出来的轨迹自然会撞桌子。第四项，确认目标状态是合法的、没有自碰撞，这个后面会专门讲，因为它伪装成 CHOMP 失败的概率极高。

4.2 在 RViz 里手动验证一轮

RViz 的 MotionPlanning 面板是最好的调试入口。切到 Planning 标签，Planner 选 Chomp，然后打开 Context 标签里的 Scene Geometry，把碰撞体都显示出来。给一个位姿目标，点 Plan，观察几件事：轨迹是不是平滑的连续曲线；轨迹和障碍物之间是不是有可见的间隙；规划时间大概是多少（面板上会显示）。我第一次配好之后规划出来的轨迹贴着桌面滑过去，看着挺近，量了一下离桌面只有不到 1 厘米，后来把collision_threshold从 0.02 提到 0.05 才拉开。

如果规划失败，别急着改参数，先把animate_path设成 true 重启一次。开启之后 CHOMP 会在迭代过程中把轨迹的演化过程画出来，你能直观看到它是怎么把轨迹一点点推出去的，也能看到它是在哪一步卡住的——是卡在某个障碍的拐角来回震荡，还是压根没动。这个可视化对理解局部极小值特别有帮助，我看过一次之后对“为什么初始轨迹不能深穿障碍”这件事再也没疑惑过。调试完记得关掉，它会拖慢规划速度。

4.3 用脚本做 A/B 对比，别靠感觉

RViz 手动点几下只能看个大概，要做定量对比还是得写脚本。下面这段 Python 直接调/plan_kinematic_path服务，可以精确控制规划器、起始状态和目标约束，并且能测出真实耗时：

#!/usr/bin/env python import time import rospy from moveit_msgs.srv import GetMotionPlan, GetMotionPlanRequest from moveit_msgs.msg import MotionPlanRequest, Constraints, JointConstraint import moveit_commander def build_request(group, planner_id, joints, allowed_time=5.0): moveit_commander.roscpp_initialize([]) robot = moveit_commander.RobotCommander() mpr = MotionPlanRequest() mpr.group_name = group mpr.planner_id = planner_id mpr.num_planning_attempts = 1 mpr.allowed_planning_time = allowed_time # 起始状态用当前机器人状态，避免模型和实物不一致 current = robot.get_current_state() mpr.start_state = current cons = Constraints() for name, value in joints.items(): jc = JointConstraint() jc.joint_name = name jc.position = value jc.tolerance_above = 0.001 jc.tolerance_below = 0.001 jc.weight = 1.0 cons.joint_constraints.append(jc) mpr.goal_constraints.append(cons) return mpr def bench(group, planner_id, joints, n=20): rospy.wait_for_service("/plan_kinematic_path") proxy = rospy.ServiceProxy("/plan_kinematic_path", GetMotionPlan) latencies, successes = [], 0 for _ in range(n): req = GetMotionPlanRequest() req.motion_plan_request = build_request(group, planner_id, joints) t0 = time.time() try: res = proxy(req) dt = time.time() - t0 ok = res.motion_plan_response.error_code.val == 1 except Exception as exc: dt = time.time() - t0 ok = False rospy.logwarn("plan call failed: %s", exc) latencies.append(dt) successes += 1 if ok else 0 time.sleep(0.1) latencies.sort() print("planner=%s success=%d/%d p50=%.3fs p95=%.3fs" % (planner_id, successes, n, latencies[len(latencies) // 2], latencies[int(len(latencies) * 0.95)])) if __name__ == "__main__": rospy.init_node("planner_bench") goal = {"panda_joint1": 0.3, "panda_joint2": -0.5, "panda_joint3": 0.2, "panda_joint4": -2.0, "panda_joint5": 0.1, "panda_joint6": 1.6, "panda_joint7": 0.8} bench("panda_arm", "Chomp", goal, 20) bench("panda_arm", "RRTConnect", goal, 20)

这个脚本有几个设计点值得说。目标约束我用的是关节约束而不是位姿约束，因为关节空间的目标对 CHOMP 是最直接的输入形式，避免了 IK 环节引入的额外不确定性；num_planning_attempts设成 1，是为了测单次规划的真实耗时，不掺入 MoveIt 内部的重试；每次请求之间 sleep 0.1 秒，防止把服务打爆。跑出来的 p50 和 p95 才有参考意义，因为 CHOMP 的耗时跟初始轨迹质量关系很大，平均值会被极端值带偏。

4.4 实测数据与执行表现

在 7 自由度机械臂、规划场景里放三到四个障碍物、目标点在工作空间中部这种典型配置下，我这边测到的量级是这样：场景首次加载后的第一次规划明显慢，因为要建距离场，通常在几百毫秒到两秒之间，具体取决于场景体素分辨率和物体数量；之后场景不变的话，后续规划稳定在几十毫秒量级，p95 大概在 100 到 200 毫秒。同场景下 RRTConnect 反而是每次都在几十到一百多毫秒，波动更大，而且路径质量参差不齐。所以 CHOMP 的优势不在单次速度，而在“场景固定、反复规划”这个模式下的稳定性和路径质量。

执行层的表现也要说一句。CHOMP 返回的轨迹本身就带速度信息，因为它的优化就是建立在时间域上的，理论上不需要再挂一遍时间参数化后处理。但我在实际测试中发现，直接把规划结果丢给控制器时，某些关节在中段会有轻微的顿挫感，尤其是从静止到运动的起步阶段。我的处理方式是检查一下轨迹的resample_dt，默认 0.1 秒，如果你的控制器周期是 1 毫秒，那中间就有一百个插值点要控制器自己补，补的方式各家不一样。稳妥做法是在控制器侧再做一次自己的插值，或者挂一个速度平滑的后处理。这个不是 CHOMP 的锅，是整个链路里时间参数化的责任划分问题，但确实会让人误以为是 CHOMP 规划得不好。

5. 常见问题与排查技巧实录

排查这件事最怕的就是没有章法。下面这张表是我这几年攒下来的，按“现象—可能原因—怎么验证—怎么解决”四列整理，遇到问题直接对号入座：

有几个坑我想单独展开说，因为它们在表里一行写不完。

第一个是“IK 失败伪装成 CHOMP 失败”。这个坑我踩过不止一次。你给的是末端位姿目标，MoveIt 会先做一次逆解得到关节目标，再交给 CHOMP。如果逆解失败，报出来的错往往是规划层面的，你会以为 CHOMP 有问题。验证方法很简单：换成一个明确的关节空间目标再试一次，如果这次成功了，那就不是 CHOMP 的锅，去查 IK 求解器配置。另外注意，CHOMP 本身只吃关节空间的目标，任何笛卡尔空间的东西都得先转成关节目标，路径约束类的需求（比如“末端必须保持竖直”）它是不处理的，这类需求得换别的方案或者在上层做检查。

第二个是自碰撞和夹持物体的处理。MoveIt 里 CHOMP 的障碍项梯度主要来自机器人与世界环境物体构建的距离场，自碰撞并不在这个梯度的直接优化目标里。这意味着它可能规划出一条自己蹭自己的轨迹，或者至少不保证最优。所以规划完我习惯自己再调一次碰撞检查，用checkSelfCollision过一遍，作为上层的安全门。夹持物体的情况更微妙：被夹住的物体在模型里属于机器人一侧的 attached body，它跟世界距离场之间的关系在不同版本里处理方式不完全一致。做抓取场景的时候，一定要在仿真里把完整的取放路径跑一遍，看手里拿着东西的时候会不会蹭到周围。我遇到过手里拿着料盒、CHOMP 规划出来的轨迹让料盒从料架边缘擦过去的情况，最后是在规划前临时往场景里加了一个代理碰撞体解决的。

第三个是延迟的隐性成本。前面提过失败恢复机制会让最坏延迟成倍增长。还有一个容易被忽略的是距离场重建。如果你的上层逻辑会在每次规划前动态往场景里添加和删除障碍物（比如从视觉检测结果里更新料框位置），那距离场可能每次都要重建，CHOMP 的速度优势就没了。实测下来，这种情况下的整体耗时可能比 RRTConnect 还长，因为采样规划器不需要建场。所以用 CHOMP 有个隐含前提：场景尽量静态。如果确实需要动态更新，建议把更新频率压到最低，或者只在检测结果变化超过一定阈值的时候才刷新场景。

第四个是可复现性。产线调试最头疼的就是“同一段代码，昨天跑得好好的，今天结果不一样”。use_stochastic_descent开启的时候，CHOMP 的迭代带随机性，结果会有细微差异。如果你的下游逻辑对轨迹的数值有依赖（比如把轨迹存下来做过对比、或者有个基于历史轨迹的预测模块），建议在调试和验证阶段把它关掉，确认功能正确之后再按需打开。这个开关本身是个性能与可复现性的权衡，没有绝对的对错。

6. 我对 CHOMP 适用边界的个人判断

用了这么久，我对 CHOMP 的态度是工具而不是信仰。它最舒服的场景特征是三个：环境基本静态、需要反复规划同一个工作空间内的动作、对轨迹平滑度有要求。典型的比如抓取台的分拣循环、打磨工位、实验室里的移液操作，这些场景下它的稳定性和路径质量能带来实实在在的收益，节拍也能算得准。它最难受的场景也很清晰：环境里的东西频繁变动、规划组自由度特别大（双臂协同这种十几个自由度的情况，度量矩阵的规模会爆炸，求解时间直接上不去）、需要笛卡尔路径约束、以及需要严格保证找到解的场景。采样规划器的概率完备性在这时候反而是优势，哪怕路径丑一点，能出解就比什么都重要。

还有一个经常被忽略的点是 STOMP。它和 CHOMP 都属于优化规划这一支，思路接近但在梯度估计和噪声利用上走了不同路线，对初始轨迹的依赖相对小一些，而且它对环境距离场的预计算依赖没有 CHOMP 那么强。如果你试了 CHOMP 觉得被静态场景这个前提卡住了，值得花半天时间把 STOMP 也配起来做个横向对比。我自己的习惯是把两个规划器都挂在同一个 YAML 里，做个脚本自动跑一批目标点，把成功率、路径长度、最小间隙、p95 延迟四个指标拉出来对比，然后再决定这个项目用哪个。凭感觉选规划器是最容易翻车的事情。

最后分享一个我觉得最实用的小技巧。配 CHOMP 的时候，先把collision_threshold设得很大，比如 0.25 米，跑一次规划。这种配置下 CHOMP 的避障行为会被放大得非常明显，你能一眼看出它的“推力”是从哪个方向来的、轨迹在哪个位置被推开。理解了这个行为之后，再把阈值调到正常值，你就有了一个明确的参照系，知道正常值下的轨迹到底算贴障碍还是算安全。我就是靠这一招，从最初的“看着挺近但不知道算不算危险”，变成了能凭 RViz 里轨迹的形态大致判断阈值是否合适的。这个方法对刚上手的人来说，比看十页文档都管用。