基于运动学模型的无人机模型预测控制(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=vxvx˙=ux=vyvy˙=uy
其中 n x : 状 态 变 量 量 个 数 , n u : 控 制 变 量 个 数 , n m : 输 出 变 量 个 数 n_x:状态变量量个数,n_u:控制变量个数,n_m:输出变量个数 nxnu:nm:,我们得到如下状态空间:
[ x ˙ v ˙ x y ˙ v ˙ y ] = [ 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 ] [ x v x y v y ] + [ 0 0 1 0 0 0 0 1 ] [ u x u y ] \begin{bmatrix} \dot{x}\\ \dot v_x \\ \dot y\\ \dot 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} x\\ v_x \\ y\\ v_y \end{bmatrix}+ \begin{bmatrix} 0 & 0\\ 1 & 0 \\ 0 & 0\\ 0 & 1\\ \end{bmatrix} \begin{bmatrix} u_x \\ u_y\\ \end{bmatrix} x˙v˙xy˙v˙y=0000100000000010xvxyvy+01000001[uxuy]
[ x y ] = [ 1 0 0 0 0 0 1 0 ] [ x v x y v y ] \begin{bmatrix} x\\ y \end{bmatrix} = \begin{bmatrix} 1&0&0&0\\ 0&0&1&0\\ \end{bmatrix} \begin{bmatrix} x\\ v_x\\ y\\ v_y \end{bmatrix} [xy]=[10000100]xvxyvy
其中
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 ] x ( k ) = [ x v x y v y ] u ( k ) = [ u x u y ] 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 C=\begin{bmatrix} 1&0&0&0\\ 0&0&1&0\\ \end{bmatrix}\quad x(k)=\begin{bmatrix} x\\ v_x \\ y\\ v_y \end{bmatrix} \quad u(k)=\begin{bmatrix} u_x \\ u_y\\ \end{bmatrix} \quad A=0000100000000010B=01000001C=[10000100]x(k)=xvxyvyu(k)=[uxuy]

令:
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
将上述模型离散化,我们得到:
A k = A ∗ Δ t + I B k = B ∗ Δ t \begin{aligned} &A_k=A*\Delta t+I\\ &B_k=B*\Delta t \end{aligned} Ak=AΔt+IBk=BΔt

即我们得到系统方程:
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)=Akxm(k)+Bkum(k)ym(k+1)=Cxm(k+1)=CAkxm(k)+CBkum(k)xm(k+1)nx×1Aknx×nxBknx×nu

构建差分系统方程:
Δ x m ( k + 1 ) = x m ( k + 1 ) − x m ( k ) = A k Δ x m ( k ) + B k Δ u m ( k ) \Delta x_m(k+1)=x_m(k+1)-x_m(k)=A_k\Delta x_m(k)+B_k\Delta u_m(k) Δxm(k+1)=xm(k+1)xm(k)=AkΔxm(k)+BkΔum(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×nx0nx×nmInm×nm][Δxm(k)nx×1ym(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 Δ x m ( k + 1 ) = C A k Δ x m ( k ) + C B k Δ u m ( k ) y_m(k+1)-y_m(k)=C(x_m(k+1)-x_m(k))=C\Delta x_m(k+1)=CA_k\Delta x_m(k)+CB_k\Delta u_m(k) ym(k+1)ym(k)=C(xm(k+1)xm(k))=CΔxm(k+1)=CAkΔxm(k)+CBkΔum(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×1Au=[(Ak)nx×nx(CAk)nm×nx0nx×nmInm×nm]Bu=[(Bk)nx×nu(CBk)nm×nu]Cu=[0nm×nxInm×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+1ki)Δx(ki+2ki)Δx(ki+3ki)Δx(ki+Npki)=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)+AuNp1BuΔu(ki)+AuNp2BuΔu(ki+1)++AuNpNcBuΔu(ki+Nc1)

{ Δ 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+1ki)Δy(ki+2ki)Δy(ki+3ki)Δy(ki+Npki)=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)+CuAuNp1BuΔu(ki)+CuAuNp2BuΔu(ki+1)++CuAuNpNcBuΔu(ki+Nc1)

即得到如下递推方程:
Y = F Δ x ( k i ) + Φ U Y = F\Delta x(k_i)+\Phi U Y=FΔx(ki)+Φ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=(RsY)T(RsY)+UTRU=(RsFx(ki)ΦU)T(RsFx(ki)ΦU)+UTRU=(RsFx(ki))T(RsFx(ki))2UTΦ(RsFx(ki))+UT(ΦTΦ+R)U

∂ J ∂ U \frac{\partial J}{\partial U} UJ得:
∂ 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} UJU=2ΦT(RsFx(ki))+2(ΦTΦ+R)U=0=(ΦTΦ+R)1ΦT(RsFx(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)+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
  • 5
    点赞
  • 55
    收藏
    觉得还不错? 一键收藏
  • 2
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值