通过一个二维数组表示障碍物与通畅,简陋的模拟对智能车寻找路径的算法。
第一次做虚拟化,感觉很有挑战,也挺好玩。
1.广度优先搜索法( search_breadth_first_1D 函数),由起点找到终点。trick:一个点的下一结点有若干个,而上一结点只有一个。
2.这里没有写A*算法的程序。A*程序是有一个启发式heuristics的“地图”,上面标注了当前点到终点的距离,通过比较“已走路程”+“剩余路程”的最小值,确定下一步前进的方向。因为算法简单,而且heuristics变量需要先验的测量或者训练,所以没有写出来。
3.动态规划( compute_value 函数)。思路是先利用广度优先搜索,计算出每个点距离终点的距离(类似于搜索出来一个heuristics变量),然后计算出来一条最佳路径。其实我做的跟广度优先搜索没区别。没有使用动态规划,因为太慢了。
4.汽车有了头尾与朝向,汽车可以前进一格,左转进一格、右转进一格,每个方向的cost不同,找出最佳路径( optimum_policy2D 函数)。可以认为是2D编程,跟坦克大战差不多。这时要用4维的变量来表示每一点各个方向距离终点的距离。我开始想了特别糟糕的算法,最后发现居然无法表示各个方向,是个错误的算法。这个用的是动态规划,很慢,一点一点的改变。
5.一维动态规划+方向随机概率( stochastic_value函数)。汽车朝某个方向前进时,可能落在其左边或者右边,所以可能误入障碍物,或者超出边界,这时需要惩罚cost。汽车朝一个方向总的cost = sum(此方向概率 * 惩罚cost或者距终点的值) + step_cost
第一次做虚拟化,感觉很有挑战,也挺好玩。
1.广度优先搜索法( search_breadth_first_1D 函数),由起点找到终点。trick:一个点的下一结点有若干个,而上一结点只有一个。
2.这里没有写A*算法的程序。A*程序是有一个启发式heuristics的“地图”,上面标注了当前点到终点的距离,通过比较“已走路程”+“剩余路程”的最小值,确定下一步前进的方向。因为算法简单,而且heuristics变量需要先验的测量或者训练,所以没有写出来。
3.动态规划( compute_value 函数)。思路是先利用广度优先搜索,计算出每个点距离终点的距离(类似于搜索出来一个heuristics变量),然后计算出来一条最佳路径。其实我做的跟广度优先搜索没区别。没有使用动态规划,因为太慢了。
4.汽车有了头尾与朝向,汽车可以前进一格,左转进一格、右转进一格,每个方向的cost不同,找出最佳路径( optimum_policy2D 函数)。可以认为是2D编程,跟坦克大战差不多。这时要用4维的变量来表示每一点各个方向距离终点的距离。我开始想了特别糟糕的算法,最后发现居然无法表示各个方向,是个错误的算法。这个用的是动态规划,很慢,一点一点的改变。
5.一维动态规划+方向随机概率( stochastic_value函数)。汽车朝某个方向前进时,可能落在其左边或者右边,所以可能误入障碍物,或者超出边界,这时需要惩罚cost。汽车朝一个方向总的cost = sum(此方向概率 * 惩罚cost或者距终点的值) + step_cost
6.最后给出了解决动态规划慢的一个思路:n叉树
Python3源码:
# 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 ] # Make sure that the goal definition stays in the function.
delta = [[ - 1 , 0 ], # go up
[ 0 , - 1 ], # go left
[ 1 , 0 ], # go down
[ 0 , 1 ]] # go right
delta_name = [ '^' , '<' , 'v' , '>' ]
cost = 1
def print_expand( expand ):
for i in range( len( expand )):
print( expand [ i ])
def print_path( action ):
path = [[ ' ' for col in range( len( grid [ 0 ]))] for row in range( len( grid ))]
x , y = goal [ 0 ], goal [ 1 ]
path [ goal [ 0 ]][ goal [ 1 ]] = '*'
while x != init [ 0 ] or y != init [ 1 ]:
# 下面x,y 都要变化,所以这里提取temp,delta_name取下标就不会错
temp = action [ x ][ y ]
opposite = ( temp - 2) % len( delta)
x += delta [ opposite ][ 0 ]
y += delta [ opposite ][ 1 ]
path [ x ][ y ] = delta_name [ temp ]
for i in range( len( path )):
print( path [ i ])
# breadth-first search shorest path, and print the track.
def search_breadth_first_1D ():
xboundary = len( grid) - 1
yboundary = len( grid [ 0 ]) - 1
search_list = [[ 0 , init [ 0 ], init [ 1 ]]]
result = []
action = [[ - 1 for col in range( yboundary + 1 )] for row in range( xboundary + 1 )]
closed = [[ 0 for col in range( yboundary + 1 )] for row in range( xboundary + 1 )]
closed [ init [ 0 ]][ init [ 1 ]] = 1
expand = [[ - 1 for col in range( yboundary + 1 )] for row in range( xboundary + 1 )]
expand [ init [ 0 ]][ init [ 1 ]] = 0
expand_count = 1
while search_list :
#search_list.sort() breadth-first 已经是按照g-value(距离Start点的距离) 排序好的。
current = search_list . pop( 0)
for i in range( len( delta )):
x1 = current [ 1 ] + delta [ i ][ 0 ]
y1 = current [ 2 ] + delta [ i ][ 1 ]
if x1 < 0 or x1 > xboundary or y1 < 0 or y1 > yboundary or grid [ x1 ][ y1 ] or closed [ x1 ][ y1 ]:
continue
if x1 == goal [ 0 ] and y1 == goal [ 1 ]:
result . append ([ current [ 0 ] + cost , x1 , y1 ])
# 终点赋值一次,expand和 action 就都不再赋值,这样能得到最短路径
if expand [ goal [ 0 ]][ goal [ 1 ]] == - 1 : # 一般(expand[goal[0]][goal[1]] > expand_count) 条件不成立,因为短路径先到达终点,且expand_count 是递增赋值的
expand [ x1 ][ y1 ] = expand_count
expand_count += 1
action [ x1 ][ y1 ] = i # for back propagation
else :
search_list . append ([ current [ 0 ] + cost , x1 , y1 ])
closed [ x1 ][ y1 ] = 1
expand [ x1 ][ y1 ] = expand_count
expand_count += 1
action [ x1 ][ y1 ] = i # for back propagation
print_expand(
# 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 ] # Make sure that the goal definition stays in the function.
delta = [[ - 1 , 0 ], # go up
[ 0 , - 1 ], # go left
[ 1 , 0 ], # go down
[ 0 , 1 ]] # go right
delta_name = [ '^' , '<' , 'v' , '>' ]
cost = 1
def print_expand( expand ):
for i in range( len( expand )):
print( expand [ i ])
def print_path( action ):
path = [[ ' ' for col in range( len( grid [ 0 ]))] for row in range( len( grid ))]
x , y = goal [ 0 ], goal [ 1 ]
path [ goal [ 0 ]][ goal [ 1 ]] = '*'
while x != init [ 0 ] or y != init [ 1 ]:
# 下面x,y 都要变化,所以这里提取temp,delta_name取下标就不会错
temp = action [ x ][ y ]
opposite = ( temp - 2) % len( delta)
x += delta [ opposite ][ 0 ]
y += delta [ opposite ][ 1 ]
path [ x ][ y ] = delta_name [ temp ]
for i in range( len( path )):
print( path [ i ])
# breadth-first search shorest path, and print the track.
def search_breadth_first_1D ():
xboundary = len( grid) - 1
yboundary = len( grid [ 0 ]) - 1
search_list = [[ 0 , init [ 0 ], init [ 1 ]]]
result = []
action = [[ - 1 for col in range( yboundary + 1 )] for row in range( xboundary + 1 )]
closed = [[ 0 for col in range( yboundary + 1 )] for row in range( xboundary + 1 )]
closed [ init [ 0 ]][ init [ 1 ]] = 1
expand = [[ - 1 for col in range( yboundary + 1 )] for row in range( xboundary + 1 )]
expand [ init [ 0 ]][ init [ 1 ]] = 0
expand_count = 1
while search_list :
#search_list.sort() breadth-first 已经是按照g-value(距离Start点的距离) 排序好的。
current = search_list . pop( 0)
for i in range( len( delta )):
x1 = current [ 1 ] + delta [ i ][ 0 ]
y1 = current [ 2 ] + delta [ i ][ 1 ]
if x1 < 0 or x1 > xboundary or y1 < 0 or y1 > yboundary or grid [ x1 ][ y1 ] or closed [ x1 ][ y1 ]:
continue
if x1 == goal [ 0 ] and y1 == goal [ 1 ]:
result . append ([ current [ 0 ] + cost , x1 , y1 ])
# 终点赋值一次,expand和 action 就都不再赋值,这样能得到最短路径
if expand [ goal [ 0 ]][ goal [ 1 ]] == - 1 : # 一般(expand[goal[0]][goal[1]] > expand_count) 条件不成立,因为短路径先到达终点,且expand_count 是递增赋值的
expand [ x1 ][ y1 ] = expand_count
expand_count += 1
action [ x1 ][ y1 ] = i # for back propagation
else :
search_list . append ([ current [ 0 ] + cost , x1 , y1 ])
closed [ x1 ][ y1 ] = 1
expand [ x1 ][ y1 ] = expand_count
expand_count += 1
action [ x1 ][ y1 ] = i # for back propagation
print_expand(