基本A 算法python实现

太过爱你忘了你带给我的痛 2022-02-14 00:49 223阅读 0赞

本文由恋花蝶发表于[http://blog.csdn.net/lanphaday][http_blog.csdn.net_lanphaday]，可以在保证全文完整的前提下任意形式自由传播，但必须保留本声明，违反必究。

最近因为一个任务要用到A\*算法，就用C++实现了一份。不过只是用A\*来检测从A点到B点有无通路，不必输出路径，后来想把代码贴出来，但又觉得不如实现一个简单的寻路应用好一些，就用python写了一个版本贴上来。  
 A\*算法不仅仅可以用来寻路，寻路也不仅仅使用A\*算法。这是使用学习和使用A\*算法最要谨记的一点吧~  
 A\*算法用以寻路实现算不得是人工智能，他本质上是一种启发式的试探回溯算法，不过业界似乎喜欢把它称为游戏人工智能（GameAI）的一个组成部分，听起来就“豪华”得多了。A\*算法需要很大的内存（相对于深度优先搜索），需要很实现比较复杂的逻辑，容易出错。  
 A\*过程：  
 1.将开始节点放入开放列表(开始节点的F和G值都视为0);  
 2.重复一下步骤:  
  i.在开放列表中查找具有最小F值的节点,并把查找到的节点作为当前节点;  
  ii.把当前节点从开放列表删除, 加入到封闭列表;  
  iii.对当前节点相邻的每一个节点依次执行以下步骤:  
   1.如果该相邻节点不可通行或者该相邻节点已经在封闭列表中,则什么操作也不执行,继续检验下一个节点;  
   2.如果该相邻节点不在开放列表中,则将该节点添加到开放列表中, 并将该相邻节点的父节点设为当前节点,同时保存该相邻节点的G和F值;  
   3.如果该相邻节点在开放列表中, 则判断若经由当前节点到达该相邻节点的G值是否小于原来保存的G值,若小于,则将该相邻节点的父节点设为当前节点,并重新设置该相邻节点的G和F值.  
  iv.循环结束条件:  
   当终点节点被加入到开放列表作为待检验节点时, 表示路径被找到,此时应终止循环;  
   或者当开放列表为空,表明已无可以添加的新节点,而已检验的节点中没有终点节点则意味着路径无法被找到,此时也结束循环;  
 3.从终点节点开始沿父节点遍历, 并保存整个遍历到的节点坐标,遍历所得的节点就是最后得到的路径;

好了，废话不多说，看代码吧，带详尽注释，但可能存在bug~，另：本示例程序未作优化。

参考资料：  
[http://www.gamedev.net/reference/programming/features/astar/default.asp][http_www.gamedev.net_reference_programming_features_astar_default.asp]

[http://blog.csdn.net/win32asn/archive/2006/03/17/627098.aspx][http_blog.csdn.net_win32asn_archive_2006_03_17_627098.aspx]

![None.gif][] \#  -\*- coding: utf-8 -\*-  
![None.gif][] import  math  
![None.gif][]  
![None.gif][] \# 地图  
![None.gif][] tm  =  \[  
![None.gif][] ' \#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\# ' ,  
![None.gif][] ' \#..........................................................\# ' ,  
![None.gif][] ' \#.............................\#............................\# ' ,  
![None.gif][] ' \#.............................\#............................\# ' ,  
![None.gif][] ' \#.............................\#............................\# ' ,  
![None.gif][] ' \#.......S.....................\#............................\# ' ,  
![None.gif][] ' \#.............................\#............................\# ' ,  
![None.gif][] ' \#.............................\#............................\# ' ,  
![None.gif][] ' \#.............................\#............................\# ' ,  
![None.gif][] ' \#.............................\#............................\# ' ,  
![None.gif][] ' \#.............................\#............................\# ' ,  
![None.gif][] ' \#.............................\#............................\# ' ,  
![None.gif][] ' \#.............................\#............................\# ' ,  
![None.gif][] ' \#\#\#\#\#\#\#.\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#............\# ' ,  
![None.gif][] ' \#....\#........\#............................................\# ' ,  
![None.gif][] ' \#....\#........\#............................................\# ' ,  
![None.gif][] ' \#....\#\#\#\#\#\#\#\#\#\#............................................\# ' ,  
![None.gif][] ' \#..........................................................\# ' ,  
![None.gif][] ' \#..........................................................\# ' ,  
![None.gif][] ' \#..........................................................\# ' ,  
![None.gif][] ' \#..........................................................\# ' ,  
![None.gif][] ' \#..........................................................\# ' ,  
![None.gif][] ' \#...............................\#\#\#\#\#\#\#\#\#\#\#\#\#\#.............\# ' ,  
![None.gif][] ' \#...............................\#........E...\#.............\# ' ,  
![None.gif][] ' \#...............................\#............\#.............\# ' ,  
![None.gif][] ' \#...............................\#............\#.............\# ' ,  
![None.gif][] ' \#...............................\#............\#.............\# ' ,  
![None.gif][] ' \#...............................\#\#\#\#\#\#\#\#\#\#\#..\#.............\# ' ,  
![None.gif][] ' \#..........................................................\# ' ,  
![None.gif][] ' \#..........................................................\# ' ,  
![None.gif][] ' \#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\# ' \]  
![None.gif][]  
![None.gif][] \# 因为python里string不能直接改变某一元素，所以用test\_map来存储搜索时的地图  
![None.gif][] test\_map  =  \[\]  
![None.gif][]  
![None.gif][] \# \#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#  
![None.gif][] class  Node\_Elem:  
![None.gif][]     """  
![None.gif][]    开放列表和关闭列表的元素类型，parent用来在成功的时候回溯路径  
![None.gif][]     """  
![None.gif][]     def   \_\_init\_\_ (self, parent, x, y, dist):  
![None.gif][]        self.parent  =  parent  
![None.gif][]        self.x  =  x  
![None.gif][]        self.y  =  y  
![None.gif][]        self.dist  =  dist  
![None.gif][]          
![None.gif][] class  A\_Star:  
![None.gif][]     """  
![None.gif][]    A星算法实现类  
![None.gif][]     """  
![None.gif][]     \# 注意w,h两个参数，如果你修改了地图，需要传入一个正确值或者修改这里的默认参数  
![None.gif][]      def   \_\_init\_\_ (self, s\_x, s\_y, e\_x, e\_y, w = 60 , h = 30 ):  
![None.gif][]        self.s\_x  =  s\_x  
![None.gif][]        self.s\_y  =  s\_y  
![None.gif][]        self.e\_x  =  e\_x  
![None.gif][]        self.e\_y  =  e\_y  
![None.gif][]          
![None.gif][]        self.width  =  w  
![None.gif][]        self.height  =  h  
![None.gif][]          
![None.gif][]        self.open  =  \[\]  
![None.gif][]        self.close  =  \[\]  
![None.gif][]        self.path  =  \[\]  
![None.gif][]          
![None.gif][]     \# 查找路径的入口函数  
![None.gif][]      def  find\_path(self):  
![None.gif][]         \# 构建开始节点  
![None.gif][]         p  =  Node\_Elem(None, self.s\_x, self.s\_y,  0.0 )  
![None.gif][]         while  True:  
![None.gif][]             \# 扩展F值最小的节点  
![None.gif][]             self.extend\_round(p)  
![None.gif][]             \# 如果开放列表为空，则不存在路径，返回  
![None.gif][]              if   not  self.open:  
![None.gif][]                 return  
![None.gif][]             \# 获取F值最小的节点  
![None.gif][]             idx, p  =  self.get\_best()  
![None.gif][]             \# 找到路径，生成路径，返回  
![None.gif][]              if  self.is\_target(p):  
![None.gif][]                self.make\_path(p)  
![None.gif][]                 return  
![None.gif][]             \# 把此节点压入关闭列表，并从开放列表里删除  
![None.gif][]             self.close.append(p)  
![None.gif][]             del  self.open\[idx\]  
![None.gif][]              
![None.gif][]     def  make\_path(self,p):  
![None.gif][]         \# 从结束点回溯到开始点，开始点的parent == None  
![None.gif][]          while  p:  
![None.gif][]            self.path.append((p.x, p.y))  
![None.gif][]            p  =  p.parent  
![None.gif][]          
![None.gif][]     def  is\_target(self, i):  
![None.gif][]         return  i.x  ==  self.e\_x  and  i.y  ==  self.e\_y  
![None.gif][]          
![None.gif][]     def  get\_best(self):  
![None.gif][]        best  =  None  
![None.gif][]        bv  =   1000000   \# 如果你修改的地图很大，可能需要修改这个值  
![None.gif][]         bi  =   - 1  
![None.gif][]         for  idx, i  in  enumerate(self.open):  
![None.gif][]            value  =  self.get\_dist(i) \# 获取F值  
![None.gif][]              if  value  <  bv: \# 比以前的更好，即F值更小  
![None.gif][]                 best  =  i  
![None.gif][]                bv  =  value  
![None.gif][]                bi  =  idx  
![None.gif][]         return  bi, best  
![None.gif][]          
![None.gif][]     def  get\_dist(self, i):  
![None.gif][]         \#  F = G + H  
![None.gif][]          \#  G 为已经走过的路径长度， H为估计还要走多远  
![None.gif][]          \#  这个公式就是A\*算法的精华了。  
![None.gif][]          return  i.dist  +  math.sqrt(  
![None.gif][]            (self.e\_x - i.x) \* (self.e\_x - i.x)  
![None.gif][]             +  (self.e\_y - i.y) \* (self.e\_y - i.y)) \* 1.2  
![None.gif][]          
![None.gif][]     def  extend\_round(self, p):  
![None.gif][]         \# 可以从8个方向走  
![None.gif][]         xs  =  ( - 1 , 0,  1 ,  - 1 ,  1 ,  - 1 , 0,  1 )  
![None.gif][]        ys  =  ( - 1 , - 1 , - 1 ,  0, 0,   1 ,  1 ,  1 )  
![None.gif][]         \# 只能走上下左右四个方向  
![None.gif][]\#         xs = (0, -1, 1, 0)  
![None.gif][]\#         ys = (-1, 0, 0, 1)  
![None.gif][]          for  x, y  in  zip(xs, ys):  
![None.gif][]            new\_x, new\_y  =  x  +  p.x, y  +  p.y  
![None.gif][]             \# 无效或者不可行走区域，则勿略  
![None.gif][]              if   not  self.is\_valid\_coord(new\_x, new\_y):  
![None.gif][]                 continue  
![None.gif][]             \# 构造新的节点  
![None.gif][]             node  =  Node\_Elem(p, new\_x, new\_y, p.dist + self.get\_cost(  
![None.gif][]                        p.x, p.y, new\_x, new\_y))  
![None.gif][]             \# 新节点在关闭列表，则忽略  
![None.gif][]              if  self.node\_in\_close(node):  
![None.gif][]                 continue  
![None.gif][]            i  =  self.node\_in\_open(node)  
![None.gif][]             if  i  !=   - 1 :  
![None.gif][]                 \# 新节点在开放列表  
![None.gif][]                  if  self.open\[i\].dist  >  node.dist:  
![None.gif][]                     \# 现在的路径到比以前到这个节点的路径更好~  
![None.gif][]                      \# 则使用现在的路径  
![None.gif][]                     self.open\[i\].parent  =  p  
![None.gif][]                    self.open\[i\].dist  =  node.dist  
![None.gif][]                 continue  
![None.gif][]            self.open.append(node)  
![None.gif][]              
![None.gif][]     def  get\_cost(self, x1, y1, x2, y2):  
![None.gif][]         """  
![None.gif][]        上下左右直走，代价为1.0，斜走，代价为1.4  
![None.gif][]         """  
![None.gif][]         if  x1  ==  x2  or  y1  ==  y2:  
![None.gif][]             return   1.0  
![None.gif][]         return   1.4  
![None.gif][]          
![None.gif][]     def  node\_in\_close(self, node):  
![None.gif][]         for  i  in  self.close:  
![None.gif][]             if  node.x  ==  i.x  and  node.y  ==  i.y:  
![None.gif][]                 return  True  
![None.gif][]         return  False  
![None.gif][]          
![None.gif][]     def  node\_in\_open(self, node):  
![None.gif][]         for  i, n  in  enumerate(self.open):  
![None.gif][]             if  node.x  ==  n.x  and  node.y  ==  n.y:  
![None.gif][]                 return  i  
![None.gif][]         return   - 1  
![None.gif][]          
![None.gif][]     def  is\_valid\_coord(self, x, y):  
![None.gif][]         if  x  <  0  or  x  >=  self.width  or  y  <  0  or  y  >=  self.height:  
![None.gif][]             return  False  
![None.gif][]         return  test\_map\[y\]\[x\]  !=   ' \# '  
![None.gif][]      
![None.gif][]     def  get\_searched(self):  
![None.gif][]        l  =  \[\]  
![None.gif][]         for  i  in  self.open:  
![None.gif][]            l.append((i.x, i.y))  
![None.gif][]         for  i  in  self.close:  
![None.gif][]            l.append((i.x, i.y))  
![None.gif][]         return  l  
![None.gif][]          
![None.gif][] \# \#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#  
![None.gif][] def  print\_test\_map():  
![None.gif][]     """  
![None.gif][]    打印搜索后的地图  
![None.gif][]     """  
![None.gif][]     for  line  in  test\_map:  
![None.gif][]         print   '' .join(line)  
![None.gif][]  
![None.gif][] def  get\_start\_XY():  
![None.gif][]     return  get\_symbol\_XY( ' S ' )  
![None.gif][]      
![None.gif][] def  get\_end\_XY():  
![None.gif][]     return  get\_symbol\_XY( ' E ' )  
![None.gif][]      
![None.gif][] def  get\_symbol\_XY(s):  
![None.gif][]     for  y, line  in  enumerate(test\_map):  
![None.gif][]         try :  
![None.gif][]            x  =  line.index(s)  
![None.gif][]         except :  
![None.gif][]             continue  
![None.gif][]         else :  
![None.gif][]             break  
![None.gif][]     return  x, y  
![None.gif][]          
![None.gif][] \# \#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#\#  
![None.gif][] def  mark\_path(l):  
![None.gif][]    mark\_symbol(l,  ' \* ' )  
![None.gif][]      
![None.gif][] def  mark\_searched(l):  
![None.gif][]    mark\_symbol(l,  '   ' )  
![None.gif][]      
![None.gif][] def  mark\_symbol(l, s):  
![None.gif][]     for  x, y  in  l:  
![None.gif][]        test\_map\[y\]\[x\]  =  s  
![None.gif][]      
![None.gif][] def  mark\_start\_end(s\_x, s\_y, e\_x, e\_y):  
![None.gif][]    test\_map\[s\_y\]\[s\_x\]  =   ' S '  
![None.gif][]    test\_map\[e\_y\]\[e\_x\]  =   ' E '  
![None.gif][]      
![None.gif][] def  tm\_to\_test\_map():  
![None.gif][]     for  line  in  tm:  
![None.gif][]        test\_map.append(list(line))  
![None.gif][]          
![None.gif][] def  find\_path():  
![None.gif][]    s\_x, s\_y  =  get\_start\_XY()  
![None.gif][]    e\_x, e\_y  =  get\_end\_XY()  
![None.gif][]    a\_star  =  A\_Star(s\_x, s\_y, e\_x, e\_y)  
![None.gif][]    a\_star.find\_path()  
![None.gif][]    searched  =  a\_star.get\_searched()  
![None.gif][]    path  =  a\_star.path  
![None.gif][]     \# 标记已搜索区域  
![None.gif][]     mark\_searched(searched)  
![None.gif][]     \# 标记路径  
![None.gif][]     mark\_path(path)  
![None.gif][]     print   " path length is %d " % (len(path))  
![None.gif][]     print   " searched squares count is %d " % (len(searched))  
![None.gif][]     \# 标记开始、结束点  
![None.gif][]     mark\_start\_end(s\_x, s\_y, e\_x, e\_y)  
![None.gif][]      
![None.gif][] if   \_\_name\_\_   ==   " \_\_main\_\_ " :  
![None.gif][]     \# 把字符串转成列表  
![None.gif][]     tm\_to\_test\_map()  
![None.gif][]    find\_path()  
![None.gif][]    print\_test\_map()  
![None.gif][]

[http_blog.csdn.net_lanphaday]: http://blog.csdn.net/lanphaday
[http_www.gamedev.net_reference_programming_features_astar_default.asp]: http://www.gamedev.net/reference/programming/features/astar/default.asp
[http_blog.csdn.net_win32asn_archive_2006_03_17_627098.aspx]: http://blog.csdn.net/win32asn/archive/2006/03/17/627098.aspx
[None.gif]: /images/20220214/7499388a269f47b18332e2f3aa0406a1.png