移动机器人D*Lite路径规划算法设计、仿真及源码

转自：https://blog.csdn.net/ayawaya/article/details/70155932

• Dstar Lite路径规划算法简介
• Linux系统简要说明
• Dstar 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
rhs(s)={0ifs=sstartmins′∈Pred(s)(g(s′)+c(s′,s))otherwise

D*Lite算法继承了rhs()的概念，但D*Lite算法是从目标节点向起始节点搜索。
为了让节点v的启发函数值随着起点位置变化而变化， Koenig S和Likhachev M给出了两种方法：一是，根据新的起点位置，将优先队列中所有节点的启发函数值重新计算；二是，并不重新计算队列中的启发函数值，而是在计算新添加到优先队列中的节点的启发函数值时，加上一个修饰符 ,表示机器人移动距离的叠加。

D* Lite Pseudo Code:

CaculateKey(s)

return [min(g(s),rhs(s))+h(sstart , s)+km; min(g(s),rhs(s))];

Initialize()

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

UpdateVertex(μ)(μ)

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

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

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

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

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

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

else

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

Main()

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

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

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

Update the edge cost c(u,v)c(u,v); 
Update Vertex(u)(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
1

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

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

下载后解压，进入解压后的目录：

$ cd dstar
$ ls
1
2

然后使用make编译

$ make
1

完毕，运行程序:

$ ./dstar
1

仿真程序操作命令：

[q/Q] - 退出
[r/R] - 再次规划路径
[a/A] - 切换自动规划
[c/C] - 清屏（重启）
鼠标左键 - 设置障碍物
鼠标中间 - 移动目标点
鼠标右键 - 移动起始点

程序提供的Dstar类可以单独调用，使用vim编辑器编写程序：

$ vim DstarDraw.cpp
1
输入以下内容：

#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;
}