RRT路径规划算法

最近读的论文有讲到关于RRT算法,现在搬运一下网上看到的一些知识。

以及:

首先传统的路径规划算法有一下这么几个
人工势场法、模糊规则法、遗传算法、神经网络、模拟退火算法、蚁群优化算法等。但这些方法都需要在一个确定的空间内对障碍物进行建模,计算复杂度与机器人自由度呈指数关系,不适合解决多自由度机器人在复杂环境中的规划。
。基于快速扩展随机树(RRT / rapidly exploring random tree)的路径规划算法,通过对状态空间中的采样点进行碰撞检测,避免了对空间的建模,能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间,通过状态空间的随机采样点,把搜索导向空白区域,从而寻找到一条从起始点到目标点的规划路径,适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。与PRM类似,该方法是概率完备且不最优的。

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<p<Prob的时候,随机树朝目标点生长行;当Prob<p<1.0时,随机树朝一个随机方向生长。

这里提供了github中的RRT C++实现
github源码:https://github.com/longjianquan/add--planner