别再死记硬背公式了！用Python+NumPy手把手实现离散LQR控制器（附完整代码）

矢锋

105人浏览 · 2026-04-28 10:06:06

矢锋 · 2026-04-28 10:06:06 发布

用Python+NumPy实战离散LQR控制器：从理论到代码的完整实现

在控制理论领域，线性二次调节器（LQR）一直被视为经典的最优控制方法。然而许多工程师和学生在学习过程中常常陷入一个困境：虽然能够理解LQR的数学推导，却不知道如何将这些抽象的矩阵方程转化为实际可运行的代码。本文将彻底改变这一现状，通过Python和NumPy带你一步步实现离散LQR控制器，让你真正掌握从理论到实践的完整链路。

1. 环境准备与基础概念

在开始编码之前，我们需要确保开发环境配置正确。推荐使用Python 3.8+版本，并安装以下关键库：

pip install numpy matplotlib scipy

LQR控制器的核心是解决代数Riccati方程（DARE）。离散时间LQR问题可以表述为：对于一个离散线性系统

$$ x_{k+1} = Ax_k + Bu_k $$

我们需要找到控制序列$u_k$，使得以下代价函数最小化：

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

其中：

$Q$：状态权重矩阵（半正定）
$R$：控制权重矩阵（正定）
$K$：反馈增益矩阵

提示：在实际应用中，Q和R通常选择对角矩阵，对角线元素决定了对应状态或控制输入的权重。

2. DARE求解器的实现

离散代数Riccati方程（DARE）是LQR控制器的核心，其标准形式为：

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

我们将使用迭代法来实现DARE求解器。以下是完整的Python实现：

import numpy as np

def dare_solver(A, B, Q, R, max_iter=1000, tol=1e-6):
    """
    离散代数Riccati方程(DARE)求解器
    
    参数:
        A, B: 系统矩阵
        Q, R: 权重矩阵
        max_iter: 最大迭代次数
        tol: 收敛容忍度
        
    返回:
        P: Riccati方程的解
    """
    n = A.shape[0]
    P = Q.copy()  # 初始猜测
    
    for _ in range(max_iter):
        P_new = A.T @ P @ A - A.T @ P @ B @ np.linalg.inv(R + B.T @ P @ B) @ B.T @ P @ A + Q
        if np.max(np.abs(P_new - P)) < tol:
            break
        P = P_new
    
    return P

这个实现中需要注意几个关键点：

我们使用矩阵乘法运算符@而不是np.dot，因为前者更清晰
初始猜测选择$Q$矩阵，这通常能加速收敛
收敛条件基于矩阵元素的最大变化量

注意：对于大规模系统，这种直接矩阵求逆的方法可能效率不高，可以考虑使用更高效的数值方法。

3. 反馈增益计算与控制器实现

得到Riccati方程的解$P$后，我们可以计算反馈增益矩阵$K$：

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

对应的Python实现如下：

def compute_lqr_gain(A, B, Q, R):
    """计算LQR反馈增益矩阵K"""
    P = dare_solver(A, B, Q, R)
    K = np.linalg.inv(R + B.T @ P @ B) @ B.T @ P @ A
    return K

现在，我们可以将这些组件组合成一个完整的LQR控制器类：

class DiscreteLQR:
    def __init__(self, A, B, Q, R):
        self.A = A
        self.B = B
        self.Q = Q
        self.R = R
        self.K = None
        
    def compute_gain(self):
        """计算并返回反馈增益矩阵"""
        self.K = compute_lqr_gain(self.A, self.B, self.Q, self.R)
        return self.K
    
    def control_input(self, x):
        """计算控制输入u=-Kx"""
        if self.K is None:
            self.compute_gain()
        return -self.K @ x

4. 弹簧质量阻尼器系统案例

为了验证我们的实现，我们考虑一个经典的弹簧质量阻尼器系统。系统的离散状态空间方程为：

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

其中状态向量$x = [位置, 速度]^T$，控制输入$u$是施加的力。假设系统参数如下：

质量$m = 1.0$ kg
弹簧常数$k = 1.0$ N/m
阻尼系数$c = 0.1$ Ns/m
采样时间$dt = 0.1$ s

我们可以先定义系统矩阵：

# 系统参数
m = 1.0   # 质量
k = 1.0   # 弹簧常数
c = 0.1   # 阻尼系数
dt = 0.1  # 采样时间

# 连续时间系统矩阵
A_cont = np.array([[0, 1],
                   [-k/m, -c/m]])
B_cont = np.array([[0],
                   [1/m]])

# 离散化系统矩阵
A = np.eye(2) + A_cont * dt
B = B_cont * dt

接下来，我们选择合适的权重矩阵并设计LQR控制器：

# 权重矩阵
Q = np.diag([10, 1])  # 更重视位置误差
R = np.array([[0.1]])  # 控制权重

# 创建LQR控制器
lqr = DiscreteLQR(A, B, Q, R)
K = lqr.compute_gain()

print("反馈增益矩阵K:")
print(K)

典型的输出可能类似于：

反馈增益矩阵K:
[[ 8.66025404  5.47722558]]

5. 系统仿真与性能分析

有了控制器后，我们可以进行闭环系统仿真。假设初始条件为$x_0 = [1, 0]^T$（初始位移1米，速度0）：

def simulate_system(A, B, controller, x0, steps=100):
    """仿真闭环系统响应"""
    x = x0.copy()
    x_history = [x.copy()]
    
    for _ in range(steps):
        u = controller.control_input(x)
        x = A @ x + B @ u
        x_history.append(x.copy())
    
    return np.array(x_history).T

# 初始条件
x0 = np.array([1, 0])  # 初始位移1m，速度为0

# 仿真
x_history = simulate_system(A, B, lqr, x0)

# 绘制结果
import matplotlib.pyplot as plt

plt.figure(figsize=(10, 6))
plt.plot(x_history[0], label='位置')
plt.plot(x_history[1], label='速度')
plt.xlabel('时间步')
plt.ylabel('状态值')
plt.title('LQR控制下的系统响应')
plt.legend()
plt.grid(True)
plt.show()

仿真结果将展示系统状态如何随时间收敛到零。通过调整Q和R矩阵，我们可以观察到不同的控制效果：

Q矩阵设置	R值	收敛速度	控制力度
diag([10,1])	0.1	较快	中等
diag([1,1])	0.1	中等	较小
diag([10,1])	1.0	较慢	很小

6. 数值稳定性与调试技巧

在实际实现中，我们可能会遇到各种数值问题。以下是几个常见问题及其解决方案：

DARE不收敛：
- 检查系统是否可控
- 增加最大迭代次数
- 尝试不同的初始猜测
矩阵求逆问题：
- 确保R矩阵是正定的
- 使用伪逆np.linalg.pinv代替inv
- 添加小的正则化项：R + eps*np.eye(R.shape[0])
性能不佳：
- 调整Q和R的权重比例
- 检查系统离散化是否正确
- 验证系统矩阵A,B的准确性

改进版的DARE求解器可以加入这些增强：

def robust_dare_solver(A, B, Q, R, max_iter=1000, tol=1e-6, eps=1e-8):
    """更鲁棒的DARE求解器"""
    n = A.shape[0]
    P = Q.copy()
    R_reg = R + eps * np.eye(R.shape[0])  # 正则化
    
    for i in range(max_iter):
        inv_term = np.linalg.pinv(R_reg + B.T @ P @ B)
        P_new = A.T @ P @ A - A.T @ P @ B @ inv_term @ B.T @ P @ A + Q
        
        diff = np.max(np.abs(P_new - P))
        P = P_new
        if diff < tol:
            break
    
    if i == max_iter - 1:
        print("警告: 达到最大迭代次数，可能未收敛")
    
    return P

7. 进阶应用与扩展

掌握了基本LQR实现后，我们可以考虑以下扩展方向：

跟踪问题：修改代价函数以实现轨迹跟踪
时变系统：处理A,B矩阵随时间变化的情况
输出反馈：当无法测量所有状态时，结合状态观测器
非线性系统：在操作点附近线性化并应用LQR

例如，对于跟踪问题，我们可以修改控制策略：

def tracking_control(controller, x, x_ref):
    """跟踪控制: u = -K(x - x_ref)"""
    return controller.control_input(x - x_ref)

在倒立摆等更复杂的系统中，LQR表现同样出色。关键步骤包括：

在平衡点附近线性化系统
设计合适的Q,R矩阵
验证控制器在非线性模型上的表现

8. 性能优化技巧

对于需要实时运行的应用，我们可以优化代码性能：

预计算增益矩阵：离线计算K矩阵
使用更高效的求解器：如scipy.linalg.solve_discrete_are
代码向量化：处理批量状态时使用矩阵运算
利用稀疏性：对于大型稀疏系统使用稀疏矩阵

比较不同求解方法的性能：

from scipy.linalg import solve_discrete_are
import time

# 自定义DARE求解器
start = time.time()
P_custom = dare_solver(A, B, Q, R)
print(f"自定义求解器时间: {time.time()-start:.4f}s")

# SciPy提供的求解器
start = time.time()
P_scipy = solve_discrete_are(A, B, Q, R)
print(f"SciPy求解器时间: {time.time()-start:.4f}s")

# 验证结果一致性
print("解矩阵差异:", np.max(np.abs(P_custom - P_scipy)))

在实际项目中，根据系统规模和要求选择合适的实现方式。对于大多数中小型系统，SciPy提供的求解器已经足够高效。

AI Agent技术社区

Agent 垂直技术社区，欢迎活跃、内容共建。

更多推荐