LQR控制器

一·前言

在学习最优控制时接触到了LQR控制器，LQR控制器优点明显：

最优性：能够在给定的性能指标下找到最优控制策略。稳定性：保证闭环系统的渐近稳定性（前提是系统可控）。灵活性：可以通过调整 Q 和 R 矩阵来满足不同的控制需求。

但是也存在一些缺点，如依赖精确模型，计算较复杂。不过LQR控制还是很好用的，因此我们就简单介绍一下LQR控制器。

二·LQR原理

LQR（线性二次型调节器）通过设计一个反馈控制律，使得线性系统在满足一定约束条件下，使一个二次型性能指标达到最小。具体步骤如下：

1.系统建模：

将实际的控制系统抽象为线性时不变系统的状态空间模型：

2.定义性能指标：

其中，Q 是 n*n 的半正定状态加权矩阵，用于惩罚系统状态偏离期望状态的程度；R 是 m*m  的正定控制加权矩阵，用于惩罚控制输入的能量消耗。

3.求解最优控制率：

通过求解代数 Riccati 方程：

得到对称正定矩阵 P。然后，最优反馈增益矩阵 K 可以计算为：

最优控制输入为：

4.闭环控制系统：

将控制输入 u(t) 代入系统状态空间模型，得到闭环系统的状态方程：

三·示例：倒立摆控制系统

四·代码实例

import numpy as np
import scipy.linalg
import matplotlib.pyplot as plt

# 定义系统参数
g = 9.81  # 重力加速度
l = 1.0   # 摆杆长度
m = 1.0   # 摆杆质量

# 系统矩阵 A 和输入矩阵 B
A = np.array([[0, 1], [g / l, 0]])
B = np.array([[0], [1 / (m * l**2)]])

# 定义状态加权矩阵 Q 和控制加权矩阵 R
Q = np.array([[10, 0], [0, 1]])
R = np.array([[1]])

def lqr(A, B, Q, R):
    """
    求解 LQR 控制器的反馈增益矩阵 K
    :param A: 系统矩阵
    :param B: 输入矩阵
    :param Q: 状态加权矩阵
    :param R: 控制加权矩阵
    :return: 反馈增益矩阵 K
    """
    # 求解代数 Riccati 方程
    P = scipy.linalg.solve_continuous_are(A, B, Q, R)
    # 计算反馈增益矩阵 K
    K = np.linalg.inv(R) @ B.T @ P
    return K

# 求解反馈增益矩阵 K
K = lqr(A, B, Q, R)
print("反馈增益矩阵 K:")
print(K)

# 定义仿真参数
dt = 0.01  # 时间步长
T = 10     # 仿真时间
t = np.arange(0, T, dt)
N = len(t)

# 初始化状态向量
x = np.array([[np.pi / 6], [0]])  # 初始角度为 30 度，初始角速度为 0

# 存储状态和控制输入
x_history = np.zeros((2, N))
u_history = np.zeros((1, N))

# 仿真循环
for i in range(N):
    # 计算控制输入
    u = -K @ x
    # 更新系统状态
    dx = A @ x + B @ u
    x = x + dx * dt
    # 存储状态和控制输入
    x_history[:, i] = x.flatten()
    u_history[:, i] = u.flatten()

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

plt.subplot(2, 1, 1)
plt.plot(t, x_history[0, :], label='角度 (rad)')
plt.plot(t, x_history[1, :], label='角速度 (rad/s)')
plt.xlabel('时间 (s)')
plt.ylabel('状态')
plt.legend()

plt.subplot(2, 1, 2)
plt.plot(t, u_history[0, :], label='控制输入')
plt.xlabel('时间 (s)')
plt.ylabel('控制输入')
plt.legend()

plt.show()

五·代码解释

1. 系统建模：定义了倒立摆系统的系统矩阵 A 和输入矩阵 B。
2. LQR 控制器设计：通过 lqr 函数求解反馈增益矩阵 K，该函数使用 scipy.linalg.solve_continuous_are 函数求解代数 Riccati 方程。
3. 仿真循环：在仿真循环中，根据当前状态计算控制输入 u，并更新系统状态。
4. 结果绘制：使用 matplotlib 库绘制系统状态和控制输入随时间的变化曲线。

通过这个例子，我们可以看到 LQR 控制器如何通过反馈控制律将倒立摆系统的状态稳定到期望状态。

六·公式推导