参考文章:原理推导
基于离散模型,首先复述其首先原理,然后基于简单的无人机模型进行仿真。
原理
问题描述
考虑离散系统:
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 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用的,懒得翻译了)。