参考文章：原理推导

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

原理

问题描述

考虑离散系统：

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​(XkT​QXk​+ukT​Ruk​)+XNT​FXN​=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⋮AN​​xk​+​OBAB⋮AN−1B​B⋮AN−2B​⋱⋯​B​​Uk​=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)​​T​Q​Q​⋱​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​R​R​⋱​R​​​u(k∣k)u(k+1∣k)⋮u(k+N−1∣k)​​=XkT​Q​Xk​+UkT​RUk​​

然后将上述的矩阵形式代入到代价函数中，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​=XkT​Q​Xk​+UkT​RUk​=(Mxk​+CUk​)TQ​(Mxk​+CUk​)+UkR​RUk​=xkT​MTQˉ​Mxk​+xkT​MTQˉ​CUk​+UkT​CTQˉ​Mxk​+UkT​CTQˉ​CUk​+UkT​RˉUk​=xkT​MTQˉ​Mxk​+2xkT​MTQˉ​CUk​+UkT​(CTQˉ​C+Rˉ)Uk​=xkT​Gxk​+2xkT​EUk​+UkT​HUk​​

最终，代价函数可以表示为：

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=xkT​Gxk​+2xkT​EUk​+UkT​HUk​G=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˙xs​v˙x​a˙x​​​=​0000​0−Td​1​00​1Td​1​0a1x​​0−11a2x​​​​xvxs​vx​ax​​​+​000b1x​​​θref​​y˙​v˙ys​v˙y​a˙y​​​=​0000​0−Td​1​00​1Td​1​0a1y​​0−11a2y​​​​yvys​vy​ay​​​+​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 ModelSim_Time = 10;Ts = 0.01;[Ad, Bd] = UAV_Model(Ts);%% InitializePredict_Step = 20;Q = diag([200, 30, 20, 1]); % Elements depends on statesF = diag([200, 30, 20, 1]); % Elements depends on statesR = 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 Simulatefor 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 Directionsubplot(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 Directionsubplot(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 Plotfigure('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 accelerationin 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 = [00 1 0;0 -1/Td 1/Td -1;00 0 1;00 a1 a2];B = [00 0 b1]';C = [10 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 lengthn = size(A, 1);p = size(B, 2);M = [ eye(n);zeros(N*n, n)];C = zeros((N + 1)*n, N*p); % Caculate M and Ctmp = 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_barQ_bar = blkdiag(kron(eye(N), Q), F);R_bar = kron(eye(N), R);% Caculate G, E, HG = 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 formu_k = U_k(1, 1); % Select the first action as outputend

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

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