RRT笔记

相关背景以及定义：
原文: https://blog.youkuaiyun.com/minfanphd/article/details/120666884

1. 定义清楚问题
   输入、输出、优化目标、约束条件
   输入：二维平面地图，地图的大小是已知的吗？障碍是已知的吗（有可能走到面前才知道，并没有上帝视角）？地图上的区域有差异吗（距离与路程是否一定成正比，如果有沼泽地就不一定了）？
   输出：从原点到终点的路径
   优化目标：1）路径最短；2）搜索时间最短。其实是多目标优化。
   约束条件：障碍物，导致有些顶点不可达

   输入：1) 起点坐标，终点坐标，障碍物坐标及其影响范围。2) 地图大小对于算法来说是未知的，终点在哪儿也是未知的，只是知道终点的方向。3) 障碍对算法来说是未知的，走到面前才知道，没有上帝视角。4) 地图上的区域有差异，分为障碍物区域和非障碍物区域，障碍物区域不可达，非障碍物区域内距离和路程一定成正比。

   输出：从起点到终点的避开障碍物的一条无碰撞路径。

   优化目标：1) 次优最短路径 2) 搜索时间最短

   约束条件：障碍物，导致的障碍物区域不可达。

2. 给出一个最简单粗暴的算法，最好是确定性的
   穷举法、动态规划算法（自底向上）
   Dijkstra

3. 寻找更优的算法，有可能是不确定性的智能算法
   分治法（减少时间的常用方法）

路径规划中一般分为三类规划算法.

1. 基于图搜索的路径规划算法，
   1) 代表算法Dijkstra, A*算法。需要获得全局的地图信息，无法进行动态搜索。
   2) 适合规模较小的地图。
   3) 能够得到最优解。
2. 基于抽样的路径规划算法，
   1）代表算法如快速搜索随机树( rapidly - exploring random tree, RRT)，PRM算法,在高维度复杂规划问题中得到了广泛应用。
   基于采样的路径规划算法依赖算法执行过程中对节点的采样,不需要构建完整地图也可实现路径规划。
   局限性：
   1. 空白区域采样点密集,障碍物密集处采样点又相对较少，可能无法得到最短路径。
   2. 参数的设置对路.径规划的结果影响较大,采样点的数量、邻域的大小.设置不合理均可能导致路径规划失败,如采样点设置过少导致生成的路径过少未覆盖起终点、邻域设置过大导致过多的路径无法通过碰撞检测。
   3. 常用的基于采样的路径规划算法几乎无法得到最优解,往往会收敛到一个非最优解。
3. 启发式的路径规划算法
   1. 代表算法有蚁群算法，遗传算法。
      从初始位置开始进行领域搜索，并根据一定的准则更新当前状态。
      在动态路径规划中有一定的优势。
      以蚁群算法为例：
   2. 蚁群算法原理参考的是自然界蚁群寻找食物的过程,蚂蚁在寻找食物 的过程中，蚂蚁会在经过的路径上留下特异的信号分子,且信号分子的多 少与路径的长度成反比。因此,较短的路径信号分子的浓度将逐渐增加,形 成–种正反馈机制,吸引附近其他蚂蚁,而其余路径上的信号分子逐渐消失, 最终绝大多数蚂蚁都会集中到最短路径上。
      局限性：
      1)需要选取合适的参数才能更好的模拟仿生行为。最大限度地发挥出算法优势。
      2)相比较于图搜索算法,在高维规划中表现更佳该算法缺陷是蚁群只有在接近障碍物时才会避障,因此无法预先规避障碍物;初始阶段盲目搜索时产生大量局部交叉路径。
      3)在复杂环境中算法收敛速度较慢，容易陷人局部最优导致路径规划失败。

RRT（Rapidly-Exploring Random Trees)，是基于抽样的一种代表算法，RRT算法则从某个点出发一边搜索，一边抽样并建图。

RRT算法是概率完备的：只要路径存在，且规划的时间足够长，就一定能确保找到一条路径解。注意“且规划的时间足够长”这一前提条件，说明了如果规划器的参数设置不合理（如搜索次数限制太少、采样点过少等），就可能找不到解。其次，RRT找到的通路一般为次优解，即只是可行，但不一定最优。

传统RRT存在的一些问题：1. 狭窄通路问题。如果起点与终点之间存在如下图所示的狭窄路段，则有可能延伸的树枝永远到达不了终点。2.收敛较慢，在障碍物较多的情况下，可能花费的时间较长，时间效果上可能不太好。

RRT算法伪代码如下：

1. 构造相关环境。
起点：(0, 0)，图中显示为绿色。
终点：(15, 12)，图中显示为红色。
障碍物：设置了5个障碍物，均为黑色，不过半径不一样。
实际地图范围：x轴0-15， y轴0-12。

2.算法具体流程：
1) RRT初始相关参数：
$start$: 起点，即开始的位置。形式为$(x,y)$。
$goal$: 终点，目标位置。形式为$(x,y)$。
$obstacles$：障碍物。在这里使用圆形障碍物，形式为$(x,y,effect\_scope)$,其中effect_scope为影响半径。
$step\_size$：RRT每次延伸的步伐长度，后面详细解释这一过程。默认为2。
$prob$: 选择随机点或终点的概率，后面详细解释。
2) 算法过程：
(1) 随机一个概率$p$，如果$p<prob$,则在地图中随机选择一个点，作为$x_{rand}$。若$p>prob$,则以$goal$作为$x_{rand}$。

(2) 最开始以$start$作为$x_{init}$,开始延伸RRT。然后寻找RRT中结点距离$x_{rand}$最近的点，第一次延申最近点为$start$作为$x_{near}$，做两点之间的连线：

(3) 然后在这条连线上找距离$x_{near}$距离为$step\_size$的点，一般能够找到两个。如果两个$x_{new}$都没在障碍物中或者超出地图的边界，则选择距离$goal$最近的点。不然选择未在障碍物中的点。否则$x_{new}$返回None，此次延申失败，重新选择随机点. 在这里则选择图中的$x\_new\_1$。

(4) 得到新结点，加入至树中。
(5) 重复上述过程，直到$distance(x_{new},goal)<=step\_size$，此时RRT延伸完成。从最后生成的$x_{new}$寻找其父节点，就可以一直找到$start$,一路经过的结点则为生成路径。

gif 如图所示，左下角为起点，右上角为终点，蓝色为延伸的路径，黄色为障碍物：

3. 有关多颗RRT的延申：
1) 设置RRT的根节点数目，这里设置了三个根节点数目，以$X$轴作为划分依据，这里$X$轴长度为15,则以15 / (3 + 1),划分4块区域，在中间的区域生成随机点，只要随机点不在障碍物中即可。

2) 然后第一颗RRT树以start点为起点，root1为终点。第二颗RRT树以root1为起点，root2为终点。第三棵树以root2为起点，goal为终点。如果划分区域不同，RRT树划分方式也如同这样。然后分别开始进行延申。RRT原理如上述所示。
gif 显示如下：

结论： 在本次实验中，同一环境下，单颗RRT一共延伸了191个枝丫。而划分3颗RRT树，一共延伸了126颗枝丫。可以看到，提升还是比较明显的。