最优化理论与自动驾驶（六）：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是状态向量，u_k是控制输入，w_k是过程噪声，假设 w_k为均值为零的高斯白噪声，其协方差矩阵为 Q。

观测方程：

其中，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 的估计为且初始协方差为P_0，Kalman 滤波器的推导如下：

预测步骤：

预测协方差矩阵：

更新步骤：

更新状态估计：

更新协方差矩阵：

通过Kalman滤波器，系统可以得到噪声干扰下的状态估计​。

1.3.2 LQR 推导

LQR 的目的是最小化某个性能指标 J，通过找到最优控制律。性能指标定义为：

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

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

控制增益矩阵 K 为：

因此，LQR 控制律是：

1.3.3 LQG 控制律

将 Kalman 滤波器得到的状态估计代入 LQR 控制律