自动驾驶：LQR、ILQR和DDP原理、公式推导以及代码演示（三、DDP篇）

机械心

已于 2024-09-11 23:57:02 修改

阅读量1.1k

点赞数 29

分类专栏：最优化理论文章标签：自动驾驶人工智能机器学习

于 2024-09-08 08:27:59 首次发布

本文链接：https://blog.csdn.net/a8598671/article/details/141993409

版权

最优化理论专栏收录该内容

9 篇文章 1 订阅

订阅专栏

（三）DDP原理、公式推导以及代码演示

1. DDP 原理

DDP（Differential Dynamic Programming, DDP） 是一种用于求解非线性最优控制问题的强大方法。DDP是iLQR的扩展，iLQR使用的是一阶线性近似非线性系统，而DDP则使用了二阶展开来处理系统的非线性部分。DDP 的一个关键优势是它能够处理高度非线性和大规模系统的控制问题，相对于 LQR（线性二次调节器）和 iLQR（迭代线性二次调节器），它更适用于具有强非线性的复杂动力学系统。

iLQR 只需要计算梯度（即一阶导数），所以每次迭代的计算负担较轻，但优化方向的精度可能不足。DDP 则需要计算梯度和 Hessian（即二阶导数），使得每次迭代的计算量增加，但优化方向会更准确，尤其是当系统非线性很强时。在收敛性方面，iLQR 的收敛性依赖于问题的非线性程度和初始解的质量。如果系统的非线性较强，或者初始轨迹离最优解较远，iLQR 可能需要更多迭代才能找到近似最优解，甚至可能陷入局部最优。而DDP 由于考虑了二阶信息，能够更精确地描述系统的动态行为，通常能更快收敛到最优解，尤其是在系统具有显著非线性时。在稳定性与鲁棒性方面，iLQR 在系统非线性不强或控制量较小时通常能保持较好的稳定性，但在系统非线性较强时，忽略二阶效应可能会导致控制策略不够鲁棒。DDP 因为捕捉了二阶导数，能够对系统的非线性特性做出更好的响应，因此具有更好的鲁棒性和稳定性。

但是，Hessian 矩阵通常是关于状态和控制的二阶导数，因此对于大规模系统，存储和计算这些矩阵的成本会随着系统维度呈二次增长。具体差异如下：

iLQR 计算复杂度：每次迭代的复杂度主要与线性化的求导过程相关，其复杂度通常是 $O(n^3)$ （n 为状态和控制的总维度）。

全DDP 计算复杂度：每次迭代不仅需要计算梯度，还需要计算 Hessian，复杂度增加至 $O(n^4)$ 。

2. 应用场景

iLQR ：

系统非线性较弱、规模较大。
实时控制需求较高的场景，例如机器人、自动驾驶。
计算资源有限的情况，iLQR 可以快速收敛并生成较优解。

DDP：

高度非线性系统，例如航空航天、复杂机械系统控制。
需要高精度控制策略的任务，尤其是长期规划时精度要求较高。
对计算时间要求不那么严格，或可以并行计算的场景。

3. DDP问题定义

DDP的问题定义和ILQR是一致的，都是建立离散时间系统的状态方程和代价函数，目标是找到控制序列 $\{u_0, u_1, \dots, u_{N-1}\}$ 最小化成本函数。

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

$\min_{\{u_0, u_1, \dots, u_{N-1}\}} \sum_{k=0}^{N-1} l(x_k, u_k) + l_f(x_N)$

4. 公式推导

公式推导与ILQR推导方式一致，区别是系统方程不再是线性方程，而是二阶泰勒展开。为了和ILQR对比，保持二者目录一致，但只展示二者不同的部分。

4.1. 二阶近似

这里不再是一阶近似，而是对 $x_{k+1} = f(x_k, u_k)$ 进行二阶泰勒展开：

$f(\mathbf{x}_t + \delta \mathbf{x}, \mathbf{u}_t + \delta \mathbf{u}) \approx f(\mathbf{x}_t, \mathbf{u}_t) + f_{\mathbf{x}} \delta \mathbf{x} + f_{\mathbf{u}} \delta \mathbf{u} + \frac{1}{2} \delta \mathbf{x}^T f_{\mathbf{xx}} \delta \mathbf{x} + \frac{1}{2} \delta \mathbf{u}^T f_{\mathbf{uu}} \delta \mathbf{u} + \delta \mathbf{x}^T f_{\mathbf{xu}} \delta \mathbf{u}$

其中：

$f_{\mathbf{x}} = \frac{\partial f(\mathbf{x}_t, \mathbf{u}_t)}{\partial \mathbf{x}_t}$ 是动力学方程对状态的线性化矩阵， $f_{\mathbf{x}} \in \mathbb{R}^{n \times n}$
$f_{\mathbf{u}} = \frac{\partial f(\mathbf{x}_t, \mathbf{u}_t)}{\partial \mathbf{u}_t}$ 是动力学方程对控制的线性化矩阵， $f_{\mathbf{u}} \in \mathbb{R}^{n \times m}$
$f_{\mathbf{xx}} = \frac{\partial^2 f(\mathbf{x}_t, \mathbf{u}_t)}{\partial \mathbf{x}_t^2}$ 是动力学方程对状态的二阶导数（Hessian）， $f_{\mathbf{xx}} \in \mathbb{R}^{n \times n \times n}$
$f_{\mathbf{uu}} = \frac{\partial^2 f(\mathbf{x}_t, \mathbf{u}_t)}{\partial \mathbf{u}_t^2}$ 是动力学方程对控制的二阶导数， $f_{\mathbf{uu}} \in \mathbb{R}^{n \times m \times m}$
$f_{\mathbf{xu}} = \frac{\partial^2 f(\mathbf{x}_t, \mathbf{u}_t)}{\partial \mathbf{x}_t \partial \mathbf{u}_t}$ 是动力学方程对状态和控制的交叉二阶导数， $f_{\mathbf{xu}} \in \mathbb{R}^{n \times n \times m}$

4.2. Q函数定义与展开

Q函数的定义为：

$Q_t(\mathbf{x}_t, \mathbf{u}_t) = \ell(\mathbf{x}_t, \mathbf{u}_t) + V_{t+1}(\mathbf{x}_{t+1})$

将 $\mathbf{x}_{k+1} = \mathbf{f}(\mathbf{x}_k, \mathbf{u}_k)$ 代入到Q函数：

$Q_t(\mathbf{x}_t, \mathbf{u}_t) = \ell(\mathbf{x}_t, \mathbf{u}_t) + V_{t+1}(f(\mathbf{x}_t, \mathbf{u}_t))$

4.2.1. 对状态 $\mathbf{x}_t$ 的一阶导数：

$Q_{\mathbf{x}} = \frac{\partial Q_t(\mathbf{x}_t, \mathbf{u}_t)}{\partial \mathbf{x}_t} = \ell_{\mathbf{x}} + \frac{\partial V_{t+1}(f(\mathbf{x}_t, \mathbf{u}_t))}{\partial \mathbf{x}_t}$

通过链式法则对 $V_{t+1}(f(\mathbf{x}_t, \mathbf{u}_t))$ 进行求导：

$Q_{\mathbf{x}} = \ell_{\mathbf{x}} + f_{\mathbf{x}}^T V_{\mathbf{x}, t+1}$

4.2.2. 对控制输入 $\mathbf{u}_t$ 的一阶导数：

$Q_{\mathbf{u}} = \frac{\partial Q_t(\mathbf{x}_t, \mathbf{u}_t)}{\partial \mathbf{u}_t} = \ell_{\mathbf{u}} + \frac{\partial V_{t+1}(f(\mathbf{x}_t, \mathbf{u}_t))}{\partial \mathbf{u}_t}$

$Q_{\mathbf{u}} = \ell_{\mathbf{u}} + f_{\mathbf{u}}^T V_{\mathbf{x}, t+1}$

4.2.3. 对状态的二阶导数：

$Q_{\mathbf{xx}} = \frac{\partial^2 Q_t(\mathbf{x}_t, \mathbf{u}_t)}{\partial \mathbf{x}_t^2} = \ell_{\mathbf{xx}} + \frac{\partial}{\partial \mathbf{x}_t} \left(f_{\mathbf{x}}^T V_{\mathbf{x}, t+1}\right)$

进一步展开为：

$Q_{\mathbf{xx}} = \ell_{\mathbf{xx}} + f_{\mathbf{x}}^T V_{\mathbf{xx}, t+1} f_{\mathbf{x}} + V_{\mathbf{x}, t+1}^T f_{\mathbf{xx}}$

4.2.4. 对控制的二阶导数：

$Q_{\mathbf{uu}} = \ell_{\mathbf{uu}} + f_{\mathbf{u}}^T V_{\mathbf{xx}, t+1} f_{\mathbf{u}} + V_{\mathbf{x}, t+1}^T f_{\mathbf{uu}}$

4.2.5. 对状态和控制的混合二阶导数：

$Q_{\mathbf{xu}} = \ell_{\mathbf{xu}} + f_{\mathbf{x}}^T V_{\mathbf{xx}, t+1} f_{\mathbf{u}} + V_{\mathbf{x}, t+1}^T f_{\mathbf{xu}}$

5. 值函数递推公式

5.1. 最优控制偏差量

使 Q 函数关于控制 $\mathbf{u}_t$ 的梯度为零，即：

$Q_{\mathbf{u}} + Q_{\mathbf{uu}} \delta \mathbf{u}^* + Q_{\mathbf{xu}} \delta \mathbf{x} = 0$

解得：

$\delta \mathbf{u}_t^* = - Q_{\mathbf{uu}}^{-1} \left( Q_{\mathbf{u}} + Q_{\mathbf{xu}} \delta \mathbf{x}_t \right)$

5.2. 值函数的更新

与ILQR值函数更新一致，值函数的梯度更新：

$V_{\mathbf{x}, t} = Q_{\mathbf{x}} - Q_{\mathbf{xu}} Q_{\mathbf{uu}}^{-1} Q_{\mathbf{u}}$

值函数的 Hessian 更新：

$V_{\mathbf{xx}, t} = Q_{\mathbf{xx}} - Q_{\mathbf{xu}} Q_{\mathbf{uu}}^{-1} Q_{\mathbf{xu}}^T$

6. 注意事项

6.1. 数值稳定性

Hessian 矩阵 $Q_{\mathbf{uu}}$ 的正定性：在每个时间步 ttt，控制的二阶导数矩阵 $Q_{\mathbf{uu}}$ 必须是正定的，以确保最优控制输入增量 $\delta \mathbf{u}_t^*$ 的可行性。若 $Q_{\mathbf{uu}}$ 不是正定矩阵，逆矩阵 $Q_{\mathbf{uu}}^{-1}$ 将无法计算，算法将会失效。通常会在此时加入正则化方法来修正 $Q_{\mathbf{uu}}$ 的正定性。
数值正则化：为了避免 Hessian 失去正定性，DDP 通常会在 $Q_{\mathbf{uu}}$ 上加上一个小的正定项来保证矩阵的可逆性。常用的策略是：
$Q_{\mathbf{uu}} \rightarrow Q_{\mathbf{uu}} + \lambda I$
其中 $\lambda$ 是正则化系数， $I$ 是单位矩阵。当 $\lambda$ 较小时不会影响最优解，但能提升数值稳定性。
梯度计算的精度：由于 DDP 依赖状态方程和代价函数的梯度和 Hessian，若模型或代价函数的导数不精确，可能会导致不准确的优化结果。在求解导数时，最好使用自动微分工具或符号微分方法。

6.2. 初始条件和参考轨迹

初始控制输入轨迹选择：DDP是一个局部优化算法，依赖于初始的控制输入轨迹。如果初始控制输入离最优解较远，算法可能会收敛到局部最优解，而不是全局最优。因此，选择一个好的初始轨迹可以帮助算法更快地收敛并提高最终的控制性能。
参考轨迹的引入：当使用 DDP 来优化非线性系统时，如果能引入一个近似的参考轨迹，算法将会更快收敛。参考轨迹通常通过 LQR 或类似的线性化方法得到。

6.3. 收敛性和迭代策略

信赖域调整：在前向传播时，如果新状态和控制轨迹导致代价函数增加，可能需要引入信赖域策略，限制每次迭代的控制输入变化量，以确保每一步的更新不会偏离最优解。通常的方法是缩小步长或调整正则化参数 $\lambda$ 。
步长调节：在前向传播中，如果代价函数减小不明显，可能需要调整步长 α\alphaα 以控制每一步的更新幅度。典型的步长搜索是指数递减的 $\alpha = 1, 0.5, 0.25, \dots$ 直到找到合适的步长。
收敛判定：可以通过以下方式来判断收敛：
- 控制输入的增量 $\delta \mathbf{u}_t^*$ 足够小。
- 总代价 $J(\mathbf{x}_0, \mathbf{u}_{0:N-1})$ 在连续迭代中变化很小。
- 状态偏差 $\delta \mathbf{x}_t$ 的减小程度也可以作为收敛标准。

6.4. 系统非线性处理

非线性程度的影响：DDP 依赖于状态方程 $f(\mathbf{x}_t, \mathbf{u}_t)$ 的局部线性化和二阶近似。因此，系统的非线性程度会影响 DDP 的表现。对于高度非线性系统，DDP 可能会产生较大的偏差，无法有效收敛。
线性化区间的大小：当系统的非线性较强时，算法中的线性化区间（或时间步）可以调整得更小，以减轻二阶近似带来的误差。通过这种方式，DDP 可以在局部近似内获得较好的收敛结果。

6.5. 代价函数设计

终端代价的选择：DDP 通过递归方式传播终端状态的代价。因此，终端代价函数 $\ell_N(\mathbf{x}_N)$ 的设计对算法的收敛性和性能有直接影响。如果终端代价不足以引导系统向目标状态收敛，算法可能无法找到理想的解。
运行代价函数的设计：运行代价函数 $\ell(\mathbf{x}_t, \mathbf{u}_t)$ 应当是连续的、可导的，并且与系统的物理特性相匹配。代价函数的形式对优化结果有重要影响。

6.6. 维度问题

高维状态和控制输入：对于高维系统（即状态向量和控制输入的维度较大），DDP 的 Hessian 计算和矩阵求逆可能变得昂贵。针对这种情况，可以考虑引入稀疏矩阵方法或其他降维技巧，减少计算量。
维度对存储和计算量的影响：由于 DDP 中会涉及大量的矩阵（如 $Q_{\mathbf{xx}}$ ， $Q_{\mathbf{uu}}$ ， $Q_{\mathbf{xu}}$ 等），当状态和控制维度增加时，矩阵的存储和计算开销也会迅速增加。因此，对于大规模问题，需考虑矩阵的稀疏性或采用近似的计算方法。

6.7. 处理约束

控制输入约束：DDP 本身不直接处理约束。如果存在控制输入约束（如边界约束或输入饱和），则需要将这些约束通过其他手段处理。常用的方法包括：
- 投影法：每次更新控制输入后，将其投影到允许的输入域内。
- 惩罚函数法：在代价函数中增加额外的约束惩罚项。
状态约束：状态约束也可以通过惩罚函数或约束投影来实现。

6.8. 实时性

实时控制应用：DDP 由于是一个迭代算法，通常需要多次迭代来更新控制输入。如果应用场景要求实时性（如控制系统实时运行），需要加快 DDP 的迭代过程或者减少每次迭代的计算量。可以考虑使用并行计算、向量化操作或近似求解方法。

6.9. 算法的收敛性与局部最优

局部最优解：DDP 是基于梯度的局部优化方法，因此容易陷入局部最优解。对复杂的非凸问题，可能无法找到全局最优解。在这些情况下，结合其他全局优化方法（如随机搜索或遗传算法）可以帮助寻找更好的初始解。
收敛速度与条件：收敛速度依赖于初始控制策略、正则化参数、系统的非线性程度等因素。合适的初始条件和合理的正则化处理将有助于加速收敛。

6.10. 算法实现与调试

代码调试和可视化：为了调试和改进 DDP 算法，推荐可视化系统的状态、控制输入、代价函数变化等指标，这可以帮助发现不收敛的原因或控制律更新中的错误。
数值求解：在计算梯度、Hessian 或状态转移时，推荐使用自动微分工具或符号微分方法，以避免数值误差积累。

7. python代码示例

与上一章节相同，我们用一个简单的非线性系统，其状态 $x_k$ 和控制输入 $u_k$ 满足以下非线性动力学方程： $x_{k+1} = x_k + \sin(u_k)$ ，这里的非线性在于控制输入的影响是通过正弦函数进入系统的。我们希望通过控制输入 $u_k$ 使状态 $x_k$ 快速收敛到目标值 0，同时控制量尽可能小，因此二次成本函数：

$J = \sum_{k=0}^{N-1} \left( \frac{1}{2} x_k^2 + \frac{1}{2} u_k^2 \right)$ 。

python代码如下：

import numpy as np
import pandas as pd

# System dynamics (nonlinear)
def system_dynamics(x, u):
    return x + np.sin(u)

# Cost function for a single step
def cost_function(x, u):
    return 0.5 * (x**2 + u**2)

# Derivatives of the cost function
def cost_x(x):
    return x

def cost_u(u):
    return u

def cost_xx():
    return 1

def cost_uu():
    return 1

def cost_xu():
    return 0

# System dynamics derivatives
def dynamics_x():
    return 1

def dynamics_u(u):
    return np.cos(u)

def dynamics_uu(u):
    return -np.sin(u)

def dynamics_xu():
    return 0

def dynamics_xx():
    return 0

# Compute initial trajectory based on the initial control input (all zeros in this case)
def compute_initial_trajectory(x0, u):
    x = np.zeros(N+1)
    x[0] = x0
    for k in range(N):
        x[k+1] = system_dynamics(x[k], u[k])
    return x

# DDP algorithm with initial trajectory
def ddp(x, u, iterations, epsilon):
    for i in range(iterations):
        # Backward pass
        V_x = np.zeros(N+1)
        V_xx = np.zeros(N+1)
        V_x[-1] = x[-1]  # Terminal value for V_x
        V_xx[-1] = 1  # Terminal value for V_xx (quadratic cost on terminal state)

        K = np.zeros(N)  # Feedback gains
        d = np.zeros(N)  # Feedforward gains

        # Backward pass: compute Q-function and update control strategy
        for k in range(N-1, -1, -1):
            f_x = dynamics_x()
            f_u = dynamics_u(u[k])
            f_uu = dynamics_uu(u[k])
            f_xu = dynamics_xu()
            f_xx = dynamics_xx()

            # Q-function terms
            Q_x = cost_x(x[k]) + f_x * V_x[k+1]
            Q_u = cost_u(u[k]) + f_u * V_x[k+1]
            Q_xx = cost_xx() + f_x**2 * V_xx[k+1] + f_xx * V_x[k+1]
            Q_uu = cost_uu() + f_u**2 * V_xx[k+1] + f_uu * V_x[k+1]
            Q_xu = cost_xu() + f_x * V_xx[k+1] * f_u + f_xu * V_x[k+1]

            # Compute control update
            K[k] = - Q_uu**(-1) * Q_xu
            d[k] = - Q_uu**(-1) * Q_u

            # Update value function
            V_x[k] = Q_x + Q_xu * d[k]
            V_xx[k] = Q_xx + Q_xu * K[k]

        # Forward pass: update trajectory
        x_new = np.zeros(N+1)
        u_new = np.zeros(N)
        x_new[0] = x0

        for k in range(N):
            u_new[k] = u[k] + d[k] + K[k] * (x_new[k] - x[k])
            x_new[k+1] = system_dynamics(x_new[k], u_new[k])

        # Check for convergence
        if np.max(np.abs(d)) < epsilon:
            print(f"Converged at iteration {i}")
            break

        x = x_new
        u = u_new

    return x, u, i

if __name__ == "__main__":


    # DDP parameters
    N = 3  # Number of time steps
    x0 = 1  # Initial state
    iterations = 10  # Maximum number of iterations
    epsilon = 1e-3  # Tolerance for control input changes

    # Initialize control sequence and state trajectory
    u = np.zeros(N)  # Initial control sequence
    x = np.zeros(N+1)  # State trajectory


    # Compute initial trajectory
    x_initial = compute_initial_trajectory(x0, u)

    # Run DDP with initial trajectory
    x_final, u_final, num_iterations = ddp(x_initial, u, iterations, epsilon)


    print(x_final, u_final, num_iterations)

执行结果如下：

从结果上看，DDP收敛的速度明显快于ILQR，但每次迭代的计算量也较大。