m基于flocking算法的无人机群空间避障飞行matlab仿真,对比二维场景和三维场景

1.算法描述

        近年来，随着通信网络、人工智能、自主系统、大数据的前沿技术的发展， 无人机蜂群作战也正在由概念变成现实，从理论走向实践。航迹规划，多机协作， 集群控制等问题成为当下研究热点。在军事作战中派遣多无人机协同作战相比于 单无人机能够在穿透敌方防御系统，探测目标以及执行攻击任务等方面更具优势 [1]，采用大规模、低成本的无人机蜂群进行低空突防,实施饱和攻击已成为一种全 新的"非对称"对抗战术[2]。在各种民事应用中，无人机蜂群已经被广泛用于环境 和自然灾害监测、边境监视、突发事件援助、搜索和救援、货物传递和建筑等任务。随着航空技术与人工智能的迅速发展，无人机以其操作灵活，成本低廉等特 点，在民用及军事领域都有广阔的应用前景。

        基于无人机(Unmanned Aerial Vehicle, UAV)的各种应用是一个近年来得到飞速发展的高技术领域。由于单架 UAV 存在能力受限、可靠性弱等缺点，由多 UAV 协作构成多 UAV 蜂群可以更高效、更可靠、更低代价地支持完成各种任务。

       无人机蜂群是由若干配备多种任务载荷的低成本小型无人机组成，参照蜜蜂 等昆虫的集体行动模式，以智能化无人控制技术和网络信息系统为支撑，在人类指挥或监督下共同完成特定作战任务。无人机蜂群可以是同构的,也可以是异构 的，其以数量众多、群体智能为典型特征,可实现协同任务分配、协同探测和协同 攻击，能够极大增强无人机的通信能力和抗毁伤能力，扩展无人机对战场信息的 感知获取能力，提高无人机协同执行任务的能力。

       Flocking(有时也称为是warming或herding)，拥有4项简单的规则，把它们组合在一起时，为自治主体群给出一个类似于鸟群、鱼群的群体行为的逼真形式。定向行为规则：分离原则、列队原则、聚合原则、躲避原则

        分离原则：定向时要避免与本地flock同伴拥挤。即定时检测邻近同伴，避免拥挤；

        列队原则：驶向本地flock同伴的平均航向。即检测邻近同伴航向、速度，获取平均值并调整自己；

        聚合原则：定向时朝着本地flock同伴的平均位置移动。即检测邻近同伴，平均位置然后调整其匹配航向；

        躲避原则：使避免撞上局部区域内的障碍或敌人。即“向前看一段距离”，遇到障碍物、敌人调整航向、速度进行躲避。

       无人机蜂群网络是一种由大量节点组成的分层、多簇结构的无线通信网络。 针对多无人机协同任务分配问题，提出了一种新的系统框架。将其转化为一个组合优化问题，并用改进的聚类算法进行求解。目的是使多无人机以低能耗完 成任务。随着无人机数量的增加，无人机之间的碰撞等飞行安全问题也随之出现，提出了一种基于改进人工势场的多无人机抗碰撞改进方法。针对联网无人机易受到各种网络攻击的问题，提出了一种基于入侵检测系统 (IDS)的多无人机执行任务时抵抗网络攻击的方法。为了进一步提高任务分配的 准确性，提出了一种改进的方法。此外，提出了一种在线实时路径规划和任务重 新分配方法，以增强多无人机的鲁棒性，特别是在任务中增加一个紧急目标等突 发问题的响应。最后，数值仿真和真实的物理飞行实验表明，该方法为多无人机 任务分配提供了一种可行的解决方案，与其他任务分配方法相比，该方法具有良好的性能。

2.仿真效果预览

matlab2022a仿真结果如下：

3.MATLAB核心程序

for i = 1:UAV_num        
    if norm(Speed_0(i,:))~=0 
       Speed_0(i,:) = Speed_0(i,:)/norm(Speed_0(i,:));
    end
    while 1
        %初始化随机位置
        Position_0(i,:) = [rand*Width/4,randn*Width/40+Width/2, randn*Width/40+Width/2,atan2(Speed_0(i,2), Speed_0(i,1))];
        %计算任意两个无人机之间的距离
        d = func_dist_btuav(Position_0(1:i-1,1:3), Position_0(i,1:3));
        if( all(d>0.5))
            break
        end
    end
end
 
Position = Position_0; 
Speed    = Speed_0;
 
..............................................................
for t = 0:ts:TIMES
    t
    i=i+1;
    %flocking控制，输出速度变量
    Speed1          = func_flocking_Speed(Position(:,[1,2,4]),Speed(:,[1,2]),Radius);
    Speed2          = func_flocking_Speed(Position(:,[2,3,4]),Speed(:,[2,3]),Radius);
    Speed3          = func_flocking_Speed(Position(:,[1,3,4]),Speed(:,[1,3]),Radius); 
    Speed           = [Speed1(:,1)+Speed3(:,1),Speed1(:,2)+Speed2(:,1),Speed2(:,2)+Speed3(:,2)]/2;
 
    
    for j = 1:UAV_num
        theta  = atan((Goal(2)-Position(j,2))/(Goal(1)-Position(j,1))); 
        theta_ = atan((Goal(3)-Position(j,3))/(Goal(2)-Position(j,2))); 
        for jc = 1:2
            dist(jc) = sqrt((Position(j,1) - C(jc,1))^2 + (Position(j,2) - C(jc,2))^2 + (Position(j,3) - C(jc,3))^2);
        end
        %检测无人机和障碍物之间的间距，通过flocking控制器和PPO，作用到无人机，做出避障的运动决策
        %注意，如果采用PID+flocking的控制方式，响应速度kg大约为0.6左右，不同控制策略，响应速度不一样，如果要对比则修改kg参数
        [V,I] = min(dist);   
        if Position(j,1)<=max(C(1,1),C(2,1))
        
        if Position(j,2)>1.02*(C(1,2)+R(1))  &  Position(j,2)<0.98*(C(2,2)-R(2))
           vv(j,:)  = [0.8*Speedv,0,Speedv*sin(theta)*sin(theta_)]; 
        else
            if Position(j,2)<=1.02*(C(1,2)+R(1)) %第二个障碍物
               if dist(I) >= (1+6*Kg)*R(I)%不避障
                  vv(j,:)  = [0.8*Speedv,0,Speedv*sin(theta)*sin(theta_)];  
               else%切线方式避障
                  theta0 = atan((C(1,2)-Position(j,2))/(C(1,1)-Position(j,1)));
                  d0     = sqrt((C(1,2)-Position(j,2))^2+(C(1,1)-Position(j,1))^2); 
                  theta1 = -real(asin(1.3*R(1)/d0));
                  if Position(j,2)<C(1,2)
                     thetas=theta1;
                  else
                     thetas=theta0+theta1;  
                  end
                  vv(j,:)= 1.5*[Speedv*cos(thetas),Speedv*sin(thetas),Speedv*sin(theta)*sin(theta_)]; 
               end
            end
            if Position(j,2)>=0.98*(C(2,2)-R(2))%第一个障碍物
               if dist(I) >= (1+6*Kg)*R(I)%不避障
                  vv(j,:)  = [0.8*Speedv,0,Speedv*sin(theta)*sin(theta_)];   
               else%切线方式避障
                  theta0 = atan((C(2,2)-Position(j,2))/(C(2,1)-Position(j,1)));
                  d0     = sqrt((C(2,2)-Position(j,2))^2+(C(2,1)-Position(j,1))^2); 
                  theta1 = real(asin(1.3*R(2)/d0));  
                  if Position(j,2)<C(2,2)
                     thetas=theta0+theta1;
                  else
                     thetas=theta1;  
                  end
                  vv(j,:)= 1.5*[Speedv*cos(thetas),Speedv*sin(thetas),Speedv*sin(theta)*sin(theta_)]; 
               end
            end
        end
        else
 
            
            vv(j,:)=[Speedv*cos(theta),Speedv*sin(theta)*cos(theta_),Speedv*sin(theta)*sin(theta_)];
        end
    end