STOMP 运动规划器

运动规划的随机轨迹优化 (STOMP) 是一个概率优化框架（Kalakrishnan 等人 2011）。 STOMP 在合理的规划时间内生成平滑、行为良好的无碰撞路径。该方法依赖于生成随机噪声轨迹来探索初始（可能不可行）参考轨迹周围的空间，以生成具有较低成本的新组合轨迹。轨迹成本由问题特定的成本函数计算，该函数计算碰撞、航点约束违规、平滑度和控制属性的航点成本惩罚。优化过程以迭代方式运行，因此参考轨迹不断改进而不会引入跳跃。

STOMP 的优化算法不需要梯度信息，甚至允许包含不支持计算导数的成本函数（例如，对应于约束和电机扭矩的成本）。 STOMP 的主要优点是它能够合并其他目标函数，例如扭矩限制、能量和工具约束。目前正在研究通过 MoveIt 的规划插件 API 传递自定义成本函数的支持。

入门

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

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

先决条件

为您的 ROS 2 发行版最近构建的 MoveIt main。 STOMP 库作为受支持的 ROS 2 发行版的单独 ROS 包提供，应通过“rosdep”安装。
要将 STOMP 与机器人一起使用，最好从机器人的功能性 MoveIt 配置包开始。为了进行测试，您还可以从“moveit/panda_moveit_config <https://github.com/moveit/panda_moveit_config>”中的 Panda 机器人开始，如本教程中所述。

将 STOMP 与机器人一起使用

**注意：**如果您使用“moveit/panda_moveit_config <https://github.com/moveit/panda_moveit_config>”存储库中的“panda_moveit_config”来遵循此演示，则这些步骤已经为您完成，您可以直接启动演示以在 RViZ 中测试 STOMP。

#。只需将 stomp_planning.yaml 配置文件添加到 MoveIt 配置包的 config 目录中即可。它包含插件标识符、规划管道适配器列表和 STOMP 规划参数。配置文件应如下例所示：:

planning_plugins:
  - stomp_moveit/StompPlanner
request_adapters:
  - default_planning_request_adapters/ResolveConstraintFrames
  - default_planning_request_adapters/ValidateWorkspaceBounds
  - default_planning_request_adapters/CheckStartStateBounds
  - default_planning_request_adapters/CheckStartStateCollision
response_adapters:
  - default_planning_response_adapters/AddTimeOptimalParameterization
  - default_planning_response_adapters/ValidateSolution
  - default_planning_response_adapters/DisplayMotionPath

stomp_moveit:
  num_timesteps: 60
  num_iterations: 40
  num_iterations_after_valid: 0
  num_rollouts: 30
  max_rollouts: 30
  exponentiated_cost_sensitivity: 0.5
  control_cost_weight: 0.1
  delta_t: 0.1

通过在 MoveItConfiguration 启动语句中将“stomp”添加到“ompl”和其他规划器旁边，配置 MoveIt 以加载 STOMP 规划管道。您可以在 Panda 配置的 demo.launch.py 中找到此示例。

运行演示

如果您拥有来自 moveit/moveit_resources 存储库的 panda_moveit_config，您应该能够简单地启动演示设置并开始在 RViZ 中使用 STOMP 进行规划

ros2 launch moveit_resources_panda_moveit_config demo.launch.py

STOMP 参数

STOMP 的参数可使用 stomp_planning.yaml 进行配置。所有参数均由 stomp_moveit.yaml 定义，并用作 generate_parameter_library 的输入。在该文件中，您还可以找到更多信息，例如默认值和允许范围：

优化参数：

num_timesteps：用于轨迹的时间步数 - 此数字直接转换为用于轨迹初始化、规划期间和解决方案输出的航点数量。
num_iterations：这是规划器在优化过程中可以进行的总迭代次数，以找到良好的解决方案。
num_iterations_after_valid：规划器在找到已经有效的解决方案后继续优化的迭代次数。
num_rollouts：这是每次迭代生成的噪声轨迹数量。
max_rollouts：在任何迭代中考虑用于优化的新旧结转推出的最大组合数量。
*exponentiated_cost_sensitivity：*用于调整用于概率计算的指数成本的因子。更高的因子使 STOMP 优化和收敛速度更快，但缺点是稳定性降低，结果可重复性降低。
control_cost_weight：这是在总成本计算中应用的控制成本因子。STOMP 试图最小化假设相等航路点时间步长所需的加速度平方和，以平滑最终轨迹。
delta_t：假设连续点之间的时间变化。
path_marker_topic：RViZ 订阅的主题名称，用于可选的路径可视化。如果未设置，则不会可视化路径。

提供的默认参数应该可以让 STOMP 在大多数环境中表现良好。但是，您可以增加时间步长数或推出次数，以使 STOMP 在更复杂的环境下表现良好。这可能需要增加 STOMP 终止的允许规划时间。

STOMP、CHOMP 和 OMPL 获得的计划之间的差异

在本节中，将区分从 STOMP、CHOMP 和 OMPL 获得的路径。一些 MoveIt 规划器往往会产生不稳定的轨迹，并可能引入不必要的机器人运动。通常需要后处理平滑步骤。相比之下，STOMP 往往会在短时间内产生平滑、表现良好的运动计划，因此可能不需要其他一些运动规划器所要求的后处理平滑步骤。

CHOMP 是一种优化规划器，它基于协变和函数梯度方法优化给定的初始简单轨迹。

OMPL 是一个基于采样的运动规划算法的开源库，主要依赖于随机采样和图形搜索。基于采样的算法在概率上是完整的：如果存在解决方案，最终会找到解决方案，但是无法报告不存在解决方案。这些算法非常高效，通常可以快速找到解决方案。

以下是比较这些不同方法的规划器质量的简要概述：

局部最小值处理：由于 STOMP 具有随机性，因此可以避免局部最小值。然而，CHOMP 容易陷入局部最小值，并且经常陷入局部最小值，从而无法找到最佳解决方案。根据 STOMP 和 CHOMP 论文，STOMP 在大多数情况下表现更好。
规划时间：STOMP 和 CHOMP 的规划时间相当，尽管 CHOMP 需要比 STOMP 更多的迭代才能取得成功。这主要是因为 STOMP 的每次迭代都需要多次轨迹成本评估，但可以比 CHOMP 梯度更新规则更稳定地迈出更大的步伐。OMPL 算法（至少是那些不优化解决方案的算法）通常更快，在困难的规划场景中甚至会花费更少的时间，但可能会损害路径长度或平滑度等质量。
参数调整：CHOMP 通常需要比 STOMP 进行额外的参数调整才能获得成功的解决方案。

OMPL 不需要进行大量参数调整；在大多数情况下，默认参数效果很好。

障碍物处理：对于包含障碍物的场景，由于 STOMP 具有随机性，因此通常能够成功避开障碍物。

然而，CHOMP 通过在机器人动态量（如加速度、速度）的成本函数中添加一些噪声（ridge_factor）来生成不喜欢平滑轨迹的路径。OMPL 还会在存在障碍物的情况下生成无碰撞的平滑路径。