使用 CHOMP 规划器

协变汉密尔顿优化运动规划 (CHOMP) 是一种基于梯度的轨迹优化程序，可使许多日常运动规划问题变得简单且易于训练 (Ratliff 等人，2009c)。虽然大多数高维运动规划器将轨迹生成分为不同的规划和优化阶段，但该算法利用协变梯度和函数梯度方法进行优化阶段，设计完全基于轨迹优化的运动规划算法。给定一个不可行的简单轨迹，CHOMP 会对周围环境做出反应，快速将轨迹拉出碰撞，同时优化关节速度和加速度等动态量。它快速收敛到可以在机器人上有效执行的平滑无碰撞轨迹。更多信息

入门

如果您还没有这样做，请确保您已经完成了入门中的步骤。

您还应该已经完成了使用 MoveIt RViz 插件进行可视化中的步骤

先决条件

要将 CHOMP 与您的机器人一起使用，您必须拥有适用于您的机器人的 MoveIt 配置包。例如，如果您有 Panda 机器人，它被称为 panda_moveit_config，可以在此处找到。这些通常使用 MoveIt Setup Assistant 进行配置。

将 CHOMP 与机器人配合使用

**注意：**如果您计划使用 moveit/moveit_resources 存储库中的 panda_moveit_config 包，这些步骤已经为您完成，您可以跳过此部分。否则，要为您的机器人添加配置，您必须：

在您的 MoveIt 配置包的启动目录中创建一个 chomp_demo.launch.py 文件。

#. 修改 chomp_demo.launch.py 中对 Panda 的所有引用以指向您的自定义配置 #.确保您已在 MoveIt 配置包的配置目录中包含 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。

向场景添加障碍物

要向场景添加障碍物，我们可以使用 this node 来创建带有障碍物的场景。

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

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

在第二个 shell 中，运行命令：:

ros2 run moveit2_tutorials collision_scene_example

接下来，在 RViz 中，在 Context 选项卡下的 MotionPlanning 面板中选择 CHOMP。通过使用标记移动末端执行器来设置所需的起始和目标状态，然后单击 MotionPlanning 面板中 Planning 选项卡下的 Plan 按钮开始规划。规划器现在将尝试在给定的起始和结束位置之间找到可行的解决方案。

修改 CHOMP 的参数

CHOMP 有一些与之相关的优化参数。这些参数可以根据您正在使用的特定环境/机器人进行修改，并且通常存在于您正在使用的机器人的配置文件夹中的 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：与速度、加速度和急动度的成本相关的变量。
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 将调整某些参数以尝试找到解决方案，当使用“chomp_planning.yaml”文件中指定的默认参数不存在解决方案时。
max_recovery_attempts：在第一次尝试使用默认参数失败后，CHOMP 使用一组不同参数运行的最大次数。
trajectory_initializaiton_method：为 CHOMP 提供的轨迹初始化类型，可以是“五次样条”、“线性”、“三次”或“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 并为 OMPL 提供后端以处理机器人问题。

CHOMP：虽然大多数高维运动规划器将轨迹生成分为不同的规划和优化阶段，但 CHOMP 利用协变梯度和函数梯度方法进行优化阶段，设计完全基于轨迹优化的运动规划算法。给定一个不可行的简单轨迹，CHOMP 会对周围环境做出反应，快速将轨迹拉出碰撞，同时优化动态量，例如关节速度和加速度。它快速收敛到可以在机器人上有效执行的平滑、无碰撞轨迹。协变更新规则确保 CHOMP 快速收敛到局部最优轨迹。

对于包含障碍物的场景，CHOMP 通常会通过在机器人动态量（如加速度、速度）的成本函数中添加一些噪声（ridge_factor）来生成不喜欢平滑轨迹的路径。 CHOMP 在大多数情况下能够避开障碍物，但如果由于对轨迹的初始猜测错误而陷入局部最小值，它可能会失败。OMPL 可用于为 CHOMP 生成无碰撞种子轨迹以缓解此问题。