python - A* algorithm problem when it comes to go through one way diagonal - Stack Overflow

admin管理员组

文章数量:1410705

I'm trying to make path finder, but I can't fix it to work both at the same time:

• not cutting corners

• going diagonal when it's the only way to go or it's necessary

My current code is:

def heuristic(a, b):
    """ Manhattan distance heuristic to encourage straight-line movement first """
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def get_neighbors(grid, node):
    x, y = node
    neighbors = []
    rows = len(grid)
    cols = len(grid[0]) if rows > 0 else 0

    # Straight moves (up, down, left, right) - Cost 1
    for dx, dy in [(-1,0), (1,0), (0,-1), (0,1)]:
        nx, ny = x + dx, y + dy
        if 0 <= nx < cols and 0 <= ny < rows and grid[ny][nx] == 1:
            neighbors.append(((nx, ny), 1))

    # Diagonal moves - Cost 2.1 (should always be avoided unless necessary)
    for dx, dy in [(-1,-1), (-1,1), (1,-1), (1,1)]:
        nx, ny = x + dx, y + dy
        if 0 <= nx < cols and 0 <= ny < rows and grid[ny][nx] == 1:
            # Allow diagonal move unless both adjacent straight cells are blocked
            if not (grid[y][nx] == 0 and grid[ny][x] == 0):
                neighbors.append(((nx, ny), 2.1))

    return neighbors

def astar(grid, start, goal):
    """ A* Pathfinding Algorithm """
    open_set = []
    heapq.heappush(open_set, (0, start))
    came_from = {}
    g_score = {start: 0}
    f_score = {start: heuristic(start, goal)}

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

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

        for neighbor, move_cost in get_neighbors(grid, current):
            tentative_g_score = g_score[current] + move_cost
            if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g_score
                f_score[neighbor] = g_score[neighbor] + heuristic(neighbor, goal)
                heapq.heappush(open_set, (f_score[neighbor], neighbor))
    
    return None

def reconstruct_path(came_from, current):
    """ Reconstructs path from goal to start """
    path = [current]
    while current in came_from:
        current = came_from[current]
        path.append(current)
    path.reverse()
    return path

image showing the result and expected path

The code works fine when it comes to non-diagonal movements, but when there is only one way that requires diagonal it doesn't work (on screenshoot red square, if we start from there it doesn't find path to "G", green way is the supposed way to go).

All the time either it starts to go only diagonals - then it finds way from red square to G or it's avoiding cutting corners, but then diagonals doesn't work.

I would like to achieve it finds blue line path for example... (# on screen is a non-walkable sqm, . is a walkable sqm).

I'm trying to make path finder, but I can't fix it to work both at the same time:

• not cutting corners

• going diagonal when it's the only way to go or it's necessary

My current code is:

def heuristic(a, b):
    """ Manhattan distance heuristic to encourage straight-line movement first """
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def get_neighbors(grid, node):
    x, y = node
    neighbors = []
    rows = len(grid)
    cols = len(grid[0]) if rows > 0 else 0

    # Straight moves (up, down, left, right) - Cost 1
    for dx, dy in [(-1,0), (1,0), (0,-1), (0,1)]:
        nx, ny = x + dx, y + dy
        if 0 <= nx < cols and 0 <= ny < rows and grid[ny][nx] == 1:
            neighbors.append(((nx, ny), 1))

    # Diagonal moves - Cost 2.1 (should always be avoided unless necessary)
    for dx, dy in [(-1,-1), (-1,1), (1,-1), (1,1)]:
        nx, ny = x + dx, y + dy
        if 0 <= nx < cols and 0 <= ny < rows and grid[ny][nx] == 1:
            # Allow diagonal move unless both adjacent straight cells are blocked
            if not (grid[y][nx] == 0 and grid[ny][x] == 0):
                neighbors.append(((nx, ny), 2.1))

    return neighbors

def astar(grid, start, goal):
    """ A* Pathfinding Algorithm """
    open_set = []
    heapq.heappush(open_set, (0, start))
    came_from = {}
    g_score = {start: 0}
    f_score = {start: heuristic(start, goal)}

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

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

        for neighbor, move_cost in get_neighbors(grid, current):
            tentative_g_score = g_score[current] + move_cost
            if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g_score
                f_score[neighbor] = g_score[neighbor] + heuristic(neighbor, goal)
                heapq.heappush(open_set, (f_score[neighbor], neighbor))
    
    return None

def reconstruct_path(came_from, current):
    """ Reconstructs path from goal to start """
    path = [current]
    while current in came_from:
        current = came_from[current]
        path.append(current)
    path.reverse()
    return path

image showing the result and expected path

The code works fine when it comes to non-diagonal movements, but when there is only one way that requires diagonal it doesn't work (on screenshoot red square, if we start from there it doesn't find path to "G", green way is the supposed way to go).

All the time either it starts to go only diagonals - then it finds way from red square to G or it's avoiding cutting corners, but then diagonals doesn't work.

I would like to achieve it finds blue line path for example... (# on screen is a non-walkable sqm, . is a walkable sqm).

• python
• path-finding
• a-star

Share Improve this question Follow edited Mar 9 at 21:03 mkrieger1 23.5k77 gold badges6464 silver badges8282 bronze badges asked Mar 9 at 20:54 HowcioHowcio 3111 silver badge22 bronze badges 4

• I get what you mean, but which English word is "sqm" to represent? – mkrieger1 Commented Mar 9 at 21:06
• Just a minor observation: you are pushing to the min-heap open_set priorities based on f_score, but for the start location you push 0 as its f_score and not heuristic(start, goal) as I think it should be (at least according to Wikipedia pseudocode). I am not entirely certain right now if this can play a role here, but just saying. That is, you should do heapq.heappush(open_set, (heuristic(start, goal), start)) I think (instead of pushing 0). – gthanop Commented Mar 9 at 22:06
• 1 I'll note that, apart from your problem, a cost of 2.1 will prevent the algorithm from going diagonal (cost 2.1) instead of horizontal + vertical (cost 2), but it will still take shortcuts. For example, in a 3x3 map, with the center and lower right filled, starting in the centre bottom and the goal centre right: going around a single block could be left, up, up, right, right, down (cost 6) or diagonal up&right (cost 2.1). So it won't prefer the long way round and it won't just pick diagonal when "necessary". – Grismar Commented Mar 9 at 22:54
• As for the image, the green (intended) way to go contains a diagonal where both adjacent straight cells are blocked, which is explicitly disallowed from your code in get_neighbors. In particular because of the condition if not (grid[y][nx] == 0 and grid[ny][x] == 0): the top right cell of the diagonal is not considered a neighbor (it is not returned in the list neighbors thus never evaluated). Thus, any cell inside the red rectangle is unreachable from the goal (and vice versa). – gthanop Commented Mar 9 at 23:37

Add a comment  | 

1 Answer 1

Sorted by: Reset to default 1

First, let's add some code that you should really have included in the question, allowing people to reproduce the issue (also, see Why should I not upload images of code/data/errors?):

map_data = [
    [1, 7, 1, 2, 4, 1, 4],
    [10, 1, 4, 5],
    [0, 11, 9],
    [0, 9, 1, 1, 6, 2, 1],
    [0, 10, 9, 1],
    [0, 7, 1, 3, 8, 1],
    [0, 9, 1, 3, 5, 2],
    [4, 2, 6, 2, 5, 1],
    [1, 2, 1, 2, 1, 4, 1, 2, 4, 2],
    [1, 10, 1, 2, 6],
    [1, 10, 2, 1, 5, 1],
    [1, 10, 2, 1, 4, 2],
    [1, 10, 2, 1, 4, 1, 1],
    [1, 10, 1, 1, 6, 1],
    [1, 10, 1, 2, 5, 1],
    [5, 1, 4, 1, 1, 2, 4, 2],
    [2, 1, 1, 2, 1, 1, 2, 1, 1, 2, 4, 2],
    [2, 6, 2, 1, 1, 2, 5, 1]
]


def build_map(data: list[list[int]], start: tuple[int, int], goal: tuple[int, int]) -> list[list[int]]:
    from itertools import cycle
    pattern = cycle([0, 1])

    # pad data with zeroes at the end of rows, to get all even length rows
    data = [row+[0] if len(row) % 2 else row for row in data]

    width = sum(data[0])
    assert all(sum(data[i]) == width for i in range(len(data)))
    assert 0 <= start[0] < width and 0 <= goal[0] < width
    assert 0 <= start[1] < len(data) and 0 <= goal[1] < len(data)

    return [[x for count in counts for x in [next(pattern)] * count] for counts in data]


def print_map(map: list[list[int]], start: tuple[int, int] = None, goal: tuple[int, int] = None,
              path: list[tuple[int, int]] = None):
    if path is None:
        path = []
    for y, row in enumerate(map):
        for x, cell in enumerate(row):
            if (x, y) == start:
                print('S', end='')
            elif (x, y) == goal:
                print('G', end='')
            elif (x, y) in path:
                print('P', end='')
            else:
                print('.' if cell == 1 else '#', end='')
            print('  ', end='')
        print()


def run(data, start, goal):
    map = build_map(data, start, goal)

    path = astar(map, start, goal)
    print(path)

    print_map(map, start, goal, [] if path is None else path)


if __name__ == '__main__':
    run(map_data, (10, 8), (12, 8)

This reproduces the problem and shows the outcome you were getting:

[(10, 8), (9, 8), (8, 8), (7, 8), (7, 9), (6, 9), (5, 9), (5, 8), (5, 7), (5, 6), (6, 6), (7, 6), (8, 6), (8, 5), (9, 5), (10, 5), (10, 6), (11, 6), (12, 6), (12, 7), (12, 8)]
#  .  .  .  .  .  .  .  #  .  .  #  #  #  #  .  #  #  #  #  
#  #  #  #  #  #  #  #  #  #  .  #  #  #  #  .  .  .  .  .  
.  .  .  .  .  .  .  .  .  .  .  #  #  #  #  #  #  #  #  #  
.  .  .  .  .  .  .  .  .  #  .  #  #  #  #  #  #  .  .  #  
.  .  .  .  .  .  .  .  .  .  #  #  #  #  #  #  #  #  #  .  
.  .  .  .  .  .  .  #  P  P  P  #  #  #  #  #  #  #  #  .  
.  .  .  .  .  P  P  P  P  #  P  P  P  #  #  #  #  #  .  .  
#  #  #  #  .  P  #  #  #  #  #  #  P  .  #  #  #  #  #  .  
#  .  .  #  .  P  #  P  P  P  S  #  G  .  #  #  #  #  .  .  
#  .  .  .  .  P  P  P  .  .  .  #  .  .  #  #  #  #  #  #  
#  .  .  .  .  .  .  .  .  .  .  #  #  .  #  #  #  #  #  .  
#  .  .  .  .  .  .  .  .  .  .  #  #  .  #  #  #  #  .  .  
#  .  .  .  .  .  .  .  .  .  .  #  #  .  #  #  #  #  .  #  
#  .  .  .  .  .  .  .  .  .  .  #  .  #  #  #  #  #  #  .  
#  .  .  .  .  .  .  .  .  .  .  #  .  .  #  #  #  #  #  .  
#  #  #  #  #  .  #  #  #  #  .  #  .  .  #  #  #  #  .  .  
#  #  .  #  .  .  #  .  #  #  .  #  .  .  #  #  #  #  .  .  
#  #  .  .  .  .  .  .  #  #  .  #  .  .  #  #  #  #  #  .  

It also reproduces the issue when the goal is (12, 14) instead:

None
#  .  .  .  .  .  .  .  #  .  .  #  #  #  #  .  #  #  #  #  
#  #  #  #  #  #  #  #  #  #  .  #  #  #  #  .  .  .  .  .  
.  .  .  .  .  .  .  .  .  .  .  #  #  #  #  #  #  #  #  #  
.  .  .  .  .  .  .  .  .  #  .  #  #  #  #  #  #  .  .  #  
.  .  .  .  .  .  .  .  .  .  #  #  #  #  #  #  #  #  #  .  
.  .  .  .  .  .  .  #  .  .  .  #  #  #  #  #  #  #  #  .  
.  .  .  .  .  .  .  .  .  #  .  .  .  #  #  #  #  #  .  .  
#  #  #  #  .  .  #  #  #  #  #  #  .  .  #  #  #  #  #  .  
#  .  .  #  .  .  #  .  .  .  S  #  .  .  #  #  #  #  .  .  
#  .  .  .  .  .  .  .  .  .  .  #  .  .  #  #  #  #  #  #  
#  .  .  .  .  .  .  .  .  .  .  #  #  .  #  #  #  #  #  .  
#  .  .  .  .  .  .  .  .  .  .  #  #  .  #  #  #  #  .  .  
#  .  .  .  .  .  .  .  .  .  .  #  #  .  #  #  #  #  .  #  
#  .  .  .  .  .  .  .  .  .  .  #  .  #  #  #  #  #  #  .  
#  .  .  .  .  .  .  .  .  .  .  #  G  .  #  #  #  #  #  .  
#  #  #  #  #  .  #  #  #  #  .  #  .  .  #  #  #  #  .  .  
#  #  .  #  .  .  #  .  #  #  .  #  .  .  #  #  #  #  .  .  
#  #  .  .  .  .  .  .  #  #  .  #  .  .  #  #  #  #  #  .  

The problem is in the line user @gthanop pointed out:

if not(grid[y][nx] == 0 and grid[ny][x] == 0):
    neighbors.append(((nx, ny), 2.1))

You're allowing adding (nx, ny) to the path only if it is NOT true that both (nx, y) and (x, ny) are walls. But you want to allow adding this to the path only when both ARE walls. So, changing that line to:

if grid[y][nx] == 0 and grid[ny][x] == 0:
    neighbors.append(((nx, ny), 2.1))

And running again:

[(10, 8), (10, 9), (9, 9), (8, 9), (7, 9), (6, 9), (5, 9), (5, 8), (5, 7), (5, 6), (6, 6), (7, 6), (8, 6), (8, 5), (9, 5), (10, 5), (10, 6), (11, 6), (12, 6), (12, 7), (12, 8), (12, 9), (13, 9), (13, 10), (13, 11), (13, 12), (12, 13), (12, 14)]
#  .  .  .  .  .  .  .  #  .  .  #  #  #  #  .  #  #  #  #  
#  #  #  #  #  #  #  #  #  #  .  #  #  #  #  .  .  .  .  .  
.  .  .  .  .  .  .  .  .  .  .  #  #  #  #  #  #  #  #  #  
.  .  .  .  .  .  .  .  .  #  .  #  #  #  #  #  #  .  .  #  
.  .  .  .  .  .  .  .  .  .  #  #  #  #  #  #  #  #  #  .  
.  .  .  .  .  .  .  #  P  P  P  #  #  #  #  #  #  #  #  .  
.  .  .  .  .  P  P  P  P  #  P  P  P  #  #  #  #  #  .  .  
#  #  #  #  .  P  #  #  #  #  #  #  P  .  #  #  #  #  #  .  
#  .  .  #  .  P  #  .  .  .  S  #  P  .  #  #  #  #  .  .  
#  .  .  .  .  P  P  P  P  P  P  #  P  P  #  #  #  #  #  #  
#  .  .  .  .  .  .  .  .  .  .  #  #  P  #  #  #  #  #  .  
#  .  .  .  .  .  .  .  .  .  .  #  #  P  #  #  #  #  .  .  
#  .  .  .  .  .  .  .  .  .  .  #  #  P  #  #  #  #  .  #  
#  .  .  .  .  .  .  .  .  .  .  #  P  #  #  #  #  #  #  .  
#  .  .  .  .  .  .  .  .  .  .  #  G  .  #  #  #  #  #  .  
#  #  #  #  #  .  #  #  #  #  .  #  .  .  #  #  #  #  .  .  
#  #  .  #  .  .  #  .  #  #  .  #  .  .  #  #  #  #  .  .  
#  #  .  .  .  .  .  .  #  #  .  #  .  .  #  #  #  #  #  .  

However, your algorithm still has an issue. Consider this:

if __name__ == '__main__':
    run([[0, 3], [0, 1, 1, 1,], [0, 2, 1]], (1, 2), (2, 0))
    run([[0, 1, 1, 1], [0, 1, 1, 1,], [0, 2, 1]], (1, 2), (2, 0))

Output:

[(1, 2), (2, 0)]
.  .  G  
.  #  P  
.  S  #  
[(1, 2), (2, 0)]
.  #  G  
.  #  P  
.  S  #  

Clearly, in the first case the result should have been:

[(1, 2), (0, 2), (0, 1), (0, 0), (1, 0), (2, 0)]
P  P  G  
P  #  .  
P  S  #  

Which it will be if you disallow diagonals. This shows that your current algorithm does not correctly weigh diagonal moves.

Since you only ever want the diagonal to be taken if it is absolutely required, you can increase its weight to be the full size of the map - that's an overestimate, but certain to be sufficient to be avoided unless needed:

if grid[y][nx] == 0 and grid[ny][x] == 0:
    neighbors.append(((nx, ny), len(grid) * len(grid[0])))

Now the output for my example becomes correctly:

[(1, 2), (0, 2), (0, 1), (0, 0), (1, 0), (2, 0)]
P  P  G  
P  #  .  
P  S  #  
[(1, 2), (2, 1), (2, 0)]
.  #  G  
.  #  P  
.  S  # 

Of course you should avoid computing the weight on every iteration, but you can figure out how to clean up the code from here, I'm sure. It's also possible to compute a tighter lower bound on a sufficient weight for diagonal moves, but it's not trivial and likely not needed.

本文标签： pythonA* algorithm problem when it comes to go through one way diagonalStack Overflow