如果满足，则结束；否则更新乘子：  

  并增大惩罚参数 $ \mu \leftarrow \rho \mu $。  

非线性规划中的差分动态规划（DDP）算法进阶题：带路径约束的轨迹优化问题 题目描述 考虑一个离散时间最优控制问题，其目标是通过非线性规划最小化一个代价函数，同时满足系统动力学方程和路径约束。该问题描述如下： 寻找控制序列 \( u_ 0, u_ 1, \dots, u_ {N-1} \) 和状态序列 \( x_ 0, x_ 1, \dots, x_ N \)，最小化总代价： \[ J = \phi(x_ N) + \sum_ {k=0}^{N-1} L(x_ k, u_ k) \] 其中，\( x_ k \in \mathbb{R}^n \) 是状态向量，\( u_ k \in \mathbb{R}^m \) 是控制向量。 满足： 系统动力学（等式约束）： \[ x_ {k+1} = f(x_ k, u_ k), \quad k = 0,1,\dots,N-1 \] 路径约束（不等式约束）： \[ g(x_ k, u_ k) \le 0, \quad k = 0,1,\dots,N-1 \] 初始状态固定： \[ x_ 0 = x_ {\text{init}} \] 这里，\( \phi \) 是终端代价函数，\( L \) 是阶段代价函数，\( f \) 是非线性状态转移函数，\( g \) 是非线性路径约束函数。 我们要求用差分动态规划（Differential Dynamic Programming, DDP）算法求解此问题，并详细解释如何处理路径约束。 解题过程循序渐进讲解 差分动态规划（DDP）是一种基于动态规划的最优控制算法，通过二次逼近和迭代求解局部最优控制策略。它在无约束问题中表现优异，但处理路径约束时需要额外技巧。下面将DDP扩展到带路径约束的问题，关键步骤是使用增广拉格朗日法或松弛变量法将约束纳入目标函数，然后迭代求解。 步骤1：问题重构与增广拉格朗日法 由于原始DDP只能处理无约束问题，我们首先引入松弛变量或增广拉格朗日项将路径约束转化为惩罚项，从而将约束问题转化为一系列无约束子问题。 1.1 引入松弛变量和惩罚项 将路径约束 \( g(x_ k, u_ k) \le 0 \) 改写为等式形式： \[ g(x_ k, u_ k) + s_ k = 0, \quad s_ k \ge 0 \] 其中 \( s_ k \in \mathbb{R}^p \) 是非负松弛变量。 然后，在目标函数中加入二次惩罚项： \[ J_ {\text{aug}} = J + \sum_ {k=0}^{N-1} \left[ \lambda_ k^\top (g(x_ k, u_ k) + s_ k) + \frac{\mu}{2} \| g(x_ k, u_ k) + s_ k \|^2 \right ] \] 这里，\( \lambda_ k \) 是拉格朗日乘子估计，\( \mu > 0 \) 是惩罚参数。 通过调整 \( \lambda_ k \) 和 \( \mu \)，逐步逼近原约束问题的解。 1.2 迭代框架 整个算法在一个外层循环中迭代更新 \( \lambda_ k \) 和 \( \mu \)，每个外层迭代内，用DDP求解当前惩罚项下的无约束问题（固定 \( \lambda_ k, \mu \)）。 步骤2：DDP的核心——动态规划与二次逼近 对于每个外层迭代，我们求解如下无约束问题： 最小化 \( J_ {\text{aug}} \) 满足动力学 \( x_ {k+1} = f(x_ k, u_ k) \)。 DDP从最后一步向前递推，计算最优控制修正量。 2.1 定义值函数 在时间步 \( k \)，定义值函数 \( V_ k(x) \) 为从状态 \( x \) 出发到终点的最小代价： \[ V_ k(x) = \min_ {u_ k, \dots, u_ {N-1}} \left[ \phi(x_ N) + \sum_ {j=k}^{N-1} \left( L(x_ j, u_ j) + \lambda_ j^\top (g(x_ j, u_ j) + s_ j) + \frac{\mu}{2} \| g(x_ j, u_ j) + s_ j \|^2 \right) \right ] \] 注意松弛变量 \( s_ j \) 在每一步可独立优化，通常有闭式解（例如通过投影到非负象限），此处我们先将其视为控制变量的一部分。 2.2 二次逼近与贝尔曼方程 DDP的关键是在当前轨迹 \( (\bar{x}_ k, \bar{u}_ k) \) 附近对值函数和动力学进行二阶泰勒展开。 定义状态和控制偏差： \( \delta x_ k = x_ k - \bar{x}_ k \)，\( \delta u_ k = u_ k - \bar{u}_ k \)。 在 \( (\bar{x} k, \bar{u} k) \) 处展开增量代价 \( Q_ k \)： \[ Q_ k(\delta x, \delta u) = \ell_ k + \ell_ x^\top \delta x + \ell_ u^\top \delta u + \frac{1}{2} \begin{bmatrix} \delta x \\ \delta u \end{bmatrix}^\top \begin{bmatrix} \ell {xx} & \ell {xu} \\ \ell_ {ux} & \ell_ {uu} \end{bmatrix} \begin{bmatrix} \delta x \\ \delta u \end{bmatrix} \] 其中，\( \ell_ k = L(\bar{x}_ k, \bar{u}_ k) + \lambda_ k^\top (g(\bar{x}_ k, \bar{u} k) + s_ k) + \frac{\mu}{2} \| g(\bar{x} k, \bar{u} k) + s_ k \|^2 \)，而 \( \ell_ x, \ell_ u, \ell {xx}, \ell {xu}, \ell {uu} \) 是 \( \ell_ k \) 对 \( x_ k, u_ k \) 的一阶和二阶导数（注意 \( s_ k \) 可视为 \( u_ k \) 的函数或独立变量，此处为简化，我们先将其与 \( u_ k \) 合并处理）。 展开动力学： \[ f(x_ k, u_ k) \approx \bar{x}_ {k+1} + A_ k \delta x_ k + B_ k \delta u_ k \] 其中 \( A_ k = \frac{\partial f}{\partial x}(\bar{x}_ k, \bar{u}_ k) \)，\( B_ k = \frac{\partial f}{\partial u}(\bar{x}_ k, \bar{u}_ k) \)。 步骤3：DDP反向传播 从终端 \( k = N \) 开始，向后递推计算值函数的二次逼近系数。 3.1 终端条件 在 \( k = N \)： \[ V_ N(x) = \phi(x_ N) \] 在 \( \bar{x} N \) 附近展开： \[ V_ N \approx \phi_ N + \phi_ x^\top \delta x_ N + \frac{1}{2} \delta x_ N^\top \phi {xx} \delta x_ N \] 所以初始化： \( V_ x^{(N)} = \phi_ x \)，\( V_ {xx}^{(N)} = \phi_ {xx} \)。 3.2 反向递推公式 对于 \( k = N-1, \dots, 0 \)： 假设已知下一步的值函数近似 \( V_ {k+1}(x) \approx V_ {k+1} + V_ x^{(k+1)\top} \delta x_ {k+1} + \frac{1}{2} \delta x_ {k+1}^\top V_ {xx}^{(k+1)} \delta x_ {k+1} \)。 将 \( \delta x_ {k+1} = A_ k \delta x_ k + B_ k \delta u_ k \) 代入 \( Q_ k \) 表达式，得到关于 \( \delta u_ k \) 的二次函数： \[ Q_ k(\delta x_ k, \delta u_ k) = \text{常数} + q_ x^\top \delta x_ k + q_ u^\top \delta u_ k + \frac{1}{2} \begin{bmatrix} \delta x_ k \\ \delta u_ k \end{bmatrix}^\top \begin{bmatrix} Q_ {xx} & Q_ {xu} \\ Q_ {ux} & Q_ {uu} \end{bmatrix} \begin{bmatrix} \delta x_ k \\ \delta u_ k \end{bmatrix} \] 其中系数 \( q_ x, q_ u, Q_ {xx}, Q_ {xu}, Q_ {uu} \) 由 \( \ell_ k \) 的导数、\( A_ k, B_ k \) 和 \( V_ {k+1} \) 的系数组合而成（具体公式略，可参考标准DDP推导）。 3.3 求解最优控制修正 最小化 \( Q_ k \) 得到： \[ \delta u_ k^* = -Q_ {uu}^{-1} (q_ u + Q_ {ux} \delta x_ k) = l_ k + L_ k \delta x_ k \] 其中 \( l_ k = -Q_ {uu}^{-1} q_ u \)，\( L_ k = -Q_ {uu}^{-1} Q_ {ux} \)。 3.4 更新值函数系数 将 \( \delta u_ k^* \) 代入 \( Q_ k \)，得到 \( V_ k \) 的二次近似系数： \[ V_ x^{(k)} = q_ x + L_ k^\top Q_ {uu} l_ k + L_ k^\top q_ u + Q_ {xu}^\top l_ k \] \[ V_ {xx}^{(k)} = Q_ {xx} + L_ k^\top Q_ {uu} L_ k + L_ k^\top Q_ {ux} + Q_ {xu}^\top L_ k \] （注意，实际计算中常用更稳定的公式避免数值问题）。 步骤4：前向模拟与线搜索 反向传播完成后，我们得到一系列反馈控制律 \( \delta u_ k = l_ k + L_ k \delta x_ k \)。然后进行前向模拟，从 \( x_ 0 \) 开始，应用更新后的控制序列，并计算实际代价。由于二次逼近仅在局部有效，通常需要线搜索保证收敛。 4.1 控制更新公式 \[ u_ k^{\text{new}} = \bar{u}_ k + \alpha l_ k + L_ k (x_ k^{\text{new}} - \bar{x} k) \] 其中 \( \alpha \in (0,1] \) 是线搜索步长，通过回溯法选择，使得实际代价 \( J {\text{aug}} \) 下降。 4.2 更新轨迹 用新的控制序列前向积分动力学 \( x_ {k+1} = f(x_ k, u_ k) \)，得到新轨迹 \( \{ x_ k^{\text{new}}, u_ k^{\text{new}} \} \)。 如果代价下降不足，减小 \( \alpha \) 并重试。 步骤5：处理路径约束的完整算法 将DDP嵌套在增广拉格朗日法的外层循环中： 初始化 ：给定初始猜测轨迹 \( \{ \bar{x}_ k, \bar{u}_ k \} \)，初始乘子 \( \lambda_ k = 0 \)，惩罚参数 \( \mu > 0 \)，放大因子 \( \rho > 1 \)，容忍误差 \( \epsilon \)。 外层循环 （乘子更新）： a. 用当前 \( \lambda_ k, \mu \) 构造增广代价 \( J_ {\text{aug}} \)。 b. 调用DDP内层循环，求解无约束问题，得到新轨迹。 c. 检查约束违反度： \[ \| g(x_ k, u_ k)^+ \| < \epsilon \quad (\text{其中 } g^+ = \max(g,0)) \] 如果满足，则结束；否则更新乘子： \[ \lambda_ k \leftarrow \lambda_ k + \mu \, g(x_ k, u_ k)^+ \] 并增大惩罚参数 \( \mu \leftarrow \rho \mu \)。 内层循环 （DDP迭代）： a. 反向传播计算 \( l_ k, L_ k \)。 b. 前向模拟和线搜索得到新轨迹。 c. 检查DDP收敛（如控制变化很小），若不收敛则返回a，用新轨迹作为参考轨迹继续迭代。 输出 ：满足约束的最优轨迹 \( \{ x_ k^ , u_ k^ \} \)。 关键点总结 DDP本质 ：利用动态规划的贝尔曼最优性原理，通过二阶展开高效求解局部最优控制律。 约束处理 ：通过增广拉格朗日法（或松弛变量）将约束转化为目标函数的惩罚项，使DDP可处理路径约束。 数值稳定性 ：需谨慎处理 \( Q_ {uu} \) 的正定性（可加正则化项），并采用线搜索保证全局收敛。 计算复杂度 ：与状态维度的三次方相关，但对控制维度依赖较低，适合控制维度较小的问题。 该进阶题展示了如何将经典DDP扩展到带约束的轨迹优化，结合了动态规划、二次逼近和约束优化技巧，是机器人运动规划、航天器轨迹设计等领域的核心算法之一。