C++实现规划算法之迪杰斯特拉Dijkstra

前言

本文为规划算法系列第一期（还没规划后续，哈哈），主讲 Dijkstra算法原理及c++实现； 为什么不用Python实现呢？确实Python实现起来更方便：环境准备更容易，代码也更容易运行；但是考虑到实际开发中，主流公司都是用c++，所以选择了用c++实。如果根据教程亲自去实现的话，这个系列教程也不失为一个锻炼的机会：不仅可以学习到dijkstra等算法原理，还有实现过程中涉及的一些其他支线问题，比如如何对结果进行可视化展示？如何将展示结果保存为GIF？还可以在准备编译环境的过程中学习到如何安装第三方库等等...

环境准备

• Visual Studio Community 2022 ：其他版本或其他IDE也可以
• Opencv ：必须的，用于结果可视化
• GraphicsMagick：是一个小巧但强大的图片处理工具和库集合，非必须，这里用于将可视化结果生成gif
• c++ 17及以上

[!note] 我是在window下配置的编程环境，编程基础比较好的可以自己在linux下进行配置。对c++编程环境感兴趣的话，请让我知道，后续可能会出一期环境配置的教程。

算法原理

Dijkstra算法是一种最短路径规划算法，该算法大概在三十年前被开发，对后来的规划算法发展影响极大。后来的A star算法就是由此发展而来的。实际上这两个算法基本上是一样的，区别仅在于二者所使用的优先队列的优先排序方式是不一样的。这里先给出算法伪代码：

在离散规划问题的图论表示中，每条边$e\in{E}$都有一个相应的非负代价$l(e)$，他是应用行动$u\in{U}$后产生的。对于每个状态$x\in{X}$应用动作$u$后产生的转移代价记为$l(x,u)$。从起点到目标节点路径上边代价的总和就是规划的总代价。优先级队列Q被按照称为已付代价函数$C(x)$进行排序。$C^*(x)$称为起点$x_I$到节点$x$的最优已付代价。对在$x_I$到节点$x$的代价$l(e)$求和，其中最低的就是最优已付代价。如果不是最优的就记为$C(x)$。从起点开始搜索，计算搜到的节点的已付代价。初始时，$C^*(x_I)=0$。对起点应用所有动作$u$得到转移后的节点$x'=f(x，u)$，（式中的$f$称为状态转移函数），并计算生成节点的$C(x') = C^*(x_I)+l(x,u)$。这里$C(x')$是目前发现的最好已付代价，但没有被记为$C^*(x')$，因为还没有发现最优的路径。如果$x'$已经在$Q$中，那么新发现的到达$x'$的路径可能更优。如果真是这样的话，就得降低$x'$ 的已付代价$C(x')$，$Q$也要重新排序。

算法从最优代价已经确定的节点开始，向周围相邻节点探索，并计算所有相邻节点的$C(X’)=C^*(X0)+l(X0，X')$，然后将所有X‘节点放入优先队列。之后再从优先队列中的第一个节点开始，继续访问后续可能节点，并计算后续节点的C，如果后续节点已经访问过，并且在本次计算中得到的已付代价值比原来小，就更新该节点的已付代价值。重复此过程，直到所有节点的已付代价都为最优时，算法终止。

什么时候已付代价从$C(x)$变成$C^*(x)$呢？当使用$Q.GetFirst()$将$x$从$Q$中删除时，状态$x$将变成不活动状态，并且意味着不能以更小的代价代价到达该节点。证明如下：

首先可以将所有节点分成三类：

• 未访问的：还未被访问到的节点。
• 不活动的：当探索完某一结点的所有边后，该节点变成不活动的。（从Q中弹出来后，就变成不活动的）
• 活动的：已经遇到的还未被访问过的相邻节点是活动的。如Q就存储着所有活动节点。 （下图展示了这三种状态的区别，图中算法从A开始，向B、C探索）

计算$C(Q[0])$时已经考虑了所有经过不活动节点的路径，后续探索过程不会再探索不活动节点。如果存在从起点到达$Q[0]$的更短路径，后续探索一定要经过Q中的其他节点（首先不活动节点不再被探索，这是由搜索的性质决定的，不活动节点对于探索过程不再能提供有效信息；其次，还未访问的节点必须要经过活动节点，即Q中的节点，才能得到探索）。而这些节点已经具有更高代价（Q中节点是按照cost大小排序的，队首元素cost最小）。 以上证明了队列首元素的最优，还不能证明算法的最优性，感兴趣的话可以参考《Planning Algorithm》2.3.3小节。

[!note] 我在写文章过程中所参考的资料里针对代价一词用了C和G这种表示方法，个人猜测这里的符号$C$和$G$分别代表了cost to Come，和cost to Go；本文暂不涉及到G(X)是如何用到的，读者可先不必在意。但要理解好已付代价$C(X)$，因为dijkstra算法主要计算的就是每一个状态的最优已付代价。

一个详细的例子

先从一个简单的图开始，了解算法的基本过程。下图最左侧描述的是一幅无向节点图，起点是A，终点是D。目标是求解从起点到终点的最短路径（路径节点总代价最小）。 初始时，优先级队列只有A一个元素。另外使用一张表来记录每个节点的$C(x)$--起点到该点的路径代价和，以及到达该节点的上一节点p_node。一开始，只有$C(A)$记为0，其他为无穷。p_node初始均记为NULL。

第一次探索从Q中首个节点A开始。从A能探索到B和C，分别计算B和C的代价，并按照代价大小，有序添加到Q中；由于新计算的B、C节点的代价比原来低，于是更新记录表中B、C的cost为更小值。p_node更新为B、C的上一节点A。由于B是第一个元素，所以可以给B节点打上一个星号标记为B节点的最优代价。

第二次探索从队列首个元素B开始。这里访问到了C、D两个节点，其中C的代价被更新为更小值，而且已经在队列中；D为新访问到的节点，要添加到队列中去，由于$C(D)$比更新后的$C(C)$要大，于是$C$节点排在前头，成了队首元素；队首元素C的cost就是最优的，我们可以给$C(C)$打上一个星号了。

第三次探索从队首元素C开始。从C开始可以访问到A、B、D三个节点，其中A和B节点的最优代价已经求得，无需更新，只要计算出D节点新的cost为4，比原来小，更新cost为新计算的路径代价。 此时，队列里只剩D节点了，也就是队首元素，其cost自然也就是最优的了，我们可以给$C(D)$也打上一个星号。

至此，全部节点的最优cost均已求得（都有星号）。根据探索过程中记录下的路径节点，可以很容易地从任意终点回溯出到起点的路径。比如，如果终点是$D$，那么$D$到起点$A$的路径为：$D$ ->$C$->$B$->$A$.

小测试

这里有几个问题可以用来检测自己是否掌握算法核心。

1. 基础 -- 为什么优先队列$Q$中的第一个元素的$C(x)$一定是最优的？（答案就在本文）
2. 进阶 -- 凭什么认为Dijistra算法计算出的路径是最优的？（参考《Planning Algorithm》2.2.3节）
3. 挑战 -- 自主完成下述项目：给定一个格子世界的网格地图，节点间存在转移关系$f$，通过对某一状态施加动作$u$可以得到转移后的另一状态$x’=f(x,u)$。这里的动作$u$是动作集合$U$ = ｛左移，右移，上移，下移｝的成员；同时状态转移过程需要耗费代价，我们用转移代价函数来表示：$l(x，u)$。在格子世界中，代价可以用格子间的距离表示，相邻的格子转移代价就是1（格子大小为1by1），到邻角的格子的转移代价就是$\sqrt{2}$。请使用Dijistra算法计算起点到某一目标节点的最短路径序列是什么？并对求解结果进行可视化，实现下图类似效果。（答案参考我的Dijistra仓库）

参考资料

• 参考github项目：onlytailei/CppRobotics
• 《Planning Algorithm》2.2~2.3节
• 《天勤笔记-数据结构》第十一版（2023）7.5.1节：迪杰斯特拉算法

PS 欢迎提出宝贵建议~