第一部分:广度优先搜索
如图所示,从坐标(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代表没有,从中可以看出,很多小方格没有扩散到,这种方式相比广度优先搜索,就提高了效率