第四课：路径规划

1. 从迪杰斯特拉开始说起

老生常谈，具体实现直接上对应代码：

"""
迪杰斯特拉算法用于路径规划
"""
"""
迪杰斯特拉算法用于路径规划
其实这个算法的思路很明确，就是一个优先队列
从某点开始，
1. 弹出头
2. 将弹出头的周围一圈符合要求的装进队列中
循环退出条件：
1. 找到终点
2. 没找到终点，但是队列长度已经为空
"""


grid = [[0, 0, 1, 0, 0, 0],
        [0, 0, 1, 0, 0, 0],
        [0, 1, 0, 0, 1, 0],
        [0, 1, 0, 1, 1, 0],
        [0, 0, 0, 0, 1, 0]]
init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1]
cost = 1

delta = [[-1, 0], # go up
         [ 0,-1], # go left
         [ 1, 0], # go down
         [ 0, 1]] # go right

delta_name = ['^', '<', 'v', '>']


def search(grid,init,goal,cost):
    flag = [[0 for j in range(len(grid[i]))]for i in range(len(grid))]  # 标识是否访问过
    expension = [[-1 for j in range(len(grid[i]))] for i in range(len(grid))]
    expension[0][0] = 0
    this_map = [[' ' for col in range(len(grid[0]))] for row in range(len(grid))]

    # 存储访问过的节点，并从中挑选cost最小的节点
    L = []
    L_map = []
    count = 0
    # 标识当前所处位置
    now = [0,0,0]
    flag[0][0] = 1

    while now[:2] != goal:
        next_cost = now[2]
        now_next = [[0, 0]+now, [0, 0]+now, [0, 0]+now, [0, 0]+now]

        for i in range(len(delta)):
            for j in range(len(delta[i])):
                now_next[i][j] = delta[i][j] + now[j]
        for item in now_next:
            if 0<=item[0]<len(grid) and 0<=item[1]<len(grid[0]):
                if flag[item[0]][item[1]] == 0 and grid[item[0]][item[1]] == 0 :
                    item[4] += 1
                    flag[item[0]][item[1]] = 1
                    if item not in L_map:
                        L_map.append(item)

                    if item[:2]+[item[4]] not in L:
                        L.append(item[:2]+[item[4]])
                        count = count + 1
                        expension[item[0]][item[1]] = count

        L.sort(key=lambda cost: cost[2])

        if len(L) == 0 and now[:2] != goal:
            return 'fail'

        # 更新now为当前
        now = L.pop(0)

    L_map.reverse()
    location_list = []
    location_list.append(L_map[0][:2])
    for i in range(len(L_map)):
        if L_map[i][:2] == location_list[-1]:
            location_list.append(L_map[i][2:4])

    location_list.reverse()
    this_move = [0, 0]
    for i in range(len(location_list) - 1):
        this_move[0] = location_list[i+1][0]-location_list[i][0]
        this_move[1] = location_list[i+1][1]-location_list[i][1]
        a = delta.index(this_move)
        this_map[location_list[i][0]][location_list[i][1]]  = delta_name[a]
    this_map[goal[0]][goal[1]] = '*'

    return this_map


"""
# 这部分是老师给的答案，他的思路还是很清晰的
def search(grid, init, goal, cost):
    # ----------------------------------------
    # modify code below
    # ----------------------------------------
    closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
    closed[init[0]][init[1]] = 1

    x = init[0]
    y = init[1]
    g = 0

    open = [[g, x, y]]

    found = False  # flag that is set when search is complete
    resign = False  # flag set if we can't find expand

    while not found and not resign:
        if len(open) == 0:
            resign = True
            return 'fail'
        else:
            open.sort()
            open.reverse()
            next = open.pop()
            x = next[1]
            y = next[2]
            g = next[0]

            if x == goal[0] and y == goal[1]:
                found = True
            else:
                for i in range(len(delta)):
                    x2 = x + delta[i][0]
                    y2 = y + delta[i][1]
                    if x2 >= 0 and x2 < len(grid) and y2 >= 0 and y2 < len(grid[0]):
                        if closed[x2][y2] == 0 and grid[x2][y2] == 0:
                            g2 = g + cost
                            open.append([g2, x2, y2])
                            closed[x2][y2] = 1

    return open  # make sure you return the shortest path
"""
a= search(grid,init,goal,cost)

for i in range(len(a)):
    print(a[i])

2. A*算法