移動機器人D*Lite路徑規劃算法設計、仿真及源碼

Dstar Lite路徑規劃算法簡介

D*Lite算法是Koenig S和Likhachev M基於LPA*算法基礎上提出的路徑規劃算法。 LPA*算法本是基於Dijkstra最短路徑算法而產生的定起點、定目標點的路徑規劃算法。 通過對LPA*算法的改造，使LPA*算法的思想能應用到諸如車輛動態導航這樣的問題。

LPA*算法區別於其他算法 的一個重要特點是rhs()的定義：

rhs(s)={0mins′∈Pred(s)(g(s′)+c(s′,s))ifs=sstartotherwise

D*Lite算法繼承了rhs()的概念，但D*Lite算法是從目標節點向起始節點搜索。

爲了讓節點v的啓發函數值隨着起點位置變化而變化， Koenig S和Likhachev M給出了兩種方法：一是，根據新的起點位置，將優先隊列中所有節點的啓發函數值重新計算；二是，並不重新計算隊列中的啓發函數值，而是在計算新添加到優先隊列中的節點的啓發函數值時，加上一個修飾符 ,表示機器人移動距離的疊加。

---

D* Lite Pseudo Code:

CaculateKey(s)

return [min(g(s),rhs(s))+h(s_start , s)+k_m; min(g(s),rhs(s))];

Initialize()

U: =0;
k_m =0;
for all s ∈ S, rhs(s) = g(s) = ∞ ;
rhs(s_goal) = 0;
U.Insert(s_goal), CaculateKey(s_goal));

UpdateVertex(μ)

if (μ≠sgoal) , rhs(μ) = mins′∈Succ(μ)(c(μ,s′)+g(s′)) ;
if (μ∈U) , U.Remove(μ)
if (g(μ)≠rhs(μ)) , U.Insert(μ,CaculateKey(μ)) ;

ComputeShortestPath()
while (U.TopKey() < CaculateKey(Sstart ) or rhs(sstart)≠g(sstart) )

kold = U.TopKey();
μ = U.Pop();
if (kold < CaculateKey(μ ))

U.Insert(μ , CaculateKey(μ ));

elseif (g(μ)>rhs(μ) )

g(μ)=rhs(μ)
for all s∈Pred(μ) , UpdateVertex(s);

else

g(μ)=∞ ;
for all s∈Pred(μ)∪μ , UpdateVertex(s);

Main()

Slast=Sstart ;
Initialize();
ComputeShortestPath();
while(Sstart≠Sgoal )

/* if (g(Sstart=∞) ) then there is no known path */
Sstart=argmins′∈Succ(μ)(c(μ,s′)+g(s′)) ;
Move to Sstart ;
Scan graph for changed edge costs;
if any edge costs changed

km=km+h(slast,sstart) ;
Slast=Sstart ;
for all directed edges (u,v) with changed edge costs

Update the edge cost c(u,v) ;
Update Vertex(u) ;

Compute ShortestPath();

---

更詳細的算法說明，請查閱有關文獻資料。

Linux系統簡要說明

Linux是一套免費使用和自由傳播的類Unix操作系統，是一個基於POSIX和UNIX的多用戶、多任務、支持多線程和多CPU的操作系統。它能運行主要的UNIX工具軟件、應用程序和網絡協議。它支持32位和64位硬件。Linux繼承了Unix以網絡爲核心的設計思想，是一個性能穩定的多用戶網絡操作系統。
在做算法程序開發之前，應對Linux系統基本操作有一定的瞭解，才能方便上手，在這裏向同學們推薦一款教程：

鳥哥的 Linux 私房菜

該教程內容詳實全面，是Linux入門的好材料。
這裏用到一些Linux下的基本操作，客戶端可以選擇PUTTY，至少掌握：

ls
cd
tar
man
gcc
make
vim
nano

命令不能一一列舉。

Dstar Lite程序使用說明

該程序調用一些GNU庫，請在類Unix系統下編譯使用。
如果系統沒有安裝編譯工具，則需要先安裝 (Ubuntu)：

$ sudo apt-get install build-essential

下載源程序：
Dstar.rar
dstar.tar.gz
（若不能下載刷新一下頁面）

CSDN下載：
Dstar.rar
dstar.tar.gz

---

下載後解壓，進入解壓後的目錄：

$ cd dstar
$ ls

然後使用make編譯

$ make

完畢，運行程序:

$ ./dstar

---

仿真程序操作命令：

• [q/Q] - 退出
• [r/R] - 再次規劃路徑
• [a/A] - 切換自動規劃
• [c/C] - 清屏（重啓）
• 鼠標左鍵 - 設置障礙物
• 鼠標中間 - 移動目標點
• 鼠標右鍵 - 移動起始點

---

程序提供的Dstar類可以單獨調用，使用vim編輯器編寫程序：

$ vim DstarDraw.cpp

輸入以下內容：

#include "Dstar.h"
int main() {
    Dstar *dstar = new Dstar();
    list<state> mypath;
    dstar->init(0,0,10,5);         // set start to (0,0) and goal to (10,5)
    dstar->updateCell(3,4,-1);     // set cell (3,4) to be non traversable
    dstar->updateCell(2,2,42.432); // set set (2,2) to have cost 42.432
    dstar->replan();               // plan a path
    mypath = dstar->getPath();     // retrieve path
    dstar->updateStart(10,2);      // move start to (10,2)
    dstar->replan();               // plan a path
    mypath = dstar->getPath();     // retrieve path
    dstar->updateGoal(0,1);        // move goal to (0,1)
    dstar->replan();               // plan a path
    mypath = dstar->getPath();     // retrieve path
    return 0;
}

---

該算法還有多種改進分支，在此基礎上進一步研究。

---