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

基于误差模型的无人机模型预测控制(MPC)

1. 模型建立

无人机运动学模型：
$\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​​
领航者模型：
$\left\{ \begin{aligned} \dot x_r & = v_{x_r} \qquad \dot{v_{x_r}}=u_{x_r}\\ y_r & = v_{y_r} \qquad \dot{v_{y_r}}=u_{y_r} \\ \end{aligned} \right.${x˙r​yr​​=vxr​​vxr​​˙​=uxr​​=vyr​​vyr​​˙​=uyr​​​
令误差向量为：
$\left\{ \begin{aligned} \widetilde{x}&=x-x_r \qquad\widetilde{y}=y-y_r\\ \widetilde{v}_x&=v_x-v_{x_r} \qquad \widetilde{v}_y=v_y-v_{y_r}\\ \end{aligned} \right.${xvx​​=x−xr​y​=y−yr​=vx​−vxr​​vy​=vy​−vyr​​​
其中 $n_x：状态变量量个数，n_u:控制变量个数，n_m:输出变量个数$nx​：状态变量量个数，nu​:控制变量个数，nm​:输出变量个数，我们得到如下状态空间：
$\begin{bmatrix} \dot{\widetilde{x}}\\ \dot{\widetilde{v}}_x \\ \dot{\widetilde{y}}\\ \dot{\widetilde{v}}_y \end{bmatrix}= \begin{bmatrix} 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 1\\ 0 & 0 & 0 & 0\\ \end{bmatrix} \begin{bmatrix} \widetilde{x}\\ \widetilde{v}_x \\ \widetilde{y}\\ \widetilde{v}_y \end{bmatrix}+ \begin{bmatrix} 0 & 0\\ 1 & 0 \\ 0 & 0\\ 0 & 1\\ \end{bmatrix} \begin{bmatrix} \widetilde{u}_x \\ \widetilde{u}_y\\ \end{bmatrix}$⎣⎢⎢⎡​x˙v˙x​y​˙​v˙y​​⎦⎥⎥⎤​=⎣⎢⎢⎡​0000​1000​0000​0010​⎦⎥⎥⎤​⎣⎢⎢⎡​xvx​y​vy​​⎦⎥⎥⎤​+⎣⎢⎢⎡​0100​0001​⎦⎥⎥⎤​[ux​uy​​]
其中
$A = \begin{bmatrix} 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 1\\ 0 & 0 & 0 & 0\\ \end{bmatrix} \quad B = \begin{bmatrix} 0 & 0\\ 1 & 0 \\ 0 & 0\\ 0 & 1\\ \end{bmatrix} \quad x(k)=\begin{bmatrix} \widetilde{x}\\ \widetilde{v}_x \\ \widetilde{y}\\ \widetilde{v}_y \end{bmatrix} \quad u(k)=\begin{bmatrix} \widetilde{u}_x \\ \widetilde{u}_y\\ \end{bmatrix} \quad$A=⎣⎢⎢⎡​0000​1000​0000​0010​⎦⎥⎥⎤​B=⎣⎢⎢⎡​0100​0001​⎦⎥⎥⎤​x(k)=⎣⎢⎢⎡​xvx​y​vy​​⎦⎥⎥⎤​u(k)=[ux​uy​​]
将上述模型离散化，我们得到：
$\begin{aligned} &A_k=A*\Delta t+I\\ &B_k=B*\Delta t \end{aligned}$​Ak​=A∗Δt+IBk​=B∗Δt​
即我们得到系统方程：
$\begin{aligned} &x(k+1)=A_k*x(k)+B_ku(k)\\ &x(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}$​x(k+1)=Ak​∗x(k)+Bk​u(k)x(k+1)∈nx​×1Ak​∈nx​×nx​Bk​∈nx​×nu​​

递推公式推导：
$\left\{ \begin{aligned} &x(k_i+1|k_i)=A_kx(k_i)+B_ku(k_i)\\ &x(k_i+2|k_i)=A_k^{2}x(k_i)+A_kB_ku(k_i)+B_ku(k_i+1)\\ &x(k_i+3|k_i)=A_k^{3}x(k_i)+A_k^{2}B_ku(k_i)+A_kB_ku(k_i+1)+B_ku(k_i+2)\\ &\qquad\vdots\\ &x(k_i+N_p|k_i)=A_k^{N_p}x(k_i)+A_k^{N_p-1}B_ku(k_i)+A_k^{N_p-2}B_ku(k_i+1)+\cdots +A_k^{N_p-N_c}B_ku(k_i+N_c-1)\\ \end{aligned} \right.$⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧​​x(ki​+1∣ki​)=Ak​x(ki​)+Bk​u(ki​)x(ki​+2∣ki​)=Ak2​x(ki​)+Ak​Bk​u(ki​)+Bk​u(ki​+1)x(ki​+3∣ki​)=Ak3​x(ki​)+Ak2​Bk​u(ki​)+Ak​Bk​u(ki​+1)+Bk​u(ki​+2)⋮x(ki​+Np​∣ki​)=AkNp​​x(ki​)+AkNp​−1​Bk​u(ki​)+AkNp​−2​Bk​u(ki​+1)+⋯+AkNp​−Nc​​Bk​u(ki​+Nc​−1)​
令
$\begin{aligned} &\Delta X=[x(k_i+1|k_i) \quad x(k_i+2|k_i) \quad x(k_i+1|k_i)\cdots x(k_i+N_p|k_i)]^{T}_{(Np *n_x)\times 1}\\ &\Delta U = [u(k_i)\quad u(k_{i}+1)\cdots u(k_{i}+N_u)]^{T}_{(N_c*n_u)\times 1}\\ \end{aligned}$​ΔX=[x(ki​+1∣ki​)x(ki​+2∣ki​)x(ki​+1∣ki​)⋯x(ki​+Np​∣ki​)](Np∗nx​)×1T​ΔU=[u(ki​)u(ki​+1)⋯u(ki​+Nu​)](Nc​∗nu​)×1T​​

$\begin{aligned} &F=[A_k \quad A^{2}_k\quad\cdots A^{N_p}_k]^{T}_{(N_p*n_x)\times n_x} \quad \\ &\Phi = \begin{bmatrix} B_k & 0 & 0 &\cdots &0\\ A_kB_k &B_k&0&\cdots &0\\ \vdots &\quad&\quad&\quad & \vdots\\ A^{N_p}_kB_k&A^{N_p-1}_kB_k &A^{N_p-2}_kB_k &\cdots&A^{N_p-N_c}_kB_k \\ \end{bmatrix}_{(Np*n_x)\times(n_u*N_c)} \end{aligned}$​F=[Ak​Ak2​⋯AkNp​​](Np​∗nx​)×nx​T​Φ=⎣⎢⎢⎢⎡​Bk​Ak​Bk​⋮AkNp​​Bk​​0Bk​AkNp​−1​Bk​​00AkNp​−2​Bk​​⋯⋯⋯​00⋮AkNp​−Nc​​Bk​​⎦⎥⎥⎥⎤​(Np∗nx​)×(nu​∗Nc​)​​
即
$\Delta X=Fx(k_i)+\Phi \Delta U$ΔX=Fx(ki​)+ΦΔU
性能指标：
$\begin{aligned} J&=\sum_{j=1}^{N_p}[x(k_i+j|k_i)^{T}x(k_i+j|k_i)]+\sum_{j=0}^{N_c-1}[u(k_i+j)^{T}*r_j *u(k_i+j)]\\ &=\Delta X^{T}\Delta X+\Delta U^{T}R\Delta U\\ &=[Fx(k_i)+\Phi \Delta U]^{T}[Fx(k_i)+\Phi \Delta U]+\Delta U^{T}R\Delta U \end{aligned}$J​=j=1∑Np​​[x(ki​+j∣ki​)Tx(ki​+j∣ki​)]+j=0∑Nc​−1​[u(ki​+j)T∗rj​∗u(ki​+j)]=ΔXTΔX+ΔUTRΔU=[Fx(ki​)+ΦΔU]T[Fx(ki​)+ΦΔU]+ΔUTRΔU​
求$\frac{\partial J}{\partial U}$∂U∂J​得：
$\begin{aligned} &\frac{\partial J}{\partial U}=2(\Phi^{T}\Phi+R)\Delta U+2\Phi^{T}Fx(k_i)\\ &\Delta U = -(\Phi^{T}\Phi+R)^{-1}\Phi^{T}Fx(k_i) \end{aligned}$​∂U∂J​=2(ΦTΦ+R)ΔU+2ΦTFx(ki​)ΔU=−(ΦTΦ+R)−1ΦTFx(ki​)​

即建立误差系统：
$\begin{aligned} E_x(:,i+1)&=A_kE_x(:,i)+B_k\Delta U(1:n_u)\\ X(:,i+1)&=[x_r(i+1);v_{x_r}(i+1);y_r(i+1);v_{y_r}(i+1)]+E_x(:,i+1) \end{aligned}$Ex​(:,i+1)X(:,i+1)​=Ak​Ex​(:,i)+Bk​ΔU(1:nu​)=[xr​(i+1);vxr​​(i+1);yr​(i+1);vyr​​(i+1)]+Ex​(:,i+1)​

%================无人机模型预测控制================%
clear all;clc;close all;
%% 无人机参数设定--采用运动学模型进行轨迹跟踪
x0 = 3; y0 = 3;
vx0 = 0; vy0 = 0;
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 = -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];
% 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 = 30;     % 预测步长
Nc = 5;      % 控制步长
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]; 
lena = size(A);
lenb = size(B);
R = eye(Nc*lenb(2));
Ak = A*inter + eye(lena(1));
Bk = B*inter;
F = cell(Np,1);
PHI = cell(Np,Nc);
for i = 1:1:Np         % 计算预测方程矩阵
  F{i,1} = Ak^i;
end
F = cell2mat(F);

for i = 1:1:Np
   for j = 1:1:Nc
       if (j<=i)
           PHI{i,j} = Ak^(i-j)*Bk;
       else
           PHI{i,j} = zeros(lena(1),lenb(2));
       end
   end
end
PHI = cell2mat(PHI);
k1 =2;k2 =2;
%% 迭代计算
for i = 1:1:length(t)-1
 
  U = -inv(PHI'*PHI + R)*PHI'*F*EX(:,i);
  u = U(1:2,1) + [Uxr(i);Uyr(i)];
  EX(:,i+1) = Ak*EX(:,i) + Bk*U(1:2,1);
  X(:,i+1) = [Xr(i+1);Vxr(i+1);Yr(i+1);Vyr(i+1)]+EX(:,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')


l1 = [];
l2 = [];
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)],'r');
   hold on
   plot([Xr(i) Xr(i+1)],[Yr(i) Yr(i+1)],'b');
   hold on
   l1 = plot(x(i+1),y(i+1),'r.','MarkerSize',20);
   hold on
   l2 = plot(Xr(i+1),Yr(i+1),'b.','MarkerSize',20);
%    pause(0.1);
%    addpoints(Tag1,t(i),x(i));
   drawnow;
end