A*算法 (MATLAB) -路徑搜索

A* 算法跟 Dijkstra 算法 很像，只是在下一步搜索中心的選擇的方法不一樣。Dijkstra 算法 沒有任何干預，找離起點 “最近”的鄰居作為備選點，如果有若干個鄰居都是相同距離的話，純粹就是按照找到的順序取第一個。A*算法，找與終點最近的鄰居，作為下一個搜索中心。(不過，如果若干個鄰居與終點的距離一樣呢？)

下面的代碼是從 Dijkstra 算法 拷貝來的，四個黃色的部分是修改的。

 

第二個黃色部分: 找與終點最近的鄰居作為下一個搜索點。
[~, current] = min(f(:));
[min_dist, ~] = min(distanceFromStart(:));

在

Dijkstra 算法

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

第三個黃色部分: 排除已經作為搜索點的鄰居再次被 min 到的可能性f(current) = Inf; 在

Dijkstra 算法

里，這部分是：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