首先要說明的是，機器人路徑規劃與軌跡規劃屬於兩個不同的概念，一般而言，軌跡規劃針對的物件為機器人末端座標系或者某個關節的位置速度加速度在時域的規劃，常用的方法為多項式樣條插值，梯形軌跡等等，而路徑規劃針對的是機器人的一個整體如移動機器人或者無人機在已知或者未知的環境中規劃其運動的路線，在slam機器人應用較多。然而兩者的界限有時也有交叉，如機械臂末端工具運動到操作物件時需要避障以及規劃時間時，也可以看作是路徑規劃問題。

常用的路徑規劃演算法有Dijkstra, A*，D*, RRT, PRM以及在這些演算法上演變的各種演算法，這兩天把Dijkstra Algorithm和A-star學習了下並用QT程式碼復現，

Ⅰ. 演算法步驟

說明：把最短路徑問題想象成一張圖上某個起始點initial node到終點destination node的路徑最短求解，圖上除了起始點和終點還存在一些其它的可到達點以及無法達到點(障礙物)，每兩個點之間都有一條權重不同的路徑Edge，計每個點到起始點的路徑長度為該點的距離的cost

1.標記所有節點node為未訪問狀態unvisited（未訪問節點集合），程式裡一般給這些節點的cost設定為一個足夠大的數。並建立一個{未訪問節點集合}和{已接觸節點集合}，以及{已訪問節點集合}。

2.設定起始點為迭代的第一個點，命名為current node，該點的cost顯然為應該設定為0，將其納入{已接觸節點集合}。

3.在每一次迭代中，對於當前的節點current node，計算所有與該節點能直接相連（相鄰）的節點到該節點的距離costUV，如果 newCost=[costUV+(current node).cost] 比某一個相鄰的節點的原先cost要小，則將該相鄰的節點的cost更新為newCost，同時更新總的COST為COST=newCost+hCost，這裡的hCost可以設計為該相鄰的點到目標點的直線距離或者曼哈頓距離(如畫素點情況下)，否則不做改變。將所有更新完cost的相鄰節點納入到{已接觸節點集合}。

4.當遍歷完current node所有相鄰的節點後，將current node節點從{已接觸節點集合}中移除，並納入到{已訪問節點集合}。處於{已訪問節點集合}的節點將不會在進入迭代計算過程。

5.進入下一步迭代，新的current node從{已接觸節點集合}選出，選擇COST(而不是cost)最小的那一個。

6.如果目標點destination node被納入{已訪問節點集合}（代表最短路徑已經找到）或者{已接觸節點集合}中的所有點的cost都無窮大（代表找不到路徑，有障礙），則迭代終止。

為了更好的理解上述步驟，可以參考以上的配圖，上圖是我從維基百科上擷取的，感覺比我自己做的動畫(後文貼出)要好，上圖中，紅色漸變點為visited nodes，即屬於{已訪問節點集合}，藍色點為{已接觸節點集合}，這些藍色點原本為白色背景(未訪問節點集合)，由於current node的每一次迭代更新，更新了相應的cost和COST後被歸類為已接觸點，之後在下一次迭代中作為新的current node的預備種子集合，如果其中某個點COST最小，將會被選中稱為current node，在迭代中最終會變成visited node，而只有visited node才能有機會稱為最短路徑中的一點，可謂是過五關斬六將。當迭代終止後，我們就需要從{已訪問節點集合}中選中一組構成最短路徑的節點集合，這裡需要提前將節點node的資料結構定義好，顯然的是，如果迭代找到了最終節點並停止了下來，那麼最終節點會被儲存進{已訪問節點集合}，我們從最終節點開始回溯，最終節點的上一個節點應該是哪一個？這就需要在節點node的資料機構中提前定義好一個parent id的資料成員變數，在每個迭代步驟中的第3步，如果相鄰的節點滿足更新cost的條件，我們就將該相鄰的節點成員中的parent id賦值為current node的id，沒錯，node資料結構除了parent id，還應該有一個唯一標識自身的id，這中結構類似於單向連結串列，按照這個步驟回溯到起始點，最短路徑也就找到了。

Ⅱ. 程式實現

為了便於視覺化實時的迭代過程，利用Qt的Qpainter在QWidger空間上通過定時器每隔一段時間暫停重啟迭代過程，隨機生成一個二維的圖，二維矩陣中的每個元素的值對應於不同的節點，如0代表未訪問，2代表已接觸，6代表已訪問等等，程式中因為設計到[cos+hCost]進行排序，需要用到priority queue加快速度，A-star與Dijkstra演算法大致相同，唯一的不同在於選取current node的標準不同，A-star演算法選取的代價函式為cost+hCost的和，程式碼如下：

github原始碼：https://github.com/ShieldQiQi/Path-Planning-Showed-By-Animation

 1     const std::vector<Node> motion = GetMotion();
 2 
 3     // Main loop
 4     if(!new_a_star.doneFlag)
 5     {
 6         Node current = new_a_star.open_list_.top();
 7         new_a_star.open_list_.pop();
 8         current.id_ = current.x_ * new_a_star.n + current.y_;
 9 
10         // find if current is the goal node
11         if (CompareCoordinates(current, new_a_star.goal_))
12         {
13             new_a_star.closed_list_.push_back(current);
14             grid[current.x_][current.y_] = 5;
15             //return new_a_star.closed_list_;
16             new_a_star.doneFlag = true;
17             KillTimer(NULL, 1);
18         }
19         grid[current.x_][current.y_] = 5;
20         w->update();
21 
22         // find all the neighbours and update their f(n)=g(n)+h(n)
23         for (const auto& m : motion)
24         {
25             Node new_point;
26             new_point = current + m;
27 
28             // update id and parent id
29             new_point.id_ = new_a_star.n * new_point.x_ + new_point.y_;
30             new_point.pid_ = current.id_;
31 
32             // the difference between Dijkstar and A-star Algorithm
33             // f(n)=g(n)+h(n) where the g(n)=cost while h(n)=Manhattan distance of current node and goal node
34             new_point.h_cost_ = std::abs(new_point.x_ - new_a_star.goal_.x_) + std::abs(new_point.y_ - new_a_star.goal_.y_);
35 
36             if (CompareCoordinates(new_point, new_a_star.goal_)){
37                 new_a_star.open_list_.push(new_point);
38                 break;
39             }
40 
41             // Check boundaries
42             if (checkOutsideBoundary(new_point, new_a_star.n)) {
43                 continue;}
44             // obstacle or visited
45             if (grid[new_point.x_][new_point.y_] == 1 || grid[new_point.x_][new_point.y_] == 6) {
46               continue;}
47 
48             if(costGrid[new_point.x_][new_point.y_]>new_point.cost_)
49             {
50                 // check the new_point if is already in open_list_
51                 // if new_point in open_list then
52                 new_a_star.open_list_.push(new_point);
53                 grid[new_point.x_][new_point.y_] = 2;
54                 costGrid[new_point.x_][new_point.y_] = new_point.cost_;
55                 // otherwise, update the cost of the previus one
56                 // Todo, not a serious problem, but would be better if is done
57             }
58         }
59 
60         new_a_star.closed_list_.push_back(current);
61         grid[current.x_][current.y_] = 6;
62     }else{
63         new_a_star.closed_list_.clear();
64         Node no_path_node(-1, -1, -1, -1, -1, -1);
65         new_a_star.closed_list_.push_back(no_path_node);
66         new_a_star.doneFlag = true;
67         KillTimer(NULL, 1);
68     }

Ⅲ. 迭代過程動畫

因為設定的運動方向只有上下左右，所以最終的路徑是不能走斜線的，如果設定成斜線，最短路徑可以視覺上更短，下圖中，灰色方塊表示visited nodes，藍色代表已接觸，紅色代表障礙物，最終的規劃路徑用綠色方塊表示，下圖中第一個圖為A-star演算法，第二個圖為前一節所講的Dijkstra演算法，可以看到，有A-star有針對性的篩選使得它比Dijkstra演算法擁有更快的速度。

Reference:

[1]https://en.wikipedia.org/wiki/A*_search_algorithm

[2]https://github.com/vss2sn/path_planning