2_三角形编队算法说明1.docxVIP

三角形编队算法说明 本文档介绍如何实现3个无人机绕等边三角轨迹顺次循环飞行。飞机摆放位置如下图，x轴和y轴分别表示地磁方向的北和东；该等边三角形由外接圆半径表示，其中外接圆半径为r=6米，三角形边长为：√3 图中的A、B、C等分别表示的是1、2、3号等飞机的初始位置；其中，f、g、h分别表示第一段间距、第二段间距、第三间距等；图中箭头表示飞机的运动方向。 %%%%%%%%%%%%%3机-三角形:开始 %%%%%%%%%%%%%3机-三角形:开始 theta_1=2*pi*120/360; theta_2=2*pi*150/360; r=1.2; %%等边三角形外接圆半径 pgap=sqrt(3)*r; %%等边三角形边长 vel=0.5; %%% 飞机的期望速度 clen=sqrt(3)*r; tgap=pgap/vel; %%%%飞行一个间距需要的时间。 a=floor((t+(num-1)*tgap)/(clen/vel)); %%%a表示：飞机id为num的飞机在某个间距飞行 b=mod(a,3); b=mod(a,3); %%%%8-表示8个飞机；b表示飞机在第某间距飞行，去掉周期数。 c=mod((t+(num-1)*tgap),clen/vel); %飞机飞行一段间距所需要的时间。 if b==0 %某飞机在第一段