%% 程序说明
% 本程序首先基于落水人员的位置，划分密集人群和零星落水人员
% 由高速艇前往救助密集人群，双体船救助零星落水人员
% 高速艇与密集人群中心连线的范围内的落水人员也由高速艇救助
% 最后剩余的人根据双体船搜救范围判定是否为临近人员，若是，则取两者之间的中点，加入路径规划算法中
% 由蚁群算法进行路径规划，计算最短路径，双体船沿路径进行救助
%% 1.清空环境
clc
clear all
%% 2.点集说明
% citys：初始落水人员点集
% ship： %船的初始点位置，1是快速艇，2是双体船
% Fcitys; %圆外点集：密集点之外的点集
% FCitys1; %临近点集：两个临近点所在的点集，
% FCitys2; %双体船搜救点集：圆外点集-路径点集-临近点集
% FCitys3; %路径点集：高速艇与密集人群中心连线的范围内的点集
% FCitys4：%圆外点集-路径点集
% FCitys5; %路径规划点集：圆外点集-路径点集-临近点集+圆心点集
% FCitys6; %最终点集：路径规划点集+双体船
% Route; %由FCitys6做出的最短路径规划线路
%% 3.导入落水者位置
%落水人员的位置信息，也可以用EXCLE导入
citys=[125.36874 30.54875
125.36879 30.54869
125.36871 30.54840
125.36913 30.54878
125.36881 30.54782
125.36921 30.54771
125.36828 30.54708
125.36802 30.54737
125.36812 30.54769
125.36750 30.54904
125.36825 30.54948
125.36877 30.54956
125.36875 30.54944
125.36959 30.54799
125.37020 30.54778];
ship=[125.3675 30.546
125.3676 30.5461]; %船的初始点位置，1是快速艇，2是双体船
%% 4 计算最密落水者位置，并画图
colorList=[0.9300 0.9500 0.9700
0.7900 0.8400 0.9100
0.6500 0.7300 0.8500
0.5100 0.6200 0.7900
0.3700 0.5100 0.7300
0.2700 0.4100 0.6300
0.2100 0.3200 0.4900
0.1500 0.2200 0.3500
0.0900 0.1300 0.2100
0.0300 0.0400 0.0700];
zonex=[125.366:0.0001:125.371]; %用于计算概率密度的范围和分辨率
zoney=[30.545:0.0001:30.551];
%计算落水人员概率分布
[~,~,XMesh,YMesh,ZMesh,colorList]=density2C(citys(:,1),citys(:,2),zonex,zoney,colorList);
% 找出最密集点，并将一定范围内的点纳入密集人群
xq=citys(:,1);
yq=citys(:,2);
max1=max(ZMesh,[],'all'); %ZMesh为二维概率密度分布
[max1y,max1x]=find(max1==ZMesh); %找出最大密度的坐标位置
theta=0:pi/50:2*pi;
xv = max1x/10000+zonex(1)+ 0.0005*cos(theta); %以最密集点为圆心做圆
yv = max1y/10000+zoney(1)+ 0.0005*sin(theta);
max2x=max1x/10000+zonex(1); %最密集点，此处的10000是根据zoney=[30.545:0.0001:30.551]中的0.0001取的，
max2y=max1y/10000+zoney(1);
[in,on] = inpolygon(citys(:,1),citys(:,2),xv,yv); %判断圆内外的落水人员
xa=xq(in);
ya=yq(in);
xb=xq(~in);
yb=yq(~in);
Fcitys=[xb,yb]; %圆外点集：密集点之外的点集
%% 5 快速艇初始位置到最密集点的路径
FCitys3=[]; %路径点集：高速艇与密集人群中心连线的范围内的点集
for i=1:size(Fcitys(:,1))
if Fcitys(i,2)<((Fcitys(i,1)-ship(1,1)).*(max2y-ship(1,2))/(max2x-ship(1,1))+ship(1,2)+0.0005) ...
& Fcitys(i,2)>((Fcitys(i,1)-ship(1,1)).*(max2y-ship(1,2))/(max2x-ship(1,1))+ship(1,2)-0.0005)...
& Fcitys(i,2)<-1*(max2x-ship(1,1))*(Fcitys(i,1)-max2x)./(max2y-ship(1,2))+max2y
FCitys3=[FCitys3;Fcitys(i,:)];
end
end %这个循环是用于判断落水人员是否处于最密集点与快速艇连线的一定范围之内，由快速艇施救
liney1=(zonex-ship(1,1)).*(max2y-ship(1,2))./(max2x-ship(1,1))+ship(1,2)+0.0005; %两根平行于线
liney2=(zonex-ship(1,1)).*(max2y-ship(1,2))./(max2x-ship(1,1))+ship(1,2)-0.0005;
liney3=-1*(max2x-ship(1,1))*(zonex-max2x)./(max2y-ship(1,2))+max2y; %垂直线
FCitys4=Fcitys; %密集点圆外的点
for j=1:size(FCitys3,1) %FCitys3是路径内的点
for i=1:size(Fcitys,1)
if FCitys3(j,1)==Fcitys(i,1) && FCitys3(j,2)==Fcitys(i,2)
FCitys4(i,:)=-100000;
else
continue
end
end
end
FCitys4((FCitys4(:,1)==-100000),:)=[]; %FCitys4：圆外点集-路径点集
%% 6.画出小于某半径的圆
% 计算FCitys4点集内小于双体船搜救半径的点
D1 = Distance(FCitys4); %计算各点之间的距离
n1 = size(D1,1); %判断矩阵大小
XCircle=zeros(n1,n1);
YCircle=zeros(n1,n1);
XY=[FCitys4(1,1),FCitys4(1,2)];
FCitys1=[]; %临近点集：两个临近点所在的点集，
Circle=[]; %圆心
%判断落水人员是否小于某个距离，并划分点集
for i=2:n1
if ~ismember(FCitys4(i,1),XY(:,1))
XY=[XY;FCitys4(i,1),FCitys4(i,2)];
for j=i+1:n1
if D1(i,j)<0.0005 && ~ismember(FCitys4(j,1),XY(:,1)) % && ~ismember(citys(j,2),XY(:,2))
XY=[XY;FCitys4(j,1),FCitys4(j,2)];
XCircle(i,j)=(FCitys4(i,1)+FCitys4(j,1))./2;
YCircle(i,j)=(FCitys4(i,2)+FCitys4(j,2))./2;
theta=0:pi/40:2*pi;
X = XCircle(i,j)+ D1(i,j)/2*cos(theta);
Y = YCircle(i,j)+ D1(i,j)/2*sin(theta);
FCitys1=[FCitys1;FCitys4(i,1),FCitys4(i,2);FCitys4(j,1),FCitys4(j,2)];
Circle=[Circle;XCircle(i,j),YCircle(i,j)];
break
else
continue;
end
end
else
continue;
end
end
%% 7 点集数据整理
FCitys2=FCitys4; %双体船搜救点集：圆外点集-路径点集-临近点集
for j=1:size(FCitys1,1) %FCitys1是圆内的点
for i=1:size(FCitys4,1)
if FCitys1(j,1)==FCitys4(i,1) && FCitys1(j,2)==FCitys4(i,2)
FCitys2(i,:)=-100000;
else
continue
end
end
end
FCitys2((FCitys2(:,1)==-100000),:)=[];
FCitys5=[FCitys2;Circle]; %路径规划点集：圆外点集-路径点集-临近点集+圆心点集
FCitys6=[FCitys5;ship(2,:)]; %最终点集：路径规划点集+双体船
%% 8 路径规划
D = Distance(FCitys6);
n = size(FCitys6,1); %计算落水者个数
% 初始化参数
NC_max = 200; %最大迭代次数
m = 22; %蚂蚁个数，一般是落水者人数的1.5倍
alpha = 1; % α 选择[1, 4]比较合适
beta = 4; % β 选择[3 4 5]比较合适
rho = 0.2; % ρ 选择[0.1, 0.2, 0.5]比较合适
Q = 20; % 信息素常量，
NC = 1; % 迭代次数，一开始为1
Eta = 1 ./ D; % η = 1 / D(i, j) ,这里是矩阵
Tau = ones(n, n); % Tau(i, j)表示边(i, j)的信息素量，一开始都为1
Table = zeros(m, n); % 路径记录表，可以记录蚂蚁依次经过的位置
rBest = zeros(NC_max, n); % 记录各代的最佳路线，
lBest = inf .* ones(NC_max, 1); % 记录各代的最佳路线的总长度，
lAverage = zeros(NC_max, 1); % 记录各代路线的平均长度
%% 9.迭代寻找最佳路径
while NC <= NC_max
% 第1步：随机产生各个蚂蚁的起点
start = zeros(m, 1);
for i = 1: m
temp = randperm(n);
start(i) = temp(1);
end
Table(:, 1) = start; % Tabu表的第一列即是所有蚂蚁的起点
citys_index = 1: n; % 所有落水者索引的一个集合
% 第2步：逐个蚂蚁路径选择
for i = 1: m
% 逐个蚂蚁路径选择
for j = 2: n
tabu = Table(i, 1: (j - 1)); % 蚂蚁i已经访问的落水者集合（称禁忌表）
allow_index = ~ismember(citys_index, tabu);
Allow = citys_index(allow_index); % Allow表：存放待访问的落水者
P = Allow;
% 计算从落水者j到剩下未访问的落水者的转移概率
for k = 1: size(Allow, 2) % 待访问的落水者数量
P(k) = Tau(tabu(end), Allow(k))^alpha * Eta(tabu(end), Allow(k))^beta;
end
P = P / sum(P); % 归一化
% 轮盘赌法选择下一个访问落水者（为了增加随机性）
Pc = cumsum(P);
