一文弄懂基于采样的路径规划-RRT系列（python代码）

基于采样的路径规划算法-RRT系列
VX关注晓理紫并回复rrt获取代码
[晓理紫]

1、基于采样的路径规划算法

基于抽样的规划方法（或称概率方法）通过在连续 C 空间中逐步或批量抽样，构建由离散 C 空间样本连接的树或图，从而捕捉解空间的连通性，进而得出可行或最优解，而无需明确构建解空间。其性能主要取决于采样策略、碰撞检查和邻域搜索。

1.1 概念

• 解连续空间的确定
  • 在连续的勾型空间中采样离散的样本构成树或者图结构来表征连续空间
• 两大基本任务
  • 探索解空间：获取搜索空间拓扑信息，即空间子集的连接方式。（生长路径树或者路径图的过程）
  • 增量式完善解空间：通过处理探索任务计算出的可用信息，逐步完善解决方案。（使路径树或者路径图填满解空间）
• 常用术语
  • 概率完备性（针对可行解，如果有解一定可以找到可行解）：当样本数接近无穷大时，如果存在可行解，则找到可行解的概率为 1。
  • 渐近（接近）最优性：当样本数接近无穷大时，返回解决方案的成本几乎肯定会收敛到最优值。
  • 随时，即时性：快速找到可行但不一定是最优的解决方案，然后随着时间的推移逐步改进。

2、 可行解算法

常用的可行解算法有 Probabilistic Road Map (PRM),Rapidly-exploring Random Tree (RRT)

2.1 PRM

• 什么是 PRM？（得到图结构，渐进的探索解空间的连续性）

  • 探测图形结构中的空间连通性；
  • 精细覆盖自由空间；
  • 将规划分为两个阶段：
    • 学习阶段：如何构建图的过程
    • 查询阶段：

• 学习阶段

用随机点检测 C 空间，并构建表示环境连通性的图形。步骤如下

1、在 C 空间中在随机采用 N 个点

2、删除碰撞的点与碰撞的边

（在空间中采用 N 个点，检查 N 个点是否和障碍物发生碰撞，删除发生碰撞的点，并与相邻的点进行尝试链接。并对链接的边进行碰撞检测，如果边是碰撞的删除。）

删除红色的边

• 查阅阶段

在路线图上搜索，找到从起点到目标的路径（(使用 Dijkstra 算法或 A* 算法）。路线图现在与网格图（或简化网格图）相似。）

• 优缺点

  • 优点

    可进行多次查询（静态地图）

  • 缺点

    在状态空间上构建图，但不特别注重生成路径。

• 提高效率

碰撞检查过程非常耗时，尤其是在复杂或高维环境中。

• 懒惰碰撞检测

  • 在采样点并生成线段期间，并不考虑碰撞（Lazy）。

  • 在不进行碰撞检查的情况下，在生成的路线图上寻找路径。
    

  • 如果路径存在碰撞，则删除相应的边和节点。
    

  • 重启路径查找。直到找到符合条件的路径

2.2 快速探索随机树（RRT）

通过执行随机控制，在树中生成 “next states”，从而建立一棵从起点到目标的树

• 算法步骤
  

  • 1 增量式采样，的到采样点$X_{rand}$

• 2 在已有节点中找到$X_{rand}$的最近邻节点$X_{near}$

• 3 从$X_{near}$节点指向$X_{rand}$方向进行拓展得到$X_{new}$,并连接$X_{near}$与$X_{new}$

• 4 对连接的边进行碰撞检查

• 直到找到目标节点退出(判断节点到目标节点的距离是否小于一定距离，若是小于尝试连接)

• 演示

• 优缺点
  • 优点
    • 易于实施
    • 旨在找到一条从起点到目标的路径
    • 比 PRM 更注重目标
  • 缺点
    • 不是最佳解决方案
    • 效率不高（有待改进）
    • 在整个空间中取样

3、最优路径规划方法

3.1、最优标准

动态程序设计 (DP) 函数方程（离散形式）

$f(j)=mi{n}_{i\in B(j)}\{f(i)+D(i,j)\},j\in C/\{1\}$

$B(j)$:表示 j 的前一个节点, $D(i,j)$:从节点 i 过度到节点 j 的代价, $f(j)$:从起始节点到 j 节点的代价(从起点到某一个节点的最优代价是其前面算有节点的最优代价加上前面节点到此节点代价之和的最小值）

• 例子

DP 功能方程并不构成算法！

3.2、直接 DP 方法

• 只需处理一次，就能确定每个状态的成本价值。
  • 在计算 f(j) 值时，f(i) 的所有相关值必须已经以某种方式计算过了。
  • 图必须是非循环的。

在运动规划中，状态图（或网格）几乎肯定是循环的。
每个状态可以处于任意级别（步长），只能通过范围查询或 k-NN 查询来访问状态。
基于采样的方法在增量或批量生成隐式随机几何图（RGG）时引入了某种随机性。它仍可被视为图搜索。

PRM 是先构建图在去搜索，RRT 是边构建图边搜索

3.3、快速探索随机树* (RRT*)

对于RRT的改进，渐近最优（在条件允许的情况下）

相同的迭代次数花费的时间更少

• 伪代码
  

• 分步解释

  • 获取 x_new 周边 N 个临近节点
    

  • 考虑历史成本，而不仅仅是局部信息，选择父节点

  

  • 连接边

• 调整树结构，提高局部最优性

RRT	RRT*

• 如何确定查询范围

$range=min\{\gamma .(\frac{log(card(V))}{card(V)}{)}^{\frac{1}{d}},\eta \}$

要保证$\gamma >(2(1+\frac{1}{d}{)}^{\frac{1}{d}})(\frac{\mu (X_{free})}{{\xi }_d}{)}^{\frac{1}{d}}$

其中$\eta$:生成距离（步长），$card$，势集合个数，$d$：维度，几维空间。$\gamma$:保证最优性

根据经验，对于低维规划，$\gamma$可以设定一个比步长距离稍大的恒定范围。

• 实际使用技巧

  • 偏差采样：向目标偏置样本（根据目标位置使用类似贪心策略有意向目标方向采样）
  • 剔除样本：剔除 g + h > c* 的样本
  • 分支与边界（树修剪）：修剪不具前景的子树，以降低邻居查询成本。
  • 图形稀疏化：通过解析剔除样本。引入近似最优。
  • 邻居查询：k-nearest 或 range near；近似近邻；不同维度的范围树
  • 延迟碰撞检查：根据潜在的代价值对邻居进行排序。按顺序检查碰撞，一旦发现无碰撞边，即停止检查。
  • 双向搜索
  • 条件重绕：重新布线，直到找到第一个解决方案。
  • 数据重用：存储 ChooseParent 和 Rewire 的碰撞检查结果（仅对对称成本有效）。

4、 加速收敛

4.1 RRT#

• RRT* 的缺陷

  • 过度扩展："重新连接 "无前景的顶点。

• 扩展不足： "重新调整"只在生成树中添加新节点后才会发生，而且只对其 "附近 "节点起作用。它不能保证任何中间迭代的临时路径都是最优的。

• RRT#对 RRT*进行了优化，在一定条件下可以渐近最优

RRT#	RRT*

5、提高采样

5.1、信息集合

信息集合的估计方法

• 边界框
• 路径启发式
• L2 启发式

• 准确度与召回率计算（从上图可以看出 L2 的准确率和召回率都是最好的）

• L2 启发式采样步骤

原理图

• 步骤 1

  • 先随机采样找到起点到目标点的路径 P1

• 步骤 2

  • 以起点与目标点作为椭圆（实际画出来不一定是）两个焦点，路径 P1 长度为长轴长画一个椭圆，在椭圆内部进行扩展连接删除外部的点。直到找到最优解（注意路径 P1 的 cost 要采用欧式距离 L2-Norm 进行估计才行 ）

动画演示

5.2、GuILD 引导式增量局部寻优

• L2 启发式的缺陷

  • 只有当前解较好时椭圆才能起到作用
  • 当环境特别复杂时不一定可以找到最优解
    

• GuILD 方法原理图

• 步骤 1

  • 随机采样找到最优路径 c

• 步骤 2

  • 设置信标点 b，以起点 vs 与 b 点为焦点以起点到 b 点的路径长度 g(b)为长轴进行画椭圆

• 步骤 3

  • 以信标点 b 与目标点 vt 为焦点，最优路径 C 的长度减去 g(b)为长抽画椭圆。一步步收紧直到找到最优解

  

对于信标b的选择可以参考ZJU-FAST-Lab的sampling-based-path-findingi项目中的rrt_sharp.h的beaconSelect函数

3 代码获取方式

VX关注晓理紫并回复rrt获取代码
{晓理紫|小李子}喜分享，也很需要你的支持，喜欢留下痕迹哦！

动画来源于https://github.com/ZJU-FAST-Lab/sampling-based-path-finding