A* 算法跟 很像，只是在下一步搜索中心的选择的方法不一样。 没有任何干预，找离起点 “最近”的邻居作为备选点，如果有若干个邻居都是相同距离的话，纯粹就是按照找到的顺序取第一个。A*算法，找与终点最近的邻居，作为下一个搜索中心。(不过，如果若干个邻居与终点的距离一样呢？)

下面的代码是从 拷贝来的，四个黄色的部分是修改的。

 

第二个黄色部分: 找与终点最近的邻居作为下一个搜索点。

[~, current] = min(f(:));

[min_dist, ~] = min(distanceFromStart(:));

在

里，这部分是：[min_dist, current] = min(distanceFromStart(:));

第三个黄色部分: 排除已经作为搜索点的邻居再次被 min 到的可能性f(current) = Inf; 在

里，这部分是：distanceFromStart(current) = Inf;

第四个黄色部分是新增加的：就是为了计算每一个邻居与终点的距离权值。只不过这里的距离权值预先计算好了放在H矩阵里，所以直需要从 H 里取值就好了。

所以第一黄色部分：定义f,计算H

[X, Y] = meshgrid (1:ncols, 1:nrows);

H = abs(Y - 4) + abs(X - 8);

f = Inf(nrows,ncols);

f(start_node) = H(start_node);

H 有很多种计算方法，可以直接算两点距离之类。

%% % set up color map for display cmap = [1 1 1; ...% 1 - white - clear cell         0 0 0; ...% 2 - black - obstacle                1 0 0; ...% 3 - red = visited                0 0 1; ...% 4 - blue - on list                0 1 0; ...% 5 - green - start                1 1 0];% 6 - yellow - destination colormap(cmap); map = zeros(10); % Add an obstacle map (1:5, 7) = 2; map(6, 2) = 5; % start_coords map(4, 8) = 6; % dest_coords image(1.5,1.5,map); grid on; axis image; %% nrows = 10; ncols = 10; start_node = sub2ind(size(map), 6, 2); dest_node = sub2ind(size(map), 4, 8); % Initialize distance array distanceFromStart = Inf(nrows,ncols); distanceFromStart(start_node) = 0; %====================[X, Y] = meshgrid (1:ncols, 1:nrows);H = abs(Y - 4) + abs(X - 8);f = Inf(nrows,ncols); f(start_node) = H(start_node); %=======================% For each grid cell this array holds the index of its parent parent = zeros(nrows,ncols); % Main Loop while true  % Draw current map  map(start_node) = 5;  map(dest_node) = 6;  image(1.5, 1.5, map);  grid on;  axis image;  drawnow;  %====================  % Find the node with the minimum distance  [

~, current] =

min(f(:)); [min_dist, ~] =

min(distanceFromStart(:)); %===================  if ((current == dest_node) || isinf(min_dist))        break;   end;  map(current) = 3; %============ f(current)

=

Inf; %============ [i, j] = ind2sub(size(distanceFromStart), current);  neighbor = [i-1,j;...             i+1,j;...             i,j+1;...             i,j-1] outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows) +...                   (neighbor(:,2)<1) + (neighbor(:,2)>ncols ) locate = find(outRangetest>0); neighbor(locate,:)=[] neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2)) for i=1:length(neighborIndex) if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3 && map(neighborIndex(i))~= 5)     map(neighborIndex(i)) = 4;   if distanceFromStart(neighborIndex(i))> min_dist + 1            distanceFromStart(neighborIndex(i)) = min_dist+1;         parent(neighborIndex(i)) = current;         f(neighborIndex(i))

=

H(neighborIndex(i));   end  end end end%%if (isinf(distanceFromStart(dest_node)))     route = []; else     %提取路线坐标   route = [dest_node];       while (parent(route(1)) ~= 0)               route = [parent(route(1)), route];        end   % 动态显示出路线             for k = 2:length(route) - 1           map(route(k)) = 7;                 pause(0.1);                 image(1.5, 1.5, map);               grid on;               axis image;               end end