别再死记硬背公式了！用Python从零实现LQR控制器（附完整代码与调参心得）

用Python从零构建LQR控制器：代码实现与调参实战指南

1. 为什么需要动手实现LQR？

在控制理论课上，我们经常被要求推导LQR的Riccati方程，却很少有机会真正用代码把它实现出来。直到我在研究生课题中尝试用LQR控制一个倒立摆系统时，才发现理论和实践之间存在着巨大的鸿沟——那些教科书上的矩阵运算，在实际编程中会遇到各种数值稳定性问题；而Q、R矩阵的参数调整，更像是一门玄学艺术。

本文将带你用Python从零开始构建一个完整的LQR控制器。不同于单纯的理论推导，我们会聚焦于：

• 如何用numpy和scipy可靠地求解Riccati方程
• 设计一个面向对象的LQR控制器实现
• 通过可视化调试理解Q/R参数的实际影响
• 在倒立摆系统上的完整应用案例

2. 搭建LQR算法核心框架

2.1 求解Riccati方程的数值稳定方法

连续时间的代数Riccati方程(CARE)形式为：

AᵀP + PA - PBR⁻¹BᵀP + Q = 0

在Python中，我们可以使用scipy.linalg.solve_continuous_are来求解：

import numpy as np from scipy.linalg import solve_continuous_are def solve_care(A, B, Q, R): """求解连续时间Riccati方程的稳定实现""" P = solve_continuous_are(A, B, Q, R) K = np.linalg.inv(R) @ B.T @ P # 最优反馈增益 return P, K

关键技巧：

• 检查R矩阵的条件数：np.linalg.cond(R)，过大时需调整R的对角元素
• 对于病态系统，可添加正则化项：Q += 1e-6 * np.eye(n)

2.2 离散时间LQR的实现

离散时间版本使用DARE方程，对应scipy.linalg.solve_discrete_are：

def solve_dare(A, B, Q, R): """求解离散时间Riccati方程""" P = solve_discrete_are(A, B, Q, R) K = np.linalg.inv(B.T @ P @ B + R) @ B.T @ P @ A return P, K

3. 构建LQR控制器类

让我们设计一个面向对象的实现，方便复用：

class LQRController: def __init__(self, A, B, Q, R, dt=None): """ 参数: A, B: 系统动态矩阵 Q: 状态权重矩阵 (n x n) R: 控制权重矩阵 (m x m) dt: 离散时间步长 (None表示连续时间) """ self.A = np.array(A) self.B = np.array(B) self.Q = np.array(Q) self.R = np.array(R) self.dt = dt if dt is None: self.P, self.K = solve_care(A, B, Q, R) else: self.P, self.K = solve_dare(A, B, Q, R) def compute_control(self, x): """计算控制输入 u = -Kx""" return -self.K @ np.array(x)

4. Q/R矩阵调参的艺术

4.1 参数选择的基本原则

4.2 可视化调试方法

import matplotlib.pyplot as plt def tune_lqr(A, B, Q_candidates, R): plt.figure(figsize=(12, 6)) for i, Q in enumerate(Q_candidates): lqr = LQRController(A, B, Q, R) # 模拟系统响应 x = simulate_system(lqr) plt.plot(x[:, 0], label=f'Q={np.diag(Q)}') plt.xlabel('Time steps') plt.ylabel('State value') plt.legend() plt.show()

调试心得：

1. 从对角线矩阵开始，Q = diag([1, 1, 1, 1])
2. 先调整第一个状态对应的Q值，观察响应速度
3. 逐步引入其他状态的权重
4. 最后微调R值平衡控制量大小

5. 倒立摆案例实战

5.1 系统建模

倒立摆的线性化模型：

# 参数 g = 9.8 # 重力加速度 l = 0.5 # 摆杆长度 m = 0.2 # 摆锤质量 M = 1.0 # 小车质量 # 状态矩阵 (x, x_dot, theta, theta_dot) A = np.array([ [0, 1, 0, 0], [0, 0, -m*g/M, 0], [0, 0, 0, 1], [0, 0, (M+m)*g/(M*l), 0] ]) B = np.array([[0], [1/M], [0], [-1/(M*l)]])

5.2 控制器设计与仿真

# 权重矩阵 Q = np.diag([1, 0.1, 10, 0.1]) # 重点控制角度theta R = np.array([[0.1]]) # 创建控制器 lqr = LQRController(A, B, Q, R) # 仿真 def simulate_pendulum(lqr, x0, T=10, dt=0.01): """倒立摆仿真""" n_steps = int(T/dt) x = np.zeros((n_steps, 4)) x[0] = x0 for i in range(1, n_steps): u = lqr.compute_control(x[i-1]) # 使用欧拉积分更新状态 x[i] = x[i-1] + dt*(A @ x[i-1] + B @ u) return x # 初始条件：小车偏移0.5m，摆杆倾斜10度 x0 = [0.5, 0, np.deg2rad(10), 0] trajectory = simulate_pendulum(lqr, x0)

5.3 结果可视化

plt.figure(figsize=(12, 4)) plt.subplot(121) plt.plot(trajectory[:, 0], label='Cart position') plt.plot(trajectory[:, 2], label='Pendulum angle') plt.legend() plt.subplot(122) plt.plot(trajectory[:, 1], label='Cart velocity') plt.plot(trajectory[:, 3], label='Pendulum angular velocity') plt.legend()

6. 常见问题与解决方案

问题1：Riccati方程求解失败

• 检查系统可控性：np.linalg.matrix_rank(ctrb(A, B)) == n
• 尝试调整Q/R的尺度比例

问题2：控制输入过大

• 逐步增大R的对角元素
• 添加控制量约束：

  u = np.clip(u, -u_max, u_max)

问题3：稳态误差

• 考虑引入积分项（LQI）
• 或者添加前馈补偿：

  u_ff = np.linalg.pinv(B) @ (np.eye(n) - A) @ x_target

7. 进阶技巧与性能优化

7.1 实时参数调整

class AdaptiveLQR(LQRController): def update_weights(self, Q_new, R_new): """在线更新权重矩阵""" self.Q = Q_new self.R = R_new if self.dt is None: self.P, self.K = solve_care(self.A, self.B, Q_new, R_new) else: self.P, self.K = solve_dare(self.A, self.B, Q_new, R_new)

7.2 处理延迟补偿

def predict_state(x, u, delay, dt): """预测延迟后的状态""" steps = int(delay/dt) x_pred = x.copy() for _ in range(steps): x_pred = x_pred + dt*(A @ x_pred + B @ u) return x_pred

8. 从仿真到实际系统的注意事项

1. 采样时间选择：通常为系统最快动态的1/10~1/20
2. 噪声处理：添加状态估计器（如卡尔曼滤波）
3. 执行器饱和：在仿真中加入执行器模型
4. 参数不确定性：进行鲁棒性测试

def robustness_test(lqr, param_variations): """鲁棒性测试函数""" results = [] for p in param_variations: A_perturbed = perturb_system(A, p) x = simulate_system(A_perturbed, B, lqr) results.append(compute_performance(x)) return results