元宵節過去啦, 新的學期開始了.
前一段時間學習了Coursera上的公開課: Computational Motion Planning, 這是VJKumar教授Robotics系列課程的其中一個. 這一部分, 顧名思義, 就是講運動規劃的. 這套課程並不是很深刻, 但是它的各演算法的銜接非常流暢, 每個演算法的引出非常自然. 對於我自己, 通過看論文, 每個演算法雖然都見過, 但是自己總結的不夠系統, 上過這個公開課就好很多.
感覺這套課程質量很高, 課堂討論區很活躍, 來自全世界的各路大神都分享自己做作業的感受. 很多東西自己想是絕對想不到的. 作業做完, 感受更加深刻, 對於演算法的理解以及演算法細節部分實現也有了新的認識, 下面還是把自己的一點小小的感悟寫在下面吧. 這一篇文章講的都是graph-based planning，個人理解是把所有可以走的路線看成節點的路徑規劃。

Introduction to Computational Motion Planning

介紹，路徑規劃是什麼。下面有一些英文術語: G/V/E: 一個圖G(Graph)，由頂點V(Vertices)和連線E（Edge）構成。連線把所有頂點連線起來。連線通常用數值標記，表示相關的量例如距離或者消耗。通常來講，目標都是從起始位置到終點位置路程最短。

Grassfire Algorithm

這個演算法，可以從目標開始，繞著相鄰的格子標數字。虛擬碼如圖:

從任意一個格子出發，只需要尋找格子周邊數字最小的格子，走過去就行。像下面這種一圈都是9的，那任意一個格子都可以了。

如果在標記格子的過程中，起始位置還沒有被標記到數字，標記的過程就結束了。那就說明無法從起始位置到達目標。

Dijkstra演算法（此處指適用於2D的演算法)

虛擬碼如圖，每個節點都有兩個屬性，一個是父節點，一個是距離。尋找每個節點的相鄰節點中有標記距離最小的那個點，這個點就是下一個節點。反覆迭代。

演算法效率：這裡提到了a clever data structure叫做priority queue, 可以使維數增加的時候, 計算效率不會迅速較低. 這個priority queue, 優先順序佇列, 感覺應該是資料結構裡面的內容?這個有待向學計算機的同學考證.

A*演算法:(此處指適用於2D的演算法)

上面講的都是效率比較低的，因為是全向搜尋。A*演算法效率比較高，因為使用啟發式演算法會慢慢往目標方向上靠近，而不是全方向都在搜尋。

這是虛擬碼。g是current節點和起點的距離， f是節點g值以及這個節點的啟發式成本。上一頁說，啟發式成本函式有如下特點：在目標處啟發式成本函式值為0，對於相鄰的兩個節點x和y，應該滿足h(x)<h(y)+d(x,y).常見的啟發式成本函式h包括從節點到目標的距離(直線距離)，從節點到目標橫座標差和縱座標差的和。每次迴圈都要尋找f最小的，當作下一次迴圈的起點值（就是設定成current，尋找它相鄰的節點，再迴圈）

和dijkstra演算法相比，快了很多呢。

附錄:
來自維基百科的A*虛擬碼：

function A*(start, goal)
    // The set of nodes already evaluated.
    closedSet := {}
    // The set of currently discovered nodes still to be evaluated.
    // Initially, only the start node is known.
    openSet := {start}
    // For each node, which node it can most efficiently be reached from.
    // If a node can be reached from many nodes, cameFrom will eventually contain the
    // most efficient previous step.
    cameFrom := the empty map
// For each node, the cost of getting from the start node to that node.
    gScore := map with default value of Infinity
    // The cost of going from start to start is zero.
    gScore[start] := 0 
    // For each node, the total cost of getting from the start node to the goal
    // by passing by that node. That value is partly known, partly heuristic.
    fScore := map with default value of Infinity
    // For the first node, that value is completely heuristic.
    fScore[start] := heuristic_cost_estimate(start, goal)
while openSet is not empty
        current := the node in openSet having the lowest fScore[] value
        if current = goal
            return reconstruct_path(cameFrom, current)
        openSet.Remove(current)
        closedSet.Add(current)
        for each neighbor of current
            if neighbor in closedSet
                continue        // Ignore the neighbor which is already evaluated.
            // The distance from start to a neighbor
            tentative_gScore := gScore[current] + dist_between(current, neighbor)
            if neighbor not in openSet  // Discover a new node
                openSet.Add(neighbor)
            else if tentative_gScore >= gScore[neighbor]
                continue        // This is not a better path.
// This path is the best until now. Record it!
            cameFrom[neighbor] := current
            gScore[neighbor] := tentative_gScore
            fScore[neighbor] := gScore[neighbor] + heuristic_cost_estimate(neighbor, goal)
return failure
function reconstruct_path(cameFrom, current)
    total_path := [current]
    while current in cameFrom.Keys:
        current := cameFrom[current]
        total_path.append(current)
    return total_path