实时启发式路径采样

实时启发式路径采样用于运动规划搜索

罗斯·A·克内普尔 和 马修·T·梅森

摘要

机器人运动通常源于一种无信息的路径采样过程，例如随机采样或低离散度采样。我们提出了一种替代性的路径采样方法，该方法将昂贵的碰撞检测过程形成闭环。尽管规划器已掌握进行路径碰撞检测所需的全部信息，但这些信息通常以相对不可访问的形式存储在代价地图中。通过以更易访问的形式对最显著的数据进行汇总，我们的方法在单位时间内实现了比开环采样技术更密集的自由空间采样。我们通过基于碰撞测试结果，在实时且仅使用最少信息的情况下对障碍物位置进行概率建模，从而实现这一结果。实验表明，通过碰撞检测的路径数量最多提升了780%。

1 引言

运动规划问题的目标是找到一条路径或轨迹，引导机器人从给定的起始状态到达给定的目标状态，同时满足约束条件并避开障碍物。由于解空间是高维的，运动规划算法通常通过搜索一系列较短的局部路径来分解问题，这些局部路径连接后可解决原始的运动规划问题。

构成该连接解的每条局部路径都必须遵守运动约束，并避开环境中的障碍物和危险。对于每个组件，可以考虑多种不同的局部路径，因此规划器会选择能够优化某个目标函数的路径组合。为了生成这样一组候选路径，规划器必须生成更多的候选路径，每条路径都必须在考虑之前验证其运动约束并进行碰撞检测。运动规划器通过采样——通常为随机采样或从低分散序列中采样——来生成大量路径。

所有用于寻找无碰撞路径样本的信息都存在于代价地图中，但昂贵的碰撞检测过程使得这些信息无法被规划器直接利用。负碰撞测试结果（即无碰撞）会被保留以供后续参考，但正碰撞测试结果通常被丢弃，因为该路径无法执行。此类规划器之后可能会浪费时间对与同一障碍物发生碰撞的类似路径进行采样和测试。

这种丢弃碰撞路径信息的策略凸显了一个主要的低效问题，尤其影响实时规划。每次检测到的碰撞都提供了一个已知障碍物的位置。这一观察起初可能看似不重要，因为所有检测到的碰撞在代价地图中都代表已知障碍物。然而，并非所有此类障碍物对给定的局部规划问题都具有同等的相关性，因此我们可以通过将相关的代价地图障碍物以更直接可供规划器使用的形式存储而获益。我们认为，规划器可以通过将先前碰撞检测中已知的碰撞点集合反馈给路径采样过程来提升性能，如图1。

1.1 路径采样与路径参数化

一般的路径采样问题是提供一系列不同的路径 {p₁, p₂, …} = P ⊂ X，即路径的连续空间。通常，这些路径不是直接由其几何形状参数化，而是通过其生成方式来描述。例如，一些规划器仅考虑直线路径。给定当前机器人状态 x₀ ∈ X（构型空间），一条直线路径通过连接 x₀ 到任意采样的状态 x_f ∈ X 唯一确定。在这类规划器中，假设机器人能够执行任意路径，因此边值问题容易求解，因为它是欠约束的。

图1 典型的机器人数据流在感知‐规划‐执行循环中形成闭环，但规划器本身以开环方式运行。我们通过将先前路径的碰撞检测结果用于指导路径采样，从而实现规划过程的闭环

定义 1 给定起始状态和目标状态，边界值问题（BVP）是寻找从起点到目标点的任意可行路径（即局部规划问题）。该问题的一个变体是寻找最短的此类路径。 □

某些类别的机器人系统具有速度约束，限制了它们瞬时移动的方向。这类非完整约束最著名的例子是汽车平行停车的困难。对于这种高度受限的欠驱动系统，可行路径集 F 远小于所有路径的空间 X。因此，从 X 中任意抽取的路径样本不太可能属于可行路径集 F。在这种情况下，边值问题难以求解。

对于受约束的系统，我们可以通过在动作空间 U 中进行采样来避免求解边值问题。假设我们有一个关于机器人对控制输入响应的“黑箱”模型，该模型是一个映射 M: U → F。在控制空间中采样具有多个优点：（1）所有采样的路径天然满足运动约束；（2）我们可以预先计算出大量多样化的此类路径。对于移动机器人而言，这些路径与初始位置和航向无关，仅依赖于其导数（我们忽略风或轮子打滑等外部干扰）。因此，一个相对较小的查找表就足以描述一组表达能力强的机器人运动。

1.2 相关工作

运动规划领域在非均匀采样和自适应采样方面投入了大量研究 effort。该主题的文献主要涉及概率路线图（PRMs），其对状态进行采样而非路径。

Hsu 等人[11]综述了针对 PRM 的非均匀采样的近期工作。我们在此简要介绍其中一些主要方法。

一种方法采用固定策略，将构型采样偏向于狭窄通道——由于测度较小而难以被自主采样的构型空间部分[1, 9, 17]。这些研究均将采样考虑限制在点上，而我们则直接在路径空间中进行采样，并使用根据新的碰撞检测结果动态调整的分布。

非均匀采样领域已大体转向此类自适应策略。例如，Jaillet 等人[12]将采样限制在 RRT 节点周围的大小可变球体内，以避免测试会穿过障碍物的路径。Yu 和 Gupta [19]执行基于传感器的规划，根据机器人对障碍物的部分观测逐步构建概率路线图。探索性运动通过最大化信息增益来选择。

另一种近期的自适应方法是构建一个具备多种工具的元规划器；这类规划器根据预测在特定环境下最有效的方法，采用多种采样策略[10]，或多组随机路线图规划器[16]。

我们工作的其中一个关键特征是利用所有碰撞检测的结果（包括阳性结果）来最小化用于近似障碍物位置的模型中的熵。在这方面，伯恩斯和布罗克的工作 [3–6] 与我们的工作具有相当大的相似性。他们描述了一种基于先前碰撞测试结果的构型空间中障碍物位置的自适应模型。该模型利用局部加权学习来选择能够降低模型不确定性的状态 [4] 或路径 [5] 样本。我们同样建立了障碍物位置的模型，尽管我们的模型位于工作空间中，且其简洁性更适用于实时应用。如同我们一样，伯恩斯和布罗克随后也指出，模型细化本身并非最终目的，而仅仅是寻找无碰撞路径 [6] 的手段。我们在此观察基础上进一步思考，在受限路径的背景下，根据我们愿意错过的走廊最大宽度，何种程度的模型细化是适当的。

此外，伯恩斯和布罗克描述了一种基于熵的引导方法，用于选择可能连接PRM图中两个连通分量的构型样本[3]。在后续的工作[6]中，他们引入了效用的概念来增强该方法，该概念将关于障碍物位置的信息增益与生成样本对解决规划问题的价值结合起来。基于效用引导的采样选择预期能够最高效率地解决最终规划问题的构型。我们采用了类似的方法，即采样一组路径，旨在导航空间并优化我们的障碍物模型。

2 有信息路径采样方法

在闭环路径采样过程中，我们必须将机器人可到达的障碍物信息（以碰撞检测结果的形式）反馈到路径的采样空间（无论是X还是U），以便抑制后续采样路径序列中与工作空间这些区域相交的路径。

碰撞可以描述为一个有序对 c = (p, s)，其中 p ∈ F 且 s ∈ I = [0, 1]，是一个时间/距离参数，用于描述碰撞发生在路径上的位置。路径是一个映射 p: I → X。因此，c 直接映射到一个状态 x ∈ X，标识障碍物的位置。然而，该碰撞状态是特殊的，因为它可通过一个动作 u ∈ U 到达。事实上，x 很可能可通过其他连续动作到达，我们可以为每个可能的碰撞点轻松预计算这些动作的一个采样子集。

我们针对高度受限系统中的路径采样反馈方法是将反馈完全保留在对其进行参数化的动作空间内。首先，从低分散序列[7]中抽取若干路径进行碰撞检测。在找到第一个碰撞点后，该碰撞点的位置将影响未来的动作采样。我们可以先验地构建一个可能的动作采样与对应关系之间的映射列表，以加速此过程。

通常，碰撞检测例程能够识别出发生碰撞的精确位置。一旦知道工作空间点w属于障碍物，我们就可以从采样序列中剔除所有经过cs(w)的路径，即剔除机器人构型xi ∈ X中机器人与工作空间点w相交的所有路径。为了消除这些路径，我们必须存储用于参数化这些路径的动作列表。

为了识别出经过障碍物点 w 附近且距离过短的路径集合，我们预计算了一个邻近查找表（PLUT），如图2所示。假设我们预计算的路径集 P 包含 N条路径，每条路径离散化为M个点。对于 P 中的每一对有序路径（pi, pj），PLUT存储了到 pj 上第 k个离散点的最短距离。

$$
PLUT(p_i, p_j, k) = \min_{w \in p_i} d\left(w, p_j\left(\frac{k}{M}\right)\right),
\tag{1}
$$

其中 d(w₁, w₂) 表示两点之间的欧几里得距离。

图2 给定一个包含N条路径的路径集，每条路径离散化为M个点，邻近查找表（PLUT）为每一对有序路径存储到第二条路径上各离散点的最短距离列表。因此，总共有MN²个唯一的PLUT条目

现在，给定一个碰撞 c = (pj, s)，我们希望确定另一条路径 pi 是否会与同一障碍物发生碰撞。我们只需向 PLUT 发起查询即可

$$
PLUT(p_i, p_j, sM) < r_r,
\tag{2}
$$

其中，rr是机器人（或机器人的内切圆）的半径。当此条件成立时，碰撞检测将失败。了解这一点后，我们可以在不进行测试的情况下直接排除该路径，并将CPU时间用于考虑其他路径。

然而，我们可以超越对碰撞检测的短路优化，利用局部性原理来估计障碍物位置的概率分布，该原理指出，障碍物内部的点往往出现在其他障碍物内部点的附近。我们提出了一系列局部性模型以及两个路径采样问题，并使用我们的模型加以解决。该方法成功的关键在于，其最终评估的成本低于所替代的碰撞检测。

3 概率基础

本文中，我们开发了一系列概率模型，使我们能够快速选择用于碰撞检测的路径，以最大化以下两个属性之一。首先，为了找到有效的机器人运动，我们必须对一些无碰撞路径进行采样以供执行。其次，我们希望在路径的自由空间内广泛采样，包括靠近障碍物的区域。我们对障碍物/自由空间边界的认知精度直接关系到我们期望发现的狭窄通道的尺寸。

工作空间由一组点组成，这些点分为两类：障碍物和自由。该函数

$$
obs: W \rightarrow b,
\tag{3}
$$

其中 b = {true, false} 表示某个工作空间点 w 位于障碍物内部（true）或外部（false）的测试结果。基于此类结果，我们随后描述一个感兴趣的事件。路径碰撞检测的形式为

$$
pct: P \rightarrow b,
\tag{4}
$$

返回路径的扫掠体积（或轨迹区域）内所有 w 的 obs(w) 的析取。结果为 true 表示该路径与障碍物相交。

基于这些概念，我们提出两个问题：

1. 利用 。给定工作空间内位于障碍物中的一组点 $ C = {w_1, …, w_m} $，以及一组未测试路径 $ P_{unknown} = {p_1, …, p_n} $。在仅知道连续障碍物点集中有限子集的情况下，寻找使碰撞概率最小的路径：

$$
p_{next} = \arg\min_{p_i \in P} Pr(pct(p_i, C)).
\tag{5}
$$

2. 探索 。假设我们有一个关于未测试路径集P_safe的碰撞状态的不确定性模型 U(P_unknown, C)，该模型基于已测试路径和已知障碍点集。寻找一条路径 $ p_{next} \in P_{unknown} $，使其带来期望不确定性最大程度的降低：

$$
U_{exp}(p_i) = U(P_{safe} \cup {p_i}, C) \cdot Pr(\neg pct(p_i, C)) + U(P_{safe}, C \cup {c_i}) \cdot Pr(pct(p_i, C))
\tag{6}
$$

$$
p_{next} = \arg\max_{p_i \in P_{unknown}} U(P_{safe}, C) - U_{exp}(p_i):
\tag{7}
$$

这两个考虑因素本质上与伯恩斯和布罗克效用函数中所包含的相同 [6]（见 第1.2节）。

4 局部性

到目前为止，我们已经展示了单次失败的碰撞检测如何能够排除整组未经测试的路径，因为这些路径会经过同一个障碍物点。我们可以进一步利用局部性原理将这种方法再推进一步。

定义 2 局部性原理指出，如果一个机器人状态与障碍物发生碰撞，则该状态包含在障碍物点的邻域球内。□ 产生局部性效应的因素有两个。首先，机器人的非零体积意味着即使是一个点障碍物也会导致一组与该点发生碰撞的机器人状态cs(t)。第二个因素是现实世界中的障碍物在空间中占据一定的体积。

给定一个已知碰撞点，我们采用局部性原理来定义一个函数，用于表示正在测试的新路径与同一障碍物发生碰撞的概率。局部性模型具有以下一般形式：

$$
loc(p_i | C) = Pr(pct(p_i) | C)
\tag{8}
$$

这里，C 可能是单个点的碰撞结果，也可能是多个碰撞的集合。如果省略，则假定为所有已知碰撞的集合。

该函数依赖于许多因素，包括机器人的大小和形状以及环境中障碍物大小和形状的分布。然而，最重要的参数是新路径与已知碰撞点之间的距离。因此，我们可以建立一个可快速计算的一阶局部性模型，在该模型中，我们使用单一的半径分布来抽象障碍物的大小和形状，如图3所示。在得到最终形式之前，我们将讨论几种中间的局部性模型表达形式。

图3 机器人（左侧的红色圆盘）考虑了两条路径。下方路径未通过碰撞检测。局部性模型并不知道障碍物（灰色）的完整范围，但可以使用概率分布（同心圆）来近似表示障碍物，并估计上方路径发生碰撞的可能性。

4.1 通用局部性模型

通过显式建模局部性，即使仅有关于障碍物位置的部分信息，我们也能推理出哪些路径更可能或更不可能与任何已知障碍物发生碰撞。当路径采样算法由局部性模型提供信息时，它能够根据当前已知的碰撞点，提供按碰撞可能性排序的路径序列。本文提出了一种通用的局部性模型，预计该模型能够以高概率生成无碰撞路径样本。

在构建通用局部性模型时，我们抽象掉了许多参数；我们将机器人和障碍物都视为球体（在R²或R³中），并假设障碍物的半径是均匀的。我们将在第4.4节中放宽其中一些假设。目前，这些限制使我们能够通过排除方向因素来简化模型。因此，该通用模型对未来的碰撞预测仅是已知碰撞点到路径之间距离的函数。障碍物（rₒ）和机器人（rᵣ）的固定半径形成了直观的局部性概念：其影响仅限于有限的作用距离内。

如图4所示，通用局部性模型的精确公式化基于在已知碰撞点的情况下，对障碍物质心可能位置的概率分布进行建模。在此朴素模型中，质心位置由一个均匀分布描述，该分布位于以机器人碰撞位置为中心、半径为rₒ的球体B(rₒ) 内。一条路径π扫过的轨迹区域S(pᵢ) 的宽度为 2rₒ + 2rᵣ。任何非空集合B(rₒ) ∩ S(pᵢ) 都表示存在一定的碰撞概率。该通用模型由此预测碰撞概率为

$$
loc_{general}(p_i | c) = \frac{|B(r_o) \cap S(p_i)|}{|B(r_o)|}.
\tag{9}
$$

如果我们把 pᵢ 视为一条直线，那么在二维情况下，碰撞概率就是圆弧段面积与整个圆面积之比，即 [2]：

$$
f_{segment}(r) =
\begin{cases}
\frac{1}{\pi r_e^2} \left[ r_e^2 \cos^{-1}\left(\frac{r - r_e}{r_e}\right) - (r - r_e)\sqrt{2r_e r - r^2} \right] & \text{if } 0 \leq r \leq 2r_e \
0 & \text{otherwise},
\end{cases}
\tag{10}
$$

其中 r 是路径与碰撞点之间的距离。我们将 rₑ 称为影响范围，在此处将其设为等于 rₒ。

定义3 已知碰撞点的影响范围描述了围绕该点的半径，在此半径内的路径被认为有与已知障碍物发生碰撞的风险。□

图4 一般局部性模型：a 给定一个已知与障碍物发生碰撞的点（中心红点），半径为rₒ的蓝色内圆表示障碍物质心的可能位置。绿色外圆包含障碍物某部分可能占据的点。b 新候选机器人路径无碰撞的概率等于在宽度为2rₒ + 2rᵣ 的轨迹区域之外的可能障碍物质心所占的比例。蓝色区域表示可能的质心集合，绿色区域表示可能的障碍物范围

4.2 简单局部性模型

我们现在提出一种更简单的局部性模型，该模型紧密近似于（10），但利用了现有的PLUT。我们不再考虑路径区域，而仅考虑新路径上最接近已知碰撞点的那个点。这个新的局部性模型采用了升余弦分布：

$$
f_{rcd}(r) =
\begin{cases}
\frac{1}{2r_e} \left[1 + \cos\left(\frac{\pi r}{2r_e}\right)\right] & \text{if } 0 \leq r \leq 2r_e \
0 & \text{otherwise},
\end{cases}
\tag{11}
$$

对于直线或平缓弯曲的路径，这种近似非常准确。那么，新路径 pᵢ 与由先前的碰撞 c = (pⱼ, s) 所表示的同一障碍物发生碰撞的概率仅为

$$
loc_{simple}(p_i | c) = f_{rcd}(PLUT(p_i, p_j, sM) - r_r).
\tag{12}
$$

注意，这里我们不再维护障碍物位置的显式概率分布，而是对单条路径相对于单一碰撞点的风险进行启发式估计。

4.3 处理多碰撞点

给定一个已知碰撞点，公式(9)和(12)提供了选择候选路径以最小化碰撞概率的工具。然而，我们尚未解决了多个已知碰撞点的问题。由于描述的是同一个障碍物，两个工作空间点具有相同障碍物结果的可能性与它们之间的距离密切相关。对于未测试路径的碰撞可能性估计，取决于我们在已知碰撞点之间所做出的统计独立性假设。

图5 两个碰撞点c₁和c₂彼此靠近。直观来看，在计算路径p_test的碰撞风险时，应忽略c₂。这两个碰撞点要么属于同一个障碍物，要么c₂处的障碍物被c₁处的障碍物“遮挡”。

图5描述了一种情况，其中两个碰撞点似乎存在关联。然而，对于估计一组碰撞点之间统计独立性的许多可能策略（例如聚类）而言，其计算和实现都较为复杂。相比之下，我们可以保守地假设所有碰撞都是独立的，在这种情况下，基础概率理论指出

$$
loc(p) = loc(p | {c_1, …, c_n}) = 1 - \prod_{i \in {1, …, n}} (1 - loc(p | c_i)):
\tag{13}
$$

如果某些碰撞点实际上是同一障碍物的一部分，那么我们就会高估点 p的碰撞可能性。然而，在缺乏相关性知识的情况下，最保守的策略才是最安全的。因此，我们现在有了应对问题1，利用的方法：

$$
p_{next} = \arg\min_{p_i \in P} loc(p_i).
\tag{14}
$$

在下一节中，我们将探讨一种信息理论方法，用于安全地调整这种悲观模型。

4.4 自适应局部性模型

第4.1–4.2节中提出的局部性模型仅包含正向的碰撞检测结果。这些静态模型保守地估计了障碍物分布在大但有限的影响范围内。我们现在构建一个自适应局部性模型，能够结合正向和负向的碰撞检测结果。

如果我们恰好发现一条安全路径 p_safe 穿过碰撞点 c’的影响范围，则可利用这一新信息来重新细化 c 的障碍物模型。实际上，我们调整局部性函数，使其在 p_safe 的方向上作用于更小的距离范围内。如图6a 所示，任何被 p_test 与 c分隔开的路径 p_safe 都不可能与此障碍物发生碰撞风险。这种自适应模型有效地放宽了先前对障碍物大小以及碰撞点独立性的严格假设。在建模此类几何关系时，我们不同于以往关于局部性的相关工作。

模型更新后，所有涉及c的未来概率估计都将包含这一新信息。尽管独立性假设最初可能使诸如 p_test 之类的邻近路径看起来比实际风险更高（图5），但自适应模型在找到安全路径以缩小每个碰撞点的影响范围后，会迅速抵消这种影响。

图6 a 给定一个碰撞点 c和邻近的无碰撞路径 p_safe，蓝色圆圈表示障碍物位置的分布，其中一些被 p_safe 所排除。更远的候选路径 p_test不会与由 c表示的障碍物发生碰撞风险。b和c 关于障碍物尺度和位置的两个简单假设可以解释这两个结果。图4b 中所示的分布在在线路径采样过程中更容易表示

在解决如何根据无碰撞路径自适应调整障碍物分布的问题时，存在多种方法。一种可能的方法是缩小障碍物在c处的影响范围，如图6b所示，这假设该障碍物比最初认为的要小。另一种方法是将整个分布从安全路径处移开，如图6c所示，这假设障碍物的尺寸估计正确，但其位置有偏差。

我们采取一种折衷方案。我们倾向于让碰撞点保持为分布的中心，以便通过查找表高效地进行距离检查效率。然而，我们也希望避免改变对侧的影响范围，因为我们没有关于该侧的新数据。因此，我们将有效作用距离划分为围绕每个碰撞点的若干个影响区域（影响“侧面”）。在二维情况下，障碍物有左右两个侧面，如图7 所示。在三维情况下，划分在拓扑上更具任意性，但我们通常将障碍物划分为四个侧面。

图7 碰撞点c每一侧的影响范围被分别维护。左侧范围起始于2rₒ，但在成功对路径p_safe进行碰撞检测后被缩小。

在将局部性模型划分为多个方向时，我们需要一条规则来一致地将每条路径与碰撞点的特定侧面对应起来。这些侧面是相对于机器人执行路径前的位姿定义的。各侧面在直线a处相交，该轴通过起始位姿和碰撞点。我们根据侧面相对于机器人坐标系的位置为其命名。侧面由以下方式确定

$$
left = sgn(t \times p \cdot u)
\tag{15}
$$

$$
top = sgn(t \times p \cdot a \times u),
\tag{16}
$$

其中 u表示机器人的’上向量，p为c在路径上的投影，t为该点处路径的切向量，如图8所示。这些侧面可针对每条路径预先计算。在二维中，特别方便的是在PLUT中增加一个符号，用于指示每个可能的碰撞点位于路径的哪一侧。

图8 在三维中，自适应局部性模型的影响范围被划分为四个侧面。机器人的上向量u和指向碰撞点的向量a用于确定路径p_test位于四个侧面中的哪一个。如图所示，路径位于左上侧。

图9 一个路径族，所有路径均从碰撞点c的左侧通过。尽管这些路径形状各异，但每条路径侵入c左侧影响范围的程度相同，因此它们对c左侧有效作用距离的缩减程度也相同

5 路径熵

在建立了自适应局部性模型之后，我们现在考虑如何充分利用其预测能力来解决 问题2，探索。选择用于碰撞检测的路径时，应使模型能够快速收敛到对障碍物的准确描述，同时尽量减少失败的碰撞检测。给定一组碰撞点，根据当前模型参数，具有最大熵的路径是进行碰撞检测的最佳路径。

定义4 一条未测试路径的熵（有时称为香农熵）指的是通过对其执行碰撞检测后，关于其他未测试路径安全性的预期可获得信息量。一条路径的熵是

$$
H(pct(p)) = - Pr(pct(p)) \log Pr(pct(p)) - Pr(\neg pct(p)) \log Pr(\neg pct(p)).
\tag{17}
$$

为了用尽可能少的样本最大化对障碍物真实分布的理解，我们选择采样最大熵路径：

$$
p_{test} = \arg\max_{p_i \in P} H(pct(p_i)).
\tag{18}
$$

根据当前信息，最大熵路径在与障碍物发生碰撞方面具有最大的不确定性；其碰撞概率最接近50%。测试该特定路径将比测试其他任何路径更能增加总体知识。结果要么是一条显著缩小某些已知碰撞点影响范围的路径，要么是一个远离已知碰撞的新碰撞点。无论哪种情况，模型准确性都将以最大效用得到提升。

最大化熵的策略由Jaynes [13]提出，用于估计未知分布。最大熵已被特别应用于决策理论[8]，正如我们在此处所采用的那样。使熵最大化的决策是能够最小化最坏可能结果的决策。在我们的场景中，最坏的结果是重新发现已知结果，因为它浪费了计算时间而没有任何收益。这种情况表现为两种形式：测试一条经过已知碰撞点，或重新测试已知的安全路径。这种C‐极小化方法[18]能够同时对一系列概率分布（称为C——在我们的情况下，即关于障碍物范围的各种假设）进行推理。

如果反复追求最大熵策略，路径选择将逐步发现一系列安全路径和越来越接近的碰撞点，从而精确地确定障碍物与自由空间之间的边界。了解这些边界可以加速在自由空间内更密集地采样和测试路径的过程。在相关工作 [15]中，我们提供了一种实现该过程的可能方法。

然而，细化这些边界是一个具有递减回报的过程。当我们逐步发现离障碍物更近的安全路径时，不确定性边界变得极低，以至于额外的最大熵路径样本由于多种原因带来的优势微乎其微：（1）路径越接近障碍物，碰撞检测的代价通常越高；（2）路径采样相关的计算成本与已知碰撞点的数量成正比；（3）靠近障碍物的路径不适合作为执行选择。这些因素可以整合到效用函数[6]中，但这仍是未来的工作。因此，我们不应仅追求最大熵采样策略，还应选择远离障碍物的路径样本，以最大化安全性和路径多样性。

我们采用多种策略，在混合方法中结合探索与利用。当我们的局部性模型不存在任何不确定性时（例如在发现第一个碰撞点之前），我们从低分散序列中进行采样。当存在不确定性时，我们计算已耗用的规划时间占总允许规划时间的比例 f（回想一下，我们假设是实时规划）。以概率 f 采取利用（障碍物规避）的采样策略，而以概率 1 – f 采取探索（边界发现）策略，以优化我们的局部性模型。靠近已知障碍物的采样路径会被暂存而不进行测试，留待后续使用，因为它们不适合作为通行候选路径。该阈值距离反映了在其他选择可用时，有意忽略非常狭窄通道的倾向。如果在规划周期结束时仍有剩余时间，在测试完其他路径后，我们会搜索这些高风险路径。

6 实验结果

我们在仿真中进行了一系列实验，以获得足够数量的试验来识别具有统计意义的趋势。实验设置与我们之前的工作[14]相同，其中试验包含一百个规划问题；在每个问题中，曲率约束机器人尝试穿越随机生成的二维点障碍环境，每个环境基于包含起始和目标位姿的查询生成。机器人在连续移动的同时以固定频率重新规划。启发式函数从2,401条局部路径中选择目标导向的局部路径。低保真度全局规划器帮助引导机器人到达目标。为了评估不同路径采样策略的影响，我们改变了重规划周期时间。

图10 局部性模型的模拟器示意图，显示了：无碰撞路径（蓝色）、已知碰撞点（红点）及其影响范围（同心半圆）。注意，这些点对应于构型空间障碍物的最近边——即相对于机器人当前位姿最相关的点。并非所有障碍物（黑色）都与当前局部规划相关

我们考虑了四种路径采样策略：
- 低色散——由Green-Kelly算法生成的序列 [7]
- 避障——纯利用；尽可能远离障碍物进行采样
- 寻找边界——纯探索；选择最大熵路径
- 混合方法——结合“避障”和“寻找边界”策略。

我们在图11中展示了三种策略的结果。我们看到，纯障碍物规避（最高达7.8倍）以及混合方法（最高达3倍）具有优势。然而，纯障碍物避让采样在解决规划查询时性能下降，而混合方法克服了这一问题。需要注意的是，随着允许的采样数量增加，低离散采样器的性能实际上略有下降，这与早期结果[14]一致。混合规划器在改进其局部性模型的过程中表现出性能持续提升的趋势。

图11 这些图表中的所有测试均在障碍物密度为2%的情况下进行。a 局部性模型使得每个重规划周期内的无碰撞路径比例最高提升了7.8倍。随着重规划周期时间的增加，由于对更大比例的可用路径进行了碰撞检测，性能逐渐向平均水平回归。b 在解决规划查询的成功率方面，我们发现“避障”策略性能较差，而混合方法在探索与利用局部性模型之间取得了平衡，随着有更多时间收集信息，其表现越来越好