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

（四）LQG和iLQG原理、公式推导以及代码演示

在实际工程应用中，噪声是系统无法避免的因素，主要包括过程噪声和观测噪声。在自动控制、机器人、自主驾驶等领域，噪声的影响尤其显著。为了提高控制系统的鲁棒性和性能，像LQG和iLQG这样的算法被广泛应用。其主要思想是在LQR和ILQR的状态方程中增加高斯噪声，LQG 是结合LQR和卡尔曼滤波器减少噪声干扰，ILQG通过在Q-函数对状态的二阶导数中增加噪声的协方差矩阵来更新控制律，使得系统能够在噪声环境下得到更为鲁棒的控制方案。

1. LQG(Linear Quadratic Gaussian)

1.1. LQG 原理

LQG（线性二次高斯）控制用于解决线性系统在存在高斯噪声扰动下的最优控制问题。它结合了 LQR（线性二次调节器）与 Kalman 滤波器，能够同时处理不确定性和噪声。其中，LQR解决没有噪声的最优控制问题，Kalman 滤波器解决噪声下的状态估计问题。

状态方程：

$x_{k+1} = A x_k + B u_k + w_k$

其中，x_k是状态向量，u_k是控制输入，w_k是过程噪声，假设 w_k为均值为零的高斯白噪声，其协方差矩阵为 Q。

观测方程：

$y_k = C x_k + v_k$

其中，y_k是观测向量，v_k是测量噪声，假设 v_k为均值为零的高斯白噪声，其协方差矩阵为 R。

1.2. LQG 应用场景

LQG 适用于以下场景：

系统受过程噪声和测量噪声干扰；
系统可以用线性模型描述，或者可以通过线性化处理非线性系统；
需要设计基于部分观测值进行最优控制的系统。

LQG 广泛应用于：

航空航天领域，如飞行器的轨迹跟踪和姿态控制；
机器人导航与控制；
自动驾驶中对车辆的路径跟踪；
电力系统的负载控制等。

1.3. LQG 公式推导

LQG 控制的推导分为两个主要步骤：首先是通过 Kalman 滤波器进行状态估计，然后通过 LQR 控制器设计最优控制律。

1.3.1 Kalman 滤波器推导

Kalman 滤波器的目的是在有噪声的情况下对系统状态进行最优估计。假设初始状态 x_0 的估计为 $\hat{x}_0$ 且初始协方差为P_0，Kalman 滤波器的推导如下：

预测步骤：

$\hat{x}_{k+1|k} = A \hat{x}_{k|k} + B u_k$

预测协方差矩阵：

$P_{k+1|k} = A P_{k|k} A^T + Q$

更新步骤：

$K_k = P_{k|k-1} C^T (C P_{k|k-1} C^T + R)^{-1}$

更新状态估计：

$\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (y_k - C \hat{x}_{k|k-1})$

更新协方差矩阵：

$P_{k|k} = (I - K_k C) P_{k|k-1}$

通过Kalman滤波器，系统可以得到噪声干扰下的状态估计 $\hat{x}_k$ 。

1.3.2 LQR 推导

LQR 的目的是最小化某个性能指标 J，通过找到最优控制律 $u_k = -K \hat{x}_k$ 。性能指标定义为：

$J = \sum_{k=0}^{\infty} (x_k^T Q x_k + u_k^T R u_k)$

其中，Q 是状态的权重矩阵，R 是控制输入的权重矩阵。

最优控制律可以通过解 Riccati 方程得到：

$P = A^T P A - A^T P B (B^T P B + R)^{-1} B^T P A + Q$

控制增益矩阵 K 为：

$K = (B^T P B + R)^{-1} B^T P A$

因此，LQR 控制律是：

$u_k = -K x_k$

1.3.3 LQG 控制律

将 Kalman 滤波器得到的状态估计代入 LQR 控制律，就可以得到 LQG 的控制律：

$u_k = -K \hat{x}_k$

其中 $\hat{x}_k$ 是Kalman滤波器估计得到的状态向量。

1.4 python代码

我们使用一个简单的状态方程实现LQG 。考虑如下的线性系统：

$x(k+1) = x(k) + u(k) + w(k)$

其中，x(k)是状态，u(k)是控制输入，w(k)是过程噪声。

$y(k) = x(k) + v(k)$

其中，y(k) 是观测值，v(k) 是测量噪声。

import numpy as np
import matplotlib.pyplot as plt

# 系统参数
A = np.array([[1]])  # 状态矩阵
B = np.array([[1]])  # 控制输入矩阵
Q_lqr = np.array([[1]])  # LQR 中的状态权重
R_lqr = np.array([[1]])  # LQR 中的控制权重

# 迭代求解 Riccati 方程
def solve_lqr_iterative(A, B, Q, R, max_iterations=1000, tol=1e-8):
    P = Q.copy()  # 初始化为状态权重矩阵 Q
    for i in range(max_iterations):
        # 计算控制增益矩阵 K
        K = np.linalg.inv(B.T @ P @ B + R) @ (B.T @ P @ A)
        
        # 更新 Riccati 矩阵 P
        P_new = A.T @ P @ A - A.T @ P @ B @ K + Q
        
        # 检查收敛
        if np.max(np.abs(P_new - P)) < tol:
            break
        
        P = P_new

    return K, P

# 使用迭代法求解 LQR 控制增益
K_lqr, P_lqr = solve_lqr_iterative(A, B, Q_lqr, R_lqr)

# LQR 增益已计算完毕，打印 K_lqr
print("LQR 控制增益 K_lqr:", K_lqr)

# Kalman 滤波器相关参数
Q = 1  # 状态噪声协方差
R = 1  # 测量噪声协方差
P0 = 1  # 初始协方差
n_steps = 50  # 仿真步数

# 初始化状态和估计
x_true = np.zeros(n_steps)  # 真实状态
x_est = np.zeros(n_steps)   # 估计状态
y = np.zeros(n_steps)       # 观测值
u = np.zeros(n_steps)       # 控制输入
P_est = P0                  # 初始协方差
x_true[0] = 0               # 初始状态
x_est[0] = 0                # 初始估计

# 噪声
process_noise = np.random.normal(0, np.sqrt(Q), n_steps)  # 过程噪声
measurement_noise = np.random.normal(0, np.sqrt(R), n_steps)  # 测量噪声

# 仿真循环
for k in range(1, n_steps):
    # 系统动态
    x_true[k] = x_true[k-1] + u[k-1] + process_noise[k]  # 真实状态更新
    y[k] = x_true[k] + measurement_noise[k]              # 观测值

    # Kalman 滤波器预测
    x_pred = x_est[k-1] + u[k-1]
    P_pred = P_est + Q

    # Kalman 滤波器更新
    K_kalman = P_pred / (P_pred + R)
    x_est[k] = x_pred + K_kalman * (y[k] - x_pred)
    P_est = (1 - K_kalman) * P_pred

    # LQR 控制律
    u[k] = -K_lqr[0, 0] * x_est[k]  # 这里直接用标量运算

# 绘制结果
plt.figure(figsize=(12, 6))

plt.subplot(3, 1, 1)
plt.plot(x_true, label='True State')
plt.plot(x_est, label='Estimated State', linestyle='--')
plt.legend()
plt.title('State (True vs Estimated)')

plt.subplot(3, 1, 2)
plt.plot(y, label='Measurements')
plt.legend()
plt.title('Measurements')

plt.subplot(3, 1, 3)
plt.plot(u, label='Control Input')
plt.legend()
plt.title('Control Input')

plt.tight_layout()
plt.show()

2. iLQG(iterative Linear Quadratic Gaussian)

2.1 iLQG原理

iLQG算法是iLQR的扩展版本，专门用于处理具有过程噪声的非线性系统。iLQG的基本原理是利用高斯分布来建模系统的过程噪声，并通过迭代优化策略，使得系统能够在噪声存在的情况下仍能得到一个稳定的最优控制策略。

假设系统的状态方程为：

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

其中：

$x_k$ 是系统在时间步长 $k$ 的状态向量。

$u_k$ 是控制向量。

$w_k$ 是零均值高斯过程噪声，服从分布 $w_k \sim \mathcal{N}(0, Q_w)$ ，其中 $Q_w$ 是过程噪声的协方差矩阵。

目标是最小化以下具有期望的成本函数：

$= \mathbb{E}\left[\sum_{k=0}^{N-1} l(x_k, u_k) + l_f(x_N)\right]$

其中：

$l(x_k, u_k)$ 是运行成本函数。

$l_f(x_N)$ 是终端成本。

目标是通过选择合适的 $u_k$ 来最小化整个期望成本。

2.2. 系统的线性化

和iLQR类似，我们首先对系统状态方程 $f(x_k, u_k)$ 和成本函数 $l(x_k, u_k)$ 进行泰勒展开，以便能够在线性化的基础上进行局部优化。假设我们已有当前的控制策略 $u_k = u_k^0 + \delta u_k$ ，可以对 $f(x_k, u_k)$ 进行一阶泰勒展开：

$x_{k+1} \approx f(x_k^0, u_k^0) + A_k \delta x_k + B_k \delta u_k + w_k$

其中：

$A_k = \frac{\partial f}{\partial x_k}\bigg|_{(x_k^0, u_k^0)}$ 是对状态的雅可比矩阵。

$B_k = \frac{\partial f}{\partial u_k}\bigg|_{(x_k^0, u_k^0)}$ 是对控制的雅可比矩阵。

$\delta x_k = x_k - x_k^0$ 是状态偏差。

2.3. Q-函数的展开（考虑过程噪声）

Q-函数描述的是当前状态和控制下的瞬时成本以及未来期望成本之和。它可以表示为：

$Q(x_k, u_k) = l(x_k, u_k) + \mathbb{E}[V(x_{k+1})]$

为了计算 Q-函数，需要对未来状态 $x_{k+1}$ 的期望进行计算，考虑到 $x_{k+1}$ 是随机变量，因此值函数 $V(x_{k+1})$ 也需要计算其期望值。通过状态方程我们可以知道：

$\mathbb{E}[x_{k+1}] = f(x_k^0, u_k^0) + A_k \delta x_k + B_k \delta u_k$

$Var(x_{k+1})=Q_w$

这意味着状态更新的方差 $Q_w$ 会影响未来期望成本的计算。

我们首先对值函数 $V(x_{k+1})$ 进行二阶泰勒展开：

$V(x_{k+1}) \approx V(x_k^0) + V_x^T \delta x_{k+1} + \frac{1}{2} \delta x_{k+1}^T V_{xx} \delta x_{k+1}$

带入状态更新方程：

$V(x_{k+1}) \approx V(x_k^0) + V_x^T (A_k \delta x_k + B_k \delta u_k + w_k) + \frac{1}{2} (A_k \delta x_k + B_k \delta u_k + w_k)^T V_{xx} (A_k \delta x_k + B_k \delta u_k + w_k)$

由于 $w_k$ 是零均值的高斯噪声，且 $\mathbb{E}[w_k] = 0$ ，可以得到：

$\mathbb{E}[V(x_{k+1})] \approx V(x_k^0) + V_x^T (A_k \delta x_k + B_k \delta u_k) + \frac{1}{2} \left( A_k \delta x_k + B_k \delta u_k \right)^T V_{xx} \left( A_k \delta x_k + B_k \delta u_k \right) + \frac{1}{2} \text{Tr}(V_{xx} Q_w)$

噪声项的贡献体现在最后一项 $\frac{1}{2} \text{Tr}(V_{xx} Q_w)$ ，它代表了未来状态的不确定性对期望成本的影响。这个项与过程噪声的协方差 $Q_w$ 直接相关，且通过值函数的二阶导数 $V_{xx}$ 进行权重调整。

2.4. Q-函数的梯度与二阶导数修正

现在，考虑 $Q_w$ 的影响我们对 Q-函数的导数进行详细推导。

Q-函数的一阶导数

对于状态的导数 $Q_x$ ：

$Q_x = l_x + A_k^T V_x$

这与无噪声的情形一致，因为噪声不影响状态的偏导数。

对于控制的导数 $Q_u$ ：

$Q_u = l_u + B_k^T V_x$

同样，这里的一阶导数部分没有受到噪声的直接影响。

Q-函数的二阶导数

我们重点分析二阶导数部分，因为噪声 $Q_w$ 的影响主要反映在这里。

对于状态的二阶导数 $Q_{xx}$ ：

$Q_{xx} = l_{xx} + A_k^T V_{xx} A_k$

在考虑噪声 $Q_w$ 后，未来状态的不确定性对成本的影响需要通过增加协方差的贡献，因此 $Q_{xx}$ 的修正形式为：

$Q_{xx} = l_{xx} + A_k^T V_{xx} A_k + \frac{1}{2} \text{Tr}(V_{xx} Q_w)$

对于控制的二阶导数 $Q_{uu}$ ：

$Q_{uu} = l_{uu} + B_k^T V_{xx} B_k$

这个部分并不会受到过程噪声的直接影响，因为过程噪声主要影响的是状态的传播和期望。

对于状态与控制之间的交叉导数 $Q_{xu}$ ：

$Q_{xu} = l_{xu} + A_k^T V_{xx} B_k$

这个部分也是由于噪声通过状态更新影响未来的值函数导数，但直接对控制的噪声影响较小。

基于修正后的 Q-函数导数，我们可以通过最小化 Q-函数对控制偏差 $\delta u_k$ 进行更新：

$\delta u_k = - Q_{uu}^{-1} (Q_u + Q_{xu}^T \delta x_k)$

新的控制策略仍然通过这个更新公式来迭代优化，只是现在的 Q-函数二阶导数 $Q_{xx}$ 需要考虑噪声的影响。

2.5 python代码

与上一章节相同，我们用一个简单的非线性系统，其状态 $x_k$ 和控制输入 $u_k$ 满足以下非线性动力学方程： $x_{k+1} = x_k + \sin(u_k)$ ，这里的非线性在于控制输入的影响是通过正弦函数进入系统的，噪声 $w_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)$ 。

import numpy as np

# 系统参数
N = 5  # 时间步长
x0 = np.array([1.0])  # 初始状态
Q_w = 0.05  # 过程噪声的协方差矩阵（假设标量）
state_cost_Q = 1.0  # 状态代价，原Q_x
control_cost_R = 1.0  # 控制代价，原R_u

# 初始控制输入和状态轨迹
u_init = np.zeros(N)
x_traj = np.zeros(N+1)

# 定义系统动态
def dynamics(x, u, noise):
    return x + np.sin(u) + noise

# 定义代价函数
def cost(x, u):
    return 0.5 * (state_cost_Q * x**2 + control_cost_R * u**2)

# 终端代价
def terminal_cost(x):
    return 0.5 * state_cost_Q * x**2

# iLQG算法
def ilqg(x0, u_init, N, Q_w, max_iter=100, tol=1e-4):
    # 初始控制输入
    u = u_init.copy()
    x_traj[0] = x0
    V_x = np.zeros((N, 1))  # 初始化值函数一阶导数
    V_xx = np.zeros((N, 1, 1))  # 初始化值函数二阶导数

    for iteration in range(max_iter):
        # 前向传播得到状态轨迹
        x_traj[0] = x0
        for k in range(N):
            noise = np.random.normal(0, np.sqrt(Q_w))  # 过程噪声
            x_traj[k+1] = dynamics(x_traj[k], u[k], noise)

        # 初始化终端代价
        V_x[N-1] = np.array([[state_cost_Q * x_traj[-1]]])  # 终端一阶导数 (正确初始化)
        V_xx[N-1] = np.array([[state_cost_Q]])  # 终端二阶导数 (直接为state_cost_Q)

        # 反向传播，计算Q函数导数
        for k in range(N-2, -1, -1):
            x_k = x_traj[k]
            u_k = u[k]

            # 计算Q-函数的一阶导数
            Qx_value_func = x_k + V_x[k+1]  # Q-函数对状态的导数
            Qu_value_func = control_cost_R * u_k + np.cos(u_k) * V_x[k+1]  # Q-函数对控制的导数

            # 计算Q-函数的二阶导数
            Qxx_value_func = 1.0 + V_xx[k+1]  # Q-函数对状态的二阶导数
            Quu_value_func = control_cost_R + np.cos(u_k)**2 * V_xx[k+1]  # Q-函数对控制的二阶导数
            Qxu_value_func = np.cos(u_k) * V_xx[k+1]  # 状态和控制之间的交叉项

            # 增加tr(V_xx * Q_w)项，表示噪声对Q_xx的影响
            tr_Vxx_Qw = np.trace(V_xx[k+1]) * Q_w  # V_xx是1x1的矩阵，trace就是它本身
            Qxx_value_func += tr_Vxx_Qw  # 增加tr(V_xx * Q_w)项到状态二阶导数

            # 通过过程噪声对V_xx的修正
            V_xx[k] = Qxx_value_func  # 更新值函数的二阶导数

            # 更新值函数的一阶导数
            V_x[k] = Qx_value_func - Qxu_value_func * np.linalg.inv(Quu_value_func) @ Qu_value_func  # 更新值函数的一阶导数

            # 控制更新
            du = - np.linalg.inv(Quu_value_func) @ Qu_value_func  # 控制更新量
            u[k] += du  # 更新控制输入

        # 检查收敛
        if np.max(np.abs(du)) < tol:
            print(f"Converged in {iteration} iterations.")
            break
    
    return u, x_traj

# 运行 iLQG 算法
u_opt, x_opt = ilqg(x0, u_init, N, Q_w)

# 打印结果
print("Optimal control:", u_opt)
print("Optimal trajectory:", x_opt)

设定不同的过程噪声协方差，输出结果为：

$Q_w = 0.01$ 时：

Optimal control: [-0.65172071 -0.37033912 -0.25424763 -0.16477019 0. ]
Optimal trajectory: [1. 0.65477667 0.39857155 0.26902991 0.16407018 0.15620706]

$Q_w = 0.1$ 时：

Optimal control: [-0.61400224 -0.43741123 -0.1530362 0.20908395 0. ]
Optimal trajectory: [ 1. 0.42858991 0.61577511 -0.04834357 -0.70982535 -0.8874807 ]

从上述结果可以看出， $Q_w$ 越大，控制输出的变化越小，这是因为噪声越大，系统不确定性越强，为了避免因为过大的噪声导致状态失控，算法可能减少控制量，使得控制策略更加平滑和保守。