A*路径规划算法

A* 是一种路径规划算法。

它解决的问题是：

我有一张地图 我知道起点 start 我知道终点 goal 地图里有障碍物 我要找一条从起点到终点的可走路线

比如地图是：

grid = [ [0, 0, 0, 0, 0], [1, 1, 0, 1, 0], [0, 0, 0, 1, 0], [0, 1, 0, 0, 0] ]

这里：

0 = 可以走 1 = 障碍物，不能走

你可以把它想成一个棋盘：

第0行: 0 0 0 0 0 第1行: 1 1 0 1 0 第2行: 0 0 0 1 0 第3行: 0 1 0 0 0

起点：

start = (0, 0)

终点：

goal = (4, 3)

注意坐标是：

(x, y) x = 第几列 y = 第几行

所以(0, 0)是左上角。

(4, 3)是第 4 列、第 3 行，也就是右下附近。

2. A* 的核心思想
A* 不会乱走。

它每次都会判断：

哪个点最值得我先探索？

判断方法靠这个公式：

f = g + h

这里三个字母很重要。

g = 从起点走到当前点，已经走了多少步 h = 从当前点到终点，估计还要走多少步 f = 总估计代价

比如当前有一个点：

current = (2, 1)

如果从起点走到它已经用了 3 步：

g = 3

它离终点估计还差 4 步：

h = 4

那么：

f = g + h = 7

另一个点：

g = 5 h = 6 f = 11

那 A* 会优先看f = 7的点。

因为它看起来更有希望更快到终点。

3. h 怎么算？
这里的h叫 heuristic，中文一般叫“启发式函数”。

最简单的是曼哈顿距离：

abs(x1 - x2) + abs(y1 - y2)

它的意思是：

横着差多少 + 竖着差多少

比如：

current = (2, 1) goal = (4, 3)

那：

x 差距 = |2 - 4| = 2 y 差距 = |1 - 3| = 2 h = 2 + 2 = 4

对应代码：

def heuristic(a, b): x1, y1 = a x2, y2 = b return abs(x1 - x2) + abs(y1 - y2)

逐句解释：

def heuristic(a, b):

定义一个函数，输入两个点。

比如：

a = (2, 1) b = (4, 3)

然后：

x1, y1 = a x2, y2 = b

把坐标拆开。

return abs(x1 - x2) + abs(y1 - y2)

返回横向距离加纵向距离。

4. 找邻居 get_neighbors
A* 每次处理一个点时，要看它周围哪些点能走。

比如当前点：

node = (2, 1)

它周围理论上有四个方向：

右边: (3, 1) 左边: (1, 1) 下边: (2, 2) 上边: (2, 0)

代码是：

def get_neighbors(node, grid): x, y = node directions = [ (1, 0), (-1, 0), (0, 1), (0, -1) ] neighbors = [] for dx, dy in directions: nx = x + dx ny = y + dy if 0 <= ny < len(grid) and 0 <= nx < len(grid[0]): if grid[ny][nx] == 0: neighbors.append((nx, ny)) return neighbors

这段很关键。

先看：

x, y = node

如果：

node = (2, 1)

那么：

x = 2 y = 1

再看：

directions = [ (1, 0), (-1, 0), (0, 1), (0, -1) ]

这是四个移动方向。

(1, 0) = x + 1，往右 (-1, 0) = x - 1，往左 (0, 1) = y + 1，往下 (0, -1) = y - 1，往上

然后：

neighbors = []

创建一个空 list，用来装“可以走的邻居”。

接着：

for dx, dy in directions:

意思是一个方向一个方向试。

第一次：

dx = 1 dy = 0

第二次：

dx = -1 dy = 0

第三次：

dx = 0 dy = 1

第四次：

dx = 0 dy = -1

然后：

nx = x + dx ny = y + dy

算出新点坐标。

比如当前点(2, 1)，方向(1, 0)：

nx = 2 + 1 = 3 ny = 1 + 0 = 1

所以新点是：

(3, 1)

然后检查有没有出界：

if 0 <= ny < len(grid) and 0 <= nx < len(grid[0]):

这句话意思是：

ny 必须在合法行范围内 nx 必须在合法列范围内

比如这个地图有 4 行：

len(grid) == 4

所以 y 只能是：

0, 1, 2, 3

每行有 5 列：

len(grid[0]) == 5

所以 x 只能是：

0, 1, 2, 3, 4

如果新点没出界，再看是不是障碍物：

if grid[ny][nx] == 0:

这里一定注意：

grid[ny][nx]

不是：

grid[nx][ny]

因为二维 list 是：

grid[第几行][第几列]

而：

y = 行 x = 列

所以是：

grid[y][x]

如果这个格子是0，说明可以走：

neighbors.append((nx, ny))

把这个点加入邻居列表。

最后：

return neighbors

把所有可走邻居返回。

5. A* 主函数
完整主函数是：

def astar(grid, start, goal): open_list = [] heapq.heappush(open_list, (0, start)) came_from = {} g_score = {} g_score[start] = 0 closed_set = set() while open_list: current_f, current = heapq.heappop(open_list) if current == goal: return reconstruct_path(came_from, current) closed_set.add(current) for neighbor in get_neighbors(current, grid): if neighbor in closed_set: continue tentative_g = g_score[current] + 1 if neighbor not in g_score or tentative_g < g_score[neighbor]: came_from[neighbor] = current g_score[neighbor] = tentative_g h = heuristic(neighbor, goal) f = tentative_g + h heapq.heappush(open_list, (f, neighbor)) return None

我们拆开讲。

6. open_list 是什么

open_list = []

open_list是：

等待被探索的点

也就是候选点。

然后：

heapq.heappush(open_list, (0, start))

意思是把起点放进去。

注意放进去的是：

(0, start)

不是单纯的：

start

因为 A* 要按照f值排序。

比如：

(7, (2, 1))

意思是：

点 (2,1) 的 f 值是 7

heapq的作用就是：

每次自动帮你拿出 f 最小的点

7. came_from 是什么

came_from = {}

这是一个字典。

作用是：

记录每个点是从哪里来的

比如：

came_from[(2, 1)] = (2, 0)

意思是：

(2,1) 这个点是从 (2,0) 走来的

最后找到终点之后，就靠这个字典倒推路径。

8. g_score 是什么

g_score = {} g_score[start] = 0

g_score也是字典。

它记录：

从起点到某个点已经用了多少步

起点到起点当然是 0：

g_score[start] = 0

比如之后可能有：

g_score[(2, 1)] = 3

意思是：

从起点走到 (2,1) 已经用了 3 步

9. closed_set 是什么

closed_set = set()

这是集合。

作用是：

记录已经处理过的点

比如一个点已经被处理完了，就放进去：

closed_set.add(current)

之后如果又遇到它：

if neighbor in closed_set: continue

就跳过。

这样可以避免重复绕圈。

10. while open_list 是什么

while open_list:

意思是：

只要 open_list 里还有候选点，就继续找

如果 open_list 空了，还没找到终点，说明没有路。

所以最后：

return None

意思是找不到路径。

11. 取出 f 最小的点

current_f, current = heapq.heappop(open_list)

这句意思是：

从 open_list 里面取出 f 最小的点

比如取出来：

current_f = 5 current = (2, 1)

那当前要处理的点就是(2,1)。

这就是 A* 的核心：

每次优先处理 f 最小的点

12. 判断是不是终点

if current == goal: return reconstruct_path(came_from, current)

意思是：

如果当前点就是终点，说明路径找到了

然后调用：

reconstruct_path(came_from, current)

把完整路径还原出来。

13. 把 current 标记为处理过

closed_set.add(current)

意思是：

这个点我已经检查过它的邻居了 以后别重复处理

14. 检查 current 的邻居

for neighbor in get_neighbors(current, grid):

这句意思是：

把当前点上下左右能走的点拿出来，一个一个检查

比如：

current = (2, 1)

可能返回：

[(2, 2), (2, 0)]

然后循环检查每个邻居。

15. 如果邻居处理过，就跳过

if neighbor in closed_set: continue

continue的意思是：

跳过这一次循环，直接看下一个 neighbor

也就是说：

如果这个邻居已经处理过了，就别重复处理

16. 计算 tentative_g

tentative_g = g_score[current] + 1

这句非常重要。

tentative_g的意思是：

如果我从 current 走到 neighbor，那么 neighbor 的新 g 值是多少？

因为每走一步代价是 1。

所以：

neighbor 的 g = current 的 g + 1

比如：

g_score[current] = 3

那么：

tentative_g = 4

意思是如果从当前点走到邻居，邻居离起点就是 4 步。

17. 判断这条路线是不是更好

if neighbor not in g_score or tentative_g < g_score[neighbor]:

这句意思是：

如果这个邻居以前没见过 或者 这次走到它的路线比以前更短 那就更新

比如以前：

g_score[neighbor] = 8

现在：

tentative_g = 5

说明现在这条路更短。

所以更新。

18. 更新 came_from

came_from[neighbor] = current

意思是：

neighbor 是从 current 走过来的

比如：

came_from[(2, 2)] = (2, 1)

意思是：

(2,2) 的上一站是 (2,1)

19. 更新 g_score

g_score[neighbor] = tentative_g

意思是：

记录从起点走到 neighbor 需要多少步

20. 计算 h 和 f

h = heuristic(neighbor, goal) f = tentative_g + h

这里：

tentative_g = g h = 估计到终点还差多少 f = g + h

比如：

g = 4 h = 3 f = 7

21. 把 neighbor 加入 open_list

heapq.heappush(open_list, (f, neighbor))

意思是：

把这个邻居加入候选点列表 它的优先级是 f

之后 A* 会继续从open_list里拿出f最小的点。

22. reconstruct_path 是什么
代码：

def reconstruct_path(came_from, current): path = [current] while current in came_from: current = came_from[current] path.append(current) path.reverse() return path

这个函数负责：

从终点倒着找回起点

比如最终到了：

current = (4, 3)

然后came_from里记录：

came_from[(4, 3)] = (3, 3) came_from[(3, 3)] = (2, 3) came_from[(2, 3)] = (2, 2) came_from[(2, 2)] = (2, 1) came_from[(2, 1)] = (2, 0) came_from[(2, 0)] = (1, 0) came_from[(1, 0)] = (0, 0)

那它就会倒着找：

(4,3) (3,3) (2,3) (2,2) (2,1) (2,0) (1,0) (0,0)

但这个顺序是：

终点 → 起点

所以需要：

path.reverse()

变成：

起点 → 终点

最后返回：

[(0, 0), (1, 0), (2, 0), (2, 1), (2, 2), (2, 3), (3, 3), (4, 3)]

23. 完整代码
你可以把 Day 03 的完整代码写成这样：

import heapq def heuristic(a, b): x1, y1 = a x2, y2 = b return abs(x1 - x2) + abs(y1 - y2) def get_neighbors(node, grid): x, y = node directions = [ (1, 0), (-1, 0), (0, 1), (0, -1) ] neighbors = [] for dx, dy in directions: nx = x + dx ny = y + dy if 0 <= ny < len(grid) and 0 <= nx < len(grid[0]): if grid[ny][nx] == 0: neighbors.append((nx, ny)) return neighbors def reconstruct_path(came_from, current): path = [current] while current in came_from: current = came_from[current] path.append(current) path.reverse() return path def astar(grid, start, goal): open_list = [] heapq.heappush(open_list, (0, start)) came_from = {} g_score = {} g_score[start] = 0 closed_set = set() while open_list: current_f, current = heapq.heappop(open_list) if current == goal: return reconstruct_path(came_from, current) closed_set.add(current) for neighbor in get_neighbors(current, grid): if neighbor in closed_set: continue tentative_g = g_score[current] + 1 if neighbor not in g_score or tentative_g < g_score[neighbor]: came_from[neighbor] = current g_score[neighbor] = tentative_g h = heuristic(neighbor, goal) f = tentative_g + h heapq.heappush(open_list, (f, neighbor)) return None grid = [ [0, 0, 0, 0, 0], [1, 1, 0, 1, 0], [0, 0, 0, 1, 0], [0, 1, 0, 0, 0] ] start = (0, 0) goal = (4, 3) path = astar(grid, start, goal) print(path)

24. 最后用人话串起来
A* 干的事情就是：

先把起点放进 open_list 然后每次拿出 f 最小的点 看它是不是终点 如果不是，就找它周围能走的邻居 给邻居计算 g、h、f 如果发现更短路线，就更新 came_from 和 g_score 然后把邻居放回 open_list 一直重复 直到找到终点

最重要的 5 个变量你先背下来：

open_list = 还没探索、但可以考虑的点 closed_set = 已经探索过的点 g_score = 从起点走到某点已经用了多少步 heuristic = 估计某点到终点还差多少步 came_from = 记录路径，每个点的上一站是谁