应用matlab仿真改进A算法在三维地形上的寻径过程

A算法很好的例子，很实用，适合初学者参考学习

clc
clear

%初始化地形
h=  [1000  800  900  1000  900  700  1100  900  1000  1200  1100
     800  600  800  1000  900  800  600   1100  1200  1300  1400
     700  700  900  700   900  800  600   600   900   1100  1000
     500  600  700  900   800  900   800   900   1000  800   900
     800  600  900  1000  900  1000  700   1000  1200  900   800
     500  700  800  1100  900   800  1100  1000   900   1200  1100
     700  600  900  1300  1200  1400  1200  1000  1200  800  1200
     900  1100  900  1200 1500  1200  1300  900   1200  1100  1300
     600  800  900  800   1000  1200  900   1100   1000  900  900
     900  800  600  900   800  800   800   1100   800  1100  1000
     600  900  1000  800  800  600   900   1200   900  1100  600];
h=h./10;


%% 第1轮分解，形成21*21矩阵
%X:135...21
for i=1:11
    for j=1:11
        h1（2*i-1j）=h（ij）;
    end
end

%X:246...20
for i=1:10
    for j=1:11
        h1（2*ij）=（h1（2*i-1j）+h1（2*i+1j））/2;
    end
end

%Y:135...21
for i=1:21
    for j=1:11
        h2（i2*j-1）=h1（ij）;
    end
end
 
%Y:246...20
for i=1:21
    for j=1:10
        h2（i2*j）=（h2（i2*j-1）+h2（i2*j+1））/2;
    end
end

%% 第2轮分解，形成41*41矩阵
%X:135...41
for i=1:21
    for j=1:21
        h3（2*i-1j）=h2（ij）;
    end
end

%X:246...40
for i=1:20
    for j=1:21
        h3（2*ij）=（h3（2*i-1j）+h3（2*i+1j））/2;
    end
end

%Y:135...41
for i=1:41
    for j=1:21
        h4（i2*j-1）=h3（ij）;
    end
end
 
%Y:246...40
for i=1:41
    for j=1:20
        h4（i2*j）=（h4（i2*j-1）+h4（i2*j+1））/2;
    end
end
z=h4; % 初始地形，地形栅格顶点的高程

%%
x=1:41; 
y=1:41;
figure（‘name‘‘地形‘）;
mesh（x*5y*5z）;%生成三维网格图同mesh（xyz（xy））和mesh（xyz（yx））
axis（[021002100200]）;
hold on
xlabel（‘X:m‘）;
ylabel（‘Y:m‘）;
zlabel（‘Z:m‘）;

zx=zeros（4040）;%计算地形栅格的中心点高程
gx=zeros（4040）;%以四领域计算地形栅格的中心点高程
tabu=ones（4040）;

%轮式车辆
dimao=[ 
0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	0.8	0.8	0.8	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	0.8	0.8	0.8	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.8	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	1	0.8	0.8	0.8	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.8	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	1	0.8	0.8	0.8	0.8	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.4	0.4	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	1	0.8	0.8	0.8	0.1	0.1	0.1	0.2	0.2	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.4	0.4	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.1	0.8	1	0.1	0.1	0.8	0.8	0.1	0.1	0.1	0.2	0.2	0.2	0.2	0.2	0.2	0.2
0.8	0.4	0.4	0.4	0.4	0.4	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.1	0.1	0.1	0.1	0.1	1	0.1	0.1	0.8	0.8	0.1	0.1	0.1	0.1	0

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      16580  2012-11-01 14:45  27A_Star_最少时\data.m

     文件       1895  2013-02-22 09:32  27A_Star_最少时\expand_array.m

     文件       6039  2012-11-01 15:08  27A_Star_最少时\HeightData.mat

     文件       8242  2013-02-22 09:41  27A_Star_最少时\main.m

     文件       1176  2013-02-19 08:37  27A_Star_最少时\min_fn.m

     文件     236278  2013-02-22 09:40  27A_Star_最少时\最少时.bmp

     文件     236278  2013-02-19 10:39  27A_Star_最少时\最短路.bmp

     目录          0  2013-02-26 14:56  27A_Star_最少时

----------- ---------  ---------- -----  ----

               506488                    8