用MATLAB编写一个利用线性二次型最优控制方法求解一个数值模型的程序

这里给出一个简单的例子，假设我们要求解如下的最优控制问题：

$$\min_{u}\int_{0}^{T}\left(x^{\top}(t)Qx(t)+u^{\top}(t)Ru(t)\right)dt$$
$$\text{s.t. }\dot{x}(t)=Ax(t)+Bu(t),\ x(0)=x_{0},\ x(T)=x_{T}$$

其中 $x(t)\in\mathbb{R}^{n}$ 是状态向量，$u(t)\in\mathbb{R}^{m}$ 是控制向量，$Q\in\mathbb{R}^{n\times n},\ R\in\mathbb{R}^{m\times m}$ 是正定对称矩阵，$A\in\mathbb{R}^{n\times n},\ B\in\mathbb{R}^{n\times m}$ 是系统矩阵。我们可以通过求解下面的二次型最优控制问题来得到最优控制输入 $u(t)$：

$$\min_{u}\frac{1}{2}\left\Vert Px+Qu\right\Vert^{2}+J$$
$$\text{s.t. }u_{\text{min}}\leq u(t)\leq u_{\text{max}}$$

其中 $P\in\mathbb{R}^{n\times n},\ Q\in\mathbb{R}^{m\times n}$ 是权重矩阵，$J$ 是常数，$u_{\text{min}},\ u_{\text{max}}$ 分别是控制输入 $u(t)$ 的下界和上界。根据最优控制理论，可以求得 $P$ 和 $Q$：

$$P=S(t),\ Q=2\int_{t}^{T}S(\tau)B(\tau)R^{-1}B^{\top}(\tau)S(t)d\tau$$

其中 $S(t)$ 是时刻 $t$ 时的状态向量 $x(t)$ 的李雅普诺夫方程的解：

$$\dot{S}(t)=A^{\top}S(t)+S(t)A-S(t)B(R+B^{\top}S(t)B)^{-1}B^{\top}S(t)A,\ S(T)=0$$

接下来，我们可以用 MATLAB 编写一个求解上述最优控制问题的程序：

% 系统参数
T = 1; % 时间终点
n = 2; % 状态变量个数
m = 1; % 控制变量个数
A = [0 1; -1 -1]; % 系统矩阵
B = [0; 1]; % 控制矩阵
x0 = [1; 0]; % 初始状态
xT = [0; 0]; % 终止状态
Q = eye(n); % 状态权重矩阵
R = 1; % 控制权重矩阵
umin = -Inf; % 控制下界
umax = Inf; % 控制上界

% 求解最优控制问题
tspan = [0 T];
S0 = zeros(n, n);
[~, S] = ode45(@(t, S) lyapunovEqn(t, S, A, B, R), fliplr(tspan), S0);
P = S(end, :)';
Q = 2 * quad(@(t) S_(t, S, B, R, T), 0, T)';
[uopt, Jopt] = quadprog(Q, P'*x0, [], [], [], [], umin, umax);

% 绘制结果
t = linspace(0, T, 100);
x = zeros(n, length(t));
x(:, 1) = x0;
for i = 2:length(t)
    x(:, i) = expm(A*(t(i)-t(i-1)))*x(:, i-1) + B*uopt;
end
figure;
plot(t, x(1, :), t, x(2, :), 'LineWidth', 2);
xlabel('Time');
ylabel('State');
legend('x_1', 'x_2', 'Location', 'best');

function dSdt = lyapunovEqn(t, S, A, B, R)
    S = reshape(S, size(A));
    dSdt = A'*S + S*A - S*B*(R+B'*S*B)^(-1)*B'*S*A;
    dSdt = dSdt(:);
end

function S = S_(t, S, B, R, T)
    S = reshape(S, size(B, 1), size(B, 1));
    S = S * B * inv(R) * B' * S;
end

该程序先求解李雅普诺夫方程，然后用 quadprog 函数求解二次型最优控制问题，最后用 expm 函数求解状态向量 $x(t)$，并将结果绘制出来。注意，这里的 S(t) 矩阵是按照时间从 $T$ 到 $0$ 求解的，因此在求解时需要将时间反转，然后在得到结果后再将其反转回来。