matlab三维路径规划,【路径规划】基于A星算法的三维路径规划matlab源码

%% 该函数用于演示基于A_Star算法的三维路径规划算法

%% 清空环境

clc

clear

%% 数据初始化

%下载数据

starttime=cputime;

load HeightData z zx tabu dimao

%起点终点网格点

startx=8;starty=27;

endx=35;endy=17;

%OPEN LIST STRUCTURE

%IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |g(n) |h(n)|f(n)|

OPEN=[];%开始列表

%CLOSED LIST STRUCTURE

%X val | Y val |

CLOSED=[];%结束列表

%Put all obstacles on the Closed list

k=1;

for i=1:40

for j=1:40

if(tabu(j,i) == 0)

CLOSED(k,1)=i;

CLOSED(k,2)=j;

k=k+1;

end

end

end

CLOSED_COUNT=size(CLOSED,1);%提前将不可通行点加入到CLOSED表

%% set the starting node as the first node

xNode=startx;

yNode=starty;

OPEN_COUNT=1;