移动机器人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 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;
}

---

该算法还有多种改进分支，在此基础上进一步研究。

---