【机器人路径规划算法RRT和RRG】

RRT路径规划

RRT算法：Rapid-exploration Random Tree 快速搜索随机数算法，是一种在完全已知的环境中通过随机采样扩展搜索的算法。
RRT算法是概率完备的，只要规划时间足够长，确实存在一条路径可以从起始点到终点，就一定能够找出来，但是如果规划时间不够长，采样点设置的不够，迭代次数较少，就可能无法找出实际存在的路径。主要的优点就是快，在多自由度机器人规划中发挥较大的作用，比如机械臂的规划算法大多是使用RRT算法。但是RRT算法规划的路径通常不是最优的，路径不够平滑。

算法基本流程图

鉴于采样点较多的情况下，还是不建议采用遍历的方式来找距离采样点最近的树节点，而采用KD_tree的最近邻检索方式在采样点数量较多的情况下能够节省很多的时间找到最近的树节点。

KD_tree划分方法及最近邻检索 参考：点云数据下的KD-tree

下面进行一个RRT路径规划的过程：

图1开始，设置好起始点，终止点和障碍物，就可以开始规划路径的过程了。
图2中黄色三角形为产生的随机点，从起始点向随机点连线，此时这条线未经过障碍物，随机采样点就有效，设定一定的生长步长，以起始点为一个端点，在射线上取生长步长的长度，确定一个新的树节点。

这时就有两个树节点，继续产生随机抽样点（黄色三角形示），找到离随机抽样点最近的一个树节点，由该树节点向随机抽样点做射线，在射线上取生长步长的长度，再确定一个新的树节点，没有碰到障碍物，节点有效。

图9继续生成随机抽样点，最近树节点与随机抽样点做射线，取生长步长的长度生成新的树节点（图10），此时生长的根节点在障碍物上，舍弃该随机点，再重新在地图上产生随机抽样点，重新生成根节点，反复迭代，直到新增树节点到达目标区域，就代表已经找到路径，停止迭代。

每次撒点都以新生成的树节点作为子节点，上一个根节点作为父节点，因此每一个节点都有父节点，以到达目标区域的新增树节点作为子节点反向遍历父节点，直到找到起始点，就找到了路径。

不过生成的路径都是折线并不是最优的路线。

主要的算法执行三个步骤：
循环撒点 ➡ 判断是否经过障碍物 ➡ 生成根节点 ➡ 判断是否到达目标区域

RRG路径规划

RRG是由RRT在二维平面向三维空间扩展的路径规划算法，理解了RRT算法，RRG就不难懂了。

首先，图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与它们相连，这个大小要能够使得无人机或机器人安全的穿过。

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

下面是实现RRG算法的Python程序：

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

下图是通过RRG结合A*算法获得的最优路径：

图（a）是产生的通过障碍物的飞行走廊，图（b）是飞行走廊俯视图，由具有重叠部分的安全区域球组成。