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

最新推荐文章于 2025-07-02 16:14:45 发布

Micozzc_lyy

最新推荐文章于 2025-07-02 16:14:45 发布

阅读量3.4k

点赞数 4

CC 4.0 BY-SA版权

分类专栏：算法文章标签： MPC

本文链接：https://blog.youkuaiyun.com/weixin_43822768/article/details/90736518

算法专栏收录该内容

27 篇文章

订阅专栏

本文介绍了一种基于无人机自身模型的模型预测控制方法，通过离散化连续系统模型，推导了预测控制的迭代方程，并提供了MATLAB仿真代码实现。该方法适用于无人机的路径跟踪与控制。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

基于无人机自身模型的模型预测控制-无约束情况

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.$
领航者模型：
$\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.$
其中 $n_x：状态变量量个数，n_u:控制变量个数，n_m:输出变量个数$ ，我们得到如下状态空间：
$\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}$
$\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}$
其中
$\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$

将上述模型离散化，我们得到：
$\begin{aligned} &A_k=A*\Delta t+I\\ &B_k=B*\Delta t \end{aligned}$

即我们得到系统方程：
$\begin{aligned} &x(k+1)=A_k*x(k)+B_ku(k)\\ &y(k+1) = Cx(k+1)=CA_k*x(k)+CB_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}$
递推公式推导：
$\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.$
$\left\{ \begin{aligned} &y(k_i+1|k_i)=CA_kx(k_i)+CB_ku(k_i)\\ &y(k_i+2|k_i)=CA_k^{2}x(k_i)+CA_kB_ku(k_i)+CB_ku(k_i+1)\\ &y(k_i+3|k_i)=CA_k^{3}x(k_i)+CA_k^{2}B_ku(k_i)+CA_kB_ku(k_i+1)+CB_ku(k_i+2)\\ &\qquad\vdots\\ &y(k_i+N_p|k_i)=CA_k^{N_p}x(k_i)+CA_k^{N_p-1}B_ku(k_i)+CA_k^{N_p-2}B_ku(k_i+1)+\cdots +CA_k^{N_p-N_c}B_ku(k_i+N_c-1)\\ \end{aligned} \right.$

令
$\begin{aligned} &Y=[y(k_i+1|k_i) \quad y(k_i+2|k_i) \quad y(k_i+3|k_i)\cdots y(k_i+N_p|k_i)]^{T}_{(Np *n_m)\times 1}\\ &U = [u(k_i)\quad u(k_{i}+1)\cdots u(k_{i}+N_u)]^{T}_{(N_c*n_u)\times 1}\\ \end{aligned}$

$\begin{aligned} &F=[CA_k \quad CA^{2}_k\quad\cdots CA^{N_p}_k]^{T}_{(N_p*n_m)\times n_x} \quad \\ &\Phi = \begin{bmatrix} CB_k & 0 & 0 &\cdots &0\\ CA_kB_k &CB_k&0&\cdots &0\\ \vdots &\quad&\quad&\quad & \vdots\\ CA^{N_p}_kB_k&CA^{N_p-1}_kB_k &CA^{N_p-2}_kB_k &\cdots&CA^{N_p-N_c}_kB_k \\ \end{bmatrix}_{(Np*n_m)\times(n_u*N_c)} \end{aligned}$

即我们得到：
$Y=Fx(k_i)+\Phi 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∂U\frac{\partial J}{\partial U}$ 得：
$\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}$
其中 $R_s$ 的长度为 $N_p$ 。

即我们得到迭代方程：
$X(:,i+1) = A_kX(:,i)+B_kU(1:n_m)$

此处直接取U进行计算，因为是利用系统自身模型构建的MPC。

2.matlab仿真代码

%================无人机模型预测控制-基与自身模型的模型预测================%
clear all;clc;close all;
%% 无人机参数设定--采用运动学模型进行轨迹跟踪
x0 = 10; y0 = 5;
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 = (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);
% Xr = t';
% Yr = 3*ones(length*(t),1);
%%
% 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 = 20;     % 预测步长
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]; 
C = diag([1 0 1 0]);
lena = size(A);
lenb = size(B);
R = 0.0002*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} = C*Ak^i;
end
F = cell2mat(F);

for i = 1:1:Np
   for j = 1:1:Nc
       if (j<=i)
           PHI{i,j} = C*Ak^(i-j)*Bk;
       else
           PHI{i,j} = zeros(lena(1),lenb(2));
       end
   end
end
PHI = cell2mat(PHI);
k1 =2;k2 =2;
XX = [];
%% 迭代计算
k = 1;
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);0;Yr(j);0]]; 
   end
  U = inv(PHI'*PHI + R)*PHI'*(XX- F*X(:,i));
  XX = [];
  u = U(1:2,1);
%   u = u + rand(2,1);
%   u = min(max(u,-30),30);   % 限幅
  UU(:,i) = u; 
  X(:,i+1) = Ak*X(:,i) + Bk*u;
  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')

figure

plot(t(1:length(UU)),UU(1,:),'r');
hold on
plot(t(1:length(UU)),UU(2,:),'k');
legend('ux','uy')

l1 = [];
l2 = [];
pic_num = 1;
figure
 grid minor
 axis([-10 10 -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