原理

问题描述

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

在 kkk 时刻，预测 NNN 个周期的状态 XkX_kXk 以及对应的控制输入 UkU_kUk可以表示为：
Xk=[x(k∣k)x(k+1∣k)x(k+2∣k)⋮x(k+N∣k)],Uk=[u(k∣k)u(k+1∣k)u(k+2∣k)⋮u(k+N∣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}Xk=x(k∣k)x(k+1∣k)x(k+2∣k)⋮x(k+N∣k),Uk=u(k∣k)u(k+1∣k)u(k+2∣k)⋮u(k+N∣k)

假设跟踪目标为0，那么代价函数可以表示为：
J=∑kN−1(XkTQXk+ukTRuk)+XNTFXN=∑i=0N−1(x(k+i∣k)TQx(k+i∣k)+u(k+i∣k)TRu(k+i∣k))+x(k+N)TFx(k+N)\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}J=k∑N−1(XkTQXk+ukTRuk)+XNTFXN=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)

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

计算过程

由于设计控制器目标为最优化控制输入 uuu，所以需要消除状态 xxx，于是将 XkX_kXk 展开：
x(k∣k)=xkx(k+1∣k)=Ax(k∣k)+Bu(k∣k)x(k+2∣k)=Ax(k+1∣k)+Bu(k+1∣k)=A2x(k∣k)+ABu(k∣k)+Bu(k+1∣k)⋮x(k+N∣k)=ANx(k∣k)+AN−1Bu(k∣k)+⋯+Bu(k+N−1∣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}x(k∣k)x(k+1∣k)x(k+2∣k)⋮x(k+N∣k)=xk=Ax(k∣k)+Bu(k∣k)=Ax(k+1∣k)+Bu(k+1∣k)=A2x(k∣k)+ABu(k∣k)+Bu(k+1∣k)=ANx(k∣k)+AN−1Bu(k∣k)+⋯+Bu(k+N−1∣k)

将展开后的方程组用矩阵形式表示：
Xk=[IAA2⋮AN]xk+[OBABB⋮⋮⋱AN−1BAN−2B⋯B]Uk=Mxk+CUk\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}Xk=IAA2⋮ANxk+OBAB⋮AN−1BB⋮AN−2B⋱⋯BUk=Mxk+CUk

将代价函数 JJJ 展开，可以化为：
J=∑i=0N−1(x(k+i∣k)TQx(k+i∣k)+u(k+i∣k)TRu(k+i∣k))+x(k+N)TFx(k+N)=[x(k∣k)x(k+1∣k)⋮x(k+N∣k)]T[QQ⋱F][x(k∣k)x(k+1∣k)⋮x(k+N∣k)]+[u(k∣k)u(k+1∣k)⋮u(k+N−1∣k)]T[RR⋱R][u(k∣k)u(k+1∣k)⋮u(k+N−1∣k)]=XkTQ‾Xk+UkTR‾Uk\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=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)=x(k∣k)x(k+1∣k)⋮x(k+N∣k)TQQ⋱Fx(k∣k)x(k+1∣k)⋮x(k+N∣k)+u(k∣k)u(k+1∣k)⋮u(k+N−1∣k)TRR⋱Ru(k∣k)u(k+1∣k)⋮u(k+N−1∣k)=XkTQXk+UkTRUk

然后将上述的矩阵形式代入到代价函数中，JJJ 又可以简化为：
J=XkTQ‾Xk+UkTR‾Uk=(Mxk+CUk)TQ‾(Mxk+CUk)+UkRR‾Uk=xkTMTQˉMxk+xkTMTQˉCUk+UkTCTQˉMxk+UkTCTQˉCUk+UkTRˉUk=xkTMTQˉMxk+2xkTMTQˉCUk+UkT(CTQˉC+Rˉ)Uk=xkTGxk+2xkTEUk+UkTHUk\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}J=XkTQXk+UkTRUk=(Mxk+CUk)TQ(Mxk+CUk)+UkRRUk=xkTMTQˉMxk+xkTMTQˉCUk+UkTCTQˉMxk+UkTCTQˉCUk+UkTRˉUk=xkTMTQˉMxk+2xkTMTQˉCUk+UkT(CTQˉC+Rˉ)Uk=xkTGxk+2xkTEUk+UkTHUk

最终，代价函数可以表示为：
J=xkTGxk+2xkTEUk+UkTHUkG=MTQˉME=MTQˉCH=CTQˉC+Rˉ\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} J=xkTGxk+2xkTEUk+UkTHUkG=MTQˉME=MTQˉCH=CTQˉC+Rˉ

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

Matlab 仿真

模型

下面的方程是无人机平移运动的状态空间方程。以该系统为控制对象。
[x˙v˙xsv˙xa˙x]=[00100−1Td1Td−1000100a1xa2x][xvxsvxax]+[000b1x]θref[y˙v˙ysv˙ya˙y]=[00100−1Td1Td−1000100a1ya2y][yvysvyay]+[000b1y]φref\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}}x˙v˙xsv˙xa˙x=00000−Td1001Td10a1x0−11a2xxvxsvxax+000b1xθrefy˙v˙ysv˙ya˙y=00000−Td1001Td10a1y0−11a2yyvysvyay+000b1yφref

其中，a1x=a1y=−1.4a_{1x}= a_{1y}=-1.4a1x=a1y=−1.4，a2x=a2y=−7.2a_{2x}= a_{2y}=-7.2a2x=a2y=−7.2，b1x=b1y=0.1177b_{1x}= b_{1y}=0.1177b1x=b1y=0.1177，Td=0.08T_{d}=0.08Td=0.08。要求在 5s5 s5s 以内收敛于原点，无人机位置的初始位置为 (5m,4m)(5 m, 4 m)(5m,4m)。

仿真

主函数

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 inputX = [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/Tst(i) = i*Ts;pitch_ref = Prediction(X, Y, G, E, H);roll_ref = Prediction(Y, X, G, E, H);% State update under the inputX = Ad*X + Bd*pitch_ref;Y = Ad*Y + Bd*roll_ref;% Data RecordX_Ap(i,:) = X;Y_Ap(i,:) = Y;Ref_pitch_Ap(i) = pitch_ref;Ref_roll_Ap(i) = roll_ref;
endfigure('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/Tsdrawnow;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 : Nrows = i*n + (1 : n); % the current rowC(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 outputend

将所有文件放到同一路径下，运行主函数即可。运行结果如下：

附上两张实现过程的说明 (汇报PPT用的，懒得翻译了)。

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

基于mpc（最优控制）的车辆自适应巡航控制（acc），模型预测控制，通过carsim与matlab联防实现速度与间距控制
基于mpc(最优控制)的车辆自适应巡航控制(acc),模型预测控制,通过carsim与matlab联防实现速度与间距控制. ID:4529677970280675一無所有`
模型预测控制_模型预测控制(MPC)算法之一MAC算法
引言随着自动驾驶技术以及机器人控制技术的不断发展及逐渐火热,模型预测控制(MPC)算法作为一种先进的控制算法,其应用范围与领域得到了进一步拓展与延伸.目前提出的模型预测控制算法主要有基于非参数模型的 ...
对于双输入双输出系统的模型预测控制（DMC）的MATLAB实现
对于双输入双输出系统的模型预测控制(DMC)的MATLAB实现 DMC(动态矩阵控制)的简介单变量DMC控制预测模型: 滚动优化: 反馈校正: DMC(动态矩阵控制)的简介动态矩阵控制: 基于对 ...
Apollo代码学习(六)—模型预测控制(MPC)
Apollo代码学习-模型预测控制前言模型预测控制预测模型线性化单车模型滚动优化反馈矫正总结前言非专业选手,此篇博文内容基于书本和网络资源整理,可能理解的较为狭隘,起点较低,就事论 ...
基于扩展卡尔曼滤波EKF和模型预测控制MPC，自动泊车场景建模开发
基于扩展卡尔曼滤波EKF和模型预测控制MPC,自动泊车场景建模开发,文复现. MATLAB 基于扩展卡尔曼滤波EKF和模型预测控制MPC,自动泊车场景建模开发,文复现. MATLAB(工程项目线上支持 ...
无人车系统（十一）：轨迹跟踪模型预测控制(MPC)原理与python实现【40行代码】
前面介绍的PID,pure pursuit方法,Stanley方法都只是利用当前的系统误差来设计控制器.人们对这些控制器的设计过程中都利用了构建模型对无人车未来状态的估计(或者说利用模型估计未来的运动 ...
【控制control】机器人运动控制器----基于模型预测控制MPC方法
系列文章目录提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理文章目录系列文章目录前言一.模型预测控制(MPC)的介绍及构成 1.介绍 2.构成二.模型 ...
【附C++源代码】模型预测控制(MPC)公式推导以及算法实现，Model Predictive control介绍
2022年的第一篇博客,首先祝大家新年快乐! 提示:本篇博客主要集中在对MPC的理解以及应用.这篇博客可以作为你对MPC控制器深入研究的一个开始,起到抛砖引玉,带你快速了解其原理的作用. 这篇博客将介 ...
Apollo代码学习(六)—模型预测控制(MPC)_follow轻尘的博客-CSDN博客_mpc代码
Apollo代码学习(六)-模型预测控制(MPC)_follow轻尘的博客-CSDN博客_mpc代码

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

原理

问题描述

计算过程

Matlab 仿真

模型

仿真

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

最新文章

热门文章