最优化理论与自动驾驶（四）：iLQR原理、公式及代码演示

我们考虑一个离散时间的动态系统，其状态方程为：其中，是系统在时间步k的状态，是控制输入，描述系统的动态方程。我们的目标是找到控制序列其中，是阶段成本函数，是终端成本。由于LQR算法要求系统的状态方程为线性状态方程，成本函数为二次型，因此iLQR是通过牛顿高斯方法进行迭代逼近最优解。为了方便优化和递推，我们引入值函数，它表示从时刻k开始，给定当前状态，剩余时间内的最小代价。其定义如下：值函数依赖于当

机械心

4963人浏览 · 2024-09-07 11:13:20

机械心 · 2024-09-07 11:13:20 发布

（二）iLQR原理、公式推导以及代码演示

1. iLQR 原理

iLQR（迭代线性二次调节器）是一种用于求解非线性系统最优控制问题的算法。我们在第一部分已经介绍了LQR相关的内容，iLQR其实就是LQR的迭代版本。它在每个时间步通过一阶泰勒展开线性化系统状态方程，并对成本函数进行二阶近似，然后使用线性二次调节器（LQR）求解该近似问题。通过迭代调整策略，逐步逼近最优策略。iLQR属于微分动态规划（DDP）类算法的简化版本，特别适用于解决状态空间和控制输入较大的问题。

2. 应用场景

iLQR 应用于解决许多复杂的非线性系统优化控制问题，尤其是在机器人控制、轨迹规划、自主驾驶、航天器姿态控制等领域。它通常用于以下场景：

轨迹跟踪：机器人、无人机、车辆的轨迹规划与控制。
姿态控制：如航天器的姿态调整，精确控制其方向与姿态。
动态系统优化：如机械臂在复杂任务中的路径优化。

3. iLQR 问题定义

我们考虑一个离散时间的动态系统，其状态方程为：

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

其中， $x_k \in \mathbb{R}^n$ 是系统在时间步k的状态， $u_k \in \mathbb{R}^m$ 是控制输入， $f$ 描述系统的动态方程。我们的目标是找到控制序列 $\{u_0, u_1, \dots, u_{N-1}\}$ 最小化以下成本函数：

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

其中， $l(\mathbf{x}_k, \mathbf{u}_k)$ 是阶段成本函数， $l_f(\mathbf{x}_N)$ 是终端成本。由于LQR算法要求系统的状态方程为线性状态方程，成本函数为二次型，因此iLQR是通过牛顿高斯方法进行迭代逼近最优解。

4. iLQR 公式推导

1. 值函数的定义

为了方便优化和递推，我们引入值函数 $V_k(x_k)$ ，它表示从时刻k开始，给定当前状态 $x_k$ ，剩余时间内的最小代价。其定义如下：

$V_k(x_k) = \min_{\{u_k, \dots, u_{N-1}\}} \sum_{t=k}^{N-1} l(x_t, u_t) + l_f(x_N)$

值函数 $V_k(x_k)$ 依赖于当前状态 $x_k$ ，并反映了从当前状态到终端状态所需支付的最小累积代价。终端时刻的值函数为：

$V_N(x_N) = l_f(x_N)$

2. 值函数的贝尔曼递推方程

在动态规划（Dynamic Programming）中，值函数满足贝尔曼方程。贝尔曼方程是通过递推的方式，将问题从一个长时间的优化问题简化为一步步的局部优化问题。对于k时刻的值函数，我们有：

$V_k(x_k) = \min_{u_k} \left[ l(x_k, u_k) + V_{k+1}(x_{k+1}) \right]$

其中， $x_{k+1} = f(x_k, u_k)$ 。也就是说，值函数 $V_k(x_k)$ 是当前阶段成本 $l(x_k, u_k)$ 和未来值函数 $V_{k+1}(x_{k+1})$ 之和的最小值。

3. 一阶近似与线性化

在iLQR算法中，核心思想是将非线性的动力学方程 $f(x_k, u_k)$ 和成本函数 $l(x_k, u_k)$ 线性化和二次近似化。首先对 $x_{k+1} = f(x_k, u_k)$ 进行一阶泰勒展开，得到其线性化近似：

$x_{k+1} \approx f(x_k^*, u_k^*) + f_x \delta x_k + f_u \delta u_k$

这里， $\delta x_k = x_k - x_k^*$ 和 $\delta u_k = u_k - u_k^*$ 分别是状态和控制输入的偏差， $f_x = \frac{\partial f}{\partial x}$ 和 $f_u = \frac{\partial f}{\partial u}$ 是系统动力学方程的雅可比矩阵，分别表示状态和控制输入对下一时刻状态的影响。

同时，代价函数 $l(x_k, u_k)$ 也进行二阶泰勒展开：

$l(x_k, u_k) \approx l(x_k^*, u_k^*) + l_x \delta x_k + l_u \delta u_k + \frac{1}{2} \delta x_k^T l_{xx} \delta x_k + \frac{1}{2} \delta u_k^T l_{uu} \delta u_k + \delta u_k^T l_{xu} \delta x_k$

其中， $l_x = \frac{\partial l}{\partial x}$ ， $l_u = \frac{\partial l}{\partial u}$ ， $l_{xx} = \frac{\partial^2 l}{\partial x^2}$ ， $l_{uu} = \frac{\partial^2 l}{\partial u^2}$ ， $l_{xu} = \frac{\partial^2 l}{\partial x \partial u}$ 是代价函数的梯度和海森矩阵。

4. Q函数定义与展开

为了简化推导，引入 Q 函数 $Q_k(x_k, u_k)$ ，它表示从时刻k开始，给定当前状态 $x_k$ 和控制输入 $u_k$ 时，立即执行该控制并在未来按照最优策略执行时的总成本。Q 函数定义为：

$Q_k(x_k, u_k) = l(x_k, u_k) + V_{k+1}(x_{k+1})$

我们将 $Q_k(x_k, u_k)$ 对 $x_k$ 和 $u_k$ 进行二阶泰勒展开：

$Q_k(x_k, u_k) \approx Q_k(x_k^*, u_k^*) + Q_k^{(x)} \delta x_k + Q_k^{(u)} \delta u_k + \frac{1}{2} \delta x_k^T Q_k^{(xx)} \delta x_k + \frac{1}{2} \delta u_k^T Q_k^{(uu)} \delta u_k + \delta x_k^T Q_k^{(xu)} \delta u_k$ (式子1)

其中:

$Q_k^{(x)} = \frac{\partial Q_k}{\partial x}$

$Q_k^{(u)} = \frac{\partial Q_k}{\partial u}$

$Q_k^{(xx)} = \frac{\partial^2 Q_k}{\partial x^2}$

$Q_k^{(uu)} = \frac{\partial^2 Q_k}{\partial u^2}$

$Q_k^{(xu)} = \frac{\partial^2 Q_k}{\partial x \partial u}$

是Q函数的梯度和海森矩阵。

由于 $Q_k(x_k, u_k)$ 是由瞬时代价和未来的值函数组成的，因此我们可以分别对 $l(x_k, u_k)$ 和 $V_{k+1}(x_{k+1})$ 的梯度和海森矩阵进行计算。具体来说：

$Q_k^{(x)} = l_x + f_x^T V_{k+1}^{(x)}$

$Q_k^{(u)} = l_u + f_u^T V_{k+1}^{(x)}$

$Q_k^{(xx)} = l_{xx} + f_x^T V_{k+1}^{(xx)} f_x$

$Q_k^{(uu)} = l_{uu} + f_u^T V_{k+1}^{(xx)} f_u$

$Q_k^{(xu)} = l_{xu} + f_x^T V_{k+1}^{(xx)} f_u$

5. 值函数递推公式

现在我们要根据 Q 函数来递推值函数 $V_k(x_k)$ 。根据贝尔曼方程，值函数是对控制输入 $u_k$ 优化后得到的最小代价，因此：

$V_k(x_k) = \min_{u_k} Q_k(x_k, u_k)$

由于 $Q_k(x_k, u_k)$ 是二次形式的，这里不是假设，而是由于我们对系统的动力学和代价函数都进行了线性化和二次近似的必然结果。最优控制输入 $u_k^*$ 可以通过对（式子1）关于 $u_k$ 求导得到：

$\frac{\partial Q_k}{\partial u_k} = Q_k^{(u)} + Q_k^{(uu)} \delta u_k + Q_k^{(xu)^T} \delta x_k = 0$

解得最优控制偏差：

$\delta u_k^* = - Q_k^{(uu)^{-1}} \left( Q_k^{(u)} + Q_k^{(xu)} \delta x_k \right)$
将这个解代入 $Q_k(x_k, u_k)$ ，得到值函数 $V_k(x_k)$ 的递推公式。首先我们展开 $Q_k(x_k, u_k)$ ：

$V_k(x_k) = Q_k(x_k, u_k^*) = Q_k(x_k^*, u_k^*) + Q_k^{(x)} \delta x_k + Q_k^{(u)} \delta u_k^* + \frac{1}{2} \delta x_k^T Q_k^{(xx)} \delta x_k + \frac{1}{2} \delta u_k^{*T} Q_k^{(uu)} \delta u_k^* + \delta x_k^T Q_k^{(xu)} \delta u_k^*$

现在将最优控制输入的表达式 $\delta u_k^* = - Q_k^{(uu)^{-1}} \left( Q_k^{(u)} + Q_k^{(xu)} \delta x_k \right)$ 代入上式中：

$V_k(x_k) = Q_k(x_k^*, u_k^*) + Q_k^{(x)} \delta x_k + \frac{1}{2} \delta x_k^T Q_k^{(xx)} \delta x_k - \frac{1}{2} \left( Q_k^{(u)} + Q_k^{(xu)} \delta x_k \right)^T Q_k^{(uu)^{-1}} \left( Q_k^{(u)} + Q_k^{(xu)} \delta x_k \right)$

5.1 值函数的梯度递推

为了得到 $V_k(x_k)$ 的梯度 $V_k^{(x)}$ ，我们只需要对上式中所有与 $\delta x_k$ 线性相关的项进行提取。注意到：

$V_k^{(x)} = Q_k^{(x)} - Q_k^{(xu)} Q_k^{(uu)^{-1}} Q_k^{(u)}$

这个表达式说明当前时刻的值函数梯度 $V_k^{(x)}$ 是基于 Q 函数的状态梯度 $Q_k^{(x)}$ 和控制输入对状态的混合项 $Q_k^{(xu)}$ 以及控制输入对输入的海森矩阵 $Q_k^{(uu)}$ 来递推的。

5.2 值函数的海森矩阵递推

接下来我们推导值函数 $V_k(x_k)$ 的海森矩阵 $V_k^{(xx)}$ 。注意到 $\delta x_k$ 的二次项为：

$\frac{1}{2} \delta x_k^T Q_k^{(xx)} \delta x_k - \frac{1}{2} \delta x_k^T Q_k^{(xu)^T} Q_k^{(uu)^{-1}} Q_k^{(xu)} \delta x_k$

因此，值函数的海森矩阵递推公式为：

$V_k^{(xx)} = Q_k^{(xx)} - Q_k^{(xu)} Q_k^{(uu)^{-1}} Q_k^{(xu)^T}$

这个公式表示当前时刻值函数的海森矩阵 $V_k^{(xx)}$ 是基于 Q 函数的状态二阶导数 $Q_k^{(xx)}$ 和状态-控制混合导数 $Q_k^{(xu)}$ ，并且通过控制输入的海森矩阵 $Q_k^{(uu)}$ 进行修正。

6. 最优控制律的推导

根据我们推导的结果，最优控制输入的偏差为：

$\delta u_k^* = - Q_k^{(uu)^{-1}} \left( Q_k^{(u)} + Q_k^{(xu)} \delta x_k \right)$

这意味着最优的控制律具有线性反馈形式：

$u_k^* = u_k + k_k + K_k \delta x_k$

其中：

$k_k = - Q_k^{(uu)^{-1}} Q_k^{(u)}$ 是开环控制增量；
$K_k = - Q_k^{(uu)^{-1}} Q_k^{(xu)}$ 是状态反馈增益矩阵。

这两个矩阵描述了如何根据当前的状态偏差 $\delta x_k$ 来调整控制输入，从而优化整个轨迹。有了新的轨迹后，在新轨迹附近再次进行近似优化，直到达到特定的阈值条件（如控制量变化小于阈值、代价函数变化量小于阈值或者达到最大迭代次数）停止本轮迭代。

7. 递推过程总结

后向传递 (Backward Pass)：递推计算值函数的梯度和海森矩阵，进而得到新的最优控制律。

前向传递 (Forward Pass)：利用新的控制律，更新控制输入和状态轨迹。

迭代更新：检查控制输入和代价函数的变化，如果满足收敛条件，则停止迭代，否则继续下一轮迭代。

1. 后向传递 (Backward Pass)

在后向传递过程中，从终端时刻N开始，递推计算值函数的梯度 $V_k^{(x)}$ 和海森矩阵 $V_k^{(xx)}$ ，并通过这些值函数信息来更新反馈增益和开环控制增量。

1.1 递推计算值函数

值函数的递推公式为：

梯度递推公式：

$V_k^{(x)} = Q_k^{(x)} - Q_k^{(xu)} Q_k^{(uu)^{-1}} Q_k^{(u)}$

海森矩阵递推公式：

$V_k^{(xx)} = Q_k^{(xx)} - Q_k^{(xu)} Q_k^{(uu)^{-1}} Q_k^{(xu)^T}$

这些公式是通过 Q 函数对状态 $x_k$ 和控制 $u_k$ 的二次展开推导得到的，反映了如何通过当前时刻的 Q 函数信息来更新值函数。

1.2 计算最优控制律

最优控制律的反馈形式为：

$\delta u_k^* = - Q_k^{(uu)^{-1}} \left( Q_k^{(u)} + Q_k^{(xu)} \delta x_k \right)$

这个公式表示最优控制输入 $u_k^*$ 通过 Q 函数的梯度和海森矩阵来递推计算。

1.3 计算反馈增益和开环控制增量

最优控制律可以写成开环和闭环反馈的形式：

$u_k^{new} = u_k^* + k_k + K_k \delta x_k$

其中：

开环控制增量：

$k_k = - Q_k^{(uu)^{-1}} Q_k^{(u)}$

状态反馈增益：

$K_k = - Q_k^{(uu)^{-1}} Q_k^{(xu)}$

通过这个反馈形式，后向传递完成后我们得到了更新控制输入所需的反馈增益 $K_k$ 和开环增量 $k_k$ 。

2. 前向传递 (Forward Pass)

在前向传递过程中，利用后向传递得到的最优控制律，更新控制输入并通过系统的动力学方程更新状态轨迹。

2.1 更新控制输入

通过后向传递得到的反馈增益 $K_k$ 和开环控制增量 $k_k$ ，我们可以更新控制输入：

$u_k^{new} = u_k + k_k + K_k \delta x_k$

其中：

$u_k$ 是上一次迭代的控制输入。
$\delta x_k = x_k - x_k^*$ 是状态偏差。

2.2 更新状态轨迹

在得到更新后的控制输入 $u_k^{new}$ 后，我们利用系统的动力学方程 $x_{k+1} = f(x_k, u_k)$ 来更新状态轨迹：

$x_{k+1}^{new} = f(x_k^{new}, u_k^{new})$

这样，前向传递就会生成一条新的状态轨迹 $\{x_0^{new}, x_1^{new}, \dots, x_N^{new}\}$ ，该状态轨迹对应于新的控制输入序列 $\{u_0^{new}, u_1^{new}, \dots, u_{N-1}^{new}\}$ 。

3. 迭代更新

3.1 收敛条件

在每次前向传递后，需要检查控制输入的变化或代价函数的变化，以判断是否继续迭代。常见的收敛条件包括：

控制输入的变化：检查两次迭代间控制输入的变化是否很小：

$\| u_k^{new} - u_k \| < \epsilon$

如果变化很小，说明控制输入已经趋于收敛。

代价函数的变化：计算新的代价函数 $J^{new}$ ，并比较与上一次迭代的代价函数 $J^{old}$ 的差值：

$| J^{new} - J^{old} | < \epsilon$

如果代价函数的变化很小，则认为算法已经收敛。

最大迭代次数：在实际应用中，还可以设定一个最大迭代次数 $N_{max}$ ，当迭代次数超过这个值时，算法也会停止。

3.2 继续下一次迭代

如果收敛条件未满足，则继续迭代：

将新的状态和控制输入作为参考轨迹 $\{x_k^{ref}, u_k^{ref}\}$ ，并再次进行后向传递，递推计算新的最优控制律。
利用新的控制律再次进行前向传递，生成新的状态轨迹和控制输入。

通过这种循环迭代，iLQR 逐步优化控制序列，直到找到最优解。

8. 注意事项

初始猜测：iLQR 需要一个初始的控制输入序列作为起点，初始猜测越好，收敛越快。

线性化近似：由于每次迭代使用一阶泰勒展开线性化系统动态，系统的非线性程度较高时，近似效果会较差，可能需要更多迭代。

二阶成本近似：成本函数的二次化近似也会影响最终的解的质量，尤其是在局部极小点附近。

收敛性问题：iLQR 的收敛性较强，但在某些高度非线性问题中，可能会陷入局部最优，或者在初始化较差时不收敛。

9. 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

# 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)

# Derivative of the cost function w.r.t. control input u (l_u)
def cost_u(u):
    return u

# Derivative of the cost function w.r.t. state x (l_x)
def cost_x(x):
    return x

# Second derivative of the cost function w.r.t. control input u (l_uu)
def cost_uu():
    return 1

# Second derivative of the cost function w.r.t. state x (l_xx)
def cost_xx():
    return 1


# Function to calculate the initial state trajectory based on control sequence
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

# iLQR algorithm with different stopping conditions
def ilqr_with_conditions(x, u, iterations, epsilon_u, epsilon_J, epsilon_x):
    prev_cost = np.inf
    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)

        du = np.zeros(N)  # Control updates

        # Backward pass: compute Q function and control update
        for k in range(N-1, -1, -1):
            # Compute Q-function terms
            f_u = np.cos(u[k])  # Derivative of system dynamics w.r.t. u
            Q_u = cost_u(u[k]) + f_u * V_x[k+1]  # Q_u = l_u + f_u^T * V_x(k+1)
            Q_uu = cost_uu() + f_u**2 * V_xx[k+1]  # Q_uu = l_uu + f_u^T * V_xx(k+1) * f_u
            Q_x = cost_x(x[k]) + V_x[k+1]  # Q_x = l_x + f_x^T * V_x(k+1)
            Q_xx = cost_xx() + V_xx[k+1]  # Q_xx = l_xx + f_x^T * V_xx(k+1) * f_x

            # Update control input
            du[k] = -Q_u / Q_uu  # Control update
            V_x[k] = Q_x + Q_uu * du[k]  # Update value function gradient
            V_xx[k] = Q_xx  # Update value function Hessian (V_xx)

        # Forward pass: update trajectory using the new control inputs
        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] + du[k]  # Update control
            x_new[k+1] = system_dynamics(x_new[k], u_new[k])  # Update state

        
        # Compute the total cost for the current trajectory
        current_cost = np.sum([cost_function(x_new[k], u_new[k]) for k in range(N)])

        # 1. Stop based on control input change
        if np.max(np.abs(du)) < epsilon_u:
            print(f"Stopped due to control input convergence at iteration {i}")
            break
        
        # Update for next iteration
        x = x_new
        u = u_new
        prev_cost = current_cost

    return x, u, i

if __name__ == "__main__":

    # iLQR parameters
    N = 3  # Number of time steps
    x0 = 1  # Initial state
    iterations = 50  # Maximum number of iterations
    epsilon_u = 1e-3  # Tolerance for control input changes
    epsilon_J = 1e-4  # Tolerance for cost function change
    epsilon_x = 1e-4  # Tolerance for state trajectory change

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


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

    # Run iLQR with stopping conditions
    x_final, u_final, num_iterations = ilqr_with_conditions(x_initial, u, iterations, epsilon_u, epsilon_J, epsilon_x)

    # Output the final results and number of iterations
    print(x_final, u_final, num_iterations)