基于LQE状态估计的LQR与MPC控制器比较

一、核心概念与框架

1.1 关键算法关系

LQE（线性二次估计，即卡尔曼滤波）：从带噪声的测量输出中恢复系统状态，适用于状态不可直接测量的场景。
LQR（线性二次调节器）：基于全状态反馈的最优控制，通过最小化二次型代价函数（状态误差+控制输入）设计反馈增益。
MPC（模型预测控制）：基于滚动优化的预测控制，在有限时域内优化控制序列，跟踪参考轨迹。

1.2 比较框架

graph TD
    A[带噪声系统模型] --> B[LQE状态估计（卡尔曼滤波）]
    B --> C1[LQR控制器：u=-Kx̂]
    B --> C2[MPC控制器：滚动优化u]
    C1 --> D[控制效果评估]
    C2 --> D
    D --> E[性能对比（跟踪误差、控制输入、鲁棒性）]

二、系统建模与LQE状态估计

2.1 线性系统模型（含噪声）

选择二阶弹簧-质量-阻尼系统作为被控对象，状态空间模型：

（ 状 态 方 程 ， 含 过 程 噪 声 ） （ 输 出 方 程 ， 含 测 量 噪 声 ）

其中：

状态 $x = [x_1, x_2]^T$ ： $x_1$ 为位移， $x_2$ 为速度；
控制输入 $u$ ：外力；
输出 $y = x_1$ ：位移测量（带噪声）；
噪声 $w \sim N(0, Q_w)$ ， $v \sim N(0, R_v)$ （高斯白噪声）。

参数取值：

A = [1, 0.1; 0, 0.9];  % 状态转移矩阵（采样时间0.1s）
B = [0; 0.1];          % 输入矩阵
C = [1, 0];            % 输出矩阵（仅测量位移）
Q_w = diag([0.01, 0.01]);  % 过程噪声协方差
R_v = 0.1;             % 测量噪声协方差

2.2 LQE（卡尔曼滤波）实现

功能：从带噪声输出 $y(k)$ 估计状态 $x(k)$ ，分预测和更新两步。

function [x_hat, P] = kalman_filter(A, B, C, Q_w, R_v, y, u, x_hat_prev, P_prev)
    % 卡尔曼滤波：x_hat为状态估计，P为估计协方差
    % 预测步骤
    x_pred = A * x_hat_prev + B * u;
    P_pred = A * P_prev * A' + Q_w;
    
    % 更新步骤
    y_pred = C * x_pred;
    S = C * P_pred * C' + R_v;  % 新息协方差
    K = P_pred * C' / S;         % 卡尔曼增益
    x_hat = x_pred + K * (y - y_pred);  % 状态更新
    P = (eye(2) - K * C) * P_pred;     % 协方差更新
end

三、LQR控制器设计

3.1 LQR原理

LQR通过求解Riccati方程得到最优反馈增益 $K$ ，使二次型代价函数最小：
$J = \sum_{k=0}^\infty (x(k)^T Q x(k) + u(k)^T R u(k))$
其中 $Q$ 为状态权重矩阵（ penalize 状态误差）， $R$ 为控制权重矩阵（ penalize 控制输入）。

3.2 LQR代码实现

function K = lqr_gain(A, B, Q, R)
    % 求解LQR反馈增益K（使用care函数求解Riccati方程）
    [K, ~, ~] = care(A, B, Q, R);
end

参数设置：

Q_lqr = diag([10, 1]);  % 状态权重（位移误差权重10，速度误差权重1）
R_lqr = 0.1;            % 控制权重（惩罚控制输入）
K_lqr = lqr_gain(A, B, Q_lqr, R_lqr);  % 计算LQR增益

四、MPC控制器设计

4.1 MPC原理

MPC在有限预测时域 $N_p$ 内，通过模型预测输出，优化控制序列 $u(k),...,u(k+N_c-1)$ （ $N_c$ 为控制时域），最小化代价函数：
$J = \sum_{i=1}^{N_p} \|y(k+i|k) - r(k+i)\|_Q^2 + \sum_{i=0}^{N_c-1} \|u(k+i|k)\|_R^2$
其中 $r$ 为参考轨迹。

4.2 MPC代码实现

function u_mpc = mpc_control(x_hat, A, B, C, Q_mpc, R_mpc, N_p, N_c, r)
    % MPC控制器：求解控制序列u
    n = size(A,1);  % 状态维度
    m = size(B,2);  % 控制维度
    
    % 构建预测模型：Y = Phi*U + Psi*X0
    Phi = zeros(N_p, N_c);
    Psi = zeros(N_p, n);
    for i = 1:N_p
        for j = 1:min(i, N_c)
            Phi(i,j) = C * A^(i-j) * B;
        end
        Psi(i,:) = C * A^i;
    end
    
    % 参考轨迹（未来N_p步）
    Y_ref = r * ones(N_p, 1);
    
    % 二次规划：min 0.5*U'*H*U + f'*U，s.t. U_min <= U <= U_max
    H = 2 * (Phi'*Q_mpc*Phi + R_mpc*eye(N_c));
    f = 2 * Phi'*Q_mpc*(Psi*x_hat - Y_ref);
    
    % 求解（无约束时解析解，有约束时用quadprog）
    U_opt = -H\f;  % 无约束解析解
    u_mpc = U_opt(1);  % 取第一个控制量
end

参数设置：

N_p = 10;    % 预测时域（10步）
N_c = 3;     % 控制时域（3步）
Q_mpc = 1;   % 输出误差权重
R_mpc = 0.1;  % 控制权重
r = 1;       % 参考位移（目标位置）

参考代码采用模型卡尔曼滤波，也称为线性二次估计(LQE)来恢复状态。比较两个控制器：线性二次调节器(LQR)和预测控制器(MPC) www.youwenfan.com/contentcst/160583.html

五、仿真比较与结果分析

5.1 主仿真程序

%% 基于LQE的LQR与MPC控制比较
clear; clc; close all;

% 1. 系统参数
A = [1, 0.1; 0, 0.9]; B = [0; 0.1]; C = [1, 0];
Q_w = diag([0.01, 0.01]); R_v = 0.1;  % 噪声协方差
x0 = [0; 0];  % 初始状态（位移0，速度0）
T = 100;      % 仿真步数
Ts = 0.1;     % 采样时间

% 2. 控制器参数
% LQR
Q_lqr = diag([10, 1]); R_lqr = 0.1;
K_lqr = lqr_gain(A, B, Q_lqr, R_lqr);

% MPC
N_p = 10; N_c = 3; Q_mpc = 1; R_mpc = 0.1; r = 1;

% 3. 初始化
x = x0; x_hat = x0;  % 真实状态与估计状态
P = eye(2);           % 估计协方差
u_lqr = 0; u_mpc = 0;  % 控制输入
y = C*x + sqrt(R_v)*randn(1);  % 带噪声测量

% 存储历史数据
X = zeros(2, T); Y = zeros(1, T); U_lqr = zeros(1, T); U_mpc = zeros(1, T);
X_hat = zeros(2, T);

% 4. 仿真循环
for k = 1:T-1
    % 4.1 LQE状态估计（卡尔曼滤波）
    [x_hat, P] = kalman_filter(A, B, C, Q_w, R_v, y, u_lqr, x_hat, P);
    
    % 4.2 LQR控制
    u_lqr = -K_lqr * x_hat;
    u_lqr = max(min(u_lqr, 5), -5);  % 控制限幅（-5~5N）
    
    % 4.3 MPC控制
    u_mpc = mpc_control(x_hat, A, B, C, Q_mpc, R_mpc, N_p, N_c, r);
    u_mpc = max(min(u_mpc, 5), -5);  % 控制限幅
    
    % 4.4 系统演化（加噪声）
    w = sqrt(Q_w) * randn(2, 1);  % 过程噪声
    x = A*x + B*u_lqr + w;         % 用LQR控制输入（可切换为u_mpc）
    y = C*x + sqrt(R_v)*randn(1);  % 带噪声测量
    
    % 4.5 存储数据
    X(:,k) = x; Y(k) = y; U_lqr(k) = u_lqr; U_mpc(k) = u_mpc;
    X_hat(:,k) = x_hat;
end

5.2 结果可视化与对比

% 1. 状态估计与真实状态对比
t = 0:Ts:(T-1)*Ts;
figure;
subplot(2,1,1);
plot(t, X(1,:), 'b-', t, X_hat(1,:), 'r--', 'LineWidth',1.5);
xlabel('时间 (s)'); ylabel('位移 x1');
legend('真实状态', 'LQE估计'); title('位移状态估计');
grid on;

subplot(2,1,2);
plot(t, X(2,:), 'b-', t, X_hat(2,:), 'r--', 'LineWidth',1.5);
xlabel('时间 (s)'); ylabel('速度 x2');
legend('真实状态', 'LQE估计'); title('速度状态估计');
grid on;

% 2. 控制效果对比（LQR vs MPC）
figure;
plot(t, Y, 'k:', t, r*ones(size(t)), 'r--', 'LineWidth',1.5); hold on;
plot(t, X(1,:), 'b-', 'LineWidth',1.5);
xlabel('时间 (s)'); ylabel('位移');
legend('测量输出', '参考轨迹', '真实位移'); title('位移跟踪效果');
grid on;

% 3. 控制输入对比
figure;
plot(t, U_lqr, 'b-', t, U_mpc, 'r-', 'LineWidth',1.5);
xlabel('时间 (s)'); ylabel('控制输入 u');
legend('LQR', 'MPC'); title('控制输入对比');
grid on;

六、核心结论

指标	LQR	MPC
控制思想	无限时域最优状态反馈	有限时域滚动优化
状态利用	全状态反馈（需LQE估计）	基于模型预测（可只用输出）
约束处理	需手动加限幅（硬约束）	可直接嵌入优化问题（软/硬约束）
计算复杂度	低（离线计算K，在线仅矩阵乘法）	高（在线求解二次规划）
跟踪性能	平稳，无超调（若Q/R选优）	响应快，可处理时变参考（需调参）
鲁棒性	对模型误差较敏感	对模型误差更鲁棒（滚动校正）

七、关键代码文件

kalman_filter.m：LQE状态估计
lqr_gain.m：LQR反馈增益计算
mpc_control.m：MPC控制量求解
main_simulation.m：主仿真与结果对比