环境感知与规划专题（十）——基于采样的路径规划算法（二）

前言

 上一篇介绍了快速搜索随机树（RRT）算法的原理，这是一种基于采样的路径规划算法，在地图尺寸较大时，其效率将显著的优于基于图搜索的路径规划算法（如A*）。

 然而，RRT也有其局限性，如：

• 狭窄通道情况下的搜索效率急剧下降

• 搜索得到的路径不是全局最优的

 本篇将针对RRT算法应用时出现的几个问题展开讨论，并阐述几种RRT算法的改进型。

快速获取搜索树中的相邻节点

 RRT算法中有一个重要的环节——获取搜索树种距离采样点最近的节点：

 该算法的效率直接影响了RRT算法的搜索效率，因此，本篇单独对其进行讨论。

 在工程中，我们将搜索树构建为KD-Tree（K-dimensional Tree），通过KD-Tree来搜索相邻节点，它改进的就是上图中的$Near(x_{rand},\tau )$函数的效率，那么什么是KD-Tree呢？

 KD-Tree(K-dimension tree)是一种对$k$维空间中的实例点进行存储以便对其进行快速检索的树形数据结构。KD-Tree是一种二叉树，表示对$k$维空间的一个划分，构造KD-Tree相当于不断地用垂直于坐标轴的超平面将$k$维空间切分，构成一系列的$k$维超矩形区域。KD-Tree的每个结点对应于一个$k$维超矩形区域。利用KD-Tree可以省去对大部分数据点的搜索，从而减少搜索的计算量。

 对于RRT算法，我们对搜索树进行KD-Tree构建，以二维问题为例，我们取所有节点中$x$方向的中位数节点为切割点，将所有节点分为两部分，再对所有节点中$y$方向的中位数节点为切割点分割，以此类推，最终将搜索树中的所有节点划分完毕，即得到KD-Tree。

 在得到KD-Tree后，我们可以根据需要，快速地搜索到距离$x_{rand}$节点最近的节点，大大提高$Near(x_{rand},\tau )$函数的搜索效率。