技术摘要：终端事项：基于终端代价与学习不确定性在信念状态 - 代价空间中的运动动力学规划

问题陈述

在真实环境中运行的机器人系统必须生成满足运动动力学约束且能动态可行的运动，同时需考虑由建模误差、感知噪声和复杂接触交互引起的执行不确定性。尽管现有的基于采样的运动动力学规划器（如 AO-RRT、SST）提供了概率完备性和渐近最优性，但它们通常仅优化运行代价（沿轨迹累积，如路径长度或能量），而将到达目标仅视为可行性检查。

这种方法存在两个关键局限性：

1. 终端状态质量：许多任务取决于最终状态的质量，而不仅仅是所采取的路径。例如，在平面推物体任务中，较短的路径可能导致较高的终端不确定性，从而降低成功到达目标的概率。在多目标场景（如汽车泊车）中，规划器可能会选择一个几何距离更近但较不理想或更难离开的目标，而忽略了对特定终端区域的偏好。
2. 不确定性传播：在信念空间（其中状态为概率分布）中进行规划对于鲁棒性至关重要。然而，现有方法通常依赖硬概率阈值或轨迹级代理，而非显式优化终端信念状态。此外，复杂系统（如富含接触的推物体任务）往往缺乏解析的不确定性模型，需要数据驱动的方法，而这些方法很少与信念空间规划器紧密集成。

方法论

作者提出了KiTe（带有终端代价的运动动力学规划），这是一个在渐近最优快速探索随机树（AO-RRT）基础上增加显式终端代价目标的框架。该方法论在三个主要层级上运作：

1. 状态空间中带有终端代价的 KiTe

核心算法在增广状态 - 代价空间 $\mathcal{Y} = \mathcal{X} \times \mathbb{R}^+$ 中重新表述了最优运动动力学规划问题。

• 增广动力学：状态从 $x$ 扩展为 $y = (x, c)$，其中 $c$ 累积运行代价 $\ell(x, u)$。动力学扩展为包含代价率：$\dot{y} = (f(x, u), \ell(x, u))$。
• 终端代价集成：与以往在搜索过程中忽略终端代价的公式不同，KiTe 仅在到达目标区域时评估总代价 $C_T = \int \ell dt + \phi(x(T))$。
• 搜索策略：算法维护一个代价界限 $c_{best}$。当找到解时，该界限更新为 $c_{new} + \phi(x_{new})$。采样策略随后聚焦于由该界限定义的次水平集，修剪那些无法改善总代价（运行代价 + 终端代价）的分支。
• 理论保证：作者提供了严格的证明，表明带有终端代价的 AO-RRT 保留了渐近最优性，前提是系统动力学、运行代价和终端代价满足 Lipschitz 连续性条件。

2. 扩展到信念空间

该公式被扩展到信念状态 - 代价空间 $\mathcal{Y}_b = \mathcal{B} \times \mathbb{R}^+$，其中 $\mathcal{B}$ 是状态上概率分布的空间。

• 度量：采用2-Wasserstein 距离作为信念空间的度量，因为它自然地捕捉了高斯信念均值和协方差的变化。
• 到达目标的下界：一个关键的理论贡献是证明到达目标区域 $G$（定义为半径为 $r$ 的球体）的概率由终端信念 $b(T)$ 与目标处的狄拉克测度 $\nu_g$ 之间的 Wasserstein 距离下界限定：
  $$\text{Pr}(x(T) \in G) \geq 1 - \frac{W_2^2(b(T), \nu_g)}{r^2}$$
  这使得规划器可以将 Wasserstein 距离用作终端代价 $\psi(b(T))$，直接优化到达目标成功的下界，而无需依赖硬性的、手动调整的概率阈值。
• Lipschitz 连续性：作者证明，在随机动力学的温和假设下（特别是关于过程不确定性和噪声注入的 Lipschitz 连续性），信念动力学和代价函数相对于 Wasserstein 度量保持 Lipschitz 连续。这确保了 KiTe 的渐近最优性扩展到信念空间。

3. 学习信念动力学

为了解决缺乏解析不确定性模型的系统（如富含接触的推物体任务），KiTe 集成了学习的信念动力学模型。

• 模型架构：训练一个神经网络（MLP）来预测局部随机转换。给定状态 $x_i$ 和控制 $u_i$，网络输出平均转换 $\Delta x_i$ 和由方差向量 $\sigma_i^2$ 参数化的过程协方差。
• 训练目标：该模型使用交互数据上的**负对数似然（NLL）**损失进行训练。这使得网络能够直接从数据中学习平均动力学和偶然不确定性（过程噪声）。
• 传播：学习的均值和协方差用于在规划器中向前传播信念状态（均值和协方差），支持欧几里得和李群（如 SE(2)）状态空间。

主要贡献

1. 算法扩展与证明：实现了带有显式终端代价的 AO-RRT，并附带严格证明，表明渐近最优性在状态空间和信念空间中均得以保留。
2. 与成功概率的理论联系：证明最小化终端信念与目标之间的 Wasserstein 距离会诱导到达目标成功概率的下界，为可靠性提供了一个原则性的终端目标。
3. 数据驱动的信念传播：展示了如何使用 NLL 损失直接从交互数据中学习信念动力学模型（均值和不确定性），并将其集成到 KiTe 框架中，以服务于缺乏解析模型的系統。
4. 实证验证：在三个不同的任务（Flappy Bird、汽车泊车、平面推物体）中进行了全面的评估，涵盖仿真和真实世界环境，将 KiTe 与最先进基线（AO-RRT、SST、高斯信念树）进行了比较。

实验结果

该论文在三个任务中验证了 KiTe：

• Flappy Bird（确定性）：证明了 KiTe 可以通过优化终端代价来编码目标偏好（例如，将鸟居中于缝隙中），而基线方法一旦到达目标区域就停止优化。KiTe 实现了更接近目标中心的轨迹，仅增加了少量的运行代价。
• 汽车泊车（带不确定性的 SE(2)）：在具有多个可行停车位的任务中，KiTe 成功优先选择了“首选”目标区域（前车位），并提高了到达目标的成功率，优于仅优化路径长度或使用机会约束的基线方法。带有终端代价的信念空间规划产生了最高的执行成功率。
• 平面推物体（富含接触，真实世界）：使用针对垃圾车和饼干盒等物体的学习动力学，KiTe 在执行成功率方面优于基线方法（包括带有模型不确定性的主动规划）。规划器选择了更长的几何路径，这些路径导致了更低的终端不确定性和更高的成功概率，证明了优化终端信念质量的价值。

意义与主张

该论文声称，KiTe 解决了运动动力学规划中的一个根本性差距：轨迹优化与终端状态质量的分离。通过显式优化终端状态，该方法：

• 提高可靠性：通过最小化到目标的 Wasserstein 距离，直接增加了在不确定性下到达目标的概率。
• 编码偏好：允许规划器根据期望性（例如，离开的难易程度、稳定性）而非仅仅是几何接近度来区分多个可行目标。
• 弥合学习与规划：提供了一个框架，其中学习的不确定性模型不仅用于偏置采样，而且被紧密集成到信念传播和优化循环中。

作者总结道，虽然该方法引入了设计参数（终端权重）并依赖于学习模型，但它提供了一种原则性的、理论扎实的方法，用于生成不仅在动力学上可行，而且在不确定环境中可靠且具备偏好感知能力的运动。