2. 线性二次型调节器(LQR)：从理论到制导实战

各位同学好，今天我们聊聊LQR。说实话，我在刚入行那会儿，觉得LQR就是个数学玩具——矩阵套矩阵，看着就头疼。直到有一次做导弹末端制导律设计，传统比例导引在特定场景下抖得厉害，我才真正体会到LQR的价值。嗯，今天我们就把它掰开揉碎了讲清楚。

2.1 连续时间LQR的数学推导

LQR要解决什么问题？说白了就是：给定一个线性系统，怎么设计反馈控制律，让状态尽快归零，同时控制量别太大。你想想看，这不就是制导问题的核心吗？

考虑连续时间线性系统：

ẋ(t) = A x(t) + B u(t)

我们要最小化的性能指标是：

J = ∫₀^∞ [ xᵀQx + uᵀRu ] dt

这里Q是半正定矩阵，R是正定矩阵。Q惩罚状态偏差，R惩罚控制能量。我个人习惯把Q取成对角阵，这样每个状态的权重一目了然。

根据最优控制理论，最优控制律具有如下形式：

u*(t) = -R⁻¹BᵀP x(t) = -K x(t)

其中P是下面这个代数Riccati方程(ARE)的解：

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

核心要点：LQR的本质就是通过求解ARE，把最优控制问题转化为一个静态矩阵方程。一旦P解出来，反馈增益K就确定了，控制律也就定了。

2.2 代数Riccati方程求解

ARE怎么解？我在项目中用过三种方法，各有千秋。

2.2.1 方法一：直接调用Python函数

这是最省事的办法。SciPy里封装好了求解器：

import numpy as np
from scipy.linalg import solve_continuous_are

# 定义系统矩阵
A = np.array([[0, 1], [0, 0]])
B = np.array([[0], [1]])
Q = np.diag([10, 1])
R = np.array([[1]])

# 求解ARE
P = solve_continuous_are(A, B, Q, R)

# 计算反馈增益
K = np.linalg.inv(R) @ B.T @ P

print("Riccati解 P:\n", P)
print("最优增益 K:\n", K)

避坑指南：我曾经遇到过Q矩阵选得太大，导致Riccati方程数值病态的问题。建议Q的对角元素不要超过R的100倍，否则控制量会剧烈震荡。

2.2.2 方法二：Schur分解法（自己实现）

如果你想深入理解求解过程，可以试试Schur分解。核心思路是把ARE转化为一个2n×2n的Hamiltonian矩阵的特征值问题：

H = [[A, -BR⁻¹Bᵀ], [-Q, -Aᵀ]]

然后对H做Schur分解，提取稳定特征子空间对应的解。代码实现如下：

def solve_are_schur(A, B, Q, R):
    n = A.shape[0]
    R_inv = np.linalg.inv(R)
    
    # 构造Hamiltonian矩阵
    H = np.block([
        [A, -B @ R_inv @ B.T],
        [-Q, -A.T]
    ])
    
    # Schur分解
    from scipy.linalg import schur
    T, Z = schur(H, output='real')
    
    # 提取稳定特征子空间
    # 实部小于0的特征值对应稳定子空间
    n = n
    Z11 = Z[:n, :n]
    Z21 = Z[n:, :n]
    
    P = Z21 @ np.linalg.inv(Z11)
    return P.real

注意：Schur分解法对数值精度要求高。如果系统矩阵A有接近虚轴的特征值，解可能不稳定。我建议先用np.linalg.eigvals(H)检查一下特征值分布。

2.2.3 方法三：迭代法（适合嵌入式实现）

在飞控计算机上，资源有限，没法调SciPy。这时候可以用迭代法：

def solve_are_iterative(A, B, Q, R, max_iter=1000, tol=1e-10):
    n = A.shape[0]
    P = Q.copy()  # 初始猜测
    
    for i in range(max_iter):
        P_new = Q + A.T @ P @ A - A.T @ P @ B @ np.linalg.inv(R + B.T @ P @ B) @ B.T @ P @ A
        
        if np.max(np.abs(P_new - P)) < tol:
            print(f"迭代收敛，共{i+1}步")
            return P_new
        P = P_new
    
    raise RuntimeError("ARE迭代未收敛")

经验之谈：迭代法收敛速度取决于初始猜测。我一般用Q作为初始值，如果系统是稳定的，通常20步以内就能收敛。

2.3 LQR在制导中的应用

好了，理论讲完了，我们看看LQR在制导中怎么用。我做过一个项目——拦截弹的末端制导律设计，这里分享给大家。

2.3.1 问题建模

考虑二维拦截场景，相对运动方程为：

状态向量: x = [r, θ, ṙ, rθ̇]ᵀ
控制输入: u = [a_T, a_M]ᵀ  (目标加速度、导弹加速度)

线性化后的状态空间模型：

A = [[0, 0, 1, 0],
     [0, 0, 0, 1],
     [0, 0, 0, 0],
     [0, 0, 0, 0]]

B = [[0, 0],
     [0, 0],
     [0, -1],
     [1, 0]]

2.3.2 权重矩阵设计

这里有个技巧。Q矩阵的选取直接影响脱靶量和控制能量：

状态变量物理意义推荐Q权重说明
r相对距离1距离误差权重适中
θ视线角10-100视线角偏差要严格约束
ṙ径向速度0.1速度误差权重可小
rθ̇横向速度1-10横向速度影响脱靶量

关键经验：我曾在一次仿真中发现，视线角权重取100时，导弹过载指令在末端会剧烈饱和。后来把权重降到20，配合限幅器，效果就好多了。记住：权重不是越大越好。

2.3.3 完整仿真代码

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

# 系统参数
A = np.array([[0, 0, 1, 0],
              [0, 0, 0, 1],
              [0, 0, 0, 0],
              [0, 0, 0, 0]])

B = np.array([[0, 0],
              [0, 0],
              [0, -1],
              [1, 0]])

# 权重矩阵
Q = np.diag([1, 20, 0.1, 5])
R = np.diag([0.1, 1])  # 目标加速度权重小，导弹加速度权重大

# 求解LQR
P = solve_continuous_are(A, B, Q, R)
K = np.linalg.inv(R) @ B.T @ P

print("LQR增益矩阵 K:\n", K)

# 仿真参数
dt = 0.001
t_end = 5.0
t = np.arange(0, t_end, dt)

# 初始状态 [r, θ, ṙ, rθ̇]
x = np.array([5000, 0.1, -300, 50])
x_history = []

for i in range(len(t)):
    x_history.append(x.copy())
    
    # LQR控制律
    u = -K @ x
    
    # 状态更新（欧拉法）
    x_dot = A @ x + B @ u
    x = x + x_dot * dt

x_history = np.array(x_history)

# 绘制结果
plt.figure(figsize=(12, 8))
plt.subplot(2,2,1)
plt.plot(t, x_history[:,0])
plt.xlabel('时间(s)')
plt.ylabel('相对距离(m)')
plt.grid(True)

plt.subplot(2,2,2)
plt.plot(t, x_history[:,1]*180/np.pi)
plt.xlabel('时间(s)')
plt.ylabel('视线角(deg)')
plt.grid(True)

plt.subplot(2,2,3)
plt.plot(t, x_history[:,2])
plt.xlabel('时间(s)')
plt.ylabel('径向速度(m/s)')
plt.grid(True)

plt.subplot(2,2,4)
plt.plot(t, x_history[:,3])
plt.xlabel('时间(s)')
plt.ylabel('横向速度(m/s)')
plt.grid(True)

plt.tight_layout()
plt.show()

2.4 LQR制导的核心逻辑图

下面这张图是我自己总结的LQR制导设计流程，画成SVG方便大家理解：

LQR制导律设计流程 步骤1：系统建模 建立相对运动方程 步骤2：线性化 得到A、B矩阵 步骤3：权重设计 选取Q、R矩阵 步骤4：求解ARE 得到P矩阵 步骤5：计算增益 K = R⁻¹BᵀP 步骤6：仿真验证 检查脱靶量和过载 不满足要求时调整权重 核心：通过调整Q、R权重，在脱靶量和控制能量之间取得平衡 状态权重Q 控制权重R Riccati方程求解

2.5 工程实践中的注意事项

最后，我把自己踩过的坑总结一下：

  1. 数值稳定性：求解ARE时，如果矩阵条件数太大，解可能不准确。我建议先用np.linalg.cond检查一下。
  2. 权重调参：别指望一次调好。我一般先用Bryson法则确定初始值，再手动微调。
  3. 执行器饱和：LQR算出来的控制量可能很大，一定要加限幅器。我在项目中吃过这个亏，仿真时好好的，一上硬件就炸了。
  4. 离散化问题：如果要在数字计算机上实现，记得把连续LQR离散化。可以用scipy.signal.cont2discrete。

重要提醒：LQR假设系统是线性的，但实际制导问题往往是非线性的。我建议在平衡点附近线性化，或者配合增益调度使用。千万别直接套用，否则会出大问题。

好了，关于LQR的内容就讲到这里。代码都给你们了，回去跑一跑，改改权重参数，看看效果变化。有什么问题随时交流。

公众号:蓝海资料掘金营，微信deep3321

← 上一章 📖 返回目录 下一章 →