D*算法动态环境路径规划笔记

D*算法是Dynamic A的简称，是一种适应于动态环境的启发式的路径搜索算法。同A算法类似，，D通过维护一个优先队列openList来对场景中的路径节点进行搜索。不同的是，D从目标点开始搜索，通过将目标点置于openList中来开始搜索，直到机器人当前位置节点从openList出队为止。
D分为两个阶段：第一阶段基于Dijkstra/A算法从目标点往起点进行搜索，得到搜索区域节点距离目标点最短路径的信息；第二阶段是动态避障搜索阶段。因此D算法主要分为两个部分，第一部分是Process_state，主要用于处理节点信息；第二部分是Modify_cost，主要用于修正若干个受障碍物影响而导致代价值发生变化的那些节点信息。
算法将每一个节点的标识分为三类：还未被遍历到的节点为new，已经在openList的为open、曾经在openList但现在已经被移出的为closed。
每个节点到目标点G的代价为h；X到终点的代价h（X）的父节点Y到终点的代价+X和Y之间的代价，即：h（X）=h（Y）+C（X,Y）；节点X在不断遍历过程中，与目标点的代价h会增大或减小，设k始终保持变化的h的最小值。也就是说，对于标识为new的点，表明还未遍历到，k=h=inf；对于标识为open或closed的点，k=min{k，h_new}。节点X根据k与h的大小还有两种状态：若h=k，记为Lower态；若h>k，记为Raise态，表明有更优的路径。
由于A和D*的路径搜索方向相反，为避免混淆不同算法所定义的父节点、子节点概念，统一将离搜索起点更近的节点作为父节点。