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