Udacity 無人駕駛課程第三階段第一部分 search——最短路徑搜尋
阿新 • • 發佈:2018-12-13
第一部分:廣度優先搜尋
如圖所示,從座標(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代表沒有,從中可以看出,很多小方格沒有擴散到,這種方式相比廣度優先搜尋,就提高了效率