基于智能优化算法的无人机路径规划（Matlab代码实现）

function [fit,result,x0]=aimFcn_1(x,option,data)
x0=x;
%% 
x=reshape(x,data.mapSize0);
%%
S=data.S0;
E=data.E0;
flag=x*0;
path=S;
map=data.map;
while sum(S==E)~=3
    % 可移动点
    nextN=repmat(S,length(data.direction(:,1)),1)+data.direction;
    % 剔除超界点
    flag=nextN(:,1)*0;
    for i=1:length(nextN(:,1))
        for j=1:3
            if nextN(i,j)<=0 ||nextN(i,j)>data.mapSize0(j)
                flag(i)=1;
            end
        end
    end
    position=find(flag==1);
    nextN(position,:)=[];
    % 剔除不可移动点
    flag=nextN(:,1)*0;
    for i=1:length(nextN(:,1))
        no1=nextN(i,1);
        no2=nextN(i,2);
        no3=nextN(i,3);
        if map(no1,no2,no3)==1
            flag(i)=1;
        end
    end
    position=find(flag==1);
    nextN(position,:)=[];
    if isempty(nextN)
        S=path(end-1,:);
        path(end,:)=[];
        continue;
    end
    %
    D1=nextN(:,1)*0;
    D2=nextN(:,1)*0;
    pri=nextN(:,1)*0;
    for i=1:length(nextN(:,1))
        no1=nextN(i,1);
        no2=nextN(i,2);
        no3=nextN(i,3);
        D1(i)=norm(nextN(i,:)-S);
        D2(i)=norm(nextN(i,:)-E);
        pri(i)=x(no1,no2,no3);
    end
    [~,no]=min((D1+D2).*pri.^0.5);
    path=[path;nextN(no,:)];
    S=nextN(no,:);
    map(S(1),S(2),S(3))=1;
end
D=0;
for i=1:length(path(:,1))-1
    D=D+norm(path(i,:)-path(i+1,:));
end
fit=D;
if nargout>1
    result.fit=fit;    %总目标
    result.path=path;
end
end