增量采样与搜索：采样式运动规划算法解析

### 增量采样与搜索：采样式运动规划算法解析 #### 1. 路径碰撞检测与范德科普序列 在路径规划中，若路径存在碰撞情况，采用特定的采样方法往往能节省时间。这其实是一个在区间 $[0, 1]$ 上的采样问题，而范德科普序列（van der Corput sequence）能很好地解决该问题。在路径检查过程中，首先检查 $\tau(1)$，随后按范德科普序列依次检查 $\tau(0), \tau(1/2), \tau(1/4), \tau(3/4), \tau(1/8), \cdots$ 等点。若检测到碰撞或者离散度低于 $\Delta q$，则终止该过程。若 $\Delta q$ 并非恒定值，在 $q$ 允许变化范围较大的区域，可跳过序列中的部分点。 #### 2. 单查询模型下的采样式规划算法框架 单查询模型意味着对于每个机器人和障碍物集合，仅给定一次起始配置 $q_I$ 和目标配置 $q_G$，此时预计算并无优势，采样式运动规划问题可视为一种搜索问题。多数单查询、采样式规划算法遵循以下通用框架： 1. **初始化**：定义一个无向搜索图 $G(V, E)$，其中 $V$ 至少包含一个顶点，$E$ 初始为空。通常，$V$ 包含 $q_I$、$q_G$ 或两者皆有，也可包含 $C_{free}$ 中的其他点。 2. **顶点选择方法（VSM）**：从 $V$ 中选择一个待扩展的顶点 $q_{cur}$。 3. **局部规划方法（LPM）**：尝试为某个可能不在 $V$ 中的 $q_{new} \in C_{free}$ 构建一条路径 $\tau_s : [0, 1] \to C_{free}$，使得 $\tau(0) = q_{cur}$ 且 $\tau(1) = q_{new}$。需运用相关方法检查 $\tau_s$ 是否会导致碰撞，若无法生成无碰撞的路径段，则返回步骤 2。 4. **插入边**：将 $\tau_s$ 作为从 $q_{cur}$ 到 $q_{new}$ 的边插入到 $E$ 中。若 $q_{new}$ 不在 $V$ 中，则将其插入。 5. **检查解**：判断 $G$ 是否编码了一条解路径。若只有一棵搜索树，此过程较为简单；否则，可能会变得复杂且耗时。 6. **返回步骤 2**：若未找到解或未满足终止条件，则继续迭代；否则，算法报告失败。 在这个框架中，$G$ 是一个拓扑图，每个顶点代表一个配置，每条边代表连接两个配置的路径。通过改变步骤 2 和步骤 3 的实现方式，可得到一大类采样式算法。其中，VSM 的作用类似于优先队列，而 LPM 用于计算可添加到图中的无碰撞路径段，因其通常生成简单且短距离的路径，故称为局部规划。 #### 3. 基于搜索树数量的算法分类 根据搜索树的数量，采样式规划算法可分为以下几类： - **单向（单树）方法**：规划过程与离散前向搜索类似，不同算法的主要区别在于 VSM 和 LPM 的实现方式。在某些高维“陷阱”问题中，前向搜索算法可能会陷入困境，而具有贪心、最佳优先行为的反向搜索可能更易解决此类问题。 - **双向（双树）方法**：由于无法确定 $q_I$ 或 $q_G$ 是否位于“陷阱”区域，双向搜索通常更为可取。其原理是两个分别以 $q_I$ 和 $q_G$ 为中心的传播波前相遇时所覆盖的面积，小于以 $q_I$ 为中心的单个波前到达 $q_G$ 所覆盖的面积。通过 VSM 在两棵树之间交替选择顶点，LPM 有时用于探索 $C_{free}$ 的新区域，有时用于连接两棵树。 - **多向（多树）方法**：当问题存在双重“陷阱”时，可从其他位置生长搜索树，以增加从其他方向进入“陷阱”的机会。然而，这会使树的连接问题变得复杂，需要考虑选择哪些树对进行连接、选择频率以及顶点对的选择等诸多启发式参数。 以下是这三种方法的对比表格： | 方法类型 | 特点 | 适用场景 | 缺点 | | ---- | ---- | ---- | ---- | | 单向（单树）方法 | 类似离散前向搜索，区别在于 VSM 和 LPM 实现 | 简单场景 | 易陷入高维“陷阱” | | 双向（双树）方法 | 两个波前传播，VSM 交替选点，LPM 探索与连接结合 | 不确定起始或目标是否在“陷阱”的场景 | - | | 多向（多树）方法 | 从多个位置生长树 | 存在双重“陷阱”的复杂场景 | 树连接问题复杂，需大量启发式参数 | #### 4. 离散搜索算法的适配 将离散搜索算法应用于采样式规划的一种简便方法是在 $C$ 上定义网格，然后使用相关离散搜索算法进行搜索。具体步骤如下： 1. **离散化**：将 $n$ 维的 $C$ 空间表示为单位立方体 $C = [0, 1]^n / \sim$，其中 $\sim$ 表示根据 $C$ 空间拓扑对立方体各边进行标识。使用分辨率 $k_1, k_2, \cdots, k_n$ 对 $C$ 进行离散化，可采用标准网格或苏哈列夫网格。定义 $\Delta q_i = [0 \cdots 0 1/k_i 0 \cdots 0]$，网格点 $q$ 可表示为 $\sum_{i=1}^{n} j_i\Delta q_i$，其中 $j_i \in \{0, 1, \cdots, k_i\}$。 2. **邻域定义**：对于非边界网格点 $q$，其 1 - 邻域定义为 $N_1(q) = \{q + \Delta q_1, \cdots, q + \Delta q_n, q - \Delta q_1, \cdots, q - \Delta q_n\}$；2 - 邻域定义为 $N_2(q) = \{q \pm \Delta q_i \pm \Delta q_j | 1 \leq i, j \leq n, i \neq j\} \cup N_1(q)$。对于边界网格点，需特殊处理以确保符合 $C$