Нахождение оптимального пути

Question

Я пытаюсь найти оптимальный путь для автомобиля из заданного
initial_state (из списка формы [строка, столбец, ориентация]) до заданной цели (вида список [строка, столбец]) .

Автомобиль может свободно двигаться вперед в разных направлениях (вверх, влево, вниз, вправо).

Может выполнять 3 действия:
Повернуть направо и двигаться вперед (2), Продвинуться вперед (1), Повернуть влево и двигаться вперед (20)
Примечание: цифры в скобках - это затраты, связанные с выполнением соответствующих действий

Поворот происходит в той же ячейке.
2D-список (называемый grid ), который состоит из 0 и 1, задается как вход (2D нециклический мир).
0 --- навигационная ячейка
1 --- неходовая ячейка

Задача состоит в том, чтобы найти путь (с наименьшей стоимостью) из более чем одного из возможных путей.

Вот мой код.

forward = [[-1,0],[0,-1],[1,0],[0,1]]
forward_name = ['up','left','down','right']
action = [-1,0,1]  ## turnRight, noTurn, turnLeft
action_name = ['R','#','L']
grid = [[1,1,1,0,0,0],
        [1,1,1,0,1,0],
        [0,0,0,0,0,0],
        [1,1,1,0,1,1],
        [1,1,1,0,1,1]]
init = [4,3,0]  ## for init[2]-- 0-up, 1-left, 2-down, 3-right 
goal = [2,0]
cost = [2,1,20]  ## turnRight, noTurn, turnLeft

def optimum_policy2D(grid, init, goal, cost):
    value = [[999*i for i in row] for row in grid]
    #print(value)
    r,c,orientation = init
    Routes = []   ##list of routes
    Routes.append([0,[init[:2]],orientation]) 
                   ## [cost,list of cell in the route,orientation]

    while [r,c]!=goal:
        presentRoute = Routes[0]
        for i in range(len(action)):
            dr, dc = forward[(presentRoute[2]+action[i])%4]
            if r+dr in range(len(grid)) and c+dc in range(len(grid[0])) and 
            value[r+dr][c+dc]!=999:
                thisRoute = presentRoute[:]
                thisRoute[1].append([r+dr,c+dc])
                thisRoute[0] += cost[i]
                thisRoute[2] = (thisRoute[2]+action[i])%4
                Routes.append(thisRoute[:])
        Routes = Routes[1:]
        Routes.sort()
        r,c = Routes[0][1][-1]
    print(Routes[0])

optimum_policy2D(grid, init, goal, cost)   

Я создаю список-значение 2D с произвольно высоким значением 999 (выше, чем максимальная вовлеченная стоимость) в ячейках, которые не предназначены для навигации.

Далее я создаю пустой список (называемый Routes), в котором будут храниться разные маршруты. Каждый маршрут будет иметь вид - [стоимость, список ячеек в маршруте, конечная ориентация автомобиля] . Перед входом в маршрут while-Loop инициализируется начальное состояние (так как оно будет частью любого маршрута).

Проверки for-Loop для действительного соседа (Validity of Neighbor - должен быть в сетке, должен быть навигационным).

Для действительного nieghbour-
создайте копию presentRoute в thisRoute.
добавьте новые координаты ячейки в список ячеек в маршруте.
обновление стоимости в соответствии с выполненным действием.
обновление окончательной ориентации в соответствии с действием.
* Наконец, добавьте этот новый маршрут к Маршрутам.
(с каждым действительным соседом новый маршрут добавляется к Маршрутам)

Маршруты [0] теперь удалены. При сортировке маршрута первым маршрутом в списке будет маршрут с минимальной стоимостью. Переменные 'r' и 'c' обновляются последней ячейкой маршрута минимальной стоимости.

Как только [r, c] равен цели, мы нашли маршрут с минимальной стоимостью.

Вывод отличается от моего убеждения. Я ищу ошибки в этом коде.