# Adaptive Optimal Control of Unknown Nonlinear Systems via Homotopy-Based Policy Iteration

> Chen et al., IEEE Trans. Autom. Control, Vol. 69, No. 5, pp. 3396–3403, May 2024
>
> Implementation: Deep NN function approximators (CriticNN + ActorNN) trained via gradient descent

---

## 这篇文章解决了什么问题？

用强化学习做最优控制，本质上是在**模型信息不完全**的情况下，通过数据学习一个最优策略。Lewis 团队长期用 **Policy Iteration (PI)** 来解决这个问题，但 PI 有一个前提：必须先有一个 **admissible policy** 来启动迭代。

问题在于：**求解 admissible policy 本身就需要系统的模型信息**（$f(x)$ 和 $g(x)$）——这就形成了一个循环依赖：

> **PI 的初始化困境**：PI 要求初始策略是 admissible 的 → 没有模型就算不出 admissible 策略 → 没有 admissible 策略就无法启动 PI → 无法启动 PI 就学不出最优策略。

> **什么是 Admissible Policy？**
>
> 给定广义代价函数
> $$ J(x_0) = \int_0^\infty \left[ Q(x) + u^T R(x) u \right] dt $$
> 称 $u(x)$ 是 **admissible** 的，当且仅当：对任意初始状态 $x_0$，$J(x_0)$ 有限，且系统状态被驱动到原点。
>
> 直观理解：相当于要求 $u(x)$ 让系统是 **Lyapunov 稳定**的——随着 $t \to \infty$，每一步的代价必须趋近于 0，否则积分发散。

---

## 核心思想：同伦 (Homotopy)

对原始系统动态方程做一个**同伦变换**，人为引入阻尼项：

$$ \underbrace{\dot{x} = f(x) + g(x)u}_{\text{原始系统}} \quad\longrightarrow\quad \underbrace{\dot{x} = f(x) - L_i x + g(x)u_i(x)}_{\text{同伦系统}} $$

加上 $-L_i x$ 之后，即使 **$u = 0$**（零控制），系统也是稳定的——相当于给系统加了「反向阻尼」。此时 $u=0$ 天然就是 admissible 的，PI 可以直接启动。

然后通过逐步减小 $L_i$，让同伦系统**慢慢逼近**原始系统。当 $L_i$ 最终归零时，我们就得到了原始系统下的一个 admissible 策略，进而可以转入传统的 PI 求解最优控制。

> **关键点**：同伦项 $-L_i x$ 的作用是作为一个**人工阻尼**让系统自治稳定，而不是去「抵消」未知动态 $f(x)$ 和 $g(x)$。$f$ 和 $g$ 的未知性通过后面的 off-policy 数据驱动方法（积分消除 + 神经网络逼近）来绕过。

---

## 数学原理

### 问题设定

非线性系统状态方程：

$$ \dot{x} = f(x) + g(x)u $$

以及 Lipschitz 条件：

$$ \|f(z)-f(y)\| \le L_f\|z-y\|, \qquad \|g(z)-g(y)\| \le L_g\|z-y\| $$

保证系统的存在唯一性。控制目标是极小化广义非二次代价函数：

$$ J(x_0) = \int_0^\infty \left[ Q(x) + u^T R(x) u \right] dt $$

在某个策略 $u(x)$ 下的代价函数值 $V(x)$ 满足 **Hamiltonian 方程**：

$$ 0 = \left( \frac{\partial V}{\partial x} \right)^T \bigl( f(x) + g(x)u \bigr) + Q(x) + u^T R(x) u \tag{5} $$

对 (5) 关于 $u$ 求极小，得到最优策略的解析形式：

$$ u^*(x) = -\frac{1}{2} R^{-1}(x) g^T(x) \frac{\partial V(x)}{\partial x} \tag{6} $$

将 (6) 代回 (5) 得到 **HJB 方程**（$V(0)=0$）：

$$ 0 = \left( \frac{\partial V}{\partial x} \right)^T f(x) + Q(x) - \frac{1}{4} \left( \frac{\partial V}{\partial x} \right)^T g(x) R^{-1}(x) g^T(x) \frac{\partial V}{\partial x} \tag{7} $$

### 传统 PI 方法

> **Policy Iteration（策略迭代）**
>
> 1. **策略评估**：给定 $u_i^0(x)$，求解 $V_i^0(x)$（$V_i^0(0)=0$）：
>    $$ 0 = \left( \frac{\partial V_i^0}{\partial x} \right)^T \bigl( f + g u_i^0 \bigr) + Q + (u_i^0)^T R u_i^0 \tag{8} $$
> 2. **策略改进**：用 $V_i^0$ 更新策略：
>    $$ u_{i+1}^0(x) = -\frac{1}{2} R^{-1} g^T \frac{\partial V_i^0}{\partial x} \tag{9} $$

PI 保证了 $V_{i+1}^0(x) \le V_i^0(x)$，序列 $\{u_i^0\}, \{V_i^0\}$ 一致收敛到最优解。但一切的前提是：**初始策略 $u_0^0$ 必须是 admissible 的**。

### 同伦 PI 方法（Homotopy-Based PI）

先定义一个宽松的初始条件：

> **Definition 2 (Lipschitz Admissibility)**：零控制 $u=0$ 是 **Lipschitz admissible** 的，如果存在常数 $L$ 使得系统 $\dot{x} = f(x) - Lx + g(x)u$ 渐近稳定，且代价 $J(x_0)$ 有限。一个充分条件是取 $L > L_f$。

同伦 PI 的迭代格式（Lemma 2）：

- **策略评估**（同伦系统下的 Bellman 方程）：
  $$ 0 = \left( \frac{\partial V_i}{\partial x} \right)^T \bigl[ f - L_i x + g u_i \bigr] + Q + u_i^T R u_i \tag{12} $$
- **策略改进**：
  $$ u_{i+1}(x) = -\frac{1}{2} R^{-1} g^T \frac{\partial V_i}{\partial x} \tag{13} $$
- **步长选取**（保证收敛）：
  $$ (1-\gamma)Q\gamma_{i+1} \;\le\; \left( \frac{\partial V_i}{\partial x} \right)^T x \;\le\; (1-\gamma)(Q + u_{i+1}^T R u_{i+1}) + (u_i - u_{i+1})^T R (u_i - u_{i+1}) \tag{14} $$

> **收敛性保证**：
> 1. $u_{i+1}(x)$ 在系统 $\dot{x} = f - L_{i+1}x + g u_{i+1}$ 下是 admissible 的。
> 2. 若 $(\frac{\partial V_i}{\partial x})^T x \ge \beta V_i(x)$（$\beta > 0$），则**有限步内 $L_i = 0$**。

当 $L_i = 0$ 时，(12) 退化为传统 PI 的 (8)，此时 $u_{i+1}$ 就是原始系统下的 admissible 策略。

---

## 数据驱动的同伦 PI — 本实现

### Off-Policy 积分消除

核心技巧：将 $V_i$ 沿轨迹求导，消去 $f$ 和 $g$：

$$ V_i(x(t_{k+1})) - V_i(x(t_k)) - \int_{t_k}^{t_{k+1}} \left(\frac{\partial V_i}{\partial x}\right)^T L_i x\,dt
 = -\int_{t_k}^{t_{k+1}} \Bigl[ Q + u_i^T R u_i + 2u_{i+1}^T R v_i \Bigr] dt \tag{20} $$

其中 $v_i = u - u_i$ 是实际执行的控制（PE + 旧策略）与目标策略的偏差。

> **为什么这能绕过 $f$ 和 $g$？** 等式 (20) 两边只包含：(1) 状态采样值 $x(t_k), x(t_{k+1})$；(2) 已知的 $L_i$；(3) 待求的函数 $V_i, u_{i+1}$。$f(x)$ 和 $g(x)$ 被 $V_i(x(t_{k+1})) - V_i(x(t_k))$ 隐式替代。

### Deep NN 函数逼近

本实现用**深度神经网络**（而非多项式基函数）逼近 $V(x)$ 和 $u(x)$：

**CriticNN — 值函数 $V(x) \ge 0$：**
```
Linear(2, 32, bias=False) → Tanh
Linear(32, 32, bias=False) → Tanh
Linear(32, 2, bias=False)      # 两个原始输出 z1, z2
V(x) = z1(x)² + z2(x)²         # 结构保证 V≥0, V(0)=0
```
- `bias=False` 全程保证 $V(0) = 0$（结构保证）
- 两个输出头使 Hessian 在原点可达 rank-2（严格正定可能）

**ActorNN — 策略 $u(x)$：**
```
Linear(2, 32, bias=True) → Tanh
Linear(32, 32, bias=True) → Tanh
Linear(32, 1, bias=True)       # 有符号控制输出
```

### 解耦损失函数

**Critic Loss — Bellman 残差（策略评估）：**
$$ \text{Loss}_C = \mathbb{E}\left[ \left( \Delta V_k - \int \nabla V \cdot (Lx)\,dt + \int (Q + R\hat{u}^2)\,dt + \int 2R\hat{u}_{\text{new}} v\,dt \right)^2 \right] $$

其中 Actor 输出被 detach，梯度仅流向 Critic。

**Actor Loss — 最优策略回归（策略改进）：**
$$ \text{Loss}_A = \mathbb{E}\left[ \left( u_{\text{NN}}(x) - \left(-\frac{1}{2}R^{-1}g^T(x)\nabla V(x)\right) \right)^2 \right] $$

对倒立摆 ($J=1, R=1$)：$u^* = -0.5 \cdot \partial V / \partial x_2$

Critic 梯度被 detach，梯度仅流向 Actor。两个网络各有独立的 Adam 优化器。

### 算法三阶段

1. **Phase One — 寻找 L₀**：
   - 用 $u = \text{PE}$（零策略）采集一条轨迹
   - 扫描所有 $L \in [L_{\text{start}}, L_{\text{max}}]$，每个 L 训练独立 trainer
   - 选取 Bellman 残差最低的 L 作为 L₀（而非第一个正定的 L）
   - 对应的 Critic 必须通过正定性检验（Hessian at origin PD + 轨迹上 V(x)≥0）

2. **Phase Two — 同伦收缩**：
   - 用当前策略 + PE 采集轨迹
   - 训练 Critic（Bellman 残差）和 Actor（最优策略回归）
   - 用 safety constraint (14) 计算步长 α，$L \leftarrow L - \alpha$
   - PD 不通过则回退到 best weights；连续拒绝超 15 次则提前终止
   - 迭代至 $L = 0$

3. **Phase Three — 标准 PI**：
   - 设 $L = 0$，用 Phase Two 的策略作为初始策略
   - 交替 Critic 评估 + Actor 改进，直到 loss 收敛
   - 输出最终策略 $u^*(x)$ 和值函数 $V^*(x)$

---

## 总结与思考

### 本文贡献

打破了 PI 必须从 admissible 策略开始这一模型依赖的死循环：

$$ \text{同伦阻尼 } L_i x \;\xrightarrow{\text{使 }u=0\text{ 稳定}}\; \text{Lipschitz admissible} \;\xrightarrow{\text{逐步归零 }L_i}\; \text{admissible} \;\xrightarrow{\text{传统 PI}}\; \text{最优策略} $$

### 本实现的改进

1. **Deep NN 替代多项式基函数**：避免了多项式基函数的严重多重共线性问题（cond(A) ~ 1e9），使系统矩阵不再奇异
2. **梯度下降替代矩阵求逆**：用 Adam 优化 Bellman 残差 + 策略回归，彻底规避病态线性系统
3. **解耦 Actor-Critic 训练**：Actor 直接回归最优控制律 $u^* = -1/(2R)g^T\nabla V$，比联合求解更稳定
4. **Phase One 全扫描**：扫描所有 L 取最低 loss，而非取第一个正定的 L，确保找到最佳同伦起点