带有终端约束的模型预测控制 matlab代码

以下是一个简单的带有终端约束的模型预测控制（MPC）的 Matlab 代码示例。此示例使用了 Quadprog 函数进行优化。

首先，我们需要定义系统的动态模型和约束条件。在此示例中，我们使用一个简单的二次型系统：

% System model
A = [1.1, 0.4; 0.2, 1.3];
B = [1; 0.5];
C = [1, 0];
D = 0;

% Constraints
u_min = -1;
u_max = 1;
y_min = -1;
y_max = 1;

接下来，我们定义 MPC 控制器的参数。在此示例中，我们使用一个预测时间步长为 5，控制时间步长为 1，预测模型为系统模型的离散时间版本：

% MPC parameters
N = 5; % Prediction horizon
T = 1; % Control horizon
Q = eye(2); % State cost
R = 1; % Control cost
x0 = [0; 0]; % Initial state

现在我们可以开始实现 MPC 控制器。首先，我们定义一个状态向量 `x`，并将其初始化为初始状态 `x0`。然后，我们进入一个循环，在每个时间步骤中求解一个优化问题，以计算下一个时间步骤的最优控制输入 `u`。优化问题的目标是最小化预测误差的平方和，同时满足约束条件。

% MPC control loop
x = x0;
for t = 1:T
    % Build prediction model
    [Ad, Bd] = c2d(A, B, t);
    [x_min, x_max, u_min_t, u_max_t] = compute_constraints(x, u_min, u_max, y_min, y_max, C, D, Ad, Bd);
    [H, f, Aeq, beq, lb, ub] = build_qp_matrices(Ad, Bd, N, Q, R, x, x_min, x_max, u_min_t, u_max_t);
    
    % Solve QP problem
    options = optimoptions('quadprog', 'Display', 'off');
    u = quadprog(H, f, [], [], Aeq, beq, lb, ub, [], options);
    u = u(1);
    
    % Update state
    x = Ad*x + Bd*u;
    
    % Print results
    fprintf('Time step %d: u = %f, x = [%f, %f]\n', t, u, x(1), x(2));
end

在每个时间步骤中，我们需要重新计算约束条件。由于终端约束是动态的，因此需要在每个时间步骤中重新计算。我们定义了一个名为 `compute_constraints` 的辅助函数来执行此操作。

function [x_min, x_max, u_min_t, u_max_t] = compute_constraints(x, u_min, u_max, y_min, y_max, C, D, Ad, Bd)
    x_min = [y_min - C*x; -inf];
    x_max = [y_max - C*x; inf];
    u_min_t = u_min - D*x;
    u_max_t = u_max - D*x;
    for i = 2:size(Ad, 1)
        x_min = [x_min; y_min - C*Ad^(i-1)*x - C*Ad^(i-2)*Bd*u_min_t];
        x_max = [x_max; y_max - C*Ad^(i-1)*x - C*Ad^(i-2)*Bd*u_max_t];
        u_min_t = [u_min_t; u_min - D*Ad^(i-2)*Bd*u_min_t];
        u_max_t = [u_max_t; u_max - D*Ad^(i-2)*Bd*u_max_t];
    end
end

最后，我们定义一个名为 `build_qp_matrices` 的辅助函数来构建优化问题的矩阵形式。

function [H, f, Aeq, beq, lb, ub] = build_qp_matrices(Ad, Bd, N, Q, R, x, x_min, x_max, u_min, u_max)
    n = size(Ad, 1);
    m = size(Bd, 2);
    
    % Build cost matrices
    Q_bar = kron(eye(N), Q);
    R_bar = kron(eye(N), R);
    H = Bd'*kron(eye(N+1), Ad)*Q_bar*kron(eye(N+1), Ad')*Bd + R_bar;
    f = 2*Bd'*kron(ones(N+1, 1), Ad')*Q_bar*(Ad*x) + 2*R_bar*kron(ones(N, 1), u_min);
    
    % Build constraint matrices
    Aeq = [kron(eye(N), -Bd), kron(eye(N+1), Ad) - eye(n*(N+1))];
    beq = [-Ad*x; zeros(n*N, 1)];
    lb = [kron(ones(N, 1), u_min); u_min];
    ub = [kron(ones(N, 1), u_max); u_max];
    for i = 1:N+1
        lb = [lb; x_min];
        ub = [ub; x_max];
    end
end

这就完成了一个简单的带有终端约束的 MPC 控制器的 Matlab 代码示例。