devbox
AllinToyou
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

A*算法求解迷宫问题（算法讲解与证明、python实现与可视化）_a*算法求解迷宫寻路问题实验

作者：AllinToyou | 2024-05-12 23:55:44

赞

踩

a*算法求解迷宫寻路问题实验

目录

一、引入

二、具体细节

1、BFS（Breadth First Search）

2、Dijkstra（Uniform Cost Search）

3、启发式（Heuristic search）

4、A*算法

4.1 算法细节

4.2 A与A*算法

4.3 A*算法证明

4.4 算法过程

三、具体实现

1、实验要求

2、代码实现

四、源代码

一、引入

       当我开始学习该算法时，网上已经有了很多优秀的介绍性文章了。如果你看到了文章中出现了以下有着红色星星★和紫色×的路径图：

        那么该文章很大概率参考了Red Blob Games（点击跳转）的A*算法教程。该教程是一个很不错的引入教程，有着很生动的交互式图表和简单易懂的描述过程。

        以下通过BFS、贪婪BFS、Dijkstra算法作为引入逐步引入A*算法，使得对于A*算法有一个初步的整体了解。

        Red Blob Games里面有三张图来说明BFS、Dijkstra、A*算法的的区别，如果有点基础的其实看完就知道个大概了：

        BFS（Breadth First Search ）：每一个方向都平等的扩展，因此它的探索轨迹是一圈又一圈的同心圆，像涟漪一样一圈一圈均匀地往外扩展。

BFS算法
BFS算法

        我们知道BFS扩展的时候是对一个节点的所有邻接节点依次扩展，每个节点被扩展的机会是公平的，即各个邻接节点的扩展机会一样，顺序任意。在迷宫问题中表现为，选一个节点的右、上、左、下（不一定要右上左下，你想上下左右都可以）节点加入到队列里面，然后按放进去的顺序从队列取出来节点继续扩展，同样是将这个节点的右、上、左、下节点加入队列。那么在迷宫上展现的过程就是对这个节点距离为1的一圈搜索一遍，然后又搜索距离为2的一圈、距离为3的一圈……

        具体动画效果如下：

        从上面可以看到，对于迷宫问题以及其他路径权重相等的图搜索中，BFS是一种非常有用的算法。BFS一定可以搜索到所有可以到达的节点，它是一种暴力的穷尽查找算法，并且能找到最短路径（前提所有边权相等）。

        Dijkstra算法（Dijkstra’s Algorithm ）：某些节点（方向、路径）会优先被探索，一般是那些具有更小代价的节点和路径会被优先探索。因为该算法是用来求最短路的，算法正确性要求每次都要选择当前已知的最短路进行探索，因此它的探索轨迹是随机的、不均匀的，就像山脊的等高线图一样。

Dijkstra算法

        Dijkstra相比BFS的区别就是此时图的各条边的边权不同了，此时BFS不再适用。

        还是迷宫问题，方块里面的值表示起点★到该方块的代价（cost）(可以理解为距离、花费的成本)，左图中移动到相邻方块的代价都是1，右图中移动到相邻方块的权值视方块颜色而定：移动到棕色■的代价是1，移动到绿色■的代价是5，灰色■表示不可穿越的障碍。

左图是BFS，右图是Dijkstra

        左图中因为各个方块代价一致，使用BFS算法，算法会直接穿过绿色区域到达终点×；右图中因为方块代价不同，使用Dijkstra算法，算法则会绕过绿色区域到达终点×。

        它俩算法的执行动画如下：       

左图为BFS，右图为Dijkstra

        具体BFS和Dijkstra算法过程下面会详细介绍，这里只需要知道他们应用的区别。

        A*算法：它优先考虑“似乎”更接近目标的路径。该算法也是不断寻找估计的“当前最有潜力”的节点，和Dijkstra算法一样都是不均匀的山脊图。但相比Dijkstra算法的杂乱无章的山脊轨迹，它是有目的性、方向性的，轨迹扩展方向总是选择更靠近目标的那一侧，下面的图可以看到一直往尖端的一侧伸展，因为在A*算法眼中那里更接近终点。

A*算法

        它是对 Dijkstra 算法的修改，针对单个目标进行了优化。Dijkstra的算法可以发现到达所有位置的路径。但是在我们寻路算法中，我们可能只需要寻找到达一个位置的路径，这意味着Dijkstra算法中的一些额外开销是不必要的。A*算法是一种单点对单点的路径寻找算法，就像在LOL中点击小地图的某个位置，系统会自动寻路，得到通往目的位置的一条“白线”，来表示它是最短的路径。

        它会利用自己一些已知的启发信息，来合理规划自己的探索方向，避免了Dijkstra的一些盲目性。

二、具体细节

        在这里需要提一点的是，下面讨论的A*算法仅仅作为一种寻路算法，讨论的也是仅限于图中路径的搜索。但其实A*算法不止适用于路径搜索，它是一种启发式的思想，它还可以用于其他问题如八数码问题的求解，只是大多数问题最后化简之后可以归结为图论（或者树）的路径求解，因此我们只需理解路径搜索就能理解整个算法思想。

这是一个使用了A*算法的八数码问题的求解过程，如果我们把每个正方形都看作图的节点，指针看作权重为1的边，那就是一棵树（图的特例）最短路径的求解过程

        这一点正如BFS算法，BFS也不仅仅适用于路径搜索，例如算法题目中常用的暴力穷举，但我们学习该算法的时候也只关心路径的搜索，因为暴力穷举最后实质上也是一棵根节点开始的搜索树。

         同时为了方便理解，采用的都是网格图，但是其实应用在所有类型的图结构中都是一样的、正确的。这不难理解，因为网格图最终也可以化成节点+边权为1的一般图。

        BFS和Dijkstra算法我们再熟悉不过了，事实上不必多讲。我们的重点是A*算法，这里具体展开BFS和Dijkstra的算法过程原因是，一方面是希望大家清楚地知道A*算法是如何得来的；另一方面，是我个人太喜欢这个网站的代码图和交互动画了，真的是百看不厌。

1、BFS（Breadth First Search）

       BFS思想的关键就是不断地扩展边界（frontier）。

       BFS的python代码如下：

BFS最基础的代码

        思路是：

        先创建一个队列(Queue，先进先出：先放进的节点先扩展)frontier，frontier用来存储待扩展的节点，因此初始时需要将起点start★放进frontier；

        再创建一个集合(Set，集合的元素无序且不会重复)reached，reached用来存储已经到过的节点，就是我们常命名的visit。

        只要frontier不为空，就从frontier中取出一个元素current进行扩展。对于的所有current邻居 next，只要next不在reached里面（即没有到达过），就把next放进frontier里面，然后放到reached标记为已经到达。

        上面的BFS代码没有构建出一条路径，仅仅告诉了我们如何遍历图上的所有点。因此需要进行修改，记录我们是怎么到达每一个点的路径。

        黄色部分是修改的部分：

改进后的BFS代码，能记录路径信息

        这里用came_from替换了reached。

        came_from是字典(Dictionary，键值对，一个key对应value)，came_from不仅可以表示和reached一样的功能（判断一个节点是否到达过），方法是判断came_from里面是不是存在key为这个节点的键值对；还能记录每个点的前序节点，用came_from[节点i]来记录，这样当找到终点时就能顺着前序节点一直寻找到起点。

        还有一处修改部分是：之前是判断是否在reached里面，不在的话直接放到reached集合里面；现在是判断是否在came_from里面，不在的话存储came_from[该节点的邻居next]为当前扩展的current。

         当每个节点都存储了从哪里来的信息后，整张图的情况就是下面这样：

         上面的指示箭头就告诉了前序节点是谁，这样当我们BFS找到终点时，我们就有了足够的信息知道我们是怎么到达终点的，也就能重建这一条路径。方法如下：

根据前序节点信息寻找路径的方法

         从goal开始，顺着came_from存储的前序节点，一个一个地回溯到起点，期间不断将路径放到数组path里面，因为越靠近终点的节点越早放进去，所以存储的path最后存储的是一条从终点到起点的反向路径，最后要将整个path反过来。

        上面的BFS最后会遍历完图里的所有节点，也就是会知道起点到所有节点的最短路径（前提是图上边权都相等，否则不是最短）。但实际上我们一般只需要求到某一个目标节点的路径，因此很多过程是不必要的。原有的BFS一定是所有节点遍历完才终止，而我们现在只需要遍历到目标节点后就可以停下了，故我们可以及时终止。

        过程如下，一旦找到目标节点，BFS就停止继续扩展：

BFS找到终点后，不管此时frontier里面还有哪些节点没扩展，都应该立刻停下来

         修改后的代码如下，只需要加入一条及时终止的条件：

完全版的BFS代码，寻找某个目标节点的最短路径且能及时结束

        当发现当前正要扩展的节点就是终点goal×时，代码就结束了。

2、Dijkstra（Uniform Cost Search）

        上面讨论的BFS只能用于图上每一条路径的权重都相等的情况，如果图中各条路径权重不完全相同，那么再次采用BFS也能遍历所有节点，但得不到最短路径，因为最先遍历到的不一定就是最短的。

        Dijkstra算法也很简单，就是从起点开始，不断扩展到当前耗费总代价最短的节点，直到到达终点（前提是边权非负）。简单证明思路就是，目前选择的耗费最短的节点，之后再通过其他节点到达该节点的代价一定大于目前的代价。

        因为需要寻找当前耗费总代价最短的节点，所以需要将原本的队列（queue，先进先出）修改为优先队列（priority queue，元素存在优先级，可以返回最大优先级的元素）。

        同时除了记录这个节点的前序节点came_from，还需要记录当前到达这个节点的代价cost_so_far。

        在BFS代码基础上进行修改：

         首先创建优先队列frontier，将起点放进去，并且代价设置为0（python中的PriorityQueue的优先级的值越小，则表明优先级越高，越先被取出。但我印象中PriortyQueue的put([priority, value])的第一个参数是优先级第二个参数才是值）。

        接着创建came_from用来记录从哪来的，创建cost_so_far用来记录目前到达的各个节点所花费的路径总代价。

        然后不断地从frontier取出目前最小的代价的节点current进行扩展，直到最后到达goal结束。

        扩展的方法变为：查找current的所有邻接节点next，计算next的new_cost，计算方法是将current的当前代价cost_so_far[current]加上这一条邻边的代价graph.cost(current,next)。如果next是没有到达过的节点或者new_cost小于已知的最短路节点，那么添加或者修改当前next的代价cost_so_far[next]，优先级设置为新的代价new_cost，并且将这一键值对加入到扩展的优先队列frontier里面，最后记录next的前序节点是current。

        值得一提的是，Dijkstra算法的执行一般要求没有负边，但上面的Dijkstra实现代码是可以处理含有正负边的图的，只是不能处理含有负环的图。

        原因在于，当扩展时发现一条更短的路时，会将其加入到优先队列。一般的Dijkstra算法所有节点只会进入到优先队列一次，但上述代码一旦发现通过其他节点到达的某个节点x路径更短，就会将节点x放入到优先队列，而不管这个节点是否被扩展过，也就是给了这个节点再次修改最短路的机会。所以如果图有负边没负环（意味着所有节点都存在一条最短路），使用上面代码也能找到最短路。

        效果图如下：

        

        上图中走绿色■方格耗费的代价大于棕色■，可以看到会先去探索一些棕色的格子然后再探索一些绿色的格子，即每次选择当前耗费总路程最短的格子在扩展。

3、启发式（Heuristic search）

        上面两种方法都是往各个方向扩展，当我们的目标是寻找到所有位置或者多个位置的路径时这是合理的，但是如果我们仅仅是需要到一个位置的路径，这样的时间代价是不必要的。

        我们的目的是让边界的扩展方向朝着目标位置扩展，而不是朝其他方向盲目扩展。为了实现上述目的，需要定义一个启发式函数（heuristic function），它将用来衡量我们目前的状态离终点还有多远。

        在网格图中常用的是曼哈顿距离，定义方式如下：

         如果我们只用启发式函数计算出的距离作为优先级，即总是优先扩展离终点近的点，那么会得到如下结果：        

        可以看到在启发式函数的帮助下，更快地寻找到了终点，而这正是它的优势：速度快。

        这种搜索方法是贪婪最优先搜索算法（Greedy Best First Search）

        但是如果存在有障碍物的情况下，仅仅用启发式函数的计算结果作为优先级可能不能得到正确结果。如下所示：       

         可以看到，仅靠启发式函数计算的优先级并不能得出最短路径，它舍弃了Dijstra算法每次扩展最短路径节点这一保证正确性的优势，也就不能保证得到一条最短路径。

        有没有能同时兼顾速度和正确性的方法？那就是要下面要介绍的A*算法。

4、A*算法

4.1 算法细节

        Dijkstra算法可以很好的找到最短路径，但是浪费了不必要的时间去探索没有希望的方向；仅仅使用启发式的贪婪最优先搜索算法（Greedy Best First Search）总是选择最有希望的方向进行探索，但它可能找不到最优路径。

        A*算法同时使用同时使用了上述两种方法的信息：从起点到目前位置的实际距离和目前位置到终点的估计距离。

        A*算法通过如下公式来综合考虑每个待扩展节点的优先级：

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

        其中：

        f(x)即待扩展节点x的综合优先级，他由g(x)和h(x)计算而来，我们仍然选择待扩展节点中f(x)最小的进行扩展。

        g(x)是节点x距离起点的代价。

        h(x)是节点x距离终点的估计代价。

        从上面公式来看，整体上继承了Dijkstra算法的思想，总是拓展最短f(x)的节点，这样能保证一旦搜索到终点时必然是最短路径；同时f(x)的计算上又考虑了离终点的预估距离，减少或避免扩展个别没有希望的节点，使得整体搜索过程趋向于有希望的方向。

        应注意的是，上述h(x)的选取不是任意的，它是保证最终结果正确与否、搜索速度快慢的关键。h(x)越大，那么搜索速度越快，但也不是无限大，它有自己的限制条件。如果设x距离终点的真正代价为h^{'}(x)，那么h(x)必须满足如下要求才能保证寻找到最优解，即永远不能大于真正的距离。

        h(x) \leq h^{'}(x) 

        可以直观地认为，h(x)是一种保守估计。

4.2 A与A*算法

        要注意的一点是A算法和A*算法的区别。目前查找的资料没有明确说明，对于两者的定义也有些模糊，以下是两种A*算法的说法：

        第一种是认为A*算法即上面的思想。即上述h^{'}(x)就是h^{*}(x)，对于所有h(x)都满足如下公式的A算法就是A*算法。

        h(x) \leq h^{*}(x)

        第二种是认为在算法中对于h(x)往往存在了很多种估价函数，例如我们既可以采用曼哈顿距离，也可以采用对角距离，还可以采用欧几里得距离，那么必然有多种估价函数h_{1}(x)、h_{2}(x)、h_{3}(x)等等。我们如果对A算法进一步限制，即如果g^{*}(x)>0且h_{i}(x)\leq h^{*}(x)（即h^{*}(x)大于等于任意的估价函数），那么该算法就是A*算法。可以看到，这种定义下A*是最优的A算法。但实际应用中，我们往往难以判断或者寻找到最优的估价函数，因此A*算法和A算法的区别并不是很重要，常常用A*算法表示这一种思想。

4.3 A*算法证明

        对于上面A*算法的思想，我给出如下一种简单的反证思路，可能有所纰漏，但是希望可以帮助理解：

        假设A*算法找出的路径不是最短路径，那么A*算法结束时说明找到了一条更长的从起点到终点的路径。我们要证明矛盾，只需要证明A*算法在这一条更长的路径上不会顺利地执行下去即可。

        设起点为s，终点为t。设最短路径为T_{1}，A*算法找的路径为T_{2}。T_{1}这条路径上与T_{2}路径上第1个不同的节点为a,接下来依次是b,c,d,e…t（这些节点中可能有些与T_{2}路径上的相同，但无所谓，此时已经是一条不同的路径）。设T_{2}路径上，t节点的前一个节点为m。同时令h^{'}(x)表示节点x到终点t的实际距离。如下所示：

        假设当A*算法运行至m时，不出意外的话就要扩展t，即此时节点t的f(t)是所有待扩展节点中最小的，所以会选择t。而我们要证明的恰恰就是这个“意外”，使得A*算法不会在m之后选择t，也就不会在算法结束时选择一条比最短路还长的路。

        我们知道h^{'}(t)=0（t到t本身的实际距离为0），而h(t)是t到t的估计距离，必然小于h^{'}(t)，即h(t) \leq h^{'}(t)=0，所以此时h(t)也是0。因此:

f(t)=g(t)+h(t)=g(t)

        而g(t)表示的是目前s到t的实际距离，也就是T_{2}路径的长度。已知T_{2}的路径长度大于T_{1}路径的长度，而T_{1}路径的长度可以表示为g(a)+h^{'}(a)，所以：

        g(t) \geq g(a)+h^{'}(a)

        而h(a)是a到t的估计距离，一定小于等于a到t的实际距离h^{'}(a)，所以：

g(a)+h^{'}(a) \geq g(a)+h(a)

        所以：

       g(t) \geq g(a)+h(a)

        即：

        g(t)+h(t) \geq g(a)+h(a)

        也就是：

        f(t) \geq f(a)

        所以我们知道，此时待扩展节点中，t并不是最小值，我们有更小的节点a来进行扩展。

        当扩展a之后，因为g(b)+h^{'}(b) \leq g(t)，同理可推出f(t) \geq f(b)，所以接下来拓展的就是b节点。我们可以类推最短路径T_{1}路径上的余下的所有节点c,d,e…，不妨设为i，它们都满足： g(i)+h^{'}(i) \leq g(t)，可以同理推出f(t) \geq f(i)。

        也就是t节点的f(t)永远不会是待扩展节点中最小的，直到最短路径T_{1}上的余下节点被扩展完，节点t都不会被扩展。当最短路径T_{1} 最后一个非t节点被扩展后，自然扩展的就是t节点，此时算法结束。我们可以知道，结束时我们所找到的s到t的路径正是T_{1}而非T_{2}，与我们假设的矛盾。

        所以如果h(x)始终小于等于节点x到终点的代价，则A*算法保证一定能够找到最短路径。当h(x)的值越小，算法将遍历越多的节点，也就导致算法越慢。 如果h(x)很大，以至于完全等于节点x到终点的真实代价，则A*算法将找到最佳路径，并且速度很快。可惜的是，并非所有场景下都能做到这一点。因为在没有达到终点之前，我们很难确切算出距离终点还有多远。

        对于评价函数f(x)=g(x)+h(x)，我们可以发现以下有趣的事情：

        ①当h(x)=0时，f(x)=g(x)，说明此时完全依据所到达节点中的最短距离，就是Dijkstra算法。

        ②当g(x)=0时，就是贪婪最优先搜索算法（Greedy Best First Search）

4.4 算法过程

        以下是算法的伪代码，相比前面所说的Dijkstra算法过程只是加入了启发式信息：

        上面过程和Dijkstra过程较为相似，这里不再描述。

        对于目前网上搜索的资料，与上述的过程基本相似，但是具体细节和叫法有所差别。一般说的open_set就是上述代码的frontier。close_set类似于放入到cost_so_far后的节点，但是区别在于上面伪代码是可以处理负边无负环的图，而一般的代码不能处理。以下是另一种版本的算法过程：

  1. 1.初始化open_set和close_set；
  2. 2.将起点加入open_set中，并设置优先级为0（优先级越小表示优先级越高）；
  3. 3.如果open_set不为空，则从open_set中选取优先级最高的节点x：
  4.     ①如果节点x为终点，则：
  5.         从终点开始逐步追踪parent节点，一直到达起点，返回找到的结果路径，算法结束；
  6.     ②如果节点x不是终点，则：
  7.         1.将节点x从open_set中删除，并加入close_set中；
  8.         2.遍历节点x所有的邻近节点：
  9.             ①如果邻近节点y在close_set中，则：
  10.                 跳过，选取下一个邻近节点
  11.             ②如果邻近节点y不在open_set中，则：
  12.                 设置节点m的parent为节点x，计算节点m的优先级，将节点m加入open_set中

        在代码实现时，我主要依据第一个伪代码来实现。

三、具体实现

1、实验要求

        迷宫问题是实验心理学中一个古典问题。迷宫从入口到出口可能有若干条通路，本实验要求求出从入口到出口的最短路径。

        下图是一个4×4的迷宫问题的示意图，每个位置用平面坐标系中的点表示，如图所示，入口位置点的坐标(1,1)，出口位置点的坐标为(4,4)。两个点之间有线相连则代表两个位置相通。若没有线相连，则表示不通。

2、代码实现

        为了解决上述迷宫问题，我的思路是对上述矩形的迷宫的每个节点编号，从(1,1)开始依次从左到右是0，1，2，3……这样编号还有一个好处是可以很方便的直到该节点位于第几行第几列。

        每个节点的邻接表记录相邻的节点，因为是无向边，所以一条边会被记录两次。

        具体算法过程根据上述伪代码来编写。

        以下是实现代码：

  1. import numpy as np
  2. from queue import PriorityQueue
  3.  
  4.  
  5. class Map:  # 地图
  6.     def __init__(self, width, height) -> None:
  7.         # 迷宫的尺寸
  8.         self.width = width
  9.         self.height = height
  10.         # 创建size x size 的点的邻接表
  11.         self.neighbor = [[] for i in range(width*height)]
  12.  
  13.     # 添加边
  14.     def addEdge(self, from_: int, to_: int):
  15.         if (from_ not in range(self.width*self.height)) or (to_ not in range(self.width*self.height)):
  16.             return 0
  17.         self.neighbor[from_].append(to_)
  18.         self.neighbor[to_].append(from_)
  19.         return 1
  20.  
  21.     # 由序号获得该点在迷宫的x、y坐标
  22.     def get_x_y(self, num: int):
  23.         if num not in range(self.width*self.height):
  24.             return -1, -1
  25.         x = num % self.width
  26.         y = num // self.width
  27.         return x, y
  28.  
  29.  
  30. class Astar:  # A*寻路算法
  31.     def __init__(self, _map: Map, start: int, end: int) -> None:
  32.         # 地图
  33.         self.run_map = _map
  34.         # 起点和终点
  35.         self.start = start
  36.         self.end = end
  37.         # open集
  38.         self.open_set = PriorityQueue()
  39.         # cost_so_far表示到达某个节点的代价，也可相当于close集使用
  40.         self.cost_so_far = dict()
  41.         # 每个节点的前序节点
  42.         self.came_from = dict()
  43.  
  44.         # 将起点放入,优先级设为0，无所谓设置多少，因为总是第一个被取出
  45.         self.open_set.put((0, start))
  46.         self.came_from[start] = -1
  47.         self.cost_so_far[start] = 0
  48.  
  49.     # h函数计算,即启发式信息
  50.     def heuristic(self, a, b):
  51.         x1, y1 = self.run_map.get_x_y(a)
  52.         x2, y2 = self.run_map.get_x_y(b)
  53.         return abs(x1-x2) + abs(y1-y2)
  54.  
  55.     # 运行A*寻路算法，如果没找到路径返回0，找到返回1
  56.     def find_way(self):
  57.         # open表不为空
  58.         while not self.open_set.empty():
  59.             # 从优先队列中取出代价最短的节点作为当前遍历的节点，类型为(priority,node)
  60.             current = self.open_set.get()
  61.             # 找到终点
  62.             if current[1] == self.end:
  63.                 break
  64.             # 遍历邻接节点
  65.             for next in self.run_map.neighbor[current[1]]:
  66.                 # 新的代价
  67.                 new_cost = self.cost_so_far[current[1]]+1
  68.                 # 没有到达过的点 或 比原本已经到达过的点的代价更小
  69.                 if (next not in self.cost_so_far) or (new_cost < self.cost_so_far[next]):
  70.                     self.cost_so_far[next] = new_cost
  71.                     priority = new_cost+self.heuristic(next, self.end)
  72.                     self.open_set.put((priority, next))
  73.                     self.came_from[next] = current[1]
  74.  
  75.         if self.end not in self.cost_so_far:
  76.             return 0
  77.         return 1
  78.  
  79.     def show_way(self):
  80.         # 记录路径经过的节点
  81.         result = []
  82.         current = self.end
  83.         # 不断寻找前序节点
  84.         while self.came_from[current] != -1:
  85.             result.append(current)
  86.             current = self.came_from[current]
  87.         # 加上起点
  88.         result.append(current)
  89.         # 翻转路径
  90.         result.reverse()
  91.         print(result)
  92.  
  93.  
  94. # 初始化迷宫
  95. theMap = Map(4, 4)
  96. # 添加边
  97. theMap.addEdge(0, 1)
  98. theMap.addEdge(1, 2)
  99. theMap.addEdge(2, 6)
  100. theMap.addEdge(3, 7)
  101. theMap.addEdge(4, 5)
  102. theMap.addEdge(5, 6)
  103. theMap.addEdge(6, 7)
  104. theMap.addEdge(4, 8)
  105. theMap.addEdge(5, 9)
  106. theMap.addEdge(7, 11)
  107. theMap.addEdge(8, 9)
  108. theMap.addEdge(9, 10)
  109. theMap.addEdge(10, 11)
  110. theMap.addEdge(8, 12)
  111. theMap.addEdge(10, 14)
  112. theMap.addEdge(12, 13)
  113. theMap.addEdge(13, 14)
  114. theMap.addEdge(14, 15)
  115. # A* 算法寻路
  116. theAstar = Astar(theMap, 0, 15)
  117. theAstar.find_way()
  118. theAstar.show_way()

        运行之后得到如下结果：

[0, 1, 2, 6, 7, 11, 10, 14, 15]

        也就是在图上的路径为：

         上述是代码的主体，为了更好地实现结果的可视化，我使用python的matploblib库来可视化。

         matploblib库一般用来可视化数据图表，我的思路是采用其画圆函数Circle来绘制节点，画矩形函数Rectangle来绘制边，然后使用plt（matplotlib.pyplot）的ion()函数打开交互，绘制动态图，呈现查找中的每一个阶段。具体细节如下：

  1. import numpy as np
  2. from queue import PriorityQueue
  3. import matplotlib.pyplot as plt
  4. import matplotlib.patches as mpathes
  5. import random
  6.  
  7. # 画布
  8. fig, ax = plt.subplots()
  9.  
  10.  
  11. class Map:  # 地图
  12.     def __init__(self, width, height) -> None:
  13.         # 迷宫的尺寸
  14.         self.width = width
  15.         self.height = height
  16.         # 创建size x size 的点的邻接表
  17.         self.neighbor = [[] for i in range(width*height)]
  18.  
  19.     def addEdge(self, from_: int, to_: int):    # 添加边
  20.         if (from_ not in range(self.width*self.height)) or (to_ not in range(self.width*self.height)):
  21.             return 0
  22.         self.neighbor[from_].append(to_)
  23.         self.neighbor[to_].append(from_)
  24.         return 1
  25.  
  26.     def get_x_y(self, num: int):    # 由序号获得该点在迷宫的x、y坐标
  27.         if num not in range(self.width*self.height):
  28.             return -1, -1
  29.         x = num % self.width
  30.         y = num // self.width
  31.         return x, y
  32.  
  33.     def drawCircle(self, num, color):    # 绘制圆形
  34.         x, y = self.get_x_y(num)
  35.         thePoint = mpathes.Circle(np.array([x+1, y+1]), 0.1, color=color)
  36.         # 声明全局变量
  37.         global ax
  38.         ax.add_patch(thePoint)
  39.  
  40.     def drawEdge(self, from_, to_, color):    # 绘制边
  41.         # 转化为(x,y)
  42.         x1, y1 = self.get_x_y(from_)
  43.         x2, y2 = self.get_x_y(to_)
  44.         # 整体向右下方移动一个单位
  45.         x1, y1 = x1+1, y1+1
  46.         x2, y2 = x2+1, y2+1
  47.         # 绘长方形代表边
  48.         offset = 0.05
  49.         global ax
  50.         if from_-to_ == 1:  # ← 方向的边
  51.             rect = mpathes.Rectangle(
  52.                 np.array([x2-offset, y2-offset]), 1+2*offset, 2*offset, color=color)
  53.             ax.add_patch(rect)
  54.         elif from_-to_ == -1:  # → 方向的边
  55.             rect = mpathes.Rectangle(
  56.                 np.array([x1-offset, y1-offset]), 1+2*offset, 2*offset, color=color)
  57.             ax.add_patch(rect)
  58.         elif from_-to_ == self.width:  # ↑ 方向的边
  59.             rect = mpathes.Rectangle(
  60.                 np.array([x2-offset, y2-offset]), 2*offset, 1+2*offset, color=color)
  61.             ax.add_patch(rect)
  62.         else:  # ↓ 方向的边
  63.             rect = mpathes.Rectangle(
  64.                 np.array([x1-offset, y1-offset]), 2*offset, 1+2*offset, color=color)
  65.             ax.add_patch(rect)
  66.  
  67.     def initMap(self):    # 绘制初始的迷宫
  68.         # 先绘制边
  69.         for i in range(self.width*self.height):
  70.             for next in self.neighbor[i]:
  71.                 self.drawEdge(i, next, '#afeeee')
  72.  
  73.         # 再绘制点
  74.         for i in range(self.width*self.height):
  75.             self.drawCircle(i, '#87cefa')
  76.  
  77.  
  78. class Astar:  # A*寻路算法
  79.     def __init__(self, _map: Map, start: int, end: int) -> None:
  80.         # 地图
  81.         self.run_map = _map
  82.         # 起点和终点
  83.         self.start = start
  84.         self.end = end
  85.         # open集
  86.         self.open_set = PriorityQueue()
  87.         # cost_so_far表示到达某个节点的代价，也可相当于close集使用
  88.         self.cost_so_far = dict()
  89.         # 每个节点的前序节点
  90.         self.came_from = dict()
  91.  
  92.         # 将起点放入,优先级设为0，无所谓设置多少，因为总是第一个被取出
  93.         self.open_set.put((0, start))
  94.         self.came_from[start] = -1
  95.         self.cost_so_far[start] = 0
  96.  
  97.         # 标识起点和终点
  98.         self.run_map.drawCircle(start, '#ff8099')
  99.         self.run_map.drawCircle(end, '#ff4d40')
  100.  
  101.     def heuristic(self, a, b):    # h函数计算,即启发式信息
  102.         x1, y1 = self.run_map.get_x_y(a)
  103.         x2, y2 = self.run_map.get_x_y(b)
  104.         return abs(x1-x2) + abs(y1-y2)
  105.  
  106.     def find_way(self):    # 运行A*寻路算法，如果没找到路径返回0，找到返回1
  107.         while not self.open_set.empty():  # open表不为空
  108.             # 从优先队列中取出代价最短的节点作为当前遍历的节点，类型为(priority,node)
  109.             current = self.open_set.get()
  110.  
  111.             # 展示A*算法的执行过程
  112.             if current[1] != self.start:
  113.                 # 当前节点的前序
  114.                 pre = self.came_from[current[1]]
  115.                 # 可视化
  116.                 self.run_map.drawEdge(pre, current[1], '#fffdd0')
  117.                 if pre != self.start:
  118.                     self.run_map.drawCircle(pre, '#99ff4d')
  119.                 else:  # 起点不改色
  120.                     self.run_map.drawCircle(pre, '#ff8099')
  121.                 if current[1] != self.end:
  122.                     self.run_map.drawCircle(current[1], '#99ff4d')
  123.                 else:
  124.                     self.run_map.drawCircle(current[1], '#ff4d40')
  125.                 # 显示当前状态
  126.                 plt.show()
  127.                 plt.pause(0.5)
  128.  
  129.             # 找到终点
  130.             if current[1] == self.end:
  131.                 break
  132.             # 遍历邻接节点
  133.             for next in self.run_map.neighbor[current[1]]:
  134.                 # 新的代价
  135.                 new_cost = self.cost_so_far[current[1]]+1
  136.                 # 没有到达过的点 或 比原本已经到达过的点的代价更小
  137.                 if (next not in self.cost_so_far) or (new_cost < self.cost_so_far[next]):
  138.                     self.cost_so_far[next] = new_cost
  139.                     priority = new_cost+self.heuristic(next, self.end)
  140.                     self.open_set.put((priority, next))
  141.                     self.came_from[next] = current[1]
  142.  
  143.     def show_way(self):  # 显示最短路径
  144.         # 记录路径经过的节点
  145.         result = []
  146.         current = self.end
  147.  
  148.         if current not in self.cost_so_far:
  149.             return
  150.  
  151.         # 不断寻找前序节点
  152.         while self.came_from[current] != -1:
  153.             result.append(current)
  154.             current = self.came_from[current]
  155.         # 加上起点
  156.         result.append(current)
  157.         # 翻转路径
  158.         result.reverse()
  159.         # 生成路径
  160.         for point in result:
  161.             if point != self.start:  # 不是起点
  162.                 # 当前节点的前序
  163.                 pre = self.came_from[point]
  164.                 # 可视化
  165.                 self.run_map.drawEdge(pre, point, '#ff2f76')
  166.                 if pre == self.start:  # 起点颜色
  167.                     self.run_map.drawCircle(pre, '#ff8099')
  168.                 elif point == self.end:  # 终点颜色
  169.                     self.run_map.drawCircle(point, '#ff4d40')
  170.                 # 显示当前状态
  171.                 plt.show()
  172.                 plt.pause(0.1)
  173.  
  174.     def get_cost(self):  # 返回最短路径
  175.         if self.end not in self.cost_so_far:
  176.             return -1
  177.         return self.cost_so_far[self.end]
  178.  
  179.  
  180. # 初始化迷宫
  181. theMap = Map(4, 4)
  182.  
  183. # 设置迷宫显示的一些参数
  184. plt.xlim(0, theMap.width+1)
  185. plt.ylim(0, theMap.height+1)
  186. # 将x轴的位置设置在顶部
  187. ax.xaxis.set_ticks_position('top')
  188. # y轴反向
  189. ax.invert_yaxis()
  190. # 等距
  191. plt.axis('equal')
  192. # 不显示背景的网格线
  193. plt.grid(False)
  194. # 允许动态
  195. plt.ion()
  196. # 添加边
  197. theMap.addEdge(0, 1)
  198. theMap.addEdge(1, 2)
  199. theMap.addEdge(2, 6)
  200. theMap.addEdge(3, 7)
  201. theMap.addEdge(4, 5)
  202. theMap.addEdge(5, 6)
  203. theMap.addEdge(6, 7)
  204. theMap.addEdge(4, 8)
  205. theMap.addEdge(5, 9)
  206. theMap.addEdge(7, 11)
  207. theMap.addEdge(8, 9)
  208. theMap.addEdge(9, 10)
  209. theMap.addEdge(10, 11)
  210. theMap.addEdge(8, 12)
  211. theMap.addEdge(10, 14)
  212. theMap.addEdge(12, 13)
  213. theMap.addEdge(13, 14)
  214. theMap.addEdge(14, 15)
  215.  
  216. # 初始化迷宫
  217. theMap.initMap()
  218.  
  219. # A* 算法寻路
  220. theAstar = Astar(theMap, 0, 15)
  221. theAstar.find_way()
  222. theAstar.show_way()
  223.  
  224. # 输出最短路径长度
  225. theCost = theAstar.get_cost()
  226. if theCost == -1:
  227.     print("不存在该路径！")
  228. else:
  229.     print("从起点到终点的最短路径长度为: ", theCost)
  230.  
  231. # 关闭交互，展示结果
  232. plt.ioff()
  233. plt.show()

        运行效果如下：

         输出结果如下：

从起点到终点的最短路径长度为:  8

         对于稍微大一点的图(6x6)进行测试：

  1. # 初始化迷宫
  2. theMap = Map(6, 6)
  3.  
  4. # 设置迷宫显示的一些参数
  5. plt.xlim(0, theMap.width+1)
  6. plt.ylim(0, theMap.height+1)
  7. # 将x轴的位置设置在顶部
  8. ax.xaxis.set_ticks_position('top')
  9. # y轴反向
  10. ax.invert_yaxis()
  11. # 等距
  12. plt.axis('equal')
  13. # 不显示背景的网格线
  14. plt.grid(False)
  15. # 允许动态
  16. plt.ion()
  17.  
  18. # 添加边
  19. theMap.addEdge(0, 1)
  20. theMap.addEdge(1, 2)
  21. theMap.addEdge(2, 3)
  22. theMap.addEdge(3, 4)
  23. theMap.addEdge(4, 5)
  24. theMap.addEdge(1, 7)
  25. theMap.addEdge(3, 9)
  26. theMap.addEdge(4, 10)
  27. theMap.addEdge(5, 11)
  28. theMap.addEdge(6, 7)
  29. theMap.addEdge(8, 9)
  30. theMap.addEdge(6, 12)
  31. theMap.addEdge(7, 13)
  32. theMap.addEdge(8, 14)
  33. theMap.addEdge(10, 16)
  34. theMap.addEdge(11, 17)
  35. theMap.addEdge(12, 13)
  36. theMap.addEdge(13, 14)
  37. theMap.addEdge(15, 16)
  38. theMap.addEdge(16, 17)
  39. theMap.addEdge(14, 20)
  40. theMap.addEdge(15, 21)
  41. theMap.addEdge(16, 22)
  42. theMap.addEdge(17, 23)
  43. theMap.addEdge(18, 19)
  44. theMap.addEdge(19, 20)
  45. theMap.addEdge(20, 21)
  46. theMap.addEdge(22, 23)
  47. theMap.addEdge(18, 24)
  48. theMap.addEdge(19, 25)
  49. theMap.addEdge(20, 26)
  50. theMap.addEdge(22, 28)
  51. theMap.addEdge(26, 27)
  52. theMap.addEdge(27, 28)
  53. theMap.addEdge(24, 30)
  54. theMap.addEdge(27, 33)
  55. theMap.addEdge(29, 35)
  56. theMap.addEdge(30, 31)
  57. theMap.addEdge(31, 32)
  58. theMap.addEdge(33, 34)
  59. theMap.addEdge(34, 35)
  60.  
  61. # 初始化迷宫
  62. theMap.initMap()
  63.  
  64. # A* 算法寻路
  65. theAstar = Astar(theMap, 0, 35)
  66. theAstar.find_way()
  67. theAstar.show_way()
  68.  
  69. # 输出最短路径长度
  70. theCost = theAstar.get_cost()
  71. if theCost == -1:
  72.     print("不存在该路径！")
  73. else:
  74.     print("从起点到终点的最短路径长度为: ", theCost)
  75.  
  76. # 关闭交互，展示结果
  77. plt.ioff()
  78. plt.show()

        运行结果：

        

         输出结果：

从起点到终点的最短路径长度为:  10

        可以知道，运行结果正确。

        但我们发现，每一次输入一个新的图都得输入一大堆边，对于复杂一点的图很不方便调试。有没有一种方法，能在我们设置迷宫的大小后让程序自己随机生成迷宫？

        为此，我们可以编写一个随机生成迷宫的函数。

        我采用的随机生成方法是简单的深度搜索法。初始状态下的迷宫没有边，只有指定大小的节点阵列。从起点开始，依次探索四个方向（四个方向的探索顺序随机），如果该方向的邻接点没有被探索过，那么生成一条边，同时前进到该点。对于该点继续重复上面过程，直到所有点被探索完，算法终止。

  1.     # 寻找
  2.     def search(self, current: int):
  3.         # 四个方向的顺序
  4.         sequence = [i for i in range(4)]
  5.         # 打乱顺序
  6.         random.shuffle(sequence)
  7.         # 依次选择四个方向
  8.         for i in sequence:
  9.             # 要探索的位置
  10.             x = self.direction[i]+current
  11.  
  12.             # 跨了一行
  13.             if (current % self.width == self.width-1 and self.direction[i] == 1) or (current % self.width == 0 and self.direction[i] == -1):
  14.                 continue
  15.  
  16.             # 要探索的位置没有超出范围 且 该位置没有被探索过
  17.             if 0 <= x < self.width*self.height and self.visited[x] == 0:
  18.                 self.addEdge(current, x)
  19.                 self.visited[x] = 1
  20.                 self.search(x)
  21.  
  22.  
  23.     def randomCreateMap(self, start, k):  # 随机生成迷宫
  24.         # 标识每个节点是否被探索过
  25.         self.visited = np.zeros(self.width*self.height)
  26.         self.visited[start] = 1
  27.         # 四个方向,分别代表上、下、左、右
  28.         self.direction = {0: -self.width,
  29.                           1: self.width,
  30.                           2: -1,
  31.                           3: 1}
  32.         # 从起点开始
  33.         self.search(start)

        以下是随机生成的10x10、20x20、30x25迷宫：

10x10，起点在0，终点在99
20x20，起点在0，终点在399

         

30x25，起点在0，终点在500

         可以看到生成的迷宫效果不错，可以满足基本需要。但因为生成迷宫的算法使用的是深度搜索，所以导致起点到终点的路径有且仅有一条。这对于我们寻找最短路径而言，似乎无法说明，因为一旦找到了终点那必定是最短路。因此我们对迷宫增加复杂度，也就是随机在迷宫里面添加k条边，使得图存在多条路径。

  1.  
  2.     # 随机添加k条边
  3.     def randomAddEdges(self, k):
  4.         # 循环k次(可能不止k次)
  5.         for i in range(k):
  6.             node = random.randint(0, self.width*self.height)
  7.             # 随机添加一个方向
  8.             sequence = [i for i in range(4)]
  9.             random.shuffle(sequence)
  10.             isPick = 0
  11.             for d in sequence:
  12.                 # 跨了一行,不存在该方向的边
  13.                 if (node % self.width == self.width-1 and self.direction[d] == 1) or (node % self.width == 0 and self.direction[d] == -1):
  14.                     continue
  15.                 x = self.direction[d]+node
  16.                 # 该边存在
  17.                 if x in self.neighbor[node]:
  18.                     continue
  19.                 # 该边不存在
  20.                 self.addEdge(node, x)
  21.                 isPick = 1
  22.             # 重新添加一条边,即重新循环一次
  23.             if isPick == 0:
  24.                 if i == 0:  # 第一次
  25.                     i = 0
  26.                 else:
  27.                     i -= 1
  28.

        生成后的迷宫如下：

        可以看到多了很多冗余路径，使得起点到终点的路径不止一条。

        将A*算法应用于随机生成的迷宫：

        

         输出结果如下：

从起点到终点的最短路径长度为:  18

        

        输出结果如下：

从起点到终点的最短路径长度为:  28

        

         输出结果如下：

从起点到终点的最短路径长度为:  50

四、源代码

  1. import numpy as np
  2. from queue import PriorityQueue
  3. import matplotlib.pyplot as plt
  4. import matplotlib.patches as mpathes
  5. import random
  6.  
  7. # 画布
  8. fig, ax = plt.subplots()
  9.  
  10.  
  11. class Map:  # 地图
  12.     def __init__(self, width, height) -> None:
  13.         # 迷宫的尺寸
  14.         self.width = width
  15.         self.height = height
  16.         # 创建size x size 的点的邻接表
  17.         self.neighbor = [[] for i in range(width*height)]
  18.  
  19.     def addEdge(self, from_: int, to_: int):    # 添加边
  20.         if (from_ not in range(self.width*self.height)) or (to_ not in range(self.width*self.height)):
  21.             return 0
  22.         self.neighbor[from_].append(to_)
  23.         self.neighbor[to_].append(from_)
  24.         return 1
  25.  
  26.     def get_x_y(self, num: int):    # 由序号获得该点在迷宫的x、y坐标
  27.         if num not in range(self.width*self.height):
  28.             return -1, -1
  29.         x = num % self.width
  30.         y = num // self.width
  31.         return x, y
  32.  
  33.     def drawCircle(self, num, color):    # 绘制圆形
  34.         x, y = self.get_x_y(num)
  35.         thePoint = mpathes.Circle(np.array([x+1, y+1]), 0.1, color=color)
  36.         # 声明全局变量
  37.         global ax
  38.         ax.add_patch(thePoint)
  39.  
  40.     def drawEdge(self, from_, to_, color):    # 绘制边
  41.         # 转化为(x,y)
  42.         x1, y1 = self.get_x_y(from_)
  43.         x2, y2 = self.get_x_y(to_)
  44.         # 整体向右下方移动一个单位
  45.         x1, y1 = x1+1, y1+1
  46.         x2, y2 = x2+1, y2+1
  47.         # 绘长方形代表边
  48.         offset = 0.05
  49.         global ax
  50.         if from_-to_ == 1:  # ← 方向的边
  51.             rect = mpathes.Rectangle(
  52.                 np.array([x2-offset, y2-offset]), 1+2*offset, 2*offset, color=color)
  53.             ax.add_patch(rect)
  54.         elif from_-to_ == -1:  # → 方向的边
  55.             rect = mpathes.Rectangle(
  56.                 np.array([x1-offset, y1-offset]), 1+2*offset, 2*offset, color=color)
  57.             ax.add_patch(rect)
  58.         elif from_-to_ == self.width:  # ↑ 方向的边
  59.             rect = mpathes.Rectangle(
  60.                 np.array([x2-offset, y2-offset]), 2*offset, 1+2*offset, color=color)
  61.             ax.add_patch(rect)
  62.         else:  # ↓ 方向的边
  63.             rect = mpathes.Rectangle(
  64.                 np.array([x1-offset, y1-offset]), 2*offset, 1+2*offset, color=color)
  65.             ax.add_patch(rect)
  66.  
  67.     def initMap(self):    # 绘制初始的迷宫
  68.         # 先绘制边
  69.         for i in range(self.width*self.height):
  70.             for next in self.neighbor[i]:
  71.                 self.drawEdge(i, next, '#afeeee')
  72.  
  73.         # 再绘制点
  74.         for i in range(self.width*self.height):
  75.             self.drawCircle(i, '#87cefa')
  76.  
  77.     # 寻找
  78.     def search(self, current: int):
  79.         # 四个方向的顺序
  80.         sequence = [i for i in range(4)]
  81.         # 打乱顺序
  82.         random.shuffle(sequence)
  83.         # 依次选择四个方向
  84.         for i in sequence:
  85.             # 要探索的位置
  86.             x = self.direction[i]+current
  87.  
  88.             # 跨了一行
  89.             if (current % self.width == self.width-1 and self.direction[i] == 1) or (current % self.width == 0 and self.direction[i] == -1):
  90.                 continue
  91.  
  92.             # 要探索的位置没有超出范围 且 该位置没有被探索过
  93.             if 0 <= x < self.width*self.height and self.visited[x] == 0:
  94.                 self.addEdge(current, x)
  95.                 self.visited[x] = 1
  96.                 self.search(x)
  97.  
  98.     # 随机添加k条边
  99.     def randomAddEdges(self, k):
  100.         # 循环k次(可能不止k次)
  101.         for i in range(k):
  102.             node = random.randint(0, self.width*self.height)
  103.             # 随机添加一个方向
  104.             sequence = [i for i in range(4)]
  105.             random.shuffle(sequence)
  106.             isPick = 0
  107.             for d in sequence:
  108.                 # 跨了一行,不存在该方向的边
  109.                 if (node % self.width == self.width-1 and self.direction[d] == 1) or (node % self.width == 0 and self.direction[d] == -1):
  110.                     continue
  111.                 x = self.direction[d]+node
  112.                 # 该边存在
  113.                 if x in self.neighbor[node]:
  114.                     continue
  115.                 # 该边不存在
  116.                 self.addEdge(node, x)
  117.                 isPick = 1
  118.             # 重新添加一条边,即重新循环一次
  119.             if isPick == 0:
  120.                 if i == 0:  # 第一次
  121.                     i = 0
  122.                 else:
  123.                     i -= 1
  124.  
  125.     def randomCreateMap(self, start, k):  # 随机生成迷宫
  126.         # 标识每个节点是否被探索过
  127.         self.visited = np.zeros(self.width*self.height)
  128.         self.visited[start] = 1
  129.         # 四个方向,分别代表上、下、左、右
  130.         self.direction = {0: -self.width,
  131.                           1: self.width,
  132.                           2: -1,
  133.                           3: 1}
  134.         # 从起点开始
  135.         self.search(start)
  136.         # 随机添加k条边，使得迷宫尽可能出现多条到达终点的路径
  137.         self.randomAddEdges(k)
  138.  
  139.  
  140. class Astar:  # A*寻路算法
  141.     def __init__(self, _map: Map, start: int, end: int) -> None:
  142.         # 地图
  143.         self.run_map = _map
  144.         # 起点和终点
  145.         self.start = start
  146.         self.end = end
  147.         # open集
  148.         self.open_set = PriorityQueue()
  149.         # cost_so_far表示到达某个节点的代价，也可相当于close集使用
  150.         self.cost_so_far = dict()
  151.         # 每个节点的前序节点
  152.         self.came_from = dict()
  153.  
  154.         # 将起点放入,优先级设为0，无所谓设置多少，因为总是第一个被取出
  155.         self.open_set.put((0, start))
  156.         self.came_from[start] = -1
  157.         self.cost_so_far[start] = 0
  158.  
  159.         # 标识起点和终点
  160.         self.run_map.drawCircle(start, '#ff8099')
  161.         self.run_map.drawCircle(end, '#ff4d40')
  162.  
  163.     def heuristic(self, a, b):    # h函数计算,即启发式信息
  164.         x1, y1 = self.run_map.get_x_y(a)
  165.         x2, y2 = self.run_map.get_x_y(b)
  166.         return abs(x1-x2) + abs(y1-y2)
  167.  
  168.     def find_way(self):    # 运行A*寻路算法，如果没找到路径返回0，找到返回1
  169.         while not self.open_set.empty():  # open表不为空
  170.             # 从优先队列中取出代价最短的节点作为当前遍历的节点，类型为(priority,node)
  171.             current = self.open_set.get()
  172.  
  173.             # 展示A*算法的执行过程
  174.             if current[1] != self.start:
  175.                 # 当前节点的前序
  176.                 pre = self.came_from[current[1]]
  177.                 # 可视化
  178.                 self.run_map.drawEdge(pre, current[1], '#fffdd0')
  179.                 if pre != self.start:
  180.                     self.run_map.drawCircle(pre, '#99ff4d')
  181.                 else:  # 起点不改色
  182.                     self.run_map.drawCircle(pre, '#ff8099')
  183.                 if current[1] != self.end:
  184.                     self.run_map.drawCircle(current[1], '#99ff4d')
  185.                 else:
  186.                     self.run_map.drawCircle(current[1], '#ff4d40')
  187.                 # 显示当前状态
  188.                 plt.show()
  189.                 plt.pause(0.01)
  190.  
  191.             # 找到终点
  192.             if current[1] == self.end:
  193.                 break
  194.             # 遍历邻接节点
  195.             for next in self.run_map.neighbor[current[1]]:
  196.                 # 新的代价
  197.                 new_cost = self.cost_so_far[current[1]]+1
  198.                 # 没有到达过的点 或 比原本已经到达过的点的代价更小
  199.                 if (next not in self.cost_so_far) or (new_cost < self.cost_so_far[next]):
  200.                     self.cost_so_far[next] = new_cost
  201.                     priority = new_cost+self.heuristic(next, self.end)
  202.                     self.open_set.put((priority, next))
  203.                     self.came_from[next] = current[1]
  204.  
  205.     def show_way(self):  # 显示最短路径
  206.         # 记录路径经过的节点
  207.         result = []
  208.         current = self.end
  209.  
  210.         if current not in self.cost_so_far:
  211.             return
  212.  
  213.         # 不断寻找前序节点
  214.         while self.came_from[current] != -1:
  215.             result.append(current)
  216.             current = self.came_from[current]
  217.         # 加上起点
  218.         result.append(current)
  219.         # 翻转路径
  220.         result.reverse()
  221.         # 生成路径
  222.         for point in result:
  223.             if point != self.start:  # 不是起点
  224.                 # 当前节点的前序
  225.                 pre = self.came_from[point]
  226.                 # 可视化
  227.                 self.run_map.drawEdge(pre, point, '#ff2f76')
  228.                 if pre == self.start:  # 起点颜色
  229.                     self.run_map.drawCircle(pre, '#ff8099')
  230.                 elif point == self.end:  # 终点颜色
  231.                     self.run_map.drawCircle(point, '#ff4d40')
  232.                 # 显示当前状态
  233.                 plt.show()
  234.                 plt.pause(0.005)
  235.  
  236.     def get_cost(self):  # 返回最短路径
  237.         if self.end not in self.cost_so_far:
  238.             return -1
  239.         return self.cost_so_far[self.end]
  240.  
  241.  
  242. # 初始化迷宫，设置宽度和高度
  243. theMap = Map(20, 20)
  244.  
  245. # 设置迷宫显示的一些参数
  246. plt.xlim(0, theMap.width+1)
  247. plt.ylim(0, theMap.height+1)
  248. # 将x轴的位置设置在顶部
  249. ax.xaxis.set_ticks_position('top')
  250. # y轴反向
  251. ax.invert_yaxis()
  252. # 等距
  253. plt.axis('equal')
  254. # 不显示背景的网格线
  255. plt.grid(False)
  256. # 允许动态
  257. plt.ion()
  258.  
  259. # 随机添加边，生成迷宫，第一个参数为起点；第二个参数为额外随机生成的边，可以表示为图的复杂程度
  260. theMap.randomCreateMap(0, 20)
  261.  
  262. # 初始化迷宫
  263. theMap.initMap()
  264.  
  265. # A* 算法寻路
  266. theAstar = Astar(theMap, 0, 399)  # 设置起点和终点
  267. theAstar.find_way()  # 寻路
  268. theAstar.show_way()  # 显示最短路径
  269.  
  270. # 输出最短路径长度
  271. theCost = theAstar.get_cost()
  272. if theCost == -1:
  273.     print("不存在该路径！")
  274. else:
  275.     print("从起点到终点的最短路径长度为: ", theCost)
  276.  
  277. # 关闭交互，展示结果
  278. plt.ioff()
  279. plt.show()

声明：本文内容由网友自发贡献，转载请注明出处：【wpsshop博客】
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号