MoveIt!运动规划库OMPL和路径规划算法

运动规划（Motion Planning）：要让一个机器人实现运动规划，需要先将机器人抽象到构形空间（C-Space）。MoveIt就可以帮大家把这些工作给做了，只需提供机器人URDF模型，就可以调用几大运动规划库的规划算法（如OMPL，SBPL，CHMOP），自动生成机器人运动轨迹。之前跟着教程走的时候，很多内部结构不是很清晰，它就像是一个黑匣子，只管启动launch，就会返回机器人所需的运动路径，置于它使用的到底是什么运动规划库，什么路径规划算法，这些细节都没有关注过。

运动规划器 （motion planner）：MoveIt! 通过插件机制（plugin interface）与运动规划器（motion planner）进行交互，可以使用多个库的不同运动规划器，使得 MoveIt! 扩展性更强！默认使用的运动规划器是 OMPL（Open Motion Planning Library） 库。如下图所示，Planning Pipeline提供了很多规划器的接口，通过导入planning scene 导入octomap作为机器人环境信息https://blog.csdn.net/qq_34935373/article/details/104794634，然后通过路径规划算法，完成避障的路径规划。

路径规划算法：先放上官方链接，方便查看 http://ompl.kavrakilab.org/planners.html

OMPL是基于采样方法的运动规划库，其规划算法可以分为两类：

• Geometric planners
• Control-based planners

特性：基于采样，概率完备，非最优/渐进最优

1. Geometric planners

• 多查询规划器
  这些规划器构建了一个可用于多个查询的整个环境的路线图。
  • 概率路线图法
    这是基于采样的算法。使用一个线程来构造路线图，而另一个线程则检查路径是否存在于起始状态和目标状态之间的路线图中。OMPL包含一些PRM的变体：
    • LazyPRM
      这个规划器类似于常规的PRM，但是“懒惰地”检查顶点或边的有效性，也就是说，只有当它是候选解决方案路径的一部分时。
    • PRM*
      当常规PRM尝试将状态连接到一个固定数量的邻居时，PRM*随着路线图的增长而逐渐增加连接尝试的次数，从而提供对最优路径的收敛性。
    • LazyPRM*
      带有延迟状态有效性检查的PRM*版本。
  • SPArse Roadmap Spanner algorithm (SPARS)
    SPS是一个提供渐进性的计划器。近-最优性(在最优解的常数范围内的解)，并包含有意义的停止准则。虽然它不能保证最优性，其收敛速度往往远高于PRM*。
  • SPARS 2
    SPARS 2是SPS算法的变体，它通过类似的机制工作，但使用不同的方法来识别接口和通过所述接口计算最短路径。
• 单查询规划师
  这些规划器通常会生长一棵由有效运动连接的状态树。这些规划器在他们用来控制的启发法上有所不同。
  • 快速探索随机树(RRT)
    该算法易于理解，易于实现。已提出了许多、许多不同的RRT方案。OMPL包含几个RRT变体：
    • RRT连接(RRTConnect)
      此计划器是RRT的双向版本(即它生长了两棵树)。它通常优于原RRT算法。
    • RRT*
      RRT的一个渐近最优版本：该算法作为时间函数收敛于最优路径上。这是第一个可证明的渐近计划者(与PRM一起)。自从RRT*发布以来，出现了几种提高RRT*收敛速度的算法，例如RRT#和RRTX。
    • 下界树RRT(LBTRRT)
      LBTRRT是RRT的一个渐近接近最优的版本：它保证收敛到一个在最优解的常数因子内的解。
    • 稀疏稳定RRT
      SST是RRT的渐近最优增量版本.
    • 基于过渡的RRT(T-RRT)
      T-RRT没有提供任何硬的最优性保证，而是试图找到简短、低成本的路径.
    • 矢量场RRT
      VF-RRT是一种基于树的运动规划器，它试图最小化路径的所谓上游成本。上游成本由用户定义的向量场上的积分定义.
    • 平行RRT(PRRT)
      针对基于抽样的规划者，包括RRT，提出了许多不同的并行化方案.在此实现中，多个线程同时将状态添加到同一棵树中。一旦找到解决方案，所有线程都会终止。
    • 懒惰RRT(LazyRRT)
      此计划器执行延迟状态有效性检查(类似于LazyPRM)。这不是试验性的，但根据我们的经验，在任何一类问题上，它似乎都没有比其他规划者好得多。
  • 扩展空间树(EST)
    这位规划器大约是在RRT出版的同时出版的。在我们的经验中，它对有一个好的距离度量不那么敏感，这对于复杂的高维状态空间来说是很难定义的。实际上，EST有三个版本：原版接近第一次出版，双向版本，以及基于投影的版本。低维投影用于跟踪如何探索状态空间。大多数情况下，OMPL可以自动确定合理的投影。我们实施了一些规划者，这些计划不一定是EST的简单变体，而是有着相同的扩展策略：
    • 单查询双向延迟碰撞检查计划器(SBL)
      此计划器本质上是带有延迟状态有效性检查的双向EST版本。
    • 并行单查询双向延迟碰撞检查计划器(PSBL)
      这个计划器在SBL中并行地生长两棵树。
    • 内-外单元探索的运动学规划
      KPIECA是一种基于树的规划器，它使用离散化(通常是多个层次)来指导(连续)状态空间的探索。OMPL的实现是一个简化的实现，使用一个级别的离散化：一个网格。网格被强加于投影状态空间。在探索空间时，优先考虑到到目前为止已经探索过的那部分网格的边界。边界被定义为一组小于2的网格单元。n中的非对角线非空相邻网格单元。n-维投影空间KPI有两种变体：
      • 双向KPIECA(BKPIECA)
      • 懒惰双向KPIECA(LBKPIECA)
  • 具有分辨率独立密度估计的搜索树
  • 路径定向细分树
  • 快速移动树算法(fmt?)
  • 双向快速行军树算法(bfmt?)
• 优化规划器
  近年来，提出了几种基于抽样的规划算法，这些算法仍然提供了一些最优性保证。通常，最优解被假定为最短路径。在OMPL中，我们有一个更通用的表示状态和路径代价的框架，它允许您，例如，最大化路径上的最小间隙，最小化机械工作，或一些用户定义的任意优化准则，器使用这个一般的成本框架，但是要记住，收敛到最优性是无保证当对路径长度以外的内容进行优化时。
  • PRM*
  • LazyPRM*
  • RRT*
  • RRT#
  • RRTX
  • ... .... ....

OMPL如何选择几何规划器
如果您使用ompl：geometric：：SimpleSetup类(强烈推荐)来定义和解决您的运动规划问题，然后OMPL将自动选择适当的计划器(除非您已经明确指定了)。如果状态空间使用的是任何内置状态空间，那么它将使用LBKPI如果可以使用双向规划器，否则它将使用KPIECA。这些默认配置已经被证明在许多现实世界的运动规划问题上一直工作得很好，这就是为什么这些规划器是默认的选择。如果状态空间没有默认配置，RRTConnect或正常RRT将被使用，这取决于是否可以使用双向规划器。

2. Control-based planners

如果所考虑的系统受到不同约束，则使用基于控制的计划器。这些规划者依靠状态传播而不是简单的插值来产生运动。这些规划器不需要转向功能，但是如果用户实现它，所有这些(KPIECA除外)都将使用它。下面的前两个规划算法是上面相应的Geometric planners的替代

• 快速探索随机树(RRT)
• 稀疏稳定RRT
  SST是RRT的渐近最优增量版本.
• 扩展空间树(EST)
• 基于决策树采样的运动规划算法(KPIECE)
• 路径定向细分树
  基于控制的PDST版本实际上出现在几何版本之前。考虑到基于控制的版本，实现几何版本也很简单。
• Syclop，一种使用较低层次的规划器的元规划器。
  Syclop是一种元规划器，它将在状态空间分解的基础上计算的高级与低级规划算法相结合。低级别规划器所取得的进展反馈给高级规划器，后者利用这些信息更新。Syclop有两个不同的版本：
  • 以RRT为底层规划器的Syclop
  • 以EST为底层规划器的Syclop
• 线性时序逻辑规划器
  LTLPlanner为目标由线性时序逻辑(LTL)规范指定的运动规划问题找到解决方案。

OMPL如何选择基于控制的计划器

如果您使用ompl：：Control：：SimpleSetup类来定义和解决您的运动规划问题，然后OMPL将自动选择适当的计划器(除非您已经明确指定了)。如果状态空间使用的是任何内置状态空间，那么它将使用KPIECA。这些默认配置已经被证明在许多现实世界的运动规划问题上一直工作得很好，这就是为什么这些规划器是默认的选择。如果状态空间没有默认配置，RRT会被使用。

OMPL自定义运动规划算法的方法

自从看了交大邱博的路径规划的一个视频之后（如下其中之一的截图），才知道这里面包含的方法有很多，才发现ROS是多么的强大，帮我这种小硕铺了一条康庄大道，很多东西直接拿来就用了。

当然对于大神来说，折腾总是在路上的，看了那么多算法，虽然很多都不适用于机械臂这种高维空间的路径规划，但是还是免不了想试试看自己导入一些算法。一番搜索，发现虽然尝试的人貌似很少，可还是有大神以及做了：

https://blog.csdn.net/sinat_23853639/article/details/87854461 ，https://blog.csdn.net/weixin_36965307/article/details/105312020前提就是不能以Binary的方式在ROS中安装MoveIt!，而以source的方式来安装，就可以将自己写的运动算法（比如在OMPL中的算法基础上进行修改）集成到MoveIt!中。先占坑！！！