D-Star 寻路算法
下面简写 D-Star 为 D*
-
D算法：D 算法”的名称源自 Dynamic A Star,最初由Anthony Stentz于“Optimal and Efficient Path Planning for Partially-Known Environments”中介绍。它是一种启发式的路径搜索算法， 适合面对周围环境未知或者周围环境存在动态变化的场景。
-
同 A算法类似，D 通过维护一个优先队列 OpenList 来对场景中的路径节点进行搜索，不同的是 D* 不是从起点节点开始搜索，而是从目标点开始搜索，首先将目标点放置进OpenList开始搜索，直到起点节点从OpenList队列中出队为止，即为搜索完成，否则视为搜索失败。
-
D*算法采用反向搜索的目的在于后期需要重新规划路径的时候，能够用到之前搜索到的最短路径信息，减少搜索量，以为从目标节点到起始节点进行搜索得到的最短路径，是以目标点为中心辐射出的最短路径图，图上目标点到各个点之间的路径都是最短的，因此当在既定路径上遇到障碍需要重新规划路径的时候，可以很好的利用之前得到的信息。
-
而从起点节点向目标节点搜索得到的最短路径图，是以起点为中心辐射出的最短路径图，当沿着路径前行遇到障碍后，需要重新进行路径规划时，没有办法很好的利用原先搜索到的信息。
-
E-Star 算法分为两个阶段
第一个阶段：使用 Dijkstra/A*算法找到从目标点到起始点的路径，然后机器人从开始节点向目标点移动。
第二阶段：是动态避障搜索阶段，当机器人移动到一个节点要向下一给节点移动的时候，发现下一个节点由可行走变成障碍时，需要重新规划路径。 -
参考D论文
D算法有几个重要的概念及函数
6.1. G ： Goal State目标节点6.2. **State ：**路径节点，路径节点包含以下几个信息
6.2.1. BackPointer：指向前一个 State 的指针，一般 Dijkstra/A 用 Parent表示，路径搜索结束后，机器人从所在的 State，通过 BackPointer 即可一步一步地移动到目标 Goal State
6.2.2. b(X) = Y 表示 X 的BackPointer*(父节点)是 Y
6.2.3. New：该State 从未被放置于 OpenList 中
Open：该State 此时正存放于 OpenList中
**Closed：**该 State 已经从 OpenList 中出队6.3. H(X)：代价函数，表示当前 State 到 G 的开销计算，如果节点X的父节点是Y，H(X) = H(Y) + C(X,Y)
6.4. K(X)：Key Function，该值是优先队列OpenList中的排序依据，K 值最小的State位于队列头（Dijkstra/A中 OpenList 排序是以H值为排序依据），D是针对动态环境设计的算法，由于环境的改变节点的H值可能发生改变，而节点的K值记录的是该点的最小H值，也就是说对于为遍历到的点，K=H=inf，对于表示为 Open或Closed的节点 K = min(K,H_new)
6.5. C(X,Y)：表示X与Y之间的路径开销
注意：OpenList 是依据节点K值由小到大进行排序的优先队列
-
算法最主要的函数：
PROCESS-STATE：计算到目标G的最优路径
MODIFY-COST：改变两个 State 之间的开销C(X,Y)，并将受影响的State置于OpenList 中
INSERT: 用来修改节点 X 状态以及 H(X)值和K(X)
D* 寻路算法伪代码如下
下面代码是论文中的代码
下面是代码的注释翻译
-- 下面代码包含：
-- 开始寻路过程
-- 行走过程
-- 遇到障碍再寻路的过程
{
-- 初始设置目标节点 H 值为 0，将 G 加入到 OpenList
h(G)=0;
-- 开始寻路过程
do
{
-- 循环调用 PROCESS-STATE(), 函数返回当前 OpenList 优先队列中节点K值最小的K值
-- 如果 OpenList 优先队列中没有节点则返回 -1
kmin=PROCESS-STATE();
-- while 判定条件
-- kmin = -1：说明还没有找到 开始节点(start state),OpenList 优先队列中没有节点了，则寻路失败
-- start state not removed from open list：开始节点(start state)从 OpenList 队列出队,则已经从目标节点找到 开始节点了
}while(kmin != -1 && start state not removed from open list);
if(kmin == -1)
{
-- 如果 kmin = -1 说明寻路失败返回，退出
goal unreachable;
exit;
}
else
{
-- 寻路成功，在 do-while 中包含了 行走、
do{
-- 行走过程
do{
-- 迭代行走
trace optimal path();
-- while 判定条件
-- goal is not reached：没有到大 G 目标节点
-- map == environment：假如当前走到节点X，要向下一个节点Y行走时，判断节点Y状态发生了变化（变成了障碍等）
}while ( goal is not reached && map == environment);
-- 如果已经到大 G 目标点，退出
if (goal_is_reached)
{
exit;
}
else
{
-- 没有到达目标点，肯定是行走过程中一个本来可以通过的节点，状态发生了变化
-- 机器人行走过程中发现障碍时所在的 state 节点X
-- 向节点Y行走时发现节点Y状态发生变化了，导致路径开销的更改已经传播到了节点X
Y = State of discrepancy reached trying to move from some State X;
-- 改变节点Y、X 的Cost
MODIFY-COST(Y,X,newc(Y,X));
-- 遇到障碍再寻路的过程
do
{
kmin=PROCESS-STATE();
-- while 判定条件
-- kmin< h(X)：经过不断地处理直到 kmin 小于节点 X 的 H值
-- kmin != -1：当 kmin = -1 时表示寻路失败
}while(kmin< h(X) && kmin != -1);
-- 寻路失败，退出
if(kmin==-1)
exit();
}
}while(1);
}
}
另论文中另一个版本的逻辑如下
两者的不同在于遇到障碍重新规划路径的 do-while 中的 while 部分
两个代码不同点只在下面 while 部分，经过测试，两种判定都是可以完成再次寻路的，时间原因论文没有仔细阅读，有疑问的读着可以自行去看论文，然后给我说一下结果
do
{
kmin=PROCESS-STATE();
}while(kmin< h(X) && kmin != -1);
do
{
kmin=PROCESS-STATE();
}while( k(Y) < h(Y) && kmin != -1);
PROCESS-STATE() 函数
MODIFY-COST(X,Y,cval)
MIN-STATE()
GET-KMIN()
INSERT(X, newCost)
上面截图中函数代码不全，下面是各个函数补齐
MODIFY-COST(X,Y,cval)
c(X,Y)=cval
h(Y) = cval
if t(X) =CLOSED then INSERT (X,h(X))
Return GET-MIN ( )
INSERT(X,Hnew)
if t(X) = NEW then
k(X)=hnew
-- 然后就是 X 加入到 OpenList 队列这部分
X and to OpenList
if t(X) = OPEN then
k(X)=min(k(X),hnew)
if t(X) = CLOSED then
k(X)=min(k(X),hnew)
-- 然后就是 X 加入到 OpenList 队列这部分
X and to OpenList
-- 漏掉了 h(X) = hnew
h(X) = hnew
t(X)= OPEN
Sort open list based on increasing k values;
MIN-STATE()
返回 OpenList 优先队列中 节点K 值最小的 节点
GET-KMIN()
返回 OpenList 优先队列中 节点K 值最小的 节点的 K 值
一个 使用 Unity 实现的 Demo 链接
算法在路径 PathFindingUnity\Assets\Script\PathFinding\Algorithms\DStar
D* 核心逻辑就是上面几个截图
Originally stated D* Algorithm 或者 D* Algorithm, again
加 下面几个方法
PROCESS-STATE()
MODIFY-COST(X,Y,cval)
MIN-STATE()
GET-KMIN()
INSERT(X, newCost)
