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與它們相連，這個大小要能夠使得無人機或機器人安全的穿過。