A*算法 python实现 讲解

import heapq

def heuristic(a, b):
    # 曼哈顿距离（适合栅格地图）
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def a_star(start, goal, grid):
    rows, cols = len(grid), len(grid[0])

    def neighbors(node):
        x, y = node
        dirs = [(1,0), (-1,0), (0,1), (0,-1)]
        result = []
        for dx, dy in dirs:
            nx, ny = x + dx, y + dy
            if 0 <= nx < rows and 0 <= ny < cols:
                if grid[nx][ny] == 0:  # 0表示可走
                    result.append((nx, ny))
        return result

    open_list = []
    heapq.heappush(open_list, (0, start))

    came_from = {}
    g = {start: 0}

    while open_list:
        _, current = heapq.heappop(open_list)

        if current == goal:
            # 回溯路径
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            path.append(start)
            path.reverse()
            return path

        for nxt in neighbors(current):
            new_cost = g[current] + 1

            if nxt not in g or new_cost < g[nxt]:
                g[nxt] = new_cost
                priority = new_cost + heuristic(nxt, goal)
                heapq.heappush(open_list, (priority, nxt))
                came_from[nxt] = current

    return None

PART-1导入模块

import heapq

heapq=python里的优先队列，A*的核心就是：每次选 f 最小的节点，open_list 里面一堆点，谁的 f 最小，谁先被处理

PART2-启发函数

def heuristic(a, b):
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

这是曼哈顿距离（两点最短距离的一种计算方法）

PART3-主函数

def a_star(start, goal, grid):

start：起点

goal：终点

grid：地图（0=可走，1=障碍）

PART4-地图尺寸

rows, cols = len(grid), len(grid[0])

PART5-定义邻居函数

    rows, cols = len(grid), len(grid[0])

    def neighbors(node):
        x, y = node    #当前点坐标拆开
        dirs = [(1,0), (-1,0), (0,1), (0,-1)]    #表示四个方向：下、上、右、左
        result = []    #存合法邻居
        for dx, dy in dirs:
            nx, ny = x + dx, y + dy    #尝试走一步
            if 0 <= nx < rows and 0 <= ny < cols:    #防止走出地图
                if grid[nx][ny] == 0:  # 0表示可走，1表示障碍
                    result.append((nx, ny))
        return result    #返回所有能走的邻居

见注释

PART6-初始化open_list

open_list = []
heapq.heappush(open_list, (0, start))

压进去的是：(priority,node)        priority就是f

一开始f(start)=0

open_list=[]        是一个待检查的节点列表

heapq.heappush(open_list,(0,start))        把起点放进待检查列表，但不是直接放坐标，而是放一个元组：（优先级priority,坐标node）

(0, start) 的 priority为什么=0？

A*的公式：f=g+h

• g：从起点走到当前点的实际步数 / 代价
• h：从当前点到终点的预估距离（heuristic）
• f：总优先级（f 越小，越优先走）

起点的情况：还没走任何路，所以g=0，h是起点到终点的预估距离，但算法规定：起点的f直接填0即可，即f(起点)=0，所以 (f值,坐标) = (0,start) 

heapq 是什么？

heapq是Python的优先队列，它的作用是每次自动拿出f最小的点先走，存进去的格式必须是(优先级f，坐标)，heapq会按第一个数字排序。

PART-7路径记录

came_from = {}

用来记录：当前点 ← 从哪里来的，例如(2,2) ← (1,2)

PART8-g值（真实代价）

g = {start: 0}

表示：起点到起点 = 0

PART9-主循环

while open_list:

只要还有点可以探索，就继续

    _, current = heapq.heappop(open_list)

取出f最小的那个点

heapq.heappop(open_list)        从优先队列open_list里弹出一个元素，这个元素是我们之前存的(优先级f值，坐标点)，比如(12,(3,5))

_,current=...        这是多元赋值，左边是两个变量，接收右边元组的两个值，。_就是优先级f值，下划线表示“我拿到了，但暂时不用它。current接收坐标点，比如(3,5)

if current == goal:

判断是否到达终点。找到了，准备回溯路径

path = []
while current in came_from:
    path.append(current)
    current = came_from[current]

回溯路径：终点 → 上一个 → 上一个 → 起点，倒着走回家，然后把路线翻正，变成起点到终点的路径

nxt是next的缩写

字典came_from        came_from[nxt] = current        意思是”我从哪里走到这个点的“，比如came_from[(2,3)] = (2,2) → 走到 (2,3) 是从 (2,2) 过来的

path=[]        创建一个空列表，用来存倒着的路径。

while current in came_from        只要当前点还能找到 “上一步是从哪来的”，就继续往回走。

path.append(current)        把当前点放进路径里。

current=came_from[current]        把当前点变成它的上一个点

path.append(start)
path.reverse()
return path

这个好理解，加上起点并且反转一下

for nxt in neighbors(current):

把当前点的 “上下左右” 四个邻居，一个一个拿出来检查

new_cost = g[current] + 1

走到下一个点的总代价 = 走到现在的代价 + 走一步的代价

if nxt not in g or new_cost < g[nxt]:

nxt not in g        从来没来过这个点，所以要走过去看看

new_cost<g[nxt]        以前来过，但现在这条路更短，所以要更新成最短路径

g[nxt] = new_cost
priority = new_cost + heuristic(nxt, goal)

A*核心公式：f=g+h

g=new_cost        已经走了多远

h=heuristic(...)        估计还要走多远

f=priority        总优先级，越小越优先走

heapq.heappush(open_list, (priority, nxt))

把这个点，按优先级放进 “待探索列表”，等着以后检查。优先队列会自动排序，永远先拿 f 最小的点走。

came_from[nxt] = current

记录：我是从哪个点走到这里的。

PART10-如果找不到路径

return None

说明被障碍完全挡住