OMPL 接口

Open Motion Planning Library（开放运动规划库，OMPL）是一个强大的算法集合，包含多种最先进的基于采样的运动规划算法，也是 MoveIt 的默认规划器。更多信息请参见项目主页。

OMPL 设置

这里我们回顾 OMPL 的重要配置项。这些配置通常可以在机器人 moveit_config 包中的 ompl_planning.yaml 文件里找到。

Longest Valid Segment Fraction

longest_valid_segment_fraction 定义了用于碰撞检测的机器人运动离散化程度，对基于 OMPL 的求解性能和可靠性有很大影响。在此上下文中，“运动”（motion）可以看作是图中两个节点之间的一条边，其中节点是轨迹上的路点。OMPL 中默认的运动碰撞检测器只是把这条边离散化为若干子状态进行碰撞检测。OMPL/MoveIt 目前还不支持连续碰撞检测，不过这是当前正在讨论的领域。

具体来说，longest_valid_segment_fraction 是机器人状态空间的一个比例：在机器人当前不处于碰撞状态的前提下，我们假设机器人可以在此距离内运动而保持无碰撞。例如，如果 longest_valid_segment_fraction = 0.01，那么我们就假设：若两个节点之间的边小于状态空间的 1/100，则无需显式检查该边上的任何子状态，只需检查它所连接的两个节点即可。

除了 ompl_planning.yaml 文件中的 longest_valid_segment_fraction 参数之外，还有一个 maximum_waypoint_distance 参数，它位于 dynamic reconfigure 文件中。maximum_waypoint_distance 同样定义了用于碰撞检测的机器人运动离散化程度，但它使用的是绝对数值而非状态空间的比例。例如，如果 maximum_waypoint_distance = 0.1，那么当一条边在状态空间距离上短于 0.1 时，我们就不显式检查该边上的任何子状态。

如果同时设置了 longest_valid_segment_fraction 和 maximum_waypoint_distance，则会选择离散化更保守（即在给定边上生成最多待碰撞检测状态）的那个参数。

将 longest_valid_segment_fraction（或 maximum_waypoint_distance）设置得过低，碰撞检测和运动规划会非常缓慢；设置得过高，则会漏掉小物体或狭窄物体附近的碰撞。此外，过高的碰撞检测分辨率会导致路径平滑器输出难以理解的运动，因为平滑器能够“捕获”无效路径，并尝试通过在路径周围采样来修复，但修复并不完美。

关于该参数对 MoveIt 教程中两个示例的影响的快速分析，记录在此处。

Projection Evaluator

projection_evaluator 可以接受一个关节或连杆列表，以近似估计配置空间的覆盖情况。此设置被 KPIECE、BKPIECE、LBKPIECE 和 PDST 等规划器使用。更多信息请阅读相应的论文。

强制在关节空间中规划

根据规划问题不同，MoveIt 会在 joint space（关节空间）和 cartesian space（笛卡尔空间）之间选择问题的表示方式。 设置组参数 enforce_joint_model_state_space 会强制所有规划都使用 joint space（关节空间）。

默认情况下，带有方向路径约束的规划请求会在 cartesian space（笛卡尔空间）中采样，这样调用 IK 即可充当生成式采样器。

强制使用 joint space（关节空间）时，规划过程将使用拒绝采样来寻找有效请求。 请注意，这可能会显著增加规划时间。

其他设置

根据你使用的规划器不同，还有其他可用于调参或参数扫描的设置。这些设置的默认值由 MoveIt Setup Assistant 自动生成，并列在 ompl_planning.yaml 文件中——建议你自行调整它们。

速度与最优性之间的权衡

OMPL 中的许多规划器（包括默认规划器）更看重找到解路径的速度，而非路径质量。一条可行路径会在后处理阶段被平滑和缩短，以获得更接近最优的路径。然而，由于 OMPL 中的算法是概率性的，因此无法保证能找到全局最优解，也无法保证每次都能找到相同的解。其他库，例如基于搜索的规划库（Search Based Planning Library，SBPL），则提供确定性结果：在给定相同的环境、起点和终点的情况下，你总能得到相同的路径。SBPL 基于 A*，因此在你选择的搜索分辨率内能得到最优结果。不过，SBPL 也有缺点，例如难以在合适的分辨率下定义状态空间栅格（比如，如何为关节角度或末端执行器位姿定义良好的离散化？），以及需要调优专门的启发式函数。

OMPL 中有几个规划器可以给出理论上的最优性保证，但通常只是渐近意义上的：它们会收敛到最优解，但收敛可能很慢。这些规划器使用的优化目标通常是路径长度最小化，但也可以使用其他优化目标。

OMPL 优化目标

OMPL 规划库中的多个规划器能够针对指定的优化目标进行优化。本教程描述了配置这些优化目标所需的步骤。当前在 MoveIt 中可用的渐近（近）最优规划器有：

• RRT*
• PRM*
• LazyPRM*
• BFMT
• FMT
• Lower Bound Tree RRT (LBTRRT)
• SPARS
• SPARS2
• Transition-based RRT (T-RRT)

OMPL 还提供了一种名为 AnytimePathShortening 的元优化算法，它会反复并行运行多个规划器，并穿插使用路径缩短和路径杂交这两种对解路径进行局部优化的技术。虽然未被证明最优，但在实践中往往是获得近最优解路径的有效策略。

OMPL 中还有其他最优规划器，但尚未在 MoveIt 中开放：

• RRT#
• RRTX
• Informed RRT*
• Batch Informed Trees (BIT*)
• Sparse Stable RRT
• CForest

并且可以使用以下优化目标：

• PathLengthOptimizationObjective (Default)
• MechanicalWorkOptimizationObjective
• MaximizeMinClearanceObjective
• StateCostIntegralObjective
• MinimaxObjective

这些优化目标的配置可以在 ompl_planning.yaml 中完成：添加一个名为 optimization_objective 的参数作为配置参数，将其值设置为所选优化目标的名称。例如，要配置 RRTstar 使用 MaximizeMinClearanceObjective，ompl_planning.yaml 中的规划器条目如下所示：

    RRTstarkConfigDefault:
        type: geometric::RRTstar
        optimization_objective: MaximizeMinClearanceObjective
        range: 0.0
        goal_bias: 0.05
        delay_collision_checking: 1

其他优化目标可以通过编程方式定义。关于 OMPL 最优规划器的更多信息，请参阅 OMPL - Optimal Planning 文档。

OMPL 规划器终止条件

OMPL 中的规划器通常在超过给定时间限制后终止。不过，也可以在 ompl_planning.yaml 中通过 termination_condition 参数为每个规划器配置指定额外的终止条件。可选值有：

• Iteration[num]：在 num 次迭代后终止。此处 num 应替换为正整数。
• CostConvergence[solutionsWindow,epsilon]：在代价（由优化目标指定）收敛后终止。参数 solutionsWindow 指定用于判断规划器是否已收敛的最少解数量。参数 epsilon 是判定收敛的阈值，应是一个接近 0 的正数。如果在找到新的更优解之后，累计移动平均值的变化不超过 epsilon 的相对比例，则认为已达到收敛。此终止条件仅适用于 OMPL 1.5.0 及更新版本。
• ExactSolution：一旦找到精确解或发生超时即终止。这会修改 anytime/优化型规划器的行为，使其在发现第一个可行解时即终止。

在所有情况下，规划器都会在满足用户指定的终止条件或达到 MotionPlanRequest 消息中的 allowed_planning_time 时终止，以先发生者为准。

例如，要指定 RRTstar 在收敛时终止，可以使用以下设置：

    RRTstarkConfigDefault:
        type: geometric::RRTstar
        termination_condition: CostConvergence[10,.1]
        range: 0.0
        goal_bias: 0.05
        delay_collision_checking: 1

请注意，这里没有指定优化目标，因此将使用默认的 PathLengthOptimizationObjective。

后处理平滑

需要注意的是，平滑对减少迂回路径的帮助有限。另外请注意，这里只讨论基于几何（运动学）的平滑。速度、加速度和加加速度的平滑在别处处理，参见时间参数化。

你可以通过增加规划时间来调整 MoveIt 在平滑上花费的时间。在找到初始规划之后、allowed_planning_time 用尽之前的任何剩余时间都将用于平滑。MoveIt 还会进行路径杂交，即取 N 次不同规划运行的最佳部分拼接在一起。因此，num_planning_attempts 也会影响质量。

虽然目前还没有在 MoveIt 的顶层开放（TODO），但通过将 model_based_planning_context.cpp 中的简化时长设置为 0（无限制），可以实现更多的平滑。这将启用 OMPL 的 simplifyMax() 函数。

除了 OMPL 内部的平滑器之外，最近也有使用 STOMP/CHOMP 进行后处理的工作。参见这篇博客文章。

持久化路图

默认情况下，规划算法每次收到运动规划请求都会从头开始。然而，对于某些会构建环境路图的规划器来说，如果规划场景基本保持静态，复用之前运动规划请求生成的路图可能会更有利。考虑以下规划配置：

    PersistentLazyPRMstar: # use this with a representative environment to create a roadmap
        type: geometric::LazyPRMstar
        multi_query_planning_enabled: 1
        store_planner_data: 1
        load_planner_data: 0
        planner_data_path: /tmp/roadmap.graph
    PersistentLazyPRM: # use this to load a previously created roadmap
        type: geometric::LazyPRM
        multi_query_planning_enabled: 1
        store_planner_data: 0
        load_planner_data: 1
        planner_data_path: /tmp/roadmap.graph
    SemiPersistentLazyPRMstar: # reuses roadmap during lifetime of node but doesn't save/load roadmap to/from disk
        type: geometric::LazyPRMstar
        multi_query_planning_enabled: 1
        store_planner_data: 0
        load_planner_data: 0
    SemiPersistentLazyPRM: # reuses roadmap during lifetime of node but doesn't save/load roadmap to/from disk
        type: geometric::LazyPRM
        multi_query_planning_enabled: 1
        store_planner_data: 0
        load_planner_data: 0

第一个规划器配置 PersistentLazyPRMstar 将使用 LazyPRM*，在每次运动规划请求时持续扩展一张连接采样机器人配置的渐近最优路径路图。在规划器实例销毁时，它会将路图保存到磁盘。PersistentLazyPRM 配置类似，不同之处在于它会从磁盘加载路图，但在销毁时不会保存它。SemiPersistent（半持久）规划器配置不会加载或保存路图，但会在每次运动规划请求时持续扩展路图（而不是默认的每次规划前清空路图的行为）。支持持久规划特性的四种规划器是 PRM、PRM*、LazyPRM 和 LazyPRM*。它们之间的关键区别在于，lazy 变体在搜索路图中的有效路径时会按需重新验证节点和边的有效性；非 lazy 变体则不会检查路图对当前环境是否仍然有效。换句话说，静态环境使用非 lazy 变体，小变化环境使用 lazy 变体，而环境可能发生显著变化时使用非持久规划器。

最初使用 PersistentLazyPRMstar 或 PersistentPRMstar 来创建并存储路图是合理的，因为它们是渐近最优规划器。路图可用后，后续的规划尝试通常使用 PersistentLazyPRM 或 PersistentPRM。它们通常比 star 变体规划更快，同时从缓存的路图开始规划意味着解仍接近最优。不过，这个顺序并不是强制性的。

注意：保存和加载路图功能仅适用于 OMPL 1.5.0 及更新版本。