基于差分模型的无人机模型预测控制(MPC)-无约束情况
1. 模型建立
无人机运动学模型:
{x˙y=vxvx˙=ux=vyvy˙=uy
其中 nx:状态变量量个数,nu:控制变量个数,nm:输出变量个数,我们得到如下状态空间:
⎣⎢⎢⎡x˙v˙xy˙v˙y⎦⎥⎥⎤=⎣⎢⎢⎡0000100000000010⎦⎥⎥⎤⎣⎢⎢⎡xvxyvy⎦⎥⎥⎤+⎣⎢⎢⎡01000001⎦⎥⎥⎤[uxuy]
[xy]=[10000100]⎣⎢⎢⎡xvxyvy⎦⎥⎥⎤
其中
A=⎣⎢⎢⎡0000100000000010⎦⎥⎥⎤B=⎣⎢⎢⎡01000001⎦⎥⎥⎤C=[10000100]x(k)=⎣⎢⎢⎡xvxyvy⎦⎥⎥⎤u(k)=[uxuy]
令:
xm(k)=[x(k)vx(k)y(k)vy(k)]Tum(k)=[ux(k)uy(k)]T
将上述模型离散化,我们得到:
Ak=A∗Δt+IBk=B∗Δt
即我们得到系统方程:
xm(k+1)=Akxm(k)+Bkum(k)ym(k+1)=Cxm(k+1)=CAkxm(k)+CBkum(k)xm(k+1)∈nx×1Ak∈nx×nxBk∈nx×nu
构建差分系统方程:
Δxm(k+1)=xm(k+1)−xm(k)=AkΔxm(k)+BkΔum(k)
即得到:
[Δxm(k+1)ym(k+1)]=[(Ak)nx×nx(CAk)nm×nx0nx×nmInm×nm][Δxm(k)nx×1ym(k)nm×1]+[(Bk)nx×nu(CBk)nm×nu]Δum(k)nu×1
ym(k+1)−ym(k)=C(xm(k+1)−xm(k))=CΔxm(k+1)=CAkΔxm(k)+CBkΔum(k)
我们得到如下差分系统方程:
Δx(k+1)Δy(k)=AuΔx(k)+BuΔu(k)=CuΔx(k)
其中:
Δx(k+1)=[Δxm(k+1);ym(k+1)](nx+nm)×1Δu(k)nu×1=Δum(k)Δy(k)nm×1Au=[(Ak)nx×nx(CAk)nm×nx0nx×nmInm×nm]Bu=[(Bk)nx×nu(CBk)nm×nu]Cu=[0nm×nxInm×nm]
递推公式推导:
⎩⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎧Δ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)+AuBuΔu(ki)+BuΔu(ki+1)=Au3Δx(ki)+Au2BuΔu(ki)+AuBuΔu(ki+1)+BuΔu(ki+2)=AuNpΔx(ki)+AuNp−1BuΔu(ki)+AuNp−2BuΔu(ki+1)+⋯+AuNp−NcBuΔu(ki+Nc−1)
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧Δy(ki+1∣ki)Δy(ki+2∣ki)Δy(ki+3∣ki)⋮Δy(ki+Np∣ki)=CuAuΔx(ki)+CuBuΔu(ki)=CuAu2Δx(ki)+CuAuBuΔu(ki)+CuBuΔu(ki+1)=CuAu3Δx(ki)+CuAu2BuΔu(ki)+CuAuBuΔu(ki+1)+CuBuΔu(ki+2)=CuAuNpΔx(ki)+CuAuNp−1BuΔu(ki)+CuAuNp−2BuΔu(ki+1)+⋯+CuAuNp−NcBuΔu(ki+Nc−1)
即得到如下递推方程:
Y=FΔx(ki)+ΦU
性能指标:
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
求∂U∂J得:
∂U∂JU=−2ΦT(Rs−Fx(ki))+2(ΦTΦ+R)U=0=(ΦTΦ+R)−1ΦT(Rs−Fx(ki))
即差分方程迭代:
Δx(ki+1)UX(:,i+1)=AkΔx(:,ki)+BkU(1:nm)=U(1:nm)+oldU=AkX(:,ki)+BkU
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