如何建设网站服务器,榆林做网站的公司,广告策划书的格式,南通市住房和城乡建设厅网站D-Star 寻路算法
下面简写 D-Star 为 D* D算法#xff1a;D 算法”的名称源自 Dynamic A Star,最初由Anthony Stentz于“Optimal and Efficient Path Planning for Partially-Known Environments”中介绍。它是一种启发式的路径搜索算法#xff0c; 适合面对周围环境未知或者…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的父节点是YH(X) H(Y) C(X,Y) 6.4. K(X)Key Function该值是优先队列OpenList中的排序依据K 值最小的State位于队列头Dijkstra/A中 OpenList 排序是以H值为排序依据D是针对动态环境设计的算法由于环境的改变节点的H值可能发生改变而节点的K值记录的是该点的最小H值也就是说对于为遍历到的点KHinf对于表示为 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 加入到 OpenListh(G)0;-- 开始寻路过程do{-- 循环调用 PROCESS-STATE(), 函数返回当前 OpenList 优先队列中节点K值最小的K值-- 如果 OpenList 优先队列中没有节点则返回 -1kminPROCESS-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状态发生变化了导致路径开销的更改已经传播到了节点XY State of discrepancy reached trying to move from some State X;-- 改变节点Y、X 的CostMODIFY-COST(Y,X,newc(Y,X));-- 遇到障碍再寻路的过程do{kminPROCESS-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
{kminPROCESS-STATE();
}while(kmin h(X) kmin ! -1);do
{kminPROCESS-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)cvalh(Y) cvalif t(X) CLOSED then INSERT (X,h(X))Return GET-MIN ( )INSERT(X,Hnew) if t(X) NEW thenk(X)hnew -- 然后就是 X 加入到 OpenList 队列这部分X and to OpenListif 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) hnewh(X) hnewt(X) OPENSort 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 效果如下
