路径规划——搜索算法详解（五）：Dynamic A Star(D*)算法详解与Matlab代码

昨天休息了一天，今天继续学习搜索算法！前几天已经分别介绍了Dijkstra算法、Floyd算法、RRT算法、A*算法，无独有偶，上述算法都只适用于静态环境下两点规划的场景，但是大部分场景是实时变化的，这对规划算法提出了更高的要求，Dynamic A star算法应运而生。

Dynamic A star简称D*在“Optimal and Efficient Path Planning for Partially-Known Environments”中提出，笔者前期收集了很多D*算法的信息，刚开始看的时候也是云里雾里，看了很多遍才懂，其实基础原理并不复杂，但是描述起来比较绕，大家可以看多一遍，会有收获的。

D*算法解决的是在动态环境中计算从起点到终点距离的问题，其搜索过程是基于在初始环境下采用A*搜索的原始结果下的，具体细节接下来会一一介绍：

一、D*算法流程与案例讲解：

1.算法基础：

在该算法中提出了许多新的名词与流程，笔者认为很需要拿出来单独解释以下，留个印象即可，后面提到会再解释，建议看完再回来看，相信会加深你对D*算法的理解：

• 与A*算法类似，D*通过维护一个优先队列openlist来存储待搜索的路径节点，但与A*不同的是，D*算法是从目标点开始搜索，通过将目标点置于openlist中来启动搜索，直到起点位置从openlist中弹出时该次搜索完成；
• D*算法分为两个阶段：第一阶段为采用A*算法从目标点反向搜索，得到搜索路径以及每一个区域节点的信息（包含h（当前启发值）、k（最优启发值）、b（父节点））。第二阶段是动态避障搜索阶段，第一阶段的信息获取对于第二阶段的快速动态搜索起着至关重要的作用！
• D*算法对应地分为两个部分，第一部分为Process_state，主要用于处理节点扩展；第二部分为Modify_cost，这是D*算法的核心，其用于更新受障碍物影响而导致代价值发生变化的节点信息；
• 对于每一个搜索的节点，其标识被分为了三类：new（未被遍历节点）、open（openlist中的节点）、closed（在openlist中被移除的节点）；
• 每个节点到目标点G的代价为h（参考自A*算法的符号），节点X与节点Y之间的代价为C(X,Y)， X到达终点的代价为h(X)=X的父节点Y到达终点的代价+X与Y之间的代价，即：h(X)=h(Y)+C(X,Y)；
• 节点X在不断遍历的过程中，与目标点的代价h(X)会实时改变，k(X)始终保持h(X)变化中的最小值。标识为new的点指的是未遍历点，对于其进行初始化k=h=inf；对于标识为open、closed的点，k=min{k，h_new}。
• 对于遍历到的节点X，其根据k与h的大小关系将被标记为两种状态，当h=k时，记为Lower态，表示该点记录的最小代价h(X)与k(X)是相等的，直观上可以看作是周围环境未发生改变，或者是周围环境发生改变但是未影响到X点到达终点的代价；当h>k时，记为Raise态，其直观上可以看作是由于环境变化导致实时变化的h(X)受到了影响，更改周围连接的路径点可以达到更短的路径。
• 为了避免混淆，笔者统一将离终点近的节点作为父节点；

2.算法流程（讲解案例来自于古月学院）：

(1)算法代码逻辑：

这是原文中的伪代码

(2)案例讲解：

如上图所示，白色为自由栅格，灰色为障碍物，橙色为openlist中的栅格，蓝色为第一阶段通过A*算法反向搜索所得到的路径。起点为（2，1）、终点为（7，6），其中每个节点有三个信息，b为父节点、h为实时更新的代价值、k为记录h的最小值。

假设当前环境发生变化（4，3）变成了障碍物，此时机器人从起点（2，1）开始运动，当运动到（3，2）后机器人发现（4，3）处变成了障碍物，此时（4，3）处的h更新为h=inf，即k<h，变为了Raise态，此时到了上述伪代码的步骤2。

根据步骤2的流程，此时将遍历（4，3）的周围节点，查找能够使h降低的父节点，若存在一个使得（4，3）的h值恢复到h=k的父节点则将其恢复到Lower态，即表明图中还存在另外一条与原有路径等价的路径。

但是由于（4，3）为障碍物且不可能存在能够将其恢复到Lower态的父节点，降低后的h仍然满足h>k。此时跳过步骤3，直接进入步骤4（可以理解为（4，3）变成障碍物对其原子节点（3，2）会造成影响，这一步是为了动态传播障碍物信息，这也是D*算法的核心思想）

进入步骤4，由于不存在能够将（4，3）恢复到Lower态的节点，意味着此时由于障碍物的变化，不能够找到一条等价的路径，以（4，3）作为父节点的节点必定会受到其影响，步骤4将检查其周围节点，其中（4，4）、（5，4）、（5，3）、（5，2）没有受到其影响，而（3，3）、（3，4）、（4，2）本身为障碍物，此次循环中只有（3，2）受到了障碍物变化的影响，由于h（3，2）=h（4，3）+ d（两节点之间的距离）= inf + d（两节点之间的距离）= inf 此时（3，2）也变为了Raise态，此时对应着步骤4.1的操作，将（3，2）插入到openlist中。意味着正处于（3，2）的机器人需要寻找其他可行节点

由于k（3，2）= 5.6，从closelist中弹出并开始新一轮的循环，此时弹出k（3，2），进入步骤2，遍历周围节点并尝试寻找新的父节点X满足h（3，2）=h（X）+d（X与当前节点的距离），此时遍历所有节点可以发现（4，1）满足要求，以（4，1）作为父节点可以将h（3，2）更新为h=7.6=k恢复到Lower态。

因此，只需要将（2，3）的父节点从（4，3）更改为（4，1）后即可，由于初始搜索时每个节点都已经记录了其父节点，所以直接回溯即可得到更改后的新路径，不必再次搜索，进而保证了在动态环境下再规划的效率。

大家可以对着流程理解下，多看几次就能懂了，希望对你们有所帮助！

二、D*算法MATLAB代码

照着课程打了一遍，已经上传到本人github中，需要的朋友自取：

Adamaser/Path-Planning (github.com)