機器人路徑規劃其一 Dijkstra Algorithm【附動態圖原始碼】

我叫平沢唯發表於2021-06-24
機器人Go原始碼

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

常用的路徑規劃演算法有Dijkstra, A*，D*, RRT, PRM以及在這些演算法上演變的各種演算法，這兩天把Dijkstra Algorithm學習了下並用QT程式碼復現，所以本節先從Dijkstra講起。Dijkstra是個人名，scientist Edsger W. Dijkstra 是個電腦科學家，這個演算法的來源非常接地氣，當時一天早上這位老哥和他的未婚妻在阿姆斯特丹逛街，然後在一個咖啡廳休息，休息的時候他在思考從城市鹿特丹到城市格羅寧根的最短路徑問題，據他所述，這個演算法被他在20分鐘內想了出來，但是卻在三年後才發表出來。

 

 

Ⅰ. 演算法步驟

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

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

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

3. 在每一次迭代中，對於當前的節點current node，計算所有與該節點能直接相連（相鄰）的節點到該節點的距離hCost，如果 newCost=[hCost+(current node).cost] 比某一個相鄰的節點的原先cost要小，則將該相鄰的節點的cost更新為newCost，否則不做改變。將所有更新完cost的相鄰節點納入到{已接觸節點集合}。

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

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

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

為了更好的理解上述步驟，可以參考以上的配圖，上圖是我從維基百科上擷取的，感覺比我自己做的動畫(後文貼出)要好，上圖中，紅色漸變點為visited nodes，即屬於{已訪問節點集合}，藍色點為{已接觸節點集合}，這些藍色點原本為白色背景(未訪問節點集合)，由於current node的每一次迭代更新，更新了相應的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，這中結構類似於單向連結串列，按照這個步驟回溯到起始點，最短路徑也就找到了。

那麼該如何驗證演算法的正確性呢？

設想，在每一次迭代中，current node都是已接觸節點中離起始節點最近的那個點，如果說我們找到的最短路徑不是真正的最短路徑的話，即最終節點d的父節點不是我們找到的最短路徑的倒數第二個節點v，而是另外一個節點w，那麼d的cost應該被更新為w.cost+dist[w2d]，d的父節點應該被相應更新為w，而不是v，與實際相矛盾，依次類推，可證。

Ⅱ. 程式實現

為了便於視覺化實時的迭代過程，利用Qt的Qpainter在QWidger空間上通過定時器每隔一段時間暫停重啟迭代過程，隨機生成一個二維的圖，二維矩陣中的每個元素的值對應於不同的節點，如0代表未訪問，2代表已接觸，6代表已訪問等等，程式中因為設計到cost進行排序，需要用到priority queue加快速度，Dijkstra核心程式碼如下：

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

 1     start_ = start_in;
 2     goal_ = goal_in;
 3     n = grid.size();
 4 
 5     // Get possible motions--> right, down and left, up
 6 //    std::vector<Node> motion = GetMotion();
 7     // set the start_ node as the current vertex
 8     open_list_.push(start_);
 9     //qDebug()<<open_list_.top().cost_<<" "<<open_list_.top().h_cost_;
10 
11     // Main loop
12 
13     SetTimer(NULL, 0, 50, (TIMERPROC)TimerProc);
14 
15     while (!open_list_.empty())
16     {
17         // get the minist-cost node as a new one in each iteration
18         Node current = open_list_.top();
19         open_list_.pop();
20         // assign the order as the id
21         current.id_ = current.x_ * n + current.y_;
22 
23         // find if current is the goal node
24         if (CompareCoordinates(current, goal_))
25         {
26             // if yes, than the work is done and exit
27             closed_list_.push_back(current);
28             grid[current.x_][current.y_] = 2;
29             KillTimer(NULL, 0);
30             return closed_list_;
31         }
32         grid[current.x_][current.y_] = 2;  // Point opened
33 
34         // find all the neighbours and update their distance to the original node
35         for (const auto& m : motion)
36         {
37             Node new_point;
38             //qDebug()<<new_point.cost_<<" "<<new_point.h_cost_;
39             //qDebug()<<m.cost_<<" "<<m.h_cost_;
40             new_point = current + m;
41             //qDebug()<<new_point.cost_<<" "<<new_point.h_cost_;
42 
43             new_point.id_ = n * new_point.x_ + new_point.y_;
44             new_point.pid_ = current.id_;
45 
46             if (CompareCoordinates(new_point, goal_))
47             {
48                 open_list_.push(new_point);
49                 break;
50             }
51             // Check boundaries
52             if (new_point.x_ < 0 || new_point.y_ < 0 || new_point.x_ >= n || new_point.y_ >= n)
53             {
54                 continue;
55             }
56             // obstacle or visited
57             if (grid[new_point.x_][new_point.y_] != 0)
58             {
59                 continue;
60             }
61             // push all the mearsured node into a priority queue which the minist-cost one will be in the top
62             open_list_.push(new_point);
63         }
64         closed_list_.push_back(current);
65         // update the grid to get a dynamic-view
66 
67     }
68 
69     KillTimer(NULL, 0);
70 
71     // no solution founded
72     closed_list_.clear();
73     Node no_path_node(-1, -1, -1, -1, -1, -1);
74     closed_list_.push_back(no_path_node);

Ⅲ. 迭代過程動畫

因為設定的運動方向只有上下左右，所以最終的路徑是不能走斜線的，如果設定成斜線，最短路徑可以視覺上更短，下圖中，灰色方塊表示visited nodes，藍色代表已接觸，紅色代表障礙物，最終的規劃路徑用綠色方塊表示：

Reference:

[1] https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm#Applications\

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

相關文章