基于运动学模型的无人机模型预测控制(MPC)-3_无人机 模型预测控制

基于差分模型的无人机模型预测控制(MPC)-无约束情况

1. 模型建立

无人机运动学模型：
{ x ˙ = v x v x ˙ = u x y = v y v y ˙ = u y \left\{

$$\begin{aligned} \dot x & = v_x \qquad \dot{v_x}=u_x\\ y & = v_y \qquad \dot{v_y}=u_y \\ \end{aligned}$$ \right. {x˙y​=vx​vx​˙​=ux​=vy​vy​˙​=uy​​
其中 $n_x：状态变量量个数，n_u:控制变量个数，n_m:输出变量个数$，我们得到如下状态空间：
$$\left[\begin{array}{c}\dot{x}\\ {\dot{v}}_x\\ \dot{y}\\ {\dot{v}}_y\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}x\\ v_x\\ y\\ v_y\end{array}\right]+\left[\begin{array}{cc}0& 0\\ 1& 0\\ 0& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{u}_x\\ u_y\end{array}\right]$$
$$\left[\begin{array}{c}x\\ y\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}x\\ v_x\\ y\\ v_y\end{array}\right]$$
其中
$$A=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\end{array}\right]B=\left[\begin{array}{cc}0& 0\\ 1& 0\\ 0& 0\\ 0& 1\end{array}\right]C=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]x(k)=\left[\begin{array}{c}x\\ v_x\\ y\\ v_y\end{array}\right]u(k)=\left[\begin{array}{c}{u}_x\\ u_y\end{array}\right]$$

令：
x m ( k ) = [ x ( k ) v x ( k ) y ( k ) v y ( k ) ] T u m ( k ) = [ u x ( k ) u y ( k ) ] T

$$\begin{aligned} &x_m(k)= \begin{bmatrix} x(k) \quad v_x(k)\quad y(k)\quad v_y(k) \end{bmatrix}^{T}\\ &u_m(k)=[u_x(k)\quad u_y(k)]^{T} \end{aligned}$$ ​xm​(k)=[x(k)vx​(k)y(k)vy​(k)​]Tum​(k)=[ux​(k)uy​(k)]T​
将上述模型离散化，我们得到：
$$\begin{array}{rl}& A_k=A\ast \Delta t+I\\ & B_k=B\ast \Delta t\end{array}$$

即我们得到系统方程：
x m ( k + 1 ) = A k x m ( k ) + B k u m ( k ) y m ( k + 1 ) = C x m ( k + 1 ) = C A k x m ( k ) + C B k u m ( k ) x m ( k + 1 ) ∈ n x × 1 A k ∈ n x × n x B k ∈ n x × n u

$$\begin{aligned} &x_m(k+1)=A_kx_m(k)+B_ku_m(k)\\ &y_m(k+1) = Cx_m(k+1)=CA_kx_m(k)+CB_ku_m(k)\\ &x_m(k+1)\in{n_{x}\times1}\quad A_k\in{n_{x}\times n_{x}}\quad B_k\in{n_{x}\times n_{u}} \end{aligned}$$ ​xm​(k+1)=Ak​xm​(k)+Bk​um​(k)ym​(k+1)=Cxm​(k+1)=CAk​xm​(k)+CBk​um​(k)xm​(k+1)∈nx​×1Ak​∈nx​×nx​Bk​∈nx​×nu​​

构建差分系统方程：
$$\Delta x_m(k+1)=x_m(k+1)-x_m(k)=A_k\Delta x_m(k)+B_k\Delta u_m(k)$$
即得到：
[ Δ x m ( k + 1 ) y m ( k + 1 ) ] = [ ( A k ) n x × n x 0 n x × n m ( C A k ) n m × n x I n m × n m ] [ Δ x m ( k ) n x × 1 y m ( k ) n m × 1 ] + [ ( B k ) n x × n u ( C B k ) n m × n u ] Δ u m ( k ) n u × 1

$$\begin{aligned} \begin{bmatrix} \Delta x_m(k+1)\\ y_m(k+1) \end{bmatrix}= \begin{bmatrix} &(A_k)_{ {n_x\times n_x}}&0_{n_x\times n_m}\\ &(CA_k)_{ {n_m\times n_x}}&I_{n_m\times n_m} \end{bmatrix} \begin{bmatrix} &\Delta x_m(k)_{n_x\times 1}\\ &y_m(k)_{n_m\times 1} \end{bmatrix}+ \begin{bmatrix} (B_k)_{ {n_x\times n_u}}\\ (CB_k)_{ {n_m\times n_u}} \end{bmatrix} \Delta u_m(k)_{n_u\times 1} \end{aligned}$$ [Δxm​(k+1)ym​(k+1)​]=[​(Ak​)nx​×nx​​(CAk​)nm​×nx​​​0nx​×nm​​Inm​×nm​​​][​Δxm​(k)nx​×1​ym​(k)nm​×1​​]+[(Bk​)nx​×nu​​(CBk​)nm​×nu​​​]Δum​(k)nu​×1​​

$$y_m(k+1)-y_m(k)=C(x_m(k+1)-x_m(k))=C\Delta x_m(k+1)=C{A}_k\Delta x_m(k)+C{B}_k\Delta u_m(k)$$

我们得到如下差分系统方程：
Δ x ( k + 1 ) = A u Δ x ( k ) + B u Δ u ( k ) Δ y ( k ) = C u Δ x ( k )

$$\begin{aligned} \Delta x(k+1)&=A_u\Delta x(k)+B_u\Delta u(k)\\ \Delta y(k)&=C_u\Delta x(k) \end{aligned}$$ Δx(k+1)Δy(k)​=Au​Δx(k)+Bu​Δu(k)=Cu​Δx(k)​

其中：
Δ x ( k + 1 ) = [ Δ x m ( k + 1 ) ; y m ( k + 1 ) ] ( n x + n m ) × 1 Δ u ( k ) n u × 1 = Δ u m ( k ) Δ y ( k ) n m × 1 A u = [ ( A k ) n x × n x 0 n x × n m ( C A k ) n m × n x I n m × n m ] B u = [ ( B k ) n x × n u ( C B k ) n m × n u ] C u = [ 0 n m × n x I n m × n m ]

$$\begin{aligned} &\Delta x(k+1)=[\Delta x_m(k+1);y_m(k+1)]_{(n_x+n_m)\times 1}\\ &\Delta u(k)_{n_u\times 1}=\Delta u_m(k)\\ &\Delta y(k)_{n_m\times 1}\\ &A_u= \begin{bmatrix} &(A_k)_{ {n_x\times n_x}}&0_{n_x\times n_m}\\ &(CA_k)_{ {n_m\times n_x}}&I_{n_m\times n_m} \end{bmatrix}\quad B_u= \begin{bmatrix} (B_k)_{ {n_x\times n_u}}\\ (CB_k)_{ {n_m\times n_u}} \end{bmatrix} \quad C_u= \begin{bmatrix} 0_{n_m\times n_x}\quad I_{n_m\times n_m} \end{bmatrix} \end{aligned}$$ ​Δx(k+1)=[Δxm​(k+1);ym​(k+1)](nx​+nm​)×1​Δu(k)nu​×1​=Δum​(k)Δy(k)nm​×1​Au​=[​(Ak​)nx​×nx​​(CAk​)nm​×nx​​​0nx​×nm​​Inm​×nm​​​]Bu​=[(Bk​)nx​×nu​​(CBk​)nm​×nu​​​]Cu​=[0nm​×nx​​Inm​×nm​​​]​

递推公式推导：
{ Δ x ( k i + 1 ∣ k i ) = A u Δ x ( k i ) + B u Δ u ( k i ) Δ x ( k i + 2 ∣ k i ) = A u 2 Δ x ( k i ) + A u B u Δ u ( k i ) + B u Δ u ( k i + 1 ) Δ x ( k i + 3 ∣ k i ) = A u 3 Δ x ( k i ) + A u 2 B u Δ u ( k i ) + A u B u Δ u ( k i + 1 ) + B u Δ u ( k i + 2 ) ⋮ Δ x ( k i + N p ∣ k i ) = A u N p Δ x ( k i ) + A u N p − 1 B u Δ u ( k i ) + A u N p − 2 B u Δ u ( k i + 1 ) + ⋯ + A u N p − N c B u Δ u ( k i + N c − 1 ) \left\{

$$\begin{aligned} \Delta x(k_i+1|k_i)&=A_u\Delta x(k_i)+B_u\Delta u(k_i)\\ \Delta x(k_i+2|k_i)&=A_u^{2}\Delta x(k_i)+A_uB_u \Delta u(k_i)+B_u\Delta u(k_i+1)\\ \Delta x(k_i+3|k_i)&=A_u^{3}\Delta x(k_i)+A_u^{2}B_u\Delta u(k_i)+A_uB_u\Delta u(k_i+1)+B_u\Delta u(k_i+2)\\ \quad\vdots\\ \Delta x(k_i+N_p|k_i)&=A_u^{N_p}\Delta x(k_i)+A_u^{N_p-1}B_u\Delta u(k_i)+A_u^{N_p-2}B_u\Delta u(k_i+1)+\cdots +A_u^{N_p-N_c}B_u\Delta u(k_i+N_c-1)\\ \end{aligned}$$ \right. ⎩⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎧​Δx(ki​+1∣ki​)Δx(ki​+2∣ki​)Δx(ki​+3∣ki​)⋮Δx(ki​+Np​∣ki​)​=Au​Δx(ki​)+Bu​Δu(ki​)=Au2​Δx(ki​)+Au​Bu​Δu(ki​)+Bu​Δu(ki​+1)=Au3​Δx(ki​)+Au2​Bu​Δu(ki​)+Au​Bu​Δu(ki​+1)+Bu​Δu(ki​+2)=AuNp​​Δx(ki​)+AuNp​−1​Bu​Δu(ki​)+AuNp​−2​Bu​Δu(ki​+1)+⋯+AuNp​−Nc​​Bu​Δu(ki​+Nc​−1)​

{ Δ y ( k i + 1 ∣ k i ) = C u A u Δ x ( k i ) + C u B u Δ u ( k i ) Δ y ( k i + 2 ∣ k i ) = C u A u 2 Δ x ( k i ) + C u A u B u Δ u ( k i ) + C u B u Δ u ( k i + 1 ) Δ y ( k i + 3 ∣ k i ) = C u A u 3 Δ x ( k i ) + C u A u 2 B u Δ u ( k i ) + C u A u B u Δ u ( k i + 1 ) + C u B u Δ u ( k i + 2 ) ⋮ Δ y ( k i + N p ∣ k i ) = C u A u N p Δ x ( k i ) + C u A u N p − 1 B u Δ u ( k i ) + C u A u N p − 2 B u Δ u ( k i + 1 ) + ⋯ + C u A u N p − N c B u Δ u ( k i + N c − 1 ) \left\{

$$\begin{aligned} \Delta y(k_i+1|k_i)&=C_uA_u\Delta x(k_i)+C_uB_u\Delta u(k_i)\\ \Delta y(k_i+2|k_i)&=C_uA_u^{2}\Delta x(k_i)+C_uA_uB_u\Delta u(k_i)+C_uB_u\Delta u(k_i+1)\\ \Delta y(k_i+3|k_i)&=C_uA_u^{3}\Delta x(k_i)+C_uA_u^{2}B_u\Delta u(k_i)+C_uA_uB_u\Delta u(k_i+1)+C_uB_u\Delta u(k_i+2)\\ \quad\vdots\\ \Delta y(k_i+N_p|k_i)&=C_uA_u^{N_p}\Delta x(k_i)+C_uA_u^{N_p-1}B_u\Delta u(k_i)+C_uA_u^{N_p-2}B_u\Delta u(k_i+1)+\cdots \\ &+C_uA_u^{N_p-N_c}B_u\Delta u(k_i+N_c-1)\\ \end{aligned}$$ \right. ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧​Δy(ki​+1∣ki​)Δy(ki​+2∣ki​)Δy(ki​+3∣ki​)⋮Δy(ki​+Np​∣ki​)​=Cu​Au​Δx(ki​)+Cu​Bu​Δu(ki​)=Cu​Au2​Δx(ki​)+Cu​Au​Bu​Δu(ki​)+Cu​Bu​Δu(ki​+1)=Cu​Au3​Δx(ki​)+Cu​Au2​Bu​Δu(ki​)+Cu​Au​Bu​Δu(ki​+1)+Cu​Bu​Δu(ki​+2)=Cu​AuNp​​Δx(ki​)+Cu​AuNp​−1​Bu​Δu(ki​)+Cu​AuNp​−2​Bu​Δu(ki​+1)+⋯+Cu​AuNp​−Nc​​Bu​Δu(ki​+Nc​−1)​

即得到如下递推方程：
$$Y=F\Delta x(k_i)+\Phi U$$

性能指标：
J = ( R s − Y ) T ( R s − Y ) + U T R U = ( R s − F x ( k i ) − Φ U ) T ( R s − F x ( k i ) − Φ U ) + U T R U = ( R s − F x ( k i ) ) T ( R s − F x ( k i ) ) − 2 U T Φ ( R s − F x ( k i ) ) + U T ( Φ T Φ + R ) U

$$\begin{aligned} J&=(R_s-Y)^{T}(R_s-Y)+U^{T}RU\\ &=(R_s-Fx(k_i)-\Phi U)^{T}(R_s-Fx(k_i)-\Phi U)+U^{T}RU\\ &=(R_s-Fx(k_i))^{T}(R_s-Fx(k_i))-2U^{T}\Phi (R_s-Fx(k_i))+U^{T}(\Phi^{T}\Phi+R)U \end{aligned}$$ J​=(Rs​−Y)T(Rs​−Y)+UTRU=(Rs​−Fx(ki​)−ΦU)T(Rs​−Fx(ki​)−ΦU)+UTRU=(Rs​−Fx(ki​))T(Rs​−Fx(ki​))−2UTΦ(Rs​−Fx(ki​))+UT(ΦTΦ+R)U​

求$\frac{\partial J}{\partial U}$得：
∂ J ∂ U = − 2 Φ T ( R s − F x ( k i ) ) + 2 ( Φ T Φ + R ) U = 0 U = ( Φ T Φ + R ) − 1 Φ T ( R s − F x ( k i ) )

$$\begin{aligned} \frac{\partial J}{\partial U}&=-2\Phi^{T}(R_s-Fx(k_i))+2(\Phi^{T}\Phi+R)U=0\\ U&=(\Phi^{T}\Phi+R)^{-1}\Phi^{T}(R_s-Fx(k_i)) \end{aligned}$$ ∂U∂J​U​=−2ΦT(Rs​−Fx(ki​))+2(ΦTΦ+R)U=0=(ΦTΦ+R)−1ΦT(Rs​−Fx(ki​))​

即差分方程迭代：
Δ x ( k i + 1 ) = A k Δ x ( : , k i ) + B k U ( 1 : n m ) U = U ( 1 : n m ) + o l d U X ( : , i + 1 ) = A k X ( : , k i ) + B k U

$$\begin{aligned} \Delta x(k_i+1)&=A_k\Delta x(:,k_i)+B_kU(1:n_m)\\ U &= U(1:n_m)+oldU\\ X(:,i+1)&=A_kX(:,k_i)+B_kU \end{aligned}$$ Δx(ki​+1)UX(:,i+1)​=Ak​Δx(:,ki​)+Bk​U(1:nm​)=U(1:nm​)+oldU=Ak​X(:,ki​)+Bk​U​

2. matlab 仿真代码

%================无人机模型预测控制-基于差分模型的模型预测================%
clear all;clc;close all;
%% 无人机参数设定--采用运动学模型进行轨迹跟踪
x0 = 10; y0 = 5; x1 = 11; y1 = 6;
vx0 = 0; vy0 = 0; vx1 = 1; vy1 = 1;
x(1) = x0; y(1) = y0;vx(1) = vx0;vy(1) = vy0;
%% 领航者参数设定
inter = 0.05;  % 采样周期
time = 60;  % 总时长
R = 2;
omega = 2;
t = 0:inter:time;
%% 八字形
for i = 1:1:length(t)
   if (mod(floor(omega*t(i)/(2*pi)),2) == 0)
    Xr(i) = R*cos(omega*t(i))-R;
    Yr(i) = R*sin(omega*t(i));
    Vxr(i) = -R*sin(omega*t(i))*omega;
    Vyr(i) = R*cos(omega*t(i))*omega;
    Uxr(i) = -R*cos(omega*t(i))*omega^2;
    Uyr(i) = -R*sin(omega*t(i))*omega^2;
   else
    Xr(i) = -R*cos(omega*t(i))+R;
    Yr(i) = R*sin(omega*t(i));   
    Vxr(i) = R*sin(omega*t(i))*omega;
    Vyr(i) = R*cos(omega*t(i))*omega;
    Uxr(i) = R*cos(omega*t(i))*omega^2;
    Uyr(i) = -R*sin(omega*t(i))*omega^2;
   end

end
%% 直线
% Xr = (2*t)';
% Yr = 3*ones(length(t),1);
% Vxr = 2*ones(length(t),1);
% Vyr = 2*zeros(length(t),1);
% Uxr = zeros(length(t),1);
% Uyr = zeros(length(t),1);
%% 圆形
% Xr = -R*cos(t);
% Yr = R*sin(t);
% Vxr = R*sin(t);
% Vyr = R*cos(t);
% Uxr = R*cos(t);
% Uyr = -R*sin(t);
%%
% EX(:,1) = [x0 - Xr(1);vx0 - Vxr(1);y0 - Yr(1);vy0 - Vyr(1)];

X(:,1) = [x0;vx0;y0;vy0];
deltaX(:,1) = [0;0;0;0;x0;y0];
%% 领航者轨迹
% figure
% grid minor
% l1 = [];
% axis([-7 7 -7 7]);
% axis equal
% for i = 2:1:length(t)
%   hold on
%   plot([Xr(i) Xr(i-1)],[Yr(i) Yr(i-1)],'b');
%   hold on
%   delete(l1);
%   l1 =  plot(Xr(i),Yr(i),'r.','MarkerSize',20);
%   pause(0.1);
%   
% end
%% 模型预测控制参数设定
Np = 20;     % 预测步长
Nc = 10;      % 控制步长
A = [0 1 0 0;0 0 0 0;0 0 0 1;0 0 0 0];  B = [0 0;1 0;0 0;0 1]; 
C = [1 0 0 0;0 0 1 0];
nx = size(A);
nx = nx(1);
nu = size(B);
nu = nu(2);
nm = 2;
 R = 0.002*eye(Nc*nu);
Ak = A*inter + eye(nx);
Bk = B*inter;
Au = [Ak zeros(nx,nm);C*Ak eye(nm)];
Bu = [Bk;C*Bk];
Cu = [zeros(nm,nx) eye(nm)];

F = cell(Np,1);
PHI = cell(Np,Nc);
for i = 1:1:Np         % 计算预测方程矩阵
  F{i,1} = Cu*Au^i;
end
F = cell2mat(F);

for i = 1:1:Np
   for j = 1:1:Nc
       if (j<=i)
           PHI{i,j} = Cu*Au^(i-j)*Bu;
       else
           PHI{i,j} = zeros(nm,nu);
       end
   end
end
PHI = cell2mat(PHI);
k1 =2;k2 =2;
XX = [];
%% 迭代计算
k = 1;
oldU = [0;0];
for i = 1:1:length(t)-1
   for j = i:1:(Np+i-1)
     if j >= length(Xr)
         j = length(Xr);
     end
     XX = [XX;[Xr(j);Yr(j)]]; 
   end
  U = inv(PHI'*PHI + R)*PHI'*(XX- F*deltaX(:,i));
  XX = [];
  u = U(1:2,1) + oldU;
  oldU = u;
  X(:,i+1) = Ak*X(:,i) + Bk*u;
  deltaX(:,i+1) = Au*deltaX(:,i) + Bu*U(1:2,1);
  
%   err =[X(:,i+1) - [Xr(i+1);Vxr(i+1);Yr(i+1);Vyr(i+1)]] ;
end
x = (X(1,:))';
vx = (X(2,:))';
y = (X(3,:))';
vy = (X(4,:))';
% VV = vecnorm([Vxr;Vyr]);
% VX = vecnorm([vx;vy]);
% plot(t,VV,'r')
% hold on
% plot(t,VX(1:length(t)),'b')
figure
thetr = atan2(Yr,Xr);
thet = atan2(y,x);
plot(t,thetr(1:length(t)),'r');
hold on
plot(t,thet(1:length(t)),'k');
legend('Leader','follower1')

l1 = [];
l2 = [];
pic_num = 1;
figure
 grid minor
%  axis([-5 5 -5 5])
axis equal
Tag1 = animatedline('Color','r');
for i = 1:1:length(Xr)-1
    
    hold on
    delete(l1);
   delete(l2);

    plot([x(i) x(i+1)],[y(i) y(i+1)],'b');
   hold on
   plot([Xr(i) Xr(i+1)],[Yr(i) Yr(i+1)],'r');
   hold on
   l1 = plot(x(i+1),y(i+1),'b.','MarkerSize',20);
   hold on
   l2 = plot(Xr(i+1),Yr(i+1),'r.','MarkerSize',20);
   pause(0.1);
%    addpoints(Tag1,t(i),x(i));
%    drawnow;
%     F=getframe(gcf);
%     I=frame2im(F);
%     [I,map]=rgb2ind(I,256);
%     if pic_num == 1
%         imwrite(I,map,'test.gif','gif', 'Loopcount',inf,'DelayTime',0.2);
%     else
%         imwrite(I,map,'test.gif','gif','WriteMode','append','DelayTime',0.2);
%     end
%     pic_num = pic_num + 1;
    F = getframe(gcf);
    I = frame2im(F);
    [I,map] = rgb2ind(I,256);
    if pic_num == 1
        imwrite(I,map,'test.gif','gif','Loopcount',inf,'DelayTime',0.2);
    else
        imwrite(I,map,'test.gif','gif','WriteMode','append','DelayTime',0.2);
    end
    pic_num = pic_num + 1;
     
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181