路径规划之采样搜索算法

采样搜索算法基础

两项基本任务

• Exploration

  获得关于搜索空间拓扑结构的信息，即空间的子集是如何连接的。

• Exploitation

  通过处理探索任务计算出的可用信息，逐步改进解决方案。

术语

• Probabilistic Completeness(概率完备)

  如果存在一个可行的解决方案，那么当样本数量接近无穷大时，将以1的概率找到该解决方案。

• Asymptotical(Near) Optimality(渐进最优/几乎最优)

  当样本数接近无穷大时，返回的解决方案的成本几乎肯定会收敛到最优。

• Anytime

  快速找到一个可行的但不一定是最优的解决方案，然后随着时间的推移逐步改进。

概率路图算法(Probabilistic Road Map, PRM)

Learning phase

在配置空间中随机撒点，构建一个可以代表环境的连通图

• 在配置空间中随机采样$N$$N$个点

  

• 检测出无碰撞的采样点

  

• 连接最近的点生成边

  

• 删除有碰撞的边

  

Query phase

通过图搜索算法找到一条从起点通往终点的路径

提升效率的方法

Lazy Collision-Checking

• 在不考虑碰撞的情况下由采样点生成边

• 在不进行碰撞检测的情况下生成路径

  

• 检测路径上的点和边是否是无碰撞的

  

快速生成随机树算法(Rapidly-exploring Random Trees, RRT)

RRT以一个初始点作为根节点，通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中找到一条由从初始点到目标点的路径。

RRT算法流程

• 在配置空间中随机采样得到采样点$x_{rand}$$x_{rand}$

• 在搜索树中找出距离采样点$x_{rand}$$x_{rand}$最近的节点$x_{near}$$x_{near}$

• 计算$x_{rand}$$x_{rand}$和$x_{near}$$x_{near}$之间的距离，如果距离大于步长，则从$x_{near}$$x_{near}$向$x_{rand}$$x_{rand}$移动一个步长后得到新节点$x_{new}$$x_{new}$；否则在$x_{rand}$$x_{rand}$位置生成新节点 。

• 如果$x_{new}$$x_{new}$和$x_{near}$$x_{near}$间存在直线通路，则将$x_{new}$$x_{new}$加入搜索树，它的父节点为$x_{near}$$x_{near}$；否则进入下一轮循环

如下图所示：

算法效果如下：

提高效率的方法

RRT-Connet

在RRT的基础上引入双树拓展环节，即分别以起点和目标点为根节点生成两棵树进行双向拓展。通过一次采样得到一个采样点$x_{rand}$$x_{rand}$，然后两棵搜索树同时向采样点$x_{rand}$$x_{rand}$方向进行扩展，加快两棵树建立连接的速度。

算法效果如下：

RRT*

RRT*算法流程

其算法流程与RRT算法流程基本相同，变化在于在最后将$x_{new}$$x_{new}$加入到搜索树时，增加一步选择父节点的策略。

在选择父节点时，在以$x_{new}$$x_{new}$为圆心，半径为$r$$r$的邻域内，找到与$x_{new}$$x_{new}$连接后路径代价最小的节点，选择$x_{min}$$x_{min}$作为$x_{new}$$x_{new}$的父节点。

示意图如下：

算法效果如下：

提高效率的方法

Bias Sampling

向目标节点采样

Sample Rejection

当$g+h>c$$g + h > c$时，拒绝这个采样点

Branch-and-bound (Tree Pruning)

将无希望的子树直接放弃

Graph Sparsify

按分辨率拒绝采样点

Delay Collision Check

按潜在的代价值对邻居进行排序。按顺序检查碰撞，一旦发现无碰撞的边就停止。

Bi-directional search

从起点和终点同时生长树

Conditional Rewire

重新布线直到找到第一个解决方案。

Data Re-use

为选择父节点和重新布线存储碰撞检测的结果

RRT#

RRT*的缺陷

over-exploitation

对无希望的节点重新布线

under-exploitation

• 只有在新节点添加到生成树时才重新布线，并且只在该节点的附件重新布线

• 中间迭代的最优路径不能保证是最优的

Informed Sampling

Informed Set

• 如图(1)所示，深灰色部分为障碍物，浅灰色部分为理想的搜索范围。

• 估计方法表示如下：

  • Bonding boxes

    矩形、立方体形式的边界，如图(2)所示

  • Path heuristics

    在规划好的路径周围形成边界，如图(3)所示

  • $\mathcal{L}2$$\mathcal{L}2$ heuristics

    以椭圆为边界，如图(4)所示

Guided Incremental Local Densification (GuILD)

Local Subsets

如图所示，LSs具有如下特性：

• LSs是IS的子集

• LSs的范围不会超过IS