STOMP 运动规划器

Stochastic Trajectory Optimization for Motion Planning（STOMP，随机轨迹优化运动规划）是一个概率优化框架（Kalakrishnan et al. 2011）。STOMP 能够在合理的规划时间内生成平滑、无碰撞的路径。该方法的核心思想是：通过生成随机噪声轨迹来探索初始参考轨迹（可能不可行）周围的空间，从而组合出一条代价更低的新轨迹。轨迹代价由针对具体问题设计的代价函数计算，这些函数会根据碰撞、路径点约束违反、平滑性和控制属性等计算惩罚。优化过程迭代执行，因此参考轨迹会不断得到改进而不会引入突变。

STOMP 的优化算法不需要梯度信息，因此可以包含不支持导数计算的代价函数（例如与约束和电机转矩对应的代价）。STOMP 的主要优势在于它能够纳入额外的目标函数，如转矩限制、能量和工具约束。目前，通过 MoveIt 的规划插件 API 传递自定义代价函数的功能正在开发中。

入门指南

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

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

前提条件

为你的 ROS 2 发行版构建最新版本的 MoveIt main。STOMP 库作为独立 ROS 包为受支持的 ROS 2 发行版提供，应通过 rosdep 安装。
要在你的机器人上使用 STOMP，最好从一个功能完备的 MoveIt 配置包开始。为了测试，你也可以像本教程描述的那样，从 moveit/panda_moveit_config 开始使用 Panda 机器人。

在你的机器人上使用 STOMP

请注意： 如果你使用 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

配置 MoveIt 加载 STOMP 规划管线：在你的 MoveItConfiguration launch 语句中，在 “ompl” 和其他规划器旁边添加 “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：在任何一次迭代中，纳入优化考虑的新旧结转 rollout 的总数上限。
exponentiated_cost_sensitivity：用于调整概率计算中指数化代价的因子。因子越大，STOMP 优化和收敛越快，但代价是稳健性降低、结果可重复性变差。
control_cost_weight：在总代价计算中应用的控制代价因子。STOMP 试图最小化假设等时间步长下的加速度平方和，以平滑生成的轨迹。
delta_t：连续点之间假设的时间间隔。
path_marker_topic：RViz 为可选的路径可视化所订阅的话题名称。如果未设置，路径将不会被可视化。

提供的默认参数应该能让 STOMP 在大多数环境中表现良好。不过在更复杂的环境下，你可以增加时间步数或 rollout 数量以获得更好的效果。这可能还需要相应增加允许 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 在存在障碍物的情况下也能生成无碰撞的平滑路径。