Udacity 无人驾驶课程第三阶段第一部分 search——最短路径搜索

第一部分：广度优先搜索

如图所示，从坐标（0,0）找到一条最短路径到达终点G（4,5）代码如下：

grid中的1代表障碍方格，open代表前进到这个方格时前进的步数，也就是g值

初始时，设置一个closed array，与grid一致，open=[0,0,0],closed[0][0]=1,扩散一步后，此时open有两个元素[1,1,0],[1,0,1]

closed=[1][0]=1 closed[0][1]=1，第二步扩散时，从open list取出的元素是[1,0,1],因为向左走的这一步到达[0,0]，而在closed中已经是1了，所以open list不会添加这个坐标，此时open list两个元素[1,1,0],[2,1,1]

pop()取出open list最小的g-value

# ----------
# User Instructions:
# 
# Define a function, search() that returns a list
# in the form of [optimal path length, row, col]. For
# the grid shown below, your function should output
# [11, 4, 5].
#
# If there is no valid path from the start point
# to the goal, your function should return the string
# 'fail'
# ----------

# Grid format:
#   0 = Navigable space
#   1 = Occupied space

grid = [[0, 0, 1, 0, 0, 0],
        [0, 0, 1, 0, 0, 0],
        [0, 0, 0, 0, 1, 0],
        [0, 0, 1, 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):
    # ----------------------------------------
    # insert code here
    # ----------------------------------------
    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
    resign=False
    
    print("initial open list:")
    for i in range(len(open)):
        print(" ",open[i])
    print("----")
    
    while found is False and resign is False:
        
        #check if we still have elements on the open list
        if len(open)==0:
            resign=True
            print("fail")
            
            
        else:
            #remove node from list
            open.sort()
            open.reverse()
            next=open.pop()
            print ("take list item")
            print(next)
            x=next[1]
            y=next[2]
            g=next[0]
            
            #check if we are done
            if x==goal[0] and y==goal[1]:
                found=True
                print(next)
            else:
                #expand winning element and add to new open list
                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])
                            print("append list item")
                            print([g2,x2,y2])
                            closed[x2][y2]=1
search(grid,init,goal,cost)
#[11, 4, 5]

第二部分 A*算法

g值还是前进的步数，h（x,y)代表曼哈顿距离，即grid中某个点到终点的距离等于|X1-X2|+|Y1-Y2|

简单点说，此时以f值的大小作为判断标准，列如：当移动到坐标（4,2）时，此时g为6，f为9，如果向上移动一步，坐标变为[3,2],g值为7，f值为11，而如果向右移动一步，坐标变[4,3],g值为7，f值为9，因此再下一轮弹出最小的f值时，会从坐标[4,3]开始扩散

代码如下：

# AAAAAA-----------
# User Instructions:
#
# Modify the the search function so that it becomes
# an A* search algorithm as defined in the previous
# lectures.
#
# Your function should return the expanded grid
# which shows, for each element, the count when
# it was expanded or -1 if the element was never expanded.
# 
# If there is no path from init to goal,
# the function should return the string 'fail'
# ----------

grid = [[0, 1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0, 0],
        [0, 0, 0, 0, 1, 0]]
heuristic = [[9, 8, 7, 6, 5, 4],
             [8, 7, 6, 5, 4, 3],
             [7, 6, 5, 4, 3, 2],
             [6, 5, 4, 3, 2, 1],
             [5, 4, 3, 2, 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,heuristic):
    # ----------------------------------------
    # modify the code below
    # ----------------------------------------
    closed = [[0 for col in range(len(grid[0]))] for row in range(len(grid))]
    closed[init[0]][init[1]] = 1

    expand = [[-1 for col in range(len(grid[0]))] for row in range(len(grid))]
    action = [[-1 for col in range(len(grid[0]))] for row in range(len(grid))]

    x = init[0]
    y = init[1]
    g = 0
    h=heuristic[x][y]
    f=g+h

    open = [[f,g,h,x,y]]

    found = False  # flag that is set when search is complete
    resign = False # flag set if we can't find expand
    count = 0
    
    while not found and not resign:
        if len(open) == 0:
            resign = True
            return "Fail"
        else:
            open.sort()
            open.reverse()
            next = open.pop()
            x = next[3]
            y = next[4]
            g = next[1]
            #count的意思是记录这个方格是第几个进入list
            expand[x][y] = count
            count += 1
            
            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
                            h2=heuristic[x2][y2]
                            f2=g2+h2
                            open.append([f2,g2,h2,x2,y2])
                            closed[x2][y2] = 1

    return expand
search(grid,init,goal,cost,heuristic)
'''[[0, -1, -1, -1, -1, -1],
 [1, -1, -1, -1, -1, -1],
 [2, -1, -1, -1, -1, -1],
 [3, -1, 8, 9, 10, 11],
 [4, 5, 6, 7, -1, 12]]'''

expand array中的数字代表是第几个进入open list，-1代表没有，从中可以看出，很多小方格没有扩散到，这种方式相比广度优先搜索，就提高了效率