从零开始的OMPL库算法学习(1)RRT算法

从零开始的OMPL库算法学习(1)RRT算法

简介

RRT 算法(快速扩展随机树,rapidly exploring random tree)是一种随机性算法,它可以直接应用于非完整约束系统的规划,不需进行路径转换,所以它的算法复杂度较小,尤为适用于高维多自由度的系统。
缺点是得到的路径质量不是很好。
其思想是快速扩张一群像树一样的路径以探索(填充)空间的大部分区域,伺机找到可行的路径。
RRT 的基本步骤是:
  1. 起点作为一颗种子,从它开始生长枝丫;
  2. 在机器人的“构型”空间中,生成一个随机点X;
  3. 在树上找到距离X最近的那个点,记为Y吧;
  4. 朝着X的方向生长,如果没有碰到障碍物就把生长后的树枝和端点添加到树上,返回 2;
  六维空间
 六维空间的RRT
  在这里插入图片描述
  实际效果如图。

伪代码


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

你可能感兴趣的:(ompl)