A*算法是在Dijkstra算法上进行改进，毕竟，我们是知道终点和起点的位置信息的，Dijkstra算法完全是四面八方全都找，然而我们既然已经知道，比方说重点在起点的北方，那么完全可以直接往北方搜索。

所以算法综合了Best-First Search和Dijkstra算法的优点：在进行启发式搜索提高算法效率的同时，可以保证找到一条最优路径（基于评估函数）。

在此算法中，如果以g(n)表示从起点到任意顶点n的实际距离，h(n)表示任意顶点n到目标顶点的估算距离（根据所采用的评估函数的不同而变化），那么A*算法的估算函数为：

$$f(x)=g(x)+h(x)$$

• 如果g(n)为0，即只计算任意顶点 n到目标的评估函数 h(n)，而不计算起点到顶点n的距离，则算法转化为使用贪心策略的Best-First Search，速度最快，但可能得不出最优解；
• 如果h(n)不高于实际到目标顶点的距离，则一定可以求出最优解，而且h(n)越小，需要计算的节点越多，算法效率越低，常见的评估函数有——欧几里得距离、曼哈顿距离、切比雪夫距离；

• 如果h(n)为0，即只需求出起点到任意顶点n的最短路径 g(n)，而不计算任何评估函数h(n)，则转化为单源最短路径问题，即Dijkstra算法，此时需要计算最多的定点；

• 4方向用$L_1$距离

• 8方向用$L_\infty$距离，也就是max
• 任意方向用$L_2$距离

heuristic function

这个评估函数，其实就是给搜索算法一个知识，告诉搜索算法往哪个方向比较对。
评估函数有这么几个特性：

• 将每一个节点映射到非负实数$H:node\rightarrow \{x|x\geq0,x\in \mathbb{R}\}$
• $H(goal)=0$
• 对任意两个相邻的节点$x,y$
  
  • $H(x) \le H(y)+d(x,y)$
  • $d(x,y) = \frac{weight}{height}$ of edge from $x$ to $y$

这些属性可以令：

$$\forall n, H(n)\le\ shortest\ path\ from\ n\ to\ goal$$
关于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

结果：