卡尔曼滤波+LQR实战：用Python手写一个LQG控制器（附Jupyter Notebook）

卡尔曼滤波LQR实战用Python手写一个LQG控制器附Jupyter Notebook在机器人控制和自动化系统设计中LQGLinear Quadratic Gaussian控制是一种经典且强大的控制策略。它巧妙地将卡尔曼滤波的状态估计能力与LQRLinear Quadratic Regulator的最优控制能力相结合为系统提供在噪声环境下的最优控制方案。本文将带您从零开始用Python实现一个完整的LQG控制器并通过Jupyter Notebook交互式环境展示每个关键步骤的实现细节。1. LQG控制基础架构LQG控制的核心思想是将状态估计和最优控制两个过程分离处理。这种分离原理使得我们可以独立设计状态观测器和控制器大大简化了系统设计的复杂性。LQG控制器由两大组件构成卡尔曼滤波器用于在存在测量噪声和过程噪声的情况下对系统状态进行最优估计LQR控制器基于估计的状态计算最优控制输入这种架构特别适合处理现实世界中的控制系统因为我们几乎无法直接测量所有状态变量且系统总会受到各种噪声干扰。2. 系统建模与问题设定为了具体说明LQG的实现我们以一个经典的倒立摆控制系统为例。倒立摆是一个典型的非线性不稳定系统常用于测试控制算法的有效性。2.1 倒立摆的线性化模型倒立摆的非线性动力学方程可以通过在小角度附近线性化得到import numpy as np from scipy.linalg import solve_continuous_are # 系统参数 M 1.0 # 小车质量 (kg) m 0.1 # 摆杆质量 (kg) l 0.5 # 摆杆长度 (m) g 9.8 # 重力加速度 (m/s^2) # 线性化后的状态空间模型 A np.array([ [0, 1, 0, 0], [0, 0, -m*g/M, 0], [0, 0, 0, 1], [0, 0, (Mm)*g/(M*l), 0] ]) B np.array([ [0], [1/M], [0], [-1/(M*l)] ]) C np.array([[1, 0, 0, 0], [0, 0, 1, 0]]) # 只能观测位置和角度 D np.array([[0], [0]])2.2 离散化处理由于我们的实现将在数字计算机上运行需要将连续系统离散化from scipy.linalg import expm dt 0.02 # 采样时间 (s) # 精确离散化方法 A_d expm(A * dt) B_d np.linalg.inv(A) (A_d - np.eye(4)) B3. 卡尔曼滤波器实现卡尔曼滤波器是LQG中的状态估计器它通过融合系统模型预测和实际测量值提供最优的状态估计。3.1 卡尔曼滤波算法步骤卡尔曼滤波包含两个主要阶段预测阶段状态预测x̂ₖ⁻ A x̂ₖ₋₁ B uₖ₋₁误差协方差预测Pₖ⁻ A Pₖ₋₁ Aᵀ Q更新阶段卡尔曼增益计算Kₖ Pₖ⁻ Cᵀ (C Pₖ⁻ Cᵀ R)⁻¹状态更新x̂ₖ x̂ₖ⁻ Kₖ (yₖ - C x̂ₖ⁻)协方差更新Pₖ (I - Kₖ C) Pₖ⁻3.2 Python实现class KalmanFilter: def __init__(self, A, B, C, Q, R): self.A A # 状态转移矩阵 self.B B # 控制输入矩阵 self.C C # 观测矩阵 self.Q Q # 过程噪声协方差 self.R R # 观测噪声协方差 self.P np.eye(A.shape[0]) # 误差协方差矩阵 self.x np.zeros((A.shape[0], 1)) # 初始状态估计 def predict(self, u): self.x self.A self.x self.B u self.P self.A self.P self.A.T self.Q return self.x def update(self, y): K self.P self.C.T np.linalg.inv(self.C self.P self.C.T self.R) self.x self.x K (y - self.C self.x) self.P (np.eye(self.A.shape[0]) - K self.C) self.P return self.x3.3 噪声协方差调试技巧选择合适的Q和R矩阵对滤波器性能至关重要。这里提供一个实用的调试方法参数物理意义调试建议Q过程噪声协方差从对角线小值开始逐步增大直到滤波器响应足够快但不振荡R观测噪声协方差根据实际传感器噪声水平设置通常可从数据手册获取# 示例噪声协方差矩阵 Q np.diag([0.01, 0.01, 0.01, 0.01]) # 过程噪声 R np.diag([0.1, 0.05]) # 观测噪声假设角度测量比位置更精确4. LQR控制器设计LQR控制器通过最小化一个二次型代价函数来求解最优控制律。代价函数通常表示为J ∫(xᵀQx uᵀRu) dt4.1 求解Riccati方程连续时间代数Riccati方程的解给出了最优反馈增益矩阵# 定义LQR权重矩阵 Q_lqr np.diag([10, 1, 100, 10]) # 状态权重 R_lqr np.array([[0.1]]) # 控制输入权重 # 求解Riccati方程 P solve_continuous_are(A, B, Q_lqr, R_lqr) # 计算反馈增益矩阵 K np.linalg.inv(R_lqr) B.T P4.2 离散时间LQR实现对于离散系统我们可以使用离散代数Riccati方程from scipy.linalg import solve_discrete_are def dlqr(A, B, Q, R): P solve_discrete_are(A, B, Q, R) K np.linalg.inv(B.T P B R) (B.T P A) return K5. LQG系统集成与仿真将卡尔曼滤波器和LQR控制器组合起来就形成了完整的LQG控制器。5.1 闭环系统实现def simulate_lqg(A, B, C, Q_kf, R_kf, Q_lqr, R_lqr, x0, steps): # 初始化滤波器和控制器 kf KalmanFilter(A, B, C, Q_kf, R_kf) K dlqr(A, B, Q_lqr, R_lqr) # 存储结果 x_est np.zeros((steps, A.shape[0])) x_true np.zeros((steps, A.shape[0])) y_meas np.zeros((steps, C.shape[0])) # 初始状态 x_true[0] x0 y_meas[0] C x0 np.random.multivariate_normal(np.zeros(R_kf.shape[0]), R_kf) x_est[0] kf.x.ravel() for t in range(1, steps): # 真实系统动态含过程噪声 process_noise np.random.multivariate_normal(np.zeros(Q_kf.shape[0]), Q_kf) x_true[t] A x_true[t-1] B (-K x_est[t-1]) process_noise # 测量含观测噪声 y_meas[t] C x_true[t] np.random.multivariate_normal(np.zeros(R_kf.shape[0]), R_kf) # 卡尔曼滤波更新 kf.predict(-K kf.x) kf.update(y_meas[t]) x_est[t] kf.x.ravel() return x_true, x_est, y_meas5.2 仿真结果可视化使用Matplotlib进行结果可视化是理解系统行为的关键import matplotlib.pyplot as plt # 运行仿真 x_true, x_est, y_meas simulate_lqg(A_d, B_d, C, Q, R, Q_lqr, R_lqr, np.array([0, 0, 0.1, 0]), 500) # 绘制结果 plt.figure(figsize(12, 8)) plt.subplot(2, 1, 1) plt.plot(x_true[:, 0], labelTrue position) plt.plot(x_est[:, 0], --, labelEstimated position) plt.plot(y_meas[:, 0], ., markersize2, labelMeasured position) plt.ylabel(Position (m)) plt.legend() plt.subplot(2, 1, 2) plt.plot(x_true[:, 2], labelTrue angle) plt.plot(x_est[:, 2], --, labelEstimated angle) plt.plot(y_meas[:, 1], ., markersize2, labelMeasured angle) plt.ylabel(Angle (rad)) plt.xlabel(Time step) plt.legend() plt.tight_layout() plt.show()6. 性能优化与调试技巧在实际应用中LQG控制器的性能高度依赖于参数选择。以下是一些实用调试技巧6.1 权重矩阵选择指南LQR的Q和R矩阵选择对系统性能有决定性影响。这里提供一个系统化的调试方法确定主要控制目标是位置跟踪还是角度稳定从单位矩阵开始Q IR 1逐步调整增加Q中对关键状态的权重如角度减小R值会增加控制力度但可能导致执行器饱和使用Bryson规则进行归一化Q_ii 1 / (最大允许值)²R_jj 1 / (最大允许控制量)²6.2 卡尔曼滤波调试技巧过程噪声Q从较小值开始逐步增大直到滤波器响应足够快观测噪声R根据传感器实际精度设置协方差矩阵初始化P0应反映初始状态的不确定性提示在实际调试中可以先用较大的R值假设测量不太准确然后逐步减小以观察滤波器响应变化。7. 与Matlab实现的对比虽然Matlab提供了现成的LQG设计工具但Python实现有以下优势特性Python实现Matlab实现灵活性高可完全自定义每个步骤中等受限于工具箱函数可视化丰富可自由定制标准有一定限制部署易于集成到生产系统需要Matlab运行时成本开源免费需要商业授权交互性Jupyter Notebook提供优秀交互体验主要通过脚本和SimulinkPython特别适合需要深度定制或部署到嵌入式系统的场景而Matlab则更适合快速原型验证。