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

一、Dijkstra算法

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

 

1.1 算法原理與效果圖

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

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

 

 

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

 

二、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.py

　　主要代碼內容與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()函數：

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：