【路径规划】无人机编队路径规划matlab源码
关于编队飞行研究的问题无外乎以下4个：
编队队形的生成：如何将多个无人机进行联系起来，完成编队队形的生成。
编队队形的保持：当无人机编队队形形成之后，如何保持且按照系统设定的编队队形进行飞行。
编队队形的变换：如果需要变换无人机编队飞行的队形，如何变换无人机编队的队形，即换成队形。
编队队形的避障：系统编队飞行时，遇到障碍物时，如何运动飞行从而避开障碍物；
在研究无人机编队飞行算法前，需要去了解各个无人机之间的通信方法，通信方式可以将机群的控制方法分为以下几种：
集中式控制方法：编队系统中的个体都会互相通信，互相传递速度、坐标位置、运动状态等信息。采用此种控制方法的系统飞行效果较好，因为编队系统中的每个无人机都知道编队系统中所有的信息，能做出更加科学的飞行决策和路线。
但此方式所带来的缺点较为明显，无人机个体之间都需要进行多个通信，需要互相传递较为多的数据信息，对无人机个体的计算速度以及内存都要求比较高。数据量较大还会导致无人机产生丢失关键信息的问题。
分布式控制方法：就不存在上述信息量较大导致丢失的情况，无人机编队系统中的个体只需要和领域的无人机进行通信，通信数据包以及通信链路都比较少。与集中式控制方法相对，采用此种控制方法的编队系统飞行效果会差一些。
正因为只无人机个体只需要和领域无人机通信，反而更好在实际应用中实现该控制方法。除此之外，该控制方法不牵扯到无人机编队系统其它无人机个体，可以极为方便的对无人机编队系统中的无人机个体进行删减或增加。
比如，当编队无人机系统中的某个个体出现故障导致编队系统中的个体缺失时，对系统整体的影响较小，并可以实时的补充上其他无人机个体，让编队系统快速恢复正常的工作状态。
分散式控制方法中：编队系统中的个体无人机之间不会进行通信，在编队系统中，会约定好飞行固定点，系统中的无人机个体正常保持与固定点的相对关系就可以了。由此可见，采用此种控制方法的编队系统计算量更小，但其带来的编队飞行效果是极差的，由于编队系统中的无人机个体不相互通信，可能会带来无人机之间的碰撞从而发生严重事故。
了解通信方式之后，下面可以介绍关于无人机编队控制的一些控制方法：
1）跟随领航的控制方法。首先设定无人机编队系统中的一架作为领航无人机，其他个体为跟随无人机，在编队飞行时，跟随无人机实时跟随领航无人机进行飞行。在此种控制算法中，由于有领航无人机和跟随无人机的这种相对运动模式，我们可以将无人机编队系统的队形控制问题转换成跟随无人机跟随领航者的位置运动情况。
这种控制方法被许多学者研究并实现，其优点在于复杂的多个个体之间的问题可以转换成单个个体的运动情况研究。降低了个体研究的数量。但缺点也很明显，整个无人机编队系统的稳定性都由领航者决定，依赖性较强，一旦领航者出现问题，整个系统将出现崩溃，除此之外，编队系统也容易受外界干扰的影响。
2）虚拟结构法。虚拟结构法的主要思想是将无人机编队系统的队形组成看作是刚性的虚拟结构，在无人机编队飞行运动期间，单个无人机个体可以看做是固定在虚拟结构上的固定位置上，一旦无人机编队队形发生改变，编队系统中的无人机个体直接跟踪保持虚拟结构上的固定坐标点就可以完成设定好的编队飞行巡检路线。
3）人工势场法。人工势场法在无人机航线轨迹规划上用的较多。这是一种利用物理吸引力和排斥力的概念的虚拟力方法，障碍物和目标点会对无人机会产生不同的力，目标点对无人机具有吸引力，障碍物对无人机具有排斥力，两者的结合即合力对无人机产生加速力，从而控制无人机的运动。人工势场法的优点在于原理简单和实时性较好，但对无人机的运动学约束问题无法进行处理，所以此类方法用在无人机编队飞行的研究比较少。
clear all
close all
clc
syms x1
syms x2
grid on
hold on
axis equal
N = 4;
d = 1.5;
pos = 3*rand(2,N);
pos = [pos; rand(1,N)];
pos(1:2,1) = [5;0];
[pos,obst,target,dist_matrix,lambda,sec_dist,mu,Cr,d,tr1,tr2,tr3,tr4] = formazione_rombo(pos,d,[],[],1,0.02);
initial_pos = pos;
plot3(initial_pos(1,1),initial_pos(2,1),initial_pos(3,1),\'s\',\'MarkerSize\',10, \'MarkerFaceColor\',\'y\',\'HandleVisibility\',\'off\');
plot3(initial_pos(1,2:4),initial_pos(2,2:4),initial_pos(3,2:4),\'s\',\'MarkerSize\',10, \'MarkerFaceColor\',\'g\');
G = create_graph(pos,sqrt(2)*d);
while norm(pos(:,1)-target(:,1)) > 0.1
pos(:,1) = adjustement(pos(:,1),obst,target,x1,x2,0,10*lambda,0.8,mu,Cr,G,1);
pos = adjustement(pos,obst,[],x1,x2,dist_matrix,5*lambda,1,mu,Cr,G,2);
G = create_graph(pos,sqrt(2)*d);
check = checkDist(pos,dist_matrix,G)
%ripristino la formazione se si sminchia troppo
while check > 2
pos = adjustement(pos,obst,[],x1,x2,dist_matrix,3*lambda,sec_dist,mu,Cr,G,1);
G = create_graph(pos,sqrt(2)*d);
check = checkDist(pos,dist_matrix,G)
end
tr1 = [[tr1] [pos(:,1)]];
tr2 = [[tr2] [pos(:,2)]];
tr3 = [[tr3] [pos(:,3)]];
tr4 = [[tr4] [pos(:,4)]];
disp(norm(pos(:,1)-target(:,1)));
end
G = create_graph(pos,sqrt(2)*d);
check = checkDist(pos,dist_matrix,G);
if check >0.15
[pos,~,~,~,~,~,~,~,~,tr11,tr22,tr33,tr44] = formazione_rombo(pos,d,[],obst,2,0.02);
else
tr11 = [];
tr22 = [];
tr33 = [];
tr44 = [];
end
tr1 = [tr1 tr11];
tr2 = [tr2 tr22];
tr3 = [tr3 tr33];
tr4 = [tr4 tr44];
plot3(tr1(1,:),tr1(2,:),tr1(3,:),\'--\');
plot3(pos(1,1),pos(2,1),pos(3,1),\'s\',\'MarkerSize\',10, \'MarkerFaceColor\',\'y\');
plot3(pos(1,2:4),pos(2,2:4),pos(3,2:4),\'s\',\'MarkerSize\',10, \'MarkerFaceColor\',\'r\');
plot3(obst(1,:),obst(2,:),obst(3,:),\'*b\')
plot3(target(1,1),target(2,1),target(3,1),\'-o\',\'MarkerSize\',15);
plot3(tr2(1,:),tr2(2,:),tr2(3,:),\'--\');
plot3(tr3(1,:),tr3(2,:),tr3(3,:),\'--\');
plot3(tr4(1,:),tr4(2,:),tr4(3,:),\'--\');
legend(\'Initial Position\',\'Trajectory\',\'Leader\',\'Final Position\',\'Obstacle\',\'Final Target\',\'FontSize\',14);
