差分动态规划（DDP）在带控制饱和约束的轨迹优化问题中的基础应用
字数 4720 2025-12-14 20:03:51

差分动态规划（DDP）在带控制饱和约束的轨迹优化问题中的基础应用

我将为您讲解一个差分动态规划（Differential Dynamic Programming, DDP）算法的基础应用问题。这个问题涉及到带控制饱和约束的轨迹优化，是机器人控制、航空航天等领域的典型问题。

问题描述

考虑一个简单的离散时间非线性系统，需要将其从初始状态驱动到目标状态，同时最小化控制能量。系统模型和控制输入都受到约束。

数学模型：

  1. 系统动力学（离散时间）：

\[ x_{t+1} = f(x_t, u_t) = x_t + \Delta t \cdot \begin{bmatrix} \dot{q}_t \\ u_t \end{bmatrix} \]

其中状态 \(x_t = [q_t, \dot{q}_t]^T\)，\(q_t\) 是位置，\(\dot{q}_t\) 是速度，控制输入 \(u_t\) 是加速度。为简化，设 \(\Delta t = 0.1\) s，且 \(f\) 为线性：\(x_{t+1} = A x_t + B u_t\)，其中

\[ A = \begin{bmatrix} 1 & 0.1 \\ 0 & 1 \end{bmatrix}, \quad B = \begin{bmatrix} 0.005 \\ 0.1 \end{bmatrix}. \]

  1. 控制饱和约束：

\[ u_{\min} \leq u_t \leq u_{\max}, \quad \text{例如 } -2 \leq u_t \leq 2. \]

  1. 目标函数（总代价）：

\[ J = \sum_{t=0}^{N-1} \left( \frac{1}{2} (x_t - x_{\text{goal}})^T Q (x_t - x_{\text{goal}}) + \frac{1}{2} R u_t^2 \right) + \frac{1}{2} (x_N - x_{\text{goal}})^T Q_f (x_N - x_{\text{goal}}) \]

其中 \(x_{\text{goal}} = [1, 0]^T\) 是目标状态，\(Q = I\)（单位矩阵），\(R = 0.1\)，\(Q_f = 10I\)，总步数 \(N=50\)。

  1. 初始状态：\(x_0 = [0, 0]^T\)。

问题：在控制饱和约束下，寻找最优控制序列 \(\{u_0, u_1, ..., u_{N-1}\}\) 和状态轨迹 \(\{x_0, x_1, ..., x_N\}\)，最小化总代价 \(J\)。

解题过程循序渐进讲解

差分动态规划（DDP）是一种基于动态规划和二阶近化的高效轨迹优化算法，特别适合处理非线性动力学和约束问题。下面分步骤详细讲解。

步骤1：理解DDP的核心思想

DDP本质上是一种迭代优化算法，属于动态规划的一种近似实现。其核心思想是：

  • 反向传播：从轨迹末端开始，向后迭代计算局部二次模型（代价函数关于状态和控制的二阶展开），并求解局部线性二次调节器（LQR）问题，得到最优控制更新律。
  • 前向滚动：使用反向传播得到的控制策略，从初始状态开始前向仿真，生成一条新的轨迹。
  • 迭代改进：重复反向传播和前向滚动，直到轨迹收敛到满足最优性条件。

对于控制饱和约束，DDP通常通过投影或裁剪方式处理。

步骤2：算法初始化

  1. 给定一条初始猜测轨迹（通常从初始控制序列全为零或简单猜测开始）。

    • 初始控制序列：\(u_t^{(0)} = 0\)，\(t=0,...,N-1\)。
    • 通过前向仿真得到初始状态轨迹：\(x_0\) 已知，\(x_{t+1}^{(0)} = f(x_t^{(0)}, u_t^{(0)})\)。

  2. 设置算法参数：

    • 正则化参数 \(\mu\)（初始为0，用于保证 Hessian 正定）。
    • 步长衰减因子 \(\alpha \in (0,1)\)（用于线搜索）。
    • 收敛容差 \(\epsilon\)（例如 \(10^{-6}\)）。

步骤3：反向传播（Backward Pass）

这是DDP的核心步骤。从最后一步 \(N\) 开始，倒序计算到第一步 \(0\)。

对于每个时间步 \(t = N, N-1, ..., 0\)：

  1. 定义价值函数：\(V_t(x)\) 表示从时刻 \(t\)、状态 \(x\) 出发到终点的最小累积代价。

    • 终端条件：\(V_N(x) = \frac{1}{2} (x - x_{\text{goal}})^T Q_f (x - x_{\text{goal}})\)。

  2. 局部二次近似：
    在当前的轨迹点 \((x_t, u_t)\) 附近，对Q函数（即从当前步开始的即时代价加上下一步的价值函数）进行二阶泰勒展开：

\[ Q(\delta x, \delta u) = \text{常数} + \begin{bmatrix} Q_x \\ Q_u \end{bmatrix}^T \begin{bmatrix} \delta x \\ \delta u \end{bmatrix} + \frac{1}{2} \begin{bmatrix} \delta x \\ \delta u \end{bmatrix}^T \begin{bmatrix} Q_{xx} & Q_{xu} \\ Q_{ux} & Q_{uu} \end{bmatrix} \begin{bmatrix} \delta x \\ \delta u \end{bmatrix} \]

其中：

  • \(\delta x = x - x_t\)，\(\delta u = u - u_t\)。
  • 梯度 \(Q_x, Q_u\) 和 Hessian \(Q_{xx}, Q_{xu}, Q_{ux}, Q_{uu}\) 通过链式法则计算，涉及动力学 \(f\) 和价值函数 \(V_{t+1}\) 的导数。

具体计算公式：

\[ \begin{aligned} Q_x &= l_x + f_x^T V'_{x}, \\ Q_u &= l_u + f_u^T V'_{x}, \\ Q_{xx} &= l_{xx} + f_x^T V'_{xx} f_x + V'_x \cdot f_{xx}, \\ Q_{uu} &= l_{uu} + f_u^T V'_{xx} f_u + V'_x \cdot f_{uu}, \\ Q_{xu} &= l_{xu} + f_x^T V'_{xx} f_u + V'_x \cdot f_{xu}. \end{aligned} \]

这里 \(l(x_t, u_t)\) 是单步代价，\(V'_{x}, V'_{xx}\) 是下一步价值函数 \(V_{t+1}\) 的梯度和 Hessian（在反向传播中已计算）。

  1. 求解无约束最优控制更新：
    最小化 \(Q(\delta x, \delta u)\) 得到最优控制增量：

\[ \delta u^* = -Q_{uu}^{-1} (Q_u + Q_{ux} \delta x). \]

注意：\(Q_{uu}\) 必须正定。若不满足，可加入正则化项 \(\mu I\) 使其正定。

  1. 处理控制饱和约束：
    由于控制输入有界 \(u_{\min} \leq u \leq u_{\max}\)，在计算最优控制更新后，需要进行裁剪：

\[ u_{\text{new}} = \text{clip}(u_t + \delta u^*, u_{\min}, u_{\max}) \]

其中 \(\text{clip}(u, a, b) = \max(a, \min(u, b))\)。

  1. 更新价值函数：
    将最优控制律代入，得到价值函数的参数更新：

\[ \begin{aligned} k_t &= -Q_{uu}^{-1} Q_u, \\ K_t &= -Q_{uu}^{-1} Q_{ux}, \\ \Delta V &= -\frac{1}{2} k_t^T Q_{uu} k_t, \\ V_x &= Q_x - K_t^T Q_{uu} k_t, \\ V_{xx} &= Q_{xx} - K_t^T Q_{uu} K_t. \end{aligned} \]

这里 \(k_t\) 是开环项，\(K_t\) 是反馈增益矩阵。控制律为 \(u = u_t + k_t + K_t \delta x\)。

步骤4：前向滚动（Forward Pass）

使用反向传播得到的控制策略，从初始状态 \(x_0\) 开始，前向仿真生成新轨迹：

  1. 对于 \(t = 0, 1, ..., N-1\)：

    • 计算控制：\(u_t^{\text{new}} = u_t^{(k)} + k_t + K_t (x_t^{\text{new}} - x_t^{(k)})\)。
    • 应用饱和约束：\(u_t^{\text{new}} = \text{clip}(u_t^{\text{new}}, u_{\min}, u_{\max})\)。
    • 模拟下一步状态：\(x_{t+1}^{\text{new}} = f(x_t^{\text{new}}, u_t^{\text{new}})\)。

  2. 计算新轨迹的总代价 \(J_{\text{new}}\)。

步骤5：线搜索与接受准则

由于泰勒展开是局部近似，直接采用新轨迹可能不保证代价下降。因此需要线搜索：

  1. 比较新代价 \(J_{\text{new}}\) 与旧代价 \(J_{\text{old}}\)。
  2. 如果 \(J_{\text{new}} < J_{\text{old}}\)，接受新轨迹，并减小正则化参数 \(\mu \leftarrow \mu / 2\)。
  3. 如果 \(J_{\text{new}} \geq J_{\text{old}}\)，拒绝新轨迹，增加正则化参数 \(\mu \leftarrow \max(2\mu, \mu_{\min})\)，并缩小步长重新前向滚动（即控制更新量乘以衰减因子 \(\alpha\) 后重做前向仿真）。

步骤6：收敛判断

重复步骤3-5，直到满足以下收敛条件之一：

  • 代价下降量 \(|J_{\text{new}} - J_{\text{old}}| / |J_{\text{old}}| < \epsilon\)。
  • 控制更新量的范数 \(\|k_t\|\) 小于阈值。
  • 达到最大迭代次数。

算法特点与小结

  • DDP优势：利用动态规划的递归结构，通过二阶展开捕获非线性，通常比一阶方法收敛更快，且能自然处理状态相关约束（通过投影或罚函数）。
  • 控制饱和处理：本例通过简单的裁剪（投影）处理控制约束。更复杂的方法可结合屏障函数或增广拉格朗日法。
  • 计算效率：反向传播中需计算和存储每个时间步的增益矩阵，内存和计算量随状态和控制维度平方增长，但对于中小规模问题非常高效。

通过以上步骤，DDP 能有效求解带控制饱和约束的轨迹优化问题，找到一条从起点到目标点的能量最优轨迹。

差分动态规划（DDP）在带控制饱和约束的轨迹优化问题中的基础应用 我将为您讲解一个差分动态规划（Differential Dynamic Programming, DDP）算法的基础应用问题。这个问题涉及到带控制饱和约束的轨迹优化，是机器人控制、航空航天等领域的典型问题。 问题描述 考虑一个简单的离散时间非线性系统，需要将其从初始状态驱动到目标状态，同时最小化控制能量。系统模型和控制输入都受到约束。 数学模型 ： 系统动力学（离散时间）： \[ x_ {t+1} = f(x_ t, u_ t) = x_ t + \Delta t \cdot \begin{bmatrix} \dot{q}_ t \\ u_ t \end{bmatrix} \] 其中状态 \(x_ t = [ q_ t, \dot{q}_ t]^T\)，\(q_ t\) 是位置，\(\dot{q} t\) 是速度，控制输入 \(u_ t\) 是加速度。为简化，设 \(\Delta t = 0.1\) s，且 \(f\) 为线性：\(x {t+1} = A x_ t + B u_ t\)，其中 \[ A = \begin{bmatrix} 1 & 0.1 \\ 0 & 1 \end{bmatrix}, \quad B = \begin{bmatrix} 0.005 \\ 0.1 \end{bmatrix}. \] 控制饱和约束： \[ u_ {\min} \leq u_ t \leq u_ {\max}, \quad \text{例如 } -2 \leq u_ t \leq 2. \] 目标函数（总代价）： \[ J = \sum_ {t=0}^{N-1} \left( \frac{1}{2} (x_ t - x_ {\text{goal}})^T Q (x_ t - x_ {\text{goal}}) + \frac{1}{2} R u_ t^2 \right) + \frac{1}{2} (x_ N - x_ {\text{goal}})^T Q_ f (x_ N - x_ {\text{goal}}) \] 其中 \(x_ {\text{goal}} = [ 1, 0]^T\) 是目标状态，\(Q = I\)（单位矩阵），\(R = 0.1\)，\(Q_ f = 10I\)，总步数 \(N=50\)。 初始状态：\(x_ 0 = [ 0, 0 ]^T\)。 问题 ：在控制饱和约束下，寻找最优控制序列 \(\{u_ 0, u_ 1, ..., u_ {N-1}\}\) 和状态轨迹 \(\{x_ 0, x_ 1, ..., x_ N\}\)，最小化总代价 \(J\)。 解题过程循序渐进讲解 差分动态规划（DDP）是一种基于动态规划和二阶近化的高效轨迹优化算法，特别适合处理非线性动力学和约束问题。下面分步骤详细讲解。 步骤1：理解DDP的核心思想 DDP本质上是一种迭代优化算法，属于动态规划的一种近似实现。其核心思想是： 反向传播 ：从轨迹末端开始，向后迭代计算局部二次模型（代价函数关于状态和控制的二阶展开），并求解局部线性二次调节器（LQR）问题，得到最优控制更新律。 前向滚动 ：使用反向传播得到的控制策略，从初始状态开始前向仿真，生成一条新的轨迹。 迭代改进 ：重复反向传播和前向滚动，直到轨迹收敛到满足最优性条件。 对于控制饱和约束，DDP通常通过投影或裁剪方式处理。 步骤2：算法初始化 给定一条初始猜测轨迹（通常从初始控制序列全为零或简单猜测开始）。 初始控制序列：\(u_ t^{(0)} = 0\)，\(t=0,...,N-1\)。 通过前向仿真得到初始状态轨迹：\(x_ 0\) 已知，\(x_ {t+1}^{(0)} = f(x_ t^{(0)}, u_ t^{(0)})\)。 设置算法参数： 正则化参数 \(\mu\)（初始为0，用于保证 Hessian 正定）。 步长衰减因子 \(\alpha \in (0,1)\)（用于线搜索）。 收敛容差 \(\epsilon\)（例如 \(10^{-6}\)）。 步骤3：反向传播（Backward Pass） 这是DDP的核心步骤。从最后一步 \(N\) 开始，倒序计算到第一步 \(0\)。 对于每个时间步 \(t = N, N-1, ..., 0\)： 定义价值函数 ：\(V_ t(x)\) 表示从时刻 \(t\)、状态 \(x\) 出发到终点的最小累积代价。 终端条件：\(V_ N(x) = \frac{1}{2} (x - x_ {\text{goal}})^T Q_ f (x - x_ {\text{goal}})\)。 局部二次近似 ： 在当前的轨迹点 \((x_ t, u_ t)\) 附近，对 Q函数 （即从当前步开始的即时代价加上下一步的价值函数）进行二阶泰勒展开： \[ Q(\delta x, \delta u) = \text{常数} + \begin{bmatrix} Q_ x \\ Q_ u \end{bmatrix}^T \begin{bmatrix} \delta x \\ \delta u \end{bmatrix} + \frac{1}{2} \begin{bmatrix} \delta x \\ \delta u \end{bmatrix}^T \begin{bmatrix} Q_ {xx} & Q_ {xu} \\ Q_ {ux} & Q_ {uu} \end{bmatrix} \begin{bmatrix} \delta x \\ \delta u \end{bmatrix} \] 其中： \(\delta x = x - x_ t\)，\(\delta u = u - u_ t\)。 梯度 \(Q_ x, Q_ u\) 和 Hessian \(Q_ {xx}, Q_ {xu}, Q_ {ux}, Q_ {uu}\) 通过链式法则计算，涉及动力学 \(f\) 和价值函数 \(V_ {t+1}\) 的导数。 具体计算公式： \[ \begin{aligned} Q_ x &= l_ x + f_ x^T V' {x}, \\ Q_ u &= l_ u + f_ u^T V' {x}, \\ Q_ {xx} &= l_ {xx} + f_ x^T V' {xx} f_ x + V' x \cdot f {xx}, \\ Q {uu} &= l_ {uu} + f_ u^T V' {xx} f_ u + V' x \cdot f {uu}, \\ Q {xu} &= l_ {xu} + f_ x^T V' {xx} f_ u + V' x \cdot f {xu}. \end{aligned} \] 这里 \(l(x_ t, u_ t)\) 是单步代价，\(V' {x}, V' {xx}\) 是下一步价值函数 \(V {t+1}\) 的梯度和 Hessian（在反向传播中已计算）。 求解无约束最优控制更新 ： 最小化 \(Q(\delta x, \delta u)\) 得到最优控制增量： \[ \delta u^* = -Q_ {uu}^{-1} (Q_ u + Q_ {ux} \delta x). \] 注意：\(Q_ {uu}\) 必须正定。若不满足，可加入正则化项 \(\mu I\) 使其正定。 处理控制饱和约束 ： 由于控制输入有界 \(u_ {\min} \leq u \leq u_ {\max}\)，在计算最优控制更新后，需要进行裁剪： \[ u_ {\text{new}} = \text{clip}(u_ t + \delta u^* , u_ {\min}, u_ {\max}) \] 其中 \(\text{clip}(u, a, b) = \max(a, \min(u, b))\)。 更新价值函数 ： 将最优控制律代入，得到价值函数的参数更新： \[ \begin{aligned} k_ t &= -Q_ {uu}^{-1} Q_ u, \\ K_ t &= -Q_ {uu}^{-1} Q_ {ux}, \\ \Delta V &= -\frac{1}{2} k_ t^T Q_ {uu} k_ t, \\ V_ x &= Q_ x - K_ t^T Q_ {uu} k_ t, \\ V_ {xx} &= Q_ {xx} - K_ t^T Q_ {uu} K_ t. \end{aligned} \] 这里 \(k_ t\) 是开环项，\(K_ t\) 是反馈增益矩阵。控制律为 \(u = u_ t + k_ t + K_ t \delta x\)。 步骤4：前向滚动（Forward Pass） 使用反向传播得到的控制策略，从初始状态 \(x_ 0\) 开始，前向仿真生成新轨迹： 对于 \(t = 0, 1, ..., N-1\)： 计算控制：\(u_ t^{\text{new}} = u_ t^{(k)} + k_ t + K_ t (x_ t^{\text{new}} - x_ t^{(k)})\)。 应用饱和约束：\(u_ t^{\text{new}} = \text{clip}(u_ t^{\text{new}}, u_ {\min}, u_ {\max})\)。 模拟下一步状态：\(x_ {t+1}^{\text{new}} = f(x_ t^{\text{new}}, u_ t^{\text{new}})\)。 计算新轨迹的总代价 \(J_ {\text{new}}\)。 步骤5：线搜索与接受准则 由于泰勒展开是局部近似，直接采用新轨迹可能不保证代价下降。因此需要线搜索： 比较新代价 \(J_ {\text{new}}\) 与旧代价 \(J_ {\text{old}}\)。 如果 \(J_ {\text{new}} < J_ {\text{old}}\)，接受新轨迹，并减小正则化参数 \(\mu \leftarrow \mu / 2\)。 如果 \(J_ {\text{new}} \geq J_ {\text{old}}\)，拒绝新轨迹，增加正则化参数 \(\mu \leftarrow \max(2\mu, \mu_ {\min})\)，并缩小步长重新前向滚动（即控制更新量乘以衰减因子 \(\alpha\) 后重做前向仿真）。 步骤6：收敛判断 重复步骤3-5，直到满足以下收敛条件之一： 代价下降量 \(|J_ {\text{new}} - J_ {\text{old}}| / |J_ {\text{old}}| < \epsilon\)。 控制更新量的范数 \(\|k_ t\|\) 小于阈值。 达到最大迭代次数。 算法特点与小结 DDP优势 ：利用动态规划的递归结构，通过二阶展开捕获非线性，通常比一阶方法收敛更快，且能自然处理状态相关约束（通过投影或罚函数）。 控制饱和处理 ：本例通过简单的裁剪（投影）处理控制约束。更复杂的方法可结合屏障函数或增广拉格朗日法。 计算效率 ：反向传播中需计算和存储每个时间步的增益矩阵，内存和计算量随状态和控制维度平方增长，但对于中小规模问题非常高效。 通过以上步骤，DDP 能有效求解带控制饱和约束的轨迹优化问题，找到一条从起点到目标点的能量最优轨迹。