RRT与RRT*算法分析理解
一、背景意义
运动规划问题是要找到一条动态可行的轨迹，将机器人从初始状态带到目标状态，同时避免与障碍物发生碰撞。这对机器人实现自主运动、高效执行任务、降低人为成本等具有非常重要的意义。一个运动规划算法应该表现出两个特性：一个是形式上的完整性保证，另一个是渐进的最优性。RRT一种基于采样的快速探索随机树算法，能够相对快速地找到一个可行的运动轨迹，即使在高维状态空间中，保证了运动规划算法的完整性。但是其寻找的运动轨迹并非是当前从起点到目标点的最优轨迹。而RRT*会对树节点有个修建和重新选择的过程，即将新节点加入搜索树时父节点会有一个重连过程，也就是在以新节点为圆心，半径为r的邻域内，找到与新节点连接后路径代价(从起点移动到新节点的路径长度)最小的节点，并重新选择最短距离节点作为新节点的父节点，而不是最近节点。这样保证了寻找的路径可以收敛到最优解，同时还在概率上保证了轨迹的完整性。
二、RRT（快速探索随机树）
1. 基本思想
轨迹连通图采用树的形式，在空间中随机采样，将采样的节点加入到树中并于最近的树节点相连，然后进行一步步的扩展达到目标点后停止。需要考虑的是机器人的运动执行能力，以及是否可以通过树结构直接回溯得到可行的路径。
2. 基本流程
以起点为根节点，建立搜索树，对树进行扩张（extend）：在状态空间中，随机采样一个状态，称为Xsample；在现有的探索树查找与Xsample距离最近的节点Xnearest；然后依据Xsample和Xnearest确定新的树扩张新节点。这一步需要注意的是：二维路径规划可根据Xsample和Xnearest的方向结合步长确定新的节点Xnew，在高维路径规划可考虑机器人的系统状态方程，以Xsample和Xnearest构建新的输入u，以Xnearest作为当前状态x，根据系统的状态方程，得到下一个状态即新的扩张节点Xnew。接着要有一个判断，判断Xsample和Xnearest是否会碰到障碍物，而且不在搜索树中，如果不符合则舍去；最后终止判断，已经到达终点，规划成功，返回从初始状态到终点的节点路径。
3. 不足与可改进的点
当空间中包含大量的障碍物，或者狭窄的通道约束时，算法的收敛速度很慢，有时甚至无法找到可行解，而且得到的路径并非最优路径。针对改进，可以从以下方面着手：①算法的代码初始时，给定一定的采样概率向着目标点进行采样②采用何种方式进行采样，初始的步长是否可以调整来适应更细致的连接③如何定义和查找最近节点④树的将如何扩张来更快的找到路径
4. 确定最近距离和查找最近点
①计算距离最简单的方法是欧氏距离，但在很多场景下此种距离并不可行
②另一种可行方法是对距离的不同分量进行加权平均，不同分量的重要程度用权重大小表示
③寻找最近点有很多可行的算法，如K-D树，Hash算法等
三、RRT*（快速探索随机树的扩展）
1. 原理
首先定义一个成本函数表示从初始状态到任意状态的代价，并且设置初始状态成本为0，然后RRT* 找到最近的状态和相邻的状态，将最近的状态添加到树中，并在存在最小代价连接的情况下添加最小代价连接，最后RRT*通过连接来消除开销较大的连接。
2. 基本流程
与RRT一样，RRT*通过从空间中随机采样Xsample并求解一条轨迹Xnew，将树中最近的节点Xnearest向样本延伸来逐步建立树。如果这个轨迹没有与障碍物发生碰撞，标准的RRT将新的节点Xnew插入到以Xnearest为父节点的树中，并继续进行下一次迭代。在这里，RRT*的操作是不同的。RRT*不是选择最近的节点作为父节点，而是考虑Xnew附近的所有节点并评估选择每个节点作为父节点的成本。最后进行进一步的回溯，如果存在一条路径Xnew经过到达Xnearest花费的代价小于Xnearest的父节点到达Xnearest的代价，说明原来到达的代价不是最优的，需要选择Xnew到达Xnearest的路径作为新路径。对所有的邻近点集合内的点都做相同的操作，经过N次迭代得到接近最优的次优路径了。
3.优点与改进的地方
①相对与RRT最大的改进就是，会有一个选择重连的过程，来确保与新节点与父节点连接的成本是最小的，以达到一条渐进最优的路径
②RRT*的缺点也很明显，因为需要选择重连，计算量大，生成路径的效率低，甚至在复杂的环境或者窄缝的情况下会出现找不到路径。
③可以从扩展的范围进行改进，而不是全局进行随机采样搜索，还可以分别从起点终点建立探索树，以加快搜索效率。在步长方面是否也可以进行改进，改进为自适应步长，来更快通过无障碍区域。
四、参考文献
[1] S. Karaman and E. Frazzoli, “Sampling-based Algorithms for Optimal Motion Planning,” The International Journal of Robotics Research, vol. 30, no. 7, pp. 846–894, June 2011.
[2] Klemm S, Oberländer J, Hermann A, et, “Rrt-connect: Faster, asymptotically optimal motion planning”015 IEEE international conference on robotics and biomimetics (ROBIO). IEEE, 2015: 1670-1677.
