原标题：一分钟了解：a*算法

原文来自：CSDN      原文链接：https://blog.csdn.net/qq_41856814/article/details/101597490

# User Instructions:
#
# Implement the function optimum_policy2D below.
#
# You are given a car in grid with initial state
# init. Your task is to compute and return the car's
# optimal path to the position specified in goal;
# the costs for each motion are as defined in cost.
#
# There are four motion directions: up, left, down, and right.
# Increasing the index in this array corresponds to making a
# a left turn, and decreasing the index corresponds to making a
# right turn.
 
forward = [[-1, 0],  # go up
           [0, -1],  # go left
           [1, 0],  # go down
           [0, 1]]  # go right
forward_name = ['up', 'left', 'down', 'right']
 
# action has 3 values: right turn, no turn, left turn
 
# 只有三个方式 左 直走 右
action = [-1, 0, 1]
action_name = ['R', '#', 'L']
 
# EXAMPLE INPUTS:
# grid format:
#     0 = navigable space
#     1 = unnavigable space  要避开1的地方
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]  # given in the form [row,col,direction]      起点和上下左右
# direction = 0: up
#             1: left
#             2: down
#             3: right
 
goal = [2, 0]  # given in the form [row,col]  目标地点
 
cost = [0, 0, 1]  # cost has 3 values, corresponding to making        左右转弯花费的值
 
 
# a right turn, no turn, and a left turn
 
# EXAMPLE OUTPUT:
# calling optimum_policy2D with the given parameters should return 
# [[' ', ' ', ' ', 'R', '#', 'R'],
#  [' ', ' ', ' ', '#', ' ', '#'],
#  ['*', '#', '#', '#', '#', 'R'],
#  [' ', ' ', ' ', '#', ' ', ' '],
#  [' ', ' ', ' ', '#', ' ', ' ']]
# ----------
 
# ----------------------------------------
# modify code below
# ----------------------------------------
 
def optimum_policy2D(grid, init, goal, cost):
    value = [[[999 for row in range(len(grid[0]))] for col in range(len(grid))],
             [[999 for row in range(len(grid[0]))] for col in range(len(grid))],
             [[999 for row in range(len(grid[0]))] for col in range(len(grid))],
             [[999 for row in range(len(grid[0]))] for col in range(len(grid))]]
 
    policy = [[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
              [[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
              [[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
              [[' ' for row in range(len(grid[0]))] for col in range(len(grid))]]
 
    policy2D = [[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
 
    change = True
    while change:
        change = False
        for x in range(len(grid)):
            for y in range(len(grid[0])):
                for orientation in range(4):
                    if goal[0] == x and goal[1] == y:
                        if value[orientation][x][y] > 0:
                            change = True
                            value[orientation][x][y] = 0
                            policy[orientation][x][y] = '*'
 
                    elif grid[x][y] == 0:
                        for i in range(3):
                            # The reason why moduler 4 is for the cycular buffer for the simulated grid.
                            o2 = (orientation + action[i]) % 4
                            # this is the motion model
                            x2 = x + forward[o2][0]
                            y2 = y + forward[o2][1]
 
                            if x2 >= 0 and x2 < len(grid) and y2 >= 0 and y2 < len(grid[0]) and grid[x2][y2] == 0:
                                v2 = value[o2][x2][y2] + cost[i]
                                if v2 < value[orientation][x][y]:
                                    value[orientation][x][y] = v2
                                    policy[orientation][x][y] = action_name[i]
                                    change = True
 
    x = init[0]
    y = init[1]
    orientation = init[2]
 
    policy2D[x][y] = policy[orientation][x][y]
 
    while policy[orientation][x][y] != '*':
        if policy[orientation][x][y] == '#':
            o2 = orientation
        elif policy[orientation][x][y] == 'R':
            o2 = (orientation - 1) % 4
        elif policy[orientation][x][y] == 'L':
            o2 = (orientation + 1) % 4
        x = x + forward[o2][0]
        y = y + forward[o2][1]
        orientation = o2
        policy2D[x][y] = policy[orientation][x][y]
 
    return policy2D
 
 
policy = optimum_policy2D(grid, init, goal, cost)
 
for i in range(len(policy)):
    print(policy[i])

免责声明：本文来自互联网新闻客户端自媒体，不代表本网的观点和立场。

合作及投稿邮箱：E-mail:editor@tusaishared.com