(做封面图片,可真是费了很长时间)
路径规划涉及到很多内容。
但路径规划的起点,应该都是A*这个来自斯坦福大学演算法。
说起A*,网上有很多相关的内容。各种博客,各种解释。
但是我愣是没看明白。 可能我比较笨把。
弄明白了之后,我发现网上的很多词语有点太高大上了。
(这里声明,并不是说高大上的词不好,相反就应该用高大上的专业辞汇才能精准的表达。只不过,有些内容比如A*里面的heuristic用专业词语,就有点不明所以)
今天我就想用封面图片 + 我的大白话,解释一下A*演算法工作的过程。
首先,其实 A* 演算法的基本理念超级简单。
典型的例子就是:现在我在 X 网吧,决赛圈了,但快上课了,所以得找个去学校最快的小路,然后抄小路去,省时间, 避免在路上碰上教导主任。
这里核心问题为4个。(前提:不能翻学校的围墙)
对于踩点的老手,应该一瞬间计算好了各种路径。
A*演算法说:我也是这么想的。
A演算法是一个搜索出最优路径的演算法。只要有通向终点的路径,那么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 cars # 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