A*算法python实践

A * 算法简介

略。

流程说明

确定地图

s 为起点， e 为终点， # 为可规划路径，O 为障碍物。

 # # # O #
 # # # # #
 s # # O #
 # # # O e
 # # # O #

规定： ↑↓ ←→需要1步与↖↙↗↘ 需要1.4步
规定：向下为x轴正方向，向右为y轴正方向

流程

1、初始化栅格为一个列表

包含以下内容：

• [[父节点坐标], [当前点坐标], f, g, h, string ]
• 如起点s 我们可以定义为 [[3, 1], [3, 1], 5, 0, 5, ‘#’]
  其中代价 f = g + h
  g为已经走过的路径长度
  h为当前点到终点的曼哈顿距离
  则我们可以认定起点的父节点依旧是本身，g = 0(因为未曾出发), h = 1 + 4(x轴距离，y轴距离)

2、迭代计算

2.1 寻找子节点

本算法寻找周围8个方向的子节点，如果该子节点没有父节点且在地图上，则更新其为当前节点的子节点。
8个方向分别为：

• wn西北，n北，en东北
• w西，e东
• ws西南，s南，es东南

child = {
            'wn': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0}, 
            'n': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
            'en': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0}, 
            'w': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
            'e': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0}, 
            'ws': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
            's': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0}, 
            'es': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0}
        }

例如我们有起点

• [[3, 1], [3, 1], 5, 0, 5, ‘#’]
  对于起点周围的 8 个方向的点进行计算

child = {
			'e': {'f': 5, 'g': 1, 'h': 4, 'x': 3, 'y': 2},
			 'en': {'f': 6.4, 'g': 1.4, 'h': 5, 'x': 2, 'y': 2},
			 'es': {'f': 4.4, 'g': 1.4, 'h': 3, 'x': 4, 'y': 2},
			 'n': {'f': 7, 'g': 1, 'h': 6, 'x': 2, 'y': 1},
			 's': {'f': 5, 'g': 1, 'h': 4, 'x': 4, 'y': 1},
			 'w': {'f': 8, 'g': 1, 'h': 7, 'x': 3, 'y': -1},
			 'wn': {'f': 9.4, 'g': 1.4, 'h': 8, 'x': 2, 'y': -1},
			 'ws': {'f': 7.4, 'g': 1.4, 'h': 6, 'x': 4, 'y': -1}
		 }

坐标中存在负数我们认为该点不存在，则实际对于起点来说，子节点为：

child = {
			'e': {'f': 5, 'g': 1, 'h': 4, 'x': 3, 'y': 2},
			 'en': {'f': 6.4, 'g': 1.4, 'h': 5, 'x': 2, 'y': 2},
			 'es': {'f': 4.4, 'g': 1.4, 'h': 3, 'x': 4, 'y': 2},
			 'n': {'f': 7, 'g': 1, 'h': 6, 'x': 2, 'y': 1},
			 's': {'f': 5, 'g': 1, 'h': 4, 'x': 4, 'y': 1}
		 }

2.2更新子节点的父节点以及代价信息

如果该子节点没有父节点，且可规划（不是障碍物’O’）
更新子节点的f, g, h,以及父节点坐标。
以起点为例：

• [[3, 1], [3, 1], 5, 0, 5, ‘#’]
  可以得到周围5个有效子节点

 # #
 s #
 # #

• [[3, 1], [2, 1], 7, 1, 6, ‘#’]，[[3, 1], [2, 2], 6.4, 1.4, 5, ‘#’],
• [[3, 1], [3, 1], 5, 0, 5, ‘s’]，[[3, 1], [3, 2], 5, 1, 4, ‘#’]
• [[3, 1], [4, 1], 5, 1, 4, ‘#’]，[[3, 1], [4, 2], 4.4, 1.4, 3, ‘#’]
• g值 为父节点的 g + 父节点到当前点的g， h依旧为曼哈顿距离，f = g + h

2.3 根据子节点中的f值迭代

• 取得当前点的g值
• 计算8个方向的坐标与g值
• 将8个方向的子节点加到open list 中：

•   如果不在open list也不在close list中，直接添加
  

•   如果在open list中， 判断该点g值，是open list中的好还是child中的好（g越小越好）
    	如果child比open list中的更好，则更新open list中该点的father,cost
  

•   如果在close list中，判断该点g值，哪一个好
    	如果child比close list中的更好，则更新close list中该点的father,cost
  

• 把当前点加到close list中
• 更新当前点，即选取child中f最小的那个点
• 直至找到终点（或者搜完整个地图）

2.4完整栅格表

[[[[2, 2], [1, 1], 9.8, 2.8, 7, '#'],  [[2, 2], [1, 2], 8.4, 2.4, 6, '#'],  [[2, 2], [1, 3], 7.8, 2.8, 5, '#'],  [[0, 0], [1, 4], 0, 0, 0, 'O'],  [[2, 4], [1, 5], 7.8, 4.8, 3, '#']]

 [[[3, 1], [2, 1], 7, 1, 6, '#'],  [[3, 1], [2, 2], 6.4, 1.4, 5, '#'],  [[3, 2], [2, 3], 6.4, 2.4, 4, '#'],  [[3, 3], [2, 4], 6.4, 3.4, 3, '#'],  [[2, 4], [2, 5], 6.4, 4.4, 2, '#']]
 
 [[[3, 1], [3, 1], 5, 0, 5, 's'],  [[3, 1], [3, 2], 5, 1, 4, '#'],  [[3, 2], [3, 3], 5, 2, 3, '#'],  [[0, 0], [3, 4], 0, 0, 0, 'O'],  [[2, 4], [3, 5], 5.8, 4.8, 1, '#']]
 
 [[[3, 1], [4, 1], 5, 1, 4, '#'],  [[3, 1], [4, 2], 4.4, 1.4, 3, '#'],  [[4, 2], [4, 3], 4.4, 2.4, 2, '#'],  [[0, 0], [4, 4], 0, 0, 0, 'O'],  [[0, 0], [4, 5], 0, 0, 0, 'e']]
 
 [[[4, 1], [5, 1], 7, 2, 5, '#'],  [[4, 2], [5, 2], 6.4, 2.4, 4, '#'],  [[4, 2], [5, 3], 5.8, 2.8, 3, '#'],  [[0, 0], [5, 4], 0, 0, 0, 'O'],  [[0, 0], [5, 5], 0, 0, 0, '#']]]

图形逻辑

1、确定栅格地图

• 起点为 ‘S’ 终点为’E’, 寻找起点周围的8个方向的子节点。
  • 去除非法节点。
    • 坐标不在地图上的点
    • 子节点处于障碍物上的点
  • 计算每一个节点代价 f = g + h

• 取起点的最优子节点
  • 再次寻找8个子节点
    • 重复

不想写了

代码

代码在git上

运行结果