本
文
摘
要
Dynamic A* (D*) 算法的遍历过程
Example 1
关于D*算法论文中的符号定义描述
LOWER state
RAISE state
MODIFY_COST函数伪代码
PROCESS_STATE函数伪代码
Example 2
初始化：
所有节点 tag = NEW
h = inf
k = 0
终点
h = 0
h值的计算采用欧式距离
Example 3
将终点（7.6）加入openlist中，h=0, k=0。k值作为队列中的优先级。
Example 4
从openlist中弹出k值最小的节点（7, 6）并扩展，把邻点(6,6)、(6,5)和(7,5)加入openlist中。
步骤对应的代码部分，如PPT所示的蓝色标注部分。
Example 5
从openlist中弹出k值最小的节点（6, 6）并扩展，把邻点（5,6）和 （5,5）加入openlist中。
步骤对应的代码部分，如PPT所示的蓝色标注部分。
Example 6
从openlist中弹出k值最小的节点（7, 5）并扩展，把邻点（6,4）和 （7,4）加入openlist中。
步骤对应的代码部分，如PPT所示的蓝色标注部分。
Example 7
将(4,6)扩展后，把邻点(3,5)和(3,6)放入openlist中，但由于(3,5)和(3,6)状态是障碍物，将它们加入到openlist时，把它们的k值和h值设为10000。
步骤对应的代码部分，如PPT所示的蓝色标注部分。
Example 8
重复从openlist中弹出k值最小的节点，进行扩展，直到终点起点（2,1）被弹出，结果如图所示。
上面的遍历过程相当于Dijkstra算法的遍历过程（Dijkstra教程）。
A*是正向搜索，而D*特点是反向搜索，即从目标点开始搜索过程。在初次遍历时候，与Dijkstr算法一致，它将每个节点的信息都保存下来。
Example 9
最后 我们能看到规划出来的最优路径
下面开始进入动态规划
Example 10
机器人开始沿着最优路径移动，当走到（3,2）的时候发现（4,3）是一个障碍点，无法通过。
Example 11
由于坐标点（4,3）是障碍点，我们需要修改它的h值为10000，保持k值不变。
并把他的邻点加入到openlist中。
步骤对应的代码部分，如PPT所示的红色标注部分。
Example 12
从openlist中弹出k值最小的节点（5,4）。其k和h相同，考虑（5,4）的每个邻点。（4，3）的后指针指向（5,4），但其原始h值不等于（5,4）的h值加上转移代价了，而是由于变成障碍点，代价值提高。因此，把（4,3）加入到openlist中。请注意，由于（4,3）已在openlist中，因此保持其k值不变
（为什么要保持原k值，因为该节点一开始处于最优路径中，就算变成障碍物，但是重新规划的路径也应该在其节点附近，如果我们修改其k值，变成10000，那么最后才会从openlist中将其它弹出,这也是为什么要创建k值的原因）。
因为h值> k值，所以节点（4,3）被标记为 RAISE state。
步骤对应的代码部分，如PPT所示的红色标注部分。
Example 13
接下来，我们弹出（5,3），因为周围的节点都不是新的，并且周围像素的h值正确，所以我们不会有任何操作。同理，（4,4）也一样。
Example 14
从openlist中弹出k值最小的节点（4,3）。因为k <h，所以我们的目标是尝试减小h值。这类似于找到一个
从（4,3）到目标的更好路径，但这是不可能的，因为（4,3）是障碍。
例如，（5,3）的h值小于（4,3）的k值，但是（4,3）的h值 是小于（ 5,3）加上转移代价。
实际上，（4,3）是障碍物点，我们无法减少从（4,3）到（5,3）的代价。（5,4）和（4,4）也是如此。
步骤对应的代码部分，如PPT所示的粉红色标注部分。
因此，我们找不到通过（4,3）的任何一个邻点能减小h值的路径。接下来，我们展开（4,3），更新所有后向指针指向（4,3），
[在这种情况下，仅（3,2）]节点的h值，并加入到openlist中。
现在，（3,2）也被标记为 RAISE state。请注意，（3,2）的k值设置为其新旧h值的最小值（此设置发生在加入到openlist的函数中）。
接下来，从openlist中弹出k值最小的节点（5,2），原理同 Example 13 一样，我们不会有任何操作。
Example 15
从openlist中弹出k值最小的节点（3,2）并扩展。
第一个if判断
由于k <h，我们需要想办法减少（3,2）的h值。
于是寻找是否存在一个的邻点满足h值小于（3,2）的k值，如果能找到我们将通过该邻点重定向反向指针。但是，不存在这样的邻点。
步骤对应的代码部分，如PPT所示的粉红色标注部分。
第二个if判断，else部分
因此，寻找一个邻点，满足该节点的不是（3,2）的父节点，其h值加上转移代价小于（3,2）的h值，已经在closelist中，并且其h 值大于（3,2）的k值。
能找到的唯一邻点是（4,1）。这有可能导致路径代价降低。因此，选择邻点（4,1）是因为它可能会降低（3,2）的h值。我们将此邻点及其当前h值放到openlist中。
因为h = k，所以它被标记为 LOWER state。
步骤对应的代码部分，如PPT所示的红色标注部分。
寻找反向指针指向（3,2）并具有“不正确” 的h值的邻点，即。邻点的h值不等于（3,2）的h值加上转移代价。
能找到的邻点是（3,1），（2,1）和（2,2），更新他们的h值，并加入到openlist。请注意，这些节点的k值更新为新h值和旧h值中的最小值。
步骤对应的代码部分，如PPT所示的蓝色标注部分。
Example 16
从openlist中弹出k值最小的节点（4,1）并扩展。
由于（4,1）的h和k值相同，
寻找是否存在指针未指向（4,1）的邻点而且其h值大于（4,1）的h值加上他们的转移代价。
能找到的邻点是（3,2）和（3,1），将他们反向指针重定向为（4,1），然后将它们加入到openlist中。
在加入到openlist之前，我们需要更新它们的k值。
因为（3,2）是在closelist中（Example 15），所以它的新k值取新h值和旧h值中的最小值，并且由于k == h，它现在是一个较低的状态，即新k = min（新h，旧 h）。
因为（3,1）是在openlist中”，所以它的新k值取旧k值和新h值中的最小值，即new k = min（旧k，新h）。
步骤对应的代码部分，如PPT所示的蓝色标注部分。
Example 17
从openlist中弹出k值最小的节点（3,1）并扩展。
第一个if判断
由于其旧k值（6.6）<h值（7.2），寻找是否存在一个邻点的h值小于（3,1）的k值。
没错，我们能找到（4,1）。但是（4.1）的h值加上转移代价并不小于（3,1）的k值的，所以不需要修改。
步骤对应的代码部分，如PPT所示的粉红色标注部分。
第二个if判断，else部分
（3,1）可以用于减少邻点的h值，因此将（3,1）加入到openlist，将k值取其新h值和旧h值的最小值。因此，它被标记为 LOWER state。
步骤对应的代码部分，如PPT所示的蓝色标注部分。
Example 18
从openlist中弹出k值最小的节点（2,2）并扩展。
发现节点（2,2）的邻点中有指针指向（2,2）且h值被增加的点，所以我们重新把他们加入到openlsit。
他们分别是节点（1,1），（1,2）和（1,3），因为已经在openlsit中，所以我们更新他们的h值（实际上h值要被更新，但是示意图省略了），标记为 RASIE state。
步骤对应的代码部分，如PPT所示的蓝色标注部分。
Example 19
从openlist中弹出k值最小的节点（2,1）并扩展。
因为旧k <h，并且它不能降低与其任何邻点的代价值，所以不需要修改。
Example 20
从openlist中弹出k值最小的节点（3,1）并扩展。
发现能减少邻点（2,2）和（2,1）的h值。所以把他们加入到openlsit。
因为（2,2）和（2,1）已经在closelist中，所以它的新k值取新h值和旧h值中的最小值。
因为k = h，所以它们现在处于LOWER state。
步骤对应的代码部分，如PPT所示的蓝色标注部分。
Example 21
搜索到此结束，因为openlist上的最小k值不少于机器人当前状态的h值。因此，我们知道，从openlist中弹出下一个节点将不会减少机器人路径的代价值。
Example 22
追溯到终点，形成最优路径。
Example 23
这就是D星算法的遍历过程。
D*算法总结：
在初始时，所有节点的t（Tag）被设置为 New ，H（终点）被设置为0，终点被放置于OpenList，然后Process-State函数被不断执行，直到机器人所处节点 X被openlist弹出，然后可以通过机器人的当前节点按backpointer指向终点。当移动过程中发现新探测到的障碍时，Modify-Cost函数立刻被调用，来更正的路径代价并将受影响的节点重新置于openlist中。令Y表示robot发现障碍时所在的节点，通过不断调用Process-State直到kmin≥H（Y），这时表示路径代价的改变已经传播到了Y。此时，新的路径构建完成。
上述D*算法总结原链接 ,略有删改。
https://blog.csdn.net/a380331382/article/details/82841071
点击下方链接获取更多资料
下次算法详解预告 RRT。
