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):代价函数,表示当前 StateG 的开销计算,如果节点X的父节点是YH(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 ,对于表示为 OpenClosed 的节点 K = min(K,H_new)

    6.5. C(X,Y):表示XY之间的路径开销

    注意:OpenList 是依据节点K值由小到大进行排序的优先队列

  7. 算法最主要的函数:

    PROCESS-STATE:计算到目标G的最优路径

    MODIFY-COST:改变两个 State 之间的开销C(X,Y),并将受影响的State置于OpenList 中

    INSERT: 用来修改节点 X 状态以及 H(X)值和K(X)

D* 寻路算法伪代码如下

下面代码是论文中的代码

下面是代码的注释翻译

lua 复制代码
-- 下面代码包含:
-- 开始寻路过程
-- 行走过程
-- 遇到障碍再寻路的过程
{
    -- 初始设置目标节点 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 部分

lua 复制代码
两个代码不同点只在下面 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)

lua 复制代码
    c(X,Y)=cval
    h(Y) = cval
    if t(X) =CLOSED then INSERT (X,h(X))
    Return GET-MIN ( )

INSERT(X,Hnew)

lua 复制代码
	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()

lua 复制代码
返回 OpenList 优先队列中 节点K 值最小的 节点

GET-KMIN()

lua 复制代码
返回 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)