160_HJ可达性分析

专题 3.18：Hamilton-Jacobi 可达性分析 (Hamilton-Jacobi Reachability Analysis)¶

难度: ⭐⭐ - ⭐⭐⭐⭐ | 前置: 控制理论/40_HJB方程与黏性解, 控制理论/70_Lyapunov稳定性理论, 控制理论/80_CLF_CBF与QP安全控制

0. 前置自测 ⭐¶

在开始本章之前，请确认你能回答以下问题。如果任何一题无法回答，请回到对应的前置专题。

#	自测问题	前置专题
Q1	HJB 方程 $V_t + \min_u\{L + \nabla V \cdot f\} = 0$ 的每一项物理含义是什么？	控制理论/40_HJB方程与黏性解 §3.4.2
Q2	为什么 HJB 的经典 $C^1$ 解几乎不存在？黏性解 (viscosity solution) 通过什么机制绕过了这个困难？	控制理论/40_HJB方程与黏性解 §3.4.4
Q3	CBF 的核心不等式 $L_f h + L_g h \cdot u \ge -\alpha(h)$ 的几何含义是什么？为什么它能保证安全集前向不变？	控制理论/80_CLF_CBF与QP安全控制 §1.2
Q4	什么是 class-$\mathcal{K}$ 函数？什么是 class-$\mathcal{KL}$ 函数？	控制理论/70_Lyapunov稳定性理论 §3.7.1
Q5	写出 Dubins car 的运动学方程。	基础

Q5 参考答案（贯穿全章的 running example）：

\[\dot{x}_1 = v\cos\theta, \quad \dot{x}_2 = v\sin\theta, \quad \dot{\theta} = \omega\]

其中 $(x_1, x_2)$ 为平面位置，$\theta$ 为航向角，$v$ 为速度（通常取为常数），$\omega$ 为角速度（控制输入）。

1. 本章目标¶

学完本章后，你应当能够：

**解释**为什么 CBF 不足以保证复杂场景下的安全，以及 HJ 可达性如何弥补这一不足；
**区分**后向可达集 (Backward Reachable Set, BRS)、后向可达管 (Backward Reachable Tube, BRT) 和 Avoid 集的数学定义；
完整推导 Hamilton-Jacobi-Isaacs (HJI) 变分不等式，并理解它与 HJB 方程的关系；
使用 Level-Set 方法的数值格式（Lax-Friedrichs 数值 Hamiltonian、CFL 条件）；
**用 Python + hj_reachability 库**计算 Dubins car 的安全可达集；
了解 DeepReach（高维 HJ 的神经网络逼近）和 CBVF（CBF 与 HJ 的统一框架）的核心思想；
在累积项目中，独立完成 Dubins car 追逃博弈的 BRT 计算。

2. 为什么需要可达性分析 ⭐¶

2.1 动机：一个让你夜不能寐的问题¶

想象你是一名无人机工程师。你的四旋翼将在城市中自主飞行，周围是高楼大厦、不断变化的阵风、以及不可预测的鸟群。你的老板问你：

"不管风怎么吹，你能数学上保证无人机**永远**不会撞楼吗？"

这个问题的关键词是"不管风怎么吹"——扰动是**对抗性的**（adversarial），我们需要在**最坏情况**下保证安全。

2.2 反面：为什么 CBF 不够¶

在专题 3.8 中，我们学了 CBF 的核心思路：手动设计一个函数 $h(x)$，使得安全集 $\mathcal{C} = \{x : h(x) \ge 0\}$ 前向不变。CBF-QP 在每个时刻求解一个二次规划，修正名义控制使之满足 $\dot{h} \ge -\alpha(h)$。

这非常优雅，但有三个根本困难：

困难 1：$h(x)$ 需要手动设计。 对简单几何（圆形障碍物），$h(x) = \|x - x_{\text{obs}}\|^2 - r^2$ 很自然。但对复杂、非凸障碍物呢？对需要考虑动力学约束的场景呢？设计错误的 $h$ 可能导致安全集太大（不安全）或太小（过于保守）。

困难 2：CBF 条件是局部的。 $\dot{h} \ge -\alpha(h)$ 只看了一步的导数。如果当前安全但一秒后无论怎么控制都会不安全呢？CBF 无法回答这种"多步前瞻"问题。

困难 3：扰动处理粗糙。 标准 CBF 理论假设 $\dot{h} = L_f h + L_g h \cdot u$，即系统是控制仿射的且没有外部扰动。虽然 Robust CBF 存在，但需要额外假设且设计更加困难。

陷阱警告

一个常见的误区是："我用神经网络学一个 $h(x)$，然后用 CBF-QP 就安全了。" 但学习到的 $h$ 是否真的在**整个状态空间**上满足 CBF 条件？这需要形式化验证（SMT/SOS），而且学到的 $h$ 可能并不是最优的安全边界。HJ 可达性提供了一种**从动力学出发，自动计算**最优安全边界的方法。

2.3 HJ 可达性的核心承诺¶

Hamilton-Jacobi 可达性分析提供以下保证：

自动计算安全边界：给定动力学 $\dot{x} = f(x, u, d)$ 和危险区域 $\mathcal{T}$，自动计算所有"无论如何控制都可能进入 $\mathcal{T}$"的状态集合——这就是**后向可达管** (BRT)。
最坏情况鲁棒：将扰动 $d$ 建模为对抗性的"对手"，通过**二人零和微分博弈**框架，在最坏扰动下保证安全。
提供最优安全控制器：计算出的值函数 $V(x)$ 不仅告诉你哪些状态安全，还告诉你在每个状态该如何控制。
值函数即 CBF：计算出的 $V(x)$ 天然满足 CBF 条件——这是 CBF 与 HJ 的深层联系（第 8 节详述）。

2.4 CBF vs HJ 可达性对比表¶

维度	CBF (控制理论/80_CLF_CBF与QP安全控制)	HJ 可达性 (本章)
安全函数 $h(x)$	手动设计	自动从动力学计算
前瞻能力	一步导数（局部）	无限时域（全局）
扰动处理	需额外设计 Robust CBF	内建对抗性博弈框架
最优性	不保证最大安全集	给出最大可控不变集
计算成本	在线 QP，实时	离线网格计算，$O(N^d)$
维度限制	无（只要 QP 可解）	网格法 $d \le 6$；DeepReach 可更高
适用系统	控制仿射 $\dot{x} = f + gu$	一般非线性 $\dot{x} = f(x,u,d)$
工程用法	在线 safety filter	离线计算 + 在线查表

关键洞察：CBF 和 HJ 不是互相替代的——HJ 离线计算值函数，然后这个值函数可以作为 CBF 在线使用。这就是"先离线算清楚，再在线快速决策"的思路。

2.5 历史：从微分博弈到安全机器人¶

年代	里程碑	意义
1965	Isaacs, Differential Games	奠定二人零和微分博弈理论
1983	Crandall-Lions, 黏性解	为 HJ 方程提供了正确的弱解框架
1988	Osher-Sethian, Level-Set 方法	提供了追踪隐式曲面演化的数值工具
2000	Mitchell-Tomlin, toolboxLS	第一个开源 HJ 可达性工具箱 (MATLAB)
2005	Mitchell-Bayen-Tomlin, Automatica	奠基论文：将 Level-Set 方法与可达性分析系统化结合
2017	Bansal et al., FaSTrack	规划与安全解耦的实时框架
2021	Bansal-Tomlin, DeepReach	用神经网络突破维度限制
2021	Choi et al., CBVF	统一 CBF 与 HJ 可达性
2022	Hsu et al., `hj_reachability`	JAX 实现的现代 Python 工具

核心引用：Mitchell, Bayen, Tomlin. "A time-dependent Hamilton-Jacobi formulation of reachable sets for continuous dynamic games." IEEE Trans. Automatic Control, 50(7):947-957, 2005。这篇论文是整个领域的基石——如果只读一篇论文，就读这篇。

练习 2.1 ⭐¶

考虑一个一维系统 $\dot{x} = u + d$，其中 $u \in [-1, 1]$，$d \in [-0.5, 0.5]$，危险区域为 $x \ge 2$。

(a) 如果当前 $x = 1.8$，控制器能否保证系统永远不进入危险区域？ (b) 如果当前 $x = 1.2$ 呢？ (c) 你能找到临界值 $x^*$，使得 $x < x^*$ 时安全可保证，$x \ge x^*$ 时无法保证吗？

提示：最坏情况下 $d = 0.5$，控制器最多能产生 $u = -1$，所以净速度最小为 $-0.5$。

3. 后向可达集与后向可达管 ⭐⭐¶

3.1 目标集的 Level-Set 表示¶

一切始于定义"什么是危险"。我们用一个 Lipschitz 连续函数 $l : \mathbb{R}^n \to \mathbb{R}$ 的零水平集来表示**目标集**（target set）：

\[\mathcal{T} = \{x \in \mathbb{R}^n : l(x) \le 0\}\]

函数 $l(x)$ 称为目标集的**隐式表面函数** (implicit surface function)。约定：

$l(x) < 0$：$x$ 严格在目标集内部
$l(x) = 0$：$x$ 在目标集边界上
$l(x) > 0$：$x$ 在目标集外部

例（Dubins car 碰撞区域）：设障碍物为以原点为圆心、半径 $R$ 的圆形区域。碰撞区域（圆内）是 $\{x : x_1^2 + x_2^2 \le R^2\}$。令

\[l(x) = x_1^2 + x_2^2 - R^2\]

则 $l(x) \le 0$ 恰好是碰撞区域 $\mathcal{T}$。

陷阱警告

$l(x)$ 的符号约定至关重要。在 HJ 可达性文献中，$\mathcal{T} = \{l \le 0\}$ 是标准约定。目标集是"要到达"或"要避免"的集合，具体含义取决于你做的是"Reach"分析还是"Avoid"分析。弄反符号是初学者最常犯的错误，会导致计算出完全相反的结果。

3.2 后向可达集 (Backward Reachable Set, BRS)¶

定义。给定动力学 $\dot{x} = f(x, u, d)$、时间区间 $[t, 0]$（惯例：终端时间为 0，$t < 0$ 表示从过去回溯）和目标集 $\mathcal{T}$，**后向可达集**是所有"存在某个控制使系统能在时间 $[t, 0]$ 内到达 $\mathcal{T}$"的初始状态集合：

\[\text{BRS}(t) = \{x : \exists u(\cdot), \, \exists s \in [t, 0], \, \xi^{u}_{x,t}(s) \in \mathcal{T}\}\]

其中 $\xi^{u}_{x,t}(s)$ 是从 $(x, t)$ 出发在控制 $u(\cdot)$ 下的轨迹。

直觉：BRS 回答的问题是"从哪些状态出发，系统**有能力**在时间窗口内到达目标集？"这适用于**规划**场景——我想到达目标，哪些状态可行？

3.3 后向可达管 (Backward Reachable Tube, BRT)¶

安全分析更关心的是**不管怎么控制都可能到达危险区域**的状态。当存在对抗性扰动时，问题变成一个博弈：

\[\text{BRT}(t) = \{x : \forall u(\cdot), \, \exists d(\cdot), \, \exists s \in [t, 0], \, \xi^{u,d}_{x,t}(s) \in \mathcal{T}\}\]

直觉：BRT 回答的问题是"从哪些状态出发，不管控制器怎么努力，对手（扰动）都能在时间窗口内把系统推入危险区域？"

等价地，BRT 的补集 $\mathbb{R}^n \setminus \text{BRT}(t)$ 就是**安全集**——从这些状态出发，存在一个控制策略使系统在时间 $[t, 0]$ 内永远不进入 $\mathcal{T}$。

关键区别：BRS vs BRT

概念量词结构含义用途

BRS $\exists u, \exists s$ 能到达目标规划（reach）

BRT $\forall u, \exists d, \exists s$ 无法避免危险安全（avoid）

BRT 是 BRS 在对抗性博弈下的推广。BRS 可以看作 BRT 在 $d$ 退化（无扰动）且"控制目标是到达"时的特例。

3.4 Avoid 集¶

在安全分析语境中，我们通常重新定义：给定"约束集" $\mathcal{K}$（系统必须留在其中）和"目标集" $\mathcal{T}$（系统要避免的），Avoid BRT 定义为

\[\mathcal{V}_{\text{avoid}}(t) = \{x : \forall u(\cdot), \, \exists d(\cdot), \, \exists s \in [t, 0], \, \xi(s) \in \mathcal{T} \text{ 或 } \xi(s) \notin \mathcal{K}\}\]

即不仅要避免进入 $\mathcal{T}$，还要避免离开 $\mathcal{K}$。这是最一般的安全定义。

3.5 用 Dubins Car 画图解释¶

考虑 2D 简化 Dubins car（忽略航向角，只看位置平面）：

                    ┌──────────────────────────────┐
                    │         状态空间              │
                    │                              │
                    │     ┌───┐                    │
                    │     │   │  目标集 T           │
                    │     │   │  (危险区域)         │
                    │     └───┘                    │
                    │   ╱─────╲                    │
                    │  │  BRT  │  t = -1            │
                    │  │  ╱─╲  │                    │
                    │  │ │ T │ │                    │
                    │  │  ╲─╱  │                    │
                    │   ╲─────╱                    │
                    │  ╱────────╲                   │
                    │ │   BRT    │  t = -2           │
                    │ │  (更大)   │                   │
                    │  ╲────────╱                   │
                    │                              │
                    │  * A (安全: 在BRT外)           │
                    │                              │
                    └──────────────────────────────┘

随着 $|t|$ 增大（往更远的过去回溯），BRT 会**单调增长**：$\text{BRT}(t_1) \subseteq \text{BRT}(t_2)$ 当 $t_1 \ge t_2$。直觉上，给对手更多时间，它能威胁到的初始状态就更多。

当 $t \to -\infty$ 时，BRT 收敛到一个极限集合——这就是**无限时域 BRT**，也就是**最大可控不变集的补集**。

⚠️ 编程陷阱：$l(x)$ 的符号写反

错误做法：定义目标集 $\mathcal{T} = \{x : x_1^2 + x_2^2 \le R^2\}$ 时，写成 l = R**2 - x1**2 - x2**2（$l > 0$ 在目标集内部）。

现象：BRT 计算结果看起来"反了"——应该是危险区域的地方显示为安全，安全区域反而标记为危险。或者 BRT 从第一步就不扩张。

根本原因：HJ 可达性的标准约定是 $\mathcal{T} = \{l \le 0\}$。如果写反，$\{l \le 0\}$ 变成了目标集的补集，整个分析的含义反转。

正确做法：$l = x_1^2 + x_2^2 - R^2$。验证：在圆内 $x_1^2+x_2^2 < R^2$ 时 $l < 0$，在圆外 $l > 0$。

自检方法：在代码中打印 l(x_center_of_target) 和 l(x_far_away)，确认前者为负、后者为正。

💡 概念误区：混淆 "Reach" 与 "Avoid" 分析的量词结构

新手想法："BRS 和 BRT 不就是时间窗口不同吗？一个看终端时刻，一个看所有时刻。"

实际上：BRS 和 BRT 的区别不仅在于时间窗口，更在于**量词结构**和**物理含义**。

Reach BRS（$\exists u, \exists s$）："我能到达目标吗？"——用于规划。

Avoid BRT（$\forall u, \exists d, \exists s$）："对手能把我推入危险吗？"——用于安全。

Reach-Avoid（$\exists u, \forall d$）："在避开危险的同时，我能到达目标吗？"——用于安全+规划。

初学者常见的错误是把 Reach BRS 的代码直接用于安全分析（忘记加入对手），或者把 Avoid BRT 用于规划（控制器应该是 $\exists$ 而非 $\forall$）。

自检方法：在写代码前，先写下你的问题的量词结构，然后对照 hj_reachability 中的 control_mode 和 disturbance_mode 设置。

🧠 思维陷阱：认为 "BRT 收敛了就不需要继续计算"

新手想法："BRT 在 $t = -5$ 时看起来不再变化了，所以我用 $T = 5$ 就够了。"

实际上：BRT 的"视觉收敛"可能是假象。值函数 $V(x, t)$ 在零水平集附近可能仍在缓慢变化，只是变化太小在可视化中看不到。真正的收敛需要量化验证：$\|V(x, t) - V(x, t - \Delta t)\|_\infty < \epsilon$。

正确做法：计算相邻时间步的值函数最大差异 max_diff = jnp.max(jnp.abs(values[i] - values[i-1]))。当 max_diff < 1e-4 时才认为收敛。hj_reachability 支持 early stopping 回调来自动检测收敛。

练习 3.1 ⭐⭐¶

考虑一维系统 $\dot{x} = u + d$，$u \in [-1, 1]$，$d \in [-0.5, 0.5]$，目标集 $\mathcal{T} = \{x : x \ge 2\}$。

(a) 写出 $l(x)$ 使得 $\mathcal{T} = \{l \le 0\}$。 (b) 在最坏扰动 $d = 0.5$ 下，从 $x_0$ 出发的系统在 $T$ 时间内能到达的最远位置是多少？（假设控制器全力刹车 $u = -1$） (c) 求 $\text{BRT}(-T)$ 的解析表达式。 (d) 无限时域 BRT 是什么？

练习 3.2 ⭐⭐¶

考虑二维系统 $\dot{x}_1 = x_2$，$\dot{x}_2 = u + d$，$u \in [-2, 2]$，$d \in [-1, 1]$。目标集 $\mathcal{T} = \{x : x_1 \ge 5\}$。

(a) 这个系统的物理含义是什么？（提示：$x_1$ 是位置，$x_2$ 是速度） (b) 为什么一维分析不够，需要二维？（提示：即使位置远离目标集，如果速度很大...） (c) 画出 BRT 在 $(x_1, x_2)$ 平面上的定性形状。特别地，当 $x_2 > 0$（朝目标集运动）和 $x_2 < 0$（远离目标集运动）时，BRT 的边界有什么不同？ (d) 在边界上 $x_2 = 0$ 处，BRT 的 $x_1$ 临界值是多少？（这就是从静止状态出发的安全距离）

练习 3.3 ⭐⭐⭐¶

Reach-Avoid 问题。考虑 2D Dubins car，需要从起点到达目标区域 $\mathcal{G}$，同时避开障碍物 $\mathcal{O}$。

(a) 写出 Reach-Avoid 集的数学定义：$\text{RA}(t) = \{x_0 : \exists u(\cdot), \forall d(\cdot), \exists s \in [t, 0], \xi(s) \in \mathcal{G} \text{ 且 } \forall \tau \in [t, s], \xi(\tau) \notin \mathcal{O}\}$。解释每个量词的含义。 (b) Reach-Avoid 问题对应的 HJI 方程与纯 Avoid 问题有什么不同？（提示：需要同时有 $\max$ 和 $\min$ 两个 obstacle 算子） (c) 在 hj_reachability 中，Reach-Avoid 对应哪个 hamiltonian_postprocessor？

4. HJI 变分不等式 ⭐⭐¶

这是本章的数学核心。我们将从博弈论的角度推导出 BRT 满足的偏微分方程。

4.1 二人零和微分博弈框架¶

将安全分析建模为一个**二人零和微分博弈**：

玩家 1（控制器）：选择控制 $u(t) \in \mathcal{U}$，目标是**避免**进入目标集 $\mathcal{T}$。
玩家 2（扰动/对手）：选择扰动 $d(t) \in \mathcal{D}$，目标是**迫使**系统进入 $\mathcal{T}$。

动力学为

\[\dot{x} = f(x, u, d), \quad x(t_0) = x_0\]

其中 $f$ 对 $x$ Lipschitz 连续，$\mathcal{U}$ 和 $\mathcal{D}$ 是紧集。

博弈的"支付函数" (payoff)：控制器想**最大化** $l(\xi(s))$ 的某个度量（使系统远离目标集），扰动想**最小化**它。具体地，对 BRT 分析，我们关心的是轨迹在 $[t, 0]$ 内是否进入 $\mathcal{T}$：

\[J(x, t, u(\cdot), d(\cdot)) = \min_{s \in [t, 0]} l(\xi^{u,d}_{x,t}(s))\]

如果 $J \le 0$，则轨迹在某个时刻 $s$ 处于 $\mathcal{T}$ 内（因为 $l(\xi(s)) \le 0$）。

4.2 值函数¶

博弈的值函数定义为：

\[V(x, t) = \sup_{u(\cdot) \in \Gamma_u} \inf_{d(\cdot) \in \Gamma_d} J(x, t, u(\cdot), d(\cdot))\]

其中 $\Gamma_u, \Gamma_d$ 是**非预期策略** (non-anticipative strategies) 的集合——即控制器的策略 $\gamma_u$ 在时刻 $s$ 只能依赖于 $d$ 在 $[t, s]$ 上的信息，不能"偷看"未来的扰动。

\[\Gamma_u = \{\gamma_u : \mathcal{D}_{[t,0]} \to \mathcal{U}_{[t,0]} \mid \gamma_u \text{ 非预期}\}\]

为什么是 $\sup_u \inf_d$ 而不是 $\inf_d \sup_u$？

这里控制器先出手（选策略），扰动后出手（选扰动）——这是**上值** (upper value)。如果反过来，就是**下值** (lower value)。当 Isaacs 条件**成立时，上值 = 下值 = 博弈值，博弈存在**鞍点。

BRT 与值函数的关系：

\[\text{BRT}(t) = \{x : V(x, t) \le 0\}\]

这是一个极其优美的结果：BRT 恰好是值函数的**零水平集**。

4.3 HJI 方程的完整推导¶

现在推导 $V(x, t)$ 满足的偏微分方程。分两步：首先处理终端时刻的 BRS，然后加入"管道"约束得到 BRT。

Step 1：后向可达集 (BRS) 的 HJI 方程

如果我们暂时不考虑"管道"（即只关心终端时刻 $s = 0$ 是否在 $\mathcal{T}$ 内），值函数简化为

\[V_{\text{BRS}}(x, t) = \sup_{\gamma_u} \inf_{d(\cdot)} l(\xi^{\gamma_u(d), d}_{x,t}(0))\]

由**动态规划原理** (DPP)，对任意 $h > 0$：

\[V_{\text{BRS}}(x, t) = \sup_{\gamma_u} \inf_{d(\cdot)} V_{\text{BRS}}(\xi_{x,t}(t+h), \, t+h)\]

假设 $V_{\text{BRS}} \in C^1$（后面用黏性解处理非光滑情形），Taylor 展开：

\[V_{\text{BRS}}(\xi(t+h), t+h) = V_{\text{BRS}}(x, t) + h\frac{\partial V}{\partial t} + h\nabla_x V \cdot f(x, u, d) + O(h^2)\]

代入 DPP，消去 $V(x,t)$，除以 $h$，令 $h \to 0$：

\[0 = \frac{\partial V}{\partial t} + \sup_{u} \inf_{d} \{\nabla_x V \cdot f(x, u, d)\}\]

或者等价地（控制器最大化以远离目标集，扰动最小化以靠近目标集——注意这里 $V$ 越大越安全）：

\[\boxed{\frac{\partial V}{\partial t} + \max_{u \in \mathcal{U}} \min_{d \in \mathcal{D}} \{\nabla_x V \cdot f(x, u, d)\} = 0}\]

终端条件为 $V(x, 0) = l(x)$。

这就是 Hamilton-Jacobi-Isaacs (HJI) 方程。

Step 2：后向可达管 (BRT) 的 HJI 变分不等式

BRT 还需要考虑轨迹在**中间时刻**是否进入 $\mathcal{T}$。值函数变成

\[V(x, t) = \sup_{\gamma_u} \inf_{d(\cdot)} \min_{s \in [t, 0]} l(\xi(s))\]

关键观察：一旦 $l(\xi(s_0)) \le 0$（系统进入了目标集），则 $\min_{s} l(\xi(s)) \le 0$，之后的控制已无意义——系统已经"失败"了。这意味着值函数一旦变为非正，就不应该再增大。

数学上，DPP 变成

\[V(x, t) = \sup_{\gamma_u} \inf_{d(\cdot)} \min\{l(x), \, V(\xi(t+h), t+h)\}\]

$\min$ 操作的含义：值函数不能超过 $l(x)$ 本身——如果当前状态已经在目标集内（$l(x) \le 0$），那么不管之后发生什么，$V \le 0$。

将此 DPP 推导为 PDE，得到 HJI 变分不等式：

\[\boxed{\frac{\partial V}{\partial t} + \min\left\{0, \, \max_{u \in \mathcal{U}} \min_{d \in \mathcal{D}} \nabla_x V \cdot f(x, u, d)\right\} = 0}\]

终端条件 $V(x, 0) = l(x)$。

$\min\{0, \cdot\}$ 的直觉： - 如果 Hamiltonian $H = \max_u \min_d \nabla V \cdot f > 0$，说明值函数在增大（系统在远离目标集）——但 BRT 要求我们记住"曾经进入过目标集"，所以用 $\min\{0, H\}$ 把正的贡献截掉。 - 如果 $H \le 0$，说明值函数在减小（系统在靠近目标集），正常传播。

这就是"obstacle 算子"——它像一个障碍，阻止值函数在已被污染的区域恢复。

4.4 与控制理论/40_HJB方程与黏性解的关系¶

让我们把本节的结果与控制理论/40_HJB方程与黏性解中的 HJB 方程做精确对比：

维度	HJB (控制理论/40_HJB方程与黏性解)	HJI (本节)
方程	$V_t + \min_u\{L + \nabla V \cdot f\} = 0$	$V_t + \min\{0, \max_u \min_d \nabla V \cdot f\} = 0$
运行代价 $L$	有（积分代价）	无（只关心到达与否）
扰动 $d$	无	有（对抗性）
量词	$\min_u$（单人最优）	$\max_u \min_d$（二人博弈）
Obstacle 算子	无	$\min\{0, \cdot\}$（BRT 用）
终端条件	$V(x, T) = \Phi(x)$	$V(x, 0) = l(x)$
时间方向	后向（从 $T$ 向 $t$）	后向（从 $0$ 向 $t$）

要点：HJI 是 HJB 在以下三方面的推广：(1) 加了对手 $d$；(2) 去掉了运行代价 $L$（可达性只关心到达与否）；(3) 加了 obstacle 算子。

4.5 Isaacs 条件¶

Isaacs 条件：对所有 $(x, p) \in \mathbb{R}^n \times \mathbb{R}^n$，

\[\max_{u \in \mathcal{U}} \min_{d \in \mathcal{D}} p \cdot f(x, u, d) = \min_{d \in \mathcal{D}} \max_{u \in \mathcal{U}} p \cdot f(x, u, d)\]

即 $\max$ 和 $\min$ 可以交换顺序。这等价于博弈存在**鞍点** (saddle point)。

什么时候 Isaacs 条件成立？ 最重要的充分条件是 $f$ 关于 $(u, d)$ 可分离：

\[f(x, u, d) = f_0(x) + g_u(x)u + g_d(x)d\]

在这种情况下，$p \cdot f = p \cdot f_0 + p \cdot g_u u + p \cdot g_d d$ 关于 $u$ 和 $d$ 是分离的，max-min = min-max 由鞍点条件直接满足。

更一般地，如果 Hamiltonian $H(x, p, u, d) = p \cdot f(x, u, d)$ 关于 $u$ 是凸的、关于 $d$ 是凹的（或反之，取决于符号约定），则由 Sion 极小极大定理，Isaacs 条件成立。

Isaacs 条件的深层理解

Isaacs 条件的本质是博弈的**信息结构无关性**。让我们仔细分析为什么这很重要。

在微分博弈中，有两种信息结构： - 开环 (open-loop)：双方在博弈开始时同时宣布完整策略 $u(\cdot), d(\cdot)$。 - 闭环 (closed-loop / feedback)：双方根据当前状态实时决策。

在开环结构下，量词顺序（谁先出牌）至关重要：$\max_u \min_d \ne \min_d \max_u$（一般情况下）。但当 Isaacs 条件成立时，$\max_u \min_d = \min_d \max_u$，这意味着**博弈的值与出牌顺序无关**——对控制器来说，知道或不知道对手的当前决策，结果都一样。

为什么可分离性保证 Isaacs 条件？ 当 $f(x,u,d) = f_0(x) + g_u(x)u + g_d(x)d$ 时，Hamiltonian 分解为

\[H(x, p, u, d) = p \cdot f_0(x) + [p \cdot g_u(x)] u + [p \cdot g_d(x)] d\]

$u$ 和 $d$ 出现在**不同的**可加项中。$\max_u$ 只作用于含 $u$ 的项，$\min_d$ 只作用于含 $d$ 的项，两个优化**互不干扰**，所以交换顺序不改变结果。这在物理上对应于"控制力和扰动力作用在不同的通道上"。

Isaacs 条件不成立的例子。考虑动力学 $\dot{x} = u \cdot d$（控制和扰动耦合——两者相乘），$u \in [-1,1], d \in [-1,1]$。则

\[H = p \cdot u \cdot d\]

$\max_u \min_d (p \cdot u \cdot d)$：控制器先选 $u$，对手后选 $d$ 使 $p \cdot u \cdot d$ 最小。不管 $u$ 选什么（假设 $p > 0, u > 0$），对手选 $d = -1$。所以结果为 $\max_u |p \cdot u| \cdot (-1) = -|p|$。
$\min_d \max_u (p \cdot u \cdot d)$：对手先选 $d$，控制器后选 $u$ 使 $p \cdot u \cdot d$ 最大。如果 $d > 0$（假设 $p > 0$），控制器选 $u = 1$；如果 $d < 0$，选 $u = -1$。结果为 $\min_d |p \cdot d| = 0$（对手选 $d = 0$）。

于是 $\max_u \min_d = -|p| \ne 0 = \min_d \max_u$。Isaacs 条件不成立！物理含义：当控制和扰动耦合时，谁先出牌的信息优势真的会改变博弈结果。

Sion 极小极大定理的适用条件。Sion 定理要求：(1) $\mathcal{U}$ 和 $\mathcal{D}$ 是紧凸集；(2) $H$ 关于 $u$ 是拟凸的 (quasiconvex)，关于 $d$ 是拟凹的 (quasiconcave)。对于机器人学中常见的控制仿射系统，$H$ 关于 $u$ 和 $d$ 是线性的，线性函数既凸又凹，自然满足 Sion 条件。

陷阱警告

如果 Isaacs 条件不成立，上值和下值不同，博弈的"值"不唯一。此时需要明确指定信息结构：是控制器先承诺策略（上值），还是扰动先承诺（下值）。在安全分析中，通常取**上值**（控制器先出，扰动后出），因为这是最保守的。

4.5.1 HJI 方程每一项的物理含义深度解析¶

让我们逐项剖析 HJI 变分不等式 $V_t + \min\{0, \max_u \min_d \nabla V \cdot f\} = 0$ 的每一个组成部分，理解它们为何必须以这种形式出现。

$V_t$（时间偏导数）——"安全价值的时间变化率"

$V_t = \partial V / \partial t$ 描述了"在不考虑状态变化的情况下，纯粹因为时间流逝，安全价值如何变化"。在 BRT 分析中，$t$ 从 $0$ 向负方向演化（回到过去），所以 $V_t < 0$ 意味着"往过去追溯时，安全集在缩小"（更多状态变得不安全）。

为什么需要时间导数？因为 BRT 是一个**随时间变化**的集合——给对手更多时间，它能威胁到的状态越多。$V_t$ 正是捕捉这种时间效应的项。

如果我们只关心**无限时域** (infinite horizon) 的 BRT，则 $V$ 收敛到稳态，$V_t = 0$，方程退化为**静态 HJI**：$\min\{0, \max_u \min_d \nabla V \cdot f\} = 0$。

$\nabla_x V \cdot f(x, u, d)$（Hamiltonian 的核心）——"沿轨迹的值函数变化"

这一项等于 $\frac{d}{dt} V(\xi(t), t) - V_t$——即值函数沿系统轨迹的**物质导数**减去时间偏导。换句话说，$\nabla V \cdot f$ 描述了"因为系统状态在运动，值函数变化了多少"。

如果 $\nabla V \cdot f > 0$：轨迹在朝着值函数增大的方向运动——即远离目标集（更安全）。
如果 $\nabla V \cdot f < 0$：轨迹在朝着值函数减小的方向运动——即靠近目标集（更危险）。

$\max_u$ 表示控制器尽力让系统远离危险，$\min_d$ 表示对手尽力把系统推向危险。这两个优化嵌套在一起，描述了一个**对抗博弈**：即使在最坏扰动下，控制器能做到的最好结果。

$\min\{0, \cdot\}$（obstacle 算子）——"不可逆的失败记忆"

这是 BRT 与 BRS 的关键区别。假设没有 obstacle 算子（即 BRS 方程 $V_t + \max_u \min_d \nabla V \cdot f = 0$），那么值函数可以自由上升和下降——系统进入目标集后还可以"逃出来"，$V$ 恢复为正。

但在安全分析中，"进入过危险区域"就是**不可逆的失败**。obstacle 算子 $\min\{0, H\}$ 确保：一旦 $V \le 0$（系统在某时刻进入了 $\mathcal{T}$），值函数不会再增大。$H > 0$ 意味着系统正在远离目标集，但 $\min\{0, H\} = 0$ 把这个"好消息"截断了——因为历史上已经进入过目标集，安全已经被破坏。

类比：考试中，一旦答错了一道不可修改的题目（$V \le 0$），即使后面答对再多题目（$H > 0$），那道错题的分数也找不回来了。

终端条件 $V(x, 0) = l(x)$——"最终的安全判据"

终端时刻 $t = 0$ 时，$V$ 就是目标集的隐式函数 $l(x)$。这是一个"最终审判"——在终端时刻，你的安全状态完全由你是否在目标集内决定。方程从这个终端条件出发，反向传播安全信息。

⚠️ 概念误区：认为 HJI 方程中的 $\max_u \min_d$ 意味着"控制器比扰动强"

新手想法："$\max_u \min_d$ 表示控制器先最大化、扰动后最小化，所以控制器有优势？"

实际上：$\max_u \min_d$ 不是说控制器更强。它描述的是一种**信息结构**：控制器先承诺策略（但策略可以是状态反馈的），扰动看到策略后选择最坏的应对。这实际上是**最保守**的假设——给了扰动信息优势。如果博弈的值 $V(x) > 0$，说明即使在这种最保守的假设下，控制器仍然能保证安全。

根本原因：在非预期策略框架下，$\sup_{\gamma_u} \inf_d$ 对应的是"控制器选反馈策略，扰动选最坏的因果响应"。这与 $\inf_d \sup_u$（扰动先承诺，控制器后应对）不同。Isaacs 条件保证两者相等。

自检方法：如果你的 BRT 是空集（没有任何状态不安全），检查你的 $\max/\min$ 是否写反了。写反会让控制器和扰动"合作"而非对抗。

4.6 黏性解¶

HJI 方程的解一般不光滑（特征线交叉），需要用**黏性解** (viscosity solution) 的框架。这在控制理论/40_HJB方程与黏性解中已详细讲过，此处直接引用：

定义：$V$ 是 HJI 的黏性解，当且仅当对任何光滑测试函数 $\phi$ 使得 $V - \phi$ 在 $x_0$ 处取**局部最大值**时（viscosity subsolution 条件），有 $\phi_t(x_0) + \min\{0, \max_u \min_d \nabla\phi \cdot f\} \le 0$；对称地定义 supersolution。
存在唯一性：在适当条件下（$f$ Lipschitz、$l$ 连续、$\mathcal{U}, \mathcal{D}$ 紧），HJI 的黏性解存在且唯一（Crandall-Ishii-Lions 1992 的用户指南是标准参考）。

重要：黏性解的概念是使 HJ 可达性在数学上严格的关键。Level-Set 数值方法的收敛性分析也依赖于它——数值解是黏性解的一致逼近。

自测检查点 4 ⭐⭐¶

请回答以下问题，检验你对 HJI 方程的理解：

HJI 方程中 $\max_u \min_d$ 的物理含义是什么？ 如果改成 $\min_u \max_d$ 会发生什么？
obstacle 算子 $\min\{0, \cdot\}$ 什么时候起作用？ 如果去掉它，方程描述的是什么？
Isaacs 条件在什么物理场景下最容易成立？
写出 Dubins car 追逃博弈的 HJI 方程，其中 evader 的控制为 $\omega_e$，pursuer 的控制为 $\omega_p$，状态为相对坐标 $(x_r, y_r, \theta_r)$。

5. Level-Set 数值方法 ⭐⭐⭐¶

理论推导完毕，现在转向计算。如何在计算机上求解 HJI 变分不等式？

5.1 核心思想：在网格上演化值函数¶

Level-Set 方法的核心思想来自 Osher-Sethian 1988：

将状态空间 $\mathbb{R}^n$ 离散化为**均匀网格**，网格间距 $\Delta x_i$。
在网格上存储值函数 $V$ 的值。
从终端条件 $V(x, 0) = l(x)$ 出发，**反向**时间步进。
在每个时间步，用数值格式更新 $V$。

网格定义：对 $n$ 维系统，状态空间的每个维度用 $N_i$ 个网格点离散化，总网格点数为 $N = \prod_{i=1}^n N_i$。

5.2 数值 Hamiltonian：Lax-Friedrichs 格式¶

HJI 方程的核心难度在于 Hamiltonian

\[H(x, p) = \max_{u \in \mathcal{U}} \min_{d \in \mathcal{D}} p \cdot f(x, u, d)\]

中的 $p = \nabla V$ 需要用**空间差分**逼近。关键是要使用**迎风格式** (upwind scheme) 以保证数值稳定性。

**Lax-Friedrichs (LF) 数值 Hamiltonian**是最常用的选择：

\[\hat{H}(x, p^+, p^-) = H\left(x, \frac{p^+ + p^-}{2}\right) - \sum_{i=1}^n \alpha_i \frac{p_i^+ - p_i^-}{2}\]

其中： - $p_i^+ = \frac{V_{j+1} - V_j}{\Delta x_i}$ 是第 $i$ 维的**前向差分** - $p_i^- = \frac{V_j - V_{j-1}}{\Delta x_i}$ 是第 $i$ 维的**后向差分** - $\alpha_i = \max_{x, u, d} \left|\frac{\partial H}{\partial p_i}\right|$ 是第 $i$ 维的**数值黏性系数**

LF 格式的直觉：取前向和后向差分的平均值作为梯度的近似，然后加入人工黏性 $\alpha_i$ 来稳定格式。黏性量正比于前向和后向差分之差——在解光滑处，差分一致，黏性消失；在不光滑处，黏性起到正则化作用。

全局 LF vs 局部 LF：全局 LF 的 $\alpha_i$ 在整个网格上取最大值，简单但过于耗散。局部 LF 在每个网格点分别计算 $\alpha_i$，更精确但稍复杂。实际工程中**局部 LF 是标准选择**。

5.3 时间积分与 CFL 条件¶

空间离散化后，HJI 变成 ODE：

\[\frac{dV_j}{dt} = -\min\left\{0, \, \hat{H}(x_j, p^+_j, p^-_j)\right\}\]

时间积分通常用**TVD Runge-Kutta** 方法（Total Variation Diminishing，保证不产生伪振荡）。最简单的是 3 阶 TVD-RK3：

\[\begin{aligned} V^{(1)} &= V^n + \Delta t \, L(V^n) \\ V^{(2)} &= \frac{3}{4}V^n + \frac{1}{4}V^{(1)} + \frac{1}{4}\Delta t \, L(V^{(1)}) \\ V^{n+1} &= \frac{1}{3}V^n + \frac{2}{3}V^{(2)} + \frac{2}{3}\Delta t \, L(V^{(2)}) \end{aligned}\]

其中 $L(V)$ 是空间算子的离散化。

CFL 稳定性条件 (Courant-Friedrichs-Lewy)：时间步长必须满足

\[\Delta t \le \frac{1}{\sum_{i=1}^n \frac{\alpha_i}{\Delta x_i}}\]

其中 $\alpha_i = \max |f_i|$ 是每个维度上速度的最大值。直觉：信息传播一步不能超过一个网格单元。

陷阱警告

CFL 条件是**必要的**——违反它会导致数值爆炸。但对于非线性系统，$\alpha_i$ 依赖于当前的 $V$ 值，所以可能需要在每个时间步重新计算 CFL 条件。一些实现（如 hj_reachability）自动处理这一点。

5.3.1 为什么必须使用迎风差分 (Upwind Differencing)？¶

这个问题看似纯粹技术性，实则涉及双曲型 PDE 的核心特性——信息传播方向。

双曲型 PDE 的特征线。HJI 方程是一个一阶双曲型 PDE（不含二阶导数项）。双曲型 PDE 的核心特征是信息沿**特征线** (characteristic) 传播。对一维 advection 方程 $V_t + a V_x = 0$（$a > 0$），信息从左向右传播——解在 $(x, t)$ 处的值由 $(x - a\Delta t, t - \Delta t)$ 决定。

如果我们用**中心差分** $V_x \approx (V_{j+1} - V_{j-1})/(2\Delta x)$，则同时使用了"上游"和"下游"的信息。这看起来更"对称"，但实际上引入了与物理传播方向相反的信息——这导致**伪振荡** (spurious oscillations) 和数值不稳定。

迎风格式的核心思想：只使用**信息流上游**的差分。当 $a > 0$（信息从左向右传播），用后向差分 $V_x \approx (V_j - V_{j-1})/\Delta x$（从左边取信息）；当 $a < 0$（信息从右向左传播），用前向差分 $V_x \approx (V_{j+1} - V_j)/\Delta x$。

为什么 HJ 方程特别需要迎风？ HJ 方程的解通常会出现**扭折** (kinks)——梯度不连续的点。在扭折处，左右两侧的特征线汇聚，形成**激波** (shock)。如果不用迎风格式，数值方法在扭折附近会产生野蛮的振荡，完全破坏零水平集的准确性。

Lax-Friedrichs 的隐式迎风机制。LF 格式看起来用了"中心差分+人工黏性"，但实际上人工黏性项 $\alpha_i (p_i^+ - p_i^-)/2$ 正好实现了迎风效果：

\[\hat{H} = H\left(\frac{p^+ + p^-}{2}\right) - \sum_i \frac{\alpha_i}{2}(p_i^+ - p_i^-)\]

改写为：

\[\hat{H} = H\left(\frac{p^+ + p^-}{2}\right) - \sum_i \frac{\alpha_i}{2}\left(\frac{V_{j+1} - V_j}{\Delta x_i} - \frac{V_j - V_{j-1}}{\Delta x_i}\right)\]

\[= H\left(\frac{p^+ + p^-}{2}\right) - \sum_i \frac{\alpha_i}{2\Delta x_i}(V_{j+1} - 2V_j + V_{j-1})\]

最后一项正是**数值扩散**——形式上等价于加了一个 $\alpha_i \Delta x_i / 2$ 的黏性系数。这个人工黏性在解光滑处（$V_{j+1} - 2V_j + V_{j-1} \approx 0$）几乎消失，在扭折处（$V_{j+1} - 2V_j + V_{j-1}$ 大）提供足够的稳定化。

5.3.2 CFL 条件的严格推导¶

CFL (Courant-Friedrichs-Lewy) 条件不是一个经验法则，而是有严格的数学推导。让我们从一维情况开始。

一维 advection 方程 $V_t + aV_x = 0$ 的 CFL 推导

使用前向 Euler 时间步进 + 迎风空间差分（$a > 0$ 时用后向差分）：

\[V_j^{n+1} = V_j^n - a \frac{\Delta t}{\Delta x}(V_j^n - V_{j-1}^n) = (1 - \nu)V_j^n + \nu V_{j-1}^n\]

其中 Courant 数 $\nu = a\Delta t / \Delta x$。注意 $V_j^{n+1}$ 是 $V_j^n$ 和 $V_{j-1}^n$ 的**凸组合**——当且仅当 $0 \le \nu \le 1$。

如果 $\nu > 1$：$(1-\nu) < 0$，系数不再非负，更新变成"外推"而非"内插"，导致不稳定。
物理含义：$\nu > 1$ 意味着一个时间步内，信息传播的距离 $a\Delta t$ 超过了一个网格单元 $\Delta x$。信息"跳过"了网格点，数值格式无法捕捉。

因此 CFL 条件为 $\nu = a\Delta t/\Delta x \le 1$，即 $\Delta t \le \Delta x / a$。

推广到多维 HJI

在多维情况下，每个维度的最大信息传播速度为 $\alpha_i = \max |f_i(x,u,d)|$。由**维度分裂**的稳定性分析，各维度的 Courant 数之和不能超过 1：

\[\sum_{i=1}^n \frac{\alpha_i \Delta t}{\Delta x_i} \le 1 \quad \Rightarrow \quad \Delta t \le \frac{1}{\sum_{i=1}^n \alpha_i / \Delta x_i}\]

这就是多维 CFL 条件。注意维度越高，允许的时间步长越小——这也是高维 HJ 计算代价高的原因之一。

实际中的自适应 CFL。对非线性系统，$\alpha_i = \max_{x,u,d} |f_i(x,u,d)|$ 可能随 $V$ 的演化而变化。保险的做法是每个时间步重新计算 $\alpha_i$ 并调整 $\Delta t$。hj_reachability 库默认这样做。

⚠️ 编程陷阱：手动设置过大的 $\Delta t$

错误做法：为了加速计算，手动设置 dt = 0.1 而不检查 CFL 条件。

现象：值函数在前几个时间步看起来正常，然后突然出现 NaN 或值函数到处变成 $\pm 10^{15}$。BRT 的形状完全错乱。

根本原因：违反 CFL 条件后，数值误差以指数速度增长。对于 3D 问题，网格间距 $\Delta x \approx 0.2$，速度 $\alpha \approx 5$，CFL 要求 $\Delta t \le 1/(3 \times 5/0.2) = 0.013$——远小于 0.1。

正确做法：使用库提供的自动 CFL 计算功能，或手动计算 $\Delta t = \text{CFL}_{\text{safety}} / \sum_i (\alpha_i/\Delta x_i)$，其中 $\text{CFL}_{\text{safety}} = 0.8$（留 20% 安全余量）。

自检方法：运行计算后检查值函数的最大/最小值是否合理。如果 $|V| > 100 \times |l|_{\max}$，几乎肯定是 CFL 违例。

🧠 思维陷阱：认为"更小的 $\Delta x$ 总是更好"

新手想法："精度不够？把网格加密 2 倍就行了！"

实际上：将每维网格加密 2 倍，在 $n$ 维问题中意味着：(1) 内存增加 $2^n$ 倍；(2) CFL 条件使 $\Delta t$ 减半（$\Delta t \propto \Delta x$）；(3) 总时间步数加倍；(4) 每步计算量增加 $2^n$ 倍。总计算量增加 $2^{n+1}$ 倍！对 4D 问题，加密 2 倍 = 计算量增加 32 倍。

正确思维：在加密网格之前，先检查当前网格的收敛性——用两个分辨率计算，比较零水平集的 Hausdorff 距离。如果已经收敛，加密是浪费；如果未收敛，先尝试用更高阶的空间离散化（如 5 阶 WENO 代替 1 阶迎风）提升精度，这通常比加密网格更划算。

5.4 维度灾难¶

Level-Set 方法的致命弱点是**维度灾难** (curse of dimensionality)：

维度 $n$	每维网格点 $N_i = 100$	总网格点	内存 (float64)
2	100	$10^4$	80 KB
3	100	$10^6$	8 MB
4	100	$10^8$	800 MB
5	50	$3 \times 10^8$	2.4 GB
6	40	$4 \times 10^9$	32 GB
7	30	$2 \times 10^{10}$	160 GB

实际可算维度：$n \le 5 \sim 6$。这是 HJ 可达性的根本限制，也是 DeepReach（第 7 节）等方法出现的动机。

5.5 工具生态¶

工具	语言	维度	特点
toolboxLS (Mitchell 2004)	MATLAB	$\le 4$	经典标杆，文档完善
helperOC (Berkeley)	MATLAB	$\le 6$	基于 toolboxLS，优化了内存
BEACLS	C++ / CUDA	$\le 6$	GPU 加速
`hj_reachability` (Hsu 2022)	Python / JAX	$\le 5$	自动微分、JIT 编译
`optimized_dp`	Python	$\le 6$	基于 heterocl 的优化

本章使用 hj_reachability——它基于 JAX，代码简洁、支持自动微分，适合教学。

5.6 代码示例：2D Double Integrator 的 BRT¶

以下代码计算一个 2D 双积分器 $\dot{x}_1 = x_2, \dot{x}_2 = u + d$ 的 BRT。

"""
2D Double Integrator BRT 计算
动力学: dx1/dt = x2, dx2/dt = u + d
u in [-1, 1], d in [-0.25, 0.25]
目标集: |x1| <= 0.5 (位置在 [-0.5, 0.5] 以内视为碰撞)
"""

import jax
import jax.numpy as jnp
import hj_reachability as hj
import matplotlib.pyplot as plt

# ======== 第1步: 定义动力学 ========
class DoubleIntegrator(hj.ControlAndDisturbanceAffineDynamics):
    """双积分器: 控制仿射 + 扰动仿射."""

    def __init__(self):
        # 控制范围 u in [-1, 1], 扰动范围 d in [-0.25, 0.25]
        super().__init__(
            control_mode="max",      # 控制器最大化 V (想远离危险)
            disturbance_mode="min",  # 扰动最小化 V (想靠近危险)
            control_space=hj.sets.Box(lo=jnp.array([-1.0]),
                                      hi=jnp.array([1.0])),
            disturbance_space=hj.sets.Box(lo=jnp.array([-0.25]),
                                           hi=jnp.array([0.25])),
        )

    def open_loop_dynamics(self, x, time):
        """自由动力学 f_0(x) = [x2, 0]."""
        return jnp.array([x[1], 0.0])

    def control_jacobian(self, x, time):
        """控制矩阵 g_u(x) = [[0], [1]]."""
        return jnp.array([[0.0], [1.0]])

    def disturbance_jacobian(self, x, time):
        """扰动矩阵 g_d(x) = [[0], [1]]."""
        return jnp.array([[0.0], [1.0]])

# ======== 第2步: 定义网格 ========
grid = hj.Grid.from_lattice_parameters_and_boundary_conditions(
    domain=hj.sets.Box(
        lo=jnp.array([-3.0, -3.0]),  # 状态下界
        hi=jnp.array([3.0, 3.0]),    # 状态上界
    ),
    shape=(101, 101),  # 每维 101 个网格点
)

# ======== 第3步: 定义目标集 ========
# 目标集: |x1| <= 0.5, 即 l(x) = |x1| - 0.5 <= 0
# (注: hj_reachability 用 <= 0 作为目标集约定)
target_values = jnp.abs(grid.states[..., 0]) - 0.5

# ======== 第4步: 求解 HJI 方程 ========
dynamics = DoubleIntegrator()
solver_settings = hj.SolverSettings.with_accuracy(
    accuracy="very_high",
    hamiltonian_postprocessor=hj.solver.backwards_reachable_tube,
    # 关键: 使用 BRT (obstacle 算子)
)

# 反向时间计算 2 秒
time_steps = jnp.linspace(0, -2.0, 81)  # 从 t=0 到 t=-2
values = hj.solve(solver_settings, dynamics, grid,
                  time_steps, target_values)

# ======== 第5步: 可视化 ========
fig, axes = plt.subplots(1, 3, figsize=(15, 5))

for idx, t_idx in enumerate([0, 40, 80]):
    ax = axes[idx]
    ax.contourf(grid.coordinate_vectors[0],
                grid.coordinate_vectors[1],
                values[t_idx].T, levels=50, cmap="RdBu")
    ax.contour(grid.coordinate_vectors[0],
               grid.coordinate_vectors[1],
               values[t_idx].T, levels=[0], colors="black",
               linewidths=2)
    ax.set_xlabel("$x_1$ (position)")
    ax.set_ylabel("$x_2$ (velocity)")
    ax.set_title(f"$t = {time_steps[t_idx]:.1f}$")
    ax.set_aspect("equal")

fig.suptitle("Double Integrator BRT Evolution", fontsize=14)
plt.tight_layout()
plt.savefig("double_integrator_brt.png", dpi=150)
plt.show()

代码解读： 1. ControlAndDisturbanceAffineDynamics 是 hj_reachability 的基类，适用于 $\dot{x} = f_0(x) + g_u(x)u + g_d(x)d$ 形式。 2. control_mode="max" 和 disturbance_mode="min" 对应 $\max_u \min_d$。 3. backwards_reachable_tube 是 Hamiltonian 后处理器，实现 $\min\{0, H\}$ 的 obstacle 算子。如果不加它，计算的是 BRS 而不是 BRT。 4. 时间从 $0$ 向负方向步进——这是**后向**计算。

5.7 进阶案例：4D 双积分器（位置 + 速度，二维平面） ⭐⭐⭐¶

上面的 2D 双积分器只考虑了一个维度的位置和速度。在实际机器人中，我们通常需要考虑二维平面运动。以下是 4D 双积分器的完整实现——这已经接近了网格法的"舒适区"上限。

动力学：质点在 2D 平面上运动，状态 $(x_1, x_2, v_1, v_2)$，控制为加速度 $(a_1, a_2)$，扰动为外力 $(d_1, d_2)$。

\[\dot{x}_1 = v_1, \quad \dot{x}_2 = v_2, \quad \dot{v}_1 = a_1 + d_1, \quad \dot{v}_2 = a_2 + d_2\]

"""
4D Double Integrator BRT: 2D 平面上带扰动的质点运动.
状态: (x1, x2, v1, v2)
控制: (a1, a2) in [-1, 1]^2
扰动: (d1, d2) in [-0.3, 0.3]^2
目标集: x1^2 + x2^2 <= R^2 (圆形碰撞区域)
"""

import jax
import jax.numpy as jnp
import hj_reachability as hj

class DoubleIntegrator4D(hj.ControlAndDisturbanceAffineDynamics):
    """4D 双积分器: 2D 平面质点运动."""

    def __init__(self, u_max=1.0, d_max=0.3):
        super().__init__(
            control_mode="max",
            disturbance_mode="min",
            control_space=hj.sets.Box(
                lo=jnp.array([-u_max, -u_max]),
                hi=jnp.array([u_max, u_max])),
            disturbance_space=hj.sets.Box(
                lo=jnp.array([-d_max, -d_max]),
                hi=jnp.array([d_max, d_max])),
        )

    def open_loop_dynamics(self, x, time):
        # x = [x1, x2, v1, v2]
        return jnp.array([x[2], x[3], 0.0, 0.0])

    def control_jacobian(self, x, time):
        # 控制加速度直接作用于速度
        return jnp.array([
            [0.0, 0.0],
            [0.0, 0.0],
            [1.0, 0.0],
            [0.0, 1.0],
        ])

    def disturbance_jacobian(self, x, time):
        # 扰动也作用于加速度通道
        return jnp.array([
            [0.0, 0.0],
            [0.0, 0.0],
            [1.0, 0.0],
            [0.0, 1.0],
        ])

# 4D 网格: 每维 41 个点 (41^4 ≈ 2.8M 个网格点)
# 注意: 这已经需要约 22 MB 内存, 计算需要几分钟
grid = hj.Grid.from_lattice_parameters_and_boundary_conditions(
    domain=hj.sets.Box(
        lo=jnp.array([-4.0, -4.0, -3.0, -3.0]),
        hi=jnp.array([4.0, 4.0, 3.0, 3.0]),
    ),
    shape=(41, 41, 41, 41),
)

# 目标集: x1^2 + x2^2 <= 1 (与速度无关)
R = 1.0
target_values = grid.states[..., 0]**2 + grid.states[..., 1]**2 - R**2

dynamics = DoubleIntegrator4D()
solver_settings = hj.SolverSettings.with_accuracy(
    accuracy="very_high",
    hamiltonian_postprocessor=hj.solver.backwards_reachable_tube,
)

# 计算 1.5 秒的 BRT
time_steps = jnp.linspace(0, -1.5, 61)
values = hj.solve(solver_settings, dynamics, grid,
                  time_steps, target_values)

# 可视化: 固定 v1=0, v2=0, 看 (x1, x2) 平面上的 BRT
import matplotlib.pyplot as plt
fig, axes = plt.subplots(1, 3, figsize=(15, 5))

# v1, v2 的中间索引
v1_idx = 20  # v1 ≈ 0
v2_idx = 20  # v2 ≈ 0

for i, t_idx in enumerate([0, 30, 60]):
    ax = axes[i]
    slice_2d = values[t_idx][:, :, v1_idx, v2_idx]
    ax.contourf(grid.coordinate_vectors[0],
                grid.coordinate_vectors[1],
                slice_2d.T, levels=50, cmap="RdBu")
    ax.contour(grid.coordinate_vectors[0],
               grid.coordinate_vectors[1],
               slice_2d.T, levels=[0], colors="black", linewidths=2)
    ax.set_xlabel("$x_1$")
    ax.set_ylabel("$x_2$")
    ax.set_title(f"BRT at t={time_steps[t_idx]:.2f}, $v_1=v_2=0$")
    ax.set_aspect("equal")

plt.tight_layout()
plt.savefig("4d_double_integrator_brt.png", dpi=150)
plt.show()

代码要点： 1. 4D 网格 $41^4 \approx 2.8 \times 10^6$ 个点，float64 内存约 22 MB——还在可接受范围内。如果增加到 $81^4 \approx 4.3 \times 10^7$，内存增长到 344 MB。 2. 目标集只涉及位置 $(x_1, x_2)$，不涉及速度——这在物理上合理（碰撞只看位置）。但 BRT 同时涉及位置和速度，因为高速接近障碍物的状态更危险。 3. 可视化时需要固定两个维度（速度），看另外两个维度（位置）的 2D 切片。

5.8 进阶案例：6D 近悬停四旋翼安全分析 ⭐⭐⭐⭐¶

6D 是网格法的极限。以下展示一个简化的四旋翼近悬停模型的安全分析。

近悬停动力学（线性化模型）：考虑四旋翼在 $(x, y, z)$ 三个平移自由度上的运动，每个方向有位置和速度两个状态。令 $s = (p_x, v_x, p_y, v_y, p_z, v_z)$，近悬停线性化后：

\[\dot{p}_x = v_x, \quad \dot{v}_x = g\theta + d_x$$ $$\dot{p}_y = v_y, \quad \dot{v}_y = -g\phi + d_y$$ $$\dot{p}_z = v_z, \quad \dot{v}_z = T/m - g + d_z\]

其中 $\theta$（俯仰）和 $\phi$（滚转）是控制量，$T$ 是总推力，$(d_x, d_y, d_z)$ 是风扰动。

"""
6D 近悬停四旋翼 BRT (简化).
警告: 这需要大量内存和计算时间 (~32 GB, ~数小时).
建议在高性能服务器上运行.

状态: (px, vx, py, vy, pz, vz)
控制: (theta, phi, T) — 俯仰角, 滚转角, 总推力
扰动: (dx, dy, dz) — 风力
"""

import jax.numpy as jnp
import hj_reachability as hj

class NearHoverQuadrotor(hj.ControlAndDisturbanceAffineDynamics):
    """6D 近悬停四旋翼."""

    def __init__(self, g=9.81, m=1.0,
                 theta_max=0.3, phi_max=0.3,
                 T_min=0.5, T_max=1.5,
                 d_max=1.0):
        self.g = g
        self.m = m
        super().__init__(
            control_mode="max",
            disturbance_mode="min",
            control_space=hj.sets.Box(
                lo=jnp.array([-theta_max, -phi_max,
                              T_min * g]),  # T_min * mg
                hi=jnp.array([theta_max, phi_max,
                              T_max * g])),  # T_max * mg
            disturbance_space=hj.sets.Box(
                lo=jnp.array([-d_max, -d_max, -d_max]),
                hi=jnp.array([d_max, d_max, d_max])),
        )

    def open_loop_dynamics(self, x, time):
        # x = [px, vx, py, vy, pz, vz]
        return jnp.array([
            x[1],        # dpx/dt = vx
            0.0,         # dvx/dt (控制 + 扰动)
            x[3],        # dpy/dt = vy
            0.0,         # dvy/dt
            x[5],        # dpz/dt = vz
            -self.g,     # dvz/dt = -g (重力)
        ])

    def control_jacobian(self, x, time):
        # 控制: (theta, phi, T/m)
        return jnp.array([
            [0.0,       0.0,  0.0],
            [self.g,    0.0,  0.0],    # dvx = g * theta
            [0.0,       0.0,  0.0],
            [0.0, -self.g,    0.0],    # dvy = -g * phi
            [0.0,       0.0,  0.0],
            [0.0,       0.0,  1.0/self.m],  # dvz = T/m
        ])

    def disturbance_jacobian(self, x, time):
        return jnp.array([
            [0.0, 0.0, 0.0],
            [1.0, 0.0, 0.0],    # dx 作用于 vx
            [0.0, 0.0, 0.0],
            [0.0, 1.0, 0.0],    # dy 作用于 vy
            [0.0, 0.0, 0.0],
            [0.0, 0.0, 1.0],    # dz 作用于 vz
        ])

# 6D 网格: 每维仅 25 个点 (25^6 ≈ 2.4 亿网格点)
# 这已经需要约 1.9 GB 内存 (float64), 计算需要数小时
# 实际应用中通常用 decomposition 降维
grid = hj.Grid.from_lattice_parameters_and_boundary_conditions(
    domain=hj.sets.Box(
        lo=jnp.array([-5., -3., -5., -3., 0., -3.]),
        hi=jnp.array([5., 3., 5., 3., 10., 3.]),
    ),
    shape=(25, 25, 25, 25, 25, 25),
)

# 目标集: 地面碰撞 pz <= 0.5 且速度过大
# l(x) = 0.5 - pz  (pz <= 0.5 时 l <= 0)
target_values = 0.5 - grid.states[..., 4]

# 注意: 实际运行此代码需要大内存服务器
# 此处仅展示接口用法, 建议用 decomposition 方法处理

实际工程建议：6D 问题直接用全网格法非常昂贵。工程中常用两种降维技巧：

Decomposition（Chen et al., 2018）：将 6D 系统分解为若干低维子系统（如 2D+2D+2D），分别计算 BRT 后再组合。精度有损失但计算量大幅降低。
投影 (Projection)：如果只关心某些维度的安全约束（如位置），可以将 BRT 投影到低维空间：$\text{BRT}_{\text{proj}} = \{(p_x, p_y, p_z) : \exists (v_x, v_y, v_z), \, V(p, v) \le 0\}$。投影操作就是对速度维度取 $\min$。

练习 5.1 ⭐⭐⭐¶

修改上面的代码，完成以下任务：

(a) 将扰动范围从 $[-0.25, 0.25]$ 扩大到 $[-0.5, 0.5]$，观察 BRT 如何变化。 (b) 去掉 hamiltonian_postprocessor=hj.solver.backwards_reachable_tube，改用 BRS。比较 BRS 和 BRT 的区别。 (c) 将目标集改为圆形 $l(x) = x_1^2 + x_2^2 - 0.25$，重新计算。

6. 经典案例：Air3D 追逃博弈 ⭐⭐¶

Air3D 是 HJ 可达性最经典的 benchmark，由 Mitchell-Bayen-Tomlin 2005 提出。

6.1 问题设定¶

两架飞机（evader E 和 pursuer P）在 2D 平面上运动，都满足 Dubins car 动力学：

Evader: $\dot{x}_E = v_E \cos\theta_E, \, \dot{y}_E = v_E \sin\theta_E, \, \dot{\theta}_E = \omega_E$
Pursuer: $\dot{x}_P = v_P \cos\theta_P, \, \dot{y}_P = v_P \sin\theta_P, \, \dot{\theta}_P = \omega_P$

在 evader 的体坐标系中，定义**相对状态** $(x_r, y_r, \theta_r)$：

\[\begin{aligned} \dot{x}_r &= -v_E + v_P \cos\theta_r + \omega_E y_r \\ \dot{y}_r &= v_P \sin\theta_r - \omega_E x_r \\ \dot{\theta}_r &= \omega_P - \omega_E \end{aligned}\]

这样一个 6 维问题降成了 3 维——这是关键的降维技巧。

博弈角色： - Evader（控制器 $u = \omega_E$）：**最大化**相对距离，想逃跑 - Pursuer（扰动 $d = \omega_P$）：**最小化**相对距离，想追上

目标集（碰撞区域）：相对距离小于 $R$：

\[\mathcal{T} = \{(x_r, y_r, \theta_r) : x_r^2 + y_r^2 \le R^2\}\]

即 $l(x_r, y_r, \theta_r) = x_r^2 + y_r^2 - R^2$。

6.2 Isaacs 条件验证¶

Hamiltonian 为

\[H = p_1(-v_E + v_P\cos\theta_r + \omega_E y_r) + p_2(v_P\sin\theta_r - \omega_E x_r) + p_3(\omega_P - \omega_E)\]

关于 $\omega_E$ 和 $\omega_P$ 是**线性的**（可分离），所以 Isaacs 条件自动成立。最优策略为 bang-bang：

\[\omega_P^* = -\text{sign}(p_3) \cdot \bar{\omega}_P, \quad \omega_E^* = \text{sign}(p_1 y_r - p_2 x_r - p_3) \cdot \bar{\omega}_E\]

其中 $\bar{\omega}_P, \bar{\omega}_E$ 是角速度的上界。

6.3 代码实现¶

"""
Air3D: Dubins car 追逃博弈的 BRT 计算.

相对动力学 (evader 体坐标系):
  dx_r = -v_e + v_p cos(theta_r) + omega_e * y_r
  dy_r = v_p sin(theta_r) - omega_e * x_r
  dtheta_r = omega_p - omega_e

evader 控制 omega_e (最大化 V), pursuer 控制 omega_p (最小化 V).
"""

import jax.numpy as jnp
import hj_reachability as hj

class Air3D(hj.ControlAndDisturbanceAffineDynamics):
    """3D 相对 Dubins car 追逃博弈."""

    def __init__(self, v_e=5.0, v_p=5.0, omega_e_max=1.0, omega_p_max=1.0):
        self.v_e = v_e
        self.v_p = v_p
        super().__init__(
            control_mode="max",       # evader 最大化 V
            disturbance_mode="min",   # pursuer 最小化 V
            control_space=hj.sets.Box(
                lo=jnp.array([-omega_e_max]),
                hi=jnp.array([omega_e_max])),
            disturbance_space=hj.sets.Box(
                lo=jnp.array([-omega_p_max]),
                hi=jnp.array([omega_p_max])),
        )

    def open_loop_dynamics(self, x, time):
        # x = [x_r, y_r, theta_r]
        return jnp.array([
            -self.v_e + self.v_p * jnp.cos(x[2]),
            self.v_p * jnp.sin(x[2]),
            0.0,
        ])

    def control_jacobian(self, x, time):
        # omega_e 的贡献
        return jnp.array([
            [x[1]],     # omega_e * y_r
            [-x[0]],    # -omega_e * x_r
            [-1.0],     # -omega_e
        ])

    def disturbance_jacobian(self, x, time):
        # omega_p 的贡献
        return jnp.array([
            [0.0],
            [0.0],
            [1.0],      # omega_p
        ])

# 定义 3D 网格
grid = hj.Grid.from_lattice_parameters_and_boundary_conditions(
    domain=hj.sets.Box(
        lo=jnp.array([-6.0, -10.0, 0.0]),
        hi=jnp.array([20.0, 10.0, 2 * jnp.pi]),
    ),
    shape=(81, 81, 51),
    periodic_dims=2,  # theta_r 是周期性的
)

# 目标集: 碰撞区域 x_r^2 + y_r^2 <= R^2, R = 5
R = 5.0
target_values = grid.states[..., 0]**2 + grid.states[..., 1]**2 - R**2

# 求解
dynamics = Air3D()
solver_settings = hj.SolverSettings.with_accuracy(
    accuracy="very_high",
    hamiltonian_postprocessor=hj.solver.backwards_reachable_tube,
)

time_steps = jnp.linspace(0, -3.0, 121)
values = hj.solve(solver_settings, dynamics, grid,
                  time_steps, target_values)

# 提取 theta_r = pi 时的 2D 切片并可视化
import matplotlib.pyplot as plt
theta_idx = 25  # theta_r approximately equal to pi

fig, axes = plt.subplots(1, 3, figsize=(15, 5))
for i, t_idx in enumerate([0, 60, 120]):
    ax = axes[i]
    ax.contourf(grid.coordinate_vectors[0],
                grid.coordinate_vectors[1],
                values[t_idx][:, :, theta_idx].T,
                levels=50, cmap="RdBu")
    ax.contour(grid.coordinate_vectors[0],
               grid.coordinate_vectors[1],
               values[t_idx][:, :, theta_idx].T,
               levels=[0], colors="black", linewidths=2)
    ax.set_xlabel("$x_r$")
    ax.set_ylabel("$y_r$")
    ax.set_title(f"BRT at $t = {time_steps[t_idx]:.1f}$, "
                 f"$\\theta_r = \\pi$")
    ax.set_aspect("equal")

plt.tight_layout()
plt.savefig("air3d_brt.png", dpi=150)
plt.show()

代码要点： - periodic_dims=2 指定 $\theta_r$ 维度是周期性的——这对于角度变量至关重要。 - 目标集是**与航向角无关的圆**，但 BRT 的形状依赖于 $\theta_r$，因为动力学与 $\theta_r$ 有关。 - 可视化时取 $\theta_r$ 的某个切片来看 2D 投影。

7. DeepReach: 神经网络逼近 ⭐⭐⭐¶

7.1 动机：超过 6 维怎么办？¶

Level-Set 方法的 $O(N^d)$ 复杂度使其在 $d > 6$ 时不可行。但很多实际系统的维度远高于 6：

系统	状态维度
Dubins car (相对)	3
四旋翼 6DOF	12
机械臂 7DOF	14
多智能体 (3 个 Dubins)	9
自动驾驶 (车辆 + 行人)	10+

DeepReach (Bansal & Tomlin, 2021, IEEE RA-L) 用神经网络参数化值函数 $V_\theta(x, t)$，通过训练使其满足 HJI 方程——本质上是一个 Physics-Informed Neural Network (PINN) 思路。

7.2 SIREN 架构¶

DeepReach 使用 SIREN (Sinusoidal Representation Network, Sitzmann et al. 2020) 架构，即全连接网络但激活函数使用 $\sin$ 而非 ReLU：

\[z^{(k+1)} = \sin(\omega_0 (W^{(k)} z^{(k)} + b^{(k)}))\]

其中 $\omega_0 = 30$ 是频率超参数。为什么用 $\sin$？因为 HJI 的解通常是光滑的（或至少分段光滑的），而 ReLU 的二阶导为零，无法精确表示曲面；$\sin$ 的各阶导数都非零，表达能力更强。

网络输入：$(x, t) \in \mathbb{R}^{n+1}$ 网络输出：$V_\theta(x, t) \in \mathbb{R}$

7.3 训练目标¶

损失函数由 HJI 方程的残差构成：

\[\mathcal{L}(\theta) = \lambda_1 \mathcal{L}_{\text{PDE}} + \lambda_2 \mathcal{L}_{\text{BC}}\]

其中：

PDE 损失（HJI 残差）：

\[\mathcal{L}_{\text{PDE}} = \frac{1}{|\mathcal{S}_{\text{int}}|} \sum_{(x, t) \in \mathcal{S}_{\text{int}}} \left| \frac{\partial V_\theta}{\partial t} + \min\left\{0, \, H(x, \nabla_x V_\theta)\right\} \right|^2\]

边界条件损失（终端条件）：

\[\mathcal{L}_{\text{BC}} = \frac{1}{|\mathcal{S}_{\text{BC}}|} \sum_{x \in \mathcal{S}_{\text{BC}}} |V_\theta(x, 0) - l(x)|^2\]

训练点 $(x, t)$ 从状态-时间空间中**随机采样**——不需要网格！这是无网格方法，复杂度与维度无关（理论上）。

7.4 课程学习策略 (Curriculum Learning)¶

直接在 $t \in [t_{\min}, 0]$ 上训练效果很差——当 $|t|$ 大时，HJI 方程的解变得复杂，梯度信号微弱。DeepReach 使用**课程学习**：

先在 $t \in [-\epsilon, 0]$ 上训练（此时解接近 $l(x)$，容易学）
逐渐扩大时间范围到 $t \in [-2\epsilon, 0]$, $[-3\epsilon, 0]$, ...
最终覆盖整个 $[t_{\min}, 0]$

这模仿了 Level-Set 方法的反向时间步进——先从简单的终端条件出发，逐步传播。

7.5 代码示例框架¶

"""
DeepReach 概念代码 (简化版, 展示核心思想).
实际使用请参考 https://github.com/smlbansal/deepreach
"""

import torch
import torch.nn as nn
import numpy as np

# ======== SIREN 层 ========
class SirenLayer(nn.Module):
    def __init__(self, in_dim, out_dim, omega_0=30.0, is_first=False):
        super().__init__()
        self.omega_0 = omega_0
        self.linear = nn.Linear(in_dim, out_dim)

        # SIREN 初始化 (Sitzmann 2020)
        with torch.no_grad():
            if is_first:
                self.linear.weight.uniform_(-1 / in_dim, 1 / in_dim)
            else:
                bound = np.sqrt(6 / in_dim) / omega_0
                self.linear.weight.uniform_(-bound, bound)

    def forward(self, x):
        return torch.sin(self.omega_0 * self.linear(x))


class DeepReachNet(nn.Module):
    """SIREN 网络: 输入 (x, t), 输出 V(x, t)."""

    def __init__(self, state_dim, hidden_dim=512, num_layers=3):
        super().__init__()
        layers = [SirenLayer(state_dim + 1, hidden_dim,
                             omega_0=30.0, is_first=True)]
        for _ in range(num_layers - 1):
            layers.append(SirenLayer(hidden_dim, hidden_dim, omega_0=30.0))
        layers.append(nn.Linear(hidden_dim, 1))
        self.net = nn.Sequential(*layers)

    def forward(self, x, t):
        # x: (batch, state_dim), t: (batch, 1)
        inp = torch.cat([x, t], dim=-1)
        return self.net(inp)


def compute_loss(model, dynamics_fn, x_int, t_int, x_bc, l_fn,
                 lam1=1.0, lam2=100.0):
    """计算 DeepReach 损失函数."""

    # --- PDE 损失 ---
    x_int.requires_grad_(True)
    t_int.requires_grad_(True)
    V = model(x_int, t_int)

    # 自动微分求梯度
    grad_V = torch.autograd.grad(
        V, [x_int, t_int], grad_outputs=torch.ones_like(V),
        create_graph=True
    )
    dV_dx = grad_V[0]  # nabla_x V
    dV_dt = grad_V[1]  # partial V / partial t

    # 计算 Hamiltonian H = max_u min_d nabla_V . f(x, u, d)
    H = dynamics_fn.hamiltonian(x_int, dV_dx)

    # HJI 变分不等式残差
    hji_residual = dV_dt + torch.clamp(H, max=0.0)  # min{0, H}
    loss_pde = (hji_residual ** 2).mean()

    # --- 边界条件损失 ---
    t_zero = torch.zeros(x_bc.shape[0], 1)
    V_bc = model(x_bc, t_zero)
    loss_bc = ((V_bc.squeeze() - l_fn(x_bc)) ** 2).mean()

    return lam1 * loss_pde + lam2 * loss_bc

陷阱警告

DeepReach 虽然理论上不受维度限制，但实际中有几个问题： 1. 无收敛保证——PINN 的训练可能不收敛，且没有黏性解意义下的收敛性证明。 2. 精度不如 Level-Set——在低维情况下（$d \le 4$），网格法的精度通常优于 DeepReach。 3. 需要大量调参——SIREN 的 $\omega_0$、课程学习的时间步长、采样策略都影响结果。

因此实际工程中的建议是：$d \le 5$ 用 Level-Set，$d > 5$ 考虑 DeepReach。

7.6 SIREN 架构深度解析：为什么正弦激活至关重要 ⭐⭐⭐⭐¶

ReLU 的根本缺陷。标准 ReLU 网络 $\sigma(x) = \max(0, x)$ 的二阶导数几乎处处为零（除了 $x = 0$ 处的 delta 函数）。而 HJI 方程 $V_t + H(x, \nabla V) = 0$ 需要对 $V$ 求一阶导数 $\nabla V$，如果要用 PINN 的方式训练（最小化 PDE 残差），我们需要通过自动微分计算 $\nabla V$。

问题在于：ReLU 网络的 $\nabla V$ 是**分段常数**的——因为 ReLU 的导数是分段常数。这意味着 $\nabla V$ 的值在整个线性区域内不变，梯度信号极其稀疏。当 HJI 的解具有光滑弯曲的零水平集时，ReLU 网络需要极多的"折线段"来逼近这些曲线，效率极低。

$\sin$ 激活的三大优势：

各阶导数非零：$\sin$ 的导数是 $\cos$，二阶导是 $-\sin$，三阶导是 $-\cos$... 永远不为零。这意味着 SIREN 网络的输出关于输入的各阶导数都有丰富的非零信号，PINN 的梯度训练可以有效工作。
频率控制：超参数 $\omega_0$ 控制了网络能表示的频率范围。$\omega_0 = 30$ 意味着每个隐藏层的输入被放大 30 倍后取 $\sin$，这使得网络能表示高频振荡。HJI 值函数通常在零水平集附近有剧烈变化（从正到负），需要高频表示能力。
初始化的数学保证：Sitzmann 等人证明了，使用特定的初始化方案（第一层权重 $\sim U(-1/n, 1/n)$，后续层 $\sim U(-\sqrt{6/n}/\omega_0, \sqrt{6/n}/\omega_0)$），SIREN 网络在初始化时的输出分布是**弧正弦分布** (arcsine distribution)，且各层的激活值在训练过程中保持稳定——类似于 He/Xavier 初始化对 ReLU/Sigmoid 的作用。

$\omega_0$ 的选择指南：

$\omega_0$	效果	适用场景
$\omega_0 = 1$	低频，光滑	非常光滑的值函数
$\omega_0 = 10$	中频	一般的 HJI 问题
$\omega_0 = 30$	高频（DeepReach 默认）	值函数有尖锐零水平集
$\omega_0 = 60$	很高频	高维、复杂几何

警告：$\omega_0$ 过大会导致训练不稳定——高频振荡使损失景观变得非常崎岖。通常从 30 开始，如果结果过于平滑则增大，如果训练不稳定则减小。

7.7 超参数调优指南 ⭐⭐⭐⭐¶

DeepReach 的训练涉及多个超参数，以下是实践中的推荐设置和调优策略：

超参数	推荐值	调优方向
隐藏层数	3	维度 >8 时增加到 4-5
隐藏层宽度	512	复杂系统增加到 1024
$\omega_0$	30	见上文
学习率	$5 \times 10^{-5}$	训练不稳定时减小
batch size	65536 (PDE) + 16384 (BC)	内存允许时增大
BC 损失权重 $\lambda_2$	100	终端条件不满足时增大到 500
课程学习步数	10-20 个阶段	时间范围大时增加
训练总迭代数	100K-500K	用验证集监控收敛

训练收敛诊断

如何判断 DeepReach 训练是否成功？以下是关键诊断指标：

PDE 损失曲线：应当单调下降。如果出现大幅震荡，说明学习率过大或 $\omega_0$ 过大。
BC 损失曲线：应当在训练早期快速下降并保持很低（$< 10^{-4}$）。如果 BC 损失居高不下，增大 $\lambda_2$。
零水平集可视化：每隔一定迭代数，画出 $V_\theta(x, t) = 0$ 的等值线。好的训练应该看到零水平集逐渐变得光滑且形状合理。
与 Level-Set 对比（低维时）：在 3D-4D 问题上，用 Level-Set 方法计算"ground truth"，然后测量 DeepReach 结果的误差。常用指标：零水平集的 Hausdorff 距离和 IoU (Intersection over Union)。

# 训练诊断代码片段
def training_diagnostics(model, dynamics, grid_points, t_values,
                         l_fn, V_levelset=None):
    """计算 DeepReach 的诊断指标."""
    diagnostics = {}

    # 1. PDE 残差统计
    x_test = torch.randn(10000, state_dim)  # 随机测试点
    t_test = torch.rand(10000, 1) * t_min
    residual = compute_hji_residual(model, dynamics, x_test, t_test)
    diagnostics['pde_residual_mean'] = residual.abs().mean().item()
    diagnostics['pde_residual_max'] = residual.abs().max().item()

    # 2. 终端条件误差
    x_bc = torch.randn(5000, state_dim)
    t_zero = torch.zeros(5000, 1)
    V_pred = model(x_bc, t_zero).squeeze()
    V_true = l_fn(x_bc)
    diagnostics['bc_error'] = (V_pred - V_true).abs().mean().item()

    # 3. 与 Level-Set 对比 (如果可用)
    if V_levelset is not None:
        V_deep = model(grid_points, t_values).squeeze()
        # 零水平集 IoU
        set_deep = (V_deep <= 0).float()
        set_ls = (V_levelset <= 0).float()
        intersection = (set_deep * set_ls).sum()
        union = ((set_deep + set_ls) > 0).float().sum()
        diagnostics['iou'] = (intersection / union).item()

    return diagnostics

⚠️ 编程陷阱：DeepReach 中遗忘 create_graph=True

错误做法：在计算 HJI 残差时，使用 torch.autograd.grad(..., create_graph=False)。

现象：PDE 损失不下降或下降极慢——梯度反传时，因为计算图已被释放，优化器收到的梯度全为零。

根本原因：HJI 残差涉及 $V$ 的一阶导数 $\nabla V$。要用梯度下降来最小化残差，需要对残差再求导——即需要 $V$ 的二阶导信息。create_graph=True 告诉 PyTorch "保留自动微分的计算图，以便后续对它继续求导"。

正确做法：grad_V = torch.autograd.grad(V, [x, t], ..., create_graph=True)。同时确保输入 x 和 t 都设置了 requires_grad_(True)。

💡 概念误区：认为 DeepReach "解决了"维度灾难

新手想法："DeepReach 不用网格，所以可以处理任意维度的系统！"

实际上：DeepReach 将维度灾难从"内存"转移到了"训练难度"。高维空间中，随机采样的训练点极其稀疏——在 10D 空间中，10 万个均匀采样点的平均间距约为 $10^{10/10} = 10$ 个单位长度。值函数的零水平集（一个 9D 超曲面）在这些采样点中被"看到"的概率极低。因此 DeepReach 在 10D 以上的精度往往很差。

当前前沿：2024-2025 年的研究方向包括：(a) 自适应采样——在零水平集附近集中采样（Shao et al., 2024）；(b) 物理约束的网络架构——将 HJI 的结构编码到网络中而非仅靠损失函数；(c) 混合方法——低维用网格、高维用神经网络的混合分解。

自测检查点 7 ⭐⭐⭐¶

DeepReach 与经典 PINN 的关系是什么？核心区别在哪里？
为什么 SIREN 使用 $\sin$ 激活而不是 ReLU？从 HJ 方程解的性质角度解释。
课程学习策略模仿了 Level-Set 方法的什么特性？

8. CBVF：CBF 与 HJ 的统一 ⭐⭐⭐⭐¶

8.1 为什么需要统一？¶

回顾第 2 节的对比表，CBF 和 HJ 可达性看起来是两种完全不同的方法。但 Choi, Lee, Tomlin 2021 (IEEE RA-L) 发现了一个深刻的联系：通过引入一个**折扣参数** $\gamma \ge 0$，可以在一个统一框架下涵盖两者。

核心洞察：考虑折扣 HJI 方程

\[\frac{\partial V_\gamma}{\partial t} + \min\left\{0, \, \max_u \min_d \nabla V_\gamma \cdot f + \gamma V_\gamma\right\} = 0\]

注意多出来的 $\gamma V_\gamma$ 项——这正是类似于 CBF 条件 $\dot{h} \ge -\alpha(h)$ 中 $\alpha(h) = \gamma h$ 的线性衰减。

8.2 两个极端¶

$\gamma = 0$（经典 HJ 可达性）：

\[\frac{\partial V_0}{\partial t} + \min\left\{0, \, \max_u \min_d \nabla V_0 \cdot f\right\} = 0\]

这就是第 4 节的标准 HJI 变分不等式。$V_0$ 的零水平集给出**精确的 BRT**——最大可控不变集。

$\gamma \to \infty$（CBF 极限）：

当 $\gamma$ 很大时，$\gamma V_\gamma$ 项主导。在稳态（$V_t = 0$）下，条件变成

\[\max_u \min_d \nabla V \cdot f + \gamma V \ge 0\]

即 $\dot{V} \ge -\gamma V$，这恰好是 $\alpha(h) = \gamma h$ 的 CBF 条件！

8.3 连续谱¶

$\gamma$ 的中间值给出一个从"精确但保守"到"近似但不保守"的连续谱：

$\gamma$	含义	安全集大小	计算要求
$0$	经典 HJ	最大（精确 BRT）	需要收敛到稳态
小	接近 HJ	接近最大	收敛较快
大	接近 CBF	较小（保守）	收敛很快
$\infty$	纯 CBF	取决于 $l(x)$ 的设计	无需离线计算

关键性质（Choi 2021, Theorem 1）：

$V_\gamma(x, t)$ 随 $\gamma$ 单调递增，因此安全集 $\{V_\gamma > 0\}$ 随 $\gamma$ 单调递减。
对所有 $\gamma \ge 0$，$V_\gamma$ 的零超水平集 $\{V_\gamma \ge 0\}$ 都是**前向不变的**（控制不变集）。
$V_\gamma$ 在 $\{V_\gamma \ge 0\}$ 上自动满足 CBF 条件——即 $V_\gamma$ 可直接作为 CBF-QP 的障碍函数。

8.4 CBVF 的名称¶

Choi 等人将 $V_\gamma$ 称为 Control Barrier-Value Function (CBVF)，因为它同时具有： - 值函数 (Value Function) 的含义——来自 HJ 可达性的最优博弈值 - 障碍函数 (Barrier Function) 的含义——可直接用于 CBF-QP

工程意义：过去，工程师面临选择——是花大量离线计算得到精确 BRT（$\gamma = 0$），还是用手设计的 CBF（$\gamma \to \infty$）？CBVF 告诉你可以取一个中间值——用适当的 $\gamma$ 快速得到一个"够好"的安全控制器，既不像纯 HJ 那么贵，也不像纯 CBF 那么粗糙。

8.5 折扣 HJI 方程的数值解¶

实现上非常简单——只需要在 Hamiltonian 中加一项：

# 在 hj_reachability 中实现 CBVF
# 需要自定义 hamiltonian_postprocessor

def cbvf_postprocessor(gamma=0.1):
    """创建 CBVF 的后处理器."""
    def postprocessor(V_t_plus_H, V, t):
        # V_t + min{0, H + gamma * V} = 0
        # 其中 V_t_plus_H = V_t + H (由 solver 计算)
        # 需要加上 gamma * V 后再截断
        H = V_t_plus_H  # 这里 V_t_plus_H 实际上是 H
        return jnp.minimum(0.0, H + gamma * V)
    return postprocessor

# 使用
solver_settings = hj.SolverSettings.with_accuracy(
    accuracy="very_high",
    hamiltonian_postprocessor=cbvf_postprocessor(gamma=0.5),
)

陷阱警告

hj_reachability 的 hamiltonian_postprocessor 接口的具体签名可能因版本而异。上面的代码展示的是**核心思想**——实际实现请参考库文档。CBVF 的关键就是在 obstacle 算子之前加入 $\gamma V$ 项。

8.6 $\gamma \to 0$ 恢复经典 HJ 的严格推导 ⭐⭐⭐⭐¶

命题：当 $\gamma = 0$ 时，折扣 HJI 方程退化为标准 HJI 变分不等式。

证明：将 $\gamma = 0$ 代入折扣 HJI 方程：

\[\frac{\partial V_0}{\partial t} + \min\left\{0, \, \max_u \min_d \nabla V_0 \cdot f(x,u,d) + 0 \cdot V_0\right\} = 0\]

\[\Rightarrow \frac{\partial V_0}{\partial t} + \min\left\{0, \, \max_u \min_d \nabla V_0 \cdot f(x,u,d)\right\} = 0\]

这正是第 4 节的标准 HJI 变分不等式。$V_0$ 的零水平集给出精确的 BRT。$\square$

关键性质：$\gamma = 0$ 时，CBVF 不施加任何额外的衰减约束——值函数的演化完全由系统动力学和 obstacle 算子决定。这意味着安全集 $\{V_0 \ge 0\}$ 是**最大可控不变集**——不存在更大的安全集。

8.7 $\gamma \to \infty$ 恢复 CBF 的严格推导 ⭐⭐⭐⭐¶

命题：当 $\gamma \to \infty$ 时，CBVF 的稳态解退化为满足 CBF 条件 $\dot{h} \ge -\gamma h$ 的障碍函数，且安全集退缩到 $\{l(x) \ge 0\}$ 附近。

推导过程：

Step 1：考虑折扣 HJI 在稳态 ($V_t = 0$) 时的条件。在零水平集内部 ($V_\gamma > 0$)，obstacle 算子不起作用，方程变为：

\[\max_u \min_d \nabla V_\gamma \cdot f + \gamma V_\gamma = 0\]

即 $\max_u \min_d \nabla V_\gamma \cdot f = -\gamma V_\gamma$。

Step 2：当 $\gamma$ 很大时，$-\gamma V_\gamma$ 是一个很大的负数（在 $V_\gamma > 0$ 的区域）。这意味着即使控制器全力以赴 ($\max_u$)，$\nabla V_\gamma \cdot f$ 也必须是一个很大的负数来平衡 $\gamma V_\gamma$。

物理含义：$\gamma$ 很大时，值函数被"强制"快速衰减——只有那些 $l(x)$ 本身就很大（离目标集很远）的状态才能在衰减后仍保持 $V_\gamma > 0$。

Step 3：形式化地，令 $V_\gamma = V / \gamma$（重新标度），则方程变为

\[\max_u \min_d \nabla V \cdot f / \gamma + V = 0\]

当 $\gamma \to \infty$，$\nabla V \cdot f / \gamma \to 0$，所以 $V \to 0$——即 $V_\gamma \to 0$ 处处。安全集 $\{V_\gamma \ge 0\}$ 退缩到 $\{l(x) \ge 0\}$（目标集的补集）。

Step 4：在 $\{V_\gamma > 0\}$ 的边界附近，CBF 条件 $\max_u \min_d \nabla V_\gamma \cdot f \ge -\gamma V_\gamma$ 自动满足——这恰好是 class-$\mathcal{K}$ 函数 $\alpha(h) = \gamma h$ 对应的 CBF 不等式。换言之，$V_\gamma$ 本身就是一个合法的 CBF。

总结：$\gamma$ 越大，CBVF 越"像" CBF——它对安全集的定义越保守（更接近于 $l(x)$ 的原始设计），但也越快达到稳态（计算代价低）。$\gamma$ 越小，CBVF 越"像" HJ——安全集越接近最大可控不变集，但需要更长的时间演化和更多的计算。

8.8 数值算例：一维系统的 CBVF 连续谱¶

让我们用一个简单的一维例子完整展示 CBVF 的行为。

系统：$\dot{x} = u + d$，$u \in [-1, 1]$，$d \in [-0.5, 0.5]$。 目标集：$\mathcal{T} = \{x : x \ge 3\}$，即 $l(x) = 3 - x$。

解析分析：

$\gamma = 0$（经典 HJ）：最坏净速度 = $-1 + 0.5 = -0.5$（控制器总能克服扰动）。从 $x_0$ 出发，系统可以 $0.5$ 的速度远离目标集。因此 $\text{BRT}(t) = \{x \ge 3 + 0.5t\}$（$t < 0$）。无限时域 BRT = $\{x \ge 3\}$ = $\mathcal{T}$。安全集 = $\{x < 3\}$。
$\gamma = 0.5$（中等折扣）：稳态 CBVF 满足 $\max_u \min_d (V'(u + d)) + 0.5 V = 0$。在 $V > 0$ 区域，最坏情况 $d = 0.5$，最优控制 $u$ 由 $V'$ 的符号决定。

当 $x < 3$（安全侧），$V > 0$，$V' < 0$（值函数随 $x$ 增大而减小，因为靠近目标集更危险）。最优控制 $u = -1$（远离目标集），最坏扰动 $d = 0.5$。方程变为 $V' \cdot (-0.5) + 0.5 V = 0$，即 $V' = V$。解为 $V(x) = C e^x$，但这需要满足边界条件 $V(3) = 0$（零水平集在 $x = 3$）。所以 $V(x) = A(e^x - e^3)$... 更精确地，稳态条件给出 $V(x) = l(3) \cdot e^{\gamma(x - x^*)/v_{\min}}$，其中 $v_{\min} = 0.5$ 是最坏净速度。

关键结论：$\gamma > 0$ 时，安全集 $\{V_\gamma \ge 0\}$ 的边界仍然是 $x = 3$（因为 $\gamma V = 0$ 在边界上），但值函数的**衰减速率**更快——$V_\gamma$ 在远离边界时增长得更慢。这意味着 CBF-QP 使用 $V_\gamma$ 时，安全滤波器的介入更早、更保守。

$\gamma \to \infty$（CBF 极限）：$V_\gamma(x) \to l(x) = 3 - x$。安全集 = $\{3 - x \ge 0\} = \{x \le 3\}$——与原始 $l(x)$ 的零水平集完全一致。CBF 条件变成 $\dot{l} \ge -\gamma l$，即 $-(u + d) \ge -\gamma(3 - x)$。

🧠 思维陷阱：认为 "$\gamma$ 越小越好"

新手想法："$\gamma = 0$ 给出最大安全集，所以总应该用 $\gamma = 0$。"

实际上：$\gamma = 0$ 确实给出最大安全集，但有两个代价：(1) 需要完全收敛到稳态才准确，计算时间长；(2) 在高维问题中（DeepReach），$\gamma > 0$ 的"衰减效应"充当了正则化，使训练更稳定。Choi 2021 的实验表明，$\gamma \in [0.1, 1.0]$ 通常是安全集大小和计算效率的良好折中。

正确思维：先用 $\gamma = 0.5$ 快速得到一个初步结果，检查安全集是否满足需求。如果太保守（安全集太小），减小 $\gamma$；如果计算太慢，增大 $\gamma$。

练习 8.1 ⭐⭐⭐⭐¶

(a) 证明：如果 $V_\gamma(x) \ge 0$ 且 $\max_u \min_d \nabla V_\gamma \cdot f(x, u, d) + \gamma V_\gamma(x) \ge 0$，那么 $V_\gamma$ 是一个合法的 CBF（即安全集 $\{V_\gamma \ge 0\}$ 前向不变）。

提示：令 $h(x) = V_\gamma(x)$，$\alpha(h) = \gamma h$，验证 $\dot{h} \ge -\alpha(h)$，然后用比较引理。

(b) 直觉上解释：为什么 $\gamma$ 越大，安全集越小？

提示：$\gamma$ 越大，$\dot{h} \ge -\gamma h$ 的约束越松——相当于允许 $h$ 衰减得更快，所以只有 $h$ 很大的状态才能保证安全。

9. 应用案例 ⭐⭐¶

9.1 空中交通避碰 (Air3D)¶

这就是第 6 节的经典案例。在美国 FAA 的 ACAS X (Airborne Collision Avoidance System X) 中，HJ 可达性分析用于生成离线查找表：给定当前相对状态，查表得到是否需要机动、以及最优回避方向。

工程流程： 1. 离线：用 HJ 可达性计算 BRT 和最优控制策略，存为多维数组。 2. 在线：传感器测量相对状态 $(x_r, y_r, \theta_r)$，查表判断是否在 BRT 内。 3. 如果在 BRT 内（危险），执行查表得到的最优回避控制。 4. 如果在 BRT 外（安全），正常飞行。

这个"离线计算 + 在线查表"的模式是 HJ 可达性在工程中最常见的用法。

9.2 FaSTrack：规划与安全解耦¶

FaSTrack (Fast and Safe Tracking, Bansal et al. 2017, ICRA) 是一个优雅的框架，将路径规划和安全保证**解耦**：

核心思想： - 规划层：用简单的规划模型（如单积分器 $\dot{x}_p = v_p$）做快速路径规划。 - 跟踪层：真实系统（如四旋翼）跟踪规划路径，跟踪误差定义为 $e = x_{\text{real}} - x_{\text{plan}}$。 - 安全层：离线计算跟踪误差动力学的 BRT，得到**跟踪误差界** (Tracking Error Bound, TEB)。 - 组合：规划路径膨胀 TEB 后，只要不碰障碍物，真实系统就安全。

+---------------+     +----------------+     +---------------+
|  简单规划器    |---->|  跟踪控制器    |---->|  真实系统      |
| (单积分器)    |     |                |     | (四旋翼)      |
+---------------+     +----------------+     +---------------+
       |                                            |
       |            +----------------+              |
       +----------->|  HJ 可达性     |<-------------+
                    | (离线: TEB)    |
                    +----------------+

为什么 FaSTrack 实用？ - 规划器可以用 A*、RRT 等简单快速算法——不需要考虑真实动力学。 - 安全保证来自离线 HJ 计算——不需要在线求解 PDE。 - TEB 提供了一个**确定性的安全边界**——规划路径只需膨胀这个边界即可。

9.3 无人机安全着陆包络¶

在无人机的安全着陆场景中，HJ 可达性用于计算**安全着陆包络** (Safe Landing Envelope)：

状态：$(x, y, z, v_x, v_y, v_z)$——位置和速度
目标集：着陆区域 $\{z \le 0, \, v_z \ge -v_{\max}\}$——地面且速度够慢
约束集：$\{z \ge 0\}$——不能穿地面
扰动：风力 $d \in \mathcal{D}$

BRT 的补集就是"安全着陆包络"——从这些状态出发，存在控制策略使无人机安全着陆。如果当前状态不在包络内，无人机应该发出警报或执行应急程序。

这个问题是 6 维的，正好在 Level-Set 方法的极限附近。实际中需要用 decomposition 技巧（将 6D 分解为若干 3D 子问题）来加速计算。

9.4 多智能体可达性分析 (Multi-Agent Reachability) ⭐⭐⭐⭐¶

当场景中有多个智能体时，可达性分析变得极具挑战性——$N$ 个 Dubins car（各 3D 状态）就是 $3N$ 维问题。

NeHMO (Nested Hamilton-Jacobi for Multi-Objective) 方法（Chen, Herbert, Tomlin, 2016）提出了一种分层分解策略：

两两分析：先对每对智能体 $(i, j)$ 做相对坐标下的 HJ 分析（3D），得到两两安全控制策略。
优先级排序：当多个两两约束同时激活时，用优先级决定哪个安全约束优先。
保守组合：对每个智能体，取所有两两安全控制策略的"最保守"交集。

N 个智能体, 3D 每个 → 3N 维
直接计算: 不可行 (维度灾难)

分解策略:
  Agent 1 vs 2 → 3D BRT_{12}
  Agent 1 vs 3 → 3D BRT_{13}
  Agent 2 vs 3 → 3D BRT_{23}
  ...
  共 C(N, 2) = N(N-1)/2 个 3D 计算

Agent i 的安全控制 = argmax_u min_j V_{ij}(x_rel)

局限性：两两分解是保守的——它忽略了三个或更多智能体的联合交互。例如，Agent 1 为了避让 Agent 2 可能不得不靠近 Agent 3。真正的多体安全需要联合可达性分析，但目前仅限于 2-3 个简单智能体。

9.5 自动驾驶安全包络 ⭐⭐⭐¶

HJ 可达性在自动驾驶中有两大应用：

应用 1：安全车道变换包络

考虑自车 (ego) 和目标车道上的一辆车 (other)。状态为相对坐标 $(x_{\text{rel}}, y_{\text{rel}}, v_{\text{rel}}, \theta_{\text{rel}})$（4D），其中 $x_{\text{rel}}$ 为纵向相对距离，$y_{\text{rel}}$ 为横向相对距离，$v_{\text{rel}}$ 为相对速度，$\theta_{\text{rel}}$ 为相对航向角。

离线计算 BRT 后，在线判断：当前相对状态是否在 BRT 外？如果是，变道安全；如果不是，等待。

应用 2：行人-车辆交互安全

将行人建模为最坏情况扰动（行人可能突然改变方向），车辆为控制器。系统状态为 $(x_{\text{car}}, y_{\text{car}}, v_{\text{car}}, \theta_{\text{car}}, x_{\text{ped}}, y_{\text{ped}})$（6D）。BRT 给出"在最坏行人行为下，车辆无法避免碰撞"的状态集合。

这一应用的关键挑战是行人模型的不确定性——如果行人模型过于保守（假设行人可以瞬间改变方向），BRT 过大，车辆几乎无法通行。实际中需要用有限速度和加速度约束来限制行人的"攻击能力"。

9.6 人-机器人共享控制与安全 (Fisac et al., 2019) ⭐⭐⭐⭐¶

核心问题：在人-机器人协作场景中（如外骨骼辅助、辅助驾驶），人类操作者提供控制输入，但人类可能犯错。如何在最小干预人类意图的前提下保证安全？

Fisac 2019 的框架（"A General Safety Framework for Learning-Based Control in Uncertain Robotic Systems", IEEE Trans. Automatic Control, 2019）将这个问题建模为：

\[\dot{x} = f(x, u_h, u_s)\]

其中 $u_h$ 是人类的控制输入（视为不确定/对抗性的），$u_s$ 是安全系统的修正控制。安全系统的策略是：

正常时（$V(x) > \epsilon$，远离 BRT 边界）：不干预，$u_s = 0$，完全信任人类。
警戒时（$0 < V(x) \le \epsilon$）：逐渐介入，$u_s = (1 - V(x)/\epsilon) u_{\text{safe}}$。
危急时（$V(x) \le 0$，进入 BRT）：完全接管，$u_s = u_{\text{safe}}$。

这里 $u_{\text{safe}}$ 来自 HJ 可达性计算的最优安全控制策略。关键的创新在于"最小干预"原则——安全系统只在真正需要时才介入，避免了传统安全系统过于保守地频繁打断人类操作。

与控制理论/80_CLF_CBF与QP安全控制 CBF-QP 的联系：Fisac 的框架可以理解为一个更精细的 safety filter——CBF-QP 在每个时刻都修正控制（可能频繁微调），而 Fisac 的方法只在接近 BRT 边界时才介入（更少干预但干预力度更大）。

9.7 FaSTrack 框架完整解析 ⭐⭐⭐¶

第 9.2 节简要介绍了 FaSTrack 的思想，这里给出完整的数学推导。

问题设定： - 规划模型 (planning model): $\dot{s} = g(s, u_p)$，维度低（如 2D 或 3D） - 跟踪模型 (tracking model): $\dot{x} = f(x, u_t, d)$，维度高（如 6D 或 10D） - 跟踪误差: $e = \phi(x) - s$，其中 $\phi$ 是从高维状态到低维规划空间的投影

跟踪误差动力学：

\[\dot{e} = \frac{\partial \phi}{\partial x} f(x, u_t, d) - g(s, u_p)\]

FaSTrack 将 $u_p$（规划器的输出）视为**对抗性扰动**——规划器可以任意改变方向，跟踪器需要在最坏情况下保证跟踪误差有界。

Step 1：计算 Tracking Error Bound (TEB)

对跟踪误差动力学 $\dot{e} = h(e, x_{\text{aug}}, u_t, d, u_p)$ 做 HJ 可达性分析：

控制器 = 跟踪控制 $u_t$（最小化误差）
扰动 = $(d, u_p)$ 的联合体（最大化误差）
目标集 = $\{e : \|e\| \le r\}$（误差球）

计算 BRT 后，最大控制不变集 $\mathcal{I}(r)$ 的最大 $r$ 值就是 TEB：

\[\text{TEB} = \max\{r : \text{BRT}(-\infty, r) \subseteq \{e : \|e\| \le r\}\}\]

更实际的做法：固定一个小的初始 $r$，计算 BRT。如果 BRT $\subseteq$ 误差球，则 $r$ 有效。逐步增大 $r$ 找到最大值。

Step 2：安全规划

规划器在低维空间中规划路径 $s(t)$，但障碍物需要膨胀 TEB：

\[\mathcal{O}_{\text{inflated}} = \mathcal{O} \oplus B(\text{TEB})\]

其中 $\oplus$ 是 Minkowski 和，$B(\text{TEB})$ 是半径为 TEB 的球。只要规划路径不进入膨胀障碍物，跟踪器就能保证真实系统不碰原始障碍物。

Step 3：在线跟踪控制

在线时，跟踪控制器使用 HJ 值函数计算最优跟踪控制：

\[u_t^* = \arg\max_{u_t} \min_{(d, u_p)} \nabla V_e \cdot h(e, u_t, d, u_p)\]

这个控制器保证跟踪误差永远不超过 TEB。

FaSTrack 的核心优势总结：

维度	直接 HJ 方法	FaSTrack
规划	在高维状态空间规划	在低维规划空间规划
安全	在高维空间计算 BRT	在误差空间计算 BRT（维度可能更低）
在线计算	查高维查找表	查误差空间查找表 + 简单规划器
灵活性	规划器变更需重新计算	规划器可任意替换

💡 概念误区：认为 FaSTrack 的 TEB 是"保证误差为零"

新手想法："FaSTrack 的跟踪控制器能让系统完美跟踪规划路径。"

实际上：FaSTrack 保证的是跟踪误差**有界** ($\|e\| \le \text{TEB}$)，不是零。TEB 的大小取决于规划模型和跟踪模型之间的"差距"——差距越大，TEB 越大，障碍物膨胀越多，可行路径空间越小。

设计指南：规划模型应当尽可能接近真实系统的能力——如果真实系统最大速度 5 m/s，规划模型也应限制在 5 m/s 以内。如果规划模型允许 10 m/s 而真实系统只能 5 m/s，TEB 会非常大。

练习 9.1 ⭐⭐⭐¶

考虑以下 FaSTrack 场景：

规划模型：2D 单积分器 $\dot{s} = u_p$，$\|u_p\| \le 2$ m/s
跟踪模型：2D 双积分器 $\dot{x}_1 = x_2, \dot{x}_2 = u_t + d$，$|u_t| \le 3$ m/s$^2$，$|d| \le 0.5$ m/s$^2$（每个维度独立）
障碍物：$(0, 0)$ 为圆心，半径 $R = 2$ m 的圆

(a) 跟踪误差动力学是什么？（提示：$e = x_1 - s$，$\dot{e} = ?$） (b) TEB 计算需要求解几维的 HJ 问题？ (c) 如果 TEB = 0.8 m，膨胀后的障碍物半径是多少？

本章常见误解汇总¶

误解	正确理解
可达集和可达管是同一个概念	可达集（BRS）是在时刻 $T$ 能到达目标的初始状态集合；可达管（BRT）是在 $[0, T]$ 内任意时刻能到达目标的初始状态集合——BRT = $\bigcup_{t \le T}$ BRS($t$)，BRT 通过 $\min$ 运算在时间上取包络
HJ 可达性分析只能用于安全验证	HJ 可达性同时支持安全分析（避免坏集合）和性能分析（到达好集合），通过改变 $\min/\max$ 的符号约定即可切换；CBVF 框架统一了两者
DeepReach 完全替代了 Level-Set 方法	DeepReach 突破了维度限制（可处理 $d > 6$），但牺牲了 Level-Set 方法的收敛保证和精度控制；安全关键应用中仍需 Level-Set 作为 ground truth 或用可验证的神经网络方法
Isaacs 条件（minimax = maximin）总成立	Isaacs 条件是额外假设——当 Hamiltonian 关于控制和扰动不可分离且不满足凸-凹结构时，上值函数和下值函数不同，博弈没有鞍点
Level-Set 方法的网格分辨率越高越好	分辨率提高使计算量按 $O(n^d)$ 指数增长（$d$ 为状态维度）；实践中需在精度和计算量之间权衡，且数值耗散随网格加细而减少但 CFL 条件要求更小的时间步
HJ 可达性只适用于低维系统	通过分解方法（如 Hopf 公式的并行计算、6D 系统的 3D+3D 分解）和 DeepReach 等神经网络方法，HJ 可达性已被应用于 10D+ 的系统；关键是利用系统的结构性
CBF 和 HJ 可达性是完全独立的理论	CBVF（Choi-Lee-Herbert-Tomlin 2021）证明了 CBF 是 HJ 值函数在 $\gamma > 0$ 折扣下的特例——CBF 的 $\dot{h} + \alpha(h) \ge 0$ 条件等价于折扣 HJI 方程的黏性次解条件

10. 本章小结¶

核心概念总结¶

                    +---------------+
                    |  微分博弈      |
                    | (Isaacs 1965) |
                    +-------+-------+
                            |
                    +-------v-------+
                    |  HJI 方程      |
                    |  V_t + min{0, |
                    |  max min       |
                    |  nabla_V . f}  |
                    |  = 0          |
                    +-------+-------+
                            |
              +-------------+-------------+
              |             |             |
        +-----v----+ +-----v-----+ +-----v-----+
        | Level-Set | | DeepReach | |  CBVF     |
        | 数值方法   | | 神经网络  | | CBF+HJ   |
        | d <= 6   | | d > 6    | | 统一框架  |
        +-----+----+ +-----+-----+ +-----+-----+
              |             |             |
              +-------------+-------------+
                            |
              +-------------+-------------+
              |             |             |
        +-----v----+ +-----v-----+ +-----v-----+
        | Air3D    | | FaSTrack  | | 安全着陆  |
        | 避碰     | | 规划解耦  | | 包络      |
        +----------+ +-----------+ +-----------+

关键公式速查¶

名称	公式
BRT 定义	$\text{BRT}(t) = \{x : V(x,t) \le 0\}$
HJI (BRS)	$V_t + \max_u \min_d \nabla V \cdot f = 0$
HJI (BRT)	$V_t + \min\{0, \max_u \min_d \nabla V \cdot f\} = 0$
CBVF	$V_t + \min\{0, \max_u \min_d \nabla V \cdot f + \gamma V\} = 0$
CFL 条件	$\Delta t \le 1 / \sum_i \alpha_i / \Delta x_i$
LF Hamiltonian	$\hat{H} = H(\bar{p}) - \sum_i \alpha_i (p_i^+ - p_i^-)/2$

能力对照¶

能力	对应章节	自评
解释 HJ 可达性的动机和 CBF 的不足	S2	[ ]
区分 BRS、BRT、Avoid 集	S3	[ ]
推导 HJI 变分不等式	S4	[ ]
理解 Level-Set 数值方法	S5	[ ]
使用 `hj_reachability` 库	S5-6	[ ]
了解 DeepReach 的思路	S7	[ ]
理解 CBVF 统一框架	S8	[ ]

11. 累积项目：Dubins Car 追逃博弈的 BRT ⭐⭐⭐¶

项目目标¶

用 hj_reachability 计算 Dubins car 追逃博弈（Air3D 问题）的 BRT，并实现在线安全控制器。

阶段 1：基础 BRT 计算 ⭐⭐¶

实现 Air3D 动力学（参考第 6 节代码）。
计算 $t \in [-3, 0]$ 的 BRT。
可视化不同 $\theta_r$ 切片下的 BRT 演化。

验证：与 Mitchell-Bayen-Tomlin 2005 论文中的 Figure 1 对比。

阶段 2：最优控制提取 ⭐⭐⭐¶

从计算好的值函数 $V(x, t)$ 中提取最优 evader 控制：

\[\omega_E^*(x) = \arg\max_{\omega_E} \min_{\omega_P} \nabla V \cdot f(x, \omega_E, \omega_P)\]

对于 Air3D 动力学，这有解析形式（bang-bang 控制）。

实现在线安全控制器：
正常飞行时使用名义控制器（如 PD 控制跟踪航路点）。
当 $V(x) \le \epsilon$（接近 BRT 边界）时，切换到最优安全控制。

def safety_controller(x_relative, V_grid, grid, nominal_control,
                      threshold=0.1, omega_e_max=1.0):
    """
    安全控制器: 正常时用名义控制, 危险时切换到最优安全控制.

    Args:
        x_relative: 当前相对状态 [x_r, y_r, theta_r]
        V_grid: 预计算的值函数 (网格上的值)
        grid: hj_reachability Grid 对象
        nominal_control: 名义控制器输出的 omega_e
        threshold: 安全切换阈值
        omega_e_max: 最大角速度

    Returns:
        omega_e: 实际执行的控制
    """
    # 在网格上插值得到当前值函数值
    V_current = interpolate_grid(V_grid, grid, x_relative)

    if V_current > threshold:
        # 安全, 使用名义控制
        return nominal_control

    # 危险! 计算最优安全控制
    # 对 Air3D, 最优 evader 控制是 bang-bang:
    # omega_e* = sign(dV/d_omega_e_direction) * omega_e_max
    grad_V = interpolate_gradient(V_grid, grid, x_relative)

    # dH/d_omega_e = grad_V[0]*y_r - grad_V[1]*x_r - grad_V[2]
    dH_domega_e = (grad_V[0] * x_relative[1]
                   - grad_V[1] * x_relative[0]
                   - grad_V[2])

    omega_e_safe = jnp.sign(dH_domega_e) * omega_e_max

    return omega_e_safe

阶段 3：CBVF 比较 ⭐⭐⭐⭐¶

用不同的 $\gamma$ 值（$0, 0.1, 0.5, 1.0, 5.0$）计算 CBVF。
画出安全集随 $\gamma$ 的变化图。
比较不同 $\gamma$ 下的在线控制器性能（安全性 vs 保守性）。

阶段 4（可选）：DeepReach 实现 ⭐⭐⭐⭐¶

用 PyTorch 实现 SIREN 网络。
在 Air3D 问题上训练 DeepReach。
与 Level-Set 方法的结果对比精度。

提交要求： - 完整的可运行代码（Jupyter Notebook 或 Python 脚本） - BRT 可视化图（至少 3 个时间步 $\times$ 3 个 $\theta_r$ 切片） - 安全控制器的仿真轨迹图 - 简短报告：描述你的实现、遇到的困难、以及对 HJ 可达性方法优缺点的理解

参考文献¶

必读： 1. Mitchell, I. M., Bayen, A. M., & Tomlin, C. J. (2005). A time-dependent Hamilton-Jacobi formulation of reachable sets for continuous dynamic games. IEEE Trans. Automatic Control, 50(7), 947-957. 2. Bansal, S., Chen, M., Herbert, S., & Tomlin, C. J. (2017). Hamilton-Jacobi reachability: A brief overview and recent advances. IEEE CDC Tutorial.

推荐： 3. Choi, J. J., Lee, D., Sreenath, K., Tomlin, C. J., & Herbert, S. L. (2021). Robust control barrier-value functions for safety-critical control. IEEE CDC. 4. Bansal, S., & Tomlin, C. J. (2021). DeepReach: A deep learning approach to high-dimensional reachability. IEEE ICRA. 5. Herbert, S. L., Chen, M., Han, S., Bansal, S., Fisac, J. F., & Tomlin, C. J. (2017). FaSTrack: A modular framework for fast and guaranteed safe motion planning. IEEE CDC.

工具： 6. hj_reachability: https://github.com/StanfordASL/hj_reachability 7. toolboxLS: https://www.cs.ubc.ca/~mitchell/ToolboxLS/ 8. DeepReach: https://github.com/smlbansal/deepreach

附录 A：练习参考答案¶

练习 2.1 答案¶

(a) 当 $x = 1.8$ 时，最坏情况：扰动选 $d = 0.5$（尽量把系统推向危险区域），控制器选 $u = -1$（全力反向）。净速度为 $\dot{x} = -1 + 0.5 = -0.5 < 0$，系统单调远离危险区域。可以保证安全。

(b) 同理，从 $x = 1.2$ 出发，净速度同样为 $-0.5$，更安全。可以保证安全。

(c) 临界值 $x^* = 2$。因为控制能力 $|u_{\max}| = 1$ 大于扰动能力 $|d_{\max}| = 0.5$，所以控制器总能克服扰动。最坏净速度始终为 $-0.5 < 0$，从任何 $x < 2$ 出发系统都单调远离。只有 $x \ge 2$ 时系统已在危险区域内。所以 $\text{BRT} = \mathcal{T} = \{x \ge 2\}$。

扩展思考：如果改为 $d \in [-1.5, 1.5]$（扰动更强），则最坏净速度 $= -1 + 1.5 = 0.5 > 0$。从 $x_0$ 出发经 $T$ 时间后 $x(T) = x_0 + 0.5T$。进入 $\mathcal{T}$ 需要 $x_0 + 0.5T \ge 2$，即 $x_0 \ge 2 - 0.5T$。所以 $\text{BRT}(-T) = \{x \ge 2 - 0.5T\}$，无限时域 BRT $= \mathbb{R}$（整个状态空间都不安全）。

练习 3.1 答案¶

(a) $l(x) = 2 - x$。验证：$l(x) \le 0 \Leftrightarrow 2 - x \le 0 \Leftrightarrow x \ge 2 = \mathcal{T}$。

(b) 最坏扰动 $d = 0.5$，最优控制 $u = -1$。从 $x_0$ 出发：$x(T) = x_0 + (-1 + 0.5)T = x_0 - 0.5T$。系统单调向负方向运动，"最远位置"（在正方向上）就是初始位置 $x_0$ 本身。

(c) 由于 $|u_{\max}| = 1 > |d_{\max}| = 0.5$，控制器总能克服扰动。$\text{BRT}(-T) = \{x \ge 2\}$ 对所有 $T$，BRT 不扩展。

(d) 无限时域 BRT $= \{x \ge 2\} = \mathcal{T}$。

附录 B：HJ 可达性的 2024-2025 前沿进展¶

B.1 Scalable HJ Reachability（高维扩展）¶

传统 Level-Set 方法受限于网格维度灾难（$O(N^d)$ 内存，$d \le 6$）。2023-2025 年的主要突破方向：

方法	原理	最大维度	工具	参考
DeepReach (Bansal 2021)	SIREN 网络逼近值函数	~10D	PyTorch	ICRA 2021
HJLSS (Chen 2024)	低秩张量分解 + 交替方向	~12D	Julia	L4DC 2024
Neural BRT (Hsu 2023)	Self-supervised + physics-informed	~8D	JAX	CoRL 2023
Hopf-Lax formula	解析公式（线性系统）	任意D	MATLAB	Darbon 2016
Decomposition (Chen 2018)	系统分解 + 子问题 BRT 合成	6+6D	MATLAB	CDC 2018

DeepReach 的核心局限： 1. 训练不稳定（SIREN 对超参数敏感） 2. 无法保证值函数的单调性（BRT 可能非单调缩小） 3. 零水平集附近精度不足（安全边界不精确）

2024 年前沿（Verified Neural BRT）：用 interval arithmetic 或 Lipschitz 界验证神经网络 BRT 的正确性——即使网络有近似误差，也能给出**保守但正确**的安全保证。

B.2 Online HJ Reachability（在线安全更新）¶

离线计算 BRT 的假设是动力学和目标集不变。但在实际机器人中： - 障碍物在运动（动态避障） - 系统参数在变化（负载变化、电机老化） - 环境信息在逐步揭示（SLAM 建图）

Warm-starting HJ（Herbert et al. 2021）：当目标集/动力学发生小变化时，从上一次的值函数开始迭代——通常只需 1-3 次 PDE 步（而非从头计算 50-100 步）。

与 MPC Safety Filter 的互补： - HJ 可达性：离线计算安全边界（全局最优，但计算重） - CBF-QP：在线实时安全修正（局部，但计算轻） - 最佳实践：HJ 离线给出值函数 → 值函数在线作为 CBF → CBF-QP 实时执行

B.3 Multi-Agent HJ Reachability¶

当多个智能体需要避碰时，状态空间维度翻倍（两机器人 = 6D+6D = 12D）。

Hamilton-Jacobi-Bellman-Isaacs 多智能体博弈： $$V_t + \max_{u_1} \min_{u_2} \{\nabla V \cdot f(x_1, x_2, u_1, u_2)\} = 0$$

Chen-Tomlin 2018 分解方法：如果多智能体动力学可以**解耦**为独立子系统（除了避碰约束），则可以在低维子空间中分别计算，再用 maximum 运算合成。

练习¶

练习 B.1 ⭐⭐⭐：对一个 4D Dubins car（加入速度状态），用 hj_reachability 计算 BRT。与 3D 版本（固定速度）对比，速度维度增加了什么新的安全信息？

练习 B.2 ⭐⭐⭐⭐：实现一个简单的 DeepReach：用 SIREN 网络（2 隐藏层，256 神经元）逼近 2D 系统的值函数。与网格方法对比精度。讨论：零水平集附近的精度如何保证？

延伸阅读¶

资料	内容	难度
Mitchell-Bayen-Tomlin 2005 TAC	奠基论文，必读	⭐⭐⭐
Bansal et al. 2017 CDC Tutorial	30 页综述，最佳入门	⭐⭐
Bansal-Tomlin 2021 DeepReach	神经网络突破维度	⭐⭐⭐
Choi et al. 2021 CBVF	CBF + HJ 统一	⭐⭐⭐
Herbert et al. 2021 FaSTrack	规划安全解耦	⭐⭐⭐
`hj_reachability` GitHub	JAX 实现，最现代	⭐⭐
Osher-Fedkiw 2002 教材	Level Set 方法完整理论	⭐⭐⭐⭐

累积项目：本章新增模块¶

在"Dubins Car 追逃博弈"累积项目中，本章提供完整的安全控制器实现： 1. 阶段 1：BRT 网格计算（hj_reachability） 2. 阶段 2：最优安全控制器提取 + 在线切换逻辑 3. 阶段 3：CBVF 参数扫描 + 安全性 vs 保守性权衡 4. 阶段 4（可选）：DeepReach 实现与精度对比

本质洞察专栏¶

本质洞察：HJ 可达性分析的核心价值不在于"计算最优控制"（这一点 MPC/DDP 也能做），而在于提供**集合级别的安全保证**——它回答的是"从哪些状态出发，无论对手/扰动怎么行动，系统都能避免进入危险集"。这是一个**worst-case、全状态空间、无限时域**的保证，比 MPC（有限时域、单条轨迹）和 CBF（需要手工设计、难以验证全局有效性）都更强。代价是维度灾难——这正是 HJ 可达性在 2026 年仍未大规模部署的根本原因。

本质洞察：BRT（Backward Reachable Tube）和 BRS（Backward Reachable Set）的区别本质上是"避免 vs 到达"的对偶。BRT 回答"哪些状态无论如何都会进入目标集"（用于安全：目标集 = 危险集 → BRT = 不安全集）；BRS 回答"哪些状态可以到达目标集"（用于可达性：目标集 = 目标位置 → BRS = 可达集）。两者在数学上只差 Hamiltonian 中 min/max 的交换——这不是巧合，而是博弈论中攻守对偶的体现。

综合练习题¶

练习 HJ.1 ⭐（1D 直觉建立）：对一维系统 $\dot{x} = u + d$，$|u| \le 1$，$|d| \le 0.3$，目标集 $\mathcal{T} = [-1, 1]$。(a) 手算 BRT（提示：在 worst-case 扰动下，从 $x > 1$ 出发能在有限时间被拉回 $\mathcal{T}$ 吗？）；(b) 解析解与 HJI PDE 数值解对比；(c) 画出值函数 $V(x,t)$ 随时间的演化。

练习 HJ.2 ⭐⭐（2D Dubins 安全集）：对 Dubins car $\dot{x} = v\cos\theta$，$\dot{y} = v\sin\theta$，$\dot{\theta} = u$（$|u| \le 1$，$v = 1$），目标集为原点周围半径 0.5 的圆盘（障碍物）。用 hj_reachability 计算 BRT，可视化零水平集。讨论：当障碍物半径从 0.5 增大到 2.0 时，安全集如何变化？

练习 HJ.3 ⭐⭐⭐（CBVF 设计与对比）：对同一个 Dubins 系统，分别实现：(a) 网格 HJ 计算的精确 BRT；(b) 手工设计的 CBF $h(x) = \|x - x_{\text{obs}}\|^2 - r^2$；(c) CBVF（Choi et al. 2021，以 HJ 值函数作为 CBF）。比较三者的安全保证强度和在线计算量。指出 CBF 方法在什么情况下会失效而 CBVF 仍然安全。

练习 HJ.4 ⭐⭐⭐（跨章综合 — 安全滤波器集成）：回顾 §3.13（Safety Filter）。设你有一个 RL 策略 $\pi_\theta(x)$ 和预计算的 BRT。设计一个在线安全切换逻辑：当状态接近 BRT 边界时，从 RL 策略切换到 HJ 最优安全控制器。(a) 定义切换阈值 $V(x) \le \epsilon$ 的选择标准（需考虑控制延迟和模型误差）；(b) 证明在理想模型下的递归安全性；(c) 讨论：模型误差如何影响 BRT 的有效性？如何通过膨胀（bloating）补偿？

练习 HJ.5 ⭐⭐⭐⭐（DeepReach 复现）：对 4D 系统（Dubins + 速度），实现一个简化版 DeepReach：(a) 网络架构选择（SIREN vs ReLU，为什么 SIREN 更适合值函数？）；(b) 损失函数设计（HJI PDE 残差 + 终端条件 + 对偶条件）；(c) 训练 1000 epochs 后，将零水平集与网格方法的 ground truth 对比。量化误差：$\text{Vol}(V_{\text{net}} \le 0 \triangle V_{\text{grid}} \le 0) / \text{Vol}(V_{\text{grid}} \le 0)$。

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关节
BRT 计算结果全为正值（安全集 = 全空间）	目标集定义错误（符号反了）或 Hamiltonian 中 min/max 交换	1. 检查目标函数 $l(x)$ 的符号：$l < 0$ 应对应危险区域 2. 验证 Hamiltonian 的极小极大顺序 3. 在 1D 上用解析解验证	§A.1
值函数在网格边界出现伪影（不自然的平坦区或跳变）	边界条件处理不当	1. 检查 `hj_reachability` 的 `periodic_dims` 设置 2. 非周期维度的边界是否设了 extrapolation 3. 扩大计算域，观察伪影是否消失	§A.2
网格计算极慢（4D 系统 >30min per time step）	网格分辨率过高或 JAX 未使用 GPU	1. 检查 `jax.devices()` 确认 GPU 可用 2. 降低分辨率到 $41^4$（先验证正确性再加密） 3. 对周期维度用 FFT 加速	§A
提取的最优控制出现抖振（chattering）	值函数在零水平集附近梯度不连续	1. 增加网格分辨率使梯度估计更平滑 2. 加入 deadband：$	V(x)
DeepReach 训练损失下降但零水平集不准确	损失函数权重不平衡或采样分布偏差	1. 分别打印 PDE 残差、终端条件残差、边界条件残差 2. 在零水平集附近加密采样（importance sampling） 3. 检查时间归一化是否正确	§B

#	自测问题	前置专题
Q1	HJB 方程 \(V_t + \min_u\{L + \nabla V \cdot f\} = 0\) 的每一项物理含义是什么？	控制理论/40_HJB方程与黏性解 §3.4.2
Q2	为什么 HJB 的经典 \(C^1\) 解几乎不存在？黏性解 (viscosity solution) 通过什么机制绕过了这个困难？	控制理论/40_HJB方程与黏性解 §3.4.4
Q3	CBF 的核心不等式 \(L_f h + L_g h \cdot u \ge -\alpha(h)\) 的几何含义是什么？为什么它能保证安全集前向不变？	控制理论/80_CLF_CBF与QP安全控制 §1.2
Q4	什么是 class-\(\mathcal{K}\) 函数？什么是 class-\(\mathcal{KL}\) 函数？	控制理论/70_Lyapunov稳定性理论 §3.7.1
Q5	写出 Dubins car 的运动学方程。	基础

维度	CBF (控制理论/80_CLF_CBF与QP安全控制)	HJ 可达性 (本章)
安全函数 \(h(x)\)	手动设计	自动从动力学计算
前瞻能力	一步导数（局部）	无限时域（全局）
扰动处理	需额外设计 Robust CBF	内建对抗性博弈框架
最优性	不保证最大安全集	给出最大可控不变集
计算成本	在线 QP，实时	离线网格计算，\(O(N^d)\)
维度限制	无（只要 QP 可解）	网格法 \(d \le 6\)；DeepReach 可更高
适用系统	控制仿射 \(\dot{x} = f + gu\)	一般非线性 \(\dot{x} = f(x,u,d)\)
工程用法	在线 safety filter	离线计算 + 在线查表

概念	量词结构	含义	用途
BRS	\(\exists u, \exists s\)	能到达目标	规划（reach）
BRT	\(\forall u, \exists d, \exists s\)	无法避免危险	安全（avoid）

维度	HJB (控制理论/40_HJB方程与黏性解)	HJI (本节)
方程	\(V_t + \min_u\{L + \nabla V \cdot f\} = 0\)	\(V_t + \min\{0, \max_u \min_d \nabla V \cdot f\} = 0\)
运行代价 \(L\)	有（积分代价）	无（只关心到达与否）
扰动 \(d\)	无	有（对抗性）
量词	\(\min_u\)（单人最优）	\(\max_u \min_d\)（二人博弈）
Obstacle 算子	无	\(\min\{0, \cdot\}\)（BRT 用）
终端条件	\(V(x, T) = \Phi(x)\)	\(V(x, 0) = l(x)\)
时间方向	后向（从 \(T\) 向 \(t\)）	后向（从 \(0\) 向 \(t\)）

维度 \(n\)	每维网格点 \(N_i = 100\)	总网格点	内存 (float64)
2	100	\(10^4\)	80 KB
3	100	\(10^6\)	8 MB
4	100	\(10^8\)	800 MB
5	50	\(3 \times 10^8\)	2.4 GB
6	40	\(4 \times 10^9\)	32 GB
7	30	\(2 \times 10^{10}\)	160 GB

工具	语言	维度	特点
toolboxLS (Mitchell 2004)	MATLAB	\(\le 4\)	经典标杆，文档完善
helperOC (Berkeley)	MATLAB	\(\le 6\)	基于 toolboxLS，优化了内存
BEACLS	C++ / CUDA	\(\le 6\)	GPU 加速
`hj_reachability` (Hsu 2022)	Python / JAX	\(\le 5\)	自动微分、JIT 编译
`optimized_dp`	Python	\(\le 6\)	基于 heterocl 的优化

\(\omega_0\)	效果	适用场景
\(\omega_0 = 1\)	低频，光滑	非常光滑的值函数
\(\omega_0 = 10\)	中频	一般的 HJI 问题
\(\omega_0 = 30\)	高频（DeepReach 默认）	值函数有尖锐零水平集
\(\omega_0 = 60\)	很高频	高维、复杂几何

\(\gamma\)	含义	安全集大小	计算要求
\(0\)	经典 HJ	最大（精确 BRT）	需要收敛到稳态
小	接近 HJ	接近最大	收敛较快
大	接近 CBF	较小（保守）	收敛很快
\(\infty\)	纯 CBF	取决于 \(l(x)\) 的设计	无需离线计算

误解	正确理解
可达集和可达管是同一个概念	可达集（BRS）是在时刻 \(T\) 能到达目标的初始状态集合；可达管（BRT）是在 \([0, T]\) 内任意时刻能到达目标的初始状态集合——BRT = \(\bigcup_{t \le T}\) BRS(\(t\))，BRT 通过 \(\min\) 运算在时间上取包络
HJ 可达性分析只能用于安全验证	HJ 可达性同时支持安全分析（避免坏集合）和性能分析（到达好集合），通过改变 \(\min/\max\) 的符号约定即可切换；CBVF 框架统一了两者
DeepReach 完全替代了 Level-Set 方法	DeepReach 突破了维度限制（可处理 \(d > 6\)），但牺牲了 Level-Set 方法的收敛保证和精度控制；安全关键应用中仍需 Level-Set 作为 ground truth 或用可验证的神经网络方法
Isaacs 条件（minimax = maximin）总成立	Isaacs 条件是额外假设——当 Hamiltonian 关于控制和扰动不可分离且不满足凸-凹结构时，上值函数和下值函数不同，博弈没有鞍点
Level-Set 方法的网格分辨率越高越好	分辨率提高使计算量按 \(O(n^d)\) 指数增长（\(d\) 为状态维度）；实践中需在精度和计算量之间权衡，且数值耗散随网格加细而减少但 CFL 条件要求更小的时间步
HJ 可达性只适用于低维系统	通过分解方法（如 Hopf 公式的并行计算、6D 系统的 3D+3D 分解）和 DeepReach 等神经网络方法，HJ 可达性已被应用于 10D+ 的系统；关键是利用系统的结构性
CBF 和 HJ 可达性是完全独立的理论	CBVF（Choi-Lee-Herbert-Tomlin 2021）证明了 CBF 是 HJ 值函数在 \(\gamma > 0\) 折扣下的特例——CBF 的 \(\dot{h} + \alpha(h) \ge 0\) 条件等价于折扣 HJI 方程的黏性次解条件

名称	公式
BRT 定义	\(\text{BRT}(t) = \{x : V(x,t) \le 0\}\)
HJI (BRS)	\(V_t + \max_u \min_d \nabla V \cdot f = 0\)
HJI (BRT)	\(V_t + \min\{0, \max_u \min_d \nabla V \cdot f\} = 0\)
CBVF	\(V_t + \min\{0, \max_u \min_d \nabla V \cdot f + \gamma V\} = 0\)
CFL 条件	\(\Delta t \le 1 / \sum_i \alpha_i / \Delta x_i\)
LF Hamiltonian	\(\hat{H} = H(\bar{p}) - \sum_i \alpha_i (p_i^+ - p_i^-)/2\)

症状	可能原因	排查步骤	相关节
BRT 计算结果全为正值（安全集 = 全空间）	目标集定义错误（符号反了）或 Hamiltonian 中 min/max 交换	1. 检查目标函数 \(l(x)\) 的符号：\(l < 0\) 应对应危险区域 2. 验证 Hamiltonian 的极小极大顺序 3. 在 1D 上用解析解验证	§A.1
值函数在网格边界出现伪影（不自然的平坦区或跳变）	边界条件处理不当	1. 检查 `hj_reachability` 的 `periodic_dims` 设置 2. 非周期维度的边界是否设了 extrapolation 3. 扩大计算域，观察伪影是否消失	§A.2
网格计算极慢（4D 系统 >30min per time step）	网格分辨率过高或 JAX 未使用 GPU	1. 检查 `jax.devices()` 确认 GPU 可用 2. 降低分辨率到 \(41^4\)（先验证正确性再加密） 3. 对周期维度用 FFT 加速	§A
提取的最优控制出现抖振（chattering）	值函数在零水平集附近梯度不连续	1. 增加网格分辨率使梯度估计更平滑 2. 加入 deadband：$	V(x)
DeepReach 训练损失下降但零水平集不准确	损失函数权重不平衡或采样分布偏差	1. 分别打印 PDE 残差、终端条件残差、边界条件残差 2. 在零水平集附近加密采样（importance sampling） 3. 检查时间归一化是否正确	§B