机器人全局路径规划算法——Dijk

原文地址:

https://congleetea.github.io/blog/2019/09/06/dijkstra/

概述

• 单源最短路径法: 就是从某一个节点到 其他所有节点 的最短路径的计算。
• 不能处理负权边，但是可以将负权边全部加上一个数变成正数。
• 适合有向无环图。

执行步骤

需要的数据结构

• 一个保存图的结构，用于保存节点的邻居关系；
• 一个保存节点是否已经得到最短路径，找到最短路劲的节点标记为已处理节点；
• 保存节点目前获得的最短路径值(默认为最大值inf)及其父节点(便于后面反推路径)。

步骤

• 从起点开始，从邻居中找出代价最低的节点，那么最短时间到达这个节点的代价就求出 来了。标记该节点为已处理节点，更新该节点的最小代价值。
• 找出未处理的所有节点中，代价值最小的节点，计算这个节点到其邻居的代价值。将 代价值和这些邻居原有的代价值比较，选择较小的代价值并更新其代价值和父节点。 重复该过程，直到对图中每个节点都这么做了(实际使用时，只要目标点标记为已处理 就可以停止了)。
• 计算最终路径: 从目标点开始，依次寻找其父节点，最终连成线，就是最短路径了。

例子

如下图的树状图，线段数值表示代价值，节点0是起点。

Figure 1: 图

节点	0	1	2	3	4	5	6	7
当前最小代价值	0	inf	inf	inf	inf	inf	inf	inf
当前父节点
是否已处理	true	false	false	false	false	false	false	false

初始时有上表，算法中不断更新该表。

• a) 找出节点0的邻居(1,2)，计算对应代价值(1,5)，代价值比默认inf小，于是更新代价 值和父节点，找出未处理的节点中最小的代价值是1，将其标记为已处理：

  节点	0	1	2	3	4	5	6	7
  当前最小代价值	0	1	5	inf	inf	inf	inf	inf
  当前父节点		0	0
  是否已处理	true	true	false	false	false	false	false	false

• b) 最小代价值是节点1，找出其邻居(3,4)，更新代价值(11,10)和父节点(代价值比原有 值大就忽略):

  节点	0	1	2	3	4	5	6	7
  当前最小代价值	0	1	5	11	10	inf	inf	inf
  当前父节点		0	0	1	1
  是否已处理	true	true	false	false	false	false	false	false

• c) 找出未处理节点(2,3,4)中代价最小的节点2，将其标记为已处理, 再找出节点2的邻居 重复步骤2, 直到所有节点都已标记为已处理(或者目标点标记为已处理):

  节点	0	1	2	3	4	5	6	7
  当前最小代价值	0	1	5	11	10	inf	inf	inf
  当前父节点		0	0	1	1
  是否已处理	true	true	true	false	false	false	false	false

  下面将所有步骤进行完：

  • 节点2的邻居(5,6)，更新代价值及父节点(6/p=2,7/p=2), 未处理的最小代价是节点 5标记为已处理:

    节点	0	1	2	3	4	5	6	7
    当前最小代价值	0	1	5	11	10	6	7	inf
    当前父节点		0	0	1	1	2	2
    是否已处理	true	true	true	false	false	true	false	false

  • 节点5的邻居(7)，更新代价值及父节点(11/p=5), 未处理的最小代价是节点6标记为 已处理:

    节点	0	1	2	3	4	5	6	7
    当前最小代价值	0	1	5	11	10	6	7	11
    当前父节点		0	0	1	1	2	2	5
    是否已处理	true	true	true	false	false	true	true	false

  • 节点6的邻居(7)，更新代价值及父节点(13>11不更新), 未处理的最小代价是节点4标记为 已处理:

    节点	0	1	2	3	4	5	6	7
    当前最小代价值	0	1	5	11	10	6	7	11
    当前父节点		0	0	1	1	2	2	5
    是否已处理	true	true	true	false	true	true	true	false

    • 节点4的邻居(5,7)，更新代价值及父节点(13>6不更新,14>11不更新), 未处理的最 小代价是节点3和7标记为 已处理:

      节点	0	1	2	3	4	5	6	7
      当前最小代价值	0	1	5	11	10	6	7	11
      当前父节点		0	0	1	1	2	2	5
      是否已处理	true	true	true	true	true	true	true	true

      所有节点都已处理，结束。

  • d) 查找路径，从目标点依次找父节点，知道起始点，得到路径。比如要查询到节点7 的路径： 7->5->2->0(得到路径0,2,5,7)。如果找到节点4的：4->1->0(路径0,1,4)。

总结

• Dijkstra 保证当前选出的代价最小的节点与起点之间的路径总是最优的，(不管后面)， 所以到目标点时可以保证起点和目标点的路径也是最优的。
• Dijkstra 其实是以 breadth-first 的方法。
• Dijkstra 不能处理负权边的情况, 这时候可以先将所有节点的权重加上一个值使权重变 为正数，或者使用 Bellman-Ford 算法。
• 在Grid-map中使用
• A* 算法的比较
• Dijkstra 算法简称D算法，D* 算法全称是Dynamic A* 算法(是D算法和A*算法的结合)