Linear Quadratic Regulator

📖 1. 概述

LQR（Linear Quadratic Regulator，线性二次调节器）是一种经典的最优控制方法。其核心思想是在系统动态为线性、代价函数为二次型的假设下，求解使系统从初始状态回到平衡点且代价最小的控制问题。

🔧 2. 问题建模

2.1 线性系统状态空间模型

2.2 二次型代价函数

权重矩阵的含义

📝 3. 公式推导

3.1 连续时间无限时域 LQR

3.2 离散时间无限时域 LQR

⚖️ 4. 连续时间与离散时间对比

⏳ 5. 有限时域 LQR

有限 vs 无限时域

⚙️ 6. 求解算法

LQR 设计流程

6.1 连续时间 CARE 求解 — Hamilton 矩阵法

构造 Hamilton 矩阵：

求解 \(\mathcal{H}\) 的特征值，取左半平面的特征向量构成 \(P\)。

import numpy as np
from scipy.linalg import solve_continuous_are, inv

def lqr_continuous(A, B, Q, R):
    """求解连续时间 LQR"""
    P = solve_continuous_are(A, B, Q, R)
    K = inv(R) @ B.T @ P
    return K, P

6.2 离散时间 DARE 求解 — Schur 分解法

构造 symplectic 矩阵，通过 Schur 分解求解。

import numpy as np
from scipy.linalg import solve_discrete_are

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

6.3 迭代法（适用于大规模系统）

def lqr_iterative(A, B, Q, R, max_iter=1000, tol=1e-10):
    """迭代求解离散 Riccati 方程"""
    P = Q.copy()
    for _ in range(max_iter):
        K_t = np.linalg.inv(R + B.T @ P @ B) @ B.T @ P @ A
        P_new = A.T @ (P - P @ B @ K_t) @ A + Q
        if np.linalg.norm(P_new - P) < tol:
            break
        P = P_new
    K = np.linalg.inv(R + B.T @ P @ B) @ B.T @ P @ A
    return K, P

算法选择指南

🎯 7. 关键性质与定理

LQR 系统架构

鲁棒性（增益裕度与相位裕度）

最优性条件

• 充分性：HJB 方程的解即为最优代价函数
• 必要性：在凸优化问题中，局部最优即全局最优

🚀 8. 应用场景

# 倒立摆状态: [theta, theta_dot, x, x_dot]
g, l, m = 9.81, 1.0, 0.1

A = np.array([
    [0, 1, 0, 0],
    [g/l, 0, 0, 0],
    [0, 0, 0, 1],
    [0, 0, 0, 0]
])
B = np.array([[0], [-1/(m*l)], [0], [1]])
Q = np.diag([100, 10, 10, 1])  # 重点惩罚角度偏差
R = np.array([[0.1]])

K, P = lqr_continuous(A, B, Q, R)

# 简化的三轴姿态控制
A = np.block([
    [np.zeros((3,3)), np.eye(3)],
    [np.zeros((3,3)), np.zeros((3,3))]
])
B = np.block([
    [np.zeros((3,3))],
    [np.eye(3)]
])
Q = np.diag([10, 10, 10, 1, 1, 1])  # 姿态误差 + 角速度
R = np.diag([0.01, 0.01, 0.01])     # 力矩代价

m = 1.5  # 无人机质量
A = np.array([[0, 1], [0, 0]])
B = np.array([[0], [1/m]])
Q = np.diag([10, 1])   # 高度误差权重大
R = np.array([[0.1]])  # 推力代价

m, Iz = 1500, 2500
Cf, Cr = 80000, 80000
Lf, Lr = 1.2, 1.6
Vx = 20  # 车速

A = np.array([
    [0, 1, 0, 0],
    [0, -(Cf+Cr)/(m*Vx), (Cf+Cr)/m, (Lf*Cf-Lr*Cr)/(m*Vx)],
    [0, 0, 0, 1],
    [0, -(Lf*Cf-Lr*Cr)/(Iz*Vx), (Lf*Cf+Lr*Cr)/Iz,
     -(Lf**2*Cf+Lr**2*Cr)/(Iz*Vx)]
])
B = np.array([[0], [Cf/m], [0], [Lf*Cf/Iz]])
Q = np.diag([100, 1, 100, 1])  # 横向误差和航向误差最重要
R = np.array([[10]])            # 转向角代价

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

def lqr(A, B, Q, R):
    P = solve_continuous_are(A, B, Q, R)
    K = np.linalg.inv(R) @ B.T @ P
    return K, P

# 质量-弹簧-阻尼系统
m, b, k = 1.0, 0.1, 1.0
A = np.array([[0, 1], [-k/m, -b/m]])
B = np.array([[0], [1/m]])
Q = np.diag([10, 1])
R = np.array([[0.1]])

K, P = lqr(A, B, Q, R)
print(f"K = {K}")

# 仿真
dt, T = 0.01, 10
t = np.arange(0, T, dt)
x = np.zeros((2, len(t)))
x[:, 0] = [1, 0]  # 初始位移=1

for i in range(1, len(t)):
    u = -K @ x[:, i-1]
    x[:, i] = x[:, i-1] + dt * (A @ x[:, i-1] + B.flatten() * u)

fig, axes = plt.subplots(1, 3, figsize=(14, 4))
axes[0].plot(t, x[0]); axes[0].set_title('Position'); axes[0].grid(True)
axes[1].plot(t, x[1]); axes[1].set_title('Velocity'); axes[1].grid(True)
axes[2].plot(t, -K @ x); axes[2].set_title('Control Input'); axes[2].grid(True)
plt.tight_layout()
plt.savefig('lqr_response.png', dpi=150)
plt.show()

🔥 9. 扩展与变体

9.1 LQG (Linear Quadratic Gaussian)

当系统存在过程噪声和量测噪声时：

9.2 约束 LQR / MPC

当存在输入/状态约束时，转化为二次规划 (QP) 问题：

约束条件：

9.3 加权 LQR (Weighted LQR)

跟踪期望轨迹/输入：

9.4 iLQR (迭代 LQR)

适用于非线性系统，通过在工作点线性化后迭代求解 LQR：

1. 给定初始控制序列 \(\bar{u}\)
2. 沿 \(\bar{u}\) 正向传播状态，得到 \(\bar{x}\)
3. 在 \((\bar{x}, \bar{u})\) 处线性化系统
4. 求解 LQR 得到改进的控制律
5. 重复直到收敛

9.5 离散 vs 连续选择

📊 10. 与其他控制方法对比

决策流程图

🏗️ 11. 调参指南

11.1 如何选择 Q 和 R？

11.2 权重敏感性分析

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

def lqr(A, B, Q, R):
    P = solve_continuous_are(A, B, Q, R)
    return np.linalg.inv(R) @ B.T @ P

# 系统
A = np.array([[0, 1], [-1, -0.1]])
B = np.array([[0], [1]])

fig, axes = plt.subplots(2, 2, figsize=(10, 8))
r_vals = [0.01, 0.1, 1.0, 10.0]

for ax, r in zip(axes.flat, r_vals):
    Q = np.diag([10, 1])
    K = lqr(A, B, Q, np.array([[r]]))
    dt, T = 0.01, 15
    t = np.arange(0, T, dt)
    x = np.zeros((2, len(t))); x[:, 0] = [1, 0]
    for i in range(1, len(t)):
        u = -K @ x[:, i-1]
        x[:, i] = x[:, i-1] + dt * (A @ x[:, i-1] + B.flatten() * u)
    ax.plot(t, x[0], label='Position')
    ax.plot(t, -K @ x, label='Control', alpha=0.7)
    ax.set_title(f'R = {r}, K = [{K[0,0]:.2f}, {K[0,1]:.2f}]')
    ax.legend(); ax.grid(True)
plt.tight_layout()
plt.savefig('lqr_tuning.png', dpi=150)
plt.show()

11.3 常见调参经验

❓ 12. 常见问题

12.1 LQR 无法求解？

12.2 可控性与可观性检查

def check_controllability(A, B):
    n = A.shape[0]
    ctrb = np.hstack([A**i @ B for i in range(n)])
    rank = np.linalg.matrix_rank(ctrb)
    return rank == n, rank

def check_observability(A, C):
    n = A.shape[0]
    obsv = np.vstack([C @ A**i for i in range(n)])
    rank = np.linalg.matrix_rank(obsv)
    return rank == n, rank

# 示例
A = np.array([[0, 1], [-1, -0.1]])
B = np.array([[0], [1]])
C = np.array([[1, 0]])

ctrl_ok, r1 = check_controllability(A, B)
obs_ok, r2 = check_observability(A, C)
print(f"可控: {ctrl_ok} (rank={r1}), 可观: {obs_ok} (rank={r2})")

12.3 LQR 与 PID 的关系

对于一阶系统 \(\dot{x} = -ax + bu\)，LQR 的增益 \(K = R^{-1}B^TP\) 中，当 \(Q = q, R = r\) 时，\(K = \sqrt{q/r}\)，这类似于 PID 中比例增益的效果。

12.4 最优代价的含义

表示从初始状态 \(x_0\) 出发，采用最优控制律所能达到的最小代价。可用于：
• 比较不同初始状态的控制难度
• 设计观测器时评估估计误差的影响
• 作为控制性能的量化指标

📚 13. 参考文献

1. Anderson, B. D. O., & Moore, J. B. (2007). Optimal Control: Linear Quadratic Methods. Dover Publications.
2. Lewis, F. L., Vrabie, D., & Syrmos, V. L. (2012). Optimal Control (3rd ed.). Wiley.
3. Astrom, K. J., & Murray, R. M. (2008). Feedback Systems. Princeton University Press.
4. Bertsekas, D. P. (2005). Dynamic Programming and Optimal Control (Vol. 1). Athena Scientific.