gusucode.com > matlab编程A星算法的航迹规划3维源码程序 > code2/matlab编程A星算法的航迹规划3维源码程序/code/main_2.m

    %starA(M, A, B)
%STARA    根据A*算法计算两点间的最短路径
clear;clc

%% ==============================================
global F G parent row col  %全局变量

row = size(M, 1);
col = size(M, 2);
height = size(M,3);

F = zeros(size(M));   % F(n)
G = zeros(size(M));   % G(n)
% mov = [0 -1  0; 1 -1  0; 1  0  0; 1  1  0; 0  1  0;-1  1  0;-1  0  0;-1 -1  0;  %8个
%    0 -1 -1; 1 -1 -1; 1  0 -1; 1  1 -1; 0  1 -1; 0  0 -1;-1  1 -1;-1  0 -1;-1 -1 -1;     %9个
%    0 -1  1; 1 -1  1; 1  0  1; 1  1  1; 0  1  1; 0  0  1 ;-1  1  1;-1 0  1;-1 -1  1;];   %可移动的方向，共27个
mov =[0 1 0; 1 1 0; -1 1 0; 0 1 1; 1 1 1; -1 1 1; 0 1 -1; 1 1 -1; -1 1 -1;];
open = [A];  %将起点A加入开放列表
close = [];  %初始化关闭列表为空
parent = cell(size(M));   % 存储父节点
inOpen = false(size(M));   % 是否在开放列表中
inClose = false(size(M));  % 是否在关闭列表中
%% ================================================
%
inOpen(A(1),A(2),A(3)) = true;
G(A(1),A(2),A(3)) = 0;
F(A(1),A(2),A(3)) = hn(A,B);
[index,min] = minInOpen(open);  %找出开放列表中最小的f(n)
flag = 1; %是否成功找到路径
while min(1)~=B(1)||min(2)~=B(2)||min(3)~=B(3)   %结束条件，
    open(index,:) = [];     %从开放列表中删除
    inOpen(min(1), min(2),min(3)) = false;
    close = [close;min];
    inClose(min(1), min(2),min(3)) = true;
    
    % 计算26邻域中各点的fn，gn，hn。
    for i=1:9
        temp = min + mov(i,:);
        if temp(1)<=row&&temp(1)>0&&temp(2)<=col&&temp(2)>0&&temp(3)<=height&&temp(3)>0
            if M(temp(1),temp(2),temp(3))~=notCross && inClose(temp(1),temp(2),temp(3))==false  % 该点可以通过且不在封闭列表中
                if inOpen(temp(1),temp(2),temp(3)) == 0    %  不在开放列表中，加入open
                    parent{temp(1),temp(2),temp(3)} = min;
                    open = [open;temp];
                    G(temp(1),temp(2),temp(3)) = gn(temp);
                    F(temp(1),temp(2),temp(3)) = G(temp(1),temp(2),temp(3))+hn(temp,B);
                    inOpen(temp(1), temp(2),temp(3)) = true;
                else           % 在开放列表中
                    gnn = norm(min-temp) + G(min(1),min(2),min(3)); %
                    if gnn < G(temp(1),temp(2),temp(3))
                        parent{temp(1),temp(2),temp(3)} = min;
                    end
                end
            end
        end
    end
    if length(open) == 0
        flag = 0;
        disp('from A to B')
        disp('No route!')
         break;
    else [index,min] = minInOpen(open);
    end
end
% 回溯找路径
if(flag)
    route =[B];
    t=parent{B(1),B(2),B(3)};
    while t(1)~=A(1) || t(2)~=A(2) || t(3)~=A(3)
        route = [route;t];
        t=parent{t(1),t(2),t(3)};
    end
    route = [route;A];
    route = flip(route,1);
    disp(' The distance between A and B is :');
    disp(G(B(1),B(2),B(3)));
    disp(' Route as follows: (from A to B)');
    disp(route);
end