【ABC三维路径规划】基于matlab人工蜂群算法多无人机三维路径规划【含Matlab源码 170期】

一、无人机简介

0 引言
随着现代技术的发展，飞行器种类不断变多，应用也日趋专一化、完善化，如专门用作植保的大疆PS-X625无人机，用作街景拍摄与监控巡察的宝鸡行翼航空科技的X8无人机，以及用作水下救援的白鲨MIX水下无人机等，决定飞行器性能主要是内部的飞控系统和外部的路径规划问题。就路径问题而言，在具体实施任务时仅靠操作员手中的遥控器控制无人飞行器执行相应的工作，可能会对操作员心理以及技术提出极高的要求，为了避免个人操作失误，进而造成飞行器损坏的危险，一种解决问题的方法就是对飞行器进行航迹规划。
飞行器的测量精度，航迹路径的合理规划，飞行器工作时的稳定性、安全性等这些变化对飞行器的综合控制系统要求越来越高。无人机航路规划是为了保证无人机完成特定的飞行任务，并且能够在完成任务的过程中躲避各种障碍、威胁区域而设计出最优航迹路线的问题。

1 常见的航迹规划算法

图1 常见路径规划算法
文中主要对无人机巡航阶段的航迹规划进行研究，假设无人机在飞行中维持高度与速度不变，那么航迹规划成为一个二维平面的规划问题。在航迹规划算法中,A算法计算简单，容易实现。在改进A算法基础上，提出一种新的、易于理解的改进A算法的无人机航迹规划方法。传统A算法将规划区域栅格化，节点扩展只限于栅格线的交叉点，在栅格线的交叉点与交叉点之间往往存在一定角度的两个运动方向。将存在角度的两段路径无限放大、细化，然后分别用两段上的相应路径规划点作为切点，找到相对应的组成内切圆的圆心，然后作弧，并求出相对应的两切点之间的弧所对应的圆心角，根据下式计算出弧线的长度

式中：R———内切圆的半径；
α———切点之间弧线对应的圆心角。

二、人工蜂群算法简介

1 蜜蜂采蜜
自然界中的蜜蜂总能在任何环境下以极高的效率找到优质蜜源，且能适应环境的改变。蜜蜂群的采蜜系统由蜜源、雇佣蜂、非雇佣蜂三部分组成，其中一个蜜源的优劣有很多要素，如蜜源花蜜量的大小、离蜂巢距离的远近、提取的难易程度等；雇佣蜂和特定的蜜源联系并将蜜源信息以一定概率形式告诉同伴；非雇佣蜂的职责是寻找待开采的蜜源，分为跟随蜂和侦查蜂两类，跟随峰是在蜂巢等待而侦查蜂是探测蜂巢周围的新蜜源。蜜蜂采蜜时，蜂巢中的一部分蜜蜂作为侦查蜂，不断并随机地在蜂巢附近寻找蜜源，如果发现了花蜜量超过某个阈值的蜜源，则此侦査蜂变为雇佣蜂开始釆蜜，采蜜完成后飞回蜂巢跳摇摆舞告知跟随峰。摇摆舞是蜜蜂之间交流信息的一种基本形式，它传达了有关蜂巢周围蜜源的重要信息如蜜源方向及离巢距离等，跟随峰利用这些信息准确评价蜂巢周围的蜜源质量。当雇佣蜂跳完摇摆舞之后，就与蜂巢中的一些跟随蜂一起返回原蜜源采蜜，跟随蜂数量取决于蜜源质量。以这种方式，蜂群能快速且有效地找到花蜜量最高的蜜源。

蜜蜂采蜜的群体智能就是通过不同角色之间的交流转换及协作来实现的。具体采蜜过程如图所示。在最初阶段，蜜蜂是以侦查蜂的形式出现，且对蜂巢周闱的蜜源没有任何了解，由于蜜蜂内在动机和外在的条件不同侦查蜂有两种选择：①成为雇佣蜂，开始在蜂巢周围随机搜索蜜源，如图中路线②成为跟随峰，在观察完摇摆舞后开始搜索蜜源，如图中路线。假设发现两个蜜源和，在发现蜜源后，该侦查蜂变成一只雇佣蜂，雇佣蜂利用其自身属性记住蜜源的位置，并立刻投入到采蜜中。采蜜完成后蜜蜂带着满载花蜜返回蜂巢，将花蜜卸载到卸蜜房，卸载完成后雇佣蜂有三种可能的行为①放弃自己发现的花蜜量不高的蜜源，变成一个不受约束的非雇佣蜂，如图中的路线；②在招募区跳摇摆舞，招募一些待在蜂巢中跟随峰，带领其再次返回所发现的蜜源如图中的路线；③不招募其他蜜蜂，继续回到原来的蜜源采蜜如图中的路线。在现实生活中并不是所有的蜜蜂一开始就立刻采蜜，另外大多数蜜蜂在一次采蜜完成后都会选择回到招募区跳摇摆舞来招募更多的蜜蜂去采蜜。

2 算法模型
人工蜂群算法就是模拟蜜蜂的采蜜过程而提出的一种新型智能优化算法，它也是由食物源、雇佣蜂和非雇佣蜂三部分组成。
食物源：食物源即为蜜源。在任何一个优化问题中，问题的可行解都是以一定形式给出的。在人工蜂群算法中，食物源就是待求优化问题的可行解，是人工蜂群算法中所要处理的基本对象。食物源的优劣即可行解的好坏是用蜜源花蜜量的大小即适应度来评价的。
雇佣蜂：雇佣蜂即为引领蜂与食物源的位置相对应，一个食物源对应一个引领蜂。在人工蜂群算法中，食物源的个数与引领蜂的个数相等；引领蜂的任务是发现食物源信息并以一定的概率与跟随蜂分享；概率的计算即为人工蜂群算法中的选择策略，一般是根据适应度值以轮盘赌的方法计算。
非雇佣蜂：非雇佣蜂包括跟随蜂和侦査蜂跟随蜂在蜂巢的招募区内根据引领蜂提供的蜜源信息来选择食物源，而侦查蜂是在蜂巢附近寻找新的食物源。在人工蜂群算法中，跟随蜂依据引领蜂传递的信息，在食物源附近搜索新食物源，并进行贪婪选择。若一个食物源在经过次后仍未被更新，则此引领蜂变成侦査蜂，侦查蜂寻找新的食物源代替原来的食物源。

3 算法搜索过程

4 分类
人工蜂群算法中将人工蜂群分为引领蜂、跟随蜂和侦查蜂三类，每一次搜索过程中，引领蜂和跟随蜂是先后开采食物源，即寻找最优解，而侦查蜂是观察是否陷入局部最优，若陷入局部最优则随机地搜索其它可能的食物源。每个食物源代表问题一个可能解，食物源的花蜜量对应相应解的质量（适应度值fiti）。
（1）人工蜂群算法搜索过程中，首先需要初始化，其中包括确定种群数、最大迭代次数MCN、、控制参数limit和确定搜索空间即解的范围，在搜索空间中随机生成初始解xi(i=1,2,3,……,SN)，SN为食物源个数，每个解xi是一个D维的向量，D是问题的维数。初始化之后，整个种群将进行引领蜂、跟随蜂和侦查蜂搜寻过程的重复循环，直到达到最大迭代次数MCN或误差允许值 ε。
（2）在搜索过程开始阶段，每个引领蜂由式(2-3)产生一个新解即新食物源，
vij=xij+Φij(xij-xkj) (2-3)
式中，k∈﹛1,2,…,SN﹜,j∈{1,2,…,D},且k ≠i;Φij为[-1,1]之间的随机数。计算新解的fiti并评价它，若新解的fiti优于旧解，则引领蜂记住新解忘记旧解。反之，它将保留旧解。
（3）在所有引领蜂完成搜寻过程之后，引领蜂会在招募区跳摇摆舞把解的信息及信息与跟随蜂分享。跟随蜂根据式计算每个解的选择概率，
pi=fiti/∑k=1SNfitk。 (2-4)
然后在区间[-1,1]内随机产生一个数，如果解的概率值大于该随机数，则跟随蜂由式(2-3)产生一个新解，并检验新解的fiti，若新解的fiti比之前好，则跟随蜂将记住新解忘掉旧解；反之，它将保留旧解。
四、在所有跟随蜂完成搜寻过程之后，如果一个解经过limit次循环仍然没有被进一步更新，那么就认为此解陷入局部最优，该食物源就会被舍弃。设食物源xi被舍弃，则此食物源对应的引领蜂转成一个侦查蜂。侦察蜂由(2-5)式产生一个新的食物源代替它。

                                                             xij=xminj+rand(0,1)(xmaxj-xminj)          (2-5)

其中j∈{1,2…,D}。然后返回引领蜂搜索过程，开始重复循环。
五、人工蜂群算法的食物源质量一般是越大越好，即适应度值越大越好，而对应于要优化的问题，需要分两种情况考虑：即最小值问题、最大值问题。设fi是优化问题的目标函数，所以若优化最小值问题时，适应度函数为fi的变形，一般用式(2-6)表示；若优化最大值问题，适应度函数即目标函数。

fiti={1+abs(fi) fi>=01/1+fi fi>0 (2-6)
人工蜂群算法在评价食物源时一般进行贪婪选择按式(2-7)进行。

vi={xi fit(xi)<=fit(vi)vi fit(vi)>fit(xi) (2-7)
人工蜂群算法就是通过循环搜索，最终找到最优食物源或最优解。

5 算法步骤
人工蜂群算法具体实现步骤：
步骤1：初始化种群：初始化各个参数，蜂群总数SN、食物源被采集次数即最大迭代次数MCN及控制参数limit，确定问题搜索范围，并且在搜索范围内随机产生初始解xi(i=1,2,…SN) 。
步骤2：计算并评估每个初始解的适应度。
步骤3：设定循环条件并开始循环
步骤4：引领蜂对解xi按照式（2-3）进行邻域搜索产生新解（食物源）vi，并计算其适应度值；
步骤5：按照式(2-7)进行贪婪选择：如果vi的适应度值优于xi，则利用vi替换xi，将vi作为当前最好的解，否则保留xi不变；
步骤6：根据式(2-4)计算食物源的概率pi；
步骤7：跟随蜂依照概率pi选择解或食物源，按照式(2-3)搜索产生新解（食物源）vi，并计算其适应度。
步骤8：按式(2-7)进行贪婪选择；如果vi的适应度优于xi，则用vi代替xi，将vi作为当前最好解，否则保留xi不变；
步骤9：判断是否有要放弃的解。若有，则侦查蜂按式(2-5)随机产生新解将其替换；
步骤10：记录到目前为止的最优解；
步骤11：判断是否满足循环终止条件，若满足，循环结束，输出最优解，否则返回步骤4继续搜索。

三、部分源代码

clc
close all
clear all
%================================参数设置=============================================%
global boundary setstart setfinal node delta_H danger_xi danger_yi danger_zi danger_ri weight;
%node=12;%离终点最近的起始点到终点的节点个数(偶数)
L_FEN=26;%设置节点间的间隔距离
delta_H=[20 40];%起飞(落地)高度、飞行过程最低高度
danger_xi=[207.1 393.9];%设置威胁区域坐标
danger_yi=[333.3 414.1];
danger_zi=[389.9 349.2];
danger_ri=[0 0];%设置威胁半径
weight=[1 0.01 0.3];%路径长度、离地面高度、夹角所占权重
boundary=[500 0];%设置环境上下限
setfinal=[291.8 219.8 492.4];%设置终点
setstart_ALL=[176.8 237.4 254.5;%设置起点，依次为无人机1、2、3...161.6 363.6 429;454.5 146.5 228.2;393.9 308.1 234.3];
Vmin=3;%设置无人机飞行速度范围（m/s）
Vmax=30;
Ways=2;%Ways=1时，实现同时起飞，同时到达目标；Ways=2时，实现指定顺序依次到达目标
Order=[1 4 2 3];%无人机到达顺序
interval=9;%无人机到达间隔时间（s）
%====================================================================================%
lineX=size(setstart_ALL,1);
%======================================地形绘图=======================================%
SETenvironment;
surf(X,Y,Z);
box on;
rotate3d on;
xi=linspace(0,500,100);
yi=linspace(0,500,100);
[XI,YI]=meshgrid(xi,yi);
ZI=interp2(X,Y,Z,XI,YI,'cubic');
surf(XI,YI,ZI) %光滑曲面+等高线
hold on;
%=====================================威胁区域绘图====================================%
[x,y,z]=sphere(40);
for k=1:size(danger_xi,2)surf(danger_ri(k)*x+danger_xi(k),danger_ri(k)*y+danger_yi(k),danger_ri(k)*z+danger_zi(k));
hold on;
end
%===========================将每条路径的节点距离大致均匀化=================================%
lineX=size(setstart_ALL,1);%UAV数量
A=zeros(lineX,1);
for r=1:lineX
A(r)=sqrt(((setstart_ALL(r,:)-setfinal).^2)*ones(3,1));
end
node=floor(min(A)/L_FEN);
if mod(node,2)==1node=node+1;
endind=find(A==min(A));objval=A(ind(end))/node;orig_node=node;%记录给定的初值node
%===================================主体函数===========================================%
routes_distance=zeros(lineX,1);%初始每条路径长度为0
for r=1:lineX  B=0;if(r==ind)%=======================计算离终点直线距离最近的无人机路径==========================%    node=orig_node;setstart=setstart_ALL(r,:);C=runUAVABC4(r);%得到路径节点坐标for i=1:(size(C,1)-1)B=B+sqrt(((C(i+1,:)-C(i,:)).^2)*ones(3,1));endroutes_distance(r)=B;%==================================================================================%%==============================计算其余无人机路径===================================%elsenode=fix(A(r)/objval); if rem(node,2)~=0node=node+1;%确保node为偶数endsetstart=setstart_ALL(r,:);C=runUAVABC4(r);for i=1:(size(C,1)-1)B=B+sqrt(((C(i+1,:)-C(i,:)).^2)*ones(3,1));endroutes_distance(r)=B;end%===================================================================================%end
%======================================================================================%
distance_min=A;
time_plan=zeros(lineX,1);
distance_min%各起始点的直线距离
routes_distance%规划的航迹长度if Ways==1%同时起飞，同时到达
ind=max(routes_distance)*Vmin-min(routes_distance)*Vmax;%临界条件if(ind<=0)time_plan=max(routes_distance)/Vmax;V_plan=routes_distance./time_plan;time_plan%团队预计到达的时间V_plan%规划的各无人机速度elsefprintf('超出协同规划范围,任意两条长短路径的比值应小于无人机Vmax/Vmin');endelse%同时起飞，按指定次序先后到达time_max=routes_distance/Vmin;time_min=routes_distance/Vmax;  for k=1:lineXif k==1time_plan(Order(k))=routes_distance(Order(k))/Vmax;elsetime_plan(Order(k))=time_plan(Order(k-1))+interval;endend
ind=find(time_plan<time_min);while isempty(ind)==0
ind=ind(1);
time_plan(ind)=routes_distance(ind)/Vmax;
for a=1:lineX-1
time_plan(a)=time_plan(ind)-(ind-a)*interval;
end
ind=find(time_plan<time_min);endV_plan=routes_distance./time_plan;
ind=find(V_plan<Vmin);
if isempty(ind)~=0
time_plan%团队预计到达的时间
V_plan%规划的各无人机速度
else
fprintf('超出协同规划范围');
end
end

四、运行结果

五、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]巫茜,罗金彪,顾晓群,曾青.基于改进PSO的无人机三维航迹规划优化算法[J].兵器装备工程学报. 2021,42(08)
[4]邓叶,姜香菊.基于改进人工势场法的四旋翼无人机航迹规划算法[J].传感器与微系统. 2021,40(07)
[5]马云红,张恒,齐乐融,贺建良.基于改进A*算法的三维无人机路径规划[J].电光与控制. 2019,26(10)
[6]焦阳.基于改进蚁群算法的无人机三维路径规划研究[J].舰船电子工程. 2019,39(03)