路径规划（RRT）

路径规划的核心内容是：在有碰撞的环境中，规划出一条从起始点到目标点的无碰撞路径。

路径规划算法特点总结：

完备性：起始点与目标点之间有路径解存在，那么一定可以找到解，若找不到解则说明一定没有解存在；
概率完备性：是指若起始点与目标点之间有路径解存在，只要规划及搜索时间足够长，就一定能够确保找到一条路径解；
最优性：规划得到的路径在某个评价指标上是最优的 ；
渐进最优性：是指经过有限次规划迭代后得到的路径是接近最优的的次优路径，且每次迭代都是与最优路径更加接近，是一个逐渐收敛的过程。

关于路径规划算法，按照算法类型可以分为：
基于搜索的算法：其中重要包括Dijkstra算法、A算法、D算法等，这一类算法是完备且最优的；

基于采样的算法：RRT、RRT-Connect、RRT*（快速扩展随机树及其变种），PRM(构建概率路线图)等，由于采样点的随机性导致这类算法是概率完备的，规划出的路径不是最优的，只能说是规划出一条可行路径，其中RRT*算法是渐进最优的路径规划算法；

基于智能优化的算法：主要包括遗传算法、蚁群算法、粒子群算法等。这类算法是完备的但是不是最优的。

路径规划算法主要包括以上三种类型，从路径规划的速度方面来说：

RRT系列>A*>Dijkstra算法>智能优化算法

经过查阅相关文献可知，若用A算法进行路径规划，倘若存在最优路径必能找到，但是但对于高维空间的路径规划问题，A算法并不适用。RRT算法与A*算法相比，不需要对空间进行栅格化处理，无需对环境进行结构化建模，适合在高维空间进行路径规划。在这里主要对RRT算法进行简单的介绍。

RRT是一种基于随机采样的路径规划算法，在已知路径起始点和终点的前提下，随机产生一个采样点Xrand，在树上找到一个与Xrand最接近的节点Xnear，连接Xrand与Xnear，在该直线上以步长Ei得到节点Xnew作为显得树节点，以此方式进行树的扩展：

扩展得到下一树节点：

在进行节点扩展的过程中，需要进行碰撞检测，若新的树枝即Xnear与Xnew与障碍物之间发生碰撞，则去除该条路径；

按照以上流程进行搜索，直到Xnew与终点goal的距离小于小于我们所设定的某个极小值，停止搜索，直接将Xnew与终点相连，一条可行路径产生。

传统的RRT算法路径搜索效率低，且搜索到的路径不是最优路径，为了提高路径搜索效率，在传统的RRT算法的基础上提出了基于双向搜索的RRT-Connect算法，该算法是分别在起始点与目标点处同时扩展两棵树，扩展树的方式与传统的RRT算法相同，直至两棵树连接到一起，一条可行路径产生。

RRT*算法通过以下两种方式对RRT算法进行改进：
父节点的重新选择

重新布线

详细介绍如下：

传统的RRT算法是在产生Xnew节点后，直接将Xnear与Xnew节点连接，产生一个新的树枝；RRT*算法在这里对父节点进行了重新选择，以Xnew为圆心以R为半径进行画圆，

将Xnew与圆圈内的节点Xnear、X1、X2连接起来，比较通过何种方式到达Xnew的路径消耗最短，

若通过Xnear到达Xnew的的路径消耗要比通过Xnear到达X1或者通过Xnear到达X1、X2再到达Xnew的路径消耗短的话，直接将Xnear到达Xnew进行连接；

以上部分实现的是RRT*算法中的父节点的重新选择部分。在重新选择完父节点之后，进行重新布线操作，重新布线操作是针对周围节点来说，是否有通过新产生的节点到达周围节点的距离消耗要比原有路径要小，以图中到达X2节点的路径来说，通过新产生的节点Xnew到达X2要比原有的路径消耗要短，那么就对原有路径进行重新布线，如下与所示：

RRT*算法随着采样点的不断增加，不断优化直至找到目标点或达到最大设定循环次数；该算法随着迭代次数的不断增加，路径逐渐优化，所以该算法是一种渐进最优的路径规划算法，但是，该算法消耗时间较长，路径规划效率较低。

详细的RRT*算法的代码程序大家可以通过关注公众号获得！！