【无人机编队】狭窄空间环境中多旋翼无人机自重构 V 型编队matlab代码-优快云博客

本文聚焦狭窄空间环境下多旋翼无人机编队控制难题，提出自重构V型编队算法。该算法基于V型编队、自重构和分布式控制思想，通过初始化、局部目标计算等流程实现编队控制。仿真实验表明，算法能让无人机在狭窄空间保持稳定编队并适应空间限制，应用前景广阔。

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，代码获取、论文复现及科研仿真合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

🔥 内容介绍

在狭窄空间环境中，多旋翼无人机编队控制面临着极大的挑战。本文提出了一种新的自重构 V 型编队算法，该算法能够使无人机在狭窄空间内保持稳定的 V 型编队，并能够自动调整编队形状以适应不同的空间限制。

引言

多旋翼无人机编队控制技术在近几年得到了广泛的研究，并被应用于各种领域，如协作探索、搜索救援和编队表演等。在实际应用中，无人机编队经常需要在狭窄的空间环境中执行任务，如室内环境、隧道或峡谷等。然而，在狭窄空间中控制无人机编队面临着极大的挑战，主要表现在以下几个方面：

**空间限制：**狭窄空间对无人机的机动性提出了限制，使得无人机难以保持编队形状和相对位置。
**障碍物：**狭窄空间中通常存在大量的障碍物，如墙壁、柱子或其他物体，这些障碍物会阻碍无人机的运动并增加编队控制的难度。
**信息共享：**在狭窄空间中，无人机之间的通信可能会受到阻碍，这使得无人机难以共享信息并协调编队动作。

自重构 V 型编队算法

为了解决狭窄空间环境中多旋翼无人机编队控制的挑战，本文提出了一种新的自重构 V 型编队算法。该算法基于以下几个关键思想：

**V 型编队：**V 型编队是一种常见的编队形状，具有良好的稳定性和机动性。在狭窄空间中，V 型编队可以有效地利用空间，并减少无人机之间的碰撞风险。
**自重构：**自重构是指编队能够在没有外部干预的情况下自动调整其形状和相对位置。自重构能力对于在狭窄空间中保持稳定的编队至关重要。
**分布式控制：**分布式控制是指每个无人机根据局部信息和与相邻无人机的通信来控制自己的动作。分布式控制可以提高编队的鲁棒性和适应性。

算法流程

自重构 V 型编队算法的流程如下：

**初始化：**初始化无人机的位置和速度，并确定 V 型编队的目标形状和相对位置。
**局部目标计算：**每个无人机根据局部信息（如自身位置和速度、相邻无人机的位置和速度）计算自己的局部目标位置和速度。
**编队调整：**每个无人机根据局部目标和与相邻无人机的通信调整自己的动作，以实现编队形状和相对位置的重构。
**障碍物避障：**如果检测到障碍物，无人机会自动调整自己的动作以避开障碍物，同时保持编队形状和相对位置。
**信息共享：**无人机通过无线通信共享信息，包括位置、速度和障碍物信息。

仿真实验

为了验证算法的有效性，进行了仿真实验。仿真环境是一个狭窄的隧道，其中包含多个障碍物。实验结果表明，该算法能够使无人机在狭窄空间内保持稳定的 V 型编队，并能够自动调整编队形状以适应不同的空间限制。

结论

本文提出了一种新的自重构 V 型编队算法，该算法能够使多旋翼无人机在狭窄空间环境中保持稳定的 V 型编队。该算法基于 V 型编队、自重构和分布式控制的思想，具有良好的稳定性、适应性和鲁棒性。仿真实验结果表明，该算法能够有效地解决狭窄空间环境中多旋翼无人机编队控制的挑战，并具有广阔的应用前景。

📣 部分代码

% =========================================================================% Import the s-parameters from the .s2p data filefunction [freq,S11,S12,S22]=Reads2p()FILE_NAME=input('File name (.s2p): ','s');S= sparameters(FILE_NAME);freq = S.Frequencies;S11 = rfparam(S,1,1);S12 = rfparam(S,1,2);S22 = rfparam(S,2,2);end%% Vector Fitting for S-parameters% =========================================================================% It is a code for extracting the coupled-resonator circuit of a microwave% filter. The method used to fit the sampled data is the vector fitting (VF).% Operating steps;% 1. Remove the phase loading by using high-order rational function.% 2. Fit the sampled data by the VF.% 3. Transmission zeros selection.% 4. Calculate the transversal coupling matrix by using the Y-parameters.% 5. Transform the transversal coupling matrix into the matrix with the% target topology.% Note: only suit the filter with the real-frequency transmission zeros.% Data: 2023/06/01 By YellowBook% Ref1: Circuit Model Extraction of Parallel-Connected Dual-Passband% Coupled-Resonator Filters% Ref2: A new computer-aided tuning scheme for general lossy % coupled-resonator bandpass filters based on the Cauchy method% =========================================================================clearclose allwarning off% The warning is about the polt issue... ignore itformat shortSize = 18; % size of figureTZrange = 0.25; % for selecting TZs with the realpart lower than range% ======================= Loop Parameters =======================X = 18; % Max number of iterationserr_ak_limit = -60;    % in dBerr_cp_limit = -60;  % in dB% ======================= Filter Parameters =======================%--------------------------------------------------------------------------% Example 1 (simualted data of a 9th-order filter with 3 Tzs)：N = 9; % Order of the filterNz = 3; % Number of the transmission zeros % If the extracted Tz is not correct,plz increase Nz and % create a new parasitic coupling to explain the additional TzCF = sqrt(1920*1980)*1e6; % Hz center mapping frequencyBW = (1980-1920)*1e6; % Hz mapping bandwidthFBW = BW/CF;M = 2; % Additional order of the rational function to remove the phase loadingLine = 10; % Radius to classify the poles and zeros% % Set fitting frequency range in the lowpass rangew1 = -3.5;     w2 = 3.5;%--------------------------------------------------------------------------% Example 2 (measured data of a 6th-order filter with 2 Tzs)：% N = 6; % Order of the filter% Nz = 2; % Number of the transmission zeros % % If the extracted Tz is not correct,plz increase Nz and % % create a new parasitic coupling to explain the additional Tz% CF = sqrt(1920*1980)*1e6; % Hz center mapping frequency% BW = (1980-1920)*1e6; % Hz mapping bandwidth% FBW = BW/CF;% M = 1; % Additional order of the rational function to remove the phase loading% Line = 10; % Radius to classify the poles and zeros% % % Set fitting frequency range in the lowpass range% w1 = -4;     % w2 = 4;%--------------------------------------------------------------------------% ======================= Sampling data import =======================[F_band,Origin_S11,Origin_S12,Origin_S22]=Reads2p();Ns=length(F_band);% number of sampling dataOrigin_dBS11(:,1)=20*log10(abs(Origin_S11(:,1)));Origin_dBS12(:,1)=20*log10(abs(Origin_S12(:,1)));Origin_dBS22(:,1)=20*log10(abs(Origin_S22(:,1)));w_low=(F_band./CF-CF./F_band)/FBW;s_low=1i*w_low;[~,i1]=min(abs(w_low-ones(Ns,1)*w1));[~,i2]=min(abs(w_low-ones(Ns,1)*w2));% De_Embedding_2p is a function to fit S11 and S22 independently[S11,S12,S22]=De_Embedding_2p_onlyarg(F_band,Origin_S11,Origin_S12,Origin_S22,N,CF,BW,M,Line);% find the fitting rangeNs = i2 - i1 + 1;S11 = S11(i1:i2);S12 = S12(i1:i2);S22 = S22(i1:i2);s_low = s_low(i1:i2);F_band = F_band(i1:i2);Origin_dBS11 = Origin_dBS11(i1:i2);Origin_dBS12 = Origin_dBS12(i1:i2);Origin_dBS22 = Origin_dBS22(i1:i2);% % 添加高斯白噪声，幅度值为-80dB--0.0001% Mag_noise = 10^(-1*((100+3)/20)); %dB% noise11 = Mag_noise*(randn(Ns,1) + 1i*randn(Ns,1));% noise22 = Mag_noise*(randn(Ns,1) + 1i*randn(Ns,1));% noise12 = Mag_noise*(randn(Ns,1) + 1i*randn(Ns,1));% S11 = S11 + noise11;% S22 = S22 + noise22;% S12 = S12 + noise12;Z0=1;% ======================= Initial pole ak =======================for index=1:N    ak(index,1)=-1+2.1/(N-1)*(index-1);    ak(index,1)=-0.01*abs(ak(index,1))+ak(index,1)*1i;end% ======================= Iterate Max X times =======================A=zeros(N,N);b=ones(N,1);A1=zeros(Ns,N+1);A2=zeros(Ns,N);A3=zeros(Ns,Nz+1);M_S11 = diag(S11);M_S12 = diag(S12);M_S22 = diag(S22);% Weight of the fittingW12 = diag(1./(abs(S12).^0.5));W11 = eye(Ns);W22 = eye(Ns);tmp_ak = ak;cp_plot = zeros(N, X);err_ctl = zeros(1, X);for index=1:X        A_den=ones(Ns,1);        for index1 = 1:N        A2(:,index1) = 1./(s_low - ones(Ns,1).*ak(index1));        A_den(:,1) = A2(:,index1).*A_den(:,1);    end        for index1 = 1:N+1        A3(:,index1) = (s_low.^(index1-1)).*A_den;    end     A1 = [A2,ones(Ns,1)];    A = diag(ak);%============================ Original=====================================        left=[W11*A1           zeros(Ns,N+1)   zeros(Ns,N+1)   -1*W11*M_S11*A2;          zeros(Ns,N+1)    W12*A3          zeros(Ns,N+1)   -1*W12*M_S12*A2;          zeros(Ns,N+1)    zeros(Ns,N+1)   W22*A1          -1*W22*M_S22*A2];    right=[W11*S11;           W12*S12;           W22*S22];    Call=lsqminnorm(left,right); % This function can be replaced by \        C=mat2cell(Call,[N+1,N+1,N+1,N],1);    cp=C{4,1};    cp_plot(:,index)=10*log10(abs(cp));    ak=eig(A-b*cp.');  err_ctl(index) = sum(abs(ak - tmp_ak));  if (10*log10(err_ctl(index)) < err_ak_limit) && (max(cp_plot(:,index)) < err_cp_limit)    break  end  tmp_ak = ak;%==============================end=========================================end%==============================  Disp loop time  ======disp('Loop time:')disp(index)figure('name','MVF-error')subplot(1,2,1);plot([1:index],max(cp_plot(:,[1:index])),'b','Linewidth',1.5);legend('cp', 'Location', 'NorthEast')title('Cp error in Log')grid onsubplot(1,2,2);plot([1:index],10*log10(err_ctl([1:index])),'r','Linewidth',1.5);legend('ak', 'Location', 'NorthEast')title('ak error in Log')grid onC11=C{1,1};C12=C{2,1};C22=C{3,1};% ======================= Calculate ExtrS =======================ES11 = A1*C11;ES12 = A3*C12;ES22 = A1*C22;dBES11(:,1)=20*log10(abs(ES11(:,1))); dBES12(:,1)=20*log10(abs(ES12(:,1)));dBES22(:,1)=20*log10(abs(ES22(:,1)));figure('name','S')plot(imag(s_low),Origin_dBS11,'r',imag(s_low),Origin_dBS12,'b',imag(s_low),dBES11,'r:',imag(s_low),dBES12,'b:','linewidth',2);legend('S11 Sim.','S12 Sim.','S11 Fit.','S12 Fit.', 'Location', 'NorthWest')% legend('S11 Mea.','S12 Mea.','S11 CM.','S12 CM.', 'Location', 'NorthWest')ylabel('S-parameters (dB)','fontsize',Size);% set(gca,'FontName','Times New Roman');set(gca,'FontSize',Size);set(gca,'linewidth',1.2);xlabel('\omega (rad/s)','fontsize',Size);% set(gca,'FontName','Times New Roman');set(gca,'FontSize',Size);set(gca,'linewidth',1.2);% xlim([-5,5]);ylim([-150,0]);yticks([-150:20:0]);xlim([s_low(1)/1i,s_low(Ns)/1i]);xticks([-2:1:2]);grid onTzall = roots(flipud(C12));% Selecting TZfor i = 1:N    if abs(real(Tzall(i))) > TZrange        Tz_t(i)  = 10;    else        Tz_t(i) = Tzall(i);    endendTz_t = sort(Tz_t);Tz = Tz_t(1:Nz);figure('name','TZ')plot(real(Tzall),imag(Tzall),'bo','MarkerSize', 10,'linewidth',2);hold onplot(real(Tz),imag(Tz),'rs','MarkerSize', 15,'linewidth',2);hold onlegend('拟合的零点','选择的零点', 'Location', 'NorthWest')% legend('S11 Mea.','S12 Mea.','S11 CM.','S12 CM.', 'Location', 'NorthWest')ylabel('虚部','fontsize',Size);% set(gca,'FontName','Times New Roman');set(gca,'FontSize',Size);set(gca,'linewidth',1.2);xlabel('实部','fontsize',Size);% set(gca,'FontName','Times New Roman');set(gca,'FontSize',Size);set(gca,'linewidth',1.2);% xlim([-3,3]);% ylim([-3,3]);% yticks([-3:1:3]);% xticks([-3:1:3]);grid on% 通过留数与极点确定零点A = diag(ak);Rz11 = eig(A-b*C11(1:end-1).'./C11(end));Rz22 = eig(A-b*C22(1:end-1).'./C22(end));F = poly(Rz11);F22 = poly(Rz22);P = poly(Tz);Fit = abs(polyval(P,s_low)./polyval(F,s_low)); % 估计值Sim = abs(S12./S11); % 仿真值Er = (Sim.'*Fit)/(Sim.'*Sim);if mod(N-Nz,2) == 0    Er = Er*1i;endP = P./Er;P = [zeros(1,N-Nz), P];TE = conv(F,F22) - conv(P,P);allpole = roots(TE);figure('name','K')plot(imag(s_low),db(Sim),'b-',imag(s_low),db(Fit./Er),'k--','Linewidth',2);% legend('Pole', 'NorthWest')legend('Simulated K','Fitted K', 'NorthWest')ylabel('Magnitude (dB)','fontsize',Size);set(gca,'FontName','Times New Roman');set(gca,'FontSize',Size);set(gca,'linewidth',1.2);xlabel('\omega (rad/s)','fontsize',Size);set(gca,'FontName','Times New Roman');set(gca,'FontSize',Size);set(gca,'linewidth',1.2);grid onallpole = sort(allpole,'ComparisonMethod','real');poleL = allpole(1:N);poleR = allpole(N+1:2*N);E = poly(poleL);T = poly(poleR);figure('name','P')plot(real(poleL),imag(poleL),'bx',real(poleR),imag(poleR),'b+','Linewidth',1.5,'Markersize',10);legend('Roots of E','Roots of T', 'NorthWest')ylabel('Imaginary part','fontsize',Size);set(gca,'FontName','Times New Roman');set(gca,'FontSize',Size);set(gca,'linewidth',1.2);xlabel('Real part','fontsize',Size);set(gca,'FontName','Times New Roman');set(gca,'FontSize',Size);set(gca,'linewidth',1.2);grid onYd = E + F + F22 + T;Y11n = E - F + F22 - T;Y12n = -2*P;Y22n = E + F - F22 - T;[residueY11,Ypole,~] = residue(Y11n,Yd);[residueY12,Ypole,~] = residue(Y12n,Yd);[residueY22,Ypole,~] = residue(Y22n,Yd);% ======================= Y to TCM =======================M=zeros(N+2,N+2);for index=1:N    M(index+1,index+1)=1i*Ypole(index,1);    if abs(residueY11(index,1))>=abs(residueY22(index,1))        M(1,index+1)=sqrt(residueY11(index,1));        M(N+2,index+1)=residueY12(index,1)/sqrt(residueY11(index,1));    else if abs(residueY11(index,1))<abs(residueY22(index,1))        M(N+2,index+1)=sqrt(residueY22(index,1));        M(1,index+1)=residueY12(index,1)/sqrt(residueY22(index,1));        end    end    M(index+1,1)=M(1,index+1);    M(index+1,N+2)=M(N+2,index+1);endCM = to_foldedCM(N,M);fprintf('The extracted folded coupling matrix (real part):\n');display(real(CM));  fprintf('The extracted folded coupling matrix (imaginary part):\n');display(imag(CM)); % % ======================= Analyse CM  =======================[ExtrS11,ExtrS22,ExtrS12]=analyseCM(N,F_band,CM,CF,FBW,Ns);dBExtrS11(1,:)=20*log10(abs(ExtrS11(1,:)));dBExtrS22(1,:)=20*log10(abs(ExtrS22(1,:)));dBExtrS12(1,:)=20*log10(abs(ExtrS12(1,:)));figure('name','S-Parameters');plot(F_band/10^6,db(S11),'-','Color',[0.75, 0.75, 0.75],'Linewidth',2);hold onplot(F_band/10^6,db(S12),'-','Color',[0.75 0.75 0.75],'Linewidth',2);hold onplot(F_band/10^6,dBExtrS11,'k:',F_band/10^6,dBExtrS12,'k--','Linewidth',2);hold onlegend('S11 仿真','S12 仿真','S11 提取','S12 提取', 'Location', 'NorthWest')set(gca,'FontSize',Size);set(gca,'linewidth',1.2);ylabel('S参数幅度 (dB)','fontsize',Size);% set(gca,'FontName','Times New Roman');set(gca,'linewidth',1.2);xlabel('频率 (MHz)','fontsize',Size);% set(gca,'FontName','Times New Roman');set(gca,'linewidth',1.2);% xlim([-5,5]);ylim([-150,0]);% xticks([-5:1:5]);yticks([-150:20:0]);% grid onxlim([F_band(1)/10^6,F_band(Ns)/10^6]);% grid on