未知动力学下机会约束运动规划的一致性约束收紧

运动规划算法为自主机器人计算控制序列，驱使其到达目标区域同时避免不安全状态。然而，大多数现有方法——从采样式规划器到深度强化学习——仅在名义模型或仿真器上提供任务完成保证。当真实系统动力学未知或难以精确建模时，这些保证可能失效。本文提出一个规划器无关的约束收紧程序，利用共形预测（Conformal Prediction, CP）量化名义轨迹与真实轨迹之间的偏差，并将此偏差作为约束收紧半径，使得在收紧后的名义模型上求解的任何控制序列，都能在未知真实系统上以用户指定概率完成任务。

1. 问题动机与核心挑战

考虑一个真实动力学未知的离散时间机器人系统：

$$\mathbf{x}_{t+1} = f(\mathbf{x}_t, \mathbf{u}_t)$$

其中 $\mathbf{x}_t \in \mathcal{X} \subset \mathbb{R}^n$ 是状态，$\mathbf{u}_t \in \mathcal{U}$ 是控制输入，真实动力学 $f$ 未知。我们只能获取一个近似的名义模型：

$$\hat{\mathbf{x}}_{t+1} = \hat{f}(\hat{\mathbf{x}}_t, \mathbf{u}_t)$$

目标是在有限时域 $H$ 内，计算控制序列 $\mathbf{u}_{0:T-1}$，使得真实系统轨迹以至少 $1-\alpha$ 的概率到达目标区域 $\mathcal{X}_{\text{goal}}$ 同时始终避免不安全状态 $\mathcal{X}_{\text{obs}}$：

$$\mathbb{P}\left(\bigwedge_{t=0}^{T}(\mathbf{x}_t \in \mathcal{X}_{\text{free}}) \wedge (\mathbf{x}_T \in \mathcal{X}_{\text{goal}})\right) \geq 1 - \alpha$$

这里 $\mathcal{X}_{\text{free}} := \mathcal{X} \setminus \mathcal{X}_{\text{obs}}$ 是安全状态空间。关键在于，该概率保证是关于从分布 $\mathcal{D}_\mathcal{M}$ 中抽取的规划问题的边际概率，而非单个任务的保证——这非常适合规划器在变化环境中被反复调用的实际机器人应用。

2. 共形预测背景

共形预测提供了一种分布无关、有限样本有效的不确定性量化方法，适用于任何固定预测模型。给定校准数据集 $\mathcal{S}_{\text{cal}} = \{(X_i, Y_i)\}_{i=1}^{N_{\text{cal}}}$ 和非一致性分数（NCS）函数 $s: \mathbb{X} \times \mathbb{Y} \to \mathbb{R}$，对目标失效概率 $\alpha \in (0,1)$，CP 计算阈值 $\hat{q}_\alpha$，满足：

$$\mathbb{P}(R_{\text{test}} \leq \hat{q}_\alpha) \geq 1 - \alpha$$

其中 $\hat{q}_\alpha$ 是 $\{R_1, \dots, R_{N_{\text{cal}}}, \infty\}$ 的 $(1-\alpha)$ 经验分位数。CP 不对模型或数据分布做任何假设，仅依赖用户定义的 NCS 函数来度量预测与真实值之间的误差。

3. 用 CP 量化轨迹级模型失配

本文将 CP 应用于规划问题层面——不同于先前在状态-控制对层面应用 CP 的工作。定义非一致性分数为所有名义可行控制序列上名义到真实轨迹偏差的最坏情况：

$$R(\mathcal{M}) := \sup_{\mathbf{u}_{0:T-1} \in \mathcal{U}(\mathcal{M})} \max_{t \in \{0,\dots,T\}} \|\mathbf{x}_t - \hat{\mathbf{x}}_t\|_2$$

其中 $\mathcal{U}(\mathcal{M})$ 是所有在名义模型上可行的控制序列集合。通过对校准规划问题计算该分数并应用 CP，得到关键的名义到真实模型误差界：

$$\mathbb{P}\left(\sup_{\mathbf{u}_{0:T-1} \in \mathcal{U}(\mathcal{M})} \max_{t \in \{0,\dots,T\}} \|\mathbf{x}_t - \hat{\mathbf{x}}_t\|_2 \leq \hat{q}_\alpha\right) \geq 1 - \alpha$$

几何上，这意味着以至少 $1-\alpha$ 的概率，对每个名义可行控制序列，真实状态都落在以名义状态为中心、半径为 $\hat{q}_\alpha$ 的球内。

图1展示了本文提出的CP规划框架：共形阈值 $\hat{q}_\alpha$ 诱导出一个约束收紧的规划问题，使得任何在该收紧问题上求解的控制序列都继承了概率可达避障保证。

4. 概率安全规划与核心定理

基于上述偏差界，定义收紧的自由空间和目标区域：

$$\mathcal{X}_{\text{free}}^{\hat{q}_\alpha} := \{\mathbf{x} \in \mathcal{X} : \mathcal{B}(\mathbf{x}, \hat{q}_\alpha) \subseteq \mathcal{X}_{\text{free}}\}$$ $$\mathcal{X}_{\text{goal}}^{\hat{q}_\alpha} := \{\mathbf{x} \in \mathcal{X} : \mathcal{B}(\mathbf{x}, \hat{q}_\alpha) \subseteq \mathcal{X}_{\text{goal}}\}$$

直观上，这是通过将自由空间和目标集收缩 $\hat{q}_\alpha$ 来实现的——在平面导航中，这等价于膨胀障碍物、侵蚀工作空间边界并收缩目标区域。

命题 III.1（规划保证）：若以名义状态为中心、半径 $\hat{q}_\alpha$ 的球序列包含在自由空间内，且终端球包含在目标区域内：

$$\mathcal{B}(\hat{\mathbf{x}}_t, \hat{q}_\alpha) \subseteq \mathcal{X}_{\text{free}}, \quad \forall t \in \{0,\dots,T\}$$ $$\mathcal{B}(\hat{\mathbf{x}}_T, \hat{q}_\alpha) \subseteq \mathcal{X}_{\text{goal}}$$

则该控制序列以至少 $1-\alpha$ 的概率解决原始问题。证明利用(6)式：以至少 $1-\alpha$ 概率，$\|\mathbf{x}_t - \hat{\mathbf{x}}_t\|_2 \leq \hat{q}_\alpha$ 对所有 $t$ 成立，因此真实轨迹被球序列包含，始终保持在自由空间内并最终到达目标。

命题 III.4（规划器保证）：在名义模型上解决收紧问题 $\mathcal{M}_\alpha$ 的每个控制序列，也以至少 $1-\alpha$ 的概率解决真实系统上的原始问题 $\mathcal{M}$。这是本文最核心的结论——将未知动力学下的机会约束规划归约为名义模型上的确定性规划，可由任何能够执行收紧约束的规划器求解。

graph TD
    A[校准数据集
N_cal=100个规划问题] --> B[真实系统执行
采集真实轨迹]
    B --> C[计算非一致性分数
R_i = sup ||x_t - x_hat_t||]
    C --> D[共形预测
计算阈值 q_hat_alpha]
    D --> E[收紧自由空间
X_free^q = erode]
    D --> F[收缩目标区域
X_goal^q = erode]
    E --> G[名义模型上求解
收紧问题 M_alpha]
    F --> G
    G --> H[控制序列以1-alpha概率
在真实系统完成任务]
    style D fill:#f9f,stroke:#333,stroke-width:2px
    style H fill:#bfb,stroke:#333,stroke-width:2px

论文首页展示了CP框架的整体思路：通过校准数据量化模型偏差，收紧约束后保证真实系统的概率安全。

5. 实验验证

作者在两个场景上验证框架，代表实践中常见的模型失配类型：

5.1 实验设置

场景一：Dubins 车辆（确定性，参数失配）。名义模型使用标准 Dubins 动力学，真实系统经历平移减速：

$$\hat{\mathbf{x}}_{t+1} = \hat{\mathbf{x}}_t + \Delta t [\cos(\hat{\theta}_t), \sin(\hat{\theta}_t), u_t]^\top$$ $$\mathbf{x}_{t+1} = \mathbf{x}_t + \Delta t [(1-\beta_v)\cos(\theta_t), (1-\beta_v)\sin(\theta_t), u_t]^\top$$

参数 $\beta_v \in \{0.05, 0.10, 0.15, 0.20\}$ 控制模型失配程度，产生四个不同的真实系统模型。这模拟了名义模型未考虑的表面或传动阻力。

场景二：平面四旋翼（随机性，学习模型+噪声）。6维状态 $\mathbf{x}_t = [x_t, y_t, \theta_t, v_{x,t}, v_{y,t}, \omega_t]^\top \in \mathbb{R}^6$，2维控制 $\mathbf{u}_t = [\delta T_t, \tau_t]^\top \in \mathbb{R}^2$。真实动力学包含过程扰动：

$$v_{x,t+1} = v_{x,t} + \Delta t \left[-\frac{1}{m}(mg + \delta T_t)\sin\theta_t + w_{x,t}\right]$$ $$v_{y,t+1} = v_{y,t} + \Delta t \left[\frac{1}{m}(mg + \delta T_t)\cos\theta_t - g + w_{y,t}\right]$$ $$\omega_{t+1} = \omega_t + \Delta t \left[\frac{\tau_t}{I} + w_{\omega,t}\right]$$

其中 $m=1, g=9.8, I=0.25$，每个扰动分量独立采样自 $\mathcal{N}(0,1)$ 并截断到 $[-3,3]$。名义模型是训练好的神经网络一步预测器，失配同时来自学习模型的近似误差和随机执行噪声。

两个场景的规划问题均从分布 $\mathcal{D}_\mathcal{M}$ 采样：$25 \times 25$ 环境，5个随机放置的圆形障碍物（半径从 $[1,3]$ 采样），初始状态从自由空间均匀采样，目标区域是半径为5的圆且距初始位置至少15个单位。

5.2 评估指标

指标	定义	期望
经验覆盖率	真实轨迹始终在 $\hat{q}_\alpha$ 范围内的计划比例	≥ 1-α
规划器成功率	规划器在收紧问题上返回解的比例	随1-α增大而降低
任务完成率	返回计划的真实轨迹无碰撞到达目标的比例	≥ 1-α

5.3 Dubins 车辆结果

图2左列展示了Dubins车辆的结果。在所有情况下，经验覆盖率超过目标水平（符合(6)式）。随着模型失配增大（$\beta_v$ 增大），经验覆盖与目标之间的间隙也增大——因为更大的失配产生更大的校准分数，从而更大的 $\hat{q}_\alpha$，共形球变得更保守。

$\beta_v$	名义模型失配	对 $\hat{q}_\alpha$ 的影响
0.05	小	轻度收紧
0.10	中	中度收紧
0.15	较大	较保守
0.20	大	最保守

任务完成率始终超过 $1-\alpha$，且本框架始终优于名义规划基线，性能差距随模型失配和 $1-\alpha$ 增大而增大。

5.4 平面四旋翼结果

图2右列展示了随机四旋翼的结果。尽管同时存在学习模型误差和随机执行噪声，经验覆盖率始终超过目标水平，验证了CP保证。任务完成率从 $1-\alpha=30\%$ 时的约80%上升到 $1-\alpha=90\%$ 时的100%，而名义RRT基线仅约55%。

覆盖水平 $1-\alpha$	本方法任务完成率	名义基线
30%	~80%	~55%
90%	~100%	~55%

6. 方法核心创新与对比

与现有工作的关键区别在于：

维度	本文方法	CC-RRT [15]	SLS-MPC [31]
动力学假设	仅名义模型，真实未知	已知随机线性系统	已知模型类
CP应用层面	规划问题级	N/A	状态-控制对级
保证类型	跨规划问题分布的边际	单任务逐步	单任务闭环执行
规划器依赖	规划器无关	特定于RRT	特定于SLS-MPC

7. 局限性与扩展

局限性1：保守性。当扰动集 $\mathcal{W}$ 较大或模型失配严重时，共形阈值 $\hat{q}_\alpha$ 可能过大，导致收紧后的自由空间过小，规划器成功率显著下降。随机动力学的扩展（Remark III.3）尤其保守。

局限性2：NCS的实践近似。精确计算非一致性分数需要在所有名义可行控制序列上优化，通常不可行。实践使用有限子集近似，可能低估偏差，削弱保证的紧致性。

扩展方向：作者指出未来工作将聚焦于降低保守性，以及开发当无法以期望概率计算计划时的自适应风险规划策略。此外，CP分析可扩展到随机真实系统 $\mathbf{x}_{t+1} = f(\mathbf{x}_t, \mathbf{u}_t, \mathbf{w}_t)$，用最坏情况分数替代。

8. 总结

本文解决了未知动力学系统的运动规划问题，仅利用近似名义模型。通过共形预测推导出名义到真实轨迹偏差的概率界，并利用该界构建收紧的规划问题，使得在名义模型上求解的任何控制序列都能在真实系统上以指定概率完成任务。关键贡献包括：(1) 面向未知真实动力学的CP不确定性量化框架，使用名义模型和校准数据提供轨迹级偏差概率界；(2) 收紧约束的规划问题构建及充分性证明；(3) 在Dubins车辆和平面四旋翼上的实证验证，展示相比名义规划显著提升的任务完成率。