A*路径查找算法无法找到第二个目标的技术求助

我开发了一个程序，需要依次规划起点→目标点→终点的两段式路径。目前程序能成功找到第一个目标点，但无法定位第二个终点，具体问题表现为：

• 找到目标点后，搜索方向变得随机，类似Dijkstra算法的行为
• 若访问到终点节点，会直接将其当作空节点覆盖
• 算法不会访问第一步中已检查过的节点

原始问题代码 #

def A_Star_algorithm(draw, grid, start, end, target_check, target):

    if not target:
        count = 0
        open_set = PriorityQueue()
        open_set.put((0, count, start))
        came_from = {}
        g_score = {node: float("inf") for row in grid for node in row}
        g_score[start] = 0
        f_score = {node: float("inf") for row in grid for node in row}
        f_score[start] = h(start.get_pos(), end.get_pos())
        
        open_set_hash = {start}
        
        while not open_set.empty():
            for event in pygame.event.get():
                if event.type == pygame.QUIT:
                    pygame.quit()
            current = open_set.get()[2]
            open_set_hash.remove(current)
            
            if current == end:
                reconstruct_path(came_from, end, draw, start)
                end.make_end()
                return True
            for neighbor in current.neighbors:
                temp_g_score = g_score[current] + 1
                
                if temp_g_score < g_score[neighbor]:
                    came_from[neighbor] = current
                    g_score[neighbor] = temp_g_score
                    f_score[neighbor] = temp_g_score + h(neighbor.get_pos(), end.get_pos())
                    if neighbor not in open_set_hash:
                        count += 1
                        open_set.put((f_score[neighbor], count, neighbor))
                        open_set_hash.add(neighbor)
                        neighbor.make_open()
            draw()
            if current != start:
                current.make_closed()
                
                
                
    elif target:
        count = 0
        open_set = PriorityQueue()
        open_set.put((0, count, start))
        came_from = {}
        g_score = {node: float("inf") for row in grid for node in row}
        g_score[start] = 0
        f_score = {node: float("inf") for row in grid for node in row}
        f_score[start] = h(start.get_pos(), target.get_pos())

        open_set_hash = {start}
        
        while not open_set.empty():
            for event in pygame.event.get():
                if event.type == pygame.QUIT:
                    pygame.quit()
            current = open_set.get()[2]
            open_set_hash.remove(current)

            if current == target:
                reconstruct_path(came_from, target, draw, start)
                target.make_target()
                open_set.queue.clear()
                
            for neighbor in current.neighbors:
                temp_g_score = g_score[current] + 1

                if temp_g_score < g_score[neighbor]:
                    came_from[neighbor] = current
                    g_score[neighbor] = temp_g_score
                    f_score[neighbor] = temp_g_score + h(neighbor.get_pos(), target.get_pos())
                    if neighbor not in open_set_hash:
                        count += 1
                        open_set.put((f_score[neighbor], count, neighbor))
                        open_set_hash.add(neighbor)
                        neighbor.make_open()
            draw()
            if current != start:
                current.make_closed()
                
        count = 0
        open_set = PriorityQueue()
        open_set.put((0, count, target))
        came_from = {}
        g_score = {node: float("inf") for row in grid for node in row}
        g_score[target] = 0
        f_score = {node: float("inf") for row in grid for node in row}
        f_score[target] = h(target.get_pos(), end.get_pos())
        open_set_hash.clear()
        open_set_hash = {target}
        
        while not open_set.empty():
            for event in pygame.event.get():
                if event.type == pygame.QUIT:
                    pygame.quit()
            current = open_set.get()[2]
            open_set_hash.remove(current2)
            
            if current == end:
                reconstruct_path(came_from, end, draw, target)
                end.make_end()
                return True
            for neighbor in current.neighbors:
                temp_g_score = g_score[current2] + 1

                if temp_g_score < g_score[neighbor]:
                    came_from[neighbor] = current2
                    g_score[neighbor] = temp_g_score
                    f_score[neighbor] = temp_g_score + h(neighbor.get_pos(), end.get_pos())
                    if neighbor not in open_set_hash:
                        count += 1
                        open_set.put((f_score[neighbor], count, neighbor))
                        open_set_hash.add(neighbor)
                        neighbor.make_open()
            draw()
            if current != target:
                current.make_closed()

return False

问题根源与修复方案 #

核心问题点

1. 变量名错误：第二段搜索中使用了未定义的current2变量，导致逻辑彻底混乱，出现随机搜索的表现
2. 第一段循环未终止：找到目标点后未停止循环，继续处理目标点的邻居，干扰后续初始状态
3. 节点状态未重置：第一段搜索标记的closed节点未重置，导致第二段无法访问这些节点
4. 终点判断逻辑失效：变量错误导致终点被错误处理，出现覆盖问题

修复后的完整代码

import pygame
from queue import PriorityQueue

def h(p1, p2):
    x1, y1 = p1
    x2, y2 = p2
    return abs(x1 - x2) + abs(y1 - y2)

def reconstruct_path(came_from, current, draw, start):
    while current in came_from:
        current = came_from[current]
        current.make_path()
        draw()
    start.make_start()

def A_Star_algorithm(draw, grid, start, end, target_check, target):

    if not target:
        count = 0
        open_set = PriorityQueue()
        open_set.put((0, count, start))
        came_from = {}
        g_score = {node: float("inf") for row in grid for node in row}
        g_score[start] = 0
        f_score = {node: float("inf") for row in grid for node in row}
        f_score[start] = h(start.get_pos(), end.get_pos())
        
        open_set_hash = {start}
        
        while not open_set.empty():
            for event in pygame.event.get():
                if event.type == pygame.QUIT:
                    pygame.quit()
            current = open_set.get()[2]
            open_set_hash.remove(current)
            
            if current == end:
                reconstruct_path(came_from, end, draw, start)
                end.make_end()
                return True
            for neighbor in current.neighbors:
                temp_g_score = g_score[current] + 1
                
                if temp_g_score < g_score[neighbor]:
                    came_from[neighbor] = current
                    g_score[neighbor] = temp_g_score
                    f_score[neighbor] = temp_g_score + h(neighbor.get_pos(), end.get_pos())
                    if neighbor not in open_set_hash:
                        count += 1
                        open_set.put((f_score[neighbor], count, neighbor))
                        open_set_hash.add(neighbor)
                        neighbor.make_open()
            draw()
            if current != start:
                current.make_closed()
                
    elif target:
        count = 0
        open_set = PriorityQueue()
        open_set.put((0, count, start))
        came_from = {}
        g_score = {node: float("inf") for row in grid for node in row}
        g_score[start] = 0
        f_score = {node: float("inf") for row in grid for node in row}
        f_score[start] = h(start.get_pos(), target.get_pos())

        open_set_hash = {start}
        
        while not open_set.empty():
            for event in pygame.event.get():
                if event.type == pygame.QUIT:
                    pygame.quit()
            current = open_set.get()[2]
            open_set_hash.remove(current)

            if current == target:
                reconstruct_path(came_from, target, draw, start)
                target.make_target()
                # 找到目标点后立即终止循环
                break
                
            for neighbor in current.neighbors:
                temp_g_score = g_score[current] + 1

                if temp_g_score < g_score[neighbor]:
                    came_from[neighbor] = current
                    g_score[neighbor] = temp_g_score
                    f_score[neighbor] = temp_g_score + h(neighbor.get_pos(), target.get_pos())
                    if neighbor not in open_set_hash:
                        count += 1
                        open_set.put((f_score[neighbor], count, neighbor))
                        open_set_hash.add(neighbor)
                        neighbor.make_open()
            draw()
            if current != start:
                current.make_closed()
        
        # 重置所有节点状态，确保第二段搜索不受影响
        for row in grid:
            for node in row:
                node.reset_state()
        start.make_start()
        target.make_target()
        
        # 第二段：目标点到终点的搜索
        count = 0
        open_set = PriorityQueue()
        open_set.put((0, count, target))
        came_from = {}
        g_score = {node: float("inf") for row in grid for node in row}
        g_score[target] = 0
        f_score = {node: float("inf") for row in grid for node in row}
        f_score[target] = h(target.get_pos(), end.get_pos())
        open_set_hash = {target}
        
        while not open_set.empty():
            for event in pygame.event.get():
                if event.type == pygame.QUIT:
                    pygame.quit()
            current = open_set.get()[2]
            open_set_hash.remove(current)
            
            if current == end:
                reconstruct_path(came_from, end, draw, target)
                end.make_end()
                return True
            for neighbor in current.neighbors:
                temp_g_score = g_score[current] + 1

                if temp_g_score < g_score[neighbor]:
                    came_from[neighbor] = current
                    g_score[neighbor] = temp_g_score
                    f_score[neighbor] = temp_g_score + h(neighbor.get_pos(), end.get_pos())
                    if neighbor not in open_set_hash:
                        count += 1
                        open_set.put((f_score[neighbor], count, neighbor))
                        open_set_hash.add(neighbor)
                        neighbor.make_open()
            draw()
            if current != target:
                current.make_closed()

    return False

关键修复点

• 把第二段搜索中所有current2替换为current，修复变量未定义的逻辑错误
• 找到目标点后添加break语句，终止第一段搜索循环
• 第二段搜索前重置所有节点的open/closed状态，确保可访问历史节点
• 修正后终点判断逻辑恢复正常，不会被错误覆盖

内容的提问来源于stack exchange，提问作者Hankkk