本文转自“心之器机的ompl”中和RRTConnect相关的三篇文章。
一、从零开始的OMPL库算法学习记录（0）
写在开头
毕设要做基于moveit的零件抓取，现在进度卡在解决place_pose_orientation的倾斜问题，所以开始学习moveit下的ompl库。希望能坚持下去，并分享给大家。本学习记录只关注算法原理，不关注程序实现。
OMPL
ompl（The Open Motion Planning Library）的全称是开源运动规划库，基于采样方法，包含了各种先进的路径规划算法。其算法分为两类，Geometric planners和control-based planners。在moveit生成的ompl_planning.yaml文件中，可供选择的算法有以下几种：
- SBL - EST - LBKPIECE - BKPIECE - KPIECE - RRT - RRTConnect - RRTstar - TRRT - PRM - FMT - BFMT - PDST - STRIDE - BiTRRT - LBTRRT - BiEST - ProjEST - LazyPRM - LazyPRMstar - SPARS - SPARStwo
看起来一头雾水。其默认选择为RRTConnet。
采样算法
opml库中的算法是基于采样的，那什么是采样算法呢？采样算法又称蒙特卡罗方法。它解决的问题是：给定一个概率分布p(x)，如何让计算机生成满足这个概率分布的样本。 这个问题就叫采样。其本质是对随机现象的模拟，根据给定的概率分布 ，来模拟产生一个对应的随机事件。
首先我们来看一个简单的例子，假设我们想对下面的二项分布进行采样:
P(X=0)=0.5, P(X=1)=0.5
我们如何采样得到X的值?我们会很毫不费力地想到“抛硬币”，如果硬币正面朝上，则X=1，否则，X=0。那计算机怎么做，使用随机数生成器生成0到1之间的随机数r，k如果r<0.5，则X=1，否则，X=0。
二、从零开始的OMPL库算法学习（1）RRT算法
简介
RRT 算法（快速扩展随机树，rapidly exploring random tree）是一种随机性算法，它可以直接应用于非完整约束系统的规划，不需进行路径转换，所以它的算法复杂度较小，尤为适用于高维多自由度的系统。
缺点是得到的路径质量不是很好。
其思想是快速扩张一群像树一样的路径以探索（填充）空间的大部分区域，伺机找到可行的路径。
RRT 的基本步骤是：
- 起点作为一颗种子，从它开始生长枝丫；
- 在机器人的“构型”空间中，生成一个随机点X；
- 在树上找到距离X最近的那个点，记为Y吧；
- 朝着X的方向生长，如果没有碰到障碍物就把生长后的树枝和端点添加到树上，返回 2；
实际效果如图2。
伪代码
function BuildRRT(qinit, K, Δq) T.init(qinit) for k = 1 to K qrand = Sample() -- chooses a random configuration qnearest = Nearest(T, qrand) -- selects the node in the RRT tree that is closest to qrand if Distance(qnearest, qgoal) < Threshold then return true qnew = Extend(qnearest, qrand, Δq) -- moving from qnearest an incremental distance in the direction of qrand if qnew ≠ NULL then T.AddNode(qnew) return false function Sample() -- Alternatively,one could replace Sample with SampleFree(by using a collision detection algorithm to reject samples in C_obstacle p = Random(0, 1.0) if 0 < p < Prob then return qgoal elseif Prob < p < 1.0 then return RandomNode()
初始化时随机树T只包含一个节点：根节点qinit。首先Sample函数从状态空间中随机选择一个采样点qrand（4行）；然后Nearest函数从随机树中选择一个距离qrand最近的节点qnearest（5行）；最后Extend函数通过从qnearest向qrand扩展一段距离，得到一个新的节点qnew（8行）。如果qnew与障碍物发生碰撞，则Extend函数返回空，放弃这次生长，否则将qnew加入到随机树中。重复上述步骤直到qnearest和目标点qgaol距离小于一个阈值，则代表随机树到达了目标点，算法返回成功（6~7行）。为了使算法可控，可以设定运行时间上限或搜索次数上限（3行）。如果在限制次数内无法到达目标点，则算法返回失败。
为了加快随机树到达目标点的速度，简单的改进方法是：在随机树每次的生长过程中，根据随机概率来决定qrand是目标点还是随机点。在Sample函数中设定参数Prob，每次得到一个0到1.0的随机值p，当0<p<Prob的时候，随机树朝目标点生长行；当Prob<p<1.0时，随机树朝一个随机方向生长。
三、从零开始的OMPL库算法学习（2）RRT-connect算法
简介
RRT-connect算法是基于RRT算法的一种算法，它从起始点和终点同时生长两棵快速扩展随机树来搜索状态空间，效率会更高。其伪代码如图所示：
每一次迭代中，开始步骤与原始的RRT算法一样，都是采样随机点然后进行扩展。然后扩展完第一棵树的新节点后，以这个新的目标点作为第二棵树扩展的方向。同时第二棵树扩展的方式略有不同（15~22行），首先它会扩展第一步得到
，如果没有碰撞，继续往相同的方向扩展第二步，直到扩展失败或者
表示与第一棵树相连了，即connect了，整个算法结束。每次选择较小的那棵树进行拓展，搜索速度、搜索效率有了显著提高。