路徑規劃算法學習筆記（一）：A*算法

1、Dijkstra算法

　　Dijkstra算法從物體所在的初始點開始，訪問圖中的結點。它迭代檢查待檢查結點集中的結點，並把和該結點最靠近的還沒有檢查的結點加入待檢查結點集。該結點集從初始結點向外擴展，直到到達目標結點。Dijkstra算法保證能找到一條從初始點到目標點的最短路徑，只要全部的邊都有一個非負的代價值。node

1.1 算法原理與效果圖

　　Dijkstra算法採用貪心算法的思想，解決的問題能夠描述爲：在無向圖 G=(V,E) 中，假設每條邊 E[i] 的長度爲 w[i]，找到由頂點vs到其他各點的最短路徑。git

　　經過Dijkstra計算圖G中的最短路徑時，須要指定起點vs(即從頂點vs開始計算)。此外，引進兩個集合S和U。S的做用是記錄已求出最短路徑的頂點(以及相應的最短路徑長度)，而U則是記錄還未求出最短路徑的頂點(以及該頂點到起點vs的距離)。初始時，S中只有起點vs；U中是除vs以外的頂點，而且U中頂點的路徑是"起點vs到該頂點的路徑"。而後，從U中找出路徑最短的頂點，並將其加入到S中；接着，更新U中的頂點和頂點對應的路徑。而後，再從U中找出路徑最短的頂點，並將其加入到S中；接着，更新U中的頂點和頂點對應的路徑。重複該操做，直到遍歷完全部頂點。github

1.2 源碼分析

　　源碼來自：https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/Dijkstra/dijkstra.py算法

　　算法實現的主代碼分析：數組

def dijkstra_planning(sx, sy, gx, gy, ox, oy, reso, rr): """
 sx: start x position [m]
 sy: start y position [m]　　　　　　　　　　#起始點座標 gx: goal x position [m] gy: goal y position [m] 　　　　　　　　　　#目標點座標 ox: x position list of Obstacles [m] oy: y position list of Obstacles [m]　　　#障礙點座標 reso: grid resolution [m]　　　　　　　　　 #柵格地圖分辨率 rr: robot radius[m] #機器人的半徑 """ nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1)　　# 同除地圖分辨率，座標四捨五入後歸一化 ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1) ox = [iox / reso for iox in ox] oy = [ioy / reso for ioy in oy] obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr)　　# 障礙物地圖範圍計算、保存 motion = get_motion_model()　　# 節點移動向量共8組對應8個移動方向（x,y,cost） openset, closedset = dict(), dict()　　# 建立openset和closedset，分別存放未找到和已找到的路徑點 openset[calc_index(nstart, xw, minx, miny)] = nstart while 1: c_id = min(openset, key=lambda o: openset[o].cost)　　# 找到openlist中離出發點最近的點 current = openset[c_id]　 

        # 畫圖，畫出當前路徑點
        if show_animation: plt.plot(current.x * reso, current.y * reso, "xc") if len(closedset.keys()) % 10 == 0: plt.pause(0.001) 　　　　　

　　　　　# 若是到達目標點，結束循環 if current.x == ngoal.x and current.y == ngoal.y: print("Find goal") ngoal.pind = current.pind ngoal.cost = current.cost break

        # 將符合要求的路徑點移出openlist
        del openset[c_id] # 將符合要求的路徑點添加進closedlist
        closedset[c_id] = current # 基於移動向量，擴大搜索範圍
        for i, _ in enumerate(motion): node = Node(current.x + motion[i][0], current.y + motion[i][1], current.cost + motion[i][2], c_id) n_id = calc_index(node, xw, minx, miny) 　　　　　　　　

　　　　　　　# 若是新的路徑點不符合要求，結束本次循環 if not verify_node(node, obmap, minx, miny, maxx, maxy): continue

　　　　　　　# 若是新的路徑點已經在closedlist中，結束本次循環
            if n_id in closedset: continue
            # 若是新的路徑點在openlist中，
            if n_id in openset: if openset[n_id].cost > node.cost: openset[n_id].cost = node.cost openset[n_id].pind = c_id　　#pind相似於指針的做用，指向當前節點的上一節點，方便最後進行路徑回溯 else: openset[n_id] = node　　#向openlist中添加新的路徑點 rx, ry = calc_final_path(ngoal, closedset, reso)　　#rx,ry爲數組，存放着知足條件的路徑節點 return rx, ry

2、A*算法

　　BFS算法按照與Dijkstra算法相似的流程運行，不一樣的是它可以評估任意節點到達目標點的代價。與Dijkstra算法選擇離初始節點最近的節點不一樣，它選擇離目標最近的節點。BFS算法不能保證找到一條最短路徑，但速度比Dijkstra速度快很.A*算法就是結合了BFS算法和Dijkstra算法，具備了啓發式方法的特性，且能保證找到一條最短路徑。函數

2.1 算法原理與效果圖

啓發式搜素：啓發式搜索就是在狀態空間中的搜索對每個搜索的位置進行評估，獲得最好的位置，再從這個位置進行搜索直到目標。這樣能夠省略大量無畏的搜索路徑，提到了效率。在啓發式搜索中，對位置的估價是十分重要的。採用了不一樣的估價能夠有不一樣的效果源碼分析
估值函數（cost）：從當前節點移動到目標節點的預估費用；這個估計就是啓發式的。在尋路問題和迷宮問題中，一般用曼哈頓（manhattan）估價函數預估費用
start：路徑規劃的起始點
goal：路徑規劃的終點
g_score：從當前點（current_node）到出發點（start）的移動代價
h_score：不考慮障礙物，從當前點（current_node）到目標點的估計移動代價
f_score：f_score=g_score+h_score
openlist：尋路過程當中的待檢索節點列表
closelist：不須要再次檢索的節點列表
comaFrom：存儲父子節點關係的列表，用於回溯最優路徑，非必須，也能夠經過節點指針實現

2.2 源碼分析

　　源碼來自：https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/AStar/a_star.pyspa

　　主要代碼內容與Dijkstra類似，增長了基於歐幾里得距離的calc_heuristic()函數：指針

def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr): """ gx: goal x position [m] gy: goal y position [m] ox: x position list of Obstacles [m] oy: y position list of Obstacles [m] reso: grid resolution [m] rr: robot radius[m] """ nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1) ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1) ox = [iox / reso for iox in ox] oy = [ioy / reso for ioy in oy] obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr) motion = get_motion_model() openset, closedset = dict(), dict() openset[calc_index(nstart, xw, minx, miny)] = nstart while 1:  c_id = min(openset, key=lambda o: openset[o].cost + calc_heuristic(ngoal, openset[o])) #對應於F=G+H current = openset[c_id] # show graph
        if show_animation:  # pragma: no cover
            plt.plot(current.x * reso, current.y * reso, "xc") if len(closedset.keys()) % 10 == 0: plt.pause(0.001) if current.x == ngoal.x and current.y == ngoal.y: print("Find goal") ngoal.pind = current.pind ngoal.cost = current.cost break

        # Remove the item from the open set
        del openset[c_id] # Add it to the closed set
        closedset[c_id] = current # expand search grid based on motion model
        for i, _ in enumerate(motion): node = Node(current.x + motion[i][0], current.y + motion[i][1], current.cost + motion[i][2], c_id) n_id = calc_index(node, xw, minx, miny) if n_id in closedset: continue

            if not verify_node(node, obmap, minx, miny, maxx, maxy): continue

            if n_id not in openset: openset[n_id] = node  # Discover a new node
            else: if openset[n_id].cost >= node.cost: # This path is the best until now. record it!
                    openset[n_id] = node rx, ry = calc_final_path(ngoal, closedset, reso) return rx, ry

　　增長的calc_heuristic()函數：code

def calc_heuristic(n1, n2): w = 1.0  # weight of heuristic（H函數的權重）
    d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2)　　#歐幾里得距離計算 return d

2.3 網格地圖中的啓發式算法

　　啓發式函數h(n)告訴A*從任何結點n到目標結點的最小代價評估值。所以選擇一個好的啓發式函數很重要。

一種極端狀況，若是h(n)是0，則只有g(n)起做用，此時A* 算法演變成Dijkstra算法，就能保證找到最短路徑。

若是h(n)老是比從n移動到目標的代價小（或相等），那麼A* 保證能找到一條最短路徑。h(n)越小，A* 須要擴展的點越多，運行速度越慢。

若是h(n)正好等於從n移動到目標的代價，那麼A* 將只遵循最佳路徑而不會擴展到其餘任何結點，可以運行地很快。儘管這不可能在全部狀況下發生，但你仍能夠在某些特殊狀況下讓h(n)正好等於實際代價值。只要所給的信息完善，A* 將運行得很完美。

若是h(n)比從n移動到目標的代價高，則A* 不能保證找到一條最短路徑，但它能夠運行得更快。

另外一種極端狀況，若是h(n)比g(n)大不少，則只有h(n)起做用，同時A* 算法演變成貪婪最佳優先搜索算法（Greedy Best-First-Search）。

曼哈頓距離：
標準的啓發式函數是曼哈頓距離（Manhattan distance）。考慮你的代價函數並找到從一個位置移動到鄰近位置的最小代價D。所以，地圖中的啓發式函數應該是曼哈頓距離的D倍，經常使用於在地圖上只能先後左右移動的狀況：

h(n) = D * (abs ( n.x – goal.x ) + abs ( n.y – goal.y ) )

對角線距離：
若是在你的地圖中你容許對角運動那麼你須要一個不一樣的啓發函數。（4 east, 4 north）的曼哈頓距離將變成8*D。然而，你能夠簡單地移動（4 northeast）代替，因此啓發函數應該是4*D。這個函數使用對角線，假設直線和對角線的代價都是D：

h(n) = D * max(abs(n.x - goal.x), abs(n.y - goal.y))

歐幾里得距離：
若是你的單位能夠沿着任意角度移動（而不是網格方向），那麼你也許應該使用直線距離：

h(n) = D * sqrt((n.x-goal.x)^2 + (n.y-goal.y)^2)

然而，若是是這樣的話，直接使用A*時將會遇到麻煩，由於代價函數g不會match啓發函數h。由於歐幾里得距離比曼哈頓距離和對角線距離都短，你仍能夠獲得最短路徑，不過A*將運行得更久一些：

平方後的歐幾里得距離：
我曾經看到一些A*的網頁，其中提到讓你經過使用距離的平方而避免歐幾里得距離中昂貴的平方根運算：

h(n) = D * ((n.x-goal.x)^2 + (n.y-goal.y)^2)

不要這樣作！這明顯地致使衡量單位的問題。當A*計算f(n) = g(n) + h(n)，距離的平方將比g的代價大不少，而且你會由於啓發式函數評估值太高而中止。對於更長的距離，這樣作會靠近g(n)的極端狀況而再也不計算任何東西，A*退化成BFS：