3 基于采样的路径发现

概述

1. 基于采样的规划器（或概率方法）

   • 通过在连续的构型空间（C-space）中增量式或批量地采样离散点，并用这些点构建树（Trees）或图（Graphs），从而捕捉解空间的连通性，以找到一个可行解或最优解。
   • 它们的性能主要取决于采样策略、碰撞检测 和 邻域搜索。
   • 如果把整个路径规划看作一个大的优化问题，那么基于采样就是把大问题拆分成小问题，求解小问题的优化问题。

2. 树结构的生长过程
   

3. 图结构的构建过程
   

4. 两大基本任务

   • 探索 Exploration：获取关于搜索空间拓扑结构的信息，即：空间的子集是如何连接的
   • 利用 Exploitation：通过处理由探索任务计算出的可用信息，逐步改进解决方案

5. 核心理论

   • 概率完备性 Probabilistic Completeness：如果存在可行解，那么随着样本数量趋于无穷大，找到一个可行解的概率趋近于 1
   • 渐近最优性 Asymptotical (Near) Optimality：当样本数量趋于无穷时，返回的解的成本几乎必然收敛到最优解
   • 即时性 Anytime：快速找到一个可行但不一定最优的解，然后随时间逐步改进它

3.1 可行路径规划方法

3.1.1 可行路径图 Probabilistic Road Map (PRM)

1. 什么是 PRM

   • PRM 是一种基于采样的路径规划方法，通过在构型空间中随机采样点，构建一个“道路图”来表示自由空间的连通性。

2. 核心思想

   • 不直接搜索整个连续空间
   • 用随机采样 + 图结构来“摸清地形”

3. 三大特点

   • 探测空间连通性：用图结构表示哪些区域是连通的
   • 精细覆盖自由空间：通过大量采样实现高密度覆盖
   • 分两阶段进行：
     • 学习阶段 Learning Phase：构建道路图
     • 查询阶段 Query Phase：在图上找路径

4. 学习阶段 Learning Phase

   • 目标：用随机点探测 C-space，并构建一个表示环境连通性的图

   • S1：在 C-space 中采样 N 个点
     

     • 每个点代表机器人的一种可能状态
     • 初始时不考虑碰撞，先“撒网”

   • S2：删除碰撞的点
     

     • 只保留自由空间中的点（即不与障碍物相撞）
     • 这些点成为图的顶点（vertices）

   • S3：连接到最近的点，得到无碰撞线段
     

     • 使用 KD-tree 或 Ball tree 加速邻域搜索
     • 仅当两点之间的连线无碰撞时才建立边（edge）

   • S4：删除碰撞的边

     • 即使两个点都安全，它们之间的路径也可能穿过障碍物
     • 必须显式检查每条边是否可行

   • 最终结果：一个由 valid points + collision-free edges 构成的图（road map）

5. 查询阶段 Query Phase

   • 目标：在已构建的道路图上，从起点到终点搜索路径

   • S1：将起点和终点加入图中

     • 如果起点/终点不在图中，需将其作为新节点插入
     • 并尝试连接到附近已有节点

   • S2：在图上运行最短路径算法，如 A* 或 Dijkstra

     • 因为图已经离散化，所以可以使用标准图搜索算法
     • 路径由一系列图上的边组成

   • S3：输出路径
     

     • 将图上的路径转换为连续轨迹（可选插值）

   • 结果：此时的 road map 类似于网格地图，只是更稀疏、更智能地覆盖了自由空间。

6. PRM 的优缺点

   • 优点
     • 支持多次查询：一旦图构建完成，可以在相同地图上反复查找不同起点终点的路径
     • 适合静态环境：地图不变时，学习阶段只需做一次
     • 适用于高维空间：无需离散化整个 C-space
   • 缺点
     • 建图过程不专注于生成路径：可能产生冗余边或未充分连接的区域
     • 对动态环境不友好：地图变化后需要重新建图
     • 预处理时间长：尤其在复杂环境中，采样和碰撞检测耗时

7. 提升效率：Lazy Collision Checking

   • 问题：碰撞检测非常耗时，尤其是在高维或复杂环境中

   • 解决方案：Lazy Collision Checking（延迟碰撞检测）

   • 原理：先不检查碰撞，只生成候选边；只有在搜索过程中需要用到某条边时，才去检查它是否碰撞。

   • 步骤
     

     • 采样点并生成边时不检查碰撞（Lazy）
     • 在查询阶段，当搜索到某条边时，再进行碰撞检测，如果发生碰撞就删除相应的节点和边
     • 重新搜索路径

3.1.2 快速搜索随机树 Rapidly-exporing Random Trees

1. 什么是 RRT

   • RRT 是一种基于采样的路径规划算法，通过在构型空间中随机生成点，并逐步构建一棵从起点到终点的树来探索空间

2. 核心思想

   • 不像 PRM 那样先建图再搜索
   • 而是一边扩展树，一边向目标靠近

3. 算法伪代码
   

4. 步骤

   • 在 free space 中随机采样一个点 Xrand
     

   • 在当前树 T 中，找离 Xrand 最近的节点 Xnear
     

   • 从 Xnear 向 Xrand 移动一小段距离，并生成新节点 Xnew
     

   • 检测从 Xnear 到 Xnew 的线段是否与障碍物相交，若无碰撞，则加入树中
     

   • 重复采样，知道树抵达目标点或目标区域
     

5. 优缺点

   • 优点
     • 代码简单，只需几个函数：Sample, Near, Steer, CollisionFree
     • 目标导向：每次扩展都朝目标方向偏移（Goal Biasing）
     • PRM 是“全局建图”，RRT 是“局部探索”，更高效地逼近目标
   • 缺点
     • 找到的路径通常不是最短或最优的（可能绕远路）
     • 树可能长得“歪七扭八”，很多冗余分支
     • 随机采样可能导致大量无效探索（尤其在狭窄通道附近）

6. RRT 与 PRM 的对比
   

7. 可视化：https://github.com/ZJU-FAST-Lab/sampling-based-path-finding

3.2 最优路径规划方法

3.2.1 最优性准则 Optimal Criterion

1. 动态规划的最优性准则

   • 核心思想：从任意状态 $j$ 出发的最优代价，等于所有前驱状态 $i$ 中，$f(i)+D(i,j)$ 的最小值

   • 数学表达：
     f ( j ) = min ⁡ i ∈ B ( j ) { f ( i ) + D ( i , j ) } , j ∈ C f(j) = \min_{i\in B(j)}\{ f(i) + D(i,j) \}, j\in C f(j)=i∈B(j)min​{f(i)+D(i,j)},j∈C
     其中

     • $f(i)$：从起点到状态 $j$ 的最小代价
     • $B(j)$：状态 $j$ 的前驱集合（即能到达 $j$ 的所有状态）
     • $D(i,j)$：从状态 $i$ 到 $j$ 的转移代价

2. 注意：这不是算法！只是方程

   • 它只定义了“最优解应满足什么条件”
   • 但没有说明“如何求解”这个方程

3. 直接 DP 方法（Direct DP Methods）

   • 目标：将 DP 方程转化为可执行的算法

   • 初始化：
     $$\begin{gathered} F(1)=0 \\ F(j)=\infty ,\forall j>1 \end{gathered}$$

   • 迭代：对每个 $j=2,\cdots ,n$
     F ( j ) = min ⁡ i ∈ B ( j ) { F ( i ) + D ( i , j ) } F(j)=\min_{i\in B(j)}\{ F(i) + D(i,j) \} F(j)=i∈B(j)min​{F(i)+D(i,j)}

   • 优点

     • 每个状态只处理一次
     • 时间复杂度：O(n²)，对小图非常高效

   • 严格要求

     • 图必须是无环的（acyclic）
     • 状态必须按拓扑顺序处理（即先处理前驱，再处理后继）

4. 为什么 DP 不能用于机器人路径规划？

   • 核心问题：机器人状态图几乎总是有环
   • 状态图是循环的：在机械臂中，旋转 360° 后回到相同构型
   • 无法确定处理顺序：如果 A → B → C → A，那么谁先谁后？
   • 状态访问方式受限：在真实机器人中，你不能“按顺序遍历所有状态”，只能通过 range queries 或 k-NN queries 访问邻居

5. 采样方法如何解决这个问题？

   • 采样法的本质：隐式构建一个 RGG（Random Geometric Graph）
     • 随机生成点（样本）
     • 构建连接关系（边）
     • 在这个隐式的图上进行搜索
   • 优势
     • 不需要拓扑排序
     • 可以处理有环图
     • 支持高维连续空间

3.2.2 RRT*

1. 什么是 RRT*

   • RRT* 是 RRT 的最优性增强版
   • 它在保持 RRT 的探索能力的同时，实现了渐近最优性（Asymptotic Optimality）

2. 关键特性

   • 概率完备：如果存在路径，最终一定能找到
   • 渐近最优：随着采样次数增加，路径代价趋于全局最优
   • 适用于高维连续空间：如机械臂、无人机

3. RRT 与 RRT 的对比*
   

   • 红色线（RRT）：路径代价稳定在一个较高值 → 不最优

   • 蓝色线（RRT）：路径代价随迭代不断下降 → 最终逼近最优
     

   • RRT 的树结构：早期 - 稀疏树；中期 - 转向目标；后期 - 分支多但不优化；终局 - 多条冗余路径

   • RRT*的树结构：早期 - 稀疏树；中期 - 开始重连；后期 - 树开始“收缩”；终局 - 单条最短路径

4. RRT 伪代码*
   

   T.init();  # 初始化树，包含起点 x_init
   
   for i = 1 to n do:
       x_rand ← Sample(M);           # 1. 随机采样
       x_near ← Near(x_rand, T);     # 2. 找最近邻
       x_new ← Steer(x_rand, x_near, StepSize);  # 3. 控制执行
       if CollisionFree(M, x_new):   # 4. 检查碰撞
           X_near ← Near(x_new, T);  # 5. 找 x_new 的邻域
           x_min ← ChooseParent(X_near, x_near, x_new);  # 6. 选最优父节点
           T.addNode(x_new);         # 7. 添加节点
           T.addEdge(x_min, x_new);  # 8. 添加边
           T.rewire(X_near, x_new);  # 9. 重连邻居（关键！）
   

5. 步骤详解

   • S1：随机采样一个 Xrand，找出最近的点 Xnear，沿该方向走一步到 Xnew
     

   • S2：考虑 N 个靠近的点
     

   • S3：选择代价最小的父节点（考虑历史代价而不仅仅是局部信息）
     

   • S4：连接父节点，生成边
     

   • S5：重连接 Rewire，实现局部优化，让邻居也受益于新点
     

6. 可视化：https://github.com/ZJU-FAST-Lab/sampling-based-path-finding

7. 如何保证渐近最优性 AO

   • 原论文中查询范围 Query Range 的设计
     $$\mathrm{range}=\min \left\{\gamma \cdot {\left(\frac{\log (\mathrm{card}(V))}{\mathrm{card}(V)}\right)}^{1/d},\eta \right\}$$
     其中

     • $d$：状态空间维度，如2D 空间中 $d=2$
     • $\mathrm{card}(V)$：当前节点数
     • $\gamma$，$\eta$：参数，通常设定为常数

   • 保证 AO 的条件
     $$\gamma >{\left(2(1+\frac{1}{d})\right)}^{1/d}{\left(\frac{\mu (X_{tree})}{{\xi }_d}\right)}^{1/d}$$
     这个公式确保了：随着采样增多，搜索范围合理缩小，避免过度扩展

   • 工程经验：对于低维规划（如 2D/3D），可以设置一个略大于 step size 的固定 range，效果很好。

8. 实用实现技巧

   • 偏置采样 Bias Sampling
     • 以一定概率（如 5%）直接采样目标点 $x_{goal}$
     • 加速收敛到目标区域
   • 样本拒绝 Sample Rejection
     • 拒绝那些 $g+h>c^{\ast }$ 的样本，$c^{\ast }$是当前最优代价
     • 避免探索明显劣解区域
   • 树剪枝 Branch-and-bound
     • 剪掉那些不可能成为最优路径一部分的子树
     • 减少邻居查询成本
   • 图稀疏化 Graph Sparsify
     • 按分辨率拒绝样本（如只保留每 0.1m 一个点）
     • 引入“近似最优”，提升效率
   • 邻居查询 Neighbor Query
     • 使用 k-NN 或 range tree 加速查找
     • 对不同维度使用不同的数据结构，如 KD-tree for 2D/3D
   • 延迟碰撞检测 Delay Collision Check
     • 将邻居按“潜在代价”排序
     • 优先检查可能更优的边
     • 一旦找到无碰撞边就停止 -> 提升效率
   • 双向搜索 Bi-directional Search
     • 从起点和终点同时扩展树
     • 相遇时连接 -> 更快找到路径
   • 条件重连 Conditional Rewire
     • 只有在找到第一条路径之后才开始 rewiring
     • 避免早期浪费计算资源
   • 数据复用 Data Re-use
     • 缓存碰撞检测结果
     • 对称代价下，chooseParent 和 Rewire 可共享结果

3.3 加速收敛

3.3.1 RRT#

1. RRT 算法的两大核心缺陷

   • 过度利用 Over-exploitation
     

     • RRT 在每次添加新节点后，都会对其邻域内所有节点进行 rewire 操作
     • 即使某些节点早已被证明是“非最优”的（如远离目标的分支），仍会被反复重连
     • 图中新点 new 加入，RRT 会检查整个虚线圆内的所有节点，但是途径新点的路线的代价一定大于目前的最优代价，新点不可能成为最优路径的一部分

   • 利用不足 Under-exploitation
     

     • RRT 的 rewiring 只在新增节点之后发生，且只作用于该节点的“near”邻居
     • 导致：早期找到的路径可能长期得不到优化
     • 图中新点的加入只会使得圆圈内的节点重连接，但是虚线对应的连接也明显能降低代价

2. RRT#

   • 设计动机：平衡探索与利用
   • 核心思想：不要盲目地对每个新节点都做全局 rewiring，而是智能地选择何时、何地进行优化。
   • 三大改进
     • 延迟 rewiring：不在每步都 rewire，而是在发现更好路径时才触发
     • 局部优化：只优化当前最有潜力的区域，避免浪费资源
     • 动态调整：根据采样分布和路径质量动态决定是否需要优化

3. RRT# 与 RRT 的对比*
   

   • 红色线（RRT）：代价下降更快，收敛更早
   • 蓝色线（RRT）：初期较慢，后期逐渐逼近最优
     

4. RRT# 的简化工作机制

   for i = 1 to n do:
       x_rand ← Sample(M)
       x_near ← Near(x_rand, T)
       x_new ← Steer(x_rand, x_near, StepSize)
   
       if CollisionFree(x_new):
           X_near ← Near(x_new, T)
           x_min ← ChooseParent(X_near, x_near, x_new)
           T.addNode(x_new)
           T.addEdge(x_min, x_new)
   
           # 关键区别：不立即 rewire！
           # 等待“机会”再优化
   

5. RRT# 如何实现“智能优化”

   • 引入 “cost-to-go” 估计
     • 维护一个启发式函数 $h(x)$，估计从 $x$ 到目标的最小代价
     • 如果某个节点的 $\mathrm{text}(x)+h(x)>c^{\ast }$，则标记为“无希望”
   • 按需 rewiring
     • 当发现一条比当前最优更短的路径时，回溯并重新优化相关区域
     • 类似“局部搜索+重启策略”
   • 优先级队列管理
     • 使用优先级队列存储“潜在可优化”的节点
     • 按照“改进潜力”排序处理

3.3.2 知情采样 Informed Sampling

1. 什么是 Informed Sampling

   • 利用已知最优代价信息，引导采样集中在“可能更优”的区域
   • 核心目标
     • 避免在无效区域浪费计算资源
     • 加速收敛到全局最优解
   • 关键前提
     • 已经找到一条可行路径，其代价为 $c_{best}$
     • 理论上，任何比它更优的路径必须满足 $\mathrm{cost}<c_{best}$

2. 知情集 Informed Set

   • 定义

     • Informed Set 是对“全知集合（Omniscient Set）”的估计
     • 即：所有可能产生优于当前最优路径的构型的集合

   • 三种常见估计方法
     

     • 包围盒 Bounding Box：以起点和终点为对角线，构造一个轴对齐矩形，只在该矩形内采样
     • 路径启发式 Path Heuristic：使用启发函数 $h(x)$ 预测从 $x$ 到目标的最小代价，构造一个“路径偏置区域”，优先采样靠近理想路径的点
     • L2 启发式 L2 Heuristic：假设最优路径长度不超过 $c_{best}$，构造一个椭圆区域，使得任意两点间的欧氏距离 $\le c_{best}$

3. L2 Informed Set 的数学原理
   

   • 椭圆方程

     • 给定：起点 $x_{start}$，终点 $x_{goal}$，当前最优代价 $c_{best}$，两点间最短距离 $d=\Vert x_{start}-x_{goal}\Vert$

     • 则 L2 知情集是一个以起点和终点为焦点的椭圆，长轴为 $c_{best}$
       $$\Vert x-x_{start}\Vert +\Vert x-x_{goal}\Vert \le c_{best}$$

   • 几何意义

     • 椭圆内的任一点 $x$，都存在一条经过它的路径，总长度 $\le c_{best}$
     • 椭圆外的点不可能出现在更优路径中

4. 如何在 L2 Informed Set 中采样
   

   • 方法1：直接采样 Direct Sampling

     • 在椭圆区域内直接生成随机点
     • 使用 Rejection Sampling 或 Transformed Sampling

   • 方法2：拒绝采样 Reject Sampling

     • 先在包围矩形中采样
     • 若不在椭圆内，则拒绝并重试

   • 对比
     

     • 随着维度增加，椭圆体积占比急剧下降
     • 直接采样（绿色）比拒绝采样（蓝色）快得多
     • 在高维下，拒绝采样几乎不可行

5. 局限性：只能用于 L2 范数 cost

   • Informed RRT 的核心依赖于 L2 范数（欧氏距离） 来构造椭圆
   • 如果代价不是欧氏距离（如时间、能耗、曲率），就无法定义类似的几何区域

3.3.3 GuILD (Guided Incremental Local Densification)

1. GuILD 智能局部加密采样

   • 核心思想是：即使没有找到更短的全局路径，也可以通过“局部改进”来提升采样效率。

2. 不理想的知情集
   

   • 左图：狭窄通道，难以进入；右图：直角转弯，易于进入
   • 问题：Informed Set（椭圆）对所有情况都一样大
     • 在左图中，椭圆太大，浪费资源
     • 在右图中，椭圆太小，错过关键区域
   • GuILD 解决方案：动态调整局部采样区域，适应环境结构

3. 局部子集（Local Subsets）：GuILD 的核心机制
   

   • 定义：Local Subset（LS） 是一个比 Informed Set 更小、更聚焦的采样区域，由以下三个要素定义：
     • Beacon Node $b$：当前搜索树中的某个节点（如一个关键转折点）
     • Cost-to-come $g(b)$：从起点到 $b$ 的代价
     • Current best solution cost $c(\xi )$：当前已知最优路径总代价
   • 构造方式：以 $b$ 为灯塔，构造两个椭圆
     • Start-beacon set：从起点到 $b$ 的可能路径区域
     • $Beacon-targetset$：从 $b$ 到目标的可能路径区域
     • 总预算为 $c(\xi )-g(b)$
     • 这些椭圆是 Informed Set 的子集，因此不会扩大搜索范围

4. DuILD 的工作流程

   for i = 1 to n do:
       if found_new_shorter_path_to_some_vertex:
           // 更新 beacon node b 的 g(b)
           // 重新计算 local subsets
           update_local_subsets()
       
       x_rand ← sample_in_local_subset()  // 关键！只在 LS 内采样
       ...
   

5. 逐步演化过程
   

   • 全局的 Informed Set（知情集）保持不变。然而，GuILD 利用搜索树中已被改善的“到达代价”（cost-to-come），来更新局部子集（Local Subsets）。
   • 起点–信标集合（start-beacon set）会缩小，以进一步聚焦采样；而信标与目标之间的“代价余量”（slack）则被用来扩展信标–目标集合（beacon-target set）。
   • 这意味着，在信标与目标之间可以使用更长的路径长度预算，但仍有可能得到一条整体更短的路径**。**

6. GuILD 的优势

   • 动态聚焦：随着树优化，采样区域自动缩小并转移
   • 避免无效探索：不再浪费时间在已知不可能的区域
   • 提升收敛速度：更快找到最优解

3.3.4 发展趋势

1. 采样策略 Sampling Strategy
   • Deterministic/Random：经典方法，均匀或随机采样
   • Batch/Incremental：分批 vs 实时扩展（RRT 是增量式）
   • Informed sampling：使用启发信息缩小搜索空间（如 Informed RRT）
   • Learned sampling：用神经网络预测“有希望”的区域（前沿研究）
2. Hybrid Exploration - Exploitation Scheme
   • 结合 探索（exploration） 和 利用（exploitation）
   • 例如：先用 RRT 快速探索，再用 A 或 CHOMP 优化
3. Apply to Kinodynamic Systems
   • 考虑动力学约束（如最大加速度、转向半径）
   • 如无人机、自动驾驶车辆
   • 传统 RRT 需要修改为 RRT-Dynamics 或 BIT

化，采样区域自动缩小并转移

• 避免无效探索：不再浪费时间在已知不可能的区域
• 提升收敛速度：更快找到最优解

3.3.4 发展趋势

1. 采样策略 Sampling Strategy
   • Deterministic/Random：经典方法，均匀或随机采样
   • Batch/Incremental：分批 vs 实时扩展（RRT 是增量式）
   • Informed sampling：使用启发信息缩小搜索空间（如 Informed RRT）
   • Learned sampling：用神经网络预测“有希望”的区域（前沿研究）
2. Hybrid Exploration - Exploitation Scheme
   • 结合 探索（exploration） 和 利用（exploitation）
   • 例如：先用 RRT 快速探索，再用 A 或 CHOMP 优化
3. Apply to Kinodynamic Systems
   • 考虑动力学约束（如最大加速度、转向半径）
   • 如无人机、自动驾驶车辆
   • 传统 RRT 需要修改为 RRT-Dynamics 或 BIT