Skip to content
Anima Mundi Anima Mundi

TrajOpt 规划器

注意：TrajOpt 规划器目前仍是 MoveIt 的 alpha 阶段功能，源自 2019 年暑期实习生项目，尚需进一步打磨。如果你对其后续开发感兴趣，请联系 hello@picknik.ai。

TrajOpt 是一种用于运动规划问题的序列凸优化（sequential convex optimization）算法。它通过松弛、近似线性化和凸化，将非凸、非仿射的等式约束和不等式约束转化为凸约束，从而构造出目标函数。本工作基于 [schulman2013] 和原始实现完成。简单来说，序列凸规划就是构造一个凸模型并反复对其进行最小化。考虑以下非凸问题：

其中 f(x) 是由下式给出的最短路径：

基本思想是通过维护对解的估计值以及一个凸信任域（trust region）来迭代求解。两个关键步骤是：

  • 在信任域内对 f(x) 和 g(x) 进行凸近似，然后将近似后的函数转换为罚函数（penalty function）。
  • 在信任域内对 h(x) 进行仿射近似，然后通过取其绝对值，将近似后的 h(x) 转换为罚函数。

下图展示了 TrajOpt 算法的工作方式：

图中粗体斜体标注的参数是从 yaml 文件加载的。所有约束——包括避障、关节限位以及关节空间或笛卡尔空间中的目标位姿——都被转换为罚函数，从而构成最终的目标函数。最内层循环使用序列二次规划（sequential quadratic programming）作为信任域方法，在箱式信任域内计算二阶泰勒近似。

TrajOpt 的一个重要部分是避障约束的构造方式。在离散情况下，该约束本质上是符号距离（机器人连杆与自身或障碍物之间的距离）与安全值之差；在连续情况下，符号距离则是两个路点凸包与障碍物之间的距离。

注意：当前 TrajOpt 的实现仅支持关节空间中的约束。

开始之前

Section titled “开始之前”

如果尚未完成，请先完成 Getting Started 中的步骤。

你还应该完成 Visualization with MoveIt RViz Plugin 中的步骤。

前置条件

Section titled “前置条件”

要在你的机器人上使用 TrajOpt，你必须已经拥有该机器人的 MoveIt 配置包。例如，如果你有一台 Panda 机器人，它对应的配置包是 panda_moveit_config。该配置包通常使用 MoveIt Setup Assistant 创建。

注意：本教程使用 moveit/panda_moveit_config 仓库中的 panda_moveit_config，其中包含了 TrajOpt 规划器所需的配置文件。

运行演示

Section titled “运行演示”

首先，从 moveit/panda_moveit_config 仓库启动 panda_moveit_config，并将 trajopt 作为规划器传入：

Terminal window
roslaunch panda_moveit_config demo.launch  pipeline:=trajopt

然后运行来自 moveit/moveit_tutorials 的 TrajOpt 示例：

Terminal window
roslaunch moveit_tutorials trajopt_example_launch.launch

TrajOpt 的工作原理

Section titled “TrajOpt 的工作原理”

TrajOpt 中的运动规划问题由一组代价（COST）和约束（CNT）函数定义，这些函数通过 ConstructProblem 函数添加到 TrajOptProblem 中。该函数接收与问题相关的信息（ProblemInfo），其中 ProblemInfo 承载以下几类信息：

  • BasicInfo：保存优化算法的一般信息（这些参数将在下一节详细说明）：

    • n_steps：从起点到目标点的步数
    • convex_solver：要使用的凸求解器
    • use_time：设置为 false 时使用无量纲时间步长，此时 x1-x0 即为速度
    • start_fixed：设置为 true 时添加对当前关节值的约束

  • InitInfo：通过设置一个猜测轨迹矩阵来定义优化问题的初始化方式。该矩阵的行数等于时间步数，列数等于自由度。有三种不同的初始化类型：

    • STATIONARY：初始化矩阵在所有时间步上都保存当前状态的关节值。

    • JOINT_INTERPOLATED：初始化矩阵是一条在当前状态与用户为 data 成员提供的关节状态之间插值的轨迹。

    • GIVEN_TRAJ：用户为 data 成员提供完整的轨迹。

  • TermInfo：这是由 cost_infos 和 cnt_infos 成员携带的所有类型 COST 和 CNT 函数的基结构。COST 函数是需要最小化的目标，CNT 函数是必须满足的约束。当前实现包含 JointPoseTermInfo、JointVelTermInfo（硬编码）和 CartPoseTermInfo（部分实现）。成员 term_type 决定了所添加项的类型，可以是 TT_COST 或 TT_CNT，分别表示代价项或约束项。此外，还可以为该成员选择 TT_USE_TIME，以启用时间参数化。在这种情况下，BasicInfo 的 use_time 也应设置为 true。这些项的其他参数从 trajopt_planning.yaml 文件中加载，以下列表描述了各参数的含义：

    • coeffs：关节的权重系数

    • targets：约束处关节的目标值

    • upper_tols：约束处关节值的上限容差

    • lower_tols：约束处关节值的下限容差

    • first_step：应用该项的第一个步

    • last_step：应用该项的最后一个步

此外，算法还需要特定于 BasicTrustRegionSQP 的参数，这些参数在 yaml 文件的 trajopt_param 下定义。

以下流程图说明了问题的构造过程：

如何使用 TrajOpt

Section titled “如何使用 TrajOpt”

本节描述如何将 MoveIt 中的请求转换为 TrajOpt 约束。第一个要点是：在 TrajOpt 中，起始状态可以与当前状态不同。本质上，任何我们希望机器人经过的关节状态都只是一个需要添加到目标函数中的约束。这个关节状态可以来自 MoveIt MotionPlanRequest 中的起始状态，也可以来自规划场景中的当前状态。MoveIt 中为 TrajOpt 设计的架构会为 MotionPlanRequest 的 goal_constraints 和 start_state 中的所有约束创建 JointPoseTermInfo。另一方面，如果用户希望机器人从当前状态开始，机器人的当前状态也可以作为 JointPoseTermInfo 约束添加。下图是一个包含两个目标约束的示例：

start_fixed 是决定当前状态与起始状态之间关系的参数。如果设置为 true，算法会添加一个约束，强制轨迹从当前状态开始；如果为 false，轨迹将从请求的起始状态开始。下面两个动图演示了上述示例的实际效果——注意左图（从当前状态开始）与右图（从请求的起始状态开始）之间的区别：

一个典型用例是执行打磨（sanding）等工序：关键部分是实际的加工路径，而不是如何到达加工路径的起点。因此，我们可以先规划加工路径，让起点保持自由以获得最优路径，然后再从当前位置（起点固定）规划到加工路径的起点。

另外两个重要参数是 first_timestep 和 last_timestep，它们指定我们希望某个特定 TermInfo 生效的步范围。对于本节中的示例，我们有一个来自请求起始状态的约束、两个目标约束以及一个当前状态约束。如果步数（n_steps）设置为 20，那么最后一个目标约束生效的步索引是 19。对于由 start_fixed=true 创建的当前状态约束，用户无需设置其参数，其时间步默认为 0。因此，如果将所有约束生效的步标注出来，上图会变成下面的样子：

上述约束的 term_type 应设置为 TT_CNT，因为我们希望机器人精确地经过所有这些状态。

请注意，如果你想通过 RViz 中的 MotionPlanning Display 使用 TrajOpt，应该将 start_fixed 设置为 false，因为机器人的当前状态是通过请求的起始状态发送的——也就是说当前状态和起始状态相同，因此不需要为同一状态添加两个相同的约束。在这种情况下，不要忘记将起始状态约束的 first_timestep 和 last_timestep 设置为零。这样，上面的示例就变为以下形式：

对于任何新的关节约束，相应的信息都应添加到 yaml 文件中。当前实现只能将关节空间约束作为 JointPoseTermInfo 处理。其他一些待添加的约束类型包括 JointAccTermInfo、JointJerkTermInfo、CartPoseTermInfo、TotalTimeTermInfo 和 CollisionTermInfo。

参考资料

Section titled “参考资料”
  • [schulman2013] Schulman, John & Ho, Jonathan & Lee, Alex & Awwal, Ibrahim & Bradlow, Henry & Abbeel, Pieter. (2013). Finding Locally Optimal, Collision-Free Trajectories with Sequential Convex Optimization. 10.15607/RSS.2013.IX.031.