1. 问题的缘起：从贝尔曼“维度灾难”到局部轨迹优化
理查德·贝尔曼（Richard Bellman）在 1950 年代提出了著名的动态规划（Dynamic Programming, DP）与最优性原理。DP 的宏伟蓝图在于求出整个连续状态空间上的全局最优闭环策略 $u = \pi^*(x)$。然而在实际工程中，当状态维度 $n$ 超过 4 维时，空间网格化离散所需要的计算复杂度以指数爆炸 $\mathcal{O}(|\mathcal{S}|^n)$ 攀升，这就是臭名昭著的“维度灾难 (Curse of Dimensionality)”。
面对连续、高维、非线性的机器人多刚体动力学系统（例如一个 12 自由度四足机器人的状态维度高达 24 到 36 维），我们真的需要求解全状态空间每一个角落的最优策略吗？
答案是否定的。机器人在执行具体机动时（如从椅子上站立、越过障碍物、倒车入库），其状态演化往往紧密围绕在某条局部时空管道（Local Trajectory Tube）之内。这就催生了控制理论与现代 AI 的两大核心流派：
- ✕ 样本需求量极高：如强化学习 (PPO/SAC)，需数百万甚至数千万次交互
- ✕ 难以利用精确解析动力学：把动力学视为完全未知的黑盒环境
- ✓ 全域泛化：训练好后推理速度极快，适应广泛初始状态分布
- ✓ 高效二阶收敛：通常仅需数十次反向递推迭代即可收敛至毫秒级精度
- ✓ 深度融合物理规律：直接对分析动力学（牛顿-欧拉或拉格朗日方程）求导
- ✕ 局部最优陷阱：高度依赖初值猜测（Initial Guess），易陷入局部极小
在局部轨迹优化的世界中，通常有三大主流方法：
- 单打靶法 (Single Shooting)：直接将控制序列 $\mathbf{u} = \{u_0, \dots, u_{N-1}\}$ 作为决策变量，状态通过动力学前向积分隐式确定。缺点是对非线性长时域系统极度敏感，微小的控制扰动会导致末端状态巨幅发散。
- 直接配置法 (Direct Collocation)：将状态序列和控制序列同时离散化为决策变量，动力学校验作为等式约束。转化为一个极其庞大但稀疏的大规模非线性规划（NLP，常借助 IPOPT/SNOPT 求解）。优点是易处理复杂约束，缺点是每次迭代需分解巨型稀疏 KKT 矩阵，且不直接生成闭环反馈增益。
- 微分动态规划 (DDP / iLQR)：两者的黄金平衡点。它将贝尔曼最优性原理与二阶牛顿优化巧妙结合。沿时间反向递归时，由于贝尔曼马尔可夫性，时间复杂度关于时域长度 $N$ 保持严格线性 $\mathcal{O}(N)$（而非稠密非线性规划的 $\mathcal{O}(N^3)$）。更关键的是，它在反向递推的同时天然输出了时变局部反馈增益矩阵 $K_t$，赋予了生成的轨迹对抗扰动的鲁棒跟踪能力！
2. 轨迹优化数学表述与 Q 算子
考虑离散时间有限时域（Finite-Horizon）非线性动态系统：
其中状态向量 $x_t \in \mathbb{R}^n$，控制输入向量 $u_t \in \mathbb{R}^m$，$f: \mathbb{R}^n \times \mathbb{R}^m \to \mathbb{R}^n$ 为可微非线性动力学映射。
系统在全时域内的累积标量代价值（Total Cost Functional）定义为各步运行代价（Running Cost）与最终终端代价（Terminal Cost）之和：
$$J(x_0, \mathbf{u}) = l_f(x_N) + \sum_{t=0}^{N-1} l(x_t, u_t)$$轨迹优化的目标是：给定已知初始状态 $x_0$，寻找一个最优控制输入序列 $\mathbf{u}^* = \{u_0^*, u_1^*, \dots, u_{N-1}^*\}$，使得总体代价 $J(x_0, \mathbf{u})$ 达到极小：
$$\mathbf{u}^* = \arg\min_{\mathbf{u}} J(x_0, \mathbf{u})$$价值函数 (Value Function / Cost-to-Go)
根据动态规划思想，我们定义从第 $t$ 步状态 $x$ 开始到时域结束所能取得的最优剩余代价为价值函数 $V(x, t)$：
$$V(x, t) \triangleq \min_{\{u_t, \dots, u_{N-1}\}} \left[ l_f(x_N) + \sum_{\tau=t}^{N-1} l(x_\tau, u_\tau) \right]$$易知在终止时刻 $t = N$，根据定义有边界条件：
$$V(x_N, N) = l_f(x_N)$$贝尔曼最优性原理与连续空间的 Q 算子
贝尔曼最优性原理指出：“最优策略具有这样的性质：无论初始状态和初始决策是什么，其余的决策对于由前一次决策所产生的后续状态而言，也必须构成最优策略。”
将这一原理写成逆向递归方程式：
$$V(x_t, t) = \min_{u_t} \Big[ l(x_t, u_t) + V\big(f(x_t, u_t), t+1\big) \Big]$$在强化学习（RL）中，我们习惯定义 $Q(s, a) = R(s, a) + \gamma V(s')$。而在以最小化代价为导向的最优控制领域，我们完全对偶地定义连续状态-动作价值函数（Action-Value / Q-function）： $$Q(x_t, u_t) \triangleq l(x_t, u_t) + V\big(f(x_t, u_t), t+1\big)$$ 于是贝尔曼最优性方程可以极简地表达为： $$V(x_t, t) = \min_{u_t} Q(x_t, u_t), \qquad u_t^* = \arg\min_{u_t} Q(x_t, u_t)$$
3. 基石演进：有限时域离散时间 LQR
在剖析非线性系统的 DDP 之前，必须先彻底透彻理解线性二次型调节器（Linear Quadratic Regulator, LQR）。因为 DDP 和 iLQR 本质上是在每一轮迭代中，将非线性系统在局部标称轨迹周围近似为一个时变 LQR 问题并反向求解。
假设动力学为线性时变系统，代价函数为严格二次型：
$$x_{t+1} = A_t x_t + B_t u_t$$ $$l(x_t, u_t) = \frac{1}{2} x_t^T Q_t x_t + \frac{1}{2} u_t^T R_t u_t, \quad l_f(x_N) = \frac{1}{2} x_N^T Q_f x_N$$其中权矩阵满足 $Q_t \succeq 0$（半正定），$R_t \succ 0$（正定），$Q_f \succeq 0$。
在时刻 $N$，$V(x_N, N) = \frac{1}{2} x_N^T Q_f x_N$。显然这是一个纯二次型。我们猜想对任意时刻 $t$，价值函数均能保持为纯二次型结构：
$$V(x, t) = \frac{1}{2} x^T P_t x, \quad \text{其中 } P_N = Q_f$$假定第 $t+1$ 步猜想成立，即 $V(x_{t+1}, t+1) = \frac{1}{2} x_{t+1}^T P_{t+1} x_{t+1}$。代入第 $t$ 步的 $Q$ 函数：
$$\begin{aligned} Q(x_t, u_t) &= \frac{1}{2} x_t^T Q_t x_t + \frac{1}{2} u_t^T R_t u_t + \frac{1}{2} (A_t x_t + B_t u_t)^T P_{t+1} (A_t x_t + B_t u_t) \\ &= \frac{1}{2} x_t^T \big( Q_t + A_t^T P_{t+1} A_t \big) x_t + \frac{1}{2} u_t^T \big( R_t + B_t^T P_{t+1} B_t \big) u_t + u_t^T \big( B_t^T P_{t+1} A_t \big) x_t \end{aligned}$$由于 $R_t \succ 0$ 且 $B_t^T P_{t+1} B_t \succeq 0$，二次项系数矩阵 $R_t + B_t^T P_{t+1} B_t$ 严格正定可逆。对控制输入 $u_t$ 求偏导并令其为 0：
$$\nabla_{u_t} Q = (R_t + B_t^T P_{t+1} B_t) u_t + B_t^T P_{t+1} A_t x_t = 0$$直接解析解得最优控制律为纯线性的状态反馈控制器：
$$u_t^* = - K_t x_t, \quad K_t = \big( R_t + B_t^T P_{t+1} B_t \big)^{-1} B_t^T P_{t+1} A_t$$将最优控制律 $u_t^* = - K_t x_t$ 代回 $Q(x_t, u_t^*)$ 即可得到 $V(x_t, t)$：
$$V(x_t, t) = \frac{1}{2} x_t^T \Big[ Q_t + A_t^T P_{t+1} A_t - A_t^T P_{t+1} B_t K_t \Big] x_t = \frac{1}{2} x_t^T P_t x_t$$这就是著名的离散时间 Riccati 差分方程 (Discrete-time Riccati Difference Equation)：
$$P_t = Q_t + A_t^T P_{t+1} A_t - A_t^T P_{t+1} B_t \big( R_t + B_t^T P_{t+1} B_t \big)^{-1} B_t^T P_{t+1} A_t$$LQR 给予我们的核心启示：
- 计算极其敏捷：从终点 $t = N$（$P_N = Q_f$）向后递推计算增益矩阵 $K_t$ 和曲率矩阵 $P_t$，全程无需迭代，一步到位，时间复杂度为 $\mathcal{O}(N)$！
- 闭环鲁棒性：它不仅给出了从初始状态开始的开环轨迹，还给出了沿途每个时刻的反馈增益矩阵 $K_t$。若系统在运行中遭受微小风力或阻力扰动偏离了原定状态，$-K_t \delta x$ 会立即产生纠偏力！
4. 微分动态规划 (Full DDP) 的完整数学推导
Mayne 与 Jacobson 在 1966-1970 年间创立的微分动态规划（Differential Dynamic Programming, DDP），思想精髓就在于：如果我们面对的是高度非线性的动力学 $f(x, u)$ 与一般非线性代价函数 $l(x, u)$，能否在当前已有的候选轨迹周围建立局部二阶泰勒展开，并以此驱动贝尔曼反向更新？
4.1 扰动展开与二阶张量项
设系统当前拥有一条先验的标称轨迹 (Nominal Trajectory)：
$$\bar{\mathbf{X}} = \{\bar{x}_0, \bar{x}_1, \dots, \bar{x}_N\}, \quad \bar{\mathbf{U}} = \{\bar{u}_0, \bar{u}_1, \dots, \bar{u}_{N-1}\}$$定义在标称点附近的微小扰动偏差（Perturbations）为：
$$\delta x_t \triangleq x_t - \bar{x}_t, \quad \delta u_t \triangleq u_t - \bar{u}_t$$同样，将价值函数在标称状态 $\bar{x}_t$ 附近展开至二阶：
$$V(\bar{x}_t + \delta x_t, t) \approx V(\bar{x}_t, t) + V_x^T \delta x_t + \frac{1}{2} \delta x_t^T V_{xx} \delta x_t$$其中 $V_x \in \mathbb{R}^n$ 是价值函数关于状态的梯度向量，$V_{xx} \in \mathbb{R}^{n \times n}$ 是对称 Hessian 矩阵。
现在，我们将状态-动作价值函数 $Q(x, u)$ 在 $(\bar{x}_t, \bar{u}_t)$ 附近进行严格的二阶多元泰勒展开：
$$Q(\bar{x}_t + \delta x_t, \bar{u}_t + \delta u_t) \approx Q(\bar{x}_t, \bar{u}_t) + \begin{bmatrix} Q_x \\ Q_u \end{bmatrix}^T \begin{bmatrix} \delta x_t \\ \delta u_t \end{bmatrix} + \frac{1}{2} \begin{bmatrix} \delta x_t \\ \delta u_t \end{bmatrix}^T \begin{bmatrix} Q_{xx} & Q_{xu} \\ Q_{ux} & Q_{uu} \end{bmatrix} \begin{bmatrix} \delta x_t \\ \delta u_t \end{bmatrix}$$利用多元复合求导的链式法则，注意 $Q(x_t, u_t) = l(x_t, u_t) + V(f(x_t, u_t), t+1)$。为书写凝练，我们用上标撇号表示下一时刻变量，即 $V' \equiv V(\cdot, t+1)$，$V_x' \equiv \nabla_x V(x_{t+1}, t+1)$，$V_{xx}' \equiv \nabla_{xx}^2 V(x_{t+1}, t+1)$。
一阶偏导数（梯度项）：
$$\begin{aligned} Q_x &= l_x + f_x^T V_x' \\ Q_u &= l_u + f_u^T V_x' \end{aligned}$$二阶偏导数（包含动力学二阶张量项）：
$$\begin{aligned} Q_{xx} &= l_{xx} + f_x^T V_{xx}' f_x + \sum_{i=1}^n V_{x, i}' \cdot \nabla_{xx}^2 f^i \\ Q_{uu} &= l_{uu} + f_u^T V_{xx}' f_u + \sum_{i=1}^n V_{x, i}' \cdot \nabla_{uu}^2 f^i \\ Q_{ux} &= l_{ux} + f_u^T V_{xx}' f_x + \sum_{i=1}^n V_{x, i}' \cdot \nabla_{ux}^2 f^i \quad (Q_{xu} = Q_{ux}^T) \end{aligned}$$注：$\sum_{i=1}^n V_{x, i}' \cdot \nabla^2 f^i$ 常记作张量缩并积 $V_x' \cdot f_{xx}$。它反映了由于动力学非线性曲率所带来的额外加速度与形变响应！
4.2 反向传播 (Backward Pass)
在得到局部二次型近似模型后，我们需要在每个时刻寻找能使 $Q$ 最小的最优控制扰动量 $\delta u_t^*$：
$$\delta u_t^*(\delta x_t) = \arg\min_{\delta u_t} \Big[ Q_u^T \delta u_t + \frac{1}{2} \delta u_t^T Q_{uu} \delta u_t + \delta u_t^T Q_{ux} \delta x_t \Big]$$对 $\delta u_t$ 求偏导并令其等于零：
$$\nabla_{\delta u_t} Q = Q_u + Q_{uu} \delta u_t + Q_{ux} \delta x_t = 0$$若 $Q_{uu} \succ 0$（严格正定），两边左乘 $Q_{uu}^{-1}$，即刻得出最优控制扰动的显式解：
其中各分量具有极为优美的物理与控制学内涵：
$$\begin{aligned} \text{开环前馈步长 (Feedforward Gain)}: \quad & \mathbf{k_t = - Q_{uu}^{-1} Q_u} \\ \text{闭环状态反馈增益 (Feedback Gain)}: \quad & \mathbf{K_t = - Q_{uu}^{-1} Q_{ux}} \end{aligned}$$深入观察前馈与反馈的双重角色：
- 前馈项 $k_t$：反映了在当前状态完全不产生偏差（$\delta x_t = 0$）时，控制量沿着负梯度方向应当做出的最速调整。当算法完全收敛至最优稳态时，极值点处必有 $Q_u = 0$，此时前馈量 $k_t \to 0$。
- 反馈增益 $K_t$：度量了后续非线性回放中，如果真实状态偏离了标称轨迹（$\delta x_t \neq 0$），控制量应当如何自适应反抗以纠正偏差。
若当前状态没有发生偏离（$\delta x = 0$），仅仅执行前馈步长 $k_t$，本步代价的期望下降量（Expected Cost Reduction）为：
$$\Delta V_t = Q(\bar{x}_t, \bar{u}_t + k_t) - Q(\bar{x}_t, \bar{u}_t) = k_t^T Q_u + \frac{1}{2} k_t^T Q_{uu} k_t = -\frac{1}{2} Q_u^T Q_{uu}^{-1} Q_u$$全时域的期望总代价缩减量即为各步之和：
$$\Delta J(\alpha) = \alpha \sum_{t=0}^{N-1} k_t^T Q_u + \frac{1}{2} \alpha^2 \sum_{t=0}^{N-1} k_t^T Q_{uu} k_t$$价值函数的一阶与二阶向后传递
将求得的局部最优控制律 $\delta u_t^* = k_t + K_t \delta x_t$ 重新代回二阶 $Q$ 展开式，根据 $V(x_t, t) = Q(x_t, u_t^*)$，整理可得第 $t$ 步价值函数的更新量：
$$\begin{aligned} V_x(t) &= Q_x + K_t^T Q_u + Q_{xu} k_t + K_t^T Q_{uu} k_t \\ &= Q_x + K_t^T (Q_u + Q_{uu} k_t) + Q_{xu} k_t \end{aligned}$$注意到依据前馈增益定义，恒有 $Q_u + Q_{uu} k_t = 0$！此外由对称性 $Q_{xu} = Q_{ux}^T$，故 $Q_{xu} k_t = - K_t^T Q_{uu} k_t = K_t^T Q_u$。因此一阶项具有精炼的紧凑形式：
$$V_x(t) = Q_x + K_t^T Q_u + K_t^T Q_{uu} k_t + Q_{xu} k_t = Q_x - K_t^T Q_{uu} k_t = Q_x + K_t^T Q_u$$同理，对二阶项整理匹配系数（利用 $Q_{ux} = - Q_{uu} K_t$）：
$$V_{xx}(t) = Q_{xx} + K_t^T Q_{uu} K_t + Q_{xu} K_t + K_t^T Q_{ux} = Q_{xx} - K_t^T Q_{uu} K_t$$4.3 前向仿真回放 (Forward Pass)
在完成从 $N-1$ 到 $0$ 的完整反向传递后，我们收获了整条时间链条上的增益序列 $\{k_t, K_t\}_{t=0}^{N-1}$。现在进入前向仿真阶段，在真实非线性动力学中回放生成一条全新轨迹：
若在回放时只应用单纯的开环前馈控制 $\hat{u}_t = \bar{u}_t + \alpha k_t$（相当于普通的非线性规划线搜索），由于实际系统 $f$ 的强非线性，前面积累的微小积分误差会导致后续状态呈指数级脱轨发散，使泰勒局部展开彻底失效！
而状态反馈项 $K_t (\hat{x}_t - \bar{x}_t)$ 就像一条无形的刚度弹簧，时刻感知当前的偏航并将状态“拉拽”回标称管道附近，使大幅度步长更新在复杂物理仿真中依然保持惊人稳定。
5. 迭代 LQR (iLQR / SLQ) 与高斯-牛顿近似
既然完整的 DDP 拥有严格的二次收敛阶（Quadratic Convergence），为何在实际现代机器人开源库（如 Drake、MuJoCo、Crocoddyl、Pinocchio）中，工程师和学者们更倾向于使用 iLQR（Iterative LQR，又称 SLQ - Sequential Linear Quadratic）？
原因在于计算 $Q$ 矩阵时那项令人望而生畏的三阶动力学张量项：
$$V_x' \cdot f_{xx} = \sum_{i=1}^n V_{x, i}' \cdot \nabla_{xx}^2 f^i(x, u)$$对于一个自由度多达几十个的多刚体机械臂或双足人形机器人，$f(x, u)$ 是复杂的非线性拉格朗日欧拉方程或逆牛顿动力学。计算一阶雅可比矩阵 $f_x \in \mathbb{R}^{n \times n}$ 和 $f_u \in \mathbb{R}^{n \times m}$ 尚可借助解析空间代数（Spatial Algebra）快速求解；而计算三阶张量 $f_{xx} \in \mathbb{R}^{n \times n \times n}$ 则需要庞大的内存与昂贵的自动微分开销。
在非线性优化中，类似于从全牛顿法过渡到高斯-牛顿法，iLQR 做出如下经验近似：直接舍弃动力学方程的所有二阶导数项，即令 $f_{xx} \approx 0, f_{uu} \approx 0, f_{ux} \approx 0$。
在此近似下，$Q$ 矩阵的二阶求导公式蜕变为极其清爽的形式：
$$\begin{aligned} Q_{xx} &\approx l_{xx} + f_x^T V_{xx}' f_x \\ Q_{uu} &\approx l_{uu} + f_u^T V_{xx}' f_u \\ Q_{ux} &\approx l_{ux} + f_u^T V_{xx}' f_x \end{aligned}$$| 对比维度 | 全微分动态规划 (Full DDP) | 迭代 LQR (iLQR / SLQ) |
|---|---|---|
| 所需动力学导数 | 一阶雅可比 $f_x, f_u$ ＋ 二阶张量 $f_{xx}, f_{uu}, f_{ux}$ | 仅需一阶雅可比 $f_x, f_u$ |
| 优化类型 | 标准牛顿法（全二阶优化） | 高斯-牛顿法（Gauss-Newton） |
| 单次迭代计算耗时 | 高（受张量计算与矩阵乘法支配） | 极低（通常比 Full DDP 快 3 到 10 倍） |
| 渐进收敛速度 | 严格二次收敛 (Quadratic) | 超线性或局部线性 (Superlinear) |
| 非凸曲率敏感度 | 若二阶导不定，Hessian 极易出现负特征值 | 若 $l_{xx} \succeq 0$ 且 $l_{uu} \succ 0$，结构性半正定，更少遭遇鞍点 |
| 机器人工程主流地位 | 特定高动态、极速机动（如航天火箭/敏捷拦截） | 绝大多数机器人首选方案（如四足步态、机械臂避障） |
6. 工程落地与数值避坑技巧
在实验室从推公式到实机（Sim-to-Real）部署 DDP/iLQR 时，工程师们最常遭遇的三大痛点是：$Q_{uu}$ 不可逆或非正定、执行器力矩限幅约束、以及如何处理远离动力学可行性的离散初值。
6.1 LM 阻尼与正规化 (Regularization / Damping)
当且仅当 $Q_{uu} \succ 0$ 时，反向解出的控制扰动才代表极小化方向。若曲率矩阵存在负特征值，直接求逆会走向极大化灾难；若特征值接近零，则导致逆矩阵数值爆炸。
工程中普遍采用 Levenberg-Marquardt (LM) 风格的阻尼正规化机制：
方案 A：控制空间阻尼 (Control Regularization)
$$\tilde{Q}_{uu} = Q_{uu} + \mu I_m$$当 $\mu \to 0$ 时，算法退化为标准牛顿步；当阻尼因子 $\mu \to \infty$ 时，$k_t = -\tilde{Q}_{uu}^{-1} Q_u \approx -\frac{1}{\mu} Q_u$，算法自动优雅地蜕变为带步长衰减的最速梯度下降法！
方案 B：状态空间阻尼 (State Regularization, Todorov et al.)
$$\tilde{V}_{xx}' = V_{xx}' + \mu I_n \implies \tilde{Q}_{uu} = l_{uu} + f_u^T (V_{xx}' + \mu I_n) f_u$$通过对后续价值函数的曲率施加惩罚，间接向控制空间注入阻尼，同时保留了系统动力学的几何耦合特性。
自适应阻尼调度逻辑：
- 如果前向仿真线搜索成功找到了更低的累积代价：接受新轨迹，并降低阻尼 $\mu \leftarrow \max(\mu_{min}, \mu / \Delta_\mu)$，鼓励下一步采取更激进的牛顿步；
- 如果尝试完所有线搜索步长后代价依然上升：拒绝本次更新，激进增大阻尼 $\mu \leftarrow \min(\mu_{max}, \mu \cdot \Delta_\mu)$，退守至更加保守的梯度下降模式。
6.2 控制输入约束与 Box-QP
机器人的伺服电机绝对存在物理饱和限制，例如扭矩区间 $u_{\min} \le u_t \le u_{\max}$。如果在求出 $k_t + K_t \delta x_t$ 后暴力进行阶段截断（Clamping），相当于人为破坏了反向梯度的无偏性，极易导致迭代振荡乃至不收敛。
在反向传递求解每一步极值时，直接将问题建模为带边界约束的局部二次规划（Box-constrained QP）： $$\min_{\delta u_t} \quad q(\delta u_t) = Q_u^T \delta u_t + \frac{1}{2} \delta u_t^T Q_{uu} \delta u_t + \delta u_t^T Q_{ux} \delta x_t$$ $$\text{s.t.} \quad u_{\min} - \bar{u}_t \le \delta u_t \le u_{\max} - \bar{u}_t$$ Box-QP 算法通过维护一个自由变量集合 (Free Index Set) 与钳位变量集合 (Clamped Index Set)，仅在自由变量子空间中进行未约束反向求逆，并在钳位变量处将行与列置零。由于控制维度 $m$ 通常较小（如机械臂关节数 7，机器狗单腿关节数 3），Box-QP 通常在 3 到 5 次轻量迭代内即可求得精确解！
6.3 不可行初值与多重打靶 FDDP
传统单打靶 DDP 要求初始候选轨迹必须是动力学生成严格可行的 (Dynamically Feasible)。但如果用户给出的初值只是起点与终点之间的平直线段插值，强行前向积分常常会导致末端剧烈碰撞或倒立摆下坠。
Carlos Mastalli 等人在 2020 年著名的开源运控库 Crocoddyl 中提出了 FDDP (Feasibility-driven Differential Dynamic Programming)：
- 容许初值动力学不连贯：允许每一步存在动力学校验残差 $\bar{d}_t = f(\bar{x}_t, \bar{u}_t) - \bar{x}_{t+1} \neq 0$。
- 反向传递吸收残差：将残差线性项融合进价值反向递推方程中；
- 多重打靶渐进闭合 (Multiple Shooting Gap Closing)：在线搜索过程中，通过类似拉格朗日罚函数机制逐步将残差压制到零，赋予了算法在极复杂动作（如接触碰撞、跳跃翻滚）中令人难以置信的全局初值收敛能力。
7. 控制论与现代强化学习的交汇
微分动态规划并非与现代强化学习割裂的孤岛，二者在底层哲学与算法架构上深度交融。
1. Guided Policy Search (GPS)：局部轨迹优化作为“超级导师”
Sergey Levine 在其突破性工作（Levine & Koltun, 2013）中提出了引导策略搜索（GPS）。由于端到端深度神经网络策略在复杂高维空间中进行无模型强化学习（Model-Free RL）探索如同大海捞针，GPS 借助 DDP/iLQR 在多个局部初始点优化出高质量的轨迹分布及反馈增益，然后使用基于 Bregman 散度或 ADMM 的优化方法，将这些局部控制律蒸馏监督训练进一个单一的深度神经网络策略中，实现了兼具局部最优性与全局泛化能力的革命性突破。
2. 滚动时域模型预测控制 (Model Predictive Control, MPC)
在工业界，DDP 最普及的部署形态是 Real-time DDP-MPC。系统以例如 100Hz 的频率不断执行以下循环：
- 传感器读取当前最新状态 $x_{\text{curr}}$；
- 以前一次优化的解作为热启动初值（Warm Start），在未来 $T = 1.0\text{s}$ 的短时域内执行 1~2 次 DDP 迭代；
- 仅将计算得出的第一个控制量 $\hat{u}_0$ 注入电机；
- 时钟推进一拍，重复上述过程。
8. 完整 Python 算法架构实战
下面给出一个符合工业界工程标准、包含反向传递、前向回放、LM 阻尼与 Armijo 线搜索的纯 Python (NumPy) iLQR 核心架构实现，可以直接作为轨迹优化底座：
▶
🐍 工业级 iLQR / DDP 纯 Python (NumPy) 求解器实战架构源码
Python 3.9+ · 130 lines
import numpy as np
class IterativeLQR:
def __init__(self, dynamics, cost, n_x, n_u, horizon):
"""
dynamics: 动力学对象，提供 f(x, u), fx(x, u), fu(x, u)
cost: 代价对象，提供 l(x, u), lx, lu, lxx, luu, lux, lf(x), lfx, lfxx
"""
self.dynamics = dynamics
self.cost = cost
self.n_x = n_x
self.n_u = n_u
self.N = horizon
# 正规化超参数
self.mu = 1e-4
self.mu_min = 1e-6
self.mu_max = 1e8
self.delta_mu = 2.0
def solve(self, x0, u_guess, max_iters=50, tol=1e-5):
# 1. 初始前向推演
x_bar = np.zeros((self.N + 1, self.n_x))
u_bar = np.array(u_guess)
x_bar[0] = x0
for t in range(self.N):
x_bar[t+1] = self.dynamics.f(x_bar[t], u_bar[t])
current_cost = self._calc_total_cost(x_bar, u_bar)
for iteration in range(max_iters):
# 2. 反向传播 Backward Pass
k_seq, K_seq, expected_red, success = self._backward_pass(x_bar, u_bar)
if not success:
self.mu = min(self.mu_max, self.mu * self.delta_mu)
continue
# 3. 前向线搜索 Forward Pass with Line Search
x_new, u_new, new_cost, accepted = self._forward_pass(x_bar, u_bar, k_seq, K_seq, current_cost, expected_red)
if accepted:
cost_diff = current_cost - new_cost
x_bar, u_bar, current_cost = x_new, u_new, new_cost
# 成功步：衰减阻尼因子
self.mu = max(self.mu_min, self.mu / self.delta_mu)
if cost_diff < tol:
print(f"[iLQR] 收敛于第 {iteration+1} 次迭代，最终代价: {current_cost:.5f}")
break
else:
# 失败步：激增阻尼，退守梯度下降
self.mu = min(self.mu_max, self.mu * self.delta_mu)
return x_bar, u_bar, K_seq
def _backward_pass(self, x_bar, u_bar):
k_seq = np.zeros((self.N, self.n_u))
K_seq = np.zeros((self.N, self.n_u, self.n_x))
expected_reduction = 0.0
# 终端价值初始化
V_x = self.cost.lfx(x_bar[self.N])
V_xx = self.cost.lfxx(x_bar[self.N])
for t in reversed(range(self.N)):
xt, ut = x_bar[t], u_bar[t]
fx, fu = self.dynamics.fx(xt, ut), self.dynamics.fu(xt, ut)
# 一阶梯度 Q_x, Q_u
Q_x = self.cost.lx(xt, ut) + fx.T @ V_x
Q_u = self.cost.lu(xt, ut) + fu.T @ V_x
# 二阶矩阵 Q_xx, Q_uu, Q_ux (iLQR 高斯-牛顿近似)
Q_xx = self.cost.lxx(xt, ut) + fx.T @ V_xx @ fx
Q_uu = self.cost.luu(xt, ut) + fu.T @ V_xx @ fu
Q_ux = self.cost.lux(xt, ut) + fu.T @ V_xx @ fx
# 正规化阻尼
Q_uu_damped = Q_uu + self.mu * np.eye(self.n_u)
# Cholesky 分解确保正定性
try:
L = np.linalg.cholesky(Q_uu_damped)
except np.linalg.LinAlgError:
return None, None, 0.0, False # 矩阵非正定，反向传播失败
# 求解前馈增益 k_t 与反馈增益 K_t
k = -np.linalg.solve(L.T, np.linalg.solve(L, Q_u))
K = -np.linalg.solve(L.T, np.linalg.solve(L, Q_ux))
# 累加期望收益
expected_reduction += (-k.T @ Q_u - 0.5 * k.T @ Q_uu @ k)
# 更新下一时刻价值导数
V_x = Q_x + K.T @ Q_u + K.T @ Q_uu @ k + Q_ux.T @ k
V_xx = Q_xx + K.T @ Q_uu @ K + K.T @ Q_ux + Q_ux.T @ K
V_xx = 0.5 * (V_xx + V_xx.T) # 强制保持数值对称
k_seq[t] = k
K_seq[t] = K
return k_seq, K_seq, expected_reduction, True
def _forward_pass(self, x_bar, u_bar, k_seq, K_seq, current_cost, expected_reduction):
alphas = [1.0, 0.5, 0.25, 0.125, 0.0625, 0.0]
c1 = 1e-4
for alpha in alphas:
x_new = np.zeros_like(x_bar)
u_new = np.zeros_like(u_bar)
x_new[0] = x_bar[0]
for t in range(self.N):
# 关键：应用前馈缩放与全额状态反馈
u_new[t] = u_bar[t] + alpha * k_seq[t] + K_seq[t] @ (x_new[t] - x_bar[t])
x_new[t+1] = self.dynamics.f(x_new[t], u_new[t])
new_cost = self._calc_total_cost(x_new, u_new)
# Armijo 准则
if current_cost - new_cost >= c1 * alpha * expected_reduction:
return x_new, u_new, new_cost, True
return None, None, current_cost, False
def _calc_total_cost(self, x, u):
total = self.cost.lf(x[self.N])
for t in range(self.N):
total += self.cost.l(x[t], u[t])
return total
9. 轨迹优化方法全景对比速查
为了在架构选型时不迷失方向，以下汇总了现代运控与优化领域主流流派的综合横向评测：
| 算法名称 | 数学本质 | 时间复杂度 | 收敛速度 | 是否自带反馈增益 | 约束适应能力 | 工业级代表开源库 |
|---|---|---|---|---|---|---|
| LQR | 解析线性二次最优 | $\mathcal{O}(N)$ | 单步解析完成 | 是（天然伴生 $K_t$） | 弱（无硬约束） | SciPy / Control-Toolbox |
| iLQR / SLQ | 高斯-牛顿局部动态规划 | $\mathcal{O}(N (n+m)^3)$ | 超线性 (Superlinear) | 是（开环前馈+闭环增益） | 良（支持 Box-QP） | Crocoddyl / TrajOpt |
| Full DDP | 二阶牛顿动态规划 | $\mathcal{O}(N (n^3 + n^2 m))$ | 严格二次 (Quadratic) | 是（天然伴生 $K_t$） | 良（投影/增广拉格朗日） | Crocoddyl / DDP-Matlab |
| 直接配置法 (DIRCOL) | 稀疏大规模 NLP (SQP) | $\mathcal{O}(N^3)$ (稀疏化后 $\mathcal{O}(N)$) | 二次收敛 (IPOPT) | 否（仅产出开环轨迹） | 极优（支持任意非线性隐式约束） | Drake / CasADi / SNOPT |
| 无模型强化学习 (SAC/PPO) | 黑盒随机策略梯度搜索 | $\mathcal{O}(\text{Sample } \times \text{FLOPs})$ | 缓慢样本经验累积 | 是（神经网络闭环输出） | 差（依赖惩罚项塑形） | Stable-Baselines3 / RLlib |
10. 经典文献与进阶阅读指南
如果您希望在机器人运控、自动驾驶轨迹规划与端到端控制领域深入钻研，强烈推荐研读以下奠基性里程碑文献：
-
[1]
Mayne, David Q. (1966). "A second-order gradient method for determining optimal trajectories of non-linear discrete-time systems." International Journal of Control, 3(1), 85-95.
DDP 理论的开山鼻祖论文，首次证明了局部二阶展开与贝尔曼逆向递归的等价性。 -
[2]
Jacobson, David H., & Mayne, David Q. (1970). "Differential Dynamic Programming." Elsevier, New York.
经典学术巨著，奠定了整个现代微分动态规划数学理论大厦。 -
[3]
Li, Weiwei, & Todorov, Emanuel. (2004). "Iterative linear quadratic regulator design for nonlinear systems." ICINCO 2004.
提出 iLQR 并系统论述了高斯-牛顿近似的优越性，奠定了其在现代计算机图形学与机器人仿真的统治地位。 -
[4]
Tassa, Yuval, Erez, Tom, & Todorov, Emanuel. (2012). "Synthesis and stabilization of complex behaviors through online trajectory optimization." IROS 2012.
奠基性工程巨制，首次系统提出了 Box-QP 控制限幅机制并实现了复杂机器人的毫秒级在线实时控制。 -
[5]
Mastalli, Carlos, et al. (2020). "Crocoddyl: An efficient and versatile framework for multi-contact optimal control." ICRA 2020.
提出 FDDP，成为当代足式机器人与四足机械狗多接触运动控制的顶级工业标准库。 -
[6]
Levine, Sergey, & Koltun, Vladlen. (2013). "Guided policy search." ICML 2013.
将 iLQR 优化作为导师引入深度学习全局策略学习，现代基于模型的深度强化学习先驱论文。