微分动态规划算法推导与最优轨迹控制

考虑一个连续时间动力学系统，状态向量为 $\mathbf{x}(t) \in \mathbb{R}^N$，控制输入向量为 $\mathbf{u}(t) \in \mathbb{R}^M$，其状态转移微分方程为：

$$\frac{d\mathbf{x}}{dt} = f(\mathbf{x}(t), \mathbf{u}(t), t)$$

我们的优化目标是最小化累积代价泛函（Cost Function）：

$$J(\mathbf{x}(t_0), \pi) = \underbrace{h(\mathbf{x}(t_f), t_f)}_{\text{终端代价 (Terminal Cost)}} + \int_{t_0}^{t_f} \underbrace{g(\mathbf{x}(t), \mathbf{u}(t), t)}_{\text{运行阶段代价 (Cost-to-go)}} dt$$

最优控制问题即为寻找最优策略 $\pi^*$，使得从初始状态 $\mathbf{x}(t_0)$ 出发的总代价最小：

$$\pi^* = \arg \min_\pi J(\mathbf{x}(t_0), \pi)$$

时间离散化形式

假设系统时不变（Time-invariant），设定总时域步长为 $N$，采样时间间隔 $\Delta t = \frac{t_f - t_0}{N}$。采用一阶欧拉积分对连续动力学进行离散化：

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

对应的离散化总代价函数为：

$$J(\mathbf{x}_0, \mathbf{U}) = h(\mathbf{x}_N) + \sum_{k=0}^{N-1} g(\mathbf{x}_k, \mathbf{u}_k)$$

其中 $\mathbf{U} = [\mathbf{u}_0, \mathbf{u}_1, \dots, \mathbf{u}_{N-1}]$ 为离散控制序列，$\mathbf{X} = [\mathbf{x}_0, \mathbf{x}_1, \dots, \mathbf{x}_N]$ 为对应的状态轨迹。

为了直观理解动态规划，我们将其视作在有向图上寻找从起点到目标点的最小代价路径。每个节点代表状态 $\mathbf{x}$，拥有一个值函数（Value Function）$V(\mathbf{x})$，表示从当前状态到达目标状态的剩余最小代价：

根据理查德·贝尔曼（Richard Bellman）的贝尔曼最优性原理（Bellman Optimality Principle）：任意状态的最优价值等于当前单步代价与后续下一状态最优价值之和的最小值：

$$V^*(\mathbf{x}) = \min_{\mathbf{u}} \Big[ g(\mathbf{x}, \mathbf{u}) + V^*(\mathbf{x}') \Big]$$

其中 $\mathbf{x}' = f(\mathbf{x}, \mathbf{u})$。

由于目标状态没有后续决策，其终端价值已知：$V^*(\mathbf{x}_{\text{goal}}) = h(\mathbf{x}_{\text{goal}}) = 0$。因此，我们可以从终点反向倒推（Backward Induction），逐层计算出全图所有状态的真值！

定义状态扰动 $\delta \mathbf{x}_k = \mathbf{x}_k - \bar{\mathbf{x}}_k$，控制扰动 $\delta \mathbf{u}_k = \mathbf{u}_k - \bar{\mathbf{u}}_k$。在第 $k$ 步，考虑采取控制扰动后引起的未来总代价变化量（Q 函数）：

$$Q(\delta \mathbf{x}_k, \delta \mathbf{u}_k) = g(\bar{\mathbf{x}}_k + \delta \mathbf{x}_k, \bar{\mathbf{u}}_k + \delta \mathbf{u}_k) + V(\bar{\mathbf{x}}_{k+1} + \delta \mathbf{x}_{k+1}) - V(\bar{\mathbf{x}}_k)$$

对 $Q$ 函数关于 $(\delta \mathbf{x}_k, \delta \mathbf{u}_k)$ 进行二阶泰勒级数展开：

$$Q(\delta \mathbf{x}_k, \delta \mathbf{u}_k) \approx Q_0 + \mathbf{Q}_x^T \delta \mathbf{x}_k + \mathbf{Q}_u^T \delta \mathbf{u}_k + \frac{1}{2} \begin{bmatrix} \delta \mathbf{x}_k \\ \delta \mathbf{u}_k \end{bmatrix}^T \begin{bmatrix} \mathbf{Q}_{xx} & \mathbf{Q}_{xu} \\ \mathbf{Q}_{ux} & \mathbf{Q}_{uu} \end{bmatrix} \begin{bmatrix} \delta \mathbf{x}_k \\ \delta \mathbf{u}_k \end{bmatrix}$$

根据链式求导法则，展开系数由当前阶段代价 $g$ 与下一时刻值函数 $V_{k+1}$ 的梯度和 Hessian 矩阵复合而成：

$$\mathbf{Q}_x = \mathbf{g}_x + \mathbf{f}_x^T \mathbf{V}_x'$$ $$\mathbf{Q}_u = \mathbf{g}_u + \mathbf{f}_u^T \mathbf{V}_x'$$ $$\mathbf{Q}_{xx} = \mathbf{g}_{xx} + \mathbf{f}_x^T \mathbf{V}_{xx}' \mathbf{f}_x + \mathbf{V}_x' \cdot \mathbf{f}_{xx}$$ $$\mathbf{Q}_{uu} = \mathbf{g}_{uu} + \mathbf{f}_u^T \mathbf{V}_{xx}' \mathbf{f}_u + \mathbf{V}_x' \cdot \mathbf{f}_{uu}$$ $$\mathbf{Q}_{ux} = \mathbf{g}_{ux} + \mathbf{f}_u^T \mathbf{V}_{xx}' \mathbf{f}_x + \mathbf{V}_x' \cdot \mathbf{f}_{ux}$$

（注：在著名的 iLQR 算法中，通常忽略系统动力学的二阶项 $\mathbf{f}_{xx}, \mathbf{f}_{uu}, \mathbf{f}_{ux}$，仅保留动力学一阶导数，从而极大降低求导开销）。

为了使当前局部的代价变化最小，对 $Q(\delta \mathbf{x}_k, \delta \mathbf{u}_k)$ 关于控制扰动 $\delta \mathbf{u}_k$ 求极值：

$$\frac{\partial Q}{\partial \delta \mathbf{u}_k} = \mathbf{Q}_u + \mathbf{Q}_{uu} \delta \mathbf{u}_k + \mathbf{Q}_{ux} \delta \mathbf{x}_k = 0$$

若 $\mathbf{Q}_{uu} > 0$（正定矩阵），可直接解出最优控制增量 $\delta \mathbf{u}_k^*$：

$$\delta \mathbf{u}_k^* = \underbrace{-\mathbf{Q}_{uu}^{-1} \mathbf{Q}_u}_{\mathbf{k}_k \text{ (开环前馈增益)}} + \underbrace{-\mathbf{Q}_{uu}^{-1} \mathbf{Q}_{ux}}_{\mathbf{K}_k \text{ (闭环状态反馈增益)}} \delta \mathbf{x}_k$$ $$\delta \mathbf{u}_k^* = \mathbf{k}_k + \mathbf{K}_k \delta \mathbf{x}_k$$

将最优控制增量代回二次型中，即可反向递推更新当前时刻的值函数梯度与 Hessian 矩阵：

$$\Delta V_k = -\frac{1}{2} \mathbf{k}_k^T \mathbf{Q}_{uu} \mathbf{k}_k$$ $$\mathbf{V}_x(k) = \mathbf{Q}_x - \mathbf{K}_k^T \mathbf{Q}_{uu} \mathbf{k}_k = \mathbf{Q}_x + \mathbf{K}_k^T \mathbf{Q}_u$$ $$\mathbf{V}_{xx}(k) = \mathbf{Q}_{xx} - \mathbf{K}_k^T \mathbf{Q}_{uu} \mathbf{K}_k$$

在反向传播计算出全时域的前馈增益序列 $\{\mathbf{k}_k\}$ 与反馈增益序列 $\{\mathbf{K}_k\}$ 后，进入正向模拟阶段（Forward Pass）：

从初始状态 $\mathbf{x}_0$ 出发，使用带有步长缩放因子 $\alpha \in (0, 1]$ 的控制律前向积分生成新轨迹：

$$\hat{\mathbf{x}}_0 = \mathbf{x}_0$$ $$\hat{\mathbf{u}}_k = \bar{\mathbf{u}}_k + \alpha \, \mathbf{k}_k + \mathbf{K}_k (\hat{\mathbf{x}}_k - \bar{\mathbf{x}}_k)$$ $$\hat{\mathbf{x}}_{k+1} = f(\hat{\mathbf{x}}_k, \hat{\mathbf{u}}_k)$$

Armijo 回溯线搜索（Backtracking Line Search）

由于二阶泰勒展开仅在局部邻域内有效，直接采用全步长 $\alpha=1$ 容易引起非线性系统的剧烈超调甚至轨迹发散。通过 Armijo 条件动态回退步长：

$$J(\hat{\mathbf{X}}, \hat{\mathbf{U}}) \le J(\bar{\mathbf{X}}, \bar{\mathbf{U}}) + c_1 \alpha \sum_{k=0}^{N-1} \mathbf{k}_k^T \mathbf{Q}_u$$

每次若代价未充分下降，则令 $\alpha \leftarrow \beta \alpha$（通常 $\beta = 0.5$），直到找到满足充分下降的新轨迹。