一、什么是Dijstra算法

定义：采用优先级定义的广度优先搜索思想，以拓扑连通图的起始点为中心按照路径长度以递增方式层层向外扩展，直到扩展到终止点。

二、如何实现Dijstra算法

1、思路：

拓扑连通图由节点和连接边组成。其中节点有两个属性，分别为当前节点到起始点的最小路径长度dist和父节点parent_node；连接边具有路径长度edge.cost。首先访问拓扑连通图中的起始点，然后反复检查最接近但未检查的节点，并将其邻域结点添加到要检查的节点。

2、步骤：

（1）创建节点与起始点的距离矩阵distanceFromStart，将起始点的值设置为0，其余元素设置为无穷大inf

（2）创建节点的父节点矩阵parent，初始化为0，表示无父节点

（3）每次迭代更新时，找出distanceFromStart中未完成遍历节点中总评估成本最低的节点，设置为当前节点current_node；设置current_node.dist=inf，（防止再次遍历），并标记为已完成遍历；对curren_node的四个相邻节点中的未遍历节点进行判定，判断相邻节点N.g否大于current_node.g+edge.cost，如果大于，则N.g=current_node.g+edge.cost，N.parent_node=current_node

（4）重复步骤（3）直至current_node为目标点为止

（5）根据父节点矩阵parent回溯最优路径

3、例子：

在如下图所示的离散拓扑连通图中，要求解从起始点A到目标点G的最佳路径。在下面的求解过程中，为了方便显示，使用绿色表示起始点，橙色表示目标点，红色代表节点已经完成遍历，蓝色表示节点尚未完成遍历，白色表示该节点尚未纳入考虑范围。

在第一次迭代中A是路径长度最小的节点，所以A设置为当前节点current_node，并标记为完成遍历。将A从T中移除，加入到S。然后对A开始第一轮搜索，与A相邻的节点为B和E。对于B来说，B.dist=inf>A.dist+edge_cost=0+2，因此更新B.dist=2，B的parent为A。对于E来说，E.dist=inf>A.dist+edge_cost=0+3，因此更新E.dist=3，E的parent为A。此时S={A->A=0}，T={A->B=2, A->C=inf, A->D=inf,A->E=3, A->F=inf,A->G=inf}。

在第二次迭代中，从T中选择路径最短的节点B为current_node，并标记为完成遍历。将B从T中移除，加入S。然后对B进行搜索，与B相邻的节点为C。对于C来说，C.dist=inf>B.dist+edge_cost=2+2，因此更新C.dist=4，C的parent为B。此时S={A->A=0,A->B=2}，T={A->C=4(路径ABC), A->D=inf,A->E=3, A->F=inf,A->G=inf}。

在第三次迭代中，从T中选择路径最短的节点E为current_node，并标记为完成遍历。将E从T中移除，加入S。然后对E进行搜索，与E相邻的未遍历节点为C和F。对于C来说，C.dist=4<E.dist+edge_cost=3+7，因此保持原样。对于F来说，F.dist=inf<E.dist+edge_cost=3+3，因此更新F.dist=6，F的parent为E。此时S={A->A=0,A->B=2,A->E=3}，T={A->C=4(路径ABC), A->D=inf, A->F=6(路径AEF),A->G=inf}。

在第四次迭代中，从T中选择路径最短的节点C为current_node，并标记为完成遍历。将C从T中移除，加入S。然后对C进行搜索，与C相邻的未遍历节点为D和F。对于D来说，D.dist=inf>C.dist+edge_cost=4+3，因此更新D.dist=7，D的parent为C。对于F来说，F.dist=6<C.dist+edge_cost=4+3，因此保持原样。此时S={A->A=0,A->B=2,A->E=3, A->C=4(路径ABC)}，T={A->D=7(路径ABCD), A->F=6(路径AEF),A->G=inf}。

在第五次迭代中，从T中选择路径最短的节点F为current_node，并标记为完成遍历。将F从T中移除，加入S。然后对F进行搜索，与F相邻的未遍历节点为G。对于G来说，G.dist=inf>F.dist+edge_cost=6+3，因此更新G.dist=9，G的parent为F。此时S={A->A=0,A->B=2,A->E=3, A->C=4(路径ABC), A->F=6(路径AEF)}，T={A->D=7(路径ABCD),A->G=9(路径AEFG)}。

在第六次迭代中，从T中选择路径最短的节点D为current_node，并标记为完成遍历。将D从T中移除，加入S。然后对D进行搜索，与D相邻的未遍历节点为G。对于G来说，G.dist=9>F.dist+edge_cost=3+4，因此更新G.dist=7，G的parent为D。此时S={A->A=0,A->B=2,A->E=3, A->C=4(路径ABC), A->F=6(路径AEF) ,A->D=7(路径ABCD)}，T={A->G=9(路径AEFG)}。

在第七次迭代中，从T中选择路径最短的节点G为current_node，由于G为目标节点，因此结束迭代。此时根据节点的parent属性回溯（G-D-C-B-A），可得出最短路径A->B->C->D->G。

三、Matlab仿真效果

在Matlab软件中建立栅格地图作为拓扑连通图，将地图区域分割为若干个单位区域栅格，分为占用栅格（机器人不可以自由通过）和非占用栅格（机器人可以自由通过）。黑色栅格表示障碍物，白色栅格表示自由空间。同时假设机器人在网络中运动且只占用一个栅格。实验效果如下，迭代了1363次，具体代码在另一篇博客中详述（正在整理写作）：