使用 CHOMP 规划器

Covariant Hamiltonian Optimization for Motion Planning（CHOMP，协变哈密顿量运动规划优化）是一种基于梯度的轨迹优化方法，它让许多日常运动规划问题既简单又可训练（Ratliff et al., 2009c）。虽然大多数高维运动规划器将轨迹生成分为独立的规划与优化两个阶段，但 CHOMP 利用协变梯度（covariant gradient）和函数梯度（functional gradient）方法处理优化阶段，从而设计出一种完全基于轨迹优化的运动规划算法。给定一条不可行的初始轨迹，CHOMP 会响应周围环境，快速将轨迹拉出碰撞区域，同时优化关节速度、加速度等动力学量。它能快速收敛到一条平滑、无碰撞、可在机器人上高效执行的轨迹。更多信息

入门指南

如果你还没有完成入门指南中的步骤，请先完成。

你还应该完成 RViz 快速入门中的步骤。

前提条件

要在你的机器人上使用 CHOMP，你必须拥有针对该机器人的 MoveIt 配置包。例如，如果你使用的是 Panda 机器人，它被称为 panda_moveit_config，可以在这里找到。这些配置通常使用 MoveIt Setup Assistant 进行配置。

在你的机器人上使用 CHOMP

请注意： 如果你打算使用 moveit/moveit_resources 仓库中的 panda_moveit_config 包，这些步骤已经为你完成，可以跳过本节。否则，要为你的机器人添加配置，你需要：

在你的 MoveIt 配置包的 launch 目录中创建一个 chomp_demo.launch.py 文件。
将 chomp_demo.launch.py 中所有对 Panda 的引用改为指向你的自定义配置。
确保你的 MoveIt 配置包的 config 目录中包含了 chomp_planning.yaml 文件。
用你喜欢的编辑器打开 chomp_planning.yaml，将 animate_endeffector_segment: "panda_rightfinger" 改为适合你机器人的连杆名称。你可以根据需要随意修改其他参数。

运行演示

如果你同时拥有 moveit/moveit_resources 仓库中的 panda_moveit_config 和 moveit2_tutorials，你可以使用以下命令运行演示：

ros2 launch moveit2_tutorials chomp_demo.launch.py rviz_tutorial:=True

请注意： 为了方便起见，我们提供了一个可直接使用的 RViz 配置。将 rviz_tutorial 设置为 False 或直接省略该参数，即可按照你的个人偏好设置 RViz。

向场景中添加障碍物

要向场景中添加障碍物，我们可以使用此节点来创建带有障碍物的场景。

要运行带障碍物的 CHOMP 规划器，请打开第二个终端。在第一个终端中启动 RViz（如果你已关闭上一步的终端），并等待所有内容加载完成：

ros2 launch moveit2_tutorials chomp_demo.launch.py rviz_tutorial:=True

在第二个终端中，运行以下命令：

ros2 run moveit2_tutorials collision_scene_example

接下来，在 RViz 的 MotionPlanning 面板中的 Context 选项卡下选择 CHOMP。使用交互式标记移动末端执行器来设置所需的起始和目标状态，然后在 MotionPlanning 面板的 Planning 选项卡下点击 Plan 按钮开始规划。规划器将尝试在给定的起始位置和目标位置之间找到一条可行的路径。

修改 CHOMP 的参数

CHOMP 有一些与之关联的优化参数。这些参数可以根据你正在使用的环境和机器人进行修改，通常位于你所使用机器人的 config 文件夹中的 chomp_planning.yaml 文件里。如果你的机器人没有这个文件，可以自行创建并设置参数值。以下内容有助于你了解 chomp_planning.yaml 中各参数的用途：

planning_time_limit：优化器在终止前寻找解决方案的最大时间。
max_iterations：规划器在优化过程中寻找良好解决方案的最大迭代次数。
max_iterations_after_collision_free：找到无碰撞路径后继续执行的迭代次数上限。
smoothness_cost_weight：CHOMP 最终优化的代价函数中平滑性的权重。
obstacle_cost_weight：CHOMP 优化代价函数中障碍物所占的权重。例如，0.0 表示忽略障碍物，1.0 表示硬约束。
learning_rate：优化器在降低总代价时用于寻找局部/全局最小值的步长。
smoothness_cost_velocity, smoothness_cost_acceleration, smoothness_cost_jerk：与速度、加速度和加加速度（jerk）代价相关的变量。
ridge_factor：在目标函数中添加到总二次代价矩阵对角线上的噪声。添加少量噪声（例如 0.001）允许 CHOMP 以牺牲轨迹平滑性为代价来避开障碍物。
use_pseudo_inverse：为 true 时启用伪逆计算。
pseudo_inverse_ridge_factor：如果启用伪逆，则用于设置岭因子。
joint_update_limit：机器人关节的更新限制。
collision_clearance：避免碰撞所需与障碍物保持的最小距离。
collision_threshold：为避免碰撞必须维持的代价阈值。
use_stochastic_descent：为 true 时，在优化代价时使用随机下降。随机下降使用轨迹中的随机采样点，而非所有轨迹点。这种方式速度更快且保证收敛，但在最坏情况下可能需要更多迭代。
enable_failure_recovery：为 true 时，如果使用 chomp_planning.yaml 文件中指定的默认参数无法找到解决方案，CHOMP 将调整某些参数以再次尝试。
max_recovery_attempts：在默认参数首次尝试失败后，CHOMP 使用调整后参数集运行的最大次数。
trajectory_initializaiton_method：提供给 CHOMP 的轨迹初始化类型，可以是 quintic-spline、linear、cubic 或 fillTrajectory。前三个选项指在起始状态和目标状态之间进行轨迹初始化时使用的插值方法。fillTrajectory 则提供了一种使用现有运动规划器（如 OMPL）计算出的路径来初始化轨迹的方式。

为 CHOMP 选择参数需要基于规划环境积累一定的经验。例如，CHOMP 的默认参数在无障碍物的环境中效果很好；然而，在障碍物较多的环境中，默认参数很可能会导致 CHOMP 陷入局部最小值。通过调整参数，我们可以提高 CHOMP 生成的规划质量。

一些未使用或被注释掉的参数包括 hmc_stochasticity、hmc_annealing_factor、hmc_discretization、use_hamiltonian_montecarlo、animate_endeffector、animate_endeffector_segment、animate_path、random_jump_amount、add_randomness。

CHOMP 与 OMPL 获得的规划之间的区别

优化型规划器优化的代价函数有时可能导致出人意料的结果：穿过一个薄障碍物可能比绕开所有碰撞的长而曲折的轨迹代价更低。在本节中，我们将区分从 CHOMP 获得的路径与从 OMPL 获得的路径。

OMPL 是一个用于基于采样/随机运动规划算法的开源库。基于采样的算法是概率完备的：如果存在解决方案，最终会找到它，但无法报告解决方案不存在。这些算法高效且通常能快速找到解决方案。OMPL 不包含任何与碰撞检测或可视化相关的代码，因为 OMPL 的设计者不想将其绑定到特定的碰撞检测器或可视化前端。该库的设计使其可以轻松集成到提供这些附加组件的系统中。MoveIt 直接与 OMPL 集成，并使用 OMPL 的运动规划器作为其默认规划器集合。OMPL 中的规划器是抽象的；也就是说，OMPL 没有机器人的概念。相反，MoveIt 负责配置 OMPL 并为其提供后端，使其能够处理机器人学中的问题。

CHOMP：虽然大多数高维运动规划器将轨迹生成分为独立的规划与优化两个阶段，但 CHOMP 利用协变梯度和函数梯度方法处理优化阶段，从而设计出一种完全基于轨迹优化的运动规划算法。给定一条不可行的初始轨迹，CHOMP 会响应周围环境，快速将轨迹拉出碰撞区域，同时优化关节速度、加速度等动力学量。它能快速收敛到一条平滑、无碰撞、可在机器人上高效执行的轨迹。协变更新规则确保 CHOMP 快速收敛到局部最优轨迹。

对于包含障碍物的场景，CHOMP 通常会通过在机器人动力学量（如加速度、速度）的代价函数中添加一些噪声（ridge_factor）来生成不偏好平滑轨迹的路径。CHOMP 在大多数情况下都能避开障碍物，但如果由于初始轨迹猜测不佳而陷入局部最小值，它可能会失败。可以使用 OMPL 生成无碰撞的种子轨迹提供给 CHOMP，以缓解这个问题。