基于采样的运动规划算法-RRT（Rapidly-exploring Random Trees）
RRT：一种通过随机构建Space Filling Tree实现对非凸高维空间快速搜索的算法。该算法可以很容易的处理包含障碍物和差分运动约束的场景，被广泛的应用在各种机器人的运动规划场景中
- Basic RRT算法
原始的RRT算法中将搜索的起点位置作为根节点，然后通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树的叶子节点进入目标区域，就得到了从起点位置到目标位置的路径
伪代码如下：
上述伪代码中，M是地图环境，是起始位置，是目标位置。路径空间搜索的过程从起点开始，先随机撒点；然后查找距离最近的节点；然后沿着到方向前进stepsize的距离得到；CollisionFree(M, )方法检测Edge(,)是否与地图环境中的障碍物有碰撞，如果没有碰撞，则将成功完成一次空间搜索拓展。重复上述过程，直至达到目标位置。
RRT算法拓展过程
- 基于概率的RRT算法
为了加快随机树收敛到目标位置的速度，基于概率的RRT算法在随机树扩展的步骤中引入一个概率P，根据概率p的值来选择树的生长方向是随机生长（xrand）还是朝向目标位置（xgoal）生长，引入像目标生长的机制可以加速路径搜索的收敛速度。
伪代码如下：
- RRT Connect算法
RRT Connect算法从初始状态点和目标状态点同时扩展随机树从而实现对状态空间的快速搜索
- RRT*算法
RRT*算法的目标在于解决RRT算法难以求解最优的可行路径的问题，它在路径查找的过程中持续的优化路径，随着迭代次数和采样点的增加，得到的路径越来越优化。迭代的时间越久，就越可以得到相对满意的规划路径。
伪代码如下：
RRT*算法和RRT算法的区别主要在于两点：
- rewrite的过程。即为重新选择父节点的过程
RRT*在找到距离xrand最近的节点xnearest并通过CollisionFree检测之后，并不立即将Edge（xnearest,xrand）加入扩展树中，而是以xrand为中心，r为半径，找到所有潜在的父节点集合，并与xnearest父节点的Cost对比，看是否存在更优Cost的父节点。
- 连接xrand和xnearest
- 以xrand为中心，ri为半径，在树上搜索节点
- 找出潜在的父节点集合xpotential-parent，其目的是要更新xrand，看看有没有比它更好的父节点
- 从某一个潜在父节点开始考虑，计算出xparent（即为xnearest）作为父节点时的代价,子节点为xchild（即为xrand）
- 先不进行碰撞检测，将潜在父节点与子节点连接起来，计算该条路径的代价
- 如果代价小于原路径，则进行碰撞检测，若代价大于原路径，则考虑其他潜在父节点
- 代价小于原路径且碰撞检测通过，则在树中将之前的边删掉，将树中新的边添加进去，即连接满足要求的潜在父节点和子节点之间的边，将潜在父节点（xpotential-parent）改为父节点（xparent），遍历所有的父节点，得到更新以后的树
2.随机树重布线的过程
