机器人运动规划-Dijkstra

概述

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

执行步骤

需要的数据结构

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

步骤

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

例子

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

motion-plan-graph.jpg

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*算法的结合)。

Comments

comments powered by Disqus