A*算法是在Dijkstra算法上進行改進，畢竟，我們是知道終點和起點的位置信息的，Dijkstra算法完全是四面八方全都找，然而我們既然已經知道，比方說重點在起點的北方，那麼完全可以直接往北方搜索。
所以算法綜合了Best-First Search和Dijkstra算法的優點：在進行啓發式搜索提高算法效率的同時，可以保證找到一條最優路徑（基於評估函數）。
在此算法中，如果以g(n)表示從起點到任意頂點n的實際距離，h(n)表示任意頂點n到目標頂點的估算距離（根據所採用的評估函數的不同而變化），那麼A*算法的估算函數爲：
- 如果g(n)爲0，即只計算任意頂點 n到目標的評估函數 h(n)，而不計算起點到頂點n的距離，則算法轉化爲使用貪心策略的Best-First Search，速度最快，但可能得不出最優解；
- 如果h(n)不高於實際到目標頂點的距離，則一定可以求出最優解，而且h(n)越小，需要計算的節點越多，算法效率越低，常見的評估函數有——歐幾里得距離、曼哈頓距離、切比雪夫距離；
如果h(n)爲0，即只需求出起點到任意頂點n的最短路徑 g(n)，而不計算任何評估函數h(n)，則轉化爲單源最短路徑問題，即Dijkstra算法，此時需要計算最多的定點；
4方向用
L1 距離- 8方向用
L∞ 距離，也就是max - 任意方向用
L2 距離
heuristic function
這個評估函數，其實就是給搜索算法一個知識，告訴搜索算法往哪個方向比較對。
評估函數有這麼幾個特性：
- 將每一個節點映射到非負實數
H:node→{x|x≥0,x∈R} H(goal)=0 - 對任意兩個相鄰的節點
x,y
H(x)≤H(y)+d(x,y) d(x,y)=weightheight of edge fromx toy
這些屬性可以令：
關於H函數：
http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html
僞代碼
- 對於每一個node n
- n.f = Infinity, n.g = Infinity
- 創建一個空的List
- start.g = 0, start.f = H(start)，將start加入到list裏邊
- While(List非空)
- current = list中最小f值的那個node
- 如果current == goal，那麼就完成啦
- 對每一個node， n是這個node的臨近的節點
- if(n.g > (current.g + cost of edge from n to current))
- n.g = current.g + cost of edge from n to current
- n.f = n.g + H(n)
- n.parent = current
- add n to list if it’s not there already
matlab代碼
function [route,numExpanded] = AStarGrid (input_map, start_coords, dest_coords, drawMapEveryTime)
% Run A* algorithm on a grid.
% Inputs :
% input_map : a logical array where the freespace cells are false or 0 and
% the obstacles are true or 1
% start_coords and dest_coords : Coordinates of the start and end cell
% respectively, the first entry is the row and the second the column.
% Output :
% route : An array containing the linear indices of the cells along the
% shortest route from start to dest or an empty array if there is no
% route. This is a single dimensional vector
% numExpanded: Remember to also return the total number of nodes
% expanded during your search. Do not count the goal node as an expanded node.
% set up color map for display
% 1 - white - clear cell
% 2 - black - obstacle
% 3 - red = visited
% 4 - blue - on list
% 5 - green - start
% 6 - yellow - destination
cmap = [1 1 1; ...
0 0 0; ...
1 0 0; ...
0 0 1; ...
0 1 0; ...
1 1 0; ...
0.5 0.5 0.5];
colormap(cmap);
% variable to control if the map is being visualized on every
% iteration
drawMapEveryTime = true;
[nrows, ncols] = size(input_map);
% map - a table that keeps track of the state of each grid cell
map = zeros(nrows,ncols);
map(~input_map) = 1; % Mark free cells
map(input_map) = 2; % Mark obstacle cells
% Generate linear indices of start and dest nodes
start_node = sub2ind(size(map), start_coords(1), start_coords(2));
dest_node = sub2ind(size(map), dest_coords(1), dest_coords(2));
map(start_node) = 5;
map(dest_node) = 6;
% meshgrid will `replicate grid vectors' nrows and ncols to produce
% a full grid
% type `help meshgrid' in the Matlab command prompt for more information
parent = zeros(nrows,ncols);
%
[X, Y] = meshgrid (1:ncols, 1:nrows);
xd = dest_coords(1);
yd = dest_coords(2);
% Evaluate Heuristic function, H, for each grid cell
% Manhattan distance
H = abs(X - xd) + abs(Y - yd);
H = H';
% Initialize cost arrays
f = Inf(nrows,ncols);
g = Inf(nrows,ncols);
g(start_node) = 0;
f(start_node) = H(start_node);
% keep track of the number of nodes that are expanded
numExpanded = 0;
% Main Loop
while true
% Draw current map
map(start_node) = 5;
map(dest_node) = 6;
% make drawMapEveryTime = true if you want to see how the
% nodes are expanded on the grid.
if (drawMapEveryTime)
image(1.5, 1.5, map);
grid on;
axis image;
drawnow;
end
% Find the node with the minimum f value
[min_f, current] = min(f(:));
if ((current == dest_node) || isinf(min_f))
break;
end;
% Update input_map
map(current) = 3;
f(current) = Inf; % remove this node from further consideration
% Compute row, column coordinates of current node
[i, j] = ind2sub(size(f), current);
% *********************************************************************
% ALL YOUR CODE BETWEEN THESE LINES OF STARS
% Visit all of the neighbors around the current node and update the
% entries in the map, f, g and parent arrays
%
numExpanded = numExpanded + 1;
if(i-1>=1) %upper
id = sub2ind(size(map), i-1, j);
if((map(id) ~= 2) ... %if not obst
&& (map(id) ~= 3) ... % if not visited
&& (map(id) ~= 5)) ... % if not start
if(g(id) >= g(current) + 1)
g(id) = g(current) + 1;
f(id) = g(id) + H(id);
parent(id) = current;
map(id) = 4;
end
end
end
if(i+1 <= nrows) %lower
id = sub2ind(size(map), i+1, j);
if((map(id) ~= 2) ... %if not obst
&& (map(id) ~= 3) ... % if not visited
&& (map(id) ~= 5)) ... % if not start
if(g(id) >= g(current) + 1)
g(id) = g(current) + 1;
f(id) = g(id) + H(id);
parent(id) = current;
map(id) = 4;
end
end
end
if(j-1 >= 1) %left
id = sub2ind(size(map), i, j-1);
if((map(id) ~= 2) ... %if not obst
&& (map(id) ~= 3) ... % if not visited
&& (map(id) ~= 5)) ... % if not start
if(g(id) >= g(current) + 1)
g(id) = g(current) + 1;
f(id) = g(id) + H(id);
parent(id) = current;
map(id) = 4;
end
end
end
if(j+1 <= ncols) %left
id = sub2ind(size(map), i, j+1);
if((map(id) ~= 2) ... %if not obst
&& (map(id) ~= 3) ... % if not visited
&& (map(id) ~= 5)) ... % if not start
if(g(id) >= g(current) + 1)
g(id) = g(current) + 1;
f(id) = g(id) + H(id);
parent(id) = current;
map(id) = 4;
end
end
end
%*********************************************************************
end
%% Construct route from start to dest by following the parent links
if (isinf(f(dest_node)))
route = [];
else
route = [dest_node];
while (parent(route(1)) ~= 0)
route = [parent(route(1)), route];
end
% Snippet of code used to visualize the map and the path
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
end
測試代碼
map = false(10); %Input map parameters
map (2:10, 6) = true; %Obstacle Declaration
start_coords = [6, 2]; %Starting coordinates
dest_coords = [8, 9]; %Destination Coordinates
drawMapEveryTime = false; %Display Outputs
[route, numExpanded] = AStarGrid(map, start_coords, dest_coords,drawMapEveryTime) %Implementatio結果：
