RRT与RRG路径搜索算法

一、RRT（rapidly exploring random tree）

伪代码：

function BuildRRT(qinit, k, Δq)
    T.init(qinit)
    for k = 1 to k
        qrand = Sample()
        qnearest = Nearest(T, qrand)
        if Distance(qnearest, qgoal) < Threshold then
            return true
        qnew = Extend(qnearest, qrand. Δq)
        if qnew ≠ NULL then
            T.AddNode(qnew)
    return false

function Sample()
    p = Random(0, 1.0)
    if 0 < p < Prob then
        return qgoal
    elseif Prob < p < 1.0 then
        return RandomNode()

　　初始化时随机树T只包含一个节点：根节点qint。首先，随机函数采样函数Sample从状态空间中随机选择一个采样点qrand，然后Nearest函数从随机树中选择一个距离qrand最近的节点qnearest。通过Distance函数判断qnearest与qgoal之间的距离是否小于设定的阈值，若小于阈值,说明随机树到达了目标点，返回true，若大于阈值，则通过Extend函数从qnearest向qrand扩展一定距离，得到新的节点qnew。如果qnew与障碍物发生碰撞，则Extend函数返回NULL，放弃此次生长。为了使算法可控，可以设定运行时间上限或搜索次数上限。如果在限制次数内无法到达目标点，则算法返回失败。

　　为了加快随机树到达目标点的速度，简单的改进方法是：在随机树每次的生长过程中，根据随机概率来决定qrand是目标点还是随机点。在Sample函数中设定参数Prob，每次得到一个0到1.0的随机值p，当0<p<Prob的时候，随机树朝目标点生长行；当Prob<p<1.0时，随机树朝一个随机方向生长。
　　

二、RRG（rapidly exploring random graph）

　　伪代码：

RRG on Point Cloud
    Requires  M
    G <- Ns, i <- 0
    while i <= N do
        Cr <- sample()
        Nc <- nearest(Cr, G)
        Nn.c <- intersect(Nc.c, Cr)
        Nn.r <- radius_search(Nn.c, M)
        if Nn.r > λr then
            G <- GUNn
            connect(Nc, Nn)
            for all Nj ∈ neighbor(Nn, G) do
                if overlap_volume(Nj, Nn) > λv then
                    connect(Nj, Nn)
                end if
            end for
        end if
        i <- i + 1
    end while

　　目的：生成一个由球体安全区域构成的图

　　首先，图G由唯一节点Ns初始化，每个节点N具有两个属性：中心点3D位置N.c和半径N.r。当通过随机采样函数sample获得新的点Cr，nearest函数在G中寻找距离Cr最近的节点Nc。intersect函数生成一条由Cr到Nc.c的射线，并返回射线与Nc球体相交的点Nn.c，我们把这个点作为新节点Nn的中心位置，通过半径搜索函数radius_search在地图M中寻找一个最大的安全球体半径Nn.r，如果寻找到的半径足够大，这个新节点会被加入到G中，并将其与Nc相连。同时，我们将与Nn相交的所有节点找到，根据重叠体积的大小来决定是否将Nn与它们相连，这个大小要能够使得无人机或机器人安全的穿过。