4. LQR控制理论核心

LQR（Linear Quadratic Regulator，线性二次型调节器）是现代控制理论中最为经典且工程应用最广泛的最优控制方法之一。在无人机轨迹跟踪场景中，LQR 的核心价值在于：它能够在系统状态偏离期望轨迹时，以“最小的控制代价”将状态拉回平衡点，同时保证系统的稳定性与动态品质。与 PID 控制相比，LQR 不再依赖人工反复试凑三个增益系数，而是通过数学优化直接求解出一组最优反馈增益矩阵 K，使得性能指标 J 最小化。本章将围绕二次型性能指标的物理含义、Riccati 方程的求解路径以及最优反馈增益 K 的工程计算方法展开，帮助学员建立从理论到代码实现的完整闭环。

4.1 二次型性能指标：量化“最优”的数学标尺

在 LQR 框架中，“最优”并非主观感受，而是通过一个积分型性能指标 J 来精确定义。对于无人机轨迹跟踪问题，我们关心两类核心代价：一是状态偏差（位置误差、速度误差、姿态角误差等）带来的“偏离代价”，二是控制输入（电机推力、舵面偏转）带来的“能量代价”。二次型性能指标将这两者统一为如下形式：

J = ∫₀^∞ ( xᵀQx + uᵀRu ) dt

其中，x 为 n 维状态向量（如 [位置误差, 速度误差, 姿态角误差, 角速度误差]），u 为 m 维控制输入向量（如 [油门增量, 俯仰舵量, 滚转舵量, 偏航舵量]）。Q 为 n×n 半正定对称矩阵，用于惩罚状态偏差；R 为 m×m 正定对称矩阵，用于惩罚控制能量消耗。Q 和 R 的选取直接决定了控制系统的“性格”：Q 中对应位置误差的元素设置较大，则无人机对位置偏差更敏感，会以更快的速度修正轨迹；R 中元素设置较大，则系统倾向于使用较小的控制量，响应变得平缓，但可能牺牲跟踪精度。

在工程实践中，Q 和 R 通常取对角矩阵，以简化调参过程。例如，对于水平位置跟踪，可设置 Q(1,1)=10、Q(2,2)=10 表示位置误差权重为 10，Q(3,3)=1、Q(4,4)=1 表示速度误差权重为 1，R(1,1)=0.1 表示控制能量权重为 0.1。这种设置意味着系统将优先消除位置误差，同时允许较大的控制输出。需要特别注意的是，Q 和 R 的数值并不重要，重要的是它们之间的相对比值。若 Q/R 比值过大，控制信号可能出现饱和或高频抖动；若比值过小，则跟踪响应迟缓。

4.2 Riccati 方程：从性能指标到最优解的桥梁

求解 LQR 问题的核心在于找到一个正定对称矩阵 P，使得性能指标 J 达到最小值。这个矩阵 P 满足代数 Riccati 方程（ARE，Algebraic Riccati Equation）：

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

其中，A 和 B 是无人机线性化状态空间模型的系统矩阵。该方程是一个关于 P 的二次矩阵方程，通常无法直接手算求解，需要借助数值工具（如 MATLAB 的 lqr() 函数、Python 的 scipy.linalg.solve_continuous_are()）。Riccati 方程的解 P 具有明确的物理意义：它代表了从当前状态 x 出发，系统在未来无限时域内所需付出的最小代价，即 V(x) = xᵀPx 是系统的值函数（Value Function）。

在无人机轨迹跟踪场景中，A 矩阵通常由无人机动力学模型的线性化得到。例如，对于悬停状态附近的四旋翼，其水平位置环的状态方程可简化为：

d/dt [δx, δvx]ᵀ = [0 1; 0 0] [δx, δvx]ᵀ + [0; 1] δθ_cmd

这里 δx 为水平位置偏差，δvx 为水平速度偏差，δθ_cmd 为俯仰角指令增量。此时 A = [[0,1],[0,0]]，B = [[0],[1]]。将 A、B、Q、R 代入 Riccati 方程，即可求得 P 矩阵，进而得到最优反馈增益 K。

4.3 最优反馈增益计算：从理论到代码的落地

一旦求得 P 矩阵，最优反馈增益 K 便可通过以下公式直接计算：

K = R⁻¹BᵀP

此时，最优控制律为 u = -Kx。这意味着控制输入是当前状态偏差的线性组合。以四旋翼水平位置环为例，若状态向量 x = [δx, δvx]ᵀ，则 K 为 1×2 的行向量 [K₁, K₂]，控制律展开为：

δθ_cmd = -K₁·δx - K₂·δvx

K₁ 相当于位置误差的比例增益，K₂ 相当于速度误差的阻尼增益。与 PID 控制不同的是，LQR 的 K₁ 和 K₂ 不是独立选取的，而是通过优化自动协调，保证在给定 Q/R 权重下达到最优的折中。在 Python 环境中，计算 LQR 增益的代码示例如下：

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.array([[10, 0],
              [0, 1]])
R = np.array([[0.1]])

# 求解 Riccati 方程
P = solve_continuous_are(A, B, Q, R)

# 计算最优反馈增益
K = np.linalg.inv(R) @ B.T @ P
print("P 矩阵：\n", P)
print("K 矩阵：", K)

运行上述代码后，可得到 K ≈ [10.0, 5.48]。这意味着位置误差增益为 10，速度误差增益为 5.48。该增益组合使得闭环系统极点位于 s = -2.74 ± 2.74j，阻尼比约为 0.707，兼顾了快速响应与超调抑制。

4.4 案例拆解：四旋翼水平位置跟踪的 LQR 调参实战

下面通过一个完整的调参案例，展示从模型建立到飞行验证的闭环过程。某四旋翼无人机在悬停状态下进行水平位置跟踪实验，期望位置为 x_des = 2m，初始位置为 x₀ = 0m，初始速度为 0m/s。控制周期为 5ms，执行机构（电机）响应带宽约为 50Hz。

第一步：建立线性化模型。通过系统辨识或机理建模，得到水平位置环的状态空间表达式：

A = [[0, 1], [0, 0]], B = [[0], [1.2]]

其中 B 矩阵第二行元素 1.2 表示俯仰角指令到水平加速度的增益（单位：m/s²/rad）。

第二步：初始权重设置。取 Q = diag(5, 1)，R = [0.2]。计算得到 K = [5.0, 3.87]。仿真结果显示：位置超调量约为 12%，调节时间约 1.8s，控制量峰值约 0.35 rad（约 20°），在安全范围内。

第三步：根据飞行测试调整权重。实际飞行中发现位置超调偏大，且到达目标点后存在轻微振荡。分析原因：Q 中位置权重 5 相对速度权重 1 不够大，导致系统对位置偏差的“紧迫感”不足。将 Q 调整为 diag(15, 1)，R 保持不变。重新计算得到 K = [8.66, 4.74]。仿真显示超调降至 4%，调节时间缩短至 1.2s，但控制量峰值升至 0.42 rad（约 24°），仍在执行机构线性范围内。

第四步：验证鲁棒性。在仿真中加入 0.5m/s 的常值风扰，观察 LQR 控制下的稳态误差。由于 LQR 本身无积分作用，存在约 0.15m 的稳态误差。为消除该误差，在 LQR 基础上增加一个积分项，形成 LQI（Linear Quadratic Integral）控制器，将状态向量扩展为 [δx, δvx, ∫δx dt]ᵀ，并重新设计权重。最终稳态误差降至 0.02m 以内。

复盘经验：LQR 调参的核心并非盲目增大 Q，而是理解 Q/R 比值对闭环极点位置的影响。增大 Q 中位置项权重，相当于将闭环极点向左半平面推移，提高响应速度，但代价是控制量增大。当执行机构接近饱和时，应优先增大 R 或降低 Q，避免非线性效应导致系统失稳。此外，LQR 的“最优”是相对于线性模型而言的，实际飞行中必须预留足够的稳定裕度。

4.5 常见误区与总结复盘

在 LQR 工程应用中，初学者常陷入以下误区：

• 误区一：认为 Q 和 R 越大越好。实际上，Q 和 R 的大小无关紧要，重要的是比值。盲目增大 Q 可能导致控制量饱和，增大 R 则使响应过于迟钝。
• 误区二：忽略模型准确性。LQR 增益完全依赖于 A、B 矩阵的精度。如果模型参数误差超过 20%，计算出的 K 可能无法保证实际系统的稳定性。建议在飞行前进行模型验证。
• 误区三：将 LQR 与 PID 割裂看待。LQR 可以视为 PID 的“智能升级版”——它自动计算出最优的比例、微分（甚至积分）增益，但前提是模型准确。当模型不确定性较大时，PID 的鲁棒性反而可能更好。
• 误区四：忽视执行机构饱和。LQR 设计时未考虑控制量限幅，若计算出的 u 超出电机或舵面的物理极限，实际控制效果将严重偏离理论预期。应在仿真中加入饱和环节进行验证。

总结而言，LQR 控制理论为无人机轨迹跟踪提供了一套系统化的增益设计方法。通过合理设置 Q 和 R 矩阵，求解 Riccati 方程得到最优反馈增益，能够在保证稳定性的前提下实现良好的跟踪性能。然而，理论最优不等于工程最优，实际调参中必须结合执行机构特性、模型不确定性和外部扰动进行综合权衡。下一阶段，我们将在此基础上引入轨迹跟踪的误差动力学模型，将 LQR 从“调节问题”扩展为“跟踪问题”，实现无人机对时变参考轨迹的精确跟随。