基于蚁群算法的三维孔道路径规划(源码,全部5个M文件)

基于蚁群算法的插装阀块孔道路径规划M文件1function[ROUTES,PL,Tau]=ACASP(D,Tau,K,M,S,E,Alpha,Beta,Rho,Q,GP)%%基于蚁群算法的三维路径规划%%输入参数列表%D邻接矩阵%Tau初始信息素矩阵%K迭代次数（指蚂蚁出动多少波）%M蚂蚁个数（每一波蚂蚁有多少个）%S起始点（最短路径的起始点）%E终止点（最短路径的目的点）%Alpha表征信息素重要程度的参数%Beta表征启发式因子重要程度的参数%Rho信息素蒸发系数%Q信息素增加强度系数%%输出参数列表%ROUTES每一代的每一只蚂蚁的爬行路线%PL每一代的每一只蚂蚁的爬行路线长度%Tau输出动态修正过的信息素%%--------------------变量初始化----------------------------------N=size(D,1);%N表示问题的规模Eta=ones(1,N);%启发式信息，取为至目标点的直线距离的倒数ROUTES=cell(K,M);%用细胞结构存储每一代的每一只蚂蚁的爬行路线PL=zeros(K,M);%用矩阵存储每一代的每一只蚂蚁的爬行路线长度%%-----------启动K轮蚂蚁觅食活动，每轮派出M只蚂蚁--------------------fork=1:Kdisp(k);form=1:M%%第一步：状态初始化W=S;%当前节点初始化为起始点Path=S;%爬行路线初始化PLkm=0;%爬行路线长度初始化TABUkm=ones(1,N);%禁忌表初始化TABUkm(S)=0;%已经在初始点了，因此要排除DD=D;%邻接矩阵初始化%%第二步：下一步可以前往的节点DW=DD(W,:);DW1=find(DWinf);forj=1:length(DW1)ifTABUkm(DW1(j))==0DW(j)=inf;endendLJD=find(DWinf);%可选节点集Len_LJD=length(LJD);%可选节点的个数%%觅食停止条件：蚂蚁未遇到食物或者陷入死胡同whileW~=E&&Len_LJD=1%%第三步：转轮赌法选择下一步怎么走PP=zeros(1,Len_LJD);fori=1:Len_LJDPP(i)=(Tau(W,LJD(i))^Alpha)*(Eta(LJD(i))^Beta);endPP=PP/(sum(PP));%建立概率分布Pcum=cumsum(PP);Select=find(Pcum=rand);to_visit=LJD(Select(1));%下一步将要前往的节点%%第四步：状态更新和记录Path=[Path,to_visit];%路径增加PLkm=PLkm+DD(W,to_visit);%路径长度增加W=to_visit;%蚂蚁移到下一个节点forkk=1:NifTABUkm(kk)==0DD(W,kk)=inf;DD(kk,W)=inf;endendTABUkm(W)=0;%已访问过的节点从禁忌表中删除DW=DD(W,:);DW1=find(DWinf);forj=1:length(DW1)ifTABUkm(DW1(j))==0DW(j)=inf;endendLJD=find(DWinf);%可选节点集Len_LJD=length(LJD);%可选节点的个数end%%第五步：记下每一代每一只蚂蚁的觅食路线和路线长度ROUTES{k,m}=Path;ifPath(end)==EPL(k,m)=PLkm;elsePL(k,m)=inf;endend%%第六步：更新信息素Delta_Tau=zeros(N,N);%更新量初始化form=1:MifPL(k,m)infROUT=ROUTES{k,m};TS=length(ROUT)-1;%跳数PL_km=PL(k,m)+10*RoundTime(ROUT,GP);fors=1:TSx=ROUT(s);y=ROUT(s+1);Delta_Tau(x,y)=Delta_Tau(x,y)+Q/PL_km;Delta_Tau(y,x)=Delta_Tau(y,x)+Q/PL_km;endendendTau=(1-Rho).*Tau+Delta_Tau;%信息素挥发一部分，新增加一部分EndM文件2functionT=RoundTime(Path,GP)%%统计路径转弯次数T=0;P=GP(Path,:);N=size(P,1);fori=1:N-2x1=P(i,1);y1=P(i,2);z1=P(i,3);x2=P(i+2,1);y2=P(i+2,2);z2=P(i+2,3);d=sqrt((x1-x2)^2+(y1-y2)^2+(z1-z2)^2);ifd2T=T+1;endendM文件3functionDrawRect3D(P)A0=P(1:3);B0=P(4:6);A1=A0;A2=A0;A3=A0;B1=B0;B2=B0;B3=B0;A1(1)=B0(1);A2(2)=B0(2);A3(3)=B0(3);B1(1)=A0(1);B2(2)=A0(2);B3(3)=A0(3);%A0-A1plot3([A0(1),A1(1)],[A0(2),A1(2)],[A0(3),A1(3)]);holdon%A0-A2plot3([A0(1),A2(1)],[A0(2),A2(2)],[A0(3),A2(3)]);%A0-A3plot3([A0(1),A3(1)],[A0(2),A3(2)],[A0(3),A3(3)]);%B0-B1plot3([B0(1),B1(1)],[B0(2),B1(2)],[B0(3),B1(3)]);%B0-B2plot3([B0(1),B2(1)],[B0(2),B2(2)],[B0(3),B2(3)]);%B0-B3plot3([B0(1),B3(1)],[B0(2),B3(2)],[B0(3),B3(3)]);%A1-B2plot3([A1(1),B2(1)],[A1(2),B2(2)],[A1(3),B2(3)]);%A1-B3plot3([A1(1),B3(1)],[A1(2),B3(2)],[A1(3),B3(3)]);%A2-B1plot3([A2(1),B1(1)],[A2(2),B1(2)],[A2(3),B1(3)]);%A2-B3plot3([A2(1),B3(1)],[A2(2),B3(2)],[A2(3),B3(3)]);%A3-B1plot3([A3(1),B1(1)],[A3(2),B1(2)],[A3(3),B1(3)]);%A3-B2plot3([A3(1),B2(1)],[A3(2),B2(2)],[A3(3),B2(3)]);M文件4functionF=IsInZAW(P,SZ)%%判断坐标点是否在障碍物内部的函数%%输入参数列表%P1*3矩阵，待检测的点坐标%SZN*6矩阵，障碍物的对角顶点坐标，每一行为一个障碍物的两个顶点%%输出参数列表%F在障碍物内部，为1，否则为0%%N=size(SZ,1);Y=zeros(N,1);fori=1:NZ=SZ(i,:);Y1=(P(1)-Z(1))*(P(1)-Z(4));Y2=(P(2)-Z(2))*(P(2)-Z(5));Y3=(P(3)-Z(3))*(P(3)-Z(6));if(Y1=0&&Y2=0&&Y3=0)Y(i)=1;endendif(sum(Y)0)F=1;elseF=0;EndM文件5%%基于蚁群算法的三维路径规划clcclearcloseall%%设置模型空间%模型空间的对角顶点坐标SV=[0,0,0,9,9,9];%障碍物的对角顶点坐标，每一行为一个障碍物SZ=[0,3,0,3,6,4;...2,5,6,3,4,7;...4,4,2,6,8,5;...7,5,2,9,6,4;...5,1,1,8,4,4;...1,5,1,4,8,3;...3,0,8,7,2,9;...0,5,6,5,8,8;...4,1,4,7,5,6;...7,5,7,9,8,8];%起点坐标PS=[0,9,0];%终点坐标PD=[9,0,9];%%构造网络拓扑结构NX=SV(4)-SV(1)+1;NY=SV(5)-SV(2)+1;NZ=SV(6)-SV(3)+1;NP=NX*NY*NZ;%节点个数%建立节点序号和坐标的关系列表GP=zeros(NP,3);k=1;forx=0:1:SV(4)fory=0:1:SV(5)forz=0:1:SV(6)GP(k,1)=x;GP(k,2)=y;GP(k,3)=z;k=k+1;endendend%建立节点之间的邻接矩阵D=zeros(NP,NP);fori=1:NPforj=1:NPxi=GP(i,1);yi=GP(i,2);zi=GP(i,3);xj=GP(j,1);yj=GP(j,2);zj=GP(j,3);dij=sqrt((xi-xj)^2+(yi-yj)^2+(zi-zj)^2);ifdij==1D(i,j)=1;endifdij1D(i,j)=Inf;endifdij==0D(i,j)=Inf;endendend%应用障碍物对邻接矩阵进行修正NPZ=0;fori=1:NPP=GP(i,:);F=IsInZAW(P,SZ);ifF==1D(i,:)=Inf;D(:,i)=Inf;D(i,i)=0;NPZ=NPZ+1;endend%disp('处于障碍物内部的节点所占比例为（%）');%disp(100*NPZ/NP);%%设置蚁群算法的参数%将起点和终点的坐标映射为节点序号fori=1:NPTP=GP(i,:);if((TP(1)==PS(1))&&(TP(2)==PS(2))&&(TP(3)==PS(3)))PSV=i;endif((TP(1)==PD(1))&&(TP(2)==PD(2))&&(TP(3)==PD(3)))PDV=i;endendTau=ones(NP,NP);%初始信息素矩阵K=180;%迭代次数M=200;%蚂蚁个数S=PSV;%起始点E=PDV;%终止点Alpha=1;%表征信息素重要程度的参数Beta=5;%表征启发式因子重要程度的参数Rho=0.9;%信息素蒸发系数Q=1;%信息素增加强度系数[ROUTES,PL,Tau]=ACASP(D,Tau,K,M,S,E,Alpha,Beta,Rho,Q,GP);%%整理输出结果MinPL=min(min(PL));pos=find(PL==MinPL);I=mod(pos(end),K);if(I==0)I=K;endJ=ceil(pos(end)/K);BestPath=ROUTES{I,J};BestLength=length(BestPath)-1;disp('最佳路径为');disp(GP(BestPath,:));disp('最佳路径的长度为');disp(BestLength);%%绘图figureDrawRect3D(SV);holdonNW=size(SZ,1);fori=1:NWDrawRect3D(SZ(i,:));endX=GP(BestPath,1);Y=GP(BestPath,2);Z=GP(BestPath,3);plot3(X,Y,Z,'r','LineWidth',3);