RRT与RRG路径搜索算法

一、RRT（rapidly exploring random tree）

伪代码：

 1 function BuildRRT(qinit, k, Δq)
 2     T.init(qinit)
 3     for k = 1 to k
 4         qrand = Sample()
 5         qnearest = Nearest(T, qrand)
 6         if Distance(qnearest, qgoal) < Threshold then
 7             return true
 8         qnew = Extend(qnearest, qrand. Δq)
 9         if qnew ≠ NULL then
10             T.AddNode(qnew)
11     return false
12 
13 function Sample()
14     p = Random(0, 1.0)
15     if 0 < p < Prob then
16         return qgoal
17     elseif Prob < p < 1.0 then
18         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）

　　伪代码：

 1 RRG on Point Cloud
 2     Requires  M
 3     G <- Ns, i <- 0
 4     while i <= N do
 5         Cr <- sample()
 6         Nc <- nearest(Cr, G)
 7         Nn.c <- intersect(Nc.c, Cr)
 8         Nn.r <- radius_search(Nn.c, M)
 9         if Nn.r > λr then
10             G <- GUNn
11             connect(Nc, Nn)
12             for all Nj ∈ neighbor(Nn, G) do
13                 if overlap_volume(Nj, Nn) > λv then
14                     connect(Nj, Nn)
15                 end if
16             end for
17         end if
18         i <- i + 1
19     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与它们相连，这个大小要能够使得无人机或机器人安全的穿过。