【算法】最短路径算法思路小结

一、基础：二叉树的遍历->图的遍历

提到搜索算法，就不得不说两个最基础的思想：
BFS（Breadth First Search）广度优先搜索
DFS（Depth First Search）深度优先搜索

刚开始是在二叉树遍历中接触这两个算法，从中文名字上很好理解，广度优先就是优先搜索横向结点，深度优先就是优先纵向搜索根结点。用一个示意图Fg.1和Fg.2来表示

BFS输出：0 1 2 3 4 5
DFS输出：0 1 3 4 2 5

从二叉树过渡到图有两个变化，以有向图为例子：每个节点的子节点不一定是两个，节点间有方向性（如Fg.3）。

图的遍历要注意不要重复遍历节点。依然可以用BFS和DFS来做，用一个数组来储存每个节点是否被遍历（比如已经遍历置为true，没有遍历的为false）。BFS使用队列，DFS使用栈，详细的在这里不多说。

二、进阶：最短路径Dijkstra算法

如果我们在考虑最优路径的问题，图比Fg.3的有向图要复杂一些。
比如一个城市有很多镇，每个镇之间有多条路径可以到达，A镇到D镇没有直达路，要从A走到D，可能要经过B，也可能经过C，我们通常会选择最短的距离作为最优路径，这就要求我们在图中引入权值，且对于路径问题来说，权值一定为正数（如Fg.4）。

Dijkstra可以算出起始点到任意点的最短距离。它其实是BFS的加权版，引入了贪心算法的思想，从起始位置开始遍历子节点，每次都要选最短的路径走，直到遍历到目标位置。从局部最优到全局最优。

Dijkstra需要维护一个从起点到终点到路径及距离（权值）表格，可以用二维数组来存储。因为每个点都需要遍历到其他点的路径来更新表格，所以算法复杂度O(n²)。

三、启发式的搜索：最佳优先搜索 & A*算法

现实中找路也许更加复杂。假设我是一个路痴（其实我就是），我不知道A镇到D镇要走哪个方向，难道要一圈一圈扩大搜索范围来找吗？这时候如果使用Dijkstra算法，我需要从红色起点向周围扩展8个格子，再分别考虑这8个格子周围的8个格子，太慢了! 所以启发式搜索登场了。

如Fg.5所示，我站在红色的起点，但我不是茫然无措的，我还知道终点在哪，所谓启发，就是我的搜索是有方向性的。在此介绍最佳优先搜索（Greedy Best First Search）和A*算法。

那么我到底要往哪里走？最佳优先搜索和A*算法都引入了一个启发函数。
$$最佳优先算法：F=H$$
$$A\ast 算法：F=G+H$$
两种算法每次都是选取F最小的值。在这个等式中，G是到起点的距离，为确定值；H是到目标点的距离，为预测值。为了方便计算，我们将这些值扩大10倍。
G比较好理解，比如从起点向右走一格，G值为10（即1x10），向斜上方走一格，G值为14（即$\sqrt{2}$x10）。
H即当前位置到终点的距离，有多种预测方法。在此我们展示Manhattan方法（使用曼哈顿距离），即忽略障碍物，考虑起点到终点的最短位移（只能往水平或竖直方向走），再扩大10倍得到H预测值。起点周围的方格的H值如Fg.6所示。

以A*算法为例，最后我们就可以计算出F值，如Fg.7所示，起点正右侧的格子F=G+H=10+50=60

它也是当前检索方块中F值最小的方块，就加入到路径中，如图Fg.8所示，然后再去探索当前粉色位置周围的F值（如果遇到障碍物，则忽略障碍物格子）。依次检索，直到终点。

可以看到最佳优先算法和A* 算法都是对Dijkstra算法的优化，引入启发函数使计算量大大减少，当H值为0的时候，最佳优先算法和A*就退化成了Dijkstra算法。

四、进一步优化：考虑动态环境的D*算法

A*算法用启发的方式探路，看上去已经很棒了。

还记得A*的公式吗：
F=G+H
G是当前节点到起点的确定距离，H是当前节点到终点的预测距离

不过现实总是更复杂一些，上面我们只是在图上放了几个固定的障碍物。那如果环境在变化，比如走着走着，障碍物移了动怎么办？
对于A*来说，即使障碍物只移动了很小的位移，但从当前位置到终点的路径也需要重新规划。如图Fg.9和Fg.10所示，障碍物（黑色方块）移动了1个单位位移，要重新计算一遍F值规划路径。粉色标c的部分其实是相同的路径，可否省略这部分计算呢？

D* 算法在A* 的基础上在1994年提出。针对动态环境的路径规划，增加了环境检测和更新影响路径这两个步骤。最重要的是，为了省略Fg.9和Fg.10中粉色标c的计算，D*决定从终点开始规划路径。

我们先大体看看A* 和D*的不同：

A*	D*
起点开始搜索	终点开始搜索
启发式搜索，环境变化需要重新规划路径	增量式搜索，环境变化可以利用先前已规划的信息
F=G+H，每次取F最小值	k=min { k , h_new}，每次取k最小值

详细看下D* 的公式
k=min { k , h_new}
h_new表示当前位置到终点的确定距离（和A* 的G有点像）。
简单来理解D* 算法就是从终点向起点探索出一条最短路径。探索完后，调用行进函数出发，检测到环境变化（障碍物移动，或障碍物出现），就要调用修正函数来修正这条最短路径。

那问题来了，为什么类比A* 算法，直接k=h_new，而还要取k和h_new的最小值为更新值呢？我们再深入一点看下算法的实现：
D*维护一个OpenList队列来存储搜索节点，直到起始格子出队列就完成了一个路径搜索。
在算法的实现中，每个小格子会有三个状态，

• OPEN
• CLOSE
• NEW

OPEN表示被搜索的格子，即h_new值更新的格子；
CLOSE表示已加入规划路径的格子，即从OpenList中移除的格子；
NEW表示未曾被搜索的格子，h_new值初始化为正无穷。

试想，当环境变化，比如出现了障碍物，h_new值被更新为值正无穷。此时将这个变化的格子被加入到OpenList重新规划路径，因为OpenList是按照h_new排序后依次遍历更新周围节点的，所以这个突然出现的障碍物节点就会排到最后才遍历。但我们希望马上就能遍历这个障碍物节点但周围节点，所以需要k值，这样使效率提高不少。
每次环境变化要修正路径时，计算加入到OpenList的格子的k值，变小才加入到修正的路径中，否则停止搜索。
我们按照k值公式来举例子，如图Fg.11和Fg.12：
当前位置在粉红色格子，Fg.11->Fg.12 正右侧有障碍物突然出现，即B格子h_new的值被更新为正无穷，这时候计算k值，k值保持之前的k值不更新，这个格子不会被加入OpenList中。
再举个例子，当前位置在粉红色格子，Fg.12->Fg.11 正右侧障碍物突然移开，即B格子h_new的值被更新变小，这时候计算k值就比原来的k值小，即为h_new，此时这个变化的格子B会被加入到OpenList中重新规划路径。

照这样继续搜索，k>=h_new时停止更新，这样其实Fg.9和Fg.10中标有c的部分就不会被更新，减少了许多计算量。如此就完成了路径的修正。

小结：以上算法是需要先验知识的，即起点和终点并距离信息，这些信息可以通过路径规划前的扫描建图来获取。A* 多用于静态环境路径规划，D*多用于动态环境路径规划，是目前比较优秀的最短路径搜索算法。