D-Star 寻路算法

D-Star 寻路算法

下面简写 D-Star 为 D*

1. D算法：D 算法”的名称源自 Dynamic A Star,最初由Anthony Stentz于“Optimal and Efficient Path Planning for Partially-Known Environments”中介绍。它是一种启发式的路径搜索算法， 适合面对周围环境未知或者周围环境存在动态变化的场景。

2. 同 A算法类似，D 通过维护一个优先队列 OpenList 来对场景中的路径节点进行搜索，不同的是 D* 不是从起点节点开始搜索，而是从目标点开始搜索，首先将目标点放置进OpenList开始搜索，直到起点节点从OpenList队列中出队为止，即为搜索完成，否则视为搜索失败。

3. D*算法采用反向搜索的目的在于后期需要重新规划路径的时候，能够用到之前搜索到的最短路径信息，减少搜索量，以为从目标节点到起始节点进行搜索得到的最短路径，是以目标点为中心辐射出的最短路径图，图上目标点到各个点之间的路径都是最短的，因此当在既定路径上遇到障碍需要重新规划路径的时候，可以很好的利用之前得到的信息。

4. 而从起点节点向目标节点搜索得到的最短路径图，是以起点为中心辐射出的最短路径图，当沿着路径前行遇到障碍后，需要重新进行路径规划时，没有办法很好的利用原先搜索到的信息。

5. E-Star 算法分为两个阶段
   第一个阶段：使用 Dijkstra/A*算法找到从目标点到起始点的路径，然后机器人从开始节点向目标点移动。
   第二阶段：是动态避障搜索阶段，当机器人移动到一个节点要向下一给节点移动的时候，发现下一个节点由可行走变成障碍时，需要重新规划路径。

6. 参考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值由小到大进行排序的优先队列

7. 算法最主要的函数：
   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 值

D* 核心逻辑就是上面几个截图
Originally stated D* Algorithm 或者 D* Algorithm, again
加 下面几个方法
PROCESS-STATE()
MODIFY-COST(X,Y,cval)
MIN-STATE()
GET-KMIN()
INSERT(X, newCost)

一个 使用 Unity 实现的 Demo 链接

算法在路径 PathFindingUnity\Assets\Script\PathFinding\Algorithms\DStar
效果如下