路径规划;a*算法 demo_路径规划A*算法

（做封面图片，可真是费了很长时间）

前言

路径规划涉及到很多内容。

但路径规划的起点，应该都是A*这个来自斯坦福大学算法。

说起A*，网上有很多相关的内容。各种博客，各种解释。

但是我愣是没看明白。 可能我比较笨把。

弄明白了之后，我发现网上的很多词语有点太高大上了。

（这里声明，并不是说高大上的词不好，相反就应该用高大上的专业词汇才能精准的表达。只不过，有些内容比如A*里面的heuristic用专业词语，就有点不明所以）

今天我就想用封面图片 + 我的大白话，解释一下A*算法工作的过程。

---

A* 算法理念

首先，其实 A* 算法的基本理念超级简单。

典型的例子就是：现在我在 X 网吧，决赛圈了，但快上课了，所以得找个去学校最快的小路，然后抄小路去，省时间, 避免在路上碰上教导主任。

这里核心问题为4个。（前提：不能翻学校的围墙）

1. 通往学校的路到底有多少条？
2. 我能不能飞起来？我能不能瞬间移动？我能不能跳跃失控？我能不能突然觉醒获得超能力？
3. 那么从 X 网吧出发的时候，哪条路可以最快到达学校？
4. 得绕开教导主任

对于踩点的老手，应该一瞬间计算好了各种路径。

Ａ*算法说：我也是这么想的。

Ａ算法是一个搜索出最优路径的算法。只要有通向终点的路径，那么A*给出的路径是最优的。

但是A*算法是基于一个个独立的小格子。一般机器人中叫格栅。正因为是一个个小格子，导致了A*是一个离散的算法。离散归离散，这是和控制相关的东西，我们暂且不讨论。这里就先讨论，A*怎么得到的最短的路径。

其实很简单。

A*会推演，如果自己在不撞墙，不走回头路的情况下，走出下一步，那么距离终点的位置是不是变得更近了？这样，推演的最终输出就是，可以最快到达终点的路径。

下面的图是啥呢？是我整理的Ａ*算法演示图。一张图（我认为）就可以较好的说明A*。

第一，不管是人还是机器人，都要心里有数。自己能不能飞起来，心里没数吗？

第二，没什么人（机器）想撞到墙上，所以不能去的领域，想都不用想。图片里面的表达就是，被占用的格栅。

第三，好汉不吃回头草，走过的路，就不用被考虑进路径。（好汉吃回头草？？？）图片里面的表达就是，路径。

第四，根据自己可以移动的方式来判断，哪些路径是可以被考虑的，如果移动的话，得走几步？

第五，某些路段总是能碰上麻烦事，能绕开就绕开，尽量不考虑这些路段。每一步移动，有没有让自己更接近终点？图片里面的表达就是，损失函数。

第六，可能还有其他顾虑和约束，自行定义。比如某个路段，晚上太黑，有怪叔叔之类的。

第七，考虑所有层的（贯穿所有层的那条竖线）的情况下, 得出最终优化的路径。其实就是找到损失函数为最小的的那条路径。也就是

一张图解决A*

下面是用python写的简单的A*算法。是sebastian大神的视频里面的内容。无需任何额外库。用来尝鲜，理解非常好~

# ----------
# User Instructions:
# 
# Implement the function optimum_policy2D below.
#
# You are given a car in grid with initial state
# init. Your task is to compute and return the car's 
# optimal path to the position specified in goal; 
# the costs for each motion are as defined in cost.
#
# There are four motion directions: up, left, down, and right.
# Increasing the index in this array corresponds to making a
# a left turn, and decreasing the index corresponds to making a 
# right turn.

forward = [[-1,  0], # go up
           [ 0, -1], # go left
           [ 1,  0], # go down
           [ 0,  1]] # go right
forward_name = ['up', 'left', 'down', 'right']

# action has 3 values: right turn, no turn, left turn
action = [-1, 0, 1]
action_name = ['R', '#', 'L']

# EXAMPLE INPUTS:
# grid format:
#     0 = navigable space
#     1 = unnavigable space 
grid = [[1, 1, 1, 0, 0, 0],
        [1, 1, 1, 0, 1, 0],
        [0, 0, 0, 0, 0, 0],
        [1, 1, 1, 0, 1, 1],
        [1, 1, 1, 0, 1, 1]]

init = [4, 3, 0] # given in the form [row,col,direction]
                 # direction = 0: up
                 #             1: left
                 #             2: down
                 #             3: right
                
goal = [2, 0] # given in the form [row,col]

cost = [2, 1, 20] # cost has 3 values, corresponding to making 
                  # a right turn, no turn, and a left turn

# EXAMPLE OUTPUT:
# calling optimum_policy2D with the given parameters should return 
# [[' ', ' ', ' ', 'R', '#', 'R'],
#  [' ', ' ', ' ', '#', ' ', '#'],
#  ['*', '#', '#', '#', '#', 'R'],
#  [' ', ' ', ' ', '#', ' ', ' '],
#  [' ', ' ', ' ', '#', ' ', ' ']]
# ----------

# ----------------------------------------
# modify code below
# ----------------------------------------

def optimum_policy2D(grid,init,goal,cost):
    value = [ [[999 for row in range(len(grid[0]))] for col in range(len(grid))],
        [[999 for row in range(len(grid[0]))] for col in range(len(grid))],
        [[999 for row in range(len(grid[0]))] for col in range(len(grid))],
        [[999 for row in range(len(grid[0]))] for col in range(len(grid))] ]
        
    policy = [ [[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
    [[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
    [[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
    [[' ' for row in range(len(grid[0]))] for col in range(len(grid))] ]
    
    policy2D =  [[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
    
    change =  True
    while change:
        change =  False
        for x in range(len(grid)):
            for y in range(len(grid[0])):
                for orientation in range(4):
                    if goal[0] == x and goal[1] == y:
                        if value[orientation][x][y] > 0:
                            change = True
                            value[orientation][x][y] = 0
                            policy[orientation][x][y] = '*'
                            
                    elif grid[x][y] == 0:
                        for i in range(3):
                            # The reason why moduler 4 is for the cycular buffer for the simulated grid.
                            o2 = (orientation + action[i]) % 4
                            # this is the motion model
                            x2 = x + forward[o2][0]
                            y2 = y + forward[o2][1]
    
                            if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 < len(grid[0]) and grid[x2][y2] == 0:
                                v2 = value[o2][x2][y2] + cost[i]
                                if v2 < value[orientation][x][y]:
                                    value[orientation][x][y] = v2
                                    policy[orientation][x][y] = action_name[i]
                                    change = True
                                
 

    x = init[0]
    y = init[1]
    orientation = init[2]
    
    policy2D[x][y] = policy[orientation][x][y]
    
    while policy[orientation][x][y] != '*':
        if policy[orientation][x][y] == '#':
            o2 = orientation
        elif policy[orientation][x][y] == 'R':
            o2 =  (orientation -1 ) % 4
        elif policy[orientation][x][y] == 'L':
            o2 = (orientation + 1) % 4
        x = x + forward[o2][0]
        y = y + forward[o2][1]
        orientation = o2
        policy2D[x][y] = policy[orientation][x][y]
    
       
    return policy2D
   
policy = optimum_policy2D(grid,init,goal,cost)

for i in range(len(policy)):
    print(policy[i])

---

总结

其实路径规划的内容还有很多。包括损失函数该如何生成，各个损失函数的权衡该如何平衡，不确定性该如何考虑，马尔科夫决策，路径生成和行为决策等等等。

以后有空慢慢写，不着急

20190523