search

|—search()—|—添加一個列表變量Expend,存儲每一個小格擴展時爲第幾步，可打印出

|                    |—打印運動表

|—A*—|— heuristic()

|—Dynamic programming(動態規劃)—|—Value編程

|                                                           |—應用於現實

|                                                           |—algorithm simulation

這是一種路徑規劃的算法，查找起點到終點路徑的過程稱爲規劃，這裏講的是規劃的離散方法，用這些方法進行連續運動.

 

cost：每條路徑花費的時間

 

每個MOVE前、後移動，每一次Turn left、Turn right，都耗費必定成本單元

路徑規劃或搜索問題就是尋找最短的動做序列，將機器人從開始狀態引導到結束狀態

如今的問題是是否能找出起點到終點最短路徑的程序。

解決這個問題：給小格編號，在容器open中從起始點向周圍擴展，並標上步數g值，當擴展到終點座標時，最小的g值就是起點到終點的步數（祕訣：每次只展開最小的g值節點）

程序化這個過程：

# 編寫search() 返回列表[g, row, col]. 
# 須要最終返回結果[11, 4, 5].到達不了返回'fail'

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]]

  heuristic = [[9, 8, 7, 6, 5, 4],
               [8, 7, 6, 5, 4, 3],
               [7, 6, 5, 4, 3, 2],
               [6, 5, 4, 3, 2, 1],
               [5, 4, 3, 2, 1, 0]]

init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1]
cost = 1

delta = [[-1, 0], #  up
         [ 0,-1], #  left
         [ 1, 0], #  down
         [ 0, 1]] #  right

delta_name = ['^', '<', 'v', '>']

cost = 1

def search(grid,init,goal,cost):
      closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))] #爲了不重複展開，定義一個與網格大小相同的空網格
      closed[init[0]][init[1]] = 1#將起始位置標誌爲關閉
      expand = [[-1] * len(grid[0]) for i in grid]
    action = [[-1] * len(grid[0]) for i in grid]
      #置初值
      x = init[0]
      y = init[1]
      g = 0
      h = heuristic[x][y]
 f = g + h
      open = [[g, x, y]] open = [[f, g, h, x, y]]
      expand[x][y] = g
 count = 0 #作兩個標誌值 
      found = False #找到目標時found置true 
      resign = False #找不到目標而且沒有其餘地方可展開resign置true

      while not found and not resign:
            檢查open列表裏是否有元素
            if len(open) == 0:
                resign = True
            return "fail"
            else:
            #刪除open最小g值以繼續展開
                open.sort()  #遞增排列
                open.reverse()  #顛轉，由於pop是從頭部彈出
                next = open.pop() #彈出最小
               #給要展開的x,y賦值，g必須在最前面
                x = next[1] x = next[3] # x從0開始
                y = next[2] y = next[4] # y從1開始
                g = next[0] g = next[1] 
                expand[x][y] = count #將下面的那句改爲這樣
                count += 1 # 將下面的這句移到這
               expand[x][y] = expand_counter #檢測是否達到目標
            if x == goal[0] and y == goal[1]:
                 found = True
            else: #這是沒有達到，也是核心部分
                 for i in range(len(delta)):
                      #遍歷將每一個動做都賦值給x，y
                      x2 = x + delta[i][0]
                      y2 = y + delta[i][1]
                      if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 <len(grid[0]):  #若是賦了動做的x,y仍然在網格內
                          if closed[x2][y2] == 0 and grid[x2][y2] == 0: #而且還沒有標記(經過檢查close),還有這個單元能夠走
                              g2 = g + cost
                              h2 = heuristic[x2][y2]
                              f2 = g2 + h2
                              open.append([f2，g2, x2, y2])
                              closed[x2][y2] = 1
                              count += 1
                             action[x2][y2] = i#用[x2][y2]而不用[x][y]是由於後者表明擴展，前者表明回溯

 policy = [[' '] * len(grid[0]) for i in grid]#action包含障礙物的全部方格的方向，非想要，因此要另外初始化一個用''填充的表格
 x = goal[0] #標明終點並用*表示
 y = goal[1]
 policy[x][y] = '*'
 while x != init[0] or y != init[1]: #從終點位置返回直到到達初始點爲止
 x2 = x - delta[action[x][y]][0]#經過從當前狀態作減法來座標回溯
 y2 = y - delta[action[x][y]][1]
 policy[x2][y2] = delta_name[action[x][y]]#將用二維向量表示的方向圖像化
 x = x2
 y = y2
 for row in policy:
 print row
 return policy 

如下是打印代碼

#print 'new open list:'
#for i in range(len(open))
#    print '',open[i]
#print '---------------------'

#for i in range(len(expand)):
#    print expand[i]
#print '---------------------'

  

 

expend step table

Expand是一個與網格大小相同的表格，它保存的是節點在哪一個步驟展開的信息。從未展開過的標爲-1，當一個節點展開時它會獲得一個這個表格中惟一的步驟序號，好比下面這個例子步驟序號是0~22：

修改函數search（），以便返回一個名爲expand的值的表，裏面的元素是g，即此表將跟蹤每一個節點擴展時是在第幾步。作少許修改，如下：

1.初始化一個與網格大小相同且初值全爲-1的列表。

2.還須要一個名爲count的變量來保持擴展步驟的數量。

3.在結束時，將expand [x] [y]的值設置爲count的值，並將count遞增1。

已在原代碼中以紅色代碼添加

 

Print Path

打印出最終解決方案的路徑，爲此實現一個新的數據結構。 

 

已在源代碼中以橙色代碼添加

 

A*算法

search算法的一種變種，比擴展每一個節點更高效。這種算法使用了heuristic()走最短的路徑，得出來的值小於，或者最好等於真實距離：

h(x,y) ≤ actual goal distance from x,y；

這也意味着heuristic()不精確；heuristic()以下圖所示，它能指導search：

在這裏，仍然有open列表，但在這個列表裏的元素除了g，還有g加上heuristic()的值h(x,y)，用f調用：

f = g + h(x,y)

每次移除f最小的節點，這有一個例子過程，最終肯定了一條最短路徑：

 

 完成A*算法，-1表示障礙物和未擴展的節點。

已在源代碼中用紫色代碼添加

在上面粒子中少訪問的格子可能微不足道，但當數據很是龐大時就有巨大的不一樣，特別是出現一個很是深的死衚衕效率區別就很是明顯

 

在這個模型中障礙物會隨着車輛的移動被傳感器感知而擴展，A*樹規劃的路徑會時刻更新不斷取消以前的A*樹，每一次從新規劃的時間都在10微秒以內， 完成這個跟完成A*的區別是是否能完成它的 運動模型，去轉向和直走，最明顯的區別是可否能沿原軌跡返回。

 

動態規劃

這是路徑規劃可選擇的模型之一，給它一個map和一個或更多目的地，它能返回到任意位置的最佳路徑，最重要的是它的起始位置能夠是任意!

policy是一個函數將空白小格遍歷變成動做，給一個這樣的圖和一個目的地，就能輸出帶路標的格子

 這樣圖片就包含了到達任意位置的步數

完成這樣的編程，給一個地圖，1表明障礙，0表明空位；函數能計算到每一格的最短步數值，而且返回包含這些值的表，障礙用99表示（足夠大不會混淆步數）

def compute_value():
      value = [[99 for row in range(len(grid[0]))] for col in range(len(grid))]
      change = True #標誌change在有實際updata時置True，循環中置False
      while change:
            change = False
            for x in range(len(grid)):
                 for y in range(len(grid[0])):
                      if goal[0] == x and goal[1] == y: #若是出發點就是目標點，並考慮到意外目標值非0便置0，change置True表示updata if value[x][y] > 0: 
                               value[x][y] = 0
                               policy[x][y] = ‘*’
                               change = True
                      elif grid[x][y] == 0: #出發點不是目標點狀況， for a in range(len(delta)):
                               x2 = x + delta[a][0]
                               y2 = y + delta[a][1]
                               if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 < len(grid[0]): #若是賦了動做的x,y還在大網格內（在空格子裏並不是障礙物）
                                    v2 = value[x2][y2] + cost_step #加一個成本 if v2 < value[x][y]: #取值更小的路徑，updata
                                         change = True
                                         value[x][y] = v2
                                         policy[x][y] = delta_name[a]

#如下是policy打印代碼

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

 事實證實，經過動態規劃找到最佳解決方案比A *更容易。將上述的表格轉換成動做表，修改已用梅色添加

 

 now do something fun！將動態規劃應用於現實：3D的狀態空間，每一個小格是2D，添加一個方向維度

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]]

forward = [[ ­1, 0], # up
           [ 0, ­1], # left
           [ 1, 0], # down
           [ 0, 1]] # right    

goal = [2, 0] #從左上角算起
init = [4, 3, 0] 

cost = [2, 1, 20]  # 爲cost賦三個值：右轉，不轉，左轉
action = [­1, 0, 1] 
action_name = ['R', '#', 'L']

def optimum_policy2D():
      value = [[[999 for row in range(len(grid[0]))] for col inrange(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:
                                    value[orientation][x][y] = 0
                                    policy[orientation][x][y] = '*'
                                    change = True
                               
                               elif grid[x][y] == 0: #以空格開始
                                    for i in range(3):#計算三種方法來傳播值
                                         o2 = (orientation + action[i]) % 4 #添加方向累加，取模4確保方向在3內，作一個緩衝區
                                         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]: #留下最小值
                                                  change = True
                                                  value[orientation][x][y] = v2
                                                  policy[orientation][x][y]= action_name[i] #賦值動做標號
                          
      x = init[0]
      y = init[1]
      orientation = init[2] #這裏方向是0
      policy2D[x][y] = policy[orientation][x][y] #從3D表上copy到2D上來 while policy[orientation][x][y] != '*':#在沒到目的地的狀況下檢查方向，並將方向表明的數字賦給o2 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] #更新x、y、orientation
            y = y + forward[o2][1]
            orientation = o2
            policy2D[x][y] = policy[orientation][x][y] #剛纔那句賦的是起點，這裏是剩下的所有 return policy2D 

 

 SIMULATION

程序能經過改變 cost functions 高效地駕駛這輛車

 

class世界是離散的，2個規劃算法 - A *，它使用heuristic() 來找到one path和dynamic programming，找到一個完整的策略(policy)，即爲每一個位置制定一個規劃。不只完成這兩，還在3D世界作了動態規劃。

 

so far，需瞭解如何將其轉化爲實際的機器人動做。 瞭解連續狀態空間以及用來使機器人移動的所謂「控制」。

>>>next...