【無人機】基於matlab無人機追蹤軌跡【含Matlab原始碼 1152期】

一、簡介

基於matlab無人機追蹤軌跡

二、原始碼

```c clc; clear all; %% 參考軌跡生成 N=100; T=0.05; % Xout=zeros(2N,3); % Tout=zeros(2N,1); Xout=zeros(N,3); Tout=zeros(N,1); for k=1:1:N Xout(k,1)=kT; Xout(k,2)=2; Xout(k,3)=0; Tout(k,1)=(k-1)T; end

%% Tracking a constant reference trajectory Nx=3; Nu =2; Tsim =20; X0 = [0 0 pi/3]; [Nr,Nc] = size(Xout); % Nr is the number of rows of Xout % Mobile Robot Parameters c = [1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1]; L = 1; Rr = 1; w = 1; % Mobile Robot variable Model vd1 = Rrw; % For circular trajectory vd2 = 0; x_real=zeros(Nr,Nc); x_piao=zeros(Nr,Nc); u_real=zeros(Nr,2); u_piao=zeros(Nr,2); x_real(1,:)=X0; x_piao(1,:)=x_real(1,:)-Xout(1,:); X_PIAO=zeros(Nr,NxTsim); XXX=zeros(Nr,NxTsim);%用於保持每個時刻預測的所有狀態值 q=[1 0 0;0 1 0;0 0 0.5]; Q_cell=cell(Tsim,Tsim); for i=1:1:Tsim for j=1:1:Tsim if i==j Q_cell{i,j}=q; else Q_cell{i,j}=zeros(Nx,Nx); end end end Q=cell2mat(Q_cell); R=0.1eye(NuTsim,NuTsim); for i=1:1:Nr t_d =Xout(i,3); a=[1 0 -vd1sin(t_d)T; 0 1 vd1cos(t_d)T; 0 0 1;]; b=[cos(t_d)T 0; sin(t_d)T 0; 0 T;];
A_cell=cell(Tsim,1); B_cell=cell(Tsim,Tsim); for j=1:1:Tsim A_cell{j,1}=a^j; for k=1:1:Tsim if k<=j B_cell{j,k}=(a^(j-k))*b; else B_cell{j,k}=zeros(Nx,Nu); end end end A=cell2mat(A_cell); B=cell2mat(B_cell);

b_cons=[];
lb=[-1;-1];
ub=[1;1];
tic
[X,fval(i,1),exitflag(i,1),output(i,1)]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub);
toc
X_PIAO(i,:)=(A*x_piao(i,:)'+B*X)';
if i+j<Nr
     for j=1:1:Tsim
         XXX(i,1+3*(j-1))=X_PIAO(i,1+3*(j-1))+Xout(i+j,1);
         XXX(i,2+3*(j-1))=X_PIAO(i,2+3*(j-1))+Xout(i+j,2);
         XXX(i,3+3*(j-1))=X_PIAO(i,3+3*(j-1))+Xout(i+j,3);
     end
else
     for j=1:1:Tsim
         XXX(i,1+3*(j-1))=X_PIAO(i,1+3*(j-1))+Xout(Nr,1);
         XXX(i,2+3*(j-1))=X_PIAO(i,2+3*(j-1))+Xout(Nr,2);
         XXX(i,3+3*(j-1))=X_PIAO(i,3+3*(j-1))+Xout(Nr,3);
     end
end
u_piao(i,1)=X(1,1);
u_piao(i,2)=X(2,1);
Tvec=[0:0.05:4];
X00=x_real(i,:);
vd11=vd1+u_piao(i,1);
vd22=vd2+u_piao(i,2);
XOUT=dsolve('Dx-vd11*cos(z)=0','Dy-vd11*sin(z)=0','Dz-vd22=0','x(0)=X00(1)','y(0)=X00(2)','z(0)=X00(3)');
 t=T; 
 x_real(i+1,1)=eval(XOUT.x);
 x_real(i+1,2)=eval(XOUT.y);
 x_real(i+1,3)=eval(XOUT.z);
 if(i<Nr)
     x_piao(i+1,:)=x_real(i+1,:)-Xout(i+1,:);
 end
u_real(i,1)=vd1+u_piao(i,1);
u_real(i,2)=vd2+u_piao(i,2);

figure(1);

hold on;

title('跟蹤結果對比');
xlabel('橫向位置X');
axis([-1 5 -1 3]);
ylabel('縱向位置Y');
hold on;
for k=1:1:Tsim
     X(i,k+1)=XXX(i,1+3*(k-1));
     Y(i,k+1)=XXX(i,2+3*(k-1));
end
X(i,1)=x_real(i,1);
Y(i,1)=x_real(i,2);
plot(X(i,:),Y(i,:),'y')
hold on;

end %% 以下為繪圖部分 figure(2) subplot(3,1,1);

hold on;

%grid on; %title('狀態量-橫向座標X對比'); xlabel('取樣時間T'); ylabel('橫向位置X') subplot(3,1,2); plot(Tout(1:Nr),Xout(1:Nr,2),'k--'); hold on;

%grid on; %title('狀態量-橫向座標Y對比'); xlabel('取樣時間T'); ylabel('縱向位置Y') subplot(3,1,3);

hold on;

%grid on; hold on; %title('狀態量-\theta對比'); xlabel('取樣時間T'); ylabel('\theta')

figure(3) subplot(2,1,1);

%grid on; %title('控制量-縱向速度v對比'); xlabel('取樣時間T'); ylabel('縱向速度') subplot(2,1,2)

%grid on; %title('控制量-角加速度對比'); xlabel('取樣時間T'); ylabel('角加速度')

figure(4) subplot(3,1,1);

%grid on; xlabel('取樣時間T'); ylabel('e(x)'); subplot(3,1,2);

%grid on; xlabel('取樣時間T'); ylabel('e(y)'); subplot(3,1,3);

%grid on; xlabel('取樣時間T'); ylabel('e(\theta)');

```

三、執行結果

四、備註

版本：2014a