MPC模型预测控制器

针对四足机器人传统控制方法中用到的MPC模型预测进行学习。记录了相关理论及代码实现。

基本概念

最优控制

SISO单输入单输出系统，$J=\int_0^t(qe^2+ru^2)dt$

$q>>r$说明看重误差，$r>>q$说明看重输入
MIMO多输入多输出系统，$J=\int_0^x(E^TQE+U^TRU)dt$

MPC

定义：通过模型来预测系统在某一未来时间段内的表现来进行优化控制。

步骤：

估计/测量读取当前系统状态
基于$u_k,u_{k+1},…,u_{k+N}$来进行最优化控制

$J=\sum_k^{N-1}E_k^TQE_k+u_k^TRu_k+E_N^TFE_N$
只取$u_k$

预测区间（Predictive Horizon）：y-t坐标系中，对$y_k,y_{k+1},…,y_{k+N}$的预测

控制区间（Control Horizon）：u-t坐标系中，对$u_k,u_{k+1},…,u_{k+N}$的控制

滚动优化控制（Receding Horizon Control）：$u_k$之后，即$t=k+1$时，预测区间和控制区间都要移动，再次进行同样的计算，这个过程称为滚动优化控制。

MPC对控制器的计算能力要求很高。

数学建模

二次规划

一般形式：$min(z^TQz+c^Tz)$

其中Q为对角矩阵，如$Q=\left[\begin{matrix}
q_1&0&0\
0&q_2&0\
0&0&q_3\
\end{matrix}\right]$，则是最小二乘问题。

状态方程：

$x(k+1)=Ax(k)+Bu(k)$

$J=\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)$

第一项为误差加权和，第二项为输入加权和，Q和R都是对角矩阵，最后一项是终端误差。

要将上式转化为二次规划形式

令$x_k=x(k|k)$，则

$x(k+1|k)=Ax_k+Bu(k|k)$

$x(k+2|k)=Ax_{k+1}+Bu(k+1|k)=A^2x_k+ABu(k|k)+Bu(k+1|k)$

同理可得，

$x(k+N|k)=A^Nx_k+A^{N-1}Bu(k|k)+…+Bu(k+N-1|k)$

因此，
$$
\begin{align}X_k&=\left[\begin{matrix}I\
A\
A^2\
\vdots\
A^N\end{matrix}\right]x_k+
\begin{bmatrix}0&0&\cdots&0\
B&0&\cdots&0\
AB&B&\cdots&0\
{\vdots}&{\vdots}&{\ddots}&{\vdots}\
A_{N-1}B&A_{N-2}B&\cdots&B
\end{bmatrix}
\begin{bmatrix}
u(k|k)\
u(k+1|k)\
\vdots \
u(k+N-1|k)\
\end{bmatrix}\
&=Mx_k+Cu_k
\end{align}
$$

$$
\begin{align}
J&=\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&0&\cdots&0\
0&Q&\cdots&0\
\vdots&\vdots&\ddots&\vdots\
0&0&\cdots&F\
\end{bmatrix}
\begin{bmatrix}
x(k|k)\
x(k+1|k)\
\vdots\
x(k+N|k)\
\end{bmatrix}+\cdots\
&=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^T\overline{R}u_k\
&=x_k^TM^T\overline{Q}Mx_k+x_k^TM^T\overline{Q}Cu_k+u_k^TC^T\overline{Q}Mx_k+u_k^TC^T\overline{Q}Cu_k+u_k^T\overline{R}u_k\
&=x_k^TM^T\overline{Q}Mx_k+2x_k^TM^T\overline{Q}Cu_k+u_k^T(C^T\overline{Q}C+\overline{R})u_k\
&=x_k^TGx_k+2x_k^TEu_k+u_k^THu_k
\end{align}
$$

$J$是个数，是$1*1$的，所以转置等于本身。

$x_k^TGx_k$是初始状态，即刚开始测量的值，不影响最优化的部分。

$u_k^THu_k+2x_k^TEu_k$符合二次规划的一般形式。

建模示例

$$
\begin{align}
A&=\begin{bmatrix}
1&0.1\
-1&2\
\end{bmatrix}\
B&=\begin{bmatrix}
0.2&1\
0.5&2\
\end{bmatrix}
\end{align}
$$

预测区间$N=5$

详细定义见代码

clear;close all;clc;
% 定义状态空间矩阵
A=[1 0.1;-1 2];
n=size(A,1);
B=[0.2 1;0.5 2];
p=size(B,2);
%定义权重矩阵
Q=[100 0;0 1]; %n*n
F=[100 0;0 1]; %n*n
R=[1 0;0 0.1];  %p*p
%定义step的数量k
k_steps=100;
%定义状态矩阵X_K，表示系统各个时刻的状态
X_K=zeros(n,k_steps);
%定义初始状态变量
X_K(:,1)=[20;-20];
%定义输入矩阵U_K，表示系统各个时刻的输入
U_K=zeros(p,k_steps);
% 至此，状态空间方程定义完毕

%定义预测区间N
N=5;
%计算E和H
[E,H]=MPC_Matrices(A,B,Q,R,F,N);


%计算每一步的状态变量的值
for k=1:k_steps
    U_K(:,k)=Prediction(X_K(:,k),E,H,N,p);
    X_K(:,k+1)=A*X_K(:,k)+B*U_K(:,k);
end

%绘制状态变量和输入的变化
subplot(2,1,1);
hold;
for i=1:size(X_K,1)
    plot(X_K(i,:));
end

legend("x1","x2");
hold off;
subplot(2,1,2);
hold;
for i = 1:size(U_K,1)
    plot(U_K(i,:));
end
legend("u1","u2")
lgd=legend;

function [E , H]=MPC_Matrices(A,B,Q,R,F,N)
n=size(A,1);% A 是 n x n 矩阵, 得到 n
p=size(B,2);% B 是 n x p 矩阵, 得到 p

%%%%%%%%%%%%

M=[eye(n);zeros(N*n,n)]; % 初始化 M 矩阵. M 矩阵是 (N+1)n x n的， 
% 它上面是 n x n 个 "I", 这一步先把下半部
% 分写成 0 
C=zeros((N+1)*n,N*p); % 初始化 C 矩阵, 这一步令它有 (N+1)n x NP 个 0

% 定义M 和 C 

tmp=eye(n); %定义一个n x n 的 I 矩阵

%　更新Ｍ和C

for i=1:N % 循环，i 从 1到 N
 rows =i*n+(1:n); %定义当前行数，从i x n开始，共n行 
 C(rows,:)=[tmp*B,C(rows-n, 1:end-p)]; %将c矩阵填满
 tmp= A*tmp; %每一次将tmp左乘一次A
 M(rows,:)=tmp; %将M矩阵写满
end
% 定义Q_bar和R_bar

Q_bar = kron(eye(N),Q);

Q_bar = blkdiag(Q_bar,F);

R_bar = kron(eye(N),R);

% 计算G, E, H

G=M'*Q_bar*M; % G: n x n

E=M'*Q_bar*C; % E: NP x n

H=C'*Q_bar*C+R_bar; % NP x NP 
end

function u_k= Prediction(x_k,E,H,N,p)
U_k = zeros(N*p,1); % NP x 1
U_k = quadprog(H,x_k'*E);
u_k = U_k(1:p,1); % 取第一个结果
end