模型预测控制(MPC)的简单实现 — Matlab

Ay_wanglaoda

已于 2023-01-18 23:37:56 修改

阅读量3k

点赞数 7

分类专栏：控制算法文章标签： matlab

于 2022-12-06 23:16:15 首次发布

本文链接：https://blog.csdn.net/ay_wanglaoda/article/details/128210372

版权

控制算法专栏收录该内容

3 篇文章 4 订阅

订阅专栏

参考文章：原理推导

基于离散模型，首先复述其首先原理，然后基于简单的无人机模型进行仿真。

原理

问题描述

考虑离散系统：
${x_{k + 1}} = A{x_k} + B{u_k}$

在 $k$ 时刻，预测 $N$ 个周期的状态 $X_k$ 以及对应的控制输入 $U_k$ 可以表示为：
$X_k=\begin{bmatrix} x(k|k) \\ x(k+1|k) \\ x(k+2|k) \\ \vdots\\ x(k+N|k) \end{bmatrix}, U_k=\begin{bmatrix} u(k|k) \\ u(k+1|k) \\ u(k+2|k) \\ \vdots\\ u(k+N|k) \end{bmatrix}$

假设跟踪目标为0，那么代价函数可以表示为：
$\begin{aligned}J&=\displaystyle\sum_{k}^{N-1}(X_k^TQX_k+u_k^TRu_k)+X_N^TFX_N \\ &=\displaystyle\sum_{i=0}^{N-1}(x(k+i|k)^TQx(k+i|k)+u(k+i|k)^TRu(k+i|k))+x(k+N)^TFx(k+N)\end{aligned}$

通过计算 $U_k$ 来最小化代价函数 $J$ ，那么 $u (k ∣ k)$ 即为当前时刻的最优输出，那么怎么计算 $U_k$ 就成了问题的关键。

计算过程

由于设计控制器目标为最优化控制输入 $u$ ，所以需要消除状态 $x$ ，于是将 $X_k$ 展开：
$\begin{aligned} x(k|k)&=x_k \\ x(k+1|k)&=Ax(k|k)+Bu(k|k) \\ x(k+2|k)&=Ax(k+1|k)+Bu(k+1|k)=A^2x(k|k)+ABu(k|k)+Bu(k+1|k) \\ \vdots \\ x(k+N|k)&=A^Nx(k|k)+A^{N-1}Bu(k|k)+\cdots+Bu(k+N-1|k) \end{aligned}$

将展开后的方程组用矩阵形式表示：
$\begin{aligned}X_k&=\begin{bmatrix} I \\ A \\ A^2 \\ \vdots \\ A^N\end{bmatrix}x_k+\begin{bmatrix} O \\ B \\ AB & B \\ \vdots & \vdots & \ddots \\ A^{N-1}B & A^{N-2}B & \cdots & B \end{bmatrix}U_k \\ &=Mx_k+CU_k \end{aligned}$

将代价函数 $J$ 展开，可以化为：
$\begin{aligned}J&=\displaystyle\sum_{i=0}^{N-1}(x(k+i|k)^TQx(k+i|k)+u(k+i|k)^TRu(k+i|k))+x(k+N)^TFx(k+N) \\ &=\begin{bmatrix} x(k|k) \\ x(k+1|k) \\ \vdots\\ x(k+N|k) \end{bmatrix}^T\begin{bmatrix} Q \\ & Q \\ & & \ddots \\ & & & F \end{bmatrix}\begin{bmatrix} x(k|k) \\ x(k+1|k) \\ \vdots\\ x(k+N|k) \end{bmatrix}+\begin{bmatrix}u(k|k) \\ u(k+1|k) \\ \vdots\\ u(k+N-1|k) \end{bmatrix}^T\begin{bmatrix} R \\ & R \\ & & \ddots \\ & & & R \end{bmatrix}\begin{bmatrix} u(k|k) \\ u(k+1|k) \\ \vdots\\ u(k+N-1|k) \end{bmatrix} \\ &= X_k^T\overline{Q}X_k+U_k^T\overline{R}U_k \end{aligned}$

然后将上述的矩阵形式代入到代价函数中， $J$ 又可以简化为：
$\begin{aligned}J &= X_k^T\overline{Q}X_k+U_k^T\overline{R}U_k \\ &=(Mx_k+CU_k)^T\overline {Q}(Mx_k+CU_k)+U_k^R\overline{R}U_k \\ &=x_k^T{M^T}\bar QM{x_k} + x_k^T{M^T}\bar QC{U_k} + U_k^T{C^T}\bar QM{x_k} + U_k^T{C^T}\bar QC{U_k} + U_k^T\bar R{U_k} \\ &=x_k^T{M^T}\bar QM{x_k} + 2x_k^T{M^T}\bar QC{U_k} + U_k^T({C^T}\bar QC + \bar R){U_k} \\ &=x_k^TG{x_k} + 2x_k^TE{U_k} + U_k^TH{U_k} \end{aligned}$

最终，代价函数可以表示为：
$\begin{gathered}J = x_k^TG{x_k} + 2x_k^TE{U_k} + U_k^TH{U_k} \\ G = {M^T}\bar QM \\E = {M^T}\bar QC \\H = {C^T}\bar QC + \bar R \end{gathered}$

然后通过最小化成本函数得到最优解。最简单的方法是使用二次规划(Quadratic Programming, QP)，可以直接调用 Matlab 中的 quadprog() 函数。

Matlab 仿真

模型

下面的方程是无人机平移运动的状态空间方程。以该系统为控制对象。
$\begin{bmatrix} \dot x \\ \dot v_{xs} \\ \dot v_x \\ \dot a_x \end{bmatrix}=\begin{bmatrix} 0 & 0 & 1 & 0 \\ 0 & -\cfrac{1}{T_d} & \cfrac{1}{T_d} & -1 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & a_{1x} & a_{2x} \end{bmatrix}\begin{bmatrix} x \\ v_{xs} \\ v_x \\ a_x \end{bmatrix}+\begin{bmatrix} 0 \\ 0 \\ 0 \\ b_{1x} \end{bmatrix}{\theta _{ref}} \\ \begin{bmatrix} \dot y \\ \dot v_{ys} \\ \dot v_y \\ \dot a_y \end{bmatrix}=\begin{bmatrix} 0 & 0 & 1 & 0 \\ 0 & -\cfrac{1}{T_d} & \cfrac{1}{T_d} & -1 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & a_{1y} & a_{2y} \end{bmatrix}\begin{bmatrix} y \\ v_{ys} \\ v_y \\ a_y \end{bmatrix}+\begin{bmatrix} 0 \\ 0 \\ 0 \\ b_{1y} \end{bmatrix}{\varphi_{ref}}$

其中， $a_{1x}= a_{1y}=-1.4$ ， $a_{2x}= a_{2y}=-7.2$ ， $b_{1x}= b_{1y}=0.1177$ ， $T_{d}=0.08$ 。要求在 $5 s$ 以内收敛于原点，无人机位置的初始位置为 $(5 m, 4 m)$ 。

仿真

主函数

clear;clc;
close all;

%% UAV Model
Sim_Time = 10;
Ts = 0.01;
[Ad, Bd] = UAV_Model(Ts);

%% Initialize
Predict_Step = 20;
Q = diag([200, 30, 20, 1]); % Elements depends on states
F = diag([200, 30, 20, 1]); % Elements depends on states
R = diag([0.0002]);         % Elements depends on input

X = [5, 0, 0, 0]';
Y = [4, 0, 0, 0]';

[G, E, H] = MPC_Matrices(Ad, Bd, Q, R, F, Predict_Step);    % Call MPC_Matrices

%% Start Simulate
for i = 1:1:Sim_Time/Ts
    t(i) = i*Ts;
    
    pitch_ref = Prediction(X, Y, G, E, H);
    roll_ref = Prediction(Y, X, G, E, H);
    
    % State update under the input
    X = Ad*X + Bd*pitch_ref;
    Y = Ad*Y + Bd*roll_ref;
    
    % Data Record
    X_Ap(i,:) = X;
    Y_Ap(i,:) = Y;
    Ref_pitch_Ap(i) = pitch_ref;
    Ref_roll_Ap(i) = roll_ref;
end

figure('Name','State and Reference','NumberTitle','off')
% X Direction
subplot(221)
plot(t, X_Ap(:,1),'r',t, X_Ap(:,2),t, X_Ap(:,4),'linewidth',1.5);grid;
legend('Pos','Vel','Acc');
legend('boxoff');
xlabel('Time(s)');ylabel('States');title('(a) X Direction');
subplot(222)
plot(t, Ref_pitch_Ap,'linewidth',1.5);grid;
xlabel('Time(s)');ylabel('Att(rad)');title('(b) Ref Pitch');
% Y Direction
subplot(223)
plot(t, Y_Ap(:,1),'r',t, Y_Ap(:,2),t, Y_Ap(:,4),'linewidth',1.5);grid;
legend('Pos','Vel','Acc');
legend('boxoff');
xlabel('Time(s)');ylabel('States');title('(a) Y Direction');
subplot(224)
plot(t, Ref_roll_Ap,'linewidth',1.5);grid;
xlabel('Time(s)');ylabel('Att(rad)');title('(b) Ref Roll');

% Dynamic Plot
figure('Name','Dynamic Plot','NumberTitle','off')
for i = 1:1:Sim_Time/Ts
    drawnow;
    t(i) = i*Ts;
    
    x_pos(i) = X_Ap(i,1);
    y_pos(i) = Y_Ap(i,1);
    plot(x_pos,y_pos,'b',x_pos(i),y_pos(i),'*');
    text(-3,3,['Time:  ',num2str(t(i))],'FontSize',13);
    axis([-5,5,-5,5]);
    grid on;
    pause(0.001);
end
% End Dynamic Plot

UAV_Model.m

%{
    UAV model include position, velocity, delayed-velocity and acceleration
in X-axis and Y-axis. State = [Pos, Vel-delay, Vel, Acc];
%}

function [Ad, Bd] = UAV_Model(Ts)

Td = 0.08;
a1 = -1.4;
a2 = -7.2;
b1 = 0.1177;

A = [0     0    1    0;
     0 -1/Td 1/Td   -1;
     0     0    0    1;
     0     0   a1   a2];

B = [0     0    0   b1]';

C = [1     0    0    0];
D = 0;

[Ad, Bd, Cd, Dd] = c2dm(A, B, C, D, Ts);

end

MPC_Matrices.m

function [G, E, H] = MPC_Matrices(A, B, Q, R, F, N)
% N is predict length
n = size(A, 1);
p = size(B, 2);

M = [        eye(n);     
      zeros(N*n, n)];
C = zeros((N + 1)*n, N*p); 

% Caculate M and C
tmp = eye(n);
for i = 1 : N
    rows = i*n + (1 : n); % the current row
    C(rows, :) = [tmp*B, C(rows - n, 1 : end - p)];
    tmp = A*tmp;
    M(rows,:) = tmp;
end

% Caculate Q_bar and R_bar
Q_bar = blkdiag(kron(eye(N), Q), F);
R_bar = kron(eye(N), R);

% Caculate G, E, H
G = M'*Q_bar*M;
E = M'*Q_bar*C; 
H = C'*Q_bar*C+R_bar;

end

Prediction.m

%{
    Numerical solution of MPC: Quadratic Programming.
    *x_k: Current axis state
    *y_k: Another axis state
    *G
    *E
    *H
%}
function u_k = Prediction(x_k, y_k, G, E, H)

U_k = quadprog(H, 2*E'*x_k);  % E'*x_k is to satisfied quadprog form
u_k = U_k(1, 1);   % Select the first action as output

end