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

Dstar Lite路径规划算法简介

DLite算法是Koenig S和Likhachev M基于LPA算法基础上提出的路径规划算法。 LPA算法本是基于Dijkstra最短路径算法而产生的定起点、定目标点的路径规划算法。 链接 通过对LPA算法的改造，使LPA*算法的思想能应用到诸如车辆动态导航这样的问题。

LPA算法区别于其他算法 的一个重要特点是rhs()的定义：
$$rhs(s)=\{\begin{array}{ll}0& \text{if}s=s_{start}\\ {\text{min}}_{s^{{}^{\prime }}\in Pred(s)}(g(s^{{}^{\prime }})+c(s^{{}^{\prime }},s))& \text{otherwise}\end{array}$$
DLite算法继承了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 $\in$ S, rhs(s) = g(s) = $\infty$;
rhs(s_goal) = 0;
U.Insert(s_goal), CaculateKey(s_goal));

UpdateVertex$(\mu )$

if $(\mu \ne s^{}$