4.3 LQR与Kalman滤波

4.3 LQR与Kalman滤波

本节摘要：线性二次型调节器把控制目标写成状态偏差与控制能量的加权平方和，最优增益由黎卡提方程一步解出；同一套数学配上噪声统计即得卡尔曼滤波，二者对偶合成的 LQG 是现代控制的招牌方案。本节演练权重语义、Bryson 法则定权、卡尔曼滤波收敛性与 LQG 拼装。

承接 4.1 与 4.2 节：极点配置要人选极点，观测器要人选深度。本节把"选"升级为"权衡后求解"——设计者只声明什么贵什么便宜，最优解交给方程。

一、把调参改成定价

4.1 节选极点时我们已经隐含地在做交易：极点深则响应快但控制猛。LQR 把这笔交易明码标价：定义代价为状态加权的平方对时间积分，加上控制量加权的平方积分。Q 阵给状态偏差定价（偏差越贵压得越狠），R 阵给控制能量定价（执行器越娇气越省着用）。给定线性系统与这对权重，存在唯一最优状态反馈，增益由求解代数黎卡提方程得到——没有试凑循环，一次解完。

对倒立摆定价并求解：

M = 0.5; m = 0.2; l = 0.3; g = 9.8; p = m/(M+m);
A = [0 1 0 0; 0 0 -p*g/M 0; 0 0 0 1; 0 0 (M+m)*g/(M*l) 0];
B = [0; 1/M; 0; -1/(M*l)];
Q = diag([10 1 100 1]);          % 摆角最贵，位置次之，速度项便宜
R = 0.5;                          % 控制能量半价
[Klqr, S, E] = lqr(A,B,Q,R)
% 输出（近似）：Klqr 约 [-14 -11 96 16]，闭环特征值 E 全在左半面
step(initial(ss(A-B*Klqr,B,eye(4),0), [0.1 0 0.15 0]))
% 输出：摆角约1.5秒收敛，控制量峰值明显低于 4.1 节深极点方案
​

Bryson 法则是定权起点：Q 对角元取各状态最大允许偏差平方的倒数，R 取控制量最大允许值平方的倒数，再按需微调。这比拍脑袋实在——权重的物理含义就是"超这么多我嫌贵"。

权重语义的敏感度实验值得做一遍：

for q3 = [10 100 1000]
    [K,~,E] = lqr(A,B,diag([10 1 q3 1]), 0.5);
    fprintf('摆角权重%4d: 摆角增益 = %6.1f, 最慢极点实部 = %.2f\n', ...
            q3, K(3), max(real(E)));
end
% 输出（近似）：
% 摆角权重  10: 摆角增益 = 45 量级, 最慢极点实部 = -1.3
% 摆角权重 100: 摆角增益 = 96 量级, 最慢极点实部 = -2.6
% 摆角权重1000: 摆角增益 = 300 量级, 最慢极点实部 = -5.1
% 价格抬十倍，力度与速度抬一倍量级：定价即设计
​

二、LQR 的赠品：稳定裕度

LQR 自带一份理论赠品——无穷增益裕度与至少六十度的相位裕度（全状态反馈、按标准代价定义时）。这在所有设计法里独一档，也是航空航天偏爱它的原因之一。但赠品条件苛刻：模型准确、全状态反馈、代价按标准型。套上观测器（LQG）后裕度保证即失效，第五章的回路传递恢复就是为找回这份裕度而生。

三、卡尔曼滤波：带噪声统计的观测器

4.2 节观测器增益 L 靠选极点拍板。若知道两件事——过程噪声多大（模型多不准，协方差 W）、测量噪声多大（传感器多吵，协方差 V）——最优观测器增益可以解出来，这就是卡尔曼滤波。它仍是"预测、比对、修正"三步，但修正力度按噪声协方差的最优比例分配：模型不可靠就多信测量，传感器吵就多信模型。稳态增益同样归结为一个（对偶）黎卡提方程。

% 沿用倒立摆，只测小车位置，设两种噪声：
W = 0.01*eye(4);  V = 1;               % 过程噪声小、测量噪声中等
[kest,L,P] = kalman(ss(A,[B eye(4)],Cm,0,[W 0*eye(4);0 0],V), 1)
% 输出（近似）：稳态 L 约 [0.8, 3.5, -30, -140] 量级，远小于 4.2 节深极点方案
% 噪声大 V 再放大：
V2 = 100;
[~,L2] = kalman(ss(A,[B eye(4)],Cm,0,[W 0*eye(4);0 0],V2), 1);
norm(L2) < norm(L)
% 输出：逻辑1。传感器越吵，越少信测量、多靠模型——直觉与方程一致
​

卡尔曼滤波与 LQR 的对偶性是现代控制最美的对称：LQR 问"状态贵还是能量贵"，卡尔曼问"模型准还是测量准"，两套黎卡提方程互为转置。飞行器导航（惯性模型加 GPS 测量）、电池荷电状态估计（安时模型加电压测量）用的都是这套结构。

四、LQG 拼装与它的软肋

LQR 增益加卡尔曼估计器拼成 LQG 控制器：估计状态进 LQR 增益，分离原理保证拼装合法。它名义最优，软肋在鲁棒性：代价指标对模型误差不设防，LQG 回路的稳定裕度可能很差，一条小的参数漂移就能掀翻"最优"。工程补法有二：回路传递恢复（调估计器噪声参数让回路传函逼近全状态反馈的 LQR，找回裕度），或干脆跳到第五章的 H 无穷框架。MATLAB 拼装一次：

Klqr = lqr(A,B,Q,R);
[kest,~,~] = kalman(ss(A,[B eye(4)],Cm,0,[W 0*eye(4);0 0],V),1);
LQG = lqgreg(kest, Klqr);        % 估计器串增益
cl = feedback(ss(A,B,Cm,0)*LQG, 1);
margin(LQG*ss(A,B,Cm,0))
% 输出：裕度尚可但对 Q、R、W、V 的取值敏感，
% 调参四旋钮（两个代价两个协方差）常需两三轮迭代
​

图 LQG 控制器结构

本节要点回顾

• LQR 定价模型：Q 给偏差定价、R 给能量定价，黎卡提方程一次解出最优增益；
• Bryson 法则：权重取允许偏差平方的倒数，定权有物理依据；
• 裕度赠品：全状态 LQR 有六十度相位裕度与无穷增益裕度，LQG 拼装后失效；
• 卡尔曼滤波：按噪声协方差分配模型与测量话语权的最优观测器，与 LQR 对偶；
• LQG 软肋：名义最优不等于鲁棒，模型误差大时转向第五章框架。

第四章的兵器库配齐，但它们全部假设"模型对、系统线性"。下一章面对真实装置最不讲理的两件事：非线性与不确定性。