A*算法学习

在游戏中，有一个很常见地需求，就是要让一个角色从A点走向B点，我们期望是让角色走最少的路。嗯，大家可能会说，直线就是最短的。没错，但大多数时候，A到B中间都会出现一些角色无法穿越的东西，比如墙、坑等障碍物。这个时候怎么办呢？ 是的，我们需要有一个算法来解决这个问题，算法的目标就是计算出两点之间的最短路径，而且要能避开障碍物。

1. A*简介

A算法是启发式算法重要的一种，主要是用于在两点之间选择一个最优路径，而A的实现也是通过一个估值函数。

2. F=G+H

• G表示该点到起始点位所需要的代价
• H表示该点到终点的曼哈顿距离。
• F就是G和H的总和，而最优路径也就是选择最小的F值，进行下一步移动（后边会做详细介绍）

3. 曼哈顿距离

上图中这个熊到树叶的曼哈顿距离就是蓝色线所表示的距离，这其中不考虑障碍物，假如上图每一个方格长度为1，那么此时的熊的曼哈顿距离就为9。

起点(X1, Y1)，终点(X2, Y2)，H=|X2-X1|+|Y2-Y1|

我们也可以通过几何坐标点来算出曼哈顿距离，还是以上图为例，左下角为(0, 0)点，熊的位置为(1, 4)，树叶的位置为(7, 1)，那么H=|7-1|+|1-4|=9。

4. 算法流程

A算法现在被广泛应用与电脑游戏中的路径规划问题。我们就以此为例来介绍A算法的具体实施步骤。如下图所示，其中A表示起点，B表示终点，黑色的实心方块表示障碍物。此外我们假设水平或垂直方向上相邻的两个方块之间距离是10，那么对角线方向上相邻的两个方块距离就约是14。

算法开始，我们首先搜索A相邻的所有可能的移动位置（对应于图中的绿色方块）。每个方块左上角的值G表示该点到A的距离，右上角值为H，注意H不能大于该点到B的距离，所以这里的H就取其到B的距离。最后，还要计算一个F值，F=H+G。

然后像Dijkstra算法一样，我们选一个F值最小的节点来做继续搜索。也就是上图中A的邻域中位于左上角的值（F=42）。然后更新该节点的领域值。

这时你会发现，出现了三个F值都等于48的节点。到底应该选择哪一个来继续接下来的搜索呢？这时需要考察它们中的那个H值最小，结果发现H=24是最小的，所以下面就要从该点出发继续搜索。于是更新该节点的邻域方块中的值。

这个时候再找出全局F值最小的点，结果发现有两个为48（而且它们的H值也相当），于是随机选取一个作为新的出发点并更新其邻域值（例如选择右上方的方块），然后在从全局选取F最小的更新其邻域值，于是有：

此时全局F最小的值为54，而且F=54的节点有两个，所以我们还是选择其中H值最小的来做更新。于是更新该节点邻域方块中的值。这里你需要注意的一个地方是，F=54的红色节点下方邻域（F=68）的方块中，G=38。但是，从A到该节点的最短路径应该是30。这是因为目前程序所选择的路径是下图中紫色线路所规程出来的路径，其G的增长序列是14→24→38。

不过不要紧，只要继续执行算法，更新全局F值为最小节点（F=54）的方块，上面的G值就会给更新为正确的值了。

 此时，全局F值最小的方块中F=60，所以更新该节点邻域方块中的值。

现在全局F值最小的有两个，都为68，此时先更新H最小的。这是其实程序已经发现左侧F=68的节点并不能引导一条更短的路径。于是接下来就要转向右侧F=68的节点，并以此为新起点搜索路径。

 最终反复执行上述过程，你就会得到如下图中蓝色方块所示的一条最短路径。

5. Python实现

5.1. 算法思路

A*算法实际是由广度优先遍历和Dijkstra算法演变而来的：

• 广度优先遍历主要是通过从起点依次遍历周围的点而寻找最优的路径；
• Dijkstra基本思路跟广度优先遍历一样，只不过给每次遍历的点增加了一个权值，用于表明当前移动了多少距离，然后每次从移动最短距离的点开始遍历；
• A*在Dijkstra算法增加了一个期望值（启发函数，h），最优化遍历节点的数量。
• 广度优先遍历 -> Dijkstra算法 -> A*算法。其他寻路相关的算法也很多，如JPS跳点算法，但解决问题的侧重点不同，关键是针对具体问题选择合适的算法。

我们先来看一下地图，橙点为起始点和终点：

 本文中，g为已走过的距离，h为期望距离、启发值。文中会频繁使用这两个概念。

A*算法中的h，根据实际地图的拓扑结构，可选用以下三种距离，假设A与B点横纵坐标距离x和y：

• 曼哈顿距离，只允许4个方向移动，AB的距离为：x + y；
• 对角距离，允许8方向移动，AB的距离为：x + y + (sqrt(2)-2)*min(x, y)；
• 欧几里得距离，允许任意方向移动，AB的距离为：sqrt(pow2(x)+pow2(y))；

网格结构常用的便是8方向移动，所以我们这边会选择对角距离作为h。

5.2. 数据结构

我在处理程序问题一般是：先定义数据结构，然后再补充算法本体。

我们这次先从底层的数据结构逐级往上定义。从点、路径到整个地图（我这边只展示关键的数据结构代码）：

# 点的定义
class Vector2:
    x = 0
    y = 0
 
    def __init__(self, x, y):
        self.x = x
        self.y = y
        
# 树结构，用于回溯路径
class Vector2Node:
    pos = None  # 当前的x、y位置
    frontNode = None    # 当前节点的前置节点
    childNodes = None   # 当前节点的后置节点们
    g = 0   # 起点到当前节点所经过的距离
    h = 0   # 启发值
    D = 1
 
    def __init__(self, pos):
        self.pos = pos
        self.childNodes = []
 
    def f(self):
        return self.g + self.h
 
# 地图
class Map:
    map = None  # 地图，0是空位，1是障碍
    startPoint = None   # 起始点
    endPoint = None # 终点
 
    tree = None # 已经搜寻过的节点，是closed的集合
    foundEndNode = None # 寻找到的终点，用于判断算法结束
 
    def __init__(self, startPoint, endPoint):
        self.startPoint = startPoint
        self.endPoint = endPoint
        row = [0]*MAP_SIZE
        self.map = []
        for i in range(MAP_SIZE):
            self.map.append(row.copy())
 
    # 判断当前点是否超出范围
    def isOutBound(self, pos):
        return pos.x < 0 or pos.y < 0 or pos.x >= MAP_SIZE or pos.y >= MAP_SIZE
    
    # 判断当前点是否是障碍点
    def isObstacle(self, pos):
        return self.map[pos.y][pos.x] == 1
 
    # 判断当前点是否已经遍历过
    def isClosedPos(self, pos):
        if self.tree == None:
            return False
        nodes = []
        nodes.append(self.tree)
        while len(nodes) != 0:
            node = nodes.pop()
            if node.pos == pos:
                return True
            if node.childNodes != None:
                for nodeTmp in node.childNodes:
                    nodes.append(nodeTmp)
        return False

PS.我们这边使用matplotlib作为图像输出，具体怎么使用或怎么使其更好看可以参考源代码或第一篇参考文章。

5.3. 具体实现

A*算法的大概思路是：

• 从起始点开始遍历周围的点（专业点的说法是放到了一个open集合中，而我们这边的变量名叫做willProcessNodes）；
• 从open集合中寻找估值，即使用Vector2Node.f()函数计算的值，最小的点作为下一个遍历的对象；
• 重复上面的步骤，直到找到了终点。

具体实现：

# 地图
class Map:
    def process(self):
        # 初始化open集合，并把起始点放入
        willProcessNodes = deque()
        self.tree = Vector2Node(self.startPoint)
        willProcessNodes.append(self.tree)
 
        # 开始迭代，直到找到终点，或找完了所有能找的点
        while self.foundEndNode == None and len(willProcessNodes) != 0:
            # 寻找下一个最合适的点，这里是最关键的函数，决定了使用什么算法
            node = self.popLowGHNode(willProcessNodes)
 
            if self.addNodeCallback != None:
                self.addNodeCallback(node.pos)
 
            # 获取合适点周围所有的邻居
            neighbors = self.getNeighbors(node.pos)
            for neighbor in neighbors:
                # 初始化邻居，并计算g和h
                childNode = Vector2Node(neighbor)
                childNode.frontNode = node
                childNode.calcGH(self.endPoint)
                node.childNodes.append(childNode)
 
                # 添加到open集合中
                willProcessNodes.append(childNode)
 
                # 找到了终点
                if neighbor == self.endPoint :
                    self.foundEndNode = childNode
    
    # 广度优先，直接弹出先遍历到的节点
    def popLeftNode(self, willProcessNodes):
        return willProcessNodes.popleft()
    
    # dijkstra，寻找g最小的节点
    def popLowGNode(self, willProcessNodes):
        foundNode = None
        for node in willProcessNodes:
            if foundNode == None:
                foundNode = node
            else:
                if node.g < foundNode.g:
                    foundNode = node
        if foundNode != None:
            willProcessNodes.remove(foundNode)
        return foundNode
    
    # A*，寻找f = g + h最小的节点
    def popLowGHNode(self, willProcessNodes):
        foundNode = None
        for node in willProcessNodes:
            if foundNode == None:
                foundNode = node
            else:
                if node.f() < foundNode.f():
                    foundNode = node
        if foundNode != None:
            willProcessNodes.remove(foundNode)
        return foundNode

我们可以看到在寻找点的时候使用了popLowGHNode，这是使用A*的关键函数，可以替换成上面两个函数使用不同的算法。以下展示使用不同算法的效果。

广度优先遍历（绿点代表遍历过的，蓝点代表路径结果）：

 Dijkstra算法：

 A*算法：

在A*的实现中，h的计算也是个重要的参数，像是本文上面中使用真实的预估距离作为h，为了方便我们称该值为d：

• h = 0，即Dijkstra算法；
• h < d，预估值有一定的用处，但相比 h = d 而言性能较差；
• h = d，性能最优，并且能找到最佳路径；
• h > d，性能可能进一步优化（也可能更差），但不一定是最优路径；
• h >> g，变成了最佳优先搜索。

可以尝试调节Vector2Node.D查看效果。

6. MATLAB实现

function [route,numExpanded] = AStarGrid (input_map, start_coords, dest_coords)
% 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用一个map矩阵来表示每个点的状态
% 1 - white - clear cell
% 2 - black - obstacle
% 3 - red = visited 相当于CLOSED列表的作用
% 4 - blue  - on list 相当于OPEN列表的作用
% 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,其中的current是index值，需要转换
    [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
    numExpanded=numExpanded+1;
    % 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
    %
    action=[-1 0; 1 0; 0 -1; 0 1];%上，下，左，右
    for a=1:4
        expand=[i,j]+action(a,:);
        expand1=expand(1,1);
        expand2=expand(1,2);
        %不超出边界，不穿越障碍，不在CLOSED列表里，也不是起点，则进行扩展
        if ( expand1>=1 && expand1<=nrows && expand2>=1 && expand2<=nrows && map(expand1,expand2)~=2 && map(expand1,expand2)~=3 && map(expand1,expand2)~=5)
            if ( g(expand1,expand2)> g(i,j)+1 )
                g(expand1,expand2)= g(i,j)+1;
                f(expand1,expand2)= g(expand1,expand2)+H(expand1,expand2);
                parent(expand1,expand2)=current;
                map(expand1,expand2)=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

参考文献

[Unity] A-Star(A星)寻路算法 - 我爱我家喵喵 - 博客园

A*算法详细讲解以及实现_zha_zha_wei的博客-CSDN博客_a*算法实现

Python A*算法的简单实现_暗光之痕的博客-CSDN博客_a*算法python实现