转自:https://blog.csdn.net/ayawaya/article/details/70155932
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<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;
}