（做封面圖片，可真是費了很長時間）

前言

路徑規劃涉及到很多內容。

但路徑規劃的起點，應該都是A*這個來自斯坦福大學演算法。

說起A*，網上有很多相關的內容。各種博客，各種解釋。

但是我愣是沒看明白。 可能我比較笨把。

弄明白了之後，我發現網上的很多詞語有點太高大上了。

（這裡聲明，並不是說高大上的詞不好，相反就應該用高大上的專業辭彙才能精準的表達。只不過，有些內容比如A*裡面的heuristic用專業詞語，就有點不明所以）

今天我就想用封面圖片 + 我的大白話，解釋一下A*演算法工作的過程。

A* 演算法理念

首先，其實 A* 演算法的基本理念超級簡單。

典型的例子就是：現在我在 X 網吧，決賽圈了，但快上課了，所以得找個去學校最快的小路，然後抄小路去，省時間, 避免在路上碰上教導主任。

這裡核心問題為4個。（前提：不能翻學校的圍牆）

  1. 通往學校的路到底有多少條？
  2. 我能不能飛起來？我能不能瞬間移動？我能不能跳躍失控？我能不能突然覺醒獲得超能力？
  3. 那麼從 X 網吧出發的時候，哪條路可以最快到達學校？
  4. 得繞開教導主任

對於踩點的老手，應該一瞬間計算好了各種路徑。

Ａ*演算法說：我也是這麼想的。

Ａ演算法是一個搜索出最優路徑的演算法。只要有通向終點的路徑，那麼A*給出的路徑是最優的。

但是A*演算法是基於一個個獨立的小格子。一般機器人中叫格柵。正因為是一個個小格子，導致了A*是一個離散的演算法。離散歸離散，這是和控制相關的東西，我們暫且不討論。這裡就先討論，A*怎麼得到的最短的路徑。

其實很簡單。

A*會推演，如果自己在不撞牆，不走回頭路的情況下，走出下一步，那麼距離終點的位置是不是變得更近了？這樣，推演的最終輸出就是，可以最快到達終點的路徑。

下面的圖是啥呢？是我整理的Ａ*演算法演示圖。一張圖（我認為）就可以較好的說明A*。

第一，不管是人還是機器人，都要心裡有數。自己能不能飛起來，心裡沒數嗎？

第二，沒什麼人（機器）想撞到牆上，所以不能去的領域，想都不用想。圖片裡面的表達就是，被佔用的格柵。

第三，好漢不吃回頭草，走過的路，就不用被考慮進路徑。（好漢吃回頭草？？？）圖片裡面的表達就是，路徑。

第四，根據自己可以移動的方式來判斷，哪些路徑是可以被考慮的，如果移動的話，得走幾步？

第五，某些路段總是能碰上麻煩事，能繞開就繞開，盡量不考慮這些路段。每一步移動，有沒有讓自己更接近終點？圖片裡面的表達就是，損失函數。

第六，可能還有其他顧慮和約束，自行定義。比如某個路段，晚上太黑，有怪叔叔之類的。

第七，考慮所有層的（貫穿所有層的那條豎線）的情況下, 得出最終優化的路徑。其實就是找到損失函數為最小的的那條路徑。也就是

Path_{optimal} = argmin(occupied grid, free grid, action, cost 1, cost 2, constraint cost)

一張圖解決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


推薦閱讀：
相关文章