【学习笔记003】基于采样的路径规划

Preliminary

为什么需要基于采样的方法？

低维空间可以使用栅格 A*、Dijkstra 等算法。但机器人的构型维度可能很高。例如机械臂有10个关节，每个关节离散成100种角度，那么完整栅格可能有：个状态，几乎无法显式构造和搜索。

基于采样的方法不尝试完整建立，而是：

从构型空间中采样一些点；

删除发生碰撞的样本；

尝试连接距离较近的无碰撞样本；

用这些连接组成树或图；

在树或图中寻找从起点到目标的路径。

它获得的不是整个自由空间的精确几何表示，而是自由空间的连通关系近似。

什么是基于采样的规划

配置空间往往维度很高，难以完整、精确地建立出来。基于采样的方法不会枚举整个空间，而是：

在连续的配置空间中选取一些离散样本点；

检查这些点及点之间的连线是否发生碰撞；

将能够连接的点组成树或图；

在这棵树或图上寻找起点到终点的路径。

典型算法包括：

PRM：主要构造路线图；

RRT：从起点逐渐扩展搜索树；

RRT*、PRM*：能够逐渐改善路径质量。

算法性能主要取决于三个方面：

采样策略：在哪里选取样本；

碰撞检测：样本和连线是否与障碍物相交；

邻居搜索：新样本应与哪些已有节点连接。

两个基本任务

探索（Exploration）

探索配置空间中尚未了解的区域，弄清楚哪些区域彼此连通。例如，寻找狭窄通道或者发现能够绕过障碍物的新路线。

利用（Exploitation）

利用已经获得的信息改善当前路径，例如缩短路程、减少转弯或降低能耗。

概率完备性（Probabilistic Completeness）

如果可行路径确实存在，那么随着采样数量趋近于无穷，算法找到该路径的概率趋近于 1：

但这不意味着有限时间内一定能找到，只表示采样越多，失败概率越小。

渐近最优性（Asymptotic Optimality）

随着采样数量不断增加，算法得到的路径代价几乎必然趋近于全局最优值：

其中是当前路径代价，是最优路径代价。路径代价可以是距离、时间、能耗等。

随时可用性（Anytime）

算法能够：

较快找到一条可行但不一定最优的路径；

如果继续运行，就不断优化这条路径；

随时停止时，都可以返回目前找到的最好结果。

可行路径规划方法

Probabilistic Road Map（PRM）

学习阶段

学习阶段用于建立能够反映环境连通关系的路线图。

随机采样

删除碰撞样本

连接邻近节点

检测连接边

查询阶段

当给定起点和终点后，PRM进入查询阶段。

首先，将起点和终点分别连接到路线图中附近且能够无碰撞到达的节点。然后在图上使用A*等算法寻找总代价最小的路径。

优点

可以复用路线图，适合多次路径查询；

不需要显式构造完整的配置空间；

能够用于高维运动规划；

采样足够多时，找到已有可行路径的概率趋近于1，即具有概率完备性。

缺点

建图阶段可能需要大量采样和碰撞检测；

路线图关注整个空间的连通性，不特别关注当前起点和终点，因此可能建立很多与本次路径无关的节点和边；

随机采样不容易覆盖狭窄通道；

环境发生明显变化后，原路线图中的节点和边可能失效，需要更新或重新构建；

基本PRM主要保证找到可行路径，不一定得到最优路径。

提高效率的方案就是Lazy PRM，先不做碰撞检测。只对候选路径做碰撞检测。

Rapidly-exploring Random Trees （RRT）

从起点开始建立一棵树，不断随机选方向并向该方向延伸；只要新延伸的部分不发生碰撞，就加入树中，直到树到达目标区域

随机采样

寻找最近的节点

向随机点延申

碰撞检测

判断是否到达目标。

优点

算法结构简单，容易实现；

不需要建立完整的配置空间模型；

能够处理高维配置空间；

从起点直接向外生长，比PRM更针对当前查询任务；

通常能够比较快地找到一条可行路径；

可以结合机器人动力学，通过控制输入生成新状态。

缺点

基本RRT不保证最优。它找到的通常只是第一条可行路径，路径可能：

比较曲折；

包含很多不必要的转弯；

长度明显大于最短路径。

基本RRT即使运行很久，也不保证路径逐渐收

最优路径规划方法

那么：

其中：

C：图中所有状态或节点的集合；

B(j)：能够直接到达节点 j 的所有前驱节点；

D(i,j)：从节点 i\ 移动到节点 j 的边代价；

f(i)：从起点到前驱节点 i 的最小代价；

f(j)：从起点到节点 j 的最小代价。

它表达的思想非常直观：

到达j的最优路径，一定是先以最优方式到达某个前驱i，再从 i 移动到 j。

动态规划方程给出了“什么样的路径才是最优的”；直接动态规划只有在依赖关系有序、图无环时才能一次算完，而采样式运动规划产生的图通常有环、无固定层次并且不断增长，因此还需要专门的图搜索和代价更新算法。

RRT*

在RRT基础上加入“选择更优父节点”和“重新连接”机制，使路径能够随着采样增加而不断优化。

核心区别可以概括为：

RRT只考虑“能不能把新节点接入树中”；RRT*还考虑“怎样连接总代价更小”，并利用新节点优化附近已有节点。

前三步和RRT一样

随机采样

寻找最近的节点

向随机点延申

之后RRT只检查距离随机点最近的一个节点；RRT*还会寻找周围的一组邻居。圆内的、、都可能成为的父节点。

选择总代价最小的父节点并加入树

重新连接 Rewire。这是RRT*的第二个核心操作。加入后，算法反过来检查附近已有节点。如果通过新节点到达的代价更低。则更换父节点

加速收敛

RRT#

RRT*有两个问题

过度利用（Over-exploitation）：在不可能改善当前解的节点上浪费计算；

利用不足（Under-exploitation）：有价值的代价改善只在局部传播，不能及时改善整条路径。

图1是过度利用，不值得优化的节点也被Rewire。2和3是利用不足，新节点带来一条更短的路径，那么这项改善可能应该继续向目标方向传播。但邻域之外的节点不会立刻被检查，需要等待以后恰好有新样本落在附近，才能继续更新。

RRT#的核心改进

只重点处理有希望的节点

如果一个节点不可能改善当前起点—终点解，就不急着对它进行代价传播，从而减少RRT*的过度利用。

主动传播有价值的代价改善

当某个节点发现更低代价后，RRT#会通过增量图搜索将改善继续传向相关节点，而不是只更新新节点邻域，从而缓解利用不足。

启发式采样

omniscient set（全知集合）：所有“经过它仍有可能产生优于当前最好路径”的状态所组成的集合。

启发式

设当前已经找到的最好路径代价为：

使用欧氏距离估计经过状态 x 的最短路径长度：

对应的知情集合为：

所以在找到第一条可行路径之后就可以informed采样。

义父，请我喝杯蜜雪冰城吧。