智能汽车如何寻找路径

  通过一个二维数组表示障碍物与通畅，简陋的模拟对智能车寻找路径的算法。
  第一次做虚拟化，感觉很有挑战，也挺好玩。

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(